Ask Your Question
2

Publishing a collision object to MoveIt!

asked 2015-05-27 09:23:38 -0600

Gengoz gravatar image

updated 2015-05-28 04:17:13 -0600

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;
    }
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2015-05-28 02:17:07 -0600

bluefish gravatar image

Hi Gengoz,

basically it should work. I'm doing it similarily and moveit knows about these objects and is avoiding collisions. Maybe you get in trouble when coding

      geometry_msgs::Pose pose;
      pose.position.x =  0.75;
      pose.position.y =  0;
      pose.position.z =  0.525;
      co.primitive_poses.push_back(pose);

As you have no primitive but a mesh, try

co.mesh_poses[0].position.x = 0.0;
co.mesh_poses[0].position.y = 0.0;
co.mesh_poses[0].position.z = 0.0;
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);

Best Regards

edit flag offensive delete link more

Comments

A couple of days ago I tried this and got a segmentation fault. So the code you pointed out in my class was my quick duct tape solution. I'll give it another try this afternoon. Thx for the awnser :)

Gengoz gravatar imageGengoz ( 2015-05-28 02:40:25 -0600 )edit

Couldn't wait till this afternoon, just tried it and I think I got the segmentation fault figured out. Before setting the mesh_pose I first need to resize the array with: co.mesh_poses.resize(1); This seems to work. Going to test with the robotarm now.

Gengoz gravatar imageGengoz ( 2015-05-28 03:24:49 -0600 )edit
1

All seems to work now! I'll update the OP with the working code for people with the same problem in the future. Thx for your tips bluefish!

Gengoz gravatar imageGengoz ( 2015-05-28 04:05:30 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2015-05-27 09:23:38 -0600

Seen: 2,060 times

Last updated: May 28 '15