Adding minimum X, Y and Theta speed limits to the planner

[pepeRossRobotics]

When using TEB local planner with some robots that have a lower speed limit for the motors, TEB tends to get 'suck' close to the goal due to the robot not moving.
This PR aims to avoid this via a minimum speed limit

[jcmonteiro]

Hey there. I'm not a maintainer, and it is not my decision to make, but in my opinion, it would be better to

1. rebase your commits to have a coherent/cohesive commit history, and
2. remove all indentation-related changes to keep the PR focused on the actual changes and avoid conflicts.

[jcmonteiro]

What happens if any one of vx, vy, and omega is zero?

[pepeRossRobotics]

If the speeds are zero this the planner should work as if they weren't set, in fact they are initialised as zero if not set by the user

if (vx > max_vel_x)
    ratio_x = max_vel_x / vx;
  else if (vx < min_vel_x && vx > 0.0)
    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);
  else if (std::abs(vy) < min_vel_y && std::abs(vy) > 0.0)
    ratio_y = std::abs( min_vel_y / vy);
  // Limit angular velocity
  if (omega > max_vel_theta || omega < -max_vel_theta)
    ratio_omega = std::abs(max_vel_theta / omega);
  else if (std::abs(omega) < min_vel_theta && std::abs(omega) > 0.0)
    ratio_omega = std::abs( min_vel_theta / omega);

In cfg/TebLocalPlannerReconfigure.cfg

# Robot
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)

grp_robot.add("min_vel_x", double_t, 0,
	"Minimum translational velocity of the robot",
	0.0, 0.0, 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)

grp_robot.add("min_vel_x_backwards", double_t, 0,
	"Minimum translational velocity of the robot for driving backwards",
	0.01, 0.01, 100)

grp_robot.add("max_vel_theta", double_t, 0,
	"Maximum angular velocity of the robot",
	0.3, 0.01, 100)

grp_robot.add("min_vel_theta", double_t, 0,
	"Min angular velocity of the robot",
	0.0, 0.00, 100)

grp_robot.add("acc_lim_x", double_t, 0,
	"Maximum translational acceleration of the robot",
	0.5, 0.01, 100)

grp_robot.add("acc_lim_theta", double_t, 0,
	"Maximum angular acceleration of the robot",
	0.5, 0.01, 100)

grp_robot.add("is_footprint_dynamic",   bool_t,   0,
  "If true, updated the footprint before checking trajectory feasibility",
  False)

grp_robot.add("use_proportional_saturation", bool_t, 0,
	"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",
	False)
grp_robot.add("transform_tolerance", double_t, 0,
	"Tolerance when querying the TF Tree for a transformation (seconds)",
	0.5, 0.001, 20)

[amakarow]

Thanks for this PR. However, we have some concerns merging this since overwriting the optimal motor commands with some custom values can have some unwanted side effects on the closed-loop performance. This is against the basic concept of predictive control. If you want to deal with friction forces, you have to use a dynamic model. Unfortunately, the TEB optimization model does not support this at the moment. Another option would be to switch to a custom control law very closed to the goal ensuring proper convergence under those additional constraints. Have you experienced any issues?

[corot]

We have discussed quite a lot about minimum velocities on this PR (I point to the final conclusions)

This kind of addition can be very problematic given given the general TEB architecture, so we will defer them by now, until someone with a deeper understanding takes a more rigorous look

All that said, thanks for the contribution, and please don't feel discouraged to do more in the future!