diff --git a/moveit_commander/src/moveit_commander/planning_scene_interface.py b/moveit_commander/src/moveit_commander/planning_scene_interface.py index 6d0520841d..253e6fbe2b 100644 --- a/moveit_commander/src/moveit_commander/planning_scene_interface.py +++ b/moveit_commander/src/moveit_commander/planning_scene_interface.py @@ -41,6 +41,7 @@ from shape_msgs.msg import SolidPrimitive, Plane, Mesh, MeshTriangle from .exception import MoveItCommanderException from moveit_msgs.srv import ApplyPlanningScene, ApplyPlanningSceneRequest +from std_msgs.msg import ColorRGBA try: from pyassimp import pyassimp @@ -225,6 +226,12 @@ def get_attached_objects(self, object_ids=[]): """ return self._psi.get_attached_objects(object_ids) + def get_object_colors(self): + """ + Get all available object color information. Result key corresponds to the object id. + """ + return self._psi.get_object_colors() + def apply_planning_scene(self, planning_scene_message): """ Applies the planning scene message. diff --git a/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h b/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h index 6a468ddbf1..260ab6fe9d 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h +++ b/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h @@ -98,6 +98,9 @@ class PlanningSceneInterface std::map getAttachedObjects(const std::vector& object_ids = std::vector()); + /** \brief Get all available object colors. Result key corresponds to the object id. */ + std::map getObjectColors(); + /** \brief Apply collision object to the planning scene of the move_group node synchronously. Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene */ bool applyCollisionObject(const moveit_msgs::CollisionObject& collision_object); diff --git a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp index aeea4c995e..36dbad5023 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp +++ b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp @@ -209,6 +209,25 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl return result; } + std::map getObjectColors() + { + moveit_msgs::GetPlanningScene::Request request; + moveit_msgs::GetPlanningScene::Response response; + std::map result; + request.components.components = request.components.OBJECT_COLORS; + if (!planning_scene_service_.call(request, response)) + { + ROS_WARN_NAMED(LOGNAME, "Could not call planning scene service to get object colors"); + return result; + } + + for (const moveit_msgs::ObjectColor& object_color : response.scene.object_colors) + { + result[object_color.id] = object_color.color; + } + return result; + } + bool applyPlanningScene(const moveit_msgs::PlanningScene& planning_scene) { moveit_msgs::ApplyPlanningScene::Request request; @@ -315,6 +334,11 @@ PlanningSceneInterface::getAttachedObjects(const std::vector& objec return impl_->getAttachedObjects(object_ids); } +std::map PlanningSceneInterface::getObjectColors() +{ + return impl_->getObjectColors(); +} + bool PlanningSceneInterface::applyCollisionObject(const moveit_msgs::CollisionObject& collision_object) { moveit_msgs::PlanningScene ps; diff --git a/moveit_ros/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp b/moveit_ros/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp index 1248041f0d..18bdcd1757 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp +++ b/moveit_ros/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp @@ -83,6 +83,7 @@ PYBIND11_MODULE(pymoveit_planning_scene_interface, m) .def("get_objects", &PlanningSceneInterface::getObjects, py::arg("object_ids") = std::vector{}) .def("get_attached_objects", &PlanningSceneInterface::getAttachedObjects, py::arg("object_ids") = std::vector{}) + .def("get_object_colors", &PlanningSceneInterface::getObjectColors) .def("apply_planning_scene", &PlanningSceneInterface::applyPlanningScene, py::arg("planning_scene")) // keep semicolon on next line ;