Ask Your Question
1

First attached object disappears upon attaching second object

asked 2016-12-05 06:58:44 -0500

bhavyadoshi26 gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2016-12-05 14:51:11 -0500

v4hn gravatar image

For internal reasons (that should never have influenced the message definitions imho) there is a second is_diff flag particularly for the robot_state. (planning_scene.robot_state.is_diff) You forgot to set this one too. Otherwise planning_scene.robot_state.attached_collision_objects is probably considered to be the full set of attached objects.

edit flag offensive delete link more

Comments

Indeed I had forgotten to set the robot_state.is_diff flag. I had thought that the planning_scene.is_diff would take care of the attached object. Thank you for your inputs @v4hn

bhavyadoshi26 gravatar imagebhavyadoshi26 ( 2016-12-08 02:27:47 -0500 )edit
1

answered 2016-12-05 07:04:26 -0500

rbbg gravatar image

Hi,

Have a look at this related question. When you are adding objects to the planning scene, they are overwritten if the id is the same. From what I can tell from your code, you are using the kinematic_t id for both, thus overwriting the first object with the information for the second.

Good luck

edit flag offensive delete link more

Comments

I am using different id for the second object. It is tool_t15. What might have created the confusion is that in the first node, I am only adding the object to the planning scene and not attaching them to the robot. That is done through the second node.

bhavyadoshi26 gravatar imagebhavyadoshi26 ( 2016-12-08 02:31:30 -0500 )edit

Since it is a new node, I have to create the mesh message another time and that is where I use the same id again. But the second object that I want to attach has another node with the same structure but only different id.

bhavyadoshi26 gravatar imagebhavyadoshi26 ( 2016-12-08 02:34:08 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2016-12-05 06:58:44 -0500

Seen: 168 times

Last updated: Dec 05 '16