ROS-Industrial 之 MoveIt —— 碰撞物体约束添加

在之前的ROS-Industrial Overview中，ROS-I 的高层架构包含有MoveIt层，用来完成机器人的运动规划任务。在此记录一下在使用过程中遇到的磨人的问题。

1. 问题描述

在使用ROS-I控制实体机器人的过程中，通过向MoveIt的规划环境中添加碰撞物体约束，以生成机器人运动的无碰撞轨迹。但在添加完CollisionObject约束之后，发现添加的物体不能同步到 PlanningSceneInterface 变量中，或者说不能显示到 RVIZ 的规划场景中。比如用以下代码添加场景中已固定的障碍物体：

void my_class_name::setFixedCollisionObjects()
{
	moveit_msgs::CollisionObject collision_object;
	collision_object.header.frame_id = "/world";
	collision_object.id = "table_base";

	shape_msgs::SolidPrimitive primitive;
	primitive.type = primitive.BOX;
	primitive.dimensions.resize(3);
	primitive.dimensions[0] = 0.75;
	primitive.dimensions[1] = 1.1;
	primitive.dimensions[2] = 0.85;

	geometry_msgs::Pose table_pose;
	table_pose.orientation.w = 1.0;
	table_pose.position.x = 0.375;
	table_pose.position.y = 0;
	table_pose.position.x = 0.425;

	collision_object.primitives.push_back(primitive);
	collision_object.primitive_poses.push_back(table_pose);
	collision_object.operation = collision_object.ADD;
	collision_objects.push_back(collision_object);

	planning_scene_interface.addCollisionObjects(collision_objects);
}

其中planning_scene_interface 和 collision_objects定义在类的私有变量中，而在代码执行顺序上，以上函数在类的初始化函数内执行。

void my_class_name::init()
{
	start();  //内部是启动spinner
	setFixedCollisionObjects();
}

期望的是在执行障碍物体添加代码之后，能够在RVIZ的场景中可视化碰撞的物体，也表示障碍物体已经同步到planning_scene_interface中了，即希望看到：

然而实际上如下图所示：

2. 解决方法

不能确定这个方法是不是对所有此类情景都有效，但确实是解决了我遇到的问题，即在添加碰撞物体之前，添加sleep(1.0);如下的代码形式：

void my_class_name::setFixedCollisionObjects()
{
	sleep(1.0);
	
	moveit_msgs::CollisionObject collision_object;
	collision_object.header.frame_id = "/world";
	collision_object.id = "table_base";
	
	% 接下来的其它代码省略掉了……
}

重要的是sleep()函数，而不是里面的时间参数到底是多少，也可以设置时间参数为其它数值：2.1，3.0……，然后就成了期望的情形，可以可视化，并同步到了规划场景变量中。在Moveit PlanningSceneInterface addBox not showing in Rviz中，也说明了如果不在物体添加之前加入sleep()函数，场景中碰撞物体的构建与同步会被跳过。

3. 参考资料

[1] Moveit PlanningSceneInterface addBox not showing in Rviz