Event functionality

config/events/example.yml

@@ -0,0 +1,97 @@
+# This is an example file showing how to configure different types of events on sensors that support them.
+# For more in-depth documentation on events and the concepts that drive them, check out the documentation:
+#   Triggers: https://files.microstrain.com/CV7+Online/external_content/dcp/Commands/3dm_command/data/mip_cmd_3dm_event_trigger.htm
+#   Actions: https://files.microstrain.com/CV7+Online/external_content/dcp/Commands/3dm_command/data/mip_cmd_3dm_event_action.htm
+events:
+  # Triggers are the things that drive events. Each action must be associated with exactly one trigger.
+  # Each of the below objects contains an array so that multiple triggers of the same type may be configured.
+  #   NOTE: There are a limited number of triggers available per device. See device specific documentation for more information
+  triggers:
+    # GPIO triggers will be triggered on the state of a GPIO pin.
+    #   instance - Unique identifier of the trigger
+    #   pin - GPIO pin number that will trigger this event. See https://files.microstrain.com/CV7+Online/user_manual_content/additional_features/User%20GPIO.htm for more information on available pins
+    #   mode - How the pin state affects the trigger. Options are listed below
+    #     0 - DISABLED: The pin will have no effect and the trigger will never activate
+    #     1 - WHILE_HIGH: The trigger will be active when the pin is high
+    #     2 - WHILE_LOW: The trigger will be active while the pin is low
+    #     4 - EDGE: Use if the pin is configured for timestamping via https://files.microstrain.com/CV7+Online/external_content/dcp/Commands/3dm_command/data/mip_cmd_3dm_gpio_config.htm
+    gpio:
+      # This will trigger when GPIO pin 1 is high
+      - instance: 1
+        pin: 1
+        mode: 1
+    # Threshold triggers will compare supported MIP fields meet a comparison criteria.
+    # These triggers get quite complex. For more in-depth documentation, see https://files.microstrain.com/CV7+Online/external_content/dcp/Commands/3dm_command/data/mip_cmd_3dm_event_trigger.htm
+    #   instance - Unique identifier of the trigger
+    #   descriptor_set - Descriptor set of the data quantity that we want to compare
+    #   field_descriptor - Field descriptor of the data quantity that we want to compare
+    #   param_id - 1-based index of the data quantity that we want to compare. E.g. for Scaled Accel (0x80,0x04) a value of 2 would represent the Y axis.
+    #   type - Determines the type of comparison. Options are listed below
+    #     1 - WINDOW: Trigger is active if low_threshold <= value <= high_threshold. If the thresholds are reversed, the trigger is active when value < high_thres or value > low_thres.
+    #     2 - INTERVAL: Trigger at evenly-spaced intervals. Normally used with time fields to trigger periodically.
+    #   low_threshold - Only used when type is 1. Lower bounds for when the trigger will be active.
+    #   high_threshold - Only used when type is 1. Upper bounds for when the trigger will be active.
+    #   interval_threshold - Only used when type is 2.
+    #   interval - Only used when type is 2.
+    threshold:
+      # This will trigger when the Y value of Scaled Accel (0x80, 0x04) is between 5 and 10 m/s^2
+      - instance: 2
+        descriptor_set: 0x80
+        field_descriptor: 0x04
+        param_id: 2
+        type: 1
+        low_threshold: 5.0
+        high_threshold: 10.0
+      # This will trigger when the X value of scaled accel (0x80, 0x04) is TODO: I have no idea what this does
+      - instance: 3
+        descriptor_set: 0x80
+        field_descriptor: 0x04
+        param_id: 1
+        type: 2
+        interval_threshold: 1.0
+        interval: 1.0
+    # Combination triggers will combine two to four of any of the above trigger types or a fellow combination trigger into a single trigger.
+    # These triggers are even more complex than thresholds. See https://files.microstrain.com/CV7+Online/user_manual_content/additional_features/Event%20System/Event%20Components/Event%20Trigger%20Combination.htm
+    #   instance - Unique identifier of the trigger
+    #   logic_table - The last column of a truth table describing the output given the state of each input (look at the link above, this is too much to document here)
+    #   input_triggers - List of input trigger instances
+    combination:
+      # This will trigger when both of the threshold triggers above are active
+      - instance: 4
+        logic_table: 0x8888
+        input_triggers: [2, 3]
+
+  # Actions are the things that happen when an event is triggered. Each action must be associated with exactly one trigger
+  # Each of the below objects contains an array so that multiple actions of the same type may be configured.
+  #   NOTE: There are a limited number of triggers available per device. See device specific documentation for more information
+  actions:
+    # GPIO actions will control one of the GPIO pins on the device
+    #   instance - Unique identifier for the action
+    #   trigger_instance - ID (instance) of the trigger you want to associate with this action.
+    #   pin - GPIO pin number that will be modified by this action. See https://files.microstrain.com/CV7+Online/user_manual_content/additional_features/User%20GPIO.htm for more information on available pins
+    #   mode - Behavior of the pin. Options are listed below
+    #     0 - DISABLED: Pin state will not be changed
+    #     1 - ACTIVE_HIGH: Pin will be set high when the trigger is active, and low otherwise
+    #     2 - ACTIVE_LOW: Pin will be set low when the trigger is active, and high otherwise
+    #     5 - ONESHOT_HIGH: Pin will be set high each time the trigger activates. It will not be set low
+    #     6 - ONESHOT_LOW: Pin will be set low each time the trigger activates. It will not be set high
+    #     7 - TOGGLE: Pin will change to the opposite state each time the trigger activates
+    gpio:
+      # This action will toggle GPIO pin 2. It is connected to trigger 1, so this event will toggle pin 2 whenever pin 1 goes high
+      - instance: 1
+        trigger_instance: 1
+        pin: 2
+        mode: 7
+    # Message actions will stream up to 12 data quantities when triggered.
+    #   instance - Unique identifier for the action
+    #   trigger_instance - ID (instance) of the trigger you want to associate with this action.
+    #   descriptor_set - Descriptor set of the data that will be produced by this event
+    #   hertz - Hertz to stream the data at while the event is active. If set to 0, this message will just be published once when the event activates
+    #   descriptors - Array of field descriptors to stream when the event becomes active
+    message:
+      # This action will stream Delta Theta (0x80, 0x07), Delta Velocity (0x80, 0x08), Complementary Quaternion (0x80, 0x0A) one time when the event occurs.
+      - instance: 2
+        trigger_instance: 2
+        descriptor_set: 0x80
+        hertz: 0
+        descriptors: [0x07, 0x08, 0x0A]

config/params.yml

@@ -474,6 +474,10 @@ mip_filter_gnss_dual_antenna_status_data_rate        : 0  # Rate of mip/filter/g
 mip_filter_aiding_measurement_summary_data_rate      : 0  # Rate of mip/filter/aiding_measurement_summary topic
 
 
+# Event Configuration (CV7, GV7, CV7-INS, GV7-INS only)
+events_config: false
+events_config_file: "/path/to/events.yml"
+
 # ****************************************************************** 
 # NMEA streaming settings
 # ****************************************************************** 

include/microstrain_inertial_driver_common/utils/events_yaml.h

@@ -0,0 +1,41 @@
+#ifndef MICROSTRAIN_INERTIAL_DRIVER_COMMON_UTILS_EVENTS_YAML_H
+#define MICROSTRAIN_INERTIAL_DRIVER_COMMON_UTILS_EVENTS_YAML_H
+
+#include "microstrain_inertial_driver_common/utils/ros_compat.h"
+#include "microstrain_inertial_driver_common/utils/mip/ros_mip_device_main.h"
+
+#include <yaml-cpp/yaml.h>
+
+
+namespace microstrain
+{
+
+class EventsYaml
+{
+ public:
+  EventsYaml(RosNodeType* node);
+
+  bool parseAndWriteEventConfig(std::shared_ptr<RosMipDeviceMain>& mip_device, const std::string& events_yaml_file_path);
+ protected:
+  bool parseAndWriteEventTriggerConfig(std::shared_ptr<RosMipDeviceMain>& mip_device, const YAML::Node& triggers_yaml);
+  bool parseAndWriteEventActionConfig(std::shared_ptr<RosMipDeviceMain>& mip_device, const YAML::Node& actions_yaml);
+
+  bool parseEventGpioTriggerConfig(const YAML::Node& gpio_triggers_yaml, std::vector<mip::commands_3dm::EventTrigger>* gpio_event_triggers);
+  bool parseEventThresholdTriggerConfig(const YAML::Node& threshold_triggers_yaml, std::vector<mip::commands_3dm::EventTrigger>* threshold_event_triggers);
+  bool parseEventCombinationTriggerConfig(const YAML::Node& combination_triggers_yaml, std::vector<mip::commands_3dm::EventTrigger>* combination_event_triggers);
+
+  bool parseEventGpioActionConfig(const YAML::Node& gpio_actions_yaml, std::vector<mip::commands_3dm::EventAction>* gpio_event_actions);
+  bool parseEventMessageActionConfig(std::shared_ptr<RosMipDeviceMain>& mip_device, const YAML::Node& message_actions_yaml, std::vector<mip::commands_3dm::EventAction>* message_event_actions);
+
+  template<typename T>
+  T getRequiredKeyFromYaml(const YAML::Node& node, const std::string& key);
+
+  void printEventTrigger(const mip::commands_3dm::EventTrigger& trigger);
+  void printEventAction(const mip::commands_3dm::EventAction& action);
+
+  RosNodeType* node_;
+};
+
+}
+
+#endif  // MICROSTRAIN_INERTIAL_DRIVER_COMMON_UTILS_EVENTS_YAML_H

include/microstrain_inertial_driver_common/utils/mappings/mip_publisher_mapping.h

@@ -74,8 +74,8 @@ static constexpr auto MIP_FILTER_AIDING_MEASUREMENT_SUMMARY_TOPIC = "mip/ekf/aid
 static constexpr auto NMEA_SENTENCE_TOPIC = "nmea";
 
 // Some other constants
-static constexpr float FIELD_DATA_RATE_USE_DATA_CLASS = -1;
-static constexpr float DATA_CLASS_DATA_RATE_DO_NOT_STREAM = 0;
+static constexpr float DATA_RATE_DO_NOT_STREAM = -1;  // If the data rate is set to do not stream, we will create the publisher, but not stream any data. Useful for event configuration
+static constexpr float DATA_RATE_OFF = 0;  // If the data rate is set to off, we will not stream or create a publisher
 
 /**
  * Container for both a descriptor set and field descriptor
@@ -93,7 +93,7 @@ struct MipPublisherMappingInfo
 {
   std::vector<uint8_t> descriptor_sets = {};  /// Descriptor sets used by this topic
   std::vector<MipDescriptor> descriptors = {};  /// Descriptors streamed by this topic
-  float data_rate = DATA_CLASS_DATA_RATE_DO_NOT_STREAM;  /// Data rate that this topic is streamed at
+  float data_rate = DATA_RATE_OFF;  /// Data rate that this topic is streamed at
 };
 
 /**

include/microstrain_inertial_driver_common/utils/ros_compat.h

@@ -300,6 +300,15 @@ using ParamIntVector = std::vector<int32_t>;
 
 // ROS1 functions
 
+/**
+ * \brief Checks whether ROS is currently running
+ * \return Whether ROS is currently running
+*/
+inline bool rosOk()
+{
+  return ros::ok();
+}
+
 /**
  * \brief Gets the current ROS time
  * \param node  Unused in this function as the ros time function is static
@@ -610,6 +619,15 @@ using ParamIntVector = std::vector<int64_t>;
 
 // ROS2 functions
 
+/**
+ * \brief Checks whether ROS is currently running
+ * \return Whether ROS is currently running
+*/
+inline bool rosOk()
+{
+  return rclcpp::ok();
+}
+
 /**
  * \brief Gets the current ROS time
  * \param node  Unused in this function as the ros time function is static

src/config.cpp

@@ -28,6 +28,7 @@
 #include "mip/extras/recording_connection.hpp"
 
 #include "microstrain_inertial_driver_common/utils/mappings/mip_mapping.h"
+#include "microstrain_inertial_driver_common/utils/events_yaml.h"
 #include "microstrain_inertial_driver_common/config.h"
 
 namespace microstrain
@@ -188,7 +189,7 @@ bool Config::configure(RosNodeType* node)
   {
     MICROSTRAIN_WARN(node_, "No relative position configured. We will not publish relative odometry or transforms.");
     MICROSTRAIN_WARN(node_, "  Please configure relative position to publish relative position data");
-    setParam<float>(node, mip_publisher_mapping_->static_topic_to_data_rate_config_key_mapping_.at(FILTER_ODOMETRY_MAP_TOPIC).c_str(), DATA_CLASS_DATA_RATE_DO_NOT_STREAM);
+    setParam<float>(node, mip_publisher_mapping_->static_topic_to_data_rate_config_key_mapping_.at(FILTER_ODOMETRY_MAP_TOPIC).c_str(), DATA_RATE_OFF);
   }
 
   // Connect to the device and set it up if we were asked to
@@ -313,13 +314,17 @@ bool Config::configure3DM(RosNodeType* node)
 {
   // Read local config
   bool gpio_config;
+  bool events_config;
+  std::string events_config_file;
   bool nmea_message_config;
   int filter_pps_source;
   float hardware_odometer_scaling;
   float hardware_odometer_uncertainty;
   bool sbas_enable, sbas_enable_ranging, sbas_enable_corrections, sbas_apply_integrity;
   std::vector<uint16_t> sbas_prns;
   getParam<bool>(node, "gpio_config", gpio_config, false);
+  getParam<bool>(node, "events_config", events_config, false);
+  getParam<std::string>(node, "events_config_file", events_config_file, "");
   getParam<bool>(node, "nmea_message_config", nmea_message_config, false);
   getParam<int32_t>(node, "filter_pps_source", filter_pps_source, 1);
   getParam<float>(node, "odometer_scaling", hardware_odometer_scaling, 0.0);
@@ -476,6 +481,28 @@ bool Config::configure3DM(RosNodeType* node)
     MICROSTRAIN_INFO(node_, "Note: The device does not support the SBAS settings command");
   }
 
+  // Let the Event Yaml file parser handle the event yaml configuration
+  if (mip_device_->supportsDescriptor(descriptor_set, mip::commands_3dm::CMD_EVENT_ACTION_CONFIG) && mip_device_->supportsDescriptor(descriptor_set, mip::commands_3dm::CMD_EVENT_TRIGGER_CONFIG))
+  {
+    if (events_config)
+    {
+      EventsYaml events_yaml(node_);
+      if (!events_yaml.parseAndWriteEventConfig(mip_device_, events_config_file))
+      {
+        MICROSTRAIN_ERROR(node_, "Failed to configure events");
+        return false;
+      }
+    }
+    else
+    {
+      MICROSTRAIN_INFO(node_, "Not configuring events because 'events_config' is false");
+    }
+  }
+  else
+  {
+    MICROSTRAIN_INFO(node_, "Note: The device does not support the event configuration commands");
+  }
+
   // NMEA Message format
   if (mip_device_->supportsDescriptor(descriptor_set, mip::commands_3dm::CMD_NMEA_MESSAGE_FORMAT))
   {
@@ -688,6 +715,8 @@ bool Config::configureFilter(RosNodeType* node)
       while (!transform_buffer_->canTransform(frame_id_, gnss_frame_id_[i], frame_time, RosDurationType(seconds_to_wait, 0), &tf_error_string))
       {
         MICROSTRAIN_WARN(node_, "Timed out waiting for transform from %s to %s, tf error: %s", frame_id_.c_str(), gnss_frame_id_[i].c_str(), tf_error_string.c_str());
+        if (!rosOk())
+          return false;
       }
 
       // If not using the enu frame, this can be plugged directly into the device, otherwise rotate it from the ROS body frame to our body frame

src/node_common.cpp

@@ -47,17 +47,17 @@ void NodeCommon::parseAndPublishMain()
   // This should receive all packets, populate ROS messages and publish them as well
   if (!config_.mip_device_->device().update())
   {
-    MICROSTRAIN_ERROR(node_, "Unable to update device");
+    MICROSTRAIN_ERROR(node_, "Unable to update main port");
 
     // Attempt a reconnect
     bool reconnected = false;
     int reconnect_attempt = 0;
-    while (reconnect_attempt++ < config_.reconnect_attempts_)
+    while (reconnect_attempt++ < config_.reconnect_attempts_ && rosOk())
     {
-      MICROSTRAIN_WARN(node_, "Reconnect attempt %d...", reconnect_attempt);
+      MICROSTRAIN_WARN(node_, "Main port reconnect attempt %d...", reconnect_attempt);
       if (config_.mip_device_->reconnect())
       {
-        MICROSTRAIN_INFO(node_, "Successfully reconnected to the device");
+        MICROSTRAIN_INFO(node_, "Successfully reconnected to the main port");
         if (config_.configure_after_reconnect_)
         {
           MICROSTRAIN_INFO(node_, "Reconfiguring device...");
@@ -83,7 +83,7 @@ void NodeCommon::parseAndPublishMain()
 
     if (!reconnected)
     {
-      throw std::runtime_error("Device disconnected");
+      throw std::runtime_error("Main port disconnected");
     }
   }
 
@@ -106,7 +106,34 @@ void NodeCommon::parseAndPublishMain()
 void NodeCommon::parseAndPublishAux()
 {
   // This should receive all packets and populate NMEA messages
-  config_.aux_device_->device().update();
+  if (!config_.aux_device_->device().update())
+  {
+    MICROSTRAIN_ERROR(node_, "Unable to update aux port");
+
+    // Attempt a reconnect
+    bool reconnected = false;
+    int reconnect_attempt = 0;
+    while (reconnect_attempt++ < config_.reconnect_attempts_ && rosOk())
+    {
+      MICROSTRAIN_WARN(node_, "Aux port reconnect attempt %d...", reconnect_attempt);
+      if (config_.aux_device_->reconnect())
+      {
+        MICROSTRAIN_INFO(node_, "Successfully reconnected to the aux port");
+
+        // Reconnected
+        reconnected = true;
+        break;
+      }
+
+      // Wait between attempts
+      std::this_thread::sleep_for(std::chrono::seconds(5));
+    }
+
+    if (!reconnected)
+    {
+      throw std::runtime_error("Device disconnected");
+    }
+  }
 
   // Publish the NMEA messages
   if (config_.aux_device_->shouldParseNmea())

src/publishers.cpp

@@ -1549,9 +1549,9 @@ void Publishers::handleFilterVelocityNedUncertainty(const mip::data_filter::Velo
   //       Since all it does is negate the y and z axis, but that gets squared anyways.
   // NOTE: We view the NED frame and microstrain vehicle frame as the same here since there is no transform between them.
   auto filter_odometry_map_msg = filter_odometry_map_pub_->getMessage();
-  filter_odometry_map_msg->twist.covariance[21] = pow(velocity_ned_uncertainty.north, 2);
-  filter_odometry_map_msg->twist.covariance[28] = pow(velocity_ned_uncertainty.east, 2);
-  filter_odometry_map_msg->twist.covariance[35] = pow(velocity_ned_uncertainty.down, 2);
+  filter_odometry_map_msg->twist.covariance[0] = pow(velocity_ned_uncertainty.north, 2);
+  filter_odometry_map_msg->twist.covariance[7] = pow(velocity_ned_uncertainty.east, 2);
+  filter_odometry_map_msg->twist.covariance[14] = pow(velocity_ned_uncertainty.down, 2);
 
   // Filter odometry message (not counted as updating)
   auto filter_odometry_earth_msg = filter_odometry_earth_pub_->getMessageToUpdate();