ROS小车实现检测目标3D位姿，机械臂抓取

简介

本文使用 wheeltec，代码中型号为mini_mec_moveit_six，6自由度机械臂小车。实现用深度相机，检测目标的3D位姿，从图像坐标系转换到机器人坐标系，并用机械臂moveit抓取目标。完整流程的大概实现。

开启机械臂底层节点，包含了moveit

roslaunch wheeltec_arm_pick base_serial.launch

moveit 包含在内

<!-- 开启虚拟机械臂  -->
	<include if="$(arg moveit_config)" file='/home/wheeltec/wheeltec_arm/src/$(arg car_mode)_moveit_config/launch/demo.launch'>
		 <arg if="$(eval voi_arm_1==true and moveit_num=='six_arm')" name="voi_arm" value="false"/>
	</include>

开启深度相机

小车自带的是astra深度相机

roslaunch astra_camera astra.launch 

会有相机的TF坐标无法转换的问题，在虚拟机打开rviz，配置Global Options，Fixed Frame修改为base_link，rviz中TF status Warn警告

No transform from [camera_link] to frame [base_link]

则需要配置相机的TF坐标系

配置相机的TF坐标系

修改robot_model_visualization.launch默认设备mini_mec_moveit_six

  <arg name="car_mode"  default="mini_mec_moveit_six" />

运行

roslaunch turn_on_wheeltec_robot robot_model_visualization.launch

主要用到了tf的static_transform_publisher，发布静态坐标变换，只能发布两个静止的坐标系间的坐标变换

<group if="$(eval car_mode == 'mini_mec_moveit_six')">
      <!-- 用于雷达节点，后面同理不再赘述-->
      <!--node pkg="tf" type="static_transform_publisher" name="base_to_laser" args="0.048 0.00 0.18 3.925 0 0  base_footprint laser 100" /--> <!--A2 laser-->
      <node pkg="tf" type="static_transform_publisher" name="base_to_laser" args="0.048 0.00 0.11 3.14 0 0  base_footprint laser 100" /> <!--A1 laser-->
      <!-- 用于摄像头相关节点，如3d建图导航，后面同理不再赘述-->
      <node pkg="tf" type="static_transform_publisher" name="base_to_camera" args="-0.118 0 0.55 0 0 0   base_footprint camera_link 100" />
      <node pkg="tf" type="static_transform_publisher" name="base_to_gyro" args="0 0 0 0 0 0 base_footprint gyro_link 100" />
  </group>

find-object-2d

我们希望机械臂抓住想要的任何东西，比通过贴标签和纯色物体更加智能一点，可以用find-object-2d模板匹配算法识别物体

安装

sudo apt-get install ros-$ROS_DISTRO-find-object-2d

运行

roslaunch find_object_2d find_object_3d.launch

find_object_2d所需的下面3个topic
Find-Object subscribed to :
/camera/rgb/image_rect_color
/camera/depth_registered/image_raw
/camera/rgb/camera_info
小车自带的astra深度相机启动时就默认发布的topic包含上面3个

launch源码大概如下，可以就用默认的，不用新增launch

<launch>
	<!-- Example finding 3D poses of the objects detected -->	
	<!-- Which image resolution: sd, qhd, hd -->
	<arg name="resolution" default="qhd" />

	<node name="find_object_3d" pkg="find_object_2d" type="find_object_2d" output="screen">
		<param name="gui" value="true" type="bool"/>
		<param name="settings_path" value="~/.ros/find_object_2d.ini" type="str"