Ask Your Question
0

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
{
protected:
    ros::Publisher add_collision_object_pub;
    moveit_msgs::CollisionObject co;

public:
   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);
      co.meshes.push_back(co_mesh);
      int pos = co.meshes.size() - 1;

      co.header.frame_id = "world";

      moveit_msgs::CollisionObject::_mesh_poses_type mesh_pose;
      mesh_pose.resize(1);

      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.mesh_poses.push_back(mesh_pose[0]);
      co.operation = co.ADD;
   }

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

and I call this in the main function:

ROS_INFO("Adding collision objects into the world");
collisionObjectAdder coa(n);
coa.addCollisionObject("package://myrobot_description/meshes/wall.dae",0.35,0,1.6);
coa.publishCollisionObjects();
ros::WallDuration sleep_time(2.0);
sleep_time.sleep();

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

Comments

The next time, please don't cross-post... https://groups.google.com/forum/#!top...

mgruhler gravatar imagemgruhler ( 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 imagejohnyang ( 2016-04-06 16:42:01 -0500 )edit

I still need an answer for this: http://answers.ros.org/question/23102...

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

1 Answer

Sort by ยป oldest newest most voted
0

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

johnyang gravatar image

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

planning_scene->processCollisionObjectMsg(coa.getCollisionObjects());

and add this to my collisionObjectAdder class

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

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: 2016-04-06 01:32:40 -0500

Seen: 691 times

Last updated: Apr 06 '16