diff --git a/moveit_planners/pilz_industrial_motion_planner/package.xml b/moveit_planners/pilz_industrial_motion_planner/package.xml
index f81b04af00b..fbd8588fb94 100644
--- a/moveit_planners/pilz_industrial_motion_planner/package.xml
+++ b/moveit_planners/pilz_industrial_motion_planner/package.xml
@@ -53,6 +53,9 @@
moveit_resources_prbt_moveit_config
moveit_resources_prbt_support
moveit_resources_prbt_pg70_support
+ moveit_configs_utils
+ parameter_builder
+ boost
ament_lint_auto
ament_lint_common
diff --git a/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp
index fc9471d41b9..f3c56c6806b 100644
--- a/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp
@@ -52,7 +52,7 @@
namespace pilz_industrial_motion_planner
{
-static const std::string PARAM_NAMESPACE_LIMTS = "robot_description_planning";
+static const std::string PARAM_NAMESPACE_LIMITS = "robot_description_planning";
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner");
bool CommandPlanner::initialize(const moveit::core::RobotModelConstPtr& model, const rclcpp::Node::SharedPtr& node,
@@ -67,11 +67,11 @@ bool CommandPlanner::initialize(const moveit::core::RobotModelConstPtr& model, c
// Obtain the aggregated joint limits
aggregated_limit_active_joints_ = pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(
- node, PARAM_NAMESPACE_LIMTS, model->getActiveJointModels());
+ node, PARAM_NAMESPACE_LIMITS, model->getActiveJointModels());
// Obtain cartesian limits
cartesian_limit_ =
- pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits(node, PARAM_NAMESPACE_LIMTS);
+ pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits(node, PARAM_NAMESPACE_LIMITS);
// Load the planning context loader
planner_context_loader.reset(new pluginlib::ClassLoader(
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/test/CMakeLists.txt
index e3824b63a83..a37c496d3e1 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/CMakeLists.txt
+++ b/moveit_planners/pilz_industrial_motion_planner/test/CMakeLists.txt
@@ -3,6 +3,7 @@ find_package(ament_cmake_gmock REQUIRED)
find_package(ros_testing REQUIRED)
find_package(moveit_resources_prbt_support REQUIRED)
find_package(moveit_resources_prbt_moveit_config REQUIRED)
+find_package(Boost REQUIRED thread)
find_package(${PROJECT_NAME}_testutils REQUIRED)
@@ -28,4 +29,6 @@ target_link_libraries(${PROJECT_NAME}_testhelpers ${PROJECT_NAME}_lib)
add_subdirectory(unit_tests)
# Integration tests
-add_subdirectory(integration_tests)
\ No newline at end of file
+add_subdirectory(integration_tests)
+# Install test data
+install(DIRECTORY test_data DESTINATION share/${PROJECT_NAME})
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/concept_testdata.odp b/moveit_planners/pilz_industrial_motion_planner/test/test_data/concept_testdata.odp
similarity index 100%
rename from moveit_planners/pilz_industrial_motion_planner/test/test_robots/concept_testdata.odp
rename to moveit_planners/pilz_industrial_motion_planner/test/test_data/concept_testdata.odp
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/concept_testdata.png b/moveit_planners/pilz_industrial_motion_planner/test/test_data/concept_testdata.png
similarity index 100%
rename from moveit_planners/pilz_industrial_motion_planner/test/test_robots/concept_testdata.png
rename to moveit_planners/pilz_industrial_motion_planner/test/test_data/concept_testdata.png
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/test_data/testdata_sequence.xml b/moveit_planners/pilz_industrial_motion_planner/test/test_data/frankaemika_panda/testdata_sequence.xml
similarity index 100%
rename from moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/test_data/testdata_sequence.xml
rename to moveit_planners/pilz_industrial_motion_planner/test/test_data/frankaemika_panda/testdata_sequence.xml
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_deprecated.xml b/moveit_planners/pilz_industrial_motion_planner/test/test_data/prbt/testdata_deprecated.xml
similarity index 100%
rename from moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_deprecated.xml
rename to moveit_planners/pilz_industrial_motion_planner/test/test_data/prbt/testdata_deprecated.xml
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_sequence.xml b/moveit_planners/pilz_industrial_motion_planner/test/test_data/prbt/testdata_sequence.xml
similarity index 100%
rename from moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_sequence.xml
rename to moveit_planners/pilz_industrial_motion_planner/test/test_data/prbt/testdata_sequence.xml
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_with_gripper.xml b/moveit_planners/pilz_industrial_motion_planner/test/test_data/prbt/testdata_with_gripper.xml
similarity index 100%
rename from moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_with_gripper.xml
rename to moveit_planners/pilz_industrial_motion_planner/test/test_data/prbt/testdata_with_gripper.xml
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/testpoints.py b/moveit_planners/pilz_industrial_motion_planner/test/test_data/testpoints.py
similarity index 100%
rename from moveit_planners/pilz_industrial_motion_planner/test/test_robots/testpoints.py
rename to moveit_planners/pilz_industrial_motion_planner/test/test_data/testpoints.py
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch
deleted file mode 100644
index f8ccaa62a39..00000000000
--- a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch
+++ /dev/null
@@ -1,77 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- [/move_group/fake_controller_joint_states]
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/launch/test_context.launch b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/launch/test_context.launch
deleted file mode 100644
index 49ac94b65d6..00000000000
--- a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/launch/test_context.launch
+++ /dev/null
@@ -1,43 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/launch/test_context.launch.py b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/launch/test_context.launch.py
deleted file mode 100644
index 5592ee80fe4..00000000000
--- a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/launch/test_context.launch.py
+++ /dev/null
@@ -1,75 +0,0 @@
-#import os
-#import yaml
-#import xacro
-#from ament_index_python.packages import get_package_share_directory
-#
-#def load_file(package_name, file_path):
-# package_path = get_package_share_directory(package_name)
-# absolute_file_path = os.path.join(package_path, file_path)
-#
-# try:
-# with open(absolute_file_path, "r") as file:
-# return file.read()
-# except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
-# return None
-#
-#def load_yaml(package_name, file_path):
-# package_path = get_package_share_directory(package_name)
-# absolute_file_path = os.path.join(package_path, file_path)
-#
-# try:
-# with open(absolute_file_path, "r") as file:
-# return yaml.safe_load(file)
-# except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
-# return None
-#
-#class PilzPlannerTestContext():
-# robot_description = None
-# robot_description_semantic = None
-# joint_limits = None
-# cartesian_limits = None
-# kinematics = None
-#
-# def __init__(self)
-#
-##TODO Use jafar's moveit_config_builder!
-#
-#def create_test_context(gripper):
-# # Load universal robot description format (URDF)
-# robot_description_config = xacro.process_file(
-# os.path.join(
-# get_package_share_directory("moveit_resources_prbt_support"),
-# "urdf",
-# "prbt.xacro",
-# ), gripper:=gripper)
-#
-# robot_description = {"robot_description": robot_description_config.toxml()}
-#
-#
-# # The semantic description that corresponds to the URDF
-# robot_description_semantic_config = xacro.process_file(
-# os.path.join(
-# get_package_share_directory("moveit_resources_prbt_moveit_config"),
-# "config",
-# "prbt.srdf.xacro",
-# ), gripper:=gripper)
-#
-# # Load limits again in default namespace (which may not be configurable for planners)
-# if gripper == "":
-# joint_limits = load_yaml("moveit_resources_prbt_moveit_config", "config/cartesian_limits.yaml")
-# else:
-# joint_limits = load_yaml("moveit_resources_prbt_"+ gripper + "_support", "config/cartesian_limits.yaml")
-#
-# cartesian_limits = load_yaml(
-# "moveit_resources_prbt_moveit_config", "config/cartesian_limits.yaml"
-# )
-#
-# limits = {
-# "robot_description_planning": {joint_limits, cartesian_limits}
-# }
-#
-# # Load default settings for kinematics; these settings are overridden by settings in a node's namespace
-# kinematics_yaml = load_yaml(
-# "moveit_resources_prbt_moveit_config", "config/kinematics.yaml"
-# )
-# return robot_description, robot_description_semantic_config, limits, kinematics_yaml
\ No newline at end of file
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/CMakeLists.txt
index c25f925d455..d017d844455 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/CMakeLists.txt
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/CMakeLists.txt
@@ -2,20 +2,20 @@ set(UNIT_TEST_SOURCEFILES
unittest_pilz_industrial_motion_planner_direct
unittest_velocity_profile_atrap
unittest_trajectory_generator
- #unittest_trajectory_functions
- #unittest_trajectory_blender_transition_window
- #unittest_trajectory_generator_common
- #unittest_trajectory_generator_circ
- #unittest_trajectory_generator_lin
- #unittest_trajectory_generator_ptp
- #unittest_pilz_industrial_motion_planner
+ unittest_trajectory_functions
+ unittest_trajectory_blender_transition_window
+ unittest_trajectory_generator_common
+ unittest_trajectory_generator_circ
+ unittest_trajectory_generator_lin
+ unittest_trajectory_generator_ptp
+ unittest_pilz_industrial_motion_planner
unittest_joint_limit
- #unittest_joint_limits_aggregator
+ unittest_joint_limits_aggregator
unittest_joint_limits_container
unittest_joint_limits_validator
unittest_cartesian_limits_aggregator
- #unittest_planning_context_loaders
- #unittest_planning_context
+ unittest_planning_context_loaders
+ unittest_planning_context
unittest_get_solver_tip_frame
)
@@ -37,92 +37,85 @@ ament_add_gtest(unittest_trajectory_generator
${CMAKE_SOURCE_DIR}/src/trajectory_generator.cpp)
target_link_libraries(unittest_trajectory_generator ${PROJECT_NAME}_lib)
-# # Trajectory Functions Unit Test
-# add_rostest_gtest(unittest_trajectory_functions
-# test/unittest_trajectory_functions.test
-# test/unittest_trajectory_functions.cpp
-# )
-# target_link_libraries(unittest_trajectory_functions
-# ${catkin_LIBRARIES}
-# ${PROJECT_NAME}_testhelpers
-# )
-#
-# # unittest for trajectory blender transition window
-# add_rostest_gtest(unittest_trajectory_blender_transition_window
-# test/unittest_trajectory_blender_transition_window.test
-# test/unittest_trajectory_blender_transition_window.cpp
-# src/trajectory_blender_transition_window.cpp
-# )
-# target_link_libraries(unittest_trajectory_blender_transition_window
-# ${catkin_LIBRARIES}
-# ${${PROJECT_NAME}_testutils_LIBRARIES}
-# ${PROJECT_NAME}_testhelpers
-# )
-#
-# # trajectory generator Unit Test
-# add_rostest_gtest(unittest_trajectory_generator_common
-# test/unittest_trajectory_generator_common.test
-# test/unittest_trajectory_generator_common.cpp
-# )
-# target_link_libraries(unittest_trajectory_generator_common
-# ${catkin_LIBRARIES}
-# ${PROJECT_NAME}_testhelpers
-# )
-#
-# # trajectory generator circ Unit Test
-# add_rostest_gtest(unittest_trajectory_generator_circ
-# test/unittest_trajectory_generator_circ.test
-# test/unittest_trajectory_generator_circ.cpp
-# )
-# target_link_libraries(unittest_trajectory_generator_circ
-# ${catkin_LIBRARIES}
-# ${${PROJECT_NAME}_testutils_LIBRARIES}
-# ${PROJECT_NAME}_testhelpers
-# )
-#
-# # trajectory generator lin Unit Test
-# add_rostest_gtest(unittest_trajectory_generator_lin
-# test/unittest_trajectory_generator_lin.test
-# test/unittest_trajectory_generator_lin.cpp
-# )
-# target_link_libraries(unittest_trajectory_generator_lin
-# ${catkin_LIBRARIES}
-# ${${PROJECT_NAME}_testutils_LIBRARIES}
-# ${PROJECT_NAME}_testhelpers
-# )
-#
-# # trajectory generator ptp Unit Test
-# add_rostest_gtest(unittest_trajectory_generator_ptp
-# test/unittest_trajectory_generator_ptp.test
-# test/unittest_trajectory_generator_ptp.cpp
-# )
-# target_link_libraries(unittest_trajectory_generator_ptp
-# ${catkin_LIBRARIES}
-# ${PROJECT_NAME}_testhelpers
-# )
-#
+# Trajectory Functions Unit Test
+ament_add_gtest_executable(unittest_trajectory_functions
+ src/unittest_trajectory_functions.cpp
+)
+ament_target_dependencies(unittest_trajectory_functions Boost)
+target_link_libraries(unittest_trajectory_functions
+ ${PROJECT_NAME}_lib
+ ${PROJECT_NAME}_testhelpers
+)
+add_ros_test(launch/unittest_trajectory_functions.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
+
+# unittest for trajectory blender transition window
+ament_add_gtest_executable(unittest_trajectory_blender_transition_window
+ src/unittest_trajectory_blender_transition_window.cpp
+ ${CMAKE_SOURCE_DIR}/src/trajectory_blender_transition_window.cpp
+)
+ament_target_dependencies(unittest_trajectory_blender_transition_window pilz_industrial_motion_planner_testutils)
+target_link_libraries(unittest_trajectory_blender_transition_window
+ ${PROJECT_NAME}_lib
+ ${PROJECT_NAME}_testhelpers
+)
+add_ros_test(launch/unittest_trajectory_blender_transition_window.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
+
+# trajectory generator Unit Test
+ament_add_gtest_executable(unittest_trajectory_generator_common
+ src/unittest_trajectory_generator_common.cpp
+)
+target_link_libraries(unittest_trajectory_generator_common
+ ${PROJECT_NAME}_lib
+ ${PROJECT_NAME}_testhelpers
+)
+add_ros_test(launch/unittest_trajectory_generator_common.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
+
+# trajectory generator circ Unit Test
+ament_add_gtest_executable(unittest_trajectory_generator_circ
+ src/unittest_trajectory_generator_circ.cpp
+)
+ament_target_dependencies(unittest_trajectory_generator_circ pilz_industrial_motion_planner_testutils)
+target_link_libraries(unittest_trajectory_generator_circ
+ ${PROJECT_NAME}_lib
+ ${PROJECT_NAME}_testhelpers
+)
+add_ros_test(launch/unittest_trajectory_generator_circ.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
+
+# trajectory generator lin Unit Test
+ament_add_gtest_executable(unittest_trajectory_generator_lin
+ src/unittest_trajectory_generator_lin.cpp
+)
+ament_target_dependencies(unittest_trajectory_generator_lin pilz_industrial_motion_planner_testutils)
+target_link_libraries(unittest_trajectory_generator_lin
+ ${PROJECT_NAME}_lib
+ ${PROJECT_NAME}_testhelpers
+)
+add_ros_test(launch/unittest_trajectory_generator_lin.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
+
+# trajectory generator ptp Unit Test
+ament_add_gtest_executable(unittest_trajectory_generator_ptp
+ src/unittest_trajectory_generator_ptp.cpp
+)
+target_link_libraries(unittest_trajectory_generator_ptp
+ ${PROJECT_NAME}_lib
+ ${PROJECT_NAME}_testhelpers
+)
+add_ros_test(launch/unittest_trajectory_generator_ptp.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
+
# Command Planner Unit Test
-#ament_add_gtest_executable(unittest_pilz_industrial_motion_planner src/unittest_pilz_industrial_motion_planner.cpp) TODO(sjahr): Debug
-#target_link_libraries(unittest_pilz_industrial_motion_planner ${PROJECT_NAME}_lib)
-#add_ros_test(launch/unittest_pilz_industrial_motion_planner.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
-#
+ament_add_gtest_executable(unittest_pilz_industrial_motion_planner src/unittest_pilz_industrial_motion_planner.cpp)
+target_link_libraries(unittest_pilz_industrial_motion_planner ${PROJECT_NAME}_lib)
+add_ros_test(launch/unittest_pilz_industrial_motion_planner.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
+
# JointLimits Unit Test
ament_add_gtest_executable(unittest_joint_limit src/unittest_joint_limit.cpp)
target_link_libraries(unittest_joint_limit ${PROJECT_NAME}_lib)
add_ros_test(launch/unittest_joint_limit.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
# JointLimitsAggregator Unit Test
-# ament_add_gtest_executable(unittest_joint_limits_aggregator src/unittest_joint_limits_aggregator.cpp)
-# target_link_libraries(unittest_joint_limits_aggregator ${PROJECT_NAME}_testhelpers ${PROJECT_NAME}_lib)
-# add_ros_test(launch/unittest_joint_limits_aggregator.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
-# add_rostest_gtest(unittest_joint_limits_aggregator
-# test/unittest_joint_limits_aggregator.test
-# test/unittest_joint_limits_aggregator.cpp
-# )
-# target_link_libraries(unittest_joint_limits_aggregator
-# ${catkin_LIBRARIES}
-# ${PROJECT_NAME}_lib
-# )
+ament_add_gtest_executable(unittest_joint_limits_aggregator src/unittest_joint_limits_aggregator.cpp)
+target_link_libraries(unittest_joint_limits_aggregator ${PROJECT_NAME}_lib)
+add_ros_test(launch/unittest_joint_limits_aggregator.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
# JointLimitsContainer Unit Test
ament_add_gtest(unittest_joint_limits_container
@@ -139,29 +132,29 @@ ament_add_gtest_executable(unittest_cartesian_limits_aggregator src/unittest_car
target_link_libraries(unittest_cartesian_limits_aggregator ${PROJECT_NAME}_lib)
add_ros_test(launch/unittest_cartesian_limits_aggregator.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
-# # PlanningContextLoaderPTP Unit Test
-# add_rostest_gtest(unittest_planning_context_loaders
-# test/unittest_planning_context_loaders.test
-# test/unittest_planning_context_loaders.cpp
-# )
-# target_link_libraries(unittest_planning_context_loaders
-# ${catkin_LIBRARIES}
-# ${PROJECT_NAME}_testhelpers
-# )
-#
-# # PlanningContext Unit Test (Typed test)
-# add_rostest_gtest(unittest_planning_context
-# test/unittest_planning_context.test
-# test/unittest_planning_context.cpp
-# src/planning_context_loader_circ.cpp
-# src/planning_context_loader_lin.cpp
-# src/planning_context_loader_ptp.cpp
-# )
-# target_link_libraries(unittest_planning_context
-# ${catkin_LIBRARIES}
-# ${PROJECT_NAME}_testhelpers
-# )
-#
+# PlanningContextLoaderPTP Unit Test
+ament_add_gtest_executable(unittest_planning_context_loaders
+ src/unittest_planning_context_loaders.cpp
+)
+target_link_libraries(unittest_planning_context_loaders
+ ${PROJECT_NAME}_lib
+ ${PROJECT_NAME}_testhelpers
+)
+add_ros_test(launch/unittest_planning_context_loaders.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
+
+# PlanningContext Unit Test (Typed test)
+ament_add_gtest_executable(unittest_planning_context
+ src/unittest_planning_context.cpp
+ ${CMAKE_SOURCE_DIR}/src/planning_context_loader_circ.cpp
+ ${CMAKE_SOURCE_DIR}/src/planning_context_loader_lin.cpp
+ ${CMAKE_SOURCE_DIR}/src/planning_context_loader_ptp.cpp
+)
+target_link_libraries(unittest_planning_context
+ ${PROJECT_NAME}_lib
+ ${PROJECT_NAME}_testhelpers
+)
+add_ros_test(launch/unittest_planning_context.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
+
# GetTipFrames Unit Test
ament_add_gmock(unittest_get_solver_tip_frame src/unittest_get_solver_tip_frame.cpp)
target_link_libraries(unittest_get_solver_tip_frame ${PROJECT_NAME}_lib)
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_blender_transition_window.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_blender_transition_window.yaml
new file mode 100644
index 00000000000..cc19c49bb16
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_blender_transition_window.yaml
@@ -0,0 +1,7 @@
+planning_group : "manipulator"
+target_link : "prbt_tcp"
+cartesian_velocity_tolerance : 1.0e-6
+cartesian_angular_velocity_tolerance : 1.0e-6
+joint_velocity_tolerance : 1.0e-6
+joint_acceleration_tolerance : 1.0e-2
+sampling_time : 0.01
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_functions.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_functions.yaml
new file mode 100644
index 00000000000..a642ce09ac2
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_functions.yaml
@@ -0,0 +1,5 @@
+planning_group : "manipulator"
+group_tip_link : "prbt_flange"
+tcp_link : "prbt_tcp"
+ik_fast_link : "prbt_flange"
+random_test_number : 1000
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_circ.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_circ.yaml
new file mode 100644
index 00000000000..25142d4ba49
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_circ.yaml
@@ -0,0 +1,7 @@
+planning_group : "manipulator"
+target_link : "prbt_tcp"
+cartesian_position_tolerance : 1.0e-3
+angular_acc_tolerance : 1.0e-3
+rot_axis_norm_tolerance : 1.0e-6
+acceleration_tolerance : 1.0e-3
+other_tolerance : 1.0e-6
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_common.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_common.yaml
new file mode 100644
index 00000000000..4658d75d5a9
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_common.yaml
@@ -0,0 +1,9 @@
+planning_group : "manipulator"
+target_link : "prbt_flange"
+random_trial_number : 100
+joint_position_tolerance : 1.0e-3
+joint_velocity_tolerance : 0.3
+pose_norm_tolerance : 1.0e-6
+rot_axis_norm_tolerance : 1.0e-6
+other_tolerance : 1.0e-6
+velocity_scaling_factor : 0.1
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_lin.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_lin.yaml
new file mode 100644
index 00000000000..9911c5cb3db
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_lin.yaml
@@ -0,0 +1,10 @@
+testdata_file_name : "test_data/prbt/testdata_sequence.xml" # TODO(sjahr): Decide at which point the absolute path is created
+planning_group : "manipulator"
+target_link_hand_computed_data : "prbt_flange"
+random_trial_number : 100
+joint_position_tolerance : 1.0e-3
+joint_velocity_tolerance : 0.3
+pose_norm_tolerance : 1.0e-6
+rot_axis_norm_tolerance : 1.0e-6
+other_tolerance : 1.0e-5
+velocity_scaling_factor : 0.1
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_ptp.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_ptp.yaml
new file mode 100644
index 00000000000..8d4938d6e60
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_ptp.yaml
@@ -0,0 +1,6 @@
+planning_group : "manipulator"
+target_link : "prbt_tcp"
+joint_position_tolerance : 1.0e-8
+joint_velocity_tolerance : 1.0e-8
+joint_acceleration_tolerance : 1.0e-8
+pose_norm_tolerance : 1.0e-6
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_joint_limits_aggregator.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_joint_limits_aggregator.test.py
new file mode 100644
index 00000000000..9a96ccee09a
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_joint_limits_aggregator.test.py
@@ -0,0 +1,74 @@
+import launch_testing
+import pytest
+import unittest
+from moveit_configs_utils import MoveItConfigsBuilder
+from parameter_builder import ParameterBuilder
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch_testing.util import KeepAliveProc
+
+
+@pytest.mark.rostest
+def generate_test_description():
+
+ # Load the context
+ test_config = (
+ MoveItConfigsBuilder("moveit_resources_prbt")
+ .robot_description(
+ file_path=get_package_share_directory("moveit_resources_prbt_support")
+ + "/urdf/prbt.xacro"
+ )
+ .robot_description_semantic(file_path="config/prbt.srdf.xacro")
+ .robot_description_kinematics(file_path="config/kinematics.yaml")
+ .joint_limits(file_path="config/joint_limits.yaml")
+ .cartesian_limits(file_path="config/cartesian_limits.yaml")
+ .to_moveit_configs()
+ )
+
+ test_param = (
+ ParameterBuilder("pilz_industrial_motion_planner")
+ .yaml("config/unittest_joint_limits_aggregator.yaml")
+ .to_dict()
+ )
+
+ # run test
+ unittest_joint_limits_aggregator = Node(
+ package="pilz_industrial_motion_planner",
+ executable="unittest_joint_limits_aggregator",
+ name="unittest_joint_limits_aggregator",
+ parameters=[
+ test_config.robot_description,
+ test_config.robot_description_semantic,
+ test_config.robot_description_kinematics,
+ test_config.joint_limits,
+ test_config.cartesian_limits,
+ test_param,
+ ],
+ output="screen",
+ )
+ return (
+ LaunchDescription(
+ [
+ unittest_joint_limits_aggregator,
+ KeepAliveProc(),
+ launch_testing.actions.ReadyToTest(),
+ ]
+ ),
+ {"unittest_joint_limits_aggregator": unittest_joint_limits_aggregator},
+ )
+
+
+class TestTerminatingProcessStops(unittest.TestCase):
+ def test_gtest_run_complete(self, proc_info, unittest_joint_limits_aggregator):
+ proc_info.assertWaitForShutdown(
+ process=unittest_joint_limits_aggregator, timeout=4000.0
+ )
+
+
+@launch_testing.post_shutdown_test()
+class TestOutcome(unittest.TestCase):
+ def test_exit_codes(self, proc_info, unittest_joint_limits_aggregator):
+ launch_testing.asserts.assertExitCodes(
+ proc_info, process=unittest_joint_limits_aggregator
+ )
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_pilz_industrial_motion_planner.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_pilz_industrial_motion_planner.test.py
new file mode 100644
index 00000000000..3b62baba769
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_pilz_industrial_motion_planner.test.py
@@ -0,0 +1,75 @@
+import launch_testing
+import pytest
+import unittest
+from moveit_configs_utils import MoveItConfigsBuilder
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch_testing.util import KeepAliveProc
+
+
+@pytest.mark.rostest
+def generate_test_description():
+
+ # Load the context
+ test_config = (
+ MoveItConfigsBuilder("moveit_resources_prbt")
+ .robot_description(
+ file_path=get_package_share_directory("moveit_resources_prbt_support")
+ + "/urdf/prbt.xacro"
+ )
+ .robot_description_semantic(file_path="config/prbt.srdf.xacro")
+ .robot_description_kinematics(file_path="config/kinematics.yaml")
+ .joint_limits(file_path="config/joint_limits.yaml")
+ .cartesian_limits(file_path="config/cartesian_limits.yaml")
+ .to_moveit_configs()
+ )
+
+ planning_plugin = {
+ "planning_plugin": "pilz_industrial_motion_planner::CommandPlanner"
+ }
+
+ # run test
+ unittest_pilz_industrial_motion_planner = Node(
+ package="pilz_industrial_motion_planner",
+ executable="unittest_pilz_industrial_motion_planner",
+ name="unittest_pilz_industrial_motion_planner",
+ parameters=[
+ test_config.robot_description,
+ test_config.robot_description_semantic,
+ test_config.robot_description_kinematics,
+ test_config.joint_limits,
+ test_config.cartesian_limits,
+ planning_plugin,
+ ],
+ output="screen",
+ )
+ return (
+ LaunchDescription(
+ [
+ unittest_pilz_industrial_motion_planner,
+ KeepAliveProc(),
+ launch_testing.actions.ReadyToTest(),
+ ]
+ ),
+ {
+ "unittest_pilz_industrial_motion_planner": unittest_pilz_industrial_motion_planner
+ },
+ )
+
+
+class TestTerminatingProcessStops(unittest.TestCase):
+ def test_gtest_run_complete(
+ self, proc_info, unittest_pilz_industrial_motion_planner
+ ):
+ proc_info.assertWaitForShutdown(
+ process=unittest_pilz_industrial_motion_planner, timeout=4000.0
+ )
+
+
+@launch_testing.post_shutdown_test()
+class TestOutcome(unittest.TestCase):
+ def test_exit_codes(self, proc_info, unittest_pilz_industrial_motion_planner):
+ launch_testing.asserts.assertExitCodes(
+ proc_info, process=unittest_pilz_industrial_motion_planner
+ )
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_planning_context.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_planning_context.test.py
new file mode 100644
index 00000000000..7dfbe3c1073
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_planning_context.test.py
@@ -0,0 +1,73 @@
+import launch_testing
+import pytest
+import unittest
+from moveit_configs_utils import MoveItConfigsBuilder
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch_testing.util import KeepAliveProc
+
+
+@pytest.mark.rostest
+def generate_test_description():
+
+ # Load the context
+ test_config = (
+ MoveItConfigsBuilder("moveit_resources_prbt")
+ .robot_description(
+ file_path=get_package_share_directory("moveit_resources_prbt_support")
+ + "/urdf/prbt.xacro"
+ )
+ .robot_description_semantic(file_path="config/prbt.srdf.xacro")
+ .robot_description_kinematics(file_path="config/kinematics.yaml")
+ .joint_limits(file_path="config/joint_limits.yaml")
+ .cartesian_limits(file_path="config/cartesian_limits.yaml")
+ .to_moveit_configs()
+ )
+
+ test_param = {
+ "planning_group": "manipulator",
+ "target_link": "prbt_tcp",
+ }
+
+ # run test
+ unittest_planning_context = Node(
+ package="pilz_industrial_motion_planner",
+ executable="unittest_planning_context",
+ name="unittest_planning_context",
+ parameters=[
+ test_config.robot_description,
+ test_config.robot_description_semantic,
+ test_config.robot_description_kinematics,
+ test_config.joint_limits,
+ test_config.cartesian_limits,
+ test_param,
+ ],
+ output="screen",
+ )
+
+ return (
+ LaunchDescription(
+ [
+ unittest_planning_context,
+ KeepAliveProc(),
+ launch_testing.actions.ReadyToTest(),
+ ]
+ ),
+ {"unittest_planning_context": unittest_planning_context},
+ )
+
+
+class TestTerminatingProcessStops(unittest.TestCase):
+ def test_gtest_run_complete(self, proc_info, unittest_planning_context):
+ proc_info.assertWaitForShutdown(
+ process=unittest_planning_context, timeout=4000.0
+ )
+
+
+@launch_testing.post_shutdown_test()
+class TestOutcome(unittest.TestCase):
+ def test_exit_codes(self, proc_info, unittest_planning_context):
+ launch_testing.asserts.assertExitCodes(
+ proc_info, process=unittest_planning_context
+ )
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_planning_context_loaders.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_planning_context_loaders.test.py
new file mode 100644
index 00000000000..22ef29e4175
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_planning_context_loaders.test.py
@@ -0,0 +1,67 @@
+import launch_testing
+import pytest
+import unittest
+from moveit_configs_utils import MoveItConfigsBuilder
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch_testing.util import KeepAliveProc
+
+
+@pytest.mark.rostest
+def generate_test_description():
+
+ # Load the context
+ test_config = (
+ MoveItConfigsBuilder("moveit_resources_prbt")
+ .robot_description(
+ file_path=get_package_share_directory("moveit_resources_prbt_support")
+ + "/urdf/prbt.xacro"
+ )
+ .robot_description_semantic(file_path="config/prbt.srdf.xacro")
+ .robot_description_kinematics(file_path="config/kinematics.yaml")
+ .joint_limits(file_path="config/joint_limits.yaml")
+ .cartesian_limits(file_path="config/cartesian_limits.yaml")
+ .to_moveit_configs()
+ )
+
+ # run test
+ unittest_planning_context_loaders = Node(
+ package="pilz_industrial_motion_planner",
+ executable="unittest_planning_context_loaders",
+ name="unittest_planning_context_loaders",
+ parameters=[
+ test_config.robot_description,
+ test_config.robot_description_semantic,
+ test_config.robot_description_kinematics,
+ test_config.joint_limits,
+ test_config.cartesian_limits,
+ planning_plugin,
+ ],
+ output="screen",
+ )
+ return (
+ LaunchDescription(
+ [
+ unittest_planning_context_loaders,
+ KeepAliveProc(),
+ launch_testing.actions.ReadyToTest(),
+ ]
+ ),
+ {"unittest_planning_context_loaders": unittest_planning_context_loaders},
+ )
+
+
+class TestTerminatingProcessStops(unittest.TestCase):
+ def test_gtest_run_complete(self, proc_info, unittest_planning_context_loaders):
+ proc_info.assertWaitForShutdown(
+ process=unittest_planning_context_loaders, timeout=4000.0
+ )
+
+
+@launch_testing.post_shutdown_test()
+class TestOutcome(unittest.TestCase):
+ def test_exit_codes(self, proc_info, unittest_planning_context_loaders):
+ launch_testing.asserts.assertExitCodes(
+ proc_info, process=unittest_planning_context_loaders
+ )
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_blender_transition_window.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_blender_transition_window.test.py
new file mode 100644
index 00000000000..b5dd4a2060a
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_blender_transition_window.test.py
@@ -0,0 +1,86 @@
+import launch_testing
+import pytest
+import unittest
+from moveit_configs_utils import MoveItConfigsBuilder
+from parameter_builder import ParameterBuilder
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch_testing.util import KeepAliveProc
+
+
+@pytest.mark.rostest
+def generate_test_description():
+
+ # Load the context
+ test_config = (
+ MoveItConfigsBuilder("moveit_resources_prbt")
+ .robot_description(
+ file_path=get_package_share_directory("moveit_resources_prbt_support")
+ + "/urdf/prbt.xacro"
+ )
+ .robot_description_semantic(file_path="config/prbt.srdf.xacro")
+ .robot_description_kinematics(file_path="config/kinematics.yaml")
+ .joint_limits(file_path="config/joint_limits.yaml")
+ .cartesian_limits(file_path="config/cartesian_limits.yaml")
+ .to_moveit_configs()
+ )
+
+ test_param = (
+ ParameterBuilder("pilz_industrial_motion_planner")
+ .yaml("config/unittest_trajectory_blender_transition_window.yaml")
+ .to_dict()
+ )
+
+ testdata_file_name = {
+ "testdata_file_name": get_package_share_directory(
+ "pilz_industrial_motion_planner"
+ )
+ + "/test_data/prbt/testdata_sequence.xml"
+ }
+
+ # run test
+ unittest_trajectory_blender_transition_window = Node(
+ package="pilz_industrial_motion_planner",
+ executable="unittest_trajectory_blender_transition_window",
+ name="unittest_trajectory_blender_transition_window",
+ parameters=[
+ test_config.robot_description,
+ test_config.robot_description_semantic,
+ test_config.robot_description_kinematics,
+ test_config.joint_limits,
+ test_config.cartesian_limits,
+ test_param,
+ testdata_file_name,
+ ],
+ output="screen",
+ )
+ return (
+ LaunchDescription(
+ [
+ unittest_trajectory_blender_transition_window,
+ KeepAliveProc(),
+ launch_testing.actions.ReadyToTest(),
+ ]
+ ),
+ {
+ "unittest_trajectory_blender_transition_window": unittest_trajectory_blender_transition_window
+ },
+ )
+
+
+class TestTerminatingProcessStops(unittest.TestCase):
+ def test_gtest_run_complete(
+ self, proc_info, unittest_trajectory_blender_transition_window
+ ):
+ proc_info.assertWaitForShutdown(
+ process=unittest_trajectory_blender_transition_window, timeout=4000.0
+ )
+
+
+@launch_testing.post_shutdown_test()
+class TestOutcome(unittest.TestCase):
+ def test_exit_codes(self, proc_info, unittest_trajectory_blender_transition_window):
+ launch_testing.asserts.assertExitCodes(
+ proc_info, process=unittest_trajectory_blender_transition_window
+ )
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_functions.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_functions.test.py
new file mode 100644
index 00000000000..9bdd0cd27cd
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_functions.test.py
@@ -0,0 +1,74 @@
+import launch_testing
+import pytest
+import unittest
+from moveit_configs_utils import MoveItConfigsBuilder
+from parameter_builder import ParameterBuilder
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch_testing.util import KeepAliveProc
+
+
+@pytest.mark.rostest
+def generate_test_description():
+
+ # Load the context
+ test_config = (
+ MoveItConfigsBuilder("moveit_resources_prbt")
+ .robot_description(
+ file_path=get_package_share_directory("moveit_resources_prbt_support")
+ + "/urdf/prbt.xacro"
+ )
+ .robot_description_semantic(file_path="config/prbt.srdf.xacro")
+ .robot_description_kinematics(file_path="config/kinematics.yaml")
+ .joint_limits(file_path="config/joint_limits.yaml")
+ .cartesian_limits(file_path="config/cartesian_limits.yaml")
+ .to_moveit_configs()
+ )
+
+ test_param = (
+ ParameterBuilder("pilz_industrial_motion_planner")
+ .yaml("config/unittest_trajectory_functions.yaml")
+ .to_dict()
+ )
+
+ # run test
+ unittest_trajectory_functions = Node(
+ package="pilz_industrial_motion_planner",
+ executable="unittest_trajectory_functions",
+ name="unittest_trajectory_functions",
+ parameters=[
+ test_config.robot_description,
+ test_config.robot_description_semantic,
+ test_config.robot_description_kinematics,
+ test_config.joint_limits,
+ test_config.cartesian_limits,
+ test_param,
+ ],
+ output="screen",
+ )
+ return (
+ LaunchDescription(
+ [
+ unittest_trajectory_functions,
+ KeepAliveProc(),
+ launch_testing.actions.ReadyToTest(),
+ ]
+ ),
+ {"unittest_trajectory_functions": unittest_trajectory_functions},
+ )
+
+
+class TestTerminatingProcessStops(unittest.TestCase):
+ def test_gtest_run_complete(self, proc_info, unittest_trajectory_functions):
+ proc_info.assertWaitForShutdown(
+ process=unittest_trajectory_functions, timeout=4000.0
+ )
+
+
+@launch_testing.post_shutdown_test()
+class TestOutcome(unittest.TestCase):
+ def test_exit_codes(self, proc_info, unittest_trajectory_functions):
+ launch_testing.asserts.assertExitCodes(
+ proc_info, process=unittest_trajectory_functions
+ )
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_circ.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_circ.test.py
new file mode 100644
index 00000000000..e2e885df6d2
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_circ.test.py
@@ -0,0 +1,82 @@
+import launch_testing
+import pytest
+import unittest
+from moveit_configs_utils import MoveItConfigsBuilder
+from parameter_builder import ParameterBuilder
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch_testing.util import KeepAliveProc
+
+
+@pytest.mark.rostest
+def generate_test_description():
+
+ # Load the context
+ test_config = (
+ MoveItConfigsBuilder("moveit_resources_prbt")
+ .robot_description(
+ file_path=get_package_share_directory("moveit_resources_prbt_support")
+ + "/urdf/prbt.xacro"
+ )
+ .robot_description_semantic(file_path="config/prbt.srdf.xacro")
+ .robot_description_kinematics(file_path="config/kinematics.yaml")
+ .joint_limits(file_path="config/joint_limits.yaml")
+ .cartesian_limits(file_path="config/cartesian_limits.yaml")
+ .to_moveit_configs()
+ )
+
+ test_param = (
+ ParameterBuilder("pilz_industrial_motion_planner")
+ .yaml("config/unittest_trajectory_generator_circ.yaml")
+ .to_dict()
+ )
+
+ testdata_file_name = {
+ "testdata_file_name": get_package_share_directory(
+ "pilz_industrial_motion_planner"
+ )
+ + "/test_data/prbt/testdata_sequence.xml"
+ }
+
+ # run test
+ unittest_trajectory_generator_circ = Node(
+ package="pilz_industrial_motion_planner",
+ executable="unittest_trajectory_generator_circ",
+ name="unittest_trajectory_generator_circ",
+ parameters=[
+ test_config.robot_description,
+ test_config.robot_description_semantic,
+ test_config.robot_description_kinematics,
+ test_config.joint_limits,
+ test_config.cartesian_limits,
+ test_param,
+ testdata_file_name,
+ ],
+ output="screen",
+ )
+ return (
+ LaunchDescription(
+ [
+ unittest_trajectory_generator_circ,
+ KeepAliveProc(),
+ launch_testing.actions.ReadyToTest(),
+ ]
+ ),
+ {"unittest_trajectory_generator_circ": unittest_trajectory_generator_circ},
+ )
+
+
+class TestTerminatingProcessStops(unittest.TestCase):
+ def test_gtest_run_complete(self, proc_info, unittest_trajectory_generator_circ):
+ proc_info.assertWaitForShutdown(
+ process=unittest_trajectory_generator_circ, timeout=4000.0
+ )
+
+
+@launch_testing.post_shutdown_test()
+class TestOutcome(unittest.TestCase):
+ def test_exit_codes(self, proc_info, unittest_trajectory_generator_circ):
+ launch_testing.asserts.assertExitCodes(
+ proc_info, process=unittest_trajectory_generator_circ
+ )
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_common.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_common.test.py
new file mode 100644
index 00000000000..c2ffdca76d4
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_common.test.py
@@ -0,0 +1,74 @@
+import launch_testing
+import pytest
+import unittest
+from moveit_configs_utils import MoveItConfigsBuilder
+from parameter_builder import ParameterBuilder
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch_testing.util import KeepAliveProc
+
+
+@pytest.mark.rostest
+def generate_test_description():
+
+ # Load the context
+ test_config = (
+ MoveItConfigsBuilder("moveit_resources_prbt")
+ .robot_description(
+ file_path=get_package_share_directory("moveit_resources_prbt_support")
+ + "/urdf/prbt.xacro"
+ )
+ .robot_description_semantic(file_path="config/prbt.srdf.xacro")
+ .robot_description_kinematics(file_path="config/kinematics.yaml")
+ .joint_limits(file_path="config/joint_limits.yaml")
+ .cartesian_limits(file_path="config/cartesian_limits.yaml")
+ .to_moveit_configs()
+ )
+
+ test_param = (
+ ParameterBuilder("pilz_industrial_motion_planner")
+ .yaml("config/unittest_trajectory_generator_common.yaml")
+ .to_dict()
+ )
+
+ # run test
+ unittest_trajectory_generator_common = Node(
+ package="pilz_industrial_motion_planner",
+ executable="unittest_trajectory_generator_common",
+ name="unittest_trajectory_generator_common",
+ parameters=[
+ test_config.robot_description,
+ test_config.robot_description_semantic,
+ test_config.robot_description_kinematics,
+ test_config.joint_limits,
+ test_config.cartesian_limits,
+ test_param,
+ ],
+ output="screen",
+ )
+ return (
+ LaunchDescription(
+ [
+ unittest_trajectory_generator_common,
+ KeepAliveProc(),
+ launch_testing.actions.ReadyToTest(),
+ ]
+ ),
+ {"unittest_trajectory_generator_common": unittest_trajectory_generator_common},
+ )
+
+
+class TestTerminatingProcessStops(unittest.TestCase):
+ def test_gtest_run_complete(self, proc_info, unittest_trajectory_generator_common):
+ proc_info.assertWaitForShutdown(
+ process=unittest_trajectory_generator_common, timeout=4000.0
+ )
+
+
+@launch_testing.post_shutdown_test()
+class TestOutcome(unittest.TestCase):
+ def test_exit_codes(self, proc_info, unittest_trajectory_generator_common):
+ launch_testing.asserts.assertExitCodes(
+ proc_info, process=unittest_trajectory_generator_common
+ )
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_lin.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_lin.test.py
new file mode 100644
index 00000000000..a2840328b9c
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_lin.test.py
@@ -0,0 +1,82 @@
+import launch_testing
+import pytest
+import unittest
+from moveit_configs_utils import MoveItConfigsBuilder
+from parameter_builder import ParameterBuilder
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch_testing.util import KeepAliveProc
+
+
+@pytest.mark.rostest
+def generate_test_description():
+
+ # Load the context
+ test_config = (
+ MoveItConfigsBuilder("moveit_resources_prbt")
+ .robot_description(
+ file_path=get_package_share_directory("moveit_resources_prbt_support")
+ + "/urdf/prbt.xacro"
+ )
+ .robot_description_semantic(file_path="config/prbt.srdf.xacro")
+ .robot_description_kinematics(file_path="config/kinematics.yaml")
+ .joint_limits(file_path="config/joint_limits.yaml")
+ .cartesian_limits(file_path="config/cartesian_limits.yaml")
+ .to_moveit_configs()
+ )
+
+ test_param = (
+ ParameterBuilder("pilz_industrial_motion_planner")
+ .yaml("config/unittest_trajectory_generator_lin.yaml")
+ .to_dict()
+ )
+
+ testdata_file_name = {
+ "testdata_file_name": get_package_share_directory(
+ "pilz_industrial_motion_planner"
+ )
+ + "/test_data/prbt/testdata_sequence.xml"
+ }
+
+ # run test
+ unittest_trajectory_generator_lin = Node(
+ package="pilz_industrial_motion_planner",
+ executable="unittest_trajectory_generator_lin",
+ name="unittest_trajectory_generator_lin",
+ parameters=[
+ test_config.robot_description,
+ test_config.robot_description_semantic,
+ test_config.robot_description_kinematics,
+ test_config.joint_limits,
+ test_config.cartesian_limits,
+ test_param,
+ testdata_file_name,
+ ],
+ output="screen",
+ )
+ return (
+ LaunchDescription(
+ [
+ unittest_trajectory_generator_lin,
+ KeepAliveProc(),
+ launch_testing.actions.ReadyToTest(),
+ ]
+ ),
+ {"unittest_trajectory_generator_lin": unittest_trajectory_generator_lin},
+ )
+
+
+class TestTerminatingProcessStops(unittest.TestCase):
+ def test_gtest_run_complete(self, proc_info, unittest_trajectory_generator_lin):
+ proc_info.assertWaitForShutdown(
+ process=unittest_trajectory_generator_lin, timeout=4000.0
+ )
+
+
+@launch_testing.post_shutdown_test()
+class TestOutcome(unittest.TestCase):
+ def test_exit_codes(self, proc_info, unittest_trajectory_generator_lin):
+ launch_testing.asserts.assertExitCodes(
+ proc_info, process=unittest_trajectory_generator_lin
+ )
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_ptp.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_ptp.test.py
new file mode 100644
index 00000000000..ee9853a3858
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_ptp.test.py
@@ -0,0 +1,74 @@
+import launch_testing
+import pytest
+import unittest
+from moveit_configs_utils import MoveItConfigsBuilder
+from parameter_builder import ParameterBuilder
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch_testing.util import KeepAliveProc
+
+
+@pytest.mark.rostest
+def generate_test_description():
+
+ # Load the context
+ test_config = (
+ MoveItConfigsBuilder("moveit_resources_prbt")
+ .robot_description(
+ file_path=get_package_share_directory("moveit_resources_prbt_support")
+ + "/urdf/prbt.xacro"
+ )
+ .robot_description_semantic(file_path="config/prbt.srdf.xacro")
+ .robot_description_kinematics(file_path="config/kinematics.yaml")
+ .joint_limits(file_path="config/joint_limits.yaml")
+ .cartesian_limits(file_path="config/cartesian_limits.yaml")
+ .to_moveit_configs()
+ )
+
+ test_param = (
+ ParameterBuilder("pilz_industrial_motion_planner")
+ .yaml("config/unittest_trajectory_generator_ptp.yaml")
+ .to_dict()
+ )
+
+ # run test
+ unittest_trajectory_generator_ptp = Node(
+ package="pilz_industrial_motion_planner",
+ executable="unittest_trajectory_generator_ptp",
+ name="unittest_trajectory_generator_ptp",
+ parameters=[
+ test_config.robot_description,
+ test_config.robot_description_semantic,
+ test_config.robot_description_kinematics,
+ test_config.joint_limits,
+ test_config.cartesian_limits,
+ test_param,
+ ],
+ output="screen",
+ )
+ return (
+ LaunchDescription(
+ [
+ unittest_trajectory_generator_ptp,
+ KeepAliveProc(),
+ launch_testing.actions.ReadyToTest(),
+ ]
+ ),
+ {"unittest_trajectory_generator_ptp": unittest_trajectory_generator_ptp},
+ )
+
+
+class TestTerminatingProcessStops(unittest.TestCase):
+ def test_gtest_run_complete(self, proc_info, unittest_trajectory_generator_ptp):
+ proc_info.assertWaitForShutdown(
+ process=unittest_trajectory_generator_ptp, timeout=4000.0
+ )
+
+
+@launch_testing.post_shutdown_test()
+class TestOutcome(unittest.TestCase):
+ def test_exit_codes(self, proc_info, unittest_trajectory_generator_ptp):
+ launch_testing.asserts.assertExitCodes(
+ proc_info, process=unittest_trajectory_generator_ptp
+ )
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_aggregator.cpp
index 26294d257ac..d9ae93e7796 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_aggregator.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_aggregator.cpp
@@ -41,6 +41,10 @@
#include "pilz_industrial_motion_planner/joint_limits_extension.h"
#include "pilz_industrial_motion_planner/joint_limits_interface_extension.h"
+#include "rclcpp/rclcpp.hpp"
+
+static const std::string PARAM_NAMESPACE_LIMITS = "robot_description_planning";
+
/**
* @brief Unittest of the JointLimitsAggregator class
*/
@@ -49,21 +53,20 @@ class JointLimitsAggregator : public ::testing::Test
protected:
void SetUp() override
{
- ros::NodeHandle node_handle("~");
-
- // Load robot module
- robot_model_loader::RobotModelLoader::Options opt("robot_description");
- model_loader_.reset(new robot_model_loader::RobotModelLoader(opt));
- robot_model_ = model_loader_->getModel();
-
- return;
+ rclcpp::NodeOptions node_options;
+ node_options.automatically_declare_parameters_from_overrides(true);
+ node_ = rclcpp::Node::make_shared("unittest_joint_limits_aggregator", node_options);
+
+ // load robot model
+ rdf_loader::RDFLoader rdf_loader(node_, "robot_description");
+ moveit::core::RobotModelConstPtr robot_model_ =
+ std::make_shared(rdf_loader.getURDF(), rdf_loader.getSRDF());
+ ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model";
}
- /// The robot model loader
- robot_model_loader::RobotModelLoaderPtr model_loader_;
-
- /// The robot model
- robot_model::RobotModelConstPtr robot_model_;
+protected:
+ rclcpp::Node::SharedPtr node_;
+ moveit::core::RobotModelConstPtr robot_model_;
};
/**
@@ -72,10 +75,8 @@ class JointLimitsAggregator : public ::testing::Test
*/
TEST_F(JointLimitsAggregator, ExpectedMapSize)
{
- ros::NodeHandle nh("~");
-
pilz_industrial_motion_planner::JointLimitsContainer container =
- pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(nh,
+ pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(node_, PARAM_NAMESPACE_LIMITS,
robot_model_->getActiveJointModels());
EXPECT_EQ(robot_model_->getActiveJointModels().size(), container.getCount());
@@ -87,10 +88,8 @@ TEST_F(JointLimitsAggregator, ExpectedMapSize)
*/
TEST_F(JointLimitsAggregator, CorrectOverwriteByParamterPosition)
{
- ros::NodeHandle nh("~/valid_1");
-
pilz_industrial_motion_planner::JointLimitsContainer container =
- pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(nh,
+ pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(node_, PARAM_NAMESPACE_LIMITS,
robot_model_->getActiveJointModels());
for (std::pair lim : container)
@@ -118,10 +117,8 @@ TEST_F(JointLimitsAggregator, CorrectOverwriteByParamterPosition)
*/
TEST_F(JointLimitsAggregator, CorrectOverwriteByParamterVelocity)
{
- ros::NodeHandle nh("~/valid_1");
-
pilz_industrial_motion_planner::JointLimitsContainer container =
- pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(nh,
+ pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(node_, PARAM_NAMESPACE_LIMITS,
robot_model_->getActiveJointModels());
for (std::pair lim : container)
@@ -144,10 +141,8 @@ TEST_F(JointLimitsAggregator, CorrectOverwriteByParamterVelocity)
*/
TEST_F(JointLimitsAggregator, CorrectSettingAccelerationAndDeceleration)
{
- ros::NodeHandle nh("~/valid_1");
-
pilz_industrial_motion_planner::JointLimitsContainer container =
- pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(nh,
+ pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(node_, PARAM_NAMESPACE_LIMITS,
robot_model_->getActiveJointModels());
for (std::pair lim : container)
@@ -175,16 +170,12 @@ TEST_F(JointLimitsAggregator, CorrectSettingAccelerationAndDeceleration)
*/
TEST_F(JointLimitsAggregator, LimitsViolationPosition)
{
- ros::NodeHandle nh_min("~/violate_position_min");
-
EXPECT_THROW(pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(
- nh_min, robot_model_->getActiveJointModels()),
+ node_, PARAM_NAMESPACE_LIMITS, robot_model_->getActiveJointModels()),
pilz_industrial_motion_planner::AggregationBoundsViolationException);
- ros::NodeHandle nh_max("~/violate_position_max");
-
EXPECT_THROW(pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(
- nh_max, robot_model_->getActiveJointModels()),
+ node_, PARAM_NAMESPACE_LIMITS, robot_model_->getActiveJointModels()),
pilz_industrial_motion_planner::AggregationBoundsViolationException);
}
@@ -193,15 +184,14 @@ TEST_F(JointLimitsAggregator, LimitsViolationPosition)
*/
TEST_F(JointLimitsAggregator, LimitsViolationVelocity)
{
- ros::NodeHandle nh("~/violate_velocity");
-
EXPECT_THROW(pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(
- nh, robot_model_->getActiveJointModels()),
+ node_, PARAM_NAMESPACE_LIMITS, robot_model_->getActiveJointModels()),
pilz_industrial_motion_planner::AggregationBoundsViolationException);
}
int main(int argc, char** argv)
{
+ rclcpp::init(argc, argv);
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp
index e9f33637842..ee020143bda 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp
@@ -44,16 +44,16 @@
#include "rclcpp/rclcpp.hpp"
-const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" };
-const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" };
-
static const rclcpp::Logger LOGGER = rclcpp::get_logger("unittest_pilz_industrial_motion_planner");
-class CommandPlannerTest : public testing::TestWithParam
+class CommandPlannerTest : public testing::Test
{
protected:
void SetUp() override
{
+ rclcpp::NodeOptions node_options;
+ node_options.automatically_declare_parameters_from_overrides(true);
+ node_ = rclcpp::Node::make_shared("unittest_pilz_industrial_motion_planner", node_options);
createPlannerInstance();
}
@@ -68,17 +68,15 @@ class CommandPlannerTest : public testing::TestWithParam
*/
void createPlannerInstance()
{
- std::cout << "Tres";
- // Load planner name from parameter server
- if (!node_->has_parameter("planning_plugin"))
- {
- RCLCPP_FATAL_STREAM(LOGGER, "Could not find planner plugin name");
- FAIL();
- }
- else {
- node_->get_parameter("planning_plugin", planner_plugin_name_);
- std::cout << "Found: " << planner_plugin_name_.c_str();
- }
+ // load robot model
+ rdf_loader::RDFLoader rdf_loader(node_, "robot_description");
+ moveit::core::RobotModelConstPtr robot_model_ =
+ std::make_shared(rdf_loader.getURDF(), rdf_loader.getSRDF());
+ ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model";
+
+ // Load planner name from node parameters
+ ASSERT_TRUE(node_->has_parameter("planning_plugin")) << "Could not find parameter 'planning_plugin'";
+ node_->get_parameter("planning_plugin", planner_plugin_name_);
// Load the plugin
try
@@ -91,20 +89,18 @@ class CommandPlannerTest : public testing::TestWithParam
RCLCPP_FATAL_STREAM(LOGGER, "Exception while creating planning plugin loader " << ex.what());
FAIL();
}
- std::cout << "Loaded planning plugin";
// Create planner
try
{
planner_instance_.reset(planner_plugin_loader_->createUnmanagedInstance(planner_plugin_name_));
- ASSERT_TRUE(planner_instance_->initialize(robot_model_, node_, "robot_description_planning")) // TODO(sjahr) Namespace correct?
+ ASSERT_TRUE(planner_instance_->initialize(robot_model_, node_, "robot_description_planning"))
<< "Initialzing the planner instance failed.";
}
catch (pluginlib::PluginlibException& ex)
{
FAIL() << "Could not create planner " << ex.what() << "\n";
}
- std::cout << "Created planner";
}
void TearDown() override
@@ -115,37 +111,27 @@ class CommandPlannerTest : public testing::TestWithParam
protected:
// ros stuff
rclcpp::Node::SharedPtr node_;
- moveit::core::RobotModelConstPtr robot_model_{robot_model_loader::RobotModelLoader(node_, GetParam()).getModel() };
+ moveit::core::RobotModelConstPtr robot_model_;
std::string planner_plugin_name_;
-
std::unique_ptr> planner_plugin_loader_;
-
planning_interface::PlannerManagerPtr planner_instance_;
};
-// Instantiate the test cases for robot model with and without gripper
-INSTANTIATE_TEST_SUITE_P(InstantiationName, CommandPlannerTest,
- ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME));
-
/**
* @brief Test that PTP can be loaded
* This needs to be extended with every new planning Algorithm
*/
-TEST_P(CommandPlannerTest, ObtainLoadedPlanningAlgorithms)
+TEST_F(CommandPlannerTest, ObtainLoadedPlanningAlgorithms)
{
- std::cout << "Quatre?";
// Check for the algorithms
std::vector algs;
-
- std::cout << "Check for the algorithms";
planner_instance_->getPlanningAlgorithms(algs);
ASSERT_EQ(3u, algs.size()) << "Found more or less planning algorithms as expected! Found:"
<< ::testing::PrintToString(algs);
// Collect the algorithms, check for uniqueness
std::set algs_set;
- std::cout << "Check for the algorithms";
for (const auto& alg : algs)
{
algs_set.insert(alg);
@@ -160,7 +146,7 @@ TEST_P(CommandPlannerTest, ObtainLoadedPlanningAlgorithms)
* @brief Check that all announced planning algorithms can perform the service
* request if the planner_id is set.
*/
-TEST_P(CommandPlannerTest, CheckValidAlgorithmsForServiceRequest)
+TEST_F(CommandPlannerTest, CheckValidAlgorithmsForServiceRequest)
{
// Check for the algorithms
std::vector algs;
@@ -179,7 +165,7 @@ TEST_P(CommandPlannerTest, CheckValidAlgorithmsForServiceRequest)
* @brief Check that canServiceRequest(req) returns false if planner_id is not
* supported
*/
-TEST_P(CommandPlannerTest, CheckInvalidAlgorithmsForServiceRequest)
+TEST_F(CommandPlannerTest, CheckInvalidAlgorithmsForServiceRequest)
{
planning_interface::MotionPlanRequest req;
req.planner_id = "NON_EXISTEND_ALGORITHM_HASH_da39a3ee5e6b4b0d3255bfef95601890afd80709";
@@ -190,7 +176,7 @@ TEST_P(CommandPlannerTest, CheckInvalidAlgorithmsForServiceRequest)
/**
* @brief Check that canServiceRequest(req) returns false if planner_id is empty
*/
-TEST_P(CommandPlannerTest, CheckEmptyPlannerIdForServiceRequest)
+TEST_F(CommandPlannerTest, CheckEmptyPlannerIdForServiceRequest)
{
planning_interface::MotionPlanRequest req;
req.planner_id = "";
@@ -201,7 +187,7 @@ TEST_P(CommandPlannerTest, CheckEmptyPlannerIdForServiceRequest)
/**
* @brief Check integrety against empty input
*/
-TEST_P(CommandPlannerTest, CheckPlanningContextRequestNull)
+TEST_F(CommandPlannerTest, CheckPlanningContextRequestNull)
{
moveit_msgs::msg::MotionPlanRequest req;
moveit_msgs::msg::MoveItErrorCodes error_code;
@@ -212,7 +198,7 @@ TEST_P(CommandPlannerTest, CheckPlanningContextRequestNull)
* @brief Check that for the announced algorithmns getPlanningContext does not
* return nullptr
*/
-TEST_P(CommandPlannerTest, CheckPlanningContextRequest)
+TEST_F(CommandPlannerTest, CheckPlanningContextRequest)
{
moveit_msgs::msg::MotionPlanRequest req;
moveit_msgs::msg::MoveItErrorCodes error_code;
@@ -232,7 +218,7 @@ TEST_P(CommandPlannerTest, CheckPlanningContextRequest)
/**
* @brief Check the description can be obtained and is not empty
*/
-TEST_P(CommandPlannerTest, CheckPlanningContextDescriptionNotEmptyAndStable)
+TEST_F(CommandPlannerTest, CheckPlanningContextDescriptionNotEmptyAndStable)
{
std::string desc = planner_instance_->getDescription();
EXPECT_GT(desc.length(), 0u);
@@ -240,11 +226,7 @@ TEST_P(CommandPlannerTest, CheckPlanningContextDescriptionNotEmptyAndStable)
int main(int argc, char** argv)
{
- std::cout << "Hello there\n";
- testing::InitGoogleTest(&argc, argv);
rclcpp::init(argc, argv);
- std::cout << "Uno\n";
- int result = RUN_ALL_TESTS();
- std::cout << "D\n";
- return result;
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
}
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp
index a68f2904893..de769f9a49f 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp
@@ -35,8 +35,6 @@
#include
#include
-#include
-
#include
#include
@@ -51,9 +49,6 @@
#include "test_utils.h"
-const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" };
-const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" };
-
// parameters from parameter server
const std::string PARAM_PLANNING_GROUP_NAME("planning_group");
const std::string PARAM_TARGET_LINK_NAME("target_link");
@@ -94,11 +89,23 @@ class PlanningContextTest : public ::testing::Test
protected:
void SetUp() override
{
+ rclcpp::NodeOptions node_options;
+ node_options.automatically_declare_parameters_from_overrides(true);
+ node_ = rclcpp::Node::make_shared("unittest_planning_context", node_options);
+
+ // load robot model
+ rdf_loader::RDFLoader rdf_loader(node_, "robot_description");
+ moveit::core::RobotModelConstPtr robot_model_ =
+ std::make_shared(rdf_loader.getURDF(), rdf_loader.getSRDF());
+
ASSERT_FALSE(robot_model_ == nullptr) << "There is no robot model!";
// get parameters
- ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_));
- ASSERT_TRUE(ph_.getParam(PARAM_TARGET_LINK_NAME, target_link_));
+ ASSERT_TRUE(node_->has_parameter(PARAM_PLANNING_GROUP_NAME)) << "Could not find parameter 'planning_group'";
+ node_->get_parameter(PARAM_PLANNING_GROUP_NAME, planning_group_);
+
+ ASSERT_TRUE(node_->has_parameter(PARAM_TARGET_LINK_NAME)) << "Could not find parameter 'target_link'";
+ node_->get_parameter(PARAM_TARGET_LINK_NAME, target_link_);
pilz_industrial_motion_planner::JointLimitsContainer joint_limits =
testutils::createFakeLimits(robot_model_->getVariableNames());
@@ -117,7 +124,7 @@ class PlanningContextTest : public ::testing::Test
// Define and set the current scene
planning_scene::PlanningScenePtr scene(new planning_scene::PlanningScene(robot_model_));
- robot_state::RobotState current_state(robot_model_);
+ moveit::core::RobotState current_state(robot_model_);
current_state.setToDefaultValues();
current_state.setJointGroupPositions(planning_group_, { 0, 1.57, 1.57, 0, 0.2, 0 });
scene->setCurrentState(current_state);
@@ -138,7 +145,7 @@ class PlanningContextTest : public ::testing::Test
req.max_acceleration_scaling_factor = 0.01;
// start state
- robot_state::RobotState rstate(this->robot_model_);
+ moveit::core::RobotState rstate(this->robot_model_);
rstate.setToDefaultValues();
// state state in joint space, used as initial positions, since IK does not
// work at zero positions
@@ -162,9 +169,9 @@ class PlanningContextTest : public ::testing::Test
// path constraint
req.path_constraints.name = "center";
- moveit_msgs::PositionConstraint center_point;
+ moveit_msgs::msg::PositionConstraint center_point;
center_point.link_name = this->target_link_;
- geometry_msgs::Pose center_position;
+ geometry_msgs::msg::Pose center_position;
center_position.position.x = 0.0;
center_position.position.y = 0.0;
center_position.position.z = 0.65;
@@ -176,11 +183,8 @@ class PlanningContextTest : public ::testing::Test
protected:
// ros stuff
- ros::NodeHandle ph_{ "~" };
- robot_model::RobotModelConstPtr robot_model_{
- robot_model_loader::RobotModelLoader(!T::VALUE ? PARAM_MODEL_NO_GRIPPER_NAME : PARAM_MODEL_WITH_GRIPPER_NAME)
- .getModel()
- };
+ rclcpp::Node::SharedPtr node_;
+ moveit::core::RobotModelConstPtr robot_model_;
std::unique_ptr planning_context_;
@@ -275,8 +279,7 @@ TYPED_TEST(PlanningContextTest, Clear)
int main(int argc, char** argv)
{
- ros::init(argc, argv, "unittest_planning_context");
- // ros::NodeHandle nh;
+ rclcpp::init(argc, argv);
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context_loaders.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context_loaders.cpp
index e0ecdcda0e1..f99b13c5795 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context_loaders.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context_loaders.cpp
@@ -46,8 +46,9 @@
#include "test_utils.h"
-const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" };
-const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" };
+#include "rclcpp/rclcpp.hpp"
+
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("unittest_planning_context_loader");
class PlanningContextLoadersTest : public ::testing::TestWithParam>
{
@@ -60,6 +61,15 @@ class PlanningContextLoadersTest : public ::testing::TestWithParam(rdf_loader.getURDF(), rdf_loader.getSRDF());
+
ASSERT_FALSE(robot_model_ == nullptr) << "There is no robot model!";
// Load the plugin
@@ -71,7 +81,7 @@ class PlanningContextLoadersTest : public ::testing::TestWithParam>
@@ -110,19 +120,12 @@ class PlanningContextLoadersTest : public ::testing::TestWithParam{ "pilz_industrial_motion_planner::PlanningContextLoaderPTP", "PTP",
- PARAM_MODEL_NO_GRIPPER_NAME }, // Test for PTP
- std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderPTP", "PTP",
- PARAM_MODEL_WITH_GRIPPER_NAME }, // Test for PTP
- std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderLIN", "LIN",
- PARAM_MODEL_NO_GRIPPER_NAME }, // Test for LIN
- std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderLIN", "LIN",
- PARAM_MODEL_WITH_GRIPPER_NAME }, // Test for LIN
- std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderCIRC", "CIRC",
- PARAM_MODEL_NO_GRIPPER_NAME }, // Test for CIRC
- std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderCIRC", "CIRC",
- PARAM_MODEL_WITH_GRIPPER_NAME } // Test for CIRC
- ));
+ ::testing::Values(
+ std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderPTP", "PTP" }, // Test for PTP
+ std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderLIN", "LIN" }, // Test for LIN
+ std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderCIRC", "CIRC" }
+ // Test for CIRC
+ ));
/**
* @brief Test getAlgorithm returns PTP
@@ -130,7 +133,7 @@ INSTANTIATE_TEST_SUITE_P(
TEST_P(PlanningContextLoadersTest, GetAlgorithm)
{
std::string alg = planning_context_loader_->getAlgorithm();
- EXPECT_EQ(alg, GetParam().at(1));
+ EXPECT_EQ(alg, GetParam().back());
}
/**
@@ -173,7 +176,7 @@ TEST_P(PlanningContextLoadersTest, LoadContext)
int main(int argc, char** argv)
{
- ros::init(argc, argv, "unittest_planning_context_loaders");
+ rclcpp::init(argc, argv);
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp
index 7b011c44c3d..9b948bd70a5 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp
@@ -40,7 +40,6 @@
#include
#include
#include
-#include
#if __has_include()
#include
#else
@@ -63,41 +62,105 @@
#include "pilz_industrial_motion_planner/trajectory_generator_lin.h"
#include "test_utils.h"
-const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" };
-const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" };
-
-// parameters from node parameters
-const std::string PARAM_PLANNING_GROUP_NAME("planning_group");
-const std::string PARAM_TARGET_LINK_NAME("target_link");
-const std::string CARTESIAN_VELOCITY_TOLERANCE("cartesian_velocity_tolerance");
-const std::string CARTESIAN_ANGULAR_VELOCITY_TOLERANCE("cartesian_angular_velocity_tolerance");
-const std::string JOINT_VELOCITY_TOLERANCE("joint_velocity_tolerance");
-const std::string JOINT_ACCELERATION_TOLERANCE("joint_acceleration_tolerance");
-const std::string OTHER_TOLERANCE("other_tolerance");
-const std::string SAMPLING_TIME("sampling_time");
-const std::string TEST_DATA_FILE_NAME("testdata_file_name");
+#include "rclcpp/rclcpp.hpp"
using namespace pilz_industrial_motion_planner;
using namespace pilz_industrial_motion_planner_testutils;
-class TrajectoryBlenderTransitionWindowTest : public testing::TestWithParam
+static const std::string PARAM_NAMESPACE_LIMITS = "robot_description_planning";
+class TrajectoryBlenderTransitionWindowTest : public testing::Test
{
protected:
/**
* @brief Create test scenario for trajectory blender
*
*/
- void SetUp() override;
+ void SetUp() override
+ {
+ rclcpp::NodeOptions node_options;
+ node_options.automatically_declare_parameters_from_overrides(true);
+ node_ = rclcpp::Node::make_shared("unittest_trajectory_blender_transition_window", node_options);
+
+ // load robot model
+ rdf_loader::RDFLoader rdf_loader(node_, "robot_description");
+ moveit::core::RobotModelConstPtr robot_model_ =
+ std::make_shared(rdf_loader.getURDF(), rdf_loader.getSRDF());
+ ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model";
+
+ // get parameters
+ ASSERT_TRUE(node_->has_parameter("planning_group"));
+ node_->get_parameter("planning_group", planning_group_);
+ ASSERT_TRUE(node_->has_parameter("target_link"));
+ node_->get_parameter("target_link", target_link_);
+ ASSERT_TRUE(node_->has_parameter("cartesian_velocity_tolerance"));
+ node_->get_parameter("cartesian_velocity_tolerance", cartesian_velocity_tolerance_);
+ ASSERT_TRUE(node_->has_parameter("cartesian_angular_velocity_tolerance"));
+ node_->get_parameter("cartesian_angular_velocity_tolerance", cartesian_angular_velocity_tolerance_);
+ ASSERT_TRUE(node_->has_parameter("joint_velocity_tolerance"));
+ node_->get_parameter("joint_velocity_tolerance", joint_velocity_tolerance_);
+ ASSERT_TRUE(node_->has_parameter("joint_acceleration_tolerance"));
+ node_->get_parameter("joint_acceleration_tolerance", joint_acceleration_tolerance_);
+ ASSERT_TRUE(node_->has_parameter("sampling_time"));
+ node_->get_parameter("sampling_time", sampling_time_);
+ ASSERT_TRUE(node_->has_parameter("testdata_file_name"));
+ node_->get_parameter("testdata_file_name", test_data_file_name_);
+
+ // load the test data provider
+ data_loader_.reset(new XmlTestdataLoader(test_data_file_name_, robot_model_));
+ ASSERT_NE(nullptr, data_loader_) << "Failed to load test data by provider.";
+
+ // check robot model
+ testutils::checkRobotModel(robot_model_, planning_group_, target_link_);
+
+ // create the limits container
+ pilz_industrial_motion_planner::JointLimitsContainer joint_limits =
+ pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(
+ node_, PARAM_NAMESPACE_LIMITS, robot_model_->getActiveJointModels());
+ CartesianLimit cart_limits;
+ cart_limits.setMaxRotationalVelocity(1 * M_PI);
+ cart_limits.setMaxTranslationalAcceleration(2);
+ cart_limits.setMaxTranslationalDeceleration(2);
+ cart_limits.setMaxTranslationalVelocity(1);
+ planner_limits_.setJointLimits(joint_limits);
+ planner_limits_.setCartesianLimits(cart_limits);
+
+ // initialize trajectory generators and blender
+ lin_generator_.reset(new TrajectoryGeneratorLIN(robot_model_, planner_limits_));
+ ASSERT_NE(nullptr, lin_generator_) << "failed to create LIN trajectory generator";
+ blender_.reset(new TrajectoryBlenderTransitionWindow(planner_limits_));
+ ASSERT_NE(nullptr, blender_) << "failed to create trajectory blender";
+ }
/**
* @brief Generate lin trajectories for blend sequences
*/
- std::vector generateLinTrajs(const Sequence& seq, size_t num_cmds);
+ std::vector generateLinTrajs(const Sequence& seq, size_t num_cmds)
+ {
+ std::vector responses(num_cmds);
+ for (size_t index = 0; index < num_cmds; ++index)
+ {
+ planning_interface::MotionPlanRequest req{ seq.getCmd(index).toRequest() };
+ // Set start state of request to end state of previous trajectory (except
+ // for first)
+ if (index > 0)
+ {
+ moveit::core::robotStateToRobotStateMsg(responses[index - 1].trajectory_->getLastWayPoint(), req.start_state);
+ }
+ // generate trajectory
+ planning_interface::MotionPlanResponse resp;
+ if (!lin_generator_->generate(req, resp, sampling_time_))
+ {
+ std::runtime_error("Failed to generate trajectory.");
+ }
+ responses.at(index) = resp;
+ }
+ return responses;
+ }
protected:
// ros stuff
- ros::NodeHandle ph_{ "~" };
- robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() };
+ rclcpp::Node::SharedPtr node_;
+ moveit::core::RobotModelConstPtr robot_model_;
std::unique_ptr lin_generator_;
std::unique_ptr blender_;
@@ -112,73 +175,6 @@ class TrajectoryBlenderTransitionWindowTest : public testing::TestWithParamgetActiveJointModels());
- CartesianLimit cart_limits;
- cart_limits.setMaxRotationalVelocity(1 * M_PI);
- cart_limits.setMaxTranslationalAcceleration(2);
- cart_limits.setMaxTranslationalDeceleration(2);
- cart_limits.setMaxTranslationalVelocity(1);
- planner_limits_.setJointLimits(joint_limits);
- planner_limits_.setCartesianLimits(cart_limits);
-
- // initialize trajectory generators and blender
- lin_generator_.reset(new TrajectoryGeneratorLIN(robot_model_, planner_limits_));
- ASSERT_NE(nullptr, lin_generator_) << "failed to create LIN trajectory generator";
- blender_.reset(new TrajectoryBlenderTransitionWindow(planner_limits_));
- ASSERT_NE(nullptr, blender_) << "failed to create trajectory blender";
-}
-
-std::vector
-TrajectoryBlenderTransitionWindowTest::generateLinTrajs(const Sequence& seq, size_t num_cmds)
-{
- std::vector responses(num_cmds);
-
- for (size_t index = 0; index < num_cmds; ++index)
- {
- planning_interface::MotionPlanRequest req{ seq.getCmd(index).toRequest() };
- // Set start state of request to end state of previous trajectory (except
- // for first)
- if (index > 0)
- {
- moveit::core::robotStateToRobotStateMsg(responses[index - 1].trajectory_->getLastWayPoint(), req.start_state);
- }
- // generate trajectory
- planning_interface::MotionPlanResponse resp;
- if (!lin_generator_->generate(req, resp, sampling_time_))
- {
- std::runtime_error("Failed to generate trajectory.");
- }
- responses.at(index) = resp;
- }
- return responses;
-}
-
-// Instantiate the test cases for robot model with and without gripper
-INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryBlenderTransitionWindowTest,
- ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME));
-
/**
* @brief Tests the blending of two trajectories with an invalid group name.
*
@@ -190,7 +186,7 @@ INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryBlenderTransitionWindowTes
* 1. Two linear trajectories generated.
* 2. Blending trajectory cannot be generated.
*/
-TEST_P(TrajectoryBlenderTransitionWindowTest, testInvalidGroupName)
+TEST_F(TrajectoryBlenderTransitionWindowTest, testInvalidGroupName)
{
Sequence seq{ data_loader_->getSequence("SimpleSequence") };
@@ -219,7 +215,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testInvalidGroupName)
* 1. Two linear trajectories generated.
* 2. Blending trajectory cannot be generated.
*/
-TEST_P(TrajectoryBlenderTransitionWindowTest, testInvalidTargetLink)
+TEST_F(TrajectoryBlenderTransitionWindowTest, testInvalidTargetLink)
{
Sequence seq{ data_loader_->getSequence("SimpleSequence") };
@@ -249,7 +245,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testInvalidTargetLink)
* 1. Two linear trajectories generated.
* 2. Blending trajectory cannot be generated.
*/
-TEST_P(TrajectoryBlenderTransitionWindowTest, testNegativeRadius)
+TEST_F(TrajectoryBlenderTransitionWindowTest, testNegativeRadius)
{
Sequence seq{ data_loader_->getSequence("SimpleSequence") };
@@ -278,7 +274,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNegativeRadius)
* 1. Two linear trajectories generated.
* 2. Blending trajectory cannot be generated.
*/
-TEST_P(TrajectoryBlenderTransitionWindowTest, testZeroRadius)
+TEST_F(TrajectoryBlenderTransitionWindowTest, testZeroRadius)
{
Sequence seq{ data_loader_->getSequence("SimpleSequence") };
@@ -308,7 +304,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testZeroRadius)
* 1. Two linear trajectories generated.
* 2. Blending trajectory cannot be generated.
*/
-TEST_P(TrajectoryBlenderTransitionWindowTest, testDifferentSamplingTimes)
+TEST_F(TrajectoryBlenderTransitionWindowTest, testDifferentSamplingTimes)
{
Sequence seq{ data_loader_->getSequence("SimpleSequence") };
@@ -360,7 +356,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testDifferentSamplingTimes)
* 1. Two linear trajectories generated.
* 2. Blending trajectory cannot be generated.
*/
-TEST_P(TrajectoryBlenderTransitionWindowTest, testNonUniformSamplingTime)
+TEST_F(TrajectoryBlenderTransitionWindowTest, testNonUniformSamplingTime)
{
Sequence seq{ data_loader_->getSequence("SimpleSequence") };
@@ -394,7 +390,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNonUniformSamplingTime)
* 2. Two trajectories that do not intersect.
* 2. Blending trajectory cannot be generated.
*/
-TEST_P(TrajectoryBlenderTransitionWindowTest, testNotIntersectingTrajectories)
+TEST_F(TrajectoryBlenderTransitionWindowTest, testNotIntersectingTrajectories)
{
Sequence seq{ data_loader_->getSequence("SimpleSequence") };
@@ -425,7 +421,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNotIntersectingTrajectories)
* 1. Two trajectories generated.
* 2. Blending trajectory cannot be generated.
*/
-TEST_P(TrajectoryBlenderTransitionWindowTest, testNonStationaryPoint)
+TEST_F(TrajectoryBlenderTransitionWindowTest, testNonStationaryPoint)
{
Sequence seq{ data_loader_->getSequence("SimpleSequence") };
@@ -462,7 +458,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNonStationaryPoint)
* 1. Two trajectories generated.
* 2. Blending trajectory cannot be generated.
*/
-TEST_P(TrajectoryBlenderTransitionWindowTest, testTraj1InsideBlendRadius)
+TEST_F(TrajectoryBlenderTransitionWindowTest, testTraj1InsideBlendRadius)
{
Sequence seq{ data_loader_->getSequence("SimpleSequence") };
@@ -499,7 +495,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testTraj1InsideBlendRadius)
* 1. Two trajectories generated.
* 2. Blending trajectory cannot be generated.
*/
-TEST_P(TrajectoryBlenderTransitionWindowTest, testTraj2InsideBlendRadius)
+TEST_F(TrajectoryBlenderTransitionWindowTest, testTraj2InsideBlendRadius)
{
Sequence seq{ data_loader_->getSequence("NoIntersectionTraj2") };
@@ -536,7 +532,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testTraj2InsideBlendRadius)
* 3. No bound is violated, the trajectories are continuous
* in joint and cartesian space.
*/
-TEST_P(TrajectoryBlenderTransitionWindowTest, testLinLinBlending)
+TEST_F(TrajectoryBlenderTransitionWindowTest, testLinLinBlending)
{
Sequence seq{ data_loader_->getSequence("SimpleSequence") };
@@ -579,7 +575,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testLinLinBlending)
* 3. No bound is violated, the trajectories are continuous
* in joint and cartesian space.
*/
-TEST_P(TrajectoryBlenderTransitionWindowTest, testOverlappingBlendTrajectories)
+TEST_F(TrajectoryBlenderTransitionWindowTest, testOverlappingBlendTrajectories)
{
Sequence seq{ data_loader_->getSequence("SimpleSequence") };
// Set goal of second traj to start of first traj.
@@ -625,7 +621,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testOverlappingBlendTrajectories)
* 4. No bound is violated, the trajectories are continuous
* in joint and cartesian space.
*/
-TEST_P(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending)
+TEST_F(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending)
{
const double sine_scaling_factor{ 0.01 };
const double time_scaling_factor{ 10 };
@@ -642,7 +638,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending)
auto lin_traj{ res.at(traj_index).trajectory_ };
CartesianTrajectory cart_traj;
- trajectory_msgs::JointTrajectory joint_traj;
+ trajectory_msgs::msg::JointTrajectory joint_traj;
const double duration{ lin_traj->getWayPointDurationFromStart(lin_traj->getWayPointCount()) };
// time from start zero does not work
const double time_from_start_offset{ time_scaling_factor * lin_traj->getWayPointDurations().back() };
@@ -656,7 +652,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending)
// get pose
CartesianTrajectoryPoint waypoint;
const Eigen::Isometry3d eigen_pose{ lin_traj->getWayPointPtr(i)->getFrameTransform(target_link_) };
- geometry_msgs::Pose waypoint_pose = tf2::toMsg(eigen_pose);
+ geometry_msgs::msg::Pose waypoint_pose = tf2::toMsg(eigen_pose);
// add scaled sine function
waypoint_pose.position.x += sine_scaling_factor * sin(sine_arg);
@@ -665,8 +661,8 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending)
// add to trajectory
waypoint.pose = waypoint_pose;
- waypoint.time_from_start =
- ros::Duration(time_from_start_offset + time_scaling_factor * lin_traj->getWayPointDurationFromStart(i));
+ waypoint.time_from_start = rclcpp::Duration::from_nanoseconds(
+ time_from_start_offset + time_scaling_factor * lin_traj->getWayPointDurationFromStart(i));
cart_traj.points.push_back(waypoint);
}
@@ -700,8 +696,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending)
joint_traj.points.back().velocities.assign(joint_traj.points.back().velocities.size(), 0.0);
joint_traj.points.back().accelerations.assign(joint_traj.points.back().accelerations.size(), 0.0);
- // convert trajectory_msgs::JointTrajectory to
- // robot_trajectory::RobotTrajectory
+ // convert trajectory_msgs::JointTrajectory to robot_trajectory::RobotTrajectory
sine_trajs[traj_index] = std::make_shared(robot_model_, planning_group_);
sine_trajs.at(traj_index)->setRobotTrajectoryMsg(lin_traj->getFirstWayPoint(), joint_traj);
}
@@ -725,8 +720,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending)
int main(int argc, char** argv)
{
- ros::init(argc, argv, "unittest_trajectory_blender_transition_window");
- ros::NodeHandle nh;
+ rclcpp::init(argc, argv);
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp
index a8798517dad..94ed0f1a7a0 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp
@@ -38,6 +38,7 @@
#include
#include
#include
+#include
#include
#include
@@ -50,8 +51,6 @@
#include
#include
#include
-#include
-#include
#if __has_include()
#include
#else
@@ -78,9 +77,6 @@ static constexpr double L1{ 0.3500 }; // Height of first connector
static constexpr double L2{ 0.3070 }; // Height of second connector
static constexpr double L3{ 0.0840 }; // Distance last joint to flange
-const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" };
-const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" };
-
// parameters from parameter server
const std::string PARAM_PLANNING_GROUP_NAME("planning_group");
const std::string GROUP_TIP_LINK_NAME("group_tip_link");
@@ -91,14 +87,47 @@ const std::string RANDOM_TEST_NUMBER("random_test_number");
/**
* @brief test fixtures base class
*/
-class TrajectoryFunctionsTestBase : public testing::TestWithParam
+class TrajectoryFunctionsTestBase : public testing::Test
{
protected:
/**
* @brief Create test scenario for trajectory functions
*
*/
- void SetUp() override;
+ void SetUp() override
+ {
+ rclcpp::NodeOptions node_options;
+ node_options.automatically_declare_parameters_from_overrides(true);
+ node_ = rclcpp::Node::make_shared("unittest_trajectory_functions", node_options);
+
+ // load robot model
+ rdf_loader::RDFLoader rdf_loader(node_, "robot_description");
+ moveit::core::RobotModelConstPtr robot_model_ =
+ std::make_shared(rdf_loader.getURDF(), rdf_loader.getSRDF());
+ ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model";
+
+ // get parameters
+ ASSERT_TRUE(node_->has_parameter("planning_group"));
+ node_->get_parameter("planning_group", planning_group_);
+ ASSERT_TRUE(node_->has_parameter("group_tip_link"));
+ node_->get_parameter("group_tip_link", group_tip_link_);
+ ASSERT_TRUE(node_->has_parameter("tcp_link"));
+ node_->get_parameter("tcp_link", tcp_link_);
+ ASSERT_TRUE(node_->has_parameter("ik_fast_link"));
+ node_->get_parameter("ik_fast_link", ik_fast_link_);
+ ASSERT_TRUE(node_->has_parameter("random_test_number"));
+ node_->get_parameter("random_test_number", random_test_number_);
+
+ // check robot model
+ testutils::checkRobotModel(robot_model_, planning_group_, tcp_link_);
+
+ // initialize the zero state configurationg and test joint state
+ joint_names_ = robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames();
+ for (const auto& joint_name : joint_names_)
+ {
+ zero_state_[joint_name] = 0.0;
+ }
+ }
/**
* @brief check if two transformations are close
@@ -111,8 +140,8 @@ class TrajectoryFunctionsTestBase : public testing::TestWithParam
protected:
// ros stuff
- ros::NodeHandle ph_{ "~" };
- robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() };
+ rclcpp::Node::SharedPtr node_;
+ moveit::core::RobotModelConstPtr robot_model_;
// test parameters from parameter server
std::string planning_group_, group_tip_link_, tcp_link_, ik_fast_link_;
@@ -125,26 +154,6 @@ class TrajectoryFunctionsTestBase : public testing::TestWithParam
random_numbers::RandomNumberGenerator rng_{ random_seed_ };
};
-void TrajectoryFunctionsTestBase::SetUp()
-{
- // parameters
- ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_));
- ASSERT_TRUE(ph_.getParam(GROUP_TIP_LINK_NAME, group_tip_link_));
- ASSERT_TRUE(ph_.getParam(ROBOT_TCP_LINK_NAME, tcp_link_));
- ASSERT_TRUE(ph_.getParam(IK_FAST_LINK_NAME, ik_fast_link_));
- ASSERT_TRUE(ph_.getParam(RANDOM_TEST_NUMBER, random_test_number_));
-
- // check robot model
- testutils::checkRobotModel(robot_model_, planning_group_, tcp_link_);
-
- // initialize the zero state configurationg and test joint state
- joint_names_ = robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames();
- for (const auto& joint_name : joint_names_)
- {
- zero_state_[joint_name] = 0.0;
- }
-}
-
bool TrajectoryFunctionsTestBase::tfNear(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2,
const double& epsilon)
{
@@ -171,20 +180,12 @@ class TrajectoryFunctionsTestOnlyGripper : public TrajectoryFunctionsTestBase
{
};
-// Instantiate the test cases for robot model with and without gripper
-INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryFunctionsTestFlangeAndGripper,
- ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME));
-
-// Instantiate the test cases for robot model with a gripper
-INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryFunctionsTestOnlyGripper,
- ::testing::Values(PARAM_MODEL_WITH_GRIPPER_NAME));
-
/**
* @brief Test the forward kinematics function with simple robot poses for robot
* tip link
* using robot model without gripper.
*/
-TEST_P(TrajectoryFunctionsTestFlangeAndGripper, TipLinkFK)
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, TipLinkFK)
{
Eigen::Isometry3d tip_pose;
std::map test_state = zero_state_;
@@ -214,24 +215,24 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, TipLinkFK)
/**
* @brief Test the inverse kinematics directly through ikfast solver
*/
-TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIKSolver)
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKSolver)
{
// Load solver
- const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);
+ const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);
const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance();
// robot state
- robot_state::RobotState rstate(robot_model_);
+ moveit::core::RobotState rstate(robot_model_);
while (random_test_number_ > 0)
{
// sample random robot state
rstate.setToRandomPositions(jmg, rng_);
rstate.update();
- geometry_msgs::Pose pose_expect = tf2::toMsg(rstate.getFrameTransform(ik_fast_link_));
+ geometry_msgs::msg::Pose pose_expect = tf2::toMsg(rstate.getFrameTransform(ik_fast_link_));
// prepare inverse kinematics
- std::vector ik_poses;
+ std::vector ik_poses;
ik_poses.push_back(pose_expect);
std::vector ik_seed, ik_expect, ik_actual;
for (const auto& joint_name : jmg->getActiveJointModelNames())
@@ -269,11 +270,11 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIKSolver)
* @brief Test the inverse kinematics using RobotState class (setFromIK) using
* robot model
*/
-TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotState)
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotState)
{
// robot state
- robot_state::RobotState rstate(robot_model_);
- const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);
+ moveit::core::RobotState rstate(robot_model_);
+ const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);
while (random_test_number_ > 0)
{
@@ -327,13 +328,13 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotState)
* @brief Test the wrapper function to compute inverse kinematics using robot
* model
*/
-TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIK)
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIK)
{
// robot state
- robot_state::RobotState rstate(robot_model_);
+ moveit::core::RobotState rstate(robot_model_);
const std::string frame_id = robot_model_->getModelFrame();
- const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);
+ const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);
while (random_test_number_ > 0)
{
@@ -375,7 +376,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIK)
/**
* @brief Test computePoseIK for invalid group_name
*/
-TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidGroupName)
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidGroupName)
{
const std::string frame_id = robot_model_->getModelFrame();
Eigen::Isometry3d pose_expect;
@@ -391,7 +392,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidGroupNam
/**
* @brief Test computePoseIK for invalid link_name
*/
-TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidLinkName)
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidLinkName)
{
const std::string frame_id = robot_model_->getModelFrame();
Eigen::Isometry3d pose_expect;
@@ -409,7 +410,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidLinkName
*
* Currently only robot_model_->getModelFrame() == frame_id
*/
-TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidFrameId)
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidFrameId)
{
Eigen::Isometry3d pose_expect;
@@ -426,10 +427,10 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidFrameId)
* collision without the check results in a
* valid ik solution.
*/
-TEST_P(TrajectoryFunctionsTestOnlyGripper, testComputePoseIKSelfCollisionForValidPosition)
+TEST_F(TrajectoryFunctionsTestOnlyGripper, testComputePoseIKSelfCollisionForValidPosition)
{
const std::string frame_id = robot_model_->getModelFrame();
- const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);
+ const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);
// create seed
std::vector ik_seed_states = { -0.553, 0.956, 1.758, 0.146, -1.059, 1.247 };
@@ -442,7 +443,7 @@ TEST_P(TrajectoryFunctionsTestOnlyGripper, testComputePoseIKSelfCollisionForVali
}
// create expected pose
- geometry_msgs::Pose pose;
+ geometry_msgs::msg::Pose pose;
pose.position.x = -0.454;
pose.position.y = -0.15;
pose.position.z = 0.431;
@@ -458,7 +459,7 @@ TEST_P(TrajectoryFunctionsTestOnlyGripper, testComputePoseIKSelfCollisionForVali
EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect,
frame_id, ik_seed, ik_actual1, false));
- robot_state::RobotState rstate(robot_model_);
+ moveit::core::RobotState rstate(robot_model_);
planning_scene::PlanningScene rscene(robot_model_);
std::vector ik_state;
@@ -498,13 +499,13 @@ TEST_P(TrajectoryFunctionsTestOnlyGripper, testComputePoseIKSelfCollisionForVali
* @brief Test if self collision is considered by using a pose that always has
* self collision.
*/
-TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKSelfCollisionForInvalidPose)
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKSelfCollisionForInvalidPose)
{
// robot state
- robot_state::RobotState rstate(robot_model_);
+ moveit::core::RobotState rstate(robot_model_);
const std::string frame_id = robot_model_->getModelFrame();
- const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);
+ const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);
// create seed
std::map ik_seed;
@@ -540,7 +541,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKSelfCollisionFo
* Expected Results:
* 1. Function returns 'false'.
*/
-TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsWithSmallDuration)
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsWithSmallDuration)
{
const std::map position_last, velocity_last, position_current;
double duration_last{ 0.0 };
@@ -562,7 +563,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsWithS
* Expected Results:
* 1. Function returns 'false'.
*/
-TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsVelocityViolation)
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsVelocityViolation)
{
const std::string test_joint_name{ "joint" };
@@ -595,7 +596,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsVeloc
* Expected Results:
* 1. Function returns 'false'.
*/
-TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsAccelerationViolation)
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsAccelerationViolation)
{
const std::string test_joint_name{ "joint" };
@@ -638,7 +639,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsAccel
* Expected Results:
* 1. Function returns 'false'.
*/
-TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsDecelerationViolation)
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsDecelerationViolation)
{
const std::string test_joint_name{ "joint" };
@@ -685,7 +686,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsDecel
* Expected Results:
* 1. Function returns 'false'.
*/
-TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testGenerateJointTrajectoryWithInvalidCartesianTrajectory)
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testGenerateJointTrajectoryWithInvalidCartesianTrajectory)
{
// Create random test trajectory
// Note: 'path' is deleted by KDL::Trajectory_Segment
@@ -702,7 +703,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testGenerateJointTrajectoryWithI
std::string group_name{ "invalid_group_name" };
std::map initial_joint_position;
double sampling_time{ 0.1 };
- trajectory_msgs::JointTrajectory joint_trajectory;
+ trajectory_msgs::msg::JointTrajectory joint_trajectory;
moveit_msgs::msg::MoveItErrorCodes error_code;
bool check_self_collision{ false };
@@ -734,7 +735,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testGenerateJointTrajectoryWithI
* Expected Results:
* 1. Function returns 'false'.
*/
-TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeInvalidVectorSize)
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeInvalidVectorSize)
{
robot_trajectory::RobotTrajectoryPtr first_trajectory =
std::make_shared(robot_model_, planning_group_);
@@ -743,7 +744,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTim
double epsilon{ 0.0 };
double sampling_time{ 0.0 };
- robot_state::RobotState rstate(robot_model_);
+ moveit::core::RobotState rstate(robot_model_);
first_trajectory->insertWayPoint(0, rstate, 0.1);
second_trajectory->insertWayPoint(0, rstate, 0.1);
@@ -762,7 +763,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTim
* Expected Results:
* 1. Function returns 'true'.
*/
-TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeCorrectSamplingTime)
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeCorrectSamplingTime)
{
robot_trajectory::RobotTrajectoryPtr first_trajectory =
std::make_shared(robot_model_, planning_group_);
@@ -772,7 +773,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTim
double sampling_time{ 0.0 };
double expected_sampling_time{ 0.1 };
- robot_state::RobotState rstate(robot_model_);
+ moveit::core::RobotState rstate(robot_model_);
first_trajectory->insertWayPoint(0, rstate, expected_sampling_time);
first_trajectory->insertWayPoint(1, rstate, expected_sampling_time);
@@ -796,7 +797,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTim
* Expected Results:
* 1. Function returns 'false'.
*/
-TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeViolateSamplingTime)
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeViolateSamplingTime)
{
robot_trajectory::RobotTrajectoryPtr first_trajectory =
std::make_shared(robot_model_, planning_group_);
@@ -806,7 +807,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTim
double sampling_time{ 0.0 };
double expected_sampling_time{ 0.1 };
- robot_state::RobotState rstate(robot_model_);
+ moveit::core::RobotState rstate(robot_model_);
first_trajectory->insertWayPoint(0, rstate, expected_sampling_time);
first_trajectory->insertWayPoint(1, rstate, expected_sampling_time);
first_trajectory->insertWayPoint(2, rstate, expected_sampling_time);
@@ -835,10 +836,10 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTim
* Expected Results:
* 1. Function returns 'false'.
*/
-TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualPositionUnequal)
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualPositionUnequal)
{
- robot_state::RobotState rstate_1 = robot_state::RobotState(robot_model_);
- robot_state::RobotState rstate_2 = robot_state::RobotState(robot_model_);
+ moveit::core::RobotState rstate_1 = moveit::core::RobotState(robot_model_);
+ moveit::core::RobotState rstate_2 = moveit::core::RobotState(robot_model_);
double default_joint_position[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
rstate_1.setJointGroupPositions(planning_group_, default_joint_position);
@@ -861,10 +862,10 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualPositionUne
* Expected Results:
* 1. Function returns 'false'.
*/
-TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualVelocityUnequal)
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualVelocityUnequal)
{
- robot_state::RobotState rstate_1 = robot_state::RobotState(robot_model_);
- robot_state::RobotState rstate_2 = robot_state::RobotState(robot_model_);
+ moveit::core::RobotState rstate_1 = moveit::core::RobotState(robot_model_);
+ moveit::core::RobotState rstate_2 = moveit::core::RobotState(robot_model_);
// Ensure that the joint positions of both robot state are equal
double default_joint_position[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
@@ -892,10 +893,10 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualVelocityUne
* Expected Results:
* 1. Function returns 'false'.
*/
-TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualAccelerationUnequal)
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualAccelerationUnequal)
{
- robot_state::RobotState rstate_1 = robot_state::RobotState(robot_model_);
- robot_state::RobotState rstate_2 = robot_state::RobotState(robot_model_);
+ moveit::core::RobotState rstate_1 = moveit::core::RobotState(robot_model_);
+ moveit::core::RobotState rstate_2 = moveit::core::RobotState(robot_model_);
// Ensure that the joint positions of both robot state are equal
double default_joint_position[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
@@ -928,9 +929,9 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualAcceleratio
* Expected Results:
* 1. Function returns 'false'.
*/
-TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryVelocityUnequal)
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryVelocityUnequal)
{
- robot_state::RobotState rstate_1 = robot_state::RobotState(robot_model_);
+ moveit::core::RobotState rstate_1 = moveit::core::RobotState(robot_model_);
// Ensure that the joint velocities are NOT zero
double default_joint_velocity[6] = { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
@@ -951,9 +952,9 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryVeloci
* Expected Results:
* 1. Function returns 'false'.
*/
-TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryAccelerationUnequal)
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryAccelerationUnequal)
{
- robot_state::RobotState rstate_1 = robot_state::RobotState(robot_model_);
+ moveit::core::RobotState rstate_1 = moveit::core::RobotState(robot_model_);
// Ensure that the joint velocities are zero
double default_joint_velocity[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
@@ -969,8 +970,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryAccele
int main(int argc, char** argv)
{
- ros::init(argc, argv, "unittest_trajectory_functions");
- ros::NodeHandle nh;
+ rclcpp::init(argc, argv);
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp
index 9c770b3e9b7..c8ac455a099 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp
@@ -57,48 +57,150 @@
#include "pilz_industrial_motion_planner_testutils/xml_testdata_loader.h"
#include "test_utils.h"
-const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" };
-const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" };
-
-// parameters from parameter server
-const std::string TEST_DATA_FILE_NAME("testdata_file_name");
-const std::string PARAM_PLANNING_GROUP_NAME("planning_group");
-const std::string PARAM_TARGET_LINK_NAME("target_link");
-const std::string CARTESIAN_POSITION_TOLERANCE("cartesian_position_tolerance");
-const std::string ANGULAR_ACC_TOLERANCE("angular_acc_tolerance");
-const std::string ROTATION_AXIS_NORM_TOLERANCE("rot_axis_norm_tolerance");
-const std::string ACCELERATION_TOLERANCE("acceleration_tolerance");
-const std::string OTHER_TOLERANCE("other_tolerance");
-
-#define SKIP_IF_GRIPPER \
- if (GetParam() == PARAM_MODEL_WITH_GRIPPER_NAME) \
- { \
- SUCCEED(); \
- return; \
- };
+#include "rclcpp/rclcpp.hpp"
using namespace pilz_industrial_motion_planner;
using namespace pilz_industrial_motion_planner_testutils;
-class TrajectoryGeneratorCIRCTest : public testing::TestWithParam
+static const std::string PARAM_NAMESPACE_LIMITS = "robot_description_planning";
+
+class TrajectoryGeneratorCIRCTest : public testing::Test
{
protected:
/**
* @brief Create test scenario for circ trajectory generator
*
*/
- void SetUp() override;
+ void SetUp() override
+ {
+ rclcpp::NodeOptions node_options;
+ node_options.automatically_declare_parameters_from_overrides(true);
+ node_ = rclcpp::Node::make_shared("unittest_trajectory_generator_circ", node_options);
+
+ // load robot model
+ rdf_loader::RDFLoader rdf_loader(node_, "robot_description");
+ moveit::core::RobotModelConstPtr robot_model_ =
+ std::make_shared(rdf_loader.getURDF(), rdf_loader.getSRDF());
+ ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model";
+
+ // get parameters
+ ASSERT_TRUE(node_->has_parameter("testdata_file_name"));
+ node_->get_parameter("testdata_file_name", test_data_file_name_);
+ ASSERT_TRUE(node_->has_parameter("planning_group"));
+ node_->get_parameter("planning_group", planning_group_);
+ ASSERT_TRUE(node_->has_parameter("target_link"));
+ node_->get_parameter("target_link", target_link_);
+ ASSERT_TRUE(node_->has_parameter("cartesian_position_tolerance"));
+ node_->get_parameter("cartesian_position_tolerance", cartesian_position_tolerance_);
+ ASSERT_TRUE(node_->has_parameter("angular_acc_tolerance"));
+ node_->get_parameter("angular_acc_tolerance", angular_acc_tolerance_);
+ ASSERT_TRUE(node_->has_parameter("rot_axis_norm_tolerance"));
+ node_->get_parameter("rot_axis_norm_tolerance", rot_axis_norm_tolerance_);
+ ASSERT_TRUE(node_->has_parameter("acceleration_tolerance"));
+ node_->get_parameter("acceleration_tolerance", acceleration_tolerance_);
+ ASSERT_TRUE(node_->has_parameter("other_tolerance"));
+ node_->get_parameter("other_tolerance", other_tolerance_);
+
+ // check robot model
+ testutils::checkRobotModel(robot_model_, planning_group_, target_link_);
+
+ // load the test data provider
+ tdp_.reset(new pilz_industrial_motion_planner_testutils::XmlTestdataLoader{ test_data_file_name_ });
+ ASSERT_NE(nullptr, tdp_) << "Failed to load test data by provider.";
+
+ tdp_->setRobotModel(robot_model_);
+
+ // create the limits container
+ pilz_industrial_motion_planner::JointLimitsContainer joint_limits =
+ pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(
+ node_, PARAM_NAMESPACE_LIMITS, robot_model_->getActiveJointModels());
+ CartesianLimit cart_limits;
+ // Cartesian limits are chose as such values to ease the manually compute the
+ // trajectory
+
+ cart_limits.setMaxRotationalVelocity(1 * M_PI);
+ cart_limits.setMaxTranslationalAcceleration(1 * M_PI);
+ cart_limits.setMaxTranslationalDeceleration(1 * M_PI);
+ cart_limits.setMaxTranslationalVelocity(1 * M_PI);
+ planner_limits_.setJointLimits(joint_limits);
+ planner_limits_.setCartesianLimits(cart_limits);
+
+ // initialize the LIN trajectory generator
+ circ_.reset(new TrajectoryGeneratorCIRC(robot_model_, planner_limits_));
+ ASSERT_NE(nullptr, circ_) << "failed to create CIRC trajectory generator";
+ }
void checkCircResult(const planning_interface::MotionPlanRequest& req,
- const planning_interface::MotionPlanResponse& res);
+ const planning_interface::MotionPlanResponse& res)
+ {
+ moveit_msgs::msg::MotionPlanResponse res_msg;
+ res.getMessage(res_msg);
+ EXPECT_TRUE(testutils::isGoalReached(res.trajectory_->getFirstWayPointPtr()->getRobotModel(),
+ res_msg.trajectory.joint_trajectory, req, other_tolerance_));
+
+ EXPECT_TRUE(
+ testutils::checkJointTrajectory(res_msg.trajectory.joint_trajectory, planner_limits_.getJointLimitContainer()));
+
+ EXPECT_EQ(req.path_constraints.position_constraints.size(), 1u);
+ EXPECT_EQ(req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.size(), 1u);
+
+ // Check that all point have the equal distance to the center
+ Eigen::Vector3d circ_center;
+ getCircCenter(req, res, circ_center);
+
+ for (std::size_t i = 0; i < res.trajectory_->getWayPointCount(); ++i)
+ {
+ Eigen::Affine3d waypoint_pose = res.trajectory_->getWayPointPtr(i)->getFrameTransform(target_link_);
+ EXPECT_NEAR(
+ (res.trajectory_->getFirstWayPointPtr()->getFrameTransform(target_link_).translation() - circ_center).norm(),
+ (circ_center - waypoint_pose.translation()).norm(), cartesian_position_tolerance_);
+ }
+
+ // check translational and rotational paths
+ ASSERT_TRUE(testutils::checkCartesianTranslationalPath(res.trajectory_, target_link_, acceleration_tolerance_));
+ ASSERT_TRUE(testutils::checkCartesianRotationalPath(res.trajectory_, target_link_, angular_acc_tolerance_,
+ rot_axis_norm_tolerance_));
+
+ for (size_t idx = 0; idx < res.trajectory_->getLastWayPointPtr()->getVariableCount(); ++idx)
+ {
+ EXPECT_NEAR(0.0, res.trajectory_->getLastWayPointPtr()->getVariableVelocity(idx), other_tolerance_);
+ EXPECT_NEAR(0.0, res.trajectory_->getLastWayPointPtr()->getVariableAcceleration(idx), other_tolerance_);
+ }
+ }
void getCircCenter(const planning_interface::MotionPlanRequest& req,
- const planning_interface::MotionPlanResponse& res, Eigen::Vector3d& circ_center);
+ const planning_interface::MotionPlanResponse& res, Eigen::Vector3d& circ_center)
+ {
+ if (req.path_constraints.name == "center")
+ {
+ tf2::fromMsg(req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).position,
+ circ_center);
+ }
+ else if (req.path_constraints.name == "interim")
+ {
+ Eigen::Vector3d interim;
+ tf2::fromMsg(req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).position,
+ interim);
+ Eigen::Vector3d start = res.trajectory_->getFirstWayPointPtr()->getFrameTransform(target_link_).translation();
+ Eigen::Vector3d goal = res.trajectory_->getLastWayPointPtr()->getFrameTransform(target_link_).translation();
+
+ const Eigen::Vector3d t = interim - start;
+ const Eigen::Vector3d u = goal - start;
+ const Eigen::Vector3d v = goal - interim;
+
+ const Eigen::Vector3d w = t.cross(u);
+
+ ASSERT_GT(w.norm(), 1e-8) << "Circle center not well defined for given start, interim and goal.";
+
+ circ_center = start + (u * t.dot(t) * u.dot(v) - t * u.dot(u) * t.dot(v)) * 0.5 / pow(w.norm(), 2);
+ }
+ }
protected:
// ros stuff
- ros::NodeHandle ph_{ "~" };
- robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() };
+ rclcpp::Node::SharedPtr node_;
+ moveit::core::RobotModelConstPtr robot_model_;
+
std::unique_ptr circ_;
// test data provider
std::unique_ptr tdp_;
@@ -111,119 +213,11 @@ class TrajectoryGeneratorCIRCTest : public testing::TestWithParam
LimitsContainer planner_limits_;
};
-void TrajectoryGeneratorCIRCTest::SetUp()
-{
- // get parameters
- ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name_));
- ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_));
- ASSERT_TRUE(ph_.getParam(PARAM_TARGET_LINK_NAME, target_link_));
- ASSERT_TRUE(ph_.getParam(CARTESIAN_POSITION_TOLERANCE, cartesian_position_tolerance_));
- ASSERT_TRUE(ph_.getParam(ANGULAR_ACC_TOLERANCE, angular_acc_tolerance_));
- ASSERT_TRUE(ph_.getParam(ROTATION_AXIS_NORM_TOLERANCE, rot_axis_norm_tolerance_));
- ASSERT_TRUE(ph_.getParam(ACCELERATION_TOLERANCE, acceleration_tolerance_));
- ASSERT_TRUE(ph_.getParam(OTHER_TOLERANCE, other_tolerance_));
-
- // check robot model
- testutils::checkRobotModel(robot_model_, planning_group_, target_link_);
-
- // load the test data provider
- tdp_.reset(new pilz_industrial_motion_planner_testutils::XmlTestdataLoader{ test_data_file_name_ });
- ASSERT_NE(nullptr, tdp_) << "Failed to load test data by provider.";
-
- tdp_->setRobotModel(robot_model_);
-
- // create the limits container
- pilz_industrial_motion_planner::JointLimitsContainer joint_limits =
- pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(ph_,
- robot_model_->getActiveJointModels());
- CartesianLimit cart_limits;
- // Cartesian limits are chose as such values to ease the manually compute the
- // trajectory
-
- cart_limits.setMaxRotationalVelocity(1 * M_PI);
- cart_limits.setMaxTranslationalAcceleration(1 * M_PI);
- cart_limits.setMaxTranslationalDeceleration(1 * M_PI);
- cart_limits.setMaxTranslationalVelocity(1 * M_PI);
- planner_limits_.setJointLimits(joint_limits);
- planner_limits_.setCartesianLimits(cart_limits);
-
- // initialize the LIN trajectory generator
- circ_.reset(new TrajectoryGeneratorCIRC(robot_model_, planner_limits_));
- ASSERT_NE(nullptr, circ_) << "failed to create CIRC trajectory generator";
-}
-
-void TrajectoryGeneratorCIRCTest::checkCircResult(const planning_interface::MotionPlanRequest& req,
- const planning_interface::MotionPlanResponse& res)
-{
- moveit_msgs::msg::MotionPlanResponse res_msg;
- res.getMessage(res_msg);
- EXPECT_TRUE(testutils::isGoalReached(res.trajectory_->getFirstWayPointPtr()->getRobotModel(),
- res_msg.trajectory.joint_trajectory, req, other_tolerance_));
-
- EXPECT_TRUE(
- testutils::checkJointTrajectory(res_msg.trajectory.joint_trajectory, planner_limits_.getJointLimitContainer()));
-
- EXPECT_EQ(req.path_constraints.position_constraints.size(), 1u);
- EXPECT_EQ(req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.size(), 1u);
-
- // Check that all point have the equal distance to the center
- Eigen::Vector3d circ_center;
- getCircCenter(req, res, circ_center);
-
- for (std::size_t i = 0; i < res.trajectory_->getWayPointCount(); ++i)
- {
- Eigen::Affine3d waypoint_pose = res.trajectory_->getWayPointPtr(i)->getFrameTransform(target_link_);
- EXPECT_NEAR(
- (res.trajectory_->getFirstWayPointPtr()->getFrameTransform(target_link_).translation() - circ_center).norm(),
- (circ_center - waypoint_pose.translation()).norm(), cartesian_position_tolerance_);
- }
-
- // check translational and rotational paths
- ASSERT_TRUE(testutils::checkCartesianTranslationalPath(res.trajectory_, target_link_, acceleration_tolerance_));
- ASSERT_TRUE(testutils::checkCartesianRotationalPath(res.trajectory_, target_link_, angular_acc_tolerance_,
- rot_axis_norm_tolerance_));
-
- for (size_t idx = 0; idx < res.trajectory_->getLastWayPointPtr()->getVariableCount(); ++idx)
- {
- EXPECT_NEAR(0.0, res.trajectory_->getLastWayPointPtr()->getVariableVelocity(idx), other_tolerance_);
- EXPECT_NEAR(0.0, res.trajectory_->getLastWayPointPtr()->getVariableAcceleration(idx), other_tolerance_);
- }
-}
-
-void TrajectoryGeneratorCIRCTest::getCircCenter(const planning_interface::MotionPlanRequest& req,
- const planning_interface::MotionPlanResponse& res,
- Eigen::Vector3d& circ_center)
-{
- if (req.path_constraints.name == "center")
- {
- tf2::fromMsg(req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).position,
- circ_center);
- }
- else if (req.path_constraints.name == "interim")
- {
- Eigen::Vector3d interim;
- tf2::fromMsg(req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).position,
- interim);
- Eigen::Vector3d start = res.trajectory_->getFirstWayPointPtr()->getFrameTransform(target_link_).translation();
- Eigen::Vector3d goal = res.trajectory_->getLastWayPointPtr()->getFrameTransform(target_link_).translation();
-
- const Eigen::Vector3d t = interim - start;
- const Eigen::Vector3d u = goal - start;
- const Eigen::Vector3d v = goal - interim;
-
- const Eigen::Vector3d w = t.cross(u);
-
- ASSERT_GT(w.norm(), 1e-8) << "Circle center not well defined for given start, interim and goal.";
-
- circ_center = start + (u * t.dot(t) * u.dot(v) - t * u.dot(u) * t.dot(v)) * 0.5 / pow(w.norm(), 2);
- }
-}
-
/**
* @brief Checks that each derived MoveItErrorCodeException contains the correct
* error code.
*/
-TEST(TrajectoryGeneratorCIRCTest, TestExceptionErrorCodeMapping)
+TEST_F(TrajectoryGeneratorCIRCTest, TestExceptionErrorCodeMapping)
{
{
std::shared_ptr cnp_ex{ new CircleNoPlane("") };
@@ -281,14 +275,10 @@ TEST(TrajectoryGeneratorCIRCTest, TestExceptionErrorCodeMapping)
}
}
-// Instantiate the test cases for robot model with and without gripper
-INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryGeneratorCIRCTest,
- ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME));
-
/**
* @brief Construct a TrajectoryGeneratorCirc with no limits given
*/
-TEST_P(TrajectoryGeneratorCIRCTest, noLimits)
+TEST_F(TrajectoryGeneratorCIRCTest, noLimits)
{
LimitsContainer planner_limits;
EXPECT_THROW(TrajectoryGeneratorCIRC(this->robot_model_, planner_limits), TrajectoryGeneratorInvalidLimitsException);
@@ -298,7 +288,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, noLimits)
* @brief test invalid motion plan request with incomplete start state and
* cartesian goal
*/
-TEST_P(TrajectoryGeneratorCIRCTest, incompleteStartState)
+TEST_F(TrajectoryGeneratorCIRCTest, incompleteStartState)
{
auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
@@ -315,7 +305,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, incompleteStartState)
/**
* @brief test invalid motion plan request with non zero start velocity
*/
-TEST_P(TrajectoryGeneratorCIRCTest, nonZeroStartVelocity)
+TEST_F(TrajectoryGeneratorCIRCTest, nonZeroStartVelocity)
{
moveit_msgs::msg::MotionPlanRequest req{ tdp_->getCircJointCenterCart("circ1_center_2").toRequest() };
@@ -327,7 +317,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, nonZeroStartVelocity)
req.start_state.joint_state.velocity.clear();
}
-TEST_P(TrajectoryGeneratorCIRCTest, ValidCommand)
+TEST_F(TrajectoryGeneratorCIRCTest, ValidCommand)
{
auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
@@ -339,7 +329,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, ValidCommand)
/**
* @brief Generate invalid circ with to high vel scaling
*/
-TEST_P(TrajectoryGeneratorCIRCTest, velScaleToHigh)
+TEST_F(TrajectoryGeneratorCIRCTest, velScaleToHigh)
{
auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
@@ -352,7 +342,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, velScaleToHigh)
/**
* @brief Generate invalid circ with to high acc scaling
*/
-TEST_P(TrajectoryGeneratorCIRCTest, accScaleToHigh)
+TEST_F(TrajectoryGeneratorCIRCTest, accScaleToHigh)
{
auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
@@ -366,7 +356,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, accScaleToHigh)
* @brief Use three points (with center) with a really small distance between to
* trigger a internal throw from KDL
*/
-TEST_P(TrajectoryGeneratorCIRCTest, samePointsWithCenter)
+TEST_F(TrajectoryGeneratorCIRCTest, samePointsWithCenter)
{
// Define auxiliary point and goal to be the same as the start
auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
@@ -389,7 +379,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, samePointsWithCenter)
*
* Expected: Planning should fail.
*/
-TEST_P(TrajectoryGeneratorCIRCTest, samePointsWithInterim)
+TEST_F(TrajectoryGeneratorCIRCTest, samePointsWithInterim)
{
// Define auxiliary point and goal to be the same as the start
auto circ{ tdp_->getCircCartInterimCart("circ3_interim") };
@@ -410,7 +400,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, samePointsWithInterim)
/**
* @brief test invalid motion plan request with no aux point defined
*/
-TEST_P(TrajectoryGeneratorCIRCTest, emptyAux)
+TEST_F(TrajectoryGeneratorCIRCTest, emptyAux)
{
auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
@@ -426,7 +416,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, emptyAux)
/**
* @brief test invalid motion plan request with no aux name defined
*/
-TEST_P(TrajectoryGeneratorCIRCTest, invalidAuxName)
+TEST_F(TrajectoryGeneratorCIRCTest, invalidAuxName)
{
auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
@@ -443,7 +433,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, invalidAuxName)
* @brief test invalid motion plan request with invalid link name in the
* auxiliary point
*/
-TEST_P(TrajectoryGeneratorCIRCTest, invalidAuxLinkName)
+TEST_F(TrajectoryGeneratorCIRCTest, invalidAuxLinkName)
{
auto circ{ tdp_->getCircJointInterimCart("circ3_interim") };
@@ -459,7 +449,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, invalidAuxLinkName)
/**
* @brief test the circ planner with invalid center point
*/
-TEST_P(TrajectoryGeneratorCIRCTest, invalidCenter)
+TEST_F(TrajectoryGeneratorCIRCTest, invalidCenter)
{
auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose());
@@ -475,7 +465,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, invalidCenter)
*
* Expected: Planning should fail since the path is not uniquely defined.
*/
-TEST_P(TrajectoryGeneratorCIRCTest, colinearCenter)
+TEST_F(TrajectoryGeneratorCIRCTest, colinearCenter)
{
auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose());
@@ -496,7 +486,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, colinearCenter)
* Expected: Planning should fail. These positions do not even represent a
* circle.
*/
-TEST_P(TrajectoryGeneratorCIRCTest, colinearInterim)
+TEST_F(TrajectoryGeneratorCIRCTest, colinearInterim)
{
auto circ{ tdp_->getCircCartInterimCart("circ3_interim") };
circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose());
@@ -519,7 +509,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, colinearInterim)
*
* Expected: Planning should successfully return.
*/
-TEST_P(TrajectoryGeneratorCIRCTest, colinearCenterDueToInterim)
+TEST_F(TrajectoryGeneratorCIRCTest, colinearCenterDueToInterim)
{
// get the test data from xml
auto circ{ tdp_->getCircCartInterimCart("circ3_interim") };
@@ -539,7 +529,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, colinearCenterDueToInterim)
*
* Expected: Planning should successfully return.
*/
-TEST_P(TrajectoryGeneratorCIRCTest, colinearCenterAndInterim)
+TEST_F(TrajectoryGeneratorCIRCTest, colinearCenterAndInterim)
{
auto circ{ tdp_->getCircCartInterimCart("circ3_interim") };
@@ -572,7 +562,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, colinearCenterAndInterim)
*
* Expected: Planning should successfully return.
*/
-TEST_P(TrajectoryGeneratorCIRCTest, interimLarger180Degree)
+TEST_F(TrajectoryGeneratorCIRCTest, interimLarger180Degree)
{
auto circ{ tdp_->getCircCartInterimCart("circ3_interim") };
@@ -600,10 +590,8 @@ TEST_P(TrajectoryGeneratorCIRCTest, interimLarger180Degree)
/**
* @brief test the circ planner with center point and joint goal
*/
-TEST_P(TrajectoryGeneratorCIRCTest, centerPointJointGoal)
+TEST_F(TrajectoryGeneratorCIRCTest, centerPointJointGoal)
{
- SKIP_IF_GRIPPER
-
auto circ{ tdp_->getCircJointCenterCart("circ1_center_2") };
moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
@@ -618,7 +606,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, centerPointJointGoal)
* this test a additional
* point is defined as an invalid test case
*/
-TEST_P(TrajectoryGeneratorCIRCTest, InvalidAdditionalPrimitivePose)
+TEST_F(TrajectoryGeneratorCIRCTest, InvalidAdditionalPrimitivePose)
{
auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
@@ -628,7 +616,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, InvalidAdditionalPrimitivePose)
ASSERT_EQ(req.path_constraints.position_constraints.back().constraint_region.primitive_poses.size(), 1u);
// Define a additional pose here
- geometry_msgs::Pose center_position;
+ geometry_msgs::msg::Pose center_position;
center_position.position.x = 0.0;
center_position.position.y = 0.0;
center_position.position.z = 0.65;
@@ -645,7 +633,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, InvalidAdditionalPrimitivePose)
* Here an additional joint constraints is "falsely" defined to check for the
* error.
*/
-TEST_P(TrajectoryGeneratorCIRCTest, InvalidExtraJointConstraint)
+TEST_F(TrajectoryGeneratorCIRCTest, InvalidExtraJointConstraint)
{
auto circ{ tdp_->getCircJointCenterCart("circ1_center_2") };
@@ -664,7 +652,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, InvalidExtraJointConstraint)
/**
* @brief test the circ planner with center point and pose goal
*/
-TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoal)
+TEST_F(TrajectoryGeneratorCIRCTest, CenterPointPoseGoal)
{
auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
@@ -679,7 +667,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoal)
/**
* @brief Set a frame id only on the position constrainst
*/
-TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdPositionConstraints)
+TEST_F(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdPositionConstraints)
{
auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
@@ -696,7 +684,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdPositionConstraint
/**
* @brief Set a frame id only on the orientation constrainst
*/
-TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdOrientationConstraints)
+TEST_F(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdOrientationConstraints)
{
auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
@@ -712,7 +700,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdOrientationConstra
/**
* @brief Set a frame_id on both position and orientation constraints
*/
-TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdBothConstraints)
+TEST_F(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdBothConstraints)
{
auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
@@ -731,10 +719,8 @@ TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdBothConstraints)
/**
* @brief test the circ planner with interim point with joint goal
*/
-TEST_P(TrajectoryGeneratorCIRCTest, InterimPointJointGoal)
+TEST_F(TrajectoryGeneratorCIRCTest, InterimPointJointGoal)
{
- SKIP_IF_GRIPPER
-
auto circ{ tdp_->getCircJointInterimCart("circ3_interim") };
moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
@@ -751,10 +737,8 @@ TEST_P(TrajectoryGeneratorCIRCTest, InterimPointJointGoal)
*
* The generator is expected to be robust against a velocity beeing almost zero.
*/
-TEST_P(TrajectoryGeneratorCIRCTest, InterimPointJointGoalStartVelNearZero)
+TEST_F(TrajectoryGeneratorCIRCTest, InterimPointJointGoalStartVelNearZero)
{
- SKIP_IF_GRIPPER
-
auto circ{ tdp_->getCircJointInterimCart("circ3_interim") };
moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
@@ -771,7 +755,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, InterimPointJointGoalStartVelNearZero)
/**
* @brief test the circ planner with interim point with pose goal
*/
-TEST_P(TrajectoryGeneratorCIRCTest, InterimPointPoseGoal)
+TEST_F(TrajectoryGeneratorCIRCTest, InterimPointPoseGoal)
{
auto circ{ tdp_->getCircJointInterimCart("circ3_interim") };
moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
@@ -784,8 +768,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, InterimPointPoseGoal)
int main(int argc, char** argv)
{
- ros::init(argc, argv, "unittest_trajectory_generator_circ");
- ros::NodeHandle nh;
+ rclcpp::init(argc, argv);
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp
index a002d89cd1c..86c0c257505 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp
@@ -41,22 +41,19 @@
#include
#include
#include
-#include
#include "pilz_industrial_motion_planner/joint_limits_aggregator.h"
#include "pilz_industrial_motion_planner/joint_limits_container.h"
#include "pilz_industrial_motion_planner/trajectory_generator_circ.h"
#include "pilz_industrial_motion_planner/trajectory_generator_lin.h"
#include "pilz_industrial_motion_planner/trajectory_generator_ptp.h"
+#include "pilz_industrial_motion_planner/trajectory_blender_transition_window.h"
#include "test_utils.h"
-const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" };
-const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" };
+#include "rclcpp/rclcpp.hpp"
-// parameters from parameter server
-const std::string PARAM_PLANNING_GROUP_NAME("planning_group");
-const std::string PARAM_TARGET_LINK_NAME("target_link");
+static const std::string PARAM_NAMESPACE_LIMITS = "robot_description_planning";
/**
* A value type container to combine type and value
@@ -99,16 +96,28 @@ class TrajectoryGeneratorCommonTest : public ::testing::Test
protected:
void SetUp() override
{
- ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_));
- ASSERT_TRUE(ph_.getParam(PARAM_TARGET_LINK_NAME, target_link_));
+ rclcpp::NodeOptions node_options;
+ node_options.automatically_declare_parameters_from_overrides(true);
+ node_ = rclcpp::Node::make_shared("unittest_trajectory_generator_common", node_options);
+
+ // load robot model
+ rdf_loader::RDFLoader rdf_loader(node_, "robot_description");
+ moveit::core::RobotModelConstPtr robot_model_ =
+ std::make_shared(rdf_loader.getURDF(), rdf_loader.getSRDF());
+ ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model";
+
+ // get parameters
+ ASSERT_TRUE(node_->has_parameter("planning_group"));
+ node_->get_parameter("planning_group", planning_group_);
+ ASSERT_TRUE(node_->has_parameter("target_link"));
+ node_->get_parameter("target_link", target_link_);
testutils::checkRobotModel(robot_model_, planning_group_, target_link_);
// create the limits container
- std::string robot_description_param = (!T::VALUE ? PARAM_MODEL_NO_GRIPPER_NAME : PARAM_MODEL_WITH_GRIPPER_NAME);
pilz_industrial_motion_planner::JointLimitsContainer joint_limits =
pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(
- ros::NodeHandle(robot_description_param + "_planning"), robot_model_->getActiveJointModels());
+ node_, PARAM_NAMESPACE_LIMITS, robot_model_->getActiveJointModels());
pilz_industrial_motion_planner::CartesianLimit cart_limits;
cart_limits.setMaxRotationalVelocity(0.5 * M_PI);
cart_limits.setMaxTranslationalAcceleration(2);
@@ -127,7 +136,7 @@ class TrajectoryGeneratorCommonTest : public ::testing::Test
req_.group_name = planning_group_;
req_.max_velocity_scaling_factor = 1.0;
req_.max_acceleration_scaling_factor = 1.0;
- robot_state::RobotState rstate(robot_model_);
+ moveit::core::RobotState rstate(robot_model_);
rstate.setToDefaultValues();
rstate.setJointGroupPositions(planning_group_, { 0, M_PI / 2, 0, M_PI / 2, 0, 0 });
rstate.setVariableVelocities(std::vector(rstate.getVariableCount(), 0.0));
@@ -142,11 +151,11 @@ class TrajectoryGeneratorCommonTest : public ::testing::Test
protected:
// ros stuff
- ros::NodeHandle ph_{ "~" };
- robot_model::RobotModelConstPtr robot_model_{
- robot_model_loader::RobotModelLoader(!T::VALUE ? PARAM_MODEL_NO_GRIPPER_NAME : PARAM_MODEL_WITH_GRIPPER_NAME)
- .getModel()
- };
+ rclcpp::Node::SharedPtr node_;
+ moveit::core::RobotModelConstPtr robot_model_;
+
+ std::unique_ptr lin_generator_;
+ std::unique_ptr blender_;
// trajectory generator
std::unique_ptr trajectory_generator_;
@@ -301,8 +310,8 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyGoalConstraints)
TYPED_TEST(TrajectoryGeneratorCommonTest, MultipleGoals)
{
moveit_msgs::msg::JointConstraint joint_constraint;
- moveit_msgs::PositionConstraint position_constraint;
- moveit_msgs::OrientationConstraint orientation_constraint;
+ moveit_msgs::msg::PositionConstraint position_constraint;
+ moveit_msgs::msg::OrientationConstraint orientation_constraint;
moveit_msgs::msg::Constraints goal_constraint;
// two goal constraints
@@ -378,8 +387,8 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalideJointPositionInGoal)
*/
TYPED_TEST(TrajectoryGeneratorCommonTest, InvalidLinkNameInCartesianGoal)
{
- moveit_msgs::PositionConstraint position_constraint;
- moveit_msgs::OrientationConstraint orientation_constraint;
+ moveit_msgs::msg::PositionConstraint position_constraint;
+ moveit_msgs::msg::OrientationConstraint orientation_constraint;
moveit_msgs::msg::Constraints goal_constraint;
// link name not set
goal_constraint.position_constraints.push_back(position_constraint);
@@ -410,8 +419,8 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalidLinkNameInCartesianGoal)
*/
TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyPrimitivePoses)
{
- moveit_msgs::PositionConstraint position_constraint;
- moveit_msgs::OrientationConstraint orientation_constraint;
+ moveit_msgs::msg::PositionConstraint position_constraint;
+ moveit_msgs::msg::OrientationConstraint orientation_constraint;
moveit_msgs::msg::Constraints goal_constraint;
position_constraint.link_name =
this->robot_model_->getJointModelGroup(this->planning_group_)->getLinkModelNames().back();
@@ -427,8 +436,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyPrimitivePoses)
int main(int argc, char** argv)
{
- ros::init(argc, argv, "unittest_trajectory_generator_common");
- // ros::NodeHandle nh;
+ rclcpp::init(argc, argv);
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp
index 076c65cac6d..946b31b0204 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp
@@ -48,26 +48,13 @@
#include
#include
-#include
-
-const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" };
-const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" };
-
-// parameters from parameter server
-const std::string TEST_DATA_FILE_NAME("testdata_file_name");
-const std::string PARAM_PLANNING_GROUP_NAME("planning_group");
-const std::string TARGET_LINK_HCD("target_link_hand_computed_data");
-const std::string RANDOM_TEST_TRIAL_NUM("random_trial_number");
-const std::string JOINT_POSITION_TOLERANCE("joint_position_tolerance");
-const std::string JOINT_VELOCITY_TOLERANCE("joint_velocity_tolerance");
-const std::string POSE_TRANSFORM_MATRIX_NORM_TOLERANCE("pose_norm_tolerance");
-const std::string ROTATION_AXIS_NORM_TOLERANCE("rot_axis_norm_tolerance");
-const std::string VELOCITY_SCALING_FACTOR("velocity_scaling_factor");
-const std::string OTHER_TOLERANCE("other_tolerance");
+#include "rclcpp/rclcpp.hpp"
using namespace pilz_industrial_motion_planner;
using namespace pilz_industrial_motion_planner_testutils;
+static const std::string PARAM_NAMESPACE_LIMITS = "robot_description_planning";
+
/**
* @brief Parameterized unittest of trajectory generator LIN to enable tests
* against
@@ -75,22 +62,79 @@ using namespace pilz_industrial_motion_planner_testutils;
* the
* ros parameter server.
*/
-class TrajectoryGeneratorLINTest : public testing::TestWithParam
+class TrajectoryGeneratorLINTest : public testing::Test
{
protected:
/**
* @brief Create test scenario for lin trajectory generator
*
*/
- void SetUp() override;
+ void SetUp() override
+ {
+ rclcpp::NodeOptions node_options;
+ node_options.automatically_declare_parameters_from_overrides(true);
+ node_ = rclcpp::Node::make_shared("unittest_trajectory_generator_lin", node_options);
+
+ // load robot model
+ rdf_loader::RDFLoader rdf_loader(node_, "robot_description");
+ moveit::core::RobotModelConstPtr robot_model_ =
+ std::make_shared(rdf_loader.getURDF(), rdf_loader.getSRDF());
+ ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model";
+
+ testutils::checkRobotModel(robot_model_, planning_group_, target_link_hcd_);
+
+ // load the test data provider
+ tdp_.reset(new pilz_industrial_motion_planner_testutils::XmlTestdataLoader{ test_data_file_name_ });
+ ASSERT_NE(nullptr, tdp_) << "Failed to load test data by provider.";
+
+ tdp_->setRobotModel(robot_model_);
+
+ // create the limits container
+ // TODO, move this also into test data set
+ pilz_industrial_motion_planner::JointLimitsContainer joint_limits =
+ pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(
+ node_, PARAM_NAMESPACE_LIMITS, robot_model_->getActiveJointModels());
+ CartesianLimit cart_limits;
+ cart_limits.setMaxRotationalVelocity(0.5 * M_PI);
+ cart_limits.setMaxTranslationalAcceleration(2);
+ cart_limits.setMaxTranslationalDeceleration(2);
+ cart_limits.setMaxTranslationalVelocity(1);
+ planner_limits_.setJointLimits(joint_limits);
+ planner_limits_.setCartesianLimits(cart_limits);
+
+ // initialize the LIN trajectory generator
+ lin_.reset(new TrajectoryGeneratorLIN(robot_model_, planner_limits_));
+ ASSERT_NE(nullptr, lin_) << "Failed to create LIN trajectory generator.";
+ }
bool checkLinResponse(const planning_interface::MotionPlanRequest& req,
- const planning_interface::MotionPlanResponse& res);
+ const planning_interface::MotionPlanResponse& res)
+ {
+ moveit_msgs::msg::MotionPlanResponse res_msg;
+ res.getMessage(res_msg);
+ if (!testutils::isGoalReached(robot_model_, res_msg.trajectory.joint_trajectory, req, pose_norm_tolerance_))
+ {
+ return false;
+ }
+
+ if (!testutils::checkCartesianLinearity(robot_model_, res_msg.trajectory.joint_trajectory, req,
+ pose_norm_tolerance_, rot_axis_norm_tolerance_))
+ {
+ return false;
+ }
+
+ if (!testutils::checkJointTrajectory(res_msg.trajectory.joint_trajectory, planner_limits_.getJointLimitContainer()))
+ {
+ return false;
+ }
+
+ return true;
+ }
protected:
// ros stuff
- ros::NodeHandle ph_{ "~" };
- robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() };
+ rclcpp::Node::SharedPtr node_;
+ moveit::core::RobotModelConstPtr robot_model_;
// lin trajectory generator using model without gripper
std::unique_ptr lin_;
@@ -105,75 +149,11 @@ class TrajectoryGeneratorLINTest : public testing::TestWithParam
LimitsContainer planner_limits_;
};
-void TrajectoryGeneratorLINTest::SetUp()
-{
- // get the parameters
- ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name_));
- ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_));
- ASSERT_TRUE(ph_.getParam(TARGET_LINK_HCD, target_link_hcd_));
- ASSERT_TRUE(ph_.getParam(RANDOM_TEST_TRIAL_NUM, random_trial_num_));
- ASSERT_TRUE(ph_.getParam(JOINT_POSITION_TOLERANCE, joint_position_tolerance_));
- ASSERT_TRUE(ph_.getParam(JOINT_VELOCITY_TOLERANCE, joint_velocity_tolerance_));
- ASSERT_TRUE(ph_.getParam(POSE_TRANSFORM_MATRIX_NORM_TOLERANCE, pose_norm_tolerance_));
- ASSERT_TRUE(ph_.getParam(ROTATION_AXIS_NORM_TOLERANCE, rot_axis_norm_tolerance_));
- ASSERT_TRUE(ph_.getParam(VELOCITY_SCALING_FACTOR, velocity_scaling_factor_));
- ASSERT_TRUE(ph_.getParam(OTHER_TOLERANCE, other_tolerance_));
-
- testutils::checkRobotModel(robot_model_, planning_group_, target_link_hcd_);
-
- // load the test data provider
- tdp_.reset(new pilz_industrial_motion_planner_testutils::XmlTestdataLoader{ test_data_file_name_ });
- ASSERT_NE(nullptr, tdp_) << "Failed to load test data by provider.";
-
- tdp_->setRobotModel(robot_model_);
-
- // create the limits container
- // TODO, move this also into test data set
- pilz_industrial_motion_planner::JointLimitsContainer joint_limits =
- pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(ph_,
- robot_model_->getActiveJointModels());
- CartesianLimit cart_limits;
- cart_limits.setMaxRotationalVelocity(0.5 * M_PI);
- cart_limits.setMaxTranslationalAcceleration(2);
- cart_limits.setMaxTranslationalDeceleration(2);
- cart_limits.setMaxTranslationalVelocity(1);
- planner_limits_.setJointLimits(joint_limits);
- planner_limits_.setCartesianLimits(cart_limits);
-
- // initialize the LIN trajectory generator
- lin_.reset(new TrajectoryGeneratorLIN(robot_model_, planner_limits_));
- ASSERT_NE(nullptr, lin_) << "Failed to create LIN trajectory generator.";
-}
-
-bool TrajectoryGeneratorLINTest::checkLinResponse(const planning_interface::MotionPlanRequest& req,
- const planning_interface::MotionPlanResponse& res)
-{
- moveit_msgs::msg::MotionPlanResponse res_msg;
- res.getMessage(res_msg);
- if (!testutils::isGoalReached(robot_model_, res_msg.trajectory.joint_trajectory, req, pose_norm_tolerance_))
- {
- return false;
- }
-
- if (!testutils::checkCartesianLinearity(robot_model_, res_msg.trajectory.joint_trajectory, req, pose_norm_tolerance_,
- rot_axis_norm_tolerance_))
- {
- return false;
- }
-
- if (!testutils::checkJointTrajectory(res_msg.trajectory.joint_trajectory, planner_limits_.getJointLimitContainer()))
- {
- return false;
- }
-
- return true;
-}
-
/**
* @brief Checks that each derived MoveItErrorCodeException contains the correct
* error code.
*/
-TEST(TrajectoryGeneratorLINTest, TestExceptionErrorCodeMapping)
+TEST_F(TrajectoryGeneratorLINTest, TestExceptionErrorCodeMapping)
{
{
std::shared_ptr ltcf_ex{ new LinTrajectoryConversionFailure("") };
@@ -196,15 +176,11 @@ TEST(TrajectoryGeneratorLINTest, TestExceptionErrorCodeMapping)
}
}
-// Instantiate the test cases for robot model with and without gripper
-INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryGeneratorLINTest,
- ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME));
-
/**
* @brief test the lin planner with invalid motion plan request which has non
* zero start velocity
*/
-TEST_P(TrajectoryGeneratorLINTest, nonZeroStartVelocity)
+TEST_F(TrajectoryGeneratorLINTest, nonZeroStartVelocity)
{
planning_interface::MotionPlanRequest req{ tdp_->getLinJoint("lin2").toRequest() };
@@ -220,7 +196,7 @@ TEST_P(TrajectoryGeneratorLINTest, nonZeroStartVelocity)
/**
* @brief test the lin planner with joint space goal
*/
-TEST_P(TrajectoryGeneratorLINTest, jointSpaceGoal)
+TEST_F(TrajectoryGeneratorLINTest, jointSpaceGoal)
{
planning_interface::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() };
@@ -237,7 +213,7 @@ TEST_P(TrajectoryGeneratorLINTest, jointSpaceGoal)
* @brief test the lin planner with joint space goal with start velocity almost
* zero
*/
-TEST_P(TrajectoryGeneratorLINTest, jointSpaceGoalNearZeroStartVelocity)
+TEST_F(TrajectoryGeneratorLINTest, jointSpaceGoalNearZeroStartVelocity)
{
planning_interface::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() };
@@ -257,7 +233,7 @@ TEST_P(TrajectoryGeneratorLINTest, jointSpaceGoalNearZeroStartVelocity)
/**
* @brief test the lin planner with Cartesian goal
*/
-TEST_P(TrajectoryGeneratorLINTest, cartesianSpaceGoal)
+TEST_F(TrajectoryGeneratorLINTest, cartesianSpaceGoal)
{
// construct motion plan request
moveit_msgs::msg::MotionPlanRequest lin_cart_req{ tdp_->getLinCart("lin2").toRequest() };
@@ -279,7 +255,7 @@ TEST_P(TrajectoryGeneratorLINTest, cartesianSpaceGoal)
* occur that are neither
* acceleration, constant or deceleration.
*/
-TEST_P(TrajectoryGeneratorLINTest, cartesianTrapezoidProfile)
+TEST_F(TrajectoryGeneratorLINTest, cartesianTrapezoidProfile)
{
// construct motion plan request
moveit_msgs::msg::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() };
@@ -314,7 +290,7 @@ TEST_P(TrajectoryGeneratorLINTest, cartesianTrapezoidProfile)
* Expected Results:
* 1. Function returns 'false'.
*/
-TEST_P(TrajectoryGeneratorLINTest, LinPlannerLimitViolation)
+TEST_F(TrajectoryGeneratorLINTest, LinPlannerLimitViolation)
{
LinJoint lin{ tdp_->getLinJoint("lin2") };
lin.setAccelerationScale(1.01);
@@ -334,7 +310,7 @@ TEST_P(TrajectoryGeneratorLINTest, LinPlannerLimitViolation)
* Expected Results:
* 1. Function returns 'false'.
*/
-TEST_P(TrajectoryGeneratorLINTest, LinPlannerDiscontinuousJointTraj)
+TEST_F(TrajectoryGeneratorLINTest, LinPlannerDiscontinuousJointTraj)
{
LinJoint lin{ tdp_->getLinJoint("lin2") };
// Alter goal joint configuration (represents the same cartesian pose, but
@@ -358,7 +334,7 @@ TEST_P(TrajectoryGeneratorLINTest, LinPlannerDiscontinuousJointTraj)
* Expected Results:
* 1. trajectory generation is successful.
*/
-TEST_P(TrajectoryGeneratorLINTest, LinStartEqualsGoal)
+TEST_F(TrajectoryGeneratorLINTest, LinStartEqualsGoal)
{
// construct motion plan request
moveit_msgs::msg::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() };
@@ -389,7 +365,7 @@ TEST_P(TrajectoryGeneratorLINTest, LinStartEqualsGoal)
* Expected Results:
* 1. Ctor throws exception.
*/
-TEST_P(TrajectoryGeneratorLINTest, CtorNoLimits)
+TEST_F(TrajectoryGeneratorLINTest, CtorNoLimits)
{
pilz_industrial_motion_planner::LimitsContainer planner_limits;
@@ -407,7 +383,7 @@ TEST_P(TrajectoryGeneratorLINTest, CtorNoLimits)
* Expected Results:
* 1. Function returns 'false'.
*/
-TEST_P(TrajectoryGeneratorLINTest, IncorrectJointNumber)
+TEST_F(TrajectoryGeneratorLINTest, IncorrectJointNumber)
{
// construct motion plan request
moveit_msgs::msg::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() };
@@ -425,7 +401,7 @@ TEST_P(TrajectoryGeneratorLINTest, IncorrectJointNumber)
* @brief test invalid motion plan request with incomplete start state and
* cartesian goal
*/
-TEST_P(TrajectoryGeneratorLINTest, cartGoalIncompleteStartState)
+TEST_F(TrajectoryGeneratorLINTest, cartGoalIncompleteStartState)
{
// construct motion plan request
moveit_msgs::msg::MotionPlanRequest lin_cart_req{ tdp_->getLinCart("lin2").toRequest() };
@@ -443,7 +419,7 @@ TEST_P(TrajectoryGeneratorLINTest, cartGoalIncompleteStartState)
* @brief Set a frame id in goal constraint with cartesian goal on both position
* and orientation constraints
*/
-TEST_P(TrajectoryGeneratorLINTest, cartGoalFrameIdBothConstraints)
+TEST_F(TrajectoryGeneratorLINTest, cartGoalFrameIdBothConstraints)
{
// construct motion plan request
moveit_msgs::msg::MotionPlanRequest lin_cart_req{ tdp_->getLinCart("lin2").toRequest() };
@@ -462,8 +438,7 @@ TEST_P(TrajectoryGeneratorLINTest, cartGoalFrameIdBothConstraints)
int main(int argc, char** argv)
{
- ros::init(argc, argv, "unittest_trajectory_generator_lin");
- ros::NodeHandle nh;
+ rclcpp::init(argc, argv);
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp
index 3ddc411c191..7f2e4980ca8 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp
@@ -45,9 +45,7 @@
#include
#include
-// parameters for parameterized tests
-const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" };
-const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" };
+#include "rclcpp/rclcpp.hpp"
// parameters from parameter server
const std::string PARAM_PLANNING_GROUP_NAME("planning_group");
@@ -59,14 +57,66 @@ const std::string POSE_TRANSFORM_MATRIX_NORM_TOLERANCE("pose_norm_tolerance");
using namespace pilz_industrial_motion_planner;
-class TrajectoryGeneratorPTPTest : public testing::TestWithParam
+class TrajectoryGeneratorPTPTest : public testing::Test
{
protected:
/**
* @brief Create test fixture for ptp trajectory generator
*
*/
- void SetUp() override;
+ void SetUp() override
+ {
+ rclcpp::NodeOptions node_options;
+ node_options.automatically_declare_parameters_from_overrides(true);
+ node_ = rclcpp::Node::make_shared("unittest_trajectory_generator_ptp", node_options);
+
+ // load robot model
+ rdf_loader::RDFLoader rdf_loader(node_, "robot_description");
+ moveit::core::RobotModelConstPtr robot_model_ =
+ std::make_shared(rdf_loader.getURDF(), rdf_loader.getSRDF());
+ ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model";
+
+ // get parameters from parameter server
+ ASSERT_TRUE(node_->has_parameter("planning_group"));
+ node_->get_parameter("planning_group", planning_group_);
+ ASSERT_TRUE(node_->has_parameter("target_link"));
+ node_->get_parameter("target_link", target_link_);
+ ASSERT_TRUE(node_->has_parameter("joint_position_tolerance"));
+ node_->get_parameter("joint_position_tolerance", joint_position_tolerance_);
+ ASSERT_TRUE(node_->has_parameter("joint_velocity_tolerance"));
+ node_->get_parameter("joint_velocity_tolerance", joint_velocity_tolerance_);
+ ASSERT_TRUE(node_->has_parameter("joint_acceleration_tolerance"));
+ node_->get_parameter("joint_acceleration_tolerance", joint_acceleration_tolerance_);
+ ASSERT_TRUE(node_->has_parameter("pose_norm_tolerance"));
+ node_->get_parameter("pose_norm_tolerance", pose_norm_tolerance_);
+
+ testutils::checkRobotModel(robot_model_, planning_group_, target_link_);
+
+ // create the limits container
+ JointLimitsContainer joint_limits;
+ for (const auto& jmg : robot_model_->getJointModelGroups())
+ {
+ std::vector joint_names = jmg->getActiveJointModelNames();
+ JointLimit joint_limit;
+ joint_limit.max_position = 3.124;
+ joint_limit.min_position = -3.124;
+ joint_limit.has_velocity_limits = true;
+ joint_limit.max_velocity = 1;
+ joint_limit.has_acceleration_limits = true;
+ joint_limit.max_acceleration = 0.5;
+ joint_limit.has_deceleration_limits = true;
+ joint_limit.max_deceleration = -1;
+ for (const auto& joint_name : joint_names)
+ {
+ joint_limits.addLimit(joint_name, joint_limit);
+ }
+ }
+
+ // create the trajectory generator
+ planner_limits_.setJointLimits(joint_limits);
+ ptp_.reset(new TrajectoryGeneratorPTP(robot_model_, planner_limits_));
+ ASSERT_NE(nullptr, ptp_);
+ }
/**
* @brief check the resulted joint trajectory
@@ -75,13 +125,21 @@ class TrajectoryGeneratorPTPTest : public testing::TestWithParam
* @param joint_limits
* @return
*/
- bool checkTrajectory(const trajectory_msgs::JointTrajectory& trajectory,
- const planning_interface::MotionPlanRequest& req, const JointLimitsContainer& joint_limits);
+ bool checkTrajectory(const trajectory_msgs::msg::JointTrajectory& trajectory,
+ const planning_interface::MotionPlanRequest& req, const JointLimitsContainer& joint_limits)
+ {
+ return (testutils::isTrajectoryConsistent(trajectory) &&
+ testutils::isGoalReached(trajectory, req.goal_constraints.front().joint_constraints,
+ joint_position_tolerance_, joint_velocity_tolerance_) &&
+ testutils::isPositionBounded(trajectory, joint_limits) &&
+ testutils::isVelocityBounded(trajectory, joint_limits) &&
+ testutils::isAccelerationBounded(trajectory, joint_limits));
+ }
protected:
// ros stuff
- ros::NodeHandle ph_{ "~" };
- robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() };
+ rclcpp::Node::SharedPtr node_;
+ moveit::core::RobotModelConstPtr robot_model_;
// trajectory generator
std::unique_ptr ptp_;
@@ -92,61 +150,11 @@ class TrajectoryGeneratorPTPTest : public testing::TestWithParam
double joint_position_tolerance_, joint_velocity_tolerance_, joint_acceleration_tolerance_, pose_norm_tolerance_;
};
-void TrajectoryGeneratorPTPTest::SetUp()
-{
- // get parameters from parameter server
- ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_));
- ASSERT_TRUE(ph_.getParam(PARAM_TARGET_LINK_NAME, target_link_));
- ASSERT_TRUE(ph_.getParam(JOINT_POSITION_TOLERANCE, joint_position_tolerance_));
- ASSERT_TRUE(ph_.getParam(JOINT_VELOCITY_TOLERANCE, joint_velocity_tolerance_));
- ASSERT_TRUE(ph_.getParam(JOINT_ACCELERATION_TOLERANCE, joint_acceleration_tolerance_));
- ASSERT_TRUE(ph_.getParam(POSE_TRANSFORM_MATRIX_NORM_TOLERANCE, pose_norm_tolerance_));
-
- testutils::checkRobotModel(robot_model_, planning_group_, target_link_);
-
- // create the limits container
- JointLimitsContainer joint_limits;
- for (const auto& jmg : robot_model_->getJointModelGroups())
- {
- std::vector joint_names = jmg->getActiveJointModelNames();
- JointLimit joint_limit;
- joint_limit.max_position = 3.124;
- joint_limit.min_position = -3.124;
- joint_limit.has_velocity_limits = true;
- joint_limit.max_velocity = 1;
- joint_limit.has_acceleration_limits = true;
- joint_limit.max_acceleration = 0.5;
- joint_limit.has_deceleration_limits = true;
- joint_limit.max_deceleration = -1;
- for (const auto& joint_name : joint_names)
- {
- joint_limits.addLimit(joint_name, joint_limit);
- }
- }
-
- // create the trajectory generator
- planner_limits_.setJointLimits(joint_limits);
- ptp_.reset(new TrajectoryGeneratorPTP(robot_model_, planner_limits_));
- ASSERT_NE(nullptr, ptp_);
-}
-
-bool TrajectoryGeneratorPTPTest::checkTrajectory(const trajectory_msgs::JointTrajectory& trajectory,
- const planning_interface::MotionPlanRequest& req,
- const JointLimitsContainer& joint_limits)
-{
- return (testutils::isTrajectoryConsistent(trajectory) &&
- testutils::isGoalReached(trajectory, req.goal_constraints.front().joint_constraints,
- joint_position_tolerance_, joint_velocity_tolerance_) &&
- testutils::isPositionBounded(trajectory, joint_limits) &&
- testutils::isVelocityBounded(trajectory, joint_limits) &&
- testutils::isAccelerationBounded(trajectory, joint_limits));
-}
-
/**
* @brief Checks that each derived MoveItErrorCodeException contains the correct
* error code.
*/
-TEST(TrajectoryGeneratorPTPTest, TestExceptionErrorCodeMapping)
+TEST_F(TrajectoryGeneratorPTPTest, TestExceptionErrorCodeMapping)
{
{
std::shared_ptr pvpsf_ex{ new PtpVelocityProfileSyncFailed("") };
@@ -159,13 +167,10 @@ TEST(TrajectoryGeneratorPTPTest, TestExceptionErrorCodeMapping)
}
}
-// Instantiate the test cases for robot model with and without gripper
-INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryGeneratorPTPTest,
- ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME));
/**
* @brief Construct a TrajectoryGeneratorPTP with no limits given
*/
-TEST_P(TrajectoryGeneratorPTPTest, noLimits)
+TEST_F(TrajectoryGeneratorPTPTest, noLimits)
{
LimitsContainer planner_limits;
EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits), TrajectoryGeneratorInvalidLimitsException);
@@ -181,14 +186,14 @@ TEST_P(TrajectoryGeneratorPTPTest, noLimits)
* - Expected Results:
* 1. the res.trajectory_ should be cleared (contain no waypoints)
*/
-TEST_P(TrajectoryGeneratorPTPTest, emptyRequest)
+TEST_F(TrajectoryGeneratorPTPTest, emptyRequest)
{
planning_interface::MotionPlanResponse res;
planning_interface::MotionPlanRequest req;
robot_trajectory::RobotTrajectoryPtr trajectory(
new robot_trajectory::RobotTrajectory(this->robot_model_, planning_group_));
- robot_state::RobotState state(this->robot_model_);
+ moveit::core::RobotState state(this->robot_model_);
trajectory->addPrefixWayPoint(state, 0);
res.trajectory_ = trajectory;
@@ -202,7 +207,7 @@ TEST_P(TrajectoryGeneratorPTPTest, emptyRequest)
/**
* @brief Construct a TrajectoryGeneratorPTP with missing velocity limits
*/
-TEST_P(TrajectoryGeneratorPTPTest, missingVelocityLimits)
+TEST_F(TrajectoryGeneratorPTPTest, missingVelocityLimits)
{
LimitsContainer planner_limits;
@@ -226,7 +231,7 @@ TEST_P(TrajectoryGeneratorPTPTest, missingVelocityLimits)
/**
* @brief Construct a TrajectoryGeneratorPTP missing deceleration limits
*/
-TEST_P(TrajectoryGeneratorPTPTest, missingDecelerationimits)
+TEST_F(TrajectoryGeneratorPTPTest, missingDecelerationimits)
{
LimitsContainer planner_limits;
@@ -257,7 +262,7 @@ TEST_P(TrajectoryGeneratorPTPTest, missingDecelerationimits)
* TrajectoryGeneratorInvalidLimitsException
* 2. the constructor throws no exception
*/
-TEST_P(TrajectoryGeneratorPTPTest, testInsufficientLimit)
+TEST_F(TrajectoryGeneratorPTPTest, testInsufficientLimit)
{
/**********/
/* Step 1 */
@@ -332,7 +337,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testInsufficientLimit)
/**
* @brief test the ptp trajectory generator of Cartesian space goal
*/
-TEST_P(TrajectoryGeneratorPTPTest, testCartesianGoal)
+TEST_F(TrajectoryGeneratorPTPTest, testCartesianGoal)
{
//***************************************
//*** prepare the motion plan request ***
@@ -342,7 +347,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testCartesianGoal)
testutils::createDummyRequest(robot_model_, planning_group_, req);
// cartesian goal pose
- geometry_msgs::PoseStamped pose;
+ geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = 0.1;
pose.pose.position.y = 0.2;
pose.pose.position.z = 0.65;
@@ -381,7 +386,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testCartesianGoal)
* @brief Check that missing a link_name in position or orientation constraints
* is detected
*/
-TEST_P(TrajectoryGeneratorPTPTest, testCartesianGoalMissingLinkNameConstraints)
+TEST_F(TrajectoryGeneratorPTPTest, testCartesianGoalMissingLinkNameConstraints)
{
//***************************************
//*** prepare the motion plan request ***
@@ -391,7 +396,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testCartesianGoalMissingLinkNameConstraints)
testutils::createDummyRequest(robot_model_, planning_group_, req);
// cartesian goal pose
- geometry_msgs::PoseStamped pose;
+ geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = 0.1;
pose.pose.position.y = 0.2;
pose.pose.position.z = 0.65;
@@ -419,13 +424,13 @@ TEST_P(TrajectoryGeneratorPTPTest, testCartesianGoalMissingLinkNameConstraints)
/**
* @brief test the ptp trajectory generator of invalid Cartesian space goal
*/
-TEST_P(TrajectoryGeneratorPTPTest, testInvalidCartesianGoal)
+TEST_F(TrajectoryGeneratorPTPTest, testInvalidCartesianGoal)
{
planning_interface::MotionPlanResponse res;
planning_interface::MotionPlanRequest req;
testutils::createDummyRequest(robot_model_, planning_group_, req);
- geometry_msgs::PoseStamped pose;
+ geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = 0.1;
pose.pose.position.y = 0.2;
pose.pose.position.z = 2.5;
@@ -449,7 +454,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testInvalidCartesianGoal)
* enough to the start which does not need
* to plan the trajectory
*/
-TEST_P(TrajectoryGeneratorPTPTest, testJointGoalAlreadyReached)
+TEST_F(TrajectoryGeneratorPTPTest, testJointGoalAlreadyReached)
{
planning_interface::MotionPlanResponse res;
planning_interface::MotionPlanRequest req;
@@ -477,7 +482,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testJointGoalAlreadyReached)
* @brief test scaling factor
* with zero start velocity
*/
-TEST_P(TrajectoryGeneratorPTPTest, testScalingFactor)
+TEST_F(TrajectoryGeneratorPTPTest, testScalingFactor)
{
// create ptp generator with different limits
JointLimit joint_limit;
@@ -641,7 +646,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testScalingFactor)
* @brief test the ptp trajectory generator of joint space goal
* with (almost) zero start velocity
*/
-TEST_P(TrajectoryGeneratorPTPTest, testJointGoalAndAlmostZeroStartVelocity)
+TEST_F(TrajectoryGeneratorPTPTest, testJointGoalAndAlmostZeroStartVelocity)
{
planning_interface::MotionPlanResponse res;
planning_interface::MotionPlanRequest req;
@@ -778,7 +783,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testJointGoalAndAlmostZeroStartVelocity)
* @brief test the ptp_ trajectory generator of joint space goal
* with zero start velocity
*/
-TEST_P(TrajectoryGeneratorPTPTest, testJointGoalNoStartVel)
+TEST_F(TrajectoryGeneratorPTPTest, testJointGoalNoStartVel)
{
planning_interface::MotionPlanResponse res;
planning_interface::MotionPlanRequest req;
@@ -964,8 +969,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testJointGoalNoStartVel)
int main(int argc, char** argv)
{
- ros::init(argc, argv, "unittest_trajectory_generator_ptp");
- ros::NodeHandle nh;
+ rclcpp::init(argc, argv);
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}