diff --git a/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp index 48c1892ecb..0eec42dd18 100644 --- a/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp +++ b/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 color_msg) { moveit_msgs::msg::CollisionObject::ConstSharedPtr const_ptr = std::make_shared(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. diff --git a/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.h b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.h index f914f444cd..c7c5713d9a 100644 --- a/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.h +++ b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.h @@ -37,6 +37,7 @@ #pragma once #include +#include #include #include #include diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h index bf3a175cfe..54007cb85f 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h +++ b/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& color_msg = std::nullopt); // Called to update an attached collision object in the planning scene. bool processAttachedCollisionObjectMsg( diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index 46a35e3a0a..c030a2d0a4 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/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& 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");