Ask Your Question
2

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

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

bluefish gravatar image

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

3 Answers

Sort by » oldest newest most voted
1

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

dornhege gravatar image

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 more

Comments

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

bluefish gravatar imagebluefish ( 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 gravatar imagedornhege ( 2015-03-11 11:50:16 -0600 )edit
0

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

bluefish gravatar image

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 more

Comments

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

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

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

bluefish gravatar imagebluefish ( 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 gravatar imagebluefish ( 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 gravatar imagebhavyadoshi26 ( 2016-09-09 09:32:40 -0600 )edit
0

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

jdarango gravatar image

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 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: 2015-03-11 08:57:42 -0600

Seen: 1,850 times

Last updated: Mar 23 '15