From a200a446c720a25d8dbe689905eef3056eaa7d35 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Mon, 25 Mar 2024 10:22:23 +0100 Subject: [PATCH] folder restructure --- .../ackermann_drive/AckermannDrive.cpp | 40 ++++++++++- .../ackermann_drive/AckermannDrive.hpp | 26 ++++++- .../AckermannDriveControl.cpp | 57 ++------------- .../AckermannDriveControl.hpp | 23 +----- .../AckermannDriveKinematics.cpp | 67 +++++++++++++++++ .../AckermannDriveKinematics.hpp | 72 +++++++++++++++++++ .../AckermannDriveKinematics/CMakeLists.txt | 38 ++++++++++ src/modules/ackermann_drive/CMakeLists.txt | 3 +- 8 files changed, 249 insertions(+), 77 deletions(-) create mode 100644 src/modules/ackermann_drive/AckermannDriveKinematics/AckermannDriveKinematics.cpp create mode 100644 src/modules/ackermann_drive/AckermannDriveKinematics/AckermannDriveKinematics.hpp create mode 100644 src/modules/ackermann_drive/AckermannDriveKinematics/CMakeLists.txt diff --git a/src/modules/ackermann_drive/AckermannDrive.cpp b/src/modules/ackermann_drive/AckermannDrive.cpp index 753ffb0348..284484bae2 100644 --- a/src/modules/ackermann_drive/AckermannDrive.cpp +++ b/src/modules/ackermann_drive/AckermannDrive.cpp @@ -52,6 +52,8 @@ bool AckermannDrive::init() void AckermannDrive::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); } void AckermannDrive::Run() @@ -61,7 +63,43 @@ void AckermannDrive::Run() exit_and_cleanup(); } - _ackermann_drive_control.Update(); + hrt_abstime now = hrt_absolute_time(); + + if (_parameter_update_sub.updated()) { + parameter_update_s pupdate; + if(_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 + } + } + + if (_manual_driving) { + // Manual mode + // directly produce setpoints from the manual control setpoint (joystick) + if (_manual_control_setpoint_sub.updated()) { + differential_drive_setpoint_s _differential_drive_setpoint{}; + 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); + } + } + } + + _ackermann_drive_control.control(); + _ackermann_drive_kinematics.allocate(); } int AckermannDrive::task_spawn(int argc, char *argv[]) diff --git a/src/modules/ackermann_drive/AckermannDrive.hpp b/src/modules/ackermann_drive/AckermannDrive.hpp index 64c9cd915f..b0759510a8 100644 --- a/src/modules/ackermann_drive/AckermannDrive.hpp +++ b/src/modules/ackermann_drive/AckermannDrive.hpp @@ -58,6 +58,8 @@ // Local includes #include "AckermannDriveControl/AckermannDriveControl.hpp" +//#include "AckermannDriveGuidance/AckermannDriveGuidance.hpp" +#include "AckermannDriveKinematics/AckermannDriveKinematics.hpp" using namespace time_literals; @@ -85,9 +87,29 @@ protected: private: void Run() override; - differential_drive_control::AckermannDriveControl _ackermann_drive_control{this}; //TO BE REMOVED + 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::Publication _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)}; + + AckermannDriveControl _ackermann_drive_control{this}; + //AckermannDriveGuidance _ackermann_drive_guidance{this}; + AckermannDriveKinematics _ackermann_drive_kinematics{this}; + + bool _armed = false; + bool _manual_driving = false; + float _max_speed{0.f}; + float _max_angular_velocity{0.f}; DEFINE_PARAMETERS( - (ParamInt) _param_ca_airframe //TO BE REMOVED + (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 ) }; diff --git a/src/modules/ackermann_drive/AckermannDriveControl/AckermannDriveControl.cpp b/src/modules/ackermann_drive/AckermannDriveControl/AckermannDriveControl.cpp index 42939b2a3b..82d5296c7c 100644 --- a/src/modules/ackermann_drive/AckermannDriveControl/AckermannDriveControl.cpp +++ b/src/modules/ackermann_drive/AckermannDriveControl/AckermannDriveControl.cpp @@ -44,62 +44,13 @@ AckermannDriveControl::AckermannDriveControl(ModuleParams *parent) : ModuleParam 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); } -void AckermannDriveControl::Update() +void AckermannDriveControl::control() { - 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 - } - } - - 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); - double steering = _differential_drive_setpoint.yaw_rate; - double speed = _differential_drive_setpoint.speed; - - actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults - 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); + differential_drive_setpoint_s differential_drive_control_output = _differential_drive_setpoint; + differential_drive_control_output.timestamp = hrt_absolute_time(); + _differential_drive_control_output_pub.publish(differential_drive_control_output); } diff --git a/src/modules/ackermann_drive/AckermannDriveControl/AckermannDriveControl.hpp b/src/modules/ackermann_drive/AckermannDriveControl/AckermannDriveControl.hpp index df87c5ac6b..317f01cdf7 100644 --- a/src/modules/ackermann_drive/AckermannDriveControl/AckermannDriveControl.hpp +++ b/src/modules/ackermann_drive/AckermannDriveControl/AckermannDriveControl.hpp @@ -59,17 +59,13 @@ // Local includes // #include - -namespace differential_drive_control -{ - class AckermannDriveControl : public ModuleParams { public: AckermannDriveControl(ModuleParams *parent); - ~AckermannDriveControl() override = default; + ~AckermannDriveControl() = default; - void Update(); + void control(); protected: void updateParams() override; @@ -77,22 +73,11 @@ protected: 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_control_output_pub{ORB_ID(differential_drive_control_output)}; 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, @@ -103,5 +88,3 @@ private: (ParamInt) _param_r_rev ) }; - -} // namespace differential_drive_control diff --git a/src/modules/ackermann_drive/AckermannDriveKinematics/AckermannDriveKinematics.cpp b/src/modules/ackermann_drive/AckermannDriveKinematics/AckermannDriveKinematics.cpp new file mode 100644 index 0000000000..055257a4f6 --- /dev/null +++ b/src/modules/ackermann_drive/AckermannDriveKinematics/AckermannDriveKinematics.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 "AckermannDriveKinematics.hpp" + +using namespace time_literals; +using namespace matrix; + +AckermannDriveKinematics::AckermannDriveKinematics(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); +} + +void AckermannDriveKinematics::allocate() +{ + if (_differential_drive_control_output_sub.updated()) { + _differential_drive_control_output_sub.copy(&_differential_drive_control_output); + } + + double steering = _differential_drive_control_output.yaw_rate; + double speed = _differential_drive_control_output.speed; + hrt_abstime now = hrt_absolute_time(); + + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults + 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); +} diff --git a/src/modules/ackermann_drive/AckermannDriveKinematics/AckermannDriveKinematics.hpp b/src/modules/ackermann_drive/AckermannDriveKinematics/AckermannDriveKinematics.hpp new file mode 100644 index 0000000000..4570b4f82d --- /dev/null +++ b/src/modules/ackermann_drive/AckermannDriveKinematics/AckermannDriveKinematics.hpp @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#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 AckermannDriveKinematics : public ModuleParams +{ +public: + AckermannDriveKinematics(ModuleParams *parent); + ~AckermannDriveKinematics() = default; + + void allocate(); + +private: + uORB::Subscription _differential_drive_control_output_sub{ORB_ID(differential_drive_control_output)}; + + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; + + differential_drive_setpoint_s _differential_drive_control_output{}; + + float _max_speed{0.f}; + float _max_angular_velocity{0.f}; + + DEFINE_PARAMETERS( + (ParamInt) _param_r_rev + ) +}; diff --git a/src/modules/ackermann_drive/AckermannDriveKinematics/CMakeLists.txt b/src/modules/ackermann_drive/AckermannDriveKinematics/CMakeLists.txt new file mode 100644 index 0000000000..bcbdbccc1d --- /dev/null +++ b/src/modules/ackermann_drive/AckermannDriveKinematics/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(AckermannDriveKinematics + AckermannDriveKinematics.cpp +) + +target_include_directories(AckermannDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/ackermann_drive/CMakeLists.txt b/src/modules/ackermann_drive/CMakeLists.txt index 032e17b35e..64e5621e0e 100644 --- a/src/modules/ackermann_drive/CMakeLists.txt +++ b/src/modules/ackermann_drive/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ -# add_subdirectory(DifferentialDriveKinematics) +add_subdirectory(AckermannDriveKinematics) add_subdirectory(AckermannDriveControl) @@ -43,6 +43,7 @@ px4_add_module( AckermannDrive.hpp DEPENDS AckermannDriveControl + AckermannDriveKinematics px4_work_queue # MODULE_CONFIG # module.yaml