ROS-向场景中添加障碍物

向场景中添加障碍物

//added at 1227

//增加障碍物

// 声明一个障碍物的实例，并且为其设置一个id，方便对其进行操作，该实例会发布到当前的情景实例中

moveit_msgs::CollisionObject cylinder;

cylinder.id = "arm_cylinder";

// 设置障碍物的外形、尺寸等属性

shape_msgs::SolidPrimitive primitive;

primitive.type = primitive.CYLINDER;

primitive.dimensions.resize(3);

primitive.dimensions[0] = 0.3;

primitive.dimensions[1] = 0.1;

// 设置障碍物的位置

geometry_msgs::Pose pose;

pose.orientation.w = 1.0;

pose.position.x = 0.1;

pose.position.y = 0.2;

pose.position.z = 0.2;

// 将障碍物的属性、位置加入到障碍物的实例中

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)

planning_scene_interface.addCollisionObjects(collision_objects);

其中定义了障碍物的形状和位置

关于形状的定义参考

docs.ros.org/api/shape_m…

位置就是包括坐标和方向

方向是采用四元数表示的，位置是xyz的坐标值