diff --git a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt index e7fcaa351b..c602553fcc 100644 --- a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt +++ b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt @@ -10,6 +10,7 @@ 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(generate_parameter_library REQUIRED) find_package(geometry_msgs REQUIRED) find_package(moveit_core REQUIRED) find_package(moveit_msgs REQUIRED) @@ -62,6 +63,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS Eigen3 eigen3_cmake_module # joint_limits_interface + generate_parameter_library geometry_msgs moveit_core moveit_msgs @@ -85,9 +87,15 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ## Libraries ## ############### +generate_parameter_library( + cartesian_limits_parameters # cmake target name for the parameter library + src/cartesian_limits_parameters.yaml # path to input yaml file +) + add_library(planning_context_loader_base SHARED src/planning_context_loader.cpp ) +target_link_libraries(planning_context_loader_base cartesian_limits_parameters) ament_target_dependencies(planning_context_loader_base ${THIS_PACKAGE_INCLUDE_DEPENDS}) add_library(joint_limits_common SHARED @@ -95,9 +103,8 @@ add_library(joint_limits_common SHARED src/joint_limits_container.cpp src/joint_limits_validator.cpp src/limits_container.cpp - src/cartesian_limit.cpp - src/cartesian_limits_aggregator.cpp ) +target_link_libraries(joint_limits_common cartesian_limits_parameters) ament_target_dependencies(joint_limits_common ${THIS_PACKAGE_INCLUDE_DEPENDS}) add_library(trajectory_generation_common SHARED @@ -105,6 +112,7 @@ add_library(trajectory_generation_common SHARED src/trajectory_generator.cpp src/trajectory_blender_transition_window.cpp ) +target_link_libraries(trajectory_generation_common cartesian_limits_parameters) ament_target_dependencies(trajectory_generation_common ${THIS_PACKAGE_INCLUDE_DEPENDS}) add_library(command_list_manager SHARED @@ -169,6 +177,7 @@ pluginlib_export_plugin_description_file(pilz_industrial_motion_planner plugins/ install( TARGETS ${PROJECT_NAME} + cartesian_limits_parameters joint_limits_common planning_context_loader_base planning_context_loader_ptp diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_limit.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_limit.h deleted file mode 100644 index 199791f76c..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_limit.h +++ /dev/null @@ -1,159 +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. - *********************************************************************/ - -#pragma once - -namespace pilz_industrial_motion_planner -{ -/** - * @brief Set of cartesian limits, has values for velocity, acceleration and - * deceleration of both the - * translational and rotational part. - */ -class CartesianLimit -{ -public: - CartesianLimit(); - - // Translational Velocity Limit - - /** - * @brief Check if translational velocity limit is set. - * @return True if limit was set, false otherwise - */ - bool hasMaxTranslationalVelocity() const; - - /** - * @brief Set the maximal translational velocity - * @param Maximum translational velocity [m/s] - */ - void setMaxTranslationalVelocity(double max_trans_vel); - - /** - * @brief Return the maximal translational velocity [m/s], 0 if nothing was - * set - * @return Maximum translational velocity, 0 if nothing was set - */ - double getMaxTranslationalVelocity() const; - - // Translational Acceleration Limit - - /** - * @brief Check if translational acceleration limit is set. - * @return True if limit was set false otherwise - */ - bool hasMaxTranslationalAcceleration() const; - - /** - * @brief Set the maximum translational acceleration - * @param Maximum translational acceleration [m/s^2] - */ - void setMaxTranslationalAcceleration(double max_trans_acc); - - /** - * @brief Return the maximal translational acceleration [m/s^2], 0 if nothing - * was set - * @return maximal translational acceleration, 0 if nothing was set - */ - double getMaxTranslationalAcceleration() const; - - // Translational Deceleration Limit - - /** - * @brief Check if translational deceleration limit is set. - * @return True if limit was set false otherwise - */ - bool hasMaxTranslationalDeceleration() const; - - /** - * @brief Set the maximum translational deceleration - * @param Maximum translational deceleration, always <=0 [m/s^2] - */ - void setMaxTranslationalDeceleration(double max_trans_dec); - - /** - * @brief Return the maximal translational deceleration [m/s^2], 0 if nothing - * was set - * @return maximal translational deceleration, 0 if nothing was set, always - * <=0 [m/s^2] - */ - double getMaxTranslationalDeceleration() const; - - // Rotational Velocity Limit - - /** - * @brief Check if rotational velocity limit is set. - * @return True if limit was set false otherwise - */ - bool hasMaxRotationalVelocity() const; - - /** - * @brief Set the maximum rotational velocity - * @param Maximum rotational velocity [rad/s] - */ - void setMaxRotationalVelocity(double max_rot_vel); - - /** - * @brief Return the maximal rotational velocity [rad/s], 0 if nothing was set - * @return maximal rotational velocity, 0 if nothing was set - */ - double getMaxRotationalVelocity() const; - -private: - /// Flag if a maximum translational velocity was set - bool has_max_trans_vel_; - - /// Maximum translational velocity [m/s] - double max_trans_vel_; - - /// Flag if a maximum translational acceleration was set - bool has_max_trans_acc_; - - /// Maximum translational acceleration [m/s^2] - double max_trans_acc_; - - /// Flag if a maximum translational deceleration was set - bool has_max_trans_dec_; - - /// Maximum translational deceleration, always <=0 [m/s^2] - double max_trans_dec_; - - /// Flag if a maximum rotational velocity was set - bool has_max_rot_vel_; - - /// Maximum rotational velocity [rad/s] - double max_rot_vel_; -}; - -} // namespace pilz_industrial_motion_planner 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 deleted file mode 100644 index 1d7fe4a096..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_limits_aggregator.h +++ /dev/null @@ -1,67 +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. - *********************************************************************/ - -#pragma once - -#include -#include "pilz_industrial_motion_planner/cartesian_limit.h" - -namespace pilz_industrial_motion_planner -{ -/** - * @brief Obtains cartesian limits from the node parameters - */ -class CartesianLimitsAggregator -{ -public: - /** - * @brief Loads cartesian limits from the node parameters - * - * The parameters are expected to be under "~/cartesian_limits" of the given - * node handle. - * The following limits can be specified: - * - "max_trans_vel", the maximum translational velocity [m/s] - * - "max_trans_acc, the maximum translational acceleration [m/s^2] - * - "max_trans_dec", the maximum translational deceleration (<= 0) [m/s^2] - * - "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 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 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/command_list_manager.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.h index 638b63c986..5a0a0b4ef2 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 @@ -48,6 +48,8 @@ #include "pilz_industrial_motion_planner/trajectory_blender.h" #include "pilz_industrial_motion_planner/trajectory_generation_exceptions.h" +#include + namespace pilz_industrial_motion_planner { using RobotTrajCont = std::vector; @@ -218,6 +220,9 @@ class CommandListManager //! @brief Builder to construct the container containing the final //! trajectories. PlanComponentsBuilder plan_comp_builder_; + + std::shared_ptr param_listener_; + cartesian_limits::Params params_; }; inline void CommandListManager::checkLastBlendRadiusZero(const moveit_msgs::msg::MotionSequenceRequest& req_list) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/limits_container.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/limits_container.h index 79e9d3ba00..5babb2b501 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/limits_container.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/limits_container.h @@ -34,10 +34,11 @@ #pragma once -#include "pilz_industrial_motion_planner/cartesian_limit.h" #include "pilz_industrial_motion_planner/joint_limits_container.h" #include +#include "cartesian_limits_parameters.hpp" + namespace pilz_industrial_motion_planner { /** @@ -68,24 +69,21 @@ class LimitsContainer const JointLimitsContainer& getJointLimitContainer() const; /** - * @brief Return if this LimitsContainer has defined cartesian limits - * - * @return True if container contains cartesian limits including maximum - * velocity/acceleration/deceleration + * @brief Prints the cartesian limits set by user. Default values for limits are 0.0 */ - bool hasFullCartesianLimits() const; + void printCartesianLimits() const; /** * @brief Set cartesian limits * @param cartesian_limit */ - void setCartesianLimits(CartesianLimit& cartesian_limit); + void setCartesianLimits(cartesian_limits::Params& cartesian_limit); /** * @brief Return the cartesian limits * @return the cartesian limits */ - const CartesianLimit& getCartesianLimits() const; + const cartesian_limits::Params& getCartesianLimits() const; private: /// Flag if joint limits where set @@ -98,7 +96,7 @@ class LimitsContainer bool has_cartesian_limits_; /// The cartesian limits - CartesianLimit cartesian_limit_; + cartesian_limits::Params cartesian_limits_; }; } // namespace pilz_industrial_motion_planner 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 2a100d907c..c9160e847e 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 @@ -45,6 +45,8 @@ #include #include +#include + namespace pilz_industrial_motion_planner { /** @@ -138,7 +140,8 @@ class CommandPlanner : public planning_interface::PlannerManager pilz_industrial_motion_planner::JointLimitsContainer aggregated_limit_active_joints_; /// cartesian limit - pilz_industrial_motion_planner::CartesianLimit cartesian_limit_; + std::shared_ptr param_listener_; + cartesian_limits::Params params_; }; MOVEIT_CLASS_FORWARD(CommandPlanner); diff --git a/moveit_planners/pilz_industrial_motion_planner/package.xml b/moveit_planners/pilz_industrial_motion_planner/package.xml index eb0a7bef82..be85fee214 100644 --- a/moveit_planners/pilz_industrial_motion_planner/package.xml +++ b/moveit_planners/pilz_industrial_motion_planner/package.xml @@ -22,6 +22,7 @@ + generate_parameter_library geometry_msgs moveit_ros_planning_interface moveit_msgs diff --git a/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limit.cpp b/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limit.cpp deleted file mode 100644 index 3d67a9be13..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limit.cpp +++ /dev/null @@ -1,119 +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. - *********************************************************************/ - -#include "pilz_industrial_motion_planner/cartesian_limit.h" - -pilz_industrial_motion_planner::CartesianLimit::CartesianLimit() - : has_max_trans_vel_(false) - , max_trans_vel_(0.0) - , has_max_trans_acc_(false) - , max_trans_acc_(0.0) - , has_max_trans_dec_(false) - , max_trans_dec_(0.0) - , has_max_rot_vel_(false) - , max_rot_vel_(0.0) -{ -} - -// Translational Velocity Limit - -bool pilz_industrial_motion_planner::CartesianLimit::hasMaxTranslationalVelocity() const -{ - return has_max_trans_vel_; -} - -void pilz_industrial_motion_planner::CartesianLimit::setMaxTranslationalVelocity(double max_trans_vel) -{ - has_max_trans_vel_ = true; - max_trans_vel_ = max_trans_vel; -} - -double pilz_industrial_motion_planner::CartesianLimit::getMaxTranslationalVelocity() const -{ - return max_trans_vel_; -} - -// Translational Acceleration Limit - -bool pilz_industrial_motion_planner::CartesianLimit::hasMaxTranslationalAcceleration() const -{ - return has_max_trans_acc_; -} - -void pilz_industrial_motion_planner::CartesianLimit::setMaxTranslationalAcceleration(double max_trans_acc) -{ - has_max_trans_acc_ = true; - max_trans_acc_ = max_trans_acc; -} - -double pilz_industrial_motion_planner::CartesianLimit::getMaxTranslationalAcceleration() const -{ - return max_trans_acc_; -} - -// Translational Deceleration Limit - -bool pilz_industrial_motion_planner::CartesianLimit::hasMaxTranslationalDeceleration() const -{ - return has_max_trans_dec_; -} - -void pilz_industrial_motion_planner::CartesianLimit::setMaxTranslationalDeceleration(double max_trans_dec) -{ - has_max_trans_dec_ = true; - max_trans_dec_ = max_trans_dec; -} - -double pilz_industrial_motion_planner::CartesianLimit::getMaxTranslationalDeceleration() const -{ - return max_trans_dec_; -} - -// Rotational Velocity Limit - -bool pilz_industrial_motion_planner::CartesianLimit::hasMaxRotationalVelocity() const -{ - return has_max_rot_vel_; -} - -void pilz_industrial_motion_planner::CartesianLimit::setMaxRotationalVelocity(double max_rot_vel) -{ - has_max_rot_vel_ = true; - max_rot_vel_ = max_rot_vel; -} - -double pilz_industrial_motion_planner::CartesianLimit::getMaxRotationalVelocity() const -{ - return max_rot_vel_; -} 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 deleted file mode 100644 index cdd4fd94f3..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limits_aggregator.cpp +++ /dev/null @@ -1,125 +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. - *********************************************************************/ - -#include -#include -#include -#include -#include - -#include "pilz_industrial_motion_planner/cartesian_limits_aggregator.h" - -static const std::string PARAM_CARTESIAN_LIMITS_NS = "cartesian_limits"; - -static const std::string PARAM_MAX_TRANS_VEL = "max_trans_vel"; -static const std::string PARAM_MAX_TRANS_ACC = "max_trans_acc"; -static const std::string PARAM_MAX_TRANS_DEC = "max_trans_dec"; -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 rclcpp::Node::SharedPtr& node, - const std::string& param_namespace) -{ - std::string param_prefix = param_namespace + "." + PARAM_CARTESIAN_LIMITS_NS + "."; - - pilz_industrial_motion_planner::CartesianLimit cartesian_limit; - - // translational velocity - double 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 (declareAndGetParam(max_trans_acc, param_prefix + PARAM_MAX_TRANS_ACC, node)) - { - cartesian_limit.setMaxTranslationalAcceleration(max_trans_acc); - } - - // translational deceleration - double 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 (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 (node->has_parameter(param_prefix + PARAM_MAX_ROT_ACC) || node->has_parameter(param_prefix + PARAM_MAX_ROT_DEC)) - { - 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 - - return cartesian_limit; -} diff --git a/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limits_parameters.yaml b/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limits_parameters.yaml new file mode 100644 index 0000000000..8bd85c0df9 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limits_parameters.yaml @@ -0,0 +1,17 @@ +cartesian_limits: + max_trans_vel: { + type: double, + description: "Max translation velocity", + } + max_trans_acc: { + type: double, + description: "Max translation acceleraion", + } + max_trans_dec: { + type: double, + description: "Max translation decceleration", + } + max_rot_vel: { + type: double, + description: "Max rotational velocity", + } 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 8702caac22..361d0c02b5 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 @@ -42,7 +42,8 @@ #include #include -#include "pilz_industrial_motion_planner/cartesian_limits_aggregator.h" + +#include "cartesian_limits_parameters.hpp" #include "pilz_industrial_motion_planner/joint_limits_aggregator.h" #include "pilz_industrial_motion_planner/tip_frame_getter.h" #include "pilz_industrial_motion_planner/trajectory_blend_request.h" @@ -63,13 +64,13 @@ CommandListManager::CommandListManager(const rclcpp::Node::SharedPtr& node, aggregated_limit_active_joints = pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( node_, PARAM_NAMESPACE_LIMITS, model_->getActiveJointModels()); - // Obtain cartesian limits - pilz_industrial_motion_planner::CartesianLimit cartesian_limit = - pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits(node_, PARAM_NAMESPACE_LIMITS); + param_listener_ = + std::make_shared(node, PARAM_NAMESPACE_LIMITS + ".cartesian_limits"); + params_ = param_listener_->get_params(); pilz_industrial_motion_planner::LimitsContainer limits; limits.setJointLimits(aggregated_limit_active_joints); - limits.setCartesianLimits(cartesian_limit); + limits.setCartesianLimits(params_); plan_comp_builder_.setModel(model); plan_comp_builder_.setBlender(std::unique_ptr( diff --git a/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp index 6ee51df0b6..a2a3e816f8 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp @@ -33,46 +33,49 @@ *********************************************************************/ #include "pilz_industrial_motion_planner/limits_container.h" +#include -pilz_industrial_motion_planner::LimitsContainer::LimitsContainer() - : has_joint_limits_(false), has_cartesian_limits_(false) +namespace pilz_industrial_motion_planner +{ +static const rclcpp::Logger LOGGER = rclcpp::get_logger("pilz_industrial_motion_planner.limits_container"); + +LimitsContainer::LimitsContainer() : has_joint_limits_(false), has_cartesian_limits_(false) { } -bool pilz_industrial_motion_planner::LimitsContainer::hasJointLimits() const +bool LimitsContainer::hasJointLimits() const { return has_joint_limits_; } -void pilz_industrial_motion_planner::LimitsContainer::setJointLimits( - pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) +void LimitsContainer::setJointLimits(JointLimitsContainer& joint_limits) { has_joint_limits_ = true; joint_limits_ = joint_limits; } -const pilz_industrial_motion_planner::JointLimitsContainer& -pilz_industrial_motion_planner::LimitsContainer::getJointLimitContainer() const +const JointLimitsContainer& LimitsContainer::getJointLimitContainer() const { return joint_limits_; } -bool pilz_industrial_motion_planner::LimitsContainer::hasFullCartesianLimits() const +void LimitsContainer::printCartesianLimits() const { - return (has_cartesian_limits_ && cartesian_limit_.hasMaxTranslationalVelocity() && - cartesian_limit_.hasMaxTranslationalAcceleration() && cartesian_limit_.hasMaxTranslationalDeceleration() && - cartesian_limit_.hasMaxRotationalVelocity()); + RCLCPP_DEBUG(LOGGER, + "Pilz Cartesian Limits - Max Trans Vel : %f, Max Trans Acc : %f, Max Trans Dec : %f, Max Rot Vel : %f", + cartesian_limits_.max_trans_vel, cartesian_limits_.max_trans_acc, cartesian_limits_.max_trans_dec, + cartesian_limits_.max_rot_vel); } -void pilz_industrial_motion_planner::LimitsContainer::setCartesianLimits( - pilz_industrial_motion_planner::CartesianLimit& cartesian_limit) +void LimitsContainer::setCartesianLimits(cartesian_limits::Params& cartesian_limits) { has_cartesian_limits_ = true; - cartesian_limit_ = cartesian_limit; + cartesian_limits_ = cartesian_limits; } -const pilz_industrial_motion_planner::CartesianLimit& -pilz_industrial_motion_planner::LimitsContainer::getCartesianLimits() const +const cartesian_limits::Params& LimitsContainer::getCartesianLimits() const { - return cartesian_limit_; + return cartesian_limits_; } + +} // namespace pilz_industrial_motion_planner 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 e386ef58f1..179c51b202 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 @@ -40,7 +40,7 @@ #include "pilz_industrial_motion_planner/planning_context_loader_ptp.h" #include "pilz_industrial_motion_planner/planning_exceptions.h" -#include "pilz_industrial_motion_planner/cartesian_limits_aggregator.h" +#include "cartesian_limits_parameters.hpp" #include "pilz_industrial_motion_planner/joint_limits_aggregator.h" #include @@ -69,8 +69,9 @@ bool CommandPlanner::initialize(const moveit::core::RobotModelConstPtr& model, c node, PARAM_NAMESPACE_LIMITS, model->getActiveJointModels()); // Obtain cartesian limits - cartesian_limit_ = - pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits(node, PARAM_NAMESPACE_LIMITS); + param_listener_ = + std::make_shared(node, PARAM_NAMESPACE_LIMITS + ".cartesian_limits"); + params_ = param_listener_->get_params(); // Load the planning context loader planner_context_loader = std::make_unique>( @@ -94,7 +95,7 @@ bool CommandPlanner::initialize(const moveit::core::RobotModelConstPtr& model, c pilz_industrial_motion_planner::LimitsContainer limits; limits.setJointLimits(aggregated_limit_active_joints_); - limits.setCartesianLimits(cartesian_limit_); + limits.setCartesianLimits(params_); loader_pointer->setLimits(limits); loader_pointer->setModel(model_); 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 bbcc54821c..9b95fc97e2 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp @@ -286,8 +286,8 @@ TrajectoryGenerator::cartesianTrapVelocityProfile(const double& max_velocity_sca const std::unique_ptr& path) const { std::unique_ptr vp_trans = std::make_unique( - max_velocity_scaling_factor * planner_limits_.getCartesianLimits().getMaxTranslationalVelocity(), - max_acceleration_scaling_factor * planner_limits_.getCartesianLimits().getMaxTranslationalAcceleration()); + max_velocity_scaling_factor * planner_limits_.getCartesianLimits().max_trans_vel, + max_acceleration_scaling_factor * planner_limits_.getCartesianLimits().max_trans_acc); if (path->PathLength() > std::numeric_limits::epsilon()) // avoid division by zero { 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 b0791b5592..2091def3e9 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 @@ -58,11 +58,7 @@ TrajectoryGeneratorCIRC::TrajectoryGeneratorCIRC(const moveit::core::RobotModelC const std::string& /*group_name*/) : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits) { - if (!planner_limits_.hasFullCartesianLimits()) - { - throw TrajectoryGeneratorInvalidLimitsException( - "Cartesian limits are not fully set for CIRC trajectory generator."); - } + planner_limits_.printCartesianLimits(); } void TrajectoryGeneratorCIRC::cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const @@ -239,8 +235,8 @@ std::unique_ptr TrajectoryGeneratorCIRC::setPathCIRC(const MotionPlan // The KDL::Path implementation chooses the motion with the longer duration // (translation vs. rotation) // and uses eqradius as scaling factor between the distances. - double eqradius = planner_limits_.getCartesianLimits().getMaxTranslationalVelocity() / - planner_limits_.getCartesianLimits().getMaxRotationalVelocity(); + double eqradius = + planner_limits_.getCartesianLimits().max_trans_vel / planner_limits_.getCartesianLimits().max_rot_vel; try { 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 ccd74a19b4..10f3f352e3 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 @@ -56,11 +56,7 @@ TrajectoryGeneratorLIN::TrajectoryGeneratorLIN(const moveit::core::RobotModelCon const LimitsContainer& planner_limits, const std::string& /*group_name*/) : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits) { - if (!planner_limits_.hasFullCartesianLimits()) - { - RCLCPP_ERROR(LOGGER, "Cartesian limits not set for LIN trajectory generator."); - throw TrajectoryGeneratorInvalidLimitsException("Cartesian limits are not fully set for LIN trajectory generator."); - } + planner_limits_.printCartesianLimits(); } void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, @@ -188,8 +184,8 @@ std::unique_ptr TrajectoryGeneratorLIN::setPathLIN(const Eigen::Affin KDL::Frame kdl_start_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(); + double eqradius = + planner_limits_.getCartesianLimits().max_trans_vel / planner_limits_.getCartesianLimits().max_rot_vel; KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis(); return std::unique_ptr( 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 62e8dae24e..a1ce780b3d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp @@ -787,9 +787,9 @@ bool testutils::checkBlendingCartSpaceContinuity(const pilz_industrial_motion_pl } // limits - double max_trans_velo = planner_limits.getCartesianLimits().getMaxTranslationalVelocity(); - double max_trans_acc = planner_limits.getCartesianLimits().getMaxTranslationalAcceleration(); - double max_rot_velo = planner_limits.getCartesianLimits().getMaxRotationalVelocity(); + double max_trans_velo = planner_limits.getCartesianLimits().max_trans_vel; + double max_trans_acc = planner_limits.getCartesianLimits().max_trans_acc; + double max_rot_velo = planner_limits.getCartesianLimits().max_rot_vel; double max_rot_acc = max_trans_acc / max_trans_velo * max_rot_velo; // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/CMakeLists.txt index 066754c0e8..1178f540b1 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/CMakeLists.txt +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/CMakeLists.txt @@ -13,7 +13,6 @@ set(UNIT_TEST_SOURCEFILES 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 @@ -135,11 +134,6 @@ 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 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 deleted file mode 100644 index c51e42269c..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_cartesian_limits_aggregator.yaml +++ /dev/null @@ -1,10 +0,0 @@ -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/src/unittest_cartesian_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_cartesian_limits_aggregator.cpp deleted file mode 100644 index 7e5a79b280..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_cartesian_limits_aggregator.cpp +++ /dev/null @@ -1,94 +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. - *********************************************************************/ - -#include - -#include - -#include "pilz_industrial_motion_planner/cartesian_limit.h" -#include "pilz_industrial_motion_planner/cartesian_limits_aggregator.h" - -/** - * @brief Unittest of the CartesianLimitsAggregator class - */ -class CartesianLimitsAggregator : public ::testing::Test -{ -protected: - void SetUp() override - { - node_ = rclcpp::Node::make_shared("unittest_cartesian_limits_aggregator"); - } - rclcpp::Node::SharedPtr node_; -}; - -/** - * @brief Check if only velocity is set - */ -TEST_F(CartesianLimitsAggregator, OnlyVelocity) -{ - pilz_industrial_motion_planner::CartesianLimit limit = - pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits(node_, "only_vel"); - EXPECT_TRUE(limit.hasMaxTranslationalVelocity()); - EXPECT_EQ(limit.getMaxTranslationalVelocity(), 10); - EXPECT_FALSE(limit.hasMaxTranslationalAcceleration()); - EXPECT_FALSE(limit.hasMaxTranslationalDeceleration()); - EXPECT_FALSE(limit.hasMaxRotationalVelocity()); -} - -/** - * @brief Check if all values are set correctly - */ -TEST_F(CartesianLimitsAggregator, AllValues) -{ - pilz_industrial_motion_planner::CartesianLimit limit = - pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits(node_, "all_values"); - EXPECT_TRUE(limit.hasMaxTranslationalVelocity()); - EXPECT_EQ(limit.getMaxTranslationalVelocity(), 1); - - EXPECT_TRUE(limit.hasMaxTranslationalAcceleration()); - EXPECT_EQ(limit.getMaxTranslationalAcceleration(), 2); - - EXPECT_TRUE(limit.hasMaxTranslationalDeceleration()); - EXPECT_EQ(limit.getMaxTranslationalDeceleration(), -3); - - EXPECT_TRUE(limit.hasMaxRotationalVelocity()); - EXPECT_EQ(limit.getMaxRotationalVelocity(), 4); -} - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp index 7e9b7c0d7c..d4313324db 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp @@ -107,11 +107,12 @@ class PlanningContextTest : public ::testing::Test pilz_industrial_motion_planner::JointLimitsContainer joint_limits = testutils::createFakeLimits(robot_model_->getVariableNames()); - pilz_industrial_motion_planner::CartesianLimit cartesian_limit; - cartesian_limit.setMaxRotationalVelocity(1.0 * M_PI); - cartesian_limit.setMaxTranslationalAcceleration(1.0 * M_PI); - cartesian_limit.setMaxTranslationalDeceleration(1.0 * M_PI); - cartesian_limit.setMaxTranslationalVelocity(1.0 * M_PI); + + cartesian_limits::Params cartesian_limit; + cartesian_limit.max_trans_vel = 1.0 * M_PI; + cartesian_limit.max_trans_acc = 1.0 * M_PI; + cartesian_limit.max_trans_dec = 1.0 * M_PI; + cartesian_limit.max_rot_vel = 1.0 * M_PI; pilz_industrial_motion_planner::LimitsContainer limits; limits.setJointLimits(joint_limits); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context_loaders.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context_loaders.cpp index 9c99e13fac..ac5cfc32f5 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context_loaders.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context_loaders.cpp @@ -147,11 +147,13 @@ TEST_P(PlanningContextLoadersTest, LoadContext) testutils::createFakeLimits(robot_model_->getVariableNames()); pilz_industrial_motion_planner::LimitsContainer limits; limits.setJointLimits(joint_limits); - pilz_industrial_motion_planner::CartesianLimit cart_limits; - cart_limits.setMaxRotationalVelocity(1 * M_PI); - cart_limits.setMaxTranslationalAcceleration(2); - cart_limits.setMaxTranslationalDeceleration(2); - cart_limits.setMaxTranslationalVelocity(1); + + cartesian_limits::Params cart_limits; + cart_limits.max_trans_vel = 1 * M_PI; + cart_limits.max_trans_acc = 2; + cart_limits.max_trans_dec = 2; + cart_limits.max_rot_vel = 1; + limits.setCartesianLimits(cart_limits); planning_context_loader_->setLimits(limits); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp index 695a769127..7eeb230427 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp @@ -108,11 +108,13 @@ class TrajectoryBlenderTransitionWindowTest : public testing::Test 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); + + cartesian_limits::Params cart_limits; + cart_limits.max_trans_vel = 1 * M_PI; + cart_limits.max_trans_acc = 2; + cart_limits.max_trans_dec = 2; + cart_limits.max_rot_vel = 1; + planner_limits_.setJointLimits(joint_limits); planner_limits_.setCartesianLimits(cart_limits); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp index afe519a907..44282e9d31 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp @@ -106,16 +106,17 @@ class TrajectoryGeneratorCIRCTest : public testing::Test 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 + cartesian_limits::Params cartesian_limit; + cartesian_limit.max_trans_vel = 1.0 * M_PI; + cartesian_limit.max_trans_acc = 1.0 * M_PI; + cartesian_limit.max_trans_dec = 1.0 * M_PI; + cartesian_limit.max_rot_vel = 1.0 * M_PI; - 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); + planner_limits_.setCartesianLimits(cartesian_limit); // initialize the LIN trajectory generator circ_ = std::make_unique(robot_model_, planner_limits_, planning_group_); @@ -268,16 +269,6 @@ TEST_F(TrajectoryGeneratorCIRCTest, TestExceptionErrorCodeMapping) } } -/** - * @brief Construct a TrajectoryGeneratorCirc with no limits given - */ -TEST_F(TrajectoryGeneratorCIRCTest, noLimits) -{ - LimitsContainer planner_limits; - EXPECT_THROW(TrajectoryGeneratorCIRC(this->robot_model_, planner_limits, planning_group_), - TrajectoryGeneratorInvalidLimitsException); -} - /** * @brief test invalid motion plan request with incomplete start state and * cartesian goal diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp index b4fe4c7600..fdb603ab59 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp @@ -53,6 +53,8 @@ #include "rclcpp/rclcpp.hpp" +#include "cartesian_limits_parameters.hpp" + const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; const std::string PARAM_NAMESPACE_LIMITS{ "robot_description_planning" }; @@ -121,11 +123,13 @@ class TrajectoryGeneratorCommonTest : public ::testing::Test pilz_industrial_motion_planner::JointLimitsContainer joint_limits = pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( 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); - cart_limits.setMaxTranslationalDeceleration(2); - cart_limits.setMaxTranslationalVelocity(1); + + cartesian_limits::Params cart_limits; + cart_limits.max_trans_vel = 0.5 * M_PI; + cart_limits.max_trans_acc = 2; + cart_limits.max_trans_dec = 2; + cart_limits.max_rot_vel = 1; + pilz_industrial_motion_planner::LimitsContainer planner_limits; planner_limits.setJointLimits(joint_limits); planner_limits.setCartesianLimits(cart_limits); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp index 583466e43f..56fd9e6060 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp @@ -118,11 +118,13 @@ class TrajectoryGeneratorLINTest : public testing::Test 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); + + cartesian_limits::Params cart_limits; + cart_limits.max_rot_vel = 0.5 * M_PI; + cart_limits.max_trans_acc = 2; + cart_limits.max_trans_dec = 2; + cart_limits.max_trans_vel = 1; + planner_limits_.setJointLimits(joint_limits); planner_limits_.setCartesianLimits(cart_limits); @@ -381,23 +383,6 @@ TEST_F(TrajectoryGeneratorLINTest, LinStartEqualsGoal) EXPECT_TRUE(checkLinResponse(lin_joint_req, res)); } -/** - * @brief Checks that constructor throws an exception if no limits are given. - * - * Test Sequence: - * 1. Call Ctor without set limits. - * - * Expected Results: - * 1. Ctor throws exception. - */ -TEST_F(TrajectoryGeneratorLINTest, CtorNoLimits) -{ - pilz_industrial_motion_planner::LimitsContainer planner_limits; - - EXPECT_THROW(pilz_industrial_motion_planner::TrajectoryGeneratorLIN(robot_model_, planner_limits, planning_group_), - pilz_industrial_motion_planner::TrajectoryGeneratorInvalidLimitsException); -} - /** * @brief Checks that generate() function returns 'false' if called with an * incorrect number of joints.