diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/50000_gz_rover_differential b/ROMFS/px4fmu_common/init.d-posix/airframes/50000_gz_rover_differential index 270c01c55b..c9e47be70e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/50000_gz_rover_differential +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/50000_gz_rover_differential @@ -17,6 +17,7 @@ param set-default NAV_ACC_RAD 0.5 param set-default RD_WHEEL_TRACK 0.6 param set-default RD_TRANS_DRV_TRN 0.785398 param set-default RD_TRANS_TRN_DRV 0.174533 +param set-default RD_YAW_STK_GAIN 0.6 # Rate Control Parameters param set-default RO_YAW_RATE_I 0.01 @@ -25,6 +26,8 @@ param set-default RO_YAW_RATE_LIM 250 param set-default RO_YAW_ACCEL_LIM 400 param set-default RO_YAW_DECEL_LIM 800 param set-default RO_YAW_RATE_CORR 1 +param set-default RO_YAW_EXPO 0.85 +param set-default RO_YAW_SUPEXPO 0.3 # Attitude Control Parameters param set-default RO_YAW_P 5 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann b/ROMFS/px4fmu_common/init.d-posix/airframes/51000_gz_rover_ackermann similarity index 63% rename from ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann rename to ROMFS/px4fmu_common/init.d-posix/airframes/51000_gz_rover_ackermann index 28025d79ac..96bb10862b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/51000_gz_rover_ackermann @@ -1,5 +1,5 @@ #!/bin/sh -# @name Rover Ackermann +# @name Generic Ackermann Rover # @type Rover # @class Rover @@ -14,30 +14,32 @@ param set-default SIM_GZ_EN 1 # Gazebo bridge param set-default NAV_ACC_RAD 0.5 # Ackermann Parameters -param set-default RA_WHEEL_BASE 0.321 +param set-default RA_WHEEL_BASE 0.5 param set-default RA_ACC_RAD_GAIN 2 -param set-default RA_ACC_RAD_MAX 3 +param set-default RA_ACC_RAD_MAX 1.5 param set-default RA_MAX_STR_ANG 0.5236 param set-default RA_STR_RATE_LIM 360 -# Rover Control Parameters -param set-default RO_ACCEL_LIM 3 -param set-default RO_DECEL_LIM 6 -param set-default RO_JERK_LIM 15 -param set-default RO_MAX_THR_SPEED 3.1 - -# Rover Rate Control Parameters -param set-default RO_YAW_RATE_I 0.1 -param set-default RO_YAW_RATE_P 1 -param set-default RO_YAW_RATE_LIM 180 +# Rate Control Parameters +param set-default RO_YAW_RATE_I 0.01 +param set-default RO_YAW_RATE_P 0.25 +param set-default RO_YAW_RATE_LIM 130 +param set-default RO_YAW_ACCEL_LIM 800 +param set-default RO_YAW_DECEL_LIM 800 param set-default RO_YAW_RATE_CORR 1 +param set-default RO_YAW_EXPO 0.85 +param set-default RO_YAW_SUPEXPO 0.3 -# Rover Attitude Control Parameters +# Attitude Control Parameters param set-default RO_YAW_P 3 -# Rover Velocity Control Parameters -param set-default RO_SPEED_LIM 3 -param set-default RO_SPEED_I 0.1 +# Velocity Control Parameters +param set-default RO_ACCEL_LIM 4 +param set-default RO_DECEL_LIM 6 +param set-default RO_JERK_LIM 10 +param set-default RO_MAX_THR_SPEED 3.1 +param set-default RO_SPEED_LIM 2.5 +param set-default RO_SPEED_I 0.01 param set-default RO_SPEED_P 1 param set-default RO_SPEED_RED 1 @@ -48,8 +50,8 @@ param set-default PP_LOOKAHD_MIN 1 # Wheels param set-default SIM_GZ_WH_FUNC1 101 -param set-default SIM_GZ_WH_MIN1 0 -param set-default SIM_GZ_WH_MAX1 200 +param set-default SIM_GZ_WH_MIN1 70 +param set-default SIM_GZ_WH_MAX1 130 param set-default SIM_GZ_WH_DIS1 100 # Steering diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum b/ROMFS/px4fmu_common/init.d-posix/airframes/52000_gz_rover_mecanum similarity index 64% rename from ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum rename to ROMFS/px4fmu_common/init.d-posix/airframes/52000_gz_rover_mecanum index 5d96694e83..c882924bda 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/52000_gz_rover_mecanum @@ -1,5 +1,5 @@ #!/bin/sh -# @name Aion Robotics R1 Rover +# @name Generic Mecanum Rover # @type Rover # @class Rover @@ -7,14 +7,15 @@ PX4_SIMULATOR=${PX4_SIMULATOR:=gz} PX4_GZ_WORLD=${PX4_GZ_WORLD:=rover} -PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover_mecanum} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=rover_mecanum} param set-default SIM_GZ_EN 1 # Gazebo bridge param set-default NAV_ACC_RAD 0.5 # Mecanum Parameters -param set-default RM_WHEEL_TRACK 0.3 +param set-default RM_WHEEL_TRACK 0.6 +param set-default RM_YAW_STK_GAIN 0.6 # Rover Control Parameters param set-default RO_ACCEL_LIM 3 @@ -29,15 +30,21 @@ param set-default RO_YAW_RATE_LIM 120 param set-default RO_YAW_ACCEL_LIM 240 param set-default RO_YAW_DECEL_LIM 1000 param set-default RO_YAW_RATE_CORR 1.75 +param set-default RO_YAW_EXPO 0.85 +param set-default RO_YAW_SUPEXPO 0.3 # Rover Attitude Control Parameters param set-default RO_YAW_P 5 -# Rover Velocity Control Parameters -param set-default RO_SPEED_LIM 2 -param set-default RO_SPEED_I 0.5 -param set-default RO_SPEED_P 1 -param set-default RO_SPEED_RED 1 +# Velocity Control Parameters +param set-default RO_ACCEL_LIM 4 +param set-default RO_DECEL_LIM 6 +param set-default RO_JERK_LIM 10 +param set-default RO_MAX_THR_SPEED 3.1 +param set-default RO_SPEED_LIM 2.5 +param set-default RO_SPEED_I 0.01 +param set-default RO_SPEED_P 0.1 +param set-default RO_SPEED_RED 0.5 # Pure Pursuit parameters param set-default PP_LOOKAHD_GAIN 0.5 @@ -48,24 +55,24 @@ param set-default PP_LOOKAHD_MIN 1 param set-default SENS_EN_MAGSIM 1 # Actuator mapping -param set-default SIM_GZ_WH_FUNC1 102 # right wheel front +param set-default SIM_GZ_WH_FUNC1 104 # left wheel back param set-default SIM_GZ_WH_MIN1 70 param set-default SIM_GZ_WH_MAX1 130 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_FUNC2 103 # right wheel back param set-default SIM_GZ_WH_MIN2 70 param set-default SIM_GZ_WH_MAX2 130 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_FUNC3 102 # left wheel front param set-default SIM_GZ_WH_MIN3 70 param set-default SIM_GZ_WH_MAX3 130 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_FUNC4 101 # right wheel front param set-default SIM_GZ_WH_MIN4 70 param set-default SIM_GZ_WH_MAX4 130 param set-default SIM_GZ_WH_DIS4 100 -param set-default SIM_GZ_WH_REV 10 +param set-default SIM_GZ_WH_REV 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index b068b14aa1..4cf43413f6 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -80,10 +80,8 @@ px4_add_romfs_files( 4009_gz_r1_rover 4010_gz_x500_mono_cam 4011_gz_lawnmower - 4012_gz_rover_ackermann 4013_gz_x500_lidar_2d 4014_gz_x500_mono_cam_down - 4015_gz_r1_rover_mecanum 4016_gz_x500_lidar_down 4017_gz_x500_lidar_front 4018_gz_quadtailsitter @@ -114,6 +112,8 @@ px4_add_romfs_files( 17002_flightgear_tf-g2 50000_gz_rover_differential + 51000_gz_rover_ackermann + 52000_gz_rover_mecanum 60002_gz_uuv_bluerov2_heavy diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 6cfb3e362e..b6127f4ec2 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 6cfb3e362e1424caccb7363dca7e63484e44d188 +Subproject commit b6127f4ec20de867e215fb5f78ae88b80f371909 diff --git a/docs/assets/airframes/rover/rover_simulation.png b/docs/assets/airframes/rover/rover_simulation.png index e57bb0861a..f19e512812 100644 Binary files a/docs/assets/airframes/rover/rover_simulation.png and b/docs/assets/airframes/rover/rover_simulation.png differ diff --git a/docs/assets/simulation/gazebo/vehicles/rover_ackermann.png b/docs/assets/simulation/gazebo/vehicles/rover_ackermann.png index d6b305c000..c0c487efac 100644 Binary files a/docs/assets/simulation/gazebo/vehicles/rover_ackermann.png and b/docs/assets/simulation/gazebo/vehicles/rover_ackermann.png differ diff --git a/docs/assets/simulation/gazebo/vehicles/rover_differential.png b/docs/assets/simulation/gazebo/vehicles/rover_differential.png index 4d439029cb..5fa4c40a5f 100644 Binary files a/docs/assets/simulation/gazebo/vehicles/rover_differential.png and b/docs/assets/simulation/gazebo/vehicles/rover_differential.png differ diff --git a/docs/assets/simulation/gazebo/vehicles/rover_mecanum.png b/docs/assets/simulation/gazebo/vehicles/rover_mecanum.png new file mode 100644 index 0000000000..aff7b556c8 Binary files /dev/null and b/docs/assets/simulation/gazebo/vehicles/rover_mecanum.png differ diff --git a/docs/en/frames_rover/index.md b/docs/en/frames_rover/index.md index 8ba0751a0d..09492e324c 100644 --- a/docs/en/frames_rover/index.md +++ b/docs/en/frames_rover/index.md @@ -64,9 +64,10 @@ Each wheel is driven by its own motor, and by controlling the speed and directio ## Simulation -[Gazebo](../sim_gazebo_gz/index.md) provides simulations for ackermann and differential rovers: +PX4 provides synthetic simulation models for [Gazebo](../sim_gazebo_gz/index.md) of all three rover types to test the software and validate changes and new features: - [Ackermann rover](../sim_gazebo_gz/vehicles.md#ackermann-rover) - [Differential rover](../sim_gazebo_gz/vehicles.md#differential-rover) +- [Mecanum rover](../sim_gazebo_gz/vehicles.md#mecanum-rover) ![Rover gazebo simulation](../../assets/airframes/rover/rover_simulation.png) diff --git a/docs/en/releases/main.md b/docs/en/releases/main.md index 04fa799548..3931d565b4 100644 --- a/docs/en/releases/main.md +++ b/docs/en/releases/main.md @@ -58,7 +58,10 @@ Please continue reading for [upgrade instructions](#upgrade-guide). ### Simulation -- TBD +- Overhaul rover simulation: + - Add synthetic differential rover model: [PX4-gazebo-models#107](https://github.com/PX4/PX4-gazebo-models/pull/107) + - Add synthetic mecanum rover model: [PX4-gazebo-models#113](https://github.com/PX4/PX4-gazebo-models/pull/113) + - Update synthetic ackermann rover model: [PX4-gazebo-models#117](https://github.com/PX4/PX4-gazebo-models/pull/117) ### Ethernet @@ -91,6 +94,7 @@ Please continue reading for [upgrade instructions](#upgrade-guide). - Removed deprecated rover module ([PX4-Autopilot#25054](https://github.com/PX4/PX4-Autopilot/pull/25054)). - Add support for [Apps & API](../flight_modes_rover/api.md) ([PX4-Autopilot#25074](https://github.com/PX4/PX4-Autopilot/pull/25074), [PX4-ROS2-Interface-Lib#140](https://github.com/Auterion/px4-ros2-interface-lib/pull/140)). +- Update [rover simulation](../frames_rover/index.md#simulation) ([PX4-Autopilot#25644](https://github.com/PX4/PX4-Autopilot/pull/25644)) (see [Simulation](#simulation) release note for details). ### ROS 2 diff --git a/docs/en/sim_gazebo_gz/index.md b/docs/en/sim_gazebo_gz/index.md index 5304220694..6f5dead778 100644 --- a/docs/en/sim_gazebo_gz/index.md +++ b/docs/en/sim_gazebo_gz/index.md @@ -50,22 +50,24 @@ This runs both the PX4 SITL instance and the Gazebo client. The supported vehicles and `make` commands are listed below. Note that all gazebo make targets have the prefix `gz_`. -| Vehicle | Command | `PX4_SYS_AUTOSTART` | -| ----------------------------------------------------------------------------------------------------------------------------- | ----------------------------------- | ------------------- | -| [Quadrotor (x500)](../sim_gazebo_gz/vehicles.md#x500-quadrotor) | `make px4_sitl gz_x500` | 4001 | -| [X500 Quadrotor with Depth Camera (Front-facing)](../sim_gazebo_gz/vehicles.md#x500-quadrotor-with-depth-camera-front-facing) | `make px4_sitl gz_x500_depth` | 4002 | -| [Quadrotor(x500) with Vision Odometry](../sim_gazebo_gz/vehicles.md#x500-quadrotor-with-visual-odometry) | `make px4_sitl gz_x500_vision` | 4005 | -| [Quadrotor(x500) with 1D LIDAR (Down-facing)](../sim_gazebo_gz/vehicles.md#x500-quadrotor-with-1d-lidar-down-facing) | `make px4_sitl gz_x500_lidar_down` | 4016 | -| [Quadrotor(x500) with 2D LIDAR](../sim_gazebo_gz/vehicles.md#x500-quadrotor-with-2d-lidar) | `make px4_sitl gz_x500_lidar_2d` | 4013 | -| [Quadrotor(x500) with 1D LIDAR (Front-facing)](../sim_gazebo_gz/vehicles.md#x500-quadrotor-with-1d-lidar-front-facing) | `make px4_sitl gz_x500_lidar_front` | 4017 | -| [Quadrotor(x500) with gimbal (Front-facing) in Gazebo](../sim_gazebo_gz/vehicles.md#x500-quadrotor-with-gimbal-front-facing) | `make px4_sitl gz_x500_gimbal` | 4019 | -| [VTOL](../sim_gazebo_gz/vehicles.md#standard-vtol) | `make px4_sitl gz_standard_vtol` | 4004 | -| [Plane](../sim_gazebo_gz/vehicles.md#standard-plane) | `make px4_sitl gz_rc_cessna` | 4003 | -| [Advanced Plane](../sim_gazebo_gz/vehicles.md#advanced-plane) | `make px4_sitl gz_advanced_plane` | 4008 | -| [Differential Rover](../sim_gazebo_gz/vehicles.md#differential-rover) | `make px4_sitl gz_r1_rover` | 4009 | -| [Ackermann Rover](../sim_gazebo_gz/vehicles.md#ackermann-rover) | `make px4_sitl gz_rover_ackermann` | 4012 | -| [Quad Tailsitter VTOL](../sim_gazebo_gz/vehicles.md#quad-tailsitter-vtol) | `make px4_sitl gz_quadtailsitter` | 4018 | -| [Tiltrotor VTOL](../sim_gazebo_gz/vehicles.md#tiltrotor-vtol) | `make px4_sitl gz_tiltrotor` | 4020 | +| Vehicle | Command | `PX4_SYS_AUTOSTART` | +| ----------------------------------------------------------------------------------------------------------------------------- | ------------------------------------- | ------------------- | +| [Quadrotor (x500)](../sim_gazebo_gz/vehicles.md#x500-quadrotor) | `make px4_sitl gz_x500` | 4001 | +| [X500 Quadrotor with Depth Camera (Front-facing)](../sim_gazebo_gz/vehicles.md#x500-quadrotor-with-depth-camera-front-facing) | `make px4_sitl gz_x500_depth` | 4002 | +| [Quadrotor(x500) with Vision Odometry](../sim_gazebo_gz/vehicles.md#x500-quadrotor-with-visual-odometry) | `make px4_sitl gz_x500_vision` | 4005 | +| [Quadrotor(x500) with 1D LIDAR (Down-facing)](../sim_gazebo_gz/vehicles.md#x500-quadrotor-with-1d-lidar-down-facing) | `make px4_sitl gz_x500_lidar_down` | 4016 | +| [Quadrotor(x500) with 2D LIDAR](../sim_gazebo_gz/vehicles.md#x500-quadrotor-with-2d-lidar) | `make px4_sitl gz_x500_lidar_2d` | 4013 | +| [Quadrotor(x500) with 1D LIDAR (Front-facing)](../sim_gazebo_gz/vehicles.md#x500-quadrotor-with-1d-lidar-front-facing) | `make px4_sitl gz_x500_lidar_front` | 4017 | +| [Quadrotor(x500) with gimbal (Front-facing) in Gazebo](../sim_gazebo_gz/vehicles.md#x500-quadrotor-with-gimbal-front-facing) | `make px4_sitl gz_x500_gimbal` | 4019 | +| [VTOL](../sim_gazebo_gz/vehicles.md#standard-vtol) | `make px4_sitl gz_standard_vtol` | 4004 | +| [Plane](../sim_gazebo_gz/vehicles.md#standard-plane) | `make px4_sitl gz_rc_cessna` | 4003 | +| [Advanced Plane](../sim_gazebo_gz/vehicles.md#advanced-plane) | `make px4_sitl gz_advanced_plane` | 4008 | +| [Quad Tailsitter VTOL](../sim_gazebo_gz/vehicles.md#quad-tailsitter-vtol) | `make px4_sitl gz_quadtailsitter` | 4018 | +| [Tiltrotor VTOL](../sim_gazebo_gz/vehicles.md#tiltrotor-vtol) | `make px4_sitl gz_tiltrotor` | 4020 | +| [Differential Rover](../sim_gazebo_gz/vehicles.md#differential-rover) | `make px4_sitl gz_rover_differential` | 50000 | +| [Ackermann Rover](../sim_gazebo_gz/vehicles.md#ackermann-rover) | `make px4_sitl gz_rover_ackermann` | 51000 | +| [Mecanum Rover](../sim_gazebo_gz/vehicles.md#mecanum-rover) | `make px4_sitl gz_rover_mecanum` | 52000 | + All [vehicle models](../sim_gazebo_gz/vehicles.md) (and [worlds](#specify-world)) are included as a submodule from the [Gazebo Models Repository](../sim_gazebo_gz/gazebo_models.md) repository. @@ -164,16 +166,16 @@ PX4_GZ_WORLD=windy make px4_sitl gz_x500 The [supported worlds](../sim_gazebo_gz/worlds.md) are listed below. -| World | Command | Description | -| ----------------- | ---------------------------------- | ----------------------------------------------------------- | -| `default` | `make px4_sitl *` | Empty world (a grey plane) | -| `aruco` | `make px4_sitl *_aruco` | Empty world with aruco marker for testing precision landing | -| `baylands` | `make px4_sitl *_baylands` | Baylands world surrounded by water | -| `lawn` | `make px4_sitl *_lawn` | Lawn world for testing rovers | -| `rover` | `make px4_sitl *_rover` | Rover world (optimised/preferred) | -| `walls` | `make px4_sitl *_walls` | Wall world for testing collision prevention | -| `windy` | `make px4_sitl *_windy` | Empty world with wind enabled | -| `moving_platform` | `make px4_sitl *_moving_platform` | World with moving takeoff / landing platform | +| World | Command | Description | +| ----------------- | --------------------------------- | ----------------------------------------------------------- | +| `default` | `make px4_sitl *` | Empty world (a grey plane) | +| `aruco` | `make px4_sitl *_aruco` | Empty world with aruco marker for testing precision landing | +| `baylands` | `make px4_sitl *_baylands` | Baylands world surrounded by water | +| `lawn` | `make px4_sitl *_lawn` | Lawn world for testing rovers | +| `rover` | `make px4_sitl *_rover` | Rover world (optimised/preferred) | +| `walls` | `make px4_sitl *_walls` | Wall world for testing collision prevention | +| `windy` | `make px4_sitl *_windy` | Empty world with wind enabled | +| `moving_platform` | `make px4_sitl *_moving_platform` | World with moving takeoff / landing platform | :::warning Note that if no world is specified, PX4 will use the `default` world. diff --git a/docs/en/sim_gazebo_gz/vehicles.md b/docs/en/sim_gazebo_gz/vehicles.md index 679aec83ff..10f377d465 100644 --- a/docs/en/sim_gazebo_gz/vehicles.md +++ b/docs/en/sim_gazebo_gz/vehicles.md @@ -185,7 +185,7 @@ make px4_sitl gz_tiltrotor [Differential Rover](../frames_rover/index.md#differential) uses the [rover world](../sim_gazebo_gz/worlds.md#rover) by default. ```sh -make px4_sitl gz_r1_rover +make px4_sitl gz_rover_differential ``` ![Differential Rover in Gazebo](../../assets/simulation/gazebo/vehicles/rover_differential.png) @@ -199,3 +199,13 @@ make px4_sitl gz_rover_ackermann ``` ![Ackermann Rover in Gazebo](../../assets/simulation/gazebo/vehicles/rover_ackermann.png) + +### Mecanum Rover + +[Mecanum Rover](../frames_rover/index.md#mecanum) uses the [rover world](../sim_gazebo_gz/worlds.md#rover) by default. + +```sh +make px4_sitl gz_rover_mecanum +``` + +![Mecanum Rover in Gazebo](../../assets/simulation/gazebo/vehicles/rover_mecanum.png)