MoveIt： add - remove collision object 的方法

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
    // 创建运动规划的情景，等待创建完成
    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);

1
2
3
4
    // 添加个需要删除的障碍物名称，然后通过planning scene interface完成删除
    std::vector<std::string> object_ids;
    object_ids.push_back("arm_cylinder");
    current_scene.removeCollisionObjects(object_ids);

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
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);
}