From 29c585a59c3960d781095344539067aacb1e4989 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 28 Mar 2024 09:20:35 +0100 Subject: [PATCH] PSM: keep references to scene_ valid upon receiving full scenes (#2745) plan_execution-related modules rely on `plan.planning_scene_` in many places to point to the currently monitored scene (or a diff on top of it). Before this patch, if the PSM would receive full scenes during execution, `plan.planning_scene_` would not include later incremental updates anymore because the monitor created a new diff scene. --------- Co-authored-by: v4hn --- moveit_ros/planning/package.xml | 1 + .../planning_scene_monitor/CMakeLists.txt | 10 ++ .../src/planning_scene_monitor.cpp | 33 ++-- .../launch/planning_scene_monitor.test.py | 92 +++++++++++ .../test/planning_scene_monitor_test.cpp | 154 ++++++++++++++++++ 5 files changed, 272 insertions(+), 18 deletions(-) create mode 100644 moveit_ros/planning/planning_scene_monitor/test/launch/planning_scene_monitor.test.py create mode 100644 moveit_ros/planning/planning_scene_monitor/test/planning_scene_monitor_test.cpp diff --git a/moveit_ros/planning/package.xml b/moveit_ros/planning/package.xml index a343e6f89a..51aa478867 100644 --- a/moveit_ros/planning/package.xml +++ b/moveit_ros/planning/package.xml @@ -47,6 +47,7 @@ ament_cmake_gmock ament_cmake_gtest + moveit_configs_utils ros_testing launch_testing_ament_cmake diff --git a/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt b/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt index 0189424fe8..8aa26aa4a7 100644 --- a/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt +++ b/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt @@ -43,6 +43,7 @@ install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_planning_scene_monitor_export.h if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_gmock REQUIRED) + find_package(ros_testing REQUIRED) ament_add_gmock(current_state_monitor_tests test/current_state_monitor_tests.cpp) @@ -50,4 +51,13 @@ if(BUILD_TESTING) moveit_planning_scene_monitor) ament_add_gmock(trajectory_monitor_tests test/trajectory_monitor_tests.cpp) target_link_libraries(trajectory_monitor_tests moveit_planning_scene_monitor) + + ament_add_gtest_executable(planning_scene_monitor_test + test/planning_scene_monitor_test.cpp) + target_link_libraries(planning_scene_monitor_test + moveit_planning_scene_monitor) + ament_target_dependencies(planning_scene_monitor_test moveit_core rclcpp + moveit_msgs) + add_ros_test(test/launch/planning_scene_monitor.test.py TIMEOUT 30 ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") endif() 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 2b51135d1d..d7a19e5137 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 @@ -728,7 +728,21 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::msg::Plann RCLCPP_DEBUG(logger_, "scene update %f robot stamp: %f", fmod(last_update_time_.seconds(), 10.), fmod(last_robot_motion_time_.seconds(), 10.)); old_scene_name = scene_->getName(); - result = scene_->usePlanningSceneMsg(scene); + + if (!scene.is_diff && parent_scene_) + { + // clear maintained (diff) scene_ and set the full new scene in parent_scene_ instead + scene_->clearDiffs(); + result = parent_scene_->setPlanningSceneMsg(scene); + // There were no callbacks for individual object changes, so rebuild the octree masks + excludeAttachedBodiesFromOctree(); + excludeWorldObjectsFromOctree(); + } + else + { + result = scene_->setPlanningSceneDiffMsg(scene); + } + if (octomap_monitor_) { if (!scene.is_diff && scene.world.octomap.octomap.data.empty()) @@ -740,23 +754,6 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::msg::Plann } robot_model_ = scene_->getRobotModel(); - // if we just reset the scene completely but we were maintaining diffs, we need to fix that - if (!scene.is_diff && parent_scene_) - { - // the scene is now decoupled from the parent, since we just reset it - scene_->setAttachedBodyUpdateCallback(moveit::core::AttachedBodyCallback()); - scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn()); - parent_scene_ = scene_; - scene_ = parent_scene_->diff(); - scene_const_ = scene_; - scene_->setAttachedBodyUpdateCallback([this](moveit::core::AttachedBody* body, bool attached) { - currentStateAttachedBodyUpdateCallback(body, attached); - }); - scene_->setCollisionObjectUpdateCallback( - [this](const collision_detection::World::ObjectConstPtr& object, collision_detection::World::Action action) { - currentWorldObjectUpdateCallback(object, action); - }); - } if (octomap_monitor_) { excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in diff --git a/moveit_ros/planning/planning_scene_monitor/test/launch/planning_scene_monitor.test.py b/moveit_ros/planning/planning_scene_monitor/test/launch/planning_scene_monitor.test.py new file mode 100644 index 0000000000..0e9aa717ae --- /dev/null +++ b/moveit_ros/planning/planning_scene_monitor/test/launch/planning_scene_monitor.test.py @@ -0,0 +1,92 @@ +import os +import launch +import unittest +import launch_ros +import launch_testing +from ament_index_python.packages import get_package_share_directory +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_test_description(): + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "ros2_controllers.yaml", + ) + ros2_control_node = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ros2_controllers_path], + remappings=[ + ("/controller_manager/robot_description", "/robot_description"), + ], + output="screen", + ) + + joint_state_broadcaster_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager-timeout", + "300", + "--controller-manager", + "/controller_manager", + ], + output="screen", + ) + + panda_arm_controller_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=["panda_arm_controller", "-c", "/controller_manager"], + ) + + psm_gtest = launch_ros.actions.Node( + executable=launch.substitutions.PathJoinSubstitution( + [ + launch.substitutions.LaunchConfiguration("test_binary_dir"), + "planning_scene_monitor_test", + ] + ), + parameters=[ + moveit_config.to_dict(), + ], + output="screen", + ) + + return launch.LaunchDescription( + [ + launch.actions.TimerAction(period=2.0, actions=[ros2_control_node]), + launch.actions.TimerAction( + period=4.0, actions=[joint_state_broadcaster_spawner] + ), + launch.actions.TimerAction( + period=6.0, actions=[panda_arm_controller_spawner] + ), + launch.actions.TimerAction(period=9.0, actions=[psm_gtest]), + launch_testing.actions.ReadyToTest(), + ] + ), { + "psm_gtest": psm_gtest, + } + + +class TestGTestWaitForCompletion(unittest.TestCase): + # Waits for test to complete, then waits a bit to make sure result files are generated + def test_gtest_run_complete(self, psm_gtest): + self.proc_info.assertWaitForShutdown(psm_gtest, timeout=4000.0) + + +@launch_testing.post_shutdown_test() +class TestGTestProcessPostShutdown(unittest.TestCase): + # Checks if the test has been completed with acceptable exit codes (successful codes) + def test_gtest_pass(self, proc_info, psm_gtest): + launch_testing.asserts.assertExitCodes(proc_info, process=psm_gtest) diff --git a/moveit_ros/planning/planning_scene_monitor/test/planning_scene_monitor_test.cpp b/moveit_ros/planning/planning_scene_monitor/test/planning_scene_monitor_test.cpp new file mode 100644 index 0000000000..806a88f3ac --- /dev/null +++ b/moveit_ros/planning/planning_scene_monitor/test/planning_scene_monitor_test.cpp @@ -0,0 +1,154 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, University of Hamburg + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Michael 'v4hn' Goerner + Desc: Tests for PlanningSceneMonitor +*/ + +// ROS +#include + +// Testing +#include + +// Main class +#include +#include + +class PlanningSceneMonitorTest : public ::testing::Test +{ +public: + void SetUp() override + { + test_node_ = std::make_shared("moveit_planning_scene_monitor_test"); + executor_ = std::make_shared(); + planning_scene_monitor_ = std::make_unique( + test_node_, "robot_description", "planning_scene_monitor"); + planning_scene_monitor_->monitorDiffs(true); + scene_ = planning_scene_monitor_->getPlanningScene(); + executor_->add_node(test_node_); + executor_thread_ = std::thread([this]() { executor_->spin(); }); + } + + void TearDown() override + { + scene_.reset(); + executor_->cancel(); + if (executor_thread_.joinable()) + { + executor_thread_.join(); + } + } + +protected: + std::shared_ptr test_node_; + + // Executor and a thread to run the executor. + rclcpp::Executor::SharedPtr executor_; + std::thread executor_thread_; + + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + planning_scene::PlanningScenePtr scene_; +}; + +// various code expects the monitored scene to remain the same +TEST_F(PlanningSceneMonitorTest, TestPersistentScene) +{ + auto scene{ planning_scene_monitor_->getPlanningScene() }; + moveit_msgs::msg::PlanningScene msg; + msg.is_diff = msg.robot_state.is_diff = true; + planning_scene_monitor_->newPlanningSceneMessage(msg); + EXPECT_EQ(scene, planning_scene_monitor_->getPlanningScene()); + msg.is_diff = msg.robot_state.is_diff = false; + planning_scene_monitor_->newPlanningSceneMessage(msg); + EXPECT_EQ(scene, planning_scene_monitor_->getPlanningScene()); +} + +using UpdateType = planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType; + +#define TRIGGERS_UPDATE(msg, expected_update_type) \ + { \ + planning_scene_monitor_->clearUpdateCallbacks(); \ + auto received_update_type{ UpdateType::UPDATE_NONE }; \ + planning_scene_monitor_->addUpdateCallback([&](auto type) { received_update_type = type; }); \ + planning_scene_monitor_->newPlanningSceneMessage(msg); \ + EXPECT_EQ(received_update_type, expected_update_type); \ + } + +TEST_F(PlanningSceneMonitorTest, UpdateTypes) +{ + moveit_msgs::msg::PlanningScene msg; + msg.is_diff = msg.robot_state.is_diff = true; + TRIGGERS_UPDATE(msg, UpdateType::UPDATE_NONE); + + msg.fixed_frame_transforms.emplace_back(); + msg.fixed_frame_transforms.back().header.frame_id = "base_link"; + msg.fixed_frame_transforms.back().child_frame_id = "object"; + msg.fixed_frame_transforms.back().transform.rotation.w = 1.0; + TRIGGERS_UPDATE(msg, UpdateType::UPDATE_TRANSFORMS); + msg.fixed_frame_transforms.clear(); + moveit::core::robotStateToRobotStateMsg(scene_->getCurrentState(), msg.robot_state, false); + msg.robot_state.is_diff = true; + TRIGGERS_UPDATE(msg, UpdateType::UPDATE_STATE); + + msg.robot_state.is_diff = false; + TRIGGERS_UPDATE(msg, UpdateType::UPDATE_STATE | UpdateType::UPDATE_GEOMETRY); + + msg.robot_state = moveit_msgs::msg::RobotState{}; + msg.robot_state.is_diff = true; + moveit_msgs::msg::CollisionObject collision_object; + collision_object.header.frame_id = "base_link"; + collision_object.id = "object"; + collision_object.operation = moveit_msgs::msg::CollisionObject::ADD; + collision_object.pose.orientation.w = 1.0; + collision_object.primitives.emplace_back(); + collision_object.primitives.back().type = shape_msgs::msg::SolidPrimitive::SPHERE; + collision_object.primitives.back().dimensions = { 1.0 }; + msg.world.collision_objects.emplace_back(collision_object); + TRIGGERS_UPDATE(msg, UpdateType::UPDATE_GEOMETRY); + + msg.world.collision_objects.clear(); + msg.is_diff = false; + + TRIGGERS_UPDATE(msg, UpdateType::UPDATE_SCENE); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +}