Moveit在规划路径的时候考虑如何躲避障碍物的问题，Moveit可以实时的检测空间中的障碍物，并规划处轨迹绕过障碍物。

在场景中加入障碍物方式

  1   在Moveit中 具有一个规划场景监听器的模块结构，可以用来检测机器人场景中是否存在障碍物。障碍物有几种方式可以告诉监听器：
  1. 通过Rviz界面的形式添加。
  2. 通过程序（C++/Python）编程进行添加。
  3. 通过机器人的外部传感器，例如通过Kinect深度相机实时检测到的信息加入到场景中。
在这篇文章中主要介绍前两种方法，后续还会专门介绍第三种方法。

通过Rviz界面

运行以下命令在Rviz中打开机械臂模型：   
roslaunch probot_anno_moveit_config demo.launch
  启动之后，在如图所示位置有Scene Objects选项卡，用来添加场景物体，可以添加已有模型或者Solidwors构建的模型：   2   点击左下角的Import Flie可以添加一些模型进来，例如我们直接用.gazebo中的离线模型：   3   我们选择一个碗的模型添加进来，通过箭头调整碗的位置：   4   点击Publish Scene将障碍物的位置发布出去：   5   再次回到Plan选项卡中，拖拽机械臂，使机械臂碰撞障碍物，就会发现机械臂发生碰撞的部分变成了红色。   7   再次进行归经规划的时候，机械臂就会自动避开障碍物：   8  

编程形式添加障碍物

当机械臂抓取到某个物体之后没，它的轨迹进行规划的时候，需要将抓取得到的物体当作机械臂本身的一部分，必涨的时候要考虑抓取到的物体也不能发生碰撞。 打开模型：   
roslaunch probot_anno_moveit_config demo.launch
  附着物体避障：   
rosrun probot_demo moveit_attached_object_demo.py
  运行结果:   9  

编程形式添加障碍物

当机械臂抓取到某个物体之后没，它的轨迹进行规划的时候，需要将抓取得到的物体当作机械臂本身的一部分，必涨的时候要考虑抓取到的物体也不能发生碰撞。 打开模型：   
roslaunch probot_anno_moveit_config demo.launch
  附着物体避障：   
rosrun probot_demo moveit_attached_object_demo.py
  运行结果:   10   运行过程中，抓取到的物体也会进行避障，不会碰到上面绿色的桌面。 Python源码解析：   
import rospy, sys
import thread, copy
import moveit_commander
from moveit_commander import RobotCommander, MoveGroupCommander, PlanningSceneInterface
from geometry_msgs.msg import PoseStamped, Pose
from moveit_msgs.msg import CollisionObject, AttachedCollisionObject, PlanningScene
from math import radians
from copy import deepcopy
 
class MoveAttachedObjectDemo:
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)
        
        # 初始化ROS节点
        rospy.init_node('moveit_attached_object_demo')
        
        # 初始化场景对象，用来监听外部环境的变化
        scene = PlanningSceneInterface()
        rospy.sleep(1)
                                
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = MoveGroupCommander('manipulator')
        
        # 获取终端link的名称
        end_effector_link = arm.get_end_effector_link()
        
        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.05)
       
        # 当运动规划失败后，允许重新规划
        arm.allow_replanning(True)
        arm.set_planning_time(10)
 
        # 控制机械臂回到初始化位置
        arm.set_named_target('home')
        arm.go()
        
        # 移除场景中之前运行残留的物体
        scene.remove_attached_object(end_effector_link, 'tool')
        scene.remove_world_object('table') 
        scene.remove_world_object('target')
 
        # 设置桌面的高度
        table_ground = 0.6
        
        # 设置table和tool的三维尺寸
        table_size = [0.1, 0.7, 0.01]  //设置长宽高
        tool_size = [0.2, 0.02, 0.02]
        
        # 设置tool的位姿
        p = PoseStamped() 
        #tool放置位置的参考坐标系
        p.header.frame_id = end_effector_link
        
        #tool的具体位置姿态
        p.pose.position.x = tool_size[0] / 2.0 - 0.025
        p.pose.position.y = -0.015
        p.pose.position.z = 0.0
        p.pose.orientation.x = 0
        p.pose.orientation.y = 0
        p.pose.orientation.z = 0
        p.pose.orientation.w = 1
        
        # 将tool附着到机器人的终端，该函数为附着某物体到机器人上
        scene.attach_box(end_effector_link, 'tool', p, tool_size)
 
        # 将table加入场景当中
        table_pose = PoseStamped()
        table_pose.header.frame_id = 'base_link'
        table_pose.pose.position.x = 0.25
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box('table', table_pose, table_size)  #添加障碍物
        
        rospy.sleep(2)  
 
        # 更新当前的位姿
        arm.set_start_state_to_current_state()
 
        # 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
        joint_positions = [0.827228546495185, 0.29496592875743577, 1.1185644936946095, -0.7987583317769674, -0.18950024740190782, 0.11752152218233858]
        arm.set_joint_value_target(joint_positions)
                 
        # 控制机械臂完成运动
        arm.go()
        rospy.sleep(1)
        
        # 控制机械臂回到初始化位置
        arm.set_named_target('home')
        arm.go()
 
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
 
if __name__ == "__main__":
    MoveAttachedObjectDemo()
  关键API：scene.attach_box(end_effector_link, 'tool', p, tool_size)
  • end_effector_link为机械臂终端的坐标系
  • 'tool'是要附着的对象
  • p为附着物的位置
  • tool_size为附着物体的尺寸
scene.add_box('table', table_pose, table_size)
  • 'table'是要添加的障碍物对象
  • table_pose为障碍物的姿态信息
  • table_size为障碍物的大小