From 6cbe6c0e2cc27a9769cfda635a55f819369df70d Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Mon, 26 Aug 2024 16:25:49 +0200 Subject: [PATCH] mecanum: add control folder --- msg/CMakeLists.txt | 1 + msg/RoverMecanumGuidanceStatus.msg | 5 - msg/RoverMecanumSetpoint.msg | 11 + msg/RoverMecanumStatus.msg | 7 +- src/modules/logger/logged_topics.cpp | 1 + src/modules/rover_mecanum/CMakeLists.txt | 2 + src/modules/rover_mecanum/RoverMecanum.cpp | 247 ++++-------------- src/modules/rover_mecanum/RoverMecanum.hpp | 43 +-- .../RoverMecanumControl/CMakeLists.txt | 39 +++ .../RoverMecanumControl.cpp | 212 +++++++++++++++ .../RoverMecanumControl.hpp | 127 +++++++++ .../RoverMecanumGuidance.cpp | 126 +++++---- .../RoverMecanumGuidance.hpp | 35 +-- 13 files changed, 535 insertions(+), 321 deletions(-) create mode 100644 msg/RoverMecanumSetpoint.msg create mode 100644 src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt create mode 100644 src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp create mode 100644 src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 01350b301f19..e1fddcc81c05 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -184,6 +184,7 @@ set(msg_files RoverDifferentialGuidanceStatus.msg RoverDifferentialStatus.msg RoverMecanumGuidanceStatus.msg + RoverMecanumSetpoint.msg RoverMecanumStatus.msg Rpm.msg RtlStatus.msg diff --git a/msg/RoverMecanumGuidanceStatus.msg b/msg/RoverMecanumGuidanceStatus.msg index 50063997203b..16ae72d15bd0 100644 --- a/msg/RoverMecanumGuidanceStatus.msg +++ b/msg/RoverMecanumGuidanceStatus.msg @@ -1,11 +1,6 @@ uint64 timestamp # time since system start (microseconds) -float32 desired_forward_speed # [m/s] Desired forward speed for the rover -float32 desired_lateral_speed # [m/s] Desired forward speed for the rover float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller float32 heading_error_deg # [deg] Heading error of the pure pursuit controller -float32 pid_heading_integral # Integral of the PID for the desired yaw rate during missions -float32 pid_throttle_integral # Integral of the PID for the throttle during missions -uint8 state_machine # Driving state of the rover [0: SPOT_TURNING, 1: DRIVING, 2: GOAL_REACHED] # TOPICS rover_mecanum_guidance_status diff --git a/msg/RoverMecanumSetpoint.msg b/msg/RoverMecanumSetpoint.msg new file mode 100644 index 000000000000..36290a3abefb --- /dev/null +++ b/msg/RoverMecanumSetpoint.msg @@ -0,0 +1,11 @@ +uint64 timestamp # time since system start (microseconds) + +float32 forward_speed_setpoint # [m/s or normalized [-1, 1]] Desired forward speed for the rover +float32 lateral_speed_setpoint # [m/s or normalized [-1, 1]] Desired lateral speed for the rover +float32 yaw_rate_setpoint # [rad/s or normalized [-1, 1]] Desired yaw rate for the rover +float32 yaw_setpoint # [rad] Desired yaw (heading) for the rover +bool closed_loop_speed # Use closed loop velocity controller +bool closed_loop_yaw_rate # Use closed loop yaw rate controller +bool closed_loop_yaw # Use closed loop yaw controller + +# TOPICS rover_mecanum_setpoint diff --git a/msg/RoverMecanumStatus.msg b/msg/RoverMecanumStatus.msg index 72e4aa04396c..444347fbe8eb 100644 --- a/msg/RoverMecanumStatus.msg +++ b/msg/RoverMecanumStatus.msg @@ -1,9 +1,12 @@ uint64 timestamp # time since system start (microseconds) -float32 forward_speed # [m/s] Actual forward speed of the rover -float32 lateral_speed # [m/s] Actual lateral speed of the rover +float32 actual_forward_speed # [m/s] Actual forward speed of the rover +float32 desired_forward_speed # [m/s] Desired forward speed of the rover +float32 actual_lateral_speed # [m/s] Actual lateral speed of the rover +float32 desired_lateral_speed # [m/s] Desired lateral speed of the rover float32 desired_yaw_rate_deg_s # [deg/s] Desired yaw rate float32 actual_yaw_rate_deg_s # [deg/s] Actual yaw rate of the rover +float32 actual_yaw_deg # [deg] Actual yaw of the rover float32 pid_yaw_rate_integral # Integral of the PID for the desired yaw rate controller # TOPICS rover_mecanum_status diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 2aae1f476bd1..a1ce2dcd3688 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -107,6 +107,7 @@ void LoggedTopics::add_default_topics() add_optional_topic("rover_differential_guidance_status", 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); add_topic("rtl_status", 2000); diff --git a/src/modules/rover_mecanum/CMakeLists.txt b/src/modules/rover_mecanum/CMakeLists.txt index 5469dba3eb48..0d5919f48714 100644 --- a/src/modules/rover_mecanum/CMakeLists.txt +++ b/src/modules/rover_mecanum/CMakeLists.txt @@ -32,6 +32,7 @@ ############################################################################ add_subdirectory(RoverMecanumGuidance) +add_subdirectory(RoverMecanumControl) px4_add_module( MODULE modules__rover_mecanum @@ -41,6 +42,7 @@ px4_add_module( RoverMecanum.hpp DEPENDS RoverMecanumGuidance + RoverMecanumControl px4_work_queue modules__control_allocator # for parameter CA_R_REV pure_pursuit diff --git a/src/modules/rover_mecanum/RoverMecanum.cpp b/src/modules/rover_mecanum/RoverMecanum.cpp index 38e460a54318..7995c5c3360b 100644 --- a/src/modules/rover_mecanum/RoverMecanum.cpp +++ b/src/modules/rover_mecanum/RoverMecanum.cpp @@ -40,11 +40,7 @@ RoverMecanum::RoverMecanum() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { updateParams(); - _rover_mecanum_status_pub.advertise(); - pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f); - pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); - pid_init(&_pid_lateral_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); - pid_init(&_pid_heading, PID_MODE_DERIVATIV_NONE, 0.001f); + _rover_mecanum_setpoint_pub.advertise(); } bool RoverMecanum::init() @@ -58,31 +54,6 @@ void RoverMecanum::updateParams() ModuleParams::updateParams(); _max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F; - - pid_set_parameters(&_pid_yaw_rate, - _param_rm_yaw_rate_p.get(), // Proportional gain - _param_rm_yaw_rate_i.get(), // Integral gain - 0.f, // Derivative gain - 1.f, // Integral limit - 1.f); // Output limit - pid_set_parameters(&_pid_throttle, - _param_rm_p_gain_speed.get(), // Proportional gain - _param_rm_i_gain_speed.get(), // Integral gain - 0.f, // Derivative gain - 1.f, // Integral limit - 1.f); // Output limit - pid_set_parameters(&_pid_lateral_throttle, - _param_rm_p_gain_speed.get(), // Proportional gain - _param_rm_i_gain_speed.get(), // Integral gain - 0.f, // Derivative gain - 1.f, // Integral limit - 1.f); // Output limit - pid_set_parameters(&_pid_heading, - _param_rm_p_gain_heading.get(), // Proportional gain - _param_rm_i_gain_heading.get(), // Integral gain - 0.f, // Derivative gain - _max_yaw_rate, // Integral limit - _max_yaw_rate); // Output limit } void RoverMecanum::Run() @@ -96,156 +67,96 @@ void RoverMecanum::Run() updateSubscriptions(); // Timestamps - hrt_abstime timestamp_prev = _timestamp; - _timestamp = hrt_absolute_time(); - const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + hrt_abstime timestamp = hrt_absolute_time(); // Navigation modes switch (_nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: { manual_control_setpoint_s manual_control_setpoint{}; + rover_mecanum_setpoint_s rover_mecanum_setpoint{}; if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - _mecanum_setpoint.forward_throttle = manual_control_setpoint.throttle; - _mecanum_setpoint.lateral_throttle = manual_control_setpoint.roll; - _mecanum_setpoint.yaw_rate = manual_control_setpoint.yaw * _param_rm_man_yaw_scale.get(); + rover_mecanum_setpoint.timestamp = timestamp; + rover_mecanum_setpoint.forward_speed_setpoint = manual_control_setpoint.throttle; + rover_mecanum_setpoint.lateral_speed_setpoint = manual_control_setpoint.roll; + rover_mecanum_setpoint.yaw_rate_setpoint = manual_control_setpoint.yaw * _param_rm_man_yaw_scale.get(); + rover_mecanum_setpoint.yaw_setpoint = NAN; + rover_mecanum_setpoint.closed_loop_yaw_rate = false; + rover_mecanum_setpoint.closed_loop_speed = false; + rover_mecanum_setpoint.closed_loop_yaw = false; + _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); } - - _mecanum_setpoint.closed_loop_yaw_rate = false; - _mecanum_setpoint.closed_loop_speed = false; } break; case vehicle_status_s::NAVIGATION_STATE_ACRO: { manual_control_setpoint_s manual_control_setpoint{}; + rover_mecanum_setpoint_s rover_mecanum_setpoint{}; if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - _mecanum_setpoint.forward_throttle = manual_control_setpoint.throttle; - _mecanum_setpoint.lateral_throttle = manual_control_setpoint.roll; - _mecanum_setpoint.yaw_rate = math::interpolate(manual_control_setpoint.yaw, - -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + rover_mecanum_setpoint.timestamp = timestamp; + rover_mecanum_setpoint.forward_speed_setpoint = manual_control_setpoint.throttle; + rover_mecanum_setpoint.lateral_speed_setpoint = manual_control_setpoint.roll; + rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate(manual_control_setpoint.yaw, + -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + rover_mecanum_setpoint.yaw_setpoint = NAN; + rover_mecanum_setpoint.closed_loop_yaw_rate = true; + rover_mecanum_setpoint.closed_loop_speed = false; + rover_mecanum_setpoint.closed_loop_yaw = false; + _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); } - - _mecanum_setpoint.closed_loop_yaw_rate = true; - _mecanum_setpoint.closed_loop_speed = false; } break; case vehicle_status_s::NAVIGATION_STATE_POSCTL: { manual_control_setpoint_s manual_control_setpoint{}; + rover_mecanum_setpoint_s rover_mecanum_setpoint{}; if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - _mecanum_setpoint.forward_throttle = manual_control_setpoint.throttle; - _mecanum_setpoint.lateral_throttle = manual_control_setpoint.roll; - _mecanum_setpoint.yaw_rate = math::interpolate(manual_control_setpoint.yaw, - -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - - if (_mecanum_setpoint.yaw_rate < FLT_EPSILON && _mecanum_setpoint.closed_loop_heading == false) { - _mecanum_setpoint.heading = _vehicle_yaw; - _mecanum_setpoint.closed_loop_heading = true; + rover_mecanum_setpoint.timestamp = timestamp; + rover_mecanum_setpoint.forward_speed_setpoint = math::interpolate(manual_control_setpoint.throttle, + -2.f, 2.f, -_param_rm_max_speed.get(), _param_rm_max_speed.get()); + rover_mecanum_setpoint.lateral_speed_setpoint = math::interpolate(manual_control_setpoint.roll, + -2.f, 2.f, -_param_rm_max_speed.get(), _param_rm_max_speed.get()); + rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate(manual_control_setpoint.yaw, + -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + + if (rover_mecanum_setpoint.yaw_rate_setpoint < FLT_EPSILON && _closed_loop_yaw == false) { + rover_mecanum_setpoint.yaw_setpoint = _vehicle_yaw; + rover_mecanum_setpoint.closed_loop_yaw = true; + _closed_loop_yaw = true; } else { - _mecanum_setpoint.closed_loop_heading = false; + rover_mecanum_setpoint.closed_loop_yaw = false; + _closed_loop_yaw = false; } - } - _mecanum_setpoint.closed_loop_yaw_rate = true; - _mecanum_setpoint.closed_loop_speed = true; + rover_mecanum_setpoint.closed_loop_yaw_rate = true; + rover_mecanum_setpoint.closed_loop_speed = true; + _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); + } } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: - _mecanum_setpoint = _rover_mecanum_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, - _vehicle_lateral_speed, - _nav_state); + _rover_mecanum_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, _vehicle_lateral_speed, _nav_state); break; default: // Unimplemented nav states will stop the rover - _mecanum_setpoint.forward_throttle = 0.f; - _mecanum_setpoint.lateral_throttle = 0.f; - _mecanum_setpoint.yaw_rate = 0.f; - _mecanum_setpoint.closed_loop_yaw_rate = false; - _mecanum_setpoint.closed_loop_speed = false; + rover_mecanum_setpoint_s rover_mecanum_setpoint{}; + rover_mecanum_setpoint.timestamp = timestamp; + rover_mecanum_setpoint.forward_speed_setpoint = 0.f; + rover_mecanum_setpoint.lateral_speed_setpoint = 0.f; + rover_mecanum_setpoint.yaw_rate_setpoint = 0.f; + rover_mecanum_setpoint.yaw_setpoint = NAN; + rover_mecanum_setpoint.closed_loop_yaw_rate = false; + rover_mecanum_setpoint.closed_loop_speed = false; + rover_mecanum_setpoint.closed_loop_yaw = false; + _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); break; } - float speed_diff_normalized = _mecanum_setpoint.yaw_rate; - - // Closed loop heading control - if (_mecanum_setpoint.closed_loop_heading) { - _mecanum_setpoint.yaw_rate = pid_calculate(&_pid_heading, _mecanum_setpoint.heading, _vehicle_yaw, 0, dt); - } - - // Closed loop yaw rate control - if (_mecanum_setpoint.closed_loop_yaw_rate) { - if (fabsf(_mecanum_setpoint.yaw_rate - _vehicle_body_yaw_rate) < YAW_RATE_ERROR_THRESHOLD - || fabsf(_mecanum_setpoint.yaw_rate) < FLT_EPSILON) { - speed_diff_normalized = 0.f; - pid_reset_integral(&_pid_yaw_rate); - - } else { - const float speed_diff = _mecanum_setpoint.yaw_rate * _param_rm_wheel_track.get(); // Feedforward - speed_diff_normalized = math::interpolate(speed_diff, -_param_rm_max_speed.get(), - _param_rm_max_speed.get(), -1.f, 1.f); - speed_diff_normalized = math::constrain(speed_diff_normalized + - pid_calculate(&_pid_yaw_rate, _mecanum_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt), - -1.f, 1.f); // Feedback - } - - } else { - pid_reset_integral(&_pid_yaw_rate); - } - - // Closed loop speed control - float forward_throttle = _mecanum_setpoint.forward_throttle; - float lateral_throttle = _mecanum_setpoint.lateral_throttle; - - if (_mecanum_setpoint.closed_loop_speed) { - if (fabsf(_mecanum_setpoint.forward_throttle) < FLT_EPSILON) { - pid_reset_integral(&_pid_throttle); - - } else { - forward_throttle = pid_calculate(&_pid_throttle, _mecanum_setpoint.forward_throttle, _vehicle_forward_speed, 0, - dt); - - if (_param_rm_max_speed.get() > FLT_EPSILON) { // Feed-forward term - forward_throttle += math::interpolate(_mecanum_setpoint.forward_throttle, - 0.f, _param_rm_max_speed.get(), - 0.f, 1.f); - } - } - - if (fabsf(_mecanum_setpoint.lateral_throttle) < FLT_EPSILON) { - pid_reset_integral(&_pid_lateral_throttle); - - } else { - lateral_throttle = pid_calculate(&_pid_lateral_throttle, _mecanum_setpoint.lateral_throttle, _vehicle_lateral_speed, 0, - dt); - - if (_param_rm_max_speed.get() > FLT_EPSILON) { // Feed-forward term - lateral_throttle += math::interpolate(_mecanum_setpoint.lateral_throttle, - 0.f, _param_rm_max_speed.get(), - 0.f, 1.f); - } - } - } - - // Publish rover mecanum status (logging) - rover_mecanum_status_s rover_mecanum_status{}; - rover_mecanum_status.timestamp = _timestamp; - rover_mecanum_status.forward_speed = _vehicle_forward_speed; - rover_mecanum_status.lateral_speed = _vehicle_lateral_speed; - rover_mecanum_status.desired_yaw_rate_deg_s = M_RAD_TO_DEG_F * _mecanum_setpoint.yaw_rate; - rover_mecanum_status.actual_yaw_rate_deg_s = M_RAD_TO_DEG_F * _vehicle_body_yaw_rate; - rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.integral; - _rover_mecanum_status_pub.publish(rover_mecanum_status); - - // Publish to motors - actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); - computeMotorCommands(forward_throttle, lateral_throttle, - speed_diff_normalized).copyTo(actuator_motors.control); - actuator_motors.timestamp = _timestamp; - _actuator_motors_pub.publish(actuator_motors); + _rover_mecanum_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed, + _vehicle_lateral_speed); } @@ -266,7 +177,7 @@ void RoverMecanum::updateSubscriptions() if (_vehicle_angular_velocity_sub.updated()) { vehicle_angular_velocity_s vehicle_angular_velocity{}; _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); - _vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2]; + _vehicle_yaw_rate = vehicle_angular_velocity.xyz[2]; } if (_vehicle_attitude_sub.updated()) { @@ -286,52 +197,6 @@ void RoverMecanum::updateSubscriptions() } } -matrix::Vector4f RoverMecanum::computeMotorCommands(float forward_speed, float lateral_speed, float speed_diff) -{ - // Prioritize ratio between forward and lateral speed over either magnitude - float combined_speed = fabsf(forward_speed) + fabsf(lateral_speed); - - if (combined_speed > 1.f) { - forward_speed /= combined_speed; - lateral_speed /= combined_speed; - combined_speed = 1.f; - } - - // Prioritize yaw rate over forward and lateral speed - const float total_speed = combined_speed + fabsf(speed_diff); - - if (total_speed > 1.f) { - const float excess_velocity = fabsf(total_speed - 1.f); - const float forward_speed_temp = forward_speed - sign(forward_speed) * 0.5f * excess_velocity; - const float lateral_speed_temp = lateral_speed - sign(lateral_speed) * 0.5f * excess_velocity; - - if (sign(forward_speed_temp) == sign(forward_speed) && sign(lateral_speed) == sign(lateral_speed_temp)) { - forward_speed = forward_speed_temp; - lateral_speed = lateral_speed_temp; - - } else { - forward_speed = lateral_speed = 0.f; - } - } - - // // Prioritize lateral movement over yaw rate - // const float total_speed = combined_speed + fabsf(speed_diff); - - // if (total_speed > 1.f) { - // const float excess_velocity = fabsf(total_speed - 1.f); - // speed_diff -= sign(speed_diff) * excess_velocity; - // } - - // Calculate motor commands - const float input_data[3] = {forward_speed, lateral_speed, speed_diff}; - const Matrix input(input_data); - const float m_data[12] = {1, -1, -1, 1, 1, 1, 1, 1, -1, 1, -1, 1}; - const Matrix m(m_data); - const Vector4f motor_commands = m * input; - - return motor_commands; -} - int RoverMecanum::task_spawn(int argc, char *argv[]) { RoverMecanum *instance = new RoverMecanum(); diff --git a/src/modules/rover_mecanum/RoverMecanum.hpp b/src/modules/rover_mecanum/RoverMecanum.hpp index b4775164d18b..7882253db712 100644 --- a/src/modules/rover_mecanum/RoverMecanum.hpp +++ b/src/modules/rover_mecanum/RoverMecanum.hpp @@ -42,16 +42,14 @@ // uORB includes #include -#include #include #include #include #include -#include -#include #include +#include #include -#include +#include // Standard libraries #include @@ -59,6 +57,7 @@ // Local includes #include "RoverMecanumGuidance/RoverMecanumGuidance.hpp" +#include "RoverMecanumControl/RoverMecanumControl.hpp" using namespace time_literals; @@ -80,16 +79,6 @@ class RoverMecanum : public ModuleBase, public ModuleParams, bool init(); - /** - * @brief Turn normalized speed setpoints into normalized motor commands. - * - * @param forward_speed Normalized linear speed in body forward direction [-1, 1]. - * @param lateral_speed Normalized linear speed in body lateral direction [-1, 1]. - * @param speed_diff Normalized speed difference between left and right wheels [-1, 1]. - * @return matrix::Vector4f: Normalized motor inputs [-1, 1] - */ - matrix::Vector4f computeMotorCommands(float forward_speed, float lateral_speed, float speed_diff); - protected: void updateParams() override; @@ -110,41 +99,25 @@ class RoverMecanum : public ModuleBase, public ModuleParams, uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; // uORB Publications - uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; - uORB::Publication _rover_mecanum_status_pub{ORB_ID(rover_mecanum_status)}; + uORB::Publication _rover_mecanum_setpoint_pub{ORB_ID(rover_mecanum_setpoint)}; // Instances RoverMecanumGuidance _rover_mecanum_guidance{this}; + RoverMecanumControl _rover_mecanum_control{this}; // Variables - float _vehicle_body_yaw_rate{0.f}; + float _vehicle_yaw_rate{0.f}; float _vehicle_forward_speed{0.f}; float _vehicle_lateral_speed{0.f}; float _vehicle_yaw{0.f}; float _max_yaw_rate{0.f}; int _nav_state{0}; matrix::Quatf _vehicle_attitude_quaternion{}; - hrt_abstime _timestamp{0}; - PID_t _pid_yaw_rate; // The PID controller for yaw rate - RoverMecanumGuidance::mecanum_setpoint _mecanum_setpoint; - PID_t _pid_throttle; // The PID controller for velocity - PID_t _pid_lateral_throttle; // The PID controller for velocity - PID_t _pid_heading; // The PID controller for the heading - - // Constants - static constexpr float YAW_RATE_ERROR_THRESHOLD = 1.f; // [rad/s] Error threshold for the closed loop yaw rate control + bool _closed_loop_yaw{false}; DEFINE_PARAMETERS( - (ParamFloat) _param_rm_wheel_track, (ParamFloat) _param_rm_max_speed, (ParamFloat) _param_rm_man_yaw_scale, - (ParamFloat) _param_rm_max_yaw_rate, - (ParamFloat) _param_rm_yaw_rate_p, - (ParamFloat) _param_rm_yaw_rate_i, - (ParamFloat) _param_rm_p_gain_speed, - (ParamFloat) _param_rm_i_gain_speed, - (ParamFloat) _param_rm_p_gain_heading, - (ParamFloat) _param_rm_i_gain_heading, - (ParamInt) _param_r_rev + (ParamFloat) _param_rm_max_yaw_rate ) }; diff --git a/src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt b/src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt new file mode 100644 index 000000000000..9777ae5d18e4 --- /dev/null +++ b/src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(RoverMecanumControl + RoverMecanumControl.cpp +) + +target_link_libraries(RoverMecanumControl PUBLIC pid) +target_include_directories(RoverMecanumControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp new file mode 100644 index 000000000000..357d7de862e6 --- /dev/null +++ b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp @@ -0,0 +1,212 @@ +/**************************************************************************** + * + * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "RoverMecanumControl.hpp" + +#include + +using namespace matrix; +using namespace time_literals; + +RoverMecanumControl::RoverMecanumControl(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); + _rover_mecanum_status_pub.advertise(); // TODO: Check if this works + pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f); + pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); + pid_init(&_pid_lateral_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); + pid_init(&_pid_heading, PID_MODE_DERIVATIV_NONE, 0.001f); +} + +void RoverMecanumControl::updateParams() +{ + ModuleParams::updateParams(); + _max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F; + pid_set_parameters(&_pid_yaw_rate, + _param_rm_yaw_rate_p.get(), // Proportional gain + _param_rm_yaw_rate_i.get(), // Integral gain + 0.f, // Derivative gain + 1.f, // Integral limit + 1.f); // Output limit + pid_set_parameters(&_pid_throttle, + _param_rm_p_gain_speed.get(), // Proportional gain + _param_rm_i_gain_speed.get(), // Integral gain + 0.f, // Derivative gain + 1.f, // Integral limit + 1.f); // Output limit + pid_set_parameters(&_pid_lateral_throttle, + _param_rm_p_gain_speed.get(), // Proportional gain + _param_rm_i_gain_speed.get(), // Integral gain + 0.f, // Derivative gain + 1.f, // Integral limit + 1.f); // Output limit + pid_set_parameters(&_pid_heading, + _param_rm_p_gain_heading.get(), // Proportional gain + _param_rm_i_gain_heading.get(), // Integral gain + 0.f, // Derivative gain + _max_yaw_rate, // Integral limit + _max_yaw_rate); // Output limit +} + +void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate, + const float vehicle_forward_speed, const float vehicle_lateral_speed) +{ + // Timestamps + hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + // Update mecanum setpoint + _rover_mecanum_setpoint_sub.update(&_rover_mecanum_setpoint); + + float speed_diff_normalized = _rover_mecanum_setpoint.yaw_rate_setpoint; + + // Closed loop heading control + if (_rover_mecanum_setpoint.closed_loop_yaw) { + _rover_mecanum_setpoint.yaw_rate_setpoint = pid_calculate(&_pid_heading, _rover_mecanum_setpoint.yaw_setpoint, + vehicle_yaw, 0, dt); + } + + // Closed loop yaw rate control + if (_rover_mecanum_setpoint.closed_loop_yaw_rate) { + if (fabsf(_rover_mecanum_setpoint.yaw_rate_setpoint - vehicle_yaw_rate) < YAW_RATE_ERROR_THRESHOLD + || fabsf(_rover_mecanum_setpoint.yaw_rate_setpoint) < FLT_EPSILON) { + speed_diff_normalized = 0.f; + pid_reset_integral(&_pid_yaw_rate); + + } else { + const float speed_diff = _rover_mecanum_setpoint.yaw_rate_setpoint * _param_rm_wheel_track.get(); // Feedforward + speed_diff_normalized = math::interpolate(speed_diff, -_param_rm_max_speed.get(), + _param_rm_max_speed.get(), -1.f, 1.f); + speed_diff_normalized = math::constrain(speed_diff_normalized + + pid_calculate(&_pid_yaw_rate, _rover_mecanum_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, 0, dt), + -1.f, 1.f); // Feedback + } + + } else { + pid_reset_integral(&_pid_yaw_rate); + } + + // Closed loop speed control + float forward_throttle = _rover_mecanum_setpoint.forward_speed_setpoint; + float lateral_throttle = _rover_mecanum_setpoint.lateral_speed_setpoint; + + if (_rover_mecanum_setpoint.closed_loop_speed) { + if (fabsf(_rover_mecanum_setpoint.forward_speed_setpoint) < FLT_EPSILON) { + pid_reset_integral(&_pid_throttle); + + } else { + forward_throttle = pid_calculate(&_pid_throttle, _rover_mecanum_setpoint.forward_speed_setpoint, + vehicle_forward_speed, 0, dt); + + if (_param_rm_max_speed.get() > FLT_EPSILON) { // Feed-forward term + forward_throttle += math::interpolate(_rover_mecanum_setpoint.forward_speed_setpoint, + 0.f, _param_rm_max_speed.get(), 0.f, 1.f); + } + } + + if (fabsf(_rover_mecanum_setpoint.lateral_speed_setpoint) < FLT_EPSILON) { + pid_reset_integral(&_pid_lateral_throttle); + + } else { + lateral_throttle = pid_calculate(&_pid_lateral_throttle, _rover_mecanum_setpoint.lateral_speed_setpoint, + vehicle_lateral_speed, 0, dt); + + if (_param_rm_max_speed.get() > FLT_EPSILON) { // Feed-forward term + lateral_throttle += math::interpolate(_rover_mecanum_setpoint.lateral_speed_setpoint, + 0.f, _param_rm_max_speed.get(), 0.f, 1.f); + } + } + } + + // Publish rover mecanum status (logging) + rover_mecanum_status_s rover_mecanum_status{}; + rover_mecanum_status.timestamp = _timestamp; + rover_mecanum_status.actual_forward_speed = vehicle_forward_speed; + rover_mecanum_status.desired_forward_speed = _rover_mecanum_setpoint.forward_speed_setpoint; + rover_mecanum_status.actual_lateral_speed = vehicle_lateral_speed; + rover_mecanum_status.desired_lateral_speed = _rover_mecanum_setpoint.lateral_speed_setpoint; + rover_mecanum_status.desired_yaw_rate_deg_s = M_RAD_TO_DEG_F * _rover_mecanum_setpoint.yaw_rate_setpoint; + rover_mecanum_status.actual_yaw_rate_deg_s = M_RAD_TO_DEG_F * vehicle_yaw_rate; + rover_mecanum_status.actual_yaw_deg = M_RAD_TO_DEG_F * vehicle_yaw; + rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.integral; + _rover_mecanum_status_pub.publish(rover_mecanum_status); + + // Publish to motors + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + computeInverseKinematics(forward_throttle, lateral_throttle, + speed_diff_normalized).copyTo(actuator_motors.control); + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); + +} + +matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_throttle, float lateral_throttle, + float speed_diff) +{ + // Prioritize ratio between forward and lateral speed over either magnitude + float combined_speed = fabsf(forward_throttle) + fabsf(lateral_throttle); + + if (combined_speed > 1.f) { + forward_throttle /= combined_speed; + lateral_throttle /= combined_speed; + combined_speed = 1.f; + } + + // Prioritize yaw rate over forward and lateral speed + const float total_speed = combined_speed + fabsf(speed_diff); + + if (total_speed > 1.f) { + const float excess_velocity = fabsf(total_speed - 1.f); + const float forward_throttle_temp = forward_throttle - sign(forward_throttle) * 0.5f * excess_velocity; + const float lateral_throttle_temp = lateral_throttle - sign(lateral_throttle) * 0.5f * excess_velocity; + + if (sign(forward_throttle_temp) == sign(forward_throttle) && sign(lateral_throttle) == sign(lateral_throttle_temp)) { + forward_throttle = forward_throttle_temp; + lateral_throttle = lateral_throttle_temp; + + } else { + forward_throttle = lateral_throttle = 0.f; + } + } + + // Calculate motor commands + const float input_data[3] = {forward_throttle, lateral_throttle, speed_diff}; + const Matrix input(input_data); + const float m_data[12] = {1, -1, -1, 1, 1, 1, 1, 1, -1, 1, -1, 1}; + const Matrix m(m_data); + const Vector4f motor_commands = m * input; + + return motor_commands; +} diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp new file mode 100644 index 000000000000..6247f71222c8 --- /dev/null +++ b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp @@ -0,0 +1,127 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include + +// uORB includes +#include +#include +#include +#include +#include +#include + + +// Standard libraries +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Class for mecanum rover guidance. + */ +class RoverMecanumControl : public ModuleParams +{ +public: + /** + * @brief Constructor for RoverMecanumControl. + * @param parent The parent ModuleParams object. + */ + RoverMecanumControl(ModuleParams *parent); + ~RoverMecanumControl() = default; + + /** + * @brief Compute motor commands based on setpoints + */ + void computeMotorCommands(float vehicle_yaw, float vehicle_yaw_rate, float vehicle_forward_speed, + float vehicle_lateral_speed); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Turn normalized speed setpoints into normalized motor commands. + * + * @param forward_speed Normalized linear speed in body forward direction [-1, 1]. + * @param lateral_speed Normalized linear speed in body lateral direction [-1, 1]. + * @param speed_diff Normalized speed difference between left and right wheels [-1, 1]. + * @return matrix::Vector4f: Normalized motor inputs [-1, 1] + */ + matrix::Vector4f computeInverseKinematics(float forward_speed, float lateral_speed, float speed_diff); + + // uORB subscriptions + uORB::Subscription _rover_mecanum_setpoint_sub{ORB_ID(rover_mecanum_setpoint)}; + + // uORB publications + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _rover_mecanum_status_pub{ORB_ID(rover_mecanum_status)}; + + // Variables + rover_mecanum_setpoint_s _rover_mecanum_setpoint{}; + hrt_abstime _timestamp{0}; + float _max_yaw_rate{0.f}; + + // Controllers + PID_t _pid_throttle; // The PID controller for velocity + PID_t _pid_lateral_throttle; // The PID controller for velocity + PID_t _pid_heading; // The PID controller for the heading + PID_t _pid_yaw_rate; // The PID controller for yaw rate + + // Constants + static constexpr float YAW_RATE_ERROR_THRESHOLD = 1.f; // [rad/s] Error threshold for the closed loop yaw rate control + + // Parameters + DEFINE_PARAMETERS( + (ParamFloat) _param_rm_wheel_track, + (ParamFloat) _param_rm_max_speed, + (ParamFloat) _param_rm_max_yaw_rate, + (ParamFloat) _param_rm_yaw_rate_p, + (ParamFloat) _param_rm_yaw_rate_i, + (ParamFloat) _param_rm_p_gain_speed, + (ParamFloat) _param_rm_i_gain_speed, + (ParamFloat) _param_rm_p_gain_heading, + (ParamFloat) _param_rm_i_gain_heading, + (ParamInt) _param_r_rev + ) +}; diff --git a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp index 9f7c91384c55..8dd57e06de15 100644 --- a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp +++ b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp @@ -47,22 +47,71 @@ RoverMecanumGuidance::RoverMecanumGuidance(ModuleParams *parent) : ModuleParams( void RoverMecanumGuidance::updateParams() { ModuleParams::updateParams(); - - _max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F; } -RoverMecanumGuidance::mecanum_setpoint RoverMecanumGuidance::computeGuidance(const float yaw, +void RoverMecanumGuidance::computeGuidance(const float yaw, const float forward_speed, const float lateral_speed, const int nav_state) { + updateSubscriptions(); + // Initializations - bool mission_finished{false}; - float desired_speed{0.f}; + float desired_speed{_param_rm_miss_spd_def.get()}; Vector2f desired_velocity(0.f, 0.f); - // hrt_abstime timestamp_prev = _timestamp; - // _timestamp = hrt_absolute_time(); - // const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + const float distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), + _next_wp(0), _next_wp(1)); + const float distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), + _curr_wp(0), _curr_wp(1)); + + // Calculate desired speed + if (_theta < SLOW_DOWN_THRESHOLD) { + if (_param_rm_max_jerk.get() > FLT_EPSILON && _param_rm_max_accel.get() > FLT_EPSILON) { + desired_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rm_max_jerk.get(), + _param_rm_max_accel.get(), distance_to_curr_wp, 0.0f); + desired_speed = math::constrain(desired_speed, -_param_rm_max_speed.get(), _param_rm_max_speed.get()); + } + + } + + // Calculate heading error + const float desired_heading = _pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, + desired_speed); + const float heading_error = matrix::wrap_pi(desired_heading - yaw); + + // Catch return to launch + if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { + _mission_finished = distance_to_next_wp < _param_nav_acc_rad.get(); + } + + // Guidance logic + if (!_mission_finished && distance_to_curr_wp > _param_nav_acc_rad.get()) { + desired_velocity = desired_speed * Vector2f(cosf(heading_error), sinf(heading_error)); + } - // uORB subscriber updates + // 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_deg = M_RAD_TO_DEG_F * heading_error; + _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.forward_speed_setpoint = desired_velocity(0); + rover_mecanum_setpoint.lateral_speed_setpoint = desired_velocity(1); + rover_mecanum_setpoint.yaw_rate_setpoint = NAN; + rover_mecanum_setpoint.yaw_setpoint = 0.f; // TODO: Make this variable + rover_mecanum_setpoint.closed_loop_yaw_rate = true; + rover_mecanum_setpoint.closed_loop_speed = true; + rover_mecanum_setpoint.closed_loop_yaw = true; + _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); +} + +void RoverMecanumGuidance::updateSubscriptions() +{ if (_vehicle_global_position_sub.updated()) { vehicle_global_position_s vehicle_global_position{}; _vehicle_global_position_sub.copy(&vehicle_global_position); @@ -88,7 +137,7 @@ RoverMecanumGuidance::mecanum_setpoint RoverMecanumGuidance::computeGuidance(con if (_mission_result_sub.updated()) { mission_result_s mission_result{}; _mission_result_sub.copy(&mission_result); - mission_finished = mission_result.finished; + _mission_finished = mission_result.finished; } if (_home_position_sub.updated()) { @@ -96,63 +145,6 @@ RoverMecanumGuidance::mecanum_setpoint RoverMecanumGuidance::computeGuidance(con _home_position_sub.copy(&home_position); _home_position = Vector2d(home_position.lat, home_position.lon); } - - desired_speed = _param_rm_miss_spd_def.get(); - - const float distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), - _curr_wp(0), - _curr_wp(1)); - - if (_theta < SLOW_DOWN_THRESHOLD) { - if (_param_rm_max_jerk.get() > FLT_EPSILON && _param_rm_max_accel.get() > FLT_EPSILON) { - desired_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rm_max_jerk.get(), - _param_rm_max_accel.get(), distance_to_next_wp, 0.0f); - desired_speed = math::constrain(desired_speed, -_param_rm_max_speed.get(), _param_rm_max_speed.get()); - } - - } else { - desired_speed = _param_rm_max_speed.get(); - } - - const float desired_heading = _pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, - desired_speed); - - const float heading_error = matrix::wrap_pi(desired_heading - yaw); - - - - if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL - && distance_to_next_wp < _param_nav_acc_rad.get()) { // Return to launch - mission_finished = true; - } - - if (!mission_finished) { - desired_velocity = desired_speed * Vector2f(cosf(heading_error), sinf(heading_error)); - } - - // Publish mecanum controller status (logging) - _rover_mecanum_guidance_status.timestamp = _timestamp; - _rover_mecanum_guidance_status.desired_forward_speed = desired_velocity(0); - _rover_mecanum_guidance_status.desired_lateral_speed = desired_velocity(1); - _rover_mecanum_guidance_status.pid_throttle_integral = 0.f; - _rover_mecanum_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance(); - _rover_mecanum_guidance_status.pid_heading_integral = 0.f; - _rover_mecanum_guidance_status.heading_error_deg = M_RAD_TO_DEG_F * heading_error; - _rover_mecanum_guidance_status.state_machine = (uint8_t) _currentState; - _rover_mecanum_guidance_status_pub.publish(_rover_mecanum_guidance_status); - - - // Return setpoints - mecanum_setpoint mecanum_setpoint_temp; - mecanum_setpoint_temp.forward_throttle = desired_velocity(0); - mecanum_setpoint_temp.lateral_throttle = desired_velocity(1); - mecanum_setpoint_temp.heading = 0.f; // TODO: Make this variable - mecanum_setpoint_temp.yaw_rate = 0.f; - mecanum_setpoint_temp.closed_loop_yaw_rate = true; - mecanum_setpoint_temp.closed_loop_speed = true; - mecanum_setpoint_temp.closed_loop_heading = true; - return mecanum_setpoint_temp; - } void RoverMecanumGuidance::updateWaypoints() diff --git a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp index 7f567e5f0c4f..fe22ecc1a80e 100644 --- a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp +++ b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp @@ -47,6 +47,7 @@ #include #include #include +#include // Standard libraries #include @@ -71,16 +72,6 @@ class RoverMecanumGuidance : public ModuleParams RoverMecanumGuidance(ModuleParams *parent); ~RoverMecanumGuidance() = default; - struct mecanum_setpoint { - float forward_throttle{0.f}; - float lateral_throttle{0.f}; - float yaw_rate{0.f}; - float heading{0.f}; - bool closed_loop_yaw_rate{false}; - bool closed_loop_speed{false}; - bool closed_loop_heading{false}; - }; - /** * @brief Compute guidance for the vehicle. * @param yaw The yaw orientation of the vehicle in radians. @@ -88,13 +79,7 @@ class RoverMecanumGuidance : public ModuleParams * @param dt The time step in seconds. * @param nav_state Navigation state of the rover. */ - RoverMecanumGuidance::mecanum_setpoint computeGuidance(float yaw, float forward_speed, float lateral_speed, - int nav_state); - - /** - * @brief Update global/ned waypoint coordinates - */ - void updateWaypoints(); + void computeGuidance(float yaw, float forward_speed, float lateral_speed, int nav_state); protected: /** @@ -103,6 +88,16 @@ class RoverMecanumGuidance : public ModuleParams void updateParams() override; private: + /** + * @brief Update uORB subscriptions + */ + void updateSubscriptions(); + + /** + * @brief Update global/ned waypoint coordinates + */ + void updateWaypoints(); + // uORB subscriptions uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; @@ -112,15 +107,13 @@ class RoverMecanumGuidance : public ModuleParams // uORB publications uORB::Publication _rover_mecanum_guidance_status_pub{ORB_ID(rover_mecanum_guidance_status)}; - rover_mecanum_guidance_status_s _rover_mecanum_guidance_status{}; + uORB::Publication _rover_mecanum_setpoint_pub{ORB_ID(rover_mecanum_setpoint)}; // Variables MapProjection _global_ned_proj_ref{}; // Transform global to ned coordinates. - GuidanceState _currentState{GuidanceState::DRIVING}; // The current state of guidance. PurePursuit _pure_pursuit{this}; // Pure pursuit library - hrt_abstime _timestamp{0}; - float _max_yaw_rate{0.f}; float _theta{0.f}; + bool _mission_finished{false}; // Waypoints Vector2d _curr_pos{};