First attached object disappears upon attaching second object
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 ...