Use generate_parameter_library to load pilz cartesian limit parameters (

moveit#1577)

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,26 +87,32 @@ 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
   src/joint_limits_aggregator.cpp
   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
   src/trajectory_functions.cpp
   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

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 <cartesian_limits_parameters.hpp>
+
 namespace pilz_industrial_motion_planner
 {
 using RobotTrajCont = std::vector<robot_trajectory::RobotTrajectoryPtr>;
@@ -218,6 +220,9 @@ class CommandListManager
   //! @brief Builder to construct the container containing the final
   //! trajectories.
   PlanComponentsBuilder plan_comp_builder_;
+
+  std::shared_ptr<cartesian_limits::ParamListener> param_listener_;
+  cartesian_limits::Params params_;
 };
 
 inline void CommandListManager::checkLastBlendRadiusZero(const moveit_msgs::msg::MotionSequenceRequest& req_list)

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 <math.h>
 
+#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

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.h

@@ -45,6 +45,8 @@
 #include <pluginlib/class_loader.hpp>
 #include <memory>
 
+#include <cartesian_limits_parameters.hpp>
+
 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<cartesian_limits::ParamListener> param_listener_;
+  cartesian_limits::Params params_;
 };
 
 MOVEIT_CLASS_FORWARD(CommandPlanner);

moveit_planners/pilz_industrial_motion_planner/package.xml

@@ -22,6 +22,7 @@
   <!-- TODO(henning): Enable when this is available
   <depend>joint_limits_interface</depend>
   -->
+  <depend>generate_parameter_library</depend>
   <depend>geometry_msgs</depend>
   <depend>moveit_ros_planning_interface</depend>
   <depend>moveit_msgs</depend>