pure pursuit

updated pure pursuit

temp

forgot to wrap heading error

working guidance, with every case covered

removed unwanted files

src/modules/boat/Boat.cpp

@@ -50,17 +50,13 @@ void Boat::updateParams()
 {
 	ModuleParams::updateParams();
 
-	// _boat_kinematics.setWheelBase(_param_rdd_wheel_base.get());
-
 	_max_speed = _param_bt_spd_max.get();
 	_boat_guidance.setMaxSpeed(_max_speed);
 	_boat_kinematics.setMaxSpeed(_max_speed);
-	printf("max speed: %f\n", (double)_max_speed);
 
 	_max_angular_velocity = _param_bt_ang_max.get();
 	_boat_guidance.setMaxAngularVelocity(_max_angular_velocity);
 	_boat_kinematics.setMaxAngularVelocity(_max_angular_velocity);
-	printf("max angular velocity: %f\n", (double)_max_angular_velocity);
 }
 
 void Boat::Run()
@@ -102,7 +98,7 @@ void Boat::Run()
 
 	if (_manual_driving) {
 		// Manual mode
-		// directly produce setpoints from the manual control setpoint (joystick)
+		// Directly produce setpoints from the manual control setpoint (joystick)
 		if (_manual_control_setpoint_sub.updated()) {
 			manual_control_setpoint_s manual_control_setpoint{};
 
@@ -129,13 +125,13 @@ void Boat::Run()
 
 	} else if (_mission_driving) {
 		// Mission mode
-		// directly receive setpoints from the guidance library
-		printf("I'm in mission mode\n");
+		// Directly receive setpoints from the guidance library
 		_boat_guidance.computeGuidance(
 			_boat_control.getVehicleYaw(),
 			_boat_control.getLocalPosition(),
 			dt
 		);
+		// _boat_guidance_pp.purePursuit();
 	}
 
 	_boat_control.control(dt);

src/modules/boat/BoatGuidance/BoatGuidance.cpp

@@ -67,72 +67,88 @@ void BoatGuidance::computeGuidance(float yaw, vehicle_local_position_s vehicle_l
 						     vehicle_local_position.timestamp);
 	}
 
-	const Vector2f current_waypoint_local_position = _global_local_proj_ref.project(current_waypoint(0),
+	const Vector2f current_waypoint_local_frame = _global_local_proj_ref.project(current_waypoint(0),
 			current_waypoint(1));
-	const Vector2f previous_waypoint_local_position = _global_local_proj_ref.project(previous_waypoint(0),
+	const Vector2f previous_waypoint_local_frame = _global_local_proj_ref.project(previous_waypoint(0),
 			previous_waypoint(1));
-	const Vector2f local_position = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
-	const Vector2f local_velocity = Vector2f(vehicle_local_position.vx, vehicle_local_position.vy);
+	const Vector2f local_frame_position = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
 
-	const float distance_to_next_wp = get_distance_to_next_waypoint(local_position(0), local_position(1),
-					  current_waypoint_local_position(0),
-					  current_waypoint_local_position(1));
+	const float distance_to_next_wp = get_distance_to_next_waypoint(global_position(0), global_position(1),
+					  current_waypoint(0), current_waypoint(1));
 
-	float desired_heading = get_bearing_to_next_waypoint(global_position(0), global_position(1), next_waypoint(0),
-				next_waypoint(1));
-	float heading_error = matrix::wrap_pi(desired_heading - yaw);
+	float heading_error = 0.f;
+	float speed_interpolation = 0.f;
+	float heading_to_next_waypoint = 0.f;
+	float heading_error_to_next_waypoint = 0.f;
+	float desired_speed = _param_bt_spd_cruise.get();
 
+	// Go back to driving, when the waypoint has been reached
 	if (_current_waypoint != current_waypoint) {
 		_currentState = GuidanceState::DRIVING;
 	}
 
-	// Make rover stop when it arrives at the last waypoint instead of loitering and driving around weirdly.
+	// Make boat stop when it arrives at the last waypoint
 	if ((current_waypoint == next_waypoint) && distance_to_next_wp <= _param_nav_acc_rad.get()) {
 		_currentState = GuidanceState::GOAL_REACHED;
 	}
 
-	float speed_interpolation = 0.f;
-
-	if (PX4_ISFINITE(heading_error)) {
-		speed_interpolation = math::interpolate<float>(abs(heading_error), _param_bt_min_heading_error.get() * M_PI_F / 180.f,
-				      _param_bt_max_heading_error.get() * M_PI_F / 180.f, _param_bt_spd_cruise.get(),
-				      _param_bt_spd_min.get());
-	}
-
-	float desired_speed = math::constrain(speed_interpolation, _param_bt_spd_min.get(), _max_speed);
-
 	switch (_currentState) {
 	case GuidanceState::DRIVING: {
 
 			if (PX4_ISFINITE(previous_waypoint(0)) && PX4_ISFINITE(previous_waypoint(1))) {
-				_l1_guidance.navigate_waypoints(previous_waypoint_local_position, current_waypoint_local_position, local_position,
-								local_velocity);
+
+				float look_ahead_distance = getLookAheadDistance(current_waypoint_local_frame, previous_waypoint_local_frame,
+							    local_frame_position);
+				float desired_heading = calcDesiredHeading(current_waypoint_local_frame, previous_waypoint_local_frame,
+							local_frame_position, look_ahead_distance);
+
+				heading_error = matrix::wrap_pi(desired_heading - yaw);
+				_desired_angular_velocity = heading_error;
+
+				heading_to_next_waypoint = get_bearing_to_next_waypoint(previous_waypoint(0), previous_waypoint(1), current_waypoint(0),
+							   current_waypoint(1));
 
 			} else {
-				_previous_local_position = local_position;
+				_previous_local_position = local_frame_position;
+				_previous_position = global_position;
 				_currentState = GuidanceState::DRIVING_TO_POINT;
 			}
 
 			break;
 		}
 
-	case GuidanceState::DRIVING_TO_POINT:
+	// Control logic if there is no previous waypoint
+	case GuidanceState::DRIVING_TO_POINT: {
 
-		_l1_guidance.navigate_waypoints(_previous_local_position, current_waypoint_local_position, local_position,
-						local_velocity);
-		desired_speed = _param_bt_spd_cruise.get();
-		break;
+			float look_ahead_distance = getLookAheadDistance(current_waypoint_local_frame, _previous_local_position,
+						    local_frame_position);
+			float desired_heading = calcDesiredHeading(current_waypoint_local_frame, _previous_local_position, local_frame_position,
+						look_ahead_distance);
+			heading_error = matrix::wrap_pi(desired_heading - yaw);
+			_desired_angular_velocity = heading_error;
+			heading_to_next_waypoint = get_bearing_to_next_waypoint(_previous_position(0), _previous_position(1),
+						   current_waypoint(0),
+						   current_waypoint(1));
+			break;
+		}
 
 	case GuidanceState::GOAL_REACHED:
-		// temporary till I find a better way to stop the vehicle
 		desired_speed = 0.f;
 		heading_error = 0.f;
 		_desired_angular_velocity = 0.f;
 		break;
 	}
 
-	_desired_angular_velocity = math::constrain(_l1_guidance.nav_lateral_acceleration_demand(), -_max_angular_velocity,
-				    _max_angular_velocity);
+	heading_error_to_next_waypoint = matrix::wrap_pi(heading_to_next_waypoint - yaw);
+
+	// Interpolate the speed based on the heading error
+	if (PX4_ISFINITE(heading_error_to_next_waypoint) && desired_speed > 0.1f) {
+		speed_interpolation = math::interpolate<float>(abs(heading_error_to_next_waypoint),
+				      _param_bt_min_heading_error.get() * M_PI_F / 180.f,
+				      _param_bt_max_heading_error.get() * M_PI_F / 180.f, _param_bt_spd_cruise.get(),
+				      _param_bt_spd_min.get());
+		desired_speed = math::constrain(speed_interpolation, _param_bt_spd_min.get(), _max_speed);
+	}
 
 	boat_setpoint_s output{};
 	output.speed = math::constrain(desired_speed, -_max_speed, _max_speed);
@@ -145,10 +161,73 @@ void BoatGuidance::computeGuidance(float yaw, vehicle_local_position_s vehicle_l
 	_current_waypoint = current_waypoint;
 }
 
+float BoatGuidance::getLookAheadDistance(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local,
+		const Vector2f &curr_pos_local)
+{
+	// Calculate crosstrack error
+	const Vector2f prev_wp_to_curr_wp_local = curr_wp_local - prev_wp_local;
+
+	if (prev_wp_to_curr_wp_local.norm() < FLT_EPSILON) { // Avoid division by 0 (this case should not happen)
+		return 0.f;
+	}
+
+	const Vector2f prev_wp_to_curr_pos_local = curr_pos_local - prev_wp_local;
+	const Vector2f distance_on_line_segment = ((prev_wp_to_curr_pos_local * prev_wp_to_curr_wp_local) /
+			prev_wp_to_curr_wp_local.norm()) * prev_wp_to_curr_wp_local.normalized();
+	const Vector2f crosstrack_error = (prev_wp_local + distance_on_line_segment) - curr_pos_local;
+
+	if (crosstrack_error.length() < _param_look_ahead_distance.get()) {
+		return _param_look_ahead_distance.get();
+
+	} else {
+		return crosstrack_error.length();
+	}
+}
+
+float BoatGuidance::calcDesiredHeading(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local,
+				       Vector2f const &curr_pos_local,
+				       float const &lookahead_distance)
+{
+	// Setup variables
+	const float line_segment_slope = (curr_wp_local(1) - prev_wp_local(1)) / (curr_wp_local(0) - prev_wp_local(0));
+	const float line_segment_rover_offset = prev_wp_local(1) - curr_pos_local(1) + line_segment_slope * (curr_pos_local(
+			0) - prev_wp_local(0));
+	const float a = -line_segment_slope;
+	const float c = -line_segment_rover_offset;
+	const float r = lookahead_distance;
+	const float x0 = -a * c / (a * a + 1.0f);
+	const float y0 = -c / (a * a + 1.0f);
+
+	// Calculate intersection points
+	if (c * c > r * r * (a * a + 1.0f) + FLT_EPSILON) { // No intersection points exist
+		return 0.f;
+
+	} else if (abs(c * c - r * r * (a * a + 1.0f)) < FLT_EPSILON) { // One intersection point exists
+		return atan2f(y0, x0);
+
+	} else { // Two intersetion points exist
+		const float d = r * r - c * c / (a * a + 1.0f);
+		const float mult = sqrt(d / (a * a + 1.0f));
+		const float ax = x0 + mult;
+		const float bx = x0 - mult;
+		const float ay = y0 - a * mult;
+		const float by = y0 + a * mult;
+		const Vector2f point1(ax, ay);
+		const Vector2f point2(bx, by);
+		const Vector2f distance1 = (curr_wp_local - curr_pos_local) - point1;
+		const Vector2f distance2 = (curr_wp_local - curr_pos_local) - point2;
+
+		// Return intersection point closer to current waypoint
+		if (distance1.norm_squared() < distance2.norm_squared()) {
+			return atan2f(ay, ax);
+
+		} else {
+			return atan2f(by, bx);
+		}
+	}
+}
+
 void BoatGuidance::updateParams()
 {
 	ModuleParams::updateParams();
-
-	_l1_guidance.set_l1_damping(_param_bt_l1_damping.get());
-	_l1_guidance.set_l1_period(_param_bt_l1_period.get());
 }

src/modules/boat/BoatGuidance/BoatGuidance.hpp

@@ -104,6 +104,12 @@ public:
 	 */
 	float setMaxAngularVelocity(float max_angular_velocity) { return _max_angular_velocity = max_angular_velocity; }
 
+	float calcDesiredHeading(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, const Vector2f &curr_pos_local,
+				 const float &lookahead_distance);
+
+	float getLookAheadDistance(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local,
+				      const Vector2f &curr_pos_local);
+
 protected:
 	/**
 	 * @brief Update the parameters of the module.
@@ -115,41 +121,32 @@ private:
 	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
 
 	uORB::Publication<boat_setpoint_s> _boat_setpoint_pub{ORB_ID(boat_setpoint)};
-
 	position_setpoint_triplet_s _position_setpoint_triplet{};
 	vehicle_global_position_s _vehicle_global_position{};
 
 	GuidanceState _currentState;
 
-	float _desired_angular_velocity;
-
-	float _max_speed;
-	float _max_angular_velocity;
-
-	matrix::Vector2d _current_waypoint;
-
-	VelocitySmoothing _forwards_velocity_smoothing;
-	PositionSmoothing _position_smoothing;
-
-	ECL_L1_Pos_Controller _l1_guidance;
-
-	Vector2f _previous_local_position{};
+	float _desired_angular_velocity{};
+	float _max_angular_velocity{};
+	float _look_ahead_distance{};
+	float _max_speed{};
 
+	VelocitySmoothing _forwards_velocity_smoothing{};
+	PositionSmoothing _position_smoothing{};
 	MapProjection _global_local_proj_ref{};
+	matrix::Vector2d _current_waypoint{};
+	ECL_L1_Pos_Controller _l1_guidance{};
+	Vector2f _previous_local_position{};
+	Vector2d _previous_position{};
 
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
-
-		(ParamFloat<px4::params::BT_L1_PERIOD>) _param_bt_l1_period,
-		(ParamFloat<px4::params::BT_L1_DAMPING>) _param_bt_l1_damping,
-
 		(ParamFloat<px4::params::BT_MAX_HERR>) _param_bt_max_heading_error,
 		(ParamFloat<px4::params::BT_MIN_HERR>) _param_bt_min_heading_error,
-
-		(ParamFloat<px4::params::BT_SPD_MAX>) _param_bt_spd_max,
-		(ParamFloat<px4::params::BT_SPD_MIN>) _param_bt_spd_min,
-
+		(ParamFloat<px4::params::BT_LOOKAHEAD>) _param_look_ahead_distance,
+		(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad,
 		(ParamFloat<px4::params::BT_SPD_CRUISE>) _param_bt_spd_cruise,
-		(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad
+		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
+		(ParamFloat<px4::params::BT_SPD_MAX>) _param_bt_spd_max,
+		(ParamFloat<px4::params::BT_SPD_MIN>) _param_bt_spd_min
 	)
 };

src/modules/boat/BoatGuidance/CMakeLists.txt

@@ -33,6 +33,7 @@
 
 px4_add_library(BoatGuidance
 	BoatGuidance.cpp
+	BoatGuidance.hpp
 )
 
 target_include_directories(BoatGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/boat/module.yaml

@@ -21,32 +21,6 @@ parameters:
         increment: 0.01
         decimal: 2
         default: 1
-      BT_LACC_LIM:
-        description:
-          short: Lateral acceleration limit
-        type: float
-        unit: m/s^2
-        min: 0.0
-        max: 6.0
-        decimal: 1
-        default: 2.0
-      BT_L1_PERIOD:
-        description:
-          short: L1 period
-        type: float
-        unit: s
-        min: 0.5
-        max: 50.0
-        decimal: 1
-        default: 25.0
-      BT_L1_DAMPING:
-        description:
-          short: L1 damping
-        type: float
-        min: 0.6
-        max: 0.9
-        decimal: 2
-        default: 0.9
       BT_SPD_CRUISE:
         description:
           short: Default cruise speed
@@ -165,37 +139,11 @@ parameters:
         min: 0.0
         decimal: 2
         default: 30.0
-      BT_MISSION_THR:
+      BT_LOOKAHEAD:
         description:
-          short: Mission throttle setpoint
-        type: float
-        min: -1.0
-        decimal: 2
-        default: 0.8
-      BT_MIN_THR:
-        description:
-          short: Mission minimum slowdown parameter
+          short: Lookahead distance for heading error
         type: float
+        unit: m
         min: 0.0
         decimal: 2
-        default: 0.5
-      BT_MAX_JERK:
-        description:
-          short: Maximum jerk
-        type: float
-        unit: m/s^3
-        min: 0.0
-        max: 10.0
-        decimal: 1
-        default: 5.0
-      BT_MAX_ACCEL:
-        description:
-          short: Maximum acceleration
-          long: Maximum acceleration is used to limit the acceleration of the rover
-        type: float
-        unit: m/s^2
-        min: 0
-        max: 100
-        increment: 0.01
-        decimal: 2
-        default: 0.5
+        default: 10.0