diff --git a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt index b4c68c657a..96bfcd2c6d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt +++ b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt @@ -1,79 +1,88 @@ -cmake_minimum_required(VERSION 3.1.3) +cmake_minimum_required(VERSION 3.10.2) project(pilz_industrial_motion_planner) -## Add support for C++11, supported in ROS Kinetic and newer -add_definitions(-std=c++11) -add_definitions(-Wall) -add_definitions(-Wextra) -add_definitions(-Wno-unused-parameter) +# Common cmake code applied to all moveit packages +find_package(moveit_common REQUIRED) +moveit_package() + +find_package(ament_cmake REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +# TODO(henning): Enable when this is available +# find_package(joint_limits_interface REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(moveit_core REQUIRED) +find_package(moveit_msgs REQUIRED) +find_package(moveit_ros_move_group REQUIRED) +find_package(moveit_ros_planning REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tf2_eigen_kdl REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_kdl REQUIRED) +find_package(tf2_ros REQUIRED) + +find_package(orocos_kdl REQUIRED) +find_package(Boost REQUIRED COMPONENTS) + +# TODO(henning): Remove/Port +# if(CATKIN_ENABLE_TESTING AND ENABLE_COVERAGE_TESTING) +# find_package(code_coverage REQUIRED) +# append_coverage_compiler_flags() +# endif() +# +# ################ +# ## Clang tidy ## +# ################ +# if(CATKIN_ENABLE_CLANG_TIDY) +# find_program( +# CLANG_TIDY_EXE +# NAMES "clang-tidy" +# DOC "Path to clang-tidy executable" +# ) +# if(NOT CLANG_TIDY_EXE) +# message(FATAL_ERROR "clang-tidy not found.") +# else() +# message(STATUS "clang-tidy found: ${CLANG_TIDY_EXE}") +# set(CMAKE_CXX_CLANG_TIDY "${CLANG_TIDY_EXE}") +# endif() +# endif() -find_package(catkin REQUIRED COMPONENTS - joint_limits_interface +########### +## Build ## +########### +include_directories( + include +) + +set(THIS_PACKAGE_INCLUDE_DEPENDS + Eigen3 + eigen3_cmake_module + # joint_limits_interface + geometry_msgs moveit_core moveit_msgs moveit_ros_move_group moveit_ros_planning moveit_ros_planning_interface pluginlib - roscpp + rclcpp tf2 tf2_eigen + tf2_eigen_kdl tf2_geometry_msgs tf2_kdl tf2_ros + orocos_kdl + Boost ) -find_package(orocos_kdl) -find_package(Boost REQUIRED COMPONENTS) - -if(CATKIN_ENABLE_TESTING AND ENABLE_COVERAGE_TESTING) - find_package(code_coverage REQUIRED) - append_coverage_compiler_flags() -endif() - -################ -## Clang tidy ## -################ -if(CATKIN_ENABLE_CLANG_TIDY) - find_program( - CLANG_TIDY_EXE - NAMES "clang-tidy" - DOC "Path to clang-tidy executable" - ) - if(NOT CLANG_TIDY_EXE) - message(FATAL_ERROR "clang-tidy not found.") - else() - message(STATUS "clang-tidy found: ${CLANG_TIDY_EXE}") - set(CMAKE_CXX_CLANG_TIDY "${CLANG_TIDY_EXE}") - endif() -endif() - -################################### -## catkin specific configuration ## -################################### -catkin_package( - CATKIN_DEPENDS - moveit_msgs - roscpp - tf2_geometry_msgs -) - -########### -## Build ## -########### -include_directories( - include -) - -# To avoid Warnings from clang-tidy declare system -include_directories( - SYSTEM - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) ## Declare a C++ library -add_library(${PROJECT_NAME}_lib +add_library(${PROJECT_NAME}_lib SHARED src/${PROJECT_NAME}.cpp src/planning_context_loader.cpp src/joint_limits_validator.cpp @@ -86,7 +95,7 @@ add_library(${PROJECT_NAME}_lib src/plan_components_builder.cpp ) -target_link_libraries(${PROJECT_NAME}_lib ${catkin_LIBRARIES}) +ament_target_dependencies(${PROJECT_NAME}_lib ${THIS_PACKAGE_INCLUDE_DEPENDS}) ############# ## Plugins ## @@ -100,7 +109,7 @@ add_library(${PROJECT_NAME} src/cartesian_limit.cpp src/cartesian_limits_aggregator.cpp ) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) add_library(planning_context_loader_ptp src/planning_context_loader_ptp.cpp @@ -111,7 +120,7 @@ add_library(planning_context_loader_ptp src/velocity_profile_atrap.cpp src/joint_limits_container.cpp ) -target_link_libraries(planning_context_loader_ptp ${catkin_LIBRARIES}) +ament_target_dependencies(planning_context_loader_ptp ${THIS_PACKAGE_INCLUDE_DEPENDS}) add_library(planning_context_loader_lin src/planning_context_loader_lin.cpp @@ -121,7 +130,7 @@ add_library(planning_context_loader_lin src/trajectory_generator_lin.cpp src/velocity_profile_atrap.cpp ) -target_link_libraries(planning_context_loader_lin ${catkin_LIBRARIES}) +ament_target_dependencies(planning_context_loader_lin ${THIS_PACKAGE_INCLUDE_DEPENDS}) add_library(planning_context_loader_circ src/planning_context_loader_circ.cpp @@ -131,14 +140,15 @@ add_library(planning_context_loader_circ src/trajectory_generator_circ.cpp src/path_circle_generator.cpp ) -target_link_libraries(planning_context_loader_circ ${catkin_LIBRARIES}) +ament_target_dependencies(planning_context_loader_circ ${THIS_PACKAGE_INCLUDE_DEPENDS}) add_library(command_list_manager src/command_list_manager.cpp src/plan_components_builder.cpp ) -target_link_libraries(command_list_manager ${catkin_LIBRARIES}) -add_dependencies(command_list_manager ${catkin_EXPORTED_TARGETS}) +ament_target_dependencies(command_list_manager ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# TODO(henning): Remove? +# add_dependencies(command_list_manager ${catkin_EXPORTED_TARGETS}) add_library(sequence_capability src/move_group_sequence_action.cpp @@ -152,8 +162,9 @@ add_library(sequence_capability src/cartesian_limit.cpp src/cartesian_limits_aggregator.cpp ) -target_link_libraries(sequence_capability ${catkin_LIBRARIES}) -add_dependencies(sequence_capability ${catkin_EXPORTED_TARGETS}) +ament_target_dependencies(sequence_capability ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# TODO(henning): Remove? +# add_dependencies(sequence_capability ${catkin_EXPORTED_TARGETS}) ############# ## Install ## @@ -162,351 +173,374 @@ install(FILES plugins/sequence_capability_plugin_description.xml plugins/${PROJECT_NAME}_plugin_description.xml plugins/planning_context_plugin_description.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/plugins + DESTINATION share/${PROJECT_NAME} ) ## Mark libraries for installation install( TARGETS ${PROJECT_NAME} + ${PROJECT_NAME}_lib planning_context_loader_ptp planning_context_loader_lin planning_context_loader_circ command_list_manager sequence_capability - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include ) -############# -## Testing ## -############# -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - find_package(${PROJECT_NAME}_testutils REQUIRED) - - include_directories( - ${${PROJECT_NAME}_testutils_INCLUDE_DIRS} - ) - - if(ENABLE_COVERAGE_TESTING) - include(CodeCoverage) - append_coverage_compiler_flags() - endif() - - ######################### - ####Integration Tests#### - ######################### - set(${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES - sequence_capability - ${PROJECT_NAME} - planning_context_loader_ptp - planning_context_loader_lin - planning_context_loader_circ - ) - - # Planning Integration tests - add_rostest_gmock(integrationtest_sequence_action_preemption - test/integrationtest_sequence_action_preemption.test - test/integrationtest_sequence_action_preemption.cpp - ) - target_link_libraries(integrationtest_sequence_action_preemption - ${catkin_LIBRARIES} - ${${PROJECT_NAME}_testutils_LIBRARIES} - ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} - ) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) - add_rostest_gmock(integrationtest_sequence_action - test/integrationtest_sequence_action.test - test/integrationtest_sequence_action.cpp - ) - target_link_libraries(integrationtest_sequence_action - ${catkin_LIBRARIES} - ${${PROJECT_NAME}_testutils_LIBRARIES} - ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} - ) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) - add_rostest(test/integrationtest_sequence_action_capability_with_gripper.test - DEPENDENCIES integrationtest_sequence_action - ) + # These don't pass yet, disable them for now + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cppcheck_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + set(ament_cmake_uncrustify_FOUND TRUE) - add_rostest(test/integrationtest_sequence_action_capability_frankaemika_panda.test - DEPENDENCIES integrationtest_sequence_action - ) - - add_rostest_gtest(integrationtest_sequence_service_capability - test/integrationtest_sequence_service_capability.test - test/integrationtest_sequence_service_capability.cpp - ) - target_link_libraries(integrationtest_sequence_service_capability - ${catkin_LIBRARIES} - ${${PROJECT_NAME}_testutils_LIBRARIES} - ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} - ) - - add_rostest(test/integrationtest_sequence_service_capability_frankaemika_panda.test - DEPENDENCIES integrationtest_sequence_service_capability - ) - - add_rostest(test/integrationtest_sequence_service_capability_with_gripper.test - DEPENDENCIES integrationtest_sequence_service_capability - ) - - # Planning Integration tests - add_rostest_gtest(integrationtest_command_planning - test/integrationtest_command_planning.test - test/integrationtest_command_planning.cpp - test/test_utils.cpp - ) - target_link_libraries(integrationtest_command_planning - ${catkin_LIBRARIES} - ${${PROJECT_NAME}_testutils_LIBRARIES} - ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} - ) - - add_rostest(test/integrationtest_command_planning_with_gripper.test - DEPENDENCIES integrationtest_command_planning - ) - - add_rostest(test/integrationtest_command_planning_frankaemika_panda.test - DEPENDENCIES integrationtest_command_planning - ) - - # Blending Integration tests - add_rostest_gtest(integrationtest_command_list_manager - test/integrationtest_command_list_manager.test - test/integrationtest_command_list_manager.cpp - test/test_utils.cpp - ) - target_link_libraries(integrationtest_command_list_manager - ${catkin_LIBRARIES} - ${${PROJECT_NAME}_testutils_LIBRARIES} - ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} - ) - - add_rostest_gtest(integrationtest_get_solver_tip_frame - test/integrationtest_get_solver_tip_frame.test - test/integrationtest_get_solver_tip_frame.cpp - ) - target_link_libraries(integrationtest_get_solver_tip_frame - ${catkin_LIBRARIES} - ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} - ) - - add_rostest_gtest(integrationtest_plan_components_builder - test/integrationtest_plan_components_builder.test - test/integrationtest_plan_components_builder.cpp - ) - target_link_libraries(integrationtest_plan_components_builder - ${catkin_LIBRARIES} - ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} - ) - - ################## - ####Unit Tests#### - ################## - add_library(${PROJECT_NAME}_testhelpers - test/test_utils.cpp - src/trajectory_generator.cpp - src/trajectory_generator_circ.cpp - src/trajectory_generator_lin.cpp - src/trajectory_generator_ptp.cpp - src/path_circle_generator.cpp - src/velocity_profile_atrap.cpp - ) - - target_link_libraries(${PROJECT_NAME}_testhelpers ${PROJECT_NAME}_lib) - - ## Add gtest based cpp test target and link libraries - catkin_add_gtest(unittest_${PROJECT_NAME}_direct - test/unittest_${PROJECT_NAME}_direct.cpp - src/planning_context_loader_ptp.cpp - ) - target_link_libraries(unittest_${PROJECT_NAME}_direct - ${catkin_LIBRARIES} - ${PROJECT_NAME}_testhelpers - ) - - ## Add gtest based cpp test target and link libraries - catkin_add_gtest(unittest_velocity_profile_atrap - test/unittest_velocity_profile_atrap.cpp - src/velocity_profile_atrap.cpp - ) - target_link_libraries(unittest_velocity_profile_atrap - ${catkin_LIBRARIES} - ) - - catkin_add_gtest(unittest_trajectory_generator - test/unittest_trajectory_generator.cpp - src/trajectory_generator.cpp - ) - target_link_libraries(unittest_trajectory_generator - ${catkin_LIBRARIES} - ${${PROJECT_NAME}_testutils_LIBRARIES} - ${PROJECT_NAME}_lib - ) - - # Trajectory Generator 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 - ) - - # Command Planner Unit Test - add_rostest_gtest(unittest_${PROJECT_NAME} - test/unittest_${PROJECT_NAME}.test - test/unittest_${PROJECT_NAME}.cpp - ) - target_link_libraries(unittest_${PROJECT_NAME} - ${catkin_LIBRARIES} - ${PROJECT_NAME}_lib - ) - - # JointLimits Unit Test - add_rostest_gtest(unittest_joint_limit - test/unittest_joint_limit.test - test/unittest_joint_limit.cpp - ) - target_link_libraries(unittest_joint_limit - ${catkin_LIBRARIES} - ${PROJECT_NAME}_lib - ) - - # JointLimitsAggregator Unit Test - 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 - ) - - # JointLimitsContainer Unit Test - catkin_add_gtest(unittest_joint_limits_container - test/unittest_joint_limits_container.cpp - ) - target_link_libraries(unittest_joint_limits_container - ${catkin_LIBRARIES} - ${PROJECT_NAME}_lib - ) - - # JointLimitsValidator Unit Test - catkin_add_gtest(unittest_joint_limits_validator - test/unittest_joint_limits_validator.cpp - ) - target_link_libraries(unittest_joint_limits_validator - ${catkin_LIBRARIES} - ${PROJECT_NAME}_lib - ) - - # Cartesian Limits Aggregator Unit Test - add_rostest_gtest(unittest_cartesian_limits_aggregator - test/unittest_cartesian_limits_aggregator.test - test/unittest_cartesian_limits_aggregator.cpp - ) - target_link_libraries(unittest_cartesian_limits_aggregator - ${catkin_LIBRARIES} - ${PROJECT_NAME}_lib - ) - - # 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 - ) + # Run all lint tests in package.xml except those listed above + ament_lint_auto_find_test_dependencies() +endif() - # GetTipFrames Unit Test - catkin_add_gmock(unittest_get_solver_tip_frame - test/unittest_get_solver_tip_frame.cpp - ) - target_link_libraries(unittest_get_solver_tip_frame - ${catkin_LIBRARIES} - ) +ament_package() - # to run: catkin_make -DCMAKE_BUILD_TYPE=Debug -DENABLE_COVERAGE_TESTING=ON package_name_coverage - if(ENABLE_COVERAGE_TESTING) - set(COVERAGE_EXCLUDES "*/${PROJECT_NAME}/test*") - add_code_coverage( - NAME ${PROJECT_NAME}_coverage - # specifying dependencies in a reliable way is on open issue - # see https://github.com/mikeferguson/code_coverage/pull/14 - #DEPENDENCIES tests - ) - endif() -endif() +############# +## Testing ## +############# +# TODO(henning): Enable tests +# if(CATKIN_ENABLE_TESTING) +# find_package(rostest REQUIRED) +# find_package(${PROJECT_NAME}_testutils REQUIRED) +# +# include_directories( +# ${${PROJECT_NAME}_testutils_INCLUDE_DIRS} +# ) +# +# if(ENABLE_COVERAGE_TESTING) +# include(CodeCoverage) +# append_coverage_compiler_flags() +# endif() +# +# ######################### +# ####Integration Tests#### +# ######################### +# set(${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES +# sequence_capability +# ${PROJECT_NAME} +# planning_context_loader_ptp +# planning_context_loader_lin +# planning_context_loader_circ +# ) +# +# # Planning Integration tests +# add_rostest_gmock(integrationtest_sequence_action_preemption +# test/integrationtest_sequence_action_preemption.test +# test/integrationtest_sequence_action_preemption.cpp +# ) +# target_link_libraries(integrationtest_sequence_action_preemption +# ${catkin_LIBRARIES} +# ${${PROJECT_NAME}_testutils_LIBRARIES} +# ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} +# ) +# +# add_rostest_gmock(integrationtest_sequence_action +# test/integrationtest_sequence_action.test +# test/integrationtest_sequence_action.cpp +# ) +# target_link_libraries(integrationtest_sequence_action +# ${catkin_LIBRARIES} +# ${${PROJECT_NAME}_testutils_LIBRARIES} +# ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} +# ) +# +# add_rostest(test/integrationtest_sequence_action_capability_with_gripper.test +# DEPENDENCIES integrationtest_sequence_action +# ) +# +# add_rostest(test/integrationtest_sequence_action_capability_frankaemika_panda.test +# DEPENDENCIES integrationtest_sequence_action +# ) +# +# add_rostest_gtest(integrationtest_sequence_service_capability +# test/integrationtest_sequence_service_capability.test +# test/integrationtest_sequence_service_capability.cpp +# ) +# target_link_libraries(integrationtest_sequence_service_capability +# ${catkin_LIBRARIES} +# ${${PROJECT_NAME}_testutils_LIBRARIES} +# ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} +# ) +# +# add_rostest(test/integrationtest_sequence_service_capability_frankaemika_panda.test +# DEPENDENCIES integrationtest_sequence_service_capability +# ) +# +# add_rostest(test/integrationtest_sequence_service_capability_with_gripper.test +# DEPENDENCIES integrationtest_sequence_service_capability +# ) +# +# # Planning Integration tests +# add_rostest_gtest(integrationtest_command_planning +# test/integrationtest_command_planning.test +# test/integrationtest_command_planning.cpp +# test/test_utils.cpp +# ) +# target_link_libraries(integrationtest_command_planning +# ${catkin_LIBRARIES} +# ${${PROJECT_NAME}_testutils_LIBRARIES} +# ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} +# ) +# +# add_rostest(test/integrationtest_command_planning_with_gripper.test +# DEPENDENCIES integrationtest_command_planning +# ) +# +# add_rostest(test/integrationtest_command_planning_frankaemika_panda.test +# DEPENDENCIES integrationtest_command_planning +# ) +# +# # Blending Integration tests +# add_rostest_gtest(integrationtest_command_list_manager +# test/integrationtest_command_list_manager.test +# test/integrationtest_command_list_manager.cpp +# test/test_utils.cpp +# ) +# target_link_libraries(integrationtest_command_list_manager +# ${catkin_LIBRARIES} +# ${${PROJECT_NAME}_testutils_LIBRARIES} +# ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} +# ) +# +# add_rostest_gtest(integrationtest_get_solver_tip_frame +# test/integrationtest_get_solver_tip_frame.test +# test/integrationtest_get_solver_tip_frame.cpp +# ) +# target_link_libraries(integrationtest_get_solver_tip_frame +# ${catkin_LIBRARIES} +# ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} +# ) +# +# add_rostest_gtest(integrationtest_plan_components_builder +# test/integrationtest_plan_components_builder.test +# test/integrationtest_plan_components_builder.cpp +# ) +# target_link_libraries(integrationtest_plan_components_builder +# ${catkin_LIBRARIES} +# ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} +# ) +# +# ################## +# ####Unit Tests#### +# ################## +# add_library(${PROJECT_NAME}_testhelpers +# test/test_utils.cpp +# src/trajectory_generator.cpp +# src/trajectory_generator_circ.cpp +# src/trajectory_generator_lin.cpp +# src/trajectory_generator_ptp.cpp +# src/path_circle_generator.cpp +# src/velocity_profile_atrap.cpp +# ) +# +# target_link_libraries(${PROJECT_NAME}_testhelpers ${PROJECT_NAME}_lib) +# +# ## Add gtest based cpp test target and link libraries +# catkin_add_gtest(unittest_${PROJECT_NAME}_direct +# test/unittest_${PROJECT_NAME}_direct.cpp +# src/planning_context_loader_ptp.cpp +# ) +# target_link_libraries(unittest_${PROJECT_NAME}_direct +# ${catkin_LIBRARIES} +# ${PROJECT_NAME}_testhelpers +# ) +# +# ## Add gtest based cpp test target and link libraries +# catkin_add_gtest(unittest_velocity_profile_atrap +# test/unittest_velocity_profile_atrap.cpp +# src/velocity_profile_atrap.cpp +# ) +# target_link_libraries(unittest_velocity_profile_atrap +# ${catkin_LIBRARIES} +# ) +# +# catkin_add_gtest(unittest_trajectory_generator +# test/unittest_trajectory_generator.cpp +# src/trajectory_generator.cpp +# ) +# target_link_libraries(unittest_trajectory_generator +# ${catkin_LIBRARIES} +# ${${PROJECT_NAME}_testutils_LIBRARIES} +# ${PROJECT_NAME}_lib +# ) +# +# # Trajectory Generator 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 +# ) +# +# # Command Planner Unit Test +# add_rostest_gtest(unittest_${PROJECT_NAME} +# test/unittest_${PROJECT_NAME}.test +# test/unittest_${PROJECT_NAME}.cpp +# ) +# target_link_libraries(unittest_${PROJECT_NAME} +# ${catkin_LIBRARIES} +# ${PROJECT_NAME}_lib +# ) +# +# # JointLimits Unit Test +# add_rostest_gtest(unittest_joint_limit +# test/unittest_joint_limit.test +# test/unittest_joint_limit.cpp +# ) +# target_link_libraries(unittest_joint_limit +# ${catkin_LIBRARIES} +# ${PROJECT_NAME}_lib +# ) +# +# # JointLimitsAggregator Unit Test +# 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 +# ) +# +# # JointLimitsContainer Unit Test +# catkin_add_gtest(unittest_joint_limits_container +# test/unittest_joint_limits_container.cpp +# ) +# target_link_libraries(unittest_joint_limits_container +# ${catkin_LIBRARIES} +# ${PROJECT_NAME}_lib +# ) +# +# # JointLimitsValidator Unit Test +# catkin_add_gtest(unittest_joint_limits_validator +# test/unittest_joint_limits_validator.cpp +# ) +# target_link_libraries(unittest_joint_limits_validator +# ${catkin_LIBRARIES} +# ${PROJECT_NAME}_lib +# ) +# +# # Cartesian Limits Aggregator Unit Test +# add_rostest_gtest(unittest_cartesian_limits_aggregator +# test/unittest_cartesian_limits_aggregator.test +# test/unittest_cartesian_limits_aggregator.cpp +# ) +# target_link_libraries(unittest_cartesian_limits_aggregator +# ${catkin_LIBRARIES} +# ${PROJECT_NAME}_lib +# ) +# +# # 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 +# ) +# +# # GetTipFrames Unit Test +# catkin_add_gmock(unittest_get_solver_tip_frame +# test/unittest_get_solver_tip_frame.cpp +# ) +# target_link_libraries(unittest_get_solver_tip_frame +# ${catkin_LIBRARIES} +# ) +# +# # to run: catkin_make -DCMAKE_BUILD_TYPE=Debug -DENABLE_COVERAGE_TESTING=ON package_name_coverage +# if(ENABLE_COVERAGE_TESTING) +# set(COVERAGE_EXCLUDES "*/${PROJECT_NAME}/test*") +# add_code_coverage( +# NAME ${PROJECT_NAME}_coverage +# # specifying dependencies in a reliable way is on open issue +# # see https://github.com/mikeferguson/code_coverage/pull/14 +# #DEPENDENCIES tests +# ) +# endif() +# endif() diff --git a/moveit_planners/pilz_industrial_motion_planner/COLCON_IGNORE b/moveit_planners/pilz_industrial_motion_planner/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.uxf b/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.uxf index 1a8a3bc7c5..30ff9cb0a2 100644 --- a/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.uxf +++ b/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.uxf @@ -694,7 +694,7 @@ PlanningContextLoaderCIRC -- +getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, -moveit_msgs::MoveItErrorCodes& error_code):planning_interface::PlanningContextPtr +moveit_msgs::msg::MoveItErrorCodes& error_code):planning_interface::PlanningContextPtr -- diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_limits_aggregator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_limits_aggregator.h index 796dc01c4c..6e6cea2bc8 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_limits_aggregator.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_limits_aggregator.h @@ -34,6 +34,7 @@ #pragma once +#include #include "pilz_industrial_motion_planner/cartesian_limit.h" namespace pilz_industrial_motion_planner @@ -56,10 +57,11 @@ class CartesianLimitsAggregator * - "max_rot_vel", the maximum rotational velocity [rad/s] * - "max_rot_acc", the maximum rotational acceleration [rad/s^2] * - "max_rot_dec", the maximum rotational deceleration (<= 0)[rad/s^2] - * @param nh node handle to access the parameters + * @param node node to access the parameters + * @param param_namespace the parameter name to access the parameters * @return the obtained cartesian limits */ - static CartesianLimit getAggregatedLimits(const ros::NodeHandle& nh); + static CartesianLimit getAggregatedLimits(const rclcpp::Node::SharedPtr& node, const std::string& param_namespace); }; } // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h index 4a5a57e55e..d841953d88 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h @@ -34,20 +34,20 @@ #pragma once -#include +#include -#include -#include -#include +#include +#include +#include namespace pilz_industrial_motion_planner { struct CartesianTrajectoryPoint { - geometry_msgs::Pose pose; - geometry_msgs::Twist velocity; - geometry_msgs::Twist acceleartion; - ros::Duration time_from_start; + geometry_msgs::msg::Pose pose; + geometry_msgs::msg::Twist velocity; + geometry_msgs::msg::Twist acceleartion; + rclcpp::Duration time_from_start{ 0, 0 }; }; } // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.h index ae2ea66387..a2b0be38fa 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.h @@ -40,9 +40,9 @@ #include #include -#include +#include +#include -#include "moveit_msgs/MotionSequenceRequest.h" #include "pilz_industrial_motion_planner/plan_components_builder.h" #include "pilz_industrial_motion_planner/trajectory_blender.h" #include "pilz_industrial_motion_planner/trajectory_generation_exceptions.h" @@ -52,11 +52,14 @@ namespace pilz_industrial_motion_planner using RobotTrajCont = std::vector; // List of exceptions which can be thrown by the CommandListManager class. -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NegativeBlendRadiusException, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LastBlendRadiusNotZeroException, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(StartStateSetException, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(OverlappingBlendRadiiException, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PlanningPipelineException, moveit_msgs::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NegativeBlendRadiusException, + moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LastBlendRadiusNotZeroException, + moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(StartStateSetException, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(OverlappingBlendRadiiException, + moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PlanningPipelineException, moveit_msgs::msg::MoveItErrorCodes::FAILURE); /** * @brief This class orchestrates the planning of single commands and @@ -65,7 +68,7 @@ CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PlanningPipelineException, moveit_msgs::MoveI class CommandListManager { public: - CommandListManager(const ros::NodeHandle& nh, const robot_model::RobotModelConstPtr& model); + CommandListManager(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& model); /** * @brief Generates trajectories for the specified list of motion commands. @@ -100,11 +103,11 @@ class CommandListManager */ RobotTrajCont solve(const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_pipeline::PlanningPipelinePtr& planning_pipeline, - const moveit_msgs::MotionSequenceRequest& req_list); + const moveit_msgs::msg::MotionSequenceRequest& req_list); private: using MotionResponseCont = std::vector; - using RobotState_OptRef = boost::optional; + using RobotState_OptRef = boost::optional; using RadiiCont = std::vector; using GroupNamesCont = std::vector; @@ -129,7 +132,7 @@ class CommandListManager */ MotionResponseCont solveSequenceItems(const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_pipeline::PlanningPipelinePtr& planning_pipeline, - const moveit_msgs::MotionSequenceRequest& req_list) const; + const moveit_msgs::msg::MotionSequenceRequest& req_list) const; /** * @return TRUE if the blending radii of specified trajectories overlap, @@ -152,7 +155,7 @@ class CommandListManager * from group. */ static void setStartState(const MotionResponseCont& motion_plan_responses, const std::string& group_name, - moveit_msgs::RobotState& start_state); + moveit_msgs::msg::RobotState& start_state); /** * @return Container of radii extracted from the specified request list. @@ -163,7 +166,7 @@ class CommandListManager * - blend raddi between different groups. */ static RadiiCont extractBlendRadii(const moveit::core::RobotModel& model, - const moveit_msgs::MotionSequenceRequest& req_list); + const moveit_msgs::msg::MotionSequenceRequest& req_list); /** * @return True in case of an invalid blend radii between specified @@ -171,40 +174,42 @@ class CommandListManager * - blend radii between end-effectors and * - blend raddi between different groups. */ - static bool isInvalidBlendRadii(const moveit::core::RobotModel& model, const moveit_msgs::MotionSequenceItem& item_A, - const moveit_msgs::MotionSequenceItem& item_B); + static bool isInvalidBlendRadii(const moveit::core::RobotModel& model, + const moveit_msgs::msg::MotionSequenceItem& item_A, + const moveit_msgs::msg::MotionSequenceItem& item_B); /** * @brief Checks that all blend radii are greater or equal to zero. */ - static void checkForNegativeRadii(const moveit_msgs::MotionSequenceRequest& req_list); + static void checkForNegativeRadii(const moveit_msgs::msg::MotionSequenceRequest& req_list); /** * @brief Checks that last blend radius is zero. */ - static void checkLastBlendRadiusZero(const moveit_msgs::MotionSequenceRequest& req_list); + static void checkLastBlendRadiusZero(const moveit_msgs::msg::MotionSequenceRequest& req_list); /** * @brief Checks that only the first request of the specified group has * a start state in the specified request list. */ - static void checkStartStatesOfGroup(const moveit_msgs::MotionSequenceRequest& req_list, const std::string& group_name); + static void checkStartStatesOfGroup(const moveit_msgs::msg::MotionSequenceRequest& req_list, + const std::string& group_name); /** * @brief Checks that each group in the specified request list has only * one start state. */ - static void checkStartStates(const moveit_msgs::MotionSequenceRequest& req_list); + static void checkStartStates(const moveit_msgs::msg::MotionSequenceRequest& req_list); /** * @return Returns all group names which are present in the specified * request. */ - static GroupNamesCont getGroupNames(const moveit_msgs::MotionSequenceRequest& req_list); + static GroupNamesCont getGroupNames(const moveit_msgs::msg::MotionSequenceRequest& req_list); private: - //! Node handle - ros::NodeHandle nh_; + //! Node + const rclcpp::Node::SharedPtr node_; //! Robot model moveit::core::RobotModelConstPtr model_; @@ -214,7 +219,7 @@ class CommandListManager PlanComponentsBuilder plan_comp_builder_; }; -inline void CommandListManager::checkLastBlendRadiusZero(const moveit_msgs::MotionSequenceRequest& req_list) +inline void CommandListManager::checkLastBlendRadiusZero(const moveit_msgs::msg::MotionSequenceRequest& req_list) { if (req_list.items.back().blend_radius != 0.0) { diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_aggregator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_aggregator.h index 0f60b353e5..5bdb151304 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_aggregator.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_aggregator.h @@ -37,7 +37,7 @@ #include "pilz_industrial_motion_planner/joint_limits_container.h" #include "pilz_industrial_motion_planner/joint_limits_extension.h" -#include +#include #include #include @@ -72,12 +72,13 @@ class JointLimitsAggregator * @note The acceleration/deceleration can only be set via the parameter * server since they are not supported * in the urdf so far. - * @param nh Node handle in whose namespace the joint limit parameters are - * expected. + * @param node Node to use for accessing joint limit parameters + * @param param_namespace Namespace to use for looking up node parameters * @param joint_models The joint models * @return Container containing the limits */ - static JointLimitsContainer getAggregatedLimits(const ros::NodeHandle& nh, + static JointLimitsContainer getAggregatedLimits(const rclcpp::Node::SharedPtr& node, + const std::string& param_namespace, const std::vector& joint_models); protected: diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_extension.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_extension.h index 789cafa88e..bae6d2671a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_extension.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_extension.h @@ -35,7 +35,62 @@ #ifndef JOINT_LIMITS_EXTENSION_H #define JOINT_LIMITS_EXTENSION_H -#include +// TODO(henning): Re-include when this is available, until then the headers content is copied below +// #include +///////////////////////////////////////////////////// +// start of +// ////////////////////////////////////////////////// +namespace joint_limits_interface +{ +struct JointLimits +{ + JointLimits() + : min_position(0.0) + , max_position(0.0) + , max_velocity(0.0) + , max_acceleration(0.0) + , max_jerk(0.0) + , max_effort(0.0) + , has_position_limits(false) + , has_velocity_limits(false) + , has_acceleration_limits(false) + , has_jerk_limits(false) + , has_effort_limits(false) + , angle_wraparound(false) + { + } + + double min_position; + double max_position; + double max_velocity; + double max_acceleration; + double max_jerk; + double max_effort; + + bool has_position_limits; + bool has_velocity_limits; + bool has_acceleration_limits; + bool has_jerk_limits; + bool has_effort_limits; + bool angle_wraparound; +}; + +struct SoftJointLimits +{ + SoftJointLimits() : min_position(0.0), max_position(0.0), k_position(0.0), k_velocity(0.0) + { + } + + double min_position; + double max_position; + double k_position; + double k_velocity; +}; +} // namespace joint_limits_interface +/////////////////////////////////////////////////// +// end of +// //////////////////////////////////////////////// + #include #include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h index 4791267e92..6743e1c06d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h @@ -36,62 +36,274 @@ #define JOINT_LIMITS_INTERFACE_EXTENSION_H #include "pilz_industrial_motion_planner/joint_limits_extension.h" -#include -namespace pilz_industrial_motion_planner -{ +// TODO(henning): Re-include when this is available, until then the headers content is copied below +// #include +////////////////////////////////////////////////////////////// +// start of +////////////////////////////////////////////////////////////// +/// Populate a JointLimits instance from the ROS parameter server. +/** + * It is assumed that the following parameter structure is followed on the provided NodeHandle. Unspecified parameters + * are simply not added to the joint limits specification. + * \code + * joint_limits: + * foo_joint: + * has_position_limits: true + * min_position: 0.0 + * max_position: 1.0 + * has_velocity_limits: true + * max_velocity: 2.0 + * has_acceleration_limits: true + * max_acceleration: 5.0 + * has_jerk_limits: true + * max_jerk: 100.0 + * has_effort_limits: true + * max_effort: 20.0 + * bar_joint: + * has_position_limits: false # Continuous joint + * has_velocity_limits: true + * max_velocity: 4.0 + * \endcode + * + * This specification is similar to the one used by MoveIt!, + * but additionally supports jerk and effort limits. + * + * \param[in] joint_name Name of joint whose limits are to be fetched. + * \param[in] node NodeHandle where the joint limits are specified. + * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite + * existing values. Values in \p limits not specified in the parameter server remain unchanged. + * \return True if a limits specification is found (ie. the \p joint_limits/joint_name parameter exists in \p node), + * false otherwise. + */ namespace joint_limits_interface { +inline bool getJointLimits(const std::string& joint_name, const rclcpp::Node::SharedPtr& node, JointLimits& limits) +{ + const std::string param_base_name = "joint_limits." + joint_name; + try + { + if (!node->has_parameter(param_base_name + ".has_position_limits") && + !node->has_parameter(param_base_name + ".min_position") && + !node->has_parameter(param_base_name + ".max_position") && + !node->has_parameter(param_base_name + ".has_velocity_limits") && + !node->has_parameter(param_base_name + ".min_velocity") && + !node->has_parameter(param_base_name + ".max_velocity") && + !node->has_parameter(param_base_name + ".has_acceleration_limits") && + !node->has_parameter(param_base_name + ".max_acceleration") && + !node->has_parameter(param_base_name + ".has_jerk_limits") && + !node->has_parameter(param_base_name + ".max_jerk") && + !node->has_parameter(param_base_name + ".has_effort_limits") && + !node->has_parameter(param_base_name + ".max_effort") && + !node->has_parameter(param_base_name + ".angle_wraparound") && + !node->has_parameter(param_base_name + ".has_soft_limits") && + !node->has_parameter(param_base_name + ".k_position") && + !node->has_parameter(param_base_name + ".k_velocity") && + !node->has_parameter(param_base_name + ".soft_lower_limit") && + !node->has_parameter(param_base_name + ".soft_upper_limit")) + { + RCLCPP_ERROR_STREAM(node->get_logger(), "No joint limits specification found for joint '" + << joint_name << "' in the parameter server (node: " + << std::string(node->get_name()) + " param name: " + param_base_name + << ")."); + return false; + } + } + catch (const std::exception& ex) + { + RCLCPP_ERROR_STREAM(node->get_logger(), ex.what()); + return false; + } + + // Position limits + bool has_position_limits = false; + if (node->get_parameter(param_base_name + ".has_position_limits", has_position_limits)) + { + if (!has_position_limits) + { + limits.has_position_limits = false; + } + double min_pos, max_pos; + if (has_position_limits && node->get_parameter(param_base_name + ".min_position", min_pos) && + node->get_parameter(param_base_name + ".max_position", max_pos)) + { + limits.has_position_limits = true; + limits.min_position = min_pos; + limits.max_position = max_pos; + } + + bool angle_wraparound; + if (!has_position_limits && node->get_parameter(param_base_name + ".angle_wraparound", angle_wraparound)) + { + limits.angle_wraparound = angle_wraparound; + } + } + + // Velocity limits + bool has_velocity_limits = false; + if (node->get_parameter(param_base_name + ".has_velocity_limits", has_velocity_limits)) + { + if (!has_velocity_limits) + { + limits.has_velocity_limits = false; + } + double max_vel; + if (has_velocity_limits && node->get_parameter(param_base_name + ".max_velocity", max_vel)) + { + limits.has_velocity_limits = true; + limits.max_velocity = max_vel; + } + } + + // Acceleration limits + bool has_acceleration_limits = false; + if (node->get_parameter(param_base_name + ".has_acceleration_limits", has_acceleration_limits)) + { + if (!has_acceleration_limits) + { + limits.has_acceleration_limits = false; + } + double max_acc; + if (has_acceleration_limits && node->get_parameter(param_base_name + ".max_acceleration", max_acc)) + { + limits.has_acceleration_limits = true; + limits.max_acceleration = max_acc; + } + } + + // Jerk limits + bool has_jerk_limits = false; + if (node->get_parameter(param_base_name + ".has_jerk_limits", has_jerk_limits)) + { + if (!has_jerk_limits) + { + limits.has_jerk_limits = false; + } + double max_jerk; + if (has_jerk_limits && node->get_parameter(param_base_name + ".max_jerk", max_jerk)) + { + limits.has_jerk_limits = true; + limits.max_jerk = max_jerk; + } + } + + // Effort limits + bool has_effort_limits = false; + if (node->get_parameter(param_base_name + ".has_effort_limits", has_effort_limits)) + { + if (!has_effort_limits) + { + limits.has_effort_limits = false; + } + double max_effort; + if (has_effort_limits && node->get_parameter(param_base_name + ".max_effort", max_effort)) + { + limits.has_effort_limits = true; + limits.max_effort = max_effort; + } + } + + return true; +} + +/// Populate a SoftJointLimits instance from the ROS parameter server. /** - * @see joint_limits_inteface::getJointLimits(...) + * It is assumed that the following parameter structure is followed on the provided NodeHandle. Only completely + * specified soft joint limits specifications will be considered valid. \code joint_limits: foo_joint: soft_lower_limit: + * 0.0 soft_upper_limit: 1.0 k_position: 10.0 k_velocity: 10.0 \endcode + * + * This specification is similar to the specification of the safety_controller tag in the URDF, adapted to the parameter + * server. + * + * \param[in] joint_name Name of joint whose limits are to be fetched. + * \param[in] node NodeHandle where the joint limits are specified. + * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will + * overwrite existing values. \return True if a complete soft limits specification is found (ie. if all \p k_position, + * \p k_velocity, \p soft_lower_limit and \p soft_upper_limit exist in \p joint_limits/joint_name namespace), false + * otherwise. */ -inline bool getJointLimits(const std::string& joint_name, const ros::NodeHandle& nh, - joint_limits_interface::JointLimits& limits) +inline bool getSoftJointLimits(const std::string& joint_name, const rclcpp::Node::SharedPtr& node, + SoftJointLimits& soft_limits) { - // Node handle scoped where the joint limits are - // defined (copied from ::joint_limits_interface::getJointLimits(joint_name, - // nh, limits) - ros::NodeHandle limits_nh; + const std::string param_base_name = "joint_limits." + joint_name; try { - const std::string limits_namespace = "joint_limits/" + joint_name; - if (!nh.hasParam(limits_namespace)) + if (!node->has_parameter(param_base_name + ".has_soft_limits") && + !node->has_parameter(param_base_name + ".k_velocity") && + !node->has_parameter(param_base_name + ".k_position") && + !node->has_parameter(param_base_name + ".soft_lower_limit") && + !node->has_parameter(param_base_name + ".soft_upper_limit")) { - ROS_DEBUG_STREAM("No joint limits specification found for joint '" - << joint_name << "' in the parameter server (namespace " - << nh.getNamespace() + "/" + limits_namespace << ")."); + RCLCPP_DEBUG_STREAM(node->get_logger(), "No soft joint limits specification found for joint '" + << joint_name << "' in the parameter server (node: " + << std::string(node->get_name()) + " param name: " + param_base_name + << ")."); return false; } - limits_nh = ros::NodeHandle(nh, limits_namespace); } - catch (const ros::InvalidNameException& ex) + catch (const std::exception& ex) { - ROS_ERROR_STREAM(ex.what()); + RCLCPP_ERROR_STREAM(node->get_logger(), ex.what()); return false; } + // Override soft limits if complete specification is found + bool has_soft_limits; + if (node->get_parameter(param_base_name + ".has_soft_limits", has_soft_limits)) + { + if (has_soft_limits && node->has_parameter(param_base_name + ".k_position") && + node->has_parameter(param_base_name + ".k_velocity") && + node->has_parameter(param_base_name + ".soft_lower_limit") && + node->has_parameter(param_base_name + ".soft_upper_limit")) + { + node->get_parameter(param_base_name + ".k_position", soft_limits.k_position); + node->get_parameter(param_base_name + ".k_velocity", soft_limits.k_velocity); + node->get_parameter(param_base_name + ".soft_lower_limit", soft_limits.min_position); + node->get_parameter(param_base_name + ".soft_upper_limit", soft_limits.max_position); + return true; + } + } + + return false; +} +} // namespace joint_limits_interface +////////////////////////////////////////////////////////////// +// end of +////////////////////////////////////////////////////////////// + +namespace pilz_industrial_motion_planner +{ +namespace joint_limits_interface +{ +/** + * @see joint_limits_inteface::getJointLimits(...) + */ +inline bool getJointLimits(const std::string& joint_name, const rclcpp::Node::SharedPtr& node, + joint_limits_interface::JointLimits& limits) +{ // Set the existing limits - if (!::joint_limits_interface::getJointLimits(joint_name, nh, limits)) + if (!::joint_limits_interface::getJointLimits(joint_name, node, limits)) { return false; // LCOV_EXCL_LINE // The case where getJointLimits returns // false is covered above. } - // Deceleration limits - bool has_deceleration_limits = false; - if (limits_nh.getParam("has_deceleration_limits", has_deceleration_limits)) + try { - if (!has_deceleration_limits) - { - limits.has_deceleration_limits = false; - } - double max_dec; - if (has_deceleration_limits && limits_nh.getParam("max_deceleration", max_dec)) + // Deceleration limits + const std::string limits_namespace = "joint_limits." + joint_name; + limits.has_deceleration_limits = node->declare_parameter(limits_namespace + ".has_deceleration_limits", false); + if (limits.has_deceleration_limits) { - limits.has_deceleration_limits = true; - limits.max_deceleration = max_dec; + limits.max_deceleration = + node->declare_parameter(limits_namespace + ".max_deceleration", limits.max_deceleration); } } + catch (const std::exception& ex) + { + RCLCPP_WARN_STREAM(node->get_logger(), "Failed loading deceleration limits"); + limits.has_deceleration_limits = false; + } return true; } diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_action.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_action.h index 543c701579..1e21701ad2 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_action.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_action.h @@ -36,10 +36,10 @@ #include -#include +#include #include -#include +#include namespace pilz_industrial_motion_planner { @@ -59,21 +59,23 @@ class MoveGroupSequenceAction : public move_group::MoveGroupCapability private: using ExecutableTrajs = std::vector; - using StartStateMsg = moveit_msgs::MotionSequenceResponse::_sequence_start_type; - using StartStatesMsg = std::vector; - using PlannedTrajMsgs = moveit_msgs::MotionSequenceResponse::_planned_trajectories_type; + using StartStateMsg = moveit_msgs::msg::MotionSequenceResponse::_sequence_start_type; + using StartStatesMsg = std::vector; + using PlannedTrajMsgs = moveit_msgs::msg::MotionSequenceResponse::_planned_trajectories_type; private: - void executeSequenceCallback(const moveit_msgs::MoveGroupSequenceGoalConstPtr& goal); - void executeSequenceCallbackPlanAndExecute(const moveit_msgs::MoveGroupSequenceGoalConstPtr& goal, - moveit_msgs::MoveGroupSequenceResult& action_res); - void executeMoveCallbackPlanOnly(const moveit_msgs::MoveGroupSequenceGoalConstPtr& goal, - moveit_msgs::MoveGroupSequenceResult& res); + void executeSequenceCallback( + const std::shared_ptr> goal_handle); + void + executeSequenceCallbackPlanAndExecute(const moveit_msgs::action::MoveGroupSequence::Goal::ConstSharedPtr& goal, + const moveit_msgs::action::MoveGroupSequence::Result::SharedPtr& action_res); + void executeMoveCallbackPlanOnly(const moveit_msgs::action::MoveGroupSequence::Goal::ConstSharedPtr& goal, + const moveit_msgs::action::MoveGroupSequence::Result::SharedPtr& res); void startMoveExecutionCallback(); void startMoveLookCallback(); void preemptMoveCallback(); void setMoveState(move_group::MoveGroupState state); - bool planUsingSequenceManager(const moveit_msgs::MotionSequenceRequest& req, + bool planUsingSequenceManager(const moveit_msgs::msg::MotionSequenceRequest& req, plan_execution::ExecutableMotionPlan& plan); private: @@ -81,8 +83,9 @@ class MoveGroupSequenceAction : public move_group::MoveGroupCapability PlannedTrajMsgs& plannedTrajsMsgs); private: - std::unique_ptr> move_action_server_; - moveit_msgs::MoveGroupSequenceFeedback move_feedback_; + std::shared_ptr> move_action_server_; + std::shared_ptr> goal_handle_; + moveit_msgs::action::MoveGroupSequence::Feedback::SharedPtr move_feedback_; move_group::MoveGroupState move_state_{ move_group::IDLE }; std::unique_ptr command_list_manager_; diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_service.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_service.h index 54c0af0f61..97b23120df 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_service.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_service.h @@ -34,9 +34,11 @@ #pragma once +#include + #include -#include +#include namespace pilz_industrial_motion_planner { @@ -56,10 +58,11 @@ class MoveGroupSequenceService : public move_group::MoveGroupCapability void initialize() override; private: - bool plan(moveit_msgs::GetMotionSequence::Request& req, moveit_msgs::GetMotionSequence::Response& res); + bool plan(const moveit_msgs::srv::GetMotionSequence::Request::SharedPtr req, + moveit_msgs::srv::GetMotionSequence::Response::SharedPtr res); private: - ros::ServiceServer sequence_service_; + rclcpp::Service::SharedPtr sequence_service_; std::unique_ptr command_list_manager_; }; diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.h index 5fa03f73b9..4cc430bbcf 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.h @@ -34,7 +34,7 @@ #pragma once -#include +#include #include "pilz_industrial_motion_planner/joint_limits_extension.h" #include "pilz_industrial_motion_planner/planning_context_loader.h" @@ -69,10 +69,12 @@ class CommandPlanner : public planning_interface::PlannerManager * Upon initialization this planner will look for plugins implementing * pilz_industrial_motion_planner::PlanningContextLoader. * @param model The robot model + * @param node The node * @param ns The namespace * @return true on success, false otherwise */ - bool initialize(const robot_model::RobotModelConstPtr& model, const std::string& ns) override; + bool initialize(const moveit::core::RobotModelConstPtr& model, const rclcpp::Node::SharedPtr& node, + const std::string& ns) override; /// Description of the planner std::string getDescription() const override; @@ -94,9 +96,10 @@ class CommandPlanner : public planning_interface::PlannerManager * @param error_code * @return */ - planning_interface::PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - moveit_msgs::MoveItErrorCodes& error_code) const override; + planning_interface::PlanningContextPtr + getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + moveit_msgs::msg::MoveItErrorCodes& error_code) const override; /** * @brief Checks if the request can be handled diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/plan_components_builder.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/plan_components_builder.h index a89b3fbbd3..5d7e397862 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/plan_components_builder.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/plan_components_builder.h @@ -50,10 +50,10 @@ namespace pilz_industrial_motion_planner using TipFrameFunc_t = std::function; // List of exceptions which can be thrown by the PlanComponentsBuilder class. -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoBlenderSetException, moveit_msgs::MoveItErrorCodes::FAILURE); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoTipFrameFunctionSetException, moveit_msgs::MoveItErrorCodes::FAILURE); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoRobotModelSetException, moveit_msgs::MoveItErrorCodes::FAILURE); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(BlendingFailedException, moveit_msgs::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoBlenderSetException, moveit_msgs::msg::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoTipFrameFunctionSetException, moveit_msgs::msg::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoRobotModelSetException, moveit_msgs::msg::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(BlendingFailedException, moveit_msgs::msg::MoveItErrorCodes::FAILURE); /** * @brief Helper class to encapsulate the merge and blend process of diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h index 14143a0ed8..67ac0184b4 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h @@ -37,7 +37,7 @@ #include "pilz_industrial_motion_planner/joint_limits_container.h" #include "pilz_industrial_motion_planner/trajectory_generator.h" -#include +#include #include #include @@ -107,7 +107,7 @@ class PlanningContextBase : public planning_interface::PlanningContext std::atomic_bool terminated_; /// The robot model - robot_model::RobotModelConstPtr model_; + moveit::core::RobotModelConstPtr model_; /// Joint limits to be used during planning pilz_industrial_motion_planner::LimitsContainer limits_; @@ -124,18 +124,19 @@ bool pilz_industrial_motion_planner::PlanningContextBase::solve(plan // Use current state as start state if not set if (request_.start_state.joint_state.name.empty()) { - moveit_msgs::RobotState current_state; + moveit_msgs::msg::RobotState current_state; moveit::core::robotStateToRobotStateMsg(getPlanningScene()->getCurrentState(), current_state); request_.start_state = current_state; } bool result = generator_.generate(request_, res); return result; - // res.error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN; + // res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; // return false; // TODO } - ROS_ERROR("Using solve on a terminated planning context!"); - res.error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED; + RCLCPP_ERROR(rclcpp::get_logger("moveit.pilz_industrial_motion_planner.planning_context_base"), + "Using solve on a terminated planning context!"); + res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; return false; } @@ -166,7 +167,8 @@ bool pilz_industrial_motion_planner::PlanningContextBase::solve( template bool pilz_industrial_motion_planner::PlanningContextBase::terminate() { - ROS_DEBUG_STREAM("Terminate called"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("moveit.pilz_industrial_motion_planner.planning_context_base"), + "Terminate called"); terminated_ = true; return true; } diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.h index a205780538..152c4c97fb 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.h @@ -36,7 +36,7 @@ #include "pilz_industrial_motion_planner/limits_container.h" -#include +#include #include #include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.h index 25ce821d12..9fe48e7428 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.h @@ -34,7 +34,7 @@ #include "pilz_industrial_motion_planner/limits_container.h" -#include +#include #include #include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.h index bb0839ed1e..6a2c4328f2 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.h @@ -114,8 +114,8 @@ class PlanningContextLoader moveit::core::RobotModelConstPtr model_; }; -typedef boost::shared_ptr PlanningContextLoaderPtr; -typedef boost::shared_ptr PlanningContextLoaderConstPtr; +typedef std::shared_ptr PlanningContextLoaderPtr; +typedef std::shared_ptr PlanningContextLoaderConstPtr; template bool PlanningContextLoader::loadContext(planning_interface::PlanningContextPtr& planning_context, @@ -130,12 +130,12 @@ bool PlanningContextLoader::loadContext(planning_interface::PlanningContextPtr& { if (!limits_set_) { - ROS_ERROR_STREAM("Limits are not defined. Cannot load planning context. " - "Call setLimits loadContext"); + RCLCPP_ERROR(rclcpp::get_logger("planning_context_loader"), + "Limits are not defined. Cannot load planning context. Call setLimits loadContext"); } if (!model_set_) { - ROS_ERROR_STREAM("Robot model was not set"); + RCLCPP_ERROR(rclcpp::get_logger("planning_context_loader"), "Robot model was not set"); } return false; } diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.h index c10b780753..584a6f0767 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.h @@ -36,7 +36,7 @@ #include "pilz_industrial_motion_planner/limits_container.h" -#include +#include #include #include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/tip_frame_getter.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/tip_frame_getter.h index e173bbde37..0cc8c13f7a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/tip_frame_getter.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/tip_frame_getter.h @@ -43,8 +43,8 @@ namespace pilz_industrial_motion_planner { -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoSolverException, moveit_msgs::MoveItErrorCodes::FAILURE); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(MoreThanOneTipFrameException, moveit_msgs::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoSolverException, moveit_msgs::msg::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(MoreThanOneTipFrameException, moveit_msgs::msg::MoveItErrorCodes::FAILURE); /** * @returns true if the JointModelGroup has a solver, false otherwise. diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_response.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_response.h index 880f2b777e..3f518e5737 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_response.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_response.h @@ -51,7 +51,7 @@ struct TrajectoryBlendResponse robot_trajectory::RobotTrajectoryPtr second_trajectory; // Error code - moveit_msgs::MoveItErrorCodes error_code; + moveit_msgs::msg::MoveItErrorCodes error_code; }; } // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.h index 233bdd3e31..cf1ba6fb29 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.h @@ -104,7 +104,7 @@ class TrajectoryBlenderTransitionWindow : public TrajectoryBlender * @return */ bool validateRequest(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, double& sampling_time, - moveit_msgs::MoveItErrorCodes& error_code) const; + moveit_msgs::msg::MoveItErrorCodes& error_code) const; /** * @brief searchBlendPoint * @param req: trajectory blend request diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h index cb600259b4..07d05d0ff6 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include "pilz_industrial_motion_planner/cartesian_trajectory.h" #include "pilz_industrial_motion_planner/limits_container.h" @@ -62,13 +62,13 @@ namespace pilz_industrial_motion_planner * @param timeout: timeout for IK, if not set the default solver timeout is used * @return true if succeed */ -bool computePoseIK(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name, +bool computePoseIK(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name, const std::string& link_name, const Eigen::Isometry3d& pose, const std::string& frame_id, const std::map& seed, std::map& solution, bool check_self_collision = true, const double timeout = 0.0); -bool computePoseIK(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name, - const std::string& link_name, const geometry_msgs::Pose& pose, const std::string& frame_id, +bool computePoseIK(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name, + const std::string& link_name, const geometry_msgs::msg::Pose& pose, const std::string& frame_id, const std::map& seed, std::map& solution, bool check_self_collision = true, const double timeout = 0.0); @@ -80,10 +80,10 @@ bool computePoseIK(const robot_model::RobotModelConstPtr& robot_model, const std * @param pose: pose of the link in base frame of robot model * @return true if succeed */ -bool computeLinkFK(const robot_model::RobotModelConstPtr& robot_model, const std::string& link_name, +bool computeLinkFK(const moveit::core::RobotModelConstPtr& robot_model, const std::string& link_name, const std::map& joint_state, Eigen::Isometry3d& pose); -bool computeLinkFK(const robot_model::RobotModelConstPtr& robot_model, const std::string& link_name, +bool computeLinkFK(const moveit::core::RobotModelConstPtr& robot_model, const std::string& link_name, const std::vector& joint_names, const std::vector& joint_positions, Eigen::Isometry3d& pose); @@ -122,12 +122,12 @@ bool verifySampleJointLimits(const std::map& position_last, * @param check_self_collision: check for self collision during creation * @return true if succeed */ -bool generateJointTrajectory(const robot_model::RobotModelConstPtr& robot_model, +bool generateJointTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const JointLimitsContainer& joint_limits, const KDL::Trajectory& trajectory, const std::string& group_name, const std::string& link_name, const std::map& initial_joint_position, const double& sampling_time, - trajectory_msgs::JointTrajectory& joint_trajectory, - moveit_msgs::MoveItErrorCodes& error_code, bool check_self_collision = false); + trajectory_msgs::msg::JointTrajectory& joint_trajectory, + moveit_msgs::msg::MoveItErrorCodes& error_code, bool check_self_collision = false); /** * @brief Generate joint trajectory from a MultiDOFJointTrajectory @@ -138,14 +138,14 @@ bool generateJointTrajectory(const robot_model::RobotModelConstPtr& robot_model, * @param error_code * @return true if succeed */ -bool generateJointTrajectory(const robot_model::RobotModelConstPtr& robot_model, +bool generateJointTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const JointLimitsContainer& joint_limits, const pilz_industrial_motion_planner::CartesianTrajectory& trajectory, const std::string& group_name, const std::string& link_name, const std::map& initial_joint_position, const std::map& initial_joint_velocity, - trajectory_msgs::JointTrajectory& joint_trajectory, - moveit_msgs::MoveItErrorCodes& error_code, bool check_self_collision = false); + trajectory_msgs::msg::JointTrajectory& joint_trajectory, + moveit_msgs::msg::MoveItErrorCodes& error_code, bool check_self_collision = false); /** * @brief Determines the sampling time and checks that both trajectroies use the @@ -170,7 +170,7 @@ bool determineAndCheckSamplingTime(const robot_trajectory::RobotTrajectoryPtr& f * @return True if joint positions, joint velocities and joint accelerations are * equal, otherwise false. */ -bool isRobotStateEqual(const robot_state::RobotState& state1, const robot_state::RobotState& state2, +bool isRobotStateEqual(const moveit::core::RobotState& state1, const moveit::core::RobotState& state2, const std::string& joint_group_name, double epsilon); /** @@ -180,7 +180,7 @@ bool isRobotStateEqual(const robot_state::RobotState& state1, const robot_state: * @param EPSILON * @return */ -bool isRobotStateStationary(const robot_state::RobotState& state, const std::string& group, double EPSILON); +bool isRobotStateStationary(const moveit::core::RobotState& state, const std::string& group, double EPSILON); /** * @brief Performs a linear search for the intersection point of the trajectory @@ -210,8 +210,8 @@ bool intersectionFound(const Eigen::Vector3d& p_center, const Eigen::Vector3d& p * @return */ bool isStateColliding(const bool test_for_self_collision, const moveit::core::RobotModelConstPtr& robot_model, - robot_state::RobotState* state, const robot_state::JointModelGroup* const group, + moveit::core::RobotState* state, const moveit::core::JointModelGroup* const group, const double* const ik_solution); } // namespace pilz_industrial_motion_planner -void normalizeQuaternion(geometry_msgs::Quaternion& quat); +void normalizeQuaternion(geometry_msgs::msg::Quaternion& quat); diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generation_exceptions.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generation_exceptions.h index 8348850687..daad273bf9 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generation_exceptions.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generation_exceptions.h @@ -37,12 +37,12 @@ #include #include -#include +#include namespace pilz_industrial_motion_planner { /** - * @brief Exception storing an moveit_msgs::MoveItErrorCodes value. + * @brief Exception storing an moveit_msgs::msg::MoveItErrorCodes value. */ class MoveItErrorCodeException : public std::runtime_error { @@ -58,21 +58,22 @@ class MoveItErrorCodeException : public std::runtime_error MoveItErrorCodeException& operator=(MoveItErrorCodeException&&) = default; public: - virtual const moveit_msgs::MoveItErrorCodes::_val_type& getErrorCode() const = 0; + virtual const moveit_msgs::msg::MoveItErrorCodes::_val_type& getErrorCode() const = 0; }; -template +template class TemplatedMoveItErrorCodeException : public MoveItErrorCodeException { public: TemplatedMoveItErrorCodeException(const std::string& msg); - TemplatedMoveItErrorCodeException(const std::string& msg, const moveit_msgs::MoveItErrorCodes::_val_type& error_code); + TemplatedMoveItErrorCodeException(const std::string& msg, + const moveit_msgs::msg::MoveItErrorCodes::_val_type& error_code); public: - const moveit_msgs::MoveItErrorCodes::_val_type& getErrorCode() const override; + const moveit_msgs::msg::MoveItErrorCodes::_val_type& getErrorCode() const override; private: - const moveit_msgs::MoveItErrorCodes::_val_type error_code_{ ERROR_CODE }; + const moveit_msgs::msg::MoveItErrorCodes::_val_type error_code_{ ERROR_CODE }; }; //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ @@ -80,21 +81,21 @@ inline MoveItErrorCodeException::MoveItErrorCodeException(const std::string& msg { } -template +template inline TemplatedMoveItErrorCodeException::TemplatedMoveItErrorCodeException(const std::string& msg) : MoveItErrorCodeException(msg) { } -template +template inline TemplatedMoveItErrorCodeException::TemplatedMoveItErrorCodeException( - const std::string& msg, const moveit_msgs::MoveItErrorCodes::_val_type& error_code) + const std::string& msg, const moveit_msgs::msg::MoveItErrorCodes::_val_type& error_code) : MoveItErrorCodeException(msg), error_code_(error_code) { } -template -inline const moveit_msgs::MoveItErrorCodes::_val_type& +template +inline const moveit_msgs::msg::MoveItErrorCodes::_val_type& TemplatedMoveItErrorCodeException::getErrorCode() const { return error_code_; @@ -112,7 +113,7 @@ TemplatedMoveItErrorCodeException::getErrorCode() const { \ } \ \ - EXCEPTION_CLASS_NAME(const std::string& msg, const moveit_msgs::MoveItErrorCodes::_val_type& error_code) \ + EXCEPTION_CLASS_NAME(const std::string& msg, const moveit_msgs::msg::MoveItErrorCodes::_val_type& error_code) \ : TemplatedMoveItErrorCodeException(msg, error_code) \ { \ } \ diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h index 09055fcb81..1b0dda72b4 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h @@ -52,34 +52,38 @@ using namespace pilz_industrial_motion_planner; namespace pilz_industrial_motion_planner { -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(TrajectoryGeneratorInvalidLimitsException, moveit_msgs::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(TrajectoryGeneratorInvalidLimitsException, + moveit_msgs::msg::MoveItErrorCodes::FAILURE); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(VelocityScalingIncorrect, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(AccelerationScalingIncorrect, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(UnknownPlanningGroup, moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(VelocityScalingIncorrect, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(AccelerationScalingIncorrect, + moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(UnknownPlanningGroup, moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoJointNamesInStartState, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(SizeMismatchInStartState, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointsOfStartStateOutOfRange, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NonZeroVelocityInStartState, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoJointNamesInStartState, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(SizeMismatchInStartState, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointsOfStartStateOutOfRange, + moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NonZeroVelocityInStartState, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NotExactlyOneGoalConstraintGiven, - moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(OnlyOneGoalTypeAllowed, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(OnlyOneGoalTypeAllowed, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(StartStateGoalStateMismatch, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(StartStateGoalStateMismatch, + moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointConstraintDoesNotBelongToGroup, - moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointsOfGoalOutOfRange, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointsOfGoalOutOfRange, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PositionConstraintNameMissing, - moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); CREATE_MOVEIT_ERROR_CODE_EXCEPTION(OrientationConstraintNameMissing, - moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PositionOrientationConstraintNameMismatch, - moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoIKSolverAvailable, moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoPrimitivePoseGiven, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoIKSolverAvailable, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoPrimitivePoseGiven, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); /** * @brief Base class of trajectory generators @@ -89,9 +93,9 @@ CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoPrimitivePoseGiven, moveit_msgs::MoveItErro class TrajectoryGenerator { public: - TrajectoryGenerator(const robot_model::RobotModelConstPtr& robot_model, + TrajectoryGenerator(const moveit::core::RobotModelConstPtr& robot_model, const pilz_industrial_motion_planner::LimitsContainer& planner_limits) - : robot_model_(robot_model), planner_limits_(planner_limits) + : robot_model_(robot_model), planner_limits_(planner_limits), clock_(std::make_unique()) { } @@ -151,7 +155,7 @@ class TrajectoryGenerator virtual void extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const = 0; virtual void plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, - const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) = 0; + const double& sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) = 0; private: /** @@ -159,40 +163,40 @@ class TrajectoryGenerator * trajectroy generator * Checks that: * - req.max_velocity_scaling_factor [0.0001, 1], - * moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN on failure + * moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN on failure * - req.max_acceleration_scaling_factor [0.0001, 1] , - * moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN on + * moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN on * failure * - req.group_name is a JointModelGroup of the Robotmodel, - * moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME on + * moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME on * failure * - req.start_state.joint_state is not empty, - * moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE on failure + * moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE on failure * - req.start_state.joint_state is within the limits, - * moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE on + * moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE on * failure * - req.start_state.joint_state is all zero, - * moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE on failure + * moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE on failure * - req.goal_constraints must have exactly 1 defined cartesian oder joint * constraint - * moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure + * moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure * A joint goal is checked for: * - StartState joint-names matching goal joint-names, - * moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on + * moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on * failure * - Beeing defined in the req.group_name JointModelGroup * - Beeing with the defined limits * A cartesian goal is checked for: * - A defined link_name for the constraint, - * moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure + * moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure * - Matching link_name for position and orientation constraints, - * moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure + * moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure * - A IK solver exists for the given req.group_name and constraint * link_name, - * moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION on failure + * moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION on failure * - A goal pose define in * position_constraints[0].constraint_region.primitive_poses, - * moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure + * moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure * @param req: motion plan request */ void validateRequest(const planning_interface::MotionPlanRequest& req) const; @@ -200,11 +204,11 @@ class TrajectoryGenerator /** * @brief set MotionPlanResponse from joint trajectory */ - void setSuccessResponse(const std::string& group_name, const moveit_msgs::RobotState& start_state, - const trajectory_msgs::JointTrajectory& joint_trajectory, const ros::Time& planning_start, - planning_interface::MotionPlanResponse& res) const; + void setSuccessResponse(const std::string& group_name, const moveit_msgs::msg::RobotState& start_state, + const trajectory_msgs::msg::JointTrajectory& joint_trajectory, + const rclcpp::Time& planning_start, planning_interface::MotionPlanResponse& res) const; - void setFailureResponse(const ros::Time& planning_start, planning_interface::MotionPlanResponse& res) const; + void setFailureResponse(const rclcpp::Time& planning_start, planning_interface::MotionPlanResponse& res) const; void checkForValidGroupName(const std::string& group_name) const; @@ -219,19 +223,20 @@ class TrajectoryGenerator * - The start state velocity is below * TrajectoryGenerator::VELOCITY_TOLERANCE */ - void checkStartState(const moveit_msgs::RobotState& start_state) const; + void checkStartState(const moveit_msgs::msg::RobotState& start_state) const; - void checkGoalConstraints(const moveit_msgs::MotionPlanRequest::_goal_constraints_type& goal_constraints, + void checkGoalConstraints(const moveit_msgs::msg::MotionPlanRequest::_goal_constraints_type& goal_constraints, const std::vector& expected_joint_names, const std::string& group_name) const; - void checkJointGoalConstraint(const moveit_msgs::Constraints& constraint, + void checkJointGoalConstraint(const moveit_msgs::msg::Constraints& constraint, const std::vector& expected_joint_names, const std::string& group_name) const; - void checkCartesianGoalConstraint(const moveit_msgs::Constraints& constraint, const std::string& group_name) const; + void checkCartesianGoalConstraint(const moveit_msgs::msg::Constraints& constraint, + const std::string& group_name) const; - void convertToRobotTrajectory(const trajectory_msgs::JointTrajectory& joint_trajectory, - const moveit_msgs::RobotState& start_state, + void convertToRobotTrajectory(const trajectory_msgs::msg::JointTrajectory& joint_trajectory, + const moveit_msgs::msg::RobotState& start_state, robot_trajectory::RobotTrajectory& robot_trajectory) const; private: @@ -246,25 +251,26 @@ class TrajectoryGenerator * @return True if ONE position + ONE orientation constraint given, * otherwise false. */ - static bool isCartesianGoalGiven(const moveit_msgs::Constraints& constraint); + static bool isCartesianGoalGiven(const moveit_msgs::msg::Constraints& constraint); /** * @return True if joint constraint given, otherwise false. */ - static bool isJointGoalGiven(const moveit_msgs::Constraints& constraint); + static bool isJointGoalGiven(const moveit_msgs::msg::Constraints& constraint); /** * @return True if ONLY joint constraint or * ONLY cartesian constraint (position+orientation) given, otherwise false. */ - static bool isOnlyOneGoalTypeGiven(const moveit_msgs::Constraints& constraint); + static bool isOnlyOneGoalTypeGiven(const moveit_msgs::msg::Constraints& constraint); protected: - const robot_model::RobotModelConstPtr robot_model_; + const moveit::core::RobotModelConstPtr robot_model_; const pilz_industrial_motion_planner::LimitsContainer planner_limits_; static constexpr double MIN_SCALING_FACTOR{ 0.0001 }; static constexpr double MAX_SCALING_FACTOR{ 1. }; static constexpr double VELOCITY_TOLERANCE{ 1e-8 }; + const std::unique_ptr clock_; }; inline bool TrajectoryGenerator::isScalingFactorValid(const double& scaling_factor) @@ -272,17 +278,17 @@ inline bool TrajectoryGenerator::isScalingFactorValid(const double& scaling_fact return (scaling_factor > MIN_SCALING_FACTOR && scaling_factor <= MAX_SCALING_FACTOR); } -inline bool TrajectoryGenerator::isCartesianGoalGiven(const moveit_msgs::Constraints& constraint) +inline bool TrajectoryGenerator::isCartesianGoalGiven(const moveit_msgs::msg::Constraints& constraint) { return constraint.position_constraints.size() == 1 && constraint.orientation_constraints.size() == 1; } -inline bool TrajectoryGenerator::isJointGoalGiven(const moveit_msgs::Constraints& constraint) +inline bool TrajectoryGenerator::isJointGoalGiven(const moveit_msgs::msg::Constraints& constraint) { return !constraint.joint_constraints.empty(); } -inline bool TrajectoryGenerator::isOnlyOneGoalTypeGiven(const moveit_msgs::Constraints& constraint) +inline bool TrajectoryGenerator::isOnlyOneGoalTypeGiven(const moveit_msgs::msg::Constraints& constraint) { return (isJointGoalGiven(constraint) && !isCartesianGoalGiven(constraint)) || (!isJointGoalGiven(constraint) && isCartesianGoalGiven(constraint)); diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h index 080239a581..cb70092dea 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h @@ -44,18 +44,22 @@ using namespace pilz_industrial_motion_planner; namespace pilz_industrial_motion_planner { -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircleNoPlane, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircleToSmall, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CenterPointDifferentRadius, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircTrajectoryConversionFailure, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(UnknownPathConstraintName, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoPositionConstraints, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoPrimitivePose, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircleNoPlane, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircleToSmall, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CenterPointDifferentRadius, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircTrajectoryConversionFailure, + moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(UnknownPathConstraintName, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoPositionConstraints, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoPrimitivePose, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(UnknownLinkNameOfAuxiliaryPoint, moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NumberOfConstraintsMismatch, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircJointMissingInStartState, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircInverseForGoalIncalculable, moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(UnknownLinkNameOfAuxiliaryPoint, + moveit_msgs::msg::MoveItErrorCodes::INVALID_LINK_NAME); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NumberOfConstraintsMismatch, + moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircJointMissingInStartState, + moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircInverseForGoalIncalculable, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); /** * @brief This class implements a trajectory generator of arcs in Cartesian @@ -77,7 +81,7 @@ class TrajectoryGeneratorCIRC : public TrajectoryGenerator * @throw TrajectoryGeneratorInvalidLimitsException * */ - TrajectoryGeneratorCIRC(const robot_model::RobotModelConstPtr& robot_model, + TrajectoryGeneratorCIRC(const moveit::core::RobotModelConstPtr& robot_model, const pilz_industrial_motion_planner::LimitsContainer& planner_limits); private: @@ -86,7 +90,7 @@ class TrajectoryGeneratorCIRC : public TrajectoryGenerator void extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final; void plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, - const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) override; + const double& sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) override; /** * @brief Construct a KDL::Path object for a Cartesian path of an arc. diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h index 2d17768cad..c255bb0e18 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h @@ -46,11 +46,11 @@ namespace pilz_industrial_motion_planner { // TODO date type of units -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinTrajectoryConversionFailure, moveit_msgs::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinTrajectoryConversionFailure, moveit_msgs::msg::MoveItErrorCodes::FAILURE); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointNumberMismatch, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinJointMissingInStartState, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinInverseForGoalIncalculable, moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointNumberMismatch, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinJointMissingInStartState, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinInverseForGoalIncalculable, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); /** * @brief This class implements a linear trajectory generator in Cartesian @@ -66,14 +66,14 @@ class TrajectoryGeneratorLIN : public TrajectoryGenerator * @param model: robot model * @param planner_limits: limits in joint and Cartesian spaces */ - TrajectoryGeneratorLIN(const robot_model::RobotModelConstPtr& robot_model, + TrajectoryGeneratorLIN(const moveit::core::RobotModelConstPtr& robot_model, const pilz_industrial_motion_planner::LimitsContainer& planner_limits); private: void extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final; void plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, - const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) override; + const double& sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) override; /** * @brief construct a KDL::Path object for a Cartesian straight line diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h index 09048557ba..60c99b2ae8 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h @@ -45,8 +45,8 @@ namespace pilz_industrial_motion_planner { // TODO date type of units -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PtpVelocityProfileSyncFailed, moveit_msgs::MoveItErrorCodes::FAILURE); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PtpNoIkSolutionForGoalPose, moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PtpVelocityProfileSyncFailed, moveit_msgs::msg::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PtpNoIkSolutionForGoalPose, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); /** * @brief This class implements a point-to-point trajectory generator based on @@ -60,7 +60,7 @@ class TrajectoryGeneratorPTP : public TrajectoryGenerator * @throw TrajectoryGeneratorInvalidLimitsException * @param model: a map of joint limits information */ - TrajectoryGeneratorPTP(const robot_model::RobotModelConstPtr& robot_model, + TrajectoryGeneratorPTP(const moveit::core::RobotModelConstPtr& robot_model, const pilz_industrial_motion_planner::LimitsContainer& planner_limits); private: @@ -77,12 +77,12 @@ class TrajectoryGeneratorPTP : public TrajectoryGenerator * @param sampling_time */ void planPTP(const std::map& start_pos, const std::map& goal_pos, - trajectory_msgs::JointTrajectory& joint_trajectory, const std::string& group_name, + trajectory_msgs::msg::JointTrajectory& joint_trajectory, const std::string& group_name, const double& velocity_scaling_factor, const double& acceleration_scaling_factor, const double& sampling_time); void plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, - const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) override; + const double& sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) override; private: const double MIN_MOVEMENT = 0.001; diff --git a/moveit_planners/pilz_industrial_motion_planner/package.xml b/moveit_planners/pilz_industrial_motion_planner/package.xml index 3010cd1fc3..d4065026ae 100644 --- a/moveit_planners/pilz_industrial_motion_planner/package.xml +++ b/moveit_planners/pilz_industrial_motion_planner/package.xml @@ -16,34 +16,48 @@ https://github.com/ros-planning/moveit2/issues https://github.com/ros-planning/moveit2 - catkin + ament_cmake + eigen3_cmake_module + eigen3_cmake_module + moveit_common + + geometry_msgs moveit_ros_planning_interface moveit_msgs moveit_core moveit_ros_planning moveit_ros_move_group + + liborocos-kdl-dev pluginlib - roscpp + rclcpp tf2 tf2_eigen tf2_geometry_msgs tf2_kdl + tf2_eigen_kdl tf2_ros + pilz_industrial_motion_planner_testutils + moveit_resources_panda_moveit_config moveit_resources_prbt_moveit_config moveit_resources_prbt_support moveit_resources_prbt_pg70_support - code_coverage + + ament_lint_auto + ament_lint_common diff --git a/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limits_aggregator.cpp index 30dc95ad41..f6684f67a4 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limits_aggregator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limits_aggregator.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include "ros/ros.h" +#include #include "pilz_industrial_motion_planner/cartesian_limits_aggregator.h" @@ -46,51 +46,53 @@ static const std::string PARAM_MAX_ROT_ACC = "max_rot_acc"; static const std::string PARAM_MAX_ROT_DEC = "max_rot_dec"; pilz_industrial_motion_planner::CartesianLimit -pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits(const ros::NodeHandle& nh) +pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits(const rclcpp::Node::SharedPtr& node, + const std::string& param_namespace) { - std::string param_prefix = PARAM_CARTESIAN_LIMITS_NS + "/"; + std::string param_prefix = param_namespace + "." + PARAM_CARTESIAN_LIMITS_NS + "."; pilz_industrial_motion_planner::CartesianLimit cartesian_limit; - // translational velocity - double max_trans_vel; - if (nh.getParam(param_prefix + PARAM_MAX_TRANS_VEL, max_trans_vel)) - { - cartesian_limit.setMaxTranslationalVelocity(max_trans_vel); - } + // TODO(henning): migrate parameters + // // translational velocity + // double max_trans_vel; + // if (nh.getParam(param_prefix + PARAM_MAX_TRANS_VEL, max_trans_vel)) + // { + // cartesian_limit.setMaxTranslationalVelocity(max_trans_vel); + // } - // translational acceleration - double max_trans_acc; - if (nh.getParam(param_prefix + PARAM_MAX_TRANS_ACC, max_trans_acc)) - { - cartesian_limit.setMaxTranslationalAcceleration(max_trans_acc); - } + // // translational acceleration + // double max_trans_acc; + // if (nh.getParam(param_prefix + PARAM_MAX_TRANS_ACC, max_trans_acc)) + // { + // cartesian_limit.setMaxTranslationalAcceleration(max_trans_acc); + // } - // translational deceleration - double max_trans_dec; - if (nh.getParam(param_prefix + PARAM_MAX_TRANS_DEC, max_trans_dec)) - { - cartesian_limit.setMaxTranslationalDeceleration(max_trans_dec); - } + // // translational deceleration + // double max_trans_dec; + // if (nh.getParam(param_prefix + PARAM_MAX_TRANS_DEC, max_trans_dec)) + // { + // cartesian_limit.setMaxTranslationalDeceleration(max_trans_dec); + // } - // rotational velocity - double max_rot_vel; - if (nh.getParam(param_prefix + PARAM_MAX_ROT_VEL, max_rot_vel)) - { - cartesian_limit.setMaxRotationalVelocity(max_rot_vel); - } + // // rotational velocity + // double max_rot_vel; + // if (nh.getParam(param_prefix + PARAM_MAX_ROT_VEL, max_rot_vel)) + // { + // cartesian_limit.setMaxRotationalVelocity(max_rot_vel); + // } - // rotational acceleration + deceleration deprecated - // LCOV_EXCL_START - if (nh.hasParam(param_prefix + PARAM_MAX_ROT_ACC) || nh.hasParam(param_prefix + PARAM_MAX_ROT_DEC)) - { - ROS_WARN_STREAM("Ignoring cartesian limits parameters for rotational " - "acceleration / deceleration;" - << "these parameters are deprecated and are automatically " - "calculated from" - << "translational to rotational ratio."); - } - // LCOV_EXCL_STOP + // // rotational acceleration + deceleration deprecated + // // LCOV_EXCL_START + // if (nh.hasParam(param_prefix + PARAM_MAX_ROT_ACC) || nh.hasParam(param_prefix + PARAM_MAX_ROT_DEC)) + // { + // ROS_WARN_STREAM("Ignoring cartesian limits parameters for rotational " + // "acceleration / deceleration;" + // << "these parameters are deprecated and are automatically " + // "calculated from" + // << "translational to rotational ratio."); + // } + // // LCOV_EXCL_STOP return cartesian_limit; } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp index 52941969d2..8099652140 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp @@ -40,7 +40,7 @@ #include #include -#include +#include #include "pilz_industrial_motion_planner/cartesian_limits_aggregator.h" #include "pilz_industrial_motion_planner/joint_limits_aggregator.h" @@ -51,20 +51,23 @@ namespace pilz_industrial_motion_planner { static const std::string PARAM_NAMESPACE_LIMITS = "robot_description_planning"; +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.command_list_manager"); -CommandListManager::CommandListManager(const ros::NodeHandle& nh, const moveit::core::RobotModelConstPtr& model) - : nh_(nh), model_(model) +CommandListManager::CommandListManager(const rclcpp::Node::SharedPtr& node, + const moveit::core::RobotModelConstPtr& model) + : node_(node), model_(model) { // Obtain the aggregated joint limits pilz_industrial_motion_planner::JointLimitsContainer aggregated_limit_active_joints; + // TODO(henning): pass PARAM_NAMESPACE_LIMITS aggregated_limit_active_joints = pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( - ros::NodeHandle(PARAM_NAMESPACE_LIMITS), model_->getActiveJointModels()); + node_, PARAM_NAMESPACE_LIMITS, model_->getActiveJointModels()); // Obtain cartesian limits + // TODO(henning): pass PARAM_NAMESPACE_LIMITS pilz_industrial_motion_planner::CartesianLimit cartesian_limit = - pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits( - ros::NodeHandle(PARAM_NAMESPACE_LIMITS)); + pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits(node_, PARAM_NAMESPACE_LIMITS); pilz_industrial_motion_planner::LimitsContainer limits; limits.setJointLimits(aggregated_limit_active_joints); @@ -77,7 +80,7 @@ CommandListManager::CommandListManager(const ros::NodeHandle& nh, const moveit:: RobotTrajCont CommandListManager::solve(const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_pipeline::PlanningPipelinePtr& planning_pipeline, - const moveit_msgs::MotionSequenceRequest& req_list) + const moveit_msgs::msg::MotionSequenceRequest& req_list) { if (req_list.items.empty()) { @@ -167,7 +170,7 @@ CommandListManager::getPreviousEndState(const MotionResponseCont& motion_plan_re } void CommandListManager::setStartState(const MotionResponseCont& motion_plan_responses, const std::string& group_name, - moveit_msgs::RobotState& start_state) + moveit_msgs::msg::RobotState& start_state) { RobotState_OptRef rob_state_op{ getPreviousEndState(motion_plan_responses, group_name) }; if (rob_state_op) @@ -177,8 +180,8 @@ void CommandListManager::setStartState(const MotionResponseCont& motion_plan_res } bool CommandListManager::isInvalidBlendRadii(const moveit::core::RobotModel& model, - const moveit_msgs::MotionSequenceItem& item_A, - const moveit_msgs::MotionSequenceItem& item_B) + const moveit_msgs::msg::MotionSequenceItem& item_A, + const moveit_msgs::msg::MotionSequenceItem& item_B) { // Zero blend radius is always valid if (item_A.blend_radius == 0.) @@ -189,31 +192,33 @@ bool CommandListManager::isInvalidBlendRadii(const moveit::core::RobotModel& mod // No blending between different groups if (item_A.req.group_name != item_B.req.group_name) { - ROS_WARN_STREAM("Blending between different groups (in this case: \"" - << item_A.req.group_name << "\" and \"" << item_B.req.group_name << "\") not allowed"); + RCLCPP_WARN_STREAM(LOGGER, "Blending between different groups (in this case: \"" + << item_A.req.group_name << "\" and \"" << item_B.req.group_name + << "\") not allowed"); return true; } // No blending for groups without solver if (!hasSolver(model.getJointModelGroup(item_A.req.group_name))) { - ROS_WARN_STREAM("Blending for groups without solver not allowed"); + RCLCPP_WARN_STREAM(LOGGER, "Blending for groups without solver not allowed"); return true; } return false; } -CommandListManager::RadiiCont CommandListManager::extractBlendRadii(const moveit::core::RobotModel& model, - const moveit_msgs::MotionSequenceRequest& req_list) +CommandListManager::RadiiCont +CommandListManager::extractBlendRadii(const moveit::core::RobotModel& model, + const moveit_msgs::msg::MotionSequenceRequest& req_list) { RadiiCont radii(req_list.items.size(), 0.); for (RadiiCont::size_type i = 0; i < (radii.size() - 1); ++i) { if (isInvalidBlendRadii(model, req_list.items.at(i), req_list.items.at(i + 1))) { - ROS_WARN_STREAM("Invalid blend radii between commands: [" << i << "] and [" << i + 1 - << "] => Blend radii set to zero"); + RCLCPP_WARN_STREAM(LOGGER, "Invalid blend radii between commands: [" << i << "] and [" << i + 1 + << "] => Blend radii set to zero"); continue; } radii.at(i) = req_list.items.at(i).blend_radius; @@ -224,7 +229,7 @@ CommandListManager::RadiiCont CommandListManager::extractBlendRadii(const moveit CommandListManager::MotionResponseCont CommandListManager::solveSequenceItems(const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_pipeline::PlanningPipelinePtr& planning_pipeline, - const moveit_msgs::MotionSequenceRequest& req_list) const + const moveit_msgs::msg::MotionSequenceRequest& req_list) const { MotionResponseCont motion_plan_responses; size_t curr_req_index{ 0 }; @@ -239,29 +244,29 @@ CommandListManager::solveSequenceItems(const planning_scene::PlanningSceneConstP if (res.error_code_.val != res.error_code_.SUCCESS) { std::ostringstream os; - os << "Could not solve request\n---\n" << req << "\n---\n"; + os << "Could not solve request\n"; // TODO(henning): re-enable "---\n" << req << "\n---\n"; throw PlanningPipelineException(os.str(), res.error_code_.val); } motion_plan_responses.emplace_back(res); - ROS_DEBUG_STREAM("Solved [" << ++curr_req_index << "/" << num_req << "]"); + RCLCPP_DEBUG_STREAM(LOGGER, "Solved [" << ++curr_req_index << "/" << num_req << "]"); } return motion_plan_responses; } -void CommandListManager::checkForNegativeRadii(const moveit_msgs::MotionSequenceRequest& req_list) +void CommandListManager::checkForNegativeRadii(const moveit_msgs::msg::MotionSequenceRequest& req_list) { if (!std::all_of(req_list.items.begin(), req_list.items.end(), - [](const moveit_msgs::MotionSequenceItem& req) { return (req.blend_radius >= 0.); })) + [](const moveit_msgs::msg::MotionSequenceItem& req) { return (req.blend_radius >= 0.); })) { throw NegativeBlendRadiusException("All blending radii MUST be non negative"); } } -void CommandListManager::checkStartStatesOfGroup(const moveit_msgs::MotionSequenceRequest& req_list, +void CommandListManager::checkStartStatesOfGroup(const moveit_msgs::msg::MotionSequenceRequest& req_list, const std::string& group_name) { bool first_elem{ true }; - for (const moveit_msgs::MotionSequenceItem& item : req_list.items) + for (const moveit_msgs::msg::MotionSequenceItem& item : req_list.items) { if (item.req.group_name != group_name) { @@ -285,7 +290,7 @@ void CommandListManager::checkStartStatesOfGroup(const moveit_msgs::MotionSequen } } -void CommandListManager::checkStartStates(const moveit_msgs::MotionSequenceRequest& req_list) +void CommandListManager::checkStartStates(const moveit_msgs::msg::MotionSequenceRequest& req_list) { if (req_list.items.size() <= 1) { @@ -299,11 +304,12 @@ void CommandListManager::checkStartStates(const moveit_msgs::MotionSequenceReque } } -CommandListManager::GroupNamesCont CommandListManager::getGroupNames(const moveit_msgs::MotionSequenceRequest& req_list) +CommandListManager::GroupNamesCont +CommandListManager::getGroupNames(const moveit_msgs::msg::MotionSequenceRequest& req_list) { GroupNamesCont group_names; std::for_each(req_list.items.cbegin(), req_list.items.cend(), - [&group_names](const moveit_msgs::MotionSequenceItem& item) { + [&group_names](const moveit_msgs::msg::MotionSequenceItem& item) { if (std::find(group_names.cbegin(), group_names.cend(), item.req.group_name) == group_names.cend()) { group_names.emplace_back(item.req.group_name); diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp index 5c800573b5..4e85541607 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp @@ -39,14 +39,21 @@ #include #include +namespace +{ +static const rclcpp::Logger LOGGER = + rclcpp::get_logger("moveit.pilz_industrial_motion_planner.joint_limits_aggregator"); +} pilz_industrial_motion_planner::JointLimitsContainer pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( - const ros::NodeHandle& nh, const std::vector& joint_models) + const rclcpp::Node::SharedPtr& node, const std::string& param_namespace, + const std::vector& joint_models) { JointLimitsContainer container; + // TODO(henningkayser): use param_namespace - ROS_INFO_STREAM("Reading limits from namespace " << nh.getNamespace()); + RCLCPP_INFO_STREAM(LOGGER, "Reading limits from namespace " << node->get_namespace()); // Iterate over all joint models and generate the map for (auto joint_model : joint_models) @@ -54,7 +61,7 @@ pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( JointLimit joint_limit; // If there is something defined for the joint on the parameter server - if (joint_limits_interface::getJointLimits(joint_model->getName(), nh, joint_limit)) + if (joint_limits_interface::getJointLimits(joint_model->getName(), node, joint_limit)) { if (joint_limit.has_position_limits) { @@ -105,7 +112,7 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updatePositionLimitF { // LCOV_EXCL_START case 0: - ROS_ERROR_STREAM("no bounds set for joint " << joint_model->getName()); + RCLCPP_ERROR_STREAM(LOGGER, "no bounds set for joint " << joint_model->getName()); break; // LCOV_EXCL_STOP case 1: @@ -115,7 +122,7 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updatePositionLimitF break; // LCOV_EXCL_START default: - ROS_ERROR_STREAM("Multi-DOF-Joints not supported. The robot won't move. " << joint_model->getName()); + RCLCPP_ERROR_STREAM(LOGGER, "Multi-DOF-Joints not supported. The robot won't move. " << joint_model->getName()); joint_limit.has_position_limits = true; joint_limit.min_position = 0; joint_limit.max_position = 0; @@ -123,8 +130,8 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updatePositionLimitF // LCOV_EXCL_STOP } - ROS_DEBUG_STREAM("Limit(" << joint_model->getName() << " min:" << joint_limit.min_position - << " max:" << joint_limit.max_position); + RCLCPP_DEBUG_STREAM(LOGGER, "Limit(" << joint_model->getName() << " min:" << joint_limit.min_position + << " max:" << joint_limit.max_position); } void pilz_industrial_motion_planner::JointLimitsAggregator::updateVelocityLimitFromJointModel( @@ -134,7 +141,7 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updateVelocityLimitF { // LCOV_EXCL_START case 0: - ROS_ERROR_STREAM("no bounds set for joint " << joint_model->getName()); + RCLCPP_ERROR_STREAM(LOGGER, "no bounds set for joint " << joint_model->getName()); break; // LCOV_EXCL_STOP case 1: @@ -143,7 +150,7 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updateVelocityLimitF break; // LCOV_EXCL_START default: - ROS_ERROR_STREAM("Multi-DOF-Joints not supported. The robot won't move."); + RCLCPP_ERROR_STREAM(LOGGER, "Multi-DOF-Joints not supported. The robot won't move."); joint_limit.has_velocity_limits = true; joint_limit.max_velocity = 0; break; diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp index 7bdb1d983f..4cd5a354aa 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp @@ -34,22 +34,23 @@ #include "pilz_industrial_motion_planner/joint_limits_container.h" -#include "ros/ros.h" +#include "rclcpp/rclcpp.hpp" #include namespace pilz_industrial_motion_planner { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.joint_limits_container"); bool JointLimitsContainer::addLimit(const std::string& joint_name, JointLimit joint_limit) { if (joint_limit.has_deceleration_limits && joint_limit.max_deceleration >= 0) { - ROS_ERROR_STREAM("joint_limit.max_deceleration MUST be negative!"); + RCLCPP_ERROR_STREAM(LOGGER, "joint_limit.max_deceleration MUST be negative!"); return false; } const auto& insertion_result{ container_.insert(std::pair(joint_name, joint_limit)) }; if (!insertion_result.second) { - ROS_ERROR_STREAM("joint_limit for joint " << joint_name << " already contained."); + RCLCPP_ERROR_STREAM(LOGGER, "joint_limit for joint " << joint_name << " already contained."); return false; } return true; diff --git a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp index 17793df063..b90b680d83 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp @@ -53,6 +53,8 @@ namespace pilz_industrial_motion_planner { +static const rclcpp::Logger LOGGER = + rclcpp::get_logger("moveit.pilz_industrial_motion_planner.move_group_sequence_action"); MoveGroupSequenceAction::MoveGroupSequenceAction() : MoveGroupCapability("SequenceAction") { } @@ -60,43 +62,64 @@ MoveGroupSequenceAction::MoveGroupSequenceAction() : MoveGroupCapability("Sequen void MoveGroupSequenceAction::initialize() { // start the move action server - ROS_INFO_STREAM("initialize move group sequence action"); - move_action_server_.reset(new actionlib::SimpleActionServer( - root_node_handle_, "sequence_move_group", - boost::bind(&MoveGroupSequenceAction::executeSequenceCallback, this, _1), false)); - move_action_server_->registerPreemptCallback(boost::bind(&MoveGroupSequenceAction::preemptMoveCallback, this)); - move_action_server_->start(); - - command_list_manager_.reset(new pilz_industrial_motion_planner::CommandListManager( - ros::NodeHandle("~"), context_->planning_scene_monitor_->getRobotModel())); + RCLCPP_INFO_STREAM(LOGGER, "initialize move group sequence action"); + move_action_server_ = rclcpp_action::create_server( + context_->node_, "sequence_move_group", + [](const rclcpp_action::GoalUUID& /* unused */, + std::shared_ptr /* unused */) { + RCLCPP_DEBUG(LOGGER, "Received action goal"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }, + [this](const std::shared_ptr< + rclcpp_action::ServerGoalHandle> /* unused goal_handle */) { + RCLCPP_DEBUG(LOGGER, "Canceling action goal"); + preemptMoveCallback(); + return rclcpp_action::CancelResponse::ACCEPT; + }, + [this]( + const std::shared_ptr> goal_handle) { + // Return quickly to avoid blocking executor + std::thread(std::bind(&MoveGroupSequenceAction::executeSequenceCallback, this, std::placeholders::_1), + goal_handle) + .detach(); + }); + + command_list_manager_ = std::make_unique( + context_->node_, context_->planning_scene_monitor_->getRobotModel()); } -void MoveGroupSequenceAction::executeSequenceCallback(const moveit_msgs::MoveGroupSequenceGoalConstPtr& goal) +void MoveGroupSequenceAction::executeSequenceCallback( + const std::shared_ptr> goal_handle) { + // Notify that goal is being executed + goal_handle_ = goal_handle; + const auto goal = goal_handle->get_goal(); + goal_handle->execute(); + setMoveState(move_group::PLANNING); // Handle empty requests if (goal->request.items.empty()) { - ROS_WARN("Received empty request. That's ok but maybe not what you intended."); + RCLCPP_WARN(LOGGER, "Received empty request. That's ok but maybe not what you intended."); setMoveState(move_group::IDLE); - moveit_msgs::MoveGroupSequenceResult action_res; - action_res.response.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; - move_action_server_->setSucceeded(action_res, "Received empty request."); + const auto action_res = std::make_shared(); + action_res->response.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + goal_handle->succeed(action_res); return; } // before we start planning, ensure that we have the latest robot state // received... - context_->planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now()); + context_->planning_scene_monitor_->waitForCurrentRobotState(context_->node_->now()); context_->planning_scene_monitor_->updateFrameTransforms(); - moveit_msgs::MoveGroupSequenceResult action_res; + const auto action_res = std::make_shared(); if (goal->planning_options.plan_only || !context_->allow_trajectory_execution_) { if (!goal->planning_options.plan_only) { - ROS_WARN("Only plan will be calculated, although plan_only == false."); // LCOV_EXCL_LINE + RCLCPP_WARN(LOGGER, "Only plan will be calculated, although plan_only == false."); // LCOV_EXCL_LINE } executeMoveCallbackPlanOnly(goal, action_res); } @@ -105,31 +128,31 @@ void MoveGroupSequenceAction::executeSequenceCallback(const moveit_msgs::MoveGro executeSequenceCallbackPlanAndExecute(goal, action_res); } - switch (action_res.response.error_code.val) + switch (action_res->response.error_code.val) { - case moveit_msgs::MoveItErrorCodes::SUCCESS: - move_action_server_->setSucceeded(action_res, "Success"); + case moveit_msgs::msg::MoveItErrorCodes::SUCCESS: + goal_handle->succeed(action_res); break; - case moveit_msgs::MoveItErrorCodes::PREEMPTED: - move_action_server_->setPreempted(action_res, "Preempted"); + case moveit_msgs::msg::MoveItErrorCodes::PREEMPTED: + goal_handle->canceled(action_res); break; default: - move_action_server_->setAborted(action_res, "See error code for more information"); + goal_handle->abort(action_res); break; } setMoveState(move_group::IDLE); + goal_handle_.reset(); } void MoveGroupSequenceAction::executeSequenceCallbackPlanAndExecute( - const moveit_msgs::MoveGroupSequenceGoalConstPtr& goal, moveit_msgs::MoveGroupSequenceResult& action_res) + const moveit_msgs::action::MoveGroupSequence::Goal::ConstSharedPtr& goal, + const moveit_msgs::action::MoveGroupSequence::Result::SharedPtr& action_res) { - ROS_INFO("Combined planning and execution request received for " - "MoveGroupSequenceAction."); + RCLCPP_INFO(LOGGER, "Combined planning and execution request received for MoveGroupSequenceAction."); plan_execution::PlanExecution::Options opt; - - const moveit_msgs::PlanningScene& planning_scene_diff = + const moveit_msgs::msg::PlanningScene& planning_scene_diff = moveit::core::isEmpty(goal->planning_options.planning_scene_diff.robot_state) ? goal->planning_options.planning_scene_diff : clearSceneRobotState(goal->planning_options.planning_scene_diff); @@ -144,24 +167,23 @@ void MoveGroupSequenceAction::executeSequenceCallbackPlanAndExecute( if (goal->planning_options.look_around && context_->plan_with_sensing_) { - ROS_WARN("Plan with sensing not yet implemented/tested. This option is " - "ignored."); // LCOV_EXCL_LINE + RCLCPP_WARN(LOGGER, "Plan with sensing not yet implemented/tested. This option is ignored."); // LCOV_EXCL_LINE } plan_execution::ExecutableMotionPlan plan; context_->plan_execution_->planAndExecute(plan, planning_scene_diff, opt); StartStatesMsg start_states_msg; - convertToMsg(plan.plan_components_, start_states_msg, action_res.response.planned_trajectories); + convertToMsg(plan.plan_components_, start_states_msg, action_res->response.planned_trajectories); try { - action_res.response.sequence_start = start_states_msg.at(0); + action_res->response.sequence_start = start_states_msg.at(0); } catch (std::out_of_range&) { - ROS_WARN("Can not determine start state from empty sequence."); + RCLCPP_WARN(LOGGER, "Can not determine start state from empty sequence."); } - action_res.response.error_code = plan.error_code_; + action_res->response.error_code = plan.error_code_; } void MoveGroupSequenceAction::convertToMsg(const ExecutableTrajs& trajs, StartStatesMsg& start_states_msg, @@ -171,15 +193,16 @@ void MoveGroupSequenceAction::convertToMsg(const ExecutableTrajs& trajs, StartSt planned_trajs_msgs.resize(trajs.size()); for (size_t i = 0; i < trajs.size(); ++i) { - robot_state::robotStateToRobotStateMsg(trajs.at(i).trajectory_->getFirstWayPoint(), start_states_msg.at(i)); + moveit::core::robotStateToRobotStateMsg(trajs.at(i).trajectory_->getFirstWayPoint(), start_states_msg.at(i)); trajs.at(i).trajectory_->getRobotTrajectoryMsg(planned_trajs_msgs.at(i)); } } -void MoveGroupSequenceAction::executeMoveCallbackPlanOnly(const moveit_msgs::MoveGroupSequenceGoalConstPtr& goal, - moveit_msgs::MoveGroupSequenceResult& res) +void MoveGroupSequenceAction::executeMoveCallbackPlanOnly( + const moveit_msgs::action::MoveGroupSequence::Goal::ConstSharedPtr& goal, + const moveit_msgs::action::MoveGroupSequence::Result::SharedPtr& action_res) { - ROS_INFO("Planning request received for MoveGroupSequenceAction action."); + RCLCPP_INFO(LOGGER, "Planning request received for MoveGroupSequenceAction action."); // lock the scene so that it does not modify the world representation while // diff() is called @@ -190,7 +213,7 @@ void MoveGroupSequenceAction::executeMoveCallbackPlanOnly(const moveit_msgs::Mov static_cast(lscene) : lscene->diff(goal->planning_options.planning_scene_diff); - ros::Time planning_start = ros::Time::now(); + rclcpp::Time planning_start = context_->node_->now(); RobotTrajCont traj_vec; try { @@ -198,41 +221,42 @@ void MoveGroupSequenceAction::executeMoveCallbackPlanOnly(const moveit_msgs::Mov } catch (const MoveItErrorCodeException& ex) { - ROS_ERROR_STREAM("> Planning pipeline threw an exception (error code: " << ex.getErrorCode() << "): " << ex.what()); - res.response.error_code.val = ex.getErrorCode(); + RCLCPP_ERROR_STREAM(LOGGER, "> Planning pipeline threw an exception (error code: " << ex.getErrorCode() + << "): " << ex.what()); + action_res->response.error_code.val = ex.getErrorCode(); return; } // LCOV_EXCL_START // Keep moveit up even if lower parts throw catch (const std::exception& ex) { - ROS_ERROR("Planning pipeline threw an exception: %s", ex.what()); - res.response.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE; + RCLCPP_ERROR(LOGGER, "Planning pipeline threw an exception: %s", ex.what()); + action_res->response.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; return; } // LCOV_EXCL_STOP StartStatesMsg start_states_msg; start_states_msg.resize(traj_vec.size()); - res.response.planned_trajectories.resize(traj_vec.size()); + action_res->response.planned_trajectories.resize(traj_vec.size()); for (RobotTrajCont::size_type i = 0; i < traj_vec.size(); ++i) { move_group::MoveGroupCapability::convertToMsg(traj_vec.at(i), start_states_msg.at(i), - res.response.planned_trajectories.at(i)); + action_res->response.planned_trajectories.at(i)); } try { - res.response.sequence_start = start_states_msg.at(0); + action_res->response.sequence_start = start_states_msg.at(0); } catch (std::out_of_range&) { - ROS_WARN("Can not determine start state from empty sequence."); + RCLCPP_WARN(LOGGER, "Can not determine start state from empty sequence."); } - res.response.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; - res.response.planning_time = ros::Time::now().toSec() - planning_start.toSec(); + action_res->response.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + action_res->response.planning_time = (context_->node_->now() - planning_start).seconds(); } -bool MoveGroupSequenceAction::planUsingSequenceManager(const moveit_msgs::MotionSequenceRequest& req, +bool MoveGroupSequenceAction::planUsingSequenceManager(const moveit_msgs::msg::MotionSequenceRequest& req, plan_execution::ExecutableMotionPlan& plan) { setMoveState(move_group::PLANNING); @@ -255,15 +279,16 @@ bool MoveGroupSequenceAction::planUsingSequenceManager(const moveit_msgs::Motion } catch (const MoveItErrorCodeException& ex) { - ROS_ERROR_STREAM("Planning pipeline threw an exception (error code: " << ex.getErrorCode() << "): " << ex.what()); + RCLCPP_ERROR_STREAM(LOGGER, "Planning pipeline threw an exception (error code: " << ex.getErrorCode() + << "): " << ex.what()); plan.error_code_.val = ex.getErrorCode(); return false; } // LCOV_EXCL_START // Keep MoveIt up even if lower parts throw catch (const std::exception& ex) { - ROS_ERROR_STREAM("Planning pipeline threw an exception: " << ex.what()); - plan.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE; + RCLCPP_ERROR_STREAM(LOGGER, "Planning pipeline threw an exception: " << ex.what()); + plan.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; return false; } // LCOV_EXCL_STOP @@ -277,7 +302,7 @@ bool MoveGroupSequenceAction::planUsingSequenceManager(const moveit_msgs::Motion plan.plan_components_.at(i).description_ = "plan"; } } - plan.error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + plan.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; return true; } @@ -294,8 +319,8 @@ void MoveGroupSequenceAction::preemptMoveCallback() void MoveGroupSequenceAction::setMoveState(move_group::MoveGroupState state) { move_state_ = state; - move_feedback_.state = stateToStr(state); - move_action_server_->publishFeedback(move_feedback_); + move_feedback_->state = stateToStr(state); + goal_handle_->publish_feedback(move_feedback_); } } // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp index 3ed94eb762..4eb62bcc75 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp @@ -44,6 +44,8 @@ namespace pilz_industrial_motion_planner { +static const rclcpp::Logger LOGGER = + rclcpp::get_logger("moveit.pilz_industrial_motion_planner.move_group_sequence_service"); MoveGroupSequenceService::MoveGroupSequenceService() : MoveGroupCapability("SequenceService") { } @@ -55,47 +57,49 @@ MoveGroupSequenceService::~MoveGroupSequenceService() void MoveGroupSequenceService::initialize() { command_list_manager_.reset(new pilz_industrial_motion_planner::CommandListManager( - ros::NodeHandle("~"), context_->planning_scene_monitor_->getRobotModel())); + context_->node_, context_->planning_scene_monitor_->getRobotModel())); - sequence_service_ = root_node_handle_.advertiseService(SEQUENCE_SERVICE_NAME, &MoveGroupSequenceService::plan, this); + sequence_service_ = context_->node_->create_service( + SEQUENCE_SERVICE_NAME, + std::bind(&MoveGroupSequenceService::plan, this, std::placeholders::_1, std::placeholders::_2)); } -bool MoveGroupSequenceService::plan(moveit_msgs::GetMotionSequence::Request& req, - moveit_msgs::GetMotionSequence::Response& res) +bool MoveGroupSequenceService::plan(const moveit_msgs::srv::GetMotionSequence::Request::SharedPtr req, + moveit_msgs::srv::GetMotionSequence::Response::SharedPtr res) { // TODO: Do we lock on the correct scene? Does the lock belong to the scene // used for planning? planning_scene_monitor::LockedPlanningSceneRO ps(context_->planning_scene_monitor_); - ros::Time planning_start = ros::Time::now(); + rclcpp::Time planning_start = context_->node_->now(); RobotTrajCont traj_vec; try { - traj_vec = command_list_manager_->solve(ps, context_->planning_pipeline_, req.request); + traj_vec = command_list_manager_->solve(ps, context_->planning_pipeline_, req->request); } catch (const MoveItErrorCodeException& ex) { - ROS_ERROR_STREAM("Planner threw an exception (error code: " << ex.getErrorCode() << "): " << ex.what()); - res.response.error_code.val = ex.getErrorCode(); + RCLCPP_ERROR_STREAM(LOGGER, "Planner threw an exception (error code: " << ex.getErrorCode() << "): " << ex.what()); + res->response.error_code.val = ex.getErrorCode(); return true; } // LCOV_EXCL_START // Keep moveit up even if lower parts throw catch (const std::exception& ex) { - ROS_ERROR_STREAM("Planner threw an exception: " << ex.what()); + RCLCPP_ERROR_STREAM(LOGGER, "Planner threw an exception: " << ex.what()); // If 'FALSE' then no response will be sent to the caller. return false; } // LCOV_EXCL_STOP - res.response.planned_trajectories.resize(traj_vec.size()); + res->response.planned_trajectories.resize(traj_vec.size()); for (RobotTrajCont::size_type i = 0; i < traj_vec.size(); ++i) { - move_group::MoveGroupCapability::convertToMsg(traj_vec.at(i), res.response.sequence_start, - res.response.planned_trajectories.at(i)); + move_group::MoveGroupCapability::convertToMsg(traj_vec.at(i), res->response.sequence_start, + res->response.planned_trajectories.at(i)); } - res.response.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; - res.response.planning_time = (ros::Time::now() - planning_start).toSec(); + res->response.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + res->response.planning_time = (context_->node_->now() - planning_start).seconds(); return true; } 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 c269bf7300..fc9471d41b 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 @@ -32,6 +32,8 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ +#include + #include "pilz_industrial_motion_planner/pilz_industrial_motion_planner.h" #include "pilz_industrial_motion_planner/planning_context_loader.h" @@ -51,11 +53,13 @@ namespace pilz_industrial_motion_planner { static const std::string PARAM_NAMESPACE_LIMTS = "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 std::string& ns) +bool CommandPlanner::initialize(const moveit::core::RobotModelConstPtr& model, const rclcpp::Node::SharedPtr& node, + const std::string& ns) { // Call parent class initialize - planning_interface::PlannerManager::initialize(model, ns); + planning_interface::PlannerManager::initialize(model, node, ns); // Store the model and the namespace model_ = model; @@ -63,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( - ros::NodeHandle(PARAM_NAMESPACE_LIMTS), model->getActiveJointModels()); + node, PARAM_NAMESPACE_LIMTS, model->getActiveJointModels()); // Obtain cartesian limits - cartesian_limit_ = pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits( - ros::NodeHandle(PARAM_NAMESPACE_LIMTS)); + cartesian_limit_ = + pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits(node, PARAM_NAMESPACE_LIMTS); // Load the planning context loader planner_context_loader.reset(new pluginlib::ClassLoader( @@ -81,13 +85,13 @@ bool CommandPlanner::initialize(const moveit::core::RobotModelConstPtr& model, c ss << factory << " "; } - ROS_INFO_STREAM("Available plugins: " << ss.str()); + RCLCPP_INFO_STREAM(LOGGER, "Available plugins: " << ss.str()); // Load each factory for (const auto& factory : factories) { - ROS_INFO_STREAM("About to load: " << factory); - PlanningContextLoaderPtr loader_pointer(planner_context_loader->createInstance(factory)); + RCLCPP_INFO_STREAM(LOGGER, "About to load: " << factory); + PlanningContextLoaderPtr loader_pointer(planner_context_loader->createSharedInstance(factory)); pilz_industrial_motion_planner::LimitsContainer limits; limits.setJointLimits(aggregated_limit_active_joints_); @@ -119,15 +123,18 @@ void CommandPlanner::getPlanningAlgorithms(std::vector& algs) const planning_interface::PlanningContextPtr CommandPlanner::getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, - const moveit_msgs::MotionPlanRequest& req, - moveit_msgs::MoveItErrorCodes& error_code) const + const moveit_msgs::msg::MotionPlanRequest& req, + moveit_msgs::msg::MoveItErrorCodes& error_code) const { - ROS_DEBUG_STREAM("Loading PlanningContext for request\n\n" << req << "\n"); + // TODO(henningkayser): print req + // RCLCPP_DEBUG_STREAM(LOGGER, "Loading PlanningContext for request\n\n" << req << "\n"); + RCLCPP_DEBUG(LOGGER, "Loading PlanningContext"); // Check that a loaded for this request exists if (!canServiceRequest(req)) { - ROS_ERROR_STREAM("No ContextLoader for planner_id " << req.planner_id.c_str() << " found. Planning not possible."); + RCLCPP_ERROR_STREAM(LOGGER, "No ContextLoader for planner_id " << req.planner_id.c_str() + << " found. Planning not possible."); return nullptr; } @@ -135,19 +142,19 @@ CommandPlanner::getPlanningContext(const planning_scene::PlanningSceneConstPtr& if (context_loader_map_.at(req.planner_id)->loadContext(planning_context, req.planner_id, req.group_name)) { - ROS_DEBUG_STREAM("Found planning context loader for " << req.planner_id << " group:" << req.group_name); + RCLCPP_DEBUG_STREAM(LOGGER, "Found planning context loader for " << req.planner_id << " group:" << req.group_name); planning_context->setMotionPlanRequest(req); planning_context->setPlanningScene(planning_scene); return planning_context; } else { - error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED; + error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; return nullptr; } } -bool CommandPlanner::canServiceRequest(const moveit_msgs::MotionPlanRequest& req) const +bool CommandPlanner::canServiceRequest(const moveit_msgs::msg::MotionPlanRequest& req) const { return context_loader_map_.find(req.planner_id) != context_loader_map_.end(); } @@ -159,7 +166,7 @@ void CommandPlanner::registerContextLoader( if (context_loader_map_.find(planning_context_loader->getAlgorithm()) == context_loader_map_.end()) { context_loader_map_[planning_context_loader->getAlgorithm()] = planning_context_loader; - ROS_INFO_STREAM("Registered Algorithm [" << planning_context_loader->getAlgorithm() << "]"); + RCLCPP_INFO_STREAM(LOGGER, "Registered Algorithm [" << planning_context_loader->getAlgorithm() << "]"); } else { diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader.cpp index 9a071bba58..6e9bbd05eb 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader.cpp @@ -33,7 +33,7 @@ *********************************************************************/ #include "pilz_industrial_motion_planner/planning_context_loader.h" -#include +#include pilz_industrial_motion_planner::PlanningContextLoader::PlanningContextLoader() : limits_set_(false), model_set_(false) { diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp index 4564961299..21a08886c0 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp @@ -32,6 +32,8 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ +#include + #include "pilz_industrial_motion_planner/planning_context_loader_circ.h" #include "moveit/planning_scene/planning_scene.h" #include "pilz_industrial_motion_planner/planning_context_base.h" @@ -39,6 +41,13 @@ #include +namespace +{ +static const rclcpp::Logger LOGGER = + rclcpp::get_logger("moveit.pilz_industrial_motion_planner.planning_context_loader_circ"); +} +} + pilz_industrial_motion_planner::PlanningContextLoaderCIRC::PlanningContextLoaderCIRC() { alg_ = "CIRC"; @@ -60,12 +69,11 @@ bool pilz_industrial_motion_planner::PlanningContextLoaderCIRC::loadContext( { if (!limits_set_) { - ROS_ERROR_STREAM("Limits are not defined. Cannot load planning context. " - "Call setLimits loadContext"); + RCLCPP_ERROR_STREAM(LOGGER, "Limits are not defined. Cannot load planning context. Call setLimits loadContext"); } if (!model_set_) { - ROS_ERROR_STREAM("Robot model was not set"); + RCLCPP_ERROR_STREAM(LOGGER, "Robot model was not set"); } return false; } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp index cb230ee76a..33e8d47837 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp @@ -39,6 +39,13 @@ #include +namespace +{ +static const rclcpp::Logger LOGGER = + rclcpp::get_logger("moveit.pilz_industrial_motion_planner.planning_context_loader_lin"); +} +} + pilz_industrial_motion_planner::PlanningContextLoaderLIN::PlanningContextLoaderLIN() { alg_ = "LIN"; @@ -60,12 +67,11 @@ bool pilz_industrial_motion_planner::PlanningContextLoaderLIN::loadContext( { if (!limits_set_) { - ROS_ERROR_STREAM("Limits are not defined. Cannot load planning context. " - "Call setLimits loadContext"); + RCLCPP_ERROR_STREAM(LOGGER, "Limits are not defined. Cannot load planning context. Call setLimits loadContext"); } if (!model_set_) { - ROS_ERROR_STREAM("Robot model was not set"); + RCLCPP_ERROR_STREAM(LOGGER, "Robot model was not set"); } return false; } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp index 52a0df4ddd..f0c4d76630 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp @@ -38,6 +38,12 @@ #include +namespace +{ +static const rclcpp::Logger LOGGER = + rclcpp::get_logger("moveit.pilz_industrial_motion_planner.planning_context_loader_ptp"); +} + pilz_industrial_motion_planner::PlanningContextLoaderPTP::PlanningContextLoaderPTP() { alg_ = "PTP"; @@ -59,12 +65,12 @@ bool pilz_industrial_motion_planner::PlanningContextLoaderPTP::loadContext( { if (!limits_set_) { - ROS_ERROR_STREAM("Joint Limits are not defined. Cannot load planning " - "context. Call setLimits loadContext"); + RCLCPP_ERROR_STREAM(LOGGER, + "Joint Limits are not defined. Cannot load planning context. Call setLimits loadContext"); } if (!model_set_) { - ROS_ERROR_STREAM("Robot model was not set"); + RCLCPP_ERROR_STREAM(LOGGER, "Robot model was not set"); } return false; } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp index be8d2ffa2c..b87d4b084d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp @@ -38,16 +38,22 @@ #include #include +namespace +{ +static const rclcpp::Logger LOGGER = + rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_blender_transition_window"); +} + bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, pilz_industrial_motion_planner::TrajectoryBlendResponse& res) { - ROS_INFO("Start trajectory blending using transition window."); + RCLCPP_INFO(LOGGER, "Start trajectory blending using transition window."); double sampling_time = 0.; if (!validateRequest(req, sampling_time, res.error_code)) { - ROS_ERROR("Trajectory blend request is not valid."); + RCLCPP_ERROR(LOGGER, "Trajectory blend request is not valid."); return false; } @@ -58,8 +64,8 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( std::size_t second_intersection_index; if (!searchIntersectionPoints(req, first_intersection_index, second_intersection_index)) { - ROS_ERROR("Blend radius to large."); - res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN; + RCLCPP_ERROR(LOGGER, "Blend radius to large."); + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; return false; } @@ -83,15 +89,15 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( initial_joint_velocity[joint_name] = req.first_trajectory->getWayPoint(first_intersection_index - 1).getVariableVelocity(joint_name); } - trajectory_msgs::JointTrajectory blend_joint_trajectory; - moveit_msgs::MoveItErrorCodes error_code; + trajectory_msgs::msg::JointTrajectory blend_joint_trajectory; + moveit_msgs::msg::MoveItErrorCodes error_code; if (!generateJointTrajectory(req.first_trajectory->getFirstWayPointPtr()->getRobotModel(), limits_.getJointLimitContainer(), blend_trajectory_cartesian, req.group_name, req.link_name, initial_joint_position, initial_joint_velocity, blend_joint_trajectory, error_code, true)) { // LCOV_EXCL_START - ROS_INFO("Failed to generate joint trajectory for blending trajectory."); + RCLCPP_INFO(LOGGER, "Failed to generate joint trajectory for blending trajectory."); res.error_code.val = error_code.val; return false; // LCOV_EXCL_STOP @@ -125,21 +131,21 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( // adjust the time from start res.second_trajectory->setWayPointDurationFromPrevious(0, sampling_time); - res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; return true; } bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::validateRequest( const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, double& sampling_time, - moveit_msgs::MoveItErrorCodes& error_code) const + moveit_msgs::msg::MoveItErrorCodes& error_code) const { - ROS_DEBUG("Validate the trajectory blend request."); + RCLCPP_DEBUG(LOGGER, "Validate the trajectory blend request."); // check planning group if (!req.first_trajectory->getRobotModel()->hasJointModelGroup(req.group_name)) { - ROS_ERROR_STREAM("Unknown planning group: " << req.group_name); - error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; + RCLCPP_ERROR_STREAM(LOGGER, "Unknown planning group: " << req.group_name); + error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME; return false; } @@ -147,15 +153,15 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::validate if (!req.first_trajectory->getRobotModel()->hasLinkModel(req.link_name) && !req.first_trajectory->getLastWayPoint().hasAttachedBody(req.link_name)) { - ROS_ERROR_STREAM("Unknown link name: " << req.link_name); - error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME; + RCLCPP_ERROR_STREAM(LOGGER, "Unknown link name: " << req.link_name); + error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_LINK_NAME; return false; } if (req.blend_radius <= 0) { - ROS_ERROR("Blending radius must be positive"); - error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN; + RCLCPP_ERROR(LOGGER, "Blending radius must be positive"); + error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; return false; } @@ -164,8 +170,9 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::validate if (!pilz_industrial_motion_planner::isRobotStateEqual( req.first_trajectory->getLastWayPoint(), req.second_trajectory->getFirstWayPoint(), req.group_name, EPSILON)) { - ROS_ERROR_STREAM("During blending the last point of the preceding and the first point of the succeding trajectory"); - error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN; + RCLCPP_ERROR_STREAM( + LOGGER, "During blending the last point of the preceding and the first point of the succeding trajectory"); + error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; return false; } @@ -173,7 +180,7 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::validate if (!pilz_industrial_motion_planner::determineAndCheckSamplingTime(req.first_trajectory, req.second_trajectory, EPSILON, sampling_time)) { - error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN; + error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; return false; } @@ -185,9 +192,8 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::validate !pilz_industrial_motion_planner::isRobotStateStationary(req.second_trajectory->getFirstWayPoint(), req.group_name, EPSILON)) { - ROS_ERROR("Intersection point of the blending trajectories has non-zero " - "velocities/accelerations."); - error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN; + RCLCPP_ERROR(LOGGER, "Intersection point of the blending trajectories has non-zero velocities/accelerations."); + error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; return false; } @@ -247,7 +253,7 @@ void pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blendTra // push to the trajectory waypoint.pose = tf2::toMsg(blend_sample_pose); - waypoint.time_from_start = ros::Duration((i + 1.0) * sampling_time); + waypoint.time_from_start = rclcpp::Duration::from_seconds((i + 1.0) * sampling_time); trajectory.points.push_back(waypoint); } } @@ -256,7 +262,7 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::searchIn const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, std::size_t& first_interse_index, std::size_t& second_interse_index) const { - ROS_INFO("Search for start and end point of blending trajectory."); + RCLCPP_INFO(LOGGER, "Search for start and end point of blending trajectory."); // compute the position of the center of the blend sphere // (last point of the first trajectory, first point of the second trajectory) @@ -266,19 +272,19 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::searchIn if (!linearSearchIntersectionPoint(req.link_name, circ_pose.translation(), req.blend_radius, req.first_trajectory, true, first_interse_index)) { - ROS_ERROR_STREAM("Intersection point of first trajectory not found."); + RCLCPP_ERROR_STREAM(LOGGER, "Intersection point of first trajectory not found."); return false; } - ROS_INFO_STREAM("Intersection point of first trajectory found, index: " << first_interse_index); + RCLCPP_INFO_STREAM(LOGGER, "Intersection point of first trajectory found, index: " << first_interse_index); if (!linearSearchIntersectionPoint(req.link_name, circ_pose.translation(), req.blend_radius, req.second_trajectory, false, second_interse_index)) { - ROS_ERROR_STREAM("Intersection point of second trajectory not found."); + RCLCPP_ERROR_STREAM(LOGGER, "Intersection point of second trajectory not found."); return false; } - ROS_INFO_STREAM("Intersection point of second trajectory found, index: " << second_interse_index); + RCLCPP_INFO_STREAM(LOGGER, "Intersection point of second trajectory found, index: " << second_interse_index); return true; } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index 08491ac75f..b1ed1bccc8 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -36,10 +36,15 @@ #include #include -#include -#include +#include +#include #include +namespace +{ +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_functions"); +} + bool pilz_industrial_motion_planner::computePoseIK(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name, const std::string& link_name, const Eigen::Isometry3d& pose, const std::string& frame_id, @@ -49,32 +54,32 @@ bool pilz_industrial_motion_planner::computePoseIK(const moveit::core::RobotMode { if (!robot_model->hasJointModelGroup(group_name)) { - ROS_ERROR_STREAM("Robot model has no planning group named as " << group_name); + RCLCPP_ERROR_STREAM(LOGGER, "Robot model has no planning group named as " << group_name); return false; } if (!robot_model->getJointModelGroup(group_name)->canSetStateFromIK(link_name)) { - ROS_ERROR_STREAM("No valid IK solver exists for " << link_name << " in planning group " << group_name); + RCLCPP_ERROR_STREAM(LOGGER, "No valid IK solver exists for " << link_name << " in planning group " << group_name); return false; } if (frame_id != robot_model->getModelFrame()) { - ROS_ERROR_STREAM("Given frame (" << frame_id << ") is unequal to model frame(" << robot_model->getModelFrame() - << ")"); + RCLCPP_ERROR_STREAM(LOGGER, "Given frame (" << frame_id << ") is unequal to model frame(" + << robot_model->getModelFrame() << ")"); return false; } - robot_state::RobotState rstate(robot_model); + moveit::core::RobotState rstate(robot_model); // By setting the robot state to default values, we basically allow // the user of this function to supply an incomplete or even empty seed. rstate.setToDefaultValues(); rstate.setVariablePositions(seed); moveit::core::GroupStateValidityCallbackFn ik_constraint_function; - ik_constraint_function = - boost::bind(&pilz_industrial_motion_planner::isStateColliding, check_self_collision, robot_model, _1, _2, _3); + ik_constraint_function = std::bind(&pilz_industrial_motion_planner::isStateColliding, check_self_collision, + robot_model, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); // call ik if (rstate.setFromIK(robot_model->getJointModelGroup(group_name), pose, link_name, timeout, ik_constraint_function)) @@ -88,20 +93,22 @@ bool pilz_industrial_motion_planner::computePoseIK(const moveit::core::RobotMode } else { - ROS_ERROR_STREAM("Inverse kinematics for pose \n" << pose.translation() << " has no solution."); + RCLCPP_ERROR(LOGGER, "Unable to find IK solution."); + // TODO(henning): Re-enable logging error + // RCLCPP_ERROR_STREAM(LOGGER, "Inverse kinematics for pose \n" << pose.translation() << " has no solution."); return false; } } bool pilz_industrial_motion_planner::computePoseIK(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name, const std::string& link_name, - const geometry_msgs::Pose& pose, const std::string& frame_id, + const geometry_msgs::msg::Pose& pose, const std::string& frame_id, const std::map& seed, std::map& solution, bool check_self_collision, const double timeout) { Eigen::Isometry3d pose_eigen; - tf2::fromMsg(pose, pose_eigen); + tf2::convert(pose, pose_eigen); return computePoseIK(robot_model, group_name, link_name, pose_eigen, frame_id, seed, solution, check_self_collision, timeout); } @@ -111,12 +118,12 @@ bool pilz_industrial_motion_planner::computeLinkFK(const moveit::core::RobotMode const std::map& joint_state, Eigen::Isometry3d& pose) { // create robot state - robot_state::RobotState rstate(robot_model); + moveit::core::RobotState rstate(robot_model); // check the reference frame of the target pose if (!rstate.knowsFrameTransform(link_name)) { - ROS_ERROR_STREAM("The target link " << link_name << " is not known by robot."); + RCLCPP_ERROR_STREAM(LOGGER, "The target link " << link_name << " is not known by robot."); return false; } @@ -139,7 +146,7 @@ bool pilz_industrial_motion_planner::verifySampleJointLimits( const double epsilon = 10e-6; if (duration_current <= epsilon) { - ROS_ERROR("Sample duration too small, cannot compute the velocity"); + RCLCPP_ERROR(LOGGER, "Sample duration too small, cannot compute the velocity"); return false; } @@ -151,10 +158,10 @@ bool pilz_industrial_motion_planner::verifySampleJointLimits( if (!joint_limits.verifyVelocityLimit(pos.first, velocity_current)) { - ROS_ERROR_STREAM("Joint velocity limit of " << pos.first << " violated. Set the velocity scaling factor lower!" - << " Actual joint velocity is " << velocity_current - << ", while the limit is " - << joint_limits.getLimit(pos.first).max_velocity << ". "); + RCLCPP_ERROR_STREAM(LOGGER, "Joint velocity limit of " + << pos.first << " violated. Set the velocity scaling factor lower!" + << " Actual joint velocity is " << velocity_current << ", while the limit is " + << joint_limits.getLimit(pos.first).max_velocity << ". "); return false; } @@ -165,10 +172,11 @@ bool pilz_industrial_motion_planner::verifySampleJointLimits( if (joint_limits.getLimit(pos.first).has_acceleration_limits && fabs(acceleration_current) > fabs(joint_limits.getLimit(pos.first).max_acceleration)) { - ROS_ERROR_STREAM("Joint acceleration limit of " - << pos.first << " violated. Set the acceleration scaling factor lower!" - << " Actual joint acceleration is " << acceleration_current << ", while the limit is " - << joint_limits.getLimit(pos.first).max_acceleration << ". "); + RCLCPP_ERROR_STREAM(LOGGER, "Joint acceleration limit of " + << pos.first << " violated. Set the acceleration scaling factor lower!" + << " Actual joint acceleration is " << acceleration_current + << ", while the limit is " << joint_limits.getLimit(pos.first).max_acceleration + << ". "); return false; } } @@ -178,10 +186,11 @@ bool pilz_industrial_motion_planner::verifySampleJointLimits( if (joint_limits.getLimit(pos.first).has_deceleration_limits && fabs(acceleration_current) > fabs(joint_limits.getLimit(pos.first).max_deceleration)) { - ROS_ERROR_STREAM("Joint deceleration limit of " - << pos.first << " violated. Set the acceleration scaling factor lower!" - << " Actual joint deceleration is " << acceleration_current << ", while the limit is " - << joint_limits.getLimit(pos.first).max_deceleration << ". "); + RCLCPP_ERROR_STREAM(LOGGER, "Joint deceleration limit of " + << pos.first << " violated. Set the acceleration scaling factor lower!" + << " Actual joint deceleration is " << acceleration_current + << ", while the limit is " << joint_limits.getLimit(pos.first).max_deceleration + << ". "); return false; } } @@ -195,12 +204,13 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits, const KDL::Trajectory& trajectory, const std::string& group_name, const std::string& link_name, const std::map& initial_joint_position, const double& sampling_time, - trajectory_msgs::JointTrajectory& joint_trajectory, moveit_msgs::MoveItErrorCodes& error_code, + trajectory_msgs::msg::JointTrajectory& joint_trajectory, moveit_msgs::msg::MoveItErrorCodes& error_code, bool check_self_collision) { - ROS_DEBUG("Generate joint trajectory from a Cartesian trajectory."); + RCLCPP_DEBUG(LOGGER, "Generate joint trajectory from a Cartesian trajectory."); - ros::Time generation_begin = ros::Time::now(); + rclcpp::Clock clock; + rclcpp::Time generation_begin = clock.now(); // generate the time samples const double epsilon = 10e-06; // avoid adding the last time sample twice @@ -223,14 +233,13 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( for (std::vector::const_iterator time_iter = time_samples.begin(); time_iter != time_samples.end(); ++time_iter) { - tf2::fromMsg(tf2::toMsg(trajectory.Pos(*time_iter)), pose_sample); + tf2::transformKDLToEigen(trajectory.Pos(*time_iter), pose_sample); if (!computePoseIK(robot_model, group_name, link_name, pose_sample, robot_model->getModelFrame(), ik_solution_last, ik_solution, check_self_collision)) { - ROS_ERROR("Failed to compute inverse kinematics solution for sampled " - "Cartesian pose."); - error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION; + RCLCPP_ERROR(LOGGER, "Failed to compute inverse kinematics solution for sampled Cartesian pose."); + error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION; joint_trajectory.points.clear(); return false; } @@ -252,15 +261,16 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( !verifySampleJointLimits(ik_solution_last, joint_velocity_last, ik_solution, sampling_time, duration_current_sample, joint_limits)) { - ROS_ERROR_STREAM("Inverse kinematics solution at " - << *time_iter << "s violates the joint velocity/acceleration/deceleration limits."); - error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED; + RCLCPP_ERROR_STREAM(LOGGER, "Inverse kinematics solution at " + << *time_iter + << "s violates the joint velocity/acceleration/deceleration limits."); + error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; joint_trajectory.points.clear(); return false; } // fill the point with joint values - trajectory_msgs::JointTrajectoryPoint point; + trajectory_msgs::msg::JointTrajectoryPoint point; // set joint names joint_trajectory.joint_names.clear(); @@ -269,7 +279,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( joint_trajectory.joint_names.push_back(start_joint.first); } - point.time_from_start = ros::Duration(*time_iter); + point.time_from_start = rclcpp::Duration::from_seconds(*time_iter); for (const auto& joint_name : joint_trajectory.joint_names) { point.positions.push_back(ik_solution.at(joint_name)); @@ -296,11 +306,11 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( ik_solution_last = ik_solution; } - error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; - double duration_ms = (ros::Time::now() - generation_begin).toSec() * 1000; - ROS_DEBUG_STREAM("Generate trajectory (N-Points: " << joint_trajectory.points.size() << ") took " << duration_ms - << " ms | " << duration_ms / joint_trajectory.points.size() - << " ms per Point"); + error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + double duration_ms = (clock.now() - generation_begin).seconds() * 1000; + RCLCPP_DEBUG_STREAM(LOGGER, "Generate trajectory (N-Points: " + << joint_trajectory.points.size() << ") took " << duration_ms << " ms | " + << duration_ms / joint_trajectory.points.size() << " ms per Point"); return true; } @@ -310,12 +320,14 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits, const pilz_industrial_motion_planner::CartesianTrajectory& trajectory, const std::string& group_name, const std::string& link_name, const std::map& initial_joint_position, - const std::map& initial_joint_velocity, trajectory_msgs::JointTrajectory& joint_trajectory, - moveit_msgs::MoveItErrorCodes& error_code, bool check_self_collision) + const std::map& initial_joint_velocity, + trajectory_msgs::msg::JointTrajectory& joint_trajectory, moveit_msgs::msg::MoveItErrorCodes& error_code, + bool check_self_collision) { - ROS_DEBUG("Generate joint trajectory from a Cartesian trajectory."); + RCLCPP_DEBUG(LOGGER, "Generate joint trajectory from a Cartesian trajectory."); - ros::Time generation_begin = ros::Time::now(); + rclcpp::Clock clock; + rclcpp::Time generation_begin = clock.now(); std::map ik_solution_last = initial_joint_position; std::map joint_velocity_last = initial_joint_velocity; @@ -333,9 +345,9 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( if (!computePoseIK(robot_model, group_name, link_name, trajectory.points.at(i).pose, robot_model->getModelFrame(), ik_solution_last, ik_solution, check_self_collision)) { - ROS_ERROR("Failed to compute inverse kinematics solution for sampled " - "Cartesian pose."); - error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION; + RCLCPP_ERROR(LOGGER, "Failed to compute inverse kinematics solution for sampled " + "Cartesian pose."); + error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION; joint_trajectory.points.clear(); return false; } @@ -343,13 +355,13 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( // verify the joint limits if (i == 0) { - duration_current = trajectory.points.front().time_from_start.toSec(); + duration_current = trajectory.points.front().time_from_start.seconds(); duration_last = duration_current; } else { duration_current = - trajectory.points.at(i).time_from_start.toSec() - trajectory.points.at(i - 1).time_from_start.toSec(); + trajectory.points.at(i).time_from_start.seconds() - trajectory.points.at(i - 1).time_from_start.seconds(); } if (!verifySampleJointLimits(ik_solution_last, joint_velocity_last, ik_solution, duration_last, duration_current, @@ -359,18 +371,19 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( // overload generateJointTrajectory(..., // KDL::Trajectory, ...) // TODO: refactor to avoid code duplication. - ROS_ERROR_STREAM("Inverse kinematics solution of the " << i - << "th sample violates the joint " - "velocity/acceleration/deceleration limits."); - error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED; + RCLCPP_ERROR_STREAM(LOGGER, "Inverse kinematics solution of the " + << i + << "th sample violates the joint " + "velocity/acceleration/deceleration limits."); + error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; joint_trajectory.points.clear(); return false; // LCOV_EXCL_STOP } // compute the waypoint - trajectory_msgs::JointTrajectoryPoint waypoint_joint; - waypoint_joint.time_from_start = ros::Duration(trajectory.points.at(i).time_from_start); + trajectory_msgs::msg::JointTrajectoryPoint waypoint_joint; + waypoint_joint.time_from_start = rclcpp::Duration(trajectory.points.at(i).time_from_start); for (const auto& joint_name : joint_trajectory.joint_names) { waypoint_joint.positions.push_back(ik_solution.at(joint_name)); @@ -388,12 +401,12 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( duration_last = duration_current; } - error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; - double duration_ms = (ros::Time::now() - generation_begin).toSec() * 1000; - ROS_DEBUG_STREAM("Generate trajectory (N-Points: " << joint_trajectory.points.size() << ") took " << duration_ms - << " ms | " << duration_ms / joint_trajectory.points.size() - << " ms per Point"); + double duration_ms = (clock.now() - generation_begin).seconds() * 1000; + RCLCPP_DEBUG_STREAM(LOGGER, "Generate trajectory (N-Points: " + << joint_trajectory.points.size() << ") took " << duration_ms << " ms | " + << duration_ms / joint_trajectory.points.size() << " ms per Point"); return true; } @@ -408,8 +421,7 @@ bool pilz_industrial_motion_planner::determineAndCheckSamplingTime( std::size_t n2 = second_trajectory->getWayPointCount() - 1; if ((n1 < 2) && (n2 < 2)) { - ROS_ERROR_STREAM("Both trajectories do not have enough points to determine " - "sampling time."); + RCLCPP_ERROR_STREAM(LOGGER, "Both trajectories do not have enough points to determine sampling time."); return false; } @@ -428,8 +440,9 @@ bool pilz_industrial_motion_planner::determineAndCheckSamplingTime( { if (fabs(sampling_time - first_trajectory->getWayPointDurationFromPrevious(i)) > epsilon) { - ROS_ERROR_STREAM("First trajectory violates sampline time " << sampling_time << " between points " << (i - 1) - << "and " << i << " (indices)."); + RCLCPP_ERROR_STREAM(LOGGER, "First trajectory violates sampline time " << sampling_time << " between points " + << (i - 1) << "and " << i + << " (indices)."); return false; } } @@ -438,8 +451,9 @@ bool pilz_industrial_motion_planner::determineAndCheckSamplingTime( { if (fabs(sampling_time - second_trajectory->getWayPointDurationFromPrevious(i)) > epsilon) { - ROS_ERROR_STREAM("Second trajectory violates sampline time " << sampling_time << " between points " << (i - 1) - << "and " << i << " (indices)."); + RCLCPP_ERROR_STREAM(LOGGER, "Second trajectory violates sampline time " << sampling_time << " between points " + << (i - 1) << "and " << i + << " (indices)."); return false; } } @@ -459,8 +473,8 @@ bool pilz_industrial_motion_planner::isRobotStateEqual(const moveit::core::Robot if ((joint_position_1 - joint_position_2).norm() > epsilon) { - ROS_DEBUG_STREAM("Joint positions of the two states are different. state1: " << joint_position_1 - << " state2: " << joint_position_2); + RCLCPP_DEBUG_STREAM(LOGGER, "Joint positions of the two states are different. state1: " + << joint_position_1 << " state2: " << joint_position_2); return false; } @@ -471,8 +485,8 @@ bool pilz_industrial_motion_planner::isRobotStateEqual(const moveit::core::Robot if ((joint_velocity_1 - joint_velocity_2).norm() > epsilon) { - ROS_DEBUG_STREAM("Joint velocities of the two states are different. state1: " << joint_velocity_1 - << " state2: " << joint_velocity_2); + RCLCPP_DEBUG_STREAM(LOGGER, "Joint velocities of the two states are different. state1: " + << joint_velocity_1 << " state2: " << joint_velocity_2); return false; } @@ -483,8 +497,8 @@ bool pilz_industrial_motion_planner::isRobotStateEqual(const moveit::core::Robot if ((joint_acc_1 - joint_acc_2).norm() > epsilon) { - ROS_DEBUG_STREAM("Joint accelerations of the two states are different. state1: " << joint_acc_1 - << " state2: " << joint_acc_2); + RCLCPP_DEBUG_STREAM(LOGGER, "Joint accelerations of the two states are different. state1: " + << joint_acc_1 << " state2: " << joint_acc_2); return false; } @@ -498,13 +512,13 @@ bool pilz_industrial_motion_planner::isRobotStateStationary(const moveit::core:: state.copyJointGroupVelocities(group, joint_variable); if (joint_variable.norm() > EPSILON) { - ROS_DEBUG("Joint velocities are not zero."); + RCLCPP_DEBUG(LOGGER, "Joint velocities are not zero."); return false; } state.copyJointGroupAccelerations(group, joint_variable); if (joint_variable.norm() > EPSILON) { - ROS_DEBUG("Joint accelerations are not zero."); + RCLCPP_DEBUG(LOGGER, "Joint accelerations are not zero."); return false; } return true; @@ -516,7 +530,7 @@ bool pilz_industrial_motion_planner::linearSearchIntersectionPoint(const std::st const robot_trajectory::RobotTrajectoryPtr& traj, bool inverseOrder, std::size_t& index) { - ROS_DEBUG("Start linear search for intersection point."); + RCLCPP_DEBUG(LOGGER, "Start linear search for intersection point."); const size_t waypoint_num = traj->getWayPointCount(); @@ -557,8 +571,8 @@ bool pilz_industrial_motion_planner::intersectionFound(const Eigen::Vector3d& p_ bool pilz_industrial_motion_planner::isStateColliding(const bool test_for_self_collision, const moveit::core::RobotModelConstPtr& robot_model, - robot_state::RobotState* rstate, - const robot_state::JointModelGroup* const group, + moveit::core::RobotState* rstate, + const moveit::core::JointModelGroup* const group, const double* const ik_solution) { if (!test_for_self_collision) @@ -576,9 +590,9 @@ bool pilz_industrial_motion_planner::isStateColliding(const bool test_for_self_c return !collision_res.collision; } -void normalizeQuaternion(geometry_msgs::Quaternion& quat) +void normalizeQuaternion(geometry_msgs::msg::Quaternion& quat) { tf2::Quaternion q; - tf2::fromMsg(quat, q); - quat = tf2::toMsg(q.normalize()); + tf2::convert(quat, q); + quat = tf2::toMsg(q.normalized()); } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp index 2084f11e4c..dae7225a34 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp @@ -36,6 +36,8 @@ #include +#include +#include #include #include @@ -43,6 +45,7 @@ namespace pilz_industrial_motion_planner { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_generator"); void TrajectoryGenerator::cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& /*req*/) const { // Empty implementation, in case the derived class does not want @@ -81,7 +84,7 @@ void TrajectoryGenerator::checkForValidGroupName(const std::string& group_name) } } -void TrajectoryGenerator::checkStartState(const moveit_msgs::RobotState& start_state) const +void TrajectoryGenerator::checkStartState(const moveit_msgs::msg::RobotState& start_state) const { if (start_state.joint_state.name.empty()) { @@ -107,7 +110,7 @@ void TrajectoryGenerator::checkStartState(const moveit_msgs::RobotState& start_s } } -void TrajectoryGenerator::checkJointGoalConstraint(const moveit_msgs::Constraints& constraint, +void TrajectoryGenerator::checkJointGoalConstraint(const moveit_msgs::msg::Constraints& constraint, const std::vector& expected_joint_names, const std::string& group_name) const { @@ -138,13 +141,13 @@ void TrajectoryGenerator::checkJointGoalConstraint(const moveit_msgs::Constraint } } -void TrajectoryGenerator::checkCartesianGoalConstraint(const moveit_msgs::Constraints& constraint, +void TrajectoryGenerator::checkCartesianGoalConstraint(const moveit_msgs::msg::Constraints& constraint, const std::string& group_name) const { assert(constraint.position_constraints.size() == 1); assert(constraint.orientation_constraints.size() == 1); - const moveit_msgs::PositionConstraint& pos_constraint{ constraint.position_constraints.front() }; - const moveit_msgs::OrientationConstraint& ori_constraint{ constraint.orientation_constraints.front() }; + const moveit_msgs::msg::PositionConstraint& pos_constraint{ constraint.position_constraints.front() }; + const moveit_msgs::msg::OrientationConstraint& ori_constraint{ constraint.orientation_constraints.front() }; if (pos_constraint.link_name.empty()) { @@ -179,7 +182,7 @@ void TrajectoryGenerator::checkCartesianGoalConstraint(const moveit_msgs::Constr } void TrajectoryGenerator::checkGoalConstraints( - const moveit_msgs::MotionPlanRequest::_goal_constraints_type& goal_constraints, + const moveit_msgs::msg::MotionPlanRequest::_goal_constraints_type& goal_constraints, const std::vector& expected_joint_names, const std::string& group_name) const { if (goal_constraints.size() != 1) @@ -189,7 +192,7 @@ void TrajectoryGenerator::checkGoalConstraints( throw NotExactlyOneGoalConstraintGiven(os.str()); } - const moveit_msgs::Constraints& goal_con{ goal_constraints.front() }; + const moveit_msgs::msg::Constraints& goal_con{ goal_constraints.front() }; if (!isOnlyOneGoalTypeGiven(goal_con)) { throw OnlyOneGoalTypeAllowed("Only cartesian XOR joint goal allowed"); @@ -214,8 +217,8 @@ void TrajectoryGenerator::validateRequest(const planning_interface::MotionPlanRe checkGoalConstraints(req.goal_constraints, req.start_state.joint_state.name, req.group_name); } -void TrajectoryGenerator::convertToRobotTrajectory(const trajectory_msgs::JointTrajectory& joint_trajectory, - const moveit_msgs::RobotState& start_state, +void TrajectoryGenerator::convertToRobotTrajectory(const trajectory_msgs::msg::JointTrajectory& joint_trajectory, + const moveit_msgs::msg::RobotState& start_state, robot_trajectory::RobotTrajectory& robot_trajectory) const { moveit::core::RobotState start_rs(robot_model_); @@ -224,27 +227,28 @@ void TrajectoryGenerator::convertToRobotTrajectory(const trajectory_msgs::JointT robot_trajectory.setRobotTrajectoryMsg(start_rs, joint_trajectory); } -void TrajectoryGenerator::setSuccessResponse(const std::string& group_name, const moveit_msgs::RobotState& start_state, - const trajectory_msgs::JointTrajectory& joint_trajectory, - const ros::Time& planning_start, +void TrajectoryGenerator::setSuccessResponse(const std::string& group_name, + const moveit_msgs::msg::RobotState& start_state, + const trajectory_msgs::msg::JointTrajectory& joint_trajectory, + const rclcpp::Time& planning_start, planning_interface::MotionPlanResponse& res) const { robot_trajectory::RobotTrajectoryPtr rt(new robot_trajectory::RobotTrajectory(robot_model_, group_name)); convertToRobotTrajectory(joint_trajectory, start_state, *rt); res.trajectory_ = rt; - res.error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS; - res.planning_time_ = (ros::Time::now() - planning_start).toSec(); + res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + res.planning_time_ = (clock_->now() - planning_start).seconds(); } -void TrajectoryGenerator::setFailureResponse(const ros::Time& planning_start, +void TrajectoryGenerator::setFailureResponse(const rclcpp::Time& planning_start, planning_interface::MotionPlanResponse& res) const { if (res.trajectory_) { res.trajectory_->clear(); } - res.planning_time_ = (ros::Time::now() - planning_start).toSec(); + res.planning_time_ = (clock_->now() - planning_start).seconds(); } std::unique_ptr @@ -270,8 +274,8 @@ TrajectoryGenerator::cartesianTrapVelocityProfile(const double& max_velocity_sca bool TrajectoryGenerator::generate(const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, double sampling_time) { - ROS_INFO_STREAM("Generating " << req.planner_id << " trajectory..."); - ros::Time planning_begin = ros::Time::now(); + RCLCPP_INFO_STREAM(LOGGER, "Generating " << req.planner_id << " trajectory..."); + rclcpp::Time planning_begin = clock_->now(); try { @@ -279,7 +283,7 @@ bool TrajectoryGenerator::generate(const planning_interface::MotionPlanRequest& } catch (const MoveItErrorCodeException& ex) { - ROS_ERROR_STREAM(ex.what()); + RCLCPP_ERROR_STREAM(LOGGER, ex.what()); res.error_code_.val = ex.getErrorCode(); setFailureResponse(planning_begin, res); return false; @@ -291,7 +295,7 @@ bool TrajectoryGenerator::generate(const planning_interface::MotionPlanRequest& } catch (const MoveItErrorCodeException& ex) { - ROS_ERROR_STREAM(ex.what()); + RCLCPP_ERROR_STREAM(LOGGER, ex.what()); res.error_code_.val = ex.getErrorCode(); setFailureResponse(planning_begin, res); return false; @@ -304,20 +308,20 @@ bool TrajectoryGenerator::generate(const planning_interface::MotionPlanRequest& } catch (const MoveItErrorCodeException& ex) { - ROS_ERROR_STREAM(ex.what()); + RCLCPP_ERROR_STREAM(LOGGER, ex.what()); res.error_code_.val = ex.getErrorCode(); setFailureResponse(planning_begin, res); return false; } - trajectory_msgs::JointTrajectory joint_trajectory; + trajectory_msgs::msg::JointTrajectory joint_trajectory; try { plan(req, plan_info, sampling_time, joint_trajectory); } catch (const MoveItErrorCodeException& ex) { - ROS_ERROR_STREAM(ex.what()); + RCLCPP_ERROR_STREAM(LOGGER, ex.what()); res.error_code_.val = ex.getErrorCode(); setFailureResponse(planning_begin, res); return false; diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp index 4bdc2a7ddf..91e49b573b 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp @@ -43,13 +43,17 @@ #include #include #include -#include +#include +#include #include #include +#include #include namespace pilz_industrial_motion_planner { +static const rclcpp::Logger LOGGER = + rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_generator_circ"); TrajectoryGeneratorCIRC::TrajectoryGeneratorCIRC(const moveit::core::RobotModelConstPtr& robot_model, const LimitsContainer& planner_limits) : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits) @@ -87,7 +91,7 @@ void TrajectoryGeneratorCIRC::cmdSpecificRequestValidation(const planning_interf void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, TrajectoryGenerator::MotionPlanInfo& info) const { - ROS_DEBUG("Extract necessary information from motion plan request."); + RCLCPP_DEBUG(LOGGER, "Extract necessary information from motion plan request."); info.group_name = req.group_name; std::string frame_id{ robot_model_->getModelFrame() }; @@ -132,20 +136,20 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_interface::Mo if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() || req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty()) { - ROS_WARN("Frame id is not set in position/orientation constraints of " - "goal. Use model frame as default"); + RCLCPP_WARN(LOGGER, "Frame id is not set in position/orientation constraints of " + "goal. Use model frame as default"); frame_id = robot_model_->getModelFrame(); } else { frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id; } - geometry_msgs::Pose goal_pose_msg; + geometry_msgs::msg::Pose goal_pose_msg; goal_pose_msg.position = req.goal_constraints.front().position_constraints.front().constraint_region.primitive_poses.front().position; goal_pose_msg.orientation = req.goal_constraints.front().orientation_constraints.front().orientation; normalizeQuaternion(goal_pose_msg.orientation); - tf2::fromMsg(goal_pose_msg, info.goal_pose); + tf2::convert(goal_pose_msg, info.goal_pose); } assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size()); @@ -187,7 +191,7 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_interface::Mo } void TrajectoryGeneratorCIRC::plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, - const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) + const double& sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) { std::unique_ptr cart_path(setPathCIRC(plan_info)); std::unique_ptr vel_profile( @@ -199,7 +203,7 @@ void TrajectoryGeneratorCIRC::plan(const planning_interface::MotionPlanRequest& // the ownship of Path and Velocity Profile KDL::Trajectory_Segment cart_trajectory(cart_path.get(), vel_profile.get(), false); - moveit_msgs::MoveItErrorCodes error_code; + moveit_msgs::msg::MoveItErrorCodes error_code; // sample the Cartesian trajectory and compute joint trajectory using inverse // kinematics if (!generateJointTrajectory(robot_model_, planner_limits_.getJointLimitContainer(), cart_trajectory, @@ -213,11 +217,11 @@ void TrajectoryGeneratorCIRC::plan(const planning_interface::MotionPlanRequest& std::unique_ptr TrajectoryGeneratorCIRC::setPathCIRC(const MotionPlanInfo& info) const { - ROS_DEBUG("Set Cartesian path for CIRC command."); + RCLCPP_DEBUG(LOGGER, "Set Cartesian path for CIRC command."); KDL::Frame start_pose, goal_pose; - tf2::fromMsg(tf2::toMsg(info.start_pose), start_pose); - tf2::fromMsg(tf2::toMsg(info.goal_pose), goal_pose); + tf2::transformEigenToKDL(info.start_pose, start_pose); + tf2::transformEigenToKDL(info.goal_pose, goal_pose); const auto& eigen_path_point = info.circ_path_point.second; const KDL::Vector path_point{ eigen_path_point.x(), eigen_path_point.y(), eigen_path_point.z() }; diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp index 6efc6e41ba..99728657b7 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -35,7 +35,7 @@ #include "pilz_industrial_motion_planner/trajectory_generator_lin.h" #include -#include +#include #include #include @@ -45,19 +45,23 @@ #include #include +#include #include #include +#include #include namespace pilz_industrial_motion_planner { +static const rclcpp::Logger LOGGER = + rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_generator_lin"); TrajectoryGeneratorLIN::TrajectoryGeneratorLIN(const moveit::core::RobotModelConstPtr& robot_model, const LimitsContainer& planner_limits) : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits) { if (!planner_limits_.hasFullCartesianLimits()) { - ROS_ERROR("Cartesian limits not set for LIN trajectory generator."); + RCLCPP_ERROR(LOGGER, "Cartesian limits not set for LIN trajectory generator."); throw TrajectoryGeneratorInvalidLimitsException("Cartesian limits are not fully set for LIN trajectory generator."); } } @@ -65,7 +69,7 @@ TrajectoryGeneratorLIN::TrajectoryGeneratorLIN(const moveit::core::RobotModelCon void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, TrajectoryGenerator::MotionPlanInfo& info) const { - ROS_DEBUG("Extract necessary information from motion plan request."); + RCLCPP_DEBUG(LOGGER, "Extract necessary information from motion plan request."); info.group_name = req.group_name; std::string frame_id{ robot_model_->getModelFrame() }; @@ -107,20 +111,20 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_interface::Mot if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() || req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty()) { - ROS_WARN("Frame id is not set in position/orientation constraints of " - "goal. Use model frame as default"); + RCLCPP_WARN(LOGGER, "Frame id is not set in position/orientation constraints of " + "goal. Use model frame as default"); frame_id = robot_model_->getModelFrame(); } else { frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id; } - geometry_msgs::Pose goal_pose_msg; + geometry_msgs::msg::Pose goal_pose_msg; goal_pose_msg.position = req.goal_constraints.front().position_constraints.front().constraint_region.primitive_poses.front().position; goal_pose_msg.orientation = req.goal_constraints.front().orientation_constraints.front().orientation; normalizeQuaternion(goal_pose_msg.orientation); - tf2::fromMsg(goal_pose_msg, info.goal_pose); + tf2::convert(goal_pose_msg, info.goal_pose); } assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size()); @@ -154,7 +158,7 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_interface::Mot } void TrajectoryGeneratorLIN::plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, - const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) + const double& sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) { // create Cartesian path for lin std::unique_ptr path(setPathLIN(plan_info.start_pose, plan_info.goal_pose)); @@ -169,7 +173,7 @@ void TrajectoryGeneratorLIN::plan(const planning_interface::MotionPlanRequest& r // the ownship of Path and Velocity Profile KDL::Trajectory_Segment cart_trajectory(path.get(), vp.get(), false); - moveit_msgs::MoveItErrorCodes error_code; + moveit_msgs::msg::MoveItErrorCodes error_code; // sample the Cartesian trajectory and compute joint trajectory using inverse // kinematics if (!generateJointTrajectory(robot_model_, planner_limits_.getJointLimitContainer(), cart_trajectory, @@ -185,11 +189,11 @@ void TrajectoryGeneratorLIN::plan(const planning_interface::MotionPlanRequest& r std::unique_ptr TrajectoryGeneratorLIN::setPathLIN(const Eigen::Affine3d& start_pose, const Eigen::Affine3d& goal_pose) const { - ROS_DEBUG("Set Cartesian path for LIN command."); + RCLCPP_DEBUG(LOGGER, "Set Cartesian path for LIN command."); KDL::Frame kdl_start_pose, kdl_goal_pose; - tf2::fromMsg(tf2::toMsg(start_pose), kdl_start_pose); - tf2::fromMsg(tf2::toMsg(goal_pose), kdl_goal_pose); + tf2::transformEigenToKDL(start_pose, kdl_start_pose); + tf2::transformEigenToKDL(goal_pose, kdl_goal_pose); double eqradius = planner_limits_.getCartesianLimits().getMaxTranslationalVelocity() / planner_limits_.getCartesianLimits().getMaxRotationalVelocity(); KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis(); diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp index 8b8adcb34d..6bae0e089a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp @@ -34,7 +34,7 @@ #include "pilz_industrial_motion_planner/trajectory_generator_ptp.h" #include "moveit/robot_state/conversions.h" -#include "ros/ros.h" +#include #include #include @@ -44,7 +44,9 @@ namespace pilz_industrial_motion_planner { -TrajectoryGeneratorPTP::TrajectoryGeneratorPTP(const robot_model::RobotModelConstPtr& robot_model, +static const rclcpp::Logger LOGGER = + rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_generator_ptp"); +TrajectoryGeneratorPTP::TrajectoryGeneratorPTP(const moveit::core::RobotModelConstPtr& robot_model, const LimitsContainer& planner_limits) : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits) { @@ -70,31 +72,31 @@ TrajectoryGeneratorPTP::TrajectoryGeneratorPTP(const robot_model::RobotModelCons if (!most_strict_limit.has_velocity_limits) { - ROS_ERROR_STREAM("velocity limit not set for group " << jmg->getName()); + RCLCPP_ERROR_STREAM(LOGGER, "velocity limit not set for group " << jmg->getName()); throw TrajectoryGeneratorInvalidLimitsException("velocity limit not set for group " + jmg->getName()); } if (!most_strict_limit.has_acceleration_limits) { - ROS_ERROR_STREAM("acceleration limit not set for group " << jmg->getName()); + RCLCPP_ERROR_STREAM(LOGGER, "acceleration limit not set for group " << jmg->getName()); throw TrajectoryGeneratorInvalidLimitsException("acceleration limit not set for group " + jmg->getName()); } if (!most_strict_limit.has_deceleration_limits) { - ROS_ERROR_STREAM("deceleration limit not set for group " << jmg->getName()); + RCLCPP_ERROR_STREAM(LOGGER, "deceleration limit not set for group " << jmg->getName()); throw TrajectoryGeneratorInvalidLimitsException("deceleration limit not set for group " + jmg->getName()); } most_strict_limits_.insert(std::pair(jmg->getName(), most_strict_limit)); } - ROS_INFO("Initialized Point-to-Point Trajectory Generator."); + RCLCPP_INFO(LOGGER, "Initialized Point-to-Point Trajectory Generator."); } void TrajectoryGeneratorPTP::planPTP(const std::map& start_pos, const std::map& goal_pos, - trajectory_msgs::JointTrajectory& joint_trajectory, const std::string& group_name, - const double& velocity_scaling_factor, const double& acceleration_scaling_factor, - const double& sampling_time) + trajectory_msgs::msg::JointTrajectory& joint_trajectory, + const std::string& group_name, const double& velocity_scaling_factor, + const double& acceleration_scaling_factor, const double& sampling_time) { // initialize joint names for (const auto& item : goal_pos) @@ -114,11 +116,11 @@ void TrajectoryGeneratorPTP::planPTP(const std::map& start_ } if (goal_reached) { - ROS_INFO_STREAM("Goal already reached, set one goal point explicitly."); + RCLCPP_INFO_STREAM(LOGGER, "Goal already reached, set one goal point explicitly."); if (joint_trajectory.points.empty()) { - trajectory_msgs::JointTrajectoryPoint point; - point.time_from_start = ros::Duration(sampling_time); + trajectory_msgs::msg::JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(sampling_time); for (const std::string& joint_name : joint_trajectory.joint_names) { point.positions.push_back(start_pos.at(joint_name)); @@ -195,8 +197,8 @@ void TrajectoryGeneratorPTP::planPTP(const std::map& start_ // construct joint trajectory point for (double time_stamp : time_samples) { - trajectory_msgs::JointTrajectoryPoint point; - point.time_from_start = ros::Duration(time_stamp); + trajectory_msgs::msg::JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(time_stamp); for (std::string& joint_name : joint_trajectory.joint_names) { point.positions.push_back(velocity_profile.at(joint_name).Pos(time_stamp)); @@ -236,18 +238,18 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_interface::Mot // slove the ik else { - geometry_msgs::Point p = + geometry_msgs::msg::Point p = req.goal_constraints.at(0).position_constraints.at(0).constraint_region.primitive_poses.at(0).position; p.x -= req.goal_constraints.at(0).position_constraints.at(0).target_point_offset.x; p.y -= req.goal_constraints.at(0).position_constraints.at(0).target_point_offset.y; p.z -= req.goal_constraints.at(0).position_constraints.at(0).target_point_offset.z; - geometry_msgs::Pose pose; + geometry_msgs::msg::Pose pose; pose.position = p; pose.orientation = req.goal_constraints.at(0).orientation_constraints.at(0).orientation; Eigen::Isometry3d pose_eigen; normalizeQuaternion(pose.orientation); - tf2::fromMsg(pose, pose_eigen); + tf2::convert(pose, pose_eigen); if (!computePoseIK(robot_model_, req.group_name, req.goal_constraints.at(0).position_constraints.at(0).link_name, pose_eigen, robot_model_->getModelFrame(), info.start_joint_position, info.goal_joint_position)) { @@ -257,7 +259,7 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_interface::Mot } void TrajectoryGeneratorPTP::plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, - const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) + const double& sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) { // plan the ptp trajectory planPTP(plan_info.start_joint_position, plan_info.goal_joint_position, joint_trajectory, plan_info.group_name, diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.cpp index 9eef3bf195..d4a2850562 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.cpp @@ -131,19 +131,19 @@ void IntegrationTestCommandListManager::SetUp() TEST_F(IntegrationTestCommandListManager, TestExceptionErrorCodeMapping) { std::shared_ptr nbr_ex{ new NegativeBlendRadiusException("") }; - EXPECT_EQ(nbr_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(nbr_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); std::shared_ptr lbrnz_ex{ new LastBlendRadiusNotZeroException("") }; - EXPECT_EQ(lbrnz_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(lbrnz_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); std::shared_ptr sss_ex{ new StartStateSetException("") }; - EXPECT_EQ(sss_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + EXPECT_EQ(sss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); std::shared_ptr obr_ex{ new OverlappingBlendRadiiException("") }; - EXPECT_EQ(obr_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(obr_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); std::shared_ptr pp_ex{ new PlanningPipelineException("") }; - EXPECT_EQ(pp_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + EXPECT_EQ(pp_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE); } /** @@ -179,7 +179,7 @@ TEST_F(IntegrationTestCommandListManager, concatThreeSegments) EXPECT_TRUE(hasStrictlyIncreasingTime(res123_vec.front())) << "Time steps not strictly positively increasing"; ROS_INFO("step 2: only first segment"); - moveit_msgs::MotionSequenceRequest req_1; + moveit_msgs::msg::MotionSequenceRequest req_1; req_1.items.resize(1); req_1.items.at(0).req = seq.getCmd(0).toRequest(); req_1.items.at(0).blend_radius = 0.; @@ -190,7 +190,7 @@ TEST_F(IntegrationTestCommandListManager, concatThreeSegments) req_1.items.at(0).req.start_state.joint_state.name.size()); ROS_INFO("step 3: only second segment"); - moveit_msgs::MotionSequenceRequest req_2; + moveit_msgs::msg::MotionSequenceRequest req_2; req_2.items.resize(1); req_2.items.at(0).req = seq.getCmd(1).toRequest(); req_2.items.at(0).blend_radius = 0.; @@ -201,7 +201,7 @@ TEST_F(IntegrationTestCommandListManager, concatThreeSegments) req_2.items.at(0).req.start_state.joint_state.name.size()); ROS_INFO("step 4: only third segment"); - moveit_msgs::MotionSequenceRequest req_3; + moveit_msgs::msg::MotionSequenceRequest req_3; req_3.items.resize(1); req_3.items.at(0).req = seq.getCmd(2).toRequest(); req_3.items.at(0).blend_radius = 0.; @@ -334,7 +334,7 @@ TEST_F(IntegrationTestCommandListManager, blendTwoSegments) { Sequence seq{ data_loader_->getSequence("SimpleSequence") }; ASSERT_EQ(seq.size(), 2u); - moveit_msgs::MotionSequenceRequest req{ seq.toRequest() }; + moveit_msgs::msg::MotionSequenceRequest req{ seq.toRequest() }; RobotTrajCont res_vec{ manager_->solve(scene_, pipeline_, req) }; EXPECT_EQ(res_vec.size(), 1u); EXPECT_GT(res_vec.front()->getWayPointCount(), 0u); @@ -342,12 +342,12 @@ TEST_F(IntegrationTestCommandListManager, blendTwoSegments) req.items.at(0).req.start_state.joint_state.name.size()); ros::NodeHandle nh; - ros::Publisher pub = nh.advertise("my_planned_path", 1); + ros::Publisher pub = nh.advertise("my_planned_path", 1); ros::Duration duration(1.0); // wait to notify possible subscribers duration.sleep(); - moveit_msgs::DisplayTrajectory display_trajectory; - moveit_msgs::RobotTrajectory rob_traj_msg; + moveit_msgs::msg::DisplayTrajectory display_trajectory; + moveit_msgs::msg::RobotTrajectory rob_traj_msg; res_vec.front()->getRobotTrajectoryMsg(rob_traj_msg); display_trajectory.trajectory.push_back(rob_traj_msg); pub.publish(display_trajectory); @@ -368,7 +368,7 @@ TEST_F(IntegrationTestCommandListManager, blendTwoSegments) */ TEST_F(IntegrationTestCommandListManager, emptyList) { - moveit_msgs::MotionSequenceRequest empty_list; + moveit_msgs::msg::MotionSequenceRequest empty_list; RobotTrajCont res_vec{ manager_->solve(scene_, pipeline_, empty_list) }; EXPECT_TRUE(res_vec.empty()); } @@ -405,7 +405,7 @@ TEST_F(IntegrationTestCommandListManager, startStateNotFirstGoal) Sequence seq{ data_loader_->getSequence("SimpleSequence") }; ASSERT_GE(seq.size(), 2u); const LinCart& lin{ seq.getCmd(0) }; - moveit_msgs::MotionSequenceRequest req{ seq.toRequest() }; + moveit_msgs::msg::MotionSequenceRequest req{ seq.toRequest() }; req.items.at(1).req.start_state = lin.getGoalConfiguration().toMoveitMsgsRobotState(); EXPECT_THROW(manager_->solve(scene_, pipeline_, req), StartStateSetException); } @@ -521,18 +521,18 @@ TEST_F(IntegrationTestCommandListManager, TestExecutionTime) EXPECT_EQ(res_single_vec.size(), 1u); EXPECT_GT(res_single_vec.front()->getWayPointCount(), 0u); - moveit_msgs::MotionSequenceRequest req{ seq.toRequest() }; + moveit_msgs::msg::MotionSequenceRequest req{ seq.toRequest() }; // Create large request by making copies of the original sequence commands // and adding them to the end of the original sequence. const size_t n{ req.items.size() }; for (size_t i = 0; i < n; ++i) { - moveit_msgs::MotionSequenceItem item{ req.items.at(i) }; + moveit_msgs::msg::MotionSequenceItem item{ req.items.at(i) }; if (i == 0) { // Remove start state because only the first request // is allowed to have a start state in a sequence. - item.req.start_state = moveit_msgs::RobotState(); + item.req.start_state = moveit_msgs::msg::RobotState(); } req.items.push_back(item); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.cpp index 69914a83db..2c67bae650 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.cpp @@ -129,16 +129,16 @@ TEST_F(IntegrationTestCommandPlanning, PtpJoint) auto ptp{ test_data_->getPtpJoint("Ptp1") }; moveit_msgs::GetMotionPlan srv; - moveit_msgs::MotionPlanRequest req = ptp.toRequest(); + moveit_msgs::msg::MotionPlanRequest req = ptp.toRequest(); srv.request.motion_plan_request = req; ros::ServiceClient client = node_handle.serviceClient(PLAN_SERVICE_NAME); ASSERT_TRUE(client.call(srv)); - const moveit_msgs::MotionPlanResponse& response{ srv.response.motion_plan_response }; + const moveit_msgs::msg::MotionPlanResponse& response{ srv.response.motion_plan_response }; // Check the result - ASSERT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!"; + ASSERT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!"; trajectory_msgs::JointTrajectory trajectory = response.trajectory.joint_trajectory; EXPECT_EQ(trajectory.joint_names.size(), num_joints_) << "Wrong number of jointnames"; @@ -186,10 +186,10 @@ TEST_F(IntegrationTestCommandPlanning, PtpJointCart) ros::ServiceClient client = node_handle.serviceClient(PLAN_SERVICE_NAME); ASSERT_TRUE(client.call(srv)); - const moveit_msgs::MotionPlanResponse& response{ srv.response.motion_plan_response }; + const moveit_msgs::msg::MotionPlanResponse& response{ srv.response.motion_plan_response }; // Make sure the planning succeeded - ASSERT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!"; + ASSERT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!"; // Check the result trajectory_msgs::JointTrajectory trajectory = response.trajectory.joint_trajectory; @@ -251,9 +251,9 @@ TEST_F(IntegrationTestCommandPlanning, LinJoint) ros::ServiceClient client = node_handle.serviceClient(PLAN_SERVICE_NAME); ASSERT_TRUE(client.call(srv)); - const moveit_msgs::MotionPlanResponse& response{ srv.response.motion_plan_response }; + const moveit_msgs::msg::MotionPlanResponse& response{ srv.response.motion_plan_response }; - ASSERT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!"; + ASSERT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!"; std::cout << "++++++++++" << std::endl; std::cout << "+ Step 2 +" << std::endl; @@ -302,9 +302,9 @@ TEST_F(IntegrationTestCommandPlanning, LinJointCart) ros::ServiceClient client = node_handle.serviceClient(PLAN_SERVICE_NAME); ASSERT_TRUE(client.call(srv)); - const moveit_msgs::MotionPlanResponse& response{ srv.response.motion_plan_response }; + const moveit_msgs::msg::MotionPlanResponse& response{ srv.response.motion_plan_response }; - ASSERT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!"; + ASSERT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!"; std::cout << "++++++++++" << std::endl; std::cout << "+ Step 2 +" << std::endl; @@ -341,7 +341,7 @@ TEST_F(IntegrationTestCommandPlanning, CircJointCenterCart) CircJointCenterCart circ{ test_data_->getCircJointCenterCart("circ1_center_2") }; - moveit_msgs::MotionPlanRequest req{ circ.toRequest() }; + moveit_msgs::msg::MotionPlanRequest req{ circ.toRequest() }; moveit_msgs::GetMotionPlan srv; srv.request.motion_plan_request = req; @@ -349,10 +349,10 @@ TEST_F(IntegrationTestCommandPlanning, CircJointCenterCart) ros::ServiceClient client = node_handle.serviceClient(PLAN_SERVICE_NAME); ASSERT_TRUE(client.call(srv)); - const moveit_msgs::MotionPlanResponse& response{ srv.response.motion_plan_response }; + const moveit_msgs::msg::MotionPlanResponse& response{ srv.response.motion_plan_response }; // Check the result - ASSERT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!"; + ASSERT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!"; trajectory_msgs::JointTrajectory trajectory = response.trajectory.joint_trajectory; EXPECT_EQ(trajectory.joint_names.size(), num_joints_) << "Wrong number of jointnames"; @@ -425,17 +425,17 @@ TEST_F(IntegrationTestCommandPlanning, CircCartCenterCart) ros::NodeHandle node_handle("~"); CircCenterCart circ{ test_data_->getCircCartCenterCart("circ1_center_2") }; - moveit_msgs::MotionPlanRequest req{ circ.toRequest() }; + moveit_msgs::msg::MotionPlanRequest req{ circ.toRequest() }; moveit_msgs::GetMotionPlan srv; srv.request.motion_plan_request = req; ros::ServiceClient client = node_handle.serviceClient(PLAN_SERVICE_NAME); ASSERT_TRUE(client.call(srv)); - const moveit_msgs::MotionPlanResponse& response = srv.response.motion_plan_response; + const moveit_msgs::msg::MotionPlanResponse& response = srv.response.motion_plan_response; // Check the result - ASSERT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!"; + ASSERT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!"; trajectory_msgs::JointTrajectory trajectory = response.trajectory.joint_trajectory; EXPECT_EQ(trajectory.joint_names.size(), num_joints_) << "Wrong number of jointnames"; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.cpp index 43f0415771..1e84c1183f 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.cpp @@ -82,16 +82,16 @@ void IntegrationTestPlanComponentBuilder::SetUp() TEST_F(IntegrationTestPlanComponentBuilder, TestExceptionErrorCodeMapping) { std::shared_ptr nbs_ex{ new NoBlenderSetException("") }; - EXPECT_EQ(nbs_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + EXPECT_EQ(nbs_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE); std::shared_ptr ntffse_ex{ new NoTipFrameFunctionSetException("") }; - EXPECT_EQ(ntffse_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + EXPECT_EQ(ntffse_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE); std::shared_ptr nrms_ex{ new NoRobotModelSetException("") }; - EXPECT_EQ(nrms_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + EXPECT_EQ(nrms_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE); std::shared_ptr bf_ex{ new BlendingFailedException("") }; - EXPECT_EQ(bf_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + EXPECT_EQ(bf_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE); } /** diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.cpp index a5883e71cc..d6054a6886 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.cpp @@ -83,13 +83,13 @@ class IntegrationTestSequenceAction : public testing::Test, public testing::Asyn public: MOCK_METHOD0(active_callback, void()); - MOCK_METHOD1(feedback_callback, void(const moveit_msgs::MoveGroupSequenceFeedbackConstPtr& feedback)); + MOCK_METHOD1(feedback_callback, void(const moveit_msgs::msg::MoveGroupSequenceFeedbackConstPtr& feedback)); MOCK_METHOD2(done_callback, void(const actionlib::SimpleClientGoalState& state, - const moveit_msgs::MoveGroupSequenceResultConstPtr& result)); + const moveit_msgs::msg::MoveGroupSequenceResultConstPtr& result)); protected: ros::NodeHandle ph_{ "~" }; - actionlib::SimpleActionClient ac_{ ph_, SEQUENCE_ACTION_NAME, true }; + actionlib::SimpleActionClient ac_{ ph_, SEQUENCE_ACTION_NAME, true }; std::shared_ptr move_group_; robot_model_loader::RobotModelLoader model_loader_; @@ -146,11 +146,12 @@ void IntegrationTestSequenceAction::SetUp() */ TEST_F(IntegrationTestSequenceAction, TestSendingOfEmptySequence) { - moveit_msgs::MoveGroupSequenceGoal seq_goal; + moveit_msgs::msg::MoveGroupSequenceGoal seq_goal; ac_.sendGoalAndWait(seq_goal); - moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); - EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS) << "Execution of sequence failed."; + moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + << "Execution of sequence failed."; EXPECT_TRUE(res->response.planned_trajectories.empty()); } @@ -172,12 +173,13 @@ TEST_F(IntegrationTestSequenceAction, TestDifferingGroupNames) MotionCmd& cmd{ seq.getCmd(1) }; cmd.setPlanningGroup("WrongGroupName"); - moveit_msgs::MoveGroupSequenceGoal seq_goal; + moveit_msgs::msg::MoveGroupSequenceGoal seq_goal; seq_goal.request = seq.toRequest(); ac_.sendGoalAndWait(seq_goal); - moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); - EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME) << "Incorrect error code."; + moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME) + << "Incorrect error code."; EXPECT_TRUE(res->response.planned_trajectories.empty()); } @@ -198,13 +200,13 @@ TEST_F(IntegrationTestSequenceAction, TestNegativeBlendRadius) Sequence seq{ data_loader_->getSequence("ComplexSequence") }; seq.setBlendRadius(0, -1.0); - moveit_msgs::MoveGroupSequenceGoal seq_goal; + moveit_msgs::msg::MoveGroupSequenceGoal seq_goal; seq_goal.request = seq.toRequest(); ac_.sendGoalAndWait(seq_goal); - moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); - EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN) + moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN) << "Incorrect error code."; EXPECT_TRUE(res->response.planned_trajectories.empty()); } @@ -227,13 +229,14 @@ TEST_F(IntegrationTestSequenceAction, TestOverlappingBlendRadii) Sequence seq{ data_loader_->getSequence("ComplexSequence") }; seq.setBlendRadius(0, 10 * seq.getBlendRadius(0)); - moveit_msgs::MoveGroupSequenceGoal seq_goal; + moveit_msgs::msg::MoveGroupSequenceGoal seq_goal; seq_goal.request = seq.toRequest(); ac_.sendGoalAndWait(seq_goal); - moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); - EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN) << "Incorrect error code"; + moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN) + << "Incorrect error code"; EXPECT_TRUE(res->response.planned_trajectories.empty()); } @@ -256,13 +259,13 @@ TEST_F(IntegrationTestSequenceAction, TestTooLargeBlendRadii) seq.erase(2, seq.size()); seq.setBlendRadius(0, 10 * seq.getBlendRadius(seq.size() - 2)); - moveit_msgs::MoveGroupSequenceGoal seq_goal; + moveit_msgs::msg::MoveGroupSequenceGoal seq_goal; seq_goal.request = seq.toRequest(); ac_.sendGoalAndWait(seq_goal); - moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); - EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::FAILURE) << "Incorrect error code"; + moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::FAILURE) << "Incorrect error code"; EXPECT_TRUE(res->response.planned_trajectories.empty()); } @@ -286,12 +289,12 @@ TEST_F(IntegrationTestSequenceAction, TestInvalidCmd) // Erase certain command to invalid command following the command in sequence. seq.erase(3, 4); - moveit_msgs::MoveGroupSequenceGoal seq_goal; + moveit_msgs::msg::MoveGroupSequenceGoal seq_goal; seq_goal.request = seq.toRequest(); ac_.sendGoalAndWait(seq_goal); - moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); - EXPECT_NE(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS) << "Incorrect error code."; + moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_NE(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS) << "Incorrect error code."; EXPECT_TRUE(res->response.planned_trajectories.empty()); } @@ -315,12 +318,12 @@ TEST_F(IntegrationTestSequenceAction, TestInvalidLinkName) CircInterimCart& circ{ seq.getCmd(1) }; circ.getGoalConfiguration().setLinkName("InvalidLinkName"); - moveit_msgs::MoveGroupSequenceGoal seq_goal; + moveit_msgs::msg::MoveGroupSequenceGoal seq_goal; seq_goal.request = seq.toRequest(); ac_.sendGoalAndWait(seq_goal); - moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); - EXPECT_NE(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS) << "Incorrect error code."; + moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_NE(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS) << "Incorrect error code."; EXPECT_TRUE(res->response.planned_trajectories.empty()); } @@ -333,7 +336,7 @@ MATCHER_P(FeedbackStateEq, state, "") } MATCHER(IsResultSuccess, "") { - return arg->response.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS; + return arg->response.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS; } MATCHER(IsResultNotEmpty, "") { @@ -365,7 +368,7 @@ TEST_F(IntegrationTestSequenceAction, TestActionServerCallbacks) // We do not need the complete sequence, just two commands. seq.erase(2, seq.size()); - moveit_msgs::MoveGroupSequenceGoal seq_goal; + moveit_msgs::msg::MoveGroupSequenceGoal seq_goal; seq_goal.request = seq.toRequest(); // set expectations (no guarantee, that done callback is called before idle @@ -415,13 +418,13 @@ TEST_F(IntegrationTestSequenceAction, TestPlanOnlyFlag) // We do not need the complete sequence, just two commands. seq.erase(2, seq.size()); - moveit_msgs::MoveGroupSequenceGoal seq_goal; + moveit_msgs::msg::MoveGroupSequenceGoal seq_goal; seq_goal.planning_options.plan_only = true; seq_goal.request = seq.toRequest(); ac_.sendGoalAndWait(seq_goal); - moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); - EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS) << "Sequence execution failed."; + moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS) << "Sequence execution failed."; EXPECT_FALSE(res->response.planned_trajectories.empty()) << "Planned trajectory is empty"; ASSERT_TRUE(isAtExpectedPosition(*(move_group_->getCurrentState()), start_config.toRobotState(), @@ -448,15 +451,16 @@ TEST_F(IntegrationTestSequenceAction, TestIgnoreRobotStateForPlanOnly) seq.erase(2, seq.size()); // create request - moveit_msgs::MoveGroupSequenceGoal seq_goal; + moveit_msgs::msg::MoveGroupSequenceGoal seq_goal; seq_goal.planning_options.plan_only = true; seq_goal.request = seq.toRequest(); seq_goal.planning_options.planning_scene_diff.robot_state.is_diff = true; ac_.sendGoalAndWait(seq_goal); - moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); - EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS) << "Execution of sequence failed."; + moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + << "Execution of sequence failed."; EXPECT_FALSE(res->response.planned_trajectories.empty()) << "Planned trajectory is empty"; ASSERT_TRUE(isAtExpectedPosition(*(move_group_->getCurrentState()), start_config.toRobotState(), @@ -482,14 +486,14 @@ TEST_F(IntegrationTestSequenceAction, TestNegativeBlendRadiusForPlanOnly) Sequence seq{ data_loader_->getSequence("ComplexSequence") }; seq.setBlendRadius(0, -1.0); - moveit_msgs::MoveGroupSequenceGoal seq_goal; + moveit_msgs::msg::MoveGroupSequenceGoal seq_goal; seq_goal.request = seq.toRequest(); seq_goal.planning_options.plan_only = true; ac_.sendGoalAndWait(seq_goal); - moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); - EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN) + moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN) << "Incorrect error code."; EXPECT_TRUE(res->response.planned_trajectories.empty()); } @@ -513,14 +517,15 @@ TEST_F(IntegrationTestSequenceAction, TestIgnoreRobotState) seq.erase(2, seq.size()); // create request - moveit_msgs::MoveGroupSequenceGoal seq_goal; + moveit_msgs::msg::MoveGroupSequenceGoal seq_goal; seq_goal.request = seq.toRequest(); seq_goal.planning_options.planning_scene_diff.robot_state.is_diff = true; ac_.sendGoalAndWait(seq_goal); - moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); - EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS) << "Execution of sequence failed."; + moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + << "Execution of sequence failed."; EXPECT_FALSE(res->response.planned_trajectories.empty()) << "Planned trajectory is empty"; } @@ -538,28 +543,28 @@ TEST_F(IntegrationTestSequenceAction, TestIgnoreRobotState) TEST_F(IntegrationTestSequenceAction, TestLargeRequest) { Sequence seq{ data_loader_->getSequence("ComplexSequence") }; - moveit_msgs::MotionSequenceRequest req{ seq.toRequest() }; + moveit_msgs::msg::MotionSequenceRequest req{ seq.toRequest() }; // Create large request by making copies of the original sequence commands // and adding them to the end of the original sequence. size_t n{ req.items.size() }; for (size_t i = 0; i < n; ++i) { - moveit_msgs::MotionSequenceItem item{ req.items.at(i) }; + moveit_msgs::msg::MotionSequenceItem item{ req.items.at(i) }; if (i == 0) { // Remove start state because only the first request // is allowed to have a start state in a sequence. - item.req.start_state = moveit_msgs::RobotState(); + item.req.start_state = moveit_msgs::msg::RobotState(); } req.items.push_back(item); } - moveit_msgs::MoveGroupSequenceGoal seq_goal; + moveit_msgs::msg::MoveGroupSequenceGoal seq_goal; seq_goal.request = req; ac_.sendGoalAndWait(seq_goal); - moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); - EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS) << "Incorrect error code."; + moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS) << "Incorrect error code."; EXPECT_FALSE(res->response.planned_trajectories.empty()) << "Planned trajectory is empty"; } @@ -581,12 +586,12 @@ TEST_F(IntegrationTestSequenceAction, TestComplexSequenceWithoutBlending) seq.setAllBlendRadiiToZero(); - moveit_msgs::MoveGroupSequenceGoal seq_goal; + moveit_msgs::msg::MoveGroupSequenceGoal seq_goal; seq_goal.request = seq.toRequest(); ac_.sendGoalAndWait(seq_goal); - moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); - EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); EXPECT_FALSE(res->response.planned_trajectories.empty()) << "Planned trajectory is empty"; } @@ -606,12 +611,12 @@ TEST_F(IntegrationTestSequenceAction, TestComplexSequenceWithBlending) { Sequence seq{ data_loader_->getSequence("ComplexSequence") }; - moveit_msgs::MoveGroupSequenceGoal seq_goal; + moveit_msgs::msg::MoveGroupSequenceGoal seq_goal; seq_goal.request = seq.toRequest(); ac_.sendGoalAndWait(seq_goal); - moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); - EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); EXPECT_FALSE(res->response.planned_trajectories.empty()) << "Planned trajectory is empty"; } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.cpp index bf9198d52a..fcff8cda1d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.cpp @@ -86,13 +86,13 @@ class IntegrationTestSequenceAction : public testing::Test, public testing::Asyn public: MOCK_METHOD0(active_callback, void()); - MOCK_METHOD1(feedback_callback, void(const moveit_msgs::MoveGroupSequenceFeedbackConstPtr& feedback)); + MOCK_METHOD1(feedback_callback, void(const moveit_msgs::msg::MoveGroupSequenceFeedbackConstPtr& feedback)); MOCK_METHOD2(done_callback, void(const actionlib::SimpleClientGoalState& state, - const moveit_msgs::MoveGroupSequenceResultConstPtr& result)); + const moveit_msgs::msg::MoveGroupSequenceResultConstPtr& result)); protected: ros::NodeHandle ph_{ "~" }; - actionlib::SimpleActionClient ac_{ ph_, SEQUENCE_ACTION_NAME, true }; + actionlib::SimpleActionClient ac_{ ph_, SEQUENCE_ACTION_NAME, true }; std::shared_ptr move_group_; robot_model_loader::RobotModelLoader model_loader_; @@ -152,7 +152,7 @@ TEST_F(IntegrationTestSequenceAction, TestCancellingOfGoal) { Sequence seq{ data_loader_->getSequence("ComplexSequence") }; - moveit_msgs::MoveGroupSequenceGoal seq_goal; + moveit_msgs::msg::MoveGroupSequenceGoal seq_goal; seq_goal.request = seq.toRequest(); ac_.sendGoal(seq_goal); @@ -162,8 +162,8 @@ TEST_F(IntegrationTestSequenceAction, TestCancellingOfGoal) ac_.cancelGoal(); ac_.waitForResult(ros::Duration(WAIT_FOR_RESULT_TIME_OUT)); - moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); - EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::PREEMPTED) + moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::PREEMPTED) << "Error code should be preempted."; } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.cpp index 02f4bf5aa9..e4cba8d1e5 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.cpp @@ -110,14 +110,14 @@ void IntegrationTestSequenceService::SetUp() */ TEST_F(IntegrationTestSequenceService, TestSendingOfEmptySequence) { - moveit_msgs::MotionSequenceRequest empty_list; + moveit_msgs::msg::MotionSequenceRequest empty_list; moveit_msgs::GetMotionSequence srv; srv.request.request = empty_list; ASSERT_TRUE(client_.call(srv)); - EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val) << "Planning failed."; + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val) << "Planning failed."; EXPECT_TRUE(srv.response.response.planned_trajectories.empty()); } @@ -145,7 +145,7 @@ TEST_F(IntegrationTestSequenceService, TestDifferingGroupNames) ASSERT_TRUE(client_.call(srv)); - EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME, srv.response.response.error_code.val) + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME, srv.response.response.error_code.val) << "Planning should have failed but did not."; EXPECT_TRUE(srv.response.response.planned_trajectories.empty()); } @@ -171,7 +171,7 @@ TEST_F(IntegrationTestSequenceService, TestNegativeBlendRadius) ASSERT_TRUE(client_.call(srv)); - EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN, srv.response.response.error_code.val) + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN, srv.response.response.error_code.val) << "Planning should have failed but did not."; EXPECT_TRUE(srv.response.response.planned_trajectories.empty()); } @@ -197,7 +197,7 @@ TEST_F(IntegrationTestSequenceService, TestOverlappingBlendRadii) ASSERT_TRUE(client_.call(srv)); - EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN, srv.response.response.error_code.val) + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN, srv.response.response.error_code.val) << "Incorrect error code"; EXPECT_TRUE(srv.response.response.planned_trajectories.empty()); } @@ -224,7 +224,8 @@ TEST_F(IntegrationTestSequenceService, TestTooLargeBlendRadii) ASSERT_TRUE(client_.call(srv)); - EXPECT_EQ(moveit_msgs::MoveItErrorCodes::FAILURE, srv.response.response.error_code.val) << "Incorrect error code"; + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::FAILURE, srv.response.response.error_code.val) + << "Incorrect error code"; EXPECT_TRUE(srv.response.response.planned_trajectories.empty()); } @@ -244,7 +245,7 @@ TEST_F(IntegrationTestSequenceService, TestTooLargeBlendRadii) TEST_F(IntegrationTestSequenceService, TestSecondTrajInvalidStartState) { Sequence seq{ data_loader_->getSequence("ComplexSequence") }; - moveit_msgs::MotionSequenceRequest req_list{ seq.toRequest() }; + moveit_msgs::msg::MotionSequenceRequest req_list{ seq.toRequest() }; // Set start state using std::placeholders::_1; @@ -256,7 +257,7 @@ TEST_F(IntegrationTestSequenceService, TestSecondTrajInvalidStartState) ASSERT_TRUE(client_.call(srv)); - EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE, srv.response.response.error_code.val) + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE, srv.response.response.error_code.val) << "Incorrect error code."; EXPECT_TRUE(srv.response.response.planned_trajectories.empty()); } @@ -285,7 +286,7 @@ TEST_F(IntegrationTestSequenceService, TestFirstGoalNotReachable) ASSERT_TRUE(client_.call(srv)); - EXPECT_EQ(moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION, srv.response.response.error_code.val) + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION, srv.response.response.error_code.val) << "Incorrect error code."; EXPECT_TRUE(srv.response.response.planned_trajectories.empty()); } @@ -315,7 +316,8 @@ TEST_F(IntegrationTestSequenceService, TestInvalidLinkName) ASSERT_TRUE(client_.call(srv)); - EXPECT_NE(moveit_msgs::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val) << "Incorrect error code."; + EXPECT_NE(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val) + << "Incorrect error code."; EXPECT_TRUE(srv.response.response.planned_trajectories.empty()); } @@ -333,19 +335,19 @@ TEST_F(IntegrationTestSequenceService, TestInvalidLinkName) TEST_F(IntegrationTestSequenceService, TestLargeRequest) { Sequence seq{ data_loader_->getSequence("ComplexSequence") }; - moveit_msgs::MotionSequenceRequest req{ seq.toRequest() }; + moveit_msgs::msg::MotionSequenceRequest req{ seq.toRequest() }; // Make copy of sequence commands and add them to the end of sequence. // Create large request by making copies of the original sequence commands // and adding them to the end of the original sequence. size_t n{ req.items.size() }; for (size_t i = 0; i < n; ++i) { - moveit_msgs::MotionSequenceItem item{ req.items.at(i) }; + moveit_msgs::msg::MotionSequenceItem item{ req.items.at(i) }; if (i == 0) { // Remove start state because only the first request // is allowed to have a start state in a sequence. - item.req.start_state = moveit_msgs::RobotState(); + item.req.start_state = moveit_msgs::msg::RobotState(); } req.items.push_back(item); } @@ -355,7 +357,8 @@ TEST_F(IntegrationTestSequenceService, TestLargeRequest) ASSERT_TRUE(client_.call(srv)); - EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val) << "Incorrect error code."; + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val) + << "Incorrect error code."; EXPECT_EQ(srv.response.response.planned_trajectories.size(), 1u); EXPECT_GT(srv.response.response.planned_trajectories.front().joint_trajectory.points.size(), 0u) << "Trajectory should contain points."; @@ -384,7 +387,8 @@ TEST_F(IntegrationTestSequenceService, TestComplexSequenceWithoutBlending) ASSERT_TRUE(client_.call(srv)); - EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val) << "Incorrect error code."; + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val) + << "Incorrect error code."; EXPECT_EQ(srv.response.response.planned_trajectories.size(), 1u); EXPECT_GT(srv.response.response.planned_trajectories.front().joint_trajectory.points.size(), 0u) << "Trajectory should contain points."; @@ -411,7 +415,8 @@ TEST_F(IntegrationTestSequenceService, TestComplexSequenceWithBlending) ASSERT_TRUE(client_.call(srv)); - EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val) << "Incorrect error code."; + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val) + << "Incorrect error code."; EXPECT_EQ(srv.response.response.planned_trajectories.size(), 1u); EXPECT_GT(srv.response.response.planned_trajectories.front().joint_trajectory.points.size(), 0u) << "Trajectory should contain points."; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp index 0069c414f8..c358bb37e8 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp @@ -112,7 +112,7 @@ bool testutils::getExpectedGoalPose(const moveit::core::RobotModelConstPtr& robo } bool testutils::isGoalReached(const trajectory_msgs::JointTrajectory& trajectory, - const std::vector& goal, + const std::vector& goal, const double joint_position_tolerance, const double joint_velocity_tolerance) { trajectory_msgs::JointTrajectoryPoint last_point = trajectory.points.back(); @@ -358,7 +358,7 @@ bool testutils::isVelocityBounded(const trajectory_msgs::JointTrajectory& trajec std::cerr << "[ Fail ] Joint velocity limit violated in " << i << "th waypoint of joint: " << " Joint Name: " << trajectory.joint_names.at(i) << "; Position: " << point.positions.at(i) << "; Velocity: " << point.velocities.at(i) << "; Acceleration: " << point.accelerations.at(i) - << "; Time from start: " << point.time_from_start.toSec() + << "; Time from start: " << point.time_from_start.seconds() << "; Velocity Limit: " << joint_limits.getLimit(trajectory.joint_names.at(i)).max_velocity << std::endl; @@ -402,7 +402,7 @@ bool testutils::isAccelerationBounded(const trajectory_msgs::JointTrajectory& tr std::cerr << "[ Fail ] Deceleration limit violated of joint: " << trajectory.joint_names.at(i) << ": Position: " << point.positions.at(i) << "; Velocity: " << point.velocities.at(i) << "; Acceleration: " << point.accelerations.at(i) - << "; Time from start: " << point.time_from_start.toSec() + << "; Time from start: " << point.time_from_start.seconds() << ". Deceleration Limit: " << joint_limits.getLimit(trajectory.joint_names.at(i)).max_deceleration << std::endl; return false; @@ -416,7 +416,7 @@ bool testutils::isAccelerationBounded(const trajectory_msgs::JointTrajectory& tr std::cerr << "[ Fail ] Acceleration limit violated of joint: " << trajectory.joint_names.at(i) << ": Position: " << point.positions.at(i) << "; Velocity: " << point.velocities.at(i) << "; Acceleration: " << point.accelerations.at(i) - << "; Time from start: " << point.time_from_start.toSec() + << "; Time from start: " << point.time_from_start.seconds() << ". Acceleration Limit: " << joint_limits.getLimit(trajectory.joint_names.at(i)).max_acceleration << std::endl; @@ -442,7 +442,7 @@ bool testutils::isPositionBounded(const trajectory_msgs::JointTrajectory& trajec std::cerr << "[ Fail ] Joint position limit violated in " << i << "th waypoint of joint: " << " Joint Name: " << trajectory.joint_names.at(i) << "; Position: " << point.positions.at(i) << "; Velocity: " << point.velocities.at(i) << "; Acceleration: " << point.accelerations.at(i) - << "; Time from start: " << point.time_from_start.toSec() + << "; Time from start: " << point.time_from_start.seconds() << "; Max Position: " << joint_limits.getLimit(trajectory.joint_names.at(i)).max_position << "; Min Position: " << joint_limits.getLimit(trajectory.joint_names.at(i)).min_position << std::endl; @@ -670,7 +670,7 @@ bool testutils::checkBlendingJointSpaceContinuity(const pilz_industrial_motion_p double joint_velocity_tolerance, double joint_accleration_tolerance) { // convert to msgs - moveit_msgs::RobotTrajectory first_traj, blend_traj, second_traj; + moveit_msgs::msg::RobotTrajectory first_traj, blend_traj, second_traj; res.first_trajectory->getRobotTrajectoryMsg(first_traj); res.blend_trajectory->getRobotTrajectoryMsg(blend_traj); res.second_trajectory->getRobotTrajectoryMsg(second_traj); @@ -695,7 +695,7 @@ bool testutils::checkBlendingJointSpaceContinuity(const pilz_industrial_motion_p for (std::size_t i = 0; i < first_end.positions.size(); ++i) { double blend_start_velo = - (blend_start.positions.at(i) - first_end.positions.at(i)) / blend_start.time_from_start.toSec(); + (blend_start.positions.at(i) - first_end.positions.at(i)) / blend_start.time_from_start.seconds(); if (fabs(blend_start_velo - blend_start.velocities.at(i)) > joint_velocity_tolerance) { std::cout << "Velocity computed from positions are different from the " @@ -707,7 +707,7 @@ bool testutils::checkBlendingJointSpaceContinuity(const pilz_industrial_motion_p return false; } - double blend_start_acc = (blend_start_velo - first_end.velocities.at(i)) / blend_start.time_from_start.toSec(); + double blend_start_acc = (blend_start_velo - first_end.velocities.at(i)) / blend_start.time_from_start.seconds(); if (fabs(blend_start_acc - blend_start.accelerations.at(i)) > joint_velocity_tolerance) { std::cout << "Acceleration computed from positions/velocities are " @@ -742,7 +742,7 @@ bool testutils::checkBlendingJointSpaceContinuity(const pilz_industrial_motion_p for (std::size_t i = 0; i < blend_end.positions.size(); ++i) { double second_start_velo = - (second_start.positions.at(i) - blend_end.positions.at(i)) / second_start.time_from_start.toSec(); + (second_start.positions.at(i) - blend_end.positions.at(i)) / second_start.time_from_start.seconds(); if (fabs(second_start_velo - second_start.velocities.at(i)) > joint_accleration_tolerance) { std::cout << "Velocity computed from positions are different from the " @@ -753,7 +753,7 @@ bool testutils::checkBlendingJointSpaceContinuity(const pilz_industrial_motion_p return false; } - double second_start_acc = (second_start_velo - blend_end.velocities.at(i)) / second_start.time_from_start.toSec(); + double second_start_acc = (second_start_velo - blend_end.velocities.at(i)) / second_start.time_from_start.seconds(); if (fabs(second_start_acc - second_start.accelerations.at(i)) > joint_accleration_tolerance) { std::cout << "Acceleration computed from positions/velocities are " @@ -983,7 +983,7 @@ void testutils::getOriChange(Eigen::Matrix3d& ori1, Eigen::Matrix3d& ori2) } void testutils::createFakeCartTraj(const robot_trajectory::RobotTrajectoryPtr& traj, const std::string& link_name, - moveit_msgs::RobotTrajectory& fake_traj) + moveit_msgs::msg::RobotTrajectory& fake_traj) { fake_traj.joint_trajectory.joint_names.push_back("x"); fake_traj.joint_trajectory.joint_names.push_back("y"); @@ -1105,7 +1105,7 @@ bool testutils::checkBlendResult(const pilz_industrial_motion_planner::Trajector // ++++++++++++++++++++++ // + Check trajectories + // ++++++++++++++++++++++ - moveit_msgs::RobotTrajectory traj_msg; + moveit_msgs::msg::RobotTrajectory traj_msg; blend_res.first_trajectory->getRobotTrajectoryMsg(traj_msg); if (!testutils::checkJointTrajectory(traj_msg.joint_trajectory, limits.getJointLimitContainer())) { @@ -1155,13 +1155,13 @@ bool testutils::checkBlendResult(const pilz_industrial_motion_planner::Trajector // ++++++++++++++++++++++++ // ros::NodeHandle nh; // ros::Publisher pub = - // nh.advertise("my_planned_path", 1); + // nh.advertise("my_planned_path", 1); // ros::Duration duration(1.0); // duration.sleep(); // // visualize the joint trajectory - // moveit_msgs::DisplayTrajectory displayTrajectory; - // moveit_msgs::RobotTrajectory res_first_traj_msg, res_blend_traj_msg, + // moveit_msgs::msg::DisplayTrajectory displayTrajectory; + // moveit_msgs::msg::RobotTrajectory res_first_traj_msg, res_blend_traj_msg, // res_second_traj_msg; // blend_res.first_trajectory->getRobotTrajectoryMsg(res_first_traj_msg); // blend_res.blend_trajectory->getRobotTrajectoryMsg(res_blend_traj_msg); @@ -1178,7 +1178,7 @@ bool testutils::checkBlendResult(const pilz_industrial_motion_planner::Trajector void testutils::generateRequestMsgFromBlendTestData(const moveit::core::RobotModelConstPtr& robot_model, const testutils::BlendTestData& data, const std::string& planner_id, const std::string& group_name, const std::string& link_name, - moveit_msgs::MotionSequenceRequest& req_list) + moveit_msgs::msg::MotionSequenceRequest& req_list) { // motion plan request of first trajectory planning_interface::MotionPlanRequest req_1; @@ -1226,7 +1226,7 @@ void testutils::generateRequestMsgFromBlendTestData(const moveit::core::RobotMod double blend_radius = 0.5 * std::min(dis_1, dis_2); - moveit_msgs::MotionSequenceItem blend_req_1, blend_req_2; + moveit_msgs::msg::MotionSequenceItem blend_req_1, blend_req_2; blend_req_1.req = req_1; blend_req_1.blend_radius = blend_radius; blend_req_2.req = req_2; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h index 982cb7850c..a61bf90a24 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h @@ -126,16 +126,17 @@ inline sensor_msgs::JointState generateJointState(const std::vector& pos return generateJointState(pos, std::vector(), joint_prefix); } -inline moveit_msgs::Constraints generateJointConstraint(const std::vector& pos_list, - const std::string& joint_prefix = testutils::JOINT_NAME_PREFIX) +inline moveit_msgs::msg::Constraints +generateJointConstraint(const std::vector& pos_list, + const std::string& joint_prefix = testutils::JOINT_NAME_PREFIX) { - moveit_msgs::Constraints gc; + moveit_msgs::msg::Constraints gc; auto pos_it = pos_list.begin(); for (size_t i = 0; i < pos_list.size(); ++i) { - moveit_msgs::JointConstraint jc; + moveit_msgs::msg::JointConstraint jc; jc.joint_name = testutils::getJointName(i + 1, joint_prefix); jc.position = *pos_it; gc.joint_constraints.push_back(jc); @@ -177,7 +178,7 @@ void createPTPRequest(const std::string& planning_group, const moveit::core::Rob * @return true if satisfied */ bool isGoalReached(const trajectory_msgs::JointTrajectory& trajectory, - const std::vector& goal, const double joint_position_tolerance, + const std::vector& goal, const double joint_position_tolerance, const double joint_velocity_tolerance = 1.0e-6); /** @@ -372,7 +373,7 @@ void getLinLinPosesWithoutOriChange(const std::string& frame_id, sensor_msgs::Jo void getOriChange(Eigen::Matrix3d& ori1, Eigen::Matrix3d& ori2); void createFakeCartTraj(const robot_trajectory::RobotTrajectoryPtr& traj, const std::string& link_name, - moveit_msgs::RobotTrajectory& fake_traj); + moveit_msgs::msg::RobotTrajectory& fake_traj); inline geometry_msgs::Quaternion fromEuler(double a, double b, double c) { @@ -440,7 +441,8 @@ bool generateTrajFromBlendTestData(const moveit::core::RobotModelConstPtr& robot void generateRequestMsgFromBlendTestData(const moveit::core::RobotModelConstPtr& robot_model, const BlendTestData& data, const std::string& planner_id, const std::string& group_name, - const std::string& link_name, moveit_msgs::MotionSequenceRequest& req_list); + const std::string& link_name, + moveit_msgs::msg::MotionSequenceRequest& req_list); void checkRobotModel(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name, const std::string& link_name); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_get_solver_tip_frame.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_get_solver_tip_frame.cpp index b749d48ae3..5c8bce0b93 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_get_solver_tip_frame.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_get_solver_tip_frame.cpp @@ -90,12 +90,12 @@ TEST_F(GetSolverTipFrameTest, TestExceptionErrorCodeMapping) { { std::shared_ptr nse_ex{ new NoSolverException("") }; - EXPECT_EQ(nse_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + EXPECT_EQ(nse_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE); } { std::shared_ptr ex{ new MoreThanOneTipFrameException("") }; - EXPECT_EQ(ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + EXPECT_EQ(ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE); } } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.cpp index f413cb835a..0e2999cc0a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.cpp @@ -189,8 +189,8 @@ TEST_P(CommandPlannerTest, CheckEmptyPlannerIdForServiceRequest) */ TEST_P(CommandPlannerTest, CheckPlanningContextRequestNull) { - moveit_msgs::MotionPlanRequest req; - moveit_msgs::MoveItErrorCodes error_code; + moveit_msgs::msg::MotionPlanRequest req; + moveit_msgs::msg::MoveItErrorCodes error_code; EXPECT_EQ(nullptr, planner_instance_->getPlanningContext(nullptr, req, error_code)); } @@ -200,8 +200,8 @@ TEST_P(CommandPlannerTest, CheckPlanningContextRequestNull) */ TEST_P(CommandPlannerTest, CheckPlanningContextRequest) { - moveit_msgs::MotionPlanRequest req; - moveit_msgs::MoveItErrorCodes error_code; + moveit_msgs::msg::MotionPlanRequest req; + moveit_msgs::msg::MoveItErrorCodes error_code; // Check for the algorithms std::vector algs; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner_direct.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner_direct.cpp index 35bb528d27..8736a174be 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner_direct.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner_direct.cpp @@ -106,12 +106,12 @@ TEST(CommandPlannerTestDirect, FailOnLoadContext) pilz_industrial_motion_planner::PlanningContextLoaderPtr planning_context_loader(new TestPlanningContextLoader()); planner.registerContextLoader(planning_context_loader); - moveit_msgs::MotionPlanRequest req; + moveit_msgs::msg::MotionPlanRequest req; req.planner_id = "Test_Algorithm"; - moveit_msgs::MoveItErrorCodes error_code; + moveit_msgs::msg::MoveItErrorCodes error_code; EXPECT_FALSE(planner.getPlanningContext(nullptr, req, error_code)); - EXPECT_EQ(moveit_msgs::MoveItErrorCodes::PLANNING_FAILED, error_code.val); + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED, error_code.val); } int main(int argc, char** argv) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.cpp index 016c612e0a..a68f290489 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.cpp @@ -200,7 +200,7 @@ TYPED_TEST(PlanningContextTest, NoRequest) bool result = this->planning_context_->solve(res); EXPECT_FALSE(result) << testutils::demangel(typeid(TypeParam).name()); - EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN, res.error_code_.val) + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN, res.error_code_.val) << testutils::demangel(typeid(TypeParam).name()); } @@ -218,14 +218,14 @@ TYPED_TEST(PlanningContextTest, SolveValidRequest) bool result = this->planning_context_->solve(res); EXPECT_TRUE(result) << testutils::demangel(typeid(TypeParam).name()); - EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, res.error_code_.val) + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, res.error_code_.val) << testutils::demangel(typeid(TypeParam).name()); planning_interface::MotionPlanDetailedResponse res_detailed; bool result_detailed = this->planning_context_->solve(res_detailed); EXPECT_TRUE(result_detailed) << testutils::demangel(typeid(TypeParam).name()); - EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, res.error_code_.val) + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, res.error_code_.val) << testutils::demangel(typeid(TypeParam).name()); } @@ -241,7 +241,7 @@ TYPED_TEST(PlanningContextTest, SolveValidRequestDetailedResponse) bool result = this->planning_context_->solve(res); EXPECT_TRUE(result) << testutils::demangel(typeid(TypeParam).name()); - EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, res.error_code_.val) + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, res.error_code_.val) << testutils::demangel(typeid(TypeParam).name()); } @@ -261,7 +261,7 @@ TYPED_TEST(PlanningContextTest, SolveOnTerminated) bool result = this->planning_context_->solve(res); EXPECT_FALSE(result) << testutils::demangel(typeid(TypeParam).name()); - EXPECT_EQ(moveit_msgs::MoveItErrorCodes::PLANNING_FAILED, res.error_code_.val) + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED, res.error_code_.val) << testutils::demangel(typeid(TypeParam).name()); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.cpp index 48ba1e8b9e..9cb7f37199 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.cpp @@ -681,7 +681,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending) } } - moveit_msgs::MoveItErrorCodes error_code; + moveit_msgs::msg::MoveItErrorCodes error_code; if (!generateJointTrajectory(robot_model_, planner_limits_.getJointLimitContainer(), cart_traj, planning_group_, target_link_, initial_joint_position, initial_joint_velocity, joint_traj, error_code, true)) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp index 655c34bce6..954e2e47d9 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp @@ -237,7 +237,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIKSolver) std::vector> ik_solutions; kinematics::KinematicsResult ik_result; - moveit_msgs::MoveItErrorCodes err_code; + moveit_msgs::msg::MoveItErrorCodes err_code; kinematics::KinematicsQueryOptions options = kinematics::KinematicsQueryOptions(); // compute all ik solutions @@ -695,7 +695,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testGenerateJointTrajectoryWithI std::map initial_joint_position; double sampling_time{ 0.1 }; trajectory_msgs::JointTrajectory joint_trajectory; - moveit_msgs::MoveItErrorCodes error_code; + moveit_msgs::msg::MoveItErrorCodes error_code; bool check_self_collision{ false }; EXPECT_FALSE(pilz_industrial_motion_planner::generateJointTrajectory( diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator.cpp index c1a24e7285..a4d6d5fa68 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator.cpp @@ -49,93 +49,93 @@ TEST(TrajectoryGeneratorTest, TestExceptionErrorCodeMapping) { std::shared_ptr tgil_ex{ new TrajectoryGeneratorInvalidLimitsException( "") }; - EXPECT_EQ(tgil_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + EXPECT_EQ(tgil_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE); } { std::shared_ptr vsi_ex{ new VelocityScalingIncorrect("") }; - EXPECT_EQ(vsi_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(vsi_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } { std::shared_ptr asi_ex{ new AccelerationScalingIncorrect("") }; - EXPECT_EQ(asi_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(asi_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } { std::shared_ptr upg_ex{ new UnknownPlanningGroup("") }; - EXPECT_EQ(upg_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME); + EXPECT_EQ(upg_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME); } { std::shared_ptr njniss_ex{ new NoJointNamesInStartState("") }; - EXPECT_EQ(njniss_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + EXPECT_EQ(njniss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } { std::shared_ptr smiss_ex{ new SizeMismatchInStartState("") }; - EXPECT_EQ(smiss_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + EXPECT_EQ(smiss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } { std::shared_ptr jofssoor_ex{ new JointsOfStartStateOutOfRange("") }; - EXPECT_EQ(jofssoor_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + EXPECT_EQ(jofssoor_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } { std::shared_ptr nzviss_ex{ new NonZeroVelocityInStartState("") }; - EXPECT_EQ(nzviss_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + EXPECT_EQ(nzviss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } { std::shared_ptr neogcg_ex{ new NotExactlyOneGoalConstraintGiven("") }; - EXPECT_EQ(neogcg_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(neogcg_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } { std::shared_ptr oogta_ex{ new OnlyOneGoalTypeAllowed("") }; - EXPECT_EQ(oogta_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(oogta_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } { std::shared_ptr ssgsm_ex{ new StartStateGoalStateMismatch("") }; - EXPECT_EQ(ssgsm_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(ssgsm_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } { std::shared_ptr jcdnbtg_ex{ new JointConstraintDoesNotBelongToGroup("") }; - EXPECT_EQ(jcdnbtg_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(jcdnbtg_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } { std::shared_ptr jogoor_ex{ new JointsOfGoalOutOfRange("") }; - EXPECT_EQ(jogoor_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(jogoor_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } { std::shared_ptr pcnm_ex{ new PositionConstraintNameMissing("") }; - EXPECT_EQ(pcnm_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(pcnm_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } { std::shared_ptr ocnm_ex{ new OrientationConstraintNameMissing("") }; - EXPECT_EQ(ocnm_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(ocnm_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } { std::shared_ptr pocnm_ex{ new PositionOrientationConstraintNameMismatch( "") }; - EXPECT_EQ(pocnm_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(pocnm_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } { std::shared_ptr nisa_ex{ new NoIKSolverAvailable("") }; - EXPECT_EQ(nisa_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); + EXPECT_EQ(nisa_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); } { std::shared_ptr nppg_ex{ new NoPrimitivePoseGiven("") }; - EXPECT_EQ(nppg_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(nppg_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.cpp index 779d7c0c07..f382eecb1f 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.cpp @@ -147,7 +147,7 @@ void TrajectoryGeneratorCIRCTest::SetUp() void TrajectoryGeneratorCIRCTest::checkCircResult(const planning_interface::MotionPlanRequest& req, const planning_interface::MotionPlanResponse& res) { - moveit_msgs::MotionPlanResponse res_msg; + 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_)); @@ -219,57 +219,57 @@ TEST(TrajectoryGeneratorCIRCTest, TestExceptionErrorCodeMapping) { { std::shared_ptr cnp_ex{ new CircleNoPlane("") }; - EXPECT_EQ(cnp_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(cnp_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } { std::shared_ptr cts_ex{ new CircleToSmall("") }; - EXPECT_EQ(cts_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(cts_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } { std::shared_ptr cpdr_ex{ new CenterPointDifferentRadius("") }; - EXPECT_EQ(cpdr_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(cpdr_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } { std::shared_ptr ctcf_ex{ new CircTrajectoryConversionFailure("") }; - EXPECT_EQ(ctcf_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(ctcf_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } { std::shared_ptr upcn_ex{ new UnknownPathConstraintName("") }; - EXPECT_EQ(upcn_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(upcn_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } { std::shared_ptr npc_ex{ new NoPositionConstraints("") }; - EXPECT_EQ(npc_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(npc_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } { std::shared_ptr npp_ex{ new NoPrimitivePose("") }; - EXPECT_EQ(npp_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(npp_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } { std::shared_ptr ulnoap_ex{ new UnknownLinkNameOfAuxiliaryPoint("") }; - EXPECT_EQ(ulnoap_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME); + EXPECT_EQ(ulnoap_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_LINK_NAME); } { std::shared_ptr nocm_ex{ new NumberOfConstraintsMismatch("") }; - EXPECT_EQ(nocm_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(nocm_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } { std::shared_ptr cjmiss_ex{ new CircJointMissingInStartState("") }; - EXPECT_EQ(cjmiss_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + EXPECT_EQ(cjmiss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } { std::shared_ptr cifgi_ex{ new CircInverseForGoalIncalculable("") }; - EXPECT_EQ(cifgi_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); + EXPECT_EQ(cifgi_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); } } @@ -301,7 +301,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, incompleteStartState) planning_interface::MotionPlanResponse res; EXPECT_FALSE(circ_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } /** @@ -309,13 +309,13 @@ TEST_P(TrajectoryGeneratorCIRCTest, incompleteStartState) */ TEST_P(TrajectoryGeneratorCIRCTest, nonZeroStartVelocity) { - moveit_msgs::MotionPlanRequest req{ tdp_->getCircJointCenterCart("circ1_center_2").toRequest() }; + moveit_msgs::msg::MotionPlanRequest req{ tdp_->getCircJointCenterCart("circ1_center_2").toRequest() }; // start state has non-zero velocity req.start_state.joint_state.velocity.push_back(1.0); planning_interface::MotionPlanResponse res; EXPECT_FALSE(circ_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); req.start_state.joint_state.velocity.clear(); } @@ -325,7 +325,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, ValidCommand) planning_interface::MotionPlanResponse res; EXPECT_TRUE(circ_->generate(circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); } /** @@ -338,7 +338,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, velScaleToHigh) circ.setVelocityScale(1.0); planning_interface::MotionPlanResponse res; EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::PLANNING_FAILED); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED); } /** @@ -351,7 +351,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, accScaleToHigh) circ.setAccelerationScale(1.0); planning_interface::MotionPlanResponse res; EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::PLANNING_FAILED); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED); } /** @@ -373,7 +373,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, samePointsWithCenter) planning_interface::MotionPlanResponse res; EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** @@ -396,7 +396,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, samePointsWithInterim) planning_interface::MotionPlanResponse res; EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** @@ -412,7 +412,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, emptyAux) planning_interface::MotionPlanResponse res; EXPECT_FALSE(circ_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** @@ -428,7 +428,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, invalidAuxName) planning_interface::MotionPlanResponse res; EXPECT_FALSE(circ_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** @@ -445,7 +445,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, invalidAuxLinkName) planning_interface::MotionPlanResponse res; EXPECT_FALSE(circ_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_LINK_NAME); } /** @@ -459,7 +459,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, invalidCenter) planning_interface::MotionPlanResponse res; EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** @@ -479,7 +479,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, colinearCenter) planning_interface::MotionPlanResponse res; EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** @@ -500,7 +500,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, colinearInterim) planning_interface::MotionPlanResponse res; EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** @@ -518,7 +518,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, colinearCenterDueToInterim) planning_interface::MotionPlanResponse res; ASSERT_TRUE(circ_->generate(circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); } /** @@ -547,11 +547,11 @@ TEST_P(TrajectoryGeneratorCIRCTest, colinearCenterAndInterim) circ.setAccelerationScale(0.05); circ.setVelocityScale(0.05); - moveit_msgs::MotionPlanRequest req = circ.toRequest(); + moveit_msgs::msg::MotionPlanRequest req = circ.toRequest(); planning_interface::MotionPlanResponse res; EXPECT_TRUE(circ_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -581,11 +581,11 @@ TEST_P(TrajectoryGeneratorCIRCTest, interimLarger180Degree) circ.setAccelerationScale(0.05); circ.setVelocityScale(0.05); - moveit_msgs::MotionPlanRequest req = circ.toRequest(); + moveit_msgs::msg::MotionPlanRequest req = circ.toRequest(); planning_interface::MotionPlanResponse res; EXPECT_TRUE(circ_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -597,11 +597,11 @@ TEST_P(TrajectoryGeneratorCIRCTest, centerPointJointGoal) SKIP_IF_GRIPPER auto circ{ tdp_->getCircJointCenterCart("circ1_center_2") }; - moveit_msgs::MotionPlanRequest req = circ.toRequest(); + moveit_msgs::msg::MotionPlanRequest req = circ.toRequest(); planning_interface::MotionPlanResponse res; ASSERT_TRUE(circ_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -614,7 +614,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, InvalidAdditionalPrimitivePose) { auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; - moveit_msgs::MotionPlanRequest req = circ.toRequest(); + moveit_msgs::msg::MotionPlanRequest req = circ.toRequest(); // Contains one pose (interim / center) ASSERT_EQ(req.path_constraints.position_constraints.back().constraint_region.primitive_poses.size(), 1u); @@ -628,7 +628,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, InvalidAdditionalPrimitivePose) planning_interface::MotionPlanResponse res; ASSERT_FALSE(circ_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** @@ -641,16 +641,16 @@ TEST_P(TrajectoryGeneratorCIRCTest, InvalidExtraJointConstraint) { auto circ{ tdp_->getCircJointCenterCart("circ1_center_2") }; - moveit_msgs::MotionPlanRequest req = circ.toRequest(); + moveit_msgs::msg::MotionPlanRequest req = circ.toRequest(); // Define the additional joint constraint - moveit_msgs::JointConstraint joint_constraint; + moveit_msgs::msg::JointConstraint joint_constraint; joint_constraint.joint_name = req.goal_constraints.front().joint_constraints.front().joint_name; req.goal_constraints.front().joint_constraints.push_back(joint_constraint); //<-- Additional constraint planning_interface::MotionPlanResponse res; EXPECT_FALSE(circ_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } /** @@ -660,11 +660,11 @@ TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoal) { auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; - moveit_msgs::MotionPlanRequest req = circ.toRequest(); + moveit_msgs::msg::MotionPlanRequest req = circ.toRequest(); planning_interface::MotionPlanResponse res; ASSERT_TRUE(circ_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -675,13 +675,13 @@ TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdPositionConstraint { auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; - moveit_msgs::MotionPlanRequest req = circ.toRequest(); + moveit_msgs::msg::MotionPlanRequest req = circ.toRequest(); req.goal_constraints.front().position_constraints.front().header.frame_id = robot_model_->getModelFrame(); planning_interface::MotionPlanResponse res; ASSERT_TRUE(circ_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -692,12 +692,12 @@ TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdOrientationConstra { auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; - moveit_msgs::MotionPlanRequest req = circ.toRequest(); + moveit_msgs::msg::MotionPlanRequest req = circ.toRequest(); req.goal_constraints.front().orientation_constraints.front().header.frame_id = robot_model_->getModelFrame(); planning_interface::MotionPlanResponse res; ASSERT_TRUE(circ_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -708,7 +708,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdBothConstraints) { auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; - moveit_msgs::MotionPlanRequest req = circ.toRequest(); + moveit_msgs::msg::MotionPlanRequest req = circ.toRequest(); // Both set req.goal_constraints.front().position_constraints.front().header.frame_id = robot_model_->getModelFrame(); @@ -716,7 +716,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdBothConstraints) planning_interface::MotionPlanResponse res; ASSERT_TRUE(circ_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -729,11 +729,11 @@ TEST_P(TrajectoryGeneratorCIRCTest, InterimPointJointGoal) auto circ{ tdp_->getCircJointInterimCart("circ3_interim") }; - moveit_msgs::MotionPlanRequest req = circ.toRequest(); + moveit_msgs::msg::MotionPlanRequest req = circ.toRequest(); planning_interface::MotionPlanResponse res; ASSERT_TRUE(circ_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -749,14 +749,14 @@ TEST_P(TrajectoryGeneratorCIRCTest, InterimPointJointGoalStartVelNearZero) auto circ{ tdp_->getCircJointInterimCart("circ3_interim") }; - moveit_msgs::MotionPlanRequest req = circ.toRequest(); + moveit_msgs::msg::MotionPlanRequest req = circ.toRequest(); // Set velocity near zero req.start_state.joint_state.velocity = std::vector(req.start_state.joint_state.position.size(), 1e-16); planning_interface::MotionPlanResponse res; ASSERT_TRUE(circ_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -766,11 +766,11 @@ TEST_P(TrajectoryGeneratorCIRCTest, InterimPointJointGoalStartVelNearZero) TEST_P(TrajectoryGeneratorCIRCTest, InterimPointPoseGoal) { auto circ{ tdp_->getCircJointInterimCart("circ3_interim") }; - moveit_msgs::MotionPlanRequest req = circ.toRequest(); + moveit_msgs::msg::MotionPlanRequest req = circ.toRequest(); planning_interface::MotionPlanResponse res; ASSERT_TRUE(circ_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.cpp index aaba86c0f1..a002d89cd1 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.cpp @@ -132,8 +132,8 @@ class TrajectoryGeneratorCommonTest : public ::testing::Test rstate.setJointGroupPositions(planning_group_, { 0, M_PI / 2, 0, M_PI / 2, 0, 0 }); rstate.setVariableVelocities(std::vector(rstate.getVariableCount(), 0.0)); moveit::core::robotStateToRobotStateMsg(rstate, req_.start_state, false); - moveit_msgs::Constraints goal_constraint; - moveit_msgs::JointConstraint joint_constraint; + moveit_msgs::msg::Constraints goal_constraint; + moveit_msgs::msg::JointConstraint joint_constraint; joint_constraint.joint_name = this->robot_model_->getActiveJointModels().front()->getName(); joint_constraint.position = 0.5; goal_constraint.joint_constraints.push_back(joint_constraint); @@ -178,22 +178,22 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalideScalingFactor) { this->req_.max_velocity_scaling_factor = 2.0; EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); this->req_.max_velocity_scaling_factor = 1.0; this->req_.max_acceleration_scaling_factor = 0; EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); this->req_.max_velocity_scaling_factor = 0.00001; this->req_.max_acceleration_scaling_factor = 1; EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); this->req_.max_velocity_scaling_factor = 1; this->req_.max_acceleration_scaling_factor = -1; EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** @@ -203,7 +203,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalidGroupName) { this->req_.group_name = "foot"; EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); - EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME, this->res_.error_code_.val); + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME, this->res_.error_code_.val); } /** @@ -213,7 +213,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTestNoGripper, GripperGroup) { this->req_.group_name = "gripper"; EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); - EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME, this->res_.error_code_.val); + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME, this->res_.error_code_.val); } /** @@ -223,7 +223,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTestWithGripper, GripperGroup) { this->req_.group_name = "gripper"; EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); - EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS, this->res_.error_code_.val); + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS, this->res_.error_code_.val); } /** @@ -238,7 +238,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTestWithGripper, GripperGroup) //{ // EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); // EXPECT_EQ(this->res_.error_code_.val, -// moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME); +// moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME); //} /** @@ -248,7 +248,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyJointNamesInStartState) { this->req_.start_state.joint_state.name.clear(); EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } /** @@ -258,7 +258,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InconsistentStartState) { this->req_.start_state.joint_state.name.push_back("joint_7"); EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } /** @@ -268,7 +268,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, StartPostionOutOfLimit) { this->req_.start_state.joint_state.position[0] = 100; EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } /** @@ -282,7 +282,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, StartPositionVelocityNoneZero) { this->req_.start_state.joint_state.velocity[0] = 100; EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } /** @@ -292,7 +292,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyGoalConstraints) { this->req_.goal_constraints.clear(); EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } /** @@ -300,16 +300,16 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyGoalConstraints) */ TYPED_TEST(TrajectoryGeneratorCommonTest, MultipleGoals) { - moveit_msgs::JointConstraint joint_constraint; + moveit_msgs::msg::JointConstraint joint_constraint; moveit_msgs::PositionConstraint position_constraint; moveit_msgs::OrientationConstraint orientation_constraint; - moveit_msgs::Constraints goal_constraint; + moveit_msgs::msg::Constraints goal_constraint; // two goal constraints this->req_.goal_constraints.push_back(goal_constraint); this->req_.goal_constraints.push_back(goal_constraint); EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); // one joint constraint and one orientation constraint goal_constraint.joint_constraints.push_back(joint_constraint); @@ -317,7 +317,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, MultipleGoals) this->req_.goal_constraints.clear(); this->req_.goal_constraints.push_back(goal_constraint); EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); // one joint constraint and one Cartesian constraint goal_constraint.position_constraints.push_back(position_constraint); @@ -325,7 +325,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, MultipleGoals) this->req_.goal_constraints.clear(); this->req_.goal_constraints.push_back(goal_constraint); EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); // two Cartesian constraints goal_constraint.joint_constraints.clear(); @@ -336,7 +336,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, MultipleGoals) this->req_.goal_constraints.clear(); this->req_.goal_constraints.push_back(goal_constraint); EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } /** @@ -344,11 +344,11 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, MultipleGoals) */ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalideJointNameInGoal) { - moveit_msgs::JointConstraint joint_constraint; + moveit_msgs::msg::JointConstraint joint_constraint; joint_constraint.joint_name = "test_joint_2"; this->req_.goal_constraints.front().joint_constraints[0] = joint_constraint; EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } /** @@ -356,11 +356,11 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalideJointNameInGoal) */ TYPED_TEST(TrajectoryGeneratorCommonTest, MissingJointConstraint) { - moveit_msgs::JointConstraint joint_constraint; + moveit_msgs::msg::JointConstraint joint_constraint; joint_constraint.joint_name = "test_joint_2"; this->req_.goal_constraints.front().joint_constraints.pop_back(); //<-- Missing joint constraint EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } /** @@ -370,7 +370,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalideJointPositionInGoal) { this->req_.goal_constraints.front().joint_constraints[0].position = 100; EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } /** @@ -380,14 +380,14 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalidLinkNameInCartesianGoal) { moveit_msgs::PositionConstraint position_constraint; moveit_msgs::OrientationConstraint orientation_constraint; - moveit_msgs::Constraints goal_constraint; + moveit_msgs::msg::Constraints goal_constraint; // link name not set goal_constraint.position_constraints.push_back(position_constraint); goal_constraint.orientation_constraints.push_back(orientation_constraint); this->req_.goal_constraints.clear(); this->req_.goal_constraints.push_back(goal_constraint); EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); // different link names in position and orientation goals goal_constraint.position_constraints.front().link_name = "test_link_1"; @@ -395,14 +395,14 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalidLinkNameInCartesianGoal) this->req_.goal_constraints.clear(); this->req_.goal_constraints.push_back(goal_constraint); EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); // no solver for the link goal_constraint.orientation_constraints.front().link_name = "test_link_1"; this->req_.goal_constraints.clear(); this->req_.goal_constraints.push_back(goal_constraint); EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); } /** @@ -412,7 +412,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyPrimitivePoses) { moveit_msgs::PositionConstraint position_constraint; moveit_msgs::OrientationConstraint orientation_constraint; - moveit_msgs::Constraints goal_constraint; + moveit_msgs::msg::Constraints goal_constraint; position_constraint.link_name = this->robot_model_->getJointModelGroup(this->planning_group_)->getLinkModelNames().back(); orientation_constraint.link_name = position_constraint.link_name; @@ -422,7 +422,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyPrimitivePoses) this->req_.goal_constraints.clear(); this->req_.goal_constraints.push_back(goal_constraint); EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); - EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } int main(int argc, char** argv) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.cpp index 8d5ff8e6e2..076c65cac6 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.cpp @@ -148,7 +148,7 @@ void TrajectoryGeneratorLINTest::SetUp() bool TrajectoryGeneratorLINTest::checkLinResponse(const planning_interface::MotionPlanRequest& req, const planning_interface::MotionPlanResponse& res) { - moveit_msgs::MotionPlanResponse res_msg; + moveit_msgs::msg::MotionPlanResponse res_msg; res.getMessage(res_msg); if (!testutils::isGoalReached(robot_model_, res_msg.trajectory.joint_trajectory, req, pose_norm_tolerance_)) { @@ -177,22 +177,22 @@ TEST(TrajectoryGeneratorLINTest, TestExceptionErrorCodeMapping) { { std::shared_ptr ltcf_ex{ new LinTrajectoryConversionFailure("") }; - EXPECT_EQ(ltcf_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + EXPECT_EQ(ltcf_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE); } { std::shared_ptr jnm_ex{ new JointNumberMismatch("") }; - EXPECT_EQ(jnm_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(jnm_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } { std::shared_ptr ljmiss_ex{ new LinJointMissingInStartState("") }; - EXPECT_EQ(ljmiss_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + EXPECT_EQ(ljmiss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } { std::shared_ptr lifgi_ex{ new LinInverseForGoalIncalculable("") }; - EXPECT_EQ(lifgi_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); + EXPECT_EQ(lifgi_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); } } @@ -214,7 +214,7 @@ TEST_P(TrajectoryGeneratorLINTest, nonZeroStartVelocity) // try to generate the result planning_interface::MotionPlanResponse res; EXPECT_FALSE(lin_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } /** @@ -227,7 +227,7 @@ TEST_P(TrajectoryGeneratorLINTest, jointSpaceGoal) // generate the LIN trajectory planning_interface::MotionPlanResponse res; ASSERT_TRUE(lin_->generate(lin_joint_req, res)); - EXPECT_TRUE(res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_TRUE(res.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); // check the resulted trajectory EXPECT_TRUE(checkLinResponse(lin_joint_req, res)); @@ -248,7 +248,7 @@ TEST_P(TrajectoryGeneratorLINTest, jointSpaceGoalNearZeroStartVelocity) // generate the LIN trajectory planning_interface::MotionPlanResponse res; ASSERT_TRUE(lin_->generate(lin_joint_req, res)); - EXPECT_TRUE(res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_TRUE(res.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); // check the resulted trajectory EXPECT_TRUE(checkLinResponse(lin_joint_req, res)); @@ -260,12 +260,12 @@ TEST_P(TrajectoryGeneratorLINTest, jointSpaceGoalNearZeroStartVelocity) TEST_P(TrajectoryGeneratorLINTest, cartesianSpaceGoal) { // construct motion plan request - moveit_msgs::MotionPlanRequest lin_cart_req{ tdp_->getLinCart("lin2").toRequest() }; + moveit_msgs::msg::MotionPlanRequest lin_cart_req{ tdp_->getLinCart("lin2").toRequest() }; // generate lin trajectory planning_interface::MotionPlanResponse res; ASSERT_TRUE(lin_->generate(lin_cart_req, res)); - EXPECT_TRUE(res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_TRUE(res.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); // check the resulted trajectory EXPECT_TRUE(checkLinResponse(lin_cart_req, res)); @@ -282,14 +282,14 @@ TEST_P(TrajectoryGeneratorLINTest, cartesianSpaceGoal) TEST_P(TrajectoryGeneratorLINTest, cartesianTrapezoidProfile) { // construct motion plan request - moveit_msgs::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() }; + moveit_msgs::msg::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() }; /// +++++++++++++++++++++++ /// + plan LIN trajectory + /// +++++++++++++++++++++++ planning_interface::MotionPlanResponse res; ASSERT_TRUE(lin_->generate(lin_joint_req, res, 0.01)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); ASSERT_TRUE(testutils::checkCartesianTranslationalPath(res.trajectory_, target_link_hcd_)); @@ -361,7 +361,7 @@ TEST_P(TrajectoryGeneratorLINTest, LinPlannerDiscontinuousJointTraj) TEST_P(TrajectoryGeneratorLINTest, LinStartEqualsGoal) { // construct motion plan request - moveit_msgs::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() }; + moveit_msgs::msg::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() }; moveit::core::RobotState start_state(robot_model_); jointStateToRobotState(lin_joint_req.start_state.joint_state, start_state); @@ -374,7 +374,7 @@ TEST_P(TrajectoryGeneratorLINTest, LinStartEqualsGoal) // generate the LIN trajectory planning_interface::MotionPlanResponse res; ASSERT_TRUE(lin_->generate(lin_joint_req, res)); - EXPECT_TRUE(res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_TRUE(res.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); // check the resulted trajectory EXPECT_TRUE(checkLinResponse(lin_joint_req, res)); @@ -410,7 +410,7 @@ TEST_P(TrajectoryGeneratorLINTest, CtorNoLimits) TEST_P(TrajectoryGeneratorLINTest, IncorrectJointNumber) { // construct motion plan request - moveit_msgs::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() }; + moveit_msgs::msg::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() }; // Ensure that request consists of an incorrect number of joints. lin_joint_req.goal_constraints.front().joint_constraints.pop_back(); @@ -418,7 +418,7 @@ TEST_P(TrajectoryGeneratorLINTest, IncorrectJointNumber) // generate the LIN trajectory planning_interface::MotionPlanResponse res; ASSERT_FALSE(lin_->generate(lin_joint_req, res)); - EXPECT_TRUE(res.error_code_.val == moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_TRUE(res.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } /** @@ -428,7 +428,7 @@ TEST_P(TrajectoryGeneratorLINTest, IncorrectJointNumber) TEST_P(TrajectoryGeneratorLINTest, cartGoalIncompleteStartState) { // construct motion plan request - moveit_msgs::MotionPlanRequest lin_cart_req{ tdp_->getLinCart("lin2").toRequest() }; + moveit_msgs::msg::MotionPlanRequest lin_cart_req{ tdp_->getLinCart("lin2").toRequest() }; EXPECT_GT(lin_cart_req.start_state.joint_state.name.size(), 1u); lin_cart_req.start_state.joint_state.name.resize(1); lin_cart_req.start_state.joint_state.position.resize(1); // prevent failing check for equal sizes @@ -436,7 +436,7 @@ TEST_P(TrajectoryGeneratorLINTest, cartGoalIncompleteStartState) // generate lin trajectory planning_interface::MotionPlanResponse res; EXPECT_FALSE(lin_->generate(lin_cart_req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } /** @@ -446,7 +446,7 @@ TEST_P(TrajectoryGeneratorLINTest, cartGoalIncompleteStartState) TEST_P(TrajectoryGeneratorLINTest, cartGoalFrameIdBothConstraints) { // construct motion plan request - moveit_msgs::MotionPlanRequest lin_cart_req{ tdp_->getLinCart("lin2").toRequest() }; + moveit_msgs::msg::MotionPlanRequest lin_cart_req{ tdp_->getLinCart("lin2").toRequest() }; lin_cart_req.goal_constraints.front().position_constraints.front().header.frame_id = robot_model_->getModelFrame(); lin_cart_req.goal_constraints.front().orientation_constraints.front().header.frame_id = robot_model_->getModelFrame(); @@ -454,7 +454,7 @@ TEST_P(TrajectoryGeneratorLINTest, cartGoalFrameIdBothConstraints) // generate lin trajectory planning_interface::MotionPlanResponse res; ASSERT_TRUE(lin_->generate(lin_cart_req, res)); - EXPECT_TRUE(res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_TRUE(res.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); // check the resulted trajectory EXPECT_TRUE(checkLinResponse(lin_cart_req, res)); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.cpp index b2d24dca01..3ddc411c19 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.cpp @@ -150,12 +150,12 @@ TEST(TrajectoryGeneratorPTPTest, TestExceptionErrorCodeMapping) { { std::shared_ptr pvpsf_ex{ new PtpVelocityProfileSyncFailed("") }; - EXPECT_EQ(pvpsf_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + EXPECT_EQ(pvpsf_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE); } { std::shared_ptr pnisfgp_ex{ new PtpNoIkSolutionForGoalPose("") }; - EXPECT_EQ(pnisfgp_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); + EXPECT_EQ(pnisfgp_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); } } @@ -352,7 +352,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testCartesianGoal) pose.pose.orientation.z = 0.0; std::vector tolerance_pose(3, 0.01); std::vector tolerance_angle(3, 0.01); - moveit_msgs::Constraints pose_goal = + moveit_msgs::msg::Constraints pose_goal = kinematic_constraints::constructGoalConstraints(target_link_, pose, tolerance_pose, tolerance_angle); req.goal_constraints.push_back(pose_goal); @@ -360,9 +360,9 @@ TEST_P(TrajectoryGeneratorPTPTest, testCartesianGoal) //*** test robot model without gripper *** //**************************************** ASSERT_TRUE(ptp_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); - moveit_msgs::MotionPlanResponse res_msg; + moveit_msgs::msg::MotionPlanResponse res_msg; res.getMessage(res_msg); if (!res_msg.trajectory.joint_trajectory.points.empty()) { @@ -401,19 +401,19 @@ TEST_P(TrajectoryGeneratorPTPTest, testCartesianGoalMissingLinkNameConstraints) pose.pose.orientation.z = 0.0; std::vector tolerance_pose(3, 0.01); std::vector tolerance_angle(3, 0.01); - moveit_msgs::Constraints pose_goal = + moveit_msgs::msg::Constraints pose_goal = kinematic_constraints::constructGoalConstraints(target_link_, pose, tolerance_pose, tolerance_angle); req.goal_constraints.push_back(pose_goal); planning_interface::MotionPlanRequest req_no_position_constaint_link_name = req; req_no_position_constaint_link_name.goal_constraints.front().position_constraints.front().link_name = ""; ASSERT_FALSE(ptp_->generate(req_no_position_constaint_link_name, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); planning_interface::MotionPlanRequest req_no_orientation_constaint_link_name = req; req_no_orientation_constaint_link_name.goal_constraints.front().orientation_constraints.front().link_name = ""; ASSERT_FALSE(ptp_->generate(req_no_orientation_constaint_link_name, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } /** @@ -435,12 +435,12 @@ TEST_P(TrajectoryGeneratorPTPTest, testInvalidCartesianGoal) pose.pose.orientation.z = 0.0; std::vector tolerance_pose(3, 0.01); std::vector tolerance_angle(3, 0.01); - moveit_msgs::Constraints pose_goal = + moveit_msgs::msg::Constraints pose_goal = kinematic_constraints::constructGoalConstraints(target_link_, pose, tolerance_pose, tolerance_angle); req.goal_constraints.push_back(pose_goal); ASSERT_FALSE(ptp_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); EXPECT_EQ(res.trajectory_, nullptr); } @@ -457,8 +457,8 @@ TEST_P(TrajectoryGeneratorPTPTest, testJointGoalAlreadyReached) ASSERT_TRUE(robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames().size()) << "No link exists in the planning group."; - moveit_msgs::Constraints gc; - moveit_msgs::JointConstraint jc; + moveit_msgs::msg::Constraints gc; + moveit_msgs::msg::JointConstraint jc; jc.joint_name = robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames().front(); jc.position = 0.0; gc.joint_constraints.push_back(jc); @@ -466,9 +466,9 @@ TEST_P(TrajectoryGeneratorPTPTest, testJointGoalAlreadyReached) // TODO lin and circ has different settings ASSERT_TRUE(ptp_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); - moveit_msgs::MotionPlanResponse res_msg; + moveit_msgs::msg::MotionPlanResponse res_msg; res.getMessage(res_msg); EXPECT_EQ(1u, res_msg.trajectory.joint_trajectory.points.size()); } @@ -522,8 +522,8 @@ TEST_P(TrajectoryGeneratorPTPTest, testScalingFactor) planning_interface::MotionPlanRequest req; testutils::createDummyRequest(robot_model_, planning_group_, req); req.start_state.joint_state.position[2] = 0.1; - moveit_msgs::Constraints gc; - moveit_msgs::JointConstraint jc; + moveit_msgs::msg::Constraints gc; + moveit_msgs::msg::JointConstraint jc; jc.joint_name = "prbt_joint_1"; jc.position = 1.5; gc.joint_constraints.push_back(jc); @@ -538,9 +538,9 @@ TEST_P(TrajectoryGeneratorPTPTest, testScalingFactor) req.max_acceleration_scaling_factor = 1.0 / 3.0; ASSERT_TRUE(ptp_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); - moveit_msgs::MotionPlanResponse res_msg; + moveit_msgs::msg::MotionPlanResponse res_msg; res.getMessage(res_msg); EXPECT_TRUE(checkTrajectory(res_msg.trajectory.joint_trajectory, req, planner_limits_.getJointLimitContainer())); @@ -651,8 +651,8 @@ TEST_P(TrajectoryGeneratorPTPTest, testJointGoalAndAlmostZeroStartVelocity) // Set velocity to all 1e-16 req.start_state.joint_state.velocity = std::vector(req.start_state.joint_state.position.size(), 1e-16); - moveit_msgs::Constraints gc; - moveit_msgs::JointConstraint jc; + moveit_msgs::msg::Constraints gc; + moveit_msgs::msg::JointConstraint jc; jc.joint_name = "prbt_joint_1"; jc.position = 1.5; gc.joint_constraints.push_back(jc); @@ -665,9 +665,9 @@ TEST_P(TrajectoryGeneratorPTPTest, testJointGoalAndAlmostZeroStartVelocity) req.goal_constraints.push_back(gc); ASSERT_TRUE(ptp_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); - moveit_msgs::MotionPlanResponse res_msg; + moveit_msgs::msg::MotionPlanResponse res_msg; res.getMessage(res_msg); EXPECT_TRUE(checkTrajectory(res_msg.trajectory.joint_trajectory, req, planner_limits_.getJointLimitContainer())); @@ -786,8 +786,8 @@ TEST_P(TrajectoryGeneratorPTPTest, testJointGoalNoStartVel) req.start_state.joint_state.position[4] = 0.3; req.start_state.joint_state.position[2] = 0.11; - moveit_msgs::Constraints gc; - moveit_msgs::JointConstraint jc; + moveit_msgs::msg::Constraints gc; + moveit_msgs::msg::JointConstraint jc; jc.joint_name = "prbt_joint_1"; jc.position = 1.5; @@ -807,9 +807,9 @@ TEST_P(TrajectoryGeneratorPTPTest, testJointGoalNoStartVel) req.goal_constraints.push_back(gc); ASSERT_TRUE(ptp_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); - moveit_msgs::MotionPlanResponse res_msg; + moveit_msgs::msg::MotionPlanResponse res_msg; res.getMessage(res_msg); EXPECT_TRUE(checkTrajectory(res_msg.trajectory.joint_trajectory, req, planner_limits_.getJointLimitContainer()));