fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 00:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

pure_pursuit: update library

Browse Source
This commit is contained in:
chfriedrich98
2025-03-10 11:14:00 +01:00
committed by chfriedrich98
parent fec240efba
commit d1b0be18b2
16 changed files with 277 additions and 189 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/logger/logged_topics.cpp
+1
View File
@@ -99,6 +99,7 @@ void LoggedTopics::add_default_topics()
add_topic("parameter_update");
add_topic("position_controller_status", 500);
add_topic("position_controller_landing_status", 100);
add_optional_topic("pure_pursuit_status", 100);
add_topic("goto_setpoint", 200);
add_topic("position_setpoint_triplet", 200);
add_optional_topic("px4io_status");
src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp
+22 -7
View File
@@ -41,6 +41,7 @@ AckermannPosVelControl::AckermannPosVelControl(ModuleParams *parent) : ModulePar
_rover_throttle_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
_rover_velocity_status_pub.advertise();
_pure_pursuit_status_pub.advertise();
updateParams();
}
@@ -187,12 +188,16 @@ void AckermannPosVelControl::manualPositionMode()
}
// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(),
2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment();
const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned;
const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get();
const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(_speed_body_x_setpoint) *
vector_scaling * _pos_ctl_course_direction;
float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _pos_ctl_start_position_ned,
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp;
float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _pos_ctl_start_position_ned,
_curr_pos_ned, fabsf(_speed_body_x_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
yaw_setpoint = _speed_body_x_setpoint > FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi(yaw_setpoint + M_PI_F);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
@@ -215,10 +220,15 @@ void AckermannPosVelControl::offboardPositionMode()
const float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
_param_ro_decel_limit.get(), distance_to_target, 0.f);
_speed_body_x_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp;
const float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _curr_pos_ned,
_curr_pos_ned, fabsf(_speed_body_x_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _curr_pos_ned,
_curr_pos_ned, fabsf(_speed_body_x_setpoint));
rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
} else {
@@ -264,10 +274,15 @@ void AckermannPosVelControl::autoPositionMode()
_speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, _min_speed, distance_to_prev_wp, distance_to_curr_wp,
_acceptance_radius, _prev_acceptance_radius, _param_ro_decel_limit.get(), _param_ro_jerk_limit.get(), _nav_state,
_waypoint_transition_angle, _prev_waypoint_transition_angle, _param_ro_speed_limit.get());
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp;
const float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
fabsf(_speed_body_x_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned,
_curr_pos_ned, fabsf(_speed_body_x_setpoint));
rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
}
}
src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp
+8 -5
View File
@@ -64,6 +64,7 @@
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/pure_pursuit_status.h>
using namespace matrix;
@@ -192,11 +193,12 @@ private:
offboard_control_mode_s _offboard_control_mode{};
// uORB publications
uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_velocity_status_s> _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_velocity_status_s> _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
uORB::Publication<position_controller_status_s> _position_controller_status_pub{ORB_ID(position_controller_status)};
uORB::Publication<pure_pursuit_status_s> _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
// Variables
hrt_abstime _timestamp{0};
@@ -232,7 +234,6 @@ private:
SlewRate<float> _speed_setpoint;
// Class Instances
PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library
MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
DEFINE_PARAMETERS(
@@ -245,7 +246,9 @@ private:
(ParamFloat<px4::params::RO_JERK_LIM>) _param_ro_jerk_limit,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th,
(ParamFloat<px4::params::PP_LOOKAHD_GAIN>) _param_pp_lookahd_gain,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(ParamFloat<px4::params::PP_LOOKAHD_MIN>) _param_pp_lookahd_min,
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RA_ACC_RAD_MAX>) _param_ra_acc_rad_max,
(ParamFloat<px4::params::RA_ACC_RAD_GAIN>) _param_ra_acc_rad_gain,
src/modules/rover_ackermann/CMakeLists.txt
+1
View File
@@ -47,6 +47,7 @@ px4_add_module(
AckermannPosVelControl
px4_work_queue
rover_control
pure_pursuit
MODULE_CONFIG
module.yaml
)
src/modules/rover_differential/CMakeLists.txt
+1
View File
@@ -47,6 +47,7 @@ px4_add_module(
DifferentialPosVelControl
px4_work_queue
rover_control
pure_pursuit
MODULE_CONFIG
module.yaml
)
src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp
+21 -7
View File
@@ -41,6 +41,7 @@ DifferentialPosVelControl::DifferentialPosVelControl(ModuleParams *parent) : Mod
_rover_throttle_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
_rover_velocity_status_pub.advertise();
_pure_pursuit_status_pub.advertise();
updateParams();
}
@@ -201,12 +202,16 @@ void DifferentialPosVelControl::manualPositionMode()
}
// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(),
2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment();
const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned;
const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get();
const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(_speed_body_x_setpoint) *
vector_scaling * _pos_ctl_course_direction;
float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _pos_ctl_start_position_ned,
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp;
float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _pos_ctl_start_position_ned,
_curr_pos_ned, fabsf(_speed_body_x_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
yaw_setpoint = _speed_body_x_setpoint > FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi(yaw_setpoint + M_PI_F);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
@@ -229,10 +234,15 @@ void DifferentialPosVelControl::offboardPositionMode()
const float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
_param_ro_decel_limit.get(), distance_to_target, 0.f);
_speed_body_x_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp;
const float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _curr_pos_ned,
_curr_pos_ned, fabsf(_speed_body_x_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _curr_pos_ned,
_curr_pos_ned, fabsf(_speed_body_x_setpoint));
rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
} else {
@@ -269,8 +279,12 @@ void DifferentialPosVelControl::autoPositionMode()
}
// State machine
float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
fabsf(_vehicle_speed_body_x));
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp;
float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
fabsf(_speed_body_x_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
const float heading_error = matrix::wrap_pi(yaw_setpoint - _vehicle_yaw);
if (!_mission_finished && distance_to_curr_wp > _param_nav_acc_rad.get()) {
src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp
+3 -1
View File
@@ -187,6 +187,7 @@ private:
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_velocity_status_s> _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
uORB::Publication<position_controller_status_s> _position_controller_status_pub{ORB_ID(position_controller_status)};
uORB::Publication<pure_pursuit_status_s> _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
// Variables
hrt_abstime _timestamp{0};
@@ -218,7 +219,6 @@ private:
SlewRate<float> _speed_setpoint;
// Class Instances
PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library
MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
GuidanceState _currentState{GuidanceState::DRIVING}; // The current state of the guidance.
@@ -235,7 +235,9 @@ private:
(ParamFloat<px4::params::RO_JERK_LIM>) _param_ro_jerk_limit,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th,
(ParamFloat<px4::params::PP_LOOKAHD_GAIN>) _param_pp_lookahd_gain,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(ParamFloat<px4::params::PP_LOOKAHD_MIN>) _param_pp_lookahd_min,
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad
src/modules/rover_mecanum/CMakeLists.txt
+1
View File
@@ -47,6 +47,7 @@ px4_add_module(
MecanumPosVelControl
px4_work_queue
rover_control
pure_pursuit
MODULE_CONFIG
module.yaml
)
src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.cpp
+19 -6
View File
@@ -41,6 +41,7 @@ MecanumPosVelControl::MecanumPosVelControl(ModuleParams *parent) : ModuleParams(
_rover_throttle_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
_rover_velocity_status_pub.advertise();
_pure_pursuit_status_pub.advertise();
updateParams();
}
@@ -235,11 +236,15 @@ void MecanumPosVelControl::manualPositionMode()
}
// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(),
2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment();
const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned;
const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get();
const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + vector_scaling * _pos_ctl_course_direction;
const float bearing_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _pos_ctl_start_position_ned,
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp;
const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _pos_ctl_start_position_ned,
_curr_pos_ned, velocity_magnitude_setpoint);
_pure_pursuit_status_pub.publish(pure_pursuit_status);
const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw);
_speed_body_x_setpoint = velocity_magnitude_setpoint * cosf(bearing_setpoint_body_frame);
_speed_body_y_setpoint = velocity_magnitude_setpoint * sinf(bearing_setpoint_body_frame);
@@ -271,8 +276,12 @@ void MecanumPosVelControl::offboardPositionMode()
const float velocity_magnitude_setpoint = math::min(math::trajectory::computeMaxSpeedFromDistance(
_param_ro_jerk_limit.get(),
_param_ro_decel_limit.get(), distance_to_target, 0.f), _param_ro_speed_limit.get());
const float bearing_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _curr_pos_ned,
_curr_pos_ned, fabsf(_speed_body_x_setpoint));
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp;
const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _curr_pos_ned,
_curr_pos_ned, velocity_magnitude_setpoint);
_pure_pursuit_status_pub.publish(pure_pursuit_status);
const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw);
_speed_body_x_setpoint = velocity_magnitude_setpoint * cosf(bearing_setpoint_body_frame);
_speed_body_y_setpoint = velocity_magnitude_setpoint * sinf(bearing_setpoint_body_frame);
@@ -316,8 +325,12 @@ void MecanumPosVelControl::autoPositionMode()
const float velocity_magnitude = calcVelocityMagnitude(_auto_speed, distance_to_curr_wp, _param_ro_decel_limit.get(),
_param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rm_miss_spd_gain.get(),
_nav_state);
const float bearing_setpoint = _posctl_pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp;
const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
velocity_magnitude);
_pure_pursuit_status_pub.publish(pure_pursuit_status);
const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw);
Vector2f desired_velocity(0.f, 0.f);
_speed_body_x_setpoint = _mission_finished ? 0.f : velocity_magnitude * cosf(bearing_setpoint_body_frame);
src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.hpp
+3 -1
View File
@@ -177,6 +177,7 @@ private:
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_velocity_status_s> _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
uORB::Publication<position_controller_status_s> _position_controller_status_pub{ORB_ID(position_controller_status)};
uORB::Publication<pure_pursuit_status_s> _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
// Variables
hrt_abstime _timestamp{0};
@@ -210,7 +211,6 @@ private:
SlewRate<float> _speed_y_setpoint;
// Class Instances
PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library
MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
DEFINE_PARAMETERS(
@@ -225,7 +225,9 @@ private:
(ParamFloat<px4::params::RO_JERK_LIM>) _param_ro_jerk_limit,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th,
(ParamFloat<px4::params::PP_LOOKAHD_GAIN>) _param_pp_lookahd_gain,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(ParamFloat<px4::params::PP_LOOKAHD_MIN>) _param_pp_lookahd_min,
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad