Feature/expose pilz sampling time

moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt

@@ -63,16 +63,22 @@ generate_parameter_library(
   src/cartesian_limits_parameters.yaml # path to input yaml file
 )
 
+generate_parameter_library(
+  pilz_sampling_parameters # cmake target name for the parameter library
+  src/pilz_sampling_parameters.yaml # path to input yaml file
+)
+
 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)
-target_link_libraries(joint_limits_common cartesian_limits_parameters)
+target_link_libraries(joint_limits_common cartesian_limits_parameters
+                      pilz_sampling_parameters)
 ament_target_dependencies(joint_limits_common ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 add_library(planning_context_loader_base SHARED src/planning_context_loader.cpp)
 target_link_libraries(planning_context_loader_base cartesian_limits_parameters
-                      joint_limits_common)
+                      pilz_sampling_parameters joint_limits_common)
 ament_target_dependencies(planning_context_loader_base
                           ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
@@ -81,7 +87,7 @@ add_library(
   src/trajectory_functions.cpp src/trajectory_generator.cpp
   src/trajectory_blender_transition_window.cpp)
 target_link_libraries(trajectory_generation_common cartesian_limits_parameters
-                      joint_limits_common)
+                      pilz_sampling_parameters joint_limits_common)
 ament_target_dependencies(trajectory_generation_common
                           ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
@@ -161,6 +167,7 @@ pluginlib_export_plugin_description_file(
 install(
   TARGETS pilz_industrial_motion_planner
           cartesian_limits_parameters
+          pilz_sampling_parameters
           joint_limits_common
           planning_context_loader_base
           planning_context_loader_ptp

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.hpp

@@ -46,6 +46,7 @@
 #include <memory>
 
 #include <pilz_industrial_motion_planner/cartesian_limits_parameters.hpp>
+#include <pilz_industrial_motion_planner/pilz_sampling_parameters.hpp>
 
 namespace pilz_industrial_motion_planner
 {
@@ -133,9 +134,13 @@ class CommandPlanner : public planning_interface::PlannerManager
   /// aggregated limits of the active joints
   pilz_industrial_motion_planner::JointLimitsContainer aggregated_limit_active_joints_;
 
-  /// cartesian limit
-  std::shared_ptr<cartesian_limits::ParamListener> param_listener_;
-  cartesian_limits::Params params_;
+  /// cartesian limit parameters
+  std::shared_ptr<cartesian_limits::ParamListener> cartesian_limits_param_listener_;
+  cartesian_limits::Params cartesian_limits_params_;
+
+  /// sampling parameters
+  std::shared_ptr<pilz_sampling::ParamListener> sampling_param_listener_;
+  pilz_sampling::Params sampling_params_;
 };
 
 MOVEIT_CLASS_FORWARD(CommandPlanner);

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.hpp

@@ -58,12 +58,14 @@ class PlanningContextBase : public planning_interface::PlanningContext
 public:
   PlanningContextBase<GeneratorT>(const std::string& name, const std::string& group,
                                   const moveit::core::RobotModelConstPtr& model,
-                                  const pilz_industrial_motion_planner::LimitsContainer& limits)
+                                  const pilz_industrial_motion_planner::LimitsContainer& limits,
+                                  const pilz_sampling::Params& sampling)
     : planning_interface::PlanningContext(name, group)
     , terminated_(false)
     , model_(model)
     , limits_(limits)
-    , generator_(model, limits_, group)
+    , sampling_(sampling)
+    , generator_(model, limits_, sampling_, group)
   {
   }
 
@@ -112,6 +114,9 @@ class PlanningContextBase : public planning_interface::PlanningContext
   /// Joint limits to be used during planning
   pilz_industrial_motion_planner::LimitsContainer limits_;
 
+  /// Settings to be used when sampling the output trajectory
+  pilz_sampling::Params sampling_;
+
 protected:
   GeneratorT generator_;
 };

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.hpp

@@ -58,8 +58,9 @@ class PlanningContextCIRC : public pilz_industrial_motion_planner::PlanningConte
 {
 public:
   PlanningContextCIRC(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model,
-                      const pilz_industrial_motion_planner::LimitsContainer& limits)
-    : pilz_industrial_motion_planner::PlanningContextBase<TrajectoryGeneratorCIRC>(name, group, model, limits)
+                      const pilz_industrial_motion_planner::LimitsContainer& limits,
+                      const pilz_sampling::Params& sampling)
+    : pilz_industrial_motion_planner::PlanningContextBase<TrajectoryGeneratorCIRC>(name, group, model, limits, sampling)
   {
   }
 };

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.hpp

@@ -58,8 +58,9 @@ class PlanningContextLIN : public pilz_industrial_motion_planner::PlanningContex
 {
 public:
   PlanningContextLIN(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model,
-                     const pilz_industrial_motion_planner::LimitsContainer& limits)
-    : pilz_industrial_motion_planner::PlanningContextBase<TrajectoryGeneratorLIN>(name, group, model, limits)
+                     const pilz_industrial_motion_planner::LimitsContainer& limits,
+                     const pilz_sampling::Params& sampling)
+    : pilz_industrial_motion_planner::PlanningContextBase<TrajectoryGeneratorLIN>(name, group, model, limits, sampling)
   {
   }
 };

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.hpp

@@ -35,6 +35,7 @@
 #pragma once
 
 #include <pilz_industrial_motion_planner/limits_container.hpp>
+#include <pilz_industrial_motion_planner/pilz_sampling_parameters.hpp>
 
 #include <memory>
 #include <vector>
@@ -75,6 +76,13 @@ class PlanningContextLoader
    */
   virtual bool setLimits(const pilz_industrial_motion_planner::LimitsContainer& limits);
 
+  /**
+   * @brief Sets the sampling parameters the planner can pass to the contexts
+   * @param sampling container of sampling parameters
+   * @return true if sampling parameters could be set
+   */
+  virtual bool setSampling(const pilz_sampling::Params& sampling);
+
   /**
    * @brief Return the planning context
    * @param planning_context
@@ -107,6 +115,9 @@ class PlanningContextLoader
   /// Limits to be used during planning
   pilz_industrial_motion_planner::LimitsContainer limits_;
 
+  /// Settings to be used when sampling the output trajectory
+  pilz_sampling::Params sampling_;
+
   /// True if model is set
   bool model_set_;
 
@@ -123,7 +134,7 @@ bool PlanningContextLoader::loadContext(planning_interface::PlanningContextPtr&
 {
   if (limits_set_ && model_set_)
   {
-    planning_context = std::make_shared<T>(name, group, model_, limits_);
+    planning_context = std::make_shared<T>(name, group, model_, limits_, sampling_);
     return true;
   }
   else

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_polyline.hpp

@@ -60,8 +60,10 @@ class PlanningContextPolyline : public pilz_industrial_motion_planner::PlanningC
 public:
   PlanningContextPolyline(const std::string& name, const std::string& group,
                           const moveit::core::RobotModelConstPtr& model,
-                          const pilz_industrial_motion_planner::LimitsContainer& limits)
-    : pilz_industrial_motion_planner::PlanningContextBase<TrajectoryGeneratorPolyline>(name, group, model, limits)
+                          const pilz_industrial_motion_planner::LimitsContainer& limits,
+                          const pilz_sampling::Params& sampling)
+    : pilz_industrial_motion_planner::PlanningContextBase<TrajectoryGeneratorPolyline>(name, group, model, limits,
+                                                                                       sampling)
   {
   }
 };

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.hpp

@@ -58,8 +58,9 @@ class PlanningContextPTP : public pilz_industrial_motion_planner::PlanningContex
 {
 public:
   PlanningContextPTP(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model,
-                     const pilz_industrial_motion_planner::LimitsContainer& limits)
-    : pilz_industrial_motion_planner::PlanningContextBase<TrajectoryGeneratorPTP>(name, group, model, limits)
+                     const pilz_industrial_motion_planner::LimitsContainer& limits,
+                     const pilz_sampling::Params& sampling)
+    : pilz_industrial_motion_planner::PlanningContextBase<TrajectoryGeneratorPTP>(name, group, model, limits, sampling)
   {
   }
 };

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.hpp

@@ -46,6 +46,7 @@
 
 #include <pilz_industrial_motion_planner/joint_limits_extension.hpp>
 #include <pilz_industrial_motion_planner/limits_container.hpp>
+#include <pilz_industrial_motion_planner/pilz_sampling_parameters.hpp>
 #include <pilz_industrial_motion_planner/trajectory_functions.hpp>
 #include <pilz_industrial_motion_planner/trajectory_generation_exceptions.hpp>
 
@@ -93,11 +94,20 @@ class TrajectoryGenerator
 {
 public:
   TrajectoryGenerator(const moveit::core::RobotModelConstPtr& robot_model,
-                      const pilz_industrial_motion_planner::LimitsContainer& planner_limits)
-    : robot_model_(robot_model), planner_limits_(planner_limits), clock_(std::make_unique<rclcpp::Clock>())
+                      const pilz_industrial_motion_planner::LimitsContainer& planner_limits,
+                      const pilz_sampling::Params& sampling)
+    : robot_model_(robot_model)
+    , planner_limits_(planner_limits)
+    , sampling_(sampling)
+    , clock_(std::make_unique<rclcpp::Clock>())
   {
   }
 
+  /**
+   * @brief Set the sampling time
+   */
+  void setSamplingTime(double sampling_time);
+
   virtual ~TrajectoryGenerator() = default;
 
   /**
@@ -107,7 +117,7 @@ class TrajectoryGenerator
    * @param sampling_time: sampling time of the generate trajectory
    */
   void generate(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
-                planning_interface::MotionPlanResponse& res, double sampling_time = 0.1);
+                planning_interface::MotionPlanResponse& res);
 
 protected:
   /**
@@ -276,6 +286,7 @@ class TrajectoryGenerator
 protected:
   const moveit::core::RobotModelConstPtr robot_model_;
   const pilz_industrial_motion_planner::LimitsContainer planner_limits_;
+  pilz_sampling::Params sampling_;
   double max_cartesian_speed_;
   static constexpr double MIN_SCALING_FACTOR{ 0.0001 };
   static constexpr double MAX_SCALING_FACTOR{ 1. };

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.hpp

@@ -74,13 +74,14 @@ class TrajectoryGeneratorCIRC : public TrajectoryGenerator
    * @brief Constructor of CIRC Trajectory Generator.
    *
    * @param planner_limits Limits in joint and Cartesian spaces
+   * @param sampling Sampling parameters
    *
    * @throw TrajectoryGeneratorInvalidLimitsException
    *
    */
   TrajectoryGeneratorCIRC(const moveit::core::RobotModelConstPtr& robot_model,
                           const pilz_industrial_motion_planner::LimitsContainer& planner_limits,
-                          const std::string& group_name);
+                          const pilz_sampling::Params& sampling, const std::string& group_name);
 
 private:
   void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const override;

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.hpp

@@ -63,10 +63,11 @@ class TrajectoryGeneratorLIN : public TrajectoryGenerator
    * @throw TrajectoryGeneratorInvalidLimitsException
    * @param model: robot model
    * @param planner_limits: limits in joint and Cartesian spaces
+   * @param sampling: sampling parameters
    */
   TrajectoryGeneratorLIN(const moveit::core::RobotModelConstPtr& robot_model,
                          const pilz_industrial_motion_planner::LimitsContainer& planner_limits,
-                         const std::string& group_name);
+                         const pilz_sampling::Params& sampling, const std::string& group_name);
 
 private:
   void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene,

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_polyline.hpp

@@ -67,10 +67,11 @@ class TrajectoryGeneratorPolyline : public TrajectoryGenerator
    * @throw TrajectoryGeneratorInvalidLimitsException
    * @param model: robot model
    * @param planner_limits: limits in joint and Cartesian spaces
+   * @param sampling: sampling parameters
    */
   TrajectoryGeneratorPolyline(const moveit::core::RobotModelConstPtr& robot_model,
                               const pilz_industrial_motion_planner::LimitsContainer& planner_limits,
-                              const std::string& group_name);
+                              const pilz_sampling::Params& sampling, const std::string& group_name);
 
 private:
   void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const override;

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.hpp

@@ -59,10 +59,12 @@ class TrajectoryGeneratorPTP : public TrajectoryGenerator
    * @brief Constructor of PTP Trajectory Generator
    * @throw TrajectoryGeneratorInvalidLimitsException
    * @param model: a map of joint limits information
+   * @param planner_limits: limits in joint and Cartesian spaces
+   * @param sampling: sampling parameters
    */
   TrajectoryGeneratorPTP(const moveit::core::RobotModelConstPtr& robot_model,
                          const pilz_industrial_motion_planner::LimitsContainer& planner_limits,
-                         const std::string& group_name);
+                         const pilz_sampling::Params& sampling, const std::string& group_name);
 
 private:
   void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene,

moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp

@@ -54,7 +54,7 @@ namespace pilz_industrial_motion_planner
 {
 namespace
 {
-const std::string PARAM_NAMESPACE_LIMITS = "robot_description_planning";
+const std::string PARAMETER_NAMESPACE = "robot_description_planning";
 rclcpp::Logger getLogger()
 {
   return moveit::getLogger("moveit.planners.pilz.command_planner");
@@ -73,12 +73,17 @@ bool CommandPlanner::initialize(const moveit::core::RobotModelConstPtr& model, c
 
   // Obtain the aggregated joint limits
   aggregated_limit_active_joints_ = pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(
-      node, PARAM_NAMESPACE_LIMITS, model->getActiveJointModels());
+      node, PARAMETER_NAMESPACE, model->getActiveJointModels());
 
   // Obtain cartesian limits
-  param_listener_ =
-      std::make_shared<cartesian_limits::ParamListener>(node, PARAM_NAMESPACE_LIMITS + ".cartesian_limits");
-  params_ = param_listener_->get_params();
+  cartesian_limits_param_listener_ =
+      std::make_shared<cartesian_limits::ParamListener>(node, PARAMETER_NAMESPACE + ".cartesian_limits");
+  cartesian_limits_params_ = cartesian_limits_param_listener_->get_params();
+
+  // Obtain sampling parameters
+  sampling_param_listener_ =
+      std::make_shared<pilz_sampling::ParamListener>(node, PARAMETER_NAMESPACE + ".pilz_sampling");
+  sampling_params_ = sampling_param_listener_->get_params();
 
   // Load the planning context loader
   planner_context_loader_ = std::make_unique<pluginlib::ClassLoader<PlanningContextLoader>>(
@@ -102,10 +107,11 @@ bool CommandPlanner::initialize(const moveit::core::RobotModelConstPtr& model, c
 
     pilz_industrial_motion_planner::LimitsContainer limits;
     limits.setJointLimits(aggregated_limit_active_joints_);
-    limits.setCartesianLimits(params_);
+    limits.setCartesianLimits(cartesian_limits_params_);
 
     loader_pointer->setLimits(limits);
     loader_pointer->setModel(model_);
+    loader_pointer->setSampling(sampling_params_);
 
     registerContextLoader(loader_pointer);
   }

moveit_planners/pilz_industrial_motion_planner/src/pilz_sampling_parameters.yaml

@@ -0,0 +1,9 @@
+pilz_sampling:
+  max_seconds: {
+    type: double,
+    default_value: 0.1,
+    description: "Maximum time interval between two consecutive samples in seconds",
+    validation: {
+      gt: [ 0.0 ]
+    }
+  }

moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader.cpp

@@ -57,6 +57,12 @@ bool pilz_industrial_motion_planner::PlanningContextLoader::setLimits(
   return true;
 }
 
+bool pilz_industrial_motion_planner::PlanningContextLoader::setSampling(const pilz_sampling::Params& sampling)
+{
+  sampling_ = sampling;
+  return true;
+}
+
 std::string pilz_industrial_motion_planner::PlanningContextLoader::getAlgorithm() const
 {
   return alg_;

moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp

@@ -65,7 +65,7 @@ bool pilz_industrial_motion_planner::PlanningContextLoaderCIRC::loadContext(
 {
   if (limits_set_ && model_set_)
   {
-    planning_context = std::make_shared<PlanningContextCIRC>(name, group, model_, limits_);
+    planning_context = std::make_shared<PlanningContextCIRC>(name, group, model_, limits_, sampling_);
     return true;
   }
   else

moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp

@@ -62,7 +62,7 @@ bool pilz_industrial_motion_planner::PlanningContextLoaderLIN::loadContext(
 {
   if (limits_set_ && model_set_)
   {
-    planning_context = std::make_shared<PlanningContextLIN>(name, group, model_, limits_);
+    planning_context = std::make_shared<PlanningContextLIN>(name, group, model_, limits_, sampling_);
     return true;
   }
   else

moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_polyline.cpp

@@ -63,7 +63,7 @@ bool pilz_industrial_motion_planner::PlanningContextLoaderPolyline::loadContext(
 {
   if (limits_set_ && model_set_)
   {
-    planning_context = std::make_shared<PlanningContextPolyline>(name, group, model_, limits_);
+    planning_context = std::make_shared<PlanningContextPolyline>(name, group, model_, limits_, sampling_);
     return true;
   }
   else

moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp

@@ -61,7 +61,7 @@ bool pilz_industrial_motion_planner::PlanningContextLoaderPTP::loadContext(
 {
   if (limits_set_ && model_set_)
   {
-    planning_context = std::make_shared<PlanningContextPTP>(name, group, model_, limits_);
+    planning_context = std::make_shared<PlanningContextPTP>(name, group, model_, limits_, sampling_);
     return true;
   }
   else