Moveit在规划路径的时候考虑如何躲避障碍物的问题，Moveit可以实时的检测空间中的障碍物，并规划处轨迹绕过障碍物。
在场景中加入障碍物方式
在Moveit中 具有一个规划场景监听器的模块结构，可以用来检测机器人场景中是否存在障碍物。障碍物有几种方式可以告诉监听器：
- 通过Rviz界面的形式添加。
- 通过程序（C++/Python）编程进行添加。
- 通过机器人的外部传感器，例如通过Kinect深度相机实时检测到的信息加入到场景中。
在这篇文章中主要介绍前两种方法，后续还会专门介绍第三种方法。
通过Rviz界面
运行以下命令在Rviz中打开机械臂模型：
roslaunch probot_anno_moveit_config demo.launch
启动之后，在如图所示位置有Scene Objects选项卡，用来添加场景物体，可以添加已有模型或者Solidwors构建的模型：
点击左下角的Import Flie可以添加一些模型进来，例如我们直接用.gazebo中的离线模型：
我们选择一个碗的模型添加进来，通过箭头调整碗的位置：
点击Publish Scene将障碍物的位置发布出去：
再次回到Plan选项卡中，拖拽机械臂，使机械臂碰撞障碍物，就会发现机械臂发生碰撞的部分变成了红色。
再次进行归经规划的时候，机械臂就会自动避开障碍物：
编程形式添加障碍物
当机械臂抓取到某个物体之后没，它的轨迹进行规划的时候，需要将抓取得到的物体当作机械臂本身的一部分，必涨的时候要考虑抓取到的物体也不能发生碰撞。 打开模型：
roslaunch probot_anno_moveit_config demo.launch
附着物体避障：
rosrun probot_demo moveit_attached_object_demo.py
运行结果:
编程形式添加障碍物
当机械臂抓取到某个物体之后没，它的轨迹进行规划的时候，需要将抓取得到的物体当作机械臂本身的一部分，必涨的时候要考虑抓取到的物体也不能发生碰撞。 打开模型：
roslaunch probot_anno_moveit_config demo.launch
附着物体避障：
rosrun probot_demo moveit_attached_object_demo.py
运行结果:
运行过程中，抓取到的物体也会进行避障，不会碰到上面绿色的桌面。 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为障碍物的大小
