前言

这一节玩一玩ros中的fetch机器人，和它相似的是pr2机器人，pr2有两条胳膊。   fetch:     PR2:    

fetch相关资料

http://wiki.ros.org/Robots/fetch http://docs.fetchrobotics.com/ https://github.com/fetchrobotics  

安装fetch

fetch的话现在的作者应该是还在维护更新的，最开始只支持indigo版本的，现在应该是kinetic之后的版本都支持了，这里笔者用的是kinetic版本。   安装命令：

sudo apt-get install ros-kinetic-fetch-gazebo-demo

  安装测试：

roslaunch fetch_gazebo simulation.launch

  测试结果：    

在gazebo中建立自己的场景

目标样式如图：     这里的墙体如果自己手写每个面的代码属实有点多，这里使用empy模板引擎，通过@符号作为标注在xml文件中插入python代码。   em文件代码如下：  

<?xml version="1.0" ?>
<sdf version="1.4">
<world name="aisle">
<gui>
  <camera name="camera">
    <pose>3 -2 3.5 0.0 .85 2.4</pose>
    <view_controller>orbit</view_controller>
  </camera>
</gui>
<include><uri>model://sun</uri></include>
<include><uri>model://ground_plane</uri></include>
@{from numpy import arange}@
@{bin_count = 0}
@[for side in ['left','right']]
  @[if side == 'left']
    @{y = -1.5}
    @{yaw = 3.1415}
  @[else]
    @{y = 1.5}
    @{yaw = 0}
  @[end if]
  @[for x in arange(-1.5, 1.5, 0.5)]
    <include>
      <name>bin_@(bin_count)</name>
      <pose>@(x) @(y) 0.5 0 0 @(yaw)</pose>
      <uri>model://bin</uri>
    </include>
    <model name="bin_@(bin_count)_tag">
      <static>true</static>
      <pose>@(x) @(y*1.125) 0.63 0 0 @(yaw)</pose>
      <link name="link">
        <visual name="visual">
          <geometry><box><size>0.2 0.01 0.2</size></box></geometry>
          <material>
            <script>
              <uri>model://bin/tags</uri>
              <!--<uri>model://bin/materials/textures</uri>-->
              <name>product_@(bin_count)</name>
            </script>
          </material>
        </visual>
      </link>
    </model>
    @{bin_count += 1}
  @[end for]
@[end for]
@[def wall(p1, p2, height)]
  @{wall.count += 1}
  @[if abs(p1[0]-p2[0]) < 0.01]
    @{thickness_x = 0.1}
    @{thickness_y = abs(p1[1]-p2[1])}
  @[else]
    @{thickness_x = abs(p1[0]-p2[0])}
    @{thickness_y = 0.1}
  @[end if]
  <model name="wall_@(wall.count)">
    <static>true</static>
    <pose>@((p1[0]+p2[0])/2.) @((p1[1]+p2[1])/2.) @(height/2.) 0 0 0</pose>
    <link name="link">
      <collision name='visual'>
        <geometry>
          <box>
            <size>@(thickness_x) @(thickness_y) @(height)</size>
          </box>
        </geometry>
      </collision>
      <visual name='visual'>
        <geometry>
          <box>
            <size>@(thickness_x) @(thickness_y) @(height)</size>
          </box>
        </geometry>
      </visual>
    </link>
  </model>
@[end def]
@{wall.count = 0}
@( wall((-1.75, -1.75), ( 6.00 , -1.75), 0.7) )
@( wall((-1.75, -1.75), (-1.75,   1.75), 0.7) )
@( wall((-1.75,  1.75), ( 6.00,   1.75), 0.7) )
@( wall(( 3.00,  0.75), ( 3.00,   1.75), 0.7) )
@( wall(( 3.00, -0.75), ( 3.00,  -1.75), 0.7) )
@( wall(( 6.00, -1.75), ( 6.00,  -1.00), 0.7) )
@( wall(( 6.00,  0.00), ( 6.00,   1.75), 0.7) )
@( wall(( 5.00, -1.75), ( 5.00,   1.75), 0.7) )
  <model name="counter_top">
    <static>true</static>
    <pose>4.9 0 0.7 0 0 0</pose>
    <link name="link">
      <collision name="collision">
        <geometry><box><size>0.4 3.5 0.05</size></box></geometry>
      </collision>
      <visual name="visual">
        <geometry><box><size>0.4 3.5 0.05</size></box></geometry>
      </visual>
    </link>
  </model>
</world>
</sdf>

  生成xml命令：

empy aisle.world.em > aisle.world

  生成的world文件就是我们的世界地图。

建立蓝色的木盒子

盒子的xml：  

<?xml version='1.0'?>
<sdf version ='1.4'>
  <model name ='product_0'>
    <link name ='link'>
      <inertial>
        <mass>0.1</mass>
        <inertia>
          <ixx>0.0000417</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0000417</iyy>
          <iyz>0</iyz>
          <izz>0.0000417</izz>
        </inertia>
      </inertial>
      <collision name="collision">
        <geometry>
          <box><size>0.05 0.05 0.05</size></box>
        </geometry>
        <surface>
          <friction>
            <ode>
              <mu>0.4</mu>
              <mu2>0.4</mu2>
            </ode>
          </friction>
          <contact>
            <ode>
              <max_vel>0.1</max_vel>
              <min_depth>0.0001</min_depth>
            </ode>
          </contact>
        </surface>
      </collision>
      <visual name="visual">
        <geometry>
          <box><size>0.05 0.05 0.05</size></box>
        </geometry>
        <material><script><name>Gazebo/Green</name></script></material>
      </visual>
    </link>
  </model>
</sdf>

  物块的xml：  

<?xml version='1.0'?>
<sdf version ='1.4'>
  <model name ='box'>
    <static>true</static> <!--1-->
    <link name ='bottom'> <!--2-->
      <collision name="collision_bottom">
        <geometry>
          <box>
            <size>0.4 0.4 0.02</size> <!--3-->
          </box>
        </geometry>
      </collision>
      <collision name="collision_left"> <!--4-->
        <pose>-0.2 0 0.05 0 0 0</pose>
        <geometry><box><size>0.02 0.4 0.1</size></box></geometry>
      </collision>
      <collision name="collision_right">
        <pose>0.2 0 0.05 0 0 0</pose>
        <geometry><box><size>0.02 0.4 0.1</size></box></geometry>
      </collision>
      <collision name="collision_back">
        <pose>0 0.2 0.1 0 0 0</pose>
        <geometry><box><size>0.4 0.02 0.2</size></box></geometry>
      </collision>
      <!--<collision name="collision_front">
        <pose>0 -0.2 0.025 0 0 0</pose>
        <geometry><box><size>0.4 0.02 0.05</size></box></geometry>
      </collision>-->
      <visual name="visual_bottom">
        <geometry><box><size>0.4 0.4 0.02</size></box></geometry>
        <material><script><name>Gazebo/Blue</name></script></material>
      </visual>
      <visual name="visual_left">
        <pose>-0.2 0 0.05 0 0 0</pose>
        <geometry><box><size>0.02 0.4 0.1</size></box></geometry>
        <material><script><name>Gazebo/Blue</name></script></material>
      </visual>
      <visual name="visual_right">
        <pose>0.2 0 0.05 0 0 0</pose>
        <geometry><box><size>0.02 0.4 0.1</size></box></geometry>
        <material><script><name>Gazebo/Blue</name></script></material>
      </visual>
      <visual name="visual_back">
        <pose>0 0.2 0.1 0 0 0</pose>
        <geometry><box><size>0.4 0.02 0.2</size></box></geometry>
        <material><script><name>Gazebo/Blue</name></script></material>
      </visual>
      <!--<visual name="visual_front">
        <pose>0 -0.2 0.025 0 0 0</pose>
        <geometry><box><size>0.4 0.02 0.05</size></box></geometry>
        <material><script><name>Gazebo/Blue</name></script></material>
      </visual>-->
    </link>
  </model>
</sdf>

  生成二维码描述文件的代码：  

#!/usr/bin/env python
import os
for i in xrange(0,12):
    os.system("convert ~/fetch_ws/src/fetch/models/bin/tags/MarkerData_%d.png -bordercolor white -border 100x100 " +
              "~/fetch_ws/src/fetch/models/bin/tags/MarkerData_%d.png" % i) #<2>
    with open("product_%d.material" % i, 'w') as f: #<3>
      f.write("""
material product_%d {
  receive_shadows on
  technique {
    pass {
      ambient 1.0 1.0 1.0 1.0
      diffuse 1.0 1.0 1.0 1.0
      specular 0.5 0.5 0.5 1.0
      lighting on
      shading gouraud
      texture_unit { texture MarkerData_%d.png }
    }
  }
}
""" % (i, i))

创建世界地图的启动文件stockroom.launch

<launch>
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find fetch)/worlds/aisle.world"/>
  </include>
  <include file="$(find fetch_gazebo)/launch/include/fetch.launch.xml"/>
  <node pkg="fetch" name="prepare_simulated_robot" 
    type="prepare_simulated_robot.py" output="screen">
  </node>
</launch>

启动世界地图

roslaunch fetch stockroom.launch

生成抓取物块

生成代码：  

#!/usr/bin/env python
import rospy, tf
from gazebo_msgs.srv import *
from geometry_msgs.msg import *

if __name__ == '__main__':
  rospy.init_node("stock_products")
  rospy.wait_for_service("gazebo/delete_model") # <1>
  rospy.wait_for_service("gazebo/spawn_sdf_model")
  delete_model = rospy.ServiceProxy("gazebo/delete_model", DeleteModel)
  s = rospy.ServiceProxy("gazebo/spawn_sdf_model", SpawnModel)
  orient = Quaternion(*tf.transformations.quaternion_from_euler(0, 0, 0))
  with open("models/product_0/model.sdf", "r") as f:
    product_xml = f.read() # <2>
  for product_num in xrange(0, 12):
    item_name = "product_{0}_0".format(product_num)
    delete_model(item_name) # <3>
  for product_num in xrange(0, 12):
    bin_y = 2.8 * (product_num / 6) - 1.4 # <4>
    bin_x = 0.5 * (product_num % 6) - 1.5
    item_name = "product_{0}_0".format(product_num)
    item_pose = Pose(Point(x=bin_x, y=bin_y, z=2), orient) # <5>
    s(item_name, product_xml, "", item_pose, "world") # <6>

  生成的命令：  

python stock_products.py 

  生成效果：    

场景栅格地图

    yaml描述文件：  

image: map.pgm
resolution: 0.050000
origin: [-10.000000, -10.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

  这里也可以自己用gmapping包建图。

导航脚本

go_to_bin.py  

#!/usr/bin/env python
import os, sys, rospy, tf, actionlib
from geometry_msgs.msg import *
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from tf.transformations import quaternion_from_euler
from look_at_bin import look_at_bin
import moveit_commander
from control_msgs.msg import (GripperCommandAction, GripperCommandGoal)
import sys, rospy, tf, actionlib, moveit_commander
from control_msgs.msg import (GripperCommandAction, GripperCommandGoal)
from geometry_msgs.msg import *
from tf.transformations import quaternion_from_euler
from look_at_bin import look_at_bin
from std_srvs.srv import Empty
from moveit_msgs.msg import CollisionObject
from moveit_commander import PlanningSceneInterface

if __name__ == '__main__':
  os.system("rosservice call /move_base/clear_costmaps")
  rospy.init_node('go_to_bin')
  args = rospy.myargv(argv=sys.argv)
  if len(args) != 2:
    print "usage: go_to_bin.py BIN_NUMBER"
    sys.exit(1)
  bin_number = int(args[1])
  move_base = actionlib.SimpleActionClient('move_base', MoveBaseAction)
  move_base.wait_for_server()
  goal = MoveBaseGoal()
  goal.target_pose.header.frame_id = 'map'
  clear_octomap = rospy.ServiceProxy("/clear_octomap",Empty)
  if bin_number == 100:
    goal.target_pose.pose.position.x = 3.75
    goal.target_pose.pose.position.y = 0
  else:
    goal.target_pose.pose.position.x = 0.5 * (bin_number % 6) - 1.5
    goal.target_pose.pose.position.y = 1.1 * (bin_number / 6) - 0.55

  if bin_number < 6:
    yaw = -1.57
  elif bin_number == 100:
    yaw = 0
  else:
    yaw = 1.57

  orient = Quaternion(*quaternion_from_euler(0, 0, yaw))
  goal.target_pose.pose.orientation = orient
  move_base.send_goal(goal)
  move_base.wait_for_result()
  for i in range(2):
  	look_at_bin()

fetch低头脚本

look_at_bin.py  

#!/usr/bin/env python
import sys, rospy, actionlib
from control_msgs.msg import PointHeadAction, PointHeadGoal

def look_at_bin():
  head_client = actionlib.SimpleActionClient("head_controller/point_head",
    PointHeadAction)
  head_client.wait_for_server()
  goal = PointHeadGoal()
  goal.target.header.stamp = rospy.Time.now()
  goal.target.header.frame_id = "base_link"
  goal.target.point.x = 0.7#0.7
  goal.target.point.y = 0
  goal.target.point.z = 0.4#0.4
  goal.min_duration = rospy.Duration(1.0)
  head_client.send_goal(goal)
  head_client.wait_for_result()

if __name__ == '__main__':
  rospy.init_node('look_at_bin')
  look_at_bin()

fetch抓取代码（里面的一些抓取参数需要修改）

#!/usr/bin/env python
import sys, rospy, tf, actionlib, moveit_commander
from control_msgs.msg import (GripperCommandAction, GripperCommandGoal)
from geometry_msgs.msg import *
from tf.transformations import quaternion_from_euler
from look_at_bin import look_at_bin
from std_srvs.srv import Empty
from moveit_msgs.msg import CollisionObject
from moveit_commander import PlanningSceneInterface

if __name__ == '__main__':
  moveit_commander.roscpp_initialize(sys.argv)
  rospy.init_node('pick_up_item')
  args = rospy.myargv(argv = sys.argv)
  if len(args) != 2:
    print("usage: pick_up_item.py BIN_NUMBER")
    sys.exit(1)
  item_frame = "item_%d"%int(args[1])

  rospy.wait_for_service("/clear_octomap")
  clear_octomap = rospy.ServiceProxy("/clear_octomap",Empty)

  gripper = actionlib.SimpleActionClient("gripper_controller/gripper_action",\
    GripperCommandAction)
  gripper.wait_for_server() # <1>

  arm = moveit_commander.MoveGroupCommander("arm") # <2>
  arm.allow_replanning(True)
  tf_listener = tf.TransformListener() # <3>
  rate = rospy.Rate(10)

  gripper_goal = GripperCommandGoal() # <4>
  gripper_goal.command.max_effort = 10.0

  scene = PlanningSceneInterface("base_link")

  p = Pose()
  p.position.x = 0.4
  p.position.y = -0.4
  p.position.z = 0.7
  p.orientation = Quaternion(*quaternion_from_euler(0, 1, 1))
  arm.set_pose_target(p) # <5>
  while True:
    if arm.go(True):
      break
    clear_octomap()

  look_at_bin()

  while not rospy.is_shutdown():
    rate.sleep()
    try:
      t = tf_listener.getLatestCommonTime('/base_link', item_frame) # <7>
      if (rospy.Time.now() - t).to_sec() > 0.2:
        rospy.sleep(0.1)
        continue

      (item_translation, item_orientation) = \
        tf_listener.lookupTransform('/base_link', item_frame, t) # <8>
    except(tf.LookupException,tf.Exception,\
           tf.ConnectivityException, tf.ExtrapolationException):
      print("exception!")
      continue

    gripper_goal.command.position = 0.15
    gripper.send_goal(gripper_goal) # <9>
    gripper.wait_for_result(rospy.Duration(1.0))

    print("item: " + str(item_translation))
    p.position.x = item_translation[0]
    p.position.y = item_translation[1]
    p.position.z = item_translation[2] + 0.2
    p.orientation = Quaternion(*quaternion_from_euler(0, 1.2, 0))
    arm.set_pose_target(p)
    while True:
      if arm.go(True):
        break
      clear_octomap()

    print("put down")
    p.position.x = item_translation[0] + 0.25
    p.position.y = item_translation[1] - 0.03
    p.position.z = item_translation[2] + 0.34
    p.orientation = Quaternion(*quaternion_from_euler(0, 1.2, 0))
    arm.set_pose_target(p)
    while True:
      if arm.go(True):
        break
      clear_octomap()
   
    print("catch")
    gripper_goal.command.position = 0
    gripper.send_goal(gripper_goal)
    gripper.wait_for_result(rospy.Duration(2.0))
    while True:
      if arm.go(True):
        break
      clear_octomap()

    rospy.sleep(1)
    print("put up")
    p.position.x = item_translation[0] + 0.24
    p.position.y = item_translation[1] - 0.03
    p.position.z = 0.75
    p.orientation = Quaternion(*quaternion_from_euler(0, 1.2, 0))
    arm.set_pose_target(p)
    while True:
      if arm.go(True):
        break
      clear_octomap()
    
    print("back")
    p.position.x = 0.4
    p.position.y = -0.4
    p.position.z = 0.7
    p.orientation = Quaternion(*quaternion_from_euler(0, 1.2, 0))
    arm.set_pose_target(p)
    while True:
      if arm.go(True):
        break
      clear_octomap()
    rospy.sleep(1)

    p.orientation = Quaternion(*quaternion_from_euler(0, -1.5, 0))
    arm.set_pose_target(p)
    while True:
      if arm.go(True):
        break
      clear_octomap()

    print("in")
    p.position.x = 0.38
    p.position.y = -0.25
    p.position.z = 0.6
    p.orientation = Quaternion(*quaternion_from_euler(0, -1.5, 0))
    arm.set_pose_target(p)
    while True:
      if arm.go(True):
        break
      clear_octomap()
    rospy.sleep(1)

    break # <13>

导航的launch文件

nav.launch  

<launch>
  <include file="$(find fetch_navigation)/launch/fetch_nav.launch">
    <arg name="map_file" value="$(find fetch)/map.yaml"/>
  </include>
  <node pkg="fetch" name="initial_localization" type="initial_localization.py"/>
</launch>

标签检测的launch文件

markers.launch  

<launch>
  <arg name="marker_size" default="12.3"/> <!--<1>-->
  <arg name="max_new_marker_error" default="0.2"/>
  <arg name="max_track_error" default="0.8"/>
  <arg name="cam_image_topic" default="/head_camera/rgb/image_raw"/>
  <arg name="cam_info_topic" default="/head_camera/rgb/camera_info"/>
  <arg name="output_frame" default="/base_link"/>
	<node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkersNoKinect" respawn="false" output="screen">
		<param name="marker_size"           type="double" value="$(arg marker_size)" />
		<param name="max_new_marker_error"  type="double" value="$(arg max_new_marker_error)" />
		<param name="max_track_error"       type="double" value="$(arg max_track_error)" />
		<param name="output_frame"          type="string" value="$(arg output_frame)" />

		<remap from="camera_image"  to="$(arg cam_image_topic)" />
		<remap from="camera_info"   to="$(arg cam_info_topic)" />
	</node>
  <arg name="tag_rot" default="0 0 0 0 0 -1.57"/> <!--<3>-->
  <arg name="tag_trans" default="0 -0.28 -0.6 0 0 0"/>
  <!--<4>-->
  <node pkg="tf" type="static_transform_publisher" name="map_tomark0"
        args="-1.5 -1.6875 0.63 0 0 3.1415 /map /ar_marker_0 100"/>
  <node pkg="tf" type="static_transform_publisher" name="map_tomark1"
        args="-1.0 -1.6875 0.63 0 0 3.1415 /map /ar_marker_1 100"/>
  <node pkg="tf" type="static_transform_publisher" name="map_tomark2"
        args="-0.5 -1.6875 0.63 0 0 3.1415 /map /ar_marker_2 100"/>
  <node pkg="tf" type="static_transform_publisher" name="map_tomark3"
        args="0 -1.6875 0.63 0 0 3.1415 /map /ar_marker_3 100"/>
  <node pkg="tf" type="static_transform_publisher" name="map_tomark4"
        args="0.5 -1.6875 0.63 0 0 3.1415 /map /ar_marker_4 100"/>
  <node pkg="tf" type="static_transform_publisher" name="map_tomark5"
        args="1.0 -1.6875 0.63 0 0 3.1415 /map /ar_marker_5 100"/>

  <node pkg="tf" type="static_transform_publisher" name="map_tomark6"
        args="-1.5 1.6875 0.63 0 0 0 /map /ar_marker_6 100"/>
  <node pkg="tf" type="static_transform_publisher" name="map_tomark7"
        args="-1.0 1.6875 0.63 0 0 0 /map /ar_marker_7 100"/>
  <node pkg="tf" type="static_transform_publisher" name="map_tomark8"
        args="-0.5 1.6875 0.63 0 0 0 /map /ar_marker_8 100"/>
  <node pkg="tf" type="static_transform_publisher" name="map_tomark9"
        args="-0 1.6875 0.63 0 0 0 /map /ar_marker_9 100"/>
  <node pkg="tf" type="static_transform_publisher" name="map_tomark10"
        args="0.5 1.6875 0.63 0 0 0 /map /ar_marker_10 100"/>
  <node pkg="tf" type="static_transform_publisher" name="map_tomark11"
        args="1.0 1.6875 0.63 0 0 0 /map /ar_marker_11 100"/>

  <node pkg="tf" type="static_transform_publisher" name="ar_0_up"
        args="$(arg tag_rot) ar_marker_0 ar_0_up 100"/>
  <node pkg="tf" type="static_transform_publisher" name="ar_1_up"
        args="$(arg tag_rot)  ar_marker_1 ar_1_up 100"/>
  <node pkg="tf" type="static_transform_publisher" name="ar_2_up"
        args="$(arg tag_rot) ar_marker_2 ar_2_up 100"/>
  <node pkg="tf" type="static_transform_publisher" name="ar_3_up"
        args="$(arg tag_rot) ar_marker_3 ar_3_up 100"/>
  <node pkg="tf" type="static_transform_publisher" name="ar_4_up"
        args="$(arg tag_rot) ar_marker_4 ar_4_up 100"/>
  <node pkg="tf" type="static_transform_publisher" name="ar_5_up"
        args="$(arg tag_rot) ar_marker_5 ar_5_up 100"/>
  <node pkg="tf" type="static_transform_publisher" name="ar_6_up"
        args="$(arg tag_rot) ar_marker_6 ar_6_up 100"/>
  <node pkg="tf" type="static_transform_publisher" name="ar_7_up"
        args="$(arg tag_rot) ar_marker_7 ar_7_up 100"/>
  <node pkg="tf" type="static_transform_publisher" name="ar_8_up"
        args="$(arg tag_rot) ar_marker_8 ar_8_up 100"/>
  <node pkg="tf" type="static_transform_publisher" name="ar_9_up"
        args="$(arg tag_rot) ar_marker_9 ar_9_up 100"/>
  <node pkg="tf" type="static_transform_publisher" name="ar_10_up"
        args="$(arg tag_rot) ar_marker_10 ar_10_up 100"/>
  <node pkg="tf" type="static_transform_publisher" name="ar_11_up"
        args="$(arg tag_rot) ar_marker_11 ar_11_up 100"/>

  <!--<5>-->
  <node pkg="tf" type="static_transform_publisher" name="item_0"
        args="$(arg tag_trans) ar_0_up item_0 100"/>
  <node pkg="tf" type="static_transform_publisher" name="item_1"
        args="$(arg tag_trans) ar_1_up item_1 100"/>
  <node pkg="tf" type="static_transform_publisher" name="item_2"
        args="$(arg tag_trans) ar_2_up item_2 100"/>
  <node pkg="tf" type="static_transform_publisher" name="item_3"
        args="$(arg tag_trans) ar_3_up item_3 100"/>
  <node pkg="tf" type="static_transform_publisher" name="item_4"
        args="$(arg tag_trans) ar_4_up item_4 100"/>
  <node pkg="tf" type="static_transform_publisher" name="item_5"
        args="$(arg tag_trans) ar_5_up item_5 100"/>
  <node pkg="tf" type="static_transform_publisher" name="item_6"
        args="$(arg tag_trans) ar_6_up item_6 100"/>
  <node pkg="tf" type="static_transform_publisher" name="item_7"
        args="$(arg tag_trans) ar_7_up item_7 100"/>
  <node pkg="tf" type="static_transform_publisher" name="item_8"
        args="$(arg tag_trans) ar_8_up item_8 100"/>
  <node pkg="tf" type="static_transform_publisher" name="item_9"
        args="$(arg tag_trans) ar_9_up item_9 100"/>
  <node pkg="tf" type="static_transform_publisher" name="item_10"
        args="$(arg tag_trans) ar_10_up item_10 100"/>
  <node pkg="tf" type="static_transform_publisher" name="item_11"
        args="$(arg tag_trans) ar_11_up item_11 100"/>
</launch>

启动所有功能的launch文件

fetch_all_func.launch  

<launch>
  <include file="$(find fetch)/launch/markers.launch"/>
  <include file="$(find fetch)/launch/nav.launch"/>
  <include file="$(find fetch_moveit_config)/launch/move_group.launch"/>
</launch>

启动场景的launch文件

stockroom.launch  

<launch>
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find fetch)/worlds/aisle.world"/>
  </include>
  <include file="$(find fetch_gazebo)/launch/include/fetch.launch.xml"/>
  <node pkg="fetch" name="prepare_simulated_robot" 
    type="prepare_simulated_robot.py" output="screen">
  </node>
</launch>

测试效果

roslaunch fetch stockroom.launch
roslaunch fetch fetch_all_func.launch
python stock_products.py 
roslaunch fetch go_to_bin.py 5

  机器人走到五号柜台：     抓取五号物块：

rosrun fetch pick_up_item.py 5

总体实现效果