Allow ROS namespaces for planning request adapters: pass a NodeHandle for their initialization

moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h

@@ -41,6 +41,7 @@
 #include <moveit/planning_interface/planning_interface.h>
 #include <moveit/planning_scene/planning_scene.h>
 #include <boost/function.hpp>
+#include <cxxabi.h>
 
 /** \brief Generic interface to adapting motion planning requests */
 namespace planning_request_adapter
@@ -63,6 +64,22 @@ class PlanningRequestAdapter
   {
   }
 
+  /// Initialize parameters using the passed NodeHandle
+  // TODO - Make initialize() a pure virtual function in O-turtle
+  virtual void initialize(const ros::NodeHandle& node_handle)
+  {
+    // Get name of derived adapter
+    std::string adapter_name = typeid(*this).name();
+    // Try to demangle the name
+    int status;
+    std::string demangled_name = abi::__cxa_demangle(adapter_name.c_str(), NULL, NULL, &status);
+    if (status == 0)
+      adapter_name = demangled_name;
+    ROS_WARN_NAMED("planning_request_adapter", "Implementation of function initialize() is missing from '%s'."
+                                               "Any parameters should be loaded from the passed NodeHandle.",
+                   adapter_name.c_str());
+  }
+
   /// Get a short string that identifies the planning request adapter
   virtual std::string getDescription() const
   {

moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp

@@ -58,96 +58,100 @@ namespace chomp
 class OptimizerAdapter : public planning_request_adapter::PlanningRequestAdapter
 {
 public:
-  OptimizerAdapter() : planning_request_adapter::PlanningRequestAdapter(), nh_("~")
+  OptimizerAdapter() : planning_request_adapter::PlanningRequestAdapter()
   {
-    if (!nh_.getParam("planning_time_limit", params_.planning_time_limit_))
+  }
+
+  void initialize(const ros::NodeHandle& nh) override
+  {
+    if (!nh.getParam("planning_time_limit", params_.planning_time_limit_))
     {
       params_.planning_time_limit_ = 10.0;
       ROS_INFO_STREAM("Param planning_time_limit was not set. Using default value: " << params_.planning_time_limit_);
     }
-    if (!nh_.getParam("max_iterations", params_.max_iterations_))
+    if (!nh.getParam("max_iterations", params_.max_iterations_))
     {
       params_.max_iterations_ = 200;
       ROS_INFO_STREAM("Param max_iterations was not set. Using default value: " << params_.max_iterations_);
     }
-    if (!nh_.getParam("max_iterations_after_collision_free", params_.max_iterations_after_collision_free_))
+    if (!nh.getParam("max_iterations_after_collision_free", params_.max_iterations_after_collision_free_))
     {
       params_.max_iterations_after_collision_free_ = 5;
       ROS_INFO_STREAM("Param max_iterations_after_collision_free was not set. Using default value: "
                       << params_.max_iterations_after_collision_free_);
     }
-    if (!nh_.getParam("smoothness_cost_weight", params_.smoothness_cost_weight_))
+    if (!nh.getParam("smoothness_cost_weight", params_.smoothness_cost_weight_))
     {
       params_.smoothness_cost_weight_ = 0.1;
       ROS_INFO_STREAM(
           "Param smoothness_cost_weight was not set. Using default value: " << params_.smoothness_cost_weight_);
     }
-    if (!nh_.getParam("obstacle_cost_weight", params_.obstacle_cost_weight_))
+    if (!nh.getParam("obstacle_cost_weight", params_.obstacle_cost_weight_))
     {
       params_.obstacle_cost_weight_ = 1.0;
       ROS_INFO_STREAM("Param obstacle_cost_weight was not set. Using default value: " << params_.obstacle_cost_weight_);
     }
-    if (!nh_.getParam("learning_rate", params_.learning_rate_))
+    if (!nh.getParam("learning_rate", params_.learning_rate_))
     {
       params_.learning_rate_ = 0.01;
       ROS_INFO_STREAM("Param learning_rate was not set. Using default value: " << params_.learning_rate_);
     }
-    if (!nh_.getParam("smoothness_cost_velocity", params_.smoothness_cost_velocity_))
+    if (!nh.getParam("smoothness_cost_velocity", params_.smoothness_cost_velocity_))
     {
       params_.smoothness_cost_velocity_ = 0.0;
       ROS_INFO_STREAM(
           "Param smoothness_cost_velocity was not set. Using default value: " << params_.smoothness_cost_velocity_);
     }
-    if (!nh_.getParam("smoothness_cost_acceleration", params_.smoothness_cost_acceleration_))
+    if (!nh.getParam("smoothness_cost_acceleration", params_.smoothness_cost_acceleration_))
     {
       params_.smoothness_cost_acceleration_ = 1.0;
       ROS_INFO_STREAM("Param smoothness_cost_acceleration was not set. Using default value: "
                       << params_.smoothness_cost_acceleration_);
     }
-    if (!nh_.getParam("smoothness_cost_jerk", params_.smoothness_cost_jerk_))
+    if (!nh.getParam("smoothness_cost_jerk", params_.smoothness_cost_jerk_))
     {
       params_.smoothness_cost_jerk_ = 0.0;
       ROS_INFO_STREAM(
           "Param smoothness_cost_jerk_ was not set. Using default value: " << params_.smoothness_cost_jerk_);
     }
-    if (!nh_.getParam("ridge_factor", params_.ridge_factor_))
+    if (!nh.getParam("ridge_factor", params_.ridge_factor_))
     {
       params_.ridge_factor_ = 0.0;
       ROS_INFO_STREAM("Param ridge_factor_ was not set. Using default value: " << params_.ridge_factor_);
     }
-    if (!nh_.getParam("use_pseudo_inverse", params_.use_pseudo_inverse_))
+    if (!nh.getParam("use_pseudo_inverse", params_.use_pseudo_inverse_))
     {
       params_.use_pseudo_inverse_ = 0.0;
       ROS_INFO_STREAM("Param use_pseudo_inverse_ was not set. Using default value: " << params_.use_pseudo_inverse_);
     }
-    if (!nh_.getParam("pseudo_inverse_ridge_factor", params_.pseudo_inverse_ridge_factor_))
+    if (!nh.getParam("pseudo_inverse_ridge_factor", params_.pseudo_inverse_ridge_factor_))
     {
       params_.pseudo_inverse_ridge_factor_ = 1e-4;
       ROS_INFO_STREAM("Param pseudo_inverse_ridge_factor was not set. Using default value: "
                       << params_.pseudo_inverse_ridge_factor_);
     }
-    if (!nh_.getParam("joint_update_limit", params_.joint_update_limit_))
+    if (!nh.getParam("joint_update_limit", params_.joint_update_limit_))
     {
       params_.joint_update_limit_ = 0.1;
       ROS_INFO_STREAM("Param joint_update_limit was not set. Using default value: " << params_.joint_update_limit_);
     }
-    if (!nh_.getParam("min_clearence", params_.min_clearence_))
+    if (!nh.getParam("min_clearence", params_.min_clearence_))
     {
       params_.min_clearence_ = 0.2;
       ROS_INFO_STREAM("Param min_clearence was not set. Using default value: " << params_.min_clearence_);
     }
-    if (!nh_.getParam("collision_threshold", params_.collision_threshold_))
+    if (!nh.getParam("collision_threshold", params_.collision_threshold_))
     {
       params_.collision_threshold_ = 0.07;
       ROS_INFO_STREAM("Param collision_threshold_ was not set. Using default value: " << params_.collision_threshold_);
     }
-    if (!nh_.getParam("use_stochastic_descent", params_.use_stochastic_descent_))
+    if (!nh.getParam("use_stochastic_descent", params_.use_stochastic_descent_))
     {
       params_.use_stochastic_descent_ = true;
       ROS_INFO_STREAM(
           "Param use_stochastic_descent was not set. Using default value: " << params_.use_stochastic_descent_);
     }
-    if (!nh_.getParam("trajectory_initialization_method", params_.trajectory_initialization_method_))
+    if (!nh.getParam("trajectory_initialization_method", params_.trajectory_initialization_method_))
     {
       params_.trajectory_initialization_method_ = std::string("fillTrajectory");
       ROS_INFO_STREAM("Param trajectory_initialization_method was not set. Using New value as: "
@@ -195,7 +199,6 @@ class OptimizerAdapter : public planning_request_adapter::PlanningRequestAdapter
   }
 
 private:
-  ros::NodeHandle nh_;
   chomp::ChompParameters params_;
 };
 }  // namespace chomp

moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp

@@ -142,7 +142,7 @@ void planning_pipeline::PlanningPipeline::configure()
     if (adapter_plugin_loader_)
       for (const std::string& adapter_plugin_name : adapter_plugin_names_)
       {
-        planning_request_adapter::PlanningRequestAdapterConstPtr ad;
+        planning_request_adapter::PlanningRequestAdapterPtr ad;
         try
         {
           ad = adapter_plugin_loader_->createUniqueInstance(adapter_plugin_name);
@@ -153,7 +153,10 @@ void planning_pipeline::PlanningPipeline::configure()
                                                                                << "': " << ex.what());
         }
         if (ad)
-          ads.push_back(ad);
+        {
+          ad->initialize(nh_);
+          ads.push_back(std::move(ad));
+        }
       }
     if (!ads.empty())
     {

moveit_ros/planning/planning_request_adapter_plugins/src/add_iterative_spline_parameterization.cpp

@@ -49,6 +49,10 @@ class AddIterativeSplineParameterization : public planning_request_adapter::Plan
   {
   }
 
+  void initialize(const ros::NodeHandle& nh) override
+  {
+  }
+
   std::string getDescription() const override
   {
     return "Add Time Parameterization";

moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp

@@ -51,6 +51,10 @@ class AddTimeOptimalParameterization : public planning_request_adapter::Planning
   {
   }
 
+  void initialize(const ros::NodeHandle& nh) override
+  {
+  }
+
   std::string getDescription() const override
   {
     return "Add Time Optimal Parameterization";

moveit_ros/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp

@@ -48,6 +48,10 @@ class AddTimeParameterization : public planning_request_adapter::PlanningRequest
   {
   }
 
+  void initialize(const ros::NodeHandle& nh) override
+  {
+  }
+
   std::string getDescription() const override
   {
     return "Add Time Parameterization";

moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp

@@ -53,6 +53,10 @@ class Empty : public planning_request_adapter::PlanningRequestAdapter
   {
     return planner(planning_scene, req, res);
   }
+
+  void initialize(const ros::NodeHandle& node_handle) override
+  {
+  }
 };
 }  // namespace default_planner_request_adapters
 

moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp

@@ -49,17 +49,21 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap
   static const std::string BOUNDS_PARAM_NAME;
   static const std::string DT_PARAM_NAME;
 
-  FixStartStateBounds() : planning_request_adapter::PlanningRequestAdapter(), nh_("~")
+  FixStartStateBounds() : planning_request_adapter::PlanningRequestAdapter()
   {
-    if (!nh_.getParam(BOUNDS_PARAM_NAME, bounds_dist_))
+  }
+
+  void initialize(const ros::NodeHandle& nh) override
+  {
+    if (!nh.getParam(BOUNDS_PARAM_NAME, bounds_dist_))
     {
       bounds_dist_ = 0.05;
       ROS_INFO_STREAM("Param '" << BOUNDS_PARAM_NAME << "' was not set. Using default value: " << bounds_dist_);
     }
     else
       ROS_INFO_STREAM("Param '" << BOUNDS_PARAM_NAME << "' was set to " << bounds_dist_);
 
-    if (!nh_.getParam(DT_PARAM_NAME, max_dt_offset_))
+    if (!nh.getParam(DT_PARAM_NAME, max_dt_offset_))
     {
       max_dt_offset_ = 0.5;
       ROS_INFO_STREAM("Param '" << DT_PARAM_NAME << "' was not set. Using default value: " << max_dt_offset_);
@@ -201,7 +205,6 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap
   }
 
 private:
-  ros::NodeHandle nh_;
   double bounds_dist_;
   double max_dt_offset_;
 };

moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp

@@ -49,25 +49,29 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA
   static const std::string JIGGLE_PARAM_NAME;
   static const std::string ATTEMPTS_PARAM_NAME;
 
-  FixStartStateCollision() : planning_request_adapter::PlanningRequestAdapter(), nh_("~")
+  FixStartStateCollision() : planning_request_adapter::PlanningRequestAdapter()
   {
-    if (!nh_.getParam(DT_PARAM_NAME, max_dt_offset_))
+  }
+
+  void initialize(const ros::NodeHandle& nh) override
+  {
+    if (!nh.getParam(DT_PARAM_NAME, max_dt_offset_))
     {
       max_dt_offset_ = 0.5;
       ROS_INFO_STREAM("Param '" << DT_PARAM_NAME << "' was not set. Using default value: " << max_dt_offset_);
     }
     else
       ROS_INFO_STREAM("Param '" << DT_PARAM_NAME << "' was set to " << max_dt_offset_);
 
-    if (!nh_.getParam(JIGGLE_PARAM_NAME, jiggle_fraction_))
+    if (!nh.getParam(JIGGLE_PARAM_NAME, jiggle_fraction_))
     {
       jiggle_fraction_ = 0.02;
       ROS_INFO_STREAM("Param '" << JIGGLE_PARAM_NAME << "' was not set. Using default value: " << jiggle_fraction_);
     }
     else
       ROS_INFO_STREAM("Param '" << JIGGLE_PARAM_NAME << "' was set to " << jiggle_fraction_);
 
-    if (!nh_.getParam(ATTEMPTS_PARAM_NAME, sampling_attempts_))
+    if (!nh.getParam(ATTEMPTS_PARAM_NAME, sampling_attempts_))
     {
       sampling_attempts_ = 100;
       ROS_INFO_STREAM("Param '" << ATTEMPTS_PARAM_NAME << "' was not set. Using default value: " << sampling_attempts_);
@@ -182,7 +186,6 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA
   }
 
 private:
-  ros::NodeHandle nh_;
   double max_dt_offset_;
   double jiggle_fraction_;
   int sampling_attempts_;

moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp

@@ -50,6 +50,10 @@ class FixStartStatePathConstraints : public planning_request_adapter::PlanningRe
   {
   }
 
+  void initialize(const ros::NodeHandle& nh) override
+  {
+  }
+
   std::string getDescription() const override
   {
     return "Fix Start State Path Constraints";

moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp

@@ -45,9 +45,13 @@ class FixWorkspaceBounds : public planning_request_adapter::PlanningRequestAdapt
 public:
   static const std::string WBOUNDS_PARAM_NAME;
 
-  FixWorkspaceBounds() : planning_request_adapter::PlanningRequestAdapter(), nh_("~")
+  FixWorkspaceBounds() : planning_request_adapter::PlanningRequestAdapter()
   {
-    if (!nh_.getParam(WBOUNDS_PARAM_NAME, workspace_extent_))
+  }
+
+  void initialize(const ros::NodeHandle& nh) override
+  {
+    if (!nh.getParam(WBOUNDS_PARAM_NAME, workspace_extent_))
     {
       workspace_extent_ = 10.0;
       ROS_INFO_STREAM("Param '" << WBOUNDS_PARAM_NAME << "' was not set. Using default value: " << workspace_extent_);
@@ -84,7 +88,6 @@ class FixWorkspaceBounds : public planning_request_adapter::PlanningRequestAdapt
   }
 
 private:
-  ros::NodeHandle nh_;
   double workspace_extent_;
 };
 

moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp

@@ -48,6 +48,10 @@ class ResolveConstraintFrames : public planning_request_adapter::PlanningRequest
   {
   }
 
+  void initialize(const ros::NodeHandle& nh) override
+  {
+  }
+
   std::string getDescription() const override
   {
     return "Resolve constraint frames to robot links";