通过moveit在rviz中场景中创建和操控物体

在使用Moveit联合Rviz进行运动规划时，场景中单单只有一个机器人通常是不太够的。一方面，机器人的操作对象可能是不同形状的物体，需要被添加到场景中。另一方面，操作物体的放置环境（比如桌面等等）也应该模拟出来。因此，如何在场景中添加一些必要物体是非常重要的。

Rviz中的MotionPlanning插件中，可以一个一个地添加模型文件到场景中去，但这种方式只适合简单的场景。而且，当你通过某种方式将场景配置好后，无法自动地一次性导入，因此缺乏灵活性。所以只有通过熟悉moveit提供的接口，才能真正实现自己需要的功能。

Basis

首先在moveit的tutorials中，"Planning Scene ROS API"一节中给出了创建并放置一个方块到场景中的例子。对场景物体有两类操作:

• 在场景中创建、删除和移动一个物体。这个很好理解。
• 依附一个物体到机器人，以及从机器分离一个物体。当物体依附到机器人之后，它就被视作机器人的一部分了，随机器人一起运动。例如手抓夹住一个零件后，就将其与手抓视为一体，当经过运动达到指定地点后再分离开。

场景物体的操作可以通过两种方式与moveit_group节点通信完成：

• 通过在planning_scene主题上发布moveit_msgs::PlanningScene类型消息进行操作。这是一种异步方式，发布消息后立刻返回，而不管是否已经处理完成。
• 通过moveit_msgs::ApplyPlanningScene服务类型与moveit_group提供的apply_planning_scene通信。这是一种同步方式，发送操作请求后会阻塞等待处理结果返回。

moveit_msgs::ApplyPlanningScene服务类型里面其实也是使用的moveit_msgs::PlanningScene消息。所以，这里的关键就是moveit_msgs::PlanningScene消息。其定义如下：

# 规划场景的名称
string name
# 机器人状态
RobotState robot_state
# 机器人模型名
string robot_model_name
#
geometry_msgs/TransformStamped[] fixed_frame_transforms
# 碰撞矩阵
AllowedCollisionMatrix allowed_collision_matrix
#
LinkPadding[] link_padding
#
LinkScale[] link_scale
# 指定物体、octomap或collision map的颜色
ObjectColor[] object_colors
# 规划场景的实际内容
PlanningSceneWorld world
# 指示该场景是否解释为相对于其他场景的一个diff(差异)
bool is_diff

moveit_msgs::PlanningScene