PX4 ROS 2 Navigation Interface

:::warning Experimental At the time of writing, parts of the PX4 ROS 2 Interface Library are experimental, and hence subject to change. :::

The PX4 ROS 2 Interface Library navigation interface enables developers to send their position measurements to PX4 directly from ROS 2 applications, such as a VIO system or a map matching system. The interface provides a layer of abstraction from PX4 and the uORB messaging framework, and introduces a few sanity checks on the requested state estimation updates sent via the interface. These measurements are then fused into the EKF just as though they were internal PX4 measurements.

The library provides two classes, LocalPositionMeasurementInterface and GlobalPositionMeasurementInterface, which both expose a similar update method to provide either a local position or global position update to PX4, respectively. The update method expects a position measurement struct (LocalPositionMeasurement or GlobalPositionMeasurement) which developers can populate with their own generated position measurements.

Installation and First Test

The following steps are required to get started:

Make sure you have a working ROS 2 setup, with px4_msgs in the ROS 2 workspace.
Clone the repository into the workspace:

sh cd $ros_workspace/src git clone --recursive https://github.com/Auterion/px4-ros2-interface-lib

::: info To ensure compatibility, use the latest main branches for PX4, px4_msgs and the library. See also here. :::

Build the workspace:

sh cd .. colcon build

In a different shell, start PX4 SITL:

sh cd $px4-autopilot make px4_sitl gazebo-classic

(here we use Gazebo-Classic, but you can use any model or simulator)

In yet a different shell, run the micro XRCE agent (you can keep it running afterward):

sh MicroXRCEAgent udp4 -p 8888

Back in the ROS 2 terminal, source the workspace you just built (in step 3) and run the global_navigation example, which periodically sends dummy global position updates:

sh source install/setup.bash ros2 run example_global_navigation_cpp example_global_navigation

You should get an output like this showing that the global interface is successfully sending position updates:

sh [INFO] [1702030701.836897756] [example_global_navigation_node]: example_global_navigation_node running! [DEBUG] [1702030702.837279784] [example_global_navigation_node]: Successfully sent position update to navigation interface. [DEBUG] [1702030703.837223884] [example_global_navigation_node]: Successfully sent position update to navigation interface.

In the PX4 shell, you can check that PX4 receives global position updates:

sh listener aux_global_position

The output should look like:

sh TOPIC: aux_global_position aux_global_position timestamp: 46916000 (0.528000 seconds ago) timestamp_sample: 46916000 (0 us before timestamp) lat: 12.343210 lon: 23.454320 alt: 12.40000 alt_ellipsoid: 0.00000 delta_alt: 0.00000 eph: 0.31623 epv: 0.44721 terrain_alt: 0.00000 lat_lon_reset_counter: 0 alt_reset_counter: 0 terrain_alt_valid: False dead_reckoning: False

Now you are ready to use the navigation interface to send your own position updates.

How to use the Library

To send a position measurement, you populate the position struct with the values you have measured. Then call the interface's update function with that struct as the argument.

For a basic example of how to use this interface, check out the examples in the Auterion/px4-ros2-interface-lib repository, such as examples/cpp/navigation/local_navigation or examples/cpp/navigation/global_navigation.

Local Position Updates

First ensure that the PX4 parameter EKF2_EV_CTRL is properly configured to fuse external local measurements, by setting the appropriate bits to true:

0: Horizontal position data
1: Vertical position data
2: Velocity data
3: Yaw data

To send a local position measurement to PX4:

Create a LocalPositionMeasurementInterface instance by providing it with: a ROS node, and the pose and velocity reference frames of your measurements.
Populate a LocalPositionMeasurement struct with your measurements.
Pass the struct to the LocalPositionMeasurementInterface update() method.

The available pose and velocity reference frames for your measurements are defined by the following enum:

```cpp enum class PoseFrame { Unknown, LocalNED, LocalFRD };

enum class VelocityFrame { Unknown, LocalNED, LocalFRD, BodyFRD }; ```

The LocalPositionMeasurement struct is defined as follows:

```cpp struct LocalPositionMeasurement { rclcpp::Time timestamp_sample {};

std::optional position_xy {std::nullopt}; std::optional position_xy_variance {std::nullopt}; std::optional position_z {std::nullopt}; std::optional position_z_variance {std::nullopt};

std::optional velocity_xy {std::nullopt}; std::optional velocity_xy_variance {std::nullopt}; std::optional velocity_z {std::nullopt}; std::optional velocity_z_variance {std::nullopt};

std::optional attitude_quaternion {std::nullopt}; std::optional attitude_variance {std::nullopt}; }; ```

The update() method of the local interface expects the following conditions to hold for LocalPositionMeasurement:

The sample timestamp is defined.
Values do not have a `NAN``.
If a measurement value is provided, its associated variance value is well defined (e.g. if position_xy is defined, then position_xy_variance must be defined).
If a measurement value is provided, its associated reference frame is not unknown (e.g. if position_xy is defined, then the interface was initialised with a pose frame different from PoseFrame::Unknown).

The following code snippet is an example of a ROS 2 node which uses the local navigation interface to send 3D pose updates in the North-East-Down (NED) reference frame to PX4:

```cpp class MyLocalMeasurementUpdateNode : public rclcpp::Node { public: MyLocalMeasurementUpdateNode() : Node("my_node_name") { // Set pose measurement reference frame to north-east-down const px4_ros2::PoseFrame pose_frame = px4_ros2::PoseFrame::LocalNED; // We will only send pose measurements in this example // Set velocity measurement reference frame to unknown const px4_ros2::VelocityFrame velocity_frame = px4_ros2::VelocityFrame::Unknown; // Initialize local interface [1] _local_position_measurement_interface = std::make_shared(*this, pose_frame, velocity_frame); }

void sendUpdate() { while (running) { // Potentially make method run as a callback or on a timer // Generate local position measurement rclcpp::Time timestamp_sample = ... Eigen::Vector2f position_xy = ... Eigen::Vector2f position_xy_variance = ... float position_z = ... float position_z_variance = ...

     // Populate the local position measurement struct [2]
     px4_ros2::LocalPositionMeasurement local_position_measurement{};
     local_position_measurement.timestamp_sample = timestamp_sample;
     local_position_measurement.position_xy = position_xy;
     local_position_measurement.position_xy_variance = position_xy_variance;
     local_position_measurement.position_z = position_z;
     local_position_measurement.position_z_variance = position_z_variance;

     // Send measurement to PX4 using the interface [3]
     try {
        _local_position_measurement_interface->update(local_position_measurement);
     } catch (const px4_ros2::NavigationInterfaceInvalidArgument & e) {
        // Handle exceptions caused by invalid local_position_measurement definition
        RCLCPP_ERROR(get_logger(), "Exception caught: %s", e.what());
     }
  }

}

private: std::shared_ptr _local_position_measurement_interface; }; ```

Global Position Updates

First ensure that the PX4 parameter EKF2_AGP_CTRL is properly configured to fuse external global measurements, by setting the appropriate bits to true:

0: Horizontal position data
1: Vertical position data

To send a global position measurement to PX4:

Create a GlobalPositionMeasurementInterface instance by providing it with a ROS node.
Populate a GlobalPositionMeasurement struct with your measurements.
Pass the struct to the GlobalPositionMeasurementInterface update() method.

The GlobalPositionMeasurement struct is defined as follows:

```cpp struct GlobalPositionMeasurement { rclcpp::Time timestamp_sample {};

std::optional lat_lon {std::nullopt}; std::optional horizontal_variance {std::nullopt};

std::optional altitude_msl {std::nullopt}; std::optional vertical_variance {std::nullopt}; }; ```

The update() method of the global interface expects the following conditions to hold for GlobalPositionMeasurement:

The sample timestamp_sample is defined.
Values do not have a NAN.
If a measurement value is provided, its associated variance value is well defined (e.g. if lat_lon is defined, then horizontal_variance must be defined).

The following code snippet is an example of a ROS 2 node which uses the global navigation interface to send a measurement with latitude, longitude and altitude to PX4:

```cpp class MyGlobalMeasurementUpdateNode : public rclcpp::Node { public: MyGlobalMeasurementUpdateNode() : Node("my_node_name") { // Initialize global interface [1] _global_position_measurement_interface = std::make_shared(*this); }

void sendUpdate() { while (running) { // Potentially make method run as a callback or on a timer // Generate global position measurement rclcpp::Time timestamp_sample = ... Eigen::Vector2d lat_lon = ... float horizontal_variance = ... float altitude_msl = ... float vertical_variance = ...

     // Populate the global position measurement struct [2]
     px4_ros2::GlobalPositionMeasurement global_position_measurement{};
     global_position_measurement.timestamp_sample = timestamp_sample;
     global_position_measurement.lat_lon = lat_lon;
     global_position_measurement.horizontal_variance = horizontal_variance;
     global_position_measurement.altitude_msl = altitude_msl;
     global_position_measurement.vertical_variance = vertical_variance;

     // Send measurement to PX4 using the interface [3]
     try {
        _global_position_measurement_interface->update(global_position_measurement);
     } catch (const px4_ros2::NavigationInterfaceInvalidArgument & e) {
        // Handle exceptions caused by invalid global_position_measurement definition
        RCLCPP_ERROR(get_logger(), "Exception caught: %s", e.what());
     }
  }

}

private: std::shared_ptr _global_position_measurement_interface; }; ```

Multiple Instances of an Interface

Using multiple instances of the same interface (e.g. local and local) to send estimation updates will stream all update messages to the same topic and result in cross-talk. This should not affect measurement fusion into the EKF, but different measurement sources will become indistinguishable.

Px4 Ros2 Navigation Interface