ackermann: fix naming conventions

src/modules/rover_ackermann/RoverAckermann.cpp

@@ -112,7 +112,7 @@ void RoverAckermann::Run()
 
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
-			_motor_setpoint = _ackermann_guidance.purePursuit(_nav_state);
+			_motor_setpoint = _ackermann_guidance.computeGuidance(_nav_state);
 			break;
 
 		default: // Unimplemented nav states will stop the rover

src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp

@@ -56,7 +56,7 @@ void RoverAckermannGuidance::updateParams()
 			   1); // Output limit
 }
 
-RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit(const int nav_state)
+RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(const int nav_state)
 {
 	// Initializations
 	float desired_speed{0.f};
@@ -76,12 +76,12 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit(const
 		vehicle_local_position_s local_position{};
 		_local_position_sub.copy(&local_position);
 
-		if (!_global_local_proj_ref.isInitialized()
-		    || (_global_local_proj_ref.getProjectionReferenceTimestamp() != local_position.ref_timestamp)) {
-			_global_local_proj_ref.initReference(local_position.ref_lat, local_position.ref_lon, local_position.ref_timestamp);
+		if (!_global_ned_proj_ref.isInitialized()
+		    || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != local_position.ref_timestamp)) {
+			_global_ned_proj_ref.initReference(local_position.ref_lat, local_position.ref_lon, local_position.ref_timestamp);
 		}
 
-		_curr_pos_local = Vector2f(local_position.x, local_position.y);
+		_curr_pos_ned = Vector2f(local_position.x, local_position.y);
 		const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz};
 		actual_speed = rover_velocity.norm();
 	}
@@ -162,7 +162,7 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit(const
 		}
 
 		// Calculate desired steering
-		desired_steering = calcDesiredSteering(_curr_wp_local, _prev_wp_local, _curr_pos_local, _param_ra_lookahd_gain.get(),
+		desired_steering = calcDesiredSteering(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, _param_ra_lookahd_gain.get(),
 						       _param_ra_lookahd_min.get(), _param_ra_lookahd_max.get(), _param_ra_wheel_base.get(), desired_speed, vehicle_yaw);
 		desired_steering = math::constrain(desired_steering, -_param_ra_max_steer_angle.get(),
 						   _param_ra_max_steer_angle.get());
@@ -233,36 +233,36 @@ void RoverAckermannGuidance::updateWaypoints()
 		_next_wp = _home_position; // Enables corner slow down with RTL
 	}
 
-	// Local waypoint coordinates
-	_curr_wp_local = _global_local_proj_ref.project(_curr_wp(0), _curr_wp(1));
-	_prev_wp_local = _global_local_proj_ref.project(_prev_wp(0), _prev_wp(1));
-	_next_wp_local = _global_local_proj_ref.project(_next_wp(0), _next_wp(1));
+	// NED waypoint coordinates
+	_curr_wp_ned = _global_ned_proj_ref.project(_curr_wp(0), _curr_wp(1));
+	_prev_wp_ned = _global_ned_proj_ref.project(_prev_wp(0), _prev_wp(1));
+	_next_wp_ned = _global_ned_proj_ref.project(_next_wp(0), _next_wp(1));
 
 	// Update acceptance radius
 	_prev_acceptance_radius = _acceptance_radius;
 
 	if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) {
-		_acceptance_radius = updateAcceptanceRadius(_curr_wp_local, _prev_wp_local, _next_wp_local, _param_nav_acc_rad.get(),
+		_acceptance_radius = updateAcceptanceRadius(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, _param_nav_acc_rad.get(),
 				     _param_ra_acc_rad_gain.get(), _param_ra_acc_rad_max.get(), _param_ra_wheel_base.get(), _param_ra_max_steer_angle.get());
 
 	} else {
 		_acceptance_radius = _param_nav_acc_rad.get();
 	}
 }
 
-float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local,
-		const Vector2f &next_wp_local, const float &default_acceptance_radius, const float &acceptance_radius_gain,
+float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned,
+		const Vector2f &next_wp_ned, const float &default_acceptance_radius, const float &acceptance_radius_gain,
 		const float &acceptance_radius_max, const float &wheel_base, const float &max_steer_angle)
 {
 	// Setup variables
-	const Vector2f curr_to_prev_wp_local = prev_wp_local - curr_wp_local;
-	const Vector2f curr_to_next_wp_local = next_wp_local - curr_wp_local;
+	const Vector2f curr_to_prev_wp_ned = prev_wp_ned - curr_wp_ned;
+	const Vector2f curr_to_next_wp_ned = next_wp_ned - curr_wp_ned;
 	float acceptance_radius = default_acceptance_radius;
 
 	// Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment
-	if (curr_to_next_wp_local.norm() > FLT_EPSILON && curr_to_prev_wp_local.norm() > FLT_EPSILON) {
-		const float theta = acosf((curr_to_prev_wp_local * curr_to_next_wp_local) / (curr_to_prev_wp_local.norm() *
-					  curr_to_next_wp_local.norm())) / 2.f;
+	if (curr_to_next_wp_ned.norm() > FLT_EPSILON && curr_to_prev_wp_ned.norm() > FLT_EPSILON) {
+		const float theta = acosf((curr_to_prev_wp_ned * curr_to_next_wp_ned) / (curr_to_prev_wp_ned.norm() *
+					  curr_to_next_wp_ned.norm())) / 2.f;
 		const float min_turning_radius = wheel_base / sinf(max_steer_angle);
 		const float acceptance_radius_temp = min_turning_radius / tanf(theta);
 		const float acceptance_radius_temp_scaled = acceptance_radius_gain *
@@ -279,14 +279,14 @@ float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_loc
 	return acceptance_radius;
 }
 
-float RoverAckermannGuidance::calcDesiredSteering(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local,
-		const Vector2f &curr_pos_local, const float &lookahead_gain, const float &lookahead_min, const float &lookahead_max,
+float RoverAckermannGuidance::calcDesiredSteering(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned,
+		const Vector2f &curr_pos_ned, const float &lookahead_gain, const float &lookahead_min, const float &lookahead_max,
 		const float &wheel_base, const float &desired_speed, const float &vehicle_yaw)
 {
 	// Calculate desired steering to reach lookahead point
 	const float lookahead_distance = math::constrain(lookahead_gain * desired_speed,
 					 lookahead_min, lookahead_max);
-	const float desired_heading = _pure_pursuit.calcDesiredHeading(curr_wp_local, prev_wp_local, curr_pos_local,
+	const float desired_heading = _pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned,
 				      lookahead_distance);
 	const float heading_error = matrix::wrap_pi(desired_heading - vehicle_yaw);
 	// For logging
@@ -296,12 +296,8 @@ float RoverAckermannGuidance::calcDesiredSteering(const Vector2f &curr_wp_local,
 	if (math::abs_t(heading_error) <= M_PI_2_F) {
 		return atanf(2 * wheel_base * sinf(heading_error) / lookahead_distance);
 
-	} else if (heading_error > FLT_EPSILON) {
-		return atanf(2 * wheel_base * (1.0f + sinf(heading_error - M_PI_2_F)) /
-			     lookahead_distance);
-
 	} else {
-		return atanf(2 * wheel_base * (-1.0f + sinf(heading_error + M_PI_2_F)) /
+		return atanf(2 * wheel_base * (sign(heading_error) * 1.0f + sinf(heading_error - sign(heading_error) * M_PI_2_F)) /
 			     lookahead_distance);
 	}
 

src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp

@@ -82,43 +82,43 @@ class RoverAckermannGuidance : public ModuleParams
 	 * @brief Compute guidance for ackermann rover and return motor_setpoint for throttle and steering.
 	 * @param nav_state Vehicle navigation state
 	 */
-	motor_setpoint purePursuit(const int nav_state);
+	motor_setpoint computeGuidance(const int nav_state);
 
 	/**
-	 * @brief Update global/local waypoint coordinates and acceptance radius
+	 * @brief Update global/NED waypoint coordinates and acceptance radius
 	 */
 	void updateWaypoints();
 
 	/**
 	 * @brief Returns and publishes the  acceptance radius for current waypoint based on the angle between a line segment
 	 * from the previous to the current waypoint/current to the next waypoint and maximum steer angle of
 	 * the vehicle.
-	 * @param curr_wp_local Current waypoint in local frame.
-	 * @param prev_wp_local Previous waypoint in local frame.
-	 * @param next_wp_local Next waypoint in local frame.
+	 * @param curr_wp_ned Current waypoint in NED frame.
+	 * @param prev_wp_ned Previous waypoint in NED frame.
+	 * @param next_wp_ned Next waypoint in NED frame.
 	 * @param default_acceptance_radius Default acceptance radius for waypoints.
 	 * @param acceptance_radius_gain Scaling of the geometric optimal acceptance radius for the rover to cut corners.
 	 * @param acceptance_radius_max Maximum value for the acceptance radius.
 	 * @param wheel_base Rover wheelbase.
 	 * @param max_steer_angle Rover maximum steer angle.
 	 */
-	float updateAcceptanceRadius(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local,
-				     const Vector2f &next_wp_local, const float &default_acceptance_radius, const float &acceptance_radius_gain,
+	float updateAcceptanceRadius(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned,
+				     const Vector2f &next_wp_ned, const float &default_acceptance_radius, const float &acceptance_radius_gain,
 				     const float &acceptance_radius_max, const float &wheel_base, const float &max_steer_angle);
 
 	/**
 	 * @brief Calculate and return desired steering input
-	 * @param curr_wp_local Current waypoint in local frame.
-	 * @param prev_wp_local Previous waypoint in local frame.
-	 * @param curr_pos_local Current position of the vehicle in local frame.
+	 * @param curr_wp_ned Current waypoint in NED frame.
+	 * @param prev_wp_ned Previous waypoint in NED frame.
+	 * @param curr_pos_ned Current position of the vehicle in NED frame.
 	 * @param lookahead_gain Tuning parameter for the lookahead distance pure pursuit controller.
 	 * @param lookahead_min Minimum lookahead distance.
 	 * @param lookahead_max Maximum lookahead distance.
 	 * @param wheel_base Rover wheelbase.
 	 * @param desired_speed Desired speed for the rover.
 	 * @param vehicle_yaw Current yaw of the rover.
 	 */
-	float calcDesiredSteering(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, const Vector2f &curr_pos_local,
+	float calcDesiredSteering(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned,
 				  const float &lookahead_gain, const float &lookahead_min, const float &lookahead_max, const float &wheel_base,
 				  const float &desired_speed, const float &vehicle_yaw);
 
@@ -143,12 +143,12 @@ class RoverAckermannGuidance : public ModuleParams
 	rover_ackermann_guidance_status_s _rover_ackermann_guidance_status{};
 
 
-	MapProjection _global_local_proj_ref{}; // Transform global to local coordinates.
+	MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates.
 	PurePursuit _pure_pursuit; // Pure pursuit library
 
 	// Rover variables
 	Vector2d _curr_pos{};
-	Vector2f _curr_pos_local{};
+	Vector2f _curr_pos_ned{};
 	PID_t _pid_throttle;
 	hrt_abstime _timestamp{0};
 
@@ -157,9 +157,9 @@ class RoverAckermannGuidance : public ModuleParams
 	Vector2d _next_wp{};
 	Vector2d _prev_wp{};
 	Vector2d _home_position{};
-	Vector2f _curr_wp_local{};
-	Vector2f _prev_wp_local{};
-	Vector2f _next_wp_local{};
+	Vector2f _curr_wp_ned{};
+	Vector2f _prev_wp_ned{};
+	Vector2f _next_wp_ned{};
 	float _acceptance_radius{0.5f};
 	float _prev_acceptance_radius{0.5f};