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

edit

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