From c955aaffb66a7d0cda5468160d03714e2f2686ca Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Thu, 8 Aug 2024 15:19:43 +0200 Subject: [PATCH] mecanum: add module --- .../airframes/4014_gz_r1_rover_mecanum | 61 +++++ .../init.d-posix/airframes/CMakeLists.txt | 1 + ROMFS/px4fmu_common/init.d/CMakeLists.txt | 7 + .../init.d/rc.rover_mecanum_apps | 8 + .../init.d/rc.rover_mecanum_defaults | 11 + ROMFS/px4fmu_common/init.d/rc.vehicle_setup | 9 + boards/px4/sitl/default.px4board | 1 + msg/CMakeLists.txt | 3 + msg/RoverMecanumGuidanceStatus.msg | 6 + msg/RoverMecanumSetpoint.msg | 11 + msg/RoverMecanumStatus.msg | 12 + src/lib/mixer_module/mixer_module.cpp | 1 + src/modules/control_allocator/module.yaml | 15 ++ src/modules/logger/logged_topics.cpp | 3 + src/modules/rover_mecanum/CMakeLists.txt | 51 ++++ src/modules/rover_mecanum/Kconfig | 6 + src/modules/rover_mecanum/RoverMecanum.cpp | 249 ++++++++++++++++++ src/modules/rover_mecanum/RoverMecanum.hpp | 123 +++++++++ .../RoverMecanumControl/CMakeLists.txt | 39 +++ .../RoverMecanumControl.cpp | 211 +++++++++++++++ .../RoverMecanumControl.hpp | 127 +++++++++ .../RoverMecanumGuidance/CMakeLists.txt | 39 +++ .../RoverMecanumGuidance.cpp | 191 ++++++++++++++ .../RoverMecanumGuidance.hpp | 142 ++++++++++ src/modules/rover_mecanum/module.yaml | 171 ++++++++++++ src/modules/simulation/gz_bridge/module.yaml | 2 +- 26 files changed, 1499 insertions(+), 1 deletion(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/4014_gz_r1_rover_mecanum create mode 100644 ROMFS/px4fmu_common/init.d/rc.rover_mecanum_apps create mode 100644 ROMFS/px4fmu_common/init.d/rc.rover_mecanum_defaults create mode 100644 msg/RoverMecanumGuidanceStatus.msg create mode 100644 msg/RoverMecanumSetpoint.msg create mode 100644 msg/RoverMecanumStatus.msg create mode 100644 src/modules/rover_mecanum/CMakeLists.txt create mode 100644 src/modules/rover_mecanum/Kconfig create mode 100644 src/modules/rover_mecanum/RoverMecanum.cpp create mode 100644 src/modules/rover_mecanum/RoverMecanum.hpp 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 create mode 100644 src/modules/rover_mecanum/RoverMecanumGuidance/CMakeLists.txt create mode 100644 src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp create mode 100644 src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp create mode 100644 src/modules/rover_mecanum/module.yaml diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4014_gz_r1_rover_mecanum b/ROMFS/px4fmu_common/init.d-posix/airframes/4014_gz_r1_rover_mecanum new file mode 100644 index 000000000000..30af670c82bd --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4014_gz_r1_rover_mecanum @@ -0,0 +1,61 @@ +#!/bin/sh +# @name Aion Robotics R1 Rover +# @type Rover +# @class Rover + +. ${R}etc/init.d/rc.rover_mecanum_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=rover} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover_mecanum} + +param set-default SIM_GZ_EN 1 # Gazebo bridge + +# Rover parameters +param set-default RM_WHEEL_TRACK 0.3 +param set-default RM_MAN_YAW_SCALE 0.1 +param set-default RM_YAW_RATE_I 0.1 +param set-default RM_YAW_RATE_P 0.5 +param set-default RM_MAX_ACCEL 6 +param set-default RM_MAX_JERK 30 +param set-default RM_MAX_SPEED 7 +param set-default RM_HEADING_P 5 +param set-default RM_HEADING_I 0.1 +param set-default RM_MAX_YAW_RATE 180 +param set-default RM_MISS_SPD_DEF 7 +param set-default RM_TRANS_DRV_TRN 0.349066 +param set-default RM_TRANS_TRN_DRV 0.174533 + +# Pure pursuit parameters +param set-default PP_LOOKAHD_MAX 30 +param set-default PP_LOOKAHD_MIN 2 +param set-default PP_LOOKAHD_GAIN 1 + +# Simulated sensors +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 0 +param set-default SENS_EN_MAGSIM 1 +param set-default SENS_EN_ARSPDSIM 0 + +# Actuator mapping +param set-default SIM_GZ_WH_FUNC1 102 # right wheel front +param set-default SIM_GZ_WH_MIN1 0 +param set-default SIM_GZ_WH_MAX1 200 +param set-default SIM_GZ_WH_DIS1 100 + +param set-default SIM_GZ_WH_FUNC2 101 # left wheel front +param set-default SIM_GZ_WH_MIN2 0 +param set-default SIM_GZ_WH_MAX2 200 +param set-default SIM_GZ_WH_DIS2 100 + +param set-default SIM_GZ_WH_FUNC3 104 # right wheel back +param set-default SIM_GZ_WH_MIN3 0 +param set-default SIM_GZ_WH_MAX3 200 +param set-default SIM_GZ_WH_DIS3 100 + +param set-default SIM_GZ_WH_FUNC4 103 # left wheel back +param set-default SIM_GZ_WH_MIN4 0 +param set-default SIM_GZ_WH_MAX4 200 +param set-default SIM_GZ_WH_DIS4 100 + +param set-default SIM_GZ_WH_REV 10 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 9235b2e66340..ce787d8cba1a 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -85,6 +85,7 @@ px4_add_romfs_files( 4011_gz_lawnmower 4012_gz_rover_ackermann 4013_gz_x500_lidar + 4014_gz_r1_rover_mecanum 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 3319779096c5..488b41575a59 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -92,6 +92,13 @@ if(CONFIG_MODULES_ROVER_ACKERMANN) ) endif() +if(CONFIG_MODULES_ROVER_MECANUM) + px4_add_romfs_files( + rc.rover_mecanum_apps + rc.rover_mecanum_defaults + ) +endif() + if(CONFIG_MODULES_UUV_ATT_CONTROL) px4_add_romfs_files( rc.uuv_apps diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_mecanum_apps b/ROMFS/px4fmu_common/init.d/rc.rover_mecanum_apps new file mode 100644 index 000000000000..77b131c984a0 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.rover_mecanum_apps @@ -0,0 +1,8 @@ +#!/bin/sh +# Standard apps for a mecanum drive rover. + +# Start rover mecanum drive controller. +rover_mecanum start + +# Start Land Detector. +land_detector start rover diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_mecanum_defaults b/ROMFS/px4fmu_common/init.d/rc.rover_mecanum_defaults new file mode 100644 index 000000000000..9e51a3401592 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.rover_mecanum_defaults @@ -0,0 +1,11 @@ +#!/bin/sh +# Mecanum rover parameters. + +set VEHICLE_TYPE rover_mecanum +param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER +param set-default CA_AIRFRAME 13 # Rover (Mecanum) +param set-default CA_R_REV 15 # Right and left motors reversible +param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying +param set-default NAV_ACC_RAD 0.5 # Waypoint acceptance radius +param set-default EKF2_GBIAS_INIT 0.01 +param set-default EKF2_ANGERR_INIT 0.01 diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index ef455c2ff759..ae34645ad890 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup @@ -50,6 +50,15 @@ then . ${R}etc/init.d/rc.rover_ackermann_apps fi +# +# Mecanum Rover setup. +# +if [ $VEHICLE_TYPE = rover_mecanum ] +then + # Start mecanum drive rover apps. + . ${R}etc/init.d/rc.rover_mecanum_apps +fi + # # VTOL setup. # diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index ac228113d5d8..f157cf5ae8d7 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -47,6 +47,7 @@ CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_REPLAY=y CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y +CONFIG_MODULES_ROVER_MECANUM=y CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y CONFIG_COMMON_SIMULATION=y diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 3b31b7de554d..e1fddcc81c05 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -183,6 +183,9 @@ set(msg_files RoverAckermannStatus.msg RoverDifferentialGuidanceStatus.msg RoverDifferentialStatus.msg + RoverMecanumGuidanceStatus.msg + RoverMecanumSetpoint.msg + RoverMecanumStatus.msg Rpm.msg RtlStatus.msg RtlTimeEstimate.msg diff --git a/msg/RoverMecanumGuidanceStatus.msg b/msg/RoverMecanumGuidanceStatus.msg new file mode 100644 index 000000000000..16ae72d15bd0 --- /dev/null +++ b/msg/RoverMecanumGuidanceStatus.msg @@ -0,0 +1,6 @@ +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 + +# 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 new file mode 100644 index 000000000000..444347fbe8eb --- /dev/null +++ b/msg/RoverMecanumStatus.msg @@ -0,0 +1,12 @@ +uint64 timestamp # time since system start (microseconds) + +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/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 45f56ab54025..cfa2fb0e2611 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -438,6 +438,7 @@ bool MixingOutput::update() _reversible_mask = 0; for (int i = 0; i < _max_num_outputs; ++i) { + if (_functions[i]) { all_disabled = false; diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 8683d7477d95..778007b5bb87 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -30,6 +30,7 @@ parameters: 10: Helicopter (tail ESC) 11: Helicopter (tail Servo) 12: Helicopter (Coaxial) + 13: Rover (Mecanum) default: 0 CA_METHOD: @@ -1140,3 +1141,17 @@ mixer: parameters: - label: 'Throttle spoolup time' name: COM_SPOOLUP_TIME + + 13: # Rover (Mecanum) + title: 'Rover (Mecanum)' + actuators: + - actuator_type: 'motor' + instances: + - name: 'Right Motor Front' + position: [ 1, 1., 0 ] + - name: 'Left Motor Front' + position: [ 1, -1., 0 ] + - name: 'Right Motor Back' + position: [ -1, 1., 0 ] + - name: 'Left Motor Back' + position: [ -1, -1., 0 ] diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 5be39234e34f..a1ce2dcd3688 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -106,6 +106,9 @@ void LoggedTopics::add_default_topics() add_optional_topic("rover_ackermann_status", 100); 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); add_optional_topic("sensor_airflow", 100); diff --git a/src/modules/rover_mecanum/CMakeLists.txt b/src/modules/rover_mecanum/CMakeLists.txt new file mode 100644 index 000000000000..0d5919f48714 --- /dev/null +++ b/src/modules/rover_mecanum/CMakeLists.txt @@ -0,0 +1,51 @@ +############################################################################ +# +# 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. +# +############################################################################ + +add_subdirectory(RoverMecanumGuidance) +add_subdirectory(RoverMecanumControl) + +px4_add_module( + MODULE modules__rover_mecanum + MAIN rover_mecanum + SRCS + RoverMecanum.cpp + RoverMecanum.hpp + DEPENDS + RoverMecanumGuidance + RoverMecanumControl + px4_work_queue + modules__control_allocator # for parameter CA_R_REV + pure_pursuit + MODULE_CONFIG + module.yaml +) diff --git a/src/modules/rover_mecanum/Kconfig b/src/modules/rover_mecanum/Kconfig new file mode 100644 index 000000000000..46a24b3fc899 --- /dev/null +++ b/src/modules/rover_mecanum/Kconfig @@ -0,0 +1,6 @@ +menuconfig MODULES_ROVER_MECANUM + bool "rover_mecanum" + default n + depends on MODULES_CONTROL_ALLOCATOR + ---help--- + Enable support for control of mecanum rovers diff --git a/src/modules/rover_mecanum/RoverMecanum.cpp b/src/modules/rover_mecanum/RoverMecanum.cpp new file mode 100644 index 000000000000..7995c5c3360b --- /dev/null +++ b/src/modules/rover_mecanum/RoverMecanum.cpp @@ -0,0 +1,249 @@ +/**************************************************************************** + * + * 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 "RoverMecanum.hpp" +using namespace matrix; +using namespace time_literals; + +RoverMecanum::RoverMecanum() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) +{ + updateParams(); + _rover_mecanum_setpoint_pub.advertise(); +} + +bool RoverMecanum::init() +{ + ScheduleOnInterval(10_ms); // 100 Hz + return true; +} + +void RoverMecanum::updateParams() +{ + ModuleParams::updateParams(); + + _max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F; +} + +void RoverMecanum::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + updateSubscriptions(); + + // Timestamps + 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)) { + 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); + + } + } 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)) { + 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); + } + } 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)) { + 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 { + rover_mecanum_setpoint.closed_loop_yaw = false; + _closed_loop_yaw = false; + } + + 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: + _rover_mecanum_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, _vehicle_lateral_speed, _nav_state); + break; + + default: // Unimplemented nav states will stop the rover + 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; + } + + _rover_mecanum_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed, + _vehicle_lateral_speed); + +} + +void RoverMecanum::updateSubscriptions() +{ + if (_parameter_update_sub.updated()) { + parameter_update_s parameter_update; + _parameter_update_sub.copy(¶meter_update); + updateParams(); + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); + _nav_state = vehicle_status.nav_state; + } + + if (_vehicle_angular_velocity_sub.updated()) { + vehicle_angular_velocity_s vehicle_angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); + _vehicle_yaw_rate = vehicle_angular_velocity.xyz[2]; + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); + } + + if (_vehicle_local_position_sub.updated()) { + vehicle_local_position_s vehicle_local_position{}; + _vehicle_local_position_sub.copy(&vehicle_local_position); + Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); + _vehicle_forward_speed = velocity_in_body_frame(0); + _vehicle_lateral_speed = velocity_in_body_frame(1); + } +} + +int RoverMecanum::task_spawn(int argc, char *argv[]) +{ + RoverMecanum *instance = new RoverMecanum(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int RoverMecanum::custom_command(int argc, char *argv[]) +{ + return print_usage("unk_timestampn command"); +} + +int RoverMecanum::print_usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Rover Mecanum controller. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("rover_mecanum", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +extern "C" __EXPORT int rover_mecanum_main(int argc, char *argv[]) +{ + return RoverMecanum::main(argc, argv); +} diff --git a/src/modules/rover_mecanum/RoverMecanum.hpp b/src/modules/rover_mecanum/RoverMecanum.hpp new file mode 100644 index 000000000000..7882253db712 --- /dev/null +++ b/src/modules/rover_mecanum/RoverMecanum.hpp @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Standard libraries +#include +#include + +// Local includes +#include "RoverMecanumGuidance/RoverMecanumGuidance.hpp" +#include "RoverMecanumControl/RoverMecanumControl.hpp" + +using namespace time_literals; + +class RoverMecanum : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + RoverMecanum(); + ~RoverMecanum() override = default; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +protected: + void updateParams() override; + +private: + void Run() override; + + /** + * @brief Update uORB subscriptions. + */ + void updateSubscriptions(); + + // uORB Subscriptions + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + // uORB Publications + 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_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{}; + bool _closed_loop_yaw{false}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_rm_max_speed, + (ParamFloat) _param_rm_man_yaw_scale, + (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..8b0140722f19 --- /dev/null +++ b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp @@ -0,0 +1,211 @@ +/**************************************************************************** + * + * 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) < 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/CMakeLists.txt b/src/modules/rover_mecanum/RoverMecanumGuidance/CMakeLists.txt new file mode 100644 index 000000000000..94bb824eeb51 --- /dev/null +++ b/src/modules/rover_mecanum/RoverMecanumGuidance/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(RoverMecanumGuidance + RoverMecanumGuidance.cpp +) + +target_link_libraries(RoverMecanumGuidance PUBLIC pid) +target_include_directories(RoverMecanumGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp new file mode 100644 index 000000000000..8dd57e06de15 --- /dev/null +++ b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp @@ -0,0 +1,191 @@ +/**************************************************************************** + * + * 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 "RoverMecanumGuidance.hpp" + +#include + +using namespace matrix; +using namespace time_literals; + +RoverMecanumGuidance::RoverMecanumGuidance(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); + _rover_mecanum_guidance_status_pub.advertise(); +} + +void RoverMecanumGuidance::updateParams() +{ + ModuleParams::updateParams(); +} + +void RoverMecanumGuidance::computeGuidance(const float yaw, + const float forward_speed, const float lateral_speed, const int nav_state) +{ + updateSubscriptions(); + + // Initializations + float desired_speed{_param_rm_miss_spd_def.get()}; + Vector2f desired_velocity(0.f, 0.f); + 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)); + } + + // 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); + _curr_pos = Vector2d(vehicle_global_position.lat, vehicle_global_position.lon); + } + + if (_local_position_sub.updated()) { + vehicle_local_position_s local_position{}; + _local_position_sub.copy(&local_position); + + if (!_global_ned_proj_ref.isInitialized() + || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != local_position.ref_timestamp)) { + _global_ned_proj_ref.initReference(local_position.ref_lat, local_position.ref_lon, local_position.ref_timestamp); + } + + _curr_pos_ned = Vector2f(local_position.x, local_position.y); + } + + if (_position_setpoint_triplet_sub.updated()) { + updateWaypoints(); + } + + if (_mission_result_sub.updated()) { + mission_result_s mission_result{}; + _mission_result_sub.copy(&mission_result); + _mission_finished = mission_result.finished; + } + + if (_home_position_sub.updated()) { + home_position_s home_position{}; + _home_position_sub.copy(&home_position); + _home_position = Vector2d(home_position.lat, home_position.lon); + } +} + +void RoverMecanumGuidance::updateWaypoints() +{ + position_setpoint_triplet_s position_setpoint_triplet{}; + _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); + + // Global waypoint coordinates + 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 = Vector2d(0, 0); + } + + 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 + _curr_wp_ned = _global_ned_proj_ref.project(_curr_wp(0), _curr_wp(1)); + _prev_wp_ned = _global_ned_proj_ref.project(_prev_wp(0), _prev_wp(1)); + _next_wp_ned = _global_ned_proj_ref.project(_next_wp(0), _next_wp(1)); + + // Distances + const Vector2f curr_to_next_wp_ned = _next_wp_ned - _curr_wp_ned; + const Vector2f curr_to_prev_wp_ned = _prev_wp_ned - _curr_wp_ned; + float cosin = curr_to_prev_wp_ned.unit_or_zero() * curr_to_next_wp_ned.unit_or_zero(); + cosin = math::constrain(cosin, -1.f, 1.f); // Protect against float precision problem + _theta = acosf(cosin); +} diff --git a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp new file mode 100644 index 000000000000..fe22ecc1a80e --- /dev/null +++ b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp @@ -0,0 +1,142 @@ +/**************************************************************************** + * + * 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 +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Standard libraries +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Class for mecanum rover guidance. + */ +class RoverMecanumGuidance : public ModuleParams +{ +public: + /** + * @brief Constructor for RoverMecanumGuidance. + * @param parent The parent ModuleParams object. + */ + RoverMecanumGuidance(ModuleParams *parent); + ~RoverMecanumGuidance() = default; + + /** + * @brief Compute guidance for the vehicle. + * @param yaw The yaw orientation of the vehicle in radians. + * @param forward_speed The velocity of the vehicle in m/s. + * @param dt The time step in seconds. + * @param nav_state Navigation state of the rover. + */ + void computeGuidance(float yaw, float forward_speed, float lateral_speed, int nav_state); + +protected: + /** + * @brief Update the parameters of the module. + */ + 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)}; + uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; + uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; + 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 + MapProjection _global_ned_proj_ref{}; // Transform global to ned coordinates. + PurePursuit _pure_pursuit{this}; // Pure pursuit library + float _theta{0.f}; + bool _mission_finished{false}; + + // Waypoints + Vector2d _curr_pos{}; + Vector2f _curr_pos_ned{}; + Vector2d _prev_wp{}; + Vector2f _prev_wp_ned{}; + Vector2d _curr_wp{}; + Vector2f _curr_wp_ned{}; + Vector2d _next_wp{}; + Vector2f _next_wp_ned{}; + Vector2d _home_position{}; + + // Constants + static constexpr float SLOW_DOWN_THRESHOLD = + 2.61799f; // Slow down threshold for the maximum angle between the prev-curr and curr-next waypoint segments. + + // Parameters + DEFINE_PARAMETERS( + (ParamFloat) _param_rm_max_speed, + (ParamFloat) _param_nav_acc_rad, + (ParamFloat) _param_rm_max_jerk, + (ParamFloat) _param_rm_max_accel, + (ParamFloat) _param_rm_miss_spd_def, + (ParamFloat) _param_rm_max_yaw_rate + ) +}; diff --git a/src/modules/rover_mecanum/module.yaml b/src/modules/rover_mecanum/module.yaml new file mode 100644 index 000000000000..242943fbc0af --- /dev/null +++ b/src/modules/rover_mecanum/module.yaml @@ -0,0 +1,171 @@ +module_name: Rover Mecanum + +parameters: + - group: Rover Mecanum + definitions: + RM_WHEEL_TRACK: + description: + short: Wheel track + long: Distance from the center of the right wheel to the center of the left wheel + type: float + unit: m + min: 0.001 + max: 100 + increment: 0.001 + decimal: 3 + default: 0.5 + + RM_MAX_SPEED: + description: + short: Maximum speed the rover can drive + long: This parameter is used to map desired speeds to normalized motor commands. + type: float + unit: m/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 7 + + RM_MAN_YAW_SCALE: + description: + short: Manual yaw rate scale + long: | + In manual mode the setpoint for the yaw rate received from the rc remote + is scaled by this value. + type: float + min: 0.01 + max: 1 + increment: 0.01 + decimal: 2 + default: 1 + + RM_MAX_YAW_RATE: + description: + short: Maximum allowed yaw rate for the rover. + long: | + This parameter is used to cap desired yaw rates and map controller inputs to desired yaw rates in acro mode. + type: float + unit: deg/s + min: 0.01 + max: 1000 + increment: 0.01 + decimal: 2 + default: 90 + + RM_YAW_RATE_P: + description: + short: Proportional gain for the closed-loop yaw rate controller. + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + + RM_YAW_RATE_I: + description: + short: Integral gain for the closed-loop yaw rate controller. + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0 + + RM_MAX_JERK: + description: + short: Maximum jerk + long: Limit for forwards acc/deceleration change. + type: float + unit: m/s^3 + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0.5 + + RM_MAX_ACCEL: + description: + short: Maximum acceleration + long: Maximum acceleration is used to limit the acceleration of the rover + type: float + unit: m/s^2 + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0.5 + + RM_HEADING_P: + description: + short: Proportional gain for heading controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + + RM_HEADING_I: + description: + short: Integral gain for heading controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0.1 + + RM_SPEED_P: + description: + short: Proportional gain for speed controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + + RM_SPEED_I: + description: + short: Integral gain for ground speed controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0 + + RM_MISS_SPD_DEF: + description: + short: Default rover speed during a mission + type: float + unit: m/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + + RM_TRANS_TRN_DRV: + description: + short: Heading error threshhold to switch from spot turning to driving + type: float + unit: rad + min: 0.001 + max: 180 + increment: 0.01 + decimal: 3 + default: 0.0872665 + + RM_TRANS_DRV_TRN: + description: + short: Heading error threshhold to switch from driving to spot turning + type: float + unit: rad + min: 0.001 + max: 180 + increment: 0.01 + decimal: 3 + default: 0.174533 diff --git a/src/modules/simulation/gz_bridge/module.yaml b/src/modules/simulation/gz_bridge/module.yaml index 35cccb61bcf2..1b38e13d9883 100644 --- a/src/modules/simulation/gz_bridge/module.yaml +++ b/src/modules/simulation/gz_bridge/module.yaml @@ -33,4 +33,4 @@ actuator_output: min: { min: 0, max: 200, default: 0 } max: { min: 0, max: 200, default: 200 } failsafe: { min: 0, max: 200 } - num_channels: 2 + num_channels: 4