Added Minimum Translational Velocity Limit

cfg/TebLocalPlannerReconfigure.cfg

@@ -82,11 +82,15 @@ grp_robot = gen.add_group("Robot", type="tab")
 
 grp_robot.add("max_vel_x", double_t, 0, 
 	"Maximum translational velocity of the robot",
-	0.4, 0.01, 100)   
+	0.4, 0.01, 100)
+
+grp_robot.add("min_vel_x", double_t, 0, 
+	"Minimum translational velocity of the robot",
+	0.01, 0.01, 100)   
 
 grp_robot.add("max_vel_x_backwards", double_t, 0, 
 	"Maximum translational velocity of the robot for driving backwards",
-	0.2, 0.01, 100)   
+	0.2, 0.01, 100)  
 
 grp_robot.add("max_vel_theta", double_t, 0,
 	"Maximum angular velocity of the robot", 

include/teb_local_planner/teb_config.h

@@ -92,6 +92,7 @@ class TebConfig
   struct Robot
   {
     double max_vel_x; //!< Maximum translational velocity of the robot
+    double min_vel_x; //!< Minimum translational velocity of the robot
     double max_vel_x_backwards; //!< Maximum translational velocity of the robot for driving backwards
     double max_vel_y; //!< Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)
     double max_vel_theta; //!< Maximum angular velocity of the robot
@@ -104,6 +105,7 @@ class TebConfig
     bool is_footprint_dynamic; //<! If true, updated the footprint before checking trajectory feasibility
     bool use_proportional_saturation; //<! If true, reduce all twists components (linear x and y, and angular z) proportionally if any exceed its corresponding bounds, instead of saturating each one individually
     double transform_tolerance = 0.5; //<! Tolerance when querying the TF Tree for a transformation (seconds)
+
   } robot; //!< Robot related parameters
 
   //! Goal tolerance related parameters

include/teb_local_planner/teb_local_planner_ros.h

@@ -355,7 +355,7 @@ class TebLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap
    * @param max_vel_x_backwards Maximum translational velocity for backwards driving
    */
   void saturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double max_vel_y,
-                        double max_vel_theta, double max_vel_x_backwards) const;
+                        double max_vel_theta, double max_vel_x_backwards, double min_vel_x) const;
 
 
   /**

src/teb_config.cpp

@@ -71,6 +71,7 @@ void TebConfig::loadRosParamFromNodeHandle(const ros::NodeHandle& nh)
 
   // Robot
   nh.param("max_vel_x", robot.max_vel_x, robot.max_vel_x);
+  nh.param("min_vel_x", robot.min_vel_x, robot.min_vel_x);
   nh.param("max_vel_x_backwards", robot.max_vel_x_backwards, robot.max_vel_x_backwards);
   nh.param("max_vel_y", robot.max_vel_y, robot.max_vel_y);
   nh.param("max_vel_theta", robot.max_vel_theta, robot.max_vel_theta);
@@ -83,6 +84,8 @@ void TebConfig::loadRosParamFromNodeHandle(const ros::NodeHandle& nh)
   nh.param("is_footprint_dynamic", robot.is_footprint_dynamic, robot.is_footprint_dynamic);
   nh.param("use_proportional_saturation", robot.use_proportional_saturation, robot.use_proportional_saturation);
   nh.param("transform_tolerance", robot.transform_tolerance, robot.transform_tolerance);
+
+
 
   // GoalTolerance
   nh.param("xy_goal_tolerance", goal_tolerance.xy_goal_tolerance, goal_tolerance.xy_goal_tolerance);
@@ -200,6 +203,7 @@ void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg)
 
   // Robot     
   robot.max_vel_x = cfg.max_vel_x;
+  robot.min_vel_x = cfg.min_vel_x;
   robot.max_vel_x_backwards = cfg.max_vel_x_backwards;
   robot.max_vel_y = cfg.max_vel_y;
   robot.max_vel_theta = cfg.max_vel_theta;

src/teb_local_planner_ros.cpp

@@ -421,7 +421,7 @@ uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt
 
   // Saturate velocity, if the optimization results violates the constraints (could be possible due to soft constraints).
   saturateVelocity(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z,
-                   cfg_.robot.max_vel_x, cfg_.robot.max_vel_y, cfg_.robot.max_vel_theta, cfg_.robot.max_vel_x_backwards);
+                   cfg_.robot.max_vel_x, cfg_.robot.max_vel_y, cfg_.robot.max_vel_theta, cfg_.robot.max_vel_x_backwards, cfg_.robot.min_vel_x );
 
   // convert rot-vel to steering angle if desired (carlike robot).
   // The min_turning_radius is allowed to be slighly smaller since it is a soft-constraint
@@ -869,13 +869,28 @@ double TebLocalPlannerROS::estimateLocalGoalOrientation(const std::vector<geomet
 }
 
 
-void TebLocalPlannerROS::saturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double max_vel_y, double max_vel_theta, double max_vel_x_backwards) const
+void TebLocalPlannerROS::saturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double max_vel_y, double max_vel_theta, double max_vel_x_backwards, double min_vel_x) const
 {
-  double ratio_x = 1, ratio_omega = 1, ratio_y = 1;
-  // Limit translational velocity for forward driving
-  if (vx > max_vel_x)
-    ratio_x = max_vel_x / vx;
-
+  double ratio_x = 1, ratio_omega = 1, ratio_y = 1, ratio_x_min = 1;
+
+  // backward driving
+  if(vx < 0)
+  {  
+    if (vx < -max_vel_x_backwards)
+      ratio_x = - max_vel_x_backwards / vx;
+    else if (vx > -min_vel_x)
+      ratio_x = - min_vel_x / vx;
+  }
+  else // forward driving
+  {
+    if (vx > max_vel_x)
+      ratio_x = max_vel_x / vx;
+    else if (vx < min_vel_x)
+    {
+      ratio_x =   min_vel_x / vx;
+    }
+  }
+
   // limit strafing velocity
   if (vy > max_vel_y || vy < -max_vel_y)
     ratio_y = std::abs(vy / max_vel_y);
@@ -901,9 +916,10 @@ void TebLocalPlannerROS::saturateVelocity(double& vx, double& vy, double& omega,
   }
   else
   {
-    vx *= ratio_x;
+    vx *= ratio_x; 
     vy *= ratio_y;
     omega *= ratio_omega;
+
   }
 }