Robotics StackExchange | Archived questions

(MoveIt) PlanningSceneInterface does not add collision object

The following code is mainly the same as in this MoveIt tutorial: http://docs.ros.org/hydro/api/pr2_moveit_tutorials/html/planning/src/doc/move_group_interface_tutorial.html

The code should print the name of the added collision object, but it does not print it and the object does not show up in Rviz. However, if collision objects are added by using the GUI in Rviz, they do show up and they do get printed.

  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
  moveit_msgs::CollisionObject collision_object;
  collision_object.header.frame_id = range_sensor_frame;
  collision_object.id = "sensor_mount";

  shape_msgs::SolidPrimitive primitive;
  primitive.type = primitive.BOX;
  primitive.dimensions.resize(3);
  primitive.dimensions[0] = 0.05;
  primitive.dimensions[1] = 0.8;
  primitive.dimensions[2] = 0.11;

  geometry_msgs::Pose box_pose;
  box_pose.orientation.w = 1.0;
  box_pose.position.x = 0.21;
  box_pose.position.y = 0.0;
  box_pose.position.z = 0.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);
  std::cout << "collision_objects.size(): " << collision_objects.size() << std::endl;
  std::cout << collision_objects[0] << std::endl;
  planning_scene_interface.addCollisionObjects(collision_objects);

  ros::Duration(2.0).sleep();

  std::vector<std::string> obj = planning_scene_interface.getKnownObjectNames();
  if (obj.size() > 0)
  {
    std::cout << std::endl << "-- KNOWN COLLISION OBJECTS --" << std::endl;
    for (int i = 0; i < obj.size(); ++i)
      std::cout << obj[i] << std::endl;
  }

This is in ROS Hydro. I've also posted this question https://groups.google.com/forum/#!topic/moveit-users/sfXthHuxyx0.

Asked by atp on 2015-05-15 12:14:55 UTC

Comments

Did you end up figuring this out? I'd prefer this interface over using the planning_scene directly, though at least that works.

Asked by rkeatin3 on 2017-08-04 19:32:45 UTC

Please consider using the applyCollisionObject(s) methods available starting from indigo.

In the case reported above an additional sleep before the addCollisionObjects call could help. It's likely that the topic connection to move_group was not ready yet.

Asked by v4hn on 2017-08-05 06:00:04 UTC

Both of those solutions worked, thanks!

Asked by rkeatin3 on 2017-08-05 17:53:49 UTC

I've closed this since v4hn's solutions worked for rkeatin3.

Asked by atp on 2017-08-05 22:25:51 UTC

Hi, is there a function that allows to remove the collision object immediately? Because the planning_scene_interface.removeCollisionObjects(object_ids) function requires at least 0.5 seconds delay to be effective on Rviz...

Asked by enrico on 2018-08-26 09:03:08 UTC

Answers