利用MoveIt实现碰撞检测

      最近在利用Gazebo + MoveIt搭建仿真环境，但是遇到了很多问题，尤其是碰撞检测的函数接口上，也没找到有什么清晰的文档，官方文档(1)、文档(2)只是给出了最简单的调用，而且好像并不是很管用......然后我们需要看的文档也有点多，什么PlanningScene、PlanningSceneInterface、PlanningSceneMonitor、MoveGroup等等。所以，我决定在这篇博客中给出一个可行的碰撞检测方案。

      1）基础概念

      首先，我们需要了解的是，我们的场景（PlanningScene）仅仅只是MoveGroup所持有的一个动态场景（scene）在某时刻的截图。换句话说，我们可以同时创建多个场景，而不必担心它们会相互影响。

      其次，需要知道PlanningSceneInterface是MoveIt开发者提供给我们的简单易用的接口类。比如，我们想要往场景中加一个物体，那么就可以调用该类的addCollisionObjects()函数来实现。值得注意的是，这个函数本质上就是向/planning_scene话题发布了一个PlanningScene类型的消息，而我们的MoveGroup则订阅了这个消息，因此MoveGroup就知道了这个物体的存在。

      上面提到的PlanningSceneMonitor则为我们与MoveGroup交互提供了接口。比如获取一个锁定的场景（锁定也就是说，我们这时候为MoveGroup中的动态场景拍了一张照片），我们可以调用LockedPlanningSceneRW()函数来实现。

      2）为什么现有的一些文档不是很好用？

      现有的文档一般先是介绍如何添加物体，所以定义了一个PlanningSceneInterface的对象，命名为current_scene，然后调用上面说的addCollisionObjects()函数发布一个PlanningScene的消息，此时MoveGroup知道了。然后这些文档会说我们定义一个PlanningScene：

robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
planning_scene::PlanningScene planning_scene(kinematic_model);

      利用机器人在参数服务器上的robot_description来定义一个planning_scene，可惜的是，这个planning_scene并不知道碰撞物体的存在，因为它并没有与MoveGroup通信，所以此时碰撞检测是错的。那我们就要思考一个问题了，如何让我们的planning_scene知道碰撞物体的存在？可以使用PlanningSceneMonitor。

planning_scene_monitor::PlanningSceneMonitorPtr monitor_ptr_udef = 
		boost::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
monitor_ptr_udef->requestPlanningSceneState("get_planning_scene");
planning_scene_monitor::LockedPlanningSceneRW ps(monitor_ptr_udef);
ps->getCurrentStateNonConst().update();
planning_scene::PlanningScenePtr scene = ps->diff();
scene->decoupleParent();

      这里我们先定义了一个PlanningSceneMonitorPtr (monitor_ptr_udef)，它实际上就是PlanningSceneMonitor*，注意，这里我们是用robot_description进行初始化的，所以它知道机器人的存在。然后我们利用requestPlanningSceneState()来启用get_planning_scene服务（为什么要启用这个服务？我们的LockedPlanningSceneRW其实可以等价于monitor_ptr_udef->getPlanningScene()）。接着定义一个LockedPlanningSceneRW，它可以看作是对于MoveGroup中动态场景的一次拍照，为什么要Locked，因为如果我们的场景随着MoveGroup中的场景变化，那么可能会产生一系列的问题，比如如果我们后面想要创建另一个场景，那么这里的场景也会变化，所以还是使用Locked比较好，这同时也是为什么我们要decoupleParent()的原因。ps->diff()就是创建一个副本，然后我们调用decoupleParent()将副本剪下来。

      3）如何检测碰撞？

      我们利用PlanningScene类的isStateValid()函数来进行碰撞检测，实际上这是间接的，因为这个函数返回的是我们当前的场景是否合法，如果发生自碰撞或者机器人与场景的碰撞等，该函数就回返回False，我们只需要利用“非”运算即可变为我们想要的碰撞消息。

      4）参考代码：

#include <ros/ros.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit_msgs/CollisionObject.h>
#include <sensor_msgs/JointState.h>

#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/move_group_interface/move_group.h>

#include <collision_detection/Collision.h>

std::vector<std::string> joint_names;
std::vector<double> joint_positions;

void statesMessageReceived(const sensor_msgs::JointState ¤t_state)
{
	joint_names.clear();
	joint_positions.clear();
	for(int i = 0; i < 19; i++){
		joint_names.push_back(current_state.name[i]);
		joint_positions.push_back(current_state.position[i]);
	}
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "check_collision");
	ros::NodeHandle nh;

	ros::Subscriber state_sub = nh.subscribe("/robot/joint_states", 1, &statesMessageReceived);
	ros::Publisher collision_state_pub = nh.advertise<collision_detection::Collision>("collision_detection", 1);

	moveit::planning_interface::MoveGroup control_group("both_arms");
	moveit::planning_interface::PlanningSceneInterface current_scene;

	// Wait for the planning scene interface to be created.
	sleep(5.0);

	moveit_msgs::CollisionObject collision_object;
	collision_object.header.frame_id = control_group.getPlanningFrame();
	collision_object.header.stamp = ros::Time::now();
	collision_object.id = "table";

	shape_msgs::SolidPrimitive primitive;
	primitive.type = primitive.BOX;
	primitive.dimensions.resize(3);
	primitive.dimensions[0] = 0.913;
	primitive.dimensions[1] = 0.913;
	primitive.dimensions[2] = 0.04;

	geometry_msgs::Pose table_pose;
	table_pose.position.x = 0.9;
	table_pose.position.y = 0.0;
	table_pose.position.z = -0.232;
	table_pose.orientation.w = 1.0;


	collision_object.primitives.push_back(primitive);
	collision_object.primitive_poses.push_back(table_pose);
	collision_object.operation = collision_object.ADD;
	std::vector<moveit_msgs::CollisionObject> collision_objects;
	collision_objects.push_back(collision_object);

	ROS_INFO_STREAM("Add an table into the world.");
	current_scene.addCollisionObjects(collision_objects);

	sleep(10.0);
	planning_scene_monitor::PlanningSceneMonitorPtr monitor_ptr_udef = 
		boost::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
	monitor_ptr_udef->requestPlanningSceneState("get_planning_scene");
	planning_scene_monitor::LockedPlanningSceneRW ps(monitor_ptr_udef);
	ps->getCurrentStateNonConst().update();
	planning_scene::PlanningScenePtr scene = ps->diff();
	scene->decoupleParent();

	ros::Rate rate(10);
	while(ros::ok())
	{
		ros::spinOnce();

		robot_state::RobotState state(scene->getRobotModel());

		state.setVariablePositions(joint_names, joint_positions);
		scene->setCurrentState(state);

		robot_state::RobotState& current_state = scene->getCurrentStateNonConst();
		bool flag = scene->isStateValid(current_state, "both_arms");
		// std::cout << "Robot state is valid or not: " << flag << std::endl;

		collision_detection::Collision collision_msg;
		collision_msg.header.stamp = ros::Time::now();
		collision_msg.flag = not flag;
		collision_state_pub.publish(collision_msg);

		rate.sleep();
	}
	return 0;
}

      上面的代码实时订阅机器人的关节状态，并调用setVariablePositions()以及setCurrentState()将场景中机器人的状态设置为实际状态，最后调用isStateValid()来检测合法性，转化为碰撞消息发布。

      

      本文就介绍到这里了，有什么问题欢迎交流~