From c90e831332afb189282ff6237df00b3477a1d9b8 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Mon, 9 Dec 2024 15:25:32 +0100 Subject: [PATCH] rover: deprecate guidance status messages --- msg/CMakeLists.txt | 3 --- msg/RoverAckermannGuidanceStatus.msg | 6 ------ msg/RoverDifferentialGuidanceStatus.msg | 7 ------- msg/RoverMecanumGuidanceStatus.msg | 7 ------- src/modules/logger/logged_topics.cpp | 3 --- .../RoverAckermannGuidance.cpp | 12 +----------- .../RoverAckermannGuidance.hpp | 3 --- .../RoverDifferentialGuidance.cpp | 12 +----------- .../RoverDifferentialGuidance.hpp | 2 -- .../RoverMecanumGuidance/RoverMecanumGuidance.cpp | 14 +------------- .../RoverMecanumGuidance/RoverMecanumGuidance.hpp | 2 -- 11 files changed, 3 insertions(+), 68 deletions(-) delete mode 100644 msg/RoverAckermannGuidanceStatus.msg delete mode 100644 msg/RoverDifferentialGuidanceStatus.msg delete mode 100644 msg/RoverMecanumGuidanceStatus.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index b40481d028e4..71c55becad8a 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -184,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/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/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index ba0187fa064e..b20d7924055f 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -104,13 +104,10 @@ void LoggedTopics::add_default_topics() 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/RoverAckermannGuidance/RoverAckermannGuidance.cpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp index b37b92bfe35c..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; @@ -288,10 +282,6 @@ float RoverAckermannGuidance::calcDesiredSteering(PurePursuit &pure_pursuit, con 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/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp index fe60ef7ca311..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() @@ -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; 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/RoverMecanumGuidance/RoverMecanumGuidance.cpp b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp index 501d1f2eaf66..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() @@ -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); 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