Sanitize CHOMP initialization method parameter (moveit#2540)

tylerjw · Apr 29, 2021 · 7b6a0b9 · 7b6a0b9
1 parent ac0ac4b
commit 7b6a0b9
Show file tree

Hide file tree

Showing 5 changed files with 44 additions and 5 deletions.
diff --git a/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp b/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp
@@ -69,8 +69,13 @@ void CHOMPInterface::loadParams()
   nh_.param("collision_threshold", params_.collision_threshold_, 0.07);
   // nh_.param("random_jump_amount", params_.random_jump_amount_, 1.0);
   nh_.param("use_stochastic_descent", params_.use_stochastic_descent_, true);
-  nh_.param("trajectory_initialization_method", params_.trajectory_initialization_method_,
-            std::string("quintic-spline"));
+  {
+    params_.trajectory_initialization_method_ = "quintic-spline";
+    std::string method;
+    if (nh_.getParam("trajectory_initialization_method", method) && !params_.setTrajectoryInitializationMethod(method))
+      ROS_ERROR_STREAM("Attempted to set trajectory_initialization_method to invalid value '"
+                       << method << "'. Using default '" << params_.trajectory_initialization_method_ << "' instead.");
+  }
   nh_.param("enable_failure_recovery", params_.enable_failure_recovery_, false);
   nh_.param("max_recovery_attempts", params_.max_recovery_attempts_, 5);
 }

diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h
@@ -57,6 +57,12 @@ class ChompParameters
    */
   void setRecoveryParams(double learning_rate, double ridge_factor, int planning_time_limit, int max_iterations);
 
+  /**
+   * sets a valid trajectory initialization method
+   * @return true if a valid method (one of VALID_INITIALIZATION_METHODS) was specified
+   */
+  bool setTrajectoryInitializationMethod(std::string method);
+
 public:
   double planning_time_limit_;  /// maximum time the optimizer can take to find a solution before terminating
   int max_iterations_;          /// maximum number of iterations that the planner can take to find a good solution while
@@ -90,7 +96,10 @@ class ChompParameters
   double collision_threshold_;  /// the collision threshold cost that needs to be mainted to avoid collisions
   bool filter_mode_;
   // double random_jump_amount_;
+
+  static const std::vector<std::string> VALID_INITIALIZATION_METHODS;
   std::string trajectory_initialization_method_;  /// trajectory initialization method to be specified
+
   bool enable_failure_recovery_;  /// if set to true, CHOMP tries to vary certain parameters to try and find a path if
                                   /// an initial path is not found with the specified chomp parameters
   int max_recovery_attempts_;     /// this the maximum recovery attempts to find a collision free path after an initial

diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_parameters.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_parameters.cpp
@@ -80,4 +80,18 @@ void ChompParameters::setRecoveryParams(double learning_rate, double ridge_facto
   this->planning_time_limit_ = planning_time_limit;
   this->max_iterations_ = max_iterations;
 }
+
+const std::vector<std::string> ChompParameters::VALID_INITIALIZATION_METHODS{ "quintic-spline", "linear", "cubic",
+                                                                              "fillTrajectory" };
+
+bool ChompParameters::setTrajectoryInitializationMethod(std::string method)
+{
+  if (std::find(VALID_INITIALIZATION_METHODS.cbegin(), VALID_INITIALIZATION_METHODS.cend(), method) !=
+      VALID_INITIALIZATION_METHODS.end())
+  {
+    this->trajectory_initialization_method_ = std::move(method);
+    return true;
+  }
+  return false;
+}
 }  // namespace chomp
diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp
@@ -136,7 +136,10 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s
     }
   }
   else
+  {
     ROS_ERROR_STREAM_NAMED("chomp_planner", "invalid interpolation method specified in the chomp_planner file");
+    return false;
+  }
 
   ROS_INFO_NAMED("chomp_planner", "CHOMP trajectory initialized using method: %s ",
                  (params.trajectory_initialization_method_).c_str());

diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp b/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp
@@ -146,12 +146,20 @@ class OptimizerAdapter : public planning_request_adapter::PlanningRequestAdapter
       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_))
+    // default
+    params_.trajectory_initialization_method_ = std::string("fillTrajectory");
+    std::string trajectory_initialization_method;
+    if (!nh.getParam("trajectory_initialization_method", trajectory_initialization_method))
     {
-      params_.trajectory_initialization_method_ = std::string("fillTrajectory");
-      ROS_INFO_STREAM("Param trajectory_initialization_method was not set. Using New value as: "
+      ROS_INFO_STREAM("Param trajectory_initialization_method was not set. Using value: "
                       << params_.trajectory_initialization_method_);
     }
+    else if (!params_.setTrajectoryInitializationMethod(trajectory_initialization_method))
+    {
+      ROS_ERROR_STREAM("Param trajectory_initialization_method set to invalid value '"
+                       << trajectory_initialization_method << "'. Using '" << params_.trajectory_initialization_method_
+                       << "' instead.");
+    }
   }
 
   std::string getDescription() const override