[PSM] Process collision object color when adding object trough the pl…

…anning scene monitor (#2567)

* Added an optional Collision Object color object to set the coller of the collision object when adding the collision object trough the PSM.

* Fixes for clang-tidy warnings

* fix pre-commit

* Pass by reference

moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp

@@ -42,10 +42,15 @@ namespace bind_planning_scene_monitor
 {
 
 bool processCollisionObject(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
-                            moveit_msgs::msg::CollisionObject& collision_object_msg)
+                            moveit_msgs::msg::CollisionObject& collision_object_msg,
+                            std::optional<moveit_msgs::msg::ObjectColor> color_msg)
 {
   moveit_msgs::msg::CollisionObject::ConstSharedPtr const_ptr =
       std::make_shared<const moveit_msgs::msg::CollisionObject>(collision_object_msg);
+  if (color_msg)
+  {
+    return planning_scene_monitor->processCollisionObjectMsg(const_ptr, *std::move(color_msg));
+  }
   return planning_scene_monitor->processCollisionObjectMsg(const_ptr);
 }
 
@@ -153,7 +158,7 @@ void initPlanningSceneMonitor(py::module& m)
            Clears the octomap.
            )")
       .def("process_collision_object", &moveit_py::bind_planning_scene_monitor::processCollisionObject,
-           py::arg("collision_object_msg"),  // py::arg("color_msg") = nullptr,
+           py::arg("collision_object_msg"), py::arg("color_msg") = nullptr,
            R"(
            Apply a collision object to the planning scene.
 

moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.h

@@ -37,6 +37,7 @@
 #pragma once
 
 #include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
 #include <moveit_py/moveit_py_utils/copy_ros_msg.h>
 #include <moveit_py/moveit_py_utils/ros_msg_typecasters.h>
 #include <rclcpp/rclcpp.hpp>

moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h

@@ -393,7 +393,8 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor
   bool newPlanningSceneMessage(const moveit_msgs::msg::PlanningScene& scene);
 
   // Called to update a collision object in the planning scene.
-  bool processCollisionObjectMsg(const moveit_msgs::msg::CollisionObject::ConstSharedPtr& collision_object_msg);
+  bool processCollisionObjectMsg(const moveit_msgs::msg::CollisionObject::ConstSharedPtr& collision_object_msg,
+                                 const std::optional<moveit_msgs::msg::ObjectColor>& color_msg = std::nullopt);
 
   // Called to update an attached collision object in the planning scene.
   bool processAttachedCollisionObjectMsg(

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

@@ -780,7 +780,8 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::msg::Plann
   return result;
 }
 
-bool PlanningSceneMonitor::processCollisionObjectMsg(const moveit_msgs::msg::CollisionObject::ConstSharedPtr& object)
+bool PlanningSceneMonitor::processCollisionObjectMsg(const moveit_msgs::msg::CollisionObject::ConstSharedPtr& object,
+                                                     const std::optional<moveit_msgs::msg::ObjectColor>& color_msg)
 {
   if (!scene_)
     return false;
@@ -791,6 +792,8 @@ bool PlanningSceneMonitor::processCollisionObjectMsg(const moveit_msgs::msg::Col
     last_update_time_ = rclcpp::Clock().now();
     if (!scene_->processCollisionObjectMsg(*object))
       return false;
+    if (color_msg.has_value())
+      scene_->setObjectColor(color_msg.value().id, color_msg.value().color);
   }
   triggerSceneUpdateEvent(UPDATE_GEOMETRY);
   RCLCPP_INFO(logger_, "Published update collision object");