ros2: update services and launch files to enable PX4 example

This PR updates services and launch files in the terrain planner node and rviz plugin to allow the ROS 2 port to run a PX4 example.

Figure: planned paths and the planning tree are now visible ros2_terrain_planner_nav_1

Details

Further updates to the rviz plugin to prevent race conditions
Update the pose visualisation code to display the aircraft mesh correctly
Add launch file for ArduPilot SITL using the DDS / ROS 2 packages
Update the rviz config, add more detailed names and amend the QoS for some topics

Issues

[x] The interactive markers are flakey and will sometimes not initialise properly. This prevents the start pose and goal pose from being set correctly, so the desired start and goal markers are not being passed to the corresponding terrain planning services. Update fixed in https://github.com/srmainwaring/terrain-navigation/pull/2

Other changes

Provides a launch example that uses the ArduPilot DDS launch scripts to start a SITL session.
There are dependencies on ArduPilot that we probably do not want in this repo, however having an integrated example is helpful while completing the last parts of the port.

GeographicLib

If you do not have the geoids for GeographicLib install the following:

$ sudo apt install geographiclib-tools
$ sudo geographiclib-get-geoids all

Usage PX4

The testing uses this branch of PX4 : https://github.com/srmainwaring/PX4-AutoPilot/tree/prs/pr-hinwil-testing-rebased as it contains a small fix to the height rate setpoint forward ported to main, and a DEM for Gazebo of the region used in the terrain planner example.

Start QGC:

Start a PX4 SITL session with Gazebo:

$ cd ~/PX4-Autopilot
$ make px4_sitl gz_standard_vtol

In the Gazebo Entity Tree view, right click the standard_vtol_0 and select 'Move To' to centre the camera on the plane.

Load the example mission: terrain_navigation_ros/config/davosdorf_mission.plan

Slide the bar to start the mission and enter a loiter over the park near the lake.

Figure: Gazebo and QGC in loiter px4_terrain_gz_start px4_terrain_qgc_start

Launch the terrain planner, mavproxy and rviz nodes in separate terminals. These can be combined but it helps with debugging to run them separately:

ros2 launch terrain_navigation_ros terrain_planner.launch.py rviz:=false

ros2 launch terrain_navigation_ros mavros.launch.py

ros2 launch terrain_navigation_ros rviz.launch.py

Set the start and goal positions. If the interactive marker server is not updating correctly this may done using service calls:

$ ros2 service call /terrain_planner/set_start planner_msgs/srv/SetVector3 "{vector: {x: 1570, y: -330, z: -1}}"
$ ros2 service call /terrain_planner/set_goal planner_msgs/srv/SetVector3 "{vector: {x: -100, y: -200, z: -1}}"

Trigger the planner and set the planner to navigate. The equivalent ROS 2 service calls are:

$ ros2 service call /terrain_planner/trigger_planning planner_msgs/srv/SetVector3 "{vector: {z: 10.0}}"
$ ros2 service call /terrain_planner/set_planner_state planner_msgs/srv/SetPlannerState "{state: 2}"

Figure: rviz after planning px4_terrain_rviz_plan

Finally engage the planner by switching the plane to OFFBOARD mode. Using mavros directly this is:

ros2 service call /mavros/set_mode mavros_msgs/srv/SetMode "{custom_mode: OFFBOARD}"

Figure: Gazebo and QGC at the goal px4_terrain_gz_goal px4_terrain_qgc_goal

Usage ArduPilot (partial support)

There is work in progress to support ArduPilot, however the ArduPilot plane navigation code does not currently support the an external control mode that works with the global position set points emitted by the planner

Dependencies

There is currently a dependency on the launch scripts for ArduPilot DDS. These should be installed following the instructions here: https://github.com/ArduPilot/ardupilot/blob/master/Tools/ros2/README.md.

The ArduPilot dependencies may be installed in the same workspace as terrain navigation or a separate one. If the latter the ArduPilot workspace should be sourced in addition to the terrain navigation workspace when running examples.

Running

Launch files for each node have been provided to aid debugging. These need to be started in separate terminals.

Launch the ardupilot_sitl node:

ros2 launch terrain_navigation_ros ardupilot.launch.py

Launch the terrain planner node:

ros2 launch terrain_navigation_ros terrain_planner.launch.py rviz:=false

Launch the mavros node:

ros2 launch terrain_navigation_ros mavros.launch.py

Launch rviz:

ros2 launch terrain_navigation_ros rviz.launch.py

Start an interactive mavproxy session:

mavproxy.py --master udp:127.0.0.1:14550 --console --map

Use the mavproxy mission editor to load the example mission from ./src/terrain_navigation_ros/config/davosdorf_mission.txt. The mission comprises a VTOL takeoff, a waypoint and an unlimited loiter.

In mavproxy, switch to AUTO and arm:

FBWA> auto
AUTO> arm throttle

Wait for the map to load in rviz. If it does not appear use the Load Terrain button. The console should report details such as:

[INFO] [1701179406.588577460] [terrain_planner]: resource_path: /Users/rhys/Code/ros2/humble/ros2-aerial/install/terrain_navigation_ros/share/terrain_navigation_ros/resources
[INFO] [1701179406.588670377] [terrain_planner]: map_path_: /Users/rhys/Code/ros2/humble/ros2-aerial/install/terrain_navigation_ros/share/terrain_navigation_ros/resources/davosdorf.tif
[INFO] [1701179406.588680169] [terrain_planner]: map_color_path_: /Users/rhys/Code/ros2/humble/ros2-aerial/install/terrain_navigation_ros/share/terrain_navigation_ros/resources/davosdorf_color.tif
[INFO] [1701179406.588688002] [terrain_planner]: mesh_resource_path_: /Users/rhys/Code/ros2/humble/ros2-aerial/install/terrain_navigation_ros/share/terrain_navigation_ros/resources/believer.dae
[INFO] [1701179406.588694877] [terrain_planner]: avalanche_map_path: /Users/rhys/Code/ros2/humble/ros2-aerial/install/terrain_navigation_ros/share/terrain_navigation_ros/resources/avalanche.tif
[INFO] [1701179406.588709752] [terrain_planner]: goal_radius_: 66.67
Requesting global origin messages
[TerrainPlanner] Received Global Origin from FMU
[TerrainPlanner] Local origin received, loading map

Loading GeoTIFF file for gridmap

Wkt ProjectionRef: PROJCS["CH1903 / LV03",GEOGCS["CH1903",DATUM["CH1903",SPHEROID["Bessel 1841",6377397.155,299.1528128,AUTHORITY["EPSG","7004"]],AUTHORITY["EPSG","6149"]],PRIMEM["Greenwich",0,AUTHORITY["EPSG","8901"]],UNIT["degree",0.0174532925199433,AUTHORITY["EPSG","9122"]],AUTHORITY["EPSG","4149"]],PROJECTION["Hotine_Oblique_Mercator_Azimuth_Center"],PARAMETER["latitude_of_center",46.9524055555556],PARAMETER["longitude_of_center",7.43958333333333],PARAMETER["azimuth",90],PARAMETER["rectified_grid_angle",90],PARAMETER["scale_factor",1],PARAMETER["false_easting",600000],PARAMETER["false_northing",200000],UNIT["metre",1,AUTHORITY["EPSG","9001"]],AXIS["Easting",EAST],AXIS["Northing",NORTH],AUTHORITY["EPSG","21781"]]
Width: 559 Height: 495 Resolution: 10

Loading color layer from GeoTIFF file for gridmap
Width: 559 Height: 495 Resolution: 10
[TerrainPlanner]   - Successfully loaded map: /Users/rhys/Code/ros2/humble/ros2-aerial/install/terrain_navigation_ros/share/terrain_navigation_ros/resources/davosdorf.tif
[TerrainOmplRrt] Upper bounds:    2795    2475 2963.08
[TerrainOmplRrt] Lower bounds:   -2795   -2475 1585.99
Warning: RRTstar requires a state space with symmetric distance and symmetric interpolation.
         at line 101 in /tmp/ompl-20231021-89370-hq36il/ompl-1.6.0/src/ompl/geometric/planners/rrt/src/RRTstar.cpp

In another terminal call services to set the start and goal:

$ ros2 service call /terrain_planner/set_start planner_msgs/srv/SetVector3 "{vector: {x: 1570, y: -330, z: -1}}"

A green circle near the vehicles loiter position should appear.

$ ros2 service call /terrain_planner/set_goal planner_msgs/srv/SetVector3 "{vector: {x: -100, y: -200, z: -1}}"

A green circle on the mountain near the top of the snow should appear.

The terrain planner console should show:

NAV Loiter Center
 - x_lat : 46.8146
 - y_long: 9.8491
 - alt   : 200
 - Radius: 80
mission_loiter_center_:  783936  187654 1761.47
mission_loiter_center_:  1637.97 -350.937  1761.47

Call the loiter service

ros2 service call /terrain_planner/set_start_loiter planner_msgs/srv/SetService "{}"

Put the planner in its HOLD state:

ros2 service call /terrain_planner/set_planner_state planner_msgs/srv/SetPlannerState "{state: 1}"

Switch the flight controller to GUIDED:

ros2 service call /mavros/set_mode mavros_msgs/srv/SetMode "{custom_mode: GUIDED}"

Trigger the planner (the z-component of the vector3 is the planning budget time in seconds):

ros2 service call /terrain_planner/trigger_planning planner_msgs/srv/SetVector3 "{vector: {z: 10.0}}"

Console output:

[INFO] [1701181272.559051680] [terrain_planner]: planner_time_budget: 10
Info:    RRTstar: Started planning with 10 states. Seeking a solution better than 0.00000.
Info:    RRTstar: Initial k-nearest value of 287
Info:    RRTstar: Found an initial solution with a cost of 4055.12 in 4343 iterations (117 vertices in the graph)
Info:    RRTstar: Created 824 new states. Checked 346843 rewire options. 20 goal states in tree. Final solution cost 4013.597
Info:    Solution found in 1.007225 seconds
Found Exact solution!
[INFO] [1701181274.556831676] [terrain_planner]: time_spent_planning: 2.37095
[INFO] [1701181274.556906133] [terrain_planner]: planner_time_budget: 10

Put the planner in its NAVIGATE state:

ros2 service call /terrain_planner/set_planner_state planner_msgs/srv/SetPlannerState "{state: 2}"

Verify the setpoints are being published:

$ ros2 topic echo /mavros/setpoint_raw/global --once
header:
  stamp:
    sec: 1701181676
    nanosec: 558113248
  frame_id: ''
coordinate_frame: 6
type_mask: 0
latitude: 46.815288876315954
longitude: 9.848755570268443
altitude: 82.70903778076172
velocity:
  x: 0.8028606224010265
  y: -0.596166772805929
  z: 5.0
acceleration_or_force:
  x: 0.008942054489364451
  y: 0.012042307220654344
  z: -0.0
yaw: 0.0
yaw_rate: 0.0
---

There seems to be some style changes, but the styles were formatted with clang-format-6.0 for ROS1. Could you maybe share the exact style format you are using?

Style changes are probably not intended. Some change, such as comments around unused arguments would be for clang. Line length is probably my changing from habit - working on Gazebo where there is a strict line length policy. If there is a formatter you like run I'd be happy to - @Ryanf55 do you know what is standard for ROS 2 these days?

Some of the inline implementations seems to have now gotten a cpp file. Could you maybe clarify why this is more preferred? I am fine with the changes, was just wondering why this would be better

A couple of reasons. The main one is that the header only versions lead to duplicate symbols in clang. The other reason is that inline code with branch conditions seldom gets compiled inline. Same if the functions are virtual. If the section is critical for performance then inlining may make sense, but most of the time placing the code in a translation unit is better and allows a smaller set of includes to be used in the header (which propagate) with most of the details in the cpp.

There seems to be some style changes, but the styles were formatted with clang-format-6.0 for ROS1. Could you maybe share the exact style format you are using?

Style changes are probably not intended. Some change, such as comments around unused arguments would be for clang. Line length is probably my changing from habit - working on Gazebo where there is a strict line length policy. If there is a formatter you like run I'd be happy to - @Ryanf55 do you know what is standard for ROS 2 these days?

Some of the inline implementations seems to have now gotten a cpp file. Could you maybe clarify why this is more preferred? I am fine with the changes, was just wondering why this would be better

A couple of reasons. The main one is that the header only versions lead to duplicate symbols in clang. The other reason is that inline code with branch conditions seldom gets compiled inline. Same if the functions are virtual. If the section is critical for performance then inlining may make sense, but most of the time placing the code in a translation unit is better and allows a smaller set of includes to be used in the header (which propagate) with most of the details in the cpp.

clang-format with google style guide. https://github.com/ethz-asl/grid_map_geo/pull/45

I have no perference if you want to fix formatting now or later; depends if it's going to create a huge diff or not. If the old code was formatted with version 6 or something, it would be best to preserve the old style in this PR, and then migrate versions in a follow up PR once we can enforce it in CI and ignore a single reformat commit to reduce code churn noise.

I got up to the part of launching terrain navigation but there is a problem with getting the origin. mavros log

[mavros_node-1] [WARN] [1705699705.649010553] [mavros.guided_target]: PositionTargetGlobal failed because no origin

planner log

[terrain_planner_node-1] [WARN] [1705699248.629211012] [terrain_planner]: Service [/mavros/cmd/command] not available.
[terrain_planner_node-1] Requesting global origin messages

@Jaeyoung-Lim and @Ryanf55, do you require any further changes to get this PR merged?

I've run

./Tools/fix_code_style.sh ..

on the repo, and it runs clean, although for clang-format 17.0.6 on macOS and 14.0.0-1ubuntu1.1 on Ubuntu 22.04 as these are the default versions for those platforms.

@Ryanf55 I'd prefer to get this PR merged and then apply your two changes as a follow up, as they need a rebase and this ros2 branch has been something of a WIP up until now in any case.

There seems to be some style changes, but the styles were formatted with clang-format-6.0 for ROS1. Could you maybe share the exact style format you are using?

Style changes are probably not intended. Some change, such as comments around unused arguments would be for clang. Line length is probably my changing from habit - working on Gazebo where there is a strict line length policy. If there is a formatter you like run I'd be happy to - @Ryanf55 do you know what is standard for ROS 2 these days?

Some of the inline implementations seems to have now gotten a cpp file. Could you maybe clarify why this is more preferred? I am fine with the changes, was just wondering why this would be better

A couple of reasons. The main one is that the header only versions lead to duplicate symbols in clang. The other reason is that inline code with branch conditions seldom gets compiled inline. Same if the functions are virtual. If the section is critical for performance then inlining may make sense, but most of the time placing the code in a translation unit is better and allows a smaller set of includes to be used in the header (which propagate) with most of the details in the cpp.

We can select clang-format likely - if it minimizes code churn.

Yea, feel free to merge this. I wasn't able to get it running all the way, but so far it's a big improvement.

@srmainwaring Unfortunately on myside it doesn't seem to progress after requesting for origin messages:

[terrain_planner_node-1] [INFO] [1708261393.675265756] [terrain_planner]: resource_path: /home/jaeyoung/ros2_ws/install/terrain_navigation_ros/share/terrain_navigation_ros/resources
[terrain_planner_node-1] [INFO] [1708261393.675368422] [terrain_planner]: map_path_: /home/jaeyoung/ros2_ws/install/terrain_navigation_ros/share/terrain_navigation_ros/resources/davosdorf.tif
[terrain_planner_node-1] [INFO] [1708261393.675374867] [terrain_planner]: map_color_path_: /home/jaeyoung/ros2_ws/install/terrain_navigation_ros/share/terrain_navigation_ros/resources/davosdorf_color.tif
[terrain_planner_node-1] [INFO] [1708261393.675379037] [terrain_planner]: mesh_resource_path_: /home/jaeyoung/ros2_ws/install/terrain_navigation_ros/share/terrain_navigation_ros/resources/believer.dae
[terrain_planner_node-1] [INFO] [1708261393.675382954] [terrain_planner]: avalanche_map_path: /home/jaeyoung/ros2_ws/install/terrain_navigation_ros/share/terrain_navigation_ros/resources/avalanche.tif
[terrain_planner_node-1] [INFO] [1708261393.675401365] [terrain_planner]: goal_radius_: 80
[terrain_planner_node-1] [INFO] [1708261393.675422066] [terrain_planner]: alt_control_p: 0.5
[terrain_planner_node-1] [INFO] [1708261393.675426568] [terrain_planner]: alt_control_max_climb_rate: 3
[terrain_planner_node-1] [INFO] [1708261393.675430631] [terrain_planner]: cruise_speed: 15
[terrain_planner_node-1] Requesting global origin messages

However, probably we should address this in following PRs! Thanks for all the work!

ethz-asl / terrain-navigation