feat: added geopose and geopose covariance (#1) [SC-11899]

* chore: added release yml

* add covariance error based diagnostic logic

* add TwistStamped, GeoPoseStamped, and other standard messages

* grouped repetitive logic

---------

Co-authored-by: Nathan Edwards <[email protected]>

.gitignore

@@ -1,4 +1,14 @@
 #config/rover.yaml
 .vscode
+msg/BlockHeader.msg
+__pycache__
+.idea
+*.raw
+*.pt
+node_modules
+dist
+yarn-error.log
+log
 build
-msg/BlockHeader.msg
+install
+.env

CMakeLists.txt

@@ -1,8 +1,8 @@
 cmake_minimum_required(VERSION 3.5)
 project(septentrio_gnss_driver)
 
-## Compile as C++17
-add_compile_options(-std=c++17)
+## Compile as C++20
+add_compile_options(-std=c++20)
 
 if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
 message(STATUS "Setting build type to Release as none was specified.")
@@ -166,6 +166,7 @@ elseif(ament_cmake_FOUND)
   find_package(tf2_eigen REQUIRED)
   find_package(tf2_geometry_msgs REQUIRED)
   find_package(tf2_ros REQUIRED)
+  find_package(geographic_msgs REQUIRED)
 
   ## Declare  messages
   rosidl_generate_interfaces(${PROJECT_NAME}
@@ -214,6 +215,7 @@ elseif(ament_cmake_FOUND)
   tf2_eigen
   tf2_geometry_msgs
   tf2_ros
+  geographic_msgs
   )
 
   ## shared library

config/septentrio_gnss_ins.yaml

@@ -0,0 +1,204 @@
+# Configuration Settings for the Rover Rx
+
+# GNSS/INS Parameters
+
+device: tcp://10.27.1.102:28784
+
+serial:
+  baudrate: 921600
+  hw_flow_control: "off"
+
+stream_device:
+  tcp:
+    ip_server: ""
+    port: 0
+  udp:
+    ip_server: ""
+    port: 0
+    unicast_ip: ""
+
+configure_rx: true
+
+custom_commands_file: ""
+
+login:
+  user: ""
+  password: ""  
+
+osnma:
+  mode: "off"
+  ntp_server: ""
+  keep_open: true
+
+frame_id: septentrio_gnss
+
+imu_frame_id: imu
+
+poi_frame_id: base_link
+
+vsm_frame_id: vsm
+
+aux1_frame_id: aux1
+
+vehicle_frame_id: base_link
+
+local_frame_id: odom
+
+insert_local_frame: false
+
+get_spatial_config_from_tf: false
+
+lock_utm_zone: true
+
+use_ros_axis_orientation: true
+
+receiver_type: gnss
+
+multi_antenna: true
+
+datum: Default
+
+poi_to_arp:
+  delta_e: 0.0
+  delta_n: 0.0
+  delta_u: 0.0
+
+att_offset:
+  heading: 0.0
+  pitch: 0.0
+
+ant_type: "Unknown"
+ant_serial_nr: "Unknown"
+ant_aux1_type: "Unknown"
+ant_aux1_serial_nr: "Unknown"
+
+polling_period:
+  pvt: 100
+  rest: 500
+
+# time
+use_gnss_time: false
+ntp_server: true
+ptp_server_clock: false
+latency_compensation: true
+
+# RTK
+rtk_settings:
+  ntrip_1:
+    id: ""
+    caster: ""
+    caster_port: 2101
+    username: ""
+    password: ""
+    mountpoint: ""
+    version: "v2"
+    tls: false
+    fingerprint: ""
+    rtk_standard: "auto"
+    send_gga: "auto"
+    keep_open: true
+  ip_server_1:
+    id: ""
+    port: 0
+    rtk_standard: "auto"
+    send_gga: "auto"
+    keep_open: true
+  serial_1:
+    port: ""
+    baud_rate: 115200
+    rtk_standard: "auto"
+    send_gga: "auto"
+    keep_open: true
+
+publish:
+  # For both GNSS and INS Rxs  
+  auto_publish: false
+  publish_only_valid: false
+  navsatfix: false
+  gpsfix: true
+  gpgga: false
+  gprmc: false
+  gpst: false
+  measepoch: false
+  pvtcartesian: false
+  pvtgeodetic: true
+  basevectorcart: false
+  basevectorgeod: false
+  poscovcartesian: false
+  poscovgeodetic: true
+  velcovcartesian: false
+  velcovgeodetic: true
+  atteuler: true
+  attcoveuler: true
+  pose: true
+  geopose_stamped: true
+  geopose_covariance_stamped: true
+  twist: false
+  diagnostics: false
+  aimplusstatus: true
+  galauthstatus: false
+  # For GNSS Rx only
+  gpgsa: false
+  gpgsv: false
+  # For INS Rx only
+  insnavcart: false
+  insnavgeod: false
+  extsensormeas: false
+  imusetup: false
+  velsensorsetup: false
+  exteventinsnavcart: false
+  exteventinsnavgeod: false
+  imu: false
+  localization: false
+  tf: false
+  localization_ecef: false
+  tf_ecef: false
+
+
+# INS-Specific Parameters
+
+ins_spatial_config:
+  imu_orientation:
+    theta_x: 0.0
+    theta_y: 0.0
+    theta_z: 0.0
+  poi_lever_arm:
+    delta_x: 0.0
+    delta_y: 0.0
+    delta_z: 0.0
+  ant_lever_arm:
+    x: 0.0
+    y: 0.0
+    z: 0.0
+  vsm_lever_arm:
+    vsm_x: 0.0
+    vsm_y: 0.0
+    vsm_z: 0.0
+
+ins_initial_heading: auto
+
+ins_std_dev_mask:
+  att_std_dev: 5.0
+  pos_std_dev: 10.0
+
+ins_use_poi: false
+
+ins_vsm:
+  ros:
+    source: ""
+    config: [false, false, false]
+    variances_by_parameter: false
+    variances: [0.0, 0.0, 0.0]
+  ip_server:
+    id: ""
+    port: 0
+    keep_open: true
+  serial:
+    port: ""
+    baud_rate: 115200
+    keep_open: true
+
+
+# logger
+
+activate_debug_log: false

include/septentrio_gnss_driver/abstraction/typedefs.hpp

@@ -51,9 +51,18 @@
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <diagnostic_msgs/msg/diagnostic_status.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/quaternion.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
 #include <geometry_msgs/msg/twist_with_covariance.hpp>
 #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+
+#include <geographic_msgs/msg/geo_pose_with_covariance_stamped.hpp>
+#include <geographic_msgs/msg/geo_pose_stamped.hpp>
+#include <geographic_msgs/msg/geo_point.hpp>
+
 #include <gps_msgs/msg/gps_fix.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <sensor_msgs/msg/imu.hpp>
@@ -106,9 +115,16 @@ typedef rclcpp::Time TimestampRos;
 typedef diagnostic_msgs::msg::DiagnosticArray DiagnosticArrayMsg;
 typedef diagnostic_msgs::msg::DiagnosticStatus DiagnosticStatusMsg;
 typedef geometry_msgs::msg::Quaternion QuaternionMsg;
+typedef geometry_msgs::msg::PoseStamped PoseStampedMsg;
 typedef geometry_msgs::msg::PoseWithCovarianceStamped PoseWithCovarianceStampedMsg;
+typedef geometry_msgs::msg::TwistStamped TwistStampedMsg;
 typedef geometry_msgs::msg::TwistWithCovarianceStamped TwistWithCovarianceStampedMsg;
 typedef geometry_msgs::msg::TransformStamped TransformStampedMsg;
+typedef geometry_msgs::msg::Vector3 Vector3Msg;
+
+typedef geographic_msgs::msg::GeoPoseStamped GeoPoseStampedMsg;
+typedef geographic_msgs::msg::GeoPoseWithCovarianceStamped GeoPoseWithCovarianceStampedMsg;
+
 typedef gps_msgs::msg::GPSFix GpsFixMsg;
 typedef gps_msgs::msg::GPSStatus GpsStatusMsg;
 typedef sensor_msgs::msg::NavSatFix NavSatFixMsg;

include/septentrio_gnss_driver/communication/message_handler.hpp

@@ -63,6 +63,7 @@
 #include <cstddef>
 #include <map>
 #include <sstream>
+#include <array>
 // Boost includes
 #include <boost/call_traits.hpp>
 #include <boost/format.hpp>
@@ -341,12 +342,22 @@ namespace io {
          */
         void assembleGpsFix();
 
+        /**
+         * @brief "Callback" function when constructing GeoposeStamped messages
+         */
+        void assembleGeoPoseStamped();
+
         /**
          * @brief "Callback" function when constructing PoseWithCovarianceStamped
          * messages
          */
         void assemblePoseWithCovarianceStamped();
 
+        /**
+         * @brief "Callback" function when constructing GeoPoseWithCovarianceStamped
+         */
+        void assembleGeoPoseWithCovarianceStamped();
+
         /**
          * @brief "Callback" function when constructing
          * DiagnosticArrayMsg messages
@@ -441,5 +452,30 @@ namespace io {
          * epoch
          */
         Timestamp timestampSBF(uint32_t tow, uint16_t wnc) const;
+
+        /**
+         * @brief Get current covariance
+         */
+        float getCovarianceErrorLonLat();
+
+        /**
+         * @brief Check covariance
+         */
+        bool horizontalErrorOutsideThreshold();
+
+        /**
+         * @brief Get covariance data
+         */
+        std::array<double, 36> getCovarianceData();
+
+        /**
+         * @brief Assemble PoseStamped message
+         */
+        void assemblePoseStamped();
+
+        /**
+         * @brief Assemble TwistStamped message
+         */
+        void assembleTwistStamped(bool fromIns = false); 
     };
 } // namespace io

include/septentrio_gnss_driver/communication/settings.hpp

@@ -148,6 +148,8 @@ namespace device_type {
 //! Settings struct
 struct Settings
 {
+    //! +- Threshold in which an error will be thrown if the covariance is outside 
+    float covariance_threshold;
     //! Set logger level to DEBUG
     bool activate_debug_log;
     //! Device
@@ -318,6 +320,12 @@ struct Settings
     bool publish_gpsfix;
     //! Whether or not to publish the PoseWithCovarianceStampedMsg message
     bool publish_pose;
+    //! Whether or not to publish the PoseStampedMsg message
+    bool publish_pose_stamped;
+    //! Whether or not to publish the GeoPoseStampedMsg message
+    bool publish_geopose_stamped;
+    //! Whether or not to publish the GeoPoseWithCovarianceStampedMsg message
+    bool publish_geopose_covariance_stamped;
     //! Whether or not to publish the DiagnosticArrayMsg message
     bool publish_diagnostics;
     //! Whether or not to publish the ImuMsg message
@@ -328,6 +336,8 @@ struct Settings
     bool publish_localization_ecef;
     //! Whether or not to publish the TwistWithCovarianceStampedMsg message
     bool publish_twist;
+    //! Whether or not to publish the TwistStampedMsg message
+    bool publish_twist_stamped;
     //! Whether or not to publish the tf of the localization
     bool publish_tf;
     //! Whether or not to publish the tf of the localization

include/septentrio_gnss_driver/communication/settings_helpers.hpp

@@ -175,6 +175,9 @@ namespace settings {
             settings.publish_navsatfix = true;
             settings.publish_gpsfix = true;
             settings.publish_pose = true;
+            settings.publish_pose_stamped = true;
+            settings.publish_geopose_stamped = true;
+            settings.publish_geopose_covariance_stamped = true;
             settings.publish_diagnostics = true;
             settings.publish_aimplusstatus = true;
             settings.publish_galauthstatus = true;
@@ -204,6 +207,7 @@ namespace settings {
             settings.publish_localization = true;
             settings.publish_localization_ecef = true;
             settings.publish_twist = true;
+            settings.publish_twist_stamped = true;
             if (!settings.publish_tf_ecef)
                 settings.publish_tf = true;
         } else if (settings.auto_publish && settings.configure_rx)