Collision Checking between collision object and object attached to the robot
Hi,
I have writen a node for attaching a collision object to the planning scene. I am using the Apply Planning Scene service for that. Here is my node:
Vector3d b(0.001, 0.001, 0.001);
moveit_msgs::AttachedCollisionObject attached_object; // to attach the object to the robot
moveit_msgs::CollisionObject remove_object; // to remove the object from the world
geometry_msgs::Pose pose;
moveit_msgs::PlanningScene planning_scene;
attached_object.link_name = "tool0"; //the link to which the object will be attached
attached_object.object.header.frame_id = "tool0"; // w.r.t. the link to which it is attached
attached_object.object.operation = attached_object.object.ADD;
std::string touch_links = "motor";
attached_object.touch_links.push_back(touch_links);
ROS_INFO("Attaching kinematic_e1 to motor and removing it from the world.");
attached_object.object.id = "kinematic_e1";
shapes::Mesh* m = shapes::createMeshFromResource("package://mitsubishi_rv6sd_support/meshes/objects/kinematic_e1.stl",b);
shape_msgs::Mesh mesh;
shapes::ShapeMsg mesh_msg;
shapes::constructMsgFromShape(m, mesh_msg);
mesh = boost::get<shape_msgs::Mesh>(mesh_msg);
pose.orientation.w = 1.0;
attached_object.object.meshes.push_back(mesh);
attached_object.object.mesh_poses.push_back(pose);
remove_object.id = "kinematic_e1";
remove_object.header.frame_id = "base_link";
remove_object.operation = remove_object.REMOVE;
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(remove_object); //This step removes the object from the world
planning_scene.robot_state.attached_collision_objects.push_back(attached_object); //This step attaches the object to the robot
planning_scene.is_diff = true; //Update the planning scene as a diff
planning_scene.robot_state.is_diff = true; //Update the robot state as a diff
//Using services to make changes to the planning scene
ros::ServiceClient planning_scene_diff_client = node_handle.serviceClient<moveit_msgs::ApplyPlanningScene>("apply_planning_scene");
planning_scene_diff_client.waitForExistence();
moveit_msgs::ApplyPlanningScene srv;
srv.request.scene = planning_scene;
planning_scene_diff_client.call(srv);
Then I try to check the AllowedCollisionMatrix for the attached object (kinematic_e1 in this case). I use the GetPlanningScene service to get a copy of the allowed collision matrix. But when I query the entry names in the matrix, I cannot find the name of the attached object in the list.
moveit_msgs::PlanningScene currentScene;
moveit_msgs::GetPlanningScene scene_srv;
scene_srv.request.components.components = scene_srv.request.components.ALLOWED_COLLISION_MATRIX;
if(!client_get_scene_.call(scene_srv))
{
ROS_WARN("Failed to call service /get_planning_scene");
}
else
{
ROS_INFO_STREAM("Initial scene!");
currentScene = scene_srv.response.scene;
moveit_msgs::AllowedCollisionMatrix currentACM = currentScene.allowed_collision_matrix;
ROS_ERROR_STREAM("size of acm_entry_names before " << currentACM.entry_names.size());
for (int i = 0; i < currentACM.entry_names.size(); i++)
ROS_ERROR_STREAM(" Name = " << currentACM.entry_names[i]);
ROS_ERROR_STREAM("size of acm_entry_values before " << currentACM.entry_values.size());
ROS_ERROR_STREAM("size of acm_entry_values[0].entries before " << currentACM.entry_values[0].enabled.size());
How can I find the attached object in the allowed collision matrix?
I dont know if this is related but when I give a pose goal where a definite collision between the robot and the robot attached link wil occur, the planning fails. This kind of tells me that MoveIt! plans taking into consideration the attached object. It appears correctly in RViz as well. Then why does the name of the attached object not show up in the allowed collision matrix?
