0
$\begingroup$

Rosanswers logo

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.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>

#include <geometric_shapes/shape_operations.h>

using namespace Eigen;

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

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

  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::PlanningScene planning_scene;
  moveit_msgs::AttachedCollisionObject attached_object;
 
  attached_object.link_name = "tool0";
  attached_object.object.header.frame_id = "tool0";      // 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("Mesh 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.005;
  attached_object.object.mesh_poses[0].orientation.w = 1.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;
 

  moveit_msgs::CollisionObject remove_object;
  remove_object.id = "kinematic_t";
  remove_object.header.frame_id = "base_link";
  remove_object.operation = remove_object.REMOVE;

// Note how we make sure that the diff message contains no other
// attached objects or collisions objects by clearing those fields
// first.
  // Carry out the REMOVE + ATTACH operation
 
  ROS_INFO("Attaching kinematic_t to motor and removing it from the world.");
  //planning_scene.world.collision_objects.clear();
  planning_scene.world.collision_objects.push_back(remove_object);
  planning_scene.robot_state.attached_collision_objects.push_back(attached_object);
  planning_scene.is_diff = true;
  planning_scene_diff_publisher.publish(planning_scene);

  sleep_time.sleep();

  ros::shutdown();
  return 0;
}

Likewise, another node to attach the other object (tool) to the robot.

First I run both the nodes that add the objects to the planning scene one after the other. Then I run the nodes that attach the objects to the robot one after the other.

The issue I'm facing is that when I run the node to attach the second object to the robot, the first object which was attached before disappears.

This happens even though I set the flag is_diff to true (or have I got it all wrong?)

Thanks!


Originally posted by bhavyadoshi26 on ROS Answers with karma: 95 on 2016-12-05

Post score: 1

$\endgroup$

2 Answers 2

0
$\begingroup$

Rosanswers logo

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.


Originally posted by v4hn with karma: 2950 on 2016-12-05

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by bhavyadoshi26 on 2016-12-08:
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

$\endgroup$
0
$\begingroup$

Rosanswers logo

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


Originally posted by rbbg with karma: 1823 on 2016-12-05

This answer was NOT ACCEPTED on the original site

Post score: 1


Original comments

Comment by bhavyadoshi26 on 2016-12-08:
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.

Comment by bhavyadoshi26 on 2016-12-08:
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.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.