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,21 +951,21 @@ 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)'
                 actuators:
                   - 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