Restructure and update differential rover module

[chfriedrich98]

Solved Problem

This PR adresses 2 issues:

1. Fixes naming inconsistency in the rover related modules
2. Restructures differential module and improves guidance by implementing the pure pursuit algorithm (Add pure pursuit library #23387).

Solution

1. Rename module from differential drive to rover differential (same naming convention as rover ackermann module)
2. Restructure:

2a. Bring folder structure in line with ackermann module:

2b. With the restructure in 2a the DifferentialDriveSetpoint message becomes obsolete and is instead replaced with RoverDifferentialStatus and RoverDifferentialGuidanceStatus which are used exclusively for logging purposes (in line with the ackermann module):
RoverDifferentialStatus (General logging):

Variable	Description	Unit
actual_speed	Actual speed of the rover	m/s
desired_yaw_rate	Desired yaw rate of the rover	deg/s
actual_yaw_rate	Actual yaw rate of the rover	deg/s
pid_yaw_rate_integral	Integral of the PID for the desired yaw rate controller	-

RoverDifferentialGuidanceStatus (Mission specific logging):

Variable	Description	Unit
desired_speed	Desired speed of the rover	m/s
lookahead_distance	Lookahead distance of pure the pursuit controller	m
heading_error	Heading error of the pure pursuit controller	deg
pid_heading_integral	Integral of the PID for the desired yaw rate during missions	-
pid_throttle_integral	Integral of the PID for the throttle during missions	-

2c. Implement pure pursuit in the guidance algorithm:
The differential rover guidance is now based on the same algorithm as the ackermann rover (#23387). The rover will now actually track the line segment between waypoints, rather than just target the next waypoint, leading to a lower overall crosstrack error.
To fully exploit the 'turn on the spot' capabilities of differential rovers (as opposed to ackermann) the following state machine is implemented:

This causes the rover to stop and turn on the spot if the heading error exceeds a certain threshhold.
This decreases the accumulated crosstrack error, especially whenever the waypoint triplet changes.

Alternatives

Open to any suggestions

Test coverage

• SITL tested

2024-07-19.14-52-47.mp4

• Hardware tested (Vehicle: Aion robotics R1)
  
  Flight Log.

[sfuhrer]

So we don't want to keep the inverse kinematics? I guess most rovers don't do wheel speed control anyway and don't care about it.
Looks very clean otherwise!

[chfriedrich98]

@sfuhrer

So we don't want to keep the inverse kinematics? I guess most rovers don't do wheel speed control anyway and don't care about it.

Correct me if I'm wrong but the inverse kinematics of the old implementation didn't do direct wheel speed control either:

PX4-Autopilot/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp

Lines 70 to 97 in b93dd0e

	matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate) const
	{
	if (_max_speed < FLT_EPSILON) {
	return Vector2f();
	}
	linear_velocity_x = math::constrain(linear_velocity_x, -_max_speed, _max_speed);
	yaw_rate = math::constrain(yaw_rate, -_max_angular_velocity, _max_angular_velocity);
	const float rotational_velocity = (_wheel_base / 2.f) * yaw_rate;
	float combined_velocity = fabsf(linear_velocity_x) + fabsf(rotational_velocity);
	// Compute an initial gain
	float gain = 1.0f;
	if (combined_velocity > _max_speed) {
	float excess_velocity = fabsf(combined_velocity - _max_speed);
	const float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity;
	gain = adjusted_linear_velocity / fabsf(linear_velocity_x);
	}
	// Apply the gain
	linear_velocity_x *= gain;
	// Calculate the left and right wheel speeds
	return Vector2f(linear_velocity_x - rotational_velocity,
	linear_velocity_x + rotational_velocity) / _max_speed;
	}

I implemented the same inverse kinematics, but in a "normalized" form to directly compute the motor inputs (yaw rate setpoint is already transformed into a normalized required speed_diff before going into this function) :

PX4-Autopilot/src/modules/rover_differential/RoverDifferential.cpp

Lines 193 to 205 in 817bd21

	matrix::Vector2f RoverDifferential::computeMotorCommands(float forward_speed, const float speed_diff)
	{
	float combined_velocity = fabsf(forward_speed) + fabsf(speed_diff);
	if (combined_velocity > 1.0f) { // Prioritize yaw rate
	float excess_velocity = fabsf(combined_velocity - 1.0f);
	forward_speed -= sign(forward_speed) * excess_velocity;
	}
	// Calculate the left and right wheel speeds
	return Vector2f(forward_speed - speed_diff,
	forward_speed + speed_diff);
	}

[chfriedrich98]

rebased on main and squashed

[sfuhrer]

Agree that we can simplify the inverse kinematics like you propose. If we ever have somebody with a rover driver that takes wheel speed as input (and not throttle) we can revisit it.

[DronecodeBot]

This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/px4-v1-15-public-changes-what-needs-docs/39850/13

[slgrobotics]

I tried "make px4_sitl gz_lawnmower_lawn", and it completes the mission all right. With the inertia and friction of a simulated big zero-turn lawnmower I should've adjusted default parameters to achieve smooth motion.

Overall, it's nice to see a clean implementation, I am tempted to move some of it to my custom code and see it working on my Husquarna Z254F. I am wondering if switching from L1 to Pure Pursuit will achieve better line following. Thanks for you work!

[chfriedrich98]

@slgrobotics Thanks for testing the changes!

I tried "make px4_sitl gz_lawnmower_lawn", and it completes the mission all right. With the inertia and friction of a simulated big zero-turn lawnmower I should've adjusted default parameters to achieve smooth motion.

For the r1 rover I added my tuning directly to the SITL airframe s.t. it works "out of the box" when you start the simulation, which I think is what people would expect. I should do the same for the landmower, thanks for the reminder!

Overall, it's nice to see a clean implementation, I am tempted to move some of it to my custom code and see it working on my Husquarna Z254F. I am wondering if switching from L1 to Pure Pursuit will achieve better line following. Thanks for you work!

Thank you! Please keep me updated if you use it on your rover, it's always very insightful to see how it works on different vehicles.
From my testing I found the pure pursuit algorithm to work a lot better than l1, but of course it still comes down to proper tuning.

[slgrobotics]

@chfriedrich98 Please see #23527

I am sure there's room for perfecting/tuning these parameters further, but for now the vehicle provides reasonable demo - even when running a realistic lawn mowing "survey" with 1 meter distance between the stripes.

[slgrobotics]

@chfriedrich98 @dagar - please take a look at #23611 (Stanley Pursuit implementation offered)