diff --git a/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h b/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h index cb87129c90..9b61d6c959 100644 --- a/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h +++ b/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h @@ -38,7 +38,6 @@ #define MOVEIT_PLANNING_INTERFACE_PLANNING_SCENE_INTERFACE_ #include -#include #include #include @@ -84,10 +83,9 @@ class PlanningSceneInterface /** \brief Add collision objects to the world. Make sure object.operation is set to object.ADD. */ - void addCollisionObjects(const std::vector &collision_objects, - const std::vector &object_colors = std::vector()) const; + void addCollisionObjects(const std::vector &collision_objects) const; - /** \brief Remove collision objects from the world */ + /** \brief Remove collision objects from the world. */ void removeCollisionObjects(const std::vector &object_ids) const; /**@}*/ diff --git a/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp b/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp index ff1492463a..c857c7a5f5 100644 --- a/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp +++ b/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp @@ -202,12 +202,10 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl return result; } - void addCollisionObjects(const std::vector &collision_objects, - const std::vector &object_colors) const + void addCollisionObjects(const std::vector &collision_objects) const { moveit_msgs::PlanningScene planning_scene; planning_scene.world.collision_objects = collision_objects; - planning_scene.object_colors = object_colors; planning_scene.is_diff = true; planning_scene_diff_publisher_.publish(planning_scene); } @@ -269,10 +267,9 @@ std::map PlanningSceneInterfa return impl_->getAttachedObjects(object_ids); } -void PlanningSceneInterface::addCollisionObjects(const std::vector &collision_objects, - const std::vector &object_colors) const +void PlanningSceneInterface::addCollisionObjects(const std::vector &collision_objects) const { - return impl_->addCollisionObjects(collision_objects, object_colors); + return impl_->addCollisionObjects(collision_objects); } void PlanningSceneInterface::removeCollisionObjects(const std::vector &object_ids) const