From 0e45a891bad4783be38b964b81bb3128ed4610ee Mon Sep 17 00:00:00 2001
From: chfriedrich98 <chfriedrich@student.ethz.ch>
Date: Tue, 26 Nov 2024 12:46:37 +0100
Subject: [PATCH] ackermann: offboard support through trajectorySetpoint.msg
 Supports only position setpoint

---
 .../rover_ackermann/RoverAckermann.cpp        | 43 +++++++++++++++++++
 .../rover_ackermann/RoverAckermann.hpp        |  8 +++-
 2 files changed, 49 insertions(+), 2 deletions(-)

diff --git a/src/modules/rover_ackermann/RoverAckermann.cpp b/src/modules/rover_ackermann/RoverAckermann.cpp
index 5fea7e5174a7..3a2256a4080c 100644
--- a/src/modules/rover_ackermann/RoverAckermann.cpp
+++ b/src/modules/rover_ackermann/RoverAckermann.cpp
@@ -151,6 +151,49 @@ void RoverAckermann::Run()
 		_ackermann_guidance.computeGuidance(_vehicle_forward_speed, _vehicle_yaw, _nav_state, _armed);
 		break;
 
+	case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: {
+			if (_trajectory_setpoint_sub.updated()) {
+				offboard_control_mode_s offboard_control_mode{};
+				_offboard_control_mode_sub.copy(&offboard_control_mode);
+
+				trajectory_setpoint_s trajectory_setpoint{};
+				_trajectory_setpoint_sub.copy(&trajectory_setpoint);
+
+
+				rover_ackermann_setpoint_s rover_ackermann_setpoint{};
+				rover_ackermann_setpoint.timestamp =  timestamp;
+				rover_ackermann_setpoint.forward_speed_setpoint =  NAN;
+				rover_ackermann_setpoint.forward_speed_setpoint_normalized =  NAN;
+				rover_ackermann_setpoint.steering_setpoint = NAN;
+				rover_ackermann_setpoint.steering_setpoint_normalized = NAN;
+				rover_ackermann_setpoint.lateral_acceleration_setpoint = NAN;
+
+				// Translate trajectory setpoint to rover setpoints
+				if (offboard_control_mode.position) {
+					const Vector2f target_waypoint_ned(trajectory_setpoint.position[0], trajectory_setpoint.position[1]);
+
+					if (target_waypoint_ned.isAllFinite()) {
+						const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
+
+						if (distance_to_target > _param_nav_acc_rad.get()) {
+							rover_ackermann_setpoint.forward_speed_setpoint = _param_ra_max_speed.get();
+							const float steering_setpoint = _ackermann_guidance.calcDesiredSteering(_posctl_pure_pursuit,
+											target_waypoint_ned, _pos_ctl_start_position_ned, _curr_pos_ned, _param_ra_wheel_base.get(),
+											rover_ackermann_setpoint.forward_speed_setpoint, _vehicle_yaw, _param_ra_max_steer_angle.get(), _armed);
+							rover_ackermann_setpoint.lateral_acceleration_setpoint = powf(_vehicle_forward_speed,
+									2.f) * tanf(steering_setpoint) / _param_ra_wheel_base.get();
+
+						}
+
+					}
+
+				}
+
+				_rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint);
+			}
+		} break;
+
+
 	default: // Unimplemented nav states will stop the rover
 		rover_ackermann_setpoint_s rover_ackermann_setpoint{};
 		rover_ackermann_setpoint.timestamp =  timestamp;
diff --git a/src/modules/rover_ackermann/RoverAckermann.hpp b/src/modules/rover_ackermann/RoverAckermann.hpp
index b4e5db5b5e54..57f9e544ebb4 100644
--- a/src/modules/rover_ackermann/RoverAckermann.hpp
+++ b/src/modules/rover_ackermann/RoverAckermann.hpp
@@ -50,7 +50,8 @@
 #include <uORB/topics/rover_ackermann_setpoint.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_attitude.h>
-
+#include <uORB/topics/trajectory_setpoint.h>
+#include <uORB/topics/offboard_control_mode.h>
 
 // Standard library includes
 #include <math.h>
@@ -108,6 +109,8 @@ class RoverAckermann : public ModuleBase<RoverAckermann>, public ModuleParams,
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
+	uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
 
 	// uORB publications
 	uORB::Publication<rover_ackermann_setpoint_s> _rover_ackermann_setpoint_pub{ORB_ID(rover_ackermann_setpoint)};
@@ -138,7 +141,8 @@ class RoverAckermann : public ModuleBase<RoverAckermann>, public ModuleParams,
 		(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_steer_angle,
 		(ParamFloat<px4::params::RA_MAX_SPEED>) _param_ra_max_speed,
 		(ParamFloat<px4::params::RA_MAX_LAT_ACCEL>) _param_ra_max_lat_accel,
-		(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max
+		(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
+		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad
 
 	)