diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index c87e75fb695e..71c55becad8a 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -173,6 +173,7 @@ set(msg_files PowerButtonState.msg PowerMonitor.msg PpsCapture.msg + PurePursuit.msg PwmInput.msg Px4ioStatus.msg QshellReq.msg @@ -183,13 +184,10 @@ set(msg_files RcParameterMap.msg RegisterExtComponentReply.msg RegisterExtComponentRequest.msg - RoverAckermannGuidanceStatus.msg RoverAckermannSetpoint.msg RoverAckermannStatus.msg - RoverDifferentialGuidanceStatus.msg RoverDifferentialSetpoint.msg RoverDifferentialStatus.msg - RoverMecanumGuidanceStatus.msg RoverMecanumSetpoint.msg RoverMecanumStatus.msg Rpm.msg diff --git a/msg/PurePursuit.msg b/msg/PurePursuit.msg new file mode 100644 index 000000000000..2785026c1191 --- /dev/null +++ b/msg/PurePursuit.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller +float32 target_bearing # [rad] Target bearing calculated by the pure pursuit controller +float32 crosstrack_error # [m] Shortest distance from the vehicle to the path (Positiv: Right of the path, Negativ: Left of the path) +float32 distance_to_waypoint # [m] Distance from the vehicle to the current waypoint + +# TOPICS pure_pursuit diff --git a/msg/RoverAckermannGuidanceStatus.msg b/msg/RoverAckermannGuidanceStatus.msg deleted file mode 100644 index bc096ba4d662..000000000000 --- a/msg/RoverAckermannGuidanceStatus.msg +++ /dev/null @@ -1,6 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller -float32 heading_error # [deg] Heading error of the pure pursuit controller - -# TOPICS rover_ackermann_guidance_status diff --git a/msg/RoverDifferentialGuidanceStatus.msg b/msg/RoverDifferentialGuidanceStatus.msg deleted file mode 100644 index ce3d37511163..000000000000 --- a/msg/RoverDifferentialGuidanceStatus.msg +++ /dev/null @@ -1,7 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller -float32 heading_error_deg # [deg] Heading error of the pure pursuit controller -uint8 state_machine # Driving state of the rover [0: SPOT_TURNING, 1: DRIVING, 2: GOAL_REACHED] - -# TOPICS rover_differential_guidance_status diff --git a/msg/RoverMecanumGuidanceStatus.msg b/msg/RoverMecanumGuidanceStatus.msg deleted file mode 100644 index fba920033ba7..000000000000 --- a/msg/RoverMecanumGuidanceStatus.msg +++ /dev/null @@ -1,7 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller -float32 heading_error # [rad] Heading error of the pure pursuit controller -float32 desired_speed # [m/s] Desired velocity magnitude (speed) - -# TOPICS rover_mecanum_guidance_status diff --git a/src/lib/pure_pursuit/PurePursuit.cpp b/src/lib/pure_pursuit/PurePursuit.cpp index e28e7b0924a8..099c182f8388 100644 --- a/src/lib/pure_pursuit/PurePursuit.cpp +++ b/src/lib/pure_pursuit/PurePursuit.cpp @@ -40,6 +40,7 @@ PurePursuit::PurePursuit(ModuleParams *parent) : ModuleParams(parent) _param_handles.lookahead_gain = param_find("PP_LOOKAHD_GAIN"); _param_handles.lookahead_max = param_find("PP_LOOKAHD_MAX"); _param_handles.lookahead_min = param_find("PP_LOOKAHD_MIN"); + _pure_pursuit_pub.advertise(); updateParams(); } @@ -53,43 +54,85 @@ void PurePursuit::updateParams() } -float PurePursuit::calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, - const Vector2f &curr_pos_ned, - const float vehicle_speed) +float PurePursuit::updatePurePursuit(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, + const Vector2f &curr_pos_ned, const float vehicle_speed) +{ + _target_bearing = calcTargetBearing(curr_wp_ned, prev_wp_ned, curr_pos_ned, vehicle_speed); + + if (PX4_ISFINITE(_target_bearing)) { + publishPurePursuit(); + } + + return _target_bearing; +} + +float PurePursuit::calcTargetBearing(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, + const Vector2f &curr_pos_ned, const float vehicle_speed) { // Check input validity - if (!curr_wp_ned.isAllFinite() || !curr_pos_ned.isAllFinite() || vehicle_speed < -FLT_EPSILON - || !PX4_ISFINITE(vehicle_speed) || !prev_wp_ned.isAllFinite()) { + if (!curr_wp_ned.isAllFinite() || !curr_pos_ned.isAllFinite() || !PX4_ISFINITE(vehicle_speed) + || !prev_wp_ned.isAllFinite()) { return NAN; } - _lookahead_distance = math::constrain(_params.lookahead_gain * vehicle_speed, - _params.lookahead_min, _params.lookahead_max); - // Pure pursuit - const Vector2f curr_pos_to_curr_wp = curr_wp_ned - curr_pos_ned; + _lookahead_distance = math::constrain(_params.lookahead_gain * fabsf(vehicle_speed), + _params.lookahead_min, _params.lookahead_max); + _curr_pos_to_curr_wp = curr_wp_ned - curr_pos_ned; const Vector2f prev_wp_to_curr_wp = curr_wp_ned - prev_wp_ned; + const Vector2f prev_wp_to_curr_pos = curr_pos_ned - prev_wp_ned; + const Vector2f prev_wp_to_curr_wp_u = prev_wp_to_curr_wp.unit_or_zero(); + _distance_along_path = (prev_wp_to_curr_pos * prev_wp_to_curr_wp_u) * + prev_wp_to_curr_wp_u; // Projection of prev_wp_to_curr_pos onto prev_wp_to_curr_wp + const Vector2f curr_pos_to_path = _distance_along_path - + prev_wp_to_curr_pos; // Shortest vector from the current position to the path + _crosstrack_error = sign(prev_wp_to_curr_wp(1) * curr_pos_to_path( + 0) - prev_wp_to_curr_wp(0) * curr_pos_to_path(1)) * curr_pos_to_path.norm(); - if (curr_pos_to_curr_wp.norm() < _lookahead_distance + if (_curr_pos_to_curr_wp.norm() < _lookahead_distance || prev_wp_to_curr_wp.norm() < FLT_EPSILON) { // Target current waypoint if closer to it than lookahead or waypoints overlap - return atan2f(curr_pos_to_curr_wp(1), curr_pos_to_curr_wp(0)); - } + return matrix::wrap_pi(atan2f(_curr_pos_to_curr_wp(1), _curr_pos_to_curr_wp(0))); - const Vector2f prev_wp_to_curr_pos = curr_pos_ned - prev_wp_ned; - const Vector2f prev_wp_to_curr_wp_u = prev_wp_to_curr_wp.unit_or_zero(); - _distance_on_line_segment = (prev_wp_to_curr_pos * prev_wp_to_curr_wp_u) * prev_wp_to_curr_wp_u; - _curr_pos_to_path = _distance_on_line_segment - prev_wp_to_curr_pos; + } else { + + if (fabsf(_crosstrack_error) > _lookahead_distance) { // Path segment is outside of lookahead (No intersection point) + const Vector2f prev_wp_to_closest_point_on_path = curr_pos_to_path + prev_wp_to_curr_pos; + const Vector2f curr_wp_to_closest_point_on_path = curr_pos_to_path - _curr_pos_to_curr_wp; + + if (prev_wp_to_closest_point_on_path * prev_wp_to_curr_wp < + FLT_EPSILON) { // Target previous waypoint if closest point is on the the extended path segment "behind" previous waypoint + return matrix::wrap_pi(atan2f(-prev_wp_to_curr_pos(1), -prev_wp_to_curr_pos(0))); + + } else if (curr_wp_to_closest_point_on_path * prev_wp_to_curr_wp > + FLT_EPSILON) { // Target current waypoint if closest point is on the extended path segment "ahead" of current waypoint + return matrix::wrap_pi(atan2f(_curr_pos_to_curr_wp(1), _curr_pos_to_curr_wp(0))); + + } else { // Target closest point on path + return matrix::wrap_pi(atan2f(curr_pos_to_path(1), curr_pos_to_path(0))); + } + + + } else { + const float line_extension = sqrt(powf(_lookahead_distance, 2.f) - powf(curr_pos_to_path.norm(), + 2.f)); // Length of the vector from the endpoint of distance_on_line_segment to the intersection point + const Vector2f prev_wp_to_intersection_point = _distance_along_path + line_extension * + prev_wp_to_curr_wp_u; + const Vector2f curr_pos_to_intersection_point = prev_wp_to_intersection_point - prev_wp_to_curr_pos; + return matrix::wrap_pi(atan2f(curr_pos_to_intersection_point(1), curr_pos_to_intersection_point(0))); + } - if (_curr_pos_to_path.norm() > _lookahead_distance) { // Target closest point on path if there is no intersection point - return atan2f(_curr_pos_to_path(1), _curr_pos_to_path(0)); } - const float line_extension = sqrt(powf(_lookahead_distance, 2.f) - powf(_curr_pos_to_path.norm(), - 2.f)); // Length of the vector from the endpoint of _distance_on_line_segment to the intersection point - const Vector2f prev_wp_to_intersection_point = _distance_on_line_segment + line_extension * - prev_wp_to_curr_wp_u; - const Vector2f curr_pos_to_intersection_point = prev_wp_to_intersection_point - prev_wp_to_curr_pos; - return atan2f(curr_pos_to_intersection_point(1), curr_pos_to_intersection_point(0)); +} +void PurePursuit::publishPurePursuit() +{ + pure_pursuit_s pure_pursuit{}; + pure_pursuit.timestamp = hrt_absolute_time(); + pure_pursuit.target_bearing = _target_bearing; + pure_pursuit.lookahead_distance = _lookahead_distance; + pure_pursuit.crosstrack_error = _crosstrack_error; + pure_pursuit.distance_to_waypoint = _curr_pos_to_curr_wp.norm(); + _pure_pursuit_pub.publish(pure_pursuit); } diff --git a/src/lib/pure_pursuit/PurePursuit.hpp b/src/lib/pure_pursuit/PurePursuit.hpp index 5740d0184c7a..683fe8de7a90 100644 --- a/src/lib/pure_pursuit/PurePursuit.hpp +++ b/src/lib/pure_pursuit/PurePursuit.hpp @@ -35,6 +35,8 @@ #include #include +#include +#include using namespace matrix; @@ -84,27 +86,37 @@ class PurePursuit : public ModuleParams PurePursuit(ModuleParams *parent); ~PurePursuit() = default; + + /** + * @brief Calculate and return the target bearing using the pure pursuit path following logic and publish pure pursuit logging message. + * @param curr_wp_ned North/East coordinates of current waypoint in NED frame [m]. + * @param prev_wp_ned North/East coordinates of previous waypoint in NED frame [m]. + * @param curr_pos_ned North/East coordinates of current position of the vehicle in NED frame [m]. + * @param vehicle_speed Vehicle speed [m/s]. + * @return Bearing towards the intersection point [rad] + */ + float updatePurePursuit(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, + float vehicle_speed); + /** - * @brief Return heading towards the intersection point between a circle with a radius of - * vehicle_speed * PP_LOOKAHD_GAIN around the vehicle and an extended line segment from the previous to the current waypoint. + * @brief Return bearing towards the intersection point between a circle with a radius of + * abs(vehicle_speed) * PP_LOOKAHD_GAIN around the vehicle and a line segment from the previous to the current waypoint. * Exceptions: - * Will return heading towards the current waypoint if it is closer to the vehicle than the lookahead or if the waypoints overlap. - * Will return heading towards the closest point on the path if there are no intersection points (crosstrack error bigger than lookahead). - * Will return NAN if input is invalid. + * Return bearing towards the current waypoint if it is closer to the vehicle than the lookahead or if the waypoints overlap. + * Return bearing towards the closest point on the path if there are no intersection points (crosstrack error bigger than lookahead). + * Return NAN if input is invalid. * @param curr_wp_ned North/East coordinates of current waypoint in NED frame [m]. * @param prev_wp_ned North/East coordinates of previous waypoint in NED frame [m]. * @param curr_pos_ned North/East coordinates of current position of the vehicle in NED frame [m]. * @param vehicle_speed Vehicle speed [m/s]. - * @param PP_LOOKAHD_GAIN Tuning parameter [-] - * @param PP_LOOKAHD_MAX Maximum lookahead distance [m] - * @param PP_LOOKAHD_MIN Minimum lookahead distance [m] + * @return Bearing towards the intersection point [rad]. */ - float calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, - float vehicle_speed); + float calcTargetBearing(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, + float vehicle_speed); float getLookaheadDistance() {return _lookahead_distance;}; - float getCrosstrackError() {return _curr_pos_to_path.norm();}; - float getDistanceOnLineSegment() {return _distance_on_line_segment.norm();}; + float getDistanceAlongPath() {return _distance_along_path.norm();}; + float getCrosstrackError() {return _crosstrack_error;}; protected: /** @@ -112,6 +124,9 @@ class PurePursuit : public ModuleParams */ void updateParams() override; + // uORB Publication + uORB::Publication _pure_pursuit_pub{ORB_ID(pure_pursuit)}; + struct { param_t lookahead_gain; param_t lookahead_max; @@ -124,7 +139,14 @@ class PurePursuit : public ModuleParams float lookahead_min{1.f}; } _params{}; private: - float _lookahead_distance{0.f}; // Radius of the circle around the vehicle - Vector2f _distance_on_line_segment{}; // Projection of prev_wp_to_curr_pos onto prev_wp_to_curr_wp - Vector2f _curr_pos_to_path{}; // Shortest vector from the current position to the path + /** + * @brief Publish pure pursuit message + */ + void publishPurePursuit(); + + float _target_bearing{NAN}; + float _lookahead_distance{NAN}; // Radius of the circle around the vehicle + float _crosstrack_error{NAN}; // Shortest distance from the current position to the path (Positiv: right of path, Negativ: left of path) + Vector2f _curr_pos_to_curr_wp{}; + Vector2f _distance_along_path{}; // Projection of prev_wp_to_curr_pos onto prev_wp_to_curr_wp }; diff --git a/src/lib/pure_pursuit/PurePursuitTest.cpp b/src/lib/pure_pursuit/PurePursuitTest.cpp index c5b6897fbdaf..6d99b985f858 100644 --- a/src/lib/pure_pursuit/PurePursuitTest.cpp +++ b/src/lib/pure_pursuit/PurePursuitTest.cpp @@ -65,7 +65,7 @@ * PP_LOOKAHD_GAIN = 1.f * PP_LOOKAHD_MAX = 10.f * PP_LOOKAHD_MIN = 1.f - * This way passing the vehicle_speed in calcDesiredHeading function is equivalent to passing + * This way passing the vehicle_speed in calcTargetBearing function is equivalent to passing * the lookahead distance. * ******************************************************************/ @@ -91,12 +91,9 @@ TEST_F(PurePursuitTest, InvalidSpeed) const Vector2f curr_wp_ned(10.f, 10.f); const Vector2f prev_wp_ned(0.f, 0.f); const Vector2f curr_pos_ned(10.f, 0.f); - // Negative speed - const float desired_heading1 = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, -1.f); // NaN speed - const float desired_heading2 = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, NAN); + const float desired_heading1 = pure_pursuit.calcTargetBearing(curr_wp_ned, prev_wp_ned, curr_pos_ned, NAN); EXPECT_FALSE(PX4_ISFINITE(desired_heading1)); - EXPECT_FALSE(PX4_ISFINITE(desired_heading2)); } TEST_F(PurePursuitTest, InvalidWaypoints) @@ -111,14 +108,14 @@ TEST_F(PurePursuitTest, InvalidWaypoints) const Vector2f curr_pos_ned(10.f, 0.f); const float lookahead_distance{5.f}; // Prev WP is NAN - const float desired_heading1 = pure_pursuit.calcDesiredHeading(curr_wp_ned, Vector2f(NAN, NAN), curr_pos_ned, + const float desired_heading1 = pure_pursuit.calcTargetBearing(curr_wp_ned, Vector2f(NAN, NAN), curr_pos_ned, lookahead_distance); // Curr WP is NAN - const float desired_heading2 = pure_pursuit.calcDesiredHeading(Vector2f(NAN, NAN), prev_wp_ned, curr_pos_ned, + const float desired_heading2 = pure_pursuit.calcTargetBearing(Vector2f(NAN, NAN), prev_wp_ned, curr_pos_ned, lookahead_distance); // Curr Pos is NAN - const float desired_heading3 = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, Vector2f(NAN, NAN), + const float desired_heading3 = pure_pursuit.calcTargetBearing(curr_wp_ned, prev_wp_ned, Vector2f(NAN, NAN), lookahead_distance); EXPECT_FALSE(PX4_ISFINITE(desired_heading1)); EXPECT_FALSE(PX4_ISFINITE(desired_heading2)); @@ -133,17 +130,30 @@ TEST_F(PurePursuitTest, OutOfLookahead) // / // / // P - const float desired_heading1 = pure_pursuit.calcDesiredHeading(Vector2f(10.f, 10.f), Vector2f(0.f, 0.f), Vector2f(10.f, - 0.f), - lookahead_distance); + const float desired_heading1 = pure_pursuit.calcTargetBearing(Vector2f(10.f, 10.f), Vector2f(0.f, 0.f), Vector2f(10.f, + 0.f), lookahead_distance); // V // // P ----- C - const float desired_heading2 = pure_pursuit.calcDesiredHeading(Vector2f(0.f, 20.f), Vector2f(0.f, 0.f), Vector2f(10.f, - 10.f), - lookahead_distance); - EXPECT_NEAR(desired_heading1, M_PI_2_F + M_PI_4_F, FLT_EPSILON); // Fallback: Bearing to closest point on path - EXPECT_NEAR(desired_heading2, M_PI_F, FLT_EPSILON); // Fallback: Bearing to closest point on path + const float desired_heading2 = pure_pursuit.calcTargetBearing(Vector2f(0.f, 20.f), Vector2f(0.f, 0.f), Vector2f(10.f, + 10.f), lookahead_distance); + + // // V + // // + // // P ----- C + const float desired_heading3 = pure_pursuit.calcTargetBearing(Vector2f(0.f, 20.f), Vector2f(0.f, 0.f), Vector2f(10.f, + -10.f), lookahead_distance); + + // // V + // // + // // P ----- C + const float desired_heading4 = pure_pursuit.calcTargetBearing(Vector2f(0.f, 20.f), Vector2f(0.f, 0.f), Vector2f(10.f, + 30.f), lookahead_distance); + + EXPECT_NEAR(desired_heading1, M_PI_2_F + M_PI_4_F, FLT_EPSILON); // Fallback: Bearing to closest point on path + EXPECT_NEAR(desired_heading2, -M_PI_F, FLT_EPSILON); // Fallback: Bearing to closest point on path + EXPECT_NEAR(desired_heading3, M_PI_2_F + M_PI_4_F, FLT_EPSILON); // Fallback: Bearing to previous waypoint + EXPECT_NEAR(desired_heading4, -(M_PI_2_F + M_PI_4_F), FLT_EPSILON); // Fallback: Bearing to current waypoint } TEST_F(PurePursuitTest, WaypointOverlap) @@ -154,17 +164,15 @@ TEST_F(PurePursuitTest, WaypointOverlap) // // // V - const float desired_heading1 = pure_pursuit.calcDesiredHeading(Vector2f(10.f, 10.f), Vector2f(10.f, 10.f), Vector2f(0.f, - 0.f), - lookahead_distance); + const float desired_heading1 = pure_pursuit.calcTargetBearing(Vector2f(10.f, 10.f), Vector2f(10.f, 10.f), Vector2f(0.f, + 0.f), lookahead_distance); // V // // // // C/P - const float desired_heading2 = pure_pursuit.calcDesiredHeading(Vector2f(0.f, 0.f), Vector2f(0.f, 0.f), Vector2f(10.f, - 10.f), - lookahead_distance); + const float desired_heading2 = pure_pursuit.calcTargetBearing(Vector2f(0.f, 0.f), Vector2f(0.f, 0.f), Vector2f(10.f, + 10.f), lookahead_distance); EXPECT_NEAR(desired_heading1, M_PI_4_F, FLT_EPSILON); // Fallback: Bearing to closest point on path EXPECT_NEAR(desired_heading2, -(M_PI_4_F + M_PI_2_F), FLT_EPSILON); // Fallback: Bearing to closest point on path } @@ -173,29 +181,25 @@ TEST_F(PurePursuitTest, CurrAndPrevSameNorthCoordinate) { const float lookahead_distance{5.f}; // P -- V -- C - const float desired_heading1 = pure_pursuit.calcDesiredHeading(Vector2f(0.f, 20.f), Vector2f(0.f, 0.f), Vector2f(0.f, - 10.f), - lookahead_distance); + const float desired_heading1 = pure_pursuit.calcTargetBearing(Vector2f(0.f, 20.f), Vector2f(0.f, 0.f), Vector2f(0.f, + 10.f), lookahead_distance); // V // P ------ C - const float desired_heading2 = pure_pursuit.calcDesiredHeading(Vector2f(0.f, 20.f), Vector2f(0.f, 0.f), - Vector2f(5.f / sqrtf(2.f), 10.f), - lookahead_distance); + const float desired_heading2 = pure_pursuit.calcTargetBearing(Vector2f(0.f, 20.f), Vector2f(0.f, 0.f), + Vector2f(5.f / sqrtf(2.f), 10.f), lookahead_distance); // V // C ------ P - const float desired_heading3 = pure_pursuit.calcDesiredHeading(Vector2f(0.f, 0.f), Vector2f(0.f, 20.f), - Vector2f(5.f / sqrtf(2.f), 10.f), - lookahead_distance); + const float desired_heading3 = pure_pursuit.calcTargetBearing(Vector2f(0.f, 0.f), Vector2f(0.f, 20.f), + Vector2f(5.f / sqrtf(2.f), 10.f), lookahead_distance); // V // // P ------ C - const float desired_heading4 = pure_pursuit.calcDesiredHeading(Vector2f(0.f, 20.f), Vector2f(0.f, 0.f), Vector2f(10.f, - 10.f), - lookahead_distance); + const float desired_heading4 = pure_pursuit.calcTargetBearing(Vector2f(0.f, 20.f), Vector2f(0.f, 0.f), Vector2f(10.f, + 10.f), lookahead_distance); EXPECT_NEAR(desired_heading1, M_PI_2_F, FLT_EPSILON); EXPECT_NEAR(desired_heading2, M_PI_2_F + M_PI_4_F, FLT_EPSILON); EXPECT_NEAR(desired_heading3, -(M_PI_2_F + M_PI_4_F), FLT_EPSILON); - EXPECT_NEAR(desired_heading4, M_PI_F, FLT_EPSILON); // Fallback: Bearing to closest point on path + EXPECT_NEAR(desired_heading4, -M_PI_F, FLT_EPSILON); // Fallback: Bearing to closest point on path } diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 3ff1c321e79b..b20d7924055f 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -99,17 +99,15 @@ 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", 100); add_topic("goto_setpoint", 200); add_topic("position_setpoint_triplet", 200); add_optional_topic("px4io_status"); add_topic("radio_status"); - add_optional_topic("rover_ackermann_guidance_status", 100); add_optional_topic("rover_ackermann_setpoint", 100); add_optional_topic("rover_ackermann_status", 100); - add_optional_topic("rover_differential_guidance_status", 100); add_optional_topic("rover_differential_setpoint", 100); add_optional_topic("rover_differential_status", 100); - add_optional_topic("rover_mecanum_guidance_status", 100); add_optional_topic("rover_mecanum_setpoint", 100); add_optional_topic("rover_mecanum_status", 100); add_topic("rtl_time_estimate", 1000); diff --git a/src/modules/rover_ackermann/RoverAckermann.cpp b/src/modules/rover_ackermann/RoverAckermann.cpp index 5fea7e5174a7..463d55c090ee 100644 --- a/src/modules/rover_ackermann/RoverAckermann.cpp +++ b/src/modules/rover_ackermann/RoverAckermann.cpp @@ -129,7 +129,7 @@ void RoverAckermann::Run() // 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(); + 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceAlongPath(); const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign( rover_ackermann_setpoint.forward_speed_setpoint) * vector_scaling * _pos_ctl_course_direction; diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp index e8c30083c405..30ad9b5c5755 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp @@ -40,7 +40,6 @@ using namespace time_literals; RoverAckermannGuidance::RoverAckermannGuidance(ModuleParams *parent) : ModuleParams(parent) { - _rover_ackermann_guidance_status_pub.advertise(); updateParams(); } @@ -90,14 +89,9 @@ void RoverAckermannGuidance::computeGuidance(const float vehicle_forward_speed, } - // Publish ackermann controller status (logging) - hrt_abstime timestamp = hrt_absolute_time(); - _rover_ackermann_guidance_status.timestamp = timestamp; - _rover_ackermann_guidance_status_pub.publish(_rover_ackermann_guidance_status); - // Publish speed and steering setpoints rover_ackermann_setpoint_s rover_ackermann_setpoint{}; - rover_ackermann_setpoint.timestamp = timestamp; + rover_ackermann_setpoint.timestamp = hrt_absolute_time(); rover_ackermann_setpoint.forward_speed_setpoint = _desired_speed; rover_ackermann_setpoint.forward_speed_setpoint_normalized = NAN; rover_ackermann_setpoint.steering_setpoint = NAN; @@ -157,26 +151,29 @@ void RoverAckermannGuidance::updateWaypointsAndAcceptanceRadius() _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); // Global waypoint coordinates - _prev_wp = _curr_pos.isAllFinite() ? _curr_pos : Vector2d(0, 0); // Fallback if previous waypoint is invalid - _curr_wp = _curr_pos.isAllFinite() ? _curr_pos : Vector2d(0, 0); // Fallback if current waypoint is invalid - _next_wp = _home_position.isAllFinite() ? _home_position : Vector2d(0, 0); // Enables corner slow down with RTL - if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat) && PX4_ISFINITE(position_setpoint_triplet.current.lon)) { _curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); + } else { + _curr_wp = _curr_pos; } + if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat) && PX4_ISFINITE(position_setpoint_triplet.previous.lon)) { _prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon); + } else { + _prev_wp = _curr_pos; } if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat) && PX4_ISFINITE(position_setpoint_triplet.next.lon)) { _next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon); + } else { + _next_wp = _home_position; } // NED waypoint coordinates @@ -281,14 +278,10 @@ float RoverAckermannGuidance::calcDesiredSteering(PurePursuit &pure_pursuit, con const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, const float wheel_base, const float desired_speed, const float vehicle_yaw, const float max_steering, const bool armed) { - const float desired_heading = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, + const float desired_heading = pure_pursuit.updatePurePursuit(curr_wp_ned, prev_wp_ned, curr_pos_ned, desired_speed); const float lookahead_distance = pure_pursuit.getLookaheadDistance(); const float heading_error = matrix::wrap_pi(desired_heading - vehicle_yaw); - // For logging - _rover_ackermann_guidance_status.lookahead_distance = lookahead_distance; - _rover_ackermann_guidance_status.heading_error = (heading_error * 180.f) / (M_PI_F); - float desired_steering{0.f}; if (!armed) { diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp index cf6fa56fc17a..739f330ffcf6 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp @@ -40,7 +40,6 @@ // uORB includes #include #include -#include #include #include #include @@ -167,9 +166,7 @@ class RoverAckermannGuidance : public ModuleParams // uORB publications uORB::Publication _rover_ackermann_setpoint_pub{ORB_ID(rover_ackermann_setpoint)}; - uORB::Publication _rover_ackermann_guidance_status_pub{ORB_ID(rover_ackermann_guidance_status)}; uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; - rover_ackermann_guidance_status_s _rover_ackermann_guidance_status{}; // Class instances MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates diff --git a/src/modules/rover_differential/RoverDifferential.cpp b/src/modules/rover_differential/RoverDifferential.cpp index c5fe294cd3e2..0e572b1704eb 100644 --- a/src/modules/rover_differential/RoverDifferential.cpp +++ b/src/modules/rover_differential/RoverDifferential.cpp @@ -173,12 +173,12 @@ void RoverDifferential::Run() // 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(); + 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceAlongPath(); const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign( rover_differential_setpoint.forward_speed_setpoint) * vector_scaling * _pos_ctl_course_direction; // Calculate yaw setpoint - const float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, + const float yaw_setpoint = _posctl_pure_pursuit.calcTargetBearing(target_waypoint_ned, _pos_ctl_start_position_ned, _curr_pos_ned, fabsf(_vehicle_forward_speed)); rover_differential_setpoint.yaw_setpoint = sign(rover_differential_setpoint.forward_speed_setpoint) >= 0 ? yaw_setpoint : matrix::wrap_pi(M_PI_F + yaw_setpoint); // Flip yaw setpoint when driving backwards diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp index a378de3b9428..e14e3b3dac31 100644 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp +++ b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp @@ -105,7 +105,7 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con _param_rd_max_thr_yaw_r.get(), _max_yaw_accel, _param_rd_wheel_track.get(), dt, _yaw_rate_with_accel_limit, _pid_yaw_rate, false); - } else { // Use normalized setpoint + } else if (PX4_ISFINITE(_rover_differential_setpoint.speed_diff_setpoint_normalized)) { // Use normalized setpoint speed_diff_normalized = calcNormalizedSpeedDiff(_rover_differential_setpoint.speed_diff_setpoint_normalized, vehicle_yaw_rate, _param_rd_max_thr_yaw_r.get(), _max_yaw_accel, _param_rd_wheel_track.get(), dt, _yaw_rate_with_accel_limit, diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp index bb45e6b7b99c..cbbd1cb27ba2 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp @@ -41,7 +41,6 @@ RoverDifferentialGuidance::RoverDifferentialGuidance(ModuleParams *parent) : Mod { updateParams(); _max_forward_speed = _param_rd_max_speed.get(); - _rover_differential_guidance_status_pub.advertise(); } void RoverDifferentialGuidance::updateParams() @@ -62,7 +61,7 @@ void RoverDifferentialGuidance::computeGuidance(const float vehicle_yaw, const f } // State machine - float desired_yaw = _pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, + float desired_yaw = _pure_pursuit.updatePurePursuit(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, math::max(forward_speed, 0.f)); const float heading_error = matrix::wrap_pi(desired_yaw - vehicle_yaw); @@ -125,18 +124,9 @@ void RoverDifferentialGuidance::computeGuidance(const float vehicle_yaw, const f } - // Publish differential guidance status (logging) - hrt_abstime timestamp = hrt_absolute_time(); - rover_differential_guidance_status_s rover_differential_guidance_status{}; - rover_differential_guidance_status.timestamp = timestamp; - rover_differential_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance(); - rover_differential_guidance_status.heading_error_deg = M_RAD_TO_DEG_F * heading_error; - rover_differential_guidance_status.state_machine = (uint8_t) _currentState; - _rover_differential_guidance_status_pub.publish(rover_differential_guidance_status); - // Publish setpoints rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; + rover_differential_setpoint.timestamp = hrt_absolute_time(); rover_differential_setpoint.forward_speed_setpoint = desired_forward_speed; rover_differential_setpoint.forward_speed_setpoint_normalized = NAN; rover_differential_setpoint.yaw_rate_setpoint = NAN; @@ -193,7 +183,7 @@ void RoverDifferentialGuidance::updateWaypoints() _curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); } else { - _curr_wp = Vector2d(0, 0); + _curr_wp = _curr_pos; } if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat) diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp index cc1b03363537..c983b23e48de 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp @@ -46,7 +46,6 @@ #include #include #include -#include #include // Standard libraries @@ -114,7 +113,6 @@ class RoverDifferentialGuidance : public ModuleParams // uORB publications uORB::Publication _rover_differential_setpoint_pub{ORB_ID(rover_differential_setpoint)}; - uORB::Publication _rover_differential_guidance_status_pub{ORB_ID(rover_differential_guidance_status)}; // Variables MapProjection _global_ned_proj_ref{}; // Transform global to ned coordinates. diff --git a/src/modules/rover_mecanum/RoverMecanum.cpp b/src/modules/rover_mecanum/RoverMecanum.cpp index b0729acc024d..8c4d00339862 100644 --- a/src/modules/rover_mecanum/RoverMecanum.cpp +++ b/src/modules/rover_mecanum/RoverMecanum.cpp @@ -209,9 +209,9 @@ void RoverMecanum::Run() // 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(); + 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceAlongPath(); const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + vector_scaling * _pos_ctl_course_direction; - const float desired_heading = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _pos_ctl_start_position_ned, + const float desired_heading = _posctl_pure_pursuit.calcTargetBearing(target_waypoint_ned, _pos_ctl_start_position_ned, _curr_pos_ned, desired_velocity_magnitude); const float heading_error = matrix::wrap_pi(desired_heading - _vehicle_yaw); const Vector2f desired_velocity = desired_velocity_magnitude * Vector2f(cosf(heading_error), sinf(heading_error)); diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp index c58123678452..4571ceb6e300 100644 --- a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp +++ b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp @@ -111,7 +111,7 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl _pid_yaw_rate, false); _rover_mecanum_status.adjusted_yaw_rate_setpoint = _yaw_rate_with_accel_limit.getState(); - } else { // Use normalized setpoint + } else if (PX4_ISFINITE(_rover_mecanum_setpoint.speed_diff_setpoint_normalized)) { // Use normalized setpoint speed_diff_normalized = calcNormalizedSpeedDiff(_rover_mecanum_setpoint.speed_diff_setpoint_normalized, vehicle_yaw_rate, _param_rm_max_thr_yaw_r.get(), _max_yaw_accel, _param_rm_wheel_track.get(), dt, _yaw_rate_with_accel_limit, diff --git a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp index b72478d746e2..19f65d2ee21c 100644 --- a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp +++ b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp @@ -42,7 +42,6 @@ RoverMecanumGuidance::RoverMecanumGuidance(ModuleParams *parent) : ModuleParams( { updateParams(); _max_velocity_magnitude = _param_rm_max_speed.get(); - _rover_mecanum_guidance_status_pub.advertise(); } void RoverMecanumGuidance::updateParams() @@ -72,7 +71,7 @@ void RoverMecanumGuidance::computeGuidance(const float yaw, const int nav_state) // Calculate heading error - const float desired_heading = _pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, + const float desired_heading = _pure_pursuit.updatePurePursuit(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, desired_velocity_magnitude); const float heading_error = matrix::wrap_pi(desired_heading - yaw); @@ -91,20 +90,9 @@ void RoverMecanumGuidance::computeGuidance(const float yaw, const int nav_state) desired_velocity = desired_velocity_magnitude * Vector2f(cosf(heading_error), sinf(heading_error)); } - // Timestamp - hrt_abstime timestamp = hrt_absolute_time(); - - // Publish mecanum controller status (logging) - rover_mecanum_guidance_status_s rover_mecanum_guidance_status{}; - rover_mecanum_guidance_status.timestamp = timestamp; - rover_mecanum_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance(); - rover_mecanum_guidance_status.heading_error = heading_error; - rover_mecanum_guidance_status.desired_speed = desired_velocity_magnitude; - _rover_mecanum_guidance_status_pub.publish(rover_mecanum_guidance_status); - // Publish setpoints rover_mecanum_setpoint_s rover_mecanum_setpoint{}; - rover_mecanum_setpoint.timestamp = timestamp; + rover_mecanum_setpoint.timestamp = hrt_absolute_time();; rover_mecanum_setpoint.forward_speed_setpoint = desired_velocity(0); rover_mecanum_setpoint.forward_speed_setpoint_normalized = NAN; rover_mecanum_setpoint.lateral_speed_setpoint = desired_velocity(1); @@ -163,7 +151,7 @@ void RoverMecanumGuidance::updateWaypoints() _curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); } else { - _curr_wp = Vector2d(0, 0); + _curr_wp = _curr_pos; } if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat) diff --git a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp index 0970b83646e3..07322e7d5dfa 100644 --- a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp +++ b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp @@ -46,7 +46,6 @@ #include #include #include -#include #include // Standard libraries @@ -105,7 +104,6 @@ class RoverMecanumGuidance : public ModuleParams uORB::Subscription _home_position_sub{ORB_ID(home_position)}; // uORB publications - uORB::Publication _rover_mecanum_guidance_status_pub{ORB_ID(rover_mecanum_guidance_status)}; uORB::Publication _rover_mecanum_setpoint_pub{ORB_ID(rover_mecanum_setpoint)}; // Variables