From 1f991857302b8be05ea703fe42063c1629f07615 Mon Sep 17 00:00:00 2001 From: Jens Vanhooydonck Date: Sat, 2 Dec 2023 08:25:53 +0000 Subject: [PATCH 1/4] Added an optional Collision Object color object to set the coller of the collision object when adding the collision object trough the PSM. --- .../planning_scene_monitor/planning_scene_monitor.cpp | 7 ++++--- .../planning_scene_monitor/planning_scene_monitor.h | 1 + .../moveit/planning_scene_monitor/planning_scene_monitor.h | 3 ++- .../planning_scene_monitor/src/planning_scene_monitor.cpp | 5 ++++- 4 files changed, 11 insertions(+), 5 deletions(-) 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 d72ec0358c..9a1b72bb07 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 @@ -43,11 +43,12 @@ namespace bind_planning_scene_monitor static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_py.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); - return planning_scene_monitor->processCollisionObjectMsg(const_ptr); + return planning_scene_monitor->processCollisionObjectMsg(const_ptr, color_msg); } bool processAttachedCollisionObjectMsg(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, @@ -154,7 +155,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..05325405ad 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 4921316479..a7237a9fd9 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"); From 76abbf0ce1691a8772b988d3f3231d0704021e97 Mon Sep 17 00:00:00 2001 From: Jens Vanhooydonck Date: Fri, 8 Dec 2023 10:28:28 +0000 Subject: [PATCH 2/4] Fixes for clang-tidy warnings --- .../planning_scene_monitor/planning_scene_monitor.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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 9a1b72bb07..649dff69c6 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 @@ -48,7 +48,10 @@ bool processCollisionObject(const planning_scene_monitor::PlanningSceneMonitorPt { moveit_msgs::msg::CollisionObject::ConstSharedPtr const_ptr = std::make_shared(collision_object_msg); - return planning_scene_monitor->processCollisionObjectMsg(const_ptr, color_msg); + if (color_msg) { + return planning_scene_monitor->processCollisionObjectMsg(const_ptr, *std::move(color_msg)); + } + return planning_scene_monitor->processCollisionObjectMsg(const_ptr); } bool processAttachedCollisionObjectMsg(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, From 48e59343287d6d19f2d5827097b062b910c6b5d6 Mon Sep 17 00:00:00 2001 From: Jens Vanhooydonck Date: Fri, 8 Dec 2023 10:36:04 +0000 Subject: [PATCH 3/4] fix pre-commit --- .../planning_scene_monitor/planning_scene_monitor.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 649dff69c6..9cc50e992f 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 @@ -48,8 +48,9 @@ bool processCollisionObject(const planning_scene_monitor::PlanningSceneMonitorPt { 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)); + if (color_msg) + { + return planning_scene_monitor->processCollisionObjectMsg(const_ptr, *std::move(color_msg)); } return planning_scene_monitor->processCollisionObjectMsg(const_ptr); } From cdc697426dd3b791e9169dc346256b8a03cadef3 Mon Sep 17 00:00:00 2001 From: Jens Vanhooydonck Date: Sat, 6 Jan 2024 09:56:36 +0000 Subject: [PATCH 4/4] Pass by reference --- .../moveit/planning_scene_monitor/planning_scene_monitor.h | 2 +- .../planning_scene_monitor/src/planning_scene_monitor.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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 05325405ad..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 @@ -394,7 +394,7 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor // Called to update a collision object in the planning scene. bool processCollisionObjectMsg(const moveit_msgs::msg::CollisionObject::ConstSharedPtr& collision_object_msg, - const std::optional color_msg = std::nullopt); + 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 6b13b26fc3..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 @@ -781,7 +781,7 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::msg::Plann } bool PlanningSceneMonitor::processCollisionObjectMsg(const moveit_msgs::msg::CollisionObject::ConstSharedPtr& object, - const std::optional color_msg) + const std::optional& color_msg) { if (!scene_) return false;