Insert obstacle in step-format into moveit via C++-node

asked 2015-03-11 08:57:42 -0600

bluefish
236 ●14 ●21 ●24

Hi,

I would like to insert an obstacle into the moveit planning scene. It works fine when inserting it via RVIZ. But I'd like to insert it via a C++-node. The goal is then to dynamically change its position while runtime.

I found the move-group (C++) tutorial here and there the section where it's described how to insert an object.

moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = group.getPlanningFrame();

/* The id of the object is used to identify it. */
collision_object.id = "box1";

/* Define a box to add to the world. */
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.4;
primitive.dimensions[1] = 0.1;
primitive.dimensions[2] = 0.4;

/* A pose for the box (specified relative to frame_id) */
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x =  0.6;
box_pose.position.y = -0.4;
box_pose.position.z =  1.2;

collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;

std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);


Now, let’s add the collision object into the world


ROS_INFO("Add an object into the world");
planning_scene_interface.addCollisionObjects(collision_objects);

/* Sleep so we have time to see the object in RViz */
sleep(2.0);


Planning with collision detection can be slow. Lets set the planning time to be sure the planner has enough time to plan around the box. 10 seconds should be plenty.


group.setPlanningTime(10.0);


Now when we plan a trajectory it will avoid the obstacle


group.setStartState(*group.getCurrentState());
group.setPoseTarget(target_pose1);
success = group.plan(my_plan);

ROS_INFO("Visualizing plan 5 (pose goal move around box) %s",
  success?"":"FAILED");
/* Sleep to give Rviz time to visualize the plan. */
sleep(10.0);


Now, let’s attach the collision object to the robot.


ROS_INFO("Attach the object to the robot");
group.attachObject(collision_object.id);
/* Sleep to give Rviz time to show the object attached (different color). */
sleep(4.0);

But I cannot find how to add an object from file. Does someone has experience with that?

Best regards

edit retag flag offensive close merge delete

add a comment

1

answered 2015-03-11 09:15:37 -0600

dornhege

31395 ●134 ●284 ●498

You need to parse the file and add its contents. moveit CollisionObjects have a mesh entry that should be suitable.

edit flag offensive delete link

Comments

Thanks for your answer dornhege. Unfortunately I don't really understand what you mean. Could you post an example?

bluefish ( 2015-03-11 09:58:56 -0600 )edit

1

Just check out moveit_msgs/CollisionObject. It contains meshes. You need to produce one of these.

dornhege ( 2015-03-11 11:50:16 -0600 )edit

add a comment

0

answered 2015-11-13 09:21:22 -0600

jdarango
1 ●1

This worked for me, maybe it was the .id:

moveit_msgs::CollisionObject co; //shapes::Mesh* m = shapes::createMeshFromResource("package://ur_description/meshes/BODY_X.dae"); shapes::Mesh* m = shapes::createMeshFromResource("package://ur_description/meshes/ur10/collision/base.stl"); 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.header.frame_id = "/world"; co.id = "base"; co.header.stamp = ros::Time::now(); co.meshes.resize(1); co.meshes[0] = co_mesh; co.mesh_poses.resize(1); co.mesh_poses[0].position.x = 1.0; co.mesh_poses[0].position.y = 1.0; co.mesh_poses[0].position.z = 1.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.operation = co.ADD;

pub_co.publish(co);

edit flag offensive delete link

add a comment

0

answered 2015-03-22 06:47:05 -0600

bluefish
236 ●14 ●21 ●24

updated 2015-03-23 09:30:09 -0600

Hi!

I tried hard to implement the mesh entry mentioned by dornhege. Using the code of this question and the information from here I get the following (and this does the trick):

moveit_msgs::CollisionObject co;
shapes::Mesh* m = shapes::createMeshFromResource("package://ur5_test/step/table.stl");
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.meshes[0] = co_mesh;
co.mesh_poses.resize(1);
co.mesh_poses[0].position.x = 1.0;
co.mesh_poses[0].position.y = 1.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;
//pub_co.publish(co);

co.meshes.push_back(co_mesh);
co.mesh_poses.push_back(co.mesh_poses[0]);
co.operation = co.ADD;

std::vector<moveit_msgs::CollisionObject> collision_objects;  
collision_objects.push_back(co);  

// Now, let's add the collision object into the world
ROS_INFO("Add an object into the world");  
planning_scene_interface.addCollisionObjects(collision_objects);

Unfortunately, I cannot use the part pub_co.publish(co);, as my compiler says, it does not know pub_co...

But actually don't need it as the code does what it should do now.

Thanks and regards

edit flag offensive delete link

Comments

Please edit your original question with follow-up information instead of posting an answer.

dornhege ( 2015-03-23 05:34:32 -0600 )edit

Yeah you're right. Sorry for posting an question as an answer. :/

bluefish ( 2015-03-23 09:26:54 -0600 )edit

But somehow the code is working now and I do get the the step-file in my planning scene. Thanks for your help!! But still don't know why the pub_co.publish(co) does not work. Any idea?

bluefish ( 2015-03-23 09:29:13 -0600 )edit

Hi, I would like to know what header files you used to include shapes and meshes here.

bhavyadoshi26 ( 2016-09-09 09:32:40 -0600 )edit

add a comment

Insert obstacle in step-format into moveit via C++-node

3 Answers

Comments

Comments

Question Tools

Stats

Related questions

Insert obstacle in step-format into moveit via C++-node edit

3 Answers

Comments

Comments

Question Tools

Stats

Related questions

Insert obstacle in step-format into moveit via C++-node