MoveIt： add - remove collision object 的方法

- 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 data
	moveit_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 data
        moveit_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);
}