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