如何在Moveit中生成动态的障碍物 （How to change position of collision object in Moveit）

这是我基于ROS在Moveit中生成一个可以移动的障碍物，然后导入rviz中的过程，希望能对做机械臂动态规划的朋友一点点帮助。

#include <moveit/move_group_interface/move_group.h>
    #include <moveit/planning_scene_interface/planning_scene_interface.h>
    #include <moveit_msgs/AttachedCollisionObject.h>
    #include <moveit_msgs/CollisionObject.h>
    #include "std_msgs/String.h"
    #include <sstream>


    int main(int argc, char **argv)
    {
        ros::init(argc, argv, "collision");
        ros::NodeHandle n;
        ros::AsyncSpinner spin(1);
        spin.start();
	
	ros::Publisher pub = n.advertise<geometry_msgs::Pose>("chatter", 1000);
        ros::Rate loop_rate(10);
        // 创建运动规划的情景，等待创建完成
        moveit::planning_interface::PlanningSceneInterface current_scene;
        sleep(5.0);
	 int count = 0;
        std::vector<std::string> object_ids;
	object_ids.push_back("cylinder");
        // 声明一个障碍物的实例，并且为其设置一个id，方便对其进行操作，该实例会发布到当前的情景实例中
        moveit_msgs::CollisionObject cylinder;
        cylinder.id = "cylinder";
        cylinder.header.frame_id = "/root"; 
        cylinder.header.stamp = ros::Time::now();

        // 设置障碍物的外形、尺寸等属性   
        shape_msgs::SolidPrimitive primitive;
        primitive.type = primitive.CYLINDER;
        primitive.dimensions.resize(3);
        primitive.dimensions[0] = 0.6;
        primitive.dimensions[1] = 0.2;
        // 设置障碍物的位置
        geometry_msgs::Pose pose;
        pose.orientation.w = 1.0;
        pose.position.x =  0.0;
        pose.position.y = -0.7;
        pose.position.z =  0.4;
      
        // 将障碍物的属性、位置加入到障碍物的实例中
        cylinder.primitives.push_back(primitive);
        cylinder.primitive_poses.push_back(pose);
        cylinder.operation = cylinder.ADD;

        // 创建一个障碍物的列表，把之前创建的障碍物实例加入其中
        std::vector<moveit_msgs::CollisionObject> collision_objects;
        collision_objects.push_back(cylinder);

        // 所有障碍物加入列表后（这里只有一个障碍物），再把障碍物加入到当前的情景中，如果要删除障碍物，使用removeCollisionObjects(collision_objects)
        current_scene.addCollisionObjects(collision_objects);
	
	cylinder.primitives.clear();//Clear primitives required for MOVE operation (Only to avoid a warning)
	cylinder.operation = cylinder.MOVE; //change operation to MOVE
	
        geometry_msgs::Pose pose_moving;
	pose_moving.orientation.w = 1.0;
        pose_moving.position.x =  0.0;
        pose_moving.position.y = -0.65;
        pose_moving.position.z =  0.4;
        double step = 0.05;
	while(ros::ok())
	{
	  collision_objects.clear(); 
	  cylinder.primitive_poses.clear();
	  cylinder.id = "cylinder";
          pose_moving.position.x =  pose_moving.position.x + step;
	  if(pose_moving.position.x > 0.5 || pose_moving.position.x <- 0.5)
	    step = -step;
          cylinder.primitive_poses.push_back(pose_moving);
	  collision_objects.push_back(cylinder);
	  current_scene.applyCollisionObjects(collision_objects); 
	  //applyCollisionObjects apply changes to planning interface

	   ros::Duration(0.5).sleep(); //ZzZzZ 
	   //publish topic:position of moving obstacle
	   pub.publish(pose_moving);    
	}
        current_scene.removeCollisionObjects(object_ids); //delete objects from planning interface*/
        ros::shutdown();
        return 0;
    }

           

参考的网站：https://answers.ros.org/question/276194/how-to-change-position-of-collision-object-in-moveit/