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
#include
#include
#include
#include
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("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(co_mesh_msg);
co.meshes.resize(1);
co.meshes[0] = co_mesh;
co.header.frame_id = "mainulator";
co.id = "box";
geometry_msgs::Pose pose;
pose.position.x = 0.75;
pose.position.y = 0;
pose.position.z = 0.525;
co.primitive_poses.push_back(pose);
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;
}
↧