通过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消息中最重要的就是PlanningSceneWolrd消息类型的world。PlanningSceneWolrd消息类型用于定义场景所包含的碰撞物体（场景中除机器人外都可称为碰撞物体–CollisionObject），其定义如下：

# collision objects
CollisionObject[] collision_objects
# 额外的octomap信息，先不管
octomap_msgs/OctomapWithPose octomap

这里面最有用的就是CollisionObject可变数组，CollisionObject消息类型就是用于定义实际的碰撞物体的（哈哈，终于看到实质有趣的东东了）。CollisionObject消息的定义如下：

# 头部定义，包含参考坐标系ID, 时间戳等
Header header
# 物体ID号(在MoveIt中会使用的名称)
string id
# 物体的类型，BOX、SPHERE、CYLINDER以及网格
object_recognition_msgs/ObjectType type
# 基本形状的几何定义
shape_msgs/SolidPrimitive[] primitives
geometry_msgs/Pose[] primitive_poses
# 网格形状的几何定义
shape_msgs/Mesh[] meshes
geometry_msgs/Pose[] mesh_poses
# 边界约束平面 (equation is specified, but the plane can be oriented using an additional pose)
shape_msgs/Plane[] planes
geometry_msgs/Pose[] plane_poses
# 添加物体到场景，如果已经存在，替换之
byte ADD=0
# 从场景中删除与指定ID相符的所有东西
byte REMOVE=1
# 将该形状作为部分添加到场景中已存在的物体。如果不存在，则相当于ADD
byte APPEND=2
# 如果场景中存在指定ID的物体，则将其移动到新的方位。
byte MOVE=3
# 指定物体的操作，可以是ADD、REMOVE、APPEND、MOVE
byte operation

碰撞物体分为两类，基本形状和网格形状。这和URDF中定义Link的形状基本是一致的。基本类型为box、sphere、cylinder,cone等，而网格形状则从stl、dae之类的模型文件导入。

添加物体

创建Topic Publisher

ros::Publisher planning_scene_diff_publisher = node_handle.advertise("planning_scene", 1);
ros::WallDuration sleep_t(0.5);
while (planning_scene_diff_publisher.getNumSubscribers() < 1)
{
  sleep_t.sleep();
}

基本形状物体

添加基本形状的物体的一个样例代码如下：

moveit_msgs::CollisionObject obj;
obj.header.frame_id = "base_link";
obj.id="box"
//定义物体形状尺寸
shape_msgs::SolidPrimitive primitive;
primitive.type=primitive.BOX;
primitive.dimensions.resize(3); //dimensions是一个vector,为其分配3个元素空间
primitive.dimensions[0] =0.1;   //x轴上长度
primitive.dimensions[1] =0.2;   //y轴上长度
primitive.dimensions[2] =0.3;   //z轴上长度
//定义物体方位
geometry_msgs::Pose pose;
pose.orientation.w =1.0;
//将形状添加到obj
obj.primitives.push_back(primitive);
obj.primitive_poses.push_back(pose);
//定义操作为添加
obj.operation = obj.ADD;
//定义一个PlanningScene消息
moveit_msgs::PlanningScene planning_scene;
planning_scene.world.collision_objects.push_back(obj);
planning_scene.is_diff = true;
//发布该消息
planning_scene_diff_publisher.publish(planning_scene);

其中frame_id是必须设置，指出该物体的放置是基于谁的坐标系的，这里的坐标系可以是机器人的各个关节，各个连杆，以及场景中其它物体。

网格形状物体

网格形状则比较麻烦一些。首先可以确定我们是使用CollisionObject消息中的meshes，而非基本形状的primitives。因此可以通过如下方式添加网格,其它的东西保持不变。

shape_msgs::Mesh mesh;
obj.meshs.push_back(mesh);

然而现在还并不知道如何将自定义的网格模型文件导入到shape_msgs::Mesh消息中。好在moveit提供了相应的功能，包含在如下两个文件中：

#include 
#include 
#include 

在名为shapes的namespace中提供了如下两个重要函数：

Mesh* createMeshFromResource(const std::string& resource);
bool constructMsgFromShape(const Shape* shape, ShapeMsg& shape_msg);

还有一些其它工具函数不一一列举，需要实现相应功能是可以进行查阅。使用createMeshFromResource可以读取指定的模型文件，并获得其中的模型网格数据，返回的是一个shapes::Mesh对象（注意不是shape_msgs::Mesh）。

constructMsgFromShape函数可以从一个Shape对象构建出一个ShapeMsg对象。那么shapes::Shape和shapes::Mesh是什么关系呢？通过查看源代码头文件可以看到Shapes::Mesh其实是Shapes::Shape的子类，所以由第一个函数产生的Shape::Mesh对象就可以直接作为第二个函数的第一个参数。第二个问题是Shapes::ShapeMsg是个什么东西。其定义如下：

typedef boost::variant ShapeMsg;

那么它实际上就是shape_msgs::SolidPrimitive、shape_msgs::Mesh和shape_msgs::Plane类型中任何一个，对于由Shape::Mesh这个Shape子类得到的就会是shape_msgs::Mesh。我们只需要通过boost::get(x)就可以把实际类型还原出来。到此所有问题都解决了。
下面给出网格形状物体导入的代码：

shapes::Mesh *mesh=shapes::createMeshFromResource("package:://myrobot_description/meshes/test.stl" );
if(mesh==NULL) {
    //do somethine.
}
shapes::ShapeMsg shape_msg;
shapes::constructMsgFromShape(mesh, shape_msg);
obj.meshes.push_back(boost::get(shape_msg) );
obj.mesh_poses.push_back(pose);

createMeshFromResource函数的输入是一个URI地址，可以是如下几种：

• http://
• ftp://
• package:// --指定的包中的文件
• file:// --指定本地文件

删除场景中的物体

删除场景中的物体比较简单，只需要提供一个带有REMOVE操作以及物体ID的CollisionObject对象即可：

moveit_msgs::CollisionObject remove_obj;
remove_obj.id="box";
remove_obj.operation = remoe_obj.REMOVE;
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(remove_obj);
//发布消息
planning_scene_diff_publisher.publish(planning_scene);

注意其中planning_scene对象不必重新创建，只需要将其中原有的CollisionObject先清除掉在添加即可。

移动场景中的物体

如果场景中存在某物体，则可以对其进行移动操作。移动时需要提供的有：1，物体的ID；2，使用谁的参考坐标系；3，在参考坐标系下的新位置。 其实可以将移动看作是删除后的重新创建，只不过物体的形状不需要再填写的。
示例代码如下：

moveit_msgs::CollisionObject move_obj;
move_obj.id="box";
move_obj.header.frame_id="base_link";
geometry_msgs::Pose new_pose = pose;
new_pose.position.x=pose.position.x+1;
move_obj.mesh_poses.push_back(new_pose);
move_obj.operation=move_obj.MOVE;
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(remove_obj);
//发布消息
planning_scene_diff_publisher.publish(planning_scene);

示例的作用是将物体在原参考坐标下往x轴正方向移动1个单位，因此使用了于创建时一致的参考坐标。

将物体依附到机器人

将物体依附于机器人，实际上是将其与机器人的某连杆（比如手抓）固连在一起。依附物体需要两个步骤：

• 从场景中删除原物体
• 将该物体依附到机器人

删除物体可以按照前面讲过的方法来完成。而进行依附时，需要注意四点：

• 仍需要提供物体的形状信息。
• 删除和依附可以用一个主题发布或者一个服务请求完成。
• 依附操作在物体的放置上和创建物体是相同的，物体的参考坐标系可以是任意的，但一旦依附于link固连后，就会变换到该link的坐标系随该link运动。
• 物体不能和link发生Collision，不然就无法进行运动规划了。

下面先给出一个代码示例：

/* 删除部分 */
moveit_msgs::CollisionObject remove_obj;
remove_obj.id="box"
remove_obj.operation = remove_obj.REMOVE;
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(remove_obj);
/* 依附部分 */
//还是使用原来创建的obj, 不过修改参考坐标系到被依附link
obj.header.frame_id="finger"
obj.operation=obj.ADD;
//创建AttachedCollisionObject
moveit_msgs::AttachedCollisionObject attached_obj;
attached_obj.link_name="finger";
attached_obj.object=obj;      //拷贝
planning_scene.robot_state.attached_collision_objects.push_back(attached_obj);
//发布
planning_scene_diff_publisher.pubilsh(planning_scene);

在上面的代码中我们将原先创建的obj拷贝给attached_obj，当obj包含有大型的网格时是一种低效的方式。所以如果某物体确定为被机器人操作的对象时，可以在一开始就为其创建AttachedCollisionObject对象，在其中载入网格模型，这样就可避免数据的拷贝。

解除依附

解除依附是和依附相反的步骤：

• 解除物体到机器人的依附
• 在场景中重现，也相当于是在场景中重新创建物体。
  示例代码如下：

/* 解除部分 */
moveit_msgs::AttacedCollisionObject detach_obj;
detach_obj.object.id ="box";
detach_obj.object.operation = detach_obj.REMOVE;
planning_scene.robot_state.attached_collision_objects.clear();
planning_scene.robot_state.attached_collision_objects.push_back(detach_object);
planning_scene.robot_state.is_diff = true;
/* 在场景中重现 */
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(obj);
planning_scene.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene);