Hi everyone,
I have followed the [tutorials](http://docs.ros.org/indigo/api/moveit_tutorials/html/doc/pr2_tutorials/planning/src/doc/planning_scene_ros_api_tutorial.html) 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
#include
// MoveIt!
#include
#include
#include
#include
#include
#include
#include
#include
#include
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("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(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
#include
// MoveIt!
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
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("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(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!
↧