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(); }