Mecanum rover: add dedicated module for mecanum rovers (#23708)

ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum

@@ -0,0 +1,64 @@
+#!/bin/sh
+# @name Aion Robotics R1 Rover
+# @type Rover
+# @class Rover
+
+. ${R}etc/init.d/rc.rover_mecanum_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=rover}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover_mecanum}
+
+param set-default SIM_GZ_EN 1 # Gazebo bridge
+
+# Rover parameters
+param set-default RM_WHEEL_TRACK 0.3
+param set-default RM_MAN_YAW_SCALE 0.1
+param set-default RM_YAW_RATE_I 0
+param set-default RM_YAW_RATE_P 0.01
+param set-default RM_MAX_ACCEL 3
+param set-default RM_MAX_JERK 5
+param set-default RM_MAX_SPEED 4
+param set-default RM_MAX_THR_SPD 7
+param set-default RM_MAX_THR_YAW_R 7.5
+param set-default RM_YAW_P 5
+param set-default RM_YAW_I 0.1
+param set-default RM_MAX_YAW_RATE 180
+param set-default RM_MISS_SPD_DEF 3
+param set-default RM_MISS_VEL_GAIN 1
+param set-default RM_SPEED_I 0.01
+param set-default RM_SPEED_P 0.1
+
+# Pure pursuit parameters
+param set-default PP_LOOKAHD_MAX 10
+param set-default PP_LOOKAHD_MIN 1
+param set-default PP_LOOKAHD_GAIN 0.5
+
+# 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 0
+
+# Actuator mapping
+param set-default SIM_GZ_WH_FUNC1 102 # right wheel front
+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 101 # left wheel front
+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_FUNC3 104 # right wheel back
+param set-default SIM_GZ_WH_MIN3 0
+param set-default SIM_GZ_WH_MAX3 200
+param set-default SIM_GZ_WH_DIS3 100
+
+param set-default SIM_GZ_WH_FUNC4 103 # left wheel back
+param set-default SIM_GZ_WH_MIN4 0
+param set-default SIM_GZ_WH_MAX4 200
+param set-default SIM_GZ_WH_DIS4 100
+
+param set-default SIM_GZ_WH_REV 10

ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

@@ -86,6 +86,7 @@ px4_add_romfs_files(
 	4012_gz_rover_ackermann
 	4013_gz_x500_lidar
 	4014_gz_x500_mono_cam_down
+	4015_gz_r1_rover_mecanum
 
 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post

ROMFS/px4fmu_common/init.d/CMakeLists.txt

@@ -92,6 +92,13 @@ if(CONFIG_MODULES_ROVER_ACKERMANN)
 	)
 endif()
 
+if(CONFIG_MODULES_ROVER_MECANUM)
+	px4_add_romfs_files(
+		rc.rover_mecanum_apps
+		rc.rover_mecanum_defaults
+	)
+endif()
+
 if(CONFIG_MODULES_UUV_ATT_CONTROL)
 	px4_add_romfs_files(
 		rc.uuv_apps

ROMFS/px4fmu_common/init.d/airframes/52000_generic_rover_mecanum

@@ -0,0 +1,12 @@
+#!/bin/sh
+#
+# @name Generic Rover Mecanum
+#
+# @type Rover
+# @class Rover
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.rover_mecanum_defaults

ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt

@@ -152,6 +152,13 @@ if(CONFIG_MODULES_ROVER_ACKERMANN)
 	)
 endif()
 
+if(CONFIG_MODULES_ROVER_MECANUM)
+	px4_add_romfs_files(
+		# [52000, 52999] Mecanum rovers
+		52000_generic_rover_mecanum
+	)
+endif()
+
 if(CONFIG_MODULES_ROVER_POS_CONTROL)
 	px4_add_romfs_files(
 		# [59000, 59999] Rover position control (deprecated)

ROMFS/px4fmu_common/init.d/rc.rover_mecanum_apps

@@ -0,0 +1,8 @@
+#!/bin/sh
+# Standard apps for a mecanum drive rover.
+
+# Start rover mecanum drive controller.
+rover_mecanum start
+
+# Start Land Detector.
+land_detector start rover

ROMFS/px4fmu_common/init.d/rc.rover_mecanum_defaults

@@ -0,0 +1,11 @@
+#!/bin/sh
+# Mecanum rover parameters.
+
+set VEHICLE_TYPE rover_mecanum
+param set-default MAV_TYPE 10     # MAV_TYPE_GROUND_ROVER
+param set-default CA_AIRFRAME 13  # Rover (Mecanum)
+param set-default CA_R_REV 15     # Right and left motors reversible
+param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying
+param set-default NAV_ACC_RAD 0.5 # Waypoint acceptance radius
+param set-default EKF2_GBIAS_INIT 0.01
+param set-default EKF2_ANGERR_INIT 0.01

ROMFS/px4fmu_common/init.d/rc.vehicle_setup

@@ -50,6 +50,15 @@ then
 	. ${R}etc/init.d/rc.rover_ackermann_apps
 fi
 
+#
+# Mecanum Rover setup.
+#
+if [ $VEHICLE_TYPE = rover_mecanum ]
+then
+	# Start mecanum drive rover apps.
+	. ${R}etc/init.d/rc.rover_mecanum_apps
+fi
+
 #
 # VTOL setup.
 #

boards/px4/fmu-v5/rover.px4board

@@ -19,4 +19,5 @@ CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y
 CONFIG_MODULES_ROVER_ACKERMANN=y
+CONFIG_MODULES_ROVER_MECANUM=y
 CONFIG_DRIVERS_ROBOCLAW=y

boards/px4/fmu-v5x/rover.px4board

@@ -16,4 +16,5 @@ CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y
 CONFIG_MODULES_ROVER_ACKERMANN=y
+CONFIG_MODULES_ROVER_MECANUM=y
 CONFIG_DRIVERS_ROBOCLAW=y

boards/px4/fmu-v6c/rover.px4board

@@ -15,4 +15,5 @@ CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y
 CONFIG_MODULES_ROVER_ACKERMANN=y
+CONFIG_MODULES_ROVER_MECANUM=y
 CONFIG_DRIVERS_ROBOCLAW=y

boards/px4/fmu-v6u/rover.px4board

@@ -15,4 +15,5 @@ CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y
 CONFIG_MODULES_ROVER_ACKERMANN=y
+CONFIG_MODULES_ROVER_MECANUM=y
 CONFIG_DRIVERS_ROBOCLAW=y

boards/px4/fmu-v6x/rover.px4board

@@ -15,4 +15,5 @@ CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y
 CONFIG_MODULES_ROVER_ACKERMANN=y
+CONFIG_MODULES_ROVER_MECANUM=y
 CONFIG_DRIVERS_ROBOCLAW=y

boards/px4/fmu-v6xrt/rover.px4board

@@ -16,4 +16,5 @@ CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y
 CONFIG_MODULES_ROVER_ACKERMANN=y
+CONFIG_MODULES_ROVER_MECANUM=y
 CONFIG_DRIVERS_ROBOCLAW=y

boards/px4/sitl/default.px4board

@@ -47,6 +47,7 @@ CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_REPLAY=y
 CONFIG_MODULES_ROVER_ACKERMANN=y
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y
+CONFIG_MODULES_ROVER_MECANUM=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_COMMON_SIMULATION=y

msg/CMakeLists.txt

@@ -189,6 +189,9 @@ set(msg_files
 	RoverDifferentialGuidanceStatus.msg
 	RoverDifferentialSetpoint.msg
 	RoverDifferentialStatus.msg
+	RoverMecanumGuidanceStatus.msg
+	RoverMecanumSetpoint.msg
+	RoverMecanumStatus.msg
 	Rpm.msg
 	RtlStatus.msg
 	RtlTimeEstimate.msg

msg/RoverMecanumGuidanceStatus.msg

@@ -0,0 +1,7 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller
+float32 heading_error      # [rad] Heading error of the pure pursuit controller
+float32 desired_speed      # [m/s] Desired velocity magnitude (speed)
+
+# TOPICS rover_mecanum_guidance_status

msg/RoverMecanumSetpoint.msg

@@ -0,0 +1,11 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 forward_speed_setpoint            # [m/s] Desired forward speed
+float32 forward_speed_setpoint_normalized # [-1, 1] Desired normalized forward speed
+float32 lateral_speed_setpoint            # [m/s] Desired lateral speed
+float32 lateral_speed_setpoint_normalized # [-1, 1] Desired normalized lateral speed
+float32 yaw_rate_setpoint                 # [rad/s] Desired yaw rate
+float32 yaw_rate_setpoint_normalized      # [-1, 1] Desired normalized yaw rate
+float32 yaw_setpoint 	                  # [rad] Desired yaw (heading)
+
+# TOPICS rover_mecanum_setpoint

msg/RoverMecanumStatus.msg

@@ -0,0 +1,13 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 measured_forward_speed        # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards
+float32 measured_lateral_speed        # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left
+float32 adjusted_yaw_rate_setpoint    # [rad/s] Yaw rate setpoint output of the closed loop yaw controller
+float32 measured_yaw_rate  	      # [rad/s] Measured yaw rate
+float32 measured_yaw        	      # [rad] Measured yaw
+float32 pid_yaw_rate_integral  	      # Integral of the PID for the closed loop yaw rate controller
+float32 pid_yaw_integral       	      # Integral of the PID for the closed loop yaw controller
+float32 pid_forward_throttle_integral # Integral of the PID for the closed loop forward speed controller
+float32 pid_lateral_throttle_integral # Integral of the PID for the closed loop lateral speed controller
+
+# TOPICS rover_mecanum_status

src/modules/control_allocator/module.yaml

@@ -30,6 +30,7 @@ parameters:
                 10: Helicopter (tail ESC)
                 11: Helicopter (tail Servo)
                 12: Helicopter (Coaxial)
+                13: Rover (Mecanum)
             default: 0
 
         CA_METHOD:
@@ -950,11 +951,11 @@ mixer:
                   - actuator_type: 'motor'
                     instances:
                       - name: 'Throttle'
-                        position: [ -1., 0, 0 ]
+                        position: [ -1, 0, 0 ]
                   - actuator_type: 'servo'
                     instances:
                       - name: 'Steering'
-                        position: [ 1., 0, 0 ]
+                        position: [ 1, 0, 0 ]
 
             6: # Rover (Differential)
                 title: 'Rover (Differential)'
@@ -962,9 +963,9 @@ mixer:
                   - actuator_type: 'motor'
                     instances:
                       - name: 'Right Motor'
-                        position: [ 0, 1., 0 ]
+                        position: [ 0, 1, 0 ]
                       - name: 'Left Motor'
-                        position: [ 0, -1., 0 ]
+                        position: [ 0, -1, 0 ]
 
             7: # Motors (6DOF)
                 actuators:
@@ -1140,3 +1141,17 @@ mixer:
                     parameters:
                       - label: 'Throttle spoolup time'
                         name: COM_SPOOLUP_TIME
+
+            13: # Rover (Mecanum)
+                title: 'Rover (Mecanum)'
+                actuators:
+                  - actuator_type: 'motor'
+                    instances:
+                      - name: 'Right Motor Front'
+                        position: [ 1, 1, 0 ]
+                      - name: 'Left Motor Front'
+                        position: [ 1, -1, 0 ]
+                      - name: 'Right Motor Back'
+                        position: [ -1, 1, 0 ]
+                      - name: 'Left Motor Back'
+                        position: [ -1, -1, 0 ]

src/modules/logger/logged_topics.cpp

@@ -108,6 +108,9 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic("rover_differential_guidance_status", 100);
 	add_optional_topic("rover_differential_setpoint", 100);
 	add_optional_topic("rover_differential_status", 100);
+	add_optional_topic("rover_mecanum_guidance_status", 100);
+	add_optional_topic("rover_mecanum_setpoint", 100);
+	add_optional_topic("rover_mecanum_status", 100);
 	add_topic("rtl_time_estimate", 1000);
 	add_topic("rtl_status", 2000);
 	add_optional_topic("sensor_airflow", 100);

src/modules/rover_mecanum/CMakeLists.txt

@@ -0,0 +1,50 @@
+############################################################################
+#
+#   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(RoverMecanumGuidance)
+add_subdirectory(RoverMecanumControl)
+
+px4_add_module(
+	MODULE modules__rover_mecanum
+	MAIN rover_mecanum
+	SRCS
+		RoverMecanum.cpp
+		RoverMecanum.hpp
+	DEPENDS
+		RoverMecanumGuidance
+		RoverMecanumControl
+		px4_work_queue
+		pure_pursuit
+	MODULE_CONFIG
+		module.yaml
+)

src/modules/rover_mecanum/Kconfig

@@ -0,0 +1,6 @@
+menuconfig MODULES_ROVER_MECANUM
+	bool "rover_mecanum"
+	default n
+	depends on MODULES_CONTROL_ALLOCATOR
+	---help---
+		Enable support for control of mecanum rovers

src/modules/rover_mecanum/RoverMecanum.hpp

@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ *   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 <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+
+// uORB includes
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/rover_mecanum_setpoint.h>
+
+// Standard libraries
+#include <lib/pid/pid.h>
+#include <matrix/matrix/math.hpp>
+
+// Local includes
+#include "RoverMecanumGuidance/RoverMecanumGuidance.hpp"
+#include "RoverMecanumControl/RoverMecanumControl.hpp"
+
+// Constants
+static constexpr float YAW_RATE_THRESHOLD =
+	0.02f; // [rad/s] Threshold for the yaw rate measurement to avoid stuttering when the rover is standing still
+static constexpr float SPEED_THRESHOLD =
+	0.1f; // [m/s] Threshold for the speed measurement to cut off measurement noise when the rover is standing still
+static constexpr float STICK_DEADZONE =
+	0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value
+
+using namespace time_literals;
+
+class RoverMecanum : public ModuleBase<RoverMecanum>, public ModuleParams,
+	public px4::ScheduledWorkItem
+{
+public:
+	RoverMecanum();
+	~RoverMecanum() 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;
+
+	/**
+	 * @brief Update uORB subscriptions.
+	 */
+	void updateSubscriptions();
+
+	// 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_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
+	// uORB Publications
+	uORB::Publication<rover_mecanum_setpoint_s> _rover_mecanum_setpoint_pub{ORB_ID(rover_mecanum_setpoint)};
+
+	// Instances
+	RoverMecanumGuidance _rover_mecanum_guidance{this};
+	RoverMecanumControl _rover_mecanum_control{this};
+
+	// Variables
+	matrix::Quatf _vehicle_attitude_quaternion{};
+	float _vehicle_yaw_rate{0.f};
+	float _vehicle_forward_speed{0.f};
+	float _vehicle_lateral_speed{0.f};
+	float _vehicle_yaw{0.f};
+	float _max_yaw_rate{0.f};
+	int _nav_state{0};
+	bool _yaw_ctl{false}; // Indicates if the rover is doing yaw or yaw rate control in position mode
+	float _desired_yaw{0.f}; // Yaw setpoint for position mode
+	bool _armed{false};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RM_MAX_SPEED>) _param_rm_max_speed,
+		(ParamFloat<px4::params::RM_MAN_YAW_SCALE>) _param_rm_man_yaw_scale,
+		(ParamFloat<px4::params::RM_MAX_YAW_RATE>) _param_rm_max_yaw_rate
+	)
+};

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})

src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp

@@ -0,0 +1,134 @@
+/****************************************************************************
+ *
+ *   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 <px4_platform_common/module_params.h>
+
+// uORB includes
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/rover_mecanum_setpoint.h>
+#include <uORB/topics/rover_mecanum_status.h>
+#include <uORB/topics/actuator_motors.h>
+
+
+// Standard libraries
+#include <lib/pid/pid.h>
+#include <matrix/matrix/math.hpp>
+#include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
+#include <math.h>
+
+using namespace matrix;
+
+/**
+ * @brief Class for mecanum rover guidance.
+ */
+class RoverMecanumControl : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for RoverMecanumControl.
+	 * @param parent The parent ModuleParams object.
+	 */
+	RoverMecanumControl(ModuleParams *parent);
+	~RoverMecanumControl() = default;
+
+	/**
+	 * @brief Compute motor commands based on setpoints
+	 * @param vehicle_yaw Yaw of the vehicle [rad]
+	 * @param vehicle_yaw_rate Yaw rate of the vehicle [rad/s]
+	 * @param vehicle_forward_speed Forward speed of the vehicle [m/s]
+	 * @param vehicle_lateral_speed Lateral speed of the vehicle [m/s]
+	 */
+	void computeMotorCommands(float vehicle_yaw, float vehicle_yaw_rate, float vehicle_forward_speed,
+				  float vehicle_lateral_speed);
+
+	/**
+	 * @brief Reset PID controllers
+	 */
+	void resetControllers();
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
+	/**
+	 * @brief Turn normalized speed setpoints into normalized motor commands.
+	 *
+	 * @param forward_speed Normalized linear speed in body forward direction [-1, 1].
+	 * @param lateral_speed Normalized linear speed in body lateral direction [-1, 1].
+	 * @param speed_diff Normalized speed difference between left and right wheels [-1, 1].
+	 * @return matrix::Vector4f: Normalized motor inputs [-1, 1]
+	 */
+	matrix::Vector4f computeInverseKinematics(float forward_speed, float lateral_speed, float speed_diff);
+
+	// uORB subscriptions
+	uORB::Subscription _rover_mecanum_setpoint_sub{ORB_ID(rover_mecanum_setpoint)};
+
+	// uORB publications
+	uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
+	uORB::Publication<rover_mecanum_status_s> _rover_mecanum_status_pub{ORB_ID(rover_mecanum_status)};
+
+	// Variables
+	rover_mecanum_setpoint_s _rover_mecanum_setpoint{};
+	hrt_abstime _timestamp{0};
+	float _max_yaw_rate{0.f};
+
+	// Controllers
+	PID_t _pid_forward_throttle; // PID for the closed loop forward speed control
+	PID_t _pid_lateral_throttle; // PID for the closed loop lateral speed control
+	PID_t _pid_yaw; // PID for the closed loop yaw control
+	PID_t _pid_yaw_rate; // PID for the closed loop yaw rate control
+
+	// Parameters
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RM_WHEEL_TRACK>) _param_rm_wheel_track,
+		(ParamFloat<px4::params::RM_MAX_THR_SPD>) _param_rm_max_thr_spd,
+		(ParamFloat<px4::params::RM_MAX_THR_YAW_R>) _param_rm_max_thr_yaw_r,
+		(ParamFloat<px4::params::RM_MAX_YAW_RATE>) _param_rm_max_yaw_rate,
+		(ParamFloat<px4::params::RM_YAW_RATE_P>) _param_rm_yaw_rate_p,
+		(ParamFloat<px4::params::RM_YAW_RATE_I>) _param_rm_yaw_rate_i,
+		(ParamFloat<px4::params::RM_SPEED_P>) _param_rm_p_gain_speed,
+		(ParamFloat<px4::params::RM_SPEED_I>) _param_rm_i_gain_speed,
+		(ParamFloat<px4::params::RM_YAW_P>) _param_rm_p_gain_yaw,
+		(ParamFloat<px4::params::RM_YAW_I>) _param_rm_i_gain_yaw,
+		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
+	)
+};

src/modules/rover_mecanum/RoverMecanumGuidance/CMakeLists.txt

@@ -0,0 +1,38 @@
+############################################################################
+#
+#   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(RoverMecanumGuidance
+	RoverMecanumGuidance.cpp
+)
+
+target_include_directories(RoverMecanumGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp

@@ -0,0 +1,140 @@
+/****************************************************************************
+ *
+ *   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 <px4_platform_common/module_params.h>
+#include <lib/pure_pursuit/PurePursuit.hpp>
+
+// uORB includes
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/mission_result.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/rover_mecanum_guidance_status.h>
+#include <uORB/topics/rover_mecanum_setpoint.h>
+
+// Standard libraries
+#include <matrix/matrix/math.hpp>
+#include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+#include <math.h>
+
+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;
+
+	/**
+	 * @brief Compute guidance for the vehicle.
+	 * @param yaw The yaw orientation of the vehicle in radians.
+	 * @param nav_state Navigation state of the rover.
+	 */
+	void computeGuidance(float yaw, int nav_state);
+
+	void setDesiredYaw(float desired_yaw) { _desired_yaw = desired_yaw; };
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
+	/**
+	 * @brief Update uORB subscriptions
+	 */
+	void updateSubscriptions();
+
+	/**
+	 * @brief Update global/ned waypoint coordinates
+	 */
+	void updateWaypoints();
+
+	// 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_s> _rover_mecanum_guidance_status_pub{ORB_ID(rover_mecanum_guidance_status)};
+	uORB::Publication<rover_mecanum_setpoint_s> _rover_mecanum_setpoint_pub{ORB_ID(rover_mecanum_setpoint)};
+
+	// Variables
+	MapProjection _global_ned_proj_ref{}; // Transform global to ned coordinates.
+	PurePursuit _pure_pursuit{this}; // Pure pursuit library
+	bool _mission_finished{false};
+
+	// 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{};
+	float _max_velocity_magnitude{0.f};
+	float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
+	float _desired_yaw{0.f};
+
+	// Parameters
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RM_MAX_SPEED>) _param_rm_max_speed,
+		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
+		(ParamFloat<px4::params::RM_MAX_JERK>) _param_rm_max_jerk,
+		(ParamFloat<px4::params::RM_MAX_ACCEL>) _param_rm_max_accel,
+		(ParamFloat<px4::params::RM_MISS_SPD_DEF>) _param_rm_miss_spd_def,
+		(ParamFloat<px4::params::RM_MAX_YAW_RATE>) _param_rm_max_yaw_rate,
+		(ParamFloat<px4::params::RM_MISS_VEL_GAIN>) _param_rm_miss_vel_gain
+	)
+};

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