ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Insert obstacles in Moveit! with Motion Planners/C++ API

asked 2016-04-06 01:32:40 -0500

johnyang gravatar image

updated 2016-04-06 01:34:39 -0500

I had a problem with using MoveGroupInterface (see this), so I decide to use the c++ API (from this tutorial. It's all working fine now as I can pass the trajectory to gazebo. However, when I want to insert the obstacles into the environment, I tried following this answer and come up with the following code:

class collisionObjectAdder
    ros::Publisher add_collision_object_pub;
    moveit_msgs::CollisionObject co;

   collisionObjectAdder(ros::NodeHandle& n)
        add_collision_object_pub = n.advertise<moveit_msgs::CollisionObject>("collision_object", 1000);

   void addCollisionObject(std::string modelpath, double x = 0, double y = 0, double z = 0, double ow = 1, double ox = 0, double oy = 0, double oz = 0)
      shapes::Mesh* m = shapes::createMeshFromResource(modelpath);
      ROS_INFO("mesh loaded: %i %s ",m->triangle_count,modelpath.c_str());
      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);
      int pos = co.meshes.size() - 1;

      co.header.frame_id = "world";

      moveit_msgs::CollisionObject::_mesh_poses_type mesh_pose;

      mesh_pose[0].position.x = x;
      mesh_pose[0].position.y = y;
      mesh_pose[0].position.z = z;
      mesh_pose[0].orientation.w= ow;
      mesh_pose[0].orientation.x= ox;
      mesh_pose[0].orientation.y= oy;
      mesh_pose[0].orientation.z= oz;

      co.operation = co.ADD;

   void publishCollisionObjects()
       sleep(1.0); // To make sure the node can publish
       ROS_INFO("Collision object published");

and I call this in the main function:

ROS_INFO("Adding collision objects into the world");
collisionObjectAdder coa(n);
ros::WallDuration sleep_time(2.0);

This works with the move group example as the planner will try to avoid this obstacle, but it doesn't work with my api example. I had a feeling the tutorial example does not know to fetch /collision_object topic to insert in the planning_scene (need to manually add a callback function?). Any one can suggest me what is missing in my implementation?

edit retag flag offensive close merge delete


The next time, please don't cross-post...!top...

mgruhler gravatar image mgruhler  ( 2016-04-06 05:23:29 -0500 )edit

ok. i wasn't sure where do people actually read and reply questions on different topics. Does the moveit/gazebo/ROS team actually check both sides of forum?

johnyang gravatar image johnyang  ( 2016-04-06 16:42:01 -0500 )edit

I still need an answer for this:

johnyang gravatar image johnyang  ( 2016-04-06 20:08:41 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2016-04-06 03:43:48 -0500

johnyang gravatar image

I found the solution for this now. I just need to add:


and add this to my collisionObjectAdder class

moveit_msgs::CollisionObject getCollisionObjects()
       return co;
edit flag offensive delete link more

Question Tools

1 follower


Asked: 2016-04-06 01:32:40 -0500

Seen: 1,467 times

Last updated: Apr 06 '16