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;
+}