Add sensor_msgs/msg/NavSatFix MessageParser (#11044)

Author: oxkitsune

crates/utils/re_mcap/src/layers/ros2.rs

@@ -1,9 +1,10 @@
-use crate::{
-    parsers::MessageParser,
-    parsers::ros2msg::{
+use crate::parsers::{
+    MessageParser,
+    ros2msg::{
         sensor_msgs::{
             CameraInfoMessageParser, CompressedImageMessageParser, ImageMessageParser,
-            ImuMessageParser, JointStateMessageParser, PointCloud2MessageParser,
+            ImuMessageParser, JointStateMessageParser, NavSatFixMessageParser,
+            PointCloud2MessageParser,
         },
         std_msgs::StringMessageParser,
     },
@@ -50,6 +51,7 @@ impl MessageLayer for McapRos2Layer {
                 Box::new(CompressedImageMessageParser::new(num_rows))
             }
             "sensor_msgs/msg/PointCloud2" => Box::new(PointCloud2MessageParser::new(num_rows)),
+            "sensor_msgs/msg/NavSatFix" => Box::new(NavSatFixMessageParser::new(num_rows)),
             _ => {
                 re_log::warn_once!(
                     "Message schema {:?} is currently not supported",

crates/utils/re_mcap/src/parsers/ros2msg/definitions/sensor_msgs.rs

@@ -290,3 +290,94 @@ pub struct JointState {
     /// The efforts applied in the joints.
     pub effort: Vec<f64>,
 }
+
+/// Navigation Satellite fix status information.
+#[derive(Debug, Serialize, Deserialize)]
+pub struct NavSatStatus {
+    /// Navigation satellite fix status.
+    pub status: NavSatFixStatus,
+
+    /// Navigation satellite service type.
+    pub service: NavSatService,
+}
+
+/// Navigation satellite fix status values.
+#[derive(Copy, Clone, Debug, Serialize, Deserialize)]
+#[repr(i8)]
+pub enum NavSatFixStatus {
+    /// Unable to fix position.
+    NoFix = -1,
+
+    /// Unaugmented fix.
+    Fix = 0,
+
+    /// Satellite-based augmentation.
+    SbasFix = 1,
+
+    /// Ground-based augmentation.
+    GbasFix = 2,
+}
+
+/// Navigation satellite service type values.
+#[derive(Copy, Clone, Debug, Serialize, Deserialize)]
+#[repr(u16)]
+pub enum NavSatService {
+    Gps = 1,
+    Glonass = 2,
+    Compass = 4,
+    Galileo = 8,
+}
+
+/// Position covariance type for navigation satellite fix.
+#[derive(Copy, Clone, Debug, Serialize, Deserialize)]
+#[repr(u8)]
+pub enum CovarianceType {
+    Unknown = 0,
+    Approximated = 1,
+    DiagonalKnown = 2,
+    Known = 3,
+}
+
+/// Navigation Satellite fix for any Global Navigation Satellite System
+///
+/// Specified using the WGS 84 reference ellipsoid.
+///
+/// `header.stamp` specifies the ROS time for this measurement (the
+/// corresponding satellite time may be reported using the
+/// `sensor_msgs/TimeReference` message).
+///
+/// `header.frame_id` is the frame of reference reported by the satellite
+/// receiver, usually the location of the antenna. This is a
+/// Euclidean frame relative to the vehicle, not a reference
+/// ellipsoid.
+#[derive(Debug, Serialize, Deserialize)]
+pub struct NavSatFix {
+    /// Metadata including timestamp and coordinate frame.
+    pub header: Header,
+
+    /// Satellite fix status information.
+    pub status: NavSatStatus,
+
+    /// Latitude (degrees). Positive is north of equator; negative is south.
+    pub latitude: f64,
+
+    /// Longitude (degrees). Positive is east of prime meridian; negative is west.
+    pub longitude: f64,
+
+    /// Altitude (m). Positive is above the WGS 84 ellipsoid
+    /// (quiet NaN if no altitude is available).
+    pub altitude: f64,
+
+    /// Position covariance (m^2) defined relative to a tangential plane
+    /// through the reported position. The components are East, North, and
+    /// Up (ENU), in row-major order.
+    ///
+    /// Beware: this coordinate system exhibits singularities at the poles.
+    pub position_covariance: [f64; 9],
+
+    /// If the covariance of the fix is known, fill it in completely. If the
+    /// GPS receiver provides the variance of each measurement, put them
+    /// along the diagonal. If only Dilution of Precision is available,
+    /// estimate an approximate covariance from that.
+    pub position_covariance_type: CovarianceType,
+}

crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/mod.rs

@@ -3,11 +3,13 @@ mod compressed_image;
 mod image;
 mod imu;
 mod joint_state;
+mod nav_sat_fix;
 mod point_cloud_2;
 
 pub use camera_info::*;
 pub use compressed_image::*;
 pub use image::*;
 pub use imu::*;
 pub use joint_state::*;
+pub use nav_sat_fix::*;
 pub use point_cloud_2::*;

crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/nav_sat_fix.rs

@@ -0,0 +1,152 @@
+use anyhow::Context as _;
+use arrow::array::{
+    FixedSizeListArray, FixedSizeListBuilder, Float64Builder, Int8Builder, UInt8Builder,
+    UInt16Builder,
+};
+use re_chunk::{Chunk, ChunkId};
+use re_log_types::TimeCell;
+use re_types::{
+    ComponentDescriptor, SerializedComponentColumn, archetypes::GeoPoints, components::LatLon,
+};
+
+use crate::parsers::{
+    cdr,
+    decode::{MessageParser, ParserContext},
+    ros2msg::definitions::sensor_msgs,
+    util::fixed_size_list_builder,
+};
+
+/// Plugin that parses `sensor_msgs/msg/NavSatFix` messages.
+#[derive(Default)]
+pub struct NavSatFixSchemaPlugin;
+
+pub struct NavSatFixMessageParser {
+    geo_points: Vec<LatLon>,
+    latitude: FixedSizeListBuilder<Float64Builder>,
+    longitude: FixedSizeListBuilder<Float64Builder>,
+    altitude: FixedSizeListBuilder<Float64Builder>,
+    status: FixedSizeListBuilder<Int8Builder>,
+    service: FixedSizeListBuilder<UInt16Builder>,
+    position_covariance: FixedSizeListBuilder<Float64Builder>,
+    position_covariance_type: FixedSizeListBuilder<UInt8Builder>,
+}
+
+impl NavSatFixMessageParser {
+    const ARCHETYPE_NAME: &str = "sensor_msgs.msg.NavSatFix";
+
+    pub fn new(num_rows: usize) -> Self {
+        Self {
+            geo_points: Vec::with_capacity(num_rows),
+            latitude: fixed_size_list_builder(1, num_rows),
+            longitude: fixed_size_list_builder(1, num_rows),
+            altitude: fixed_size_list_builder(1, num_rows),
+            status: fixed_size_list_builder(1, num_rows),
+            service: fixed_size_list_builder(1, num_rows),
+            position_covariance: fixed_size_list_builder(9, num_rows),
+            position_covariance_type: fixed_size_list_builder(1, num_rows),
+        }
+    }
+
+    fn create_metadata_column(name: &str, array: FixedSizeListArray) -> SerializedComponentColumn {
+        SerializedComponentColumn {
+            list_array: array.into(),
+            descriptor: ComponentDescriptor::partial(name)
+                .with_archetype(Self::ARCHETYPE_NAME.into()),
+        }
+    }
+}
+
+impl MessageParser for NavSatFixMessageParser {
+    fn append(&mut self, ctx: &mut ParserContext, msg: &mcap::Message<'_>) -> anyhow::Result<()> {
+        re_tracing::profile_function!();
+        let sensor_msgs::NavSatFix {
+            header,
+            status,
+            latitude,
+            longitude,
+            altitude,
+            position_covariance,
+            position_covariance_type,
+        } = cdr::try_decode_message::<sensor_msgs::NavSatFix>(&msg.data)
+            .context("Failed to decode sensor_msgs::NavSatFix message from CDR data")?;
+
+        // add the sensor timestamp to the context, `log_time` and `publish_time` are added automatically
+        ctx.add_time_cell(
+            "timestamp",
+            TimeCell::from_timestamp_nanos_since_epoch(header.stamp.as_nanos()),
+        );
+
+        // Store latitude/longitude as geographic points
+        let geo_point = LatLon::new(latitude, longitude);
+        self.geo_points.push(geo_point);
+
+        self.latitude.values().append_slice(&[latitude]);
+        self.latitude.append(true);
+
+        self.longitude.values().append_slice(&[longitude]);
+        self.longitude.append(true);
+
+        self.altitude.values().append_slice(&[altitude]);
+        self.altitude.append(true);
+
+        self.status.values().append_slice(&[status.status as i8]);
+        self.status.append(true);
+
+        self.service.values().append_slice(&[status.service as u16]);
+        self.service.append(true);
+
+        self.position_covariance
+            .values()
+            .append_slice(&position_covariance);
+        self.position_covariance.append(true);
+
+        self.position_covariance_type
+            .values()
+            .append_slice(&[position_covariance_type as u8]);
+        self.position_covariance_type.append(true);
+
+        Ok(())
+    }
+
+    fn finalize(self: Box<Self>, ctx: ParserContext) -> anyhow::Result<Vec<re_chunk::Chunk>> {
+        re_tracing::profile_function!();
+        let Self {
+            geo_points,
+            mut latitude,
+            mut longitude,
+            mut altitude,
+            mut status,
+            mut service,
+            mut position_covariance,
+            mut position_covariance_type,
+        } = *self;
+
+        let entity_path = ctx.entity_path().clone();
+        let timelines = ctx.build_timelines();
+
+        let mut chunk_components: Vec<_> = GeoPoints::update_fields()
+            .with_positions(geo_points)
+            .columns_of_unit_batches()?
+            .collect();
+
+        chunk_components.extend([
+            Self::create_metadata_column("latitude", latitude.finish()),
+            Self::create_metadata_column("longitude", longitude.finish()),
+            Self::create_metadata_column("altitude", altitude.finish()),
+            Self::create_metadata_column("status", status.finish()),
+            Self::create_metadata_column("service", service.finish()),
+            Self::create_metadata_column("position_covariance", position_covariance.finish()),
+            Self::create_metadata_column(
+                "position_covariance_type",
+                position_covariance_type.finish(),
+            ),
+        ]);
+
+        Ok(vec![Chunk::from_auto_row_ids(
+            ChunkId::new(),
+            entity_path.clone(),
+            timelines.clone(),
+            chunk_components.into_iter().collect(),
+        )?])
+    }
+}