Adding Collision Objects

Question

Hello,

I am trying to add collision objects to the planning scene interface. However, the moveit planning seems to ignore the collision objects. I am passing a list of boxes as collision objects. Here is the code:

moveit_utils::MicoController srv_controller;
moveit::planning_interface::MoveGroup group("arm");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
ROS_INFO("COLLISION SIZE\n");
ROS_INFO_STREAM(collision_objects.size());
sleep(2.0);
planning_scene_interface.addCollisionObjects(collision_objects);

group.setPlanningTime(10.0); //5 second maximum for collision computation
moveit::planning_interface::MoveGroup::Plan my_plan;

group.setStartState(*group.getCurrentState());
group.setPoseTarget(p_target);

ROS_INFO("[mico_moveit_cartesianpose_service.cpp] starting to plan...");
bool success = group.plan(my_plan);
if(success)
    ROS_INFO("planning successful\n");
else 
    ROS_INFO("not successful :( \n");

Thank You.

---

Originally posted by palak_hirpara on ROS Answers with karma: 1 on 2017-03-07

Post score: 0

Answer 1

Either use the applyCollisionObjects method instead of addCollisionObjects or sleep again after you added the objects. The latter function runs asynchronously and your planning request probably arrives before your collision object is processed.

Also see http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/pr2_tutorials/planning/src/doc/planning_scene_ros_api_tutorial.html#interlude-synchronous-vs-asynchronous-updates

---

Originally posted by v4hn with karma: 2950 on 2017-03-08

This answer was ACCEPTED on the original site

Post score: 1