0
$\begingroup$

Rosanswers logo

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;
    }

Originally posted by Gengoz on ROS Answers with karma: 45 on 2015-05-27

Post score: 2

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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


Originally posted by bluefish with karma: 236 on 2015-05-28

This answer was ACCEPTED on the original site

Post score: 3


Original comments

Comment by Gengoz on 2015-05-28:
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 :)

Comment by Gengoz on 2015-05-28:
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.

Comment by Gengoz on 2015-05-28:
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!

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.