From 1f332d561e8aaf60841f5321dfb95081d3c973c5 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Fri, 22 Mar 2024 14:14:52 +0100 Subject: [PATCH] folder restructure --- .../ackermann_drive/AckermannDrive.hpp | 2 +- .../AckermannDriveControl.cpp | 0 .../AckermannDriveControl.hpp | 0 .../CMakeLists.txt | 10 +- src/modules/ackermann_drive/CMakeLists.txt | 6 +- .../DifferentialDriveKinematics.cpp | 67 ------- .../DifferentialDriveKinematics.hpp | 84 --------- .../DifferentialDriveKinematicsTest.cpp | 174 ------------------ 8 files changed, 8 insertions(+), 335 deletions(-) rename src/modules/ackermann_drive/{ => AckermannDriveControl}/AckermannDriveControl.cpp (100%) rename src/modules/ackermann_drive/{ => AckermannDriveControl}/AckermannDriveControl.hpp (100%) rename src/modules/ackermann_drive/{DifferentialDriveKinematics => AckermannDriveControl}/CMakeLists.txt (83%) delete mode 100644 src/modules/ackermann_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp delete mode 100644 src/modules/ackermann_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp delete mode 100644 src/modules/ackermann_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp diff --git a/src/modules/ackermann_drive/AckermannDrive.hpp b/src/modules/ackermann_drive/AckermannDrive.hpp index 0bb17eefd4..0d807f021f 100644 --- a/src/modules/ackermann_drive/AckermannDrive.hpp +++ b/src/modules/ackermann_drive/AckermannDrive.hpp @@ -59,7 +59,7 @@ // Local includes // #include // #include "DifferentialDriveControl.hpp" -#include "AckermannDriveControl.hpp" +#include "AckermannDriveControl/AckermannDriveControl.hpp" #define DIFFERENTIAL_DRIVE 6 #define ACKERMANN_DRIVE 5 diff --git a/src/modules/ackermann_drive/AckermannDriveControl.cpp b/src/modules/ackermann_drive/AckermannDriveControl/AckermannDriveControl.cpp similarity index 100% rename from src/modules/ackermann_drive/AckermannDriveControl.cpp rename to src/modules/ackermann_drive/AckermannDriveControl/AckermannDriveControl.cpp diff --git a/src/modules/ackermann_drive/AckermannDriveControl.hpp b/src/modules/ackermann_drive/AckermannDriveControl/AckermannDriveControl.hpp similarity index 100% rename from src/modules/ackermann_drive/AckermannDriveControl.hpp rename to src/modules/ackermann_drive/AckermannDriveControl/AckermannDriveControl.hpp diff --git a/src/modules/ackermann_drive/DifferentialDriveKinematics/CMakeLists.txt b/src/modules/ackermann_drive/AckermannDriveControl/CMakeLists.txt similarity index 83% rename from src/modules/ackermann_drive/DifferentialDriveKinematics/CMakeLists.txt rename to src/modules/ackermann_drive/AckermannDriveControl/CMakeLists.txt index c352d7c597..869891f30e 100644 --- a/src/modules/ackermann_drive/DifferentialDriveKinematics/CMakeLists.txt +++ b/src/modules/ackermann_drive/AckermannDriveControl/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2023 PX4 Development Team. All rights reserved. +# 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 @@ -31,10 +31,8 @@ # ############################################################################ -px4_add_library(DifferentialDriveKinematics - DifferentialDriveKinematics.cpp +px4_add_library(AckermannDriveControl + AckermannDriveControl.cpp ) -target_include_directories(DifferentialDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) - -px4_add_unit_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics) +target_include_directories(AckermannDriveControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/ackermann_drive/CMakeLists.txt b/src/modules/ackermann_drive/CMakeLists.txt index a44114b4c0..032e17b35e 100644 --- a/src/modules/ackermann_drive/CMakeLists.txt +++ b/src/modules/ackermann_drive/CMakeLists.txt @@ -33,16 +33,16 @@ # add_subdirectory(DifferentialDriveKinematics) +add_subdirectory(AckermannDriveControl) + px4_add_module( MODULE modules__ackermann_drive MAIN ackermann_drive SRCS - AckermannDriveControl.cpp - AckermannDriveControl.hpp AckermannDrive.cpp AckermannDrive.hpp DEPENDS - # DifferentialDriveKinematics + AckermannDriveControl px4_work_queue # MODULE_CONFIG # module.yaml diff --git a/src/modules/ackermann_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp b/src/modules/ackermann_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp deleted file mode 100644 index 6c28a0b38f..0000000000 --- a/src/modules/ackermann_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp +++ /dev/null @@ -1,67 +0,0 @@ -/**************************************************************************** - * - * 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 deleted file mode 100644 index 193a7d05fb..0000000000 --- a/src/modules/ackermann_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp +++ /dev/null @@ -1,84 +0,0 @@ -/**************************************************************************** - * - * 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 deleted file mode 100644 index 6eee43c41e..0000000000 --- a/src/modules/ackermann_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp +++ /dev/null @@ -1,174 +0,0 @@ -/**************************************************************************** - * - * 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)); -}