add max vel linear

cfg/TebLocalPlannerReconfigure.cfg

@@ -141,6 +141,10 @@ grp_robot_omni = grp_robot.add_group("Omnidirectional", type="hide")
 
 grp_robot_omni.add("max_vel_y", double_t, 0, 
   "Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)",
+  0.0, 0.0, 100)
+
+grp_robot_omni.add("max_vel_linear", double_t, 0, 
+  "Maximum linear velocity of the robot. Ignore this parameter for non-holonomic robots (max_vel_linear == max_vel_x).",
   0.0, 0.0, 100) 
 
 grp_robot_omni.add("acc_lim_y", double_t, 0, 

include/teb_local_planner/g2o_types/edge_velocity.h

@@ -251,9 +251,14 @@ class EdgeVelocityHolonomic : public BaseTebMultiEdge<3, double>
     double vx = r_dx / deltaT->estimate();
     double vy = r_dy / deltaT->estimate();
     double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) / deltaT->estimate();
-
+
+    double max_vel_linear = std::max(std::max(cfg_->robot.max_vel_x, cfg_->robot.max_vel_y), cfg_->robot.max_vel_linear);
+    // double max_linear_vel_left = std::sqrt(max_vel_linear * max_vel_linear - vx * vx); // L2
+    double max_linear_vel_left = max_vel_linear - std::abs(vx); // L1
+    double max_vel_y = std::min(max_linear_vel_left, cfg_->robot.max_vel_y);
+
     _error[0] = penaltyBoundToInterval(vx, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x, cfg_->optim.penalty_epsilon);
-    _error[1] = penaltyBoundToInterval(vy, cfg_->robot.max_vel_y, 0.0); // we do not apply the penalty epsilon here, since the velocity could be close to zero
+    _error[1] = penaltyBoundToInterval(vy, max_vel_y, 0.0); // we do not apply the penalty epsilon here, since the velocity could be close to zero
     _error[2] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon);
 
     ROS_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]) && std::isfinite(_error[2]),

include/teb_local_planner/teb_config.h

@@ -105,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)
+    double max_vel_linear; //!< Maximum translational velocity of the robot for omni robots, which is different from max_vel_x
   } robot; //!< Robot related parameters
 
   //! Goal tolerance related parameters
@@ -279,6 +280,7 @@ class TebConfig
     robot.cmd_angle_instead_rotvel = false;
     robot.is_footprint_dynamic = false;
     robot.use_proportional_saturation = false;
+    robot.max_vel_linear = robot.max_vel_x;
 
     // GoalTolerance
 

include/teb_local_planner/teb_local_planner_ros.h

@@ -354,9 +354,11 @@ class TebLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap
    * @param max_vel_y Maximum strafing velocity (for holonomic robots)
    * @param max_vel_theta Maximum (absolute) angular velocity
    * @param max_vel_x_backwards Maximum translational velocity for backwards driving
+   * @param max_vel_linear Maximum translational velocity holonomic robots
    */
   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 max_vel_linear) const;
 
 
   /**

src/teb_config.cpp

@@ -84,6 +84,7 @@ 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("max_vel_linear", robot.max_vel_linear, robot.max_vel_linear);
 
   // GoalTolerance
   nh.param("xy_goal_tolerance", goal_tolerance.xy_goal_tolerance, goal_tolerance.xy_goal_tolerance);
@@ -213,6 +214,7 @@ void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg)
   robot.wheelbase = cfg.wheelbase;
   robot.cmd_angle_instead_rotvel = cfg.cmd_angle_instead_rotvel;
   robot.use_proportional_saturation = cfg.use_proportional_saturation;
+  robot.max_vel_linear = cfg.max_vel_linear;
 
   // GoalTolerance
   goal_tolerance.xy_goal_tolerance = cfg.xy_goal_tolerance;
@@ -350,6 +352,13 @@ void TebConfig::checkParameters() const
   // weights
   if (optim.weight_optimaltime <= 0)
       ROS_WARN("TebLocalPlannerROS() Param Warning: parameter weight_optimaltime shoud be > 0 (even if weight_shortest_path is in use)");
+
+  // holonomic check
+  if (robot.max_vel_y >= 0) {
+    if (robot.max_vel_linear < robot.max_vel_x || robot.max_vel_linear < robot.max_vel_y) {
+      ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_linear < max_vel_x or max_vel_linear < max_vel_y. Assuming max(max_vel_x, max_vel_y)");
+    }
+  }
 
 }    
 

src/teb_local_planner_ros.cpp

@@ -421,7 +421,8 @@ 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.max_vel_linear );
 
   // 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
@@ -867,7 +868,8 @@ 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 max_vel_linear) const
 {
   double ratio_x = 1, ratio_omega = 1, ratio_y = 1;
   // Limit translational velocity for forward driving
@@ -903,6 +905,15 @@ void TebLocalPlannerROS::saturateVelocity(double& vx, double& vy, double& omega,
     vy *= ratio_y;
     omega *= ratio_omega;
   }
+
+  max_vel_linear = std::max(std::max(max_vel_x, max_vel_y), max_vel_linear);
+  double vel_linear = std::sqrt(vx*vx + vy*vy);
+  if (vel_linear > std::abs(max_vel_linear))
+  {
+    double max_vel_linear_ratio = max_vel_linear / vel_linear;
+    vx *= max_vel_linear_ratio;
+    vy *= max_vel_linear_ratio;
+  }
 }