From 91c18d1124481c5e3e84b7af0e26efadfde22420 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 5 Aug 2024 10:42:58 -0600 Subject: [PATCH 01/14] Add parameter to load planning scene geometry on startup Signed-off-by: Paul Gesel --- moveit_ros/move_group/src/move_group.cpp | 28 ++++++++++++++++-------- 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/moveit_ros/move_group/src/move_group.cpp b/moveit_ros/move_group/src/move_group.cpp index e67ad74963..e1d739b493 100644 --- a/moveit_ros/move_group/src/move_group.cpp +++ b/moveit_ros/move_group/src/move_group.cpp @@ -34,17 +34,20 @@ /* Author: Ioan Sucan */ -#include -#include -#include -#include -#include +#include +#include +#include +#include + #include #include +#include #include -#include -#include +#include +#include +#include #include +#include static const std::string ROBOT_DESCRIPTION = "robot_description"; // name of the robot description (a param name, so it can be changed externally) @@ -287,9 +290,16 @@ int main(int argc, char** argv) // Initialize MoveItCpp const auto moveit_cpp = std::make_shared(nh, moveit_cpp_options); const auto planning_scene_monitor = moveit_cpp->getPlanningSceneMonitorNonConst(); - - if (planning_scene_monitor->getPlanningScene()) + auto ps = planning_scene_monitor->getPlanningScene(); + if (ps) { + if (nh->has_parameter("default_planning_scene")) + { + std::string path = nh->get_parameter("default_planning_scene").as_string(); + std::fstream fs; + fs.open(path, std::fstream::in); + ps->loadGeometryFromStream(fs); + } bool debug = false; for (int i = 1; i < argc; ++i) { From 36adcf48fc1cd16cc9a6a19b52308798abe168ac Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 5 Aug 2024 10:56:37 -0600 Subject: [PATCH 02/14] add error check Signed-off-by: Paul Gesel --- moveit_ros/move_group/src/move_group.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/moveit_ros/move_group/src/move_group.cpp b/moveit_ros/move_group/src/move_group.cpp index e1d739b493..4602e17dc7 100644 --- a/moveit_ros/move_group/src/move_group.cpp +++ b/moveit_ros/move_group/src/move_group.cpp @@ -35,8 +35,6 @@ /* Author: Ioan Sucan */ #include -#include -#include #include #include @@ -290,15 +288,21 @@ int main(int argc, char** argv) // Initialize MoveItCpp const auto moveit_cpp = std::make_shared(nh, moveit_cpp_options); const auto planning_scene_monitor = moveit_cpp->getPlanningSceneMonitorNonConst(); - auto ps = planning_scene_monitor->getPlanningScene(); - if (ps) + 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 fs; - fs.open(path, std::fstream::in); - ps->loadGeometryFromStream(fs); + 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(), + ("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 %s.", + path.c_str())); + } + return 0; } bool debug = false; for (int i = 1; i < argc; ++i) From 3fbe0a8d097a2481ab34728e1cae7df77d2ff4fa Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 5 Aug 2024 11:03:43 -0600 Subject: [PATCH 03/14] fix potentially insecure Signed-off-by: Paul Gesel --- moveit_ros/move_group/src/move_group.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/moveit_ros/move_group/src/move_group.cpp b/moveit_ros/move_group/src/move_group.cpp index 4602e17dc7..331296be8f 100644 --- a/moveit_ros/move_group/src/move_group.cpp +++ b/moveit_ros/move_group/src/move_group.cpp @@ -297,10 +297,12 @@ int main(int argc, char** argv) file_stream.open(path, std::fstream::in); if (!file_stream.is_open() || !ps->loadGeometryFromStream(file_stream)) { - RCLCPP_ERROR(nh->get_logger(), - ("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 %s.", - path.c_str())); + 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; } From 873bf5c4b60ecdfd7d50faf8c26172259715282d Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 5 Aug 2024 12:01:49 -0600 Subject: [PATCH 04/14] add python test launch file Signed-off-by: Paul Gesel --- moveit_ros/tests/CMakeLists.txt | 2 + moveit_ros/tests/data/scene.txt | 22 ++++ .../tests/launch/move_group_api.test.py | 100 +--------------- .../move_group_default_planning_scene.test.py | 44 +++++++ .../launch/move_group_test_description.py | 108 ++++++++++++++++++ 5 files changed, 178 insertions(+), 98 deletions(-) create mode 100644 moveit_ros/tests/data/scene.txt create mode 100644 moveit_ros/tests/launch/move_group_default_planning_scene.test.py create mode 100644 moveit_ros/tests/launch/move_group_test_description.py diff --git a/moveit_ros/tests/CMakeLists.txt b/moveit_ros/tests/CMakeLists.txt index 4f9a83b7f3..dcf6a8a262 100644 --- a/moveit_ros/tests/CMakeLists.txt +++ b/moveit_ros/tests/CMakeLists.txt @@ -17,3 +17,5 @@ if(BUILD_TESTING) add_ros_test(launch/move_group_api.test.py TIMEOUT 30 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") endif() + +install(DIRECTORY data DESTINATION share/${PROJECT_NAME}) diff --git a/moveit_ros/tests/data/scene.txt b/moveit_ros/tests/data/scene.txt new file mode 100644 index 0000000000..13ae3e59b0 --- /dev/null +++ b/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 +. diff --git a/moveit_ros/tests/launch/move_group_api.test.py b/moveit_ros/tests/launch/move_group_api.test.py index 4d66d6f018..4911b5c484 100644 --- a/moveit_ros/tests/launch/move_group_api.test.py +++ b/moveit_ros/tests/launch/move_group_api.test.py @@ -5,6 +5,7 @@ import launch_testing from ament_index_python.packages import get_package_share_directory from moveit_configs_utils import MoveItConfigsBuilder +from move_group_test_description import generate_move_group_test_description def generate_test_description(): @@ -24,104 +25,7 @@ def generate_test_description(): .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, - } + return generate_move_group_test_description(moveit_config.to_dict()) class TestGTestWaitForCompletion(unittest.TestCase): diff --git a/moveit_ros/tests/launch/move_group_default_planning_scene.test.py b/moveit_ros/tests/launch/move_group_default_planning_scene.test.py new file mode 100644 index 0000000000..b70e338de8 --- /dev/null +++ b/moveit_ros/tests/launch/move_group_default_planning_scene.test.py @@ -0,0 +1,44 @@ +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 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() + ) + 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') + + return generate_move_group_test_description() + + +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) diff --git a/moveit_ros/tests/launch/move_group_test_description.py b/moveit_ros/tests/launch/move_group_test_description.py new file mode 100644 index 0000000000..a2bdc4214b --- /dev/null +++ b/moveit_ros/tests/launch/move_group_test_description.py @@ -0,0 +1,108 @@ +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_move_group_test_description(moveit_config_dict): + # 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_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, + } From a14e2c61cf8199f09a1a323ae930de22700b2b8b Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 5 Aug 2024 13:15:32 -0600 Subject: [PATCH 05/14] add test files Signed-off-by: Paul Gesel --- moveit_ros/tests/CMakeLists.txt | 11 +++- .../tests/launch/move_group_api.test.py | 4 +- .../move_group_default_planning_scene.test.py | 11 ++-- moveit_ros/tests/moveit_ros_tests/__init__.py | 0 .../move_group_test_description.py | 6 +-- ...move_group_default_planning_scene_test.cpp | 53 +++++++++++++++++++ 6 files changed, 76 insertions(+), 9 deletions(-) create mode 100644 moveit_ros/tests/moveit_ros_tests/__init__.py rename moveit_ros/tests/{launch => moveit_ros_tests}/move_group_test_description.py (95%) create mode 100644 moveit_ros/tests/src/move_group_default_planning_scene_test.cpp diff --git a/moveit_ros/tests/CMakeLists.txt b/moveit_ros/tests/CMakeLists.txt index dcf6a8a262..78384a0697 100644 --- a/moveit_ros/tests/CMakeLists.txt +++ b/moveit_ros/tests/CMakeLists.txt @@ -2,12 +2,12 @@ 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(rclcpp REQUIRED) find_package(ros_testing REQUIRED) @@ -16,6 +16,15 @@ if(BUILD_TESTING) 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}") + + 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) + add_ros_test(launch/move_group_default_planning_scene.test.py TIMEOUT 30 ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") endif() +ament_python_install_package(moveit_ros_tests) install(DIRECTORY data DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/moveit_ros/tests/launch/move_group_api.test.py b/moveit_ros/tests/launch/move_group_api.test.py index 4911b5c484..9646a9e2a2 100644 --- a/moveit_ros/tests/launch/move_group_api.test.py +++ b/moveit_ros/tests/launch/move_group_api.test.py @@ -5,7 +5,9 @@ import launch_testing from ament_index_python.packages import get_package_share_directory from moveit_configs_utils import MoveItConfigsBuilder -from move_group_test_description import generate_move_group_test_description +from moveit_ros_tests.move_group_test_description import ( + generate_move_group_test_description, +) def generate_test_description(): diff --git a/moveit_ros/tests/launch/move_group_default_planning_scene.test.py b/moveit_ros/tests/launch/move_group_default_planning_scene.test.py index b70e338de8..057e092458 100644 --- a/moveit_ros/tests/launch/move_group_default_planning_scene.test.py +++ b/moveit_ros/tests/launch/move_group_default_planning_scene.test.py @@ -5,7 +5,9 @@ import launch_testing from ament_index_python.packages import get_package_share_directory from moveit_configs_utils import MoveItConfigsBuilder -from move_group_test_description import generate_move_group_test_description +from moveit_ros_tests.move_group_test_description import ( + generate_move_group_test_description, +) def generate_test_description(): @@ -25,10 +27,11 @@ def generate_test_description(): .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') + moveit_config_dict["default_planning_scene"] = os.path.join( + get_package_share_directory("moveit_ros_tests"), "data", "scene.txt" + ) - return generate_move_group_test_description() + return generate_move_group_test_description(moveit_config_dict) class TestGTestWaitForCompletion(unittest.TestCase): diff --git a/moveit_ros/tests/moveit_ros_tests/__init__.py b/moveit_ros/tests/moveit_ros_tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/moveit_ros/tests/launch/move_group_test_description.py b/moveit_ros/tests/moveit_ros_tests/move_group_test_description.py similarity index 95% rename from moveit_ros/tests/launch/move_group_test_description.py rename to moveit_ros/tests/moveit_ros_tests/move_group_test_description.py index a2bdc4214b..9a4f32c442 100644 --- a/moveit_ros/tests/launch/move_group_test_description.py +++ b/moveit_ros/tests/moveit_ros_tests/move_group_test_description.py @@ -71,7 +71,7 @@ def generate_move_group_test_description(moveit_config_dict): executable="robot_state_publisher", name="robot_state_publisher", output="both", - parameters=[moveit_config.robot_description], + parameters=[moveit_config_dict["robot_description"]], ) move_group_gtest = launch_ros.actions.Node( @@ -81,7 +81,7 @@ def generate_move_group_test_description(moveit_config_dict): "move_group_api_test", ] ), - parameters=[moveit_config.to_dict()], + parameters=[moveit_config_dict], output="screen", ) @@ -90,7 +90,7 @@ def generate_move_group_test_description(moveit_config_dict): launch.actions.DeclareLaunchArgument( name="test_binary_dir", description="Binary directory of package " - "containing test executables", + "containing test executables", ), static_tf_node, robot_state_publisher, diff --git a/moveit_ros/tests/src/move_group_default_planning_scene_test.cpp b/moveit_ros/tests/src/move_group_default_planning_scene_test.cpp new file mode 100644 index 0000000000..2656713b8f --- /dev/null +++ b/moveit_ros/tests/src/move_group_default_planning_scene_test.cpp @@ -0,0 +1,53 @@ +/********************************************************************* + * 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 +#include + +TEST(MoveGroup, testDefaultPlanningScene) +{ +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} From 2f8b28d93555394957127efca608fa0959bd93dd Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 5 Aug 2024 14:53:34 -0600 Subject: [PATCH 06/14] test working unless run in parallel Signed-off-by: Paul Gesel --- moveit_ros/move_group/src/move_group.cpp | 3 ++- moveit_ros/tests/CMakeLists.txt | 4 +++- moveit_ros/tests/launch/move_group_api.test.py | 14 +++++++++++++- .../move_group_default_planning_scene.test.py | 13 ++++++++++++- .../move_group_test_description.py | 13 +------------ .../move_group_default_planning_scene_test.cpp | 18 ++++++++++++++++++ 6 files changed, 49 insertions(+), 16 deletions(-) diff --git a/moveit_ros/move_group/src/move_group.cpp b/moveit_ros/move_group/src/move_group.cpp index 331296be8f..6c681b6adb 100644 --- a/moveit_ros/move_group/src/move_group.cpp +++ b/moveit_ros/move_group/src/move_group.cpp @@ -303,8 +303,9 @@ int main(int argc, char** argv) .append(path) .append(".") .c_str()); + + return 0; } - return 0; } bool debug = false; for (int i = 1; i < argc; ++i) diff --git a/moveit_ros/tests/CMakeLists.txt b/moveit_ros/tests/CMakeLists.txt index 78384a0697..588c2bffe8 100644 --- a/moveit_ros/tests/CMakeLists.txt +++ b/moveit_ros/tests/CMakeLists.txt @@ -8,6 +8,7 @@ moveit_package() if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) + find_package(moveit_ros_planning REQUIRED) find_package(rclcpp REQUIRED) find_package(ros_testing REQUIRED) @@ -19,7 +20,8 @@ if(BUILD_TESTING) 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) + ament_target_dependencies(move_group_default_planning_scene_test rclcpp + moveit_ros_planning) add_ros_test(launch/move_group_default_planning_scene.test.py TIMEOUT 30 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") endif() diff --git a/moveit_ros/tests/launch/move_group_api.test.py b/moveit_ros/tests/launch/move_group_api.test.py index 9646a9e2a2..a2e784ac51 100644 --- a/moveit_ros/tests/launch/move_group_api.test.py +++ b/moveit_ros/tests/launch/move_group_api.test.py @@ -26,8 +26,20 @@ def generate_test_description(): ) .to_moveit_configs() ) + 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 generate_move_group_test_description(moveit_config.to_dict()) + return generate_move_group_test_description( + moveit_config.to_dict(), move_group_gtest + ) class TestGTestWaitForCompletion(unittest.TestCase): diff --git a/moveit_ros/tests/launch/move_group_default_planning_scene.test.py b/moveit_ros/tests/launch/move_group_default_planning_scene.test.py index 057e092458..604ef57217 100644 --- a/moveit_ros/tests/launch/move_group_default_planning_scene.test.py +++ b/moveit_ros/tests/launch/move_group_default_planning_scene.test.py @@ -31,7 +31,18 @@ def generate_test_description(): get_package_share_directory("moveit_ros_tests"), "data", "scene.txt" ) - return generate_move_group_test_description(moveit_config_dict) + move_group_gtest = launch_ros.actions.Node( + executable=launch.substitutions.PathJoinSubstitution( + [ + launch.substitutions.LaunchConfiguration("test_binary_dir"), + "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): diff --git a/moveit_ros/tests/moveit_ros_tests/move_group_test_description.py b/moveit_ros/tests/moveit_ros_tests/move_group_test_description.py index 9a4f32c442..972cd11fef 100644 --- a/moveit_ros/tests/moveit_ros_tests/move_group_test_description.py +++ b/moveit_ros/tests/moveit_ros_tests/move_group_test_description.py @@ -7,7 +7,7 @@ from moveit_configs_utils import MoveItConfigsBuilder -def generate_move_group_test_description(moveit_config_dict): +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", @@ -74,17 +74,6 @@ def generate_move_group_test_description(moveit_config_dict): parameters=[moveit_config_dict["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_dict], - output="screen", - ) - return launch.LaunchDescription( [ launch.actions.DeclareLaunchArgument( diff --git a/moveit_ros/tests/src/move_group_default_planning_scene_test.cpp b/moveit_ros/tests/src/move_group_default_planning_scene_test.cpp index 2656713b8f..7c0856dd3a 100644 --- a/moveit_ros/tests/src/move_group_default_planning_scene_test.cpp +++ b/moveit_ros/tests/src/move_group_default_planning_scene_test.cpp @@ -38,9 +38,27 @@ #include #include +#include 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("move_group_test"); + auto planning_scene_monitor = + std::make_shared(node, "robot_description"); + planning_scene_monitor->requestPlanningSceneState(); + planning_scene::PlanningScenePtr ps = planning_scene_monitor->getPlanningScene(); + + // ensure the planning scene and world are not null + ASSERT_NE(ps, nullptr); + ASSERT_NE(ps->getWorld(), nullptr); + + // check that geometry in test file matches the currecnt values + std::unordered_set expected_ids = { "Box_0", "Cylinder_0" }; + for (const auto& id : ps->getWorld()->getObjectIds()) + { + EXPECT_NE(expected_ids.find(id), expected_ids.end()); + } } int main(int argc, char** argv) From 8a0de0e45778f92fed380e2b0304ba7d99a20887 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 5 Aug 2024 16:55:04 -0600 Subject: [PATCH 07/14] fix islated pytest Signed-off-by: Paul Gesel --- moveit_ros/tests/CMakeLists.txt | 17 +++++++++---- .../tests/launch/move_group_api.test.py | 5 ++++ ...y => move_group_default_planning_scene.py} | 25 ++++++++----------- .../move_group_test_description.py | 5 ---- 4 files changed, 27 insertions(+), 25 deletions(-) rename moveit_ros/tests/launch/{move_group_default_planning_scene.test.py => move_group_default_planning_scene.py} (67%) diff --git a/moveit_ros/tests/CMakeLists.txt b/moveit_ros/tests/CMakeLists.txt index 588c2bffe8..6687777345 100644 --- a/moveit_ros/tests/CMakeLists.txt +++ b/moveit_ros/tests/CMakeLists.txt @@ -18,15 +18,22 @@ if(BUILD_TESTING) add_ros_test(launch/move_group_api.test.py TIMEOUT 30 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + # run default_planning_scene test isolated test to avoid interference + find_package(ament_cmake_ros REQUIRED) 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) - add_ros_test(launch/move_group_default_planning_scene.test.py TIMEOUT 30 ARGS - "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") -endif() + ament_add_ros_isolated_pytest_test( + move_group_default_planning_scene_test + launch/move_group_default_planning_scene.py TIMEOUT 30 WORKING_DIRECTORY + ${CMAKE_CURRENT_BINARY_DIR}) -ament_python_install_package(moveit_ros_tests) -install(DIRECTORY data DESTINATION share/${PROJECT_NAME}) + # install Python helper script and test + ament_python_install_package(moveit_ros_tests) + install(DIRECTORY data DESTINATION share/${PROJECT_NAME}) + install(TARGETS move_group_default_planning_scene_test + RUNTIME DESTINATION lib/${PROJECT_NAME}) +endif() ament_package() diff --git a/moveit_ros/tests/launch/move_group_api.test.py b/moveit_ros/tests/launch/move_group_api.test.py index a2e784ac51..533d4b39dd 100644 --- a/moveit_ros/tests/launch/move_group_api.test.py +++ b/moveit_ros/tests/launch/move_group_api.test.py @@ -26,6 +26,11 @@ def generate_test_description(): ) .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( executable=launch.substitutions.PathJoinSubstitution( [ diff --git a/moveit_ros/tests/launch/move_group_default_planning_scene.test.py b/moveit_ros/tests/launch/move_group_default_planning_scene.py similarity index 67% rename from moveit_ros/tests/launch/move_group_default_planning_scene.test.py rename to moveit_ros/tests/launch/move_group_default_planning_scene.py index 604ef57217..c15e055f8f 100644 --- a/moveit_ros/tests/launch/move_group_default_planning_scene.test.py +++ b/moveit_ros/tests/launch/move_group_default_planning_scene.py @@ -1,4 +1,6 @@ import os +import sys + import launch import unittest import launch_ros @@ -8,8 +10,11 @@ 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") @@ -32,12 +37,8 @@ def generate_test_description(): ) move_group_gtest = launch_ros.actions.Node( - executable=launch.substitutions.PathJoinSubstitution( - [ - launch.substitutions.LaunchConfiguration("test_binary_dir"), - "move_group_default_planning_scene_test", - ] - ), + package="moveit_ros_tests", + executable="move_group_default_planning_scene_test", parameters=[moveit_config_dict], output="screen", ) @@ -45,14 +46,8 @@ def generate_test_description(): 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): +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info, move_group_gtest): + # Check that process exits with code 0: no error launch_testing.asserts.assertExitCodes(proc_info, process=move_group_gtest) diff --git a/moveit_ros/tests/moveit_ros_tests/move_group_test_description.py b/moveit_ros/tests/moveit_ros_tests/move_group_test_description.py index 972cd11fef..e910509049 100644 --- a/moveit_ros/tests/moveit_ros_tests/move_group_test_description.py +++ b/moveit_ros/tests/moveit_ros_tests/move_group_test_description.py @@ -76,11 +76,6 @@ def generate_move_group_test_description(moveit_config_dict, move_group_gtest): 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, From cbd89c577c7dc4b158a3935f700e2ad1fe06c982 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 5 Aug 2024 18:03:13 -0600 Subject: [PATCH 08/14] add comments and fix isolated Signed-off-by: Paul Gesel --- moveit_ros/tests/CMakeLists.txt | 14 ++++++-------- .../{move_group_api.test.py => move_group_api.py} | 8 ++------ .../launch/move_group_default_planning_scene.py | 13 +++++++++---- .../src/move_group_default_planning_scene_test.cpp | 7 ++++--- 4 files changed, 21 insertions(+), 21 deletions(-) rename moveit_ros/tests/launch/{move_group_api.test.py => move_group_api.py} (89%) diff --git a/moveit_ros/tests/CMakeLists.txt b/moveit_ros/tests/CMakeLists.txt index 6687777345..a3f5dfb465 100644 --- a/moveit_ros/tests/CMakeLists.txt +++ b/moveit_ros/tests/CMakeLists.txt @@ -8,6 +8,7 @@ moveit_package() if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_ros REQUIRED) find_package(moveit_ros_planning REQUIRED) find_package(rclcpp REQUIRED) find_package(ros_testing REQUIRED) @@ -15,24 +16,21 @@ if(BUILD_TESTING) 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) - # run default_planning_scene test isolated test to avoid interference - find_package(ament_cmake_ros REQUIRED) 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 ament_add_ros_isolated_pytest_test( - move_group_default_planning_scene_test - launch/move_group_default_planning_scene.py TIMEOUT 30 WORKING_DIRECTORY - ${CMAKE_CURRENT_BINARY_DIR}) + move_group_default_planning_scene + launch/move_group_default_planning_scene.py TIMEOUT 30) # install Python helper script and test ament_python_install_package(moveit_ros_tests) install(DIRECTORY data DESTINATION share/${PROJECT_NAME}) - install(TARGETS move_group_default_planning_scene_test + install(TARGETS move_group_api_test move_group_default_planning_scene_test RUNTIME DESTINATION lib/${PROJECT_NAME}) endif() diff --git a/moveit_ros/tests/launch/move_group_api.test.py b/moveit_ros/tests/launch/move_group_api.py similarity index 89% rename from moveit_ros/tests/launch/move_group_api.test.py rename to moveit_ros/tests/launch/move_group_api.py index 533d4b39dd..0dbdec64d6 100644 --- a/moveit_ros/tests/launch/move_group_api.test.py +++ b/moveit_ros/tests/launch/move_group_api.py @@ -32,12 +32,8 @@ def generate_test_description(): description="Binary directory of package " "containing test executables", ) move_group_gtest = launch_ros.actions.Node( - executable=launch.substitutions.PathJoinSubstitution( - [ - launch.substitutions.LaunchConfiguration("test_binary_dir"), - "move_group_api_test", - ] - ), + package="moveit_ros_tests", + executable="move_group_api_test", parameters=[moveit_config.to_dict()], output="screen", ) diff --git a/moveit_ros/tests/launch/move_group_default_planning_scene.py b/moveit_ros/tests/launch/move_group_default_planning_scene.py index c15e055f8f..0551283804 100644 --- a/moveit_ros/tests/launch/move_group_default_planning_scene.py +++ b/moveit_ros/tests/launch/move_group_default_planning_scene.py @@ -1,5 +1,4 @@ import os -import sys import launch import unittest @@ -46,8 +45,14 @@ def generate_test_description(): 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 TestProcessOutput(unittest.TestCase): - def test_exit_code(self, proc_info, move_group_gtest): - # Check that process exits with code 0: no error +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) diff --git a/moveit_ros/tests/src/move_group_default_planning_scene_test.cpp b/moveit_ros/tests/src/move_group_default_planning_scene_test.cpp index 7c0856dd3a..ae1fe85ce4 100644 --- a/moveit_ros/tests/src/move_group_default_planning_scene_test.cpp +++ b/moveit_ros/tests/src/move_group_default_planning_scene_test.cpp @@ -50,14 +50,15 @@ TEST(MoveGroup, testDefaultPlanningScene) planning_scene::PlanningScenePtr ps = planning_scene_monitor->getPlanningScene(); // ensure the planning scene and world are not null - ASSERT_NE(ps, nullptr); - ASSERT_NE(ps->getWorld(), nullptr); + 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 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()); + EXPECT_NE(expected_ids.find(id), expected_ids.end()) << "Unexpected object id"; } } From 5008ddaf7dbb4739b8d411b8d02053a6db52e4d3 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 5 Aug 2024 18:48:39 -0600 Subject: [PATCH 09/14] make test more robust Signed-off-by: Paul Gesel --- moveit_ros/tests/CMakeLists.txt | 7 +++---- .../tests/moveit_ros_tests/move_group_test_description.py | 2 +- .../tests/src/move_group_default_planning_scene_test.cpp | 4 +++- 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/moveit_ros/tests/CMakeLists.txt b/moveit_ros/tests/CMakeLists.txt index a3f5dfb465..43a87e9a99 100644 --- a/moveit_ros/tests/CMakeLists.txt +++ b/moveit_ros/tests/CMakeLists.txt @@ -16,16 +16,15 @@ if(BUILD_TESTING) 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.py TIMEOUT 30) + 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 - ament_add_ros_isolated_pytest_test( - move_group_default_planning_scene - launch/move_group_default_planning_scene.py TIMEOUT 30) + 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) diff --git a/moveit_ros/tests/moveit_ros_tests/move_group_test_description.py b/moveit_ros/tests/moveit_ros_tests/move_group_test_description.py index e910509049..fc1b455f24 100644 --- a/moveit_ros/tests/moveit_ros_tests/move_group_test_description.py +++ b/moveit_ros/tests/moveit_ros_tests/move_group_test_description.py @@ -71,7 +71,7 @@ def generate_move_group_test_description(moveit_config_dict, move_group_gtest): executable="robot_state_publisher", name="robot_state_publisher", output="both", - parameters=[moveit_config_dict["robot_description"]], + parameters=[{"robot_description": moveit_config_dict["robot_description"]}], ) return launch.LaunchDescription( diff --git a/moveit_ros/tests/src/move_group_default_planning_scene_test.cpp b/moveit_ros/tests/src/move_group_default_planning_scene_test.cpp index ae1fe85ce4..f93bf7c2d9 100644 --- a/moveit_ros/tests/src/move_group_default_planning_scene_test.cpp +++ b/moveit_ros/tests/src/move_group_default_planning_scene_test.cpp @@ -46,7 +46,9 @@ TEST(MoveGroup, testDefaultPlanningScene) auto node = std::make_shared("move_group_test"); auto planning_scene_monitor = std::make_shared(node, "robot_description"); - planning_scene_monitor->requestPlanningSceneState(); + 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 From fc1bac2084c75a25c0267b829f2bb089f2cb9511 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 6 Aug 2024 09:23:25 -0600 Subject: [PATCH 10/14] add locked planning scene Signed-off-by: Paul Gesel --- moveit_ros/move_group/src/move_group.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/moveit_ros/move_group/src/move_group.cpp b/moveit_ros/move_group/src/move_group.cpp index 6c681b6adb..579988c812 100644 --- a/moveit_ros/move_group/src/move_group.cpp +++ b/moveit_ros/move_group/src/move_group.cpp @@ -295,7 +295,8 @@ int main(int argc, char** argv) 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)) + planning_scene_monitor::LockedPlanningSceneRW locked_ps(planning_scene_monitor); + if (!file_stream.is_open() || !locked_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 " From 9b47cde3448b9a59ae6e7596be12963ee3ef8cd1 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 5 Nov 2024 09:55:14 -0700 Subject: [PATCH 11/14] undo header change --- moveit_ros/move_group/src/move_group.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/moveit_ros/move_group/src/move_group.cpp b/moveit_ros/move_group/src/move_group.cpp index 6a3605fae9..369cefbd1d 100644 --- a/moveit_ros/move_group/src/move_group.cpp +++ b/moveit_ros/move_group/src/move_group.cpp @@ -34,18 +34,17 @@ /* Author: Ioan Sucan */ -#include -#include - -#include -#include -#include -#include #include #include +#include +#include #include +#include +#include +#include +#include +#include #include -#include static const std::string ROBOT_DESCRIPTION = "robot_description"; // name of the robot description (a param name, so it can be changed externally) From 4eef9eeb5560ca33f9b356805b2245d8ff2c08f0 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 5 Nov 2024 09:59:31 -0700 Subject: [PATCH 12/14] revert test changes Signed-off-by: Paul Gesel --- moveit_ros/move_group/src/move_group.cpp | 1 + moveit_ros/tests/CMakeLists.txt | 23 +---- moveit_ros/tests/data/scene.txt | 22 ----- moveit_ros/tests/launch/move_group_api.py | 56 ----------- .../move_group_default_planning_scene.py | 58 ------------ moveit_ros/tests/moveit_ros_tests/__init__.py | 0 .../move_group_test_description.py | 92 ------------------- ...move_group_default_planning_scene_test.cpp | 74 --------------- 8 files changed, 4 insertions(+), 322 deletions(-) delete mode 100644 moveit_ros/tests/data/scene.txt delete mode 100644 moveit_ros/tests/launch/move_group_api.py delete mode 100644 moveit_ros/tests/launch/move_group_default_planning_scene.py delete mode 100644 moveit_ros/tests/moveit_ros_tests/__init__.py delete mode 100644 moveit_ros/tests/moveit_ros_tests/move_group_test_description.py delete mode 100644 moveit_ros/tests/src/move_group_default_planning_scene_test.cpp diff --git a/moveit_ros/move_group/src/move_group.cpp b/moveit_ros/move_group/src/move_group.cpp index 369cefbd1d..c39957289c 100644 --- a/moveit_ros/move_group/src/move_group.cpp +++ b/moveit_ros/move_group/src/move_group.cpp @@ -289,6 +289,7 @@ int main(int argc, char** argv) // Initialize MoveItCpp const auto moveit_cpp = std::make_shared(nh, moveit_cpp_options); const auto planning_scene_monitor = moveit_cpp->getPlanningSceneMonitorNonConst(); + if (auto ps = planning_scene_monitor->getPlanningScene()) { if (nh->has_parameter("default_planning_scene")) diff --git a/moveit_ros/tests/CMakeLists.txt b/moveit_ros/tests/CMakeLists.txt index 43a87e9a99..4f9a83b7f3 100644 --- a/moveit_ros/tests/CMakeLists.txt +++ b/moveit_ros/tests/CMakeLists.txt @@ -2,35 +2,18 @@ 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_ros REQUIRED) - find_package(moveit_ros_planning 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.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}) + add_ros_test(launch/move_group_api.test.py TIMEOUT 30 ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") endif() - -ament_package() diff --git a/moveit_ros/tests/data/scene.txt b/moveit_ros/tests/data/scene.txt deleted file mode 100644 index 13ae3e59b0..0000000000 --- a/moveit_ros/tests/data/scene.txt +++ /dev/null @@ -1,22 +0,0 @@ -(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 -. diff --git a/moveit_ros/tests/launch/move_group_api.py b/moveit_ros/tests/launch/move_group_api.py deleted file mode 100644 index 0dbdec64d6..0000000000 --- a/moveit_ros/tests/launch/move_group_api.py +++ /dev/null @@ -1,56 +0,0 @@ -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) diff --git a/moveit_ros/tests/launch/move_group_default_planning_scene.py b/moveit_ros/tests/launch/move_group_default_planning_scene.py deleted file mode 100644 index 0551283804..0000000000 --- a/moveit_ros/tests/launch/move_group_default_planning_scene.py +++ /dev/null @@ -1,58 +0,0 @@ -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) diff --git a/moveit_ros/tests/moveit_ros_tests/__init__.py b/moveit_ros/tests/moveit_ros_tests/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/moveit_ros/tests/moveit_ros_tests/move_group_test_description.py b/moveit_ros/tests/moveit_ros_tests/move_group_test_description.py deleted file mode 100644 index fc1b455f24..0000000000 --- a/moveit_ros/tests/moveit_ros_tests/move_group_test_description.py +++ /dev/null @@ -1,92 +0,0 @@ -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_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_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=[{"robot_description": moveit_config_dict["robot_description"]}], - ) - - return launch.LaunchDescription( - [ - 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, - } diff --git a/moveit_ros/tests/src/move_group_default_planning_scene_test.cpp b/moveit_ros/tests/src/move_group_default_planning_scene_test.cpp deleted file mode 100644 index f93bf7c2d9..0000000000 --- a/moveit_ros/tests/src/move_group_default_planning_scene_test.cpp +++ /dev/null @@ -1,74 +0,0 @@ -/********************************************************************* - * 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 -#include -#include - -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("move_group_test"); - auto planning_scene_monitor = - std::make_shared(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 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; -} From eb5e1e61fe5292f10e0e3a090db7f2b6426bc62d Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 5 Nov 2024 10:02:39 -0700 Subject: [PATCH 13/14] run format Signed-off-by: Paul Gesel --- moveit_ros/move_group/src/move_group.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/move_group/src/move_group.cpp b/moveit_ros/move_group/src/move_group.cpp index c39957289c..355a650251 100644 --- a/moveit_ros/move_group/src/move_group.cpp +++ b/moveit_ros/move_group/src/move_group.cpp @@ -289,7 +289,7 @@ int main(int argc, char** argv) // Initialize MoveItCpp const auto moveit_cpp = std::make_shared(nh, moveit_cpp_options); const auto planning_scene_monitor = moveit_cpp->getPlanningSceneMonitorNonConst(); - + if (auto ps = planning_scene_monitor->getPlanningScene()) { if (nh->has_parameter("default_planning_scene")) From 7e4e0c6b73f710b0d616b1e4418e3cc2c5f87a6f Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 5 Nov 2024 10:03:41 -0700 Subject: [PATCH 14/14] add move_group_api.test.py back Signed-off-by: Paul Gesel --- .../tests/launch/move_group_api.test.py | 137 ++++++++++++++++++ 1 file changed, 137 insertions(+) create mode 100644 moveit_ros/tests/launch/move_group_api.test.py diff --git a/moveit_ros/tests/launch/move_group_api.test.py b/moveit_ros/tests/launch/move_group_api.test.py new file mode 100644 index 0000000000..4d66d6f018 --- /dev/null +++ b/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)