diff --git a/config/events/example.yml b/config/events/example.yml new file mode 100644 index 0000000..c5d5908 --- /dev/null +++ b/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] \ No newline at end of file diff --git a/config/params.yml b/config/params.yml index 8aa9669..fab6a62 100644 --- a/config/params.yml +++ b/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 # ****************************************************************** diff --git a/include/microstrain_inertial_driver_common/utils/events_yaml.h b/include/microstrain_inertial_driver_common/utils/events_yaml.h new file mode 100644 index 0000000..d403391 --- /dev/null +++ b/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 + + +namespace microstrain +{ + +class EventsYaml +{ + public: + EventsYaml(RosNodeType* node); + + bool parseAndWriteEventConfig(std::shared_ptr& mip_device, const std::string& events_yaml_file_path); + protected: + bool parseAndWriteEventTriggerConfig(std::shared_ptr& mip_device, const YAML::Node& triggers_yaml); + bool parseAndWriteEventActionConfig(std::shared_ptr& mip_device, const YAML::Node& actions_yaml); + + bool parseEventGpioTriggerConfig(const YAML::Node& gpio_triggers_yaml, std::vector* gpio_event_triggers); + bool parseEventThresholdTriggerConfig(const YAML::Node& threshold_triggers_yaml, std::vector* threshold_event_triggers); + bool parseEventCombinationTriggerConfig(const YAML::Node& combination_triggers_yaml, std::vector* combination_event_triggers); + + bool parseEventGpioActionConfig(const YAML::Node& gpio_actions_yaml, std::vector* gpio_event_actions); + bool parseEventMessageActionConfig(std::shared_ptr& mip_device, const YAML::Node& message_actions_yaml, std::vector* message_event_actions); + + template + 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 \ No newline at end of file diff --git a/include/microstrain_inertial_driver_common/utils/mappings/mip_publisher_mapping.h b/include/microstrain_inertial_driver_common/utils/mappings/mip_publisher_mapping.h index b775901..b408eaa 100644 --- a/include/microstrain_inertial_driver_common/utils/mappings/mip_publisher_mapping.h +++ b/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 descriptor_sets = {}; /// Descriptor sets used by this topic std::vector 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 }; /** diff --git a/include/microstrain_inertial_driver_common/utils/ros_compat.h b/include/microstrain_inertial_driver_common/utils/ros_compat.h index e886665..c7542c7 100644 --- a/include/microstrain_inertial_driver_common/utils/ros_compat.h +++ b/include/microstrain_inertial_driver_common/utils/ros_compat.h @@ -300,6 +300,15 @@ using ParamIntVector = std::vector; // 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; // 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 diff --git a/src/config.cpp b/src/config.cpp index 44ef3b8..5d389ff 100644 --- a/src/config.cpp +++ b/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(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(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,6 +314,8 @@ 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; @@ -320,6 +323,8 @@ bool Config::configure3DM(RosNodeType* node) bool sbas_enable, sbas_enable_ranging, sbas_enable_corrections, sbas_apply_integrity; std::vector sbas_prns; getParam(node, "gpio_config", gpio_config, false); + getParam(node, "events_config", events_config, false); + getParam(node, "events_config_file", events_config_file, ""); getParam(node, "nmea_message_config", nmea_message_config, false); getParam(node, "filter_pps_source", filter_pps_source, 1); getParam(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 diff --git a/src/node_common.cpp b/src/node_common.cpp index d504294..763b28b 100644 --- a/src/node_common.cpp +++ b/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()) diff --git a/src/publishers.cpp b/src/publishers.cpp index ff537db..2a09c6d 100644 --- a/src/publishers.cpp +++ b/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(); diff --git a/src/utils/events_yaml.cpp b/src/utils/events_yaml.cpp new file mode 100644 index 0000000..b0200a4 --- /dev/null +++ b/src/utils/events_yaml.cpp @@ -0,0 +1,553 @@ +#include + +#include "microstrain_inertial_driver_common/utils/events_yaml.h" + +namespace microstrain +{ + +inline std::string getYamlTypeString(const YAML::Node& node) +{ + switch (node.Type()) + { + case YAML::NodeType::Map: + return "map"; + case YAML::NodeType::Scalar: + return "scalar"; + case YAML::NodeType::Sequence: + return "sequence"; + case YAML::NodeType::Undefined: + return "undefined"; + case YAML::NodeType::Null: + default: + return "null"; + } +} + +EventsYaml::EventsYaml(RosNodeType* node) : node_(node) +{ +} + +bool EventsYaml::parseAndWriteEventConfig(std::shared_ptr& mip_device, const std::string& events_yaml_file_path) +{ + // First thing to do is to make sure that the path we were handed can be opened and parsed + YAML::Node events_yaml; + try + { + MICROSTRAIN_INFO(node_, "Reading event config from %s", events_yaml_file_path.c_str()); + events_yaml = YAML::LoadFile(events_yaml_file_path); + } + catch (const YAML::BadFile& b) + { + MICROSTRAIN_ERROR(node_, "Unable to open events yaml file located at %s", events_yaml_file_path.c_str()); + MICROSTRAIN_ERROR(node_, " Exception: %s", b.what()); + return false; + } + catch (const YAML::ParserException& p) + { + MICROSTRAIN_ERROR(node_, "Opened, but failed to parse events yaml file located at %s", events_yaml_file_path.c_str()); + MICROSTRAIN_ERROR(node_, " Exception: %s", p.what()); + return false; + } + + // Make sure we can find the required information + if (!mip_device->supportsDescriptor(mip::commands_3dm::DESCRIPTOR_SET, mip::commands_3dm::CMD_EVENT_SUPPORT)) + { + MICROSTRAIN_ERROR(node_, "Device does not support the event 'Get Supported Events' command. We are unable to configure events"); + return false; + } + + // All upper level keys are optional, so if we can't find one, just log and move on + YAML::Node events_yaml_events; + if ((events_yaml_events = events_yaml["events"])) + { + MICROSTRAIN_DEBUG(node_, "Parsing 'events' section of events config"); + YAML::Node triggers_yaml, actions_yaml; + if ((triggers_yaml = events_yaml_events["triggers"])) + { + if (!parseAndWriteEventTriggerConfig(mip_device, triggers_yaml)) + return false; + } + else + { + MICROSTRAIN_INFO(node_, "Events yaml did not contain a 'triggers' key, so no triggers will be configured"); + } + if ((actions_yaml = events_yaml_events["actions"])) + { + if (!parseAndWriteEventActionConfig(mip_device, actions_yaml)) + return false; + } + else + { + MICROSTRAIN_INFO(node_, "Events yaml did not contain a 'actions' key, so no actions will be configured"); + } + } + else + { + MICROSTRAIN_WARN(node_, "Events yaml did not contain an 'events' key, so no events will be configured"); + } + + // Looks like everything worked + return true; +} + +bool EventsYaml::parseAndWriteEventTriggerConfig(std::shared_ptr& mip_device, const YAML::Node& triggers_yaml) +{ + // Before we parse the yaml, check that we support each type, and check how many we support + mip::CmdResult mip_cmd_result; + uint8_t max_triggers, num_entries, max_entries = 255; + mip::commands_3dm::GetEventSupport::Info entries[max_entries]; + if (!(mip_cmd_result = mip::commands_3dm::getEventSupport(*mip_device, mip::commands_3dm::GetEventSupport::Query::TRIGGER_TYPES, &max_triggers, &num_entries, max_entries, entries))) + { + MICROSTRAIN_MIP_SDK_ERROR(node_, mip_cmd_result, "Unable to determine the max number of supported triggers"); + return false; + } + MICROSTRAIN_DEBUG(node_, "The device supports up to %u triggers", max_triggers); + + // Parse the individual configuration options + MICROSTRAIN_DEBUG(node_, "Parsing 'triggers' section of events config"); + std::vector event_triggers; + YAML::Node gpio_triggers_yaml, threshold_triggers_yaml, combination_triggers_yaml; + if ((gpio_triggers_yaml = triggers_yaml["gpio"])) + { + if (!parseEventGpioTriggerConfig(gpio_triggers_yaml, &event_triggers)) + return false; + } + else + { + MICROSTRAIN_DEBUG(node_, "No 'gpio' section of 'triggers' config, not configuring any GPIO triggers"); + } + if ((threshold_triggers_yaml = triggers_yaml["threshold"])) + { + if (!parseEventThresholdTriggerConfig(threshold_triggers_yaml, &event_triggers)) + return false; + } + else + { + MICROSTRAIN_DEBUG(node_, "No 'threshold' section of 'triggers' config, not configuring any threshold triggers"); + } + if ((combination_triggers_yaml = triggers_yaml["combination"])) + { + if (!parseEventCombinationTriggerConfig(combination_triggers_yaml, &event_triggers)) + return false; + } + else + { + MICROSTRAIN_DEBUG(node_, "No 'combination' section of 'triggers' config, not configuring any combination triggers"); + } + + // Make sure that we don't have too many triggers + if (event_triggers.size() > max_triggers) + { + MICROSTRAIN_ERROR(node_, "Device only supports %u triggers, but you attempted to configure %lu triggers", max_triggers, event_triggers.size()); + return false; + } + + // Finally configure the triggers + for (auto& event_trigger : event_triggers) + { + event_trigger.function = mip::FunctionSelector::WRITE; + MICROSTRAIN_INFO(node_, "Configuring event trigger instance %u", event_trigger.instance); + printEventTrigger(event_trigger); + if (!(mip_cmd_result = mip_device->device().runCommand(event_trigger))) + { + MICROSTRAIN_MIP_SDK_ERROR(node_, mip_cmd_result, "Failed to write event trigger configuration"); + return false; + } + } + return true; +} + +bool EventsYaml::parseAndWriteEventActionConfig(std::shared_ptr& mip_device, const YAML::Node& actions_yaml) +{ + // Before we parse the yaml, check that we support each type, and check how many we support + mip::CmdResult mip_cmd_result; + uint8_t max_actions, num_entries, max_entries = 255; + mip::commands_3dm::GetEventSupport::Info entries[max_entries]; + if (!(mip_cmd_result = mip::commands_3dm::getEventSupport(*mip_device, mip::commands_3dm::GetEventSupport::Query::ACTION_TYPES, &max_actions, &num_entries, max_entries, entries))) + { + MICROSTRAIN_MIP_SDK_ERROR(node_, mip_cmd_result, "Unable to determine the max number of supported actions"); + return false; + } + MICROSTRAIN_DEBUG(node_, "The device supports up to %u actions", max_actions); + + // Parse the individual configuration sections + MICROSTRAIN_DEBUG(node_, "Parsing 'actions' section of events config"); + std::vector event_actions; + YAML::Node gpio_actions_yaml, message_actions_yaml; + if ((gpio_actions_yaml = actions_yaml["gpio"])) + { + if (!parseEventGpioActionConfig(gpio_actions_yaml, &event_actions)) + return false; + } + else + { + MICROSTRAIN_DEBUG(node_, "No 'gpio' section of 'actions' config, not configuring any GPIO actions"); + } + if ((message_actions_yaml = actions_yaml["message"])) + { + if (!parseEventMessageActionConfig(mip_device, message_actions_yaml, &event_actions)) + return false; + } + else + { + MICROSTRAIN_DEBUG(node_, "No 'message' section of 'actions' config, not configuring any message actions"); + } + + // Make sure that we don't have too many actions + if (event_actions.size() > max_actions) + { + MICROSTRAIN_ERROR(node_, "Device only supports %u actions, but you attempted to configure %lu triggers", max_actions, event_actions.size()); + return false; + } + + // Finally configure the actions + for (auto& event_action : event_actions) + { + event_action.function = mip::FunctionSelector::WRITE; + MICROSTRAIN_INFO(node_, "Configuring event action instance %u", event_action.instance); + printEventAction(event_action); + if (!(mip_cmd_result = mip_device->device().runCommand(event_action))) + { + MICROSTRAIN_MIP_SDK_ERROR(node_, mip_cmd_result, "Failed to write event action configuration"); + return false; + } + } + return true; +} + +bool EventsYaml::parseEventGpioTriggerConfig(const YAML::Node& gpio_triggers_yaml, std::vector* gpio_event_triggers) +{ + // Make sure that the type is a sequence or else we won't be able to iterate properly + if (!gpio_triggers_yaml.IsSequence()) + { + MICROSTRAIN_ERROR(node_, "Event triggers 'gpio' key must contain a sequence, but was of type %s", getYamlTypeString(gpio_triggers_yaml).c_str()); + return false; + } + + // Iterate and extract the triggers into the MIP objects + for (const YAML::Node& gpio_trigger_yaml : gpio_triggers_yaml) + { + // Make sure we can find all the required keys, and they are the right type + mip::commands_3dm::EventTrigger gpio_trigger; + gpio_trigger.type = mip::commands_3dm::EventTrigger::Type::GPIO; + try + { + gpio_trigger.instance = getRequiredKeyFromYaml(gpio_trigger_yaml, "instance"); + gpio_trigger.parameters.gpio.pin = getRequiredKeyFromYaml(gpio_trigger_yaml, "pin"); + gpio_trigger.parameters.gpio.mode = static_cast(getRequiredKeyFromYaml(gpio_trigger_yaml, "mode")); + gpio_event_triggers->push_back(gpio_trigger); + } + catch (const std::runtime_error& e) + { + return false; + } + } + + return true; +} + +bool EventsYaml::parseEventThresholdTriggerConfig(const YAML::Node& threshold_triggers_yaml, std::vector* threshold_event_triggers) +{ + // Make sure that the type is a sequence or else we won't be able to iterate properly + if (!threshold_triggers_yaml.IsSequence()) + { + MICROSTRAIN_ERROR(node_, "Event triggers 'threshold' key must contain a sequence, but was of type %s", getYamlTypeString(threshold_triggers_yaml).c_str()); + return false; + } + + // Iterate and extract the triggers into the MIP objects + for (const YAML::Node& threshold_trigger_yaml : threshold_triggers_yaml) + { + // Make sure we can find all the required keys, and they are the right type + mip::commands_3dm::EventTrigger threshold_trigger; + threshold_trigger.type = mip::commands_3dm::EventTrigger::Type::THRESHOLD; + try + { + threshold_trigger.instance = getRequiredKeyFromYaml(threshold_trigger_yaml, "instance"); + threshold_trigger.parameters.threshold.desc_set = getRequiredKeyFromYaml(threshold_trigger_yaml, "descriptor_set"); + threshold_trigger.parameters.threshold.field_desc = getRequiredKeyFromYaml(threshold_trigger_yaml, "field_descriptor"); + threshold_trigger.parameters.threshold.param_id = getRequiredKeyFromYaml(threshold_trigger_yaml, "param_id"); + threshold_trigger.parameters.threshold.type = static_cast(getRequiredKeyFromYaml(threshold_trigger_yaml, "type")); + switch (threshold_trigger.parameters.threshold.type) + { + case mip::commands_3dm::EventTrigger::ThresholdParams::Type::WINDOW: + threshold_trigger.parameters.threshold.low_thres = getRequiredKeyFromYaml(threshold_trigger_yaml, "low_threshold"); + threshold_trigger.parameters.threshold.high_thres = getRequiredKeyFromYaml(threshold_trigger_yaml, "high_threshold"); + break; + case mip::commands_3dm::EventTrigger::ThresholdParams::Type::INTERVAL: + threshold_trigger.parameters.threshold.int_thres = getRequiredKeyFromYaml(threshold_trigger_yaml, "interval_threshold"); + threshold_trigger.parameters.threshold.interval = getRequiredKeyFromYaml(threshold_trigger_yaml, "interval"); + break; + default: + MICROSTRAIN_ERROR(node_, "Invalid threshold type %u", static_cast(threshold_trigger.parameters.threshold.type)); + return false; + } + threshold_event_triggers->push_back(threshold_trigger); + } + catch (const std::runtime_error& e) + { + return false; + } + } + + return true; +} + +bool EventsYaml::parseEventCombinationTriggerConfig(const YAML::Node& combination_triggers_yaml, std::vector* combination_event_triggers) +{ + // Make sure that the type is a sequence or else we won't be able to iterate properly + if (!combination_triggers_yaml.IsSequence()) + { + MICROSTRAIN_ERROR(node_, "Event triggers 'combination' key must contain a sequence, but was of type %s", getYamlTypeString(combination_triggers_yaml).c_str()); + return false; + } + + // Iterate and extract the triggers into the MIP objects + for (const YAML::Node& combination_trigger_yaml : combination_triggers_yaml) + { + // Make sure we can find all the required keys, and they are the right type + mip::commands_3dm::EventTrigger combination_trigger; + combination_trigger.type = mip::commands_3dm::EventTrigger::Type::COMBINATION; + try + { + combination_trigger.instance = getRequiredKeyFromYaml(combination_trigger_yaml, "instance"); + combination_trigger.parameters.combination.logic_table = getRequiredKeyFromYaml(combination_trigger_yaml, "logic_table"); + + YAML::Node input_triggers_yaml; + const size_t max_input_triggers = sizeof(combination_trigger.parameters.combination.input_triggers) / sizeof(combination_trigger.parameters.combination.input_triggers[0]); + if ((input_triggers_yaml = combination_trigger_yaml["input_triggers"])) + { + if (input_triggers_yaml.IsSequence() && input_triggers_yaml.size() <= max_input_triggers) + { + uint8_t i = 0; + memset(combination_trigger.parameters.combination.input_triggers, 0, max_input_triggers); + for (const auto& input_trigger_yaml : input_triggers_yaml) + { + try + { + combination_trigger.parameters.combination.input_triggers[i++] = input_trigger_yaml.as(); + } + catch(const YAML::TypedBadConversion& t) + { + MICROSTRAIN_ERROR(node_, "Combination trigger parameter 'input_triggers' must only contain numbers"); + return false; + } + } + } + else + { + MICROSTRAIN_ERROR(node_, "Combination trigger parameter 'input_triggers' must be a sequence of size %lu or less", max_input_triggers); + return false; + } + } + else + { + MICROSTRAIN_ERROR(node_, "Combination trigger missing 'input_triggers' parameter"); + return false; + } + combination_event_triggers->push_back(combination_trigger); + } + catch (const std::runtime_error& e) + { + return false; + } + } + + return true; +} + +bool EventsYaml::parseEventGpioActionConfig(const YAML::Node& gpio_actions_yaml, std::vector* gpio_event_actions) +{ + // Make sure that the type is a sequence or else we won't be able to iterate properly + if (!gpio_actions_yaml.IsSequence()) + { + MICROSTRAIN_ERROR(node_, "Event actions 'gpio' key must contain a sequence, but was of type %s", getYamlTypeString(gpio_actions_yaml).c_str()); + return false; + } + + // Iterate the yaml and parse the entries into the MIP objects + for (const YAML::Node& gpio_action_yaml : gpio_actions_yaml) + { + mip::commands_3dm::EventAction gpio_action; + gpio_action.type = mip::commands_3dm::EventAction::Type::GPIO; + try + { + gpio_action.instance = getRequiredKeyFromYaml(gpio_action_yaml, "instance"); + gpio_action.trigger = getRequiredKeyFromYaml(gpio_action_yaml, "trigger_instance"); + gpio_action.parameters.gpio.pin = getRequiredKeyFromYaml(gpio_action_yaml, "pin"); + gpio_action.parameters.gpio.mode = static_cast(getRequiredKeyFromYaml(gpio_action_yaml, "mode")); + gpio_event_actions->push_back(gpio_action); + } + catch(const std::runtime_error& r) + { + return false; + } + } + + return true; +} + +bool EventsYaml::parseEventMessageActionConfig(std::shared_ptr& mip_device, const YAML::Node& message_actions_yaml, std::vector* message_event_actions) +{ + // Make sure that the type is a sequence or else we won't be able to iterate properly + if (!message_actions_yaml.IsSequence()) + { + MICROSTRAIN_ERROR(node_, "Event actions 'message' key must contain a sequence, but was of type %s", getYamlTypeString(message_actions_yaml).c_str()); + return false; + } + + // Iterate the yaml and parse the entries into the MIP objects + for (const YAML::Node& message_action_yaml : message_actions_yaml) + { + mip::commands_3dm::EventAction message_action; + message_action.type = mip::commands_3dm::EventAction::Type::MESSAGE; + try + { + message_action.instance = getRequiredKeyFromYaml(message_action_yaml, "instance"); + message_action.trigger = getRequiredKeyFromYaml(message_action_yaml, "trigger_instance"); + message_action.parameters.message.desc_set = getRequiredKeyFromYaml(message_action_yaml, "descriptor_set"); + + double actual_hertz; + float desired_hertz = getRequiredKeyFromYaml(message_action_yaml, "hertz"); + message_action.parameters.message.decimation = mip_device->getDecimationFromHertz(message_action.parameters.message.desc_set, desired_hertz, &actual_hertz); + if (actual_hertz != desired_hertz) + MICROSTRAIN_WARN(node_, "Descriptor set 0x%02x does not support running at the %f hertz. The closest we can do is %f", message_action.parameters.message.desc_set, desired_hertz, actual_hertz); + + YAML::Node descriptors_yaml; + const size_t max_descriptors = sizeof(message_action.parameters.message.descriptors) / sizeof(message_action.parameters.message.descriptors[0]); + if ((descriptors_yaml = message_action_yaml["descriptors"])) + { + if (descriptors_yaml.IsSequence() && descriptors_yaml.size() <= max_descriptors) + { + uint8_t i = 0; + for (const YAML::Node& descriptor_yaml : descriptors_yaml) + { + try + { + message_action.parameters.message.descriptors[i++] = descriptor_yaml.as(); + } + catch(const YAML::TypedBadConversion& t) + { + MICROSTRAIN_ERROR(node_, "Message action parameter 'descriptors' must only contain numbers"); + return false; + } + } + message_action.parameters.message.num_fields = descriptors_yaml.size(); + } + else + { + MICROSTRAIN_ERROR(node_, "Message action parameter 'descriptors' must be a sequence of size no greater than %lu", max_descriptors); + return false; + } + } + else + { + MICROSTRAIN_ERROR(node_, "Message action missing required parameter 'descriptors'"); + return false; + } + + message_event_actions->push_back(message_action); + } + catch (const std::runtime_error& r) + { + return false; + } + } + + return true; +} + +template +T EventsYaml::getRequiredKeyFromYaml(const YAML::Node& node, const std::string& key) +{ + if (!node[key]) + { + YAML::Emitter emitter; emitter << node; + MICROSTRAIN_ERROR(node_, "Missing required key '%s' in %s", key.c_str(), emitter.c_str()); + throw std::runtime_error(""); + } + try + { + return node[key].as(); + } + catch(const YAML::TypedBadConversion& t) + { + YAML::Emitter emitter; emitter << node; + MICROSTRAIN_ERROR(node_, "Unable to get required key '%s' of type %s in %s", key.c_str(), typeid(T).name(), emitter.c_str()); + MICROSTRAIN_ERROR(node_, " Exception: %s", t.what()); + throw std::runtime_error(""); + } +} + +void EventsYaml::printEventTrigger(const mip::commands_3dm::EventTrigger& trigger) +{ + MICROSTRAIN_INFO(node_, " instance = %u", trigger.instance); + MICROSTRAIN_INFO(node_, " type = %u", static_cast(trigger.type)); + switch (trigger.type) + { + case mip::commands_3dm::EventTrigger::Type::GPIO: + MICROSTRAIN_INFO(node_, " pin = %u", trigger.parameters.gpio.pin); + MICROSTRAIN_INFO(node_, " mode = %u", static_cast(trigger.parameters.gpio.mode)); + break; + case mip::commands_3dm::EventTrigger::Type::THRESHOLD: + MICROSTRAIN_INFO(node_, " descriptor_set = 0x%02X", trigger.parameters.threshold.desc_set); + MICROSTRAIN_INFO(node_, " field_descriptor = 0x%02X", trigger.parameters.threshold.field_desc); + MICROSTRAIN_INFO(node_, " param_id = %u", trigger.parameters.threshold.param_id); + MICROSTRAIN_INFO(node_, " threshold_type = %u", static_cast(trigger.parameters.threshold.type)); + switch (trigger.parameters.threshold.type) + { + case mip::commands_3dm::EventTrigger::ThresholdParams::Type::WINDOW: + MICROSTRAIN_INFO(node_, " low_threshold = %f", trigger.parameters.threshold.low_thres); + MICROSTRAIN_INFO(node_, " high_threshold = %f", trigger.parameters.threshold.high_thres); + break; + case mip::commands_3dm::EventTrigger::ThresholdParams::Type::INTERVAL: + MICROSTRAIN_INFO(node_, " interval_threshold = %f", trigger.parameters.threshold.int_thres); + MICROSTRAIN_INFO(node_, " interval = %f", trigger.parameters.threshold.interval); + break; + } + break; + case mip::commands_3dm::EventTrigger::Type::COMBINATION: + MICROSTRAIN_INFO(node_, " logic_table = 0x%04X", trigger.parameters.combination.logic_table); + std::stringstream ss; ss << "["; + const size_t max_input_triggers = sizeof(trigger.parameters.combination.input_triggers) / sizeof(trigger.parameters.combination.input_triggers[0]); + for (int i = 0; i < max_input_triggers; i++) + { + ss << static_cast(trigger.parameters.combination.input_triggers[i]); + if (i < max_input_triggers - 1) + ss << ", "; + } + ss << "]"; + MICROSTRAIN_INFO(node_, " input_triggers = %s", ss.str().c_str()); + break; + } +} + +void EventsYaml::printEventAction(const mip::commands_3dm::EventAction& action) +{ + MICROSTRAIN_INFO(node_, " instance = %u", action.instance); + MICROSTRAIN_INFO(node_, " trigger = %u", action.trigger); + MICROSTRAIN_INFO(node_, " type = %u", static_cast(action.type)); + switch (action.type) + { + case mip::commands_3dm::EventAction::Type::GPIO: + MICROSTRAIN_INFO(node_, " pin = %u", action.parameters.gpio.pin); + MICROSTRAIN_INFO(node_, " mode = %u", static_cast(action.parameters.gpio.mode)); + break; + case mip::commands_3dm::EventAction::Type::MESSAGE: + MICROSTRAIN_INFO(node_, " descriptor_set = 0x%02x", action.parameters.message.desc_set); + MICROSTRAIN_INFO(node_, " decimation = %u", action.parameters.message.decimation); + MICROSTRAIN_INFO(node_, " num_fields = %u", action.parameters.message.num_fields); + std::stringstream ss; ss << "["; + for (int i = 0; i < action.parameters.message.num_fields; i++) + { + ss << "0x" << std::uppercase << std::setfill('0') << std::setw(2) << std::hex << static_cast(action.parameters.message.descriptors[i]); + if (i < action.parameters.message.num_fields - 1) + ss << ", "; + } + ss << "]"; + MICROSTRAIN_INFO(node_, " descriptors = %s", ss.str().c_str()); + break; + } +} + +} \ No newline at end of file diff --git a/src/utils/mappings/mip_publisher_mapping.cpp b/src/utils/mappings/mip_publisher_mapping.cpp index 82448b2..17bc334 100644 --- a/src/utils/mappings/mip_publisher_mapping.cpp +++ b/src/utils/mappings/mip_publisher_mapping.cpp @@ -95,11 +95,7 @@ bool MipPublisherMapping::configure(RosNodeType* config_node) // Get the data rate for the topic, and if it is not the default, use it, otherwise use the data class data rate if (static_topic_to_data_rate_config_key_mapping_.find(topic) != static_topic_to_data_rate_config_key_mapping_.end()) { - getParamFloat(config_node, static_topic_to_data_rate_config_key_mapping_.at(topic), topic_info.data_rate, DATA_CLASS_DATA_RATE_DO_NOT_STREAM); - if (topic_info.data_rate == FIELD_DATA_RATE_USE_DATA_CLASS) - { - MICROSTRAIN_WARN(node_, "Data rates of %0.1f are no longer supported. Disabling topic %s", FIELD_DATA_RATE_USE_DATA_CLASS, topic.c_str()); - } + getParamFloat(config_node, static_topic_to_data_rate_config_key_mapping_.at(topic), topic_info.data_rate, DATA_RATE_OFF); } else { @@ -125,7 +121,7 @@ bool MipPublisherMapping::configure(RosNodeType* config_node) auto& descriptor_rates = streamed_descriptors_mapping_[descriptor_set]; // If the data rate is 0, do not stream any data - if (topic_info.data_rate == DATA_CLASS_DATA_RATE_DO_NOT_STREAM) + if (topic_info.data_rate == DATA_RATE_OFF) { MICROSTRAIN_DEBUG(node_, "Not configuring descriptor 0x%02x%02x to stream because it's data rate is set to 0", descriptor_set, field_descriptor); continue; @@ -244,7 +240,7 @@ float MipPublisherMapping::getDataRate(const std::string& topic) const if (topic_info_mapping_.find(topic) != topic_info_mapping_.end()) return topic_info_mapping_.at(topic).data_rate; else - return DATA_CLASS_DATA_RATE_DO_NOT_STREAM; + return DATA_RATE_OFF; } float MipPublisherMapping::getMaxDataRate(uint8_t descriptor_set) const @@ -266,7 +262,8 @@ bool MipPublisherMapping::canPublish(const std::string& topic) const bool MipPublisherMapping::shouldPublish(const std::string& topic) const { - return canPublish(topic) && getDataRate(topic) != DATA_CLASS_DATA_RATE_DO_NOT_STREAM; + const auto data_rate = getDataRate(topic); + return canPublish(topic) && (data_rate != DATA_RATE_OFF || data_rate == DATA_RATE_DO_NOT_STREAM); } const std::map MipPublisherMapping::static_topic_to_mip_type_mapping_ = diff --git a/src/utils/mip/ros_connection.cpp b/src/utils/mip/ros_connection.cpp index cce3b49..ddfd8de 100644 --- a/src/utils/mip/ros_connection.cpp +++ b/src/utils/mip/ros_connection.cpp @@ -75,6 +75,10 @@ bool RosConnection::connect(RosNodeType* config_node, const std::string& port, c struct stat port_stat; while (stat(port.c_str(), &port_stat) != 0 && (poll_tries++ < poll_max_tries || poll_max_tries == -1)) { + // If ROS isn't running anymore, return false + if (!rosOk()) + return false; + // If the error isn't that the file does not exist, polling won't help, so we can fail here if (errno != ENOENT) { diff --git a/src/utils/mip/ros_mip_device_main.cpp b/src/utils/mip/ros_mip_device_main.cpp index 3f86bf5..8688f50 100644 --- a/src/utils/mip/ros_mip_device_main.cpp +++ b/src/utils/mip/ros_mip_device_main.cpp @@ -163,9 +163,9 @@ bool RosMipDeviceMain::configure(RosNodeType* config_node) mip::CmdResult RosMipDeviceMain::forceIdle() { // Setting to idle may fail the first couple times, so call it a few times in case the device is streaming too much data - mip::CmdResult result; + mip::CmdResult result = mip::CmdResult::NACK_COMMAND_FAILED; uint8_t set_to_idle_tries = 0; - while (set_to_idle_tries++ < 3) + while (set_to_idle_tries++ < 3 && rosOk()) { if (!!(result = mip::commands_base::setIdle(*device_))) break;