Publishing a collision object to MoveIt!
I am using MoveIt! for controlling an ABB IRB1600 robotic arm. Currentyl I am able to execute trajectories with the robotic arm. The problem is that the arm moves through objects, instead of around.
To change this I tried publishing a collision object on the collision_object topic. Hoping that MoveIt! would pick this up and add it to its environment. I do this using the class shown below. In the class I publish the mesh of a ring as a collision object on the topic collision_object.
After the code in the class is run I can see the object being published on the topic (rostopic echo collision_object). Though when I replan and move the robotic arm, it still moves right through the ring.
Am I missing something? Am I falsely expecting MoveIt! to just pick up my object after publishing? Any help would be very much appreciated!
Thx
edit: Using Hydro on Ubuntu 12.04
second edit: Included working code
#include <ros/ros.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_msgs/PlanningScene.h>
#include <geometric_shapes/shape_operations.h>
class collisionObjectAdder
{
protected:
ros::NodeHandle nh;
ros::Publisher add_collision_object_pub;
//ros::Publisher planning_scene_diff_pub;
;
public:
collisionObjectAdder()
{
add_collision_object_pub = nh.advertise<moveit_msgs::CollisionObject>("collision_object", 1000);
}
void addCollisionObject()
{
sleep(1.0); // To make sure the node can publish
moveit_msgs::CollisionObject co;
shapes::Mesh* m = shapes::createMeshFromResource("package://sim_move/meshes/ring.STL");
ROS_INFO("mesh loaded");
shape_msgs::Mesh co_mesh;
shapes::ShapeMsg co_mesh_msg;
shapes::constructMsgFromShape(m, co_mesh_msg);
co_mesh = boost::get<shape_msgs::Mesh>(co_mesh_msg);
co.meshes.resize(1);
co.mesh_poses.resize(1);
co.meshes[0] = co_mesh;
co.header.frame_id = "mainulator";
co.id = "ring";
// OLD CODE, THIS IS WHERE THE PROBLEM OF THIS QUESTION LIES
// geometry_msgs::Pose pose;
// pose.position.x = 0.75;
// pose.position.y = 0;
// pose.position.z = 0.525;
// co.primitive_poses.push_back(pose);
// NEW CODE, SOLUTION TO THE PROBLEM
co.mesh_poses[0].position.x = 0.75;
co.mesh_poses[0].position.y = 0.0;
co.mesh_poses[0].position.z = 0.525;
co.mesh_poses[0].orientation.w= 1.0;
co.mesh_poses[0].orientation.x= 0.0;
co.mesh_poses[0].orientation.y= 0.0;
co.mesh_poses[0].orientation.z= 0.0;
co.meshes.push_back(co_mesh);
co.mesh_poses.push_back(co.mesh_poses[0]);
co.operation = co.ADD;
add_collision_object_pub.publish(co);
ROS_INFO("Collision object published");
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "add_collision_object");
std::cout<<"Initialized..." << std::endl;
collisionObjectAdder coAdder;
coAdder.addCollisionObject();
ros::spin();
return 0;
}