Integrating custom PX4 vehicle with gimbal, gz_ws, and world

[ph3rro]

Hi, I've been working on a drone + wave sim with the goal of using RL to land drones on boats.
I have a custom PX4-Autopilot folder, and in that folder I also have a gz_ws folder for the wave simulation, which contains models and physics for the waves and boat (courtesy of Rhys Mainwaring).

I usually run the following:

Terminal 1:

cd ~/PX4-Autopilot/gz_ws
source ./install/setup.bash
export GZ_VERSION=harmonic
export GZ_IP=127.0.0.1
export GZ_SIM_RESOURCE_PATH=
$GZ_SIM_RESOURCE_PATH:
$(pwd)/src/rs750/rs750_gazebo/models:
$(pwd)/src/rs750/rs750_gazebo/worlds:
$HOME/PX4-Autopilot/gz_ws/src/asv_wave_sim/gz-waves-models/models:
$HOME/PX4-Autopilot/gz_ws/src/asv_wave_sim/gz-waves-models/world_models:
$HOME/PX4-Autopilot/gz_ws/src/asv_wave_sim/gz-waves-models/worlds
export GZ_SIM_SYSTEM_PLUGIN_PATH=
$GZ_SIM_SYSTEM_PLUGIN_PATH:
$HOME/PX4-Autopilot/gz_ws/install/lib:
export GZ_GUI_PLUGIN_PATH=
$GZ_GUI_PLUGIN_PATH:
$HOME/PX4-Autopilot/gz_ws/src/asv_wave_sim/gz-waves/src/gui/plugins/waves_control/build
cd ..
PX4_GZ_WORLD=waves PX4_GZ_MODEL_POSE="0.3,-0.3,1.15" make px4_sitl gz_x500_gimbal

Terminal 2:
MicroXRCEAgent udp4 -p 8888

Terminal 3:
cd tracktor-beam
source install/setup.bash
ros2 launch precision_land precision_landing_system.launch.py

Terminal 4:
ros2 run rqt_image_view rqt_image_view

Terminal 5:
./Downloads/qgroundcontrol

Is there a way I can integrate all this into AAS? I'm not really familiar with Docker. Any help would be appreciated. Thanks!

[JacopoPan]

Hello @ph3rro

it should be entirely possible and I'd like to see that!
Let me try to draw a parallel between you setup and AAS (they are not too different).

---

In your setup you have 5 terminals/applications

• all on a single (I imagine Ubuntu 22 or 24) localhost 127.0.0.1
  • px4_sitl gz_x500_gimbal (Gazebo Harmonic and PX4 SITL)
  • MicroXRCEAgent udp4 -p 8888 (DDS agent to publish/subscriber PX4 SITL topics)
  • ros2 launch precision_land precision_landing_system.launch.py (you custom control application?)
  • ros2 run rqt_image_view rqt_image_view (visualizer for a ROS2 video stream)
  • ./Downloads/qgroundcontrol (ground station software connecting to PX4 SITL via MAVLink)

---

In AAS, running ./sim_run.sh spawns 3 docker containers (each with its own IPs over 2 virtual docker network)

This is in principle overkill for just simulation but it's done to make HITL and deployment to Jetson much simpler, as explained in this paper

• simulation (10.42.90.100) runs (via tmuxinator) all the terminal windows in simulation.yml.erb, including:
  • Gazebo Harmonic and
  • PX4-Autopilot/build/px4_sitl_default/bin/px4 (instead of the 2 together via make px4_sitl gz_x500_gimbal)
• aircraft (10.42.90.1/10.22.90.1) runs (via tmuxinator) all the terminal windows in aircraft.yml.erb, including:
  • MicroXRCEAgent (connecting to the PX4 SITL in the simulation container)
• ground (10.42.90.101/10.22.90.101) runs (via tmuxinator) all the terminal windows in ground.yml.erb, including:
  • QGroundControl (connecting to the PX4 SITL in the simulation container)

---

In principle you would want to

• add the wave plugin in the simulation container, I'd say just before the ros2 workspace building step
  

  aerial-autonomy-stack/scripts/docker/Dockerfile.simulation

  Lines 235 to 241 in c236b55

  	# Build the ROS 2 workspace
  	COPY simulation/simulation_ws/src /aas/simulation_ws/src
  	WORKDIR /aas/simulation_ws
  	RUN rosdep update
  	RUN rosdep install --from-paths src/ --ignore-src --rosdistro humble -y
  	# Explicitly use bash, not sh, to source and build the workspace
  	RUN bash -c "source /opt/ros/humble/setup.bash && (source /aas/github_ws/install/setup.bash || true) && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release"

• modify the lines where Gazebo and PX4 are launched in the simulation container (to add the necessary environmental variables and change to the x500 gimbal model, if necessary)
  

  aerial-autonomy-stack/simulation/simulation.yml.erb

  Lines 69 to 71 in c236b55

  	GZ_SIM_RESOURCE_PATH=$GZ_SIM_RESOURCE_PATH:/aas/simulation_resources/aircraft_models:/aas/simulation_resources/simulation_worlds
  	GZ_SIM_PHYSICS_ENGINE_PATH=$GZ_SIM_PHYSICS_ENGINE_PATH:/usr/lib/x86_64-linux-gnu/gz-physics-7/engine-plugins/
  	gz sim -v2 <%= start_as_paused ? '' : '-r' %> <%= headless ? '-s' : '' %> <%= autopilot == 'px4' ? "#{world}.sdf" : "populated_ardupilot.sdf" %>

  
  

  aerial-autonomy-stack/simulation/simulation.yml.erb

  Lines 95 to 98 in c236b55

  	PX4_GZ_MODEL_POSE=<%= "#{x_offset},#{y_offset},0.75,0,0,0" %>
  	PX4_SYS_AUTOSTART=<%= autostart %> ROS_DOMAIN_ID=<%= drone_id %> <%= gnd_container ? "AAS_GROUND_IP=#{sim_subnet}.90.#{ground_id}" : "" %>
  	PX4_UXRCE_DDS_NS="Drone<%= drone_id %>" PX4_UXRCE_DDS_AG_IP=<%= "#{sim_subnet}.90.#{drone_id}" %> PX4_UXRCE_DDS_PORT=8888
  	/aas/github_apps/PX4-Autopilot/build/px4_sitl_default/bin/px4 -i <%= sitl_instance %>

• add your ros2 launch precision_land precision_landing_system.launch.py to the aircraft container (copying it and building it in Dockerfile.aircraft and launching it in aircraft.yml.erb

A big caveat, however, is that in AAS I do not use the x500 model with gimbal: I created a custom x500 model with a fixed camera sensor whose frames I acquire in gz_gst_bridge.cpp to send over GStreamer to yolo_node.py in the aircraft container
I do not re-publish the image frame in ROS2 but you can do it with a line like this one:

aerial-autonomy-stack/aircraft/aircraft_ws/src/yolo_py/yolo_py/yolo_node.py

Line 369 in c236b55

# self.image_publisher.publish(self.bridge.cv2_to_imgmsg(frame, "bgr8"))

Alternatively, you can use the x500 with gimbal model in the simulation container and re-publish its camera topic toward the aircraft container with ros_gz_bridge similarly to how I do for the lidar

aerial-autonomy-stack/simulation/simulation.yml.erb

Lines 126 to 128 in c236b55

	ROS_DOMAIN_ID=<%= drone_id %> ros2 run ros_gz_bridge parameter_bridge
	/world/<%= world %>/model/<%= model_name %>/<%= sensor_path %>model/simple_lidar/link/lidar/base_link/sensor/lidar3d/scan/points@sensor_msgs/msg/PointCloud2[gz.msgs.PointCloudPacked
	--ros-args -r /world/<%= world %>/model/<%= model_name %>/<%= sensor_path %>model/simple_lidar/link/lidar/base_link/sensor/lidar3d/scan/points:=/lidar_points

You should be able to achieve what you want to do simply by modifying:

• Dockerfile.aircraft / Dockerfile.simulation (where resources are copied and compiled)
• and aircraft.yml.erb / simulation.yml.erb (where all the terminals are spawn)

[ph3rro]

Thanks so much @JacopoPan ! I'll keep you posted on updates.

[JacopoPan]

BTW, I checked how to use the gimbal from the PX4 x500 model with AAS:

1. copy PX4-gazebo-models's folder gimbal/ inside folder aerial-autonomy-stack/simulation/simulation_resources/aircraft_models
2. replace the fixed camera

   aerial-autonomy-stack/simulation/simulation_resources/aircraft_models/x500/model.sdf.erb

   Lines 665 to 673 in c236b55

   	<include>
   	<uri>model://sensor_camera</uri>
   	<name>simple_camera</name>
   	<pose degrees="true"><%= camera_ext %></pose>
   	</include>
   	<joint name="CameraJoint" type="fixed">
   	<parent>base_link</parent>
   	<child>simple_camera::mono_cam/base_link</child>
   	</joint>

   with the gimbal (adjust the <pose> if/as needed)

    <include merge='true'>
      <uri>model://gimbal</uri>
      <pose>0 0 0.26 0 0 3.14</pose>
    </include>
    <joint name="GimbalAttachJoint" type="fixed">
      <parent>base_link</parent>
      <child>cgo3_mount_link</child>
    </joint>

3. in file aerial-autonomy-stack/simulation/simulation_resources/aircraft_models/x500/5140_gz_aas_x500, add the gimbal configuration parameters at the end of the file

# Gimbal settings
param set-default MNT_MODE_IN 4
param set-default MNT_MODE_OUT 2
param set-default MNT_RC_IN_MODE 1

param set-default MNT_MAN_ROLL 1
param set-default MNT_MAN_PITCH 2
param set-default MNT_MAN_YAW 3

param set-default MNT_RANGE_ROLL 180
param set-default MNT_RANGE_PITCH 180
param set-default MNT_RANGE_YAW 720

4. Uncomment the image Gazebo-to-ROS2 bridge

   aerial-autonomy-stack/simulation/simulation.yml.erb

   Lines 123 to 127 in 88e43b8

   	# (optional/alternative) Use ros_gz_bridge to republish the camera frames into ROS2 and use tools like $ ros2 run rqt_image_view rqt_image_view
   	# - >
   	# ROS_DOMAIN_ID=<%= drone_id %> ros2 run ros_gz_bridge parameter_bridge
   	# /world/<%= world %>/model/<%= model_name %>/<%= sensor_path %>model/simple_camera/link/mono_cam/base_link/sensor/imager/image@sensor_msgs/msg/Image[gz.msgs.Image
   	# --ros-args -r /world/<%= world %>/model/<%= model_name %>/<%= sensor_path %>model/simple_camera/link/mono_cam/base_link/sensor/imager/image:=/camera_frames

5. re-run ./sim_build.sh, then ./sim_run.sh will start a simulation with the px4 x500 with gimbal
6. in terminal QUAD 1, run ros2 run rqt_image_view rqt_image_view and select topic /camera_frames

Then, you would "only" need to add the waves plugin to the Simulation container and your own ros2 launch precision_land precision_landing_system.launch.py to the Aircraft container

[JacopoPan]

However, if you don't need the camera to be gimballed, you can simply adjust the camera parameters (resolution/orientation) in sensor_config.yaml and have the frame published by yolo_node.py uncommenting these lines and re-build:

aerial-autonomy-stack/aircraft/aircraft_ws/src/yolo_py/yolo_py/yolo_node.py

Line 48 in 89ff6d9

# self.image_publisher = self.create_publisher(Image, 'raw_frames', 10)

aerial-autonomy-stack/aircraft/aircraft_ws/src/yolo_py/yolo_py/yolo_node.py

Lines 206 to 207 in 89ff6d9

	# if not self.headless:
	# self.image_publisher.publish(self.bridge.cv2_to_imgmsg(frame, "bgr8"))

Then, in terminal QUAD 1, run ros2 run rqt_image_view rqt_image_view and select topic /raw_frames

[JacopoPan]

With PR #69 all cameras on all vehicles are now attached to a ros2-controlled 3-axis gimbal

[JacopoPan]

also fyi @ph3rro,
if you

./sim_build.sh
./sim_run.sh

in branch feat/waterworld you get to throw a PX4 drone on a rubber duck. I'll polish and merge to main with some sort of landing barge hopefully by the week.

[ph3rro]

@JacopoPan holy shit nice! I was away on a school trip, sorry for not responding earlier. Did you get the gym working completely in the wave sim already? That's crazy. I'll take a look as soon as I can (kinda swamped with APs rn)! btw, I'm pretty new to this (started on this project year and a half ago concurrently with other stuff), but here's my repo if you're interested https://github.com/ph3rro/TRIDENT
I made a ROS node that tracks an Aruco marker on the Wam-v and lands (adapted from the tracktor-beam repo). Going to add fractal markers because I tried using two markers one big and one small and it didn't work too great (partly because they're both from the same Aruco marker dictionary and I was too lazy to change it). No RL yet, but planning on implementing it very soon

[JacopoPan]

PR #72 now includes PX4/ArduPilot, 3-axis gimbal cameras, wave plugin, and floating barges with ArUco markers so it should be more or less ready to simply add custom packages to aircraft/aircraft_ws to develop autonomous landing