- moveit添加自定义障碍物方法：
- - 1.将基座架、桌子等物体，通过urdf文件进行添加
- - 2.将相关物体保存成.stl或.dae格式，通过moveit::planning_interface::PlanningSceneInterface相关接口进行添加，两个示例：
参考：https://github.com/hcrmines/apc/blob/master/src/add_objects.cpp；
MoveIt!环境导入自定义的stl文件： https://blog.csdn.net/fei_6/article/details/80443171
1.基本简单特征物体
可通过shape_msgs::SolidPrimitive 进行定义
加载：
// 创建运动规划的情景，等待创建完成moveit::planning_interface::PlanningSceneInterface current_scene;sleep(5.0);// 声明一个障碍物的实例，并且为其设置一个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.6;primitive.dimensions[1] = 0.2;// 设置障碍物的位置geometry_msgs::Pose pose;pose.orientation.w = 1.0;pose.position.x = 0.0;pose.position.y = -0.4;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);移除：
// 添加个需要删除的障碍物名称，然后通过planning scene interface完成删除std::vector<std::string> object_ids;object_ids.push_back("arm_cylinder");current_scene.removeCollisionObjects(object_ids);
2.自定义复杂特征物体
三维软件生成.stl或.dae格式，进行加载
增加：
void AddObjects::addShelf(){// put in collision avoidance datamoveit_msgs::CollisionObject collision_object;collision_object.header.frame_id = arm.getPlanningFrame();/* The id of the object is used to identify it. */collision_object.id = "shelf";shapes::Mesh* m = shapes::createMeshFromResource("package://apc/collision_objects/pod_lowres.stl");shape_msgs::Mesh shelf_mesh;shapes::ShapeMsg shelf_mesh_msg;shapes::constructMsgFromShape(m,shelf_mesh_msg);shelf_mesh = boost::get<shape_msgs::Mesh>(shelf_mesh_msg);/* A pose for the box (specified relative to frame_id) */geometry_msgs::Pose shelf_pose;shelf_pose.orientation.w = 0.5;shelf_pose.orientation.x = 0.5;shelf_pose.orientation.y = 0.5;shelf_pose.orientation.z = 0.5;shelf_pose.position.x = 1.0;shelf_pose.position.y = 0.0;shelf_pose.position.z = -0.91;collision_object.meshes.push_back(shelf_mesh);collision_object.mesh_poses.push_back(shelf_pose);collision_object.operation = collision_object.ADD;ROS_INFO("Add an shelf into the world");moveit_msgs::PlanningScene planning_scene;planning_scene.world.collision_objects.push_back(collision_object);planning_scene.is_diff = true;planning_scene_diff_publisher.publish(planning_scene);
}void AddObjects::addScoop(std::string side){// put in collision avoidance datamoveit_msgs::AttachedCollisionObject scoop_object;if(side == "right"){scoop_object.link_name = "right_gripper";scoop_object.object.header.frame_id = "right_gripper";}else{scoop_object.link_name = "left_gripper";scoop_object.object.header.frame_id = "left_gripper";}/* The id of the object is used to identify it. */scoop_object.object.id = "scoop";shapes::Mesh* m = shapes::createMeshFromResource("package://apc/collision_objects/scoop.stl");shape_msgs::Mesh scoop_mesh;shapes::ShapeMsg scoop_mesh_msg;shapes::constructMsgFromShape(m,scoop_mesh_msg);scoop_mesh = boost::get<shape_msgs::Mesh>(scoop_mesh_msg);/* A pose for the box (specified relative to frame_id) */geometry_msgs::Pose scoop_pose;scoop_pose.orientation.w = 1.0;scoop_pose.orientation.x = 0.0;scoop_pose.orientation.y = 0.0;scoop_pose.orientation.z = 0.0;scoop_pose.position.x = 0.010;scoop_pose.position.y = 0.0;scoop_pose.position.z = 0.00;scoop_object.object.meshes.push_back(scoop_mesh);scoop_object.object.mesh_poses.push_back(scoop_pose);scoop_object.object.operation = scoop_object.object.ADD;ROS_INFO("Add scoop onto the robot");planning_scene.robot_state.attached_collision_objects.push_back(scoop_object);planning_scene.is_diff = true;planning_scene_diff_publisher.publish(planning_scene);
}
移除：
void AddObjects::removeShelf(){moveit::planning_interface::PlanningSceneInterface planning_scene_interface;std::vector<std::string> object_ids;object_ids.push_back("shelf");planning_scene_interface.removeCollisionObjects(object_ids);
}void AddObjects::removeScoop(){moveit::planning_interface::PlanningSceneInterface planning_scene_interface;arm.detachObject("scoop");std::vector<std::string> object_ids;object_ids.push_back("scoop");planning_scene_interface.removeCollisionObjects(object_ids);
}