diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 34b255e2555f..90b14f812f02 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -84,6 +84,13 @@ if(CONFIG_MODULES_DIFFERENTIAL_DRIVE) ) endif() +if(CONFIG_MODULES_ROVER_ACKERMANN) + px4_add_romfs_files( + rc.rover_ackermann_apps + rc.rover_ackermann_defaults + ) +endif() + if(CONFIG_MODULES_UUV_ATT_CONTROL) px4_add_romfs_files( rc.uuv_apps diff --git a/ROMFS/px4fmu_common/init.d/airframes/50010_ackermann_rover_generic b/ROMFS/px4fmu_common/init.d/airframes/50010_ackermann_rover_generic new file mode 100644 index 000000000000..66b3dc0cabba --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/50010_ackermann_rover_generic @@ -0,0 +1,12 @@ +#!/bin/sh +# +# @name Generic ackermann rover +# +# @type Rover +# @class Rover +# +# @board px4_fmu-v2 exclude +# @board bitcraze_crazyflie exclude +# + +. ${R}etc/init.d/rc.rover_ackermann_defaults diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index 89498206c3e9..f763a4376606 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -149,6 +149,12 @@ if(CONFIG_MODULES_DIFFERENTIAL_DRIVE) ) endif() +if(CONFIG_MODULES_ROVER_ACKERMANN) + px4_add_romfs_files( + 50010_ackermann_rover_generic + ) +endif() + if(CONFIG_MODULES_UUV_ATT_CONTROL) px4_add_romfs_files( # [60000, 61000] (Unmanned) Underwater Robots diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps b/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps new file mode 100644 index 000000000000..0c77ab9aa6d0 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps @@ -0,0 +1,11 @@ +#!/bin/sh +# Standard apps for a ackermann drive rover. + +# Start the attitude and position estimator. +ekf2 start & + +# Start rover ackermann drive controller. +rover_ackermann start + +# Start Land Detector. +land_detector start rover diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_defaults b/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_defaults new file mode 100644 index 000000000000..27cf144a329a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_defaults @@ -0,0 +1,13 @@ +#!/bin/sh +# Ackermann rover parameters. + +set VEHICLE_TYPE rover_ackermann +param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER +param set-default CA_AIRFRAME 5 # Rover (Ackermann) +param set-default CA_R_REV 1 # Motor is assumed to be reversible +param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying +param set-default EKF2_GBIAS_INIT 0.01 +param set-default EKF2_ANGERR_INIT 0.01 +param set-default NAV_ACC_RAD 0.5 # Waypoint acceptance radius +param set-default NAV_RCL_ACT 6 # Disarm on manual control loss +param set-default COM_FAIL_ACT_T 1 # Delay before failsafe after rc loss diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index 42b9c7e876d7..ef455c2ff759 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup @@ -41,6 +41,15 @@ then . ${R}etc/init.d/rc.rover_differential_apps fi +# +# Ackermann Rover setup. +# +if [ $VEHICLE_TYPE = rover_ackermann ] +then + # Start ackermann drive rover apps. + . ${R}etc/init.d/rc.rover_ackermann_apps +fi + # # VTOL setup. # diff --git a/boards/px4/fmu-v5/rover.px4board b/boards/px4/fmu-v5/rover.px4board index 3284f0bb7a1b..a0d173f4918d 100644 --- a/boards/px4/fmu-v5/rover.px4board +++ b/boards/px4/fmu-v5/rover.px4board @@ -19,3 +19,4 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_ACKERMANN=y diff --git a/boards/px4/fmu-v5x/rover.px4board b/boards/px4/fmu-v5x/rover.px4board index b8a1597b30ea..3e2b69dfac1b 100644 --- a/boards/px4/fmu-v5x/rover.px4board +++ b/boards/px4/fmu-v5x/rover.px4board @@ -14,3 +14,5 @@ CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set +CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_ACKERMANN=y diff --git a/boards/px4/fmu-v6c/rover.px4board b/boards/px4/fmu-v6c/rover.px4board index f754c54e36f7..553bc76a52c7 100644 --- a/boards/px4/fmu-v6c/rover.px4board +++ b/boards/px4/fmu-v6c/rover.px4board @@ -14,3 +14,4 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_ACKERMANN=y diff --git a/boards/px4/fmu-v6u/rover.px4board b/boards/px4/fmu-v6u/rover.px4board index f754c54e36f7..553bc76a52c7 100644 --- a/boards/px4/fmu-v6u/rover.px4board +++ b/boards/px4/fmu-v6u/rover.px4board @@ -14,3 +14,4 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_ACKERMANN=y diff --git a/boards/px4/fmu-v6x/rover.px4board b/boards/px4/fmu-v6x/rover.px4board index f754c54e36f7..553bc76a52c7 100644 --- a/boards/px4/fmu-v6x/rover.px4board +++ b/boards/px4/fmu-v6x/rover.px4board @@ -14,3 +14,4 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_ACKERMANN=y diff --git a/boards/px4/fmu-v6xrt/rover.px4board b/boards/px4/fmu-v6xrt/rover.px4board index 2e75c9bdb47b..101555842444 100644 --- a/boards/px4/fmu-v6xrt/rover.px4board +++ b/boards/px4/fmu-v6xrt/rover.px4board @@ -15,3 +15,4 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_ACKERMANN=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 7dd7ce481a3f..cfbaf4ddc998 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -45,6 +45,7 @@ CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y CONFIG_MODULES_PAYLOAD_DELIVERER=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_REPLAY=y +CONFIG_MODULES_ROVER_ACKERMANN=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 c53402bd0f35..53ec65cf9a55 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -178,6 +178,7 @@ set(msg_files RcParameterMap.msg RegisterExtComponentReply.msg RegisterExtComponentRequest.msg + RoverAckermannGuidanceStatus.msg Rpm.msg RtlStatus.msg RtlTimeEstimate.msg diff --git a/msg/RoverAckermannGuidanceStatus.msg b/msg/RoverAckermannGuidanceStatus.msg new file mode 100644 index 000000000000..3c34b63c63f2 --- /dev/null +++ b/msg/RoverAckermannGuidanceStatus.msg @@ -0,0 +1,10 @@ +uint64 timestamp # time since system start (microseconds) + +float32 actual_speed # [m/s] Rover ground speed +float32 desired_speed # [m/s] Rover desired ground speed +float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller +float32 heading_error # [deg] Heading error of the pure pursuit controller +float32 pid_throttle_integral # [-1, 1] Integral of the PID for the normalized throttle to control the rover speed during missions +float32 crosstrack_error # [m] Shortest distance from the vehicle to the path + +# TOPICS rover_ackermann_guidance_status diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 731f087f7934..b1d5b20f93b6 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -102,6 +102,7 @@ void LoggedTopics::add_default_topics() add_topic("position_setpoint_triplet", 200); add_optional_topic("px4io_status"); add_topic("radio_status"); + add_topic("rover_ackermann_guidance_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_ackermann/CMakeLists.txt b/src/modules/rover_ackermann/CMakeLists.txt new file mode 100644 index 000000000000..249a4748768d --- /dev/null +++ b/src/modules/rover_ackermann/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# 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(RoverAckermannGuidance) + +px4_add_module( + MODULE modules__rover_ackermann + MAIN rover_ackermann + SRCS + RoverAckermann.cpp + RoverAckermann.hpp + DEPENDS + RoverAckermannGuidance + px4_work_queue + MODULE_CONFIG + module.yaml +) diff --git a/src/modules/rover_ackermann/Kconfig b/src/modules/rover_ackermann/Kconfig new file mode 100644 index 000000000000..3ba13506598a --- /dev/null +++ b/src/modules/rover_ackermann/Kconfig @@ -0,0 +1,6 @@ +menuconfig MODULES_ROVER_ACKERMANN + bool "rover_ackermann" + default n + depends on MODULES_CONTROL_ALLOCATOR + ---help--- + Enable support for control of ackermann drive rovers diff --git a/src/modules/rover_ackermann/RoverAckermann.cpp b/src/modules/rover_ackermann/RoverAckermann.cpp new file mode 100644 index 000000000000..2c6b8c30d478 --- /dev/null +++ b/src/modules/rover_ackermann/RoverAckermann.cpp @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include "RoverAckermann.hpp" + +using namespace time_literals; +using namespace matrix; + +RoverAckermann::RoverAckermann() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) +{ + updateParams(); +} + +bool RoverAckermann::init() +{ + ScheduleOnInterval(10_ms); // 100 Hz + return true; +} + +void RoverAckermann::updateParams() +{ + ModuleParams::updateParams(); +} + +void RoverAckermann::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + } + + // uORB subscriber updates + if (_parameter_update_sub.updated()) { + updateParams(); + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + _vehicle_status_sub.copy(&vehicle_status); + _nav_state = vehicle_status.nav_state; + } + + // Navigation modes + switch (_nav_state) { + case vehicle_status_s::NAVIGATION_STATE_MANUAL: { + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + _motor_setpoint.steering = manual_control_setpoint.roll; + _motor_setpoint.throttle = manual_control_setpoint.throttle; + } + + } break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + _motor_setpoint = _ackermann_guidance.purePursuit(); + break; + + default: // Unimplemented nav states will stop the rover + _motor_setpoint.steering = 0.f; + _motor_setpoint.throttle = 0.f; + break; + } + + hrt_abstime now = hrt_absolute_time(); + + // Publish to wheel motors + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + actuator_motors.control[0] = _motor_setpoint.throttle; + actuator_motors.timestamp = now; + _actuator_motors_pub.publish(actuator_motors); + + // Publish to servo + actuator_servos_s actuator_servos{}; + actuator_servos.control[0] = _motor_setpoint.steering; + actuator_servos.timestamp = now; + _actuator_servos_pub.publish(actuator_servos); +} + +int RoverAckermann::task_spawn(int argc, char *argv[]) +{ + RoverAckermann *instance = new RoverAckermann(); + + 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 RoverAckermann::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int RoverAckermann::print_usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Rover state machine. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("rover_ackermann", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +extern "C" __EXPORT int rover_ackermann_main(int argc, char *argv[]) +{ + return RoverAckermann::main(argc, argv); +} diff --git a/src/modules/rover_ackermann/RoverAckermann.hpp b/src/modules/rover_ackermann/RoverAckermann.hpp new file mode 100644 index 000000000000..101647dd4e20 --- /dev/null +++ b/src/modules/rover_ackermann/RoverAckermann.hpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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 + +// Standard library includes +#include + +// Local includes +#include "RoverAckermannGuidance/RoverAckermannGuidance.hpp" + +using namespace time_literals; + +class RoverAckermann : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + /** + * @brief Constructor for RoverAckermann + */ + RoverAckermann(); + ~RoverAckermann() 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; + + // 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_status_sub{ORB_ID(vehicle_status)}; + + // uORB publications + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; + + // Instances + RoverAckermannGuidance _ackermann_guidance{this}; + + // Variables + int _nav_state{0}; + RoverAckermannGuidance::motor_setpoint _motor_setpoint; + + // Parameters + DEFINE_PARAMETERS( + (ParamInt) _param_r_rev + ) +}; diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/CMakeLists.txt b/src/modules/rover_ackermann/RoverAckermannGuidance/CMakeLists.txt new file mode 100644 index 000000000000..72928c7e25be --- /dev/null +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/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(RoverAckermannGuidance + RoverAckermannGuidance.cpp +) + +target_link_libraries(RoverAckermannGuidance PUBLIC pid) +target_include_directories(RoverAckermannGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp new file mode 100644 index 000000000000..d718df0ad49e --- /dev/null +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp @@ -0,0 +1,321 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include "RoverAckermannGuidance.hpp" + +#include + +using namespace matrix; +using namespace time_literals; + +RoverAckermannGuidance::RoverAckermannGuidance(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); + pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); +} + +void RoverAckermannGuidance::updateParams() +{ + ModuleParams::updateParams(); + pid_set_parameters(&_pid_throttle, + _param_ra_p_speed.get(), // Proportional gain + _param_ra_i_speed.get(), // Integral gain + 0, // Derivative gain + 1, // Integral limit + 1); // Output limit +} + +RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit() +{ + // Initializations + float desired_speed{0.f}; + float desired_steering{0.f}; + float vehicle_yaw{0.f}; + float actual_speed{0.f}; + + // uORB subscriber updates + 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_local_proj_ref.isInitialized() + || (_global_local_proj_ref.getProjectionReferenceTimestamp() != local_position.ref_timestamp)) { + _global_local_proj_ref.initReference(local_position.ref_lat, local_position.ref_lon, local_position.ref_timestamp); + } + + _curr_pos_local = Vector2f(local_position.x, local_position.y); + const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz}; + actual_speed = rover_velocity.norm(); + } + + if (_position_setpoint_triplet_sub.updated()) { + updateWaypoints(); + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + matrix::Quatf vehicle_attitude_quaternion = Quatf(vehicle_attitude.q); + vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi(); + } + + if (_mission_result_sub.updated()) { + mission_result_s mission_result{}; + _mission_result_sub.copy(&mission_result); + _mission_finished = mission_result.finished; + } + + // Guidance logic + if (!_mission_finished) { + // Calculate desired speed + const float distance_to_prev_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), + _prev_wp(0), + _prev_wp(1)); + + if (distance_to_prev_wp <= _prev_acc_rad) { // Cornering speed + const float cornering_speed = _param_ra_miss_vel_gain.get() / _prev_acc_rad; + desired_speed = math::constrain(cornering_speed, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get()); + + } else { // Default mission speed + desired_speed = _param_ra_miss_vel_def.get(); + } + + // Calculate desired steering + desired_steering = calcDesiredSteering(_curr_wp_local, _prev_wp_local, _curr_pos_local, _param_ra_lookahd_gain.get(), + _param_ra_lookahd_min.get(), _param_ra_lookahd_max.get(), _param_ra_wheel_base.get(), desired_speed, vehicle_yaw); + desired_steering = math::constrain(desired_steering, -_param_ra_max_steer_angle.get(), + _param_ra_max_steer_angle.get()); + } + + // Throttle PID + hrt_abstime now = hrt_absolute_time(); + const float dt = math::min((now - _time_stamp_last), 5000_ms) / 1e6f; + _time_stamp_last = now; + float throttle = 0.f; + + if (desired_speed < FLT_EPSILON) { + pid_reset_integral(&_pid_throttle); + + } else { + throttle = pid_calculate(&_pid_throttle, desired_speed, actual_speed, 0, + dt); + } + + if (_param_ra_max_speed.get() > 0.f) { // Feed-forward term + throttle += math::interpolate(desired_speed, + 0.f, _param_ra_max_speed.get(), + 0.f, 1.f); + } + + // Publish ackermann controller status (logging) + _rover_ackermann_guidance_status.timestamp = now; + _rover_ackermann_guidance_status.actual_speed = actual_speed; + _rover_ackermann_guidance_status.desired_speed = desired_speed; + _rover_ackermann_guidance_status.pid_throttle_integral = _pid_throttle.integral; + _rover_ackermann_guidance_status_pub.publish(_rover_ackermann_guidance_status); + + // Return motor setpoints + motor_setpoint motor_setpoint_temp; + motor_setpoint_temp.steering = math::interpolate(desired_steering, -_param_ra_max_steer_angle.get(), + _param_ra_max_steer_angle.get(), + -1.f, 1.f); + motor_setpoint_temp.throttle = math::constrain(throttle, 0.f, 1.f); + return motor_setpoint_temp; +} + +void RoverAckermannGuidance::updateWaypoints() +{ + position_setpoint_triplet_s position_setpoint_triplet{}; + _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); + + // Global waypoint coordinates + _curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); + + if (position_setpoint_triplet.previous.valid) { + _prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon); + + } else { + _prev_wp = _curr_pos; + } + + if (position_setpoint_triplet.next.valid) { + _next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon); + + } else { + _next_wp = _curr_wp; + } + + // Local waypoint coordinates + _curr_wp_local = _global_local_proj_ref.project(_curr_wp(0), _curr_wp(1)); + _prev_wp_local = _global_local_proj_ref.project(_prev_wp(0), _prev_wp(1)); + _next_wp_local = _global_local_proj_ref.project(_next_wp(0), _next_wp(1)); + + // Update acceptance radius + _prev_acc_rad = _acceptance_radius; + _acceptance_radius = updateAcceptanceRadius(_curr_wp_local, _prev_wp_local, _next_wp_local, _param_ra_acc_rad_def.get(), + _param_ra_acc_rad_gain.get(), _param_ra_acc_rad_max.get(), _param_ra_wheel_base.get(), _param_ra_max_steer_angle.get()); +} + +float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, + const Vector2f &next_wp_local, const float &default_acceptance_radius, const float &acceptance_radius_gain, + const float &acceptance_radius_max, const float &wheel_base, const float &max_steer_angle) +{ + // Setup variables + const Vector2f curr_to_prev_wp_local = prev_wp_local - curr_wp_local; + const Vector2f curr_to_next_wp_local = next_wp_local - curr_wp_local; + float acceptance_radius = default_acceptance_radius; + + // Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment + if (curr_to_next_wp_local.norm() > FLT_EPSILON && curr_to_prev_wp_local.norm() > FLT_EPSILON) { + const float theta = acosf((curr_to_prev_wp_local * curr_to_next_wp_local) / (curr_to_prev_wp_local.norm() * + curr_to_next_wp_local.norm())) / 2.f; + const float min_turning_radius = wheel_base / sinf(max_steer_angle); + const float acceptance_radius_temp = min_turning_radius / tanf(theta); + const float acceptance_radius_temp_scaled = acceptance_radius_gain * + acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects + acceptance_radius = math::constrain(acceptance_radius_temp_scaled, default_acceptance_radius, + acceptance_radius_max); + } + + // Publish updated acceptance radius + position_controller_status_s pos_ctrl_status{}; + pos_ctrl_status.acceptance_radius = acceptance_radius; + pos_ctrl_status.timestamp = hrt_absolute_time(); + _position_controller_status_pub.publish(pos_ctrl_status); + return acceptance_radius; +} + +float RoverAckermannGuidance::calcDesiredSteering(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, + const Vector2f &curr_pos_local, const float &lookahead_gain, const float &lookahead_min, const float &lookahead_max, + const float &wheel_base, const float &desired_speed, const float &vehicle_yaw) +{ + // Calculate crosstrack error + const Vector2f prev_wp_to_curr_wp_local = curr_wp_local - prev_wp_local; + + if (prev_wp_to_curr_wp_local.norm() < FLT_EPSILON) { // Avoid division by 0 (this case should not happen) + return 0.f; + } + + const Vector2f prev_wp_to_curr_pos_local = curr_pos_local - prev_wp_local; + const Vector2f distance_on_line_segment = ((prev_wp_to_curr_pos_local * prev_wp_to_curr_wp_local) / + prev_wp_to_curr_wp_local.norm()) * prev_wp_to_curr_wp_local.normalized(); + const Vector2f crosstrack_error = (prev_wp_local + distance_on_line_segment) - curr_pos_local; + + // Calculate desired heading towards lookahead point + float desired_heading{0.f}; + float lookahead_distance = math::constrain(lookahead_gain * desired_speed, + lookahead_min, lookahead_max); + + if (crosstrack_error.longerThan(lookahead_distance)) { + if (crosstrack_error.norm() < lookahead_max) { + lookahead_distance = crosstrack_error.norm(); // Scale lookahead radius + desired_heading = calcDesiredHeading(curr_wp_local, prev_wp_local, curr_pos_local, lookahead_distance); + + } else { // Excessively large crosstrack error + desired_heading = calcDesiredHeading(curr_wp_local, curr_pos_local, curr_pos_local, lookahead_distance); + } + + } else { // Crosstrack error smaller than lookahead + desired_heading = calcDesiredHeading(curr_wp_local, prev_wp_local, curr_pos_local, lookahead_distance); + } + + // Calculate desired steering to reach lookahead point + const float heading_error = matrix::wrap_pi(desired_heading - vehicle_yaw); + + // For logging + _rover_ackermann_guidance_status.lookahead_distance = lookahead_distance; + _rover_ackermann_guidance_status.heading_error = (heading_error * 180.f) / (M_PI_F); + _rover_ackermann_guidance_status.crosstrack_error = crosstrack_error.norm(); + + // Calculate desired steering + if (math::abs_t(heading_error) <= M_PI_2_F) { + return atanf(2 * wheel_base * sinf(heading_error) / lookahead_distance); + + } else if (heading_error > FLT_EPSILON) { + return atanf(2 * wheel_base * (1.0f + sinf(heading_error - M_PI_2_F)) / + lookahead_distance); + + } else { + return atanf(2 * wheel_base * (-1.0f + sinf(heading_error + M_PI_2_F)) / + lookahead_distance); + } +} + +float RoverAckermannGuidance::calcDesiredHeading(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, + const Vector2f &curr_pos_local, + const float &lookahead_distance) +{ + // Setup variables + const float line_segment_slope = (curr_wp_local(1) - prev_wp_local(1)) / (curr_wp_local(0) - prev_wp_local(0)); + const float line_segment_rover_offset = prev_wp_local(1) - curr_pos_local(1) + line_segment_slope * (curr_pos_local( + 0) - prev_wp_local(0)); + const float a = -line_segment_slope; + const float c = -line_segment_rover_offset; + const float r = lookahead_distance; + const float x0 = -a * c / (a * a + 1.0f); + const float y0 = -c / (a * a + 1.0f); + + // Calculate intersection points + if (c * c > r * r * (a * a + 1.0f) + FLT_EPSILON) { // No intersection points exist + return 0.f; + + } else if (abs(c * c - r * r * (a * a + 1.0f)) < FLT_EPSILON) { // One intersection point exists + return atan2f(y0, x0); + + } else { // Two intersetion points exist + const float d = r * r - c * c / (a * a + 1.0f); + const float mult = sqrt(d / (a * a + 1.0f)); + const float ax = x0 + mult; + const float bx = x0 - mult; + const float ay = y0 - a * mult; + const float by = y0 + a * mult; + const Vector2f point1(ax, ay); + const Vector2f point2(bx, by); + const Vector2f distance1 = (curr_wp_local - curr_pos_local) - point1; + const Vector2f distance2 = (curr_wp_local - curr_pos_local) - point2; + + // Return intersection point closer to current waypoint + if (distance1.norm_squared() < distance2.norm_squared()) { + return atan2f(ay, ax); + + } else { + return atan2f(by, bx); + } + } +} diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp new file mode 100644 index 000000000000..f6446cdacb8a --- /dev/null +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include + +// Standard library includes +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Class for ackermann drive guidance. + */ +class RoverAckermannGuidance : public ModuleParams +{ +public: + /** + * @brief Constructor for RoverAckermannGuidance. + * @param parent The parent ModuleParams object. + */ + RoverAckermannGuidance(ModuleParams *parent); + ~RoverAckermannGuidance() = default; + + struct motor_setpoint { + float steering{0.f}; + float throttle{0.f}; + }; + + /** + * @brief Compute guidance for ackermann rover and return motor_setpoint for throttle and steering. + */ + motor_setpoint purePursuit(); + + /** + * @brief Update global/local waypoint coordinates and acceptance radius + */ + void updateWaypoints(); + + /** + * @brief Returns and publishes the acceptance radius for current waypoint based on the angle between a line segment + * from the previous to the current waypoint/current to the next waypoint and maximum steer angle of + * the vehicle. + * @param curr_wp_local Current waypoint in local frame. + * @param prev_wp_local Previous waypoint in local frame. + * @param next_wp_local Next waypoint in local frame. + * @param default_acceptance_radius Default acceptance radius for waypoints. + * @param acceptance_radius_gain Scaling of the geometric optimal acceptance radius for the rover to cut corners. + * @param acceptance_radius_max Maximum value for the acceptance radius. + * @param wheel_base Rover wheelbase. + * @param max_steer_angle Rover maximum steer angle. + */ + float updateAcceptanceRadius(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, + const Vector2f &next_wp_local, const float &default_acceptance_radius, const float &acceptance_radius_gain, + const float &acceptance_radius_max, const float &wheel_base, const float &max_steer_angle); + + /** + * @brief Calculate and return desired steering input + * @param curr_wp_local Current waypoint in local frame. + * @param prev_wp_local Previous waypoint in local frame. + * @param curr_pos_local Current position of the vehicle in local frame. + * @param lookahead_gain Tuning parameter for the lookahead distance pure pursuit controller. + * @param lookahead_min Minimum lookahead distance. + * @param lookahead_max Maximum lookahead distance. + * @param wheel_base Rover wheelbase. + * @param desired_speed Desired speed for the rover. + * @param vehicle_yaw Current yaw of the rover. + */ + float calcDesiredSteering(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, const Vector2f &curr_pos_local, + const float &lookahead_gain, const float &lookahead_min, const float &lookahead_max, const float &wheel_base, + const float &desired_speed, const float &vehicle_yaw); + + /** + * @brief Return desired heading to the intersection point between a circle with a radius of + * lookahead_distance around the vehicle and a line segment from the previous to the current waypoint. + * @param curr_wp_local Current waypoint in local frame. + * @param prev_wp_local Previous waypoint in local frame. + * @param curr_pos_local Current position of the vehicle in local frame. + * @param lookahead_distance Radius of circle around vehicle. + */ + float calcDesiredHeading(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, const Vector2f &curr_pos_local, + const float &lookahead_distance); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + // 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 _local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; + + // uORB publications + uORB::Publication _rover_ackermann_guidance_status_pub{ORB_ID(rover_ackermann_guidance_status)}; + uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; + rover_ackermann_guidance_status_s _rover_ackermann_guidance_status{}; + + + MapProjection _global_local_proj_ref{}; // Transform global to local coordinates. + + // Rover variables + Vector2d _curr_pos{}; + Vector2f _curr_pos_local{}; + PID_t _pid_throttle; + hrt_abstime _time_stamp_last{0}; + + // Waypoint variables + Vector2d _curr_wp{}; + Vector2d _next_wp{}; + Vector2d _prev_wp{}; + Vector2f _curr_wp_local{}; + Vector2f _prev_wp_local{}; + Vector2f _next_wp_local{}; + float _acceptance_radius{0.5f}; + float _prev_acc_rad{0.f}; + bool _mission_finished{false}; + + // Parameters + DEFINE_PARAMETERS( + (ParamFloat) _param_ra_wheel_base, + (ParamFloat) _param_ra_max_steer_angle, + (ParamFloat) _param_ra_lookahd_gain, + (ParamFloat) _param_ra_lookahd_max, + (ParamFloat) _param_ra_lookahd_min, + (ParamFloat) _param_ra_acc_rad_max, + (ParamFloat) _param_ra_acc_rad_def, + (ParamFloat) _param_ra_acc_rad_gain, + (ParamFloat) _param_ra_miss_vel_def, + (ParamFloat) _param_ra_miss_vel_min, + (ParamFloat) _param_ra_miss_vel_gain, + (ParamFloat) _param_ra_p_speed, + (ParamFloat) _param_ra_i_speed, + (ParamFloat) _param_ra_max_speed, + (ParamFloat) _param_nav_acc_rad + ) +}; diff --git a/src/modules/rover_ackermann/module.yaml b/src/modules/rover_ackermann/module.yaml new file mode 100644 index 000000000000..a3152b9e77be --- /dev/null +++ b/src/modules/rover_ackermann/module.yaml @@ -0,0 +1,177 @@ +module_name: Rover Ackermann + +parameters: + - group: Rover Ackermann + definitions: + RA_WHEEL_BASE: + description: + short: Wheel base + long: Distance from the front to the rear axle + type: float + unit: m + min: 0.001 + max: 100 + increment: 0.001 + decimal: 3 + default: 0.5 + + RA_MAX_STR_ANG: + description: + short: Maximum steering angle + long: The maximum angle that the rover can steer + type: float + unit: rad + min: 0.1 + max: 1.5708 + increment: 0.01 + decimal: 2 + default: 0.5236 + + RA_LOOKAHD_GAIN: + description: + short: Tuning parameter for the pure pursuit controller + long: Lower value -> More aggressive controller (beware overshoot/oscillations) + type: float + min: 0.1 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + + RA_LOOKAHD_MAX: + description: + short: Maximum lookahead distance for the pure pursuit controller + long: | + This is the maximum crosstrack error before the controller starts + targeting the current waypoint rather then the path between the previous + and next waypoint. + type: float + unit: m + min: 0.1 + max: 100 + increment: 0.01 + decimal: 2 + default: 10 + + RA_LOOKAHD_MIN: + description: + short: Minimum lookahead distance for the pure pursuit controller + type: float + unit: m + min: 0.1 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + + RA_ACC_RAD_MAX: + description: + short: Maximum acceptance radius + long: | + The controller scales the acceptance radius based on the angle between + the previous, current and next waypoint. Used as tuning parameter. + Higher value -> smoother trajectory at the cost of how close the rover gets + to the waypoint (Set equal to RA_ACC_RAD_DEF to disable corner cutting). + type: float + unit: m + min: 0.1 + max: 100 + increment: 0.01 + decimal: 2 + default: 3 + + RA_ACC_RAD_DEF: + description: + short: Default acceptance radius + type: float + unit: m + min: 0.1 + max: 100 + increment: 0.01 + decimal: 2 + default: 0.5 + + RA_ACC_RAD_GAIN: + description: + short: Tuning parameter for corner cutting + long: | + The geometric ideal acceptance radius is multiplied by this factor + to account for kinematic and dynamic effects. + Higher value -> The rover starts to cut the corner earlier. + type: float + min: 1 + max: 100 + increment: 0.01 + decimal: 2 + default: 2 + + RA_MISS_VEL_DEF: + description: + short: Default rover velocity during a mission + type: float + unit: m/s + min: 0.1 + max: 100 + increment: 0.01 + decimal: 2 + default: 3 + + RA_MISS_VEL_MIN: + description: + short: Minimum rover velocity during a mission + long: | + The velocity off the rover is reduced based on the corner it has to take + to smooth the trajectory (To disable this feature set it equal to RA_MISSION_VEL_DEF) + type: float + unit: m/s + min: 0.1 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + + RA_MISS_VEL_GAIN: + description: + short: Tuning parameter for the velocity reduction during cornering + long: Lower value -> More velocity reduction during cornering + type: float + min: 0.1 + max: 100 + increment: 0.01 + decimal: 2 + default: 5 + + RA_SPEED_P: + description: + short: Proportional gain for ground speed controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + + RA_SPEED_I: + description: + short: Integral gain for ground speed controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + + RA_MAX_SPEED: + description: + short: Speed the rover drives at maximum throttle + long: | + This is used for the feed-forward term of the speed controller. + A value of -1 disables the feed-forward term in which case the + Integrator (RA_SPEED_I) becomes necessary to track speed setpoints. + type: float + unit: m/s + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: -1