diff --git a/.clang-tidy b/.clang-tidy index 3a103664e1..6fc43be6f7 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -9,6 +9,8 @@ Checks: '-*, modernize-use-default, modernize-use-override, modernize-loop-convert, + modernize-make-shared, + modernize-make-unique, readability-named-parameter, readability-redundant-smartptr-get, readability-redundant-string-cstr, diff --git a/.github/mergify.yml b/.github/mergify.yml new file mode 100644 index 0000000000..68006e2312 --- /dev/null +++ b/.github/mergify.yml @@ -0,0 +1,26 @@ +pull_request_rules: + - name: backport to foxy at reviewers discretion + conditions: + - base=main + - "label=backport-foxy" + actions: + backport: + branches: + - foxy + + - name: ask to resolve conflict + conditions: + - conflict + - author!=mergify + actions: + comment: + message: This pull request is in conflict. Could you fix it @{{author}}? + + - name: development targets main branch + conditions: + - base!=main + - author!=mergify + actions: + comment: + message: | + Please target the `main` branch for development, we will backport the changes to {{base}} for you if approved and if they don't break API. diff --git a/.github/workflows/backport.yaml b/.github/workflows/backport.yaml deleted file mode 100644 index 5df5a2e596..0000000000 --- a/.github/workflows/backport.yaml +++ /dev/null @@ -1,21 +0,0 @@ -# Backporting Github Actions that ports a given PR to specified branch (via backport branch_name label) if the following conditions are met: -# 1) The PR is merged -# 2) The PR has a label of "backport branch_name" and branch_name is an existing branch. -# The label can be specified before or after the original PR is merged. Once these two conditions are met the bot will open a backport PR. -name: Backport -on: - pull_request: - types: - - closed - - labeled - -jobs: - backport: - if: github.event.pull_request.merged == true - runs-on: ubuntu-latest - name: Backport - steps: - - name: Backport - uses: tibdex/backport@v1 - with: - github_token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index f5a57cfd66..611c8b8847 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -27,6 +27,8 @@ jobs: CXXFLAGS: "-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-deprecated-copy" DOCKER_IMAGE: ghcr.io/ros-planning/moveit2:${{ matrix.env.IMAGE }} UPSTREAM_WORKSPACE: moveit2.repos $(f="moveit2_$(sed 's/-.*$//' <<< "${{ matrix.env.IMAGE }}").repos"; test -r $f && echo $f) + # TODO(#885): Fix circular test dependency with moveit_resources and remove it from the target workspace + TARGET_WORKSPACE: $TARGET_REPO_PATH github:ros-planning/moveit_resources#ros2 # Pull any updates to the upstream workspace (after restoring it from cache) AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src AFTER_SETUP_DOWNSTREAM_WORKSPACE: vcs pull $BASEDIR/downstream_ws/src diff --git a/MIGRATION.md b/MIGRATION.md index c3d287c424..68b8955d16 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -4,6 +4,13 @@ API changes in MoveIt releases ## ROS Rolling - ServoServer was renamed to ServoNode +- `CollisionObject` messages are now defined with a `Pose`, and shapes and subframes are defined relative to the object's pose. This makes it easier to place objects with subframes and multiple shapes in the scene. This causes several changes: + - `getFrameTransform()` now returns this pose instead of the first shape's pose. + - The Rviz plugin's manipulation tab now uses the object's pose instead of the shape pose to evaluate if object's are in the region of interest. + - Planning scene geometry text files (`.scene`) have changed format. Add a line `0 0 0 0 0 0 1` under each line with an asterisk to upgrade old files if required. +- add API for passing RNG to setToRandomPositionsNearBy +- Static member variable interface of the CollisionDetectorAllocatorTemplate for the string NAME was replaced with a virtual method `getName`. +- Enhance `RDFLoader` to load from string parameter OR string topic (and add the ability to publish a string topic). ## ROS Noetic - RobotModel no longer overrides empty URDF collision geometry by matching the visual geometry of the link. @@ -24,6 +31,7 @@ API changes in MoveIt releases - The joint states of `passive` joints must be published in ROS and the CurrentStateMonitor will now wait for them as well. Their semantics dictate that they cannot be actively controlled, but they must be known to use the full robot state in collision checks. (https://github.com/ros-planning/moveit/pull/2663) - Removed deprecated header `moveit/macros/deprecation.h`. Use `[[deprecated]]` instead. - All uses of `MOVEIT_CLASS_FORWARD` et. al. must now be followed by a semicolon for consistency (and to get -pedantic builds to pass for the codebase). +- In case you start RViz in a namespace, the default topic for the trajectory visualization display now uses the relative instead of the absolute namespace (i.e. `/move_group/display_planned_path` instead of `/move_group/display_planned_path`). ## ROS Melodic diff --git a/moveit/package.xml b/moveit/package.xml index b22e0ce179..815b508171 100644 --- a/moveit/package.xml +++ b/moveit/package.xml @@ -1,14 +1,13 @@ + moveit 2.3.0 Meta package that contains all essential packages of MoveIt 2 - Dave Coleman - Michael Ferguson + Henning Kayser + Tyler Weaver Michael Görner Robert Haschke - Ian McMahon - Isaac I. Y. Saito BSD @@ -18,6 +17,7 @@ Ioan Sucan Sachin Chitta + Dave Coleman ament_cmake moveit_planners_ompl - ament_lint_auto ament_lint_common diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h index d6c74634b7..5dfafa7f2c 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h @@ -72,6 +72,12 @@ class StateValidityChecker : public ompl::base::StateValidityChecker return isValid(state, dist, verbose_); } + bool isValid(const ompl::base::State* state, double& dist, ompl::base::State* /*validState*/, + bool& /*validStateAvailable*/) const override + { + return isValid(state, dist, verbose_); + } + virtual bool isValid(const ompl::base::State* state, bool verbose) const; virtual bool isValid(const ompl::base::State* state, double& dist, bool verbose) const; diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index c11117a73b..f850e91c3b 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -326,27 +326,31 @@ void ompl_interface::ModelBasedPlanningContext::useConfig() if (optimizer == "PathLengthOptimizationObjective") { - objective.reset(new ompl::base::PathLengthOptimizationObjective(ompl_simple_setup_->getSpaceInformation())); + objective = + std::make_shared(ompl_simple_setup_->getSpaceInformation()); } else if (optimizer == "MinimaxObjective") { - objective.reset(new ompl::base::MinimaxObjective(ompl_simple_setup_->getSpaceInformation())); + objective = std::make_shared(ompl_simple_setup_->getSpaceInformation()); } else if (optimizer == "StateCostIntegralObjective") { - objective.reset(new ompl::base::StateCostIntegralObjective(ompl_simple_setup_->getSpaceInformation())); + objective = std::make_shared(ompl_simple_setup_->getSpaceInformation()); } else if (optimizer == "MechanicalWorkOptimizationObjective") { - objective.reset(new ompl::base::MechanicalWorkOptimizationObjective(ompl_simple_setup_->getSpaceInformation())); + objective = + std::make_shared(ompl_simple_setup_->getSpaceInformation()); } else if (optimizer == "MaximizeMinClearanceObjective") { - objective.reset(new ompl::base::MaximizeMinClearanceObjective(ompl_simple_setup_->getSpaceInformation())); + objective = + std::make_shared(ompl_simple_setup_->getSpaceInformation()); } else { - objective.reset(new ompl::base::PathLengthOptimizationObjective(ompl_simple_setup_->getSpaceInformation())); + objective = + std::make_shared(ompl_simple_setup_->getSpaceInformation()); } ompl_simple_setup_->setOptimizationObjective(objective); @@ -630,7 +634,7 @@ bool ompl_interface::ModelBasedPlanningContext::setPathConstraints(const moveit_ moveit_msgs::msg::MoveItErrorCodes* /*error*/) { // ******************* set the path constraints to use - path_constraints_.reset(new kinematic_constraints::KinematicConstraintSet(getRobotModel())); + path_constraints_ = std::make_shared(getRobotModel()); path_constraints_->add(path_constraints, getPlanningScene()->getTransforms()); path_constraints_msg_ = path_constraints; @@ -762,7 +766,7 @@ bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::Motion RCLCPP_DEBUG(LOGGER, "%s: Returning successful solution with %lu states", getName().c_str(), getOMPLSimpleSetup()->getSolutionPath().getStateCount()); - res.trajectory_.reset(new robot_trajectory::RobotTrajectory(getRobotModel(), getGroupName())); + res.trajectory_ = std::make_shared(getRobotModel(), getGroupName()); getSolutionPath(*res.trajectory_); res.planning_time_ = ptime; return true; @@ -786,7 +790,7 @@ bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::Motion res.processing_time_.push_back(ptime); res.description_.emplace_back("plan"); res.trajectory_.resize(res.trajectory_.size() + 1); - res.trajectory_.back().reset(new robot_trajectory::RobotTrajectory(getRobotModel(), getGroupName())); + res.trajectory_.back() = std::make_shared(getRobotModel(), getGroupName()); getSolutionPath(*res.trajectory_.back()); // simplify solution if time remains @@ -796,7 +800,7 @@ bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::Motion res.processing_time_.push_back(getLastSimplifyTime()); res.description_.emplace_back("simplify"); res.trajectory_.resize(res.trajectory_.size() + 1); - res.trajectory_.back().reset(new robot_trajectory::RobotTrajectory(getRobotModel(), getGroupName())); + res.trajectory_.back() = std::make_shared(getRobotModel(), getGroupName()); getSolutionPath(*res.trajectory_.back()); } @@ -807,7 +811,7 @@ bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::Motion res.processing_time_.push_back(ompl::time::seconds(ompl::time::now() - start_interpolate)); res.description_.emplace_back("interpolate"); res.trajectory_.resize(res.trajectory_.size() + 1); - res.trajectory_.back().reset(new robot_trajectory::RobotTrajectory(getRobotModel(), getGroupName())); + res.trajectory_.back() = std::make_shared(getRobotModel(), getGroupName()); getSolutionPath(*res.trajectory_.back()); } diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp index d683e5b95b..f96eb40320 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp @@ -126,8 +126,9 @@ void OMPLInterface::configureContext(const ModelBasedPlanningContextPtr& context void OMPLInterface::loadConstraintSamplers() { - constraint_sampler_manager_loader_.reset( - new constraint_sampler_manager_loader::ConstraintSamplerManagerLoader(node_, constraint_sampler_manager_)); + constraint_sampler_manager_loader_ = + std::make_shared(node_, + constraint_sampler_manager_); } bool OMPLInterface::loadPlannerConfiguration(const std::string& group_name, const std::string& planner_id, diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp index 19a977e8f6..00b45a240c 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp @@ -78,14 +78,14 @@ class OMPLPlannerManager : public planning_interface::PlannerManager } }; - output_handler_.reset(new OutputHandler()); + output_handler_ = std::make_shared(); ompl::msg::useOutputHandler(output_handler_.get()); } bool initialize(const moveit::core::RobotModelConstPtr& model, const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override { - ompl_interface_.reset(new OMPLInterface(model, node, parameter_namespace)); + ompl_interface_ = std::make_unique(model, node, parameter_namespace); config_settings_ = ompl_interface_->getPlannerConfigurations(); return true; } diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp index 74593e5908..10718eb45a 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp @@ -185,7 +185,7 @@ ompl_interface::PoseModelStateSpace::PoseComponent::PoseComponent( const moveit::core::JointModelGroup* subgroup, const moveit::core::JointModelGroup::KinematicsSolver& k) : subgroup_(subgroup), kinematics_solver_(k.allocator_(subgroup)), bijection_(k.bijection_) { - state_space_.reset(new ompl::base::SE3StateSpace()); + state_space_ = std::make_shared(); state_space_->setName(subgroup_->getName() + "_Workspace"); fk_link_.resize(1, kinematics_solver_->getTipFrame()); if (!fk_link_[0].empty() && fk_link_[0][0] == '/') diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index a6a2fa5303..f5ed95837f 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -176,7 +176,7 @@ MultiQueryPlannerAllocator::allocatePlannerImpl(const ob::SpaceInformationPtr& s RCLCPP_INFO(LOGGER, "Loading planner data"); ob::PlannerData data(si); storage_.load(file_path.c_str(), data); - planner.reset(allocatePersistentPlanner(data)); + planner = std::shared_ptr{ allocatePersistentPlanner(data) }; if (!planner) { RCLCPP_ERROR(LOGGER, @@ -187,7 +187,7 @@ MultiQueryPlannerAllocator::allocatePlannerImpl(const ob::SpaceInformationPtr& s if (!planner) { - planner.reset(new T(si)); + planner = std::make_shared(si); } if (!new_name.empty()) @@ -247,7 +247,7 @@ PlanningContextManager::PlanningContextManager(moveit::core::RobotModelConstPtr , max_solution_segment_length_(0.0) , minimum_waypoint_count_(2) { - cached_contexts_.reset(new CachedContexts()); + cached_contexts_ = std::make_shared(); registerDefaultPlanners(); registerDefaultStateSpaces(); } @@ -380,11 +380,11 @@ PlanningContextManager::getPlanningContext(const planning_interface::PlannerConf else { // Choose the correct simple setup type to load - context_spec.ompl_simple_setup_.reset(new ompl::geometric::SimpleSetup(context_spec.state_space_)); + context_spec.ompl_simple_setup_ = std::make_shared(context_spec.state_space_); } RCLCPP_DEBUG(LOGGER, "Creating new planning context"); - context.reset(new ModelBasedPlanningContext(config.name, context_spec)); + context = std::make_shared(config.name, context_spec); // Do not cache a constrained planning context, as the constraints could be changed // and need to be parsed again. diff --git a/moveit_planners/ompl/package.xml b/moveit_planners/ompl/package.xml index ec527bab0e..c606f21f8f 100644 --- a/moveit_planners/ompl/package.xml +++ b/moveit_planners/ompl/package.xml @@ -1,11 +1,11 @@ - + + + moveit_planners_ompl 2.3.0 MoveIt interface to OMPL - - Ioan Sucan - - Dave Coleman + Henning Kayser + Tyler Weaver MoveIt Release Team BSD @@ -14,6 +14,8 @@ https://github.com/ros-planning/moveit2/issues https://github.com/ros-planning/moveit2 + Ioan Sucan + ament_cmake moveit_common diff --git a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt index b4c68c657a..d0d638fb10 100644 --- a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt +++ b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt @@ -1,512 +1,215 @@ -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() +############### +## Libraries ## +############### -################ -## 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} +add_library(planning_context_loader_base SHARED + src/planning_context_loader.cpp ) +ament_target_dependencies(planning_context_loader_base ${THIS_PACKAGE_INCLUDE_DEPENDS}) -## Declare a C++ library -add_library(${PROJECT_NAME}_lib - src/${PROJECT_NAME}.cpp - src/planning_context_loader.cpp - src/joint_limits_validator.cpp +add_library(joint_limits_common SHARED src/joint_limits_aggregator.cpp src/joint_limits_container.cpp - src/cartesian_limits_aggregator.cpp - src/cartesian_limit.cpp + src/joint_limits_validator.cpp src/limits_container.cpp + src/cartesian_limit.cpp + src/cartesian_limits_aggregator.cpp +) +ament_target_dependencies(joint_limits_common ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +add_library(trajectory_generation_common SHARED src/trajectory_functions.cpp - src/plan_components_builder.cpp + src/trajectory_generator.cpp + src/trajectory_blender_transition_window.cpp ) +ament_target_dependencies(trajectory_generation_common ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(${PROJECT_NAME}_lib ${catkin_LIBRARIES}) +add_library(command_list_manager SHARED + src/command_list_manager.cpp + src/plan_components_builder.cpp +) +ament_target_dependencies(command_list_manager ${THIS_PACKAGE_INCLUDE_DEPENDS}) ############# ## Plugins ## ############# -add_library(${PROJECT_NAME} + +add_library(${PROJECT_NAME} SHARED src/${PROJECT_NAME}.cpp - src/planning_context_loader.cpp - src/joint_limits_aggregator.cpp - src/joint_limits_container.cpp - src/limits_container.cpp - 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}) +target_link_libraries(${PROJECT_NAME} + planning_context_loader_base + joint_limits_common) -add_library(planning_context_loader_ptp +add_library(planning_context_loader_ptp SHARED src/planning_context_loader_ptp.cpp - src/planning_context_loader.cpp - src/trajectory_functions.cpp - src/trajectory_generator.cpp src/trajectory_generator_ptp.cpp 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}) +target_link_libraries(planning_context_loader_ptp planning_context_loader_base joint_limits_common trajectory_generation_common) -add_library(planning_context_loader_lin +add_library(planning_context_loader_lin SHARED src/planning_context_loader_lin.cpp - src/planning_context_loader.cpp - src/trajectory_functions.cpp - src/trajectory_generator.cpp 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}) +target_link_libraries(planning_context_loader_lin planning_context_loader_base joint_limits_common trajectory_generation_common) -add_library(planning_context_loader_circ +add_library(planning_context_loader_circ SHARED src/planning_context_loader_circ.cpp - src/planning_context_loader.cpp - src/trajectory_functions.cpp - src/trajectory_generator.cpp 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}) +target_link_libraries(planning_context_loader_circ planning_context_loader_base joint_limits_common trajectory_generation_common) -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}) -add_library(sequence_capability +add_library(sequence_capability SHARED src/move_group_sequence_action.cpp src/move_group_sequence_service.cpp - src/plan_components_builder.cpp - src/command_list_manager.cpp - src/trajectory_blender_transition_window.cpp - src/joint_limits_aggregator.cpp # do we need joint limits and cartesian_limit here? - src/joint_limits_container.cpp - src/limits_container.cpp - 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}) +target_link_libraries(sequence_capability joint_limits_common command_list_manager trajectory_generation_common) ############# ## Install ## ############# -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 -) +pluginlib_export_plugin_description_file(moveit_core plugins/pilz_industrial_motion_planner_plugin_description.xml) +pluginlib_export_plugin_description_file(move_group plugins/sequence_capability_plugin_description.xml) +pluginlib_export_plugin_description_file(pilz_industrial_motion_planner plugins/planning_context_plugin_description.xml) ## Mark libraries for installation install( TARGETS ${PROJECT_NAME} + joint_limits_common + planning_context_loader_base 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} + trajectory_generation_common + EXPORT ${PROJECT_NAME}Targets + 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} - ) - - 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 - ) +install( + DIRECTORY include/ + DESTINATION include +) - # 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 - ) +ament_export_include_directories( + include +) +ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) - # 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 - ) +if(BUILD_TESTING) + # Include Tests + add_subdirectory(test) - # 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 - ) + find_package(ament_lint_auto REQUIRED) - # 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} - ) + # 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_flake8_FOUND TRUE) + set(ament_cmake_uncrustify_FOUND TRUE) + set(ament_cmake_lint_cmake_FOUND TRUE) - # 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() + # Run all lint tests in package.xml except those listed above + ament_lint_auto_find_test_dependencies() endif() + +ament_package() 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.png b/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.png index 724cdf1911..93f641245d 100644 Binary files a/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.png and b/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.png differ 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..82eefb4253 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 @@ -3,8 +3,8 @@ UMLClass - 287 - 273 + 357 + 399 567 133 @@ -23,8 +23,8 @@ UMLGeneric - 0 - 525 + 70 + 651 259 70 @@ -39,8 +39,8 @@ PlanningContextLoaderPTP UMLGeneric - 448 - 525 + 518 + 651 259 70 @@ -55,8 +55,8 @@ PlanningContextLoaderLIN Relation - 119 - 350 + 189 + 476 182 189 @@ -66,8 +66,8 @@ PlanningContextLoaderLIN Relation - 567 - 399 + 637 + 525 21 140 @@ -77,8 +77,8 @@ PlanningContextLoaderLIN UMLGeneric - 35 - 756 + 105 + 882 133 35 @@ -90,8 +90,8 @@ PlanningContextLoaderLIN UMLGeneric - 511 - 749 + 581 + 875 126 35 @@ -103,8 +103,8 @@ PlanningContextLoaderLIN UMLClass - 1260 - 616 + 1330 + 742 119 126 @@ -117,8 +117,8 @@ PlanningContextBase Text - 21 - 609 + 91 + 735 175 14 @@ -128,8 +128,8 @@ PlanningContextBase Relation - 133 - 644 + 203 + 770 1141 126 @@ -139,8 +139,8 @@ PlanningContextBase Relation - 602 - 672 + 672 + 798 672 91 @@ -150,8 +150,8 @@ PlanningContextBase Text - 490 - 609 + 560 + 735 175 14 @@ -161,8 +161,8 @@ PlanningContextBase UMLGeneric - 511 - 1001 + 581 + 1127 126 35 @@ -174,8 +174,8 @@ PlanningContextBase Relation - 98 - 588 + 168 + 714 21 182 @@ -185,8 +185,8 @@ PlanningContextBase UMLGeneric - 35 - 1001 + 105 + 1127 126 35 @@ -198,8 +198,8 @@ PlanningContextBase UMLGeneric - 1176 - 861 + 1246 + 987 308 98 @@ -207,15 +207,15 @@ PlanningContextBase -- -- TrajectoryGenerator(RobotModelConstPtr, LimitsContainer) -generate(request, response, sampling_time) +generate(scene, request, response, sampling_time) Relation - 567 - 588 + 637 + 714 21 175 @@ -225,8 +225,8 @@ generate(request, response, sampling_time) Relation - 567 - 777 + 637 + 903 21 238 @@ -237,8 +237,8 @@ m1=1 Relation - 98 - 784 + 168 + 910 21 231 @@ -249,8 +249,8 @@ m1=1 Relation - 602 - 903 + 672 + 1029 588 112 @@ -260,8 +260,8 @@ m1=1 Relation - 126 - 875 + 196 + 1001 1064 140 @@ -271,8 +271,8 @@ m1=1 UMLNote - 0 - 441 + 70 + 567 98 49 @@ -283,8 +283,8 @@ m1=1 Relation - 42 - 483 + 112 + 609 21 56 @@ -294,8 +294,8 @@ m1=1 UMLClass - 1589 - 525 + 1659 + 651 147 112 @@ -309,8 +309,8 @@ m1=1 UMLClass - 1792 - 525 + 1862 + 651 147 112 @@ -330,8 +330,8 @@ m1=1 UMLClass - 1673 - 399 + 1743 + 525 196 91 @@ -349,8 +349,8 @@ m1=1 Relation - 1764 - 483 + 1834 + 609 126 56 @@ -361,8 +361,8 @@ m1=1 Relation - 1659 - 483 + 1729 + 609 70 56 @@ -373,8 +373,8 @@ m1=1 UMLClass - 168 - 1232 + 238 + 1358 126 42 @@ -388,8 +388,8 @@ m1=1 UMLClass - 28 - 1302 + 98 + 1428 147 63 @@ -407,8 +407,8 @@ m1=1 Relation - 126 - 1246 + 196 + 1372 56 70 @@ -418,8 +418,8 @@ m1=1 Relation - 91 - 1029 + 161 + 1155 21 287 @@ -430,8 +430,8 @@ m1=n UMLClass - 525 - 1316 + 595 + 1442 91 42 @@ -445,8 +445,8 @@ Path_Line UMLClass - 833 - 1316 + 903 + 1442 84 42 @@ -460,8 +460,8 @@ Path_Circle UMLClass - 980 - 1316 + 1050 + 1442 105 42 @@ -475,8 +475,8 @@ Path_Circle_Wrapper UMLNote - 1393 - 1176 + 1463 + 1302 98 49 @@ -487,8 +487,8 @@ bg=white Relation - 1330 - 1190 + 1400 + 1316 77 21 @@ -498,8 +498,8 @@ bg=white Relation - 560 - 1029 + 630 + 1155 21 301 @@ -509,8 +509,8 @@ bg=white UMLGeneric - 882 - 1001 + 952 + 1127 168 35 @@ -522,8 +522,8 @@ bg=white Relation - 1022 - 1029 + 1092 + 1155 21 301 @@ -533,8 +533,8 @@ bg=white Relation - 581 - 1197 + 651 + 1323 693 133 @@ -544,8 +544,8 @@ bg=white Relation - 889 - 1232 + 959 + 1358 385 98 @@ -555,8 +555,8 @@ bg=white Relation - 1050 - 1267 + 1120 + 1393 224 63 @@ -566,8 +566,8 @@ bg=white UMLGeneric - 896 - 742 + 966 + 868 126 35 @@ -579,8 +579,8 @@ bg=white Relation - 952 - 770 + 1022 + 896 21 245 @@ -591,8 +591,8 @@ m1=1 Relation - 987 - 700 + 1057 + 826 287 56 @@ -602,8 +602,8 @@ m1=1 UMLGeneric - 826 - 525 + 896 + 651 259 70 @@ -618,8 +618,8 @@ PlanningContextLoaderCIRC Relation - 952 - 588 + 1022 + 714 21 168 @@ -629,8 +629,8 @@ PlanningContextLoaderCIRC Text - 875 - 609 + 945 + 735 175 14 @@ -640,8 +640,8 @@ PlanningContextLoaderCIRC Relation - 987 - 931 + 1057 + 1057 203 84 @@ -651,8 +651,8 @@ PlanningContextLoaderCIRC Relation - 847 - 350 + 917 + 476 119 189 @@ -662,8 +662,8 @@ PlanningContextLoaderCIRC Relation - 91 - 469 + 161 + 595 434 70 @@ -673,8 +673,8 @@ PlanningContextLoaderCIRC Relation - 91 - 448 + 161 + 574 812 91 @@ -684,8 +684,8 @@ PlanningContextLoaderCIRC UMLClass - 287 - 126 + 357 + 252 567 63 @@ -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 -- @@ -702,8 +702,8 @@ moveit_msgs::MoveItErrorCodes& error_code):planning_interface::PlanningConte Relation - 567 - 182 + 637 + 308 21 105 @@ -714,8 +714,8 @@ m1=n UMLClass - 476 - 0 + 546 + 126 203 49 @@ -729,8 +729,8 @@ m1=n Relation - 567 - 42 + 637 + 168 21 98 @@ -740,8 +740,8 @@ m1=n UMLClass - 1211 - 518 + 1281 + 644 203 49 @@ -755,8 +755,8 @@ m1=n Relation - 1309 - 560 + 1379 + 686 21 77 @@ -766,8 +766,8 @@ m1=n Relation - 910 - 1330 + 980 + 1456 84 21 @@ -777,8 +777,8 @@ m1=n UMLClass - 308 - 1302 + 378 + 1428 147 63 @@ -796,8 +796,8 @@ VelocityProfile_Trap Relation - 420 - 1008 + 490 + 1134 476 308 @@ -807,8 +807,8 @@ VelocityProfile_Trap UMLClass - 770 - 1141 + 840 + 1267 126 42 @@ -822,8 +822,8 @@ Trajectory_Segment Relation - 889 - 1029 + 959 + 1155 63 147 @@ -833,8 +833,8 @@ Trajectory_Segment Relation - 287 - 1246 + 357 + 1372 63 70 @@ -844,8 +844,8 @@ Trajectory_Segment UMLClass - 784 - 1064 + 854 + 1190 98 35 @@ -859,8 +859,8 @@ Trajectory_Segment Relation - 826 - 1092 + 896 + 1218 21 63 @@ -870,8 +870,8 @@ Trajectory_Segment UMLClass - 1260 - 1183 + 1330 + 1309 77 112 @@ -884,8 +884,8 @@ Trajectory_Segment Relation - 588 - 1029 + 658 + 1155 196 147 @@ -895,8 +895,8 @@ Trajectory_Segment Relation - 357 - 1008 + 427 + 1134 168 308 diff --git a/moveit_planners/pilz_industrial_motion_planner/include/joint_limits/joint_limits.hpp b/moveit_planners/pilz_industrial_motion_planner/include/joint_limits/joint_limits.hpp new file mode 100644 index 0000000000..8ebfe2f4b5 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/joint_limits/joint_limits.hpp @@ -0,0 +1,134 @@ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#ifndef JOINT_LIMITS__JOINT_LIMITS_HPP_ +#define JOINT_LIMITS__JOINT_LIMITS_HPP_ + +// TODO(henning): This file is copied from the DRAFT PR https://github.com/ros-controls/ros2_control/pull/462 since the +// current ros2_control implementation does not offer the desired joint limits API, yet. Remove when ros2_control has an +// upstream version of this. + +#include +#include +#include + +namespace joint_limits +{ +struct JointLimits +{ + JointLimits() + : min_position(std::numeric_limits::quiet_NaN()) + , max_position(std::numeric_limits::quiet_NaN()) + , max_velocity(std::numeric_limits::quiet_NaN()) + , max_acceleration(std::numeric_limits::quiet_NaN()) + , max_jerk(std::numeric_limits::quiet_NaN()) + , max_effort(std::numeric_limits::quiet_NaN()) + , 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; + + std::string to_string() // NOLINT: Ignore case style + { + std::stringstream ss_output; + + if (has_position_limits) + { + ss_output << " position limits: " + << "[" << min_position << ", " << max_position << "]\n"; + } + if (has_velocity_limits) + { + ss_output << " velocity limit: " + << "[" << max_velocity << "]\n"; + } + if (has_acceleration_limits) + { + ss_output << " acceleration limit: " + << "[" << max_acceleration << "]\n"; + } + if (has_jerk_limits) + { + ss_output << " jerk limit: " + << "[" << max_acceleration << "]\n"; + } + if (has_effort_limits) + { + ss_output << " effort limit: " + << "[" << max_acceleration << "]\n"; + } + if (angle_wraparound) + { + ss_output << " angle wraparound is active."; + } + + return ss_output.str(); + } + + std::string debug_to_string() // NOLINT: Ignore case style + { + std::stringstream ss_output; + + ss_output << " has position limits: " << (has_position_limits ? "true" : "false") << "[" << min_position << ", " + << max_position << "]\n"; + ss_output << " has velocity limits: " << (has_velocity_limits ? "true" : "false") << "[" << max_velocity << "]\n"; + ss_output << " has acceleration limits: " << (has_acceleration_limits ? "true" : "false") << " [" + << max_acceleration << "]\n"; + ss_output << " has jerk limits: " << (has_jerk_limits ? "true" : "false") << "[" << max_jerk << "]\n"; + ss_output << " has effort limits: " << (has_effort_limits ? "true" : "false") << "[" << max_effort << "]\n"; + ss_output << " angle wraparound: " << (angle_wraparound ? "true" : "false"); + + return ss_output.str(); + } +}; + +struct SoftJointLimits +{ + SoftJointLimits() + : min_position(std::numeric_limits::quiet_NaN()) + , max_position(std::numeric_limits::quiet_NaN()) + , k_position(std::numeric_limits::quiet_NaN()) + , k_velocity(std::numeric_limits::quiet_NaN()) + { + } + + double min_position; + double max_position; + double k_position; + double k_velocity; +}; + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_LIMITS_HPP_ diff --git a/moveit_planners/pilz_industrial_motion_planner/include/joint_limits/joint_limits_rosparam.hpp b/moveit_planners/pilz_industrial_motion_planner/include/joint_limits/joint_limits_rosparam.hpp new file mode 100644 index 0000000000..672ba60cba --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/joint_limits/joint_limits_rosparam.hpp @@ -0,0 +1,291 @@ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#ifndef JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_ +#define JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_ + +// TODO(henning): This file is copied from the DRAFT PR https://github.com/ros-controls/ros2_control/pull/462 since the +// current ros2_control implementation does not offer the desired joint limits API, yet. Remove when ros2_control has an +// upstream version of this. + +#include +#include + +#include "joint_limits/joint_limits.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace joint_limits +{ +inline bool declare_parameters(const std::string& joint_name, const rclcpp::Node::SharedPtr& node, + const std::string& param_ns) +{ + const std::string param_base_name = (param_ns.empty() ? "" : param_ns + ".") + "joint_limits." + joint_name; + try + { + node->declare_parameter(param_base_name + ".has_position_limits", false); + node->declare_parameter(param_base_name + ".min_position", std::numeric_limits::quiet_NaN()); + node->declare_parameter(param_base_name + ".max_position", std::numeric_limits::quiet_NaN()); + node->declare_parameter(param_base_name + ".has_velocity_limits", false); + node->declare_parameter(param_base_name + ".min_velocity", std::numeric_limits::quiet_NaN()); + node->declare_parameter(param_base_name + ".max_velocity", std::numeric_limits::quiet_NaN()); + node->declare_parameter(param_base_name + ".has_acceleration_limits", false); + node->declare_parameter(param_base_name + ".max_acceleration", std::numeric_limits::quiet_NaN()); + node->declare_parameter(param_base_name + ".has_jerk_limits", false); + node->declare_parameter(param_base_name + ".max_jerk", std::numeric_limits::quiet_NaN()); + node->declare_parameter(param_base_name + ".has_effort_limits", false); + node->declare_parameter(param_base_name + ".max_effort", std::numeric_limits::quiet_NaN()); + node->declare_parameter(param_base_name + ".angle_wraparound", false); + node->declare_parameter(param_base_name + ".has_soft_limits", false); + node->declare_parameter(param_base_name + ".k_position", std::numeric_limits::quiet_NaN()); + node->declare_parameter(param_base_name + ".k_velocity", std::numeric_limits::quiet_NaN()); + node->declare_parameter(param_base_name + ".soft_lower_limit", std::numeric_limits::quiet_NaN()); + node->declare_parameter(param_base_name + ".soft_upper_limit", std::numeric_limits::quiet_NaN()); + } + catch (const std::exception& ex) + { + RCLCPP_ERROR(node->get_logger(), "%s", ex.what()); + return false; + } + return true; +} + +/// 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. + */ +inline bool get_joint_limits(const std::string& joint_name, const rclcpp::Node::SharedPtr& node, + const std::string& param_ns, JointLimits& limits) +{ + const std::string param_base_name = (param_ns.empty() ? "" : param_ns + ".") + "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(node->get_logger(), + "No joint limits specification found for joint '%s' in the parameter server " + "(node: %s param name: %s).", + joint_name.c_str(), node->get_name(), param_base_name.c_str()); + return false; + } + } + catch (const std::exception& ex) + { + RCLCPP_ERROR(node->get_logger(), "%s", 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. +/** + * 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 get_joint_limits(const std::string& joint_name, const rclcpp::Node::SharedPtr& node, + const std::string& param_ns, SoftJointLimits& soft_limits) +{ + const std::string param_base_name = (param_ns.empty() ? "" : param_ns + ".") + "joint_limits." + joint_name; + try + { + 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")) + { + RCLCPP_DEBUG(node->get_logger(), + "No soft joint limits specification found for joint '%s' in the parameter server " + "(node: %s param name: %s).", + joint_name.c_str(), node->get_name(), param_base_name.c_str()); + return false; + } + } + catch (const std::exception& ex) + { + RCLCPP_ERROR(node->get_logger(), "%s", 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 + +#endif // JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_ 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..1d7fe4a096 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,18 +34,19 @@ #pragma once +#include #include "pilz_industrial_motion_planner/cartesian_limit.h" namespace pilz_industrial_motion_planner { /** - * @brief Obtains cartesian limits from the parameter server + * @brief Obtains cartesian limits from the node parameters */ class CartesianLimitsAggregator { public: /** - * @brief Loads cartesian limits from the parameter server + * @brief Loads cartesian limits from the node parameters * * The parameters are expected to be under "~/cartesian_limits" of the given * node handle. @@ -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..4835dc8876 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 acceleration; + 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..aad70af031 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 @@ -49,7 +49,7 @@ namespace pilz_industrial_motion_planner { /** * @brief Unifies the joint limits from the given joint models with joint - * limits from the parameter server. + * limits from the node parameters. * * Does not support MultiDOF joints. */ @@ -58,26 +58,27 @@ class JointLimitsAggregator public: /** * @brief Aggregates(combines) the joint limits from joint model and - * parameter server. + * node parameters. * The rules for the combination are: - * 1. Position and velocity limits on the parameter server must be stricter + * 1. Position and velocity limits in the node parameters must be stricter * or equal if they are defined. - * 2. Limits on the parameter server where the corresponding + * 2. Limits in the node parameters where the corresponding * has__limits are „false“ * are considered undefined(see point 1). - * 3. Not all joints have to be limited by the parameter server. Selective + * 3. Not all joints have to be limited by the node parameters. Selective * limitation is possible. * 4. If max_deceleration is unset, it will be set to: max_deceleration = - * max_acceleration. - * @note The acceleration/deceleration can only be set via the parameter - * server since they are not supported + * @note The acceleration/deceleration can only be set via the node parameters parameter + * 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: @@ -135,7 +136,7 @@ class AggregationException : public std::runtime_error /** * @class AggregationJointMissingException - * @brief Thrown the limits from the parameter server are weaker(forbidden) than + * @brief Thrown the limits from the node parameter are weaker(forbidden) than * the ones defined in the urdf * */ 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..dd00949a65 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,10 @@ #ifndef JOINT_LIMITS_EXTENSION_H #define JOINT_LIMITS_EXTENSION_H -#include +#include + +#include + #include #include @@ -47,7 +50,7 @@ namespace joint_limits_interface * @brief Extends joint_limits_interface::JointLimits with a deceleration * parameter */ -struct JointLimits : ::joint_limits_interface::JointLimits +struct JointLimits : joint_limits::JointLimits { JointLimits() : max_deceleration(0.0), has_deceleration_limits(false) { 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..106ce081bb 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,51 @@ #define JOINT_LIMITS_INTERFACE_EXTENSION_H #include "pilz_industrial_motion_planner/joint_limits_extension.h" -#include +#include + +#include namespace pilz_industrial_motion_planner { namespace joint_limits_interface { /** - * @see joint_limits_inteface::getJointLimits(...) + * @see joint_limits::declare_parameters(...) */ -inline bool getJointLimits(const std::string& joint_name, const ros::NodeHandle& nh, - joint_limits_interface::JointLimits& limits) +inline bool declareParameters(const std::string& joint_name, const std::string& param_ns, + const rclcpp::Node::SharedPtr& node) { - // Node handle scoped where the joint limits are - // defined (copied from ::joint_limits_interface::getJointLimits(joint_name, - // nh, limits) - ros::NodeHandle limits_nh; - try - { - const std::string limits_namespace = "joint_limits/" + joint_name; - if (!nh.hasParam(limits_namespace)) - { - ROS_DEBUG_STREAM("No joint limits specification found for joint '" - << joint_name << "' in the parameter server (namespace " - << nh.getNamespace() + "/" + limits_namespace << ")."); - return false; - } - limits_nh = ros::NodeHandle(nh, limits_namespace); - } - catch (const ros::InvalidNameException& ex) - { - ROS_ERROR_STREAM(ex.what()); - return false; - } - - // Set the existing limits - if (!::joint_limits_interface::getJointLimits(joint_name, nh, limits)) + return joint_limits::declare_parameters(joint_name, node, param_ns); +} +/** + * @see joint_limits::get_joint_limits(...) + */ +inline bool getJointLimits(const std::string& joint_name, const std::string& param_ns, + const rclcpp::Node::SharedPtr& node, joint_limits_interface::JointLimits& limits) +{ + // Load the existing limits + if (!joint_limits::get_joint_limits(joint_name, node, param_ns, 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 param_base_name = (param_ns.empty() ? "" : param_ns + ".") + "joint_limits." + joint_name; + + limits.has_deceleration_limits = node->declare_parameter(param_base_name + ".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(param_base_name + ".max_deceleration", std::numeric_limits::quiet_NaN()); } } + 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/joint_limits_validator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_validator.h index 8b95269266..b7af47bfa7 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_validator.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_validator.h @@ -111,8 +111,8 @@ class ValidationException : public std::runtime_error /** * @class ValidationJointMissingException - * @brief Thrown the limits for a joint are defined in the urdf but not on the - * parameter server (loaded from yaml) + * @brief Thrown the limits for a joint are defined in the urdf but not in the + * node parameters (loaded from yaml) * */ class ValidationJointMissingException : public ValidationException 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..48d4ab0e6f 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,14 +36,15 @@ #include -#include +#include #include -#include +#include namespace pilz_industrial_motion_planner { class CommandListManager; +using MoveGroupSequenceGoalHandle = rclcpp_action::ServerGoalHandle; /** * @brief Provide action to handle multiple trajectories and execute the result @@ -59,21 +60,22 @@ 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,10 @@ class MoveGroupSequenceAction : public move_group::MoveGroupCapability PlannedTrajMsgs& plannedTrajsMsgs); private: - std::unique_ptr> move_action_server_; - moveit_msgs::MoveGroupSequenceFeedback move_feedback_; + rclcpp::CallbackGroup::SharedPtr action_callback_group_; + 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..5214b906b6 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 @@ -116,7 +119,7 @@ class CommandPlanner : public planning_interface::PlannerManager private: /// Plugin loader - boost::scoped_ptr> planner_context_loader; + std::unique_ptr> planner_context_loader; /// Mapping from command to loader std::map context_loader_map_; 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..ca2a894ee1 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 @@ -39,6 +39,7 @@ #include #include +#include #include "pilz_industrial_motion_planner/trajectory_blend_request.h" #include "pilz_industrial_motion_planner/trajectory_blender.h" @@ -50,10 +51,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 @@ -87,13 +88,16 @@ class PlanComponentsBuilder * appended/attached to the newly created empty trajectory: * - if the given and previous trajectory are from different groups. * + * @param planning_scene The scene planning is occurring in. + * * @param other Trajectories which has to be added to the trajectory container * under construction. * * @param blend_radius The blending radius between the previous and the * specified trajectory. */ - void append(const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius); + void append(const planning_scene::PlanningSceneConstPtr& planning_scene, + const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius); /** * @brief Clears the trajectory container under construction. @@ -106,7 +110,8 @@ class PlanComponentsBuilder std::vector build() const; private: - void blend(const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius); + void blend(const planning_scene::PlanningSceneConstPtr& planning_scene, + const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius); private: /** 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..3cd479e52d 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); + bool result = generator_.generate(getPlanningScene(), 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 5a6784079e..47346e7779 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, @@ -123,19 +123,19 @@ bool PlanningContextLoader::loadContext(planning_interface::PlanningContextPtr& { if (limits_set_ && model_set_) { - planning_context.reset(new T(name, group, model_, limits_)); + planning_context = std::make_shared(name, group, model_, limits_); return true; } else { 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.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender.h index ce647b5479..6c7667fe1d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender.h @@ -36,6 +36,7 @@ #include "pilz_industrial_motion_planner/limits_container.h" #include +#include #include "pilz_industrial_motion_planner/trajectory_blend_request.h" #include "pilz_industrial_motion_planner/trajectory_blend_response.h" @@ -58,11 +59,13 @@ class TrajectoryBlender /** * @brief Blend two robot trajectories with the given blending radius + * @param planning_scene: planning scene * @param req: trajectory blend request * @param res: trajectroy blend response * @return true if blend succeed */ - virtual bool blend(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + virtual bool blend(const planning_scene::PlanningSceneConstPtr& planning_scene, + const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, pilz_industrial_motion_planner::TrajectoryBlendResponse& res) = 0; protected: 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..e737f25aa6 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 @@ -34,6 +34,7 @@ #pragma once +#include #include "pilz_industrial_motion_planner/cartesian_trajectory.h" #include "pilz_industrial_motion_planner/cartesian_trajectory_point.h" #include "pilz_industrial_motion_planner/trajectory_blend_request.h" @@ -64,6 +65,7 @@ class TrajectoryBlenderTransitionWindow : public TrajectoryBlender * @brief Blend two trajectories using transition window. The trajectories * have to be equally and uniformly * discretized. + * @param planning_scene: The scene planning is occurring in. * @param req: following fields need to be filled for a valid request: * - group_name : name of the planning group * - link_name : name of the target link @@ -91,7 +93,8 @@ class TrajectoryBlenderTransitionWindow : public TrajectoryBlender * error_code: information of failed blend * @return true if succeed */ - bool blend(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + bool blend(const planning_scene::PlanningSceneConstPtr& planning_scene, + const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, pilz_industrial_motion_planner::TrajectoryBlendResponse& res) override; private: @@ -104,7 +107,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..fdb8e8f3d4 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,8 @@ #include #include #include -#include +#include +#include #include "pilz_industrial_motion_planner/cartesian_trajectory.h" #include "pilz_industrial_motion_planner/limits_container.h" @@ -50,7 +51,7 @@ namespace pilz_industrial_motion_planner /** * @brief compute the inverse kinematics of a given pose, also check robot self * collision - * @param robot_model: kinematic model of the robot + * @param scene: planning scene * @param group_name: name of planning group * @param link_name: name of target link * @param pose: target pose in IK solver Frame @@ -62,13 +63,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 planning_scene::PlanningSceneConstPtr& scene, 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 planning_scene::PlanningSceneConstPtr& scene, 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 +81,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); @@ -107,7 +108,7 @@ bool verifySampleJointLimits(const std::map& position_last, /** * @brief Generate joint trajectory from a KDL Cartesian trajectory - * @param robot_model: robot kinematics model + * @param scene: planning scene * @param joint_limits: joint limits * @param trajectory: KDL Cartesian trajectory * @param group_name: name of the planning group @@ -122,15 +123,16 @@ 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 planning_scene::PlanningSceneConstPtr& scene, 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 + * @param scene: planning scene * @param trajectory: Cartesian trajectory * @param info: motion plan information * @param sampling_time @@ -138,14 +140,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 planning_scene::PlanningSceneConstPtr& scene, 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,17 +172,17 @@ 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); /** - * @brief check if the robot state have zero velocity/acceleartion + * @brief check if the robot state have zero velocity/acceleration * @param state * @param group * @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 @@ -202,6 +204,7 @@ bool intersectionFound(const Eigen::Vector3d& p_center, const Eigen::Vector3d& p /** * @brief Checks if current robot state is in self collision. + * @param scene: planning scene. * @param test_for_self_collision Flag to deactivate this check during IK. * @param robot_model: robot kinematics model. * @param state Robot state instance used for . @@ -209,9 +212,9 @@ bool intersectionFound(const Eigen::Vector3d& p_center, const Eigen::Vector3d& p * @param ik_solution * @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, +bool isStateColliding(const bool test_for_self_collision, const planning_scene::PlanningSceneConstPtr& scene, + 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..b02bab06e5 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 @@ -42,6 +42,7 @@ #include #include #include +#include #include "pilz_industrial_motion_planner/joint_limits_extension.h" #include "pilz_industrial_motion_planner/limits_container.h" @@ -52,34 +53,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 +94,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()) { } @@ -103,10 +108,10 @@ class TrajectoryGenerator * @param res: motion plan response * @param sampling_time: sampling time of the generate trajectory * @return motion plan succeed/fail, detailed information in motion plan - * responce + * response */ - bool generate(const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - double sampling_time = 0.1); + bool generate(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, double sampling_time = 0.1); protected: /** @@ -144,14 +149,17 @@ class TrajectoryGenerator * @brief Extract needed information from a motion plan request in order to * simplify * further usages. + * @param scene: planning scene * @param req: motion plan request * @param info: information extracted from motion plan request which is * necessary for the planning */ - virtual void extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const = 0; + virtual void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, + 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; + virtual void plan(const planning_scene::PlanningSceneConstPtr& scene, + const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, + const double& sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) = 0; private: /** @@ -159,40 +167,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 +208,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 moveit::core::RobotState& start_rs, const std::string& group_name, + 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,20 +227,17 @@ 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 convertToRobotTrajectory(const trajectory_msgs::JointTrajectory& joint_trajectory, - const moveit_msgs::RobotState& start_state, - robot_trajectory::RobotTrajectory& robot_trajectory) const; + void checkCartesianGoalConstraint(const moveit_msgs::msg::Constraints& constraint, + const std::string& group_name) 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..2940be8bb4 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 @@ -37,6 +37,7 @@ #include #include #include +#include #include "pilz_industrial_motion_planner/trajectory_generator.h" @@ -44,18 +45,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,16 +82,18 @@ 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: void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const override; - void extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final; + void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, + 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; + void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, + const MotionPlanInfo& plan_info, 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..3501ca7d3c 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 @@ -36,6 +36,7 @@ #include #include +#include #include "pilz_industrial_motion_planner/trajectory_generator.h" #include "pilz_industrial_motion_planner/velocity_profile_atrap.h" @@ -46,11 +47,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 +67,16 @@ 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 extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, + 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; + void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, + const MotionPlanInfo& plan_info, 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..6ad5c9b51f 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 @@ -34,6 +34,8 @@ #pragma once +#include + #include "eigen3/Eigen/Eigen" #include "pilz_industrial_motion_planner/trajectory_generation_exceptions.h" #include "pilz_industrial_motion_planner/trajectory_generator.h" @@ -45,8 +47,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,11 +62,12 @@ 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: - void extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const override; + void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, + const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const override; /** * @brief plan ptp joint trajectory with zero start velocity @@ -77,12 +80,13 @@ 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; + void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, + const MotionPlanInfo& plan_info, 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..51eea4f9eb 100644 --- a/moveit_planners/pilz_industrial_motion_planner/package.xml +++ b/moveit_planners/pilz_industrial_motion_planner/package.xml @@ -1,7 +1,8 @@ + pilz_industrial_motion_planner - 1.1.5 + 1.1.6 MoveIt plugin to generate industrial trajectories PTP, LIN, CIRC and sequences thereof. Alexander Gutenkunst @@ -16,39 +17,46 @@ 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 - orocos_kdl - liborocos-kdl-dev + orocos_kdl pluginlib - roscpp + rclcpp tf2 tf2_eigen tf2_geometry_msgs tf2_kdl + tf2_eigen_kdl tf2_ros - - rostest - rosunit - cmake_modules - moveit_resources_panda_moveit_config 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 + + boost + + ament_lint_auto + ament_lint_common - - - + ament_cmake diff --git a/moveit_planners/pilz_industrial_motion_planner/plugins/pilz_industrial_motion_planner_plugin_description.xml b/moveit_planners/pilz_industrial_motion_planner/plugins/pilz_industrial_motion_planner_plugin_description.xml index 3a6f16cf50..6efba3a067 100644 --- a/moveit_planners/pilz_industrial_motion_planner/plugins/pilz_industrial_motion_planner_plugin_description.xml +++ b/moveit_planners/pilz_industrial_motion_planner/plugins/pilz_industrial_motion_planner_plugin_description.xml @@ -1,5 +1,7 @@ - - + + Pilz Industrial Motion Planner diff --git a/moveit_planners/pilz_industrial_motion_planner/plugins/planning_context_plugin_description.xml b/moveit_planners/pilz_industrial_motion_planner/plugins/planning_context_plugin_description.xml index 0ff208f2a2..4c95fcb49e 100644 --- a/moveit_planners/pilz_industrial_motion_planner/plugins/planning_context_plugin_description.xml +++ b/moveit_planners/pilz_industrial_motion_planner/plugins/planning_context_plugin_description.xml @@ -1,18 +1,23 @@ - - - Loader for PTP Context - - - - - Loader for LIN Context - - - - - Loader for CIRC Context - - + + + + Loader for PTP Context + + + + + Loader for LIN Context + + + + + Loader for CIRC Context + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/plugins/sequence_capability_plugin_description.xml b/moveit_planners/pilz_industrial_motion_planner/plugins/sequence_capability_plugin_description.xml index 9daf67146c..0628310f2e 100644 --- a/moveit_planners/pilz_industrial_motion_planner/plugins/sequence_capability_plugin_description.xml +++ b/moveit_planners/pilz_industrial_motion_planner/plugins/sequence_capability_plugin_description.xml @@ -1,4 +1,4 @@ - + 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..ed2fbbb8f8 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,8 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include "ros/ros.h" +#include +#include #include "pilz_industrial_motion_planner/cartesian_limits_aggregator.h" @@ -45,50 +46,75 @@ static const std::string PARAM_MAX_ROT_VEL = "max_rot_vel"; static const std::string PARAM_MAX_ROT_ACC = "max_rot_acc"; static const std::string PARAM_MAX_ROT_DEC = "max_rot_dec"; +// TODO(sjahr) Refactor and use repository wide solution +bool declareAndGetParam(double& output_value, const std::string& param_name, const rclcpp::Node::SharedPtr& node) +{ + try + { + if (!node->has_parameter(param_name)) + { + node->declare_parameter(param_name, std::numeric_limits::quiet_NaN()); + } + node->get_parameter(param_name, output_value); + if (std::isnan(output_value)) + { + RCLCPP_ERROR(node->get_logger(), "Parameter \'%s\', is not set in the config file.", param_name.c_str()); + return false; + } + return true; + } + catch (const rclcpp::exceptions::InvalidParameterTypeException& e) + { + RCLCPP_WARN(node->get_logger(), "InvalidParameterTypeException(\'%s\'): %s", param_name.c_str(), e.what()); + RCLCPP_ERROR(node->get_logger(), "Error getting parameter \'%s\', check parameter type in YAML file", + param_name.c_str()); + throw e; + } +} + 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)) + if (declareAndGetParam(max_trans_vel, param_prefix + PARAM_MAX_TRANS_VEL, node)) { cartesian_limit.setMaxTranslationalVelocity(max_trans_vel); } // translational acceleration double max_trans_acc; - if (nh.getParam(param_prefix + PARAM_MAX_TRANS_ACC, max_trans_acc)) + if (declareAndGetParam(max_trans_acc, param_prefix + PARAM_MAX_TRANS_ACC, node)) { cartesian_limit.setMaxTranslationalAcceleration(max_trans_acc); } // translational deceleration double max_trans_dec; - if (nh.getParam(param_prefix + PARAM_MAX_TRANS_DEC, max_trans_dec)) + if (declareAndGetParam(max_trans_dec, param_prefix + PARAM_MAX_TRANS_DEC, node)) { cartesian_limit.setMaxTranslationalDeceleration(max_trans_dec); } // rotational velocity double max_rot_vel; - if (nh.getParam(param_prefix + PARAM_MAX_ROT_VEL, max_rot_vel)) + if (declareAndGetParam(max_rot_vel, param_prefix + PARAM_MAX_ROT_VEL, node)) { 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)) + if (node->has_parameter(param_prefix + PARAM_MAX_ROT_ACC) || node->has_parameter(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."); + RCLCPP_WARN(node->get_logger(), + "Ignoring cartesian limits parameters for rotational acceleration / deceleration; these parameters are " + "deprecated and are automatically calculated from translational to rotational ratio."); } // LCOV_EXCL_STOP 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..7d746cecd5 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,21 @@ 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; 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 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 +78,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()) { @@ -97,7 +98,7 @@ RobotTrajCont CommandListManager::solve(const planning_scene::PlanningSceneConst plan_comp_builder_.reset(); for (MotionResponseCont::size_type i = 0; i < resp_cont.size(); ++i) { - plan_comp_builder_.append(resp_cont.at(i).trajectory_, + plan_comp_builder_.append(planning_scene, resp_cont.at(i).trajectory_, // The blend radii has to be "attached" to // the second part of a blend trajectory, // therefore: "i-1". @@ -167,7 +168,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 +178,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 +190,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 +227,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 +242,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 +288,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 +302,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..cf65718581 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,22 +39,33 @@ #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; - ROS_INFO_STREAM("Reading limits from namespace " << nh.getNamespace()); + RCLCPP_INFO_STREAM(LOGGER, "Reading limits from namespace " << param_namespace); // Iterate over all joint models and generate the map for (auto joint_model : joint_models) { 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)) + // NOTE: declareParameters fails (=returns false) if the parameters have already been declared. + // The function should be checked in the if condition below when we disable + // 'NodeOptions::automatically_declare_parameters_from_overrides(true)' + joint_limits_interface::declareParameters(joint_model->getName(), param_namespace, node); + + // If there is something defined for the joint in the node parameters + if (joint_limits_interface::getJointLimits(joint_model->getName(), param_namespace, node, joint_limit)) { if (joint_limit.has_position_limits) { @@ -76,7 +87,7 @@ pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( } else { - // If there is nothing defined for this joint on the parameter server just + // If there is nothing defined for this joint in the node parameters just // update the values by the values of // the urdf @@ -105,7 +116,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 +126,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 +134,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 +145,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 +154,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..bd10650652 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 @@ -47,12 +47,16 @@ #include #include #include +#include #include "pilz_industrial_motion_planner/command_list_manager.h" #include "pilz_industrial_motion_planner/trajectory_generation_exceptions.h" 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 +64,63 @@ 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"); + action_callback_group_ = + context_->moveit_cpp_->getNode()->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + move_action_server_ = rclcpp_action::create_server( + context_->moveit_cpp_->getNode(), "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 /* unused goal_handle */) { + RCLCPP_DEBUG(LOGGER, "Canceling action goal"); + preemptMoveCallback(); + return rclcpp_action::CancelResponse::ACCEPT; + }, + [this](const std::shared_ptr goal_handle) { + RCLCPP_DEBUG(LOGGER, "Accepting new action goal"); + executeSequenceCallback(goal_handle); + }, + rcl_action_server_get_default_options(), action_callback_group_); + + command_list_manager_ = std::make_unique( + context_->moveit_cpp_->getNode(), 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()); + auto node = context_->moveit_cpp_->getNode(); + context_->planning_scene_monitor_->waitForCurrentRobotState(node->get_clock()->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 +129,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 +168,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 +194,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,49 +214,61 @@ 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_->moveit_cpp_->getNode()->now(); RobotTrajCont traj_vec; try { - traj_vec = command_list_manager_->solve(the_scene, context_->planning_pipeline_, goal->request); + // Select planning_pipeline to handle request + // All motions in the SequenceRequest need to use the same planning pipeline (but can use different planners) + const planning_pipeline::PlanningPipelinePtr planning_pipeline = + resolvePlanningPipeline(goal->request.items[0].req.pipeline_id); + if (!planning_pipeline) + { + RCLCPP_ERROR_STREAM(LOGGER, "Could not load planning pipeline " << goal->request.items[0].req.pipeline_id); + action_res->response.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; + return; + } + + traj_vec = command_list_manager_->solve(the_scene, planning_pipeline, goal->request); } 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_->moveit_cpp_->getNode()->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); @@ -247,7 +283,7 @@ bool MoveGroupSequenceAction::planUsingSequenceManager(const moveit_msgs::Motion resolvePlanningPipeline(req.items[0].req.pipeline_id); if (!planning_pipeline) { - ROS_ERROR_STREAM("Could not load planning pipeline " << req.items[0].req.pipeline_id); + RCLCPP_ERROR_STREAM(LOGGER, "Could not load planning pipeline " << req.items[0].req.pipeline_id); return false; } @@ -255,15 +291,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 +314,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 +331,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..7235462b1e 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 @@ -42,8 +42,11 @@ #include "pilz_industrial_motion_planner/command_list_manager.h" #include "pilz_industrial_motion_planner/trajectory_generation_exceptions.h" +#include 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") { } @@ -54,48 +57,69 @@ MoveGroupSequenceService::~MoveGroupSequenceService() void MoveGroupSequenceService::initialize() { - command_list_manager_.reset(new pilz_industrial_motion_planner::CommandListManager( - ros::NodeHandle("~"), context_->planning_scene_monitor_->getRobotModel())); + command_list_manager_ = std::make_unique( + context_->moveit_cpp_->getNode(), context_->planning_scene_monitor_->getRobotModel()); - sequence_service_ = root_node_handle_.advertiseService(SEQUENCE_SERVICE_NAME, &MoveGroupSequenceService::plan, this); + sequence_service_ = context_->moveit_cpp_->getNode()->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) { + // Handle empty requests + if (req->request.items.empty()) + { + RCLCPP_WARN(LOGGER, "Received empty request. That's ok but maybe not what you intended."); + res->response.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + return true; + } + // 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_->moveit_cpp_->getNode()->now(); RobotTrajCont traj_vec; try { - traj_vec = command_list_manager_->solve(ps, context_->planning_pipeline_, req.request); + // Select planning_pipeline to handle request + // All motions in the SequenceRequest need to use the same planning pipeline (but can use different planners) + const planning_pipeline::PlanningPipelinePtr planning_pipeline = + resolvePlanningPipeline(req->request.items[0].req.pipeline_id); + if (!planning_pipeline) + { + RCLCPP_ERROR_STREAM(LOGGER, "Could not load planning pipeline " << req->request.items[0].req.pipeline_id); + res->response.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; + return false; + } + + 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_->moveit_cpp_->getNode()->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..940c8b8eaf 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" @@ -50,12 +52,14 @@ namespace pilz_industrial_motion_planner { -static const std::string PARAM_NAMESPACE_LIMTS = "robot_description_planning"; +static const std::string PARAM_NAMESPACE_LIMITS = "robot_description_planning"; +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner"); -bool CommandPlanner::initialize(const moveit::core::RobotModelConstPtr& model, const 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,15 +67,15 @@ 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_LIMITS, 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_LIMITS); // Load the planning context loader - planner_context_loader.reset(new pluginlib::ClassLoader( - "pilz_industrial_motion_planner", "pilz_industrial_motion_planner::PlanningContextLoader")); + planner_context_loader = std::make_unique>( + "pilz_industrial_motion_planner", "pilz_industrial_motion_planner::PlanningContextLoader"); // List available plugins const std::vector& factories = planner_context_loader->getDeclaredClasses(); @@ -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/plan_components_builder.cpp b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp index 175a7deca9..fe078b2ef4 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp @@ -68,7 +68,8 @@ void PlanComponentsBuilder::appendWithStrictTimeIncrease(robot_trajectory::Robot } } -void PlanComponentsBuilder::blend(const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius) +void PlanComponentsBuilder::blend(const planning_scene::PlanningSceneConstPtr& planning_scene, + const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius) { if (!blender_) { @@ -86,7 +87,7 @@ void PlanComponentsBuilder::blend(const robot_trajectory::RobotTrajectoryPtr& ot blend_request.link_name = getSolverTipFrame(model_->getJointModelGroup(blend_request.group_name)); pilz_industrial_motion_planner::TrajectoryBlendResponse blend_response; - if (!blender_->blend(blend_request, blend_response)) + if (!blender_->blend(planning_scene, blend_request, blend_response)) { throw BlendingFailedException("Blending failed"); } @@ -98,7 +99,8 @@ void PlanComponentsBuilder::blend(const robot_trajectory::RobotTrajectoryPtr& ot traj_tail_ = blend_response.second_trajectory; // first for next blending segment } -void PlanComponentsBuilder::append(const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius) +void PlanComponentsBuilder::append(const planning_scene::PlanningSceneConstPtr& planning_scene, + const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius) { if (!model_) { @@ -131,7 +133,7 @@ void PlanComponentsBuilder::append(const robot_trajectory::RobotTrajectoryPtr& o return; } - blend(other, blend_radius); + blend(planning_scene, other, blend_radius); } } // namespace pilz_industrial_motion_planner 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..fb603e8f91 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,6 @@ *********************************************************************/ #include "pilz_industrial_motion_planner/planning_context_loader.h" -#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..a281a2116c 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,12 @@ #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"; @@ -53,19 +61,18 @@ bool pilz_industrial_motion_planner::PlanningContextLoaderCIRC::loadContext( { if (limits_set_ && model_set_) { - planning_context.reset(new PlanningContextCIRC(name, group, model_, limits_)); + planning_context = std::make_shared(name, group, model_, limits_); return true; } else { 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..a8f1458448 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,12 @@ #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"; @@ -53,19 +59,18 @@ bool pilz_industrial_motion_planner::PlanningContextLoaderLIN::loadContext( { if (limits_set_ && model_set_) { - planning_context.reset(new PlanningContextLIN(name, group, model_, limits_)); + planning_context = std::make_shared(name, group, model_, limits_); return true; } else { 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..437d881e7a 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"; @@ -52,19 +58,19 @@ bool pilz_industrial_motion_planner::PlanningContextLoaderPTP::loadContext( { if (limits_set_ && model_set_) { - planning_context.reset(new PlanningContextPTP(name, group, model_, limits_)); + planning_context = std::make_shared(name, group, model_, limits_); return true; } else { 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 4f1154f733..8f22632585 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 @@ -35,23 +35,32 @@ #include "pilz_industrial_motion_planner/trajectory_blender_transition_window.h" #include +#include #include #if __has_include() #include #else #include #endif +#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 planning_scene::PlanningSceneConstPtr& planning_scene, 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; } @@ -62,8 +71,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; } @@ -87,26 +96,26 @@ 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; - 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)) + trajectory_msgs::msg::JointTrajectory blend_joint_trajectory; + moveit_msgs::msg::MoveItErrorCodes error_code; + + if (!generateJointTrajectory(planning_scene, 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 } - res.first_trajectory = std::shared_ptr( - new robot_trajectory::RobotTrajectory(req.first_trajectory->getRobotModel(), req.first_trajectory->getGroup())); - res.blend_trajectory = std::shared_ptr( - new robot_trajectory::RobotTrajectory(req.first_trajectory->getRobotModel(), req.first_trajectory->getGroup())); - res.second_trajectory = std::shared_ptr( - new robot_trajectory::RobotTrajectory(req.first_trajectory->getRobotModel(), req.first_trajectory->getGroup())); + res.first_trajectory = std::make_shared(req.first_trajectory->getRobotModel(), + req.first_trajectory->getGroup()); + res.blend_trajectory = std::make_shared(req.first_trajectory->getRobotModel(), + req.first_trajectory->getGroup()); + res.second_trajectory = std::make_shared(req.first_trajectory->getRobotModel(), + req.first_trajectory->getGroup()); // set the three trajectories after blending in response // erase the points [first_intersection_index, back()] from the first @@ -129,21 +138,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; } @@ -151,15 +160,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; } @@ -168,8 +177,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; } @@ -177,7 +187,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; } @@ -189,9 +199,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; } @@ -251,7 +260,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); } } @@ -260,7 +269,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) @@ -270,19 +279,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 466cc70fdc..767973a045 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -36,57 +36,57 @@ #include #include + +#include #if __has_include() #include -#else -#include -#endif -#if __has_include() -#include -#else -#include -#endif -#if __has_include() #include #else +#include #include #endif -bool pilz_industrial_motion_planner::computePoseIK(const moveit::core::RobotModelConstPtr& robot_model, +namespace +{ +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_functions"); +} + +bool pilz_industrial_motion_planner::computePoseIK(const planning_scene::PlanningSceneConstPtr& scene, 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, const double timeout) { + const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel(); 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, scene, + 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)) @@ -100,21 +100,23 @@ 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, +bool pilz_industrial_motion_planner::computePoseIK(const planning_scene::PlanningSceneConstPtr& scene, 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); - return computePoseIK(robot_model, group_name, link_name, pose_eigen, frame_id, seed, solution, check_self_collision, + tf2::convert(pose, pose_eigen); + return computePoseIK(scene, group_name, link_name, pose_eigen, frame_id, seed, solution, check_self_collision, timeout); } @@ -123,12 +125,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; } @@ -151,7 +153,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; } @@ -163,10 +165,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; } @@ -177,10 +179,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; } } @@ -190,10 +193,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; } } @@ -203,16 +207,18 @@ bool pilz_industrial_motion_planner::verifySampleJointLimits( } bool pilz_industrial_motion_planner::generateJointTrajectory( - const moveit::core::RobotModelConstPtr& robot_model, + const planning_scene::PlanningSceneConstPtr& scene, 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(); + const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel(); + 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 @@ -235,14 +241,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, + if (!computePoseIK(scene, 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; } @@ -264,15 +269,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(); @@ -281,7 +287,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)); @@ -308,26 +314,29 @@ 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; } bool pilz_industrial_motion_planner::generateJointTrajectory( - const moveit::core::RobotModelConstPtr& robot_model, + const planning_scene::PlanningSceneConstPtr& scene, 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(); + const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel(); + 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; @@ -342,12 +351,12 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( for (size_t i = 0; i < trajectory.points.size(); ++i) { // compute inverse kinematics - if (!computePoseIK(robot_model, group_name, link_name, trajectory.points.at(i).pose, robot_model->getModelFrame(), + if (!computePoseIK(scene, 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; } @@ -355,13 +364,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, @@ -371,18 +380,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 = 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)); @@ -400,12 +410,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; } @@ -420,8 +430,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; } @@ -440,8 +449,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; } } @@ -450,8 +460,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; } } @@ -471,8 +482,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; } @@ -483,8 +494,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; } @@ -495,8 +506,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; } @@ -510,13 +521,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; @@ -528,7 +539,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(); @@ -568,9 +579,9 @@ 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, + const planning_scene::PlanningSceneConstPtr& scene, + moveit::core::RobotState* rstate, + const moveit::core::JointModelGroup* const group, const double* const ik_solution) { if (!test_for_self_collision) @@ -582,15 +593,15 @@ bool pilz_industrial_motion_planner::isStateColliding(const bool test_for_self_c rstate->update(); collision_detection::CollisionRequest collision_req; collision_req.group_name = group->getName(); + collision_req.verbose = true; collision_detection::CollisionResult collision_res; - planning_scene::PlanningScene(robot_model).checkSelfCollision(collision_req, collision_res, *rstate); - + scene->checkSelfCollision(collision_req, collision_res, *rstate); 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..6a6f091376 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,13 @@ #include +#if __has_include() +#include +#include +#else +#include +#include +#endif #include #include @@ -43,6 +50,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 +89,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 +115,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 +146,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 +187,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 +197,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,37 +222,27 @@ 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, - robot_trajectory::RobotTrajectory& robot_trajectory) const -{ - moveit::core::RobotState start_rs(robot_model_); - start_rs.setToDefaultValues(); - moveit::core::robotStateMsgToRobotState(start_state, start_rs, false); - 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 moveit::core::RobotState& start_state, const std::string& group_name, + 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); + rt->setRobotTrajectoryMsg(start_state, joint_trajectory); 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 @@ -267,11 +265,12 @@ TrajectoryGenerator::cartesianTrapVelocityProfile(const double& max_velocity_sca return vp_trans; } -bool TrajectoryGenerator::generate(const planning_interface::MotionPlanRequest& req, +bool TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& scene, + 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 +278,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 +290,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; @@ -300,30 +299,32 @@ bool TrajectoryGenerator::generate(const planning_interface::MotionPlanRequest& MotionPlanInfo plan_info; try { - extractMotionPlanInfo(req, plan_info); + extractMotionPlanInfo(scene, req, plan_info); } 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); + plan(scene, 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; } - setSuccessResponse(req.group_name, req.start_state, joint_trajectory, planning_begin, res); + moveit::core::RobotState start_state(scene->getCurrentState()); + moveit::core::robotStateMsgToRobotState(req.start_state, start_state, true); + setSuccessResponse(start_state, req.group_name, joint_trajectory, planning_begin, res); return true; } 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 feeb891e75..f5f6e00a29 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,25 +43,21 @@ #include #include #include -#include +#include + +#include #if __has_include() #include -#else -#include -#endif -#if __has_include() -#include -#else -#include -#endif -#if __has_include() #include #else +#include #include #endif 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) @@ -96,10 +92,11 @@ void TrajectoryGeneratorCIRC::cmdSpecificRequestValidation(const planning_interf } } -void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, +void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, + 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() }; @@ -144,20 +141,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()); @@ -179,7 +176,7 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_interface::Mo // check goal pose ik before Cartesian motion plan starts std::map ik_solution; - if (!computePoseIK(robot_model_, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, + if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, ik_solution)) { // LCOV_EXCL_START @@ -198,8 +195,9 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_interface::Mo info.circ_path_point.second = circ_path_point; } -void TrajectoryGeneratorCIRC::plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, - const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) +void TrajectoryGeneratorCIRC::plan(const planning_scene::PlanningSceneConstPtr& scene, + const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, + const double& sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) { std::unique_ptr cart_path(setPathCIRC(plan_info)); std::unique_ptr vel_profile( @@ -211,12 +209,12 @@ 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, - plan_info.group_name, plan_info.link_name, plan_info.start_joint_position, sampling_time, - joint_trajectory, error_code)) + if (!generateJointTrajectory(scene, planner_limits_.getJointLimitContainer(), cart_trajectory, plan_info.group_name, + plan_info.link_name, plan_info.start_joint_position, sampling_time, joint_trajectory, + error_code)) { throw CircTrajectoryConversionFailure("Failed to generate valid joint trajectory from the Cartesian path", error_code.val); @@ -225,11 +223,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 d158c0d0e7..d483ae4d3e 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,39 +45,36 @@ #include #include -#if __has_include() -#include -#else -#include -#endif +#include +#include #if __has_include() #include -#else -#include -#endif -#if __has_include() #include #else +#include #include #endif 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."); } } -void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, +void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, + 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() }; @@ -119,20 +116,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()); @@ -156,7 +153,7 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_interface::Mot // check goal pose ik before Cartesian motion plan starts std::map ik_solution; - if (!computePoseIK(robot_model_, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, + if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, ik_solution)) { std::ostringstream os; @@ -165,8 +162,9 @@ 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) +void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& scene, + const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, + 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)); @@ -181,12 +179,12 @@ 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, - plan_info.group_name, plan_info.link_name, plan_info.start_joint_position, sampling_time, - joint_trajectory, error_code)) + if (!generateJointTrajectory(scene, planner_limits_.getJointLimitContainer(), cart_trajectory, plan_info.group_name, + plan_info.link_name, plan_info.start_joint_position, sampling_time, joint_trajectory, + error_code)) { std::ostringstream os; os << "Failed to generate valid joint trajectory from the Cartesian path"; @@ -197,11 +195,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 da9cce40c6..c009488317 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 @@ -52,7 +52,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) { @@ -66,7 +68,7 @@ TrajectoryGeneratorPTP::TrajectoryGeneratorPTP(const robot_model::RobotModelCons // collect most strict joint limits for each group in robot model for (const auto& jmg : robot_model->getJointModelGroups()) { - auto active_joints = jmg->getActiveJointModelNames(); + const auto& active_joints = jmg->getActiveJointModelNames(); // no active joints if (active_joints.empty()) @@ -78,31 +80,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) @@ -122,11 +124,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)); @@ -203,8 +205,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)); @@ -220,7 +222,8 @@ void TrajectoryGeneratorPTP::planPTP(const std::map& start_ 0.0); } -void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, +void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, + const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const { info.group_name = req.group_name; @@ -244,19 +247,19 @@ 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); - if (!computePoseIK(robot_model_, req.group_name, req.goal_constraints.at(0).position_constraints.at(0).link_name, + tf2::convert(pose, pose_eigen); + if (!computePoseIK(scene, 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)) { throw PtpNoIkSolutionForGoalPose("No IK solution for goal pose"); @@ -264,8 +267,9 @@ 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) +void TrajectoryGeneratorPTP::plan(const planning_scene::PlanningSceneConstPtr& scene, + const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, + 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/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/test/CMakeLists.txt new file mode 100644 index 0000000000..79c8e0d015 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/CMakeLists.txt @@ -0,0 +1,31 @@ +find_package(ament_cmake_gtest REQUIRED) +find_package(ament_cmake_gmock REQUIRED) +find_package(ros_testing REQUIRED) +find_package(moveit_resources_prbt_support REQUIRED) +find_package(moveit_resources_prbt_moveit_config REQUIRED) +find_package(Boost REQUIRED thread) + +find_package(${PROJECT_NAME}_testutils REQUIRED) + +# Add test directory as include directory +include_directories( + ${CMAKE_CURRENT_SOURCE_DIR} +) + +# pilz_industrial_motion_testhelpers +add_library(${PROJECT_NAME}_test_utils + test_utils.cpp +) +ament_target_dependencies(${PROJECT_NAME}_test_utils ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(${PROJECT_NAME}_test_utils + joint_limits_common + trajectory_generation_common +) + +# Unit tests +add_subdirectory(unit_tests) + +# Integration tests +add_subdirectory(integration_tests) +# Install test data +install(DIRECTORY test_data DESTINATION share/${PROJECT_NAME}) diff --git a/moveit_planners/chomp/COLCON_IGNORE b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/CMakeLists.txt similarity index 100% rename from moveit_planners/chomp/COLCON_IGNORE rename to moveit_planners/pilz_industrial_motion_planner/test/integration_tests/CMakeLists.txt diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.test b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_command_list_manager.test similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.test rename to moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_command_list_manager.test diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.test b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_command_planning.test similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.test rename to moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_command_planning.test diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_frankaemika_panda.test b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_command_planning_frankaemika_panda.test similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_frankaemika_panda.test rename to moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_command_planning_frankaemika_panda.test diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_with_gripper.test b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_command_planning_with_gripper.test similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_with_gripper.test rename to moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_command_planning_with_gripper.test diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_get_solver_tip_frame.test b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_get_solver_tip_frame.test similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/test/integrationtest_get_solver_tip_frame.test rename to moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_get_solver_tip_frame.test diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.test b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_plan_components_builder.test similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.test rename to moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_plan_components_builder.test diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.test b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_action.test similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.test rename to moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_action.test diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_frankaemika_panda.test b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_action_capability_frankaemika_panda.test similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_frankaemika_panda.test rename to moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_action_capability_frankaemika_panda.test diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_with_gripper.test b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_action_capability_with_gripper.test similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_with_gripper.test rename to moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_action_capability_with_gripper.test diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.test b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_action_preemption.test similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.test rename to moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_action_preemption.test diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.test b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_service_capability.test similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.test rename to moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_service_capability.test diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_frankaemika_panda.test b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_service_capability_frankaemika_panda.test similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_frankaemika_panda.test rename to moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_service_capability_frankaemika_panda.test diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_with_gripper.test b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_service_capability_with_gripper.test similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_with_gripper.test rename to moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/integrationtest_sequence_service_capability_with_gripper.test diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_command_list_manager.cpp similarity index 94% rename from moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_command_list_manager.cpp index 9483c4bc8d..1931b1aecf 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_command_list_manager.cpp @@ -117,8 +117,8 @@ void IntegrationTestCommandListManager::SetUp() ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name)); // load the test data provider - data_loader_.reset( - new pilz_industrial_motion_planner_testutils::XmlTestdataLoader{ test_data_file_name, robot_model_ }); + data_loader_ = + std::make_unique(test_data_file_name, robot_model_); ASSERT_NE(nullptr, data_loader_) << "Failed to load test data by provider."; // Define and set the current scene and manager test object @@ -135,19 +135,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); } /** @@ -183,7 +183,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.; @@ -194,7 +194,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.; @@ -205,7 +205,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.; @@ -338,7 +338,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); @@ -346,12 +346,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); @@ -372,7 +372,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()); } @@ -409,7 +409,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); } @@ -525,18 +525,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/integration_tests/src/integrationtest_command_planning.cpp similarity index 92% rename from moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_command_planning.cpp index 7c6c49c41a..0eba81911a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_command_planning.cpp @@ -65,7 +65,7 @@ const double EPSILON = 1.0e-6; const std::string PLAN_SERVICE_NAME = "/plan_kinematic_path"; -// Parameters from parameter server +// Parameters from the node const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); const std::string POSE_TRANSFORM_MATRIX_NORM_TOLERANCE("pose_norm_tolerance"); const std::string ORIENTATION_NORM_TOLERANCE("orientation_norm_tolerance"); @@ -114,8 +114,8 @@ void IntegrationTestCommandPlanning::SetUp() ASSERT_TRUE(ros::service::waitForService(PLAN_SERVICE_NAME, ros::Duration(testutils::DEFAULT_SERVICE_TIMEOUT))); // load the test data provider - test_data_.reset( - new pilz_industrial_motion_planner_testutils::XmlTestdataLoader{ test_data_file_name_, robot_model_ }); + test_data_ = + std::make_unique(test_data_file_name_, robot_model_); ASSERT_NE(nullptr, test_data_) << "Failed to load test data by provider."; num_joints_ = robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames().size(); @@ -137,16 +137,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"; @@ -194,10 +194,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; @@ -259,9 +259,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; @@ -310,9 +310,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; @@ -349,7 +349,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; @@ -357,10 +357,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"; @@ -433,17 +433,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_get_solver_tip_frame.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_get_solver_tip_frame.cpp similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/test/integrationtest_get_solver_tip_frame.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_get_solver_tip_frame.cpp diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_plan_components_builder.cpp similarity index 86% rename from moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_plan_components_builder.cpp index 43f0415771..81a8eeba9e 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_plan_components_builder.cpp @@ -60,6 +60,7 @@ class IntegrationTestPlanComponentBuilder : public testing::Test protected: ros::NodeHandle ph_{ "~" }; robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(ROBOT_DESCRIPTION_STR).getModel() }; + planning_scene::PlanningSceneConstPtr planning_scene_{ new planning_scene::PlanningScene(robot_model_) }; std::string planning_group_; }; @@ -82,16 +83,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); } /** @@ -103,7 +104,7 @@ TEST_F(IntegrationTestPlanComponentBuilder, TestModelSet) robot_trajectory::RobotTrajectoryPtr traj{ new robot_trajectory::RobotTrajectory(robot_model_, planning_group_) }; PlanComponentsBuilder builder; - EXPECT_THROW(builder.append(traj, 1.0), NoRobotModelSetException); + EXPECT_THROW(builder.append(planning_scene_, traj, 1.0), NoRobotModelSetException); } /** @@ -116,9 +117,9 @@ TEST_F(IntegrationTestPlanComponentBuilder, TestNoBlenderSet) PlanComponentsBuilder builder; builder.setModel(robot_model_); - builder.append(traj, 0.0); + builder.append(planning_scene_, traj, 0.0); - EXPECT_THROW(builder.append(traj, 1.0), NoBlenderSetException); + EXPECT_THROW(builder.append(planning_scene_, traj, 1.0), NoBlenderSetException); } int main(int argc, char** argv) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_sequence_action.cpp similarity index 81% rename from moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_sequence_action.cpp index a5883e71cc..4d15492136 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_sequence_action.cpp @@ -64,7 +64,7 @@ static constexpr int WAIT_FOR_ACTION_SERVER_TIME_OUT{ 10 }; // seconds const std::string SEQUENCE_ACTION_NAME("/sequence_move_group"); -// Parameters from parameter server +// Parameters from the node const std::string JOINT_POSITION_TOLERANCE("joint_position_tolerance"); // events for callback tests @@ -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_; @@ -113,7 +113,7 @@ void IntegrationTestSequenceAction::SetUp() robot_model_ = model_loader_.getModel(); - data_loader_.reset(new XmlTestdataLoader(test_data_file_name_, robot_model_)); + data_loader_ = std::make_unique(test_data_file_name_, robot_model_); ASSERT_NE(nullptr, data_loader_) << "Failed to load test data by provider."; // wait for action server @@ -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/integration_tests/src/integrationtest_sequence_action_preemption.cpp similarity index 90% rename from moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_sequence_action_preemption.cpp index bf9198d52a..8ef2fc37aa 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_sequence_action_preemption.cpp @@ -67,7 +67,7 @@ static constexpr double WAIT_FOR_ACTION_SERVER_TIME_OUT{ 10. }; // seconds const std::string SEQUENCE_ACTION_NAME("/sequence_move_group"); -// Parameters from parameter server +// Parameters from the node const std::string JOINT_POSITION_TOLERANCE("joint_position_tolerance"); // events for callback tests @@ -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_; @@ -116,7 +116,7 @@ void IntegrationTestSequenceAction::SetUp() robot_model_ = model_loader_.getModel(); - data_loader_.reset(new XmlTestdataLoader(test_data_file_name_, robot_model_)); + data_loader_ = std::make_unique(test_data_file_name_, robot_model_); ASSERT_NE(nullptr, data_loader_) << "Failed to load test data by provider."; // wait for action server @@ -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/integration_tests/src/integrationtest_sequence_service_capability.cpp similarity index 88% rename from moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_sequence_service_capability.cpp index 02f4bf5aa9..2b46292a98 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_sequence_service_capability.cpp @@ -56,7 +56,7 @@ #include "moveit_msgs/MotionSequenceRequest.h" #include "pilz_industrial_motion_planner/capability_names.h" -// Parameters from parameter server +// Parameters from the node const std::string TEST_DATA_FILE_NAME("testdata_file_name"); using namespace pilz_industrial_motion_planner_testutils; @@ -88,7 +88,7 @@ void IntegrationTestSequenceService::SetUp() robot_model_loader::RobotModelLoader model_loader; robot_model_ = model_loader.getModel(); - data_loader_.reset(new XmlTestdataLoader(test_data_file_name_, robot_model_)); + data_loader_ = std::make_unique(test_data_file_name_, robot_model_); ASSERT_NE(nullptr, data_loader_) << "Failed to load test data by provider."; ASSERT_TRUE(ros::service::waitForService(pilz_industrial_motion_planner::SEQUENCE_SERVICE_NAME, ros::Duration(10))) @@ -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_robots/concept_testdata.odp b/moveit_planners/pilz_industrial_motion_planner/test/test_data/concept_testdata.odp similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/test/test_robots/concept_testdata.odp rename to moveit_planners/pilz_industrial_motion_planner/test/test_data/concept_testdata.odp diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/concept_testdata.png b/moveit_planners/pilz_industrial_motion_planner/test/test_data/concept_testdata.png similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/test/test_robots/concept_testdata.png rename to moveit_planners/pilz_industrial_motion_planner/test/test_data/concept_testdata.png diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/test_data/testdata_sequence.xml b/moveit_planners/pilz_industrial_motion_planner/test/test_data/frankaemika_panda/testdata_sequence.xml similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/test_data/testdata_sequence.xml rename to moveit_planners/pilz_industrial_motion_planner/test/test_data/frankaemika_panda/testdata_sequence.xml diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_deprecated.xml b/moveit_planners/pilz_industrial_motion_planner/test/test_data/prbt/testdata_deprecated.xml similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_deprecated.xml rename to moveit_planners/pilz_industrial_motion_planner/test/test_data/prbt/testdata_deprecated.xml diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_sequence.xml b/moveit_planners/pilz_industrial_motion_planner/test/test_data/prbt/testdata_sequence.xml similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_sequence.xml rename to moveit_planners/pilz_industrial_motion_planner/test/test_data/prbt/testdata_sequence.xml diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_with_gripper.xml b/moveit_planners/pilz_industrial_motion_planner/test/test_data/prbt/testdata_with_gripper.xml similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_with_gripper.xml rename to moveit_planners/pilz_industrial_motion_planner/test/test_data/prbt/testdata_with_gripper.xml diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/testpoints.py b/moveit_planners/pilz_industrial_motion_planner/test/test_data/testpoints.py similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/test/test_robots/testpoints.py rename to moveit_planners/pilz_industrial_motion_planner/test/test_data/testpoints.py diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch deleted file mode 100644 index f8ccaa62a3..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch +++ /dev/null @@ -1,77 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [/move_group/fake_controller_joint_states] - - - - - - - - - - - - - - diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/launch/test_context.launch b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/launch/test_context.launch deleted file mode 100644 index 49ac94b65d..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/launch/test_context.launch +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_cartesian_limits_aggregator/test_cartesian_limit_all.yaml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_cartesian_limits_aggregator/test_cartesian_limit_all.yaml deleted file mode 100644 index bd165e6ebc..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_cartesian_limits_aggregator/test_cartesian_limit_all.yaml +++ /dev/null @@ -1,37 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2018 Pilz GmbH & Co. KG -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Pilz GmbH & Co. KG nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -cartesian_limits: - max_trans_vel: 1 - max_trans_acc: 2 - max_trans_dec: -3 - max_rot_vel: 4 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_cartesian_limits_aggregator/test_cartesian_limit_only_vel.yaml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_cartesian_limits_aggregator/test_cartesian_limit_only_vel.yaml deleted file mode 100644 index 4ac9ff1ae8..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_cartesian_limits_aggregator/test_cartesian_limit_only_vel.yaml +++ /dev/null @@ -1,34 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2018 Pilz GmbH & Co. KG -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Pilz GmbH & Co. KG nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -cartesian_limits: - max_trans_vel: 10 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_valid_1.yaml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_valid_1.yaml deleted file mode 100644 index dd953add3d..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_valid_1.yaml +++ /dev/null @@ -1,52 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2018 Pilz GmbH & Co. KG -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Pilz GmbH & Co. KG nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -joint_limits: - prbt_joint_1: - has_position_limits: true - min_position: -2 - max_position: 2 - prbt_joint_2: - has_position_limits: false - min_position: -2 - max_position: 2 - prbt_joint_3: - has_velocity_limits: true - max_velocity: 1.1 - has_acceleration_limits: false - max_acceleration: 5.5 - prbt_joint_4: - has_acceleration_limits: true - max_acceleration: 5.5 - prbt_joint_5: - has_deceleration_limits: true - max_deceleration: -6.6 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_position_max.yaml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_position_max.yaml deleted file mode 100644 index 7aecb01bc7..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_position_max.yaml +++ /dev/null @@ -1,37 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2018 Pilz GmbH & Co. KG -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Pilz GmbH & Co. KG nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -joint_limits: - prbt_joint_3: - has_position_limits: true - min_position: -1 - max_position: 4 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_position_min.yaml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_position_min.yaml deleted file mode 100644 index 50612ed284..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_position_min.yaml +++ /dev/null @@ -1,37 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2018 Pilz GmbH & Co. KG -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Pilz GmbH & Co. KG nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -joint_limits: - prbt_joint_2: - has_position_limits: true - min_position: -4 - max_position: 2 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_velocity.yaml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_velocity.yaml deleted file mode 100644 index 8373e05b28..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_velocity.yaml +++ /dev/null @@ -1,36 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2018 Pilz GmbH & Co. KG -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Pilz GmbH & Co. KG nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -joint_limits: - prbt_joint_3: - has_velocity_limits: true - max_velocity: -90 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 cb54ed4aa8..e7109fb2ee 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp @@ -46,8 +46,12 @@ #include #endif +#include #include "test_utils.h" +// Logger +static const rclcpp::Logger LOGGER = rclcpp::get_logger("pilz_industrial_motion_planner.test_utils"); + pilz_industrial_motion_planner::JointLimitsContainer testutils::createFakeLimits(const std::vector& joint_names) { @@ -110,7 +114,7 @@ bool testutils::getExpectedGoalPose(const moveit::core::RobotModelConstPtr& robo // ++++++++++++++++++++++++++++++++++++++ // TODO frame id link_name = req.goal_constraints.front().position_constraints.front().link_name; - 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; @@ -119,11 +123,11 @@ bool testutils::getExpectedGoalPose(const moveit::core::RobotModelConstPtr& robo return true; } -bool testutils::isGoalReached(const trajectory_msgs::JointTrajectory& trajectory, - const std::vector& goal, +bool testutils::isGoalReached(const trajectory_msgs::msg::JointTrajectory& trajectory, + const std::vector& goal, const double joint_position_tolerance, const double joint_velocity_tolerance) { - trajectory_msgs::JointTrajectoryPoint last_point = trajectory.points.back(); + trajectory_msgs::msg::JointTrajectoryPoint last_point = trajectory.points.back(); for (unsigned int i = 0; i < trajectory.joint_names.size(); ++i) { @@ -155,7 +159,7 @@ bool testutils::isGoalReached(const trajectory_msgs::JointTrajectory& trajectory } bool testutils::isGoalReached(const moveit::core::RobotModelConstPtr& robot_model, - const trajectory_msgs::JointTrajectory& trajectory, + const trajectory_msgs::msg::JointTrajectory& trajectory, const planning_interface::MotionPlanRequest& req, const double pos_tolerance, const double orientation_tolerance) { @@ -167,7 +171,7 @@ bool testutils::isGoalReached(const moveit::core::RobotModelConstPtr& robot_mode } // compute the actual goal pose in model frame - trajectory_msgs::JointTrajectoryPoint last_point = trajectory.points.back(); + trajectory_msgs::msg::JointTrajectoryPoint last_point = trajectory.points.back(); Eigen::Isometry3d goal_pose_actual; std::map joint_state; @@ -217,14 +221,14 @@ bool testutils::isGoalReached(const moveit::core::RobotModelConstPtr& robot_mode } bool testutils::isGoalReached(const moveit::core::RobotModelConstPtr& robot_model, - const trajectory_msgs::JointTrajectory& trajectory, + const trajectory_msgs::msg::JointTrajectory& trajectory, const planning_interface::MotionPlanRequest& req, const double tolerance) { return isGoalReached(robot_model, trajectory, req, tolerance, tolerance); } bool testutils::checkCartesianLinearity(const moveit::core::RobotModelConstPtr& robot_model, - const trajectory_msgs::JointTrajectory& trajectory, + const trajectory_msgs::msg::JointTrajectory& trajectory, const planning_interface::MotionPlanRequest& req, const double translation_norm_tolerance, const double rot_axis_norm_tolerance, const double rot_angle_tolerance) @@ -237,7 +241,7 @@ bool testutils::checkCartesianLinearity(const moveit::core::RobotModelConstPtr& } // compute start pose - robot_state::RobotState rstate(robot_model); + moveit::core::RobotState rstate(robot_model); rstate.setToDefaultValues(); moveit::core::jointStateToRobotState(req.start_state.joint_state, rstate); Eigen::Isometry3d start_pose = rstate.getFrameTransform(link_name); @@ -246,7 +250,7 @@ bool testutils::checkCartesianLinearity(const moveit::core::RobotModelConstPtr& Eigen::AngleAxisd start_goal_aa(start_pose.rotation().transpose() * goal_pose_expect.rotation()); // check the linearity - for (trajectory_msgs::JointTrajectoryPoint way_point : trajectory.points) + for (trajectory_msgs::msg::JointTrajectoryPoint way_point : trajectory.points) { Eigen::Isometry3d way_point_pose; std::map way_point_joint_state; @@ -326,13 +330,13 @@ bool testutils::computeLinkFK(const moveit::core::RobotModelConstPtr& robot_mode const std::map& joint_state, Eigen::Isometry3d& pose) { // create robot state - robot_state::RobotState rstate(robot_model); + moveit::core::RobotState rstate(robot_model); rstate.setToDefaultValues(); // 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; } @@ -354,7 +358,7 @@ bool testutils::computeLinkFK(const moveit::core::RobotModelConstPtr& robot_mode return true; } -bool testutils::isVelocityBounded(const trajectory_msgs::JointTrajectory& trajectory, +bool testutils::isVelocityBounded(const trajectory_msgs::msg::JointTrajectory& trajectory, const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) { for (const auto& point : trajectory.points) @@ -366,7 +370,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: " << rclcpp::Duration(point.time_from_start).seconds() << "; Velocity Limit: " << joint_limits.getLimit(trajectory.joint_names.at(i)).max_velocity << std::endl; @@ -378,7 +382,7 @@ bool testutils::isVelocityBounded(const trajectory_msgs::JointTrajectory& trajec return true; } -bool testutils::isTrajectoryConsistent(const trajectory_msgs::JointTrajectory& trajectory) +bool testutils::isTrajectoryConsistent(const trajectory_msgs::msg::JointTrajectory& trajectory) { for (const auto& point : trajectory.points) { @@ -395,7 +399,7 @@ bool testutils::isTrajectoryConsistent(const trajectory_msgs::JointTrajectory& t return true; } -bool testutils::isAccelerationBounded(const trajectory_msgs::JointTrajectory& trajectory, +bool testutils::isAccelerationBounded(const trajectory_msgs::msg::JointTrajectory& trajectory, const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) { for (const auto& point : trajectory.points) @@ -410,7 +414,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: " << rclcpp::Duration(point.time_from_start).seconds() << ". Deceleration Limit: " << joint_limits.getLimit(trajectory.joint_names.at(i)).max_deceleration << std::endl; return false; @@ -424,7 +428,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: " << rclcpp::Duration(point.time_from_start).seconds() << ". Acceleration Limit: " << joint_limits.getLimit(trajectory.joint_names.at(i)).max_acceleration << std::endl; @@ -437,7 +441,7 @@ bool testutils::isAccelerationBounded(const trajectory_msgs::JointTrajectory& tr return true; } -bool testutils::isPositionBounded(const trajectory_msgs::JointTrajectory& trajectory, +bool testutils::isPositionBounded(const trajectory_msgs::msg::JointTrajectory& trajectory, const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) { for (const auto& point : trajectory.points) @@ -450,7 +454,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: " << rclcpp::Duration(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; @@ -463,7 +467,7 @@ bool testutils::isPositionBounded(const trajectory_msgs::JointTrajectory& trajec return true; } -bool testutils::checkJointTrajectory(const trajectory_msgs::JointTrajectory& trajectory, +bool testutils::checkJointTrajectory(const trajectory_msgs::msg::JointTrajectory& trajectory, const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) { if (!testutils::isTrajectoryConsistent(trajectory)) @@ -514,13 +518,13 @@ void testutils::createDummyRequest(const moveit::core::RobotModelConstPtr& robot req.group_name = planning_group; req.max_velocity_scaling_factor = 1.0; req.max_acceleration_scaling_factor = 1.0; - robot_state::RobotState rstate(robot_model); + moveit::core::RobotState rstate(robot_model); rstate.setToDefaultValues(); moveit::core::robotStateToRobotStateMsg(rstate, req.start_state, false); } -void testutils::createPTPRequest(const std::string& planning_group, const robot_state::RobotState& start_state, - const robot_state::RobotState& goal_state, planning_interface::MotionPlanRequest& req) +void testutils::createPTPRequest(const std::string& planning_group, const moveit::core::RobotState& start_state, + const moveit::core::RobotState& goal_state, planning_interface::MotionPlanRequest& req) { // valid motion plan request with goal in joint space req.planner_id = "PTP"; @@ -535,7 +539,7 @@ void testutils::createPTPRequest(const std::string& planning_group, const robot_ } bool testutils::toTCPPose(const moveit::core::RobotModelConstPtr& robot_model, const std::string& link_name, - const std::vector& joint_values, geometry_msgs::Pose& pose, + const std::vector& joint_values, geometry_msgs::msg::Pose& pose, const std::string& joint_prefix) { { @@ -678,13 +682,13 @@ 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); // check the continuity between first trajectory and blend trajectory - trajectory_msgs::JointTrajectoryPoint first_end, blend_start; + trajectory_msgs::msg::JointTrajectoryPoint first_end, blend_start; first_end = first_traj.joint_trajectory.points.back(); blend_start = blend_traj.joint_trajectory.points.front(); @@ -702,8 +706,8 @@ bool testutils::checkBlendingJointSpaceContinuity(const pilz_industrial_motion_p // check the velocity and acceleration 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(); + double blend_start_velo = (blend_start.positions.at(i) - first_end.positions.at(i)) / + rclcpp::Duration(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 " @@ -715,7 +719,8 @@ 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)) / rclcpp::Duration(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 " @@ -728,7 +733,7 @@ bool testutils::checkBlendingJointSpaceContinuity(const pilz_industrial_motion_p } // check the continuity between blend trajectory and second trajectory - trajectory_msgs::JointTrajectoryPoint blend_end, second_start; + trajectory_msgs::msg::JointTrajectoryPoint blend_end, second_start; blend_end = blend_traj.joint_trajectory.points.back(); second_start = second_traj.joint_trajectory.points.front(); @@ -749,8 +754,8 @@ bool testutils::checkBlendingJointSpaceContinuity(const pilz_industrial_motion_p // check the velocity and acceleration 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(); + double second_start_velo = (second_start.positions.at(i) - blend_end.positions.at(i)) / + rclcpp::Duration(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 " @@ -761,7 +766,8 @@ 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)) / rclcpp::Duration(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 " @@ -965,8 +971,9 @@ void testutils::computeCartVelocity(const Eigen::Isometry3d& pose_1, const Eigen w(2) = w_hat(1, 0); } -void testutils::getLinLinPosesWithoutOriChange(const std::string& frame_id, sensor_msgs::JointState& initialJointState, - geometry_msgs::PoseStamped& p1, geometry_msgs::PoseStamped& p2) +void testutils::getLinLinPosesWithoutOriChange(const std::string& frame_id, + sensor_msgs::msg::JointState& initialJointState, + geometry_msgs::msg::PoseStamped& p1, geometry_msgs::msg::PoseStamped& p2) { initialJointState = testutils::generateJointState({ 0., 0.007881892504574495, -1.8157263253868452, 0., 1.8236082178909834, 0. }); @@ -991,7 +998,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"); @@ -1002,8 +1009,8 @@ void testutils::createFakeCartTraj(const robot_trajectory::RobotTrajectoryPtr& t for (size_t i = 0; i < traj->getWayPointCount(); ++i) { - trajectory_msgs::JointTrajectoryPoint waypoint; - waypoint.time_from_start = ros::Duration(traj->getWayPointDurationFromStart(i)); + trajectory_msgs::msg::JointTrajectoryPoint waypoint; + waypoint.time_from_start = rclcpp::Duration::from_seconds(traj->getWayPointDurationFromStart(i)); Eigen::Isometry3d waypoint_pose = traj->getWayPointPtr(i)->getFrameTransform(link_name); Eigen::Vector3d waypoint_position = waypoint_pose.translation(); waypoint.positions.push_back(waypoint_position(0)); @@ -1019,17 +1026,28 @@ void testutils::createFakeCartTraj(const robot_trajectory::RobotTrajectoryPtr& t } } -bool testutils::getBlendTestData(const ros::NodeHandle& nh, const size_t& dataset_num, const std::string& name_prefix, - std::vector& datasets) +bool testutils::getBlendTestData(const rclcpp::Node::SharedPtr& node, const size_t& dataset_num, + const std::string& name_prefix, std::vector& datasets) { datasets.clear(); testutils::BlendTestData dataset; + auto get_parameter = [&](const std::string& name, std::vector& parameter, + const rclcpp::Node::SharedPtr& node) { + if (node->has_parameter(name)) + { + node->get_parameter>(name, parameter); + return true; + } + else + return false; + }; + for (size_t i = 1; i < dataset_num + 1; ++i) { std::string data_set_name = "blend_set_" + std::to_string(i); - if (nh.getParam(name_prefix + data_set_name + "/start_position", dataset.start_position) && - nh.getParam(name_prefix + data_set_name + "/mid_position", dataset.mid_position) && - nh.getParam(name_prefix + data_set_name + "/end_position", dataset.end_position)) + if (get_parameter(name_prefix + data_set_name + "/start_position", dataset.start_position, node) && + get_parameter(name_prefix + data_set_name + "/mid_position", dataset.mid_position, node) && + get_parameter(name_prefix + data_set_name + "/end_position", dataset.end_position, node)) { datasets.push_back(dataset); } @@ -1042,31 +1060,33 @@ bool testutils::getBlendTestData(const ros::NodeHandle& nh, const size_t& datase } bool testutils::generateTrajFromBlendTestData( - const robot_model::RobotModelConstPtr& robot_model, + const planning_scene::PlanningSceneConstPtr& scene, const std::shared_ptr& tg, const std::string& group_name, const std::string& link_name, const testutils::BlendTestData& data, const double& sampling_time_1, const double& sampling_time_2, planning_interface::MotionPlanResponse& res_1, planning_interface::MotionPlanResponse& res_2, double& dis_1, double& dis_2) { + const moveit::core::RobotModelConstPtr robot_model = scene->getRobotModel(); + // generate first trajectory planning_interface::MotionPlanRequest req_1; req_1.group_name = group_name; req_1.max_velocity_scaling_factor = 0.1; req_1.max_acceleration_scaling_factor = 0.1; // start state - robot_state::RobotState start_state(robot_model); + moveit::core::RobotState start_state(robot_model); start_state.setToDefaultValues(); start_state.setJointGroupPositions(group_name, data.start_position); moveit::core::robotStateToRobotStateMsg(start_state, req_1.start_state, false); // goal constraint - robot_state::RobotState goal_state_1(robot_model); + moveit::core::RobotState goal_state_1(robot_model); goal_state_1.setToDefaultValues(); goal_state_1.setJointGroupPositions(group_name, data.mid_position); req_1.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints( goal_state_1, goal_state_1.getRobotModel()->getJointModelGroup(group_name))); // trajectory generation - if (!tg->generate(req_1, res_1, sampling_time_1)) + if (!tg->generate(scene, req_1, res_1, sampling_time_1)) { std::cout << "Failed to generate first trajectory." << std::endl; return false; @@ -1080,13 +1100,13 @@ bool testutils::generateTrajFromBlendTestData( // start state moveit::core::robotStateToRobotStateMsg(goal_state_1, req_2.start_state, false); // goal state - robot_state::RobotState goal_state_2(robot_model); + moveit::core::RobotState goal_state_2(robot_model); goal_state_2.setToDefaultValues(); goal_state_2.setJointGroupPositions(group_name, data.end_position); req_2.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints( goal_state_2, goal_state_2.getRobotModel()->getJointModelGroup(group_name))); // trajectory generation - if (!tg->generate(req_2, res_2, sampling_time_2)) + if (!tg->generate(scene, req_2, res_2, sampling_time_2)) { std::cout << "Failed to generate second trajectory." << std::endl; return false; @@ -1113,7 +1133,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())) { @@ -1163,13 +1183,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); @@ -1186,7 +1206,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; @@ -1195,12 +1215,12 @@ void testutils::generateRequestMsgFromBlendTestData(const moveit::core::RobotMod req_1.max_velocity_scaling_factor = 0.1; req_1.max_acceleration_scaling_factor = 0.1; // start state - robot_state::RobotState start_state(robot_model); + moveit::core::RobotState start_state(robot_model); start_state.setToDefaultValues(); start_state.setJointGroupPositions(group_name, data.start_position); moveit::core::robotStateToRobotStateMsg(start_state, req_1.start_state, false); // goal constraint - robot_state::RobotState goal_state_1(robot_model); + moveit::core::RobotState goal_state_1(robot_model); goal_state_1.setToDefaultValues(); goal_state_1.setJointGroupPositions(group_name, data.mid_position); req_1.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints( @@ -1216,7 +1236,7 @@ void testutils::generateRequestMsgFromBlendTestData(const moveit::core::RobotMod // moveit::core::robotStateToRobotStateMsg(goal_state_1, req_2.start_state, // false); // goal state - robot_state::RobotState goal_state_2(robot_model); + moveit::core::RobotState goal_state_2(robot_model); goal_state_2.setToDefaultValues(); goal_state_2.setJointGroupPositions(group_name, data.end_position); req_2.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints( @@ -1234,7 +1254,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; @@ -1251,7 +1271,7 @@ void testutils::checkRobotModel(const moveit::core::RobotModelConstPtr& robot_mo ASSERT_FALSE(robot_model->isEmpty()) << "robot model is empty"; ASSERT_TRUE(robot_model->hasJointModelGroup(group_name)) << group_name << " is not known to robot"; ASSERT_TRUE(robot_model->hasLinkModel(link_name)) << link_name << " is not known to robot"; - ASSERT_TRUE(robot_state::RobotState(robot_model).knowsFrameTransform(link_name)) + ASSERT_TRUE(moveit::core::RobotState(robot_model).knowsFrameTransform(link_name)) << "Transform of " << link_name << " is unknown"; } @@ -1341,7 +1361,7 @@ ::testing::AssertionResult testutils::hasTrapezoidVelocity(std::vector a debug_stream << *it << "(Dec)" << std::endl; } - ROS_DEBUG_STREAM(debug_stream.str()); + RCLCPP_DEBUG_STREAM(LOGGER, debug_stream.str()); return ::testing::AssertionSuccess(); } 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 fd1ebfbe5e..beda550ddc 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h @@ -42,7 +42,7 @@ #define TYPED_TEST_SUITE(...) TYPED_TEST_CASE(__VA_ARGS__) #endif -#include "moveit_msgs/MotionSequenceRequest.h" +#include #include "pilz_industrial_motion_planner/limits_container.h" #include "pilz_industrial_motion_planner/trajectory_blend_request.h" #include "pilz_industrial_motion_planner/trajectory_blend_response.h" @@ -52,10 +52,9 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include #include #include #include @@ -89,7 +88,7 @@ inline std::string getJointName(size_t joint_number, const std::string& joint_pr /** * @brief Create limits for tests to avoid the need to get the limits from the - * parameter server + * node parameter */ pilz_industrial_motion_planner::JointLimitsContainer createFakeLimits(const std::vector& joint_names); @@ -102,10 +101,10 @@ inline std::string demangel(char const* name) // Motion plan requests //******************************************** -inline sensor_msgs::JointState generateJointState(const std::vector& pos, const std::vector& vel, - const std::string& joint_prefix = testutils::JOINT_NAME_PREFIX) +inline sensor_msgs::msg::JointState generateJointState(const std::vector& pos, const std::vector& vel, + const std::string& joint_prefix = testutils::JOINT_NAME_PREFIX) { - sensor_msgs::JointState state; + sensor_msgs::msg::JointState state; auto posit = pos.cbegin(); size_t i = 0; @@ -124,22 +123,23 @@ inline sensor_msgs::JointState generateJointState(const std::vector& pos return state; } -inline sensor_msgs::JointState generateJointState(const std::vector& pos, - const std::string& joint_prefix = testutils::JOINT_NAME_PREFIX) +inline sensor_msgs::msg::JointState generateJointState(const std::vector& pos, + const std::string& joint_prefix = testutils::JOINT_NAME_PREFIX) { 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); @@ -180,8 +180,8 @@ void createPTPRequest(const std::string& planning_group, const moveit::core::Rob * @param joint_velocity_tolerance * @return true if satisfied */ -bool isGoalReached(const trajectory_msgs::JointTrajectory& trajectory, - const std::vector& goal, const double joint_position_tolerance, +bool isGoalReached(const trajectory_msgs::msg::JointTrajectory& trajectory, + const std::vector& goal, const double joint_position_tolerance, const double joint_velocity_tolerance = 1.0e-6); /** @@ -194,19 +194,20 @@ bool isGoalReached(const trajectory_msgs::JointTrajectory& trajectory, * @param joint_velocity_tolerance * @return */ -bool isGoalReached(const robot_model::RobotModelConstPtr& robot_model, - const trajectory_msgs::JointTrajectory& trajectory, const planning_interface::MotionPlanRequest& req, - const double pos_tolerance, const double orientation_tolerance); +bool isGoalReached(const moveit::core::RobotModelConstPtr& robot_model, + const trajectory_msgs::msg::JointTrajectory& trajectory, + const planning_interface::MotionPlanRequest& req, const double pos_tolerance, + const double orientation_tolerance); bool isGoalReached(const moveit::core::RobotModelConstPtr& robot_model, - const trajectory_msgs::JointTrajectory& trajectory, const planning_interface::MotionPlanRequest& req, - const double tolerance); + const trajectory_msgs::msg::JointTrajectory& trajectory, + const planning_interface::MotionPlanRequest& req, const double tolerance); /** * @brief Check that given trajectory is straight line. */ -bool checkCartesianLinearity(const robot_model::RobotModelConstPtr& robot_model, - const trajectory_msgs::JointTrajectory& trajectory, +bool checkCartesianLinearity(const moveit::core::RobotModelConstPtr& robot_model, + const trajectory_msgs::msg::JointTrajectory& trajectory, const planning_interface::MotionPlanRequest& req, const double translation_norm_tolerance, const double rot_axis_norm_tolerance, const double rot_angle_tolerance = 10e-5); @@ -244,7 +245,7 @@ inline int getWayPointIndex(const robot_trajectory::RobotTrajectoryPtr& trajecto * @param joint_limits * @return */ -bool checkJointTrajectory(const trajectory_msgs::JointTrajectory& trajectory, +bool checkJointTrajectory(const trajectory_msgs::msg::JointTrajectory& trajectory, const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); /** @@ -264,7 +265,7 @@ ::testing::AssertionResult hasStrictlyIncreasingTime(const robot_trajectory::Rob * @param trajectory * @return */ -bool isTrajectoryConsistent(const trajectory_msgs::JointTrajectory& trajectory); +bool isTrajectoryConsistent(const trajectory_msgs::msg::JointTrajectory& trajectory); /** * @brief is Position Bounded @@ -272,7 +273,7 @@ bool isTrajectoryConsistent(const trajectory_msgs::JointTrajectory& trajectory); * @param joint_limits * @return */ -bool isPositionBounded(const trajectory_msgs::JointTrajectory& trajectory, +bool isPositionBounded(const trajectory_msgs::msg::JointTrajectory& trajectory, const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); /** @@ -281,7 +282,7 @@ bool isPositionBounded(const trajectory_msgs::JointTrajectory& trajectory, * @param joint_limits * @return */ -bool isVelocityBounded(const trajectory_msgs::JointTrajectory& trajectory, +bool isVelocityBounded(const trajectory_msgs::msg::JointTrajectory& trajectory, const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); /** @@ -290,7 +291,7 @@ bool isVelocityBounded(const trajectory_msgs::JointTrajectory& trajectory, * @param joint_limits * @return */ -bool isAccelerationBounded(const trajectory_msgs::JointTrajectory& trajectory, +bool isAccelerationBounded(const trajectory_msgs::msg::JointTrajectory& trajectory, const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); /** @@ -303,7 +304,7 @@ bool isAccelerationBounded(const trajectory_msgs::JointTrajectory& trajectory, * @return false if forward kinematics failed */ bool toTCPPose(const moveit::core::RobotModelConstPtr& robot_model, const std::string& link_name, - const std::vector& joint_values, geometry_msgs::Pose& pose, + const std::vector& joint_values, geometry_msgs::msg::Pose& pose, const std::string& joint_prefix = testutils::JOINT_NAME_PREFIX); /** @@ -314,7 +315,7 @@ bool toTCPPose(const moveit::core::RobotModelConstPtr& robot_model, const std::s * @param pose * @return */ -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); /** @@ -370,15 +371,15 @@ void computeCartVelocity(const Eigen::Isometry3d& pose_1, const Eigen::Isometry3 * @param p1 (0.05, 0, 0.65, 0, 0, 0) * @param p2 (0.05, 0.4, 0.65, 0, 0, 0) */ -void getLinLinPosesWithoutOriChange(const std::string& frame_id, sensor_msgs::JointState& initialJointState, - geometry_msgs::PoseStamped& p1, geometry_msgs::PoseStamped& p2); +void getLinLinPosesWithoutOriChange(const std::string& frame_id, sensor_msgs::msg::JointState& initialJointState, + geometry_msgs::msg::PoseStamped& p1, geometry_msgs::msg::PoseStamped& p2); 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) +inline geometry_msgs::msg::Quaternion fromEuler(double a, double b, double c) { tf2::Vector3 qvz(0., 0., 1.); tf2::Vector3 qvy(0., 1., 0.); @@ -402,11 +403,11 @@ struct BlendTestData }; /** - * @brief fetch test datasets from parameter server + * @brief fetch test datasets from node parameters * @param nh * @return */ -bool getBlendTestData(const ros::NodeHandle& nh, const size_t& dataset_num, const std::string& name_prefix, +bool getBlendTestData(const rclcpp::Node::SharedPtr& node, const size_t& dataset_num, const std::string& name_prefix, std::vector& datasets); /** @@ -434,7 +435,7 @@ bool checkBlendResult(const pilz_industrial_motion_planner::TrajectoryBlendReque * @param[out] dis_lin_2: translational distance of the second LIN * @return true if succeed */ -bool generateTrajFromBlendTestData(const moveit::core::RobotModelConstPtr& robot_model, +bool generateTrajFromBlendTestData(const planning_scene::PlanningSceneConstPtr& scene, const std::shared_ptr& tg, const std::string& group_name, const std::string& link_name, const BlendTestData& data, const double& sampling_time_1, @@ -444,7 +445,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/unit_tests/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/CMakeLists.txt new file mode 100644 index 0000000000..066754c0e8 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/CMakeLists.txt @@ -0,0 +1,176 @@ +set(UNIT_TEST_SOURCEFILES + unittest_pilz_industrial_motion_planner_direct + unittest_velocity_profile_atrap + unittest_trajectory_generator + unittest_trajectory_functions + unittest_trajectory_blender_transition_window + unittest_trajectory_generator_common + unittest_trajectory_generator_circ + unittest_trajectory_generator_lin + unittest_trajectory_generator_ptp + unittest_pilz_industrial_motion_planner + unittest_joint_limit + unittest_joint_limits_aggregator + unittest_joint_limits_container + unittest_joint_limits_validator + unittest_cartesian_limits_aggregator + unittest_planning_context_loaders + unittest_planning_context + unittest_get_solver_tip_frame +) + +# Direct Command Planner Unit Test +ament_add_gtest(unittest_pilz_industrial_motion_planner_direct + src/unittest_pilz_industrial_motion_planner_direct.cpp) +target_link_libraries(unittest_pilz_industrial_motion_planner_direct + ${PROJECT_NAME} + planning_context_loader_ptp +) + +# Asymmetric Trapezoidal Velocity Profile Unit Test +ament_add_gtest(unittest_velocity_profile_atrap + src/unittest_velocity_profile_atrap.cpp + ${CMAKE_SOURCE_DIR}/src/velocity_profile_atrap.cpp) +target_link_libraries(unittest_velocity_profile_atrap ${PROJECT_NAME}) + +# Trajectory Generator Unit Test +ament_add_gtest(unittest_trajectory_generator + src/unittest_trajectory_generator.cpp + ${CMAKE_SOURCE_DIR}/src/trajectory_generator.cpp) +target_link_libraries(unittest_trajectory_generator ${PROJECT_NAME}) + +# Trajectory Functions Unit Test +ament_add_gtest_executable(unittest_trajectory_functions + src/unittest_trajectory_functions.cpp +) +ament_target_dependencies(unittest_trajectory_functions Boost) +target_link_libraries(unittest_trajectory_functions + ${PROJECT_NAME}_test_utils + trajectory_generation_common + joint_limits_common +) +add_ros_test(launch/unittest_trajectory_functions.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + +# unittest for trajectory blender transition window +ament_add_gtest_executable(unittest_trajectory_blender_transition_window + src/unittest_trajectory_blender_transition_window.cpp + ${CMAKE_SOURCE_DIR}/src/trajectory_blender_transition_window.cpp +) +ament_target_dependencies(unittest_trajectory_blender_transition_window pilz_industrial_motion_planner_testutils) +target_link_libraries(unittest_trajectory_blender_transition_window + ${PROJECT_NAME}_test_utils + trajectory_generation_common + planning_context_loader_lin +) +add_ros_test(launch/unittest_trajectory_blender_transition_window.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + +# trajectory generator Unit Test +ament_add_gtest_executable(unittest_trajectory_generator_common + src/unittest_trajectory_generator_common.cpp +) +target_link_libraries(unittest_trajectory_generator_common + ${PROJECT_NAME}_test_utils + trajectory_generation_common + planning_context_loader_lin + planning_context_loader_ptp + planning_context_loader_circ +) +add_ros_test(launch/unittest_trajectory_generator_common.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + +# trajectory generator circ Unit Test +ament_add_gtest_executable(unittest_trajectory_generator_circ + src/unittest_trajectory_generator_circ.cpp +) +ament_target_dependencies(unittest_trajectory_generator_circ pilz_industrial_motion_planner_testutils) +target_link_libraries(unittest_trajectory_generator_circ + ${PROJECT_NAME}_test_utils + planning_context_loader_circ +) +add_ros_test(launch/unittest_trajectory_generator_circ.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + +# trajectory generator lin Unit Test +ament_add_gtest_executable(unittest_trajectory_generator_lin + src/unittest_trajectory_generator_lin.cpp +) +ament_target_dependencies(unittest_trajectory_generator_lin pilz_industrial_motion_planner_testutils) +target_link_libraries(unittest_trajectory_generator_lin + ${PROJECT_NAME}_test_utils + planning_context_loader_lin +) +add_ros_test(launch/unittest_trajectory_generator_lin.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + +# trajectory generator ptp Unit Test +ament_add_gtest_executable(unittest_trajectory_generator_ptp + src/unittest_trajectory_generator_ptp.cpp +) +target_link_libraries(unittest_trajectory_generator_ptp + ${PROJECT_NAME}_test_utils + trajectory_generation_common + planning_context_loader_ptp +) +add_ros_test(launch/unittest_trajectory_generator_ptp.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + +# Command Planner Unit Test +ament_add_gtest_executable(unittest_pilz_industrial_motion_planner src/unittest_pilz_industrial_motion_planner.cpp) +target_link_libraries(unittest_pilz_industrial_motion_planner ${PROJECT_NAME}) +add_ros_test(launch/unittest_pilz_industrial_motion_planner.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + +# JointLimits Unit Test +ament_add_gtest_executable(unittest_joint_limit src/unittest_joint_limit.cpp) +target_link_libraries(unittest_joint_limit joint_limits_common) +add_ros_test(launch/unittest_joint_limit.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + +# JointLimitsAggregator Unit Test +ament_add_gtest_executable(unittest_joint_limits_aggregator src/unittest_joint_limits_aggregator.cpp) +target_link_libraries(unittest_joint_limits_aggregator joint_limits_common) +add_ros_test(launch/unittest_joint_limits_aggregator.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + +# JointLimitsContainer Unit Test +ament_add_gtest(unittest_joint_limits_container + src/unittest_joint_limits_container.cpp) +target_link_libraries(unittest_joint_limits_container joint_limits_common) + +# JointLimitsValidator Unit Test +ament_add_gtest(unittest_joint_limits_validator + src/unittest_joint_limits_validator.cpp) +target_link_libraries(unittest_joint_limits_validator joint_limits_common) + +# Cartesian Limits Aggregator Unit Test +ament_add_gtest_executable(unittest_cartesian_limits_aggregator src/unittest_cartesian_limits_aggregator.cpp) +target_link_libraries(unittest_cartesian_limits_aggregator joint_limits_common) +add_ros_test(launch/unittest_cartesian_limits_aggregator.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + +# PlanningContextLoaderPTP Unit Test +ament_add_gtest_executable(unittest_planning_context_loaders + src/unittest_planning_context_loaders.cpp +) +target_link_libraries(unittest_planning_context_loaders + ${PROJECT_NAME} + ${PROJECT_NAME}_test_utils +) +add_ros_test(launch/unittest_planning_context_loaders.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + +# PlanningContext Unit Test (Typed test) +ament_add_gtest_executable(unittest_planning_context + src/unittest_planning_context.cpp +) +target_link_libraries(unittest_planning_context + ${PROJECT_NAME} + ${PROJECT_NAME}_test_utils + planning_context_loader_lin + planning_context_loader_ptp + planning_context_loader_circ +) +add_ros_test(launch/unittest_planning_context.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + +# GetTipFrames Unit Test +ament_add_gmock(unittest_get_solver_tip_frame src/unittest_get_solver_tip_frame.cpp) +target_link_libraries(unittest_get_solver_tip_frame ${PROJECT_NAME}) + +install(TARGETS ${UNIT_TEST_SOURCEFILES} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_cartesian_limits_aggregator.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_cartesian_limits_aggregator.yaml new file mode 100644 index 0000000000..c51e42269c --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_cartesian_limits_aggregator.yaml @@ -0,0 +1,10 @@ +only_vel: + cartesian_limits: + max_trans_vel: 10.0 + +all_values: + cartesian_limits: + max_trans_vel: 1.0 + max_trans_acc: 2.0 + max_trans_dec: -3.0 + max_rot_vel: 4.0 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_joint_limit_config.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_joint_limit_config.yaml new file mode 100644 index 0000000000..f9d715b961 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_joint_limit_config.yaml @@ -0,0 +1,55 @@ +joint_limits: + joint_1: + has_position_limits: false + has_velocity_limits: false + has_acceleration_limits: true + max_acceleration: 1.0 + has_deceleration_limits: true + max_deceleration: -1.0 + has_jerk_limits: false + has_soft_limits: false + joint_2: + has_position_limits: false + has_velocity_limits: false + has_acceleration_limits: true + max_acceleration: 2.0 + has_deceleration_limits: true + max_deceleration: -2.0 + has_jerk_limits: false + has_soft_limits: false + joint_3: + has_position_limits: false + has_velocity_limits: false + has_acceleration_limits: true + max_acceleration: 3.0 + has_deceleration_limits: true + max_deceleration: -3.0 + has_jerk_limits: false + has_soft_limits: false + joint_4: + has_position_limits: false + has_velocity_limits: false + has_acceleration_limits: true + max_acceleration: 4.0 + has_deceleration_limits: true + max_deceleration: -4.0 + has_jerk_limits: false + has_soft_limits: false + joint_5: + has_position_limits: false + has_velocity_limits: false + has_acceleration_limits: true + max_acceleration: 5.0 + has_deceleration_limits: true + max_deceleration: -5.0 + has_jerk_limits: false + has_soft_limits: false + joint_6: + has_position_limits: false + has_velocity_limits: false + has_acceleration_limits: true + max_acceleration: 6.0 + has_deceleration_limits: true + max_deceleration: -6.0 + has_jerk_limits: false + has_soft_limits: false diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_joint_limits_aggregator.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_joint_limits_aggregator.yaml new file mode 100644 index 0000000000..5051be43a6 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_joint_limits_aggregator.yaml @@ -0,0 +1,41 @@ +all_valid: + joint_limits: + prbt_joint_1: + has_position_limits: true + min_position: -2.0 + max_position: 2.0 + prbt_joint_2: + has_position_limits: false + min_position: -2.0 + max_position: 2.0 + prbt_joint_3: + has_velocity_limits: true + max_velocity: 1.1 + has_acceleration_limits: false + max_acceleration: 5.5 + prbt_joint_4: + has_acceleration_limits: true + max_acceleration: 5.5 + prbt_joint_5: + has_deceleration_limits: true + max_deceleration: -6.6 + +violate_position_max: + joint_limits: + prbt_joint_3: + has_position_limits: true + min_position: -1.0 + max_position: 4.0 + +violate_position_min: + joint_limits: + prbt_joint_2: + has_position_limits: true + min_position: -4.0 + max_position: 2.0 + +violate_velocity: + joint_limits: + prbt_joint_3: + has_velocity_limits: true + max_velocity: -90.0 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_blender_transition_window.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_blender_transition_window.yaml new file mode 100644 index 0000000000..cc19c49bb1 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_blender_transition_window.yaml @@ -0,0 +1,7 @@ +planning_group : "manipulator" +target_link : "prbt_tcp" +cartesian_velocity_tolerance : 1.0e-6 +cartesian_angular_velocity_tolerance : 1.0e-6 +joint_velocity_tolerance : 1.0e-6 +joint_acceleration_tolerance : 1.0e-2 +sampling_time : 0.01 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_functions.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_functions.yaml new file mode 100644 index 0000000000..a642ce09ac --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_functions.yaml @@ -0,0 +1,5 @@ +planning_group : "manipulator" +group_tip_link : "prbt_flange" +tcp_link : "prbt_tcp" +ik_fast_link : "prbt_flange" +random_test_number : 1000 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_circ.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_circ.yaml new file mode 100644 index 0000000000..25142d4ba4 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_circ.yaml @@ -0,0 +1,7 @@ +planning_group : "manipulator" +target_link : "prbt_tcp" +cartesian_position_tolerance : 1.0e-3 +angular_acc_tolerance : 1.0e-3 +rot_axis_norm_tolerance : 1.0e-6 +acceleration_tolerance : 1.0e-3 +other_tolerance : 1.0e-6 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_common.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_common.yaml new file mode 100644 index 0000000000..4658d75d5a --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_common.yaml @@ -0,0 +1,9 @@ +planning_group : "manipulator" +target_link : "prbt_flange" +random_trial_number : 100 +joint_position_tolerance : 1.0e-3 +joint_velocity_tolerance : 0.3 +pose_norm_tolerance : 1.0e-6 +rot_axis_norm_tolerance : 1.0e-6 +other_tolerance : 1.0e-6 +velocity_scaling_factor : 0.1 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_lin.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_lin.yaml new file mode 100644 index 0000000000..b36b328fed --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_lin.yaml @@ -0,0 +1,9 @@ +planning_group : "manipulator" +target_link_hand_computed_data : "prbt_flange" +random_trial_number : 100 +joint_position_tolerance : 1.0e-3 +joint_velocity_tolerance : 0.3 +pose_norm_tolerance : 1.0e-6 +rot_axis_norm_tolerance : 1.0e-6 +other_tolerance : 1.0e-5 +velocity_scaling_factor : 0.1 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_ptp.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_ptp.yaml new file mode 100644 index 0000000000..8d4938d6e6 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_ptp.yaml @@ -0,0 +1,6 @@ +planning_group : "manipulator" +target_link : "prbt_tcp" +joint_position_tolerance : 1.0e-8 +joint_velocity_tolerance : 1.0e-8 +joint_acceleration_tolerance : 1.0e-8 +pose_norm_tolerance : 1.0e-6 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/common_parameters.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/common_parameters.py new file mode 100644 index 0000000000..e983c856f1 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/common_parameters.py @@ -0,0 +1,109 @@ +from ament_index_python.packages import get_package_share_directory +import xacro +import yaml +import os + +# TODO(henningkayser): Switch to ParameterBuilder once #591 is merged +# from moveit_configs_utils import MoveItConfigsBuilder +# from parameter_builder import ParameterBuilder + + +def _load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def load_yaml(package_name, file_path): + # TODO(henningkayser): Switch to ParameterBuilder once #591 is merged + # return ( + # ParameterBuilder(package_name) + # .yaml(file_path) + # .to_dict() + # ) + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +class MoveItConfigs: + robot_description = {} + robot_description_semantic = {} + robot_description_kinematics = {} + robot_description_planning = {} + planning_plugin = {} + + +def load_moveit_config(): + moveit_config_package_name = "moveit_resources_prbt_moveit_config" + description_package_name = "moveit_resources_prbt_support" + description_xacro_file = "urdf/prbt.xacro" + robot_description_semantic_file = "config/prbt.srdf.xacro" + robot_description_kinematics_file = "config/kinematics.yaml" + joint_limits_file = "config/joint_limits.yaml" + cartesian_limits_file = "config/cartesian_limits.yaml" + # TODO(henningkayser): Switch to MoveItConfigsBuilder once #591 is merged + # return ( + # MoveItConfigsBuilder(moveit_config_package_name) + # .robot_description( + # file_path=get_package_share_directory(description_package_name) + # + "/" + description_xacro_file + # ) + # .robot_description_semantic(file_path=robot_description_semantic_file) + # .robot_description_kinematics(file_path=robot_description_kinematics_file) + # .joint_limits(file_path=joint_limits_file) + # .cartesian_limits(file_path=cartesian_limits_file) + # .to_moveit_configs() + # ) + + configs = MoveItConfigs() + + # planning_context + robot_description = xacro.process_file( + os.path.join( + get_package_share_directory(description_package_name), + description_xacro_file, + ) + ) + + configs.robot_description = {"robot_description": robot_description.toxml()} + + semantic_xacro = xacro.process_file( + os.path.join( + get_package_share_directory(moveit_config_package_name), + robot_description_semantic_file, + ) + ) + + configs.robot_description_semantic = { + "robot_description_semantic": semantic_xacro.toxml() + } + + configs.robot_description_kinematics = { + "robot_description_kinematics": load_yaml( + moveit_config_package_name, robot_description_kinematics_file + ) + } + + configs.robot_description_planning = { + "robot_description_planning": { + **load_yaml(moveit_config_package_name, joint_limits_file), + **load_yaml(moveit_config_package_name, cartesian_limits_file), + } + } + + planning_plugin = { + "planning_plugin": "pilz_industrial_motion_planner::CommandPlanner" + } + + return configs diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_cartesian_limits_aggregator.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_cartesian_limits_aggregator.test.py new file mode 100644 index 0000000000..e2947edada --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_cartesian_limits_aggregator.test.py @@ -0,0 +1,75 @@ +import launch_testing +import os +import pytest +import unittest +import xacro +import yaml +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_testing.util import KeepAliveProc + + +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + try: + with open(absolute_file_path, "r") as file: + return yaml.full_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +@pytest.mark.rostest +def generate_test_description(): + cartesian_limits_yaml = load_yaml( + "pilz_industrial_motion_planner", + "config/unittest_cartesian_limits_aggregator.yaml", + ) + + # run test + unittest_cartesian_limits_aggregator = Node( + package="pilz_industrial_motion_planner", + executable="unittest_cartesian_limits_aggregator", + name="unittest_cartesian_limits_aggregator", + parameters=[ + cartesian_limits_yaml, + ], + output="screen", + ) + return ( + LaunchDescription( + [ + unittest_cartesian_limits_aggregator, + KeepAliveProc(), + launch_testing.actions.ReadyToTest(), + ] + ), + {"unittest_cartesian_limits_aggregator": unittest_cartesian_limits_aggregator}, + ) + + +class TestTerminatingProcessStops(unittest.TestCase): + def test_gtest_run_complete(self, proc_info, unittest_cartesian_limits_aggregator): + proc_info.assertWaitForShutdown( + process=unittest_cartesian_limits_aggregator, timeout=4000.0 + ) + + +@launch_testing.post_shutdown_test() +class TestOutcome(unittest.TestCase): + def test_exit_codes(self, proc_info, unittest_cartesian_limits_aggregator): + launch_testing.asserts.assertExitCodes( + proc_info, process=unittest_cartesian_limits_aggregator + ) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_joint_limit.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_joint_limit.test.py new file mode 100644 index 0000000000..0cd8987124 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_joint_limit.test.py @@ -0,0 +1,70 @@ +import launch_testing +import os +import pytest +import unittest +import xacro +import yaml +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_testing.util import KeepAliveProc + + +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + try: + with open(absolute_file_path, "r") as file: + return yaml.full_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +@pytest.mark.rostest +def generate_test_description(): + joint_limits_yaml = load_yaml( + "pilz_industrial_motion_planner", "config/unittest_joint_limit_config.yaml" + ) + + # run test + unittest_joint_limit = Node( + package="pilz_industrial_motion_planner", + executable="unittest_joint_limit", + name="unittest_joint_limit", + parameters=[ + joint_limits_yaml, + ], + output="screen", + ) + return ( + LaunchDescription( + [ + unittest_joint_limit, + KeepAliveProc(), + launch_testing.actions.ReadyToTest(), + ] + ), + {"unittest_joint_limit": unittest_joint_limit}, + ) + + +class TestTerminatingProcessStops(unittest.TestCase): + def test_gtest_run_complete(self, proc_info, unittest_joint_limit): + proc_info.assertWaitForShutdown(process=unittest_joint_limit, timeout=4000.0) + + +@launch_testing.post_shutdown_test() +class TestOutcome(unittest.TestCase): + def test_exit_codes(self, proc_info, unittest_joint_limit): + launch_testing.asserts.assertExitCodes(proc_info, process=unittest_joint_limit) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_joint_limits_aggregator.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_joint_limits_aggregator.test.py new file mode 100644 index 0000000000..8caf1a875d --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_joint_limits_aggregator.test.py @@ -0,0 +1,64 @@ +import launch_testing +import pytest +import unittest +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_testing.util import KeepAliveProc + +import sys +import os + +sys.path.append(os.path.dirname(__file__)) +from common_parameters import load_moveit_config, load_yaml + + +@pytest.mark.rostest +def generate_test_description(): + + # Load the context + test_config = load_moveit_config() + + test_param = load_yaml( + "pilz_industrial_motion_planner", "config/unittest_joint_limits_aggregator.yaml" + ) + + # run test + unittest_joint_limits_aggregator = Node( + package="pilz_industrial_motion_planner", + executable="unittest_joint_limits_aggregator", + name="unittest_joint_limits_aggregator", + parameters=[ + test_config.robot_description, + test_config.robot_description_semantic, + test_config.robot_description_kinematics, + test_config.robot_description_planning, + test_param, + ], + output="screen", + ) + return ( + LaunchDescription( + [ + unittest_joint_limits_aggregator, + KeepAliveProc(), + launch_testing.actions.ReadyToTest(), + ] + ), + {"unittest_joint_limits_aggregator": unittest_joint_limits_aggregator}, + ) + + +class TestTerminatingProcessStops(unittest.TestCase): + def test_gtest_run_complete(self, proc_info, unittest_joint_limits_aggregator): + proc_info.assertWaitForShutdown( + process=unittest_joint_limits_aggregator, timeout=4000.0 + ) + + +@launch_testing.post_shutdown_test() +class TestOutcome(unittest.TestCase): + def test_exit_codes(self, proc_info, unittest_joint_limits_aggregator): + launch_testing.asserts.assertExitCodes( + proc_info, process=unittest_joint_limits_aggregator + ) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_pilz_industrial_motion_planner.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_pilz_industrial_motion_planner.test.py new file mode 100644 index 0000000000..47b1766bea --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_pilz_industrial_motion_planner.test.py @@ -0,0 +1,68 @@ +import launch_testing +import pytest +import unittest +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_testing.util import KeepAliveProc + +import sys +import os + +sys.path.append(os.path.dirname(__file__)) +from common_parameters import load_moveit_config + + +@pytest.mark.rostest +def generate_test_description(): + + # Load the context + test_config = load_moveit_config() + + planning_plugin = { + "planning_plugin": "pilz_industrial_motion_planner/CommandPlanner" + } + + # run test + unittest_pilz_industrial_motion_planner = Node( + package="pilz_industrial_motion_planner", + executable="unittest_pilz_industrial_motion_planner", + name="unittest_pilz_industrial_motion_planner", + parameters=[ + test_config.robot_description, + test_config.robot_description_semantic, + test_config.robot_description_kinematics, + test_config.robot_description_planning, + planning_plugin, + ], + output="screen", + ) + return ( + LaunchDescription( + [ + unittest_pilz_industrial_motion_planner, + KeepAliveProc(), + launch_testing.actions.ReadyToTest(), + ] + ), + { + "unittest_pilz_industrial_motion_planner": unittest_pilz_industrial_motion_planner + }, + ) + + +class TestTerminatingProcessStops(unittest.TestCase): + def test_gtest_run_complete( + self, proc_info, unittest_pilz_industrial_motion_planner + ): + proc_info.assertWaitForShutdown( + process=unittest_pilz_industrial_motion_planner, timeout=4000.0 + ) + + +@launch_testing.post_shutdown_test() +class TestOutcome(unittest.TestCase): + def test_exit_codes(self, proc_info, unittest_pilz_industrial_motion_planner): + launch_testing.asserts.assertExitCodes( + proc_info, process=unittest_pilz_industrial_motion_planner + ) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_planning_context.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_planning_context.test.py new file mode 100644 index 0000000000..fcb897a7c4 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_planning_context.test.py @@ -0,0 +1,66 @@ +import launch_testing +import pytest +import unittest +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_testing.util import KeepAliveProc + +import sys +import os + +sys.path.append(os.path.dirname(__file__)) +from common_parameters import load_moveit_config + + +@pytest.mark.rostest +def generate_test_description(): + + # Load the context + test_config = load_moveit_config() + + test_param = { + "planning_group": "manipulator", + "target_link": "prbt_tcp", + } + + # run test + unittest_planning_context = Node( + package="pilz_industrial_motion_planner", + executable="unittest_planning_context", + name="unittest_planning_context", + parameters=[ + test_config.robot_description, + test_config.robot_description_semantic, + test_config.robot_description_kinematics, + test_config.robot_description_planning, + test_param, + ], + output="screen", + ) + + return ( + LaunchDescription( + [ + unittest_planning_context, + KeepAliveProc(), + launch_testing.actions.ReadyToTest(), + ] + ), + {"unittest_planning_context": unittest_planning_context}, + ) + + +class TestTerminatingProcessStops(unittest.TestCase): + def test_gtest_run_complete(self, proc_info, unittest_planning_context): + proc_info.assertWaitForShutdown( + process=unittest_planning_context, timeout=4000.0 + ) + + +@launch_testing.post_shutdown_test() +class TestOutcome(unittest.TestCase): + def test_exit_codes(self, proc_info, unittest_planning_context): + launch_testing.asserts.assertExitCodes( + proc_info, process=unittest_planning_context + ) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_planning_context_loaders.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_planning_context_loaders.test.py new file mode 100644 index 0000000000..0ce8f1e378 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_planning_context_loaders.test.py @@ -0,0 +1,60 @@ +import launch_testing +import pytest +import unittest +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_testing.util import KeepAliveProc + +import sys +import os + +sys.path.append(os.path.dirname(__file__)) +from common_parameters import load_moveit_config + + +@pytest.mark.rostest +def generate_test_description(): + + # Load the context + test_config = load_moveit_config() + + # run test + unittest_planning_context_loaders = Node( + package="pilz_industrial_motion_planner", + executable="unittest_planning_context_loaders", + name="unittest_planning_context_loaders", + parameters=[ + test_config.robot_description, + test_config.robot_description_semantic, + test_config.robot_description_kinematics, + test_config.robot_description_planning, + test_config.planning_plugin, + ], + output="screen", + ) + return ( + LaunchDescription( + [ + unittest_planning_context_loaders, + KeepAliveProc(), + launch_testing.actions.ReadyToTest(), + ] + ), + {"unittest_planning_context_loaders": unittest_planning_context_loaders}, + ) + + +class TestTerminatingProcessStops(unittest.TestCase): + def test_gtest_run_complete(self, proc_info, unittest_planning_context_loaders): + proc_info.assertWaitForShutdown( + process=unittest_planning_context_loaders, timeout=4000.0 + ) + + +@launch_testing.post_shutdown_test() +class TestOutcome(unittest.TestCase): + def test_exit_codes(self, proc_info, unittest_planning_context_loaders): + launch_testing.asserts.assertExitCodes( + proc_info, process=unittest_planning_context_loaders + ) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_blender_transition_window.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_blender_transition_window.test.py new file mode 100644 index 0000000000..20a83da0cf --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_blender_transition_window.test.py @@ -0,0 +1,77 @@ +import launch_testing +import pytest +import unittest +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_testing.util import KeepAliveProc + +import sys +import os + +sys.path.append(os.path.dirname(__file__)) +from common_parameters import load_moveit_config, load_yaml + + +@pytest.mark.rostest +def generate_test_description(): + + # Load the context + test_config = load_moveit_config() + + test_param = load_yaml( + "pilz_industrial_motion_planner", + "config/unittest_trajectory_blender_transition_window.yaml", + ) + + testdata_file_name = { + "testdata_file_name": get_package_share_directory( + "pilz_industrial_motion_planner" + ) + + "/test_data/prbt/testdata_sequence.xml" + } + + # run test + unittest_trajectory_blender_transition_window = Node( + package="pilz_industrial_motion_planner", + executable="unittest_trajectory_blender_transition_window", + name="unittest_trajectory_blender_transition_window", + parameters=[ + test_config.robot_description, + test_config.robot_description_semantic, + test_config.robot_description_kinematics, + test_config.robot_description_planning, + test_param, + testdata_file_name, + ], + output="screen", + ) + return ( + LaunchDescription( + [ + unittest_trajectory_blender_transition_window, + KeepAliveProc(), + launch_testing.actions.ReadyToTest(), + ] + ), + { + "unittest_trajectory_blender_transition_window": unittest_trajectory_blender_transition_window + }, + ) + + +class TestTerminatingProcessStops(unittest.TestCase): + def test_gtest_run_complete( + self, proc_info, unittest_trajectory_blender_transition_window + ): + proc_info.assertWaitForShutdown( + process=unittest_trajectory_blender_transition_window, timeout=4000.0 + ) + + +@launch_testing.post_shutdown_test() +class TestOutcome(unittest.TestCase): + def test_exit_codes(self, proc_info, unittest_trajectory_blender_transition_window): + launch_testing.asserts.assertExitCodes( + proc_info, process=unittest_trajectory_blender_transition_window + ) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_functions.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_functions.test.py new file mode 100644 index 0000000000..066600f68c --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_functions.test.py @@ -0,0 +1,64 @@ +import launch_testing +import pytest +import unittest +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_testing.util import KeepAliveProc + +import sys +import os + +sys.path.append(os.path.dirname(__file__)) +from common_parameters import load_moveit_config, load_yaml + + +@pytest.mark.rostest +def generate_test_description(): + + # Load the context + test_config = load_moveit_config() + + test_param = load_yaml( + "pilz_industrial_motion_planner", "config/unittest_trajectory_functions.yaml" + ) + + # run test + unittest_trajectory_functions = Node( + package="pilz_industrial_motion_planner", + executable="unittest_trajectory_functions", + name="unittest_trajectory_functions", + parameters=[ + test_config.robot_description, + test_config.robot_description_semantic, + test_config.robot_description_kinematics, + test_config.robot_description_planning, + test_param, + ], + output="screen", + ) + return ( + LaunchDescription( + [ + unittest_trajectory_functions, + KeepAliveProc(), + launch_testing.actions.ReadyToTest(), + ] + ), + {"unittest_trajectory_functions": unittest_trajectory_functions}, + ) + + +class TestTerminatingProcessStops(unittest.TestCase): + def test_gtest_run_complete(self, proc_info, unittest_trajectory_functions): + proc_info.assertWaitForShutdown( + process=unittest_trajectory_functions, timeout=4000.0 + ) + + +@launch_testing.post_shutdown_test() +class TestOutcome(unittest.TestCase): + def test_exit_codes(self, proc_info, unittest_trajectory_functions): + launch_testing.asserts.assertExitCodes( + proc_info, process=unittest_trajectory_functions + ) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_circ.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_circ.test.py new file mode 100644 index 0000000000..ad01a4d1cf --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_circ.test.py @@ -0,0 +1,73 @@ +import launch_testing +import pytest +import unittest +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_testing.util import KeepAliveProc + +import sys +import os + +sys.path.append(os.path.dirname(__file__)) +from common_parameters import load_moveit_config, load_yaml + + +@pytest.mark.rostest +def generate_test_description(): + + # Load the context + test_config = load_moveit_config() + + test_param = load_yaml( + "pilz_industrial_motion_planner", + "config/unittest_trajectory_generator_circ.yaml", + ) + + testdata_file_name = { + "testdata_file_name": get_package_share_directory( + "pilz_industrial_motion_planner" + ) + + "/test_data/prbt/testdata_sequence.xml" + } + + # run test + unittest_trajectory_generator_circ = Node( + package="pilz_industrial_motion_planner", + executable="unittest_trajectory_generator_circ", + name="unittest_trajectory_generator_circ", + parameters=[ + test_config.robot_description, + test_config.robot_description_semantic, + test_config.robot_description_kinematics, + test_config.robot_description_planning, + test_param, + testdata_file_name, + ], + output="screen", + ) + return ( + LaunchDescription( + [ + unittest_trajectory_generator_circ, + KeepAliveProc(), + launch_testing.actions.ReadyToTest(), + ] + ), + {"unittest_trajectory_generator_circ": unittest_trajectory_generator_circ}, + ) + + +class TestTerminatingProcessStops(unittest.TestCase): + def test_gtest_run_complete(self, proc_info, unittest_trajectory_generator_circ): + proc_info.assertWaitForShutdown( + process=unittest_trajectory_generator_circ, timeout=4000.0 + ) + + +@launch_testing.post_shutdown_test() +class TestOutcome(unittest.TestCase): + def test_exit_codes(self, proc_info, unittest_trajectory_generator_circ): + launch_testing.asserts.assertExitCodes( + proc_info, process=unittest_trajectory_generator_circ + ) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_common.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_common.test.py new file mode 100644 index 0000000000..4b2a7b1ec9 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_common.test.py @@ -0,0 +1,65 @@ +import launch_testing +import pytest +import unittest +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_testing.util import KeepAliveProc + +import sys +import os + +sys.path.append(os.path.dirname(__file__)) +from common_parameters import load_moveit_config, load_yaml + + +@pytest.mark.rostest +def generate_test_description(): + + # Load the context + test_config = load_moveit_config() + + test_param = load_yaml( + "pilz_industrial_motion_planner", + "config/unittest_trajectory_generator_common.yaml", + ) + + # run test + unittest_trajectory_generator_common = Node( + package="pilz_industrial_motion_planner", + executable="unittest_trajectory_generator_common", + name="unittest_trajectory_generator_common", + parameters=[ + test_config.robot_description, + test_config.robot_description_semantic, + test_config.robot_description_kinematics, + test_config.robot_description_planning, + test_param, + ], + output="screen", + ) + return ( + LaunchDescription( + [ + unittest_trajectory_generator_common, + KeepAliveProc(), + launch_testing.actions.ReadyToTest(), + ] + ), + {"unittest_trajectory_generator_common": unittest_trajectory_generator_common}, + ) + + +class TestTerminatingProcessStops(unittest.TestCase): + def test_gtest_run_complete(self, proc_info, unittest_trajectory_generator_common): + proc_info.assertWaitForShutdown( + process=unittest_trajectory_generator_common, timeout=4000.0 + ) + + +@launch_testing.post_shutdown_test() +class TestOutcome(unittest.TestCase): + def test_exit_codes(self, proc_info, unittest_trajectory_generator_common): + launch_testing.asserts.assertExitCodes( + proc_info, process=unittest_trajectory_generator_common + ) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_lin.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_lin.test.py new file mode 100644 index 0000000000..5cd903f8cb --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_lin.test.py @@ -0,0 +1,73 @@ +import launch_testing +import pytest +import unittest +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_testing.util import KeepAliveProc + +import sys +import os + +sys.path.append(os.path.dirname(__file__)) +from common_parameters import load_moveit_config, load_yaml + + +@pytest.mark.rostest +def generate_test_description(): + + # Load the context + test_config = load_moveit_config() + + test_param = load_yaml( + "pilz_industrial_motion_planner", + "config/unittest_trajectory_generator_lin.yaml", + ) + + testdata_file_name = { + "testdata_file_name": get_package_share_directory( + "pilz_industrial_motion_planner" + ) + + "/test_data/prbt/testdata_sequence.xml" + } + + # run test + unittest_trajectory_generator_lin = Node( + package="pilz_industrial_motion_planner", + executable="unittest_trajectory_generator_lin", + name="unittest_trajectory_generator_lin", + parameters=[ + test_config.robot_description, + test_config.robot_description_semantic, + test_config.robot_description_kinematics, + test_config.robot_description_planning, + test_param, + testdata_file_name, + ], + output="screen", + ) + return ( + LaunchDescription( + [ + unittest_trajectory_generator_lin, + KeepAliveProc(), + launch_testing.actions.ReadyToTest(), + ] + ), + {"unittest_trajectory_generator_lin": unittest_trajectory_generator_lin}, + ) + + +class TestTerminatingProcessStops(unittest.TestCase): + def test_gtest_run_complete(self, proc_info, unittest_trajectory_generator_lin): + proc_info.assertWaitForShutdown( + process=unittest_trajectory_generator_lin, timeout=4000.0 + ) + + +@launch_testing.post_shutdown_test() +class TestOutcome(unittest.TestCase): + def test_exit_codes(self, proc_info, unittest_trajectory_generator_lin): + launch_testing.asserts.assertExitCodes( + proc_info, process=unittest_trajectory_generator_lin + ) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_ptp.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_ptp.test.py new file mode 100644 index 0000000000..eff917f49e --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_ptp.test.py @@ -0,0 +1,65 @@ +import launch_testing +import pytest +import unittest +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_testing.util import KeepAliveProc + +import sys +import os + +sys.path.append(os.path.dirname(__file__)) +from common_parameters import load_moveit_config, load_yaml + + +@pytest.mark.rostest +def generate_test_description(): + + # Load the context + test_config = load_moveit_config() + + test_param = load_yaml( + "pilz_industrial_motion_planner", + "config/unittest_trajectory_generator_ptp.yaml", + ) + + # run test + unittest_trajectory_generator_ptp = Node( + package="pilz_industrial_motion_planner", + executable="unittest_trajectory_generator_ptp", + name="unittest_trajectory_generator_ptp", + parameters=[ + test_config.robot_description, + test_config.robot_description_semantic, + test_config.robot_description_kinematics, + test_config.robot_description_planning, + test_param, + ], + output="screen", + ) + return ( + LaunchDescription( + [ + unittest_trajectory_generator_ptp, + KeepAliveProc(), + launch_testing.actions.ReadyToTest(), + ] + ), + {"unittest_trajectory_generator_ptp": unittest_trajectory_generator_ptp}, + ) + + +class TestTerminatingProcessStops(unittest.TestCase): + def test_gtest_run_complete(self, proc_info, unittest_trajectory_generator_ptp): + proc_info.assertWaitForShutdown( + process=unittest_trajectory_generator_ptp, timeout=4000.0 + ) + + +@launch_testing.post_shutdown_test() +class TestOutcome(unittest.TestCase): + def test_exit_codes(self, proc_info, unittest_trajectory_generator_ptp): + launch_testing.asserts.assertExitCodes( + proc_info, process=unittest_trajectory_generator_ptp + ) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_cartesian_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_cartesian_limits_aggregator.cpp similarity index 91% rename from moveit_planners/pilz_industrial_motion_planner/test/unittest_cartesian_limits_aggregator.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_cartesian_limits_aggregator.cpp index 1809ada9c7..7e5a79b280 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_cartesian_limits_aggregator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_cartesian_limits_aggregator.cpp @@ -34,7 +34,7 @@ #include -#include +#include #include "pilz_industrial_motion_planner/cartesian_limit.h" #include "pilz_industrial_motion_planner/cartesian_limits_aggregator.h" @@ -44,6 +44,12 @@ */ class CartesianLimitsAggregator : public ::testing::Test { +protected: + void SetUp() override + { + node_ = rclcpp::Node::make_shared("unittest_cartesian_limits_aggregator"); + } + rclcpp::Node::SharedPtr node_; }; /** @@ -51,10 +57,8 @@ class CartesianLimitsAggregator : public ::testing::Test */ TEST_F(CartesianLimitsAggregator, OnlyVelocity) { - ros::NodeHandle nh("~/vel_only"); - pilz_industrial_motion_planner::CartesianLimit limit = - pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits(nh); + pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits(node_, "only_vel"); EXPECT_TRUE(limit.hasMaxTranslationalVelocity()); EXPECT_EQ(limit.getMaxTranslationalVelocity(), 10); EXPECT_FALSE(limit.hasMaxTranslationalAcceleration()); @@ -67,10 +71,8 @@ TEST_F(CartesianLimitsAggregator, OnlyVelocity) */ TEST_F(CartesianLimitsAggregator, AllValues) { - ros::NodeHandle nh("~/all"); - pilz_industrial_motion_planner::CartesianLimit limit = - pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits(nh); + pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits(node_, "all_values"); EXPECT_TRUE(limit.hasMaxTranslationalVelocity()); EXPECT_EQ(limit.getMaxTranslationalVelocity(), 1); @@ -86,7 +88,7 @@ TEST_F(CartesianLimitsAggregator, AllValues) int main(int argc, char** argv) { - ros::init(argc, argv, "unittest_cartesian_limits_aggregator"); + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_get_solver_tip_frame.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_get_solver_tip_frame.cpp similarity index 96% rename from moveit_planners/pilz_industrial_motion_planner/test/unittest_get_solver_tip_frame.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/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/unit_tests/src/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_joint_limit.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limit.cpp similarity index 82% rename from moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limit.cpp index 488a27bb00..451d04f8c2 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limit.cpp @@ -32,12 +32,10 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include "ros/ros.h" +#include #include -#include - #include "pilz_industrial_motion_planner/joint_limits_extension.h" #include "pilz_industrial_motion_planner/joint_limits_interface_extension.h" @@ -48,17 +46,21 @@ namespace pilz_extensions_tests { class JointLimitTest : public ::testing::Test { +protected: + void SetUp() override + { + node_ = rclcpp::Node::make_shared("unittest_joint_limits_extended"); + } + rclcpp::Node::SharedPtr node_; }; TEST_F(JointLimitTest, SimpleRead) { - ros::NodeHandle node_handle("~"); - // Joints limits interface JointLimits joint_limits_extended; - JointLimits joint_limits; - getJointLimits("joint_1", node_handle, joint_limits_extended); + EXPECT_TRUE(declareParameters("joint_1", "", node_)); + EXPECT_TRUE(getJointLimits("joint_1", "", node_, joint_limits_extended)); EXPECT_EQ(1, joint_limits_extended.max_acceleration); EXPECT_EQ(-1, joint_limits_extended.max_deceleration); @@ -66,13 +68,10 @@ TEST_F(JointLimitTest, SimpleRead) TEST_F(JointLimitTest, readNonExistingJointLimit) { - ros::NodeHandle node_handle("~"); - // Joints limits interface JointLimits joint_limits_extended; - JointLimits joint_limits; - EXPECT_FALSE(getJointLimits("anything", node_handle, joint_limits_extended)); + EXPECT_FALSE(getJointLimits("anything", "", node_, joint_limits_extended)); } /** @@ -82,22 +81,19 @@ TEST_F(JointLimitTest, readNonExistingJointLimit) */ TEST_F(JointLimitTest, readInvalidParameterName) { - ros::NodeHandle node_handle("~"); - // Joints limits interface JointLimits joint_limits_extended; - JointLimits joint_limits; - EXPECT_FALSE(getJointLimits("~anything", node_handle, joint_limits_extended)); + EXPECT_FALSE(getJointLimits("~anything", "", node_, joint_limits_extended)); } TEST_F(JointLimitTest, OldRead) { - ros::NodeHandle node_handle("~"); - // Joints limits interface JointLimits joint_limits; - getJointLimits("joint_1", node_handle, joint_limits); + + EXPECT_TRUE(declareParameters("joint_1", "", node_)); + EXPECT_TRUE(getJointLimits("joint_1", "", node_, joint_limits)); EXPECT_EQ(1, joint_limits.max_acceleration); } @@ -105,7 +101,7 @@ TEST_F(JointLimitTest, OldRead) int main(int argc, char** argv) { - ros::init(argc, argv, "unittest_joint_limits_extended"); + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_aggregator.cpp similarity index 79% rename from moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_aggregator.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_aggregator.cpp index 06827e3284..5d9e6b3a31 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_aggregator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_aggregator.cpp @@ -41,6 +41,8 @@ #include "pilz_industrial_motion_planner/joint_limits_extension.h" #include "pilz_industrial_motion_planner/joint_limits_interface_extension.h" +#include "rclcpp/rclcpp.hpp" + /** * @brief Unittest of the JointLimitsAggregator class */ @@ -49,21 +51,19 @@ class JointLimitsAggregator : public ::testing::Test protected: void SetUp() override { - ros::NodeHandle node_handle("~"); - - // Load robot module - robot_model_loader::RobotModelLoader::Options opt("robot_description"); - model_loader_.reset(new robot_model_loader::RobotModelLoader(opt)); - robot_model_ = model_loader_->getModel(); - - return; + rclcpp::NodeOptions node_options; + // node_options.automatically_declare_parameters_from_overrides(true); + node_ = rclcpp::Node::make_shared("unittest_joint_limits_aggregator", node_options); + + // load robot model + robot_model_loader::RobotModelLoader rm_loader(node_); + robot_model_ = rm_loader.getModel(); + ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model"; } - /// The robot model loader - robot_model_loader::RobotModelLoaderPtr model_loader_; - - /// The robot model - robot_model::RobotModelConstPtr robot_model_; +protected: + rclcpp::Node::SharedPtr node_; + moveit::core::RobotModelConstPtr robot_model_; }; /** @@ -72,28 +72,24 @@ class JointLimitsAggregator : public ::testing::Test */ TEST_F(JointLimitsAggregator, ExpectedMapSize) { - ros::NodeHandle nh("~"); - pilz_industrial_motion_planner::JointLimitsContainer container = - pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(nh, + pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(node_, "", robot_model_->getActiveJointModels()); EXPECT_EQ(robot_model_->getActiveJointModels().size(), container.getCount()); } /** - * @brief Check that the value in the parameter server correctly overrides the + * @brief Check that the value in the node parameters correctly overrides the * position(if within limits) */ TEST_F(JointLimitsAggregator, CorrectOverwriteByParamterPosition) { - ros::NodeHandle nh("~/valid_1"); - pilz_industrial_motion_planner::JointLimitsContainer container = - pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(nh, + pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(node_, "all_valid", robot_model_->getActiveJointModels()); - for (std::pair lim : container) + for (const auto& lim : container) { // Check for the overwrite if (lim.first == "prbt_joint_1") @@ -113,18 +109,16 @@ TEST_F(JointLimitsAggregator, CorrectOverwriteByParamterPosition) } /** - * @brief Check that the value in the parameter server correctly overrides the + * @brief Check that the value in the node parameter correctly overrides the * velocity(if within limits) */ TEST_F(JointLimitsAggregator, CorrectOverwriteByParamterVelocity) { - ros::NodeHandle nh("~/valid_1"); - pilz_industrial_motion_planner::JointLimitsContainer container = - pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(nh, + pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(node_, "all_valid", robot_model_->getActiveJointModels()); - for (std::pair lim : container) + for (const auto& lim : container) { // Check that velocity was only changed in joint "prbt_joint_3" if (lim.first == "prbt_joint_3") @@ -144,13 +138,11 @@ TEST_F(JointLimitsAggregator, CorrectOverwriteByParamterVelocity) */ TEST_F(JointLimitsAggregator, CorrectSettingAccelerationAndDeceleration) { - ros::NodeHandle nh("~/valid_1"); - pilz_industrial_motion_planner::JointLimitsContainer container = - pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(nh, + pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(node_, "all_valid", robot_model_->getActiveJointModels()); - for (std::pair lim : container) + for (const auto& lim : container) { if (lim.first == "prbt_joint_4") { @@ -159,12 +151,12 @@ TEST_F(JointLimitsAggregator, CorrectSettingAccelerationAndDeceleration) } else if (lim.first == "prbt_joint_5") { - EXPECT_EQ(0, container.getLimit(lim.first).max_acceleration) << lim.first; + EXPECT_TRUE(std::isnan(container.getLimit(lim.first).max_acceleration)) << lim.first; EXPECT_EQ(-6.6, container.getLimit(lim.first).max_deceleration) << lim.first; } else { - EXPECT_EQ(0, container.getLimit(lim.first).max_acceleration) << lim.first; + EXPECT_TRUE(std::isnan(container.getLimit(lim.first).max_acceleration)) << lim.first; EXPECT_EQ(0, container.getLimit(lim.first).max_deceleration) << lim.first; } } @@ -175,16 +167,12 @@ TEST_F(JointLimitsAggregator, CorrectSettingAccelerationAndDeceleration) */ TEST_F(JointLimitsAggregator, LimitsViolationPosition) { - ros::NodeHandle nh_min("~/violate_position_min"); - EXPECT_THROW(pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( - nh_min, robot_model_->getActiveJointModels()), + node_, "violate_position_min", robot_model_->getActiveJointModels()), pilz_industrial_motion_planner::AggregationBoundsViolationException); - ros::NodeHandle nh_max("~/violate_position_max"); - EXPECT_THROW(pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( - nh_max, robot_model_->getActiveJointModels()), + node_, "violate_position_max", robot_model_->getActiveJointModels()), pilz_industrial_motion_planner::AggregationBoundsViolationException); } @@ -193,16 +181,14 @@ TEST_F(JointLimitsAggregator, LimitsViolationPosition) */ TEST_F(JointLimitsAggregator, LimitsViolationVelocity) { - ros::NodeHandle nh("~/violate_velocity"); - EXPECT_THROW(pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( - nh, robot_model_->getActiveJointModels()), + node_, "violate_velocity", robot_model_->getActiveJointModels()), pilz_industrial_motion_planner::AggregationBoundsViolationException); } int main(int argc, char** argv) { - ros::init(argc, argv, "unittest_joint_limits_aggregator"); + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_container.cpp similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_container.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_container.cpp diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_validator.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_validator.cpp similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_validator.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_validator.cpp diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp similarity index 74% rename from moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp index f413cb835a..c6065924f2 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp @@ -42,14 +42,18 @@ #include "pilz_industrial_motion_planner/pilz_industrial_motion_planner.h" #include "test_utils.h" -const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; -const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; +#include "rclcpp/rclcpp.hpp" -class CommandPlannerTest : public testing::TestWithParam +static const rclcpp::Logger LOGGER = rclcpp::get_logger("unittest_pilz_industrial_motion_planner"); + +class CommandPlannerTest : public testing::Test { protected: void SetUp() override { + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + node_ = rclcpp::Node::make_shared("unittest_pilz_industrial_motion_planner", node_options); createPlannerInstance(); } @@ -64,22 +68,24 @@ class CommandPlannerTest : public testing::TestWithParam */ void createPlannerInstance() { - // Load planner name from parameter server - if (!ph_.getParam("planning_plugin", planner_plugin_name_)) - { - ROS_FATAL_STREAM("Could not find planner plugin name"); - FAIL(); - } + // load robot model + robot_model_loader::RobotModelLoader rm_loader(node_); + robot_model_ = rm_loader.getModel(); + ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model"; + + // Load planner name from node parameters + ASSERT_TRUE(node_->has_parameter("planning_plugin")) << "Could not find parameter 'planning_plugin'"; + node_->get_parameter("planning_plugin", planner_plugin_name_); // Load the plugin try { - planner_plugin_loader_.reset(new pluginlib::ClassLoader( - "moveit_core", "planning_interface::PlannerManager")); + planner_plugin_loader_ = std::make_unique>( + "moveit_core", "planning_interface::PlannerManager"); } catch (pluginlib::PluginlibException& ex) { - ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what()); + RCLCPP_FATAL_STREAM(LOGGER, "Exception while creating planning plugin loader " << ex.what()); FAIL(); } @@ -87,8 +93,7 @@ class CommandPlannerTest : public testing::TestWithParam try { planner_instance_.reset(planner_plugin_loader_->createUnmanagedInstance(planner_plugin_name_)); - ASSERT_TRUE(planner_instance_->initialize(robot_model_, ph_.getNamespace())) - << "Initialzing the planner instance failed."; + ASSERT_TRUE(planner_instance_->initialize(robot_model_, node_, "")) << "Initialzing the planner instance failed."; } catch (pluginlib::PluginlibException& ex) { @@ -103,29 +108,22 @@ class CommandPlannerTest : public testing::TestWithParam protected: // ros stuff - ros::NodeHandle ph_{ "~" }; - robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() }; + rclcpp::Node::SharedPtr node_; + moveit::core::RobotModelConstPtr robot_model_; std::string planner_plugin_name_; - std::unique_ptr> planner_plugin_loader_; - planning_interface::PlannerManagerPtr planner_instance_; }; -// Instantiate the test cases for robot model with and without gripper -INSTANTIATE_TEST_SUITE_P(InstantiationName, CommandPlannerTest, - ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME)); - /** * @brief Test that PTP can be loaded * This needs to be extended with every new planning Algorithm */ -TEST_P(CommandPlannerTest, ObtainLoadedPlanningAlgorithms) +TEST_F(CommandPlannerTest, ObtainLoadedPlanningAlgorithms) { // Check for the algorithms std::vector algs; - planner_instance_->getPlanningAlgorithms(algs); ASSERT_EQ(3u, algs.size()) << "Found more or less planning algorithms as expected! Found:" << ::testing::PrintToString(algs); @@ -146,7 +144,7 @@ TEST_P(CommandPlannerTest, ObtainLoadedPlanningAlgorithms) * @brief Check that all announced planning algorithms can perform the service * request if the planner_id is set. */ -TEST_P(CommandPlannerTest, CheckValidAlgorithmsForServiceRequest) +TEST_F(CommandPlannerTest, CheckValidAlgorithmsForServiceRequest) { // Check for the algorithms std::vector algs; @@ -165,7 +163,7 @@ TEST_P(CommandPlannerTest, CheckValidAlgorithmsForServiceRequest) * @brief Check that canServiceRequest(req) returns false if planner_id is not * supported */ -TEST_P(CommandPlannerTest, CheckInvalidAlgorithmsForServiceRequest) +TEST_F(CommandPlannerTest, CheckInvalidAlgorithmsForServiceRequest) { planning_interface::MotionPlanRequest req; req.planner_id = "NON_EXISTEND_ALGORITHM_HASH_da39a3ee5e6b4b0d3255bfef95601890afd80709"; @@ -176,7 +174,7 @@ TEST_P(CommandPlannerTest, CheckInvalidAlgorithmsForServiceRequest) /** * @brief Check that canServiceRequest(req) returns false if planner_id is empty */ -TEST_P(CommandPlannerTest, CheckEmptyPlannerIdForServiceRequest) +TEST_F(CommandPlannerTest, CheckEmptyPlannerIdForServiceRequest) { planning_interface::MotionPlanRequest req; req.planner_id = ""; @@ -187,10 +185,10 @@ TEST_P(CommandPlannerTest, CheckEmptyPlannerIdForServiceRequest) /** * @brief Check integrety against empty input */ -TEST_P(CommandPlannerTest, CheckPlanningContextRequestNull) +TEST_F(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)); } @@ -198,10 +196,10 @@ TEST_P(CommandPlannerTest, CheckPlanningContextRequestNull) * @brief Check that for the announced algorithmns getPlanningContext does not * return nullptr */ -TEST_P(CommandPlannerTest, CheckPlanningContextRequest) +TEST_F(CommandPlannerTest, CheckPlanningContextRequest) { - moveit_msgs::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; @@ -218,7 +216,7 @@ TEST_P(CommandPlannerTest, CheckPlanningContextRequest) /** * @brief Check the description can be obtained and is not empty */ -TEST_P(CommandPlannerTest, CheckPlanningContextDescriptionNotEmptyAndStable) +TEST_F(CommandPlannerTest, CheckPlanningContextDescriptionNotEmptyAndStable) { std::string desc = planner_instance_->getDescription(); EXPECT_GT(desc.length(), 0u); @@ -226,7 +224,7 @@ TEST_P(CommandPlannerTest, CheckPlanningContextDescriptionNotEmptyAndStable) int main(int argc, char** argv) { - ros::init(argc, argv, "unittest_pilz_industrial_motion_planner"); + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } 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/unit_tests/src/unittest_pilz_industrial_motion_planner_direct.cpp similarity index 96% rename from moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner_direct.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/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/unit_tests/src/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/unit_tests/src/unittest_planning_context.cpp similarity index 87% rename from moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp index 016c612e0a..ff8799469b 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp @@ -35,8 +35,6 @@ #include #include -#include - #include #include @@ -51,9 +49,6 @@ #include "test_utils.h" -const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; -const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; - // parameters from parameter server const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); const std::string PARAM_TARGET_LINK_NAME("target_link"); @@ -94,11 +89,21 @@ class PlanningContextTest : public ::testing::Test protected: void SetUp() override { + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + node_ = rclcpp::Node::make_shared("unittest_planning_context", node_options); + + // load robot model + robot_model_loader::RobotModelLoader rm_loader(node_); + robot_model_ = rm_loader.getModel(); ASSERT_FALSE(robot_model_ == nullptr) << "There is no robot model!"; // get parameters - ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_)); - ASSERT_TRUE(ph_.getParam(PARAM_TARGET_LINK_NAME, target_link_)); + ASSERT_TRUE(node_->has_parameter(PARAM_PLANNING_GROUP_NAME)) << "Could not find parameter 'planning_group'"; + node_->get_parameter(PARAM_PLANNING_GROUP_NAME, planning_group_); + + ASSERT_TRUE(node_->has_parameter(PARAM_TARGET_LINK_NAME)) << "Could not find parameter 'target_link'"; + node_->get_parameter(PARAM_TARGET_LINK_NAME, target_link_); pilz_industrial_motion_planner::JointLimitsContainer joint_limits = testutils::createFakeLimits(robot_model_->getVariableNames()); @@ -117,7 +122,7 @@ class PlanningContextTest : public ::testing::Test // Define and set the current scene planning_scene::PlanningScenePtr scene(new planning_scene::PlanningScene(robot_model_)); - robot_state::RobotState current_state(robot_model_); + moveit::core::RobotState current_state(robot_model_); current_state.setToDefaultValues(); current_state.setJointGroupPositions(planning_group_, { 0, 1.57, 1.57, 0, 0.2, 0 }); scene->setCurrentState(current_state); @@ -138,7 +143,7 @@ class PlanningContextTest : public ::testing::Test req.max_acceleration_scaling_factor = 0.01; // start state - robot_state::RobotState rstate(this->robot_model_); + moveit::core::RobotState rstate(this->robot_model_); rstate.setToDefaultValues(); // state state in joint space, used as initial positions, since IK does not // work at zero positions @@ -162,9 +167,9 @@ class PlanningContextTest : public ::testing::Test // path constraint req.path_constraints.name = "center"; - moveit_msgs::PositionConstraint center_point; + moveit_msgs::msg::PositionConstraint center_point; center_point.link_name = this->target_link_; - geometry_msgs::Pose center_position; + geometry_msgs::msg::Pose center_position; center_position.position.x = 0.0; center_position.position.y = 0.0; center_position.position.z = 0.65; @@ -176,11 +181,8 @@ class PlanningContextTest : public ::testing::Test protected: // ros stuff - ros::NodeHandle ph_{ "~" }; - robot_model::RobotModelConstPtr robot_model_{ - robot_model_loader::RobotModelLoader(!T::VALUE ? PARAM_MODEL_NO_GRIPPER_NAME : PARAM_MODEL_WITH_GRIPPER_NAME) - .getModel() - }; + rclcpp::Node::SharedPtr node_; + moveit::core::RobotModelConstPtr robot_model_; std::unique_ptr planning_context_; @@ -188,7 +190,7 @@ class PlanningContextTest : public ::testing::Test }; // Define the types we need to test -TYPED_TEST_SUITE(PlanningContextTest, PlanningContextTestTypes); +TYPED_TEST_SUITE(PlanningContextTest, PlanningContextTestTypes, /* ... */); /** * @brief No request is set. Check the output of solve. Using robot model @@ -200,7 +202,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 +220,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 +243,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 +263,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()); } @@ -275,8 +277,7 @@ TYPED_TEST(PlanningContextTest, Clear) int main(int argc, char** argv) { - ros::init(argc, argv, "unittest_planning_context"); - // ros::NodeHandle nh; + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context_loaders.cpp similarity index 71% rename from moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context_loaders.cpp index e0ecdcda0e..39d25365d3 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context_loaders.cpp @@ -46,8 +46,9 @@ #include "test_utils.h" -const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; -const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; +#include "rclcpp/rclcpp.hpp" + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("unittest_planning_context_loader"); class PlanningContextLoadersTest : public ::testing::TestWithParam> { @@ -60,18 +61,25 @@ class PlanningContextLoadersTest : public ::testing::TestWithParam( - "pilz_industrial_motion_planner", "pilz_industrial_motion_planner::PlanningContextLoader")); + planning_context_loader_class_loader_ = + std::make_unique>( + "pilz_industrial_motion_planner", "pilz_industrial_motion_planner::PlanningContextLoader"); } catch (pluginlib::PluginlibException& ex) { - ROS_FATAL_STREAM("Exception while creating planning context loader " << ex.what()); + RCLCPP_FATAL_STREAM(LOGGER, "Exception while creating planning context loader " << ex.what()); FAIL(); } @@ -96,11 +104,11 @@ class PlanningContextLoadersTest : public ::testing::TestWithParam> + std::unique_ptr> planning_context_loader_class_loader_; pilz_industrial_motion_planner::PlanningContextLoaderPtr planning_context_loader_; @@ -110,19 +118,11 @@ class PlanningContextLoadersTest : public ::testing::TestWithParam{ "pilz_industrial_motion_planner::PlanningContextLoaderPTP", "PTP", - PARAM_MODEL_NO_GRIPPER_NAME }, // Test for PTP - std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderPTP", "PTP", - PARAM_MODEL_WITH_GRIPPER_NAME }, // Test for PTP - std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderLIN", "LIN", - PARAM_MODEL_NO_GRIPPER_NAME }, // Test for LIN - std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderLIN", "LIN", - PARAM_MODEL_WITH_GRIPPER_NAME }, // Test for LIN - std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderCIRC", "CIRC", - PARAM_MODEL_NO_GRIPPER_NAME }, // Test for CIRC - std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderCIRC", "CIRC", - PARAM_MODEL_WITH_GRIPPER_NAME } // Test for CIRC - )); + ::testing::Values( + std::vector{ "pilz_industrial_motion_planner/PlanningContextLoaderPTP", "PTP" }, // Test for PTP + std::vector{ "pilz_industrial_motion_planner/PlanningContextLoaderLIN", "LIN" }, // Test for LIN + std::vector{ "pilz_industrial_motion_planner/PlanningContextLoaderCIRC", "CIRC" } // Test for CIRC + )); /** * @brief Test getAlgorithm returns PTP @@ -130,7 +130,7 @@ INSTANTIATE_TEST_SUITE_P( TEST_P(PlanningContextLoadersTest, GetAlgorithm) { std::string alg = planning_context_loader_->getAlgorithm(); - EXPECT_EQ(alg, GetParam().at(1)); + EXPECT_EQ(alg, GetParam().back()); } /** @@ -173,7 +173,7 @@ TEST_P(PlanningContextLoadersTest, LoadContext) int main(int argc, char** argv) { - ros::init(argc, argv, "unittest_planning_context_loaders"); + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp similarity index 77% rename from moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp index dbec61604f..37f48ba94f 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp @@ -40,7 +40,6 @@ #include #include #include -#include #if __has_include() #include #else @@ -63,41 +62,106 @@ #include "pilz_industrial_motion_planner/trajectory_generator_lin.h" #include "test_utils.h" -const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; -const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; - -// parameters from parameter server -const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); -const std::string PARAM_TARGET_LINK_NAME("target_link"); -const std::string CARTESIAN_VELOCITY_TOLERANCE("cartesian_velocity_tolerance"); -const std::string CARTESIAN_ANGULAR_VELOCITY_TOLERANCE("cartesian_angular_velocity_tolerance"); -const std::string JOINT_VELOCITY_TOLERANCE("joint_velocity_tolerance"); -const std::string JOINT_ACCELERATION_TOLERANCE("joint_acceleration_tolerance"); -const std::string OTHER_TOLERANCE("other_tolerance"); -const std::string SAMPLING_TIME("sampling_time"); -const std::string TEST_DATA_FILE_NAME("testdata_file_name"); +#include "rclcpp/rclcpp.hpp" using namespace pilz_industrial_motion_planner; using namespace pilz_industrial_motion_planner_testutils; -class TrajectoryBlenderTransitionWindowTest : public testing::TestWithParam +static const std::string PARAM_NAMESPACE_LIMITS = "robot_description_planning"; +class TrajectoryBlenderTransitionWindowTest : public testing::Test { protected: /** * @brief Create test scenario for trajectory blender * */ - void SetUp() override; + void SetUp() override + { + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + node_ = rclcpp::Node::make_shared("unittest_trajectory_blender_transition_window", node_options); + + // load robot model + robot_model_loader::RobotModelLoader rm_loader(node_); + robot_model_ = rm_loader.getModel(); + ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model"; + planning_scene_ = std::make_shared(robot_model_); + + // get parameters + ASSERT_TRUE(node_->has_parameter("planning_group")); + node_->get_parameter("planning_group", planning_group_); + ASSERT_TRUE(node_->has_parameter("target_link")); + node_->get_parameter("target_link", target_link_); + ASSERT_TRUE(node_->has_parameter("cartesian_velocity_tolerance")); + node_->get_parameter("cartesian_velocity_tolerance", cartesian_velocity_tolerance_); + ASSERT_TRUE(node_->has_parameter("cartesian_angular_velocity_tolerance")); + node_->get_parameter("cartesian_angular_velocity_tolerance", cartesian_angular_velocity_tolerance_); + ASSERT_TRUE(node_->has_parameter("joint_velocity_tolerance")); + node_->get_parameter("joint_velocity_tolerance", joint_velocity_tolerance_); + ASSERT_TRUE(node_->has_parameter("joint_acceleration_tolerance")); + node_->get_parameter("joint_acceleration_tolerance", joint_acceleration_tolerance_); + ASSERT_TRUE(node_->has_parameter("sampling_time")); + node_->get_parameter("sampling_time", sampling_time_); + ASSERT_TRUE(node_->has_parameter("testdata_file_name")); + node_->get_parameter("testdata_file_name", test_data_file_name_); + + // load the test data provider + data_loader_ = std::make_unique(test_data_file_name_, robot_model_); + ASSERT_NE(nullptr, data_loader_) << "Failed to load test data by provider."; + + // check robot model + testutils::checkRobotModel(robot_model_, planning_group_, target_link_); + + // create the limits container + pilz_industrial_motion_planner::JointLimitsContainer joint_limits = + pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( + node_, PARAM_NAMESPACE_LIMITS, robot_model_->getActiveJointModels()); + CartesianLimit cart_limits; + cart_limits.setMaxRotationalVelocity(1 * M_PI); + cart_limits.setMaxTranslationalAcceleration(2); + cart_limits.setMaxTranslationalDeceleration(2); + cart_limits.setMaxTranslationalVelocity(1); + planner_limits_.setJointLimits(joint_limits); + planner_limits_.setCartesianLimits(cart_limits); + + // initialize trajectory generators and blender + lin_generator_ = std::make_unique(robot_model_, planner_limits_); + ASSERT_NE(nullptr, lin_generator_) << "failed to create LIN trajectory generator"; + blender_ = std::make_unique(planner_limits_); + ASSERT_NE(nullptr, blender_) << "failed to create trajectory blender"; + } /** * @brief Generate lin trajectories for blend sequences */ - std::vector generateLinTrajs(const Sequence& seq, size_t num_cmds); + std::vector generateLinTrajs(const Sequence& seq, size_t num_cmds) + { + std::vector responses(num_cmds); + for (size_t index = 0; index < num_cmds; ++index) + { + planning_interface::MotionPlanRequest req{ seq.getCmd(index).toRequest() }; + // Set start state of request to end state of previous trajectory (except + // for first) + if (index > 0) + { + moveit::core::robotStateToRobotStateMsg(responses[index - 1].trajectory_->getLastWayPoint(), req.start_state); + } + // generate trajectory + planning_interface::MotionPlanResponse resp; + if (!lin_generator_->generate(planning_scene_, req, resp, sampling_time_)) + { + std::runtime_error("Failed to generate trajectory."); + } + responses.at(index) = resp; + } + return responses; + } protected: // ros stuff - ros::NodeHandle ph_{ "~" }; - robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() }; + rclcpp::Node::SharedPtr node_; + moveit::core::RobotModelConstPtr robot_model_; + planning_scene::PlanningSceneConstPtr planning_scene_; std::unique_ptr lin_generator_; std::unique_ptr blender_; @@ -112,73 +176,6 @@ class TrajectoryBlenderTransitionWindowTest : public testing::TestWithParamgetActiveJointModels()); - CartesianLimit cart_limits; - cart_limits.setMaxRotationalVelocity(1 * M_PI); - cart_limits.setMaxTranslationalAcceleration(2); - cart_limits.setMaxTranslationalDeceleration(2); - cart_limits.setMaxTranslationalVelocity(1); - planner_limits_.setJointLimits(joint_limits); - planner_limits_.setCartesianLimits(cart_limits); - - // initialize trajectory generators and blender - lin_generator_.reset(new TrajectoryGeneratorLIN(robot_model_, planner_limits_)); - ASSERT_NE(nullptr, lin_generator_) << "failed to create LIN trajectory generator"; - blender_.reset(new TrajectoryBlenderTransitionWindow(planner_limits_)); - ASSERT_NE(nullptr, blender_) << "failed to create trajectory blender"; -} - -std::vector -TrajectoryBlenderTransitionWindowTest::generateLinTrajs(const Sequence& seq, size_t num_cmds) -{ - std::vector responses(num_cmds); - - for (size_t index = 0; index < num_cmds; ++index) - { - planning_interface::MotionPlanRequest req{ seq.getCmd(index).toRequest() }; - // Set start state of request to end state of previous trajectory (except - // for first) - if (index > 0) - { - moveit::core::robotStateToRobotStateMsg(responses[index - 1].trajectory_->getLastWayPoint(), req.start_state); - } - // generate trajectory - planning_interface::MotionPlanResponse resp; - if (!lin_generator_->generate(req, resp, sampling_time_)) - { - std::runtime_error("Failed to generate trajectory."); - } - responses.at(index) = resp; - } - return responses; -} - -// Instantiate the test cases for robot model with and without gripper -INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryBlenderTransitionWindowTest, - ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME)); - /** * @brief Tests the blending of two trajectories with an invalid group name. * @@ -190,7 +187,7 @@ INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryBlenderTransitionWindowTes * 1. Two linear trajectories generated. * 2. Blending trajectory cannot be generated. */ -TEST_P(TrajectoryBlenderTransitionWindowTest, testInvalidGroupName) +TEST_F(TrajectoryBlenderTransitionWindowTest, testInvalidGroupName) { Sequence seq{ data_loader_->getSequence("SimpleSequence") }; @@ -205,7 +202,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testInvalidGroupName) blend_req.second_trajectory = res.at(1).trajectory_; blend_req.blend_radius = seq.getBlendRadius(0); - EXPECT_FALSE(blender_->blend(blend_req, blend_res)); + EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res)); } /** @@ -219,7 +216,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testInvalidGroupName) * 1. Two linear trajectories generated. * 2. Blending trajectory cannot be generated. */ -TEST_P(TrajectoryBlenderTransitionWindowTest, testInvalidTargetLink) +TEST_F(TrajectoryBlenderTransitionWindowTest, testInvalidTargetLink) { Sequence seq{ data_loader_->getSequence("SimpleSequence") }; @@ -234,7 +231,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testInvalidTargetLink) blend_req.second_trajectory = res.at(1).trajectory_; blend_req.blend_radius = seq.getBlendRadius(0); - EXPECT_FALSE(blender_->blend(blend_req, blend_res)); + EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res)); } /** @@ -249,7 +246,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testInvalidTargetLink) * 1. Two linear trajectories generated. * 2. Blending trajectory cannot be generated. */ -TEST_P(TrajectoryBlenderTransitionWindowTest, testNegativeRadius) +TEST_F(TrajectoryBlenderTransitionWindowTest, testNegativeRadius) { Sequence seq{ data_loader_->getSequence("SimpleSequence") }; @@ -264,7 +261,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNegativeRadius) blend_req.second_trajectory = res.at(1).trajectory_; blend_req.blend_radius = -0.1; - EXPECT_FALSE(blender_->blend(blend_req, blend_res)); + EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res)); } /** @@ -278,7 +275,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNegativeRadius) * 1. Two linear trajectories generated. * 2. Blending trajectory cannot be generated. */ -TEST_P(TrajectoryBlenderTransitionWindowTest, testZeroRadius) +TEST_F(TrajectoryBlenderTransitionWindowTest, testZeroRadius) { Sequence seq{ data_loader_->getSequence("SimpleSequence") }; @@ -293,7 +290,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testZeroRadius) blend_req.second_trajectory = res.at(1).trajectory_; blend_req.blend_radius = 0.; - EXPECT_FALSE(blender_->blend(blend_req, blend_res)); + EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res)); } /** @@ -308,7 +305,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testZeroRadius) * 1. Two linear trajectories generated. * 2. Blending trajectory cannot be generated. */ -TEST_P(TrajectoryBlenderTransitionWindowTest, testDifferentSamplingTimes) +TEST_F(TrajectoryBlenderTransitionWindowTest, testDifferentSamplingTimes) { Sequence seq{ data_loader_->getSequence("SimpleSequence") }; @@ -328,7 +325,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testDifferentSamplingTimes) } // generate trajectory planning_interface::MotionPlanResponse resp; - if (!lin_generator_->generate(req, resp, sampling_time_)) + if (!lin_generator_->generate(planning_scene_, req, resp, sampling_time_)) { std::runtime_error("Failed to generate trajectory."); } @@ -343,7 +340,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testDifferentSamplingTimes) blend_req.first_trajectory = responses[0].trajectory_; blend_req.second_trajectory = responses[1].trajectory_; blend_req.blend_radius = seq.getBlendRadius(0); - EXPECT_FALSE(blender_->blend(blend_req, blend_res)); + EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res)); } /** @@ -360,7 +357,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testDifferentSamplingTimes) * 1. Two linear trajectories generated. * 2. Blending trajectory cannot be generated. */ -TEST_P(TrajectoryBlenderTransitionWindowTest, testNonUniformSamplingTime) +TEST_F(TrajectoryBlenderTransitionWindowTest, testNonUniformSamplingTime) { Sequence seq{ data_loader_->getSequence("SimpleSequence") }; @@ -378,7 +375,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNonUniformSamplingTime) blend_req.first_trajectory = res.at(0).trajectory_; blend_req.second_trajectory = res.at(1).trajectory_; blend_req.blend_radius = seq.getBlendRadius(0); - EXPECT_FALSE(blender_->blend(blend_req, blend_res)); + EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res)); } /** @@ -394,7 +391,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNonUniformSamplingTime) * 2. Two trajectories that do not intersect. * 2. Blending trajectory cannot be generated. */ -TEST_P(TrajectoryBlenderTransitionWindowTest, testNotIntersectingTrajectories) +TEST_F(TrajectoryBlenderTransitionWindowTest, testNotIntersectingTrajectories) { Sequence seq{ data_loader_->getSequence("SimpleSequence") }; @@ -410,7 +407,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNotIntersectingTrajectories) // intersect blend_req.second_trajectory = res.at(0).trajectory_; blend_req.blend_radius = seq.getBlendRadius(0); - EXPECT_FALSE(blender_->blend(blend_req, blend_res)); + EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res)); } /** @@ -425,7 +422,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNotIntersectingTrajectories) * 1. Two trajectories generated. * 2. Blending trajectory cannot be generated. */ -TEST_P(TrajectoryBlenderTransitionWindowTest, testNonStationaryPoint) +TEST_F(TrajectoryBlenderTransitionWindowTest, testNonStationaryPoint) { Sequence seq{ data_loader_->getSequence("SimpleSequence") }; @@ -446,7 +443,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNonStationaryPoint) blend_req.first_trajectory->getLastWayPointPtr()->setVariableVelocity(0, 1.0); blend_req.second_trajectory->getFirstWayPointPtr()->setVariableVelocity(0, 1.0); - EXPECT_FALSE(blender_->blend(blend_req, blend_res)); + EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res)); } /** @@ -462,7 +459,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNonStationaryPoint) * 1. Two trajectories generated. * 2. Blending trajectory cannot be generated. */ -TEST_P(TrajectoryBlenderTransitionWindowTest, testTraj1InsideBlendRadius) +TEST_F(TrajectoryBlenderTransitionWindowTest, testTraj1InsideBlendRadius) { Sequence seq{ data_loader_->getSequence("SimpleSequence") }; @@ -483,7 +480,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testTraj1InsideBlendRadius) blend_req.first_trajectory = res.at(0).trajectory_; blend_req.second_trajectory = res.at(1).trajectory_; - EXPECT_FALSE(blender_->blend(blend_req, blend_res)); + EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res)); } /** @@ -499,7 +496,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testTraj1InsideBlendRadius) * 1. Two trajectories generated. * 2. Blending trajectory cannot be generated. */ -TEST_P(TrajectoryBlenderTransitionWindowTest, testTraj2InsideBlendRadius) +TEST_F(TrajectoryBlenderTransitionWindowTest, testTraj2InsideBlendRadius) { Sequence seq{ data_loader_->getSequence("NoIntersectionTraj2") }; @@ -515,7 +512,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testTraj2InsideBlendRadius) blend_req.first_trajectory = res.at(0).trajectory_; blend_req.second_trajectory = res.at(1).trajectory_; - EXPECT_FALSE(blender_->blend(blend_req, blend_res)); + EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res)); } /** @@ -536,7 +533,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testTraj2InsideBlendRadius) * 3. No bound is violated, the trajectories are continuous * in joint and cartesian space. */ -TEST_P(TrajectoryBlenderTransitionWindowTest, testLinLinBlending) +TEST_F(TrajectoryBlenderTransitionWindowTest, testLinLinBlending) { Sequence seq{ data_loader_->getSequence("SimpleSequence") }; @@ -552,7 +549,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testLinLinBlending) blend_req.first_trajectory = res.at(0).trajectory_; blend_req.second_trajectory = res.at(1).trajectory_; - EXPECT_TRUE(blender_->blend(blend_req, blend_res)); + EXPECT_TRUE(blender_->blend(planning_scene_, blend_req, blend_res)); EXPECT_TRUE(testutils::checkBlendResult(blend_req, blend_res, planner_limits_, joint_velocity_tolerance_, joint_acceleration_tolerance_, cartesian_velocity_tolerance_, @@ -579,7 +576,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testLinLinBlending) * 3. No bound is violated, the trajectories are continuous * in joint and cartesian space. */ -TEST_P(TrajectoryBlenderTransitionWindowTest, testOverlappingBlendTrajectories) +TEST_F(TrajectoryBlenderTransitionWindowTest, testOverlappingBlendTrajectories) { Sequence seq{ data_loader_->getSequence("SimpleSequence") }; // Set goal of second traj to start of first traj. @@ -595,7 +592,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testOverlappingBlendTrajectories) blend_req.first_trajectory = res.at(0).trajectory_; blend_req.second_trajectory = res.at(1).trajectory_; blend_req.blend_radius = seq.getBlendRadius(0); - EXPECT_TRUE(blender_->blend(blend_req, blend_res)); + EXPECT_TRUE(blender_->blend(planning_scene_, blend_req, blend_res)); EXPECT_TRUE(testutils::checkBlendResult(blend_req, blend_res, planner_limits_, joint_velocity_tolerance_, joint_acceleration_tolerance_, cartesian_velocity_tolerance_, @@ -625,7 +622,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testOverlappingBlendTrajectories) * 4. No bound is violated, the trajectories are continuous * in joint and cartesian space. */ -TEST_P(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending) +TEST_F(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending) { const double sine_scaling_factor{ 0.01 }; const double time_scaling_factor{ 10 }; @@ -642,7 +639,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending) auto lin_traj{ res.at(traj_index).trajectory_ }; CartesianTrajectory cart_traj; - trajectory_msgs::JointTrajectory joint_traj; + trajectory_msgs::msg::JointTrajectory joint_traj; const double duration{ lin_traj->getWayPointDurationFromStart(lin_traj->getWayPointCount()) }; // time from start zero does not work const double time_from_start_offset{ time_scaling_factor * lin_traj->getWayPointDurations().back() }; @@ -656,7 +653,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending) // get pose CartesianTrajectoryPoint waypoint; const Eigen::Isometry3d eigen_pose{ lin_traj->getWayPointPtr(i)->getFrameTransform(target_link_) }; - geometry_msgs::Pose waypoint_pose = tf2::toMsg(eigen_pose); + geometry_msgs::msg::Pose waypoint_pose = tf2::toMsg(eigen_pose); // add scaled sine function waypoint_pose.position.x += sine_scaling_factor * sin(sine_arg); @@ -665,8 +662,8 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending) // add to trajectory waypoint.pose = waypoint_pose; - waypoint.time_from_start = - ros::Duration(time_from_start_offset + time_scaling_factor * lin_traj->getWayPointDurationFromStart(i)); + waypoint.time_from_start = rclcpp::Duration::from_seconds( + time_from_start_offset + time_scaling_factor * lin_traj->getWayPointDurationFromStart(i)); cart_traj.points.push_back(waypoint); } @@ -689,8 +686,8 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending) } } - moveit_msgs::MoveItErrorCodes error_code; - if (!generateJointTrajectory(robot_model_, planner_limits_.getJointLimitContainer(), cart_traj, planning_group_, + moveit_msgs::msg::MoveItErrorCodes error_code; + if (!generateJointTrajectory(planning_scene_, planner_limits_.getJointLimitContainer(), cart_traj, planning_group_, target_link_, initial_joint_position, initial_joint_velocity, joint_traj, error_code, true)) { @@ -700,8 +697,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending) joint_traj.points.back().velocities.assign(joint_traj.points.back().velocities.size(), 0.0); joint_traj.points.back().accelerations.assign(joint_traj.points.back().accelerations.size(), 0.0); - // convert trajectory_msgs::JointTrajectory to - // robot_trajectory::RobotTrajectory + // convert trajectory_msgs::JointTrajectory to robot_trajectory::RobotTrajectory sine_trajs[traj_index] = std::make_shared(robot_model_, planning_group_); sine_trajs.at(traj_index)->setRobotTrajectoryMsg(lin_traj->getFirstWayPoint(), joint_traj); } @@ -716,7 +712,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending) blend_req.first_trajectory = sine_trajs.at(0); blend_req.second_trajectory = sine_trajs.at(1); - EXPECT_TRUE(blender_->blend(blend_req, blend_res)); + EXPECT_TRUE(blender_->blend(planning_scene_, blend_req, blend_res)); EXPECT_TRUE(testutils::checkBlendResult(blend_req, blend_res, planner_limits_, joint_velocity_tolerance_, joint_acceleration_tolerance_, cartesian_velocity_tolerance_, @@ -725,8 +721,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending) int main(int argc, char** argv) { - ros::init(argc, argv, "unittest_trajectory_blender_transition_window"); - ros::NodeHandle nh; + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp similarity index 76% rename from moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp index 9678580556..26ae21852e 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include @@ -50,8 +51,6 @@ #include #include #include -#include -#include #if __has_include() #include #else @@ -78,9 +77,6 @@ static constexpr double L1{ 0.3500 }; // Height of first connector static constexpr double L2{ 0.3070 }; // Height of second connector static constexpr double L3{ 0.0840 }; // Distance last joint to flange -const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; -const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; - // parameters from parameter server const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); const std::string GROUP_TIP_LINK_NAME("group_tip_link"); @@ -91,14 +87,47 @@ const std::string RANDOM_TEST_NUMBER("random_test_number"); /** * @brief test fixtures base class */ -class TrajectoryFunctionsTestBase : public testing::TestWithParam +class TrajectoryFunctionsTestBase : public testing::Test { protected: /** * @brief Create test scenario for trajectory functions * */ - void SetUp() override; + void SetUp() override + { + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + node_ = rclcpp::Node::make_shared("unittest_trajectory_functions", node_options); + + // load robot model + robot_model_loader::RobotModelLoader rm_loader(node_); + robot_model_ = rm_loader.getModel(); + ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model"; + planning_scene_ = std::make_shared(robot_model_); + + // get parameters + ASSERT_TRUE(node_->has_parameter("planning_group")); + node_->get_parameter("planning_group", planning_group_); + ASSERT_TRUE(node_->has_parameter("group_tip_link")); + node_->get_parameter("group_tip_link", group_tip_link_); + ASSERT_TRUE(node_->has_parameter("tcp_link")); + node_->get_parameter("tcp_link", tcp_link_); + ASSERT_TRUE(node_->has_parameter("ik_fast_link")); + node_->get_parameter("ik_fast_link", ik_fast_link_); + ASSERT_TRUE(node_->has_parameter("random_test_number")); + node_->get_parameter("random_test_number", random_test_number_); + + // check robot model + testutils::checkRobotModel(robot_model_, planning_group_, tcp_link_); + + // initialize the zero state configurationg and test joint state + joint_names_ = robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames(); + for (const auto& joint_name : joint_names_) + { + zero_state_[joint_name] = 0.0; + } + } /** * @brief check if two transformations are close @@ -111,8 +140,9 @@ class TrajectoryFunctionsTestBase : public testing::TestWithParam protected: // ros stuff - ros::NodeHandle ph_{ "~" }; - robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() }; + rclcpp::Node::SharedPtr node_; + moveit::core::RobotModelConstPtr robot_model_; + planning_scene::PlanningSceneConstPtr planning_scene_; // test parameters from parameter server std::string planning_group_, group_tip_link_, tcp_link_, ik_fast_link_; @@ -125,26 +155,6 @@ class TrajectoryFunctionsTestBase : public testing::TestWithParam random_numbers::RandomNumberGenerator rng_{ random_seed_ }; }; -void TrajectoryFunctionsTestBase::SetUp() -{ - // parameters - ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_)); - ASSERT_TRUE(ph_.getParam(GROUP_TIP_LINK_NAME, group_tip_link_)); - ASSERT_TRUE(ph_.getParam(ROBOT_TCP_LINK_NAME, tcp_link_)); - ASSERT_TRUE(ph_.getParam(IK_FAST_LINK_NAME, ik_fast_link_)); - ASSERT_TRUE(ph_.getParam(RANDOM_TEST_NUMBER, random_test_number_)); - - // check robot model - testutils::checkRobotModel(robot_model_, planning_group_, tcp_link_); - - // initialize the zero state configurationg and test joint state - joint_names_ = robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames(); - for (const auto& joint_name : joint_names_) - { - zero_state_[joint_name] = 0.0; - } -} - bool TrajectoryFunctionsTestBase::tfNear(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2, const double& epsilon) { @@ -164,27 +174,20 @@ class TrajectoryFunctionsTestFlangeAndGripper : public TrajectoryFunctionsTestBa { }; -/** - * @brief Parametrized class for tests, that only run with a gripper - */ -class TrajectoryFunctionsTestOnlyGripper : public TrajectoryFunctionsTestBase -{ -}; - -// Instantiate the test cases for robot model with and without gripper -INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryFunctionsTestFlangeAndGripper, - ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME)); - -// Instantiate the test cases for robot model with a gripper -INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryFunctionsTestOnlyGripper, - ::testing::Values(PARAM_MODEL_WITH_GRIPPER_NAME)); +// TODO(henningkayser): re-enable gripper tests +// /** +// * @brief Parametrized class for tests, that only run with a gripper +// */ +// class TrajectoryFunctionsTestOnlyGripper : public TrajectoryFunctionsTestBase +// { +// }; /** * @brief Test the forward kinematics function with simple robot poses for robot * tip link * using robot model without gripper. */ -TEST_P(TrajectoryFunctionsTestFlangeAndGripper, TipLinkFK) +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, TipLinkFK) { Eigen::Isometry3d tip_pose; std::map test_state = zero_state_; @@ -214,24 +217,24 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, TipLinkFK) /** * @brief Test the inverse kinematics directly through ikfast solver */ -TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIKSolver) +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKSolver) { // Load solver - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance(); // robot state - robot_state::RobotState rstate(robot_model_); + moveit::core::RobotState rstate(robot_model_); while (random_test_number_ > 0) { // sample random robot state rstate.setToRandomPositions(jmg, rng_); rstate.update(); - geometry_msgs::Pose pose_expect = tf2::toMsg(rstate.getFrameTransform(ik_fast_link_)); + geometry_msgs::msg::Pose pose_expect = tf2::toMsg(rstate.getFrameTransform(ik_fast_link_)); // prepare inverse kinematics - std::vector ik_poses; + std::vector ik_poses; ik_poses.push_back(pose_expect); std::vector ik_seed, ik_expect, ik_actual; for (const auto& joint_name : jmg->getActiveJointModelNames()) @@ -245,7 +248,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 @@ -269,11 +272,11 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIKSolver) * @brief Test the inverse kinematics using RobotState class (setFromIK) using * robot model */ -TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotState) +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotState) { // robot state - robot_state::RobotState rstate(robot_model_); - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); + moveit::core::RobotState rstate(robot_model_); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); while (random_test_number_ > 0) { @@ -327,13 +330,13 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotState) * @brief Test the wrapper function to compute inverse kinematics using robot * model */ -TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIK) +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIK) { // robot state - robot_state::RobotState rstate(robot_model_); + moveit::core::RobotState rstate(robot_model_); const std::string frame_id = robot_model_->getModelFrame(); - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); while (random_test_number_ > 0) { @@ -359,7 +362,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIK) // compute the ik std::map ik_actual; - EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect, + EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, tcp_link_, pose_expect, frame_id, ik_seed, ik_actual, false)); // compare ik solution and expected value @@ -375,7 +378,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIK) /** * @brief Test computePoseIK for invalid group_name */ -TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidGroupName) +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidGroupName) { const std::string frame_id = robot_model_->getModelFrame(); Eigen::Isometry3d pose_expect; @@ -384,14 +387,14 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidGroupNam // compute the ik std::map ik_actual; - EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(robot_model_, "InvalidGroupName", tcp_link_, pose_expect, - frame_id, ik_seed, ik_actual, false)); + EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, "InvalidGroupName", tcp_link_, + pose_expect, frame_id, ik_seed, ik_actual, false)); } /** * @brief Test computePoseIK for invalid link_name */ -TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidLinkName) +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidLinkName) { const std::string frame_id = robot_model_->getModelFrame(); Eigen::Isometry3d pose_expect; @@ -400,7 +403,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidLinkName // compute the ik std::map ik_actual; - EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, "WrongLink", pose_expect, + EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, "WrongLink", pose_expect, frame_id, ik_seed, ik_actual, false)); } @@ -409,7 +412,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidLinkName * * Currently only robot_model_->getModelFrame() == frame_id */ -TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidFrameId) +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidFrameId) { Eigen::Isometry3d pose_expect; @@ -417,94 +420,94 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidFrameId) // compute the ik std::map ik_actual; - EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect, + EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, tcp_link_, pose_expect, "InvalidFrameId", ik_seed, ik_actual, false)); } -/** - * @brief Test if activated self collision for a pose that would be in self - * collision without the check results in a - * valid ik solution. - */ -TEST_P(TrajectoryFunctionsTestOnlyGripper, testComputePoseIKSelfCollisionForValidPosition) -{ - const std::string frame_id = robot_model_->getModelFrame(); - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); - - // create seed - std::vector ik_seed_states = { -0.553, 0.956, 1.758, 0.146, -1.059, 1.247 }; - auto joint_names = jmg->getActiveJointModelNames(); - - std::map ik_seed; - for (unsigned int i = 0; i < ik_seed_states.size(); ++i) - { - ik_seed[joint_names[i]] = ik_seed_states[i]; - } - - // create expected pose - geometry_msgs::Pose pose; - pose.position.x = -0.454; - pose.position.y = -0.15; - pose.position.z = 0.431; - pose.orientation.y = 0.991562; - pose.orientation.w = -0.1296328; - Eigen::Isometry3d pose_expect; - normalizeQuaternion(pose.orientation); - tf2::fromMsg(pose, pose_expect); - - // compute the ik without self collision check and expect the resulting pose - // to be in self collission. - std::map ik_actual1; - EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect, - frame_id, ik_seed, ik_actual1, false)); - - robot_state::RobotState rstate(robot_model_); - planning_scene::PlanningScene rscene(robot_model_); - - std::vector ik_state; - std::transform(ik_actual1.begin(), ik_actual1.end(), std::back_inserter(ik_state), - boost::bind(&std::map::value_type::second, _1)); - - rstate.setJointGroupPositions(jmg, ik_state); - rstate.update(); - - collision_detection::CollisionRequest collision_req; - collision_req.group_name = jmg->getName(); - collision_detection::CollisionResult collision_res; - - rscene.checkSelfCollision(collision_req, collision_res, rstate); - - EXPECT_TRUE(collision_res.collision); - - // compute the ik with collision detection activated and expect the resulting - // pose to be without self collision. - std::map ik_actual2; - EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect, - frame_id, ik_seed, ik_actual2, true)); - - std::vector ik_state2; - std::transform(ik_actual2.begin(), ik_actual2.end(), std::back_inserter(ik_state2), - boost::bind(&std::map::value_type::second, _1)); - rstate.setJointGroupPositions(jmg, ik_state2); - rstate.update(); - - collision_detection::CollisionResult collision_res2; - rscene.checkSelfCollision(collision_req, collision_res2, rstate); - - EXPECT_FALSE(collision_res2.collision); -} +// /** +// * @brief Test if activated self collision for a pose that would be in self +// * collision without the check results in a +// * valid ik solution. +// */ +// TEST_F(TrajectoryFunctionsTestOnlyGripper, testComputePoseIKSelfCollisionForValidPosition) +// { +// const std::string frame_id = robot_model_->getModelFrame(); +// const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); +// +// // create seed +// std::vector ik_seed_states = { -0.553, 0.956, 1.758, 0.146, -1.059, 1.247 }; +// auto joint_names = jmg->getActiveJointModelNames(); +// +// std::map ik_seed; +// for (unsigned int i = 0; i < ik_seed_states.size(); ++i) +// { +// ik_seed[joint_names[i]] = ik_seed_states[i]; +// } +// +// // create expected pose +// geometry_msgs::msg::Pose pose; +// pose.position.x = -0.454; +// pose.position.y = -0.15; +// pose.position.z = 0.431; +// pose.orientation.y = 0.991562; +// pose.orientation.w = -0.1296328; +// Eigen::Isometry3d pose_expect; +// normalizeQuaternion(pose.orientation); +// tf2::fromMsg(pose, pose_expect); +// +// // compute the ik without self collision check and expect the resulting pose +// // to be in self collission. +// std::map ik_actual1; +// EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, tcp_link_, pose_expect, +// frame_id, ik_seed, ik_actual1, false)); +// +// moveit::core::RobotState rstate(robot_model_); +// planning_scene::PlanningScene rscene(robot_model_); +// +// std::vector ik_state; +// std::transform(ik_actual1.begin(), ik_actual1.end(), std::back_inserter(ik_state), +// boost::bind(&std::map::value_type::second, _1)); +// +// rstate.setJointGroupPositions(jmg, ik_state); +// rstate.update(); +// +// collision_detection::CollisionRequest collision_req; +// collision_req.group_name = jmg->getName(); +// collision_detection::CollisionResult collision_res; +// +// rscene.checkSelfCollision(collision_req, collision_res, rstate); +// +// EXPECT_TRUE(collision_res.collision); +// +// // compute the ik with collision detection activated and expect the resulting +// // pose to be without self collision. +// std::map ik_actual2; +// EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect, +// frame_id, ik_seed, ik_actual2, true)); +// +// std::vector ik_state2; +// std::transform(ik_actual2.begin(), ik_actual2.end(), std::back_inserter(ik_state2), +// boost::bind(&std::map::value_type::second, _1)); +// rstate.setJointGroupPositions(jmg, ik_state2); +// rstate.update(); +// +// collision_detection::CollisionResult collision_res2; +// rscene.checkSelfCollision(collision_req, collision_res2, rstate); +// +// EXPECT_FALSE(collision_res2.collision); +// } /** * @brief Test if self collision is considered by using a pose that always has * self collision. */ -TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKSelfCollisionForInvalidPose) +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKSelfCollisionForInvalidPose) { // robot state - robot_state::RobotState rstate(robot_model_); + moveit::core::RobotState rstate(robot_model_); const std::string frame_id = robot_model_->getModelFrame(); - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); // create seed std::map ik_seed; @@ -522,11 +525,11 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKSelfCollisionFo // compute the ik with disabled collision check std::map ik_actual; - EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect, + EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, tcp_link_, pose_expect, frame_id, ik_seed, ik_actual, false)); // compute the ik with enabled collision check - EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect, + EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, tcp_link_, pose_expect, frame_id, ik_seed, ik_actual, true)); } @@ -540,7 +543,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKSelfCollisionFo * Expected Results: * 1. Function returns 'false'. */ -TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsWithSmallDuration) +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsWithSmallDuration) { const std::map position_last, velocity_last, position_current; double duration_last{ 0.0 }; @@ -562,7 +565,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsWithS * Expected Results: * 1. Function returns 'false'. */ -TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsVelocityViolation) +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsVelocityViolation) { const std::string test_joint_name{ "joint" }; @@ -595,7 +598,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsVeloc * Expected Results: * 1. Function returns 'false'. */ -TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsAccelerationViolation) +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsAccelerationViolation) { const std::string test_joint_name{ "joint" }; @@ -638,7 +641,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsAccel * Expected Results: * 1. Function returns 'false'. */ -TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsDecelerationViolation) +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsDecelerationViolation) { const std::string test_joint_name{ "joint" }; @@ -685,7 +688,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsDecel * Expected Results: * 1. Function returns 'false'. */ -TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testGenerateJointTrajectoryWithInvalidCartesianTrajectory) +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testGenerateJointTrajectoryWithInvalidCartesianTrajectory) { // Create random test trajectory // Note: 'path' is deleted by KDL::Trajectory_Segment @@ -702,12 +705,12 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testGenerateJointTrajectoryWithI std::string group_name{ "invalid_group_name" }; std::map initial_joint_position; double sampling_time{ 0.1 }; - trajectory_msgs::JointTrajectory joint_trajectory; - moveit_msgs::MoveItErrorCodes error_code; + trajectory_msgs::msg::JointTrajectory joint_trajectory; + moveit_msgs::msg::MoveItErrorCodes error_code; bool check_self_collision{ false }; EXPECT_FALSE(pilz_industrial_motion_planner::generateJointTrajectory( - robot_model_, joint_limits, kdl_trajectory, group_name, tcp_link_, initial_joint_position, sampling_time, + planning_scene_, joint_limits, kdl_trajectory, group_name, tcp_link_, initial_joint_position, sampling_time, joint_trajectory, error_code, check_self_collision)); std::map initial_joint_velocity; @@ -719,7 +722,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testGenerateJointTrajectoryWithI cart_traj.points.push_back(cart_traj_point); EXPECT_FALSE(pilz_industrial_motion_planner::generateJointTrajectory( - robot_model_, joint_limits, cart_traj, group_name, tcp_link_, initial_joint_position, initial_joint_velocity, + planning_scene_, joint_limits, cart_traj, group_name, tcp_link_, initial_joint_position, initial_joint_velocity, joint_trajectory, error_code, check_self_collision)); } @@ -734,7 +737,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testGenerateJointTrajectoryWithI * Expected Results: * 1. Function returns 'false'. */ -TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeInvalidVectorSize) +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeInvalidVectorSize) { robot_trajectory::RobotTrajectoryPtr first_trajectory = std::make_shared(robot_model_, planning_group_); @@ -743,7 +746,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTim double epsilon{ 0.0 }; double sampling_time{ 0.0 }; - robot_state::RobotState rstate(robot_model_); + moveit::core::RobotState rstate(robot_model_); first_trajectory->insertWayPoint(0, rstate, 0.1); second_trajectory->insertWayPoint(0, rstate, 0.1); @@ -762,7 +765,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTim * Expected Results: * 1. Function returns 'true'. */ -TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeCorrectSamplingTime) +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeCorrectSamplingTime) { robot_trajectory::RobotTrajectoryPtr first_trajectory = std::make_shared(robot_model_, planning_group_); @@ -772,7 +775,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTim double sampling_time{ 0.0 }; double expected_sampling_time{ 0.1 }; - robot_state::RobotState rstate(robot_model_); + moveit::core::RobotState rstate(robot_model_); first_trajectory->insertWayPoint(0, rstate, expected_sampling_time); first_trajectory->insertWayPoint(1, rstate, expected_sampling_time); @@ -796,7 +799,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTim * Expected Results: * 1. Function returns 'false'. */ -TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeViolateSamplingTime) +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeViolateSamplingTime) { robot_trajectory::RobotTrajectoryPtr first_trajectory = std::make_shared(robot_model_, planning_group_); @@ -806,7 +809,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTim double sampling_time{ 0.0 }; double expected_sampling_time{ 0.1 }; - robot_state::RobotState rstate(robot_model_); + moveit::core::RobotState rstate(robot_model_); first_trajectory->insertWayPoint(0, rstate, expected_sampling_time); first_trajectory->insertWayPoint(1, rstate, expected_sampling_time); first_trajectory->insertWayPoint(2, rstate, expected_sampling_time); @@ -835,10 +838,10 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTim * Expected Results: * 1. Function returns 'false'. */ -TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualPositionUnequal) +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualPositionUnequal) { - robot_state::RobotState rstate_1 = robot_state::RobotState(robot_model_); - robot_state::RobotState rstate_2 = robot_state::RobotState(robot_model_); + moveit::core::RobotState rstate_1 = moveit::core::RobotState(robot_model_); + moveit::core::RobotState rstate_2 = moveit::core::RobotState(robot_model_); double default_joint_position[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; rstate_1.setJointGroupPositions(planning_group_, default_joint_position); @@ -861,10 +864,10 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualPositionUne * Expected Results: * 1. Function returns 'false'. */ -TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualVelocityUnequal) +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualVelocityUnequal) { - robot_state::RobotState rstate_1 = robot_state::RobotState(robot_model_); - robot_state::RobotState rstate_2 = robot_state::RobotState(robot_model_); + moveit::core::RobotState rstate_1 = moveit::core::RobotState(robot_model_); + moveit::core::RobotState rstate_2 = moveit::core::RobotState(robot_model_); // Ensure that the joint positions of both robot state are equal double default_joint_position[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; @@ -892,10 +895,10 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualVelocityUne * Expected Results: * 1. Function returns 'false'. */ -TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualAccelerationUnequal) +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualAccelerationUnequal) { - robot_state::RobotState rstate_1 = robot_state::RobotState(robot_model_); - robot_state::RobotState rstate_2 = robot_state::RobotState(robot_model_); + moveit::core::RobotState rstate_1 = moveit::core::RobotState(robot_model_); + moveit::core::RobotState rstate_2 = moveit::core::RobotState(robot_model_); // Ensure that the joint positions of both robot state are equal double default_joint_position[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; @@ -928,9 +931,9 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualAcceleratio * Expected Results: * 1. Function returns 'false'. */ -TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryVelocityUnequal) +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryVelocityUnequal) { - robot_state::RobotState rstate_1 = robot_state::RobotState(robot_model_); + moveit::core::RobotState rstate_1 = moveit::core::RobotState(robot_model_); // Ensure that the joint velocities are NOT zero double default_joint_velocity[6] = { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; @@ -951,9 +954,9 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryVeloci * Expected Results: * 1. Function returns 'false'. */ -TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryAccelerationUnequal) +TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryAccelerationUnequal) { - robot_state::RobotState rstate_1 = robot_state::RobotState(robot_model_); + moveit::core::RobotState rstate_1 = moveit::core::RobotState(robot_model_); // Ensure that the joint velocities are zero double default_joint_velocity[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; @@ -969,8 +972,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryAccele int main(int argc, char** argv) { - ros::init(argc, argv, "unittest_trajectory_functions"); - ros::NodeHandle nh; + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator.cpp similarity index 70% rename from moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/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/unit_tests/src/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/unit_tests/src/unittest_trajectory_generator_circ.cpp similarity index 55% rename from moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp index bf5913906b..cf781b9502 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp @@ -57,48 +57,151 @@ #include "pilz_industrial_motion_planner_testutils/xml_testdata_loader.h" #include "test_utils.h" -const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; -const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; - -// parameters from parameter server -const std::string TEST_DATA_FILE_NAME("testdata_file_name"); -const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); -const std::string PARAM_TARGET_LINK_NAME("target_link"); -const std::string CARTESIAN_POSITION_TOLERANCE("cartesian_position_tolerance"); -const std::string ANGULAR_ACC_TOLERANCE("angular_acc_tolerance"); -const std::string ROTATION_AXIS_NORM_TOLERANCE("rot_axis_norm_tolerance"); -const std::string ACCELERATION_TOLERANCE("acceleration_tolerance"); -const std::string OTHER_TOLERANCE("other_tolerance"); - -#define SKIP_IF_GRIPPER \ - if (GetParam() == PARAM_MODEL_WITH_GRIPPER_NAME) \ - { \ - SUCCEED(); \ - return; \ - }; +#include "rclcpp/rclcpp.hpp" using namespace pilz_industrial_motion_planner; using namespace pilz_industrial_motion_planner_testutils; -class TrajectoryGeneratorCIRCTest : public testing::TestWithParam +static const std::string PARAM_NAMESPACE_LIMITS = "robot_description_planning"; + +class TrajectoryGeneratorCIRCTest : public testing::Test { protected: /** * @brief Create test scenario for circ trajectory generator * */ - void SetUp() override; + void SetUp() override + { + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + node_ = rclcpp::Node::make_shared("unittest_trajectory_generator_circ", node_options); + + // load robot model + robot_model_loader::RobotModelLoader rm_loader(node_); + robot_model_ = rm_loader.getModel(); + ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model"; + planning_scene_ = std::make_shared(robot_model_); + + // get parameters + ASSERT_TRUE(node_->has_parameter("testdata_file_name")); + node_->get_parameter("testdata_file_name", test_data_file_name_); + ASSERT_TRUE(node_->has_parameter("planning_group")); + node_->get_parameter("planning_group", planning_group_); + ASSERT_TRUE(node_->has_parameter("target_link")); + node_->get_parameter("target_link", target_link_); + ASSERT_TRUE(node_->has_parameter("cartesian_position_tolerance")); + node_->get_parameter("cartesian_position_tolerance", cartesian_position_tolerance_); + ASSERT_TRUE(node_->has_parameter("angular_acc_tolerance")); + node_->get_parameter("angular_acc_tolerance", angular_acc_tolerance_); + ASSERT_TRUE(node_->has_parameter("rot_axis_norm_tolerance")); + node_->get_parameter("rot_axis_norm_tolerance", rot_axis_norm_tolerance_); + ASSERT_TRUE(node_->has_parameter("acceleration_tolerance")); + node_->get_parameter("acceleration_tolerance", acceleration_tolerance_); + ASSERT_TRUE(node_->has_parameter("other_tolerance")); + node_->get_parameter("other_tolerance", other_tolerance_); + + // check robot model + testutils::checkRobotModel(robot_model_, planning_group_, target_link_); + + // load the test data provider + tdp_ = std::make_unique(test_data_file_name_); + ASSERT_NE(nullptr, tdp_) << "Failed to load test data by provider."; + + tdp_->setRobotModel(robot_model_); + + // create the limits container + pilz_industrial_motion_planner::JointLimitsContainer joint_limits = + pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( + node_, PARAM_NAMESPACE_LIMITS, robot_model_->getActiveJointModels()); + CartesianLimit cart_limits; + // Cartesian limits are chose as such values to ease the manually compute the + // trajectory + + cart_limits.setMaxRotationalVelocity(1 * M_PI); + cart_limits.setMaxTranslationalAcceleration(1 * M_PI); + cart_limits.setMaxTranslationalDeceleration(1 * M_PI); + cart_limits.setMaxTranslationalVelocity(1 * M_PI); + planner_limits_.setJointLimits(joint_limits); + planner_limits_.setCartesianLimits(cart_limits); + + // initialize the LIN trajectory generator + circ_ = std::make_unique(robot_model_, planner_limits_); + ASSERT_NE(nullptr, circ_) << "failed to create CIRC trajectory generator"; + } void checkCircResult(const planning_interface::MotionPlanRequest& req, - const planning_interface::MotionPlanResponse& res); + const planning_interface::MotionPlanResponse& res) + { + moveit_msgs::msg::MotionPlanResponse res_msg; + res.getMessage(res_msg); + EXPECT_TRUE(testutils::isGoalReached(res.trajectory_->getFirstWayPointPtr()->getRobotModel(), + res_msg.trajectory.joint_trajectory, req, other_tolerance_)); + + EXPECT_TRUE( + testutils::checkJointTrajectory(res_msg.trajectory.joint_trajectory, planner_limits_.getJointLimitContainer())); + + EXPECT_EQ(req.path_constraints.position_constraints.size(), 1u); + EXPECT_EQ(req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.size(), 1u); + + // Check that all point have the equal distance to the center + Eigen::Vector3d circ_center; + getCircCenter(req, res, circ_center); + + for (std::size_t i = 0; i < res.trajectory_->getWayPointCount(); ++i) + { + Eigen::Affine3d waypoint_pose = res.trajectory_->getWayPointPtr(i)->getFrameTransform(target_link_); + EXPECT_NEAR( + (res.trajectory_->getFirstWayPointPtr()->getFrameTransform(target_link_).translation() - circ_center).norm(), + (circ_center - waypoint_pose.translation()).norm(), cartesian_position_tolerance_); + } + + // check translational and rotational paths + ASSERT_TRUE(testutils::checkCartesianTranslationalPath(res.trajectory_, target_link_, acceleration_tolerance_)); + ASSERT_TRUE(testutils::checkCartesianRotationalPath(res.trajectory_, target_link_, angular_acc_tolerance_, + rot_axis_norm_tolerance_)); + + for (size_t idx = 0; idx < res.trajectory_->getLastWayPointPtr()->getVariableCount(); ++idx) + { + EXPECT_NEAR(0.0, res.trajectory_->getLastWayPointPtr()->getVariableVelocity(idx), other_tolerance_); + EXPECT_NEAR(0.0, res.trajectory_->getLastWayPointPtr()->getVariableAcceleration(idx), other_tolerance_); + } + } void getCircCenter(const planning_interface::MotionPlanRequest& req, - const planning_interface::MotionPlanResponse& res, Eigen::Vector3d& circ_center); + const planning_interface::MotionPlanResponse& res, Eigen::Vector3d& circ_center) + { + if (req.path_constraints.name == "center") + { + tf2::fromMsg(req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).position, + circ_center); + } + else if (req.path_constraints.name == "interim") + { + Eigen::Vector3d interim; + tf2::fromMsg(req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).position, + interim); + Eigen::Vector3d start = res.trajectory_->getFirstWayPointPtr()->getFrameTransform(target_link_).translation(); + Eigen::Vector3d goal = res.trajectory_->getLastWayPointPtr()->getFrameTransform(target_link_).translation(); + + const Eigen::Vector3d t = interim - start; + const Eigen::Vector3d u = goal - start; + const Eigen::Vector3d v = goal - interim; + + const Eigen::Vector3d w = t.cross(u); + + ASSERT_GT(w.norm(), 1e-8) << "Circle center not well defined for given start, interim and goal."; + + circ_center = start + (u * t.dot(t) * u.dot(v) - t * u.dot(u) * t.dot(v)) * 0.5 / pow(w.norm(), 2); + } + } protected: // ros stuff - ros::NodeHandle ph_{ "~" }; - robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() }; + rclcpp::Node::SharedPtr node_; + moveit::core::RobotModelConstPtr robot_model_; + planning_scene::PlanningSceneConstPtr planning_scene_; + std::unique_ptr circ_; // test data provider std::unique_ptr tdp_; @@ -111,184 +214,72 @@ class TrajectoryGeneratorCIRCTest : public testing::TestWithParam LimitsContainer planner_limits_; }; -void TrajectoryGeneratorCIRCTest::SetUp() -{ - // get parameters - ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name_)); - ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_)); - ASSERT_TRUE(ph_.getParam(PARAM_TARGET_LINK_NAME, target_link_)); - ASSERT_TRUE(ph_.getParam(CARTESIAN_POSITION_TOLERANCE, cartesian_position_tolerance_)); - ASSERT_TRUE(ph_.getParam(ANGULAR_ACC_TOLERANCE, angular_acc_tolerance_)); - ASSERT_TRUE(ph_.getParam(ROTATION_AXIS_NORM_TOLERANCE, rot_axis_norm_tolerance_)); - ASSERT_TRUE(ph_.getParam(ACCELERATION_TOLERANCE, acceleration_tolerance_)); - ASSERT_TRUE(ph_.getParam(OTHER_TOLERANCE, other_tolerance_)); - - // check robot model - testutils::checkRobotModel(robot_model_, planning_group_, target_link_); - - // load the test data provider - tdp_.reset(new pilz_industrial_motion_planner_testutils::XmlTestdataLoader{ test_data_file_name_ }); - ASSERT_NE(nullptr, tdp_) << "Failed to load test data by provider."; - - tdp_->setRobotModel(robot_model_); - - // create the limits container - pilz_industrial_motion_planner::JointLimitsContainer joint_limits = - pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(ph_, - robot_model_->getActiveJointModels()); - CartesianLimit cart_limits; - // Cartesian limits are chose as such values to ease the manually compute the - // trajectory - - cart_limits.setMaxRotationalVelocity(1 * M_PI); - cart_limits.setMaxTranslationalAcceleration(1 * M_PI); - cart_limits.setMaxTranslationalDeceleration(1 * M_PI); - cart_limits.setMaxTranslationalVelocity(1 * M_PI); - planner_limits_.setJointLimits(joint_limits); - planner_limits_.setCartesianLimits(cart_limits); - - // initialize the LIN trajectory generator - circ_.reset(new TrajectoryGeneratorCIRC(robot_model_, planner_limits_)); - ASSERT_NE(nullptr, circ_) << "failed to create CIRC trajectory generator"; -} - -void TrajectoryGeneratorCIRCTest::checkCircResult(const planning_interface::MotionPlanRequest& req, - const planning_interface::MotionPlanResponse& res) -{ - moveit_msgs::MotionPlanResponse res_msg; - res.getMessage(res_msg); - EXPECT_TRUE(testutils::isGoalReached(res.trajectory_->getFirstWayPointPtr()->getRobotModel(), - res_msg.trajectory.joint_trajectory, req, other_tolerance_)); - - EXPECT_TRUE( - testutils::checkJointTrajectory(res_msg.trajectory.joint_trajectory, planner_limits_.getJointLimitContainer())); - - EXPECT_EQ(req.path_constraints.position_constraints.size(), 1u); - EXPECT_EQ(req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.size(), 1u); - - // Check that all point have the equal distance to the center - Eigen::Vector3d circ_center; - getCircCenter(req, res, circ_center); - - for (std::size_t i = 0; i < res.trajectory_->getWayPointCount(); ++i) - { - Eigen::Affine3d waypoint_pose = res.trajectory_->getWayPointPtr(i)->getFrameTransform(target_link_); - EXPECT_NEAR( - (res.trajectory_->getFirstWayPointPtr()->getFrameTransform(target_link_).translation() - circ_center).norm(), - (circ_center - waypoint_pose.translation()).norm(), cartesian_position_tolerance_); - } - - // check translational and rotational paths - ASSERT_TRUE(testutils::checkCartesianTranslationalPath(res.trajectory_, target_link_, acceleration_tolerance_)); - ASSERT_TRUE(testutils::checkCartesianRotationalPath(res.trajectory_, target_link_, angular_acc_tolerance_, - rot_axis_norm_tolerance_)); - - for (size_t idx = 0; idx < res.trajectory_->getLastWayPointPtr()->getVariableCount(); ++idx) - { - EXPECT_NEAR(0.0, res.trajectory_->getLastWayPointPtr()->getVariableVelocity(idx), other_tolerance_); - EXPECT_NEAR(0.0, res.trajectory_->getLastWayPointPtr()->getVariableAcceleration(idx), other_tolerance_); - } -} - -void TrajectoryGeneratorCIRCTest::getCircCenter(const planning_interface::MotionPlanRequest& req, - const planning_interface::MotionPlanResponse& res, - Eigen::Vector3d& circ_center) -{ - if (req.path_constraints.name == "center") - { - tf2::fromMsg(req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).position, - circ_center); - } - else if (req.path_constraints.name == "interim") - { - Eigen::Vector3d interim; - tf2::fromMsg(req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).position, - interim); - Eigen::Vector3d start = res.trajectory_->getFirstWayPointPtr()->getFrameTransform(target_link_).translation(); - Eigen::Vector3d goal = res.trajectory_->getLastWayPointPtr()->getFrameTransform(target_link_).translation(); - - const Eigen::Vector3d t = interim - start; - const Eigen::Vector3d u = goal - start; - const Eigen::Vector3d v = goal - interim; - - const Eigen::Vector3d w = t.cross(u); - - ASSERT_GT(w.norm(), 1e-8) << "Circle center not well defined for given start, interim and goal."; - - circ_center = start + (u * t.dot(t) * u.dot(v) - t * u.dot(u) * t.dot(v)) * 0.5 / pow(w.norm(), 2); - } -} - /** * @brief Checks that each derived MoveItErrorCodeException contains the correct * error code. */ -TEST(TrajectoryGeneratorCIRCTest, TestExceptionErrorCodeMapping) +TEST_F(TrajectoryGeneratorCIRCTest, TestExceptionErrorCodeMapping) { { std::shared_ptr cnp_ex{ new CircleNoPlane("") }; - 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); } } -// Instantiate the test cases for robot model with and without gripper -INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryGeneratorCIRCTest, - ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME)); - /** * @brief Construct a TrajectoryGeneratorCirc with no limits given */ -TEST_P(TrajectoryGeneratorCIRCTest, noLimits) +TEST_F(TrajectoryGeneratorCIRCTest, noLimits) { LimitsContainer planner_limits; EXPECT_THROW(TrajectoryGeneratorCIRC(this->robot_model_, planner_limits), TrajectoryGeneratorInvalidLimitsException); @@ -298,7 +289,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, noLimits) * @brief test invalid motion plan request with incomplete start state and * cartesian goal */ -TEST_P(TrajectoryGeneratorCIRCTest, incompleteStartState) +TEST_F(TrajectoryGeneratorCIRCTest, incompleteStartState) { auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; @@ -308,65 +299,65 @@ TEST_P(TrajectoryGeneratorCIRCTest, incompleteStartState) req.start_state.joint_state.position.resize(1); // prevent failing check for equal sizes planning_interface::MotionPlanResponse res; - EXPECT_FALSE(circ_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + EXPECT_FALSE(circ_->generate(planning_scene_, req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } /** * @brief test invalid motion plan request with non zero start velocity */ -TEST_P(TrajectoryGeneratorCIRCTest, nonZeroStartVelocity) +TEST_F(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_FALSE(circ_->generate(planning_scene_, req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); req.start_state.joint_state.velocity.clear(); } -TEST_P(TrajectoryGeneratorCIRCTest, ValidCommand) +TEST_F(TrajectoryGeneratorCIRCTest, ValidCommand) { auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; planning_interface::MotionPlanResponse res; - EXPECT_TRUE(circ_->generate(circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_TRUE(circ_->generate(planning_scene_, circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); } /** * @brief Generate invalid circ with to high vel scaling */ -TEST_P(TrajectoryGeneratorCIRCTest, velScaleToHigh) +TEST_F(TrajectoryGeneratorCIRCTest, velScaleToHigh) { auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; 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_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED); } /** * @brief Generate invalid circ with to high acc scaling */ -TEST_P(TrajectoryGeneratorCIRCTest, accScaleToHigh) +TEST_F(TrajectoryGeneratorCIRCTest, accScaleToHigh) { auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; 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_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED); } /** * @brief Use three points (with center) with a really small distance between to * trigger a internal throw from KDL */ -TEST_P(TrajectoryGeneratorCIRCTest, samePointsWithCenter) +TEST_F(TrajectoryGeneratorCIRCTest, samePointsWithCenter) { // Define auxiliary point and goal to be the same as the start auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; @@ -380,8 +371,8 @@ TEST_P(TrajectoryGeneratorCIRCTest, samePointsWithCenter) circ.getGoalConfiguration().getPose().position.z -= 1e-8; planning_interface::MotionPlanResponse res; - EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** @@ -389,7 +380,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, samePointsWithCenter) * * Expected: Planning should fail. */ -TEST_P(TrajectoryGeneratorCIRCTest, samePointsWithInterim) +TEST_F(TrajectoryGeneratorCIRCTest, samePointsWithInterim) { // Define auxiliary point and goal to be the same as the start auto circ{ tdp_->getCircCartInterimCart("circ3_interim") }; @@ -403,14 +394,14 @@ TEST_P(TrajectoryGeneratorCIRCTest, samePointsWithInterim) circ.getGoalConfiguration().getPose().position.z -= 1e-8; planning_interface::MotionPlanResponse res; - EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** * @brief test invalid motion plan request with no aux point defined */ -TEST_P(TrajectoryGeneratorCIRCTest, emptyAux) +TEST_F(TrajectoryGeneratorCIRCTest, emptyAux) { auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; @@ -419,14 +410,14 @@ TEST_P(TrajectoryGeneratorCIRCTest, emptyAux) req.path_constraints.position_constraints.clear(); planning_interface::MotionPlanResponse res; - EXPECT_FALSE(circ_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_FALSE(circ_->generate(planning_scene_, req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** * @brief test invalid motion plan request with no aux name defined */ -TEST_P(TrajectoryGeneratorCIRCTest, invalidAuxName) +TEST_F(TrajectoryGeneratorCIRCTest, invalidAuxName) { auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; @@ -435,15 +426,15 @@ TEST_P(TrajectoryGeneratorCIRCTest, invalidAuxName) req.path_constraints.name = ""; planning_interface::MotionPlanResponse res; - EXPECT_FALSE(circ_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_FALSE(circ_->generate(planning_scene_, req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** * @brief test invalid motion plan request with invalid link name in the * auxiliary point */ -TEST_P(TrajectoryGeneratorCIRCTest, invalidAuxLinkName) +TEST_F(TrajectoryGeneratorCIRCTest, invalidAuxLinkName) { auto circ{ tdp_->getCircJointInterimCart("circ3_interim") }; @@ -452,22 +443,22 @@ TEST_P(TrajectoryGeneratorCIRCTest, invalidAuxLinkName) req.path_constraints.position_constraints.front().link_name = "INVALID"; planning_interface::MotionPlanResponse res; - EXPECT_FALSE(circ_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME); + EXPECT_FALSE(circ_->generate(planning_scene_, req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_LINK_NAME); } /** * @brief test the circ planner with invalid center point */ -TEST_P(TrajectoryGeneratorCIRCTest, invalidCenter) +TEST_F(TrajectoryGeneratorCIRCTest, invalidCenter) { auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose()); circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.y += 1; planning_interface::MotionPlanResponse res; - EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** @@ -475,7 +466,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, invalidCenter) * * Expected: Planning should fail since the path is not uniquely defined. */ -TEST_P(TrajectoryGeneratorCIRCTest, colinearCenter) +TEST_F(TrajectoryGeneratorCIRCTest, colinearCenter) { auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose()); @@ -486,8 +477,8 @@ TEST_P(TrajectoryGeneratorCIRCTest, colinearCenter) circ.getGoalConfiguration().getPose().position.x += 0.1; planning_interface::MotionPlanResponse res; - EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** @@ -496,7 +487,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, colinearCenter) * Expected: Planning should fail. These positions do not even represent a * circle. */ -TEST_P(TrajectoryGeneratorCIRCTest, colinearInterim) +TEST_F(TrajectoryGeneratorCIRCTest, colinearInterim) { auto circ{ tdp_->getCircCartInterimCart("circ3_interim") }; circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose()); @@ -507,8 +498,8 @@ TEST_P(TrajectoryGeneratorCIRCTest, colinearInterim) circ.getGoalConfiguration().getPose().position.x += 0.1; planning_interface::MotionPlanResponse res; - EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** @@ -519,14 +510,14 @@ TEST_P(TrajectoryGeneratorCIRCTest, colinearInterim) * * Expected: Planning should successfully return. */ -TEST_P(TrajectoryGeneratorCIRCTest, colinearCenterDueToInterim) +TEST_F(TrajectoryGeneratorCIRCTest, colinearCenterDueToInterim) { // get the test data from xml auto circ{ tdp_->getCircCartInterimCart("circ3_interim") }; planning_interface::MotionPlanResponse res; - ASSERT_TRUE(circ_->generate(circ.toRequest(), res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + ASSERT_TRUE(circ_->generate(planning_scene_, circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); } /** @@ -539,7 +530,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, colinearCenterDueToInterim) * * Expected: Planning should successfully return. */ -TEST_P(TrajectoryGeneratorCIRCTest, colinearCenterAndInterim) +TEST_F(TrajectoryGeneratorCIRCTest, colinearCenterAndInterim) { auto circ{ tdp_->getCircCartInterimCart("circ3_interim") }; @@ -555,11 +546,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_TRUE(circ_->generate(planning_scene_, req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -572,7 +563,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, colinearCenterAndInterim) * * Expected: Planning should successfully return. */ -TEST_P(TrajectoryGeneratorCIRCTest, interimLarger180Degree) +TEST_F(TrajectoryGeneratorCIRCTest, interimLarger180Degree) { auto circ{ tdp_->getCircCartInterimCart("circ3_interim") }; @@ -589,27 +580,25 @@ 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_TRUE(circ_->generate(planning_scene_, req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } /** * @brief test the circ planner with center point and joint goal */ -TEST_P(TrajectoryGeneratorCIRCTest, centerPointJointGoal) +TEST_F(TrajectoryGeneratorCIRCTest, centerPointJointGoal) { - SKIP_IF_GRIPPER - auto circ{ tdp_->getCircJointCenterCart("circ1_center_2") }; - moveit_msgs::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); + ASSERT_TRUE(circ_->generate(planning_scene_, req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -618,25 +607,25 @@ TEST_P(TrajectoryGeneratorCIRCTest, centerPointJointGoal) * this test a additional * point is defined as an invalid test case */ -TEST_P(TrajectoryGeneratorCIRCTest, InvalidAdditionalPrimitivePose) +TEST_F(TrajectoryGeneratorCIRCTest, InvalidAdditionalPrimitivePose) { auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; - 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); // Define a additional pose here - geometry_msgs::Pose center_position; + geometry_msgs::msg::Pose center_position; center_position.position.x = 0.0; center_position.position.y = 0.0; center_position.position.z = 0.65; req.path_constraints.position_constraints.back().constraint_region.primitive_poses.push_back(center_position); planning_interface::MotionPlanResponse res; - ASSERT_FALSE(circ_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + ASSERT_FALSE(circ_->generate(planning_scene_, req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** @@ -645,103 +634,101 @@ TEST_P(TrajectoryGeneratorCIRCTest, InvalidAdditionalPrimitivePose) * Here an additional joint constraints is "falsely" defined to check for the * error. */ -TEST_P(TrajectoryGeneratorCIRCTest, InvalidExtraJointConstraint) +TEST_F(TrajectoryGeneratorCIRCTest, InvalidExtraJointConstraint) { auto circ{ tdp_->getCircJointCenterCart("circ1_center_2") }; - 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_FALSE(circ_->generate(planning_scene_, req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } /** * @brief test the circ planner with center point and pose goal */ -TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoal) +TEST_F(TrajectoryGeneratorCIRCTest, CenterPointPoseGoal) { auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; - 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); + ASSERT_TRUE(circ_->generate(planning_scene_, req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } /** * @brief Set a frame id only on the position constrainst */ -TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdPositionConstraints) +TEST_F(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdPositionConstraints) { auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; - 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); + ASSERT_TRUE(circ_->generate(planning_scene_, req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } /** * @brief Set a frame id only on the orientation constrainst */ -TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdOrientationConstraints) +TEST_F(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdOrientationConstraints) { auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; - 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); + ASSERT_TRUE(circ_->generate(planning_scene_, req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } /** * @brief Set a frame_id on both position and orientation constraints */ -TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdBothConstraints) +TEST_F(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdBothConstraints) { auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; - 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(); 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); + ASSERT_TRUE(circ_->generate(planning_scene_, req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } /** * @brief test the circ planner with interim point with joint goal */ -TEST_P(TrajectoryGeneratorCIRCTest, InterimPointJointGoal) +TEST_F(TrajectoryGeneratorCIRCTest, InterimPointJointGoal) { - SKIP_IF_GRIPPER - auto circ{ tdp_->getCircJointInterimCart("circ3_interim") }; - moveit_msgs::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); + ASSERT_TRUE(circ_->generate(planning_scene_, req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -751,41 +738,38 @@ TEST_P(TrajectoryGeneratorCIRCTest, InterimPointJointGoal) * * The generator is expected to be robust against a velocity beeing almost zero. */ -TEST_P(TrajectoryGeneratorCIRCTest, InterimPointJointGoalStartVelNearZero) +TEST_F(TrajectoryGeneratorCIRCTest, InterimPointJointGoalStartVelNearZero) { - SKIP_IF_GRIPPER - auto circ{ tdp_->getCircJointInterimCart("circ3_interim") }; - moveit_msgs::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); + ASSERT_TRUE(circ_->generate(planning_scene_, req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } /** * @brief test the circ planner with interim point with pose goal */ -TEST_P(TrajectoryGeneratorCIRCTest, InterimPointPoseGoal) +TEST_F(TrajectoryGeneratorCIRCTest, InterimPointPoseGoal) { auto circ{ tdp_->getCircJointInterimCart("circ3_interim") }; - moveit_msgs::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); + ASSERT_TRUE(circ_->generate(planning_scene_, req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } int main(int argc, char** argv) { - ros::init(argc, argv, "unittest_trajectory_generator_circ"); - ros::NodeHandle nh; + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp similarity index 63% rename from moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp index aaba86c0f1..01f2eccb3f 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp @@ -41,22 +41,21 @@ #include #include #include -#include #include "pilz_industrial_motion_planner/joint_limits_aggregator.h" #include "pilz_industrial_motion_planner/joint_limits_container.h" #include "pilz_industrial_motion_planner/trajectory_generator_circ.h" #include "pilz_industrial_motion_planner/trajectory_generator_lin.h" #include "pilz_industrial_motion_planner/trajectory_generator_ptp.h" +#include "pilz_industrial_motion_planner/trajectory_blender_transition_window.h" #include "test_utils.h" +#include "rclcpp/rclcpp.hpp" + const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; - -// parameters from parameter server -const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); -const std::string PARAM_TARGET_LINK_NAME("target_link"); +const std::string PARAM_NAMESPACE_LIMITS{ "robot_description_planning" }; /** * A value type container to combine type and value @@ -99,16 +98,29 @@ class TrajectoryGeneratorCommonTest : public ::testing::Test protected: void SetUp() override { - ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_)); - ASSERT_TRUE(ph_.getParam(PARAM_TARGET_LINK_NAME, target_link_)); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + node_ = rclcpp::Node::make_shared("unittest_trajectory_generator_common", node_options); + + // load robot model + // const std::string robot_description_param = (!T::VALUE ? PARAM_MODEL_NO_GRIPPER_NAME : PARAM_MODEL_WITH_GRIPPER_NAME); + robot_model_loader::RobotModelLoader rm_loader(node_, PARAM_MODEL_NO_GRIPPER_NAME); + robot_model_ = rm_loader.getModel(); + ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model"; + planning_scene_ = std::make_shared(robot_model_); + + // get parameters + ASSERT_TRUE(node_->has_parameter("planning_group")); + node_->get_parameter("planning_group", planning_group_); + ASSERT_TRUE(node_->has_parameter("target_link")); + node_->get_parameter("target_link", target_link_); testutils::checkRobotModel(robot_model_, planning_group_, target_link_); // create the limits container - std::string robot_description_param = (!T::VALUE ? PARAM_MODEL_NO_GRIPPER_NAME : PARAM_MODEL_WITH_GRIPPER_NAME); pilz_industrial_motion_planner::JointLimitsContainer joint_limits = pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( - ros::NodeHandle(robot_description_param + "_planning"), robot_model_->getActiveJointModels()); + node_, PARAM_NAMESPACE_LIMITS, robot_model_->getActiveJointModels()); pilz_industrial_motion_planner::CartesianLimit cart_limits; cart_limits.setMaxRotationalVelocity(0.5 * M_PI); cart_limits.setMaxTranslationalAcceleration(2); @@ -127,13 +139,13 @@ class TrajectoryGeneratorCommonTest : public ::testing::Test req_.group_name = planning_group_; req_.max_velocity_scaling_factor = 1.0; req_.max_acceleration_scaling_factor = 1.0; - robot_state::RobotState rstate(robot_model_); + moveit::core::RobotState rstate(robot_model_); rstate.setToDefaultValues(); rstate.setJointGroupPositions(planning_group_, { 0, M_PI / 2, 0, M_PI / 2, 0, 0 }); rstate.setVariableVelocities(std::vector(rstate.getVariableCount(), 0.0)); 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); @@ -142,11 +154,9 @@ class TrajectoryGeneratorCommonTest : public ::testing::Test protected: // ros stuff - ros::NodeHandle ph_{ "~" }; - robot_model::RobotModelConstPtr robot_model_{ - robot_model_loader::RobotModelLoader(!T::VALUE ? PARAM_MODEL_NO_GRIPPER_NAME : PARAM_MODEL_WITH_GRIPPER_NAME) - .getModel() - }; + rclcpp::Node::SharedPtr node_; + moveit::core::RobotModelConstPtr robot_model_; + planning_scene::PlanningSceneConstPtr planning_scene_; // trajectory generator std::unique_ptr trajectory_generator_; @@ -156,19 +166,20 @@ class TrajectoryGeneratorCommonTest : public ::testing::Test std::string planning_group_, target_link_; }; // Define the types we need to test -TYPED_TEST_SUITE(TrajectoryGeneratorCommonTest, TrajectoryGeneratorCommonTestTypes); +TYPED_TEST_SUITE(TrajectoryGeneratorCommonTest, TrajectoryGeneratorCommonTestTypes, /* ... */); template class TrajectoryGeneratorCommonTestNoGripper : public TrajectoryGeneratorCommonTest { }; -TYPED_TEST_SUITE(TrajectoryGeneratorCommonTestNoGripper, TrajectoryGeneratorCommonTestTypesNoGripper); +TYPED_TEST_SUITE(TrajectoryGeneratorCommonTestNoGripper, TrajectoryGeneratorCommonTestTypesNoGripper, /* ... */); -template -class TrajectoryGeneratorCommonTestWithGripper : public TrajectoryGeneratorCommonTest -{ -}; -TYPED_TEST_SUITE(TrajectoryGeneratorCommonTestWithGripper, TrajectoryGeneratorCommonTestTypesWithGripper); +// TODO(henningkayser):Enable tests with gripper support +// template +// class TrajectoryGeneratorCommonTestWithGripper : public TrajectoryGeneratorCommonTest +// { +// }; +// TYPED_TEST_SUITE(TrajectoryGeneratorCommonTestWithGripper, TrajectoryGeneratorCommonTestTypesWithGripper, /* ... */); /** * @brief test invalid scaling factor. The scaling factor must be in the range @@ -177,23 +188,23 @@ TYPED_TEST_SUITE(TrajectoryGeneratorCommonTestWithGripper, TrajectoryGeneratorCo 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_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + 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_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + 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_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + 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_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } /** @@ -202,8 +213,8 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalideScalingFactor) 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_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME, this->res_.error_code_.val); } /** @@ -212,19 +223,19 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalidGroupName) 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_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME, this->res_.error_code_.val); } /** * @brief Test invalid motion plan request for all trajectory generators */ -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); -} +// TYPED_TEST(TrajectoryGeneratorCommonTestWithGripper, GripperGroup) +// { +// this->req_.group_name = "gripper"; +// EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); +// EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS, this->res_.error_code_.val); +// } /** * @brief Test if there is a valid inverse kinematics solver for this planning @@ -238,7 +249,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); //} /** @@ -247,8 +258,8 @@ TYPED_TEST(TrajectoryGeneratorCommonTestWithGripper, GripperGroup) 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_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } /** @@ -257,8 +268,8 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyJointNamesInStartState) 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_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } /** @@ -267,8 +278,8 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InconsistentStartState) 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_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } /** @@ -281,8 +292,8 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, StartPostionOutOfLimit) 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_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } /** @@ -291,8 +302,8 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, StartPositionVelocityNoneZero) 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_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } /** @@ -300,32 +311,32 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyGoalConstraints) */ TYPED_TEST(TrajectoryGeneratorCommonTest, MultipleGoals) { - moveit_msgs::JointConstraint joint_constraint; - moveit_msgs::PositionConstraint position_constraint; - moveit_msgs::OrientationConstraint orientation_constraint; - moveit_msgs::Constraints goal_constraint; + moveit_msgs::msg::JointConstraint joint_constraint; + moveit_msgs::msg::PositionConstraint position_constraint; + moveit_msgs::msg::OrientationConstraint orientation_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_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + 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); 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_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + 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); 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_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); // two Cartesian constraints goal_constraint.joint_constraints.clear(); @@ -335,8 +346,8 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, MultipleGoals) 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_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } /** @@ -344,11 +355,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_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } /** @@ -356,11 +367,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_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } /** @@ -369,8 +380,8 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, MissingJointConstraint) 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_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } /** @@ -378,31 +389,31 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalideJointPositionInGoal) */ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalidLinkNameInCartesianGoal) { - moveit_msgs::PositionConstraint position_constraint; - moveit_msgs::OrientationConstraint orientation_constraint; - moveit_msgs::Constraints goal_constraint; + moveit_msgs::msg::PositionConstraint position_constraint; + moveit_msgs::msg::OrientationConstraint orientation_constraint; + moveit_msgs::msg::Constraints goal_constraint; // link name not set goal_constraint.position_constraints.push_back(position_constraint); 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_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + 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"; goal_constraint.orientation_constraints.front().link_name = "test_link_2"; 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_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + 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_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); } /** @@ -410,9 +421,9 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalidLinkNameInCartesianGoal) */ TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyPrimitivePoses) { - moveit_msgs::PositionConstraint position_constraint; - moveit_msgs::OrientationConstraint orientation_constraint; - moveit_msgs::Constraints goal_constraint; + moveit_msgs::msg::PositionConstraint position_constraint; + moveit_msgs::msg::OrientationConstraint orientation_constraint; + moveit_msgs::msg::Constraints goal_constraint; position_constraint.link_name = this->robot_model_->getJointModelGroup(this->planning_group_)->getLinkModelNames().back(); orientation_constraint.link_name = position_constraint.link_name; @@ -421,14 +432,13 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyPrimitivePoses) 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_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } int main(int argc, char** argv) { - ros::init(argc, argv, "unittest_trajectory_generator_common"); - // ros::NodeHandle nh; + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp similarity index 62% rename from moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp index 8d5ff8e6e2..5b92b83faf 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp @@ -48,12 +48,14 @@ #include #include -#include +#include "rclcpp/rclcpp.hpp" -const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; -const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; +using namespace pilz_industrial_motion_planner; +using namespace pilz_industrial_motion_planner_testutils; -// parameters from parameter server +static const std::string PARAM_NAMESPACE_LIMITS = "robot_description_planning"; + +// Parameter names const std::string TEST_DATA_FILE_NAME("testdata_file_name"); const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); const std::string TARGET_LINK_HCD("target_link_hand_computed_data"); @@ -65,9 +67,6 @@ const std::string ROTATION_AXIS_NORM_TOLERANCE("rot_axis_norm_tolerance"); const std::string VELOCITY_SCALING_FACTOR("velocity_scaling_factor"); const std::string OTHER_TOLERANCE("other_tolerance"); -using namespace pilz_industrial_motion_planner; -using namespace pilz_industrial_motion_planner_testutils; - /** * @brief Parameterized unittest of trajectory generator LIN to enable tests * against @@ -75,22 +74,92 @@ using namespace pilz_industrial_motion_planner_testutils; * the * ros parameter server. */ -class TrajectoryGeneratorLINTest : public testing::TestWithParam +class TrajectoryGeneratorLINTest : public testing::Test { protected: /** * @brief Create test scenario for lin trajectory generator * */ - void SetUp() override; + void SetUp() override + { + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + node_ = rclcpp::Node::make_shared("unittest_trajectory_generator_lin", node_options); + + // load robot model + robot_model_loader::RobotModelLoader rm_loader(node_); + robot_model_ = rm_loader.getModel(); + ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model"; + planning_scene_ = std::make_shared(robot_model_); + + // get parameters + ASSERT_TRUE(node_->get_parameter(TEST_DATA_FILE_NAME, test_data_file_name_)); + ASSERT_TRUE(node_->get_parameter(PARAM_PLANNING_GROUP_NAME, planning_group_)); + ASSERT_TRUE(node_->get_parameter(TARGET_LINK_HCD, target_link_hcd_)); + ASSERT_TRUE(node_->get_parameter(RANDOM_TEST_TRIAL_NUM, random_trial_num_)); + ASSERT_TRUE(node_->get_parameter(JOINT_POSITION_TOLERANCE, joint_position_tolerance_)); + ASSERT_TRUE(node_->get_parameter(JOINT_VELOCITY_TOLERANCE, joint_velocity_tolerance_)); + ASSERT_TRUE(node_->get_parameter(POSE_TRANSFORM_MATRIX_NORM_TOLERANCE, pose_norm_tolerance_)); + ASSERT_TRUE(node_->get_parameter(ROTATION_AXIS_NORM_TOLERANCE, rot_axis_norm_tolerance_)); + ASSERT_TRUE(node_->get_parameter(VELOCITY_SCALING_FACTOR, velocity_scaling_factor_)); + ASSERT_TRUE(node_->get_parameter(OTHER_TOLERANCE, other_tolerance_)); + + testutils::checkRobotModel(robot_model_, planning_group_, target_link_hcd_); + + // load the test data provider + tdp_ = std::make_unique(test_data_file_name_); + ASSERT_NE(nullptr, tdp_) << "Failed to load test data by provider."; + + tdp_->setRobotModel(robot_model_); + + // create the limits container + // TODO, move this also into test data set + pilz_industrial_motion_planner::JointLimitsContainer joint_limits = + pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( + node_, PARAM_NAMESPACE_LIMITS, robot_model_->getActiveJointModels()); + CartesianLimit cart_limits; + cart_limits.setMaxRotationalVelocity(0.5 * M_PI); + cart_limits.setMaxTranslationalAcceleration(2); + cart_limits.setMaxTranslationalDeceleration(2); + cart_limits.setMaxTranslationalVelocity(1); + planner_limits_.setJointLimits(joint_limits); + planner_limits_.setCartesianLimits(cart_limits); + + // initialize the LIN trajectory generator + lin_ = std::make_unique(robot_model_, planner_limits_); + ASSERT_NE(nullptr, lin_) << "Failed to create LIN trajectory generator."; + } bool checkLinResponse(const planning_interface::MotionPlanRequest& req, - const planning_interface::MotionPlanResponse& res); + const planning_interface::MotionPlanResponse& res) + { + moveit_msgs::msg::MotionPlanResponse res_msg; + res.getMessage(res_msg); + if (!testutils::isGoalReached(robot_model_, res_msg.trajectory.joint_trajectory, req, pose_norm_tolerance_)) + { + return false; + } + + if (!testutils::checkCartesianLinearity(robot_model_, res_msg.trajectory.joint_trajectory, req, + pose_norm_tolerance_, rot_axis_norm_tolerance_)) + { + return false; + } + + if (!testutils::checkJointTrajectory(res_msg.trajectory.joint_trajectory, planner_limits_.getJointLimitContainer())) + { + return false; + } + + return true; + } protected: // ros stuff - ros::NodeHandle ph_{ "~" }; - robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() }; + rclcpp::Node::SharedPtr node_; + moveit::core::RobotModelConstPtr robot_model_; + planning_scene::PlanningSceneConstPtr planning_scene_; // lin trajectory generator using model without gripper std::unique_ptr lin_; @@ -105,106 +174,38 @@ class TrajectoryGeneratorLINTest : public testing::TestWithParam LimitsContainer planner_limits_; }; -void TrajectoryGeneratorLINTest::SetUp() -{ - // get the parameters - ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name_)); - ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_)); - ASSERT_TRUE(ph_.getParam(TARGET_LINK_HCD, target_link_hcd_)); - ASSERT_TRUE(ph_.getParam(RANDOM_TEST_TRIAL_NUM, random_trial_num_)); - ASSERT_TRUE(ph_.getParam(JOINT_POSITION_TOLERANCE, joint_position_tolerance_)); - ASSERT_TRUE(ph_.getParam(JOINT_VELOCITY_TOLERANCE, joint_velocity_tolerance_)); - ASSERT_TRUE(ph_.getParam(POSE_TRANSFORM_MATRIX_NORM_TOLERANCE, pose_norm_tolerance_)); - ASSERT_TRUE(ph_.getParam(ROTATION_AXIS_NORM_TOLERANCE, rot_axis_norm_tolerance_)); - ASSERT_TRUE(ph_.getParam(VELOCITY_SCALING_FACTOR, velocity_scaling_factor_)); - ASSERT_TRUE(ph_.getParam(OTHER_TOLERANCE, other_tolerance_)); - - testutils::checkRobotModel(robot_model_, planning_group_, target_link_hcd_); - - // load the test data provider - tdp_.reset(new pilz_industrial_motion_planner_testutils::XmlTestdataLoader{ test_data_file_name_ }); - ASSERT_NE(nullptr, tdp_) << "Failed to load test data by provider."; - - tdp_->setRobotModel(robot_model_); - - // create the limits container - // TODO, move this also into test data set - pilz_industrial_motion_planner::JointLimitsContainer joint_limits = - pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(ph_, - robot_model_->getActiveJointModels()); - CartesianLimit cart_limits; - cart_limits.setMaxRotationalVelocity(0.5 * M_PI); - cart_limits.setMaxTranslationalAcceleration(2); - cart_limits.setMaxTranslationalDeceleration(2); - cart_limits.setMaxTranslationalVelocity(1); - planner_limits_.setJointLimits(joint_limits); - planner_limits_.setCartesianLimits(cart_limits); - - // initialize the LIN trajectory generator - lin_.reset(new TrajectoryGeneratorLIN(robot_model_, planner_limits_)); - ASSERT_NE(nullptr, lin_) << "Failed to create LIN trajectory generator."; -} - -bool TrajectoryGeneratorLINTest::checkLinResponse(const planning_interface::MotionPlanRequest& req, - const planning_interface::MotionPlanResponse& res) -{ - moveit_msgs::MotionPlanResponse res_msg; - res.getMessage(res_msg); - if (!testutils::isGoalReached(robot_model_, res_msg.trajectory.joint_trajectory, req, pose_norm_tolerance_)) - { - return false; - } - - if (!testutils::checkCartesianLinearity(robot_model_, res_msg.trajectory.joint_trajectory, req, pose_norm_tolerance_, - rot_axis_norm_tolerance_)) - { - return false; - } - - if (!testutils::checkJointTrajectory(res_msg.trajectory.joint_trajectory, planner_limits_.getJointLimitContainer())) - { - return false; - } - - return true; -} - /** * @brief Checks that each derived MoveItErrorCodeException contains the correct * error code. */ -TEST(TrajectoryGeneratorLINTest, TestExceptionErrorCodeMapping) +TEST_F(TrajectoryGeneratorLINTest, TestExceptionErrorCodeMapping) { { std::shared_ptr ltcf_ex{ new LinTrajectoryConversionFailure("") }; - 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); } } -// Instantiate the test cases for robot model with and without gripper -INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryGeneratorLINTest, - ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME)); - /** * @brief test the lin planner with invalid motion plan request which has non * zero start velocity */ -TEST_P(TrajectoryGeneratorLINTest, nonZeroStartVelocity) +TEST_F(TrajectoryGeneratorLINTest, nonZeroStartVelocity) { planning_interface::MotionPlanRequest req{ tdp_->getLinJoint("lin2").toRequest() }; @@ -213,21 +214,21 @@ 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_FALSE(lin_->generate(planning_scene_, req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } /** * @brief test the lin planner with joint space goal */ -TEST_P(TrajectoryGeneratorLINTest, jointSpaceGoal) +TEST_F(TrajectoryGeneratorLINTest, jointSpaceGoal) { planning_interface::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() }; // 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); + ASSERT_TRUE(lin_->generate(planning_scene_, lin_joint_req, res)); + EXPECT_TRUE(res.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); // check the resulted trajectory EXPECT_TRUE(checkLinResponse(lin_joint_req, res)); @@ -237,7 +238,7 @@ TEST_P(TrajectoryGeneratorLINTest, jointSpaceGoal) * @brief test the lin planner with joint space goal with start velocity almost * zero */ -TEST_P(TrajectoryGeneratorLINTest, jointSpaceGoalNearZeroStartVelocity) +TEST_F(TrajectoryGeneratorLINTest, jointSpaceGoalNearZeroStartVelocity) { planning_interface::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() }; @@ -247,8 +248,8 @@ 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); + ASSERT_TRUE(lin_->generate(planning_scene_, lin_joint_req, res)); + EXPECT_TRUE(res.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); // check the resulted trajectory EXPECT_TRUE(checkLinResponse(lin_joint_req, res)); @@ -257,15 +258,15 @@ TEST_P(TrajectoryGeneratorLINTest, jointSpaceGoalNearZeroStartVelocity) /** * @brief test the lin planner with Cartesian goal */ -TEST_P(TrajectoryGeneratorLINTest, cartesianSpaceGoal) +TEST_F(TrajectoryGeneratorLINTest, cartesianSpaceGoal) { // construct motion plan request - moveit_msgs::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); + ASSERT_TRUE(lin_->generate(planning_scene_, lin_cart_req, res)); + EXPECT_TRUE(res.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); // check the resulted trajectory EXPECT_TRUE(checkLinResponse(lin_cart_req, res)); @@ -279,17 +280,17 @@ TEST_P(TrajectoryGeneratorLINTest, cartesianSpaceGoal) * occur that are neither * acceleration, constant or deceleration. */ -TEST_P(TrajectoryGeneratorLINTest, cartesianTrapezoidProfile) +TEST_F(TrajectoryGeneratorLINTest, cartesianTrapezoidProfile) { // construct motion plan request - moveit_msgs::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); + ASSERT_TRUE(lin_->generate(planning_scene_, lin_joint_req, res, 0.01)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); ASSERT_TRUE(testutils::checkCartesianTranslationalPath(res.trajectory_, target_link_hcd_)); @@ -314,13 +315,13 @@ TEST_P(TrajectoryGeneratorLINTest, cartesianTrapezoidProfile) * Expected Results: * 1. Function returns 'false'. */ -TEST_P(TrajectoryGeneratorLINTest, LinPlannerLimitViolation) +TEST_F(TrajectoryGeneratorLINTest, LinPlannerLimitViolation) { LinJoint lin{ tdp_->getLinJoint("lin2") }; lin.setAccelerationScale(1.01); planning_interface::MotionPlanResponse res; - ASSERT_FALSE(lin_->generate(lin.toRequest(), res)); + ASSERT_FALSE(lin_->generate(planning_scene_, lin.toRequest(), res)); } /** @@ -334,7 +335,7 @@ TEST_P(TrajectoryGeneratorLINTest, LinPlannerLimitViolation) * Expected Results: * 1. Function returns 'false'. */ -TEST_P(TrajectoryGeneratorLINTest, LinPlannerDiscontinuousJointTraj) +TEST_F(TrajectoryGeneratorLINTest, LinPlannerDiscontinuousJointTraj) { LinJoint lin{ tdp_->getLinJoint("lin2") }; // Alter goal joint configuration (represents the same cartesian pose, but @@ -346,7 +347,7 @@ TEST_P(TrajectoryGeneratorLINTest, LinPlannerDiscontinuousJointTraj) lin.setAccelerationScale(1.0); planning_interface::MotionPlanResponse res; - ASSERT_FALSE(lin_->generate(lin.toRequest(), res)); + ASSERT_FALSE(lin_->generate(planning_scene_, lin.toRequest(), res)); } /** @@ -358,10 +359,10 @@ TEST_P(TrajectoryGeneratorLINTest, LinPlannerDiscontinuousJointTraj) * Expected Results: * 1. trajectory generation is successful. */ -TEST_P(TrajectoryGeneratorLINTest, LinStartEqualsGoal) +TEST_F(TrajectoryGeneratorLINTest, LinStartEqualsGoal) { // construct motion plan request - moveit_msgs::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); @@ -373,8 +374,8 @@ 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); + ASSERT_TRUE(lin_->generate(planning_scene_, lin_joint_req, res)); + EXPECT_TRUE(res.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); // check the resulted trajectory EXPECT_TRUE(checkLinResponse(lin_joint_req, res)); @@ -389,7 +390,7 @@ TEST_P(TrajectoryGeneratorLINTest, LinStartEqualsGoal) * Expected Results: * 1. Ctor throws exception. */ -TEST_P(TrajectoryGeneratorLINTest, CtorNoLimits) +TEST_F(TrajectoryGeneratorLINTest, CtorNoLimits) { pilz_industrial_motion_planner::LimitsContainer planner_limits; @@ -407,54 +408,54 @@ TEST_P(TrajectoryGeneratorLINTest, CtorNoLimits) * Expected Results: * 1. Function returns 'false'. */ -TEST_P(TrajectoryGeneratorLINTest, IncorrectJointNumber) +TEST_F(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(); // 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); + ASSERT_FALSE(lin_->generate(planning_scene_, lin_joint_req, res)); + EXPECT_TRUE(res.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } /** * @brief test invalid motion plan request with incomplete start state and * cartesian goal */ -TEST_P(TrajectoryGeneratorLINTest, cartGoalIncompleteStartState) +TEST_F(TrajectoryGeneratorLINTest, cartGoalIncompleteStartState) { // construct motion plan request - moveit_msgs::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 // 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_FALSE(lin_->generate(planning_scene_, lin_cart_req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } /** * @brief Set a frame id in goal constraint with cartesian goal on both position * and orientation constraints */ -TEST_P(TrajectoryGeneratorLINTest, cartGoalFrameIdBothConstraints) +TEST_F(TrajectoryGeneratorLINTest, cartGoalFrameIdBothConstraints) { // construct motion plan request - moveit_msgs::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(); // 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); + ASSERT_TRUE(lin_->generate(planning_scene_, lin_cart_req, res)); + EXPECT_TRUE(res.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); // check the resulted trajectory EXPECT_TRUE(checkLinResponse(lin_cart_req, res)); @@ -462,8 +463,7 @@ TEST_P(TrajectoryGeneratorLINTest, cartGoalFrameIdBothConstraints) int main(int argc, char** argv) { - ros::init(argc, argv, "unittest_trajectory_generator_lin"); - ros::NodeHandle nh; + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp similarity index 86% rename from moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp index b2d24dca01..b817452ea2 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp @@ -45,9 +45,7 @@ #include #include -// parameters for parameterized tests -const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; -const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; +#include "rclcpp/rclcpp.hpp" // parameters from parameter server const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); @@ -59,14 +57,66 @@ const std::string POSE_TRANSFORM_MATRIX_NORM_TOLERANCE("pose_norm_tolerance"); using namespace pilz_industrial_motion_planner; -class TrajectoryGeneratorPTPTest : public testing::TestWithParam +class TrajectoryGeneratorPTPTest : public testing::Test { protected: /** * @brief Create test fixture for ptp trajectory generator * */ - void SetUp() override; + void SetUp() override + { + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + node_ = rclcpp::Node::make_shared("unittest_trajectory_generator_ptp", node_options); + + // load robot model + robot_model_loader::RobotModelLoader rm_loader(node_); + robot_model_ = rm_loader.getModel(); + ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model"; + planning_scene_ = std::make_shared(robot_model_); + + // get parameters from parameter server + ASSERT_TRUE(node_->has_parameter("planning_group")); + node_->get_parameter("planning_group", planning_group_); + ASSERT_TRUE(node_->has_parameter("target_link")); + node_->get_parameter("target_link", target_link_); + ASSERT_TRUE(node_->has_parameter("joint_position_tolerance")); + node_->get_parameter("joint_position_tolerance", joint_position_tolerance_); + ASSERT_TRUE(node_->has_parameter("joint_velocity_tolerance")); + node_->get_parameter("joint_velocity_tolerance", joint_velocity_tolerance_); + ASSERT_TRUE(node_->has_parameter("joint_acceleration_tolerance")); + node_->get_parameter("joint_acceleration_tolerance", joint_acceleration_tolerance_); + ASSERT_TRUE(node_->has_parameter("pose_norm_tolerance")); + node_->get_parameter("pose_norm_tolerance", pose_norm_tolerance_); + + testutils::checkRobotModel(robot_model_, planning_group_, target_link_); + + // create the limits container + JointLimitsContainer joint_limits; + for (const auto& jmg : robot_model_->getJointModelGroups()) + { + std::vector joint_names = jmg->getActiveJointModelNames(); + JointLimit joint_limit; + joint_limit.max_position = 3.124; + joint_limit.min_position = -3.124; + joint_limit.has_velocity_limits = true; + joint_limit.max_velocity = 1; + joint_limit.has_acceleration_limits = true; + joint_limit.max_acceleration = 0.5; + joint_limit.has_deceleration_limits = true; + joint_limit.max_deceleration = -1; + for (const auto& joint_name : joint_names) + { + joint_limits.addLimit(joint_name, joint_limit); + } + } + + // create the trajectory generator + planner_limits_.setJointLimits(joint_limits); + ptp_ = std::make_unique(robot_model_, planner_limits_); + ASSERT_NE(nullptr, ptp_); + } /** * @brief check the resulted joint trajectory @@ -75,13 +125,22 @@ class TrajectoryGeneratorPTPTest : public testing::TestWithParam * @param joint_limits * @return */ - bool checkTrajectory(const trajectory_msgs::JointTrajectory& trajectory, - const planning_interface::MotionPlanRequest& req, const JointLimitsContainer& joint_limits); + bool checkTrajectory(const trajectory_msgs::msg::JointTrajectory& trajectory, + const planning_interface::MotionPlanRequest& req, const JointLimitsContainer& joint_limits) + { + return (testutils::isTrajectoryConsistent(trajectory) && + testutils::isGoalReached(trajectory, req.goal_constraints.front().joint_constraints, + joint_position_tolerance_, joint_velocity_tolerance_) && + testutils::isPositionBounded(trajectory, joint_limits) && + testutils::isVelocityBounded(trajectory, joint_limits) && + testutils::isAccelerationBounded(trajectory, joint_limits)); + } protected: // ros stuff - ros::NodeHandle ph_{ "~" }; - robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() }; + rclcpp::Node::SharedPtr node_; + moveit::core::RobotModelConstPtr robot_model_; + planning_scene::PlanningSceneConstPtr planning_scene_; // trajectory generator std::unique_ptr ptp_; @@ -92,80 +151,27 @@ class TrajectoryGeneratorPTPTest : public testing::TestWithParam double joint_position_tolerance_, joint_velocity_tolerance_, joint_acceleration_tolerance_, pose_norm_tolerance_; }; -void TrajectoryGeneratorPTPTest::SetUp() -{ - // get parameters from parameter server - ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_)); - ASSERT_TRUE(ph_.getParam(PARAM_TARGET_LINK_NAME, target_link_)); - ASSERT_TRUE(ph_.getParam(JOINT_POSITION_TOLERANCE, joint_position_tolerance_)); - ASSERT_TRUE(ph_.getParam(JOINT_VELOCITY_TOLERANCE, joint_velocity_tolerance_)); - ASSERT_TRUE(ph_.getParam(JOINT_ACCELERATION_TOLERANCE, joint_acceleration_tolerance_)); - ASSERT_TRUE(ph_.getParam(POSE_TRANSFORM_MATRIX_NORM_TOLERANCE, pose_norm_tolerance_)); - - testutils::checkRobotModel(robot_model_, planning_group_, target_link_); - - // create the limits container - JointLimitsContainer joint_limits; - for (const auto& jmg : robot_model_->getJointModelGroups()) - { - std::vector joint_names = jmg->getActiveJointModelNames(); - JointLimit joint_limit; - joint_limit.max_position = 3.124; - joint_limit.min_position = -3.124; - joint_limit.has_velocity_limits = true; - joint_limit.max_velocity = 1; - joint_limit.has_acceleration_limits = true; - joint_limit.max_acceleration = 0.5; - joint_limit.has_deceleration_limits = true; - joint_limit.max_deceleration = -1; - for (const auto& joint_name : joint_names) - { - joint_limits.addLimit(joint_name, joint_limit); - } - } - - // create the trajectory generator - planner_limits_.setJointLimits(joint_limits); - ptp_.reset(new TrajectoryGeneratorPTP(robot_model_, planner_limits_)); - ASSERT_NE(nullptr, ptp_); -} - -bool TrajectoryGeneratorPTPTest::checkTrajectory(const trajectory_msgs::JointTrajectory& trajectory, - const planning_interface::MotionPlanRequest& req, - const JointLimitsContainer& joint_limits) -{ - return (testutils::isTrajectoryConsistent(trajectory) && - testutils::isGoalReached(trajectory, req.goal_constraints.front().joint_constraints, - joint_position_tolerance_, joint_velocity_tolerance_) && - testutils::isPositionBounded(trajectory, joint_limits) && - testutils::isVelocityBounded(trajectory, joint_limits) && - testutils::isAccelerationBounded(trajectory, joint_limits)); -} - /** * @brief Checks that each derived MoveItErrorCodeException contains the correct * error code. */ -TEST(TrajectoryGeneratorPTPTest, TestExceptionErrorCodeMapping) +TEST_F(TrajectoryGeneratorPTPTest, TestExceptionErrorCodeMapping) { { std::shared_ptr pvpsf_ex{ new PtpVelocityProfileSyncFailed("") }; - 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); } } -// Instantiate the test cases for robot model with and without gripper -INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryGeneratorPTPTest, - ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME)); /** * @brief Construct a TrajectoryGeneratorPTP with no limits given */ -TEST_P(TrajectoryGeneratorPTPTest, noLimits) +TEST_F(TrajectoryGeneratorPTPTest, noLimits) { LimitsContainer planner_limits; EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits), TrajectoryGeneratorInvalidLimitsException); @@ -181,20 +187,20 @@ TEST_P(TrajectoryGeneratorPTPTest, noLimits) * - Expected Results: * 1. the res.trajectory_ should be cleared (contain no waypoints) */ -TEST_P(TrajectoryGeneratorPTPTest, emptyRequest) +TEST_F(TrajectoryGeneratorPTPTest, emptyRequest) { planning_interface::MotionPlanResponse res; planning_interface::MotionPlanRequest req; robot_trajectory::RobotTrajectoryPtr trajectory( new robot_trajectory::RobotTrajectory(this->robot_model_, planning_group_)); - robot_state::RobotState state(this->robot_model_); + moveit::core::RobotState state(this->robot_model_); trajectory->addPrefixWayPoint(state, 0); res.trajectory_ = trajectory; EXPECT_FALSE(res.trajectory_->empty()); - EXPECT_FALSE(ptp_->generate(req, res)); + EXPECT_FALSE(ptp_->generate(planning_scene_, req, res)); EXPECT_TRUE(res.trajectory_->empty()); } @@ -202,7 +208,7 @@ TEST_P(TrajectoryGeneratorPTPTest, emptyRequest) /** * @brief Construct a TrajectoryGeneratorPTP with missing velocity limits */ -TEST_P(TrajectoryGeneratorPTPTest, missingVelocityLimits) +TEST_F(TrajectoryGeneratorPTPTest, missingVelocityLimits) { LimitsContainer planner_limits; @@ -226,7 +232,7 @@ TEST_P(TrajectoryGeneratorPTPTest, missingVelocityLimits) /** * @brief Construct a TrajectoryGeneratorPTP missing deceleration limits */ -TEST_P(TrajectoryGeneratorPTPTest, missingDecelerationimits) +TEST_F(TrajectoryGeneratorPTPTest, missingDecelerationimits) { LimitsContainer planner_limits; @@ -257,7 +263,7 @@ TEST_P(TrajectoryGeneratorPTPTest, missingDecelerationimits) * TrajectoryGeneratorInvalidLimitsException * 2. the constructor throws no exception */ -TEST_P(TrajectoryGeneratorPTPTest, testInsufficientLimit) +TEST_F(TrajectoryGeneratorPTPTest, testInsufficientLimit) { /**********/ /* Step 1 */ @@ -332,7 +338,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testInsufficientLimit) /** * @brief test the ptp trajectory generator of Cartesian space goal */ -TEST_P(TrajectoryGeneratorPTPTest, testCartesianGoal) +TEST_F(TrajectoryGeneratorPTPTest, testCartesianGoal) { //*************************************** //*** prepare the motion plan request *** @@ -342,7 +348,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testCartesianGoal) testutils::createDummyRequest(robot_model_, planning_group_, req); // cartesian goal pose - geometry_msgs::PoseStamped pose; + geometry_msgs::msg::PoseStamped pose; pose.pose.position.x = 0.1; pose.pose.position.y = 0.2; pose.pose.position.z = 0.65; @@ -352,17 +358,17 @@ 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); //**************************************** //*** test robot model without gripper *** //**************************************** - ASSERT_TRUE(ptp_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + ASSERT_TRUE(ptp_->generate(planning_scene_, req, res)); + 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()) { @@ -381,7 +387,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testCartesianGoal) * @brief Check that missing a link_name in position or orientation constraints * is detected */ -TEST_P(TrajectoryGeneratorPTPTest, testCartesianGoalMissingLinkNameConstraints) +TEST_F(TrajectoryGeneratorPTPTest, testCartesianGoalMissingLinkNameConstraints) { //*************************************** //*** prepare the motion plan request *** @@ -391,7 +397,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testCartesianGoalMissingLinkNameConstraints) testutils::createDummyRequest(robot_model_, planning_group_, req); // cartesian goal pose - geometry_msgs::PoseStamped pose; + geometry_msgs::msg::PoseStamped pose; pose.pose.position.x = 0.1; pose.pose.position.y = 0.2; pose.pose.position.z = 0.65; @@ -401,31 +407,31 @@ 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); + ASSERT_FALSE(ptp_->generate(planning_scene_, req_no_position_constaint_link_name, res)); + 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); + ASSERT_FALSE(ptp_->generate(planning_scene_, req_no_orientation_constaint_link_name, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } /** * @brief test the ptp trajectory generator of invalid Cartesian space goal */ -TEST_P(TrajectoryGeneratorPTPTest, testInvalidCartesianGoal) +TEST_F(TrajectoryGeneratorPTPTest, testInvalidCartesianGoal) { planning_interface::MotionPlanResponse res; planning_interface::MotionPlanRequest req; testutils::createDummyRequest(robot_model_, planning_group_, req); - geometry_msgs::PoseStamped pose; + geometry_msgs::msg::PoseStamped pose; pose.pose.position.x = 0.1; pose.pose.position.y = 0.2; pose.pose.position.z = 2.5; @@ -435,12 +441,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); + ASSERT_FALSE(ptp_->generate(planning_scene_, req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); EXPECT_EQ(res.trajectory_, nullptr); } @@ -449,7 +455,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testInvalidCartesianGoal) * enough to the start which does not need * to plan the trajectory */ -TEST_P(TrajectoryGeneratorPTPTest, testJointGoalAlreadyReached) +TEST_F(TrajectoryGeneratorPTPTest, testJointGoalAlreadyReached) { planning_interface::MotionPlanResponse res; planning_interface::MotionPlanRequest req; @@ -457,18 +463,18 @@ 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); req.goal_constraints.push_back(gc); // TODO lin and circ has different settings - ASSERT_TRUE(ptp_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + ASSERT_TRUE(ptp_->generate(planning_scene_, req, res)); + 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()); } @@ -477,7 +483,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testJointGoalAlreadyReached) * @brief test scaling factor * with zero start velocity */ -TEST_P(TrajectoryGeneratorPTPTest, testScalingFactor) +TEST_F(TrajectoryGeneratorPTPTest, testScalingFactor) { // create ptp generator with different limits JointLimit joint_limit; @@ -516,14 +522,14 @@ TEST_P(TrajectoryGeneratorPTPTest, testScalingFactor) planner_limits.setJointLimits(joint_limits); // create the generator with new limits - ptp_.reset(new TrajectoryGeneratorPTP(robot_model_, planner_limits)); + ptp_ = std::make_unique(robot_model_, planner_limits); planning_interface::MotionPlanResponse res; 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); @@ -537,10 +543,10 @@ TEST_P(TrajectoryGeneratorPTPTest, testScalingFactor) req.max_velocity_scaling_factor = 0.5; 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); + ASSERT_TRUE(ptp_->generate(planning_scene_, req, res)); + 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())); @@ -641,7 +647,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testScalingFactor) * @brief test the ptp trajectory generator of joint space goal * with (almost) zero start velocity */ -TEST_P(TrajectoryGeneratorPTPTest, testJointGoalAndAlmostZeroStartVelocity) +TEST_F(TrajectoryGeneratorPTPTest, testJointGoalAndAlmostZeroStartVelocity) { planning_interface::MotionPlanResponse res; planning_interface::MotionPlanRequest req; @@ -651,8 +657,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); @@ -664,10 +670,10 @@ TEST_P(TrajectoryGeneratorPTPTest, testJointGoalAndAlmostZeroStartVelocity) gc.joint_constraints.push_back(jc); req.goal_constraints.push_back(gc); - ASSERT_TRUE(ptp_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + ASSERT_TRUE(ptp_->generate(planning_scene_, req, res)); + 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())); @@ -778,7 +784,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testJointGoalAndAlmostZeroStartVelocity) * @brief test the ptp_ trajectory generator of joint space goal * with zero start velocity */ -TEST_P(TrajectoryGeneratorPTPTest, testJointGoalNoStartVel) +TEST_F(TrajectoryGeneratorPTPTest, testJointGoalNoStartVel) { planning_interface::MotionPlanResponse res; planning_interface::MotionPlanRequest req; @@ -786,8 +792,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; @@ -806,10 +812,10 @@ TEST_P(TrajectoryGeneratorPTPTest, testJointGoalNoStartVel) gc.joint_constraints.push_back(jc); req.goal_constraints.push_back(gc); - ASSERT_TRUE(ptp_->generate(req, res)); - EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + ASSERT_TRUE(ptp_->generate(planning_scene_, req, res)); + 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())); @@ -964,8 +970,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testJointGoalNoStartVel) int main(int argc, char** argv) { - ros::init(argc, argv, "unittest_trajectory_generator_ptp"); - ros::NodeHandle nh; + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_velocity_profile_atrap.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_velocity_profile_atrap.cpp similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/test/unittest_velocity_profile_atrap.cpp rename to moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_velocity_profile_atrap.cpp diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_cartesian_limits_aggregator.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_cartesian_limits_aggregator.test deleted file mode 100644 index 382a5a5924..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_cartesian_limits_aggregator.test +++ /dev/null @@ -1,53 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit.test deleted file mode 100644 index 361e6f5102..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit.test +++ /dev/null @@ -1,40 +0,0 @@ - - - - - - - - diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit_config.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit_config.yaml deleted file mode 100644 index 4f4bfffd6e..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit_config.yaml +++ /dev/null @@ -1,31 +0,0 @@ -joint_limits: - joint_1: - has_acceleration_limits: true - max_acceleration: 1 - has_deceleration_limits: true - max_deceleration: -1 - joint_2: - has_acceleration_limits: true - max_acceleration: 2 - has_deceleration_limits: true - max_deceleration: -2 - joint_3: - has_acceleration_limits: true - max_acceleration: 3 - has_deceleration_limits: true - max_deceleration: -3 - joint_4: - has_acceleration_limits: true - max_acceleration: 4 - has_deceleration_limits: true - max_deceleration: -4 - joint_5: - has_acceleration_limits: true - max_acceleration: 5 - has_deceleration_limits: true - max_deceleration: -5 - joint_6: - has_acceleration_limits: true - max_acceleration: 6 - has_deceleration_limits: true - max_deceleration: -6 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_aggregator.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_aggregator.test deleted file mode 100644 index 28177da403..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_aggregator.test +++ /dev/null @@ -1,58 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.test deleted file mode 100644 index 574f005626..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.test +++ /dev/null @@ -1,48 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.test deleted file mode 100644 index 048e67c804..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.test +++ /dev/null @@ -1,48 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.test deleted file mode 100644 index 766bc1af98..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.test +++ /dev/null @@ -1,48 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.test deleted file mode 100644 index 2d0bf6c3d5..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.test +++ /dev/null @@ -1,65 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.test deleted file mode 100644 index ff1c41bb0f..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.test +++ /dev/null @@ -1,53 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.test deleted file mode 100644 index 6d84d42105..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.test +++ /dev/null @@ -1,61 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.test deleted file mode 100644 index 702c2dc5fc..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.test +++ /dev/null @@ -1,59 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.test deleted file mode 100644 index 9cec117a1d..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.test +++ /dev/null @@ -1,62 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.test deleted file mode 100644 index 86dbff3599..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.test +++ /dev/null @@ -1,56 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner_testutils/CMakeLists.txt index 164a04d446..f444c1f743 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/CMakeLists.txt +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/CMakeLists.txt @@ -1,62 +1,29 @@ -cmake_minimum_required(VERSION 3.1.3) +cmake_minimum_required(VERSION 3.10.2) project(pilz_industrial_motion_planner_testutils) -## 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) -add_definitions(-Werror) - -find_package(catkin REQUIRED COMPONENTS - tf2_eigen - moveit_core - moveit_msgs -) +# Common cmake code applied to all moveit packages +find_package(moveit_common REQUIRED) +moveit_package() +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(moveit_core REQUIRED) +find_package(moveit_msgs REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED COMPONENTS) -################ -## 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( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS moveit_core moveit_msgs - DEPENDS Boost -) - ########### ## Build ## ########### ## Specify additional locations of header files ## Your package locations should be listed before other locations -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} -) +include_directories(include) ## Declare a C++ library -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED src/cartesianconfiguration.cpp src/jointconfiguration.cpp src/robotconfiguration.cpp @@ -65,24 +32,47 @@ add_library(${PROJECT_NAME} ) ## Specify libraries to link a library or executable target against -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME} + Boost + Eigen3 + eigen3_cmake_module + rclcpp + moveit_core + moveit_msgs + tf2_eigen ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS}) - ############# ## Install ## ############# -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include ) -## Mark executables and/or libraries for installation -install(TARGETS ${PROJECT_NAME} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -) +install(DIRECTORY include/ DESTINATION include) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(rclcpp moveit_core moveit_msgs tf2_eigen) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # 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_flake8_FOUND TRUE) + set(ament_cmake_uncrustify_FOUND TRUE) + set(ament_cmake_lint_cmake_FOUND TRUE) + + # Run all lint tests in package.xml except those listed above + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/COLCON_IGNORE b/moveit_planners/pilz_industrial_motion_planner_testutils/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/async_test.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/async_test.h index 4b408e2bd8..14f03c82d8 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/async_test.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/async_test.h @@ -25,10 +25,11 @@ #include -#include +#include namespace testing { +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.async_test"); /** * @brief Test class that allows the handling of asynchronous test objects * @@ -113,7 +114,7 @@ class AsyncTest this->triggerClearEvent(str); \ return true; \ }) -#define ACTION_OPEN_BARRIER_VOID(str) ::testing::InvokeWithoutArgs([this](void) { this->triggerClearEvent(str); }) +#define ACTION_OPEN_BARRIER_VOID(str) ::testing::InvokeWithoutArgs([this]() { this->triggerClearEvent(str); }) inline void AsyncTest::triggerClearEvent(const std::string& event) { @@ -121,12 +122,12 @@ inline void AsyncTest::triggerClearEvent(const std::string& event) if (clear_events_.empty()) { - ROS_DEBUG_NAMED("Test", "Clearing Barricade[%s]", event.c_str()); + RCLCPP_DEBUG(LOGGER, "Clearing Barricade[%s]", event.c_str()); waitlist_.insert(event); } else if (clear_events_.erase(event) < 1) { - ROS_WARN_STREAM("Triggered event " << event << " despite not waiting for it."); + RCLCPP_WARN(LOGGER, "Triggered event " << event << " despite not waiting for it."); } cv_.notify_one(); } @@ -145,10 +146,10 @@ inline bool AsyncTest::barricade(std::initializer_list clear_events { events_stringstream << event << ", "; } - ROS_DEBUG_NAMED("Test", "Adding Barricade[%s]", events_stringstream.str().c_str()); + RCLCPP_DEBUG(LOGGER, "Adding Barricade[%s]", events_stringstream.str().c_str()); std::copy_if(clear_events.begin(), clear_events.end(), std::inserter(clear_events_, clear_events_.end()), - [this](std::string event) { return this->waitlist_.count(event) == 0; }); + [this](const std::string& event) { return this->waitlist_.count(event) == 0; }); waitlist_.clear(); auto end_time_point = std::chrono::system_clock::now() + std::chrono::milliseconds(timeout_ms); diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianconfiguration.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianconfiguration.h index dee19b7473..be43dd3d23 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianconfiguration.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianconfiguration.h @@ -39,8 +39,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -66,15 +66,15 @@ class CartesianConfiguration : public RobotConfiguration const moveit::core::RobotModelConstPtr& robot_model); public: - moveit_msgs::Constraints toGoalConstraints() const override; - moveit_msgs::RobotState toMoveitMsgsRobotState() const override; + moveit_msgs::msg::Constraints toGoalConstraints() const override; + moveit_msgs::msg::RobotState toMoveitMsgsRobotState() const override; void setLinkName(const std::string& link_name); const std::string& getLinkName() const; - void setPose(const geometry_msgs::Pose& pose); - const geometry_msgs::Pose& getPose() const; - geometry_msgs::Pose& getPose(); + void setPose(const geometry_msgs::msg::Pose& pose); + const geometry_msgs::msg::Pose& getPose() const; + geometry_msgs::msg::Pose& getPose(); void setSeed(const JointConfiguration& config); const JointConfiguration& getSeed() const; @@ -88,12 +88,12 @@ class CartesianConfiguration : public RobotConfiguration const boost::optional getAngleTolerance() const; private: - static geometry_msgs::Pose toPose(const std::vector& pose); - static geometry_msgs::PoseStamped toStampedPose(const geometry_msgs::Pose& pose); + static geometry_msgs::msg::Pose toPose(const std::vector& pose); + static geometry_msgs::msg::PoseStamped toStampedPose(const geometry_msgs::msg::Pose& pose); private: std::string link_name_; - geometry_msgs::Pose pose_; + geometry_msgs::msg::Pose pose_; //! @brief The dimensions of the sphere associated with the target region //! of the position constraint. @@ -119,22 +119,22 @@ inline const std::string& CartesianConfiguration::getLinkName() const return link_name_; } -inline void CartesianConfiguration::setPose(const geometry_msgs::Pose& pose) +inline void CartesianConfiguration::setPose(const geometry_msgs::msg::Pose& pose) { pose_ = pose; } -inline const geometry_msgs::Pose& CartesianConfiguration::getPose() const +inline const geometry_msgs::msg::Pose& CartesianConfiguration::getPose() const { return pose_; } -inline geometry_msgs::Pose& CartesianConfiguration::getPose() +inline geometry_msgs::msg::Pose& CartesianConfiguration::getPose() { return pose_; } -inline moveit_msgs::Constraints CartesianConfiguration::toGoalConstraints() const +inline moveit_msgs::msg::Constraints CartesianConfiguration::toGoalConstraints() const { if (!tolerance_pose_ || !tolerance_angle_) { diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianpathconstraintsbuilder.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianpathconstraintsbuilder.h index c30ce98360..06a64c2d54 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianpathconstraintsbuilder.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianpathconstraintsbuilder.h @@ -37,14 +37,14 @@ #include -#include +#include #include "cartesianconfiguration.h" namespace pilz_industrial_motion_planner_testutils { /** - * @brief Helper class to build moveit_msgs::Constraints from a + * @brief Helper class to build moveit_msgs::msg::Constraints from a * given configuration. */ class CartesianPathConstraintsBuilder @@ -53,7 +53,7 @@ class CartesianPathConstraintsBuilder CartesianPathConstraintsBuilder& setConstraintName(const std::string& constraint_name); CartesianPathConstraintsBuilder& setConfiguration(const CartesianConfiguration& configuration); - moveit_msgs::Constraints toPathConstraints() const; + moveit_msgs::msg::Constraints toPathConstraints() const; private: std::string constraint_name_; @@ -74,13 +74,13 @@ CartesianPathConstraintsBuilder::setConfiguration(const CartesianConfiguration& return *this; } -inline moveit_msgs::Constraints CartesianPathConstraintsBuilder::toPathConstraints() const +inline moveit_msgs::msg::Constraints CartesianPathConstraintsBuilder::toPathConstraints() const { - moveit_msgs::PositionConstraint pos_constraint; + moveit_msgs::msg::PositionConstraint pos_constraint; pos_constraint.link_name = configuration_.getLinkName(); pos_constraint.constraint_region.primitive_poses.push_back(configuration_.getPose()); - moveit_msgs::Constraints path_constraints; + moveit_msgs::msg::Constraints path_constraints; path_constraints.name = constraint_name_; path_constraints.position_constraints.push_back(pos_constraint); return path_constraints; diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/checks.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/checks.h index 1921c8ba00..865aa8f838 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/checks.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/checks.h @@ -40,8 +40,8 @@ namespace pilz_industrial_motion_planner_testutils { -::testing::AssertionResult isAtExpectedPosition(const robot_state::RobotState& expected, - const robot_state::RobotState& actual, const double epsilon, +::testing::AssertionResult isAtExpectedPosition(const moveit::core::RobotState& expected, + const moveit::core::RobotState& actual, const double epsilon, const std::string& group_name = "") { if (group_name.empty() && expected.distance(actual) <= epsilon) diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circauxiliary.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circauxiliary.h index 81958f9b67..13a36e70b5 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circauxiliary.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circauxiliary.h @@ -37,7 +37,7 @@ #include -#include +#include namespace pilz_industrial_motion_planner_testutils { @@ -54,7 +54,7 @@ class CircAuxiliary const ConfigType& getConfiguration() const; public: - moveit_msgs::Constraints toPathConstraints() const; + moveit_msgs::msg::Constraints toPathConstraints() const; private: virtual std::string getConstraintName() const = 0; @@ -82,7 +82,7 @@ inline const ConfigType& CircAuxiliary::getConfiguratio } template -inline moveit_msgs::Constraints CircAuxiliary::toPathConstraints() const +inline moveit_msgs::msg::Constraints CircAuxiliary::toPathConstraints() const { return BuilderType().setConstraintName(getConstraintName()).setConfiguration(getConfiguration()).toPathConstraints(); } diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/goalconstraintsmsgconvertible.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/goalconstraintsmsgconvertible.h index 1595570449..282fd9cc44 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/goalconstraintsmsgconvertible.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/goalconstraintsmsgconvertible.h @@ -37,19 +37,19 @@ #include -#include +#include #include namespace pilz_industrial_motion_planner_testutils { /** * @brief Interface class to express that a derived class can be converted - * into a moveit_msgs::Constaints. + * into a moveit_msgs::msg::Constaints. */ class GoalConstraintMsgConvertible { public: - virtual moveit_msgs::Constraints toGoalConstraints() const = 0; + virtual moveit_msgs::msg::Constraints toGoalConstraints() const = 0; }; } // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/jointconfiguration.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/jointconfiguration.h index b133d568c2..b040f98f26 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/jointconfiguration.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/jointconfiguration.h @@ -41,7 +41,7 @@ #include #include -#include +#include #include #include @@ -81,21 +81,21 @@ class JointConfiguration : public RobotConfiguration size_t size() const; - moveit_msgs::Constraints toGoalConstraints() const override; - moveit_msgs::RobotState toMoveitMsgsRobotState() const override; + moveit_msgs::msg::Constraints toGoalConstraints() const override; + moveit_msgs::msg::RobotState toMoveitMsgsRobotState() const override; - sensor_msgs::JointState toSensorMsg() const; + sensor_msgs::msg::JointState toSensorMsg() const; - robot_state::RobotState toRobotState() const; + moveit::core::RobotState toRobotState() const; void setCreateJointNameFunc(CreateJointNameFunc create_joint_name_func); private: - moveit_msgs::RobotState toMoveitMsgsRobotStateWithoutModel() const; - moveit_msgs::RobotState toMoveitMsgsRobotStateWithModel() const; + moveit_msgs::msg::RobotState toMoveitMsgsRobotStateWithoutModel() const; + moveit_msgs::msg::RobotState toMoveitMsgsRobotStateWithModel() const; - moveit_msgs::Constraints toGoalConstraintsWithoutModel() const; - moveit_msgs::Constraints toGoalConstraintsWithModel() const; + moveit_msgs::msg::Constraints toGoalConstraintsWithoutModel() const; + moveit_msgs::msg::Constraints toGoalConstraintsWithModel() const; private: //! Joint positions @@ -106,12 +106,12 @@ class JointConfiguration : public RobotConfiguration std::ostream& operator<<(std::ostream& /*os*/, const JointConfiguration& /*obj*/); -inline moveit_msgs::Constraints JointConfiguration::toGoalConstraints() const +inline moveit_msgs::msg::Constraints JointConfiguration::toGoalConstraints() const { return robot_model_ ? toGoalConstraintsWithModel() : toGoalConstraintsWithoutModel(); } -inline moveit_msgs::RobotState JointConfiguration::toMoveitMsgsRobotState() const +inline moveit_msgs::msg::RobotState JointConfiguration::toMoveitMsgsRobotState() const { return robot_model_ ? toMoveitMsgsRobotStateWithModel() : toMoveitMsgsRobotStateWithoutModel(); } diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotstatemsgconvertible.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotstatemsgconvertible.h index 4b9ba87df4..5945987822 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotstatemsgconvertible.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotstatemsgconvertible.h @@ -35,19 +35,19 @@ #ifndef ROBOTSTATEMSGCONVERTIBLE_H #define ROBOTSTATEMSGCONVERTIBLE_H -#include +#include #include namespace pilz_industrial_motion_planner_testutils { /** * @brief Interface class to express that a derived class can be converted - * into a moveit_msgs::RobotState. + * into a moveit_msgs::msg::RobotState. */ class RobotStateMsgConvertible { public: - virtual moveit_msgs::RobotState toMoveitMsgsRobotState() const = 0; + virtual moveit_msgs::msg::RobotState toMoveitMsgsRobotState() const = 0; }; } // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/sequence.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/sequence.h index e555b8405d..04063a15d1 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/sequence.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/sequence.h @@ -40,7 +40,7 @@ #include #include -#include +#include #include "command_types_typedef.h" #include "motioncmd.h" @@ -93,7 +93,7 @@ class Sequence */ void erase(const size_t start, const size_t end); - moveit_msgs::MotionSequenceRequest toRequest() const; + moveit_msgs::msg::MotionSequenceRequest toRequest() const; private: using TCmdRadiiPair = std::pair; diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml index af763d3e63..b5761863a2 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml @@ -1,7 +1,8 @@ - + + pilz_industrial_motion_planner_testutils - 1.1.5 + 1.1.6 Helper scripts and functionality to test industrial motion generation Alexander Gutenkunst @@ -16,14 +17,26 @@ https://github.com/ros-planning/moveit2/issues https://github.com/ros-planning/moveit2 - moveit_core - moveit_msgs + ament_cmake + eigen3_cmake_module + eigen3_cmake_module + moveit_common + + rclcpp + moveit_core + moveit_msgs + tf2_eigen - moveit_core - moveit_msgs - moveit_commander + + + ament_lint_auto + ament_lint_common - catkin + + ament_cmake + diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/src/cartesianconfiguration.cpp b/moveit_planners/pilz_industrial_motion_planner_testutils/src/cartesianconfiguration.cpp index 16dcf10ae3..57800779a9 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/src/cartesianconfiguration.cpp +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/src/cartesianconfiguration.cpp @@ -65,7 +65,7 @@ CartesianConfiguration::CartesianConfiguration(const std::string& group_name, co throw std::invalid_argument(msg); } - if (robot_model && (!robot_state::RobotState(robot_model_).knowsFrameTransform(link_name_))) + if (robot_model && (!moveit::core::RobotState(robot_model_).knowsFrameTransform(link_name_))) { std::string msg{ "Tranform of \"" }; msg.append(link_name).append("\" is unknown"); @@ -73,9 +73,9 @@ CartesianConfiguration::CartesianConfiguration(const std::string& group_name, co } } -geometry_msgs::Pose CartesianConfiguration::toPose(const std::vector& pose) +geometry_msgs::msg::Pose CartesianConfiguration::toPose(const std::vector& pose) { - geometry_msgs::Pose pose_msg; + geometry_msgs::msg::Pose pose_msg; pose_msg.position.x = pose.at(0); pose_msg.position.y = pose.at(1); pose_msg.position.z = pose.at(2); @@ -87,21 +87,21 @@ geometry_msgs::Pose CartesianConfiguration::toPose(const std::vector& po return pose_msg; } -geometry_msgs::PoseStamped CartesianConfiguration::toStampedPose(const geometry_msgs::Pose& pose) +geometry_msgs::msg::PoseStamped CartesianConfiguration::toStampedPose(const geometry_msgs::msg::Pose& pose) { - geometry_msgs::PoseStamped pose_stamped_msg; + geometry_msgs::msg::PoseStamped pose_stamped_msg; pose_stamped_msg.pose = pose; return pose_stamped_msg; } -moveit_msgs::RobotState CartesianConfiguration::toMoveitMsgsRobotState() const +moveit_msgs::msg::RobotState CartesianConfiguration::toMoveitMsgsRobotState() const { if (!robot_model_) { throw std::runtime_error("No robot model set"); } - robot_state::RobotState rstate(robot_model_); + moveit::core::RobotState rstate(robot_model_); rstate.setToDefaultValues(); if (hasSeed()) { @@ -121,7 +121,7 @@ moveit_msgs::RobotState CartesianConfiguration::toMoveitMsgsRobotState() const } // Conversion to RobotState msg type - moveit_msgs::RobotState robot_state_msg; + moveit_msgs::msg::RobotState robot_state_msg; moveit::core::robotStateToRobotStateMsg(rstate, robot_state_msg, true); return robot_state_msg; } @@ -130,7 +130,8 @@ std::ostream& operator<<(std::ostream& os, const CartesianConfiguration& obj) { os << "Group name: \"" << obj.getGroupName() << "\""; os << " | link name: \"" << obj.getLinkName() << "\""; - os << "\n" << obj.getPose(); + // TODO(henning): fix pose msg serialization + // os << "\n" << obj.getPose(); return os; } diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/src/jointconfiguration.cpp b/moveit_planners/pilz_industrial_motion_planner_testutils/src/jointconfiguration.cpp index 4ab2cbb5d9..d4b5a2538f 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/src/jointconfiguration.cpp +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/src/jointconfiguration.cpp @@ -54,18 +54,18 @@ JointConfiguration::JointConfiguration(const std::string& group_name, const std: { } -moveit_msgs::Constraints JointConfiguration::toGoalConstraintsWithoutModel() const +moveit_msgs::msg::Constraints JointConfiguration::toGoalConstraintsWithoutModel() const { if (!create_joint_name_func_) { throw JointConfigurationException("Create-Joint-Name function not set"); } - moveit_msgs::Constraints gc; + moveit_msgs::msg::Constraints gc; for (size_t i = 0; i < joints_.size(); ++i) { - moveit_msgs::JointConstraint jc; + moveit_msgs::msg::JointConstraint jc; jc.joint_name = create_joint_name_func_(i); jc.position = joints_.at(i); gc.joint_constraints.push_back(jc); @@ -74,28 +74,28 @@ moveit_msgs::Constraints JointConfiguration::toGoalConstraintsWithoutModel() con return gc; } -moveit_msgs::Constraints JointConfiguration::toGoalConstraintsWithModel() const +moveit_msgs::msg::Constraints JointConfiguration::toGoalConstraintsWithModel() const { if (!robot_model_) { throw JointConfigurationException("No robot model set"); } - robot_state::RobotState state(robot_model_); + moveit::core::RobotState state(robot_model_); state.setToDefaultValues(); state.setJointGroupPositions(group_name_, joints_); return kinematic_constraints::constructGoalConstraints(state, state.getRobotModel()->getJointModelGroup(group_name_)); } -moveit_msgs::RobotState JointConfiguration::toMoveitMsgsRobotStateWithoutModel() const +moveit_msgs::msg::RobotState JointConfiguration::toMoveitMsgsRobotStateWithoutModel() const { if (!create_joint_name_func_) { throw JointConfigurationException("Create-Joint-Name function not set"); } - moveit_msgs::RobotState robot_state; + moveit_msgs::msg::RobotState robot_state; for (size_t i = 0; i < joints_.size(); ++i) { robot_state.joint_state.name.emplace_back(create_joint_name_func_(i)); @@ -104,35 +104,35 @@ moveit_msgs::RobotState JointConfiguration::toMoveitMsgsRobotStateWithoutModel() return robot_state; } -robot_state::RobotState JointConfiguration::toRobotState() const +moveit::core::RobotState JointConfiguration::toRobotState() const { if (!robot_model_) { throw JointConfigurationException("No robot model set"); } - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); robot_state.setJointGroupPositions(group_name_, joints_); return robot_state; } -moveit_msgs::RobotState JointConfiguration::toMoveitMsgsRobotStateWithModel() const +moveit_msgs::msg::RobotState JointConfiguration::toMoveitMsgsRobotStateWithModel() const { - robot_state::RobotState start_state(toRobotState()); - moveit_msgs::RobotState rob_state_msg; + moveit::core::RobotState start_state(toRobotState()); + moveit_msgs::msg::RobotState rob_state_msg; moveit::core::robotStateToRobotStateMsg(start_state, rob_state_msg, false); return rob_state_msg; } -sensor_msgs::JointState JointConfiguration::toSensorMsg() const +sensor_msgs::msg::JointState JointConfiguration::toSensorMsg() const { if (!create_joint_name_func_) { throw JointConfigurationException("Create-Joint-Name function not set"); } - sensor_msgs::JointState state; + sensor_msgs::msg::JointState state; for (size_t i = 0; i < joints_.size(); ++i) { state.name.emplace_back(create_joint_name_func_(i)); diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/src/sequence.cpp b/moveit_planners/pilz_industrial_motion_planner_testutils/src/sequence.cpp index b606668385..e6b3043314 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/src/sequence.cpp +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/src/sequence.cpp @@ -65,21 +65,21 @@ class ToBaseVisitor : public boost::static_visitor } }; -moveit_msgs::MotionSequenceRequest Sequence::toRequest() const +moveit_msgs::msg::MotionSequenceRequest Sequence::toRequest() const { - moveit_msgs::MotionSequenceRequest req; + moveit_msgs::msg::MotionSequenceRequest req; std::vector group_names; for (const auto& cmd : cmds_) { - moveit_msgs::MotionSequenceItem item; + moveit_msgs::msg::MotionSequenceItem item; item.req = boost::apply_visitor(ToReqVisitor(), cmd.first); if (std::find(group_names.begin(), group_names.end(), item.req.group_name) != group_names.end()) { // Remove start state because only the first request of a group // 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(); } else { diff --git a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/bfs3d/BFS_3D.h b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/bfs3d/BFS_3D.h index d37da370c1..da8a8a9b8a 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/bfs3d/BFS_3D.h +++ b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/bfs3d/BFS_3D.h @@ -55,7 +55,7 @@ class BFS_3D int* queue; int queue_head, queue_tail; - boost::shared_ptr search_thread_; + std::shared_ptr search_thread_; volatile bool running; diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/BFS_3D.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/BFS_3D.cpp index e8b7ab9113..b645482500 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/BFS_3D.cpp +++ b/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/BFS_3D.cpp @@ -133,8 +133,8 @@ void BFS_3D::run(int x, int y, int z) distance_grid[origin] = 0; - search_thread_.reset( - new boost::thread(&BFS_3D::search, this, dim_x, dim_xy, distance_grid, queue, queue_head, queue_tail)); + search_thread_ = std::make_shared(&BFS_3D::search, this, dim_x, dim_xy, distance_grid, queue, + queue_head, queue_tail); running = true; } diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_meta_interface.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_meta_interface.cpp index f8e4c07185..9c284ee91e 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_meta_interface.cpp +++ b/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_meta_interface.cpp @@ -40,8 +40,8 @@ namespace sbpl_interface { SBPLMetaInterface::SBPLMetaInterface(const planning_models::RobotModelConstPtr& robot_model) { - sbpl_interface_first_.reset(new sbpl_interface::SBPLInterface(robot_model)); - sbpl_interface_second_.reset(new sbpl_interface::SBPLInterface(robot_model)); + sbpl_interface_first_ = std::make_shared(robot_model); + sbpl_interface_second_ = std::make_shared(robot_model); } bool SBPLMetaInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_scene, diff --git a/moveit_planners/sbpl/ros/sbpl_interface_ros/src/sbpl_meta_plugin.cpp b/moveit_planners/sbpl/ros/sbpl_interface_ros/src/sbpl_meta_plugin.cpp index e8fc916922..9ca7b0d4ea 100644 --- a/moveit_planners/sbpl/ros/sbpl_interface_ros/src/sbpl_meta_plugin.cpp +++ b/moveit_planners/sbpl/ros/sbpl_interface_ros/src/sbpl_meta_plugin.cpp @@ -51,7 +51,7 @@ class SBPLMetaPlanner : public planning_interface::Planner { ros::NodeHandle nh; // display_bfs_publisher_ = nh.advertise("planning_components_visualization", 10, true); - sbpl_meta_interface_.reset(new sbpl_interface::SBPLMetaInterface(model)); + sbpl_meta_interface_ = std::make_shared(model); } bool canServiceRequest(const moveit_msgs::srv::GetMotionPlan::Request& req, diff --git a/moveit_planners/sbpl/ros/sbpl_interface_ros/src/sbpl_plugin.cpp b/moveit_planners/sbpl/ros/sbpl_interface_ros/src/sbpl_plugin.cpp index 1487c89e6f..b482796f87 100644 --- a/moveit_planners/sbpl/ros/sbpl_interface_ros/src/sbpl_plugin.cpp +++ b/moveit_planners/sbpl/ros/sbpl_interface_ros/src/sbpl_plugin.cpp @@ -51,7 +51,7 @@ class SBPLPlanner : public planning_interface::Planner { ros::NodeHandle nh; display_bfs_publisher_ = nh.advertise("planning_components_visualization", 10, true); - sbpl_interface_.reset(new sbpl_interface::SBPLInterface(model)); + sbpl_interface_ = std::make_shared(model); } bool canServiceRequest(const moveit_msgs::srv::GetMotionPlan::Request& req, diff --git a/moveit_planners/trajopt/package.xml b/moveit_planners/trajopt/package.xml index acdad0f5cf..8314a2f7ee 100644 --- a/moveit_planners/trajopt/package.xml +++ b/moveit_planners/trajopt/package.xml @@ -1,10 +1,10 @@ - + + + moveit_planners_trajopt 1.1.0 TrajOpt planning plugin, an optimization based motion planner - Omid Heidari - Omid Heidari MoveIt Release Team @@ -14,6 +14,8 @@ https://github.com/ros-planning/moveit2/issues https://github.com/ros-planning/moveit2 + Omid Heidari + catkin pluginlib diff --git a/moveit_planners/trajopt/test/trajectory_test.cpp b/moveit_planners/trajopt/test/trajectory_test.cpp index a51a7996b5..2154106db6 100644 --- a/moveit_planners/trajopt/test/trajectory_test.cpp +++ b/moveit_planners/trajopt/test/trajectory_test.cpp @@ -138,7 +138,7 @@ TEST_F(TrajectoryTest, goalTolerance) // ====================================================================================== // We will now construct a loader to load a planner, by name. // Note that we are using the ROS pluginlib library here. - boost::scoped_ptr> planner_plugin_loader; + std::unique_ptr> planner_plugin_loader; planning_interface::PlannerManagerPtr planner_instance; std::string planner_plugin_name = "trajopt_interface/TrajOptPlanner"; @@ -148,8 +148,8 @@ TEST_F(TrajectoryTest, goalTolerance) EXPECT_TRUE(node_handle_.getParam("planning_plugin", planner_plugin_name)); try { - planner_plugin_loader.reset(new pluginlib::ClassLoader( - "moveit_core", "planning_interface::PlannerManager")); + planner_plugin_loader = std::make_shared>( + "moveit_core", "planning_interface::PlannerManager"); } catch (pluginlib::PluginlibException& ex) { diff --git a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp new file mode 100644 index 0000000000..388fbda4a2 --- /dev/null +++ b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp @@ -0,0 +1,289 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Ioan A. Sucan nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Ioan Sucan, Dave Coleman, Robert Haschke */ + +#include "moveit_fake_controllers.h" +#include +#include +#include +#include +#include +#include +#include + +namespace moveit_fake_controller_manager +{ +static const std::string DEFAULT_TYPE = "interpolate"; +static const std::string ROBOT_DESCRIPTION = "robot_description"; + +class MoveItFakeControllerManager : public moveit_controller_manager::MoveItControllerManager +{ +public: + MoveItFakeControllerManager() : node_handle_("~") + { + if (!node_handle_.hasParam("controller_list")) + { + ROS_ERROR_STREAM_NAMED("MoveItFakeControllerManager", "No controller_list specified."); + return; + } + + XmlRpc::XmlRpcValue controller_list; + node_handle_.getParam("controller_list", controller_list); + if (controller_list.getType() != XmlRpc::XmlRpcValue::TypeArray) + { + ROS_ERROR_NAMED("MoveItFakeControllerManager", "controller_list should be specified as an array"); + return; + } + + /* by setting latch to true we preserve the initial joint state while other nodes launch */ + bool latch = true; + pub_ = node_handle_.advertise("fake_controller_joint_states", 100, latch); + + /* publish initial pose */ + XmlRpc::XmlRpcValue initial; + if (node_handle_.getParam("initial", initial)) + { + sensor_msgs::JointState js = loadInitialJointValues(initial); + js.header.stamp = ros::Time::now(); + pub_.publish(js); + } + + /* actually create each controller */ + for (int i = 0; i < controller_list.size(); ++i) // NOLINT(modernize-loop-convert) + { + if (!controller_list[i].hasMember("name") || !controller_list[i].hasMember("joints")) + { + ROS_ERROR_NAMED("MoveItFakeControllerManager", "Name and joints must be specified for each controller"); + continue; + } + + try + { + const std::string name = std::string(controller_list[i]["name"]); + + if (controller_list[i]["joints"].getType() != XmlRpc::XmlRpcValue::TypeArray) + { + ROS_ERROR_STREAM_NAMED("MoveItFakeControllerManager", + "The list of joints for controller " << name << " is not specified as an array"); + continue; + } + std::vector joints; + joints.reserve(controller_list[i]["joints"].size()); + for (int j = 0; j < controller_list[i]["joints"].size(); ++j) + joints.emplace_back(std::string(controller_list[i]["joints"][j])); + + const std::string& type = + controller_list[i].hasMember("type") ? std::string(controller_list[i]["type"]) : DEFAULT_TYPE; + if (type == "last point") + controllers_[name] = std::make_shared(name, joints, pub_); + else if (type == "via points") + controllers_[name] = std::make_shared(name, joints, pub_); + else if (type == "interpolate") + controllers_[name] = std::make_shared(name, joints, pub_); + else + ROS_ERROR_STREAM("Unknown fake controller type: " << type); + + moveit_controller_manager::MoveItControllerManager::ControllerState state; + state.default_ = controller_list[i].hasMember("default") ? (bool)controller_list[i]["default"] : false; + state.active_ = true; + + controller_states_[name] = state; + } + catch (...) + { + ROS_ERROR_NAMED("MoveItFakeControllerManager", "Caught unknown exception while parsing controller information"); + } + } + } + + sensor_msgs::JointState loadInitialJointValues(XmlRpc::XmlRpcValue& param) const + { + sensor_msgs::JointState js; + + if (param.getType() != XmlRpc::XmlRpcValue::TypeArray || param.size() == 0) + { + ROS_ERROR_ONCE_NAMED("loadInitialJointValues", "Parameter 'initial' should be an array of (group, pose) " + "structs."); + return js; + } + + robot_model_loader::RobotModelLoader robot_model_loader(ROBOT_DESCRIPTION); + const moveit::core::RobotModelPtr& robot_model = robot_model_loader.getModel(); + moveit::core::RobotState robot_state(robot_model); + typedef std::map JointPoseMap; + JointPoseMap joints; + + robot_state.setToDefaultValues(); // initialize all joint values (just in case...) + for (int i = 0, end = param.size(); i != end; ++i) + { + try + { + std::string group_name = std::string(param[i]["group"]); + std::string pose_name = std::string(param[i]["pose"]); + if (!robot_model->hasJointModelGroup(group_name)) + { + ROS_WARN_STREAM_NAMED("loadInitialJointValues", "Unknown joint model group: " << group_name); + continue; + } + moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group_name); + const std::vector& joint_names = jmg->getActiveJointModelNames(); + + if (!robot_state.setToDefaultValues(jmg, pose_name)) + { + ROS_WARN_NAMED("loadInitialJointValues", "Unknown pose '%s' for group '%s'.", pose_name.c_str(), + group_name.c_str()); + continue; + } + ROS_INFO_NAMED("loadInitialJointValues", "Set joints of group '%s' to pose '%s'.", group_name.c_str(), + pose_name.c_str()); + + for (const std::string& joint_name : joint_names) + { + const moveit::core::JointModel* jm = robot_state.getJointModel(joint_name); + if (!jm) + { + ROS_WARN_STREAM_NAMED("loadInitialJointValues", "Unknown joint: " << joint_name); + continue; + } + if (jm->getVariableCount() != 1) + { + ROS_WARN_STREAM_NAMED("loadInitialJointValues", "Cannot handle multi-variable joint: " << joint_name); + continue; + } + + joints[joint_name] = robot_state.getJointPositions(jm)[0]; + } + } + catch (...) + { + ROS_ERROR_ONCE_NAMED("loadInitialJointValues", "Caught unknown exception while reading initial pose " + "information."); + } + } + + // fill the joint state + for (const auto& name_pos_pair : joints) + { + js.name.push_back(name_pos_pair.first); + js.position.push_back(name_pos_pair.second); + } + return js; + } + + ~MoveItFakeControllerManager() override = default; + + /* + * Get a controller, by controller name (which was specified in the controllers.yaml + */ + moveit_controller_manager::MoveItControllerHandlePtr getControllerHandle(const std::string& name) override + { + std::map::const_iterator it = controllers_.find(name); + if (it != controllers_.end()) + return it->second; + else + ROS_FATAL_STREAM("No such controller: " << name); + return moveit_controller_manager::MoveItControllerHandlePtr(); + } + + /* + * Get the list of controller names. + */ + void getControllersList(std::vector& names) override + { + for (std::map::const_iterator it = controllers_.begin(); + it != controllers_.end(); ++it) + names.push_back(it->first); + ROS_INFO_STREAM("Returned " << names.size() << " controllers in list"); + } + + /* + * Fake controllers are always active + */ + void getActiveControllers(std::vector& names) override + { + getControllersList(names); + } + + /* + * Fake controllers are always loaded + */ + virtual void getLoadedControllers(std::vector& names) + { + getControllersList(names); + } + + /* + * Get the list of joints that a controller can control. + */ + void getControllerJoints(const std::string& name, std::vector& joints) override + { + std::map::const_iterator it = controllers_.find(name); + if (it != controllers_.end()) + { + it->second->getJoints(joints); + } + else + { + ROS_WARN("The joints for controller '%s' are not known. Perhaps the controller configuration is not loaded on " + "the param server?", + name.c_str()); + joints.clear(); + } + } + + moveit_controller_manager::MoveItControllerManager::ControllerState + getControllerState(const std::string& name) override + { + return controller_states_[name]; + } + + /* Cannot switch our controllers */ + bool switchControllers(const std::vector& /*activate*/, + const std::vector& /*deactivate*/) override + { + return false; + } + +protected: + ros::NodeHandle node_handle_; + ros::Publisher pub_; + std::map controllers_; + std::map controller_states_; +}; + +} // end namespace moveit_fake_controller_manager + +PLUGINLIB_EXPORT_CLASS(moveit_fake_controller_manager::MoveItFakeControllerManager, + moveit_controller_manager::MoveItControllerManager); diff --git a/moveit_plugins/moveit_plugins/package.xml b/moveit_plugins/moveit_plugins/package.xml index 151b98c422..48f2794ed8 100644 --- a/moveit_plugins/moveit_plugins/package.xml +++ b/moveit_plugins/moveit_plugins/package.xml @@ -1,12 +1,12 @@ + moveit_plugins 2.3.0 Metapackage for MoveIt plugins. - Michael Ferguson - Ioan Sucan - + Henning Kayser + Tyler Weaver Michael Görner MoveIt Release Team @@ -14,6 +14,9 @@ http://moveit.ros.org + Michael Ferguson + Ioan Sucan + ament_cmake moveit_simple_controller_manager diff --git a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt index dd2b7e07e3..4e6f66ea69 100644 --- a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt +++ b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt @@ -76,10 +76,11 @@ EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin +INCLUDES DESTINATION include ) ## Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ +install(DIRECTORY include/ DESTINATION include ) diff --git a/moveit_plugins/moveit_ros_control_interface/package.xml b/moveit_plugins/moveit_ros_control_interface/package.xml index 989f29362d..e751ad6cff 100644 --- a/moveit_plugins/moveit_ros_control_interface/package.xml +++ b/moveit_plugins/moveit_ros_control_interface/package.xml @@ -1,16 +1,17 @@ - + + moveit_ros_control_interface 2.3.0 ros_control controller manager interface for MoveIt - - Mathias Lüdtke - - Mathias Lüdtke + Henning Kayser + Tyler Weaver MoveIt Release Team BSD + Mathias Lüdtke + ament_cmake moveit_common rclcpp_action diff --git a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp index 30195241a3..53ffd22fe2 100644 --- a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp +++ b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp @@ -87,8 +87,12 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll std::string ns_; pluginlib::ClassLoader loader_; typedef std::map ControllersMap; + + /** @brief Controllers that can be activated and deactivated by this plugin. */ ControllersMap managed_controllers_; + /** @brief Controllers that are currently active. */ ControllersMap active_controllers_; + typedef std::map AllocatorsMap; AllocatorsMap allocators_; @@ -96,7 +100,12 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll HandleMap handles_; rclcpp::Time controllers_stamp_{ 0, 0, RCL_ROS_TIME }; + + /** + * @brief Protects access to managed_controllers_, active_controllers_, allocators_, handles_, and controllers_stamp. + */ std::mutex controllers_mutex_; + rclcpp::Node::SharedPtr node_; rclcpp::Client::SharedPtr list_controllers_service_; rclcpp::Client::SharedPtr switch_controller_service_; @@ -141,23 +150,32 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll const auto& result = result_future.get(); for (const controller_manager_msgs::msg::ControllerState& controller : result->controller) { + // If the controller is active, add it to the map of active controllers. if (isActive(controller)) { + // Get the names of the interfaces currently claimed by the active controller. auto& claimed_interfaces = active_controllers_.insert(std::make_pair(controller.name, controller)) .first->second.claimed_interfaces; // without namespace + // Modify the claimed interface names in-place to only include the name of the joint and not the command type + // (e.g. position, velocity, etc.). std::transform(claimed_interfaces.cbegin(), claimed_interfaces.cend(), claimed_interfaces.begin(), [](const std::string& claimed_interface) { return parseJointNameFromResource(claimed_interface); }); } + + // Instantiate a controller handle if one is available for this type of controller. if (loader_.isClassAvailable(controller.type)) { std::string absname = getAbsName(controller.name); auto controller_it = managed_controllers_.insert(std::make_pair(absname, controller)).first; // with namespace - auto& claimed_interfaces = controller_it->second.claimed_interfaces; - std::transform(claimed_interfaces.cbegin(), claimed_interfaces.cend(), claimed_interfaces.begin(), - [](const std::string& claimed_interface) { - return parseJointNameFromResource(claimed_interface); + // Get the names of the interfaces that would be claimed by this currently-inactive controller if it was activated. + auto& required_interfaces = controller_it->second.required_command_interfaces; + // Modify the required interface names in-place to only include the name of the joint and not the command type + // (e.g. position, velocity, etc.). + std::transform(required_interfaces.cbegin(), required_interfaces.cend(), required_interfaces.begin(), + [](const std::string& required_interface) { + return parseJointNameFromResource(required_interface); }); allocate(absname, controller_it->second); } @@ -236,6 +254,9 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll getAbsName("controller_manager/list_controllers")); switch_controller_service_ = node_->create_client( getAbsName("controller_manager/switch_controller")); + + std::scoped_lock lock(controllers_mutex_); + discover(true); } /** * \brief Find and return the pre-allocated handle for the given controller. @@ -244,7 +265,7 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll */ moveit_controller_manager::MoveItControllerHandlePtr getControllerHandle(const std::string& name) override { - std::unique_lock lock(controllers_mutex_); + std::scoped_lock lock(controllers_mutex_); HandleMap::iterator it = handles_.find(name); if (it != handles_.end()) { // controller is is manager by this interface @@ -259,7 +280,7 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll */ void getControllersList(std::vector& names) override { - std::unique_lock lock(controllers_mutex_); + std::scoped_lock lock(controllers_mutex_); discover(); for (std::pair& managed_controller : @@ -275,7 +296,7 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll */ void getActiveControllers(std::vector& names) override { - std::unique_lock lock(controllers_mutex_); + std::scoped_lock lock(controllers_mutex_); discover(); for (std::pair& managed_controller : @@ -287,19 +308,19 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll } /** - * \brief Read resources from cached controller states + * \brief Read interface names required by each controller from the cached controller state info. * @param[in] name name of controller (with namespace) * @param[out] joints */ void getControllerJoints(const std::string& name, std::vector& joints) override { - std::unique_lock lock(controllers_mutex_); + std::scoped_lock lock(controllers_mutex_); ControllersMap::iterator it = managed_controllers_.find(name); if (it != managed_controllers_.end()) { - for (const auto& claimed_resource : it->second.claimed_interfaces) + for (const auto& required_resource : it->second.required_command_interfaces) { - joints.push_back(claimed_resource); + joints.push_back(required_resource); } } } @@ -311,7 +332,7 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll */ ControllerState getControllerState(const std::string& name) override { - std::unique_lock lock(controllers_mutex_); + std::scoped_lock lock(controllers_mutex_); discover(); ControllerState c; @@ -332,7 +353,7 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll */ bool switchControllers(const std::vector& activate, const std::vector& deactivate) override { - std::unique_lock lock(controllers_mutex_); + std::scoped_lock lock(controllers_mutex_); discover(true); typedef boost::bimap resources_bimap; @@ -360,15 +381,17 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll } } + // For each controller to activate, find conflicts between the interfaces required by that controller and the + // interfaces claimed by currently-active controllers. for (const std::string& it : activate) { ControllersMap::iterator c = managed_controllers_.find(it); if (c != managed_controllers_.end()) { // controller belongs to this manager request->start_controllers.push_back(c->second.name); - for (const auto& claimed_resource : c->second.claimed_interfaces) + for (const auto& required_resource : c->second.required_command_interfaces) { - resources_bimap::right_const_iterator res = claimed_resources.right.find(claimed_resource); + resources_bimap::right_const_iterator res = claimed_resources.right.find(required_resource); if (res != claimed_resources.right.end()) { // resource is claimed request->stop_controllers.push_back(res->second); // add claiming controller to stop list @@ -377,6 +400,9 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll } } } + + // Setting level to STRICT means that the controller switch will only be committed if all controllers are + // successfully activated or deactivated. request->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; if (!request->start_controllers.empty() || !request->stop_controllers.empty()) diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h index 541d3aa03c..1187a5c38d 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h @@ -70,8 +70,8 @@ class ActionBasedControllerHandleBase : public moveit_controller_manager::MoveIt MOVEIT_CLASS_FORWARD( ActionBasedControllerHandleBase); // Defines ActionBasedControllerHandleBasePtr, ConstPtr, WeakPtr... etc -/* - * This is a simple base class, which handles all of the action creation/etc +/** + * @brief Base class for controller handles that interact with a controller through a ROS action server. */ template class ActionBasedControllerHandle : public ActionBasedControllerHandleBase @@ -79,44 +79,18 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase public: ActionBasedControllerHandle(const rclcpp::Node::SharedPtr& node, const std::string& name, const std::string& ns, const std::string& logger_name) - : ActionBasedControllerHandleBase(name, logger_name), node_(node), done_(true), namespace_(ns) + : ActionBasedControllerHandleBase(name, logger_name), done_(true), namespace_(ns) { - controller_action_client_ = rclcpp_action::create_client(node_, getActionName()); - - unsigned int attempts = 0; - double timeout; - node_->get_parameter_or("trajectory_execution.controller_connection_timeout", timeout, 15.0); - - if (timeout == 0.0) - { - while (rclcpp::ok() && !controller_action_client_->wait_for_action_server(std::chrono::seconds(5))) - { - RCLCPP_WARN_STREAM(LOGGER, "Waiting for " << getActionName() << " to come up"); - } - } - else - { - while (rclcpp::ok() && - !controller_action_client_->wait_for_action_server(std::chrono::duration(timeout / 3)) && - ++attempts < 3) - { - RCLCPP_WARN_STREAM(LOGGER, "Waiting for " << getActionName() << " to come up"); - } - } - if (!controller_action_client_->action_server_is_ready()) - { - RCLCPP_ERROR_STREAM(LOGGER, "Action client not connected: " << getActionName()); - controller_action_client_.reset(); - } - + // Creating the action client does not ensure that the action server is actually running. Executing trajectories + // through the controller handle will fail if the server is not running when an action goal message is sent. + controller_action_client_ = rclcpp_action::create_client(node, getActionName()); last_exec_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED; } - bool isConnected() const - { - return static_cast(controller_action_client_); - } - + /** + * @brief Cancels trajectory execution by triggering the controller action server's cancellation callback. + * @return True if cancellation was accepted, false if cancellation failed. + */ bool cancelExecution() override { if (!controller_action_client_) @@ -136,9 +110,18 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase return true; } + /** + * @brief Callback function to call when the result is received from the controller action server. + * @param wrapped_result + */ virtual void controllerDoneCallback(const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) = 0; + /** + * @brief Blocks waiting for the action result to be received. + * @param timeout Duration to wait for a result before failing. Default value indicates no timeout. + * @return True if a result was received, false on timeout. + */ bool waitForExecution(const rclcpp::Duration& timeout = rclcpp::Duration::from_seconds(-1.0)) override { auto result_callback_done = std::make_shared>(); @@ -181,7 +164,19 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase } protected: - const rclcpp::Node::SharedPtr node_; + /** + * @brief Check if the controller's action server is ready to receive action goals. + * @return True if the action server is ready, false if it is not ready or does not exist. + */ + bool isConnected() const + { + return controller_action_client_->action_server_is_ready(); + } + + /** + * @brief Get the full name of the action using the action namespace and base name. + * @return The action name. + */ std::string getActionName() const { if (namespace_.empty()) @@ -190,6 +185,11 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase return name_ + "/" + namespace_; } + /** + * @brief Indicate that the controller handle is done executing the trajectory and set the controller manager handle's + * ExecutionStatus based on the received action ResultCode. + * @param rclcpp_action::ResultCode to convert to moveit_controller_manager::ExecutionStatus. + */ void finishControllerExecution(const rclcpp_action::ResultCode& state) { RCLCPP_DEBUG_STREAM(LOGGER, "Controller " << name_ << " is done with state " << static_cast(state)); @@ -206,21 +206,35 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase done_ = true; } - /* execution status */ + /** + * @brief Status after last trajectory execution. + */ moveit_controller_manager::ExecutionStatus last_exec_; + + /** + * @brief Indicates whether the controller handle is done executing its current trajectory. + */ bool done_; - /* the controller namespace, for instance, topics will map to name/ns/goal, - * name/ns/result, etc */ + /** + * @brief The controller namespace. The controller action server's topics will map to name/ns/goal, name/ns/result, etc. + */ std::string namespace_; - /* the joints controlled by this controller */ + /** + * @brief The joints controlled by this controller. + */ std::vector joints_; - /* action client */ + /** + * @brief Action client to send trajectories to the controller's action server. + */ typename rclcpp_action::Client::SharedPtr controller_action_client_; - /* Current goal that have been sent to the action server */ + + /** + * @brief Current goal that has been sent to the action server. + */ typename rclcpp_action::ClientGoalHandle::SharedPtr current_goal_; }; -} // end namespace moveit_simple_controller_manager +} // namespace moveit_simple_controller_manager diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h index 98d5849b53..81741d4484 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h @@ -66,6 +66,12 @@ class GripperControllerHandle : public ActionBasedControllerHandle + + + moveit_simple_controller_manager 2.3.0 A generic, simple controller manager plugin for MoveIt. - - Michael Ferguson - - Michael Ferguson Michael Görner + Henning Kayser + Tyler Weaver MoveIt Release Team BSD @@ -15,6 +15,8 @@ https://github.com/ros-planning/moveit2/issues https://github.com/ros-planning/moveit2 + Michael Ferguson + ament_cmake moveit_common diff --git a/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp b/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp index cd59042bf1..9c0b299402 100644 --- a/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp +++ b/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp @@ -48,6 +48,12 @@ bool FollowJointTrajectoryControllerHandle::sendTrajectory(const moveit_msgs::ms if (!controller_action_client_) return false; + if (!isConnected()) + { + RCLCPP_ERROR_STREAM(LOGGER, "Action client not connected to action server: " << getActionName()); + return false; + } + if (done_) RCLCPP_INFO_STREAM(LOGGER, "sending trajectory to " << name_); else @@ -216,13 +222,13 @@ void FollowJointTrajectoryControllerHandle::controllerDoneCallback( { // Output custom error message for FollowJointTrajectoryResult if necessary if (!wrapped_result.result) - RCLCPP_WARN_STREAM(LOGGER, "Controller " << name_ << " done, no result returned"); + RCLCPP_WARN_STREAM(LOGGER, "Controller '" << name_ << "' done, no result returned"); else if (wrapped_result.result->error_code == control_msgs::action::FollowJointTrajectory::Result::SUCCESSFUL) - RCLCPP_INFO_STREAM(LOGGER, "Controller " << name_ << " successfully finished"); + RCLCPP_INFO_STREAM(LOGGER, "Controller '" << name_ << "' successfully finished"); else - RCLCPP_WARN_STREAM(LOGGER, "Controller " << name_ << " failed with error " - << errorCodeToMessage(wrapped_result.result->error_code) << ": " - << wrapped_result.result->error_string); + RCLCPP_WARN_STREAM(LOGGER, "Controller '" << name_ << "' failed with error " + << errorCodeToMessage(wrapped_result.result->error_code) << ": " + << wrapped_result.result->error_string); finishControllerExecution(wrapped_result.code); } diff --git a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp index acee0040ef..6a6575b6e0 100644 --- a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp +++ b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp @@ -44,6 +44,38 @@ #include #include +namespace +{ +/** + * @brief Create a string by adding a provided separator character between each of a variable number of input arguments. + * @param separator Char to use as the separator. + * @param content Variable number of input arguments to concatentate. + * @return Concatenated result string. + */ +template +std::string concatenateWithSeparator(char separator, T... content) +{ + std::string result; + (result.append(content).append({ separator }), ...); + result.erase(result.end() - 1); // delete trailing separator + return result; +} + +/** + * @brief Compose a parameter name used to load controller configuration from a ROS parameter by concatenating strings + * separated by the `.` character. + * @param strings One or more strings, each corresponding to a part of the full parameter name. + * @return std::string concatenating the input parameters separated by the `.` character. For example: + * base_namespace.controller_name.param_name + */ +template +std::string makeParameterName(T... strings) +{ + return concatenateWithSeparator('.', strings...); +} + +} // namespace + namespace moveit_simple_controller_manager { static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.plugins.moveit_simple_controller_manager"); @@ -58,13 +90,13 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo void initialize(const rclcpp::Node::SharedPtr& node) override { node_ = node; - if (!node_->has_parameter(PARAM_BASE_NAME + ".controller_names")) + if (!node_->has_parameter(makeParameterName(PARAM_BASE_NAME, "controller_names"))) { RCLCPP_ERROR_STREAM(LOGGER, "No controller_names specified."); return; } rclcpp::Parameter controller_names_param; - node_->get_parameter(PARAM_BASE_NAME + ".controller_names", controller_names_param); + node_->get_parameter(makeParameterName(PARAM_BASE_NAME, "controller_names"), controller_names_param); if (controller_names_param.get_type() != rclcpp::ParameterType::PARAMETER_STRING_ARRAY) { RCLCPP_ERROR(LOGGER, "Parameter controller_names should be specified as a string array"); @@ -77,22 +109,23 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo try { std::string action_ns; - if (!node_->get_parameter(PARAM_BASE_NAME + "." + controller_name + ".action_ns", action_ns)) + const std::string& action_ns_param = makeParameterName(PARAM_BASE_NAME, controller_name, "action_ns"); + if (!node_->get_parameter(action_ns_param, action_ns)) { - RCLCPP_ERROR_STREAM(LOGGER, "No action namespace specified for controller " << controller_name); - RCLCPP_ERROR_STREAM(LOGGER, "Parameter: " << (PARAM_BASE_NAME + "." + controller_name + ".action")); + RCLCPP_ERROR_STREAM(LOGGER, "No action namespace specified for controller `" + << controller_name << "` through parameter `" << action_ns_param << "`"); continue; } std::string type; - if (!node_->get_parameter(PARAM_BASE_NAME + "." + controller_name + ".type", type)) + if (!node_->get_parameter(makeParameterName(PARAM_BASE_NAME, controller_name, "type"), type)) { RCLCPP_ERROR_STREAM(LOGGER, "No type specified for controller " << controller_name); continue; } std::vector controller_joints; - if (!node_->get_parameter(PARAM_BASE_NAME + "." + controller_name + ".joints", controller_joints) || + if (!node_->get_parameter(makeParameterName(PARAM_BASE_NAME, controller_name, "joints"), controller_joints) || controller_joints.empty()) { RCLCPP_ERROR_STREAM(LOGGER, "No joints specified for controller " << controller_name); @@ -103,46 +136,39 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo if (type == "GripperCommand") { new_handle = std::make_shared(node_, controller_name, action_ns); - if (static_cast(new_handle.get())->isConnected()) + bool parallel_gripper = false; + if (node_->get_parameter(makeParameterName(PARAM_BASE_NAME, "parallel"), parallel_gripper) && parallel_gripper) { - bool parallel_gripper = false; - if (node_->get_parameter(PARAM_BASE_NAME + ".parallel", parallel_gripper) && parallel_gripper) + if (controller_joints.size() != 2) { - if (controller_joints.size() != 2) - { - RCLCPP_ERROR_STREAM(LOGGER, "Parallel Gripper requires exactly two joints, " << controller_joints.size() - << " are specified"); - continue; - } - static_cast(new_handle.get()) - ->setParallelJawGripper(controller_joints[0], controller_joints[1]); + RCLCPP_ERROR_STREAM(LOGGER, "Parallel Gripper requires exactly two joints, " << controller_joints.size() + << " are specified"); + continue; } - else - { - std::string command_joint; - if (!node_->get_parameter(PARAM_BASE_NAME + ".command_joint", command_joint)) - command_joint = controller_joints[0]; + static_cast(new_handle.get()) + ->setParallelJawGripper(controller_joints[0], controller_joints[1]); + } + else + { + std::string command_joint; + if (!node_->get_parameter(makeParameterName(PARAM_BASE_NAME, "command_joint"), command_joint)) + command_joint = controller_joints[0]; - static_cast(new_handle.get())->setCommandJoint(command_joint); - } + static_cast(new_handle.get())->setCommandJoint(command_joint); + } - bool allow_failure; - node_->get_parameter_or(PARAM_BASE_NAME + ".allow_failure", allow_failure, false); - static_cast(new_handle.get())->allowFailure(allow_failure); + bool allow_failure; + node_->get_parameter_or(makeParameterName(PARAM_BASE_NAME, "allow_failure"), allow_failure, false); + static_cast(new_handle.get())->allowFailure(allow_failure); - RCLCPP_INFO_STREAM(LOGGER, "Added GripperCommand controller for " << controller_name); - controllers_[controller_name] = new_handle; - } + RCLCPP_INFO_STREAM(LOGGER, "Added GripperCommand controller for " << controller_name); + controllers_[controller_name] = new_handle; } else if (type == "FollowJointTrajectory") { - auto h = new FollowJointTrajectoryControllerHandle(node_, controller_name, action_ns); - new_handle.reset(h); - if (h->isConnected()) - { - RCLCPP_INFO_STREAM(LOGGER, "Added FollowJointTrajectory controller for " << controller_name); - controllers_[controller_name] = new_handle; - } + new_handle = std::make_shared(node_, controller_name, action_ns); + RCLCPP_INFO_STREAM(LOGGER, "Added FollowJointTrajectory controller for " << controller_name); + controllers_[controller_name] = new_handle; } else { @@ -156,7 +182,7 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo } moveit_controller_manager::MoveItControllerManager::ControllerState state; - node_->get_parameter_or(PARAM_BASE_NAME + "." + controller_name + ".default", state.default_, false); + node_->get_parameter_or(makeParameterName(PARAM_BASE_NAME, controller_name, "default"), state.default_, false); state.active_ = true; controller_states_[controller_name] = state; diff --git a/moveit_ros/README.md b/moveit_ros/README.md index 5755639551..ca7caf0a67 100644 --- a/moveit_ros/README.md +++ b/moveit_ros/README.md @@ -1,4 +1,4 @@ -#MoveIt ROS +# MoveIt ROS This repository includes components of MoveIt that use ROS. This is where much of the functionality MoveIt provides is put together. Libraries from MoveIt Core and various plugins are used to provide that functionality. - planning diff --git a/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp b/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp index f0fbe945c4..1cbebf3769 100644 --- a/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp +++ b/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp @@ -102,8 +102,8 @@ moveit_benchmarks::BenchmarkExecution::BenchmarkExecution(const planning_scene:: // load the pluginlib class loader try { - planner_plugin_loader_.reset(new pluginlib::ClassLoader( - "moveit_core", "planning_interface::PlannerManager")); + planner_plugin_loader_ = std::make_shared>( + "moveit_core", "planning_interface::PlannerManager"); } catch (pluginlib::PluginlibException& ex) { @@ -278,7 +278,7 @@ void moveit_benchmarks::BenchmarkExecution::runAllBenchmarks(BenchmarkType type) if (got_robot_state) { - start_state_to_use.reset(new moveit_msgs::msg::RobotState(*robot_state)); + start_state_to_use = std::make_sharedname = val; bpo->runs = default_run_count; } diff --git a/moveit_ros/benchmarks/package.xml b/moveit_ros/benchmarks/package.xml index 9ff5953441..52f162aec6 100644 --- a/moveit_ros/benchmarks/package.xml +++ b/moveit_ros/benchmarks/package.xml @@ -1,19 +1,20 @@ - + + + moveit_ros_benchmarks 2.3.0 - Enhanced tools for benchmarks in MoveIt - - Ryan Luna - - Dave Coleman + Henning Kayser + Tyler Weaver MoveIt Release Team + BSD + http://moveit.ros.org https://github.com/ros-planning/moveit2/issues https://github.com/ros-planning/moveit2 - BSD + Ryan Luna ament_cmake moveit_common diff --git a/moveit_ros/benchmarks/src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp b/moveit_ros/benchmarks/src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp index 940bdffd8d..dfc904bebe 100644 --- a/moveit_ros/benchmarks/src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp +++ b/moveit_ros/benchmarks/src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp @@ -63,7 +63,7 @@ class CombinePredefinedPosesBenchmark : public BenchmarkExecutor { // Load planning scene if (!psm_) - psm_.reset(new planning_scene_monitor::PlanningSceneMonitor(node_, "robot_description")); + psm_ = std::make_shared(node_, "robot_description"); if (!psm_->newPlanningSceneMessage(scene_msg)) { RCLCPP_ERROR(LOGGER, "Failed to load planning scene"); diff --git a/moveit_ros/hybrid_planning/CMakeLists.txt b/moveit_ros/hybrid_planning/CMakeLists.txt new file mode 100644 index 0000000000..26ee4035d8 --- /dev/null +++ b/moveit_ros/hybrid_planning/CMakeLists.txt @@ -0,0 +1,121 @@ +cmake_minimum_required(VERSION 3.10.2) +project(moveit_hybrid_planning) + +# Common cmake code applied to all moveit packages +find_package(moveit_common REQUIRED) +moveit_package() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(moveit_core REQUIRED) +find_package(moveit_msgs REQUIRED) +find_package(moveit_ros_planning REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(trajectory_msgs REQUIRED) + +set(LIBRARIES + # Components + moveit_global_planner_component + moveit_hybrid_planning_manager + moveit_local_planner_component + # Plugins + forward_trajectory_plugin + motion_planning_pipeline_plugin + replan_invalidated_trajectory_plugin + simple_sampler_plugin + single_plan_execution_plugin +) + +set(THIS_PACKAGE_INCLUDE_DEPENDS + rclcpp + rclcpp_components + rclcpp_action + moveit_core + moveit_ros_planning + moveit_ros_planning_interface + moveit_msgs + pluginlib + std_msgs + std_srvs + tf2_ros + trajectory_msgs +) + +set(THIS_PACKAGE_INCLUDE_DIRS + global_planner/global_planner_component/include/ + global_planner/global_planner_plugins/include/ + hybrid_planning_manager/hybrid_planning_manager_component/include/ + hybrid_planning_manager/planner_logic_plugins/include/ + local_planner/local_planner_component/include/ + local_planner/trajectory_operator_plugins/include/ + local_planner/local_constraint_solver_plugins/include/ +) +include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) + +add_subdirectory(hybrid_planning_manager) +add_subdirectory(global_planner) +add_subdirectory(local_planner) +add_subdirectory(test) + +rclcpp_components_register_nodes(moveit_hybrid_planning_manager "moveit::hybrid_planning::HybridPlanningManager") +rclcpp_components_register_nodes(moveit_global_planner_component "moveit::hybrid_planning::GlobalPlannerComponent") +rclcpp_components_register_nodes(moveit_local_planner_component "moveit::hybrid_planning::LocalPlannerComponent") + +install(TARGETS ${LIBRARIES} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} + INCLUDES DESTINATION include) + +install(TARGETS hybrid_planning_demo_node + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} + INCLUDES DESTINATION include) + +install(DIRECTORY ${THIS_PACKAGE_INCLUDE_DIRS} DESTINATION include) + +install(DIRECTORY test/launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}) + +pluginlib_export_plugin_description_file(moveit_hybrid_planning single_plan_execution_plugin.xml) +pluginlib_export_plugin_description_file(moveit_hybrid_planning moveit_planning_pipeline_plugin.xml) +pluginlib_export_plugin_description_file(moveit_hybrid_planning simple_sampler_plugin.xml) +pluginlib_export_plugin_description_file(moveit_hybrid_planning replan_invalidated_trajectory_plugin.xml) +pluginlib_export_plugin_description_file(moveit_hybrid_planning forward_trajectory_plugin.xml) + +ament_export_include_directories(include) +ament_export_libraries(${LIBRARIES}) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gtest REQUIRED) + find_package(ros_testing REQUIRED) + find_package(Boost REQUIRED COMPONENTS filesystem) + + # 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_flake8_FOUND TRUE) + set(ament_cmake_uncrustify_FOUND TRUE) + + # Run all lint tests in package.xml except those listed above + ament_lint_auto_find_test_dependencies() + + # Basic launch test + add_ros_test(test/launch/test_basic_launching.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + +endif() + +ament_package() diff --git a/moveit_ros/hybrid_planning/README.md b/moveit_ros/hybrid_planning/README.md new file mode 100644 index 0000000000..defb1475c7 --- /dev/null +++ b/moveit_ros/hybrid_planning/README.md @@ -0,0 +1,24 @@ +# Hybrid Planning +A Hybrid Planning architecture. You can find more information in the project's issues[#300](https://github.com/ros-planning/moveit2/issues/300), [#433](https://github.com/ros-planning/moveit2/issues/433) and on the [MoveIt 2 roadmap](https://moveit.ros.org/documentation/contributing/roadmap/). Furthermore, there is an extensive tutorial available [here](https://github.com/ros-planning/moveit2_tutorials/pull/97). + +## Getting started +To start the demo run: +``` +ros2 launch moveit_hybrid_planning hybrid_planning_demo.launch.py +``` +You can exchange the planner logic plugin in the hybrid_planning_manager.yaml. Currently available demo plugins are: +- planner logic plugins: + - replan_invalidated_trajectory: Runs the global planner once and starts executing the global solution + with the local planner. In case the local planner detects a collision the global planner is rerun to update the + invalidated global trajectory. + - single_plan_execution: Run the global planner once and starts executing the global solution + with the local planner +- global_planner plugins: + - moveit_planning_pipeline: Global planner plugin that utilizes MoveIt's planning pipeline accessed via the MoveItCpp API. +- trajectory operator plugins: + - simple_sampler: Samples the next global trajectory waypoint as local goal constraint + based on the current robot state. When the waypoint is reached the index that marks the current local goal constraint + is updated to the next global trajectory waypoint. Global trajectory updates simply replace the reference trajectory. +- local solver plugins: + - forward_trajectory: Forwards the next waypoint of the sampled local trajectory. + Additionally, it is possible to enable collision checking, which lets the robot stop in front of a collision object. diff --git a/moveit_ros/hybrid_planning/forward_trajectory_plugin.xml b/moveit_ros/hybrid_planning/forward_trajectory_plugin.xml new file mode 100644 index 0000000000..3bc40fe17d --- /dev/null +++ b/moveit_ros/hybrid_planning/forward_trajectory_plugin.xml @@ -0,0 +1,7 @@ + + + + Simple local solver plugin that forwards the next waypoint of the sampled local trajectory. + + + diff --git a/moveit_ros/hybrid_planning/global_planner/CMakeLists.txt b/moveit_ros/hybrid_planning/global_planner/CMakeLists.txt new file mode 100644 index 0000000000..be7649e42a --- /dev/null +++ b/moveit_ros/hybrid_planning/global_planner/CMakeLists.txt @@ -0,0 +1,2 @@ +add_subdirectory(global_planner_component) +add_subdirectory(global_planner_plugins) diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_component/CMakeLists.txt b/moveit_ros/hybrid_planning/global_planner/global_planner_component/CMakeLists.txt new file mode 100644 index 0000000000..e39676fd67 --- /dev/null +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_component/CMakeLists.txt @@ -0,0 +1,6 @@ +# Global planner component +add_library(moveit_global_planner_component SHARED + src/global_planner_component.cpp +) +set_target_properties(moveit_global_planner_component PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(moveit_global_planner_component ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_component.h b/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_component.h new file mode 100644 index 0000000000..ac86ee1a93 --- /dev/null +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_component.h @@ -0,0 +1,93 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Sebastian Jahr + Description: A global planner component node that is customizable through plugins. + */ + +#pragma once + +#include +#include + +#include + +#include + +#include +#include +#include + +namespace moveit::hybrid_planning +{ +// Component node containing the global planner +class GlobalPlannerComponent +{ +public: + GlobalPlannerComponent(const rclcpp::NodeOptions& options); + + // This function is required to make this class a valid NodeClass + // see https://docs.ros2.org/foxy/api/rclcpp_components/register__node__macro_8hpp.html + // Skip linting due to unconventional function naming + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() // NOLINT + { + return node_->get_node_base_interface(); // NOLINT + } + +private: + std::shared_ptr node_; + + std::string planner_plugin_name_; + + // Global planner plugin loader + std::unique_ptr> global_planner_plugin_loader_; + + // Global planner instance + std::shared_ptr global_planner_instance_; + + // Global planning request action server + rclcpp_action::Server::SharedPtr global_planning_request_server_; + + // Global trajectory publisher + rclcpp::Publisher::SharedPtr global_trajectory_pub_; + + // Goal callback for global planning request action server + void globalPlanningRequestCallback( + std::shared_ptr> goal_handle); + + // Initialize planning scene monitor and load pipelines + bool initializeGlobalPlanner(); +}; + +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_interface.h b/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_interface.h new file mode 100644 index 0000000000..8cef1260c1 --- /dev/null +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_interface.h @@ -0,0 +1,78 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Sebastian Jahr + Description: Defines an interface for a global motion planner plugin implementation used in the global planner + component node. + */ + +#pragma once + +#include +#include + +#include + +#include +#include + +namespace moveit::hybrid_planning +{ +/** + * Class GlobalPlannerInterface - Base class for a global planner implementation + */ +class GlobalPlannerInterface +{ +public: + /** + * Initialize global planner + * @return True if initialization was successful + */ + virtual bool initialize(const rclcpp::Node::SharedPtr& node) = 0; + + /** + * Solve global planning problem + * @param global_goal_handle Action goal handle to access the planning goal and publish feedback during planning + * @return Motion Plan that contains the solution for a given motion planning problem + */ + virtual moveit_msgs::msg::MotionPlanResponse plan( + const std::shared_ptr> global_goal_handle) = 0; + + /** + * Reset global planner plugin. This should never fail. + * @return True if reset was successful + */ + virtual bool reset() noexcept = 0; +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp b/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp new file mode 100644 index 0000000000..e4e7ae6826 --- /dev/null +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp @@ -0,0 +1,145 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include +#include +#include + +#include +#include + +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("global_planner_component"); +} + +namespace moveit::hybrid_planning +{ +using namespace std::chrono_literals; +const std::string UNDEFINED = ""; + +GlobalPlannerComponent::GlobalPlannerComponent(const rclcpp::NodeOptions& options) + : node_{ std::make_shared("global_planner_component", options) } +{ + if (!initializeGlobalPlanner()) + { + throw std::runtime_error("Failed to initialize Global Planner Component"); + } +} + +bool GlobalPlannerComponent::initializeGlobalPlanner() +{ + // Initialize global planning request action server + global_planning_request_server_ = rclcpp_action::create_server( + node_, "global_planning_action", + [](const rclcpp_action::GoalUUID& /*unused*/, + std::shared_ptr /*unused*/) { + RCLCPP_INFO(LOGGER, "Received global planning goal request"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }, + [this](const std::shared_ptr>& /*unused*/) { + RCLCPP_INFO(LOGGER, "Received request to cancel global planning goal"); + if (!global_planner_instance_->reset()) + { + throw std::runtime_error("Failed to reset the global planner while aborting current global planning"); + } + return rclcpp_action::CancelResponse::ACCEPT; + }, + std::bind(&GlobalPlannerComponent::globalPlanningRequestCallback, this, std::placeholders::_1)); + + global_trajectory_pub_ = node_->create_publisher("global_trajectory", 1); + + // Load global planner plugin + planner_plugin_name_ = node_->declare_parameter("global_planner_name", UNDEFINED); + + try + { + global_planner_plugin_loader_ = std::make_unique>( + "moveit_hybrid_planning", "moveit::hybrid_planning::GlobalPlannerInterface"); + } + catch (pluginlib::PluginlibException& ex) + { + RCLCPP_ERROR(LOGGER, "Exception while creating global planner plugin loader: '%s'", ex.what()); + return false; + } + try + { + global_planner_instance_ = global_planner_plugin_loader_->createUniqueInstance(planner_plugin_name_); + } + catch (pluginlib::PluginlibException& ex) + { + RCLCPP_ERROR(LOGGER, "Exception while loading global planner '%s': '%s'", planner_plugin_name_.c_str(), ex.what()); + return false; + } + + // Initialize global planner plugin + if (!global_planner_instance_->initialize(node_)) + { + RCLCPP_ERROR(LOGGER, "Unable to initialize global planner plugin '%s'", planner_plugin_name_.c_str()); + return false; + } + RCLCPP_INFO(LOGGER, "Using global planner plugin '%s'", planner_plugin_name_.c_str()); + return true; +} + +void GlobalPlannerComponent::globalPlanningRequestCallback( + std::shared_ptr> goal_handle) +{ + // Plan global trajectory + moveit_msgs::msg::MotionPlanResponse planning_solution = global_planner_instance_->plan(goal_handle); + + // Send action response + auto result = std::make_shared(); + result->response = planning_solution; + + if (planning_solution.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + // Publish global planning solution to the local planner + global_trajectory_pub_->publish(planning_solution); + goal_handle->succeed(result); + } + else + { + goal_handle->abort(result); + } + + // Reset the global planner + global_planner_instance_->reset(); +}; +} // namespace moveit::hybrid_planning + +// Register the component with class_loader +#include +RCLCPP_COMPONENTS_REGISTER_NODE(moveit::hybrid_planning::GlobalPlannerComponent) diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/CMakeLists.txt b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/CMakeLists.txt new file mode 100644 index 0000000000..05fba889c9 --- /dev/null +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/CMakeLists.txt @@ -0,0 +1,5 @@ +add_library(motion_planning_pipeline_plugin SHARED + src/moveit_planning_pipeline.cpp +) +set_target_properties(motion_planning_pipeline_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(motion_planning_pipeline_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/include/moveit/global_planner/moveit_planning_pipeline.h b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/include/moveit/global_planner/moveit_planning_pipeline.h new file mode 100644 index 0000000000..0db65f68b2 --- /dev/null +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/include/moveit/global_planner/moveit_planning_pipeline.h @@ -0,0 +1,64 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Sebastian Jahr + Description: Global planner plugin that utilizes MoveIt's planning pipeline accessed via the MoveItCpp API*/ + +#pragma once + +#include +#include + +// MoveitCpp +#include +#include + +namespace moveit::hybrid_planning +{ +class MoveItPlanningPipeline : public GlobalPlannerInterface +{ +public: + MoveItPlanningPipeline() = default; + ~MoveItPlanningPipeline() = default; + bool initialize(const rclcpp::Node::SharedPtr& node) override; + bool reset() noexcept override; + moveit_msgs::msg::MotionPlanResponse + plan(const std::shared_ptr> global_goal_handle) + override; + +private: + rclcpp::Node::SharedPtr node_ptr_; + std::shared_ptr moveit_cpp_; +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp new file mode 100644 index 0000000000..7f0f9e7d0d --- /dev/null +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp @@ -0,0 +1,144 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include +#include + +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("global_planner_component"); +} + +namespace moveit::hybrid_planning +{ +const std::string PLANNING_SCENE_MONITOR_NS = "planning_scene_monitor_options."; +const std::string PLANNING_PIPELINES_NS = "planning_pipelines."; +const std::string PLAN_REQUEST_PARAM_NS = "plan_request_params."; +const std::string UNDEFINED = ""; + +bool MoveItPlanningPipeline::initialize(const rclcpp::Node::SharedPtr& node) +{ + // TODO(andyz): how to standardize this for planning pipelines other than ompl? + // Maybe use loadPlanningPipelines() from moveit_cpp.cpp + + // Declare planning pipeline paramter + node->declare_parameter>(PLANNING_PIPELINES_NS + "pipeline_names", + std::vector({ UNDEFINED })); + node->declare_parameter(PLANNING_PIPELINES_NS + "namespace", UNDEFINED); + + // Default PlanRequestParameters. These can be overriden when plan() is called + node->declare_parameter(PLAN_REQUEST_PARAM_NS + "planner_id", UNDEFINED); + node->declare_parameter(PLAN_REQUEST_PARAM_NS + "planning_pipeline", UNDEFINED); + node->declare_parameter(PLAN_REQUEST_PARAM_NS + "planning_attempts", 5); + node->declare_parameter(PLAN_REQUEST_PARAM_NS + "planning_time", 1.0); + node->declare_parameter(PLAN_REQUEST_PARAM_NS + "max_velocity_scaling_factor", 1.0); + node->declare_parameter(PLAN_REQUEST_PARAM_NS + "max_acceleration_scaling_factor", 1.0); + node->declare_parameter("ompl.planning_plugin", "ompl_interface/OMPLPlanner"); + + // Trajectory Execution Functionality (required by the MoveItPlanningPipeline but not used within hybrid planning) + node->declare_parameter("moveit_controller_manager", UNDEFINED); + + node_ptr_ = node; + + // Initialize MoveItCpp API + moveit_cpp::MoveItCpp::Options moveit_cpp_options(node); + moveit_cpp_ = std::make_shared(node, moveit_cpp_options); + + return true; +} + +bool MoveItPlanningPipeline::reset() noexcept +{ + // Do Nothing + return true; +} + +moveit_msgs::msg::MotionPlanResponse MoveItPlanningPipeline::plan( + const std::shared_ptr> global_goal_handle) +{ + moveit_msgs::msg::MotionPlanResponse response; + + if ((global_goal_handle->get_goal())->motion_sequence.items.empty()) + { + RCLCPP_WARN(LOGGER, "Global planner received motion sequence request with no items. At least one is needed."); + response.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; + return response; + } + + // Process goal + if ((global_goal_handle->get_goal())->motion_sequence.items.size() > 1) + { + RCLCPP_WARN(LOGGER, "Global planner received motion sequence request with more than one item but the " + "'moveit_planning_pipeline' plugin only accepts one item. Just using the first item as global " + "planning goal!"); + } + auto motion_plan_req = (global_goal_handle->get_goal())->motion_sequence.items[0].req; + + // Set parameters required by the planning component + moveit_cpp::PlanningComponent::PlanRequestParameters plan_params; + plan_params.planner_id = motion_plan_req.planner_id; + plan_params.planning_pipeline = motion_plan_req.pipeline_id; + plan_params.planning_attempts = motion_plan_req.num_planning_attempts; + plan_params.planning_time = motion_plan_req.allowed_planning_time; + plan_params.max_velocity_scaling_factor = motion_plan_req.max_velocity_scaling_factor; + plan_params.max_acceleration_scaling_factor = motion_plan_req.max_acceleration_scaling_factor; + + // Create planning component + auto planning_components = std::make_shared(motion_plan_req.group_name, moveit_cpp_); + + // Copy goal constraint into planning component + planning_components->setGoal(motion_plan_req.goal_constraints); + + // Plan motion + auto plan_solution = planning_components->plan(plan_params); + if (plan_solution.error_code != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + response.error_code = plan_solution.error_code; + return response; + } + + // Transform solution into MotionPlanResponse and publish it + response.trajectory_start = plan_solution.start_state; + response.group_name = motion_plan_req.group_name; + plan_solution.trajectory->getRobotTrajectoryMsg(response.trajectory); + response.error_code = plan_solution.error_code; + + return response; +} +} // namespace moveit::hybrid_planning + +#include + +PLUGINLIB_EXPORT_CLASS(moveit::hybrid_planning::MoveItPlanningPipeline, moveit::hybrid_planning::GlobalPlannerInterface); diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/CMakeLists.txt b/moveit_ros/hybrid_planning/hybrid_planning_manager/CMakeLists.txt new file mode 100644 index 0000000000..13ac734a43 --- /dev/null +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/CMakeLists.txt @@ -0,0 +1,2 @@ +add_subdirectory(hybrid_planning_manager_component) +add_subdirectory(planner_logic_plugins) diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/CMakeLists.txt b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/CMakeLists.txt new file mode 100644 index 0000000000..c3948543bc --- /dev/null +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/CMakeLists.txt @@ -0,0 +1,4 @@ +# Hybrid planning manager component +add_library(moveit_hybrid_planning_manager SHARED src/hybrid_planning_manager.cpp) +set_target_properties(moveit_hybrid_planning_manager PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(moveit_hybrid_planning_manager ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_events.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_events.h new file mode 100644 index 0000000000..4deeebdee1 --- /dev/null +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_events.h @@ -0,0 +1,62 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Sebastian Jahr + Description: Defines events that could occur during hybrid planning + */ +#pragma once + +namespace moveit::hybrid_planning +{ +/** + * Enum class HybridPlanningEvent - This class defines the most basic events that are likely to occur during hybrid planning + */ +enum class HybridPlanningEvent +{ + // Occurs when the hybrid planning manager receives a planning request + HYBRID_PLANNING_REQUEST_RECEIVED, + // Result of the global planning action + GLOBAL_PLANNING_ACTION_SUCCESSFUL, + GLOBAL_PLANNING_ACTION_ABORTED, + GLOBAL_PLANNING_ACTION_CANCELED, + // Indicates that the global planner found a solution (This solution is not necessarily the last or best solution) + GLOBAL_SOLUTION_AVAILABLE, + // Result of the local planning action + LOCAL_PLANNING_ACTION_SUCCESSFUL, + LOCAL_PLANNING_ACTION_ABORTED, + LOCAL_PLANNING_ACTION_CANCELED, + // Undefined event to allow empty reaction events to indicate failure + UNDEFINED +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_manager.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_manager.h new file mode 100644 index 0000000000..d8543a0d78 --- /dev/null +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_manager.h @@ -0,0 +1,132 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Sebastian Jahr + Description: The hybrid planning manager component node that serves as the control unit of the whole architecture. + */ + +#pragma once + +#include +#include + +#include +#include +#include + +#include + +#include + +namespace moveit::hybrid_planning +{ +/** + * Class HybridPlanningManager - ROS 2 component node that implements the hybrid planning manager. + */ +class HybridPlanningManager : public rclcpp::Node +{ +public: + /** \brief Constructor */ + HybridPlanningManager(const rclcpp::NodeOptions& options); + + /** + * Allows creation of a smart pointer that references to instances of this object + * @return shared pointer of the HybridPlanningManager instance that called the function + */ + std::shared_ptr shared_from_this() + { + return std::static_pointer_cast(Node::shared_from_this()); + } + + /** + * Load and initialized planner logic plugin and ROS 2 action and topic interfaces + * @return Initialization successfull yes/no + */ + bool initialize(); + + /** + * Hybrid planning goal callback for hybrid planning request server + * @param goal_handle Hybrid planning goal handle to access feedback and response + */ + void hybridPlanningRequestCallback( + std::shared_ptr> goal_handle); + + /** + * Send global planning request to global planner component + * @return Global planner successfully started yes/no + */ + bool sendGlobalPlannerAction(); + + /** + * Send local planning request to local planner component + * @return Local planner successfully started yes/no + */ + bool sendLocalPlannerAction(); + + /** + * Send back hybrid planning response + * @param success Indicates whether hybrid planning was successful + */ + void sendHybridPlanningResponse(bool success); + +private: + // Planner logic plugin loader + std::unique_ptr> planner_logic_plugin_loader_; + + // Planner logic instance to implement reactive behavior + std::shared_ptr planner_logic_instance_; + + // Timer to trigger events periodically + rclcpp::TimerBase::SharedPtr timer_; + + // Flag that indicates whether the manager is initialized + bool initialized_; + + // Shared hybrid planning goal handle + std::shared_ptr> hybrid_planning_goal_handle_; + + // Frequently updated feedback for the hybrid planning action requester + std::shared_ptr hybrid_planning_progess_; + + // Planning request action clients + rclcpp_action::Client::SharedPtr local_planner_action_client_; + rclcpp_action::Client::SharedPtr global_planner_action_client_; + + // Hybrid planning request action server + rclcpp_action::Server::SharedPtr hybrid_planning_request_server_; + + // Global solution subscriber + rclcpp::Subscription::SharedPtr global_solution_sub_; +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/moveit_error_code_interface.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/moveit_error_code_interface.h new file mode 100644 index 0000000000..8f60e16a8c --- /dev/null +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/moveit_error_code_interface.h @@ -0,0 +1,71 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Sebastian Jahr + Description: Defines an interface for setting and comparing MoveIt error codes. + */ + +#pragma once + +namespace moveit::hybrid_planning +{ +class MoveItErrorCode : public moveit_msgs::msg::MoveItErrorCodes +{ +public: + MoveItErrorCode() + { + val = 0; + } + MoveItErrorCode(int code) + { + val = code; + } + MoveItErrorCode(const moveit_msgs::msg::MoveItErrorCodes& code) + { + val = code.val; + } + explicit operator bool() const + { + return val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + } + bool operator==(const int code) const + { + return val == code; + } + bool operator!=(const int code) const + { + return val != code; + } +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h new file mode 100644 index 0000000000..d42a3eeae8 --- /dev/null +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h @@ -0,0 +1,132 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Sebastian Jahr + Description: Defines an interface for a planner logic plugin for the hybrid planning manager component node. + */ + +#pragma once + +#include +#include +#include +#include + +namespace moveit::hybrid_planning +{ +// Describes the outcome of a reaction to an event in the hybrid planning architecture +struct ReactionResult +{ + ReactionResult(const HybridPlanningEvent& planning_event, const std::string& error_msg, int error_code) + : error_message(error_msg), error_code(error_code) + { + switch (planning_event) + { + case HybridPlanningEvent::HYBRID_PLANNING_REQUEST_RECEIVED: + event = "Hybrid planning request received"; + break; + case HybridPlanningEvent::GLOBAL_PLANNING_ACTION_SUCCESSFUL: + event = "Global planning action successful"; + break; + case HybridPlanningEvent::GLOBAL_PLANNING_ACTION_ABORTED: + event = "Global planning action aborted"; + break; + case HybridPlanningEvent::GLOBAL_PLANNING_ACTION_CANCELED: + event = "Global planning action canceled"; + break; + case HybridPlanningEvent::GLOBAL_SOLUTION_AVAILABLE: + event = "Global solution available"; + break; + case HybridPlanningEvent::LOCAL_PLANNING_ACTION_SUCCESSFUL: + event = "Local planning action successful"; + break; + case HybridPlanningEvent::LOCAL_PLANNING_ACTION_ABORTED: + event = "Local planning action aborted"; + break; + case HybridPlanningEvent::LOCAL_PLANNING_ACTION_CANCELED: + event = "Local planning action canceled"; + break; + case HybridPlanningEvent::UNDEFINED: + event = "Undefined event"; + } + }; + ReactionResult(const std::string& event, const std::string& error_msg, int error_code) + : event(event), error_message(error_msg), error_code(error_code){}; + + // Event that triggered the reaction + std::string event; + + // Additional error description + std::string error_message; + + // Error code + MoveItErrorCode error_code; +}; + +class HybridPlanningManager; // Forward declaration + +/** + * Class PlannerLogicInterface - Base class for a planner logic. The logic defines how to react to different events that + * occur during hybrid planning. Events can be triggered by callback functions of the hybrid planning manager's ROS 2 + * interfaces or timers. They are encoded either inside an enum class or as a string to easily include custom events. + */ +class PlannerLogicInterface +{ +public: + /** + * Initialize the planner logic + * @param hybrid_planning_manager The hybrid planning manager instance to initialize this logic with. + * @return true if initialization was successful + */ + virtual bool initialize(const std::shared_ptr& hybrid_planning_manager) = 0; + + /** + * React to event defined in HybridPlanningEvent enum + * @param event Basic hybrid planning event + * @return Reaction result that summarizes the outcome of the reaction + */ + virtual ReactionResult react(const HybridPlanningEvent& event) = 0; + + /** + * React to custom event + * @param event Encoded as string + * @return Reaction result that summarizes the outcome of the reaction + */ + virtual ReactionResult react(const std::string& event) = 0; + +protected: + // The hybrid planning manager instance that runs this logic plugin + std::shared_ptr hybrid_planning_manager_ = nullptr; +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp new file mode 100644 index 0000000000..a7575cd087 --- /dev/null +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp @@ -0,0 +1,335 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include + +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("hybrid_planning_manager"); +} + +namespace moveit::hybrid_planning +{ +using namespace std::chrono_literals; + +HybridPlanningManager::HybridPlanningManager(const rclcpp::NodeOptions& options) + : Node("hybrid_planning_manager", options), initialized_(false) +{ + // Initialize hybrid planning component after after construction + // TODO(sjahr) Remove once life cycle component nodes are available + timer_ = this->create_wall_timer(1ms, [this]() { + if (initialized_) + { + timer_->cancel(); + } + else + { + if (!this->initialize()) + { + const std::string error = "Failed to initialize global planner"; + timer_->cancel(); + throw std::runtime_error(error); + } + initialized_ = true; + } + }); +} + +bool HybridPlanningManager::initialize() +{ + // Load planning logic plugin + try + { + planner_logic_plugin_loader_ = std::make_unique>( + "moveit_hybrid_planning", "moveit::hybrid_planning::PlannerLogicInterface"); + } + catch (pluginlib::PluginlibException& ex) + { + RCLCPP_ERROR(LOGGER, "Exception while creating planner logic plugin loader '%s'", ex.what()); + } + // TODO(sjahr) Refactor parameter declaration and use repository wide solution + std::string logic_plugin_name = ""; + if (this->has_parameter("planner_logic_plugin_name")) + { + this->get_parameter("planner_logic_plugin_name", logic_plugin_name); + } + else + { + logic_plugin_name = this->declare_parameter("planner_logic_plugin_name", + "moveit::hybrid_planning/ReplanInvalidatedTrajectory"); + } + try + { + planner_logic_instance_ = planner_logic_plugin_loader_->createUniqueInstance(logic_plugin_name); + if (!planner_logic_instance_->initialize(HybridPlanningManager::shared_from_this())) + { + throw std::runtime_error("Unable to initialize planner logic plugin"); + } + RCLCPP_INFO(LOGGER, "Using planner logic interface '%s'", logic_plugin_name.c_str()); + } + catch (pluginlib::PluginlibException& ex) + { + RCLCPP_ERROR(LOGGER, "Exception while loading planner logic '%s': '%s'", logic_plugin_name.c_str(), ex.what()); + } + + // Initialize local planning action client + local_planner_action_client_ = + rclcpp_action::create_client(this, "local_planning_action"); + if (!local_planner_action_client_->wait_for_action_server(2s)) + { + RCLCPP_ERROR(LOGGER, "Local planner action server not available after waiting"); + return false; + } + + // Initialize global planning action client + global_planner_action_client_ = + rclcpp_action::create_client(this, "global_planning_action"); + if (!global_planner_action_client_->wait_for_action_server(2s)) + { + RCLCPP_ERROR(LOGGER, "Global planner action server not available after waiting"); + return false; + } + + // Initialize hybrid planning action server + hybrid_planning_request_server_ = rclcpp_action::create_server( + this->get_node_base_interface(), this->get_node_clock_interface(), this->get_node_logging_interface(), + this->get_node_waitables_interface(), "run_hybrid_planning", + [](const rclcpp_action::GoalUUID& /*unused*/, + std::shared_ptr /*unused*/) { + RCLCPP_INFO(LOGGER, "Received goal request"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }, + [](const std::shared_ptr>& /*unused*/) { + RCLCPP_INFO(LOGGER, "Received request to cancel goal"); + return rclcpp_action::CancelResponse::ACCEPT; + }, + std::bind(&HybridPlanningManager::hybridPlanningRequestCallback, this, std::placeholders::_1)); + + // Initialize global solution subscriber + global_solution_sub_ = create_subscription( + "global_trajectory", rclcpp::SystemDefaultsQoS(), + [this](const moveit_msgs::msg::MotionPlanResponse::SharedPtr msg) { + // react is defined in a hybrid_planning_manager plugin + ReactionResult reaction_result = planner_logic_instance_->react(HybridPlanningEvent::GLOBAL_SOLUTION_AVAILABLE); + if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + auto result = std::make_shared(); + result->error_code.val = reaction_result.error_code.val; + result->error_message = reaction_result.error_message; + hybrid_planning_goal_handle_->abort(result); + RCLCPP_ERROR(LOGGER, "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); + } + }); + return true; +} + +bool HybridPlanningManager::sendGlobalPlannerAction() +{ + auto global_goal_options = rclcpp_action::Client::SendGoalOptions(); + + // Add goal response callback + global_goal_options.goal_response_callback = + [this](std::shared_future::SharedPtr> future) { + auto const& goal_handle = future.get(); + auto planning_progress = std::make_shared(); + auto& feedback = planning_progress->feedback; + if (!goal_handle) + { + feedback = "Global goal was rejected by server"; + } + else + { + feedback = "Global goal accepted by server"; + } + hybrid_planning_goal_handle_->publish_feedback(planning_progress); + }; + // Add result callback + global_goal_options.result_callback = + [this](const rclcpp_action::ClientGoalHandle::WrappedResult& global_result) { + // Reaction result from the latest event + ReactionResult reaction_result = + ReactionResult(HybridPlanningEvent::UNDEFINED, "", moveit_msgs::msg::MoveItErrorCodes::FAILURE); + switch (global_result.code) + { + case rclcpp_action::ResultCode::SUCCEEDED: + reaction_result = planner_logic_instance_->react(HybridPlanningEvent::GLOBAL_PLANNING_ACTION_SUCCESSFUL); + break; + case rclcpp_action::ResultCode::CANCELED: + reaction_result = planner_logic_instance_->react(HybridPlanningEvent::GLOBAL_PLANNING_ACTION_CANCELED); + break; + case rclcpp_action::ResultCode::ABORTED: + reaction_result = planner_logic_instance_->react(HybridPlanningEvent::GLOBAL_PLANNING_ACTION_ABORTED); + break; + default: + break; + } + // Abort hybrid planning if reaction fails + if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + auto result = std::make_shared(); + result->error_code.val = reaction_result.error_code.val; + result->error_message = reaction_result.error_message; + + hybrid_planning_goal_handle_->abort(result); + RCLCPP_ERROR(LOGGER, "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); + } + }; + + // Forward global trajectory goal from hybrid planning request TODO(sjahr) pass goal as function argument + auto global_goal_msg = moveit_msgs::action::GlobalPlanner::Goal(); + global_goal_msg.motion_sequence = + (hybrid_planning_goal_handle_->get_goal())->motion_sequence; // latest desired motion sequence; + global_goal_msg.planning_group = (hybrid_planning_goal_handle_->get_goal())->planning_group; // planning_group_; + // Send global planning goal and wait until it's accepted + auto goal_handle_future = global_planner_action_client_->async_send_goal(global_goal_msg, global_goal_options); + return true; // return always success TODO(sjahr) add more error checking +}; + +bool HybridPlanningManager::sendLocalPlannerAction() +{ + // Setup empty dummy goal (Global trajectory is subscribed by the local planner) TODO(sjahr) pass goal as function argument + auto local_goal_msg = moveit_msgs::action::LocalPlanner::Goal(); + auto local_goal_options = rclcpp_action::Client::SendGoalOptions(); + rclcpp_action::ClientGoalHandle::SharedPtr goal_handle; + + // Add goal response callback + local_goal_options.goal_response_callback = + [this](std::shared_future::SharedPtr> future) { + auto const& goal_handle = future.get(); + auto planning_progress = std::make_shared(); + auto& feedback = planning_progress->feedback; + if (!goal_handle) + { + feedback = "Local goal was rejected by server"; + } + else + { + feedback = "Local goal accepted by server"; + } + hybrid_planning_goal_handle_->publish_feedback(planning_progress); + }; + + // Add feedback callback + local_goal_options.feedback_callback = + [this](rclcpp_action::ClientGoalHandle::SharedPtr /*unused*/, + const std::shared_ptr local_planner_feedback) { + // react is defined in a hybrid_planning_manager plugin + ReactionResult reaction_result = planner_logic_instance_->react(local_planner_feedback->feedback); + if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + auto result = std::make_shared(); + result->error_code.val = reaction_result.error_code.val; + result->error_message = reaction_result.error_message; + hybrid_planning_goal_handle_->abort(result); + RCLCPP_ERROR(LOGGER, "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); + } + }; + + // Add result callback to print the result + local_goal_options.result_callback = + [this](const rclcpp_action::ClientGoalHandle::WrappedResult& local_result) { + // Reaction result from the latest event + ReactionResult reaction_result = + ReactionResult(HybridPlanningEvent::UNDEFINED, "", moveit_msgs::msg::MoveItErrorCodes::FAILURE); + switch (local_result.code) + { + case rclcpp_action::ResultCode::SUCCEEDED: + reaction_result = planner_logic_instance_->react(HybridPlanningEvent::LOCAL_PLANNING_ACTION_SUCCESSFUL); + break; + case rclcpp_action::ResultCode::CANCELED: + reaction_result = planner_logic_instance_->react(HybridPlanningEvent::LOCAL_PLANNING_ACTION_CANCELED); + break; + case rclcpp_action::ResultCode::ABORTED: + reaction_result = planner_logic_instance_->react(HybridPlanningEvent::LOCAL_PLANNING_ACTION_ABORTED); + break; + default: + break; + } + // Abort hybrid planning if reaction fails + if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + auto result = std::make_shared(); + result->error_code.val = reaction_result.error_code.val; + result->error_message = reaction_result.error_message; + + hybrid_planning_goal_handle_->abort(result); + RCLCPP_ERROR(LOGGER, "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); + } + }; + + // Send global planning goal + auto goal_handle_future = local_planner_action_client_->async_send_goal(local_goal_msg, local_goal_options); + return true; // return always success TODO(sjahr) add more error checking +} + +void HybridPlanningManager::hybridPlanningRequestCallback( + std::shared_ptr> goal_handle) +{ + // Pass goal handle to class member + hybrid_planning_goal_handle_ = std::move(goal_handle); + + // react is defined in a hybrid_planning_manager plugin + ReactionResult reaction_result = + planner_logic_instance_->react(HybridPlanningEvent::HYBRID_PLANNING_REQUEST_RECEIVED); + if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + auto result = std::make_shared(); + result->error_code.val = reaction_result.error_code.val; + result->error_message = reaction_result.error_message; + hybrid_planning_goal_handle_->abort(result); + RCLCPP_ERROR(LOGGER, "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); + } +} + +void HybridPlanningManager::sendHybridPlanningResponse(bool success) +{ + // Return hybrid planning action result dependend on the function's argument + auto result = std::make_shared(); + if (success) + { + result->error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + hybrid_planning_goal_handle_->succeed(result); + } + else + { + result->error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; + hybrid_planning_goal_handle_->abort(result); + } +} +} // namespace moveit::hybrid_planning + +// Register the component with class_loader +#include +RCLCPP_COMPONENTS_REGISTER_NODE(moveit::hybrid_planning::HybridPlanningManager) diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/CMakeLists.txt b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/CMakeLists.txt new file mode 100644 index 0000000000..1e103bb714 --- /dev/null +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/CMakeLists.txt @@ -0,0 +1,13 @@ +add_library(single_plan_execution_plugin SHARED + src/single_plan_execution.cpp +) +set_target_properties(single_plan_execution_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(single_plan_execution_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(single_plan_execution_plugin moveit_hybrid_planning_manager) + +add_library(replan_invalidated_trajectory_plugin SHARED + src/replan_invalidated_trajectory.cpp +) +set_target_properties(replan_invalidated_trajectory_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(replan_invalidated_trajectory_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(replan_invalidated_trajectory_plugin moveit_hybrid_planning_manager single_plan_execution_plugin) diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/replan_invalidated_trajectory.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/replan_invalidated_trajectory.h new file mode 100644 index 0000000000..6ce0f65dc3 --- /dev/null +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/replan_invalidated_trajectory.h @@ -0,0 +1,53 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Sebastian Jahr + Description: Simple hybrid planning logic that runs the global planner once and starts executing the global solution + with the local planner. In case the local planner detects a collision the global planner is rerun to update the + invalidated global trajectory. + */ + +#include + +namespace moveit::hybrid_planning +{ +class ReplanInvalidatedTrajectory : public SinglePlanExecution // Inherit from SinglePlanExecution because we just want + // alter the reaction to local planner events. +{ +public: + ReplanInvalidatedTrajectory() = default; + ~ReplanInvalidatedTrajectory() = default; + ReactionResult react(const std::string& event) override; +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h new file mode 100644 index 0000000000..339631ef9d --- /dev/null +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h @@ -0,0 +1,57 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Sebastian Jahr + Description: This planner logic plugin runs the global planner once and starts executing the global solution + with the local planner. + */ + +#include +#include + +namespace moveit::hybrid_planning +{ +class SinglePlanExecution : public PlannerLogicInterface +{ +public: + SinglePlanExecution() = default; + ~SinglePlanExecution() = default; + bool initialize(const std::shared_ptr& hybrid_planning_manager) override; + ReactionResult react(const HybridPlanningEvent& event) override; + ReactionResult react(const std::string& event) override; + +private: + bool local_planner_started_ = false; +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/replan_invalidated_trajectory.cpp b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/replan_invalidated_trajectory.cpp new file mode 100644 index 0000000000..1a3384695b --- /dev/null +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/replan_invalidated_trajectory.cpp @@ -0,0 +1,73 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("hybrid_planning_manager"); +} + +namespace moveit::hybrid_planning +{ +ReactionResult ReplanInvalidatedTrajectory::react(const std::string& event) +{ + if (event == "collision_ahead") + { + if (!hybrid_planning_manager_->sendGlobalPlannerAction()) // Start global planning + { + hybrid_planning_manager_->sendHybridPlanningResponse(false); + } + return ReactionResult(event, "", moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + } + if (event == "local_planner_stuck") + { + if (!hybrid_planning_manager_->sendGlobalPlannerAction()) // Start global planning + { + hybrid_planning_manager_->sendHybridPlanningResponse(false); + } + return ReactionResult(event, "", moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + } + else + { + return ReactionResult(event, "'ReplanInvalidatedTrajectory' plugin cannot handle this event.", + moveit_msgs::msg::MoveItErrorCodes::FAILURE); + } +}; +} // namespace moveit::hybrid_planning + +#include + +PLUGINLIB_EXPORT_CLASS(moveit::hybrid_planning::ReplanInvalidatedTrajectory, + moveit::hybrid_planning::PlannerLogicInterface) diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp new file mode 100644 index 0000000000..f0306aa3b3 --- /dev/null +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp @@ -0,0 +1,104 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("hybrid_planning_manager"); +} + +namespace moveit::hybrid_planning +{ +bool SinglePlanExecution::initialize(const std::shared_ptr& hybrid_planning_manager) +{ + hybrid_planning_manager_ = hybrid_planning_manager; + return true; +} + +ReactionResult SinglePlanExecution::react(const HybridPlanningEvent& event) +{ + switch (event) + { + case HybridPlanningEvent::HYBRID_PLANNING_REQUEST_RECEIVED: + // Handle new hybrid planning request + if (!hybrid_planning_manager_->sendGlobalPlannerAction()) // Start global planning + { + hybrid_planning_manager_->sendHybridPlanningResponse(false); + } + // Reset local planner started flag + local_planner_started_ = false; + return ReactionResult(event, "", moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + case HybridPlanningEvent::GLOBAL_SOLUTION_AVAILABLE: + // Do nothing since we wait for the global planning action to finish + return ReactionResult(event, "Do nothing", moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + case HybridPlanningEvent::GLOBAL_PLANNING_ACTION_SUCCESSFUL: + // Activate local planner once global solution is available + if (!local_planner_started_) + { // ensure the local planner is not started twice + if (!hybrid_planning_manager_->sendLocalPlannerAction()) // Start local planning + { + hybrid_planning_manager_->sendHybridPlanningResponse(false); + } + local_planner_started_ = true; + } + return ReactionResult(event, "", moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + case HybridPlanningEvent::GLOBAL_PLANNING_ACTION_ABORTED: + // Abort hybrid planning if no global solution is found + return ReactionResult(event, "Global planner failed to find a solution", + moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED); + case HybridPlanningEvent::LOCAL_PLANNING_ACTION_SUCCESSFUL: + // Finish hybrid planning action successfully because local planning action succeeded + hybrid_planning_manager_->sendHybridPlanningResponse(true); + return ReactionResult(event, "", moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + case HybridPlanningEvent::LOCAL_PLANNING_ACTION_ABORTED: + // Local planning failed so abort hybrid planning + return ReactionResult(event, "Local planner failed to find a solution", + moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED); + default: + // Unknown event, abort hybrid planning + return ReactionResult(event, "Unknown event", moveit_msgs::msg::MoveItErrorCodes::FAILURE); + } +} + +ReactionResult SinglePlanExecution::react(const std::string& event) +{ + return ReactionResult(event, "'Single-Plan-Execution' plugin cannot handle events given as string.", + moveit_msgs::msg::MoveItErrorCodes::FAILURE); +}; +} // namespace moveit::hybrid_planning + +#include + +PLUGINLIB_EXPORT_CLASS(moveit::hybrid_planning::SinglePlanExecution, moveit::hybrid_planning::PlannerLogicInterface) diff --git a/moveit_ros/hybrid_planning/local_planner/CMakeLists.txt b/moveit_ros/hybrid_planning/local_planner/CMakeLists.txt new file mode 100644 index 0000000000..27438c8897 --- /dev/null +++ b/moveit_ros/hybrid_planning/local_planner/CMakeLists.txt @@ -0,0 +1,3 @@ +add_subdirectory(local_planner_component) +add_subdirectory(trajectory_operator_plugins) +add_subdirectory(local_constraint_solver_plugins) diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt new file mode 100644 index 0000000000..fa68289600 --- /dev/null +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt @@ -0,0 +1,5 @@ +add_library(forward_trajectory_plugin SHARED + src/forward_trajectory.cpp +) +set_target_properties(forward_trajectory_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(forward_trajectory_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.h b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.h new file mode 100644 index 0000000000..8e27edb685 --- /dev/null +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.h @@ -0,0 +1,73 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Sebastian Jahr + Description: Simple local solver plugin that forwards the next waypoint of the sampled local trajectory. + The local solver stops for two conditions: invalid waypoint (likely due to collision) or if it has been stuck for + several iterations. + */ + +#pragma once + +#include +#include + +namespace moveit::hybrid_planning +{ +class ForwardTrajectory : public LocalConstraintSolverInterface +{ +public: + ForwardTrajectory() = default; + ~ForwardTrajectory() = default; + bool initialize(const rclcpp::Node::SharedPtr& node, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const std::string& group_name) override; + bool reset() override; + + moveit_msgs::action::LocalPlanner::Feedback + solve(const robot_trajectory::RobotTrajectory& local_trajectory, + const std::shared_ptr local_goal, + trajectory_msgs::msg::JointTrajectory& local_solution) override; + +private: + rclcpp::Node::SharedPtr node_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + bool path_invalidation_event_send_; // Send path invalidation event only once + bool stop_before_collision_; + + // Detect when the local planner gets stuck + size_t num_iterations_stuck_; + moveit::core::RobotStatePtr prev_waypoint_target_; +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp new file mode 100644 index 0000000000..41313fe238 --- /dev/null +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp @@ -0,0 +1,174 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include +#include + +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("local_planner_component"); +// If stuck for this many iterations or more, abort the local planning action +constexpr size_t STUCK_ITERATIONS_THRESHOLD = 10; +constexpr double STUCK_THRESHOLD_RAD = 1e-4; // L1-norm sum across all joints +} // namespace + +namespace moveit::hybrid_planning +{ +bool ForwardTrajectory::initialize(const rclcpp::Node::SharedPtr& node, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const std::string& group_name) +{ + // Load parameter & initialize member variables + if (node->has_parameter("stop_before_collision")) + node->get_parameter("stop_before_collision", stop_before_collision_); + else + stop_before_collision_ = node->declare_parameter("stop_before_collision", false); + planning_scene_monitor_ = planning_scene_monitor; + node_ = node; + path_invalidation_event_send_ = false; + num_iterations_stuck_ = 0; + return true; +} + +bool ForwardTrajectory::reset() +{ + num_iterations_stuck_ = 0; + prev_waypoint_target_.reset(); + path_invalidation_event_send_ = false; + return true; +}; + +moveit_msgs::action::LocalPlanner::Feedback +ForwardTrajectory::solve(const robot_trajectory::RobotTrajectory& local_trajectory, + const std::shared_ptr local_goal, + trajectory_msgs::msg::JointTrajectory& local_solution) +{ + // A message every once in awhile is useful in case the local planner gets stuck + RCLCPP_INFO_THROTTLE(LOGGER, *node_->get_clock(), 2000 /* ms */, "The local planner is solving..."); + + // Create controller command trajectory + robot_trajectory::RobotTrajectory robot_command(local_trajectory.getRobotModel(), local_trajectory.getGroupName()); + + // Feedback + moveit_msgs::action::LocalPlanner::Feedback feedback_result; + + // If this flag is set, ignore collisions + if (!stop_before_collision_) + { + robot_command.addSuffixWayPoint(local_trajectory.getWayPoint(0), 0.0); + } + else + { + // Get current planning scene + planning_scene_monitor_->updateFrameTransforms(); + + moveit::core::RobotStatePtr current_state; + bool is_path_valid = false; + // Lock the planning scene as briefly as possible + { + planning_scene_monitor::LockedPlanningSceneRO locked_planning_scene(planning_scene_monitor_); + current_state = std::make_shared(locked_planning_scene->getCurrentState()); + is_path_valid = locked_planning_scene->isPathValid(local_trajectory, local_trajectory.getGroupName(), false); + } + + // Check if path is valid + if (is_path_valid) + { + if (path_invalidation_event_send_) + { + path_invalidation_event_send_ = false; // Reset flag + } + // Forward next waypoint to the robot controller + robot_command.addSuffixWayPoint(local_trajectory.getWayPoint(0), 0.0); + } + else + { + if (!path_invalidation_event_send_) + { // Send feedback only once + feedback_result.feedback = "collision_ahead"; + path_invalidation_event_send_ = true; // Set feedback flag + } + RCLCPP_INFO(LOGGER, "Collision ahead, hold current position"); + // Keep current position + moveit::core::RobotState current_state_command(*current_state); + if (current_state_command.hasVelocities()) + { + current_state_command.zeroVelocities(); + } + if (current_state_command.hasAccelerations()) + { + current_state_command.zeroAccelerations(); + } + robot_command.empty(); + robot_command.addSuffixWayPoint(*current_state, 0.0); + } + + // Detect if the local solver is stuck + if (!prev_waypoint_target_) + { + // Just initialize if this is the first iteration + prev_waypoint_target_ = robot_command.getFirstWayPointPtr(); + } + else + { + if (prev_waypoint_target_->distance(*robot_command.getFirstWayPointPtr()) <= STUCK_THRESHOLD_RAD) + { + ++num_iterations_stuck_; + if (num_iterations_stuck_ > STUCK_ITERATIONS_THRESHOLD) + { + num_iterations_stuck_ = 0; + prev_waypoint_target_ = nullptr; + feedback_result.feedback = "local_planner_stuck"; + path_invalidation_event_send_ = true; // Set feedback flag + RCLCPP_INFO(LOGGER, "The local planner has been stuck for several iterations. Aborting."); + } + } + prev_waypoint_target_ = robot_command.getFirstWayPointPtr(); + } + } + + // Transform robot trajectory into joint_trajectory message + moveit_msgs::msg::RobotTrajectory robot_command_msg; + robot_command.getRobotTrajectoryMsg(robot_command_msg); + local_solution = robot_command_msg.joint_trajectory; + + return feedback_result; +} +} // namespace moveit::hybrid_planning + +#include + +PLUGINLIB_EXPORT_CLASS(moveit::hybrid_planning::ForwardTrajectory, + moveit::hybrid_planning::LocalConstraintSolverInterface); diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/CMakeLists.txt b/moveit_ros/hybrid_planning/local_planner/local_planner_component/CMakeLists.txt new file mode 100644 index 0000000000..5c5a80b20d --- /dev/null +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/CMakeLists.txt @@ -0,0 +1,6 @@ +# Local_planner_component +add_library(moveit_local_planner_component SHARED + src/local_planner_component.cpp +) +set_target_properties(moveit_local_planner_component PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(moveit_local_planner_component ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_constraint_solver_interface.h b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_constraint_solver_interface.h new file mode 100644 index 0000000000..a09e514080 --- /dev/null +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_constraint_solver_interface.h @@ -0,0 +1,88 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Sebastian Jahr + Description: Defines an interface for a local constraint solver plugin implementation for the local planner component node. + */ + +#pragma once + +#include +#include + +#include +#include + +#include +#include + +#include + +#include + +namespace moveit::hybrid_planning +{ +/** + * Class LocalConstraintSolverInterface - Base class for a local constrain solver. + */ +class LocalConstraintSolverInterface +{ +public: + /** + * Initialize local constraint solver + * @return True if initialization was successful + */ + virtual bool initialize(const rclcpp::Node::SharedPtr& node, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const std::string& group_name) = 0; + + /** + * Solve local planning problem for the current iteration + * @param local_trajectory The local trajectory to pursue + * @param local_goal Local goal constraints + * @param local_solution solution plan in joint space + * @return Feedback event from the current solver call i.e. "Collision detected" + */ + virtual moveit_msgs::action::LocalPlanner::Feedback + solve(const robot_trajectory::RobotTrajectory& local_trajectory, + const std::shared_ptr local_goal, + trajectory_msgs::msg::JointTrajectory& local_solution) = 0; + + /** + * Reset local constraint solver to some user-defined initial state + * @return True if reset was successful + */ + virtual bool reset() = 0; +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.h b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.h new file mode 100644 index 0000000000..aec567b8a9 --- /dev/null +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.h @@ -0,0 +1,209 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Sebastian Jahr + Description: A local planner component node that is customizable through plugins that implement the local planning + problem solver algorithm and the trajectory matching and blending. + */ + +#pragma once + +#include +#include + +#include + +#include +#include +#include + +#include +#include + +#include "trajectory_msgs/msg/joint_trajectory.hpp" + +#include +#include +#include +#include + +#include +#include + +namespace moveit::hybrid_planning +{ +// TODO(sjahr) Refactor and use repository wide solution +template +void declareOrGetParam(const std::string& param_name, T& output_value, const T& default_value, + const rclcpp::Node::SharedPtr& node) +{ + try + { + if (node->has_parameter(param_name)) + node->get_parameter(param_name, output_value); + else + output_value = node->declare_parameter(param_name, default_value); + } + catch (const rclcpp::exceptions::InvalidParameterTypeException& e) + { + RCLCPP_ERROR_STREAM(node->get_logger(), + "Error getting parameter '" << param_name << "', check parameter type in YAML file"); + throw e; + } +} + +/// Internal local planner states +/// TODO(sjahr) Use lifecycle node? +enum class LocalPlannerState : int8_t +{ + ABORT = -1, + ERROR = 0, + UNCONFIGURED = 1, + AWAIT_GLOBAL_TRAJECTORY = 2, + LOCAL_PLANNING_ACTIVE = 3 +}; + +/** + * Class LocalPlannerComponent - ROS 2 component node that implements a local planner. + */ +class LocalPlannerComponent +{ +public: + /// Struct that contains configuration of the local planner component node + struct LocalPlannerConfig + { + void load(const rclcpp::Node::SharedPtr& node) + { + std::string undefined = ""; + declareOrGetParam("group_name", group_name, undefined, node); + declareOrGetParam("trajectory_operator_plugin_name", trajectory_operator_plugin_name, undefined, + node); + declareOrGetParam("local_constraint_solver_plugin_name", local_constraint_solver_plugin_name, + undefined, node); + declareOrGetParam("local_planning_frequency", local_planning_frequency, 1.0, node); + declareOrGetParam("global_solution_topic", global_solution_topic, undefined, node); + declareOrGetParam("local_solution_topic", local_solution_topic, undefined, node); + declareOrGetParam("local_solution_topic_type", local_solution_topic_type, undefined, node); + declareOrGetParam("publish_joint_positions", publish_joint_positions, false, node); + declareOrGetParam("publish_joint_velocities", publish_joint_velocities, false, node); + } + + std::string group_name; + std::string robot_description; + std::string robot_description_semantic; + std::string publish_planning_scene_topic; + std::string trajectory_operator_plugin_name; + std::string local_constraint_solver_plugin_name; + std::string global_solution_topic; + std::string local_solution_topic; + std::string local_solution_topic_type; + bool publish_joint_positions; + bool publish_joint_velocities; + double local_planning_frequency; + }; + + /** \brief Constructor */ + LocalPlannerComponent(const rclcpp::NodeOptions& options); + + /** + * Initialize and start planning scene monitor to listen to the planning scene topic. + * Load trajectory_operator and constraint solver plugin. + * Initialize ROS 2 interfaces + * @return true if scene monitor and plugins are successfully initialized + */ + bool initialize(); + + /** + * Handle the planners current job based on the internal state each iteration when the planner is started. + */ + void executeIteration(); + + // This function is required to make this class a valid NodeClass + // see https://docs.ros2.org/foxy/api/rclcpp_components/register__node__macro_8hpp.html + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() // NOLINT + { + return node_->get_node_base_interface(); // NOLINT + } + +private: + /** \brief Reset internal data members including state_ = LocalPlannerState::AWAIT_GLOBAL_TRAJECTORY */ + void reset(); + + std::shared_ptr node_; + + // Planner configuration + LocalPlannerConfig config_; + + // Current planner state + LocalPlannerState state_; + + // Timer to periodically call executeIteration() + rclcpp::TimerBase::SharedPtr timer_; + + // Latest action goal handle + std::shared_ptr> local_planning_goal_handle_; + + // Local planner feedback + std::shared_ptr local_planner_feedback_; + + // Planning scene monitor to get the current planning scene + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + + // TF + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // Global solution listener + rclcpp::Subscription::SharedPtr global_solution_subscriber_; + + // Local planning request action server + rclcpp_action::Server::SharedPtr local_planning_request_server_; + + // Local solution publisher + rclcpp::Publisher::SharedPtr local_trajectory_publisher_; + rclcpp::Publisher::SharedPtr local_solution_publisher_; + + // Local constraint solver plugin loader + std::unique_ptr> local_constraint_solver_plugin_loader_; + + // Local constrain solver instance to compute a local solution each iteration + std::shared_ptr local_constraint_solver_instance_; + + // Trajectory operator plugin + std::unique_ptr> trajectory_operator_loader_; + + // Trajectory_operator instance handle trajectory matching and blending + std::shared_ptr trajectory_operator_instance_; +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/trajectory_operator_interface.h b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/trajectory_operator_interface.h new file mode 100644 index 0000000000..35ca5940c5 --- /dev/null +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/trajectory_operator_interface.h @@ -0,0 +1,109 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Sebastian Jahr + Description: Defines an interface for a trajectory operator plugin implementation for the local planner component node. + */ + +#pragma once + +#include +#include + +#include +#include + +#include + +#include +#include + +namespace moveit::hybrid_planning +{ +/** + * Class TrajectoryOperatorInterface - Base class for a trajectory operator. The operator's task is manage the local + * planner's global reference trajectory. This includes trajectory matching based on the current state to identify the + * current planning problem and blending of new global trajectory updates into the currently processed reference + * trajectory. + */ +class TrajectoryOperatorInterface +{ +public: + /** + * Initialize trajectory operator + * @param node Node handle to access parameters + * @param robot_model Robot model + * @param group_name Name of the joint group the trajectory uses + * @return True if initialization was successful + */ + virtual bool initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& robot_model, + const std::string& group_name) = 0; + + /** + * Add a new reference trajectory segment to the vector of global trajectory segments to process + * @param new_trajectory New reference trajectory segment to add + * @return True if segment was successfully added + */ + virtual moveit_msgs::action::LocalPlanner::Feedback + addTrajectorySegment(const robot_trajectory::RobotTrajectory& new_trajectory) = 0; + + /** + * Return the current local constraints based on the newest robot state + * @param current_state Current RobotState + * @return Current local constraints that define the local planning goal + */ + virtual moveit_msgs::action::LocalPlanner::Feedback + getLocalTrajectory(const moveit::core::RobotState& current_state, + robot_trajectory::RobotTrajectory& local_trajectory) = 0; + + /** + * Return the processing status of the reference trajectory's execution based on a user defined + * metric. + * @param current_state Current RobotState + * @return A value between 0.0 (start) to 1.0 (completion). + */ + virtual double getTrajectoryProgress(const moveit::core::RobotState& current_state) = 0; + + /** + * Reset trajectory operator to some user-defined initial state + * @return True if reset was successful + */ + virtual bool reset() = 0; + +protected: + // Reference trajectory to be precessed + robot_trajectory::RobotTrajectoryPtr reference_trajectory_; + std::string group_; +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp b/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp new file mode 100644 index 0000000000..bf507a363e --- /dev/null +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp @@ -0,0 +1,336 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +#include +#include + +#include + +#include + +namespace moveit::hybrid_planning +{ +using namespace std::chrono_literals; + +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("local_planner_component"); + +// If the trajectory progress reaches more than 0.X the global goal state is considered as reached +constexpr float PROGRESS_THRESHOLD = 0.995; +} // namespace + +LocalPlannerComponent::LocalPlannerComponent(const rclcpp::NodeOptions& options) + : node_{ std::make_shared("local_planner_component", options) } +{ + state_ = LocalPlannerState::UNCONFIGURED; + local_planner_feedback_ = std::make_shared(); + + if (!this->initialize()) + { + throw std::runtime_error("Failed to initialize local planner component"); + } +} + +bool LocalPlannerComponent::initialize() +{ + // Load planner parameter + config_.load(node_); + + // Validate config + if (config_.local_solution_topic_type == "std_msgs/Float64MultiArray") + { + if ((config_.publish_joint_positions && config_.publish_joint_velocities) || + (!config_.publish_joint_positions && !config_.publish_joint_velocities)) + { + RCLCPP_ERROR(LOGGER, "When publishing a std_msgs/Float64MultiArray, you must select positions OR velocities. " + "Enabling both or none is not possible!"); + return false; + } + } + + // Configure planning scene monitor + planning_scene_monitor_ = std::make_shared( + node_, "robot_description", tf_buffer_, "local_planner/planning_scene_monitor"); + if (!planning_scene_monitor_->getPlanningScene()) + { + RCLCPP_ERROR(LOGGER, "Unable to configure planning scene monitor"); + return false; + } + + // Start state and scene monitors + RCLCPP_INFO(LOGGER, "Starting planning scene monitors"); + planning_scene_monitor_->startSceneMonitor(); + planning_scene_monitor_->startWorldGeometryMonitor(); + planning_scene_monitor_->startStateMonitor(); + + // Load trajectory operator plugin + try + { + trajectory_operator_loader_ = std::make_unique>( + "moveit_hybrid_planning", "moveit::hybrid_planning::TrajectoryOperatorInterface"); + } + catch (pluginlib::PluginlibException& ex) + { + RCLCPP_ERROR(LOGGER, "Exception while creating trajectory operator plugin loader: '%s'", ex.what()); + return false; + } + try + { + trajectory_operator_instance_ = + trajectory_operator_loader_->createUniqueInstance(config_.trajectory_operator_plugin_name); + if (!trajectory_operator_instance_->initialize(node_, planning_scene_monitor_->getRobotModel(), + config_.group_name)) // TODO(sjahr) add default group param + throw std::runtime_error("Unable to initialize trajectory operator plugin"); + RCLCPP_INFO(LOGGER, "Using trajectory operator interface '%s'", config_.trajectory_operator_plugin_name.c_str()); + } + catch (pluginlib::PluginlibException& ex) + { + RCLCPP_ERROR(LOGGER, "Exception while loading trajectory operator '%s': '%s'", + config_.trajectory_operator_plugin_name.c_str(), ex.what()); + return false; + } + + // Load local constraint solver + try + { + local_constraint_solver_plugin_loader_ = std::make_unique>( + "moveit_hybrid_planning", "moveit::hybrid_planning::LocalConstraintSolverInterface"); + } + catch (pluginlib::PluginlibException& ex) + { + RCLCPP_ERROR(LOGGER, "Exception while creating constraint solver plugin loader '%s'", ex.what()); + return false; + } + try + { + local_constraint_solver_instance_ = + local_constraint_solver_plugin_loader_->createUniqueInstance(config_.local_constraint_solver_plugin_name); + if (!local_constraint_solver_instance_->initialize(node_, planning_scene_monitor_, config_.group_name)) + throw std::runtime_error("Unable to initialize constraint solver plugin"); + RCLCPP_INFO(LOGGER, "Using constraint solver interface '%s'", config_.local_constraint_solver_plugin_name.c_str()); + } + catch (pluginlib::PluginlibException& ex) + { + RCLCPP_ERROR(LOGGER, "Exception while loading constraint solver '%s': '%s'", + config_.local_constraint_solver_plugin_name.c_str(), ex.what()); + return false; + } + + // Initialize local planning request action server + using namespace std::placeholders; + local_planning_request_server_ = rclcpp_action::create_server( + node_, "local_planning_action", + [](const rclcpp_action::GoalUUID& /*unused*/, + std::shared_ptr /*unused*/) { + RCLCPP_INFO(LOGGER, "Received local planning goal request"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }, + [](const std::shared_ptr>& /*unused*/) { + RCLCPP_INFO(LOGGER, "Received request to cancel local planning goal"); + return rclcpp_action::CancelResponse::ACCEPT; + }, + [this](std::shared_ptr> goal_handle) { + local_planning_goal_handle_ = std::move(goal_handle); + // Start local planning loop when an action request is received + timer_ = node_->create_wall_timer(1s / config_.local_planning_frequency, + std::bind(&LocalPlannerComponent::executeIteration, this)); + }); + + // Initialize global trajectory listener + global_solution_subscriber_ = node_->create_subscription( + config_.global_solution_topic, 1, [this](const moveit_msgs::msg::MotionPlanResponse::SharedPtr msg) { + // Add received trajectory to internal reference trajectory + robot_trajectory::RobotTrajectory new_trajectory(planning_scene_monitor_->getRobotModel(), msg->group_name); + moveit::core::RobotState start_state(planning_scene_monitor_->getRobotModel()); + moveit::core::robotStateMsgToRobotState(msg->trajectory_start, start_state); + new_trajectory.setRobotTrajectoryMsg(start_state, msg->trajectory); + *local_planner_feedback_ = trajectory_operator_instance_->addTrajectorySegment(new_trajectory); + + // Feedback is only send when the hybrid planning architecture should react to a discrete event that occurred + // when the reference trajectory is updated + if (!local_planner_feedback_->feedback.empty()) + { + local_planning_goal_handle_->publish_feedback(local_planner_feedback_); + } + + // Update local planner state + state_ = LocalPlannerState::LOCAL_PLANNING_ACTIVE; + }); + + // Initialize local solution publisher + RCLCPP_INFO(LOGGER, "Using '%s' as local solution topic type", config_.local_solution_topic_type.c_str()); + if (config_.local_solution_topic_type == "trajectory_msgs/JointTrajectory") + { + local_trajectory_publisher_ = + node_->create_publisher(config_.local_solution_topic, 1); + } + else if (config_.local_solution_topic_type == "std_msgs/Float64MultiArray") + { + local_solution_publisher_ = + node_->create_publisher(config_.local_solution_topic, 1); + } + else if (config_.local_solution_topic_type == "CUSTOM") + { + // Local solution publisher is defined by the local constraint solver plugin + } + + state_ = LocalPlannerState::AWAIT_GLOBAL_TRAJECTORY; + return true; +} + +void LocalPlannerComponent::executeIteration() +{ + auto result = std::make_shared(); + + // Do different things depending on the planner's internal state + switch (state_) + { + // Wait for global solution to be published + case LocalPlannerState::AWAIT_GLOBAL_TRAJECTORY: + // Do nothing + break; + // Notify action client that local planning failed + case LocalPlannerState::ABORT: + { + result->error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; + result->error_message = "Local planner is in an aborted state. Resetting."; + local_planning_goal_handle_->abort(result); + reset(); + break; + } + // If the planner received an action request and a global solution it starts to plan locally + case LocalPlannerState::LOCAL_PLANNING_ACTIVE: + { + // Read current planning scene + planning_scene_monitor_->updateSceneWithCurrentState(); + planning_scene_monitor_->lockSceneRead(); // LOCK planning scene + planning_scene::PlanningScenePtr planning_scene = planning_scene_monitor_->getPlanningScene(); + planning_scene_monitor_->unlockSceneRead(); // UNLOCK planning scene + + // Get current state + auto current_robot_state = planning_scene->getCurrentStateNonConst(); + + // Check if the global goal is reached + if (trajectory_operator_instance_->getTrajectoryProgress(current_robot_state) > PROGRESS_THRESHOLD) + { + local_planning_goal_handle_->succeed(result); + reset(); + break; + } + + // Get local goal trajectory to follow + robot_trajectory::RobotTrajectory local_trajectory = + robot_trajectory::RobotTrajectory(planning_scene_monitor_->getRobotModel(), config_.group_name); + *local_planner_feedback_ = + trajectory_operator_instance_->getLocalTrajectory(current_robot_state, local_trajectory); + + // Feedback is only sent when the hybrid planning architecture should react to a discrete event that occurred + // during the identification of the local planning problem + if (!local_planner_feedback_->feedback.empty()) + { + local_planning_goal_handle_->publish_feedback(local_planner_feedback_); + } + + // Solve local planning problem + trajectory_msgs::msg::JointTrajectory local_solution; + + // Feedback is only send when the hybrid planning architecture should react to a discrete event that occurred + // while computing a local solution + *local_planner_feedback_ = local_constraint_solver_instance_->solve( + local_trajectory, local_planning_goal_handle_->get_goal(), local_solution); + + // Feedback is only send when the hybrid planning architecture should react to a discrete event + if (!local_planner_feedback_->feedback.empty()) + { + local_planning_goal_handle_->publish_feedback(local_planner_feedback_); + } + + // Use a configurable message interface like MoveIt servo + // (See https://github.com/ros-planning/moveit2/blob/main/moveit_ros/moveit_servo/src/servo_calcs.cpp) + // Format outgoing msg in the right format + // (trajectory_msgs/JointTrajectory or joint positions/velocities in form of std_msgs/Float64MultiArray). + if (config_.local_solution_topic_type == "trajectory_msgs/JointTrajectory") + { + local_trajectory_publisher_->publish(local_solution); + } + else if (config_.local_solution_topic_type == "std_msgs/Float64MultiArray") + { + // Transform "trajectory_msgs/JointTrajectory" to "std_msgs/Float64MultiArray" + auto joints = std::make_unique(); + if (!local_solution.points.empty()) + { + if (config_.publish_joint_positions) + { + joints->data = local_solution.points[0].positions; + } + else if (config_.publish_joint_velocities) + { + joints->data = local_solution.points[0].velocities; + } + } + local_solution_publisher_->publish(std::move(joints)); + } + else if (config_.local_solution_topic_type == "CUSTOM") + { + // Local solution publisher is defined by the local constraint solver plugin + } + break; + } + default: + { + result->error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; + result->error_message = "Unexpected failure."; + local_planning_goal_handle_->abort(result); + RCLCPP_ERROR(LOGGER, "Local planner somehow failed"); // TODO(sjahr) Add more detailed failure information + reset(); + break; + } + } +}; + +void LocalPlannerComponent::reset() +{ + local_constraint_solver_instance_->reset(); + trajectory_operator_instance_->reset(); + timer_->cancel(); + state_ = LocalPlannerState::AWAIT_GLOBAL_TRAJECTORY; +} +} // namespace moveit::hybrid_planning + +// Register the component with class_loader +#include +RCLCPP_COMPONENTS_REGISTER_NODE(moveit::hybrid_planning::LocalPlannerComponent) diff --git a/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/CMakeLists.txt b/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/CMakeLists.txt new file mode 100644 index 0000000000..18e40dfc57 --- /dev/null +++ b/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/CMakeLists.txt @@ -0,0 +1,5 @@ +add_library(simple_sampler_plugin SHARED + src/simple_sampler.cpp +) +set_target_properties(simple_sampler_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(simple_sampler_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/include/moveit/trajectory_operator_plugins/simple_sampler.h b/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/include/moveit/trajectory_operator_plugins/simple_sampler.h new file mode 100644 index 0000000000..b9efcc1554 --- /dev/null +++ b/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/include/moveit/trajectory_operator_plugins/simple_sampler.h @@ -0,0 +1,70 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Sebastian Jahr + Description: Simple trajectory operator that samples the next global trajectory waypoint as local goal constraint + based on the current robot state. When the waypoint is reached the index that marks the current local goal constraint + is updated to the next global trajectory waypoint. Global trajectory updates simply replace the reference trajectory. + */ + +#include +#include + +namespace moveit::hybrid_planning +{ +class SimpleSampler : public TrajectoryOperatorInterface +{ +public: + SimpleSampler() = default; + ~SimpleSampler() = default; + + bool initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& robot_model, + const std::string& group_name) override; + moveit_msgs::action::LocalPlanner::Feedback + addTrajectorySegment(const robot_trajectory::RobotTrajectory& new_trajectory) override; + moveit_msgs::action::LocalPlanner::Feedback + getLocalTrajectory(const moveit::core::RobotState& current_state, + robot_trajectory::RobotTrajectory& local_trajectory) override; + double getTrajectoryProgress(const moveit::core::RobotState& current_state) override; + bool reset() override; + +private: + std::size_t + next_waypoint_index_; // Indicates which reference trajectory waypoint is the current local goal constrained + bool pass_through_; // If true, the reference_trajectory is simply forwarded each time the getLocalTrajectory() function is called + moveit_msgs::action::LocalPlanner::Feedback feedback_; // Empty feedback + trajectory_processing::TimeOptimalTrajectoryGeneration time_parametrization_; + const moveit::core::JointModelGroup* joint_group_; +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/src/simple_sampler.cpp b/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/src/simple_sampler.cpp new file mode 100644 index 0000000000..1b2949808b --- /dev/null +++ b/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/src/simple_sampler.cpp @@ -0,0 +1,135 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +#include + +namespace moveit::hybrid_planning +{ +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("local_planner_component"); +constexpr double WAYPOINT_RADIAN_TOLERANCE = 0.2; // rad: L1-norm sum for all joints +} // namespace + +bool SimpleSampler::initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& robot_model, + const std::string& group_name) +{ + // Load parameter & initialize member variables + if (node->has_parameter("pass_through")) + { + node->get_parameter("pass_through", pass_through_); + } + else + { + pass_through_ = node->declare_parameter("pass_through", false); + } + reference_trajectory_ = std::make_shared(robot_model, group_name); + next_waypoint_index_ = 0; + joint_group_ = robot_model->getJointModelGroup(group_name); + return true; +} + +moveit_msgs::action::LocalPlanner::Feedback +SimpleSampler::addTrajectorySegment(const robot_trajectory::RobotTrajectory& new_trajectory) +{ + // Reset trajectory operator to delete old reference trajectory + reset(); + + // Throw away old reference trajectory and use trajectory update + reference_trajectory_ = std::make_shared(new_trajectory); + + // Parametrize trajectory and calculate velocity and accelerations + time_parametrization_.computeTimeStamps(*reference_trajectory_); + + // Return empty feedback + return feedback_; +} + +bool SimpleSampler::reset() +{ + // Reset index + next_waypoint_index_ = 0; + reference_trajectory_->clear(); + return true; +} +moveit_msgs::action::LocalPlanner::Feedback +SimpleSampler::getLocalTrajectory(const moveit::core::RobotState& current_state, + robot_trajectory::RobotTrajectory& local_trajectory) +{ + // Delete previous local trajectory + local_trajectory.clear(); + + // Determine current local trajectory based on configured behavior + if (pass_through_) + { + // Use reference_trajectory as local trajectory + local_trajectory.append(*reference_trajectory_, 0.0, 0, reference_trajectory_->getWayPointCount()); + } + else + { + // Get next desired robot state + moveit::core::RobotState next_desired_goal_state = reference_trajectory_->getWayPoint(next_waypoint_index_); + + // Check if state is reached + if (next_desired_goal_state.distance(current_state, joint_group_) <= WAYPOINT_RADIAN_TOLERANCE) + { + // Update index (and thus desired robot state) + next_waypoint_index_ = std::min(next_waypoint_index_ + 1, reference_trajectory_->getWayPointCount() - 1); + } + + // Construct local trajectory containing the next global trajectory waypoint + local_trajectory.addSuffixWayPoint(reference_trajectory_->getWayPoint(next_waypoint_index_), + reference_trajectory_->getWayPointDurationFromPrevious(next_waypoint_index_)); + } + + // Return empty feedback + return feedback_; +} + +double SimpleSampler::getTrajectoryProgress(const moveit::core::RobotState& current_state) +{ + // Check if trajectory is unwinded + if (next_waypoint_index_ >= reference_trajectory_->getWayPointCount() - 1) + { + return 1.0; + } + return 0.0; +} +} // namespace moveit::hybrid_planning + +#include + +PLUGINLIB_EXPORT_CLASS(moveit::hybrid_planning::SimpleSampler, moveit::hybrid_planning::TrajectoryOperatorInterface); diff --git a/moveit_ros/hybrid_planning/moveit_planning_pipeline_plugin.xml b/moveit_ros/hybrid_planning/moveit_planning_pipeline_plugin.xml new file mode 100644 index 0000000000..1b325280bd --- /dev/null +++ b/moveit_ros/hybrid_planning/moveit_planning_pipeline_plugin.xml @@ -0,0 +1,7 @@ + + + + Use default MoveIt planning pipeline as Global Planner + + + diff --git a/moveit_ros/hybrid_planning/package.xml b/moveit_ros/hybrid_planning/package.xml new file mode 100644 index 0000000000..61ae976d28 --- /dev/null +++ b/moveit_ros/hybrid_planning/package.xml @@ -0,0 +1,51 @@ + + moveit_hybrid_planning + 2.2.1 + Hybrid planning components of MoveIt 2 + Sebastian Jahr + + MoveIt Release Team + + BSD + + http://moveit.ros.org + https://github.com/ros-planning/moveit2/issues + https://github.com/ros-planning/moveit2 + + ament_cmake + moveit_common + + ament_index_cpp + rclcpp + rclcpp_action + rclcpp_components + moveit_msgs + moveit_core + moveit_ros_planning + moveit_ros_planning_interface + pluginlib + std_msgs + std_srvs + tf2_ros + trajectory_msgs + + controller_manager + robot_state_publisher + rviz2 + moveit_resources_panda_moveit_config + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + moveit_planners_ompl + ros_testing + + + ament_cmake + + + + + + + diff --git a/moveit_ros/hybrid_planning/replan_invalidated_trajectory_plugin.xml b/moveit_ros/hybrid_planning/replan_invalidated_trajectory_plugin.xml new file mode 100644 index 0000000000..0bceaac42d --- /dev/null +++ b/moveit_ros/hybrid_planning/replan_invalidated_trajectory_plugin.xml @@ -0,0 +1,7 @@ + + + + Simple hybrid planning logic that runs the global planner once and starts executing the global solution with the local planner. In case the local planner detects a collision the global planner is rerun to update the invalidated global trajectory. + + + diff --git a/moveit_ros/hybrid_planning/simple_sampler_plugin.xml b/moveit_ros/hybrid_planning/simple_sampler_plugin.xml new file mode 100644 index 0000000000..3678ab0fef --- /dev/null +++ b/moveit_ros/hybrid_planning/simple_sampler_plugin.xml @@ -0,0 +1,7 @@ + + + + Simple trajectory operator plugin that updates the local planner's reference trajectory by simple replacing it and extracts the next trajectory point based on the current robot state and an index. + + + diff --git a/moveit_ros/hybrid_planning/single_plan_execution_plugin.xml b/moveit_ros/hybrid_planning/single_plan_execution_plugin.xml new file mode 100644 index 0000000000..847ec33f58 --- /dev/null +++ b/moveit_ros/hybrid_planning/single_plan_execution_plugin.xml @@ -0,0 +1,7 @@ + + + + Simple hybrid planning logic that runs the global planner once and execute the global solution with the local planner. + + + diff --git a/moveit_ros/hybrid_planning/test/CMakeLists.txt b/moveit_ros/hybrid_planning/test/CMakeLists.txt new file mode 100644 index 0000000000..7435522900 --- /dev/null +++ b/moveit_ros/hybrid_planning/test/CMakeLists.txt @@ -0,0 +1,3 @@ +add_executable(hybrid_planning_demo_node hybrid_planning_demo_node.cpp) +ament_target_dependencies(hybrid_planning_demo_node ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(hybrid_planning_demo_node ${LIBRARIES}) diff --git a/moveit_ros/hybrid_planning/test/config/demo_controller.yaml b/moveit_ros/hybrid_planning/test/config/demo_controller.yaml new file mode 100644 index 0000000000..f6bf462856 --- /dev/null +++ b/moveit_ros/hybrid_planning/test/config/demo_controller.yaml @@ -0,0 +1,36 @@ +controller_manager: + ros__parameters: + update_rate: 500 # Hz + panda_arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + panda_joint_group_position_controller: + type: position_controllers/JointGroupPositionController + +panda_arm_controller: + ros__parameters: + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 + command_interfaces: + - position + state_interfaces: + - position + - velocity + +panda_joint_group_position_controller: + ros__parameters: + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 diff --git a/moveit_ros/hybrid_planning/test/config/global_planner.yaml b/moveit_ros/hybrid_planning/test/config/global_planner.yaml new file mode 100644 index 0000000000..52f1b56276 --- /dev/null +++ b/moveit_ros/hybrid_planning/test/config/global_planner.yaml @@ -0,0 +1,19 @@ +global_planner_name: "moveit_hybrid_planning/MoveItPlanningPipeline" + +# The rest of these parameters are typical for moveit_cpp +planning_scene_monitor_options: + name: "planning_scene_monitor" + robot_description: "robot_description" + joint_state_topic: "/joint_states" + attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor" + publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene" + monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene" + wait_for_initial_state_timeout: 10.0 +planning_pipelines: + #namespace: "moveit_cpp" # optional, default is ~ + pipeline_names: ["ompl"] +plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 diff --git a/moveit_ros/hybrid_planning/test/config/hybrid_planning_demo.rviz b/moveit_ros/hybrid_planning/test/config/hybrid_planning_demo.rviz new file mode 100644 index 0000000000..fbb0680310 --- /dev/null +++ b/moveit_ros/hybrid_planning/test/config/hybrid_planning_demo.rviz @@ -0,0 +1,292 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PlanningScene1 + - /Trajectory1 + Splitter Ratio: 0.5 + Tree Height: 814 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: moveit_rviz_plugin/PlanningScene + Enabled: true + Move Group Namespace: "" + Name: PlanningScene + Planning Scene Topic: /planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.20000000298023224 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + - Class: moveit_rviz_plugin/Trajectory + Color Enabled: false + Enabled: true + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Name: Trajectory + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Robot Description: robot_description + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: panda_link0 + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 3.8008623123168945 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.43039798736572266 + Target Frame: + Value: Orbit (rviz) + Yaw: 6.158571720123291 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1043 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000273000003b9fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003b9000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100ffffff000000010000010f000003b9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003b9000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003aa000003b900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Trajectory - Trajectory Slider: + collapsed: false + Views: + collapsed: false + Width: 1848 + X: 72 + Y: 0 diff --git a/moveit_ros/hybrid_planning/test/config/hybrid_planning_manager.yaml b/moveit_ros/hybrid_planning/test/config/hybrid_planning_manager.yaml new file mode 100644 index 0000000000..edf1a308c8 --- /dev/null +++ b/moveit_ros/hybrid_planning/test/config/hybrid_planning_manager.yaml @@ -0,0 +1 @@ +planner_logic_plugin_name: "moveit_hybrid_planning/ReplanInvalidatedTrajectory" diff --git a/moveit_ros/hybrid_planning/test/config/local_planner.yaml b/moveit_ros/hybrid_planning/test/config/local_planner.yaml new file mode 100644 index 0000000000..db25915d98 --- /dev/null +++ b/moveit_ros/hybrid_planning/test/config/local_planner.yaml @@ -0,0 +1,16 @@ +robot_description: "robot_description" +trajectory_operator_plugin_name: "moveit_hybrid_planning/SimpleSampler" +local_constraint_solver_plugin_name: "moveit_hybrid_planning/ForwardTrajectory" +local_planning_frequency: 100.0 +global_solution_topic: "global_trajectory" +local_solution_topic: "/panda_joint_group_position_controller/commands" # or panda_arm_controller/joint_trajectory +local_solution_topic_type: "std_msgs/Float64MultiArray" # or trajectory_msgs/JointTrajectory +publish_joint_positions: true +publish_joint_velocities: false +group_name: "panda_arm" + +# SimpleSampler param +pass_through: false + +# ForwardTrajectory param +stop_before_collision: true diff --git a/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp b/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp new file mode 100644 index 0000000000..e73acc3790 --- /dev/null +++ b/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp @@ -0,0 +1,284 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Sebastian Jahr + Description: A demonstration that re-plans around a moving box. + */ + +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace std::chrono_literals; +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("test_hybrid_planning_client"); +} + +class HybridPlanningDemo +{ +public: + HybridPlanningDemo(const rclcpp::Node::SharedPtr& node) + { + node_ = node; + hp_action_client_ = rclcpp_action::create_client(node_, "run_hybrid_planning"), + robot_state_publisher_ = node_->create_publisher("display_robot_state", 1); + + collision_object_1_.header.frame_id = "panda_link0"; + collision_object_1_.id = "box1"; + + collision_object_2_.header.frame_id = "panda_link0"; + collision_object_2_.id = "box2"; + + collision_object_3_.header.frame_id = "panda_link0"; + collision_object_3_.id = "box3"; + + box_1_.type = box_1_.BOX; + box_1_.dimensions = { 0.5, 0.8, 0.01 }; + + box_2_.type = box_2_.BOX; + box_2_.dimensions = { 1.0, 0.4, 0.01 }; + + // Add new collision object as soon as global trajectory is available. + global_solution_subscriber_ = node_->create_subscription( + "global_trajectory", rclcpp::SystemDefaultsQoS(), + [this](const moveit_msgs::msg::MotionPlanResponse::SharedPtr msg) { + // Remove old collision objects + collision_object_1_.operation = collision_object_1_.REMOVE; + + // Add new collision objects + geometry_msgs::msg::Pose box_pose_2; + box_pose_2.position.x = 0.2; + box_pose_2.position.y = 0.4; + box_pose_2.position.z = 0.95; + + collision_object_2_.primitives.push_back(box_2_); + collision_object_2_.primitive_poses.push_back(box_pose_2); + collision_object_2_.operation = collision_object_2_.ADD; + + geometry_msgs::msg::Pose box_pose_3; + box_pose_3.position.x = 0.2; + box_pose_3.position.y = -0.4; + box_pose_3.position.z = 0.95; + + collision_object_3_.primitives.push_back(box_2_); + collision_object_3_.primitive_poses.push_back(box_pose_3); + collision_object_3_.operation = collision_object_3_.ADD; + + // Add object to planning scene + { // Lock PlanningScene + planning_scene_monitor::LockedPlanningSceneRW scene(planning_scene_monitor_); + scene->processCollisionObjectMsg(collision_object_2_); + scene->processCollisionObjectMsg(collision_object_3_); + scene->processCollisionObjectMsg(collision_object_1_); + } // Unlock PlanningScene + }); + } + + void run() + { + RCLCPP_INFO(LOGGER, "Initialize Planning Scene Monitor"); + tf_buffer_ = std::make_shared(node_->get_clock()); + + planning_scene_monitor_ = std::make_shared( + node_, "robot_description", tf_buffer_, "planning_scene_monitor"); + if (!planning_scene_monitor_->getPlanningScene()) + { + RCLCPP_ERROR(LOGGER, "The planning scene was not retrieved!"); + return; + } + else + { + planning_scene_monitor_->startStateMonitor(); + planning_scene_monitor_->providePlanningSceneService(); // let RViz display query PlanningScene + planning_scene_monitor_->setPlanningScenePublishingFrequency(100); + planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, + "/planning_scene"); + planning_scene_monitor_->startSceneMonitor(); + } + + if (!planning_scene_monitor_->waitForCurrentRobotState(node_->now(), 5)) + { + RCLCPP_ERROR(LOGGER, "Timeout when waiting for /joint_states updates. Is the robot running?"); + return; + } + + if (!hp_action_client_->wait_for_action_server(20s)) + { + RCLCPP_ERROR(LOGGER, "Hybrid planning action server not available after waiting"); + return; + } + + geometry_msgs::msg::Pose box_pose; + box_pose.position.x = 0.4; + box_pose.position.y = 0.0; + box_pose.position.z = 0.85; + + collision_object_1_.primitives.push_back(box_1_); + collision_object_1_.primitive_poses.push_back(box_pose); + collision_object_1_.operation = collision_object_1_.ADD; + + // Add object to planning scene + { // Lock PlanningScene + planning_scene_monitor::LockedPlanningSceneRW scene(planning_scene_monitor_); + scene->processCollisionObjectMsg(collision_object_1_); + } // Unlock PlanningScene + + RCLCPP_INFO(LOGGER, "Wait 2s for the collision object"); + rclcpp::sleep_for(2s); + + // Setup motion planning goal taken from motion_planning_api tutorial + const std::string planning_group = "panda_arm"; + robot_model_loader::RobotModelLoader robot_model_loader(node_, "robot_description"); + const moveit::core::RobotModelPtr& robot_model = robot_model_loader.getModel(); + + // Create a RobotState and JointModelGroup + const auto robot_state = std::make_shared(robot_model); + const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(planning_group); + + // Configure a valid robot state + robot_state->setToDefaultValues(joint_model_group, "ready"); + + // Create desired motion goal + moveit_msgs::msg::MotionPlanRequest goal_motion_request; + + moveit::core::robotStateToRobotStateMsg(*robot_state, goal_motion_request.start_state); + goal_motion_request.group_name = planning_group; + goal_motion_request.num_planning_attempts = 10; + goal_motion_request.max_velocity_scaling_factor = 0.1; + goal_motion_request.max_acceleration_scaling_factor = 0.1; + goal_motion_request.allowed_planning_time = 2.0; + goal_motion_request.planner_id = "ompl"; + goal_motion_request.pipeline_id = "ompl"; + + moveit::core::RobotState goal_state(robot_model); + std::vector joint_values = { 0.0, 0.0, 0.0, 0.0, 0.0, 1.571, 0.785 }; + goal_state.setJointGroupPositions(joint_model_group, joint_values); + + goal_motion_request.goal_constraints.resize(1); + goal_motion_request.goal_constraints[0] = + kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group); + + // Create Hybrid Planning action request + moveit_msgs::msg::MotionSequenceItem sequence_item; + sequence_item.req = goal_motion_request; + sequence_item.blend_radius = 0.0; // Single goal + + moveit_msgs::msg::MotionSequenceRequest sequence_request; + sequence_request.items.push_back(sequence_item); + + auto goal_action_request = moveit_msgs::action::HybridPlanner::Goal(); + goal_action_request.planning_group = planning_group; + goal_action_request.motion_sequence = sequence_request; + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.result_callback = + [](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { + switch (result.code) + { + case rclcpp_action::ResultCode::SUCCEEDED: + RCLCPP_INFO(LOGGER, "Hybrid planning goal succeeded"); + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(LOGGER, "Hybrid planning goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(LOGGER, "Hybrid planning goal was canceled"); + return; + default: + RCLCPP_ERROR(LOGGER, "Unknown hybrid planning result code"); + return; + RCLCPP_INFO(LOGGER, "Hybrid planning result received"); + } + }; + send_goal_options.feedback_callback = + [](rclcpp_action::ClientGoalHandle::SharedPtr /*unused*/, + const std::shared_ptr feedback) { + RCLCPP_INFO(LOGGER, feedback->feedback.c_str()); + }; + + RCLCPP_INFO(LOGGER, "Sending hybrid planning goal"); + // Ask server to achieve some goal and wait until it's accepted + auto goal_handle_future = hp_action_client_->async_send_goal(goal_action_request, send_goal_options); + } + +private: + rclcpp::Node::SharedPtr node_; + rclcpp_action::Client::SharedPtr hp_action_client_; + rclcpp::Publisher::SharedPtr robot_state_publisher_; + rclcpp::Subscription::SharedPtr global_solution_subscriber_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + rclcpp::TimerBase::SharedPtr timer_; + + moveit_msgs::msg::CollisionObject collision_object_1_; + moveit_msgs::msg::CollisionObject collision_object_2_; + moveit_msgs::msg::CollisionObject collision_object_3_; + shape_msgs::msg::SolidPrimitive box_1_; + shape_msgs::msg::SolidPrimitive box_2_; + + // TF + std::shared_ptr tf_buffer_; +}; + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + + rclcpp::Node::SharedPtr node = std::make_shared("hybrid_planning_test_node", "", node_options); + + HybridPlanningDemo demo(node); + std::thread run_demo([&demo]() { + rclcpp::sleep_for(5s); + demo.run(); + }); + + rclcpp::spin(node); + run_demo.join(); + rclcpp::shutdown(); + return 0; +} diff --git a/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py b/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py new file mode 100644 index 0000000000..c9eb8b0939 --- /dev/null +++ b/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py @@ -0,0 +1,211 @@ +# Return a list of nodes we commonly launch for the demo. Nice for testing use. +import launch +import os +import xacro +import yaml + +from ament_index_python.packages import get_package_share_directory +from launch.actions import ExecuteProcess +from launch_ros.actions import Node +from launch_ros.descriptions import ComposableNode +from launch_ros.actions import ComposableNodeContainer + + +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_common_hybrid_launch_description(): + # Component yaml files are grouped in separate namespaces + robot_description_config = xacro.process_file( + os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "panda.urdf.xacro", + ) + ) + robot_description = {"robot_description": robot_description_config.toxml()} + + robot_description_semantic_config = load_file( + "moveit_resources_panda_moveit_config", "config/panda.srdf" + ) + robot_description_semantic = { + "robot_description_semantic": robot_description_semantic_config + } + + kinematics_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/kinematics.yaml" + ) + + # The global planner uses the typical OMPL parameters + planning_pipelines_config = { + "ompl": { + "planning_plugin": "ompl_interface/OMPLPlanner", + "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", + "start_state_max_bounds_error": 0.1, + } + } + ompl_planning_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/ompl_planning.yaml" + ) + planning_pipelines_config["ompl"].update(ompl_planning_yaml) + + moveit_simple_controllers_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/panda_controllers.yaml" + ) + moveit_controllers = { + "moveit_simple_controller_manager": moveit_simple_controllers_yaml, + "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", + } + + # Any parameters that are unique to your plugins go here + global_planner_param = load_yaml( + "moveit_hybrid_planning", "config/global_planner.yaml" + ) + local_planner_param = load_yaml( + "moveit_hybrid_planning", "config/local_planner.yaml" + ) + hybrid_planning_manager_param = load_yaml( + "moveit_hybrid_planning", "config/hybrid_planning_manager.yaml" + ) + + # Generate launch description with multiple components + container = ComposableNodeContainer( + name="hybrid_planning_container", + namespace="/", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + ComposableNode( + package="moveit_hybrid_planning", + plugin="moveit::hybrid_planning::GlobalPlannerComponent", + name="global_planner", + parameters=[ + global_planner_param, + robot_description, + robot_description_semantic, + kinematics_yaml, + planning_pipelines_config, + moveit_controllers, + ], + ), + ComposableNode( + package="moveit_hybrid_planning", + plugin="moveit::hybrid_planning::LocalPlannerComponent", + name="local_planner", + parameters=[ + local_planner_param, + robot_description, + robot_description_semantic, + kinematics_yaml, + ], + ), + ComposableNode( + package="moveit_hybrid_planning", + plugin="moveit::hybrid_planning::HybridPlanningManager", + name="hybrid_planning_manager", + parameters=[hybrid_planning_manager_param], + ), + ], + output="screen", + ) + + # RViz + rviz_config_file = ( + get_package_share_directory("moveit_hybrid_planning") + + "/config/hybrid_planning_demo.rviz" + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[robot_description, robot_description_semantic], + ) + + # Static TF + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], + ) + + # Publish TF + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_hybrid_planning"), + "config", + "demo_controller.yaml", + ) + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, ros2_controllers_path], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + + # Load controllers + load_controllers = [] + for controller in [ + "joint_state_broadcaster", + "panda_joint_group_position_controller", + ]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner {}".format(controller)], + shell=True, + output="screen", + ) + ] + + # Demo node + demo_node = Node( + package="moveit_hybrid_planning", + executable="hybrid_planning_demo_node", + name="hybrid_planning_demo_node", + output="screen", + parameters=[robot_description, robot_description_semantic], + ) + + launched_nodes = [ + container, + static_tf, + rviz_node, + robot_state_publisher, + demo_node, + ros2_control_node, + ] + load_controllers + + return launched_nodes diff --git a/moveit_ros/hybrid_planning/test/launch/hybrid_planning_demo.launch.py b/moveit_ros/hybrid_planning/test/launch/hybrid_planning_demo.launch.py new file mode 100644 index 0000000000..bbdc881601 --- /dev/null +++ b/moveit_ros/hybrid_planning/test/launch/hybrid_planning_demo.launch.py @@ -0,0 +1,11 @@ +import launch +import os +import sys + +sys.path.append(os.path.dirname(__file__)) +from hybrid_planning_common import generate_common_hybrid_launch_description + + +def generate_launch_description(): + # generate_common_hybrid_launch_description() returns a list of nodes to launch + return launch.LaunchDescription(generate_common_hybrid_launch_description()) diff --git a/moveit_ros/hybrid_planning/test/launch/test_basic_launching.test.py b/moveit_ros/hybrid_planning/test/launch/test_basic_launching.test.py new file mode 100644 index 0000000000..927a2e31a7 --- /dev/null +++ b/moveit_ros/hybrid_planning/test/launch/test_basic_launching.test.py @@ -0,0 +1,73 @@ +# Simply check if the components stay alive after launching +import os +import sys +import time +import unittest + +import launch +import launch.actions +import launch_ros.actions +import launch_testing.actions +import launch_testing.markers +import pytest +import rclpy +from rclpy.node import Node + +sys.path.append(os.path.dirname(__file__)) +from hybrid_planning_common import generate_common_hybrid_launch_description + + +@pytest.mark.launch_test +@launch_testing.markers.keep_alive +def generate_test_description(): + path_to_test = os.path.dirname(__file__) + + # Add the usual demo nodes + ld = launch.LaunchDescription(generate_common_hybrid_launch_description()) + # Python testing requires this marker + ld.add_action(launch_testing.actions.ReadyToTest()) + return ld + + +class TestFixture(unittest.TestCase): + def test_startup(self, proc_output): + rclpy.init() + + # Wait several seconds, allowing any nodes to die + time.sleep(2) + + expected_nodes = [ + "controller_manager", + "global_planner", + "hybrid_planning_container", + "hybrid_planning_demo_node", + "joint_state_broadcaster", + "local_planner", + "robot_state_publisher", + ] + + node = MakeTestNode("test_node") + + try: + for node_name in expected_nodes: + assert node.wait_for_node(node_name, 1.0), ( + "Expected hybrid_planning node not found! Missing node: " + + node_name + ) + finally: + rclpy.shutdown() + + +class MakeTestNode(Node): + def __init__(self, name="test_node"): + super().__init__(name) + + def wait_for_node(self, node_name, timeout=8.0): + start = time.time() + flag = False + print("Waiting for node...") + while time.time() - start < timeout and not flag: + flag = node_name in self.get_node_names() + time.sleep(0.1) + + return flag diff --git a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp index 7bf92bd261..709a9cb8fc 100644 --- a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp +++ b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp @@ -46,21 +46,21 @@ move_group::MoveGroupPickPlaceAction::MoveGroupPickPlaceAction() void move_group::MoveGroupPickPlaceAction::initialize() { - pick_place_.reset(new pick_place::PickPlace(context_->planning_pipeline_)); + pick_place_ = std::make_shared(context_->planning_pipeline_); pick_place_->displayComputedMotionPlans(true); if (context_->debug_) pick_place_->displayProcessedGrasps(true); // start the pickup action server - pickup_action_server_.reset(new actionlib::SimpleActionServer( - root_node_handle_, PICKUP_ACTION, boost::bind(&MoveGroupPickPlaceAction::executePickupCallback, this, _1), false)); + pickup_action_server_ = std::make_unique>( + root_node_handle_, PICKUP_ACTION, boost::bind(&MoveGroupPickPlaceAction::executePickupCallback, this, _1), false); pickup_action_server_->registerPreemptCallback(boost::bind(&MoveGroupPickPlaceAction::preemptPickupCallback, this)); pickup_action_server_->start(); // start the place action server - place_action_server_.reset(new actionlib::SimpleActionServer( - root_node_handle_, PLACE_ACTION, boost::bind(&MoveGroupPickPlaceAction::executePlaceCallback, this, _1), false)); + place_action_server_ = std::make_unique>( + root_node_handle_, PLACE_ACTION, boost::bind(&MoveGroupPickPlaceAction::executePlaceCallback, this, _1), false); place_action_server_->registerPreemptCallback(boost::bind(&MoveGroupPickPlaceAction::preemptPlaceCallback, this)); place_action_server_->start(); } @@ -318,7 +318,8 @@ void move_group::MoveGroupPickPlaceAction::executePickupCallback( setPickupState(PLANNING); // before we start planning, ensure that we have the latest robot state received... - context_->planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now()); + auto node = context_->moveit_cpp_->getNode(); + context_->planning_scene_monitor_->waitForCurrentRobotState(node->get_clock()->now()); context_->planning_scene_monitor_->updateFrameTransforms(); moveit_msgs::action::PickupGoalConstPtr goal; @@ -365,7 +366,8 @@ void move_group::MoveGroupPickPlaceAction::executePlaceCallback(const moveit_msg setPlaceState(PLANNING); // before we start planning, ensure that we have the latest robot state received... - context_->planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now()); + auto node = context_->moveit_cpp_->getNode(); + context_->planning_scene_monitor_->waitForCurrentRobotState(node->get_clock()->now()); context_->planning_scene_monitor_->updateFrameTransforms(); moveit_msgs::action::PlaceResult action_res; diff --git a/moveit_ros/manipulation/package.xml b/moveit_ros/manipulation/package.xml index 54fc25b486..73f270b240 100644 --- a/moveit_ros/manipulation/package.xml +++ b/moveit_ros/manipulation/package.xml @@ -1,13 +1,12 @@ - + + + moveit_ros_manipulation - 1.1.5 + 1.1.6 Components of MoveIt used for manipulation - - Ioan Sucan - Sachin Chitta - Michael Görner - Michael Ferguson + Henning Kayser + Tyler Weaver MoveIt Release Team BSD @@ -16,6 +15,9 @@ https://github.com/ros-planning/moveit2/issues https://github.com/ros-planning/moveit2 + Ioan Sucan + Sachin Chitta + catkin actionlib diff --git a/moveit_ros/manipulation/pick_place/src/pick_place.cpp b/moveit_ros/manipulation/pick_place/src/pick_place.cpp index 45a34aed4c..57fc48971f 100644 --- a/moveit_ros/manipulation/pick_place/src/pick_place.cpp +++ b/moveit_ros/manipulation/pick_place/src/pick_place.cpp @@ -92,7 +92,8 @@ void PickPlacePlanBase::waitForPipeline(const ros::WallTime& endtime) PickPlace::PickPlace(const planning_pipeline::PlanningPipelinePtr& planning_pipeline) : nh_("~"), planning_pipeline_(planning_pipeline), display_computed_motion_plans_(false), display_grasps_(false) { - constraint_sampler_manager_loader_.reset(new constraint_sampler_manager_loader::ConstraintSamplerManagerLoader()); + constraint_sampler_manager_loader_ = + std::make_shared(); } void PickPlace::displayProcessedGrasps(bool flag) diff --git a/moveit_ros/manipulation/pick_place/src/place.cpp b/moveit_ros/manipulation/pick_place/src/place.cpp index 52d16e3d82..79fddddf04 100644 --- a/moveit_ros/manipulation/pick_place/src/place.cpp +++ b/moveit_ros/manipulation/pick_place/src/place.cpp @@ -72,7 +72,7 @@ struct OrderPlaceLocationQuality bool transformToEndEffectorGoal(const geometry_msgs::PoseStamped& goal_pose, const moveit::core::AttachedBody* attached_body, geometry_msgs::PoseStamped& place_pose) { - const EigenSTL::vector_Isometry3d& fixed_transforms = attached_body->getFixedTransforms(); + const EigenSTL::vector_Isometry3d& fixed_transforms = attached_body->getShapePosesInLinkFrame(); if (fixed_transforms.empty()) return false; diff --git a/moveit_ros/move_group/package.xml b/moveit_ros/move_group/package.xml index 3b2e69406d..afa3b371d0 100644 --- a/moveit_ros/move_group/package.xml +++ b/moveit_ros/move_group/package.xml @@ -1,12 +1,12 @@ - + + + moveit_ros_move_group 2.3.0 The move_group node for MoveIt - Ioan Sucan - Sachin Chitta - - Michael Ferguson Michael Görner + Henning Kayser + Tyler Weaver MoveIt Release Team BSD @@ -15,6 +15,9 @@ https://github.com/ros-planning/moveit2/issues https://github.com/ros-planning/moveit2 + Ioan Sucan + Sachin Chitta + ament_cmake moveit_common diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp index 4cc83f6fd4..8885d1b205 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp @@ -147,8 +147,8 @@ bool MoveGroupCartesianPathService::computeService(const std::shared_ptr kset; if (req->avoid_collisions || !moveit::core::isEmpty(req->path_constraints)) { - ls.reset(new planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_)); - kset.reset(new kinematic_constraints::KinematicConstraintSet((*ls)->getRobotModel())); + ls = std::make_unique(context_->planning_scene_monitor_); + kset = std::make_unique((*ls)->getRobotModel()); kset->add(req->path_constraints, (*ls)->getTransforms()); constraint_fn = boost::bind( &isStateValid, diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp index 91ac097fd8..8ce7c61fe6 100644 --- a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp @@ -80,7 +80,8 @@ void MoveGroupMoveAction::executeMoveCallback(std::shared_ptr goal RCLCPP_INFO(LOGGER, "executing.."); setMoveState(PLANNING, goal); // before we start planning, ensure that we have the latest robot state received... - context_->planning_scene_monitor_->waitForCurrentRobotState(rclcpp::Clock(RCL_ROS_TIME).now()); + auto node = context_->moveit_cpp_->getNode(); + context_->planning_scene_monitor_->waitForCurrentRobotState(node->get_clock()->now()); context_->planning_scene_monitor_->updateFrameTransforms(); auto action_res = std::make_shared(); diff --git a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp index b1d8888bda..56f8594064 100644 --- a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp @@ -65,7 +65,7 @@ TfPublisher::~TfPublisher() namespace { void publishSubframes(tf2_ros::TransformBroadcaster& broadcaster, const moveit::core::FixedTransformsMap& subframes, - const std::string& parent_object, const std::string& parent_frame, const rclcpp::Time& stamp) + const std::string& parent_object, const rclcpp::Time& stamp) { geometry_msgs::msg::TransformStamped transform; for (auto& subframe : subframes) @@ -73,7 +73,7 @@ void publishSubframes(tf2_ros::TransformBroadcaster& broadcaster, const moveit:: transform = tf2::eigenToTransform(subframe.second); transform.child_frame_id = parent_object + "/" + subframe.first; transform.header.stamp = stamp; - transform.header.frame_id = parent_frame; + transform.header.frame_id = parent_object; broadcaster.sendTransform(transform); } } @@ -96,14 +96,14 @@ void TfPublisher::publishPlanningSceneFrames() for (const auto& obj : *world) { std::string object_frame = prefix_ + obj.second->id_; - transform = tf2::eigenToTransform(obj.second->shape_poses_[0]); + transform = tf2::eigenToTransform(obj.second->pose_); transform.child_frame_id = object_frame; transform.header.stamp = stamp; transform.header.frame_id = planning_frame; broadcaster.sendTransform(transform); const moveit::core::FixedTransformsMap& subframes = obj.second->subframe_poses_; - publishSubframes(broadcaster, subframes, object_frame, planning_frame, stamp); + publishSubframes(broadcaster, subframes, object_frame, stamp); } const moveit::core::RobotState& rs = locked_planning_scene->getCurrentState(); @@ -112,14 +112,14 @@ void TfPublisher::publishPlanningSceneFrames() for (const moveit::core::AttachedBody* attached_body : attached_collision_objects) { std::string object_frame = prefix_ + attached_body->getName(); - transform = tf2::eigenToTransform(attached_body->getFixedTransforms()[0]); + transform = tf2::eigenToTransform(attached_body->getPose()); transform.child_frame_id = object_frame; transform.header.stamp = stamp; transform.header.frame_id = attached_body->getAttachedLinkName(); broadcaster.sendTransform(transform); - const moveit::core::FixedTransformsMap& subframes = attached_body->getSubframeTransforms(); - publishSubframes(broadcaster, subframes, object_frame, attached_body->getAttachedLinkName(), stamp); + const moveit::core::FixedTransformsMap& subframes = attached_body->getSubframes(); + publishSubframes(broadcaster, subframes, object_frame, stamp); } } diff --git a/moveit_ros/move_group/src/move_group.cpp b/moveit_ros/move_group/src/move_group.cpp index e8bd94e40c..c9baaf5674 100644 --- a/moveit_ros/move_group/src/move_group.cpp +++ b/moveit_ros/move_group/src/move_group.cpp @@ -121,8 +121,8 @@ class MoveGroupExe { try { - capability_plugin_loader_.reset( - new pluginlib::ClassLoader("moveit_ros_move_group", "move_group::MoveGroupCapability")); + capability_plugin_loader_ = std::make_shared>( + "moveit_ros_move_group", "move_group::MoveGroupCapability"); } catch (pluginlib::PluginlibException& ex) { @@ -141,7 +141,7 @@ class MoveGroupExe if (context_->moveit_cpp_->getNode()->get_parameter("capabilities", capability_plugins)) { boost::char_separator sep(" "); - boost::tokenizer > tok(capability_plugins, sep); + boost::tokenizer> tok(capability_plugins, sep); capabilities.insert(tok.begin(), tok.end()); } @@ -154,7 +154,7 @@ class MoveGroupExe pipeline_capabilities)) { boost::char_separator sep(" "); - boost::tokenizer > tok(pipeline_capabilities, sep); + boost::tokenizer> tok(pipeline_capabilities, sep); capabilities.insert(tok.begin(), tok.end()); } } @@ -163,8 +163,8 @@ class MoveGroupExe if (context_->moveit_cpp_->getNode()->get_parameter("disable_capabilities", capability_plugins)) { boost::char_separator sep(" "); - boost::tokenizer > tok(capability_plugins, sep); - for (boost::tokenizer >::iterator cap_name = tok.begin(); cap_name != tok.end(); + boost::tokenizer> tok(capability_plugins, sep); + for (boost::tokenizer>::iterator cap_name = tok.begin(); cap_name != tok.end(); ++cap_name) capabilities.erase(*cap_name); } @@ -198,7 +198,7 @@ class MoveGroupExe } MoveGroupContextPtr context_; - std::shared_ptr > capability_plugin_loader_; + std::shared_ptr> capability_plugin_loader_; std::vector capabilities_; }; } // namespace move_group diff --git a/moveit_ros/move_group/src/move_group_context.cpp b/moveit_ros/move_group/src/move_group_context.cpp index d2e9c70ba1..591631eb8e 100644 --- a/moveit_ros/move_group/src/move_group_context.cpp +++ b/moveit_ros/move_group/src/move_group_context.cpp @@ -76,9 +76,10 @@ move_group::MoveGroupContext::MoveGroupContext(const moveit_cpp::MoveItCppPtr& m if (allow_trajectory_execution_) { trajectory_execution_manager_ = moveit_cpp_->getTrajectoryExecutionManager(); - plan_execution_.reset(new plan_execution::PlanExecution(moveit_cpp_->getNode(), planning_scene_monitor_, - trajectory_execution_manager_)); - plan_with_sensing_.reset(new plan_execution::PlanWithSensing(moveit_cpp_->getNode(), trajectory_execution_manager_)); + plan_execution_ = std::make_shared(moveit_cpp_->getNode(), planning_scene_monitor_, + trajectory_execution_manager_); + plan_with_sensing_ = + std::make_shared(moveit_cpp_->getNode(), trajectory_execution_manager_); if (debug) plan_with_sensing_->displayCostSources(true); } diff --git a/moveit_ros/moveit_ros/package.xml b/moveit_ros/moveit_ros/package.xml index 6dcf7fae42..f6014941a9 100644 --- a/moveit_ros/moveit_ros/package.xml +++ b/moveit_ros/moveit_ros/package.xml @@ -1,15 +1,12 @@ + moveit_ros 2.3.0 Components of MoveIt that use ROS - Ioan Sucan - Sachin Chitta - Acorn Pooley - - Michael Ferguson Michael Görner - Dave Coleman + Henning Kayser + Tyler Weaver MoveIt Release Team BSD @@ -18,6 +15,11 @@ https://github.com/ros-planning/moveit2/issues https://github.com/ros-planning/moveit2 + Ioan Sucan + Sachin Chitta + Acorn Pooley + Dave Coleman + ament_cmake - - + + - + - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml index 1b06a3a55c..2e2561f8bd 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml @@ -6,7 +6,7 @@ - - + + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml index 82a5a48e6d..b6bf529185 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml @@ -5,7 +5,7 @@ - + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml index 59470ce087..484d166ac7 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml @@ -11,13 +11,13 @@ - - + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template b/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template index dc5691d6a3..d52f3a9f5f 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template @@ -28,6 +28,10 @@ rviz tf2_ros xacro + + + [OTHER_DEPENDENCIES] diff --git a/moveit_setup_assistant/test/moveit_config_data_test.cpp b/moveit_setup_assistant/test/moveit_config_data_test.cpp index 3a68763677..f89316060f 100644 --- a/moveit_setup_assistant/test/moveit_config_data_test.cpp +++ b/moveit_setup_assistant/test/moveit_config_data_test.cpp @@ -69,7 +69,7 @@ TEST_F(MoveItConfigData, ReadingControllers) // Contains all the configuration data for the setup assistant moveit_setup_assistant::MoveItConfigDataPtr config_data; - config_data.reset(new moveit_setup_assistant::MoveItConfigData()); + config_data = std::make_shared(); config_data->srdf_->srdf_model_ = srdf_model_; config_data->setRobotModel(robot_model_); @@ -92,7 +92,7 @@ TEST_F(MoveItConfigData, ReadingControllers) EXPECT_EQ(config_data->outputROSControllersYAML(test_file), true); // Reset MoveIt config MoveItConfigData - config_data.reset(new moveit_setup_assistant::MoveItConfigData()); + config_data = std::make_shared(); // Initially no controllers EXPECT_EQ(config_data->getROSControllers().size(), 0u); @@ -112,7 +112,7 @@ TEST_F(MoveItConfigData, ReadingSensorsConfig) { // Contains all the config data for the setup assistant moveit_setup_assistant::MoveItConfigDataPtr config_data; - config_data.reset(new moveit_setup_assistant::MoveItConfigData()); + config_data = std::make_shared(); boost::filesystem::path setup_assistant_path(config_data->setup_assistant_path_); @@ -139,7 +139,7 @@ TEST_F(MoveItConfigData, WritingSensorsConfig) { // Contains all the config data for the setup assistant moveit_setup_assistant::MoveItConfigDataPtr config_data; - config_data.reset(new moveit_setup_assistant::MoveItConfigData()); + config_data = std::make_shared(); // Empty Config Should have No Sensors EXPECT_EQ(config_data->getSensorPluginConfig().size(), 0u); @@ -156,20 +156,20 @@ TEST_F(MoveItConfigData, WritingSensorsConfig) (setup_assistant_path / "templates/moveit_config_pkg_template/config/sensors_3d.yaml").string(); // Read from the written file - config_data.reset(new moveit_setup_assistant::MoveItConfigData()); + config_data = std::make_shared(); EXPECT_EQ(config_data->input3DSensorsYAML(test_file), true); // Should still have No Sensors EXPECT_EQ(config_data->getSensorPluginConfig().size(), 0u); // Now load the default file and write it to a file - config_data.reset(new moveit_setup_assistant::MoveItConfigData()); + config_data = std::make_shared(); EXPECT_EQ(config_data->input3DSensorsYAML(default_file_path), true); EXPECT_EQ(config_data->getSensorPluginConfig().size(), 2u); EXPECT_EQ(config_data->output3DSensorPluginYAML(test_file), true); // Read from the written file - config_data.reset(new moveit_setup_assistant::MoveItConfigData()); + config_data = std::make_shared(); EXPECT_EQ(config_data->input3DSensorsYAML(test_file), true); // Should now have two sensors