Controller rejects goals when its last setpoint does not agree with odom velocity (on init_vel_method pid_odom)

[rokusottervanger]

Talking about this piece of code in setPlan(), intended to avoid putting steps on the velocity setpoint:

path_tracking_pid/src/path_tracking_pid_local_planner.cpp

Lines 147 to 158 in 61c6cc8

	// Feasability check, but only when not resuming with odom-vel
	if (
	pid_controller_.getConfig().init_vel_method != Pid_Odom &&
	pid_controller_.getConfig().init_vel_max_diff >= 0.0 &&
	std::abs(
	latest_odom_.twist.twist.linear.x - pid_controller_.getControllerState().current_x_vel) >
	pid_controller_.getConfig().init_vel_max_diff) {
	ROS_ERROR(
	"Significant diff between odom (%f) and controller_state (%f) detected. Aborting!",
	latest_odom_.twist.twist.linear.x, pid_controller_.getControllerState().current_x_vel);
	return false;
	}

Currently, the controller rejects any new goal when the error between actual and setpoint velocity is too large. It is unknown what happens then, because the controller will not send any velocities anymore. So this depends on what third party control action or disturbance made this happen.

IMO, we should accept this goal (as there is nothing wrong with the goal itself. The goal doesn't say anything about velocities). In computeVelocityCommands, we should check that this error does not become too large. If that does happen, we should abort the goal, set the setpoint to the current odom and decrease that to zero, respecting our acceleration limits.

The behavior of the robot stays the same in the special case this was designed for: a manual override while controlling. Because if the actual velocity is zero, the error is too large, the goal is accepted, but it is aborted right away, controlling the velocity to zero (which it already is) and resetting the controller state.

But it should now also produce the desired behavior in all other cases: where the actual velocity is nonzero, but not similar to the controller state. The goal is accepted, but aborted due to a controller error. The velocity is smoothly decreased to zero, and the controller state is again in sync with the robot state.