PSM: keep references to scene_ valid upon receiving full scenes

moveit_ros/planning/package.xml

@@ -45,6 +45,7 @@
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>moveit_configs_utils</test_depend>
   <test_depend>ros_testing</test_depend>
 
   <test_depend>moveit_resources_panda_moveit_config</test_depend>

moveit_ros/planning/planning_scene_monitor/CMakeLists.txt

@@ -46,6 +46,7 @@ install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${MOVEIT_LIB_NAME}_export.h DESTINATIO
 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
@@ -59,4 +60,10 @@ if(BUILD_TESTING)
   target_link_libraries(trajectory_monitor_tests
     ${MOVEIT_LIB_NAME}
   )
+
+  ament_add_gtest_executable(planning_scene_monitor_test test/planning_scene_monitor_test.cpp)
+  target_link_libraries(planning_scene_monitor_test ${MOVEIT_LIB_NAME})
+  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()

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

@@ -666,7 +666,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())
@@ -678,23 +692,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

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)

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 <rclcpp/rclcpp.hpp>
+
+// Testing
+#include <gtest/gtest.h>
+
+// Main class
+#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
+#include <moveit/robot_state/conversions.h>
+
+class PlanningSceneMonitorTest : public ::testing::Test
+{
+public:
+  void SetUp() override
+  {
+    test_node_ = std::make_shared<rclcpp::Node>("moveit_planning_scene_monitor_test");
+    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+    planning_scene_monitor_ = std::make_unique<planning_scene_monitor::PlanningSceneMonitor>(
+        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<rclcpp::Node> 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;
+}