From e135a126b6ee1b3e0fcf62aea8c1a94f3260b743 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Mon, 26 Aug 2024 16:25:49 +0200 Subject: [PATCH] mecanum: add control folder --- .../RoverMecanumControl/CMakeLists.txt | 39 +++++ .../RoverMecanumControl.cpp | 0 .../RoverMecanumControl.hpp | 142 ++++++++++++++++++ .../RoverMecanumGuidance.cpp | 2 +- .../RoverMecanumGuidance.hpp | 1 - 5 files changed, 182 insertions(+), 2 deletions(-) create mode 100644 src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt create mode 100644 src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp create mode 100644 src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp diff --git a/src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt b/src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt new file mode 100644 index 000000000000..9777ae5d18e4 --- /dev/null +++ b/src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(RoverMecanumControl + RoverMecanumControl.cpp +) + +target_link_libraries(RoverMecanumControl PUBLIC pid) +target_include_directories(RoverMecanumControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp new file mode 100644 index 000000000000..82d5c9d78ba4 --- /dev/null +++ b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp @@ -0,0 +1,142 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include +#include + +// uORB includes +#include +#include +#include + +// Standard libraries +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Class for mecanum rover guidance. + */ +class RoverMecanumGuidance : public ModuleParams +{ +public: + /** + * @brief Constructor for RoverMecanumGuidance. + * @param parent The parent ModuleParams object. + */ + RoverMecanumGuidance(ModuleParams *parent); + ~RoverMecanumGuidance() = default; + + struct mecanum_setpoint { + float forward_throttle{0.f}; + float lateral_throttle{0.f}; + float yaw_rate{0.f}; + float heading{0.f}; + bool closed_loop_yaw_rate{false}; + bool closed_loop_speed{false}; + bool closed_loop_heading{false}; + }; + + /** + * @brief Compute guidance for the vehicle. + * @param yaw The yaw orientation of the vehicle in radians. + * @param forward_speed The velocity of the vehicle in m/s. + * @param dt The time step in seconds. + * @param nav_state Navigation state of the rover. + */ + RoverMecanumGuidance::mecanum_setpoint computeGuidance(float yaw, float forward_speed, float lateral_speed, + int nav_state); + + /** + * @brief Update global/ned waypoint coordinates + */ + void updateWaypoints(); + +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 _mission_result_sub{ORB_ID(mission_result)}; + uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _home_position_sub{ORB_ID(home_position)}; + + // uORB publications + uORB::Publication _rover_mecanum_guidance_status_pub{ORB_ID(rover_mecanum_guidance_status)}; + rover_mecanum_guidance_status_s _rover_mecanum_guidance_status{}; + + // Variables + MapProjection _global_ned_proj_ref{}; // Transform global to ned coordinates. + PurePursuit _pure_pursuit{this}; // Pure pursuit library + hrt_abstime _timestamp{0}; + float _max_yaw_rate{0.f}; + float _theta{0.f}; + + // Waypoints + Vector2d _curr_pos{}; + Vector2f _curr_pos_ned{}; + Vector2d _prev_wp{}; + Vector2f _prev_wp_ned{}; + Vector2d _curr_wp{}; + Vector2f _curr_wp_ned{}; + Vector2d _next_wp{}; + Vector2f _next_wp_ned{}; + Vector2d _home_position{}; + + // Constants + static constexpr float SLOW_DOWN_THRESHOLD = + 2.61799f; // Slow down threshold for the maximum angle between the prev-curr and curr-next waypoint segments. + + // Parameters + DEFINE_PARAMETERS( + (ParamFloat) _param_rm_max_speed, + (ParamFloat) _param_nav_acc_rad, + (ParamFloat) _param_rm_max_jerk, + (ParamFloat) _param_rm_max_accel, + (ParamFloat) _param_rm_miss_spd_def, + (ParamFloat) _param_rm_max_yaw_rate + ) +}; diff --git a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp index 9f7c91384c55..a8670196a813 100644 --- a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp +++ b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp @@ -138,7 +138,7 @@ RoverMecanumGuidance::mecanum_setpoint RoverMecanumGuidance::computeGuidance(con _rover_mecanum_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance(); _rover_mecanum_guidance_status.pid_heading_integral = 0.f; _rover_mecanum_guidance_status.heading_error_deg = M_RAD_TO_DEG_F * heading_error; - _rover_mecanum_guidance_status.state_machine = (uint8_t) _currentState; + _rover_mecanum_guidance_status.state_machine = 0.f; _rover_mecanum_guidance_status_pub.publish(_rover_mecanum_guidance_status); diff --git a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp index 7f567e5f0c4f..0b1544d09a1b 100644 --- a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp +++ b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp @@ -116,7 +116,6 @@ class RoverMecanumGuidance : public ModuleParams // Variables MapProjection _global_ned_proj_ref{}; // Transform global to ned coordinates. - GuidanceState _currentState{GuidanceState::DRIVING}; // The current state of guidance. PurePursuit _pure_pursuit{this}; // Pure pursuit library hrt_abstime _timestamp{0}; float _max_yaw_rate{0.f};