First attached object disappears upon attaching second object

edit

Hi everyone,

I have followed the tutorials to add an object to the planning scene and also attach an object to the robot.

I have written 2 nodes for adding 2 different objects to the planning scene and 2 other nodes for attaching them to the robot. I run the nodes one after the other.

My node for adding the first object to the planning scene is:

#include <ros/ros.h>
#include <geometry_msgs/Pose.h>

// MoveIt!
#include <moveit_msgs/PlanningScene.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/GetStateValidity.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/ApplyPlanningScene.h>

#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_state/conversions.h>

#include <geometric_shapes/shape_operations.h>

using namespace Eigen;

int main(int argc, char **argv)
{
  ros::init (argc, argv, "add_kinematic");
  ros::AsyncSpinner spinner(1);
  spinner.start();

  ros::NodeHandle node_handle;
  ros::Duration sleep_time(5.0);
  sleep_time.sleep();

// Advertise the required topic
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// Note that this topic may need to be remapped in the launch file

  ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
  while(planning_scene_diff_publisher.getNumSubscribers() < 1)
  {
    ros::WallDuration sleep_t(0.5);
    sleep_t.sleep();
  }

  moveit_msgs::AttachedCollisionObject attached_object;
  attached_object.link_name = "tool0";
  attached_object.object.header.frame_id = "base_link";      // The header must contain a valid TF frame
  attached_object.object.id = "kinematic_t";     // The id of the object

    Vector3d b(0.001, 0.001, 0.001);
      shapes::Mesh* m = shapes::createMeshFromResource("package://trial/meshes/objects/kinematic_t.stl",b);
     ROS_INFO("kinematic_t loaded");
    shape_msgs::Mesh mesh;
    shapes::ShapeMsg mesh_msg; 
    shapes::constructMsgFromShape(m, mesh_msg);
    mesh = boost::get<shape_msgs::Mesh>(mesh_msg);
    attached_object.object.meshes.resize(1);
    attached_object.object.mesh_poses.resize(1);

  attached_object.object.meshes[0]=mesh;
  //geometry_msgs::Pose target_pose1;    // A default pose
  attached_object.object.mesh_poses[0].position.x = 0.329474;
  attached_object.object.mesh_poses[0].position.y = -0.301132;
  attached_object.object.mesh_poses[0].position.z = 0.3;
  attached_object.object.mesh_poses[0].orientation.w = 0.707;
  attached_object.object.mesh_poses[0].orientation.x = 0.0;
  attached_object.object.mesh_poses[0].orientation.y = 0.707;
  attached_object.object.mesh_poses[0].orientation.z = 0.0;

  attached_object.object.meshes.push_back(mesh);
  attached_object.object.mesh_poses.push_back(attached_object.object.mesh_poses[0]);

// Note that attaching an object to the robot requires
// the corresponding operation to be specified as an ADD operation

  attached_object.object.operation = attached_object.object.ADD;


// Add an object into the environment
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// Add the object into the environment by adding it to
// the set of collision objects in the "world" part of the
// planning scene. Note that we are using only the "object"
// field of the attached_object message here.

  ROS_INFO("Adding kinematic_t to the World");
  moveit_msgs::PlanningScene planning_scene;
  planning_scene.world.collision_objects.push_back(attached_object.object);
  planning_scene.is_diff = true;
  planning_scene_diff_publisher.publish(planning_scene);
  sleep_time.sleep();

  ros::shutdown();
  return 0;
}

Likewise, the second node to add another object (tool) to the planning scene at a different pose.

My node for attaching the first object to the robot is:

#include <ros/ros.h>
#include <geometry_msgs/Pose.h>

// MoveIt!
#include <moveit_msgs/PlanningScene.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/GetStateValidity.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/ApplyPlanningScene.h>

#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_state/robot_state ...