Add object colors retrieval to planning scene interface

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.

moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h

@@ -98,6 +98,9 @@ class PlanningSceneInterface
   std::map<std::string, moveit_msgs::AttachedCollisionObject>
   getAttachedObjects(const std::vector<std::string>& object_ids = std::vector<std::string>());
 
+  /** \brief Get all available object colors. Result key corresponds to the object id. */
+  std::map<std::string, std_msgs::ColorRGBA> 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);

moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp

@@ -209,6 +209,25 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl
     return result;
   }
 
+  std::map<std::string, std_msgs::ColorRGBA> getObjectColors()
+  {
+    moveit_msgs::GetPlanningScene::Request request;
+    moveit_msgs::GetPlanningScene::Response response;
+    std::map<std::string, std_msgs::ColorRGBA> 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<std::string>& objec
   return impl_->getAttachedObjects(object_ids);
 }
 
+std::map<std::string, std_msgs::ColorRGBA> PlanningSceneInterface::getObjectColors()
+{
+  return impl_->getObjectColors();
+}
+
 bool PlanningSceneInterface::applyCollisionObject(const moveit_msgs::CollisionObject& collision_object)
 {
   moveit_msgs::PlanningScene ps;

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<std::string>{})
       .def("get_attached_objects", &PlanningSceneInterface::getAttachedObjects,
            py::arg("object_ids") = std::vector<std::string>{})
+      .def("get_object_colors", &PlanningSceneInterface::getObjectColors)
       .def("apply_planning_scene", &PlanningSceneInterface::applyPlanningScene, py::arg("planning_scene"))
       // keep semicolon on next line
       ;