Add parameter default_planning_scene to load planning scene geometry on startup

moveit_ros/move_group/src/move_group.cpp

@@ -34,17 +34,18 @@
 
 /* Author: Ioan Sucan */
 
-#include <moveit/moveit_cpp/moveit_cpp.h>
-#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
-#include <tf2_ros/transform_listener.h>
-#include <moveit/move_group/move_group_capability.h>
-#include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
+#include <memory>
+#include <set>
+
 #include <boost/tokenizer.hpp>
 #include <moveit/macros/console_colors.h>
+#include <moveit/move_group/move_group_capability.h>
 #include <moveit/move_group/move_group_context.h>
-#include <memory>
-#include <set>
+#include <moveit/moveit_cpp/moveit_cpp.h>
+#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
+#include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
 #include <moveit/utils/logger.hpp>
+#include <tf2_ros/transform_listener.h>
 
 static const std::string ROBOT_DESCRIPTION =
     "robot_description";  // name of the robot description (a param name, so it can be changed externally)
@@ -287,9 +288,25 @@ int main(int argc, char** argv)
   // Initialize MoveItCpp
   const auto moveit_cpp = std::make_shared<moveit_cpp::MoveItCpp>(nh, moveit_cpp_options);
   const auto planning_scene_monitor = moveit_cpp->getPlanningSceneMonitorNonConst();
-
-  if (planning_scene_monitor->getPlanningScene())
+  if (auto ps = planning_scene_monitor->getPlanningScene())
   {
+    if (nh->has_parameter("default_planning_scene"))
+    {
+      std::string path = nh->get_parameter("default_planning_scene").as_string();
+      std::fstream file_stream;
+      file_stream.open(path, std::fstream::in);
+      if (!file_stream.is_open() || !ps->loadGeometryFromStream(file_stream))
+      {
+        RCLCPP_ERROR(nh->get_logger(), std::string("Failed to load the planning scene geometry from the file specified "
+                                                   "by the `default_planning_scene` parameter. The "
+                                                   "`default_planning_scene` parameter was set to ")
+                                           .append(path)
+                                           .append(".")
+                                           .c_str());
+
+        return 0;
+      }
+    }
     bool debug = false;
     for (int i = 1; i < argc; ++i)
     {

moveit_ros/tests/CMakeLists.txt

@@ -2,18 +2,35 @@ cmake_minimum_required(VERSION 3.22)
 project(moveit_ros_tests LANGUAGES CXX)
 
 # Common cmake code applied to all moveit packages
+find_package(ament_cmake_python REQUIRED)
 find_package(moveit_common REQUIRED)
 moveit_package()
 
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
-  find_package(ament_cmake REQUIRED)
+  find_package(ament_cmake_ros REQUIRED)
+  find_package(moveit_ros_planning REQUIRED)
   find_package(rclcpp REQUIRED)
   find_package(ros_testing REQUIRED)
 
   include_directories(include)
   ament_add_gtest_executable(move_group_api_test src/move_group_api_test.cpp)
   ament_target_dependencies(move_group_api_test rclcpp)
-  add_ros_test(launch/move_group_api.test.py TIMEOUT 30 ARGS
-               "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
+  add_ros_test(launch/move_group_api.py TIMEOUT 30 ENV "ROS_DOMAIN_ID=2")
+
+  ament_add_gtest_executable(move_group_default_planning_scene_test
+                             src/move_group_default_planning_scene_test.cpp)
+  ament_target_dependencies(move_group_default_planning_scene_test rclcpp
+                            moveit_ros_planning)
+  # run default_planning_scene test isolated test to avoid interference
+  add_ros_test(launch/move_group_default_planning_scene.py TIMEOUT 30 ENV
+               "ROS_DOMAIN_ID=2")
+
+  # install Python helper script and test
+  ament_python_install_package(moveit_ros_tests)
+  install(DIRECTORY data DESTINATION share/${PROJECT_NAME})
+  install(TARGETS move_group_api_test move_group_default_planning_scene_test
+          RUNTIME DESTINATION lib/${PROJECT_NAME})
 endif()
+
+ament_package()

moveit_ros/tests/data/scene.txt

@@ -0,0 +1,22 @@
+(noname)+
+* Box_0
+0 0.8 0.5
+0 0 0 1
+1
+box
+0.2 0.2 0.2
+0 0 0
+0 0 0 1
+0 0 0 0
+0
+* Cylinder_0
+0.69 0 0.73
+0 0 0 1
+1
+cylinder
+0.1 0.2
+0 0 0
+0 0 0 1
+0 0 0 0
+0
+.

moveit_ros/tests/launch/move_group_api.py

@@ -0,0 +1,56 @@
+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
+from moveit_ros_tests.move_group_test_description import (
+    generate_move_group_test_description,
+)
+
+
+def generate_test_description():
+    moveit_config = (
+        MoveItConfigsBuilder("moveit_resources_panda")
+        .robot_description(
+            file_path="config/panda.urdf.xacro",
+        )
+        .robot_description_semantic(file_path="config/panda.srdf")
+        .planning_scene_monitor(
+            publish_robot_description=True, publish_robot_description_semantic=True
+        )
+        .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
+        .planning_pipelines(
+            pipelines=["ompl", "chomp", "pilz_industrial_motion_planner", "stomp"]
+        )
+        .to_moveit_configs()
+    )
+
+    launch.actions.DeclareLaunchArgument(
+        name="test_binary_dir",
+        description="Binary directory of package " "containing test executables",
+    )
+    move_group_gtest = launch_ros.actions.Node(
+        package="moveit_ros_tests",
+        executable="move_group_api_test",
+        parameters=[moveit_config.to_dict()],
+        output="screen",
+    )
+
+    return generate_move_group_test_description(
+        moveit_config.to_dict(), move_group_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, move_group_gtest):
+        self.proc_info.assertWaitForShutdown(move_group_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, move_group_gtest):
+        launch_testing.asserts.assertExitCodes(proc_info, process=move_group_gtest)

moveit_ros/tests/launch/move_group_default_planning_scene.py

@@ -0,0 +1,58 @@
+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
+from moveit_ros_tests.move_group_test_description import (
+    generate_move_group_test_description,
+)
+import launch_testing
+import pytest
+
+
+@pytest.mark.launch_test
+def generate_test_description():
+    moveit_config = (
+        MoveItConfigsBuilder("moveit_resources_panda")
+        .robot_description(
+            file_path="config/panda.urdf.xacro",
+        )
+        .robot_description_semantic(file_path="config/panda.srdf")
+        .planning_scene_monitor(
+            publish_robot_description=True, publish_robot_description_semantic=True
+        )
+        .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
+        .planning_pipelines(
+            pipelines=["ompl", "chomp", "pilz_industrial_motion_planner", "stomp"]
+        )
+        .to_moveit_configs()
+    )
+    moveit_config_dict = moveit_config.to_dict()
+    moveit_config_dict["default_planning_scene"] = os.path.join(
+        get_package_share_directory("moveit_ros_tests"), "data", "scene.txt"
+    )
+
+    move_group_gtest = launch_ros.actions.Node(
+        package="moveit_ros_tests",
+        executable="move_group_default_planning_scene_test",
+        parameters=[moveit_config_dict],
+        output="screen",
+    )
+
+    return generate_move_group_test_description(moveit_config_dict, move_group_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, move_group_gtest):
+        self.proc_info.assertWaitForShutdown(move_group_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, move_group_gtest):
+        launch_testing.asserts.assertExitCodes(proc_info, process=move_group_gtest)

moveit_ros/tests/launch/move_group_api.test.py → moveit_ros/tests/moveit_ros_tests/move_group_test_description.py

@@ -7,29 +7,13 @@
 from moveit_configs_utils import MoveItConfigsBuilder
 
 
-def generate_test_description():
-    moveit_config = (
-        MoveItConfigsBuilder("moveit_resources_panda")
-        .robot_description(
-            file_path="config/panda.urdf.xacro",
-        )
-        .robot_description_semantic(file_path="config/panda.srdf")
-        .planning_scene_monitor(
-            publish_robot_description=True, publish_robot_description_semantic=True
-        )
-        .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
-        .planning_pipelines(
-            pipelines=["ompl", "chomp", "pilz_industrial_motion_planner", "stomp"]
-        )
-        .to_moveit_configs()
-    )
-
+def generate_move_group_test_description(moveit_config_dict, move_group_gtest):
     # Start the actual move_group node/action server
     move_group_node = launch_ros.actions.Node(
         package="moveit_ros_move_group",
         executable="move_group",
         output="screen",
-        parameters=[moveit_config.to_dict()],
+        parameters=[moveit_config_dict],
         arguments=["--ros-args", "--log-level", "info"],
     )
 
@@ -87,27 +71,11 @@ def generate_test_description():
         executable="robot_state_publisher",
         name="robot_state_publisher",
         output="both",
-        parameters=[moveit_config.robot_description],
-    )
-
-    move_group_gtest = launch_ros.actions.Node(
-        executable=launch.substitutions.PathJoinSubstitution(
-            [
-                launch.substitutions.LaunchConfiguration("test_binary_dir"),
-                "move_group_api_test",
-            ]
-        ),
-        parameters=[moveit_config.to_dict()],
-        output="screen",
+        parameters=[{"robot_description": moveit_config_dict["robot_description"]}],
     )
 
     return launch.LaunchDescription(
         [
-            launch.actions.DeclareLaunchArgument(
-                name="test_binary_dir",
-                description="Binary directory of package "
-                "containing test executables",
-            ),
             static_tf_node,
             robot_state_publisher,
             move_group_node,
@@ -122,16 +90,3 @@ def generate_test_description():
     ), {
         "move_group_gtest": move_group_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, move_group_gtest):
-        self.proc_info.assertWaitForShutdown(move_group_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, move_group_gtest):
-        launch_testing.asserts.assertExitCodes(proc_info, process=move_group_gtest)

moveit_ros/tests/src/move_group_default_planning_scene_test.cpp

@@ -0,0 +1,74 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2024, PickNik Inc.
+ *  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 PickNik Inc. 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: Paul Gesel
+   Description: Integration tests for the move_group default planning scene behavior
+*/
+
+#include <gtest/gtest.h>
+#include <rclcpp/rclcpp.hpp>
+#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
+
+TEST(MoveGroup, testDefaultPlanningScene)
+{
+  // PlanningSceneMonitor is used for getting the robot joint state and calculating the Cartesian pose for a given link.
+  auto node = std::make_shared<rclcpp::Node>("move_group_test");
+  auto planning_scene_monitor =
+      std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(node, "robot_description");
+  ASSERT_TRUE(planning_scene_monitor->requestPlanningSceneState()) << "Failed to get planning scene";
+  rclcpp::sleep_for(std::chrono::seconds(5));
+  ASSERT_TRUE(planning_scene_monitor->requestPlanningSceneState()) << "Failed to get planning scene";
+  planning_scene::PlanningScenePtr ps = planning_scene_monitor->getPlanningScene();
+
+  // ensure the planning scene and world are not null
+  ASSERT_NE(ps, nullptr) << "PlanningScene was null";
+  ASSERT_NE(ps->getWorld(), nullptr) << "PlanningScene->getWorld() was null";
+
+  // check that geometry in test file matches the currecnt values
+  std::unordered_set<std::string> expected_ids = { "Box_0", "Cylinder_0" };
+  EXPECT_EQ(ps->getWorld()->getObjectIds().size(), 2ul);
+  for (const auto& id : ps->getWorld()->getObjectIds())
+  {
+    EXPECT_NE(expected_ids.find(id), expected_ids.end()) << "Unexpected object id";
+  }
+}
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+  ::testing::InitGoogleTest(&argc, argv);
+  int result = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return result;
+}