Add parameter api integration test (#2662)

moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt

@@ -16,7 +16,6 @@ find_package(moveit_core REQUIRED)
 find_package(moveit_msgs REQUIRED)
 find_package(moveit_ros_move_group REQUIRED)
 find_package(moveit_ros_planning REQUIRED)
-find_package(moveit_ros_planning_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(tf2 REQUIRED)
@@ -29,19 +28,6 @@ find_package(tf2_ros REQUIRED)
 find_package(orocos_kdl REQUIRED)
 find_package(Boost REQUIRED COMPONENTS)
 
-# TODO(henning): Remove/Port if(CATKIN_ENABLE_TESTING AND
-# ENABLE_COVERAGE_TESTING) find_package(code_coverage REQUIRED)
-# append_coverage_compiler_flags() endif()
-#
-# ##############################################################################
-# ## Clang tidy ##
-# ##############################################################################
-# if(CATKIN_ENABLE_CLANG_TIDY) find_program( CLANG_TIDY_EXE NAMES "clang-tidy"
-# DOC "Path to clang-tidy executable" ) if(NOT CLANG_TIDY_EXE)
-# message(FATAL_ERROR "clang-tidy not found.") else() message(STATUS "clang-tidy
-# found: ${CLANG_TIDY_EXE}") set(CMAKE_CXX_CLANG_TIDY "${CLANG_TIDY_EXE}")
-# endif() endif()
-
 # ##############################################################################
 # Build ##
 # ##############################################################################
@@ -57,7 +43,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
     moveit_msgs
     moveit_ros_move_group
     moveit_ros_planning
-    moveit_ros_planning_interface
     pluginlib
     rclcpp
     tf2

moveit_planners/pilz_industrial_motion_planner/package.xml

@@ -24,7 +24,6 @@
   -->
   <depend>generate_parameter_library</depend>
   <depend>geometry_msgs</depend>
-  <depend>moveit_ros_planning_interface</depend>
   <depend>moveit_msgs</depend>
   <depend>moveit_core</depend>
   <depend>moveit_ros_planning</depend> <!-- RobotModelLoader -->

moveit_ros/move_group/package.xml

@@ -36,6 +36,10 @@
   <exec_depend>moveit_kinematics</exec_depend>
 
   <test_depend>moveit_resources_fanuc_moveit_config</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>moveit_configs_utils</test_depend>
+  <test_depend>moveit_resources_panda_moveit_config</test_depend>
+  <test_depend>ros_testing</test_depend>
 
   <export>
     <moveit_ros_move_group plugin="${prefix}/default_capabilities_plugin_description.xml" />

moveit_ros/planning_interface/package.xml

@@ -42,7 +42,6 @@
   <test_depend>moveit_resources_fanuc_moveit_config</test_depend>
   <test_depend>moveit_resources_panda_moveit_config</test_depend>
   <test_depend>moveit_simple_controller_manager</test_depend>
-  <test_depend>moveit_planners_ompl</test_depend>
   <test_depend>rviz2</test_depend>
   <test_depend>ros_testing</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>

moveit_ros/tests/CMakeLists.txt

@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.22)
+project(moveit_ros_tests LANGUAGES CXX)
+
+# Common cmake code applied to all moveit packages
+find_package(moveit_common REQUIRED)
+moveit_package()
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_cmake 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}")
+endif()

moveit_ros/tests/launch/move_group_api.test.py

@@ -0,0 +1,137 @@
+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",
+        )
+        .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()
+    )
+
+    # 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()],
+        arguments=["--ros-args", "--log-level", "info"],
+    )
+
+    # 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",
+            "/controller_manager",
+        ],
+        output="screen",
+    )
+
+    panda_arm_controller_spawner = launch_ros.actions.Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["panda_arm_controller", "-c", "/controller_manager"],
+    )
+
+    panda_hand_controller_spawner = launch_ros.actions.Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["panda_hand_controller", "-c", "/controller_manager"],
+    )
+
+    # Static TF
+    static_tf_node = launch_ros.actions.Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name="static_transform_publisher",
+        output="log",
+        arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"],
+    )
+
+    # Publish TF
+    robot_state_publisher = launch_ros.actions.Node(
+        package="robot_state_publisher",
+        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",
+    )
+
+    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,
+            ros2_control_node,
+            joint_state_broadcaster_spawner,
+            panda_arm_controller_spawner,
+            panda_hand_controller_spawner,
+            move_group_gtest,
+            # launch.actions.TimerAction(period=15.0, actions=[move_group_gtest]),
+            launch_testing.actions.ReadyToTest(),
+        ]
+    ), {
+        "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/package.xml

@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>moveit_ros_tests</name>
+  <version>2.9.0</version>
+  <description>Integration tests for moveit_ros</description>
+  <maintainer email="[email protected]">MoveIt Release Team</maintainer>
+
+  <license>BSD-3-Clause</license>
+
+  <url type="website">http://moveit.ros.org</url>
+  <url type="bugtracker">https://github.com/ros-planning/moveit2/issues</url>
+  <url type="repository">https://github.com/ros-planning/moveit2</url>
+
+  <author email="[email protected]">Sebastian Jahr</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>moveit_common</depend>
+  <depend>rclcpp</depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>moveit_configs_utils</test_depend>
+  <test_depend>moveit_core</test_depend>
+  <test_depend>moveit_resources_panda_moveit_config</test_depend>
+  <test_depend>moveit_ros_planning</test_depend>
+  <test_depend>moveit_ros_planning_interface</test_depend>
+  <test_depend>moveit_simple_controller_manager</test_depend>
+  <test_depend>moveit_planners_ompl</test_depend>
+  <test_depend>moveit_planners_chomp</test_depend>
+  <test_depend>moveit_planners_stomp</test_depend>
+  <test_depend>moveit_ros_move_group</test_depend>
+  <test_depend>pilz_industrial_motion_planner</test_depend>
+  <test_depend>ros_testing</test_depend>
+  <test_depend>tf2_ros</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

moveit_ros/tests/src/move_group_api_test.cpp

@@ -0,0 +1,105 @@
+/*********************************************************************
+ * 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: Sebastian Jahr
+   Description: Integration tests for the move_group interface
+*/
+
+#include <gtest/gtest.h>
+#include <rclcpp/rclcpp.hpp>
+
+#include "parameter_name_list.hpp"
+
+class MoveGroupFixture : public testing::Test
+{
+protected:
+  void SetUp() override
+  {
+    rclcpp::NodeOptions node_options;
+    node_options.automatically_declare_parameters_from_overrides(true);
+    test_node_ = std::make_shared<rclcpp::Node>("move_group_param_test_node", node_options);
+  }
+
+  void TearDown() override
+  {
+  }
+
+  std::shared_ptr<rclcpp::Node> test_node_;
+};
+
+TEST_F(MoveGroupFixture, testParamAPI)
+{
+  // Create parameter client
+  auto params_client = std::make_shared<rclcpp::SyncParametersClient>(test_node_, "move_group");
+  bool reach_param_client = params_client->wait_for_service(std::chrono::seconds(5));
+  // This pattern helps with debugging if service is not available.
+  if (!reach_param_client)
+  {
+    RCLCPP_ERROR(test_node_->get_logger(), "Couldn't reach parameter server. Is the move_group up and running?");
+  }
+  ASSERT_TRUE(reach_param_client);
+
+  // GIVEN a running move_group
+  // WHEN a parameter name from parameter API checked
+  // THEN that parameter exists
+  for (const auto& param_name : move_group_test::PARAMETER_NAME_LIST)
+  {
+    bool param_exists = false;
+    if (params_client->wait_for_service(std::chrono::milliseconds(1)))
+    {
+      param_exists = params_client->has_parameter(param_name);
+      if (!param_exists)
+      {
+        RCLCPP_ERROR(test_node_->get_logger(), "Parameter %s doesn't exists", param_name.c_str());
+      }
+    }
+    else
+    {
+      RCLCPP_ERROR(test_node_->get_logger(), "Cannot read parameter %s because service couldn't be reached",
+                   param_name.c_str());
+    }
+    EXPECT_TRUE(param_exists);
+  }
+}
+
+// TODO(sjahr): Add more API tests e.g. from move_group_interface_cpp_test.cpp
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+  ::testing::InitGoogleTest(&argc, argv);
+  int result = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return result;
+}