address feedback

moveit · Jan 17, 2023 · 7baf50d · 7baf50d
1 parent 398e11c
commit 7baf50d
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 24 deletions.
diff --git a/...tory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h b/...tory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h
@@ -247,13 +247,12 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization
   bool hasMixedJointTypes(const moveit::core::JointModelGroup* group) const;
 
   /**
-   * @brief Check if the max scaling factor is valid and if not, set it to the specified default value
-   * \param[in,out] scaling_factor The default scaling factor that should be applied
-   * if the `max_scaling_factor` is invalid
-   * \param max_scaling_factor The desired maximum scaling factor to apply to the velocity or acceleration limits
+   * @brief Check if the requested scaling factor is valid and if not, return 1.0.
+   * \param requested_scaling_factor The desired maximum scaling factor to apply to the velocity or acceleration limits
    * \param limit_type Whether the velocity or acceleration scaling factor is being verified
+   * \return The user requested scaling factor, if it is valid. Otherwise, return 1.0.
    */
-  void verifyScalingFactor(double& scaling_factor, const double max_scaling_factor, const LimitType limit_type) const;
+  double verifyScalingFactor(const double requested_scaling_factor, const LimitType limit_type) const;
 
   const double path_tolerance_;
   const double resample_dt_;

diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
@@ -53,6 +53,7 @@ static const rclcpp::Logger LOGGER =
     rclcpp::get_logger("moveit_trajectory_processing.time_optimal_trajectory_generation");
 constexpr double DEFAULT_TIMESTEP = 1e-3;
 constexpr double EPS = 1e-6;
+constexpr double DEFAULT_SCALING_FACTOR = 1.0;
 }  // namespace
 
 class LinearPathSegment : public PathSegment
@@ -880,11 +881,10 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
   }
 
   // Validate scaling
-  double velocity_scaling_factor = 1.0;
-  verifyScalingFactor(velocity_scaling_factor, max_velocity_scaling_factor, VELOCITY);
+  double velocity_scaling_factor = verifyScalingFactor(velocity_scaling_factor, max_velocity_scaling_factor, VELOCITY);
 
-  double acceleration_scaling_factor = 1.0;
-  verifyScalingFactor(acceleration_scaling_factor, max_acceleration_scaling_factor, ACCELERATION);
+  double acceleration_scaling_factor =
+      verifyScalingFactor(acceleration_scaling_factor, max_acceleration_scaling_factor, ACCELERATION);
 
   const std::vector<std::string>& vars = group->getVariableNames();
   const moveit::core::RobotModel& rmodel = group->getParentModel();
@@ -980,11 +980,10 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(
   }
 
   // Validate scaling
-  double velocity_scaling_factor = 1.0;
-  verifyScalingFactor(velocity_scaling_factor, max_velocity_scaling_factor, VELOCITY);
+  double velocity_scaling_factor = verifyScalingFactor(velocity_scaling_factor, max_velocity_scaling_factor, VELOCITY);
 
-  double acceleration_scaling_factor = 1.0;
-  verifyScalingFactor(acceleration_scaling_factor, max_acceleration_scaling_factor, ACCELERATION);
+  double acceleration_scaling_factor =
+      verifyScalingFactor(acceleration_scaling_factor, max_acceleration_scaling_factor, ACCELERATION);
 
   const unsigned num_joints = group->getVariableCount();
   const std::vector<std::string>& vars = group->getVariableNames();
@@ -1211,30 +1210,27 @@ bool TimeOptimalTrajectoryGeneration::hasMixedJointTypes(const moveit::core::Joi
   return have_prismatic && have_revolute;
 }
 
-void TimeOptimalTrajectoryGeneration::verifyScalingFactor(double& scaling_factor, const double max_scaling_factor,
-                                                          const LimitType limit_type) const
+double TimeOptimalTrajectoryGeneration::verifyScalingFactor(const double requested_scaling_factor,
+                                                            const LimitType limit_type) const
 {
   std::string limit_type_str;
-  auto limit_type_it = LIMIT_TYPES.find(limit_type);
+  double scaling_factor = DEFAULT_SCALING_FACTOR;
 
+  const auto limit_type_it = LIMIT_TYPES.find(limit_type);
   if (limit_type_it != LIMIT_TYPES.end())
   {
     limit_type_str = limit_type_it->second + "_";
   }
 
-  if (max_scaling_factor > 0.0 && max_scaling_factor <= 1.0)
+  if (requested_scaling_factor > 0.0 && requested_scaling_factor <= 1.0)
   {
-    scaling_factor = max_scaling_factor;
-  }
-  else if (max_scaling_factor == 0.0)
-  {
-    RCLCPP_DEBUG(LOGGER, "A max_%sscaling_factor of 0.0 was specified, defaulting to %f instead.",
-                 limit_type_str.c_str(), scaling_factor);
+    scaling_factor = requested_scaling_factor;
   }
   else
   {
     RCLCPP_WARN(LOGGER, "Invalid max_%sscaling_factor %f specified, defaulting to %f instead.", limit_type_str.c_str(),
-                max_scaling_factor, scaling_factor);
+                requested_scaling_factor, scaling_factor);
   }
+  return scaling_factor;
 }
 }  // namespace trajectory_processing