diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover new file mode 100644 index 0000000000..97a9b27cb9 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover @@ -0,0 +1,44 @@ +#!/bin/sh +# @name Rover Gazebo +# @type Rover +# @class Rover + +. ${R}etc/init.d/rc.rover_ackermann_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=rover} + +param set-default SIM_GZ_EN 1 # Gazebo bridge + +# 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 1 + +# Actuator mapping +param set-default SIM_GZ_WH_FUNC1 101 # right wheel +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 102 # left wheel +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_FUNC2 103 # left wheel +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_FUNC2 104 # left wheel +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_SV_FUNC1 201 +param set-default SIM_GZ_SV_FUNC2 202 + +# param set-default SIM_GZ_WH_REV 1 # reverse right wheel diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 9277ad3039..cdbcb41612 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -83,6 +83,7 @@ px4_add_romfs_files( 4009_gz_r1_rover 4010_gz_x500_mono_cam 4011_gz_lawnmower + 4012_gz_rover 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 34b255e255..6cefa75ebd 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_ACKERMANN_DRIVE) + 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/rc.rover_ackermann_apps b/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps new file mode 100644 index 0000000000..e1d25e4212 --- /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. +ackermann_drive 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 0000000000..61cf354991 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_defaults @@ -0,0 +1,12 @@ + +#!/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 3 # Right and left motors reversible + +param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index 1b7ddee5a5..7958d2fd44 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/sitl/default.px4board b/boards/px4/sitl/default.px4board index 7dd7ce481a..33f990d541 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -13,6 +13,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ACKERMANN_DRIVE=y CONFIG_MODULES_EKF2=y CONFIG_EKF2_VERBOSE_STATUS=y CONFIG_EKF2_AUX_GLOBAL_POSITION=y diff --git a/src/modules/ackermann_drive/AckermannDrive.cpp b/src/modules/ackermann_drive/AckermannDrive.cpp new file mode 100644 index 0000000000..c0b9097f58 --- /dev/null +++ b/src/modules/ackermann_drive/AckermannDrive.cpp @@ -0,0 +1,127 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 "AckermannDrive.hpp" + +using namespace time_literals; +using namespace matrix; +// namespace AckermannDrive +// { + +AckermannDrive::AckermannDrive() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) + // _differential_drive_control(this) +{ + updateParams(); +} + +bool AckermannDrive::init() +{ + ScheduleOnInterval(10_ms); // 100 Hz + return true; +} + +void AckermannDrive::updateParams() +{ + ModuleParams::updateParams(); +} + +void AckermannDrive::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + } + + if (_param_ca_airframe.get() == DIFFERENTIAL_DRIVE) { + // _differential_drive_control.Update(); + return; + + } else if (_param_ca_airframe.get() == ACKERMANN_DRIVE) { + _ackermann_drive_control.Update(); + } +} + +int AckermannDrive::task_spawn(int argc, char *argv[]) +{ + AckermannDrive *instance = new AckermannDrive(); + + 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 AckermannDrive::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int AckermannDrive::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("ackermann_drive", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +extern "C" __EXPORT int ackermann_drive_main(int argc, char *argv[]) +{ + return AckermannDrive::main(argc, argv); +} + +// } // namespace rover_control diff --git a/src/modules/ackermann_drive/AckermannDrive.hpp b/src/modules/ackermann_drive/AckermannDrive.hpp new file mode 100644 index 0000000000..0bb17eefd4 --- /dev/null +++ b/src/modules/ackermann_drive/AckermannDrive.hpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 +#include +#include + +// Standard library includes +#include + +// Local includes +// #include +// #include "DifferentialDriveControl.hpp" +#include "AckermannDriveControl.hpp" + +#define DIFFERENTIAL_DRIVE 6 +#define ACKERMANN_DRIVE 5 + +// namespace rover_control +// { + +class AckermannDrive : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + AckermannDrive(); + ~AckermannDrive() 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; + + // differential_drive_control::DifferentialDriveControl _differential_drive_control{this}; + differential_drive_control::AckermannDriveControl _ackermann_drive_control{this}; + + + DEFINE_PARAMETERS( + (ParamInt) _param_ca_airframe + ) +}; + +// } // namespace rover_control diff --git a/src/modules/ackermann_drive/AckermannDriveControl.cpp b/src/modules/ackermann_drive/AckermannDriveControl.cpp new file mode 100644 index 0000000000..61062081d6 --- /dev/null +++ b/src/modules/ackermann_drive/AckermannDriveControl.cpp @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 "AckermannDriveControl.hpp" + +using namespace time_literals; +using namespace matrix; +namespace differential_drive_control +{ + +AckermannDriveControl::AckermannDriveControl(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); +} + +void AckermannDriveControl::updateParams() +{ + ModuleParams::updateParams(); + _max_speed = _param_rdd_max_wheel_speed.get() * _param_rdd_wheel_radius.get(); + _max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f); + + // _differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get()); + // _differential_drive_kinematics.setMaxSpeed(_max_speed); + // _differential_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity); +} + +void AckermannDriveControl::Update() +{ + hrt_abstime now = hrt_absolute_time(); + + if (_parameter_update_sub.updated()) { + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + updateParams(); + } + + if (_vehicle_control_mode_sub.updated()) { + vehicle_control_mode_s vehicle_control_mode; + + if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) { + _armed = vehicle_control_mode.flag_armed; + _manual_driving = vehicle_control_mode.flag_control_manual_enabled; // change this when more modes are supported + } + } + + // printf("AckermannDriveControl::Update()\n"); + + if (_manual_driving) { + // Manual mode + // directly produce setpoints from the manual control setpoint (joystick) + if (_manual_control_setpoint_sub.updated()) { + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) { + _differential_drive_setpoint.timestamp = now; + _differential_drive_setpoint.speed = manual_control_setpoint.throttle * _param_rdd_speed_scale.get() * _max_speed; + _differential_drive_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() * + _max_angular_velocity; + _differential_drive_setpoint_pub.publish(_differential_drive_setpoint); + } + } + } + + _differential_drive_setpoint_sub.update(&_differential_drive_setpoint); + + // publish data to actuator_motors (output module) + // get the wheel speeds from the inverse kinematics class (DifferentialDriveKinematics) + // Vector2f wheel_speeds = _differential_drive_kinematics.computeInverseKinematics( + // _differential_drive_setpoint.speed, + // _differential_drive_setpoint.yaw_rate); + + double steering = _differential_drive_setpoint.yaw_rate; + double speed = _differential_drive_setpoint.speed; + + // Check if max_angular_wheel_speed is zero + // const bool setpoint_timeout = (_differential_drive_setpoint.timestamp + 100_ms) < now; + // const bool valid_max_speed = _param_rdd_speed_scale.get() > FLT_EPSILON; + + // if (!_armed || setpoint_timeout || !valid_max_speed) { + // wheel_speeds = {}; // stop + // } + + // wheel_speeds = matrix::constrain(wheel_speeds, -1.f, 1.f); + + actuator_motors_s actuator_motors{}; + // actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults + // wheel_speeds.copyTo(actuator_motors.control); + actuator_motors.control[0] = speed; + actuator_motors.control[1] = speed; + actuator_motors.timestamp = now; + _actuator_motors_pub.publish(actuator_motors); + + // publish data to actuator_servos (output module) + actuator_servos_s actuator_servos{}; + actuator_servos.control[0] = -steering; + actuator_servos.control[1] = -steering; + actuator_servos.timestamp = now; + _actuator_servos_pub.publish(actuator_servos); +} +} // namespace differential_drive_control diff --git a/src/modules/ackermann_drive/AckermannDriveControl.hpp b/src/modules/ackermann_drive/AckermannDriveControl.hpp new file mode 100644 index 0000000000..df87c5ac6b --- /dev/null +++ b/src/modules/ackermann_drive/AckermannDriveControl.hpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 +#include +#include +#include + +// Standard library includespr +#include + +// Local includes +// #include + +namespace differential_drive_control +{ + +class AckermannDriveControl : public ModuleParams +{ +public: + AckermannDriveControl(ModuleParams *parent); + ~AckermannDriveControl() override = default; + + void Update(); + +protected: + void updateParams() override; + +private: + + uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; + uORB::Publication _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)}; + + differential_drive_setpoint_s _differential_drive_setpoint{}; + // DifferentialDriveKinematics _differential_drive_kinematics{}; + + bool _armed = false; + bool _manual_driving = false; + float _max_speed{0.f}; + float _max_angular_velocity{0.f}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_rdd_speed_scale, + (ParamFloat) _param_rdd_ang_velocity_scale, + (ParamFloat) _param_rdd_max_wheel_speed, + (ParamFloat) _param_rdd_wheel_base, + (ParamFloat) _param_rdd_wheel_radius, + (ParamInt) _param_r_rev + ) +}; + +} // namespace differential_drive_control diff --git a/src/modules/ackermann_drive/CMakeLists.txt b/src/modules/ackermann_drive/CMakeLists.txt new file mode 100644 index 0000000000..a44114b4c0 --- /dev/null +++ b/src/modules/ackermann_drive/CMakeLists.txt @@ -0,0 +1,49 @@ +############################################################################ +# +# Copyright (c) 2023 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(DifferentialDriveKinematics) + +px4_add_module( + MODULE modules__ackermann_drive + MAIN ackermann_drive + SRCS + AckermannDriveControl.cpp + AckermannDriveControl.hpp + AckermannDrive.cpp + AckermannDrive.hpp + DEPENDS + # DifferentialDriveKinematics + px4_work_queue + # MODULE_CONFIG + # module.yaml +) diff --git a/src/modules/ackermann_drive/DifferentialDriveKinematics/CMakeLists.txt b/src/modules/ackermann_drive/DifferentialDriveKinematics/CMakeLists.txt new file mode 100644 index 0000000000..c352d7c597 --- /dev/null +++ b/src/modules/ackermann_drive/DifferentialDriveKinematics/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2023 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(DifferentialDriveKinematics + DifferentialDriveKinematics.cpp +) + +target_include_directories(DifferentialDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_unit_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics) diff --git a/src/modules/ackermann_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp b/src/modules/ackermann_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp new file mode 100644 index 0000000000..6c28a0b38f --- /dev/null +++ b/src/modules/ackermann_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 "DifferentialDriveKinematics.hpp" + +#include + +using namespace matrix; + +matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate) +{ + if (_max_speed < FLT_EPSILON) { + return Vector2f(); + } + + linear_velocity_x = math::constrain(linear_velocity_x, -_max_speed, _max_speed); + yaw_rate = math::constrain(yaw_rate, -_max_angular_velocity, _max_angular_velocity); + + const float rotational_velocity = (_wheel_base / 2.f) * yaw_rate; + float combined_velocity = fabsf(linear_velocity_x) + fabsf(rotational_velocity); + + // Compute an initial gain + float gain = 1.0f; + + if (combined_velocity > _max_speed) { + float excess_velocity = fabsf(combined_velocity - _max_speed); + float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity; + gain = adjusted_linear_velocity / fabsf(linear_velocity_x); + } + + // Apply the gain + linear_velocity_x *= gain; + + // Calculate the left and right wheel speeds + return Vector2f(linear_velocity_x - rotational_velocity, + linear_velocity_x + rotational_velocity) / _max_speed; +} diff --git a/src/modules/ackermann_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp b/src/modules/ackermann_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp new file mode 100644 index 0000000000..193a7d05fb --- /dev/null +++ b/src/modules/ackermann_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 + +#include + +/** + * @brief Differential Drive Kinematics class for computing the kinematics of a differential drive robot. + * + * This class provides functions to set the wheel base and radius, and to compute the inverse kinematics + * given linear velocity and yaw rate. + */ +class DifferentialDriveKinematics +{ +public: + DifferentialDriveKinematics() = default; + ~DifferentialDriveKinematics() = default; + + /** + * @brief Sets the wheel base of the robot. + * + * @param wheel_base The distance between the centers of the wheels. + */ + void setWheelBase(const float wheel_base) { _wheel_base = wheel_base; }; + + /** + * @brief Sets the maximum speed of the robot. + * + * @param max_speed The maximum speed of the robot. + */ + void setMaxSpeed(const float max_speed) { _max_speed = max_speed; }; + + /** + * @brief Sets the maximum angular speed of the robot. + * + * @param max_angular_speed The maximum angular speed of the robot. + */ + void setMaxAngularVelocity(const float max_angular_velocity) { _max_angular_velocity = max_angular_velocity; }; + + /** + * @brief Computes the inverse kinematics for differential drive. + * + * @param linear_velocity_x Linear velocity along the x-axis. + * @param yaw_rate Yaw rate of the robot. + * @return matrix::Vector2f Motor velocities for the right and left motors. + */ + matrix::Vector2f computeInverseKinematics(float linear_velocity_x, float yaw_rate); + +private: + float _wheel_base{0.f}; + float _max_speed{0.f}; + float _max_angular_velocity{0.f}; +}; diff --git a/src/modules/ackermann_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp b/src/modules/ackermann_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp new file mode 100644 index 0000000000..6eee43c41e --- /dev/null +++ b/src/modules/ackermann_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp @@ -0,0 +1,174 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 +#include "DifferentialDriveKinematics.hpp" +#include + +using namespace matrix; + +TEST(DifferentialDriveKinematicsTest, AllZeroInputCase) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(1.f); + kinematics.setMaxSpeed(10.f); + kinematics.setMaxAngularVelocity(10.f); + + // Test with zero linear velocity and zero yaw rate (stationary vehicle) + EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 0.f), Vector2f()); +} + + +TEST(DifferentialDriveKinematicsTest, InvalidParameterCase) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(0.f); + kinematics.setMaxSpeed(10.f); + kinematics.setMaxAngularVelocity(10.f); + + // Test with invalid parameters (zero wheel base and wheel radius) + EXPECT_EQ(kinematics.computeInverseKinematics(0.f, .1f), Vector2f()); +} + + +TEST(DifferentialDriveKinematicsTest, UnitCase) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(1.f); + kinematics.setMaxSpeed(10.f); + kinematics.setMaxAngularVelocity(10.f); + + // Test with unit values for linear velocity and yaw rate + EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0.05f, 0.15f)); +} + + +TEST(DifferentialDriveKinematicsTest, UnitSaturationCase) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(1.f); + kinematics.setMaxSpeed(1.f); + kinematics.setMaxAngularVelocity(1.f); + + // Test with unit values for linear velocity and yaw rate, but with max speed that requires saturation + EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0, 1)); +} + + +TEST(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(1.f); + kinematics.setMaxSpeed(1.f); + kinematics.setMaxAngularVelocity(1.f); + + // Negative linear velocity for backward motion and positive yaw rate for turning right + EXPECT_EQ(kinematics.computeInverseKinematics(-1.f, 1.f), Vector2f(-1, 0)); +} + +TEST(DifferentialDriveKinematicsTest, RandomCase) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(2.f); + kinematics.setMaxSpeed(1.f); + kinematics.setMaxAngularVelocity(1.f); + + // Negative linear velocity for backward motion and positive yaw rate for turning right + EXPECT_EQ(kinematics.computeInverseKinematics(0.5f, 0.7f), Vector2f(-0.4f, 1.0f)); +} + +TEST(DifferentialDriveKinematicsTest, RotateInPlaceCase) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(1.f); + kinematics.setMaxSpeed(1.f); + kinematics.setMaxAngularVelocity(1.f); + + // Test rotating in place (zero linear velocity, non-zero yaw rate) + EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 1.f), Vector2f(-0.5f, 0.5f)); +} + +TEST(DifferentialDriveKinematicsTest, StraightMovementCase) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(1.f); + kinematics.setMaxSpeed(1.f); + kinematics.setMaxAngularVelocity(1.f); + + // Test moving straight (non-zero linear velocity, zero yaw rate) + EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 0.f), Vector2f(1.f, 1.f)); +} + +TEST(DifferentialDriveKinematicsTest, MinInputValuesCase) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(FLT_MIN); + kinematics.setMaxSpeed(FLT_MIN); + kinematics.setMaxAngularVelocity(FLT_MIN); + + // Test with minimum possible input values + EXPECT_EQ(kinematics.computeInverseKinematics(FLT_MIN, FLT_MIN), Vector2f(0.f, 0.f)); +} + +TEST(DifferentialDriveKinematicsTest, MaxSpeedLimitCase) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(1.f); + kinematics.setMaxSpeed(1.f); + kinematics.setMaxAngularVelocity(1.f); + + // Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed + EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 10.f), Vector2f(0.f, 1.f)); +} + +TEST(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(1.f); + kinematics.setMaxSpeed(1.f); + kinematics.setMaxAngularVelocity(1.f); + + // Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed + EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 0.f), Vector2f(1.f, 1.f)); +} + +TEST(DifferentialDriveKinematicsTest, MaxAngularCase) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(2.f); + kinematics.setMaxSpeed(1.f); + kinematics.setMaxAngularVelocity(1.f); + + // Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed + EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 10.f), Vector2f(-1.f, 1.f)); +} diff --git a/src/modules/ackermann_drive/Kconfig b/src/modules/ackermann_drive/Kconfig new file mode 100644 index 0000000000..a6f125401a --- /dev/null +++ b/src/modules/ackermann_drive/Kconfig @@ -0,0 +1,6 @@ +menuconfig MODULES_ACKERMANN_DRIVE + bool "ackermann_drive" + default n + depends on MODULES_CONTROL_ALLOCATOR + ---help--- + Enable support for control of differential drive rovers diff --git a/src/modules/ackermann_drive/module.yaml b/src/modules/ackermann_drive/module.yaml new file mode 100644 index 0000000000..77bd61dda4 --- /dev/null +++ b/src/modules/ackermann_drive/module.yaml @@ -0,0 +1,55 @@ +module_name: Differential Drive Control + +parameters: + - group: Rover Differential Drive + definitions: + RDD_WHEEL_BASE: + description: + short: Wheel base + 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 + RDD_WHEEL_RADIUS: + description: + short: Wheel radius + long: Size of the wheel, half the diameter of the wheel + type: float + unit: m + min: 0.001 + max: 100 + increment: 0.001 + decimal: 3 + default: 0.1 + RDD_SPEED_SCALE: + description: + short: Manual speed scale + type: float + min: 0 + max: 1.0 + increment: 0.01 + decimal: 2 + default: 1.0 + RDD_ANG_SCALE: + description: + short: Manual angular velocity scale + type: float + min: 0 + max: 1.0 + increment: 0.01 + decimal: 2 + default: 1.0 + RDD_WHL_SPEED: + description: + short: Maximum wheel speed + type: float + unit: rad/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 10 diff --git a/src/modules/simulation/gz_bridge/module.yaml b/src/modules/simulation/gz_bridge/module.yaml index 35cccb61bc..1b38e13d98 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