Added Minimum Translational Velocity Limit

cfg/TebLocalPlannerReconfigure.cfg

@@ -86,7 +86,11 @@ grp_robot.add("max_vel_x", double_t, 0,
 
 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("min_vel_x", double_t, 0, 
+	"Minimum translational velocity of the robot",
+	0.4, 0.01, 100)    
 
 grp_robot.add("max_vel_theta", double_t, 0,
 	"Maximum angular velocity of the robot", 

cfg/planner.yaml

@@ -0,0 +1,113 @@
+# ########### Teb Local Planner
+TebLocalPlannerROS:
+
+#  odom_topic: odom
+ odom_topic: odom
+ map_frame: /odom
+ # Trajectory
+
+ teb_autosize: True
+ dt_ref: 0.3
+ dt_hysteresis: 0.1
+ max_samples: 500
+ global_plan_overwrite_orientation: True
+ allow_init_with_backwards_motion: True
+ max_global_plan_lookahead_dist: 3.0
+ global_plan_viapoint_sep: -1
+ global_plan_prune_distance: 1
+ exact_arc_length: True
+ feasibility_check_no_poses: 5
+ publish_feedback: False
+
+ # Robot
+
+ max_vel_x: 0.055
+ max_vel_x_backwards: 0.055
+ min_vel_x: 0.04
+ max_vel_y: 0.0
+ max_vel_theta: 0.45
+ acc_lim_x: 0.5
+ acc_lim_theta: 0.2
+ min_turning_radius: 0.2 # diff-drive robot (can turn on place!)
+
+ footprint_model:
+   type: "point"
+
+ # GoalTolerance
+
+ xy_goal_tolerance: 0.2
+ yaw_goal_tolerance: 0.1
+ free_goal_vel: False
+ complete_global_plan: True
+
+ # Obstacles
+
+ min_obstacle_dist: 0.25 # This value must also include our robot radius, since footprint_model is set to "point".
+ inflation_dist: 0.6
+ include_costmap_obstacles: True
+ costmap_obstacles_behind_robot_dist: 1.5
+ obstacle_poses_affected: 15
+
+ dynamic_obstacle_inflation_dist: 0.6
+ include_dynamic_obstacles: True
+
+ costmap_converter_plugin: ""
+ costmap_converter_spin_thread: True
+ costmap_converter_rate: 5
+
+ # Optimization
+
+ no_inner_iterations: 5
+ no_outer_iterations: 4
+ optimization_activate: True
+ optimization_verbose: False
+ penalty_epsilon: 0.01
+ obstacle_cost_exponent: 4
+ weight_max_vel_x: 2
+ weight_max_vel_theta: 1
+ weight_acc_lim_x: 1
+ weight_acc_lim_theta: 1
+ weight_kinematics_nh: 1000
+ weight_kinematics_forward_drive: 1
+ weight_kinematics_turning_radius: 1
+ weight_optimaltime: 1 # must be > 0
+ weight_shortest_path: 0
+ weight_obstacle: 100
+ weight_inflation: 0.2
+ weight_dynamic_obstacle: 10
+ weight_dynamic_obstacle_inflation: 0.2
+ weight_viapoint: 1
+ weight_adapt_factor: 2
+
+ # Homotopy Class Planner
+
+ enable_homotopy_class_planning: True
+ enable_multithreading: True
+ max_number_classes: 4
+ selection_cost_hysteresis: 1.0
+ selection_prefer_initial_plan: 0.9
+ selection_obst_cost_scale: 100.0
+ selection_alternative_time_cost: False
+
+ roadmap_graph_no_samples: 15
+ roadmap_graph_area_width: 5
+ roadmap_graph_area_length_scale: 1.0
+ h_signature_prescaler: 0.5
+ h_signature_threshold: 0.1
+ obstacle_heading_threshold: 0.45
+ switching_blocking_period: 0.0
+ viapoints_all_candidates: True
+ delete_detours_backwards: True
+ max_ratio_detours_duration_best_duration: 3.0
+ visualize_hc_graph: False
+ visualize_with_time_as_z_axis_scale: False
+
+# Recovery
+
+ shrink_horizon_backup: True
+ shrink_horizon_min_duration: 10
+ oscillation_recovery: True
+ oscillation_v_eps: 0.1
+ oscillation_omega_eps: 0.1
+ oscillation_recovery_min_duration: 10
+ oscillation_filter_duration: 10

include/teb_local_planner/teb_config.h

@@ -104,6 +104,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)
+    double min_vel_x;
   } 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

@@ -83,6 +83,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);
+  nh.param("min_vel_x", robot.min_vel_x, robot.min_vel_x);
+
 
   // GoalTolerance
   nh.param("xy_goal_tolerance", goal_tolerance.xy_goal_tolerance, goal_tolerance.xy_goal_tolerance);
@@ -200,6 +202,8 @@ 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,34 @@ 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;
+  bool direction = true;
+
+  // backward driving
+  if(vx < 0)
+  {
+    direction = false ;
+
+    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
+  {
+    direction = true;
+
+    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 +922,11 @@ void TebLocalPlannerROS::saturateVelocity(double& vx, double& vy, double& omega,
   }
   else
   {
-    vx *= ratio_x;
+
+    vx *= ratio_x; 
     vy *= ratio_y;
     omega *= ratio_omega;
+
   }
 }