subsystem_controllers::LocalizationControllerNode Class Reference

#include <localization_controller.hpp>

Inheritance diagram for subsystem_controllers::LocalizationControllerNode:

Collaboration diagram for subsystem_controllers::LocalizationControllerNode:

Public Member Functions
	LocalizationControllerNode ()=delete
	~LocalizationControllerNode ()=default
	LocalizationControllerNode (const rclcpp::NodeOptions &options)
	Constructor. Set explicitly to support node composition. More...
void	on_system_alert (const carma_msgs::msg::SystemAlert::UniquePtr msg)
carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &prev_state)
Public Member Functions inherited from subsystem_controllers::BaseSubsystemController
	BaseSubsystemController ()=delete
	BaseSubsystemController (const rclcpp::NodeOptions &options)
	Constructor. Set explicitly to support node composition. More...
	~BaseSubsystemController ()=default
void	set_config (BaseSubSystemControllerConfig config)
virtual void	on_system_alert (const carma_msgs::msg::SystemAlert::UniquePtr msg)
virtual carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &prev_state)
virtual carma_ros2_utils::CallbackReturn	handle_on_activate (const rclcpp_lifecycle::State &prev_state)
virtual carma_ros2_utils::CallbackReturn	handle_on_deactivate (const rclcpp_lifecycle::State &prev_state)
virtual carma_ros2_utils::CallbackReturn	handle_on_cleanup (const rclcpp_lifecycle::State &prev_state)
virtual carma_ros2_utils::CallbackReturn	handle_on_error (const rclcpp_lifecycle::State &prev_state, const std::string &exception_string)
virtual carma_ros2_utils::CallbackReturn	handle_on_shutdown (const rclcpp_lifecycle::State &prev_state)

Protected Member Functions
std::unordered_map< std::vector< SensorBooleanStatus >, SensorAlertStatus, VectorHash >	sensor_fault_map_from_json (std::string json_string)
	Helper function to convert a json string into an unordered map of sensor status's to required alerts TODO add example. More...
Protected Member Functions inherited from subsystem_controllers::BaseSubsystemController
std::vector< std::string >	get_nodes_in_namespace (const std::string &node_namespace) const
	Returns the list of fully qualified node names for all ROS2 nodes in the provided namespace. More...
std::vector< std::string >	get_non_intersecting_set (const std::vector< std::string > &set_a, const std::vector< std::string > &set_b) const
	Returns all elements of the provided set_a which are NOT contained in the provided set_b. More...

Protected Attributes
std::unordered_map< std::string, SensorBooleanStatus >	sensor_status_
LocalizationControllerConfig	config_
Protected Attributes inherited from subsystem_controllers::BaseSubsystemController
ros2_lifecycle_manager::Ros2LifecycleManager	lifecycle_mgr_
	Lifecycle Manager which will track the managed nodes and call their lifecycle services on request. More...
rclcpp::Subscription< carma_msgs::msg::SystemAlert >::SharedPtr	system_alert_sub_
	The subscriber for the system alert topic. More...
BaseSubSystemControllerConfig	base_config_
	The configuration struct. More...
bool	trigger_managed_nodes_configure_from_base_class_ = true
	Collection of flags which, if true, will cause the base class to make lifecycle service calls to managed nodes. More...
bool	trigger_managed_nodes_activate_from_base_class_ = true
bool	trigger_managed_nodes_deactivate_from_base_class_ = true
bool	trigger_managed_nodes_cleanup_from_base_class_ = true

Detailed Description

Definition at line 30 of file localization_controller.hpp.

Constructor & Destructor Documentation

LocalizationControllerNode() [1/2]

subsystem_controllers::LocalizationControllerNode::LocalizationControllerNode ( )

delete

~LocalizationControllerNode()

subsystem_controllers::LocalizationControllerNode::~LocalizationControllerNode ( )

default

LocalizationControllerNode() [2/2]

subsystem_controllers::LocalizationControllerNode::LocalizationControllerNode ( const rclcpp::NodeOptions &  options )

explicit

Constructor. Set explicitly to support node composition.

Parameters

options

The node options to use for configuring this node

Definition at line 24 of file localization_controller.cpp.

      : BaseSubsystemController(options)
  {
    config_.sensor_nodes = declare_parameter<std::vector<std::string>>("sensor_nodes", config_.sensor_nodes);
    config_.sensor_fault_map = sensor_fault_map_from_json(declare_parameter<std::string>("sensor_fault_map", ""));
  
    // Handle fact that parameter vectors cannot be empty
    if (config_.sensor_nodes.size() == 1 && config_.sensor_nodes[0].empty()) {
      config_.sensor_nodes.clear();
    }
  
  }

References config_, subsystem_controllers::LocalizationControllerConfig::sensor_fault_map, sensor_fault_map_from_json(), and subsystem_controllers::LocalizationControllerConfig::sensor_nodes.

Here is the call graph for this function:

Member Function Documentation

handle_on_configure()

carma_ros2_utils::CallbackReturn subsystem_controllers::LocalizationControllerNode::handle_on_configure ( const rclcpp_lifecycle::State &  prev_state )

virtual

Reimplemented from subsystem_controllers::BaseSubsystemController.

Definition at line 37 of file localization_controller.cpp.

                                                                                                                        {
    auto base_return = BaseSubsystemController::handle_on_configure(prev_state);
 
    if (base_return != carma_ros2_utils::CallbackReturn::SUCCESS) {
      RCLCPP_ERROR(get_logger(), "Localization Controller could not configure");
      return base_return;
    }
 
    // Reset and reload config
    config_ = LocalizationControllerConfig();
    get_parameter<std::vector<std::string>>("sensor_nodes", config_.sensor_nodes);
    
    // Handle fact that parameter vectors cannot be empty
    if (config_.sensor_nodes.size() == 1 && config_.sensor_nodes[0].empty()) {
      config_.sensor_nodes.clear();
    }
    
    std::string json_string;
    get_parameter<std::string>("sensor_fault_map", json_string);
    
    RCLCPP_INFO_STREAM(get_logger(), "Loaded sensor_fault_map json string: " << json_string);
    
    config_.sensor_fault_map = sensor_fault_map_from_json(json_string);
 
    RCLCPP_INFO_STREAM(get_logger(), "Loaded localization config: " << config_);
 
    for (auto node : config_.sensor_nodes) {
      sensor_status_[node] = SensorBooleanStatus::OPERATIONAL; // Start tracking the sensor status's
    }
 
    return carma_ros2_utils::CallbackReturn::SUCCESS;
  }

References config_, subsystem_controllers::BaseSubsystemController::handle_on_configure(), subsystem_controllers::OPERATIONAL, subsystem_controllers::LocalizationControllerConfig::sensor_fault_map, sensor_fault_map_from_json(), subsystem_controllers::LocalizationControllerConfig::sensor_nodes, and sensor_status_.

Here is the call graph for this function:

on_system_alert()

void subsystem_controllers::LocalizationControllerNode::on_system_alert ( const carma_msgs::msg::SystemAlert::UniquePtr  msg )

virtual

Reimplemented from subsystem_controllers::BaseSubsystemController.

Definition at line 70 of file localization_controller.cpp.

  {
    carma_msgs::msg::SystemAlert alert = *msg; // Keep local copy so parent method can be called with std::move
    carma_msgs::msg::SystemAlert output_alert;
    BaseSubsystemController::on_system_alert(std::move(msg)); // Call parent method
 
    // Check if our list of coordinated sensors contains the node providing the alert
    // No need to check required nodes as that has already been handled in the parent call
    if (sensor_status_.find(alert.source_node) == sensor_status_.end()) {
      RCLCPP_INFO_STREAM(get_logger(), "Alert not from a sensor node. Alerting node: " << alert.source_node);
      return;
    }
 
    // Update our tracking of sensor statuses if this node failed
    if (alert.type == carma_msgs::msg::SystemAlert::FATAL) {
      sensor_status_[alert.source_node] = SensorBooleanStatus::FAILED;
    }
 
    // Combine all sensor statuses into a single set to index into the alert map
    std::vector<SensorBooleanStatus> current_sensor_status;
    for (const auto& node : config_.sensor_nodes) {
      current_sensor_status.emplace_back(sensor_status_[node]);
    }
 
    // If the controller has not been configured to handle this fault configuration then do nothing
    if (config_.sensor_fault_map.find(current_sensor_status) == config_.sensor_fault_map.end()) {
      RCLCPP_INFO_STREAM(get_logger(), "Failure does not match with fault configuration. Assuming system remains operational ");
      return;
    }
 
    // Extract the alert required by this type of sensor statuses
    SensorAlertStatus alert_status = config_.sensor_fault_map[current_sensor_status];
    output_alert.description = "Localization optional sensor failure from: " + alert.source_node;
 
    switch(alert_status) {
      case SensorAlertStatus::FATAL:
        output_alert.type = carma_msgs::msg::SystemAlert::FATAL;
        output_alert.description = "Localization subsystem failed due to sensor failure from: " + alert.source_node;
        break;
      case SensorAlertStatus::CAUTION:
        output_alert.type = carma_msgs::msg::SystemAlert::CAUTION;
        break;
      case SensorAlertStatus::WARNING:
        output_alert.type = carma_msgs::msg::SystemAlert::WARNING;
        break;
      default:
      return; // OPERATIONAL case no-op as other subsystems do not need notification of operational status
    }
 
    // Publish the alert
    output_alert.source_node =  get_node_base_interface()->get_fully_qualified_name();
    publish_system_alert(output_alert);
  }

References subsystem_controllers::CAUTION, config_, subsystem_controllers::FAILED, subsystem_controllers::FATAL, subsystem_controllers::BaseSubsystemController::on_system_alert(), subsystem_controllers::LocalizationControllerConfig::sensor_fault_map, subsystem_controllers::LocalizationControllerConfig::sensor_nodes, sensor_status_, and subsystem_controllers::WARNING.

Here is the call graph for this function:

sensor_fault_map_from_json()

std::unordered_map< std::vector< SensorBooleanStatus >, SensorAlertStatus, VectorHash > subsystem_controllers::LocalizationControllerNode::sensor_fault_map_from_json ( std::string  json_string )

protected

Helper function to convert a json string into an unordered map of sensor status's to required alerts TODO add example.

Definition at line 124 of file localization_controller.cpp.

                                                                                                                                                              {
      
    std::unordered_map<std::vector<SensorBooleanStatus>, SensorAlertStatus, VectorHash> map;
    
    rapidjson::Document d;
 
    if(d.Parse(json_string.c_str()).HasParseError())
    {
        RCLCPP_WARN(get_logger(), "Failed to parse sensor_fault map. Invalid json structure");
        return map;
    }
 
    if (!d.HasMember("sensor_fault_map")) {
      RCLCPP_WARN(get_logger(), "No sensor_fault_map found in localization config");
      return map;
    }
 
    rapidjson::Value& map_value = d["sensor_fault_map"];
 
    if (!map_value.IsArray()) {
      RCLCPP_WARN(get_logger(), "sensor_fault_map cannot be read as array");
      return map;
    }
 
    if (map_value.Size() == 0) {
      RCLCPP_WARN(get_logger(), "sensor_fault_map has zero size");
      return map;
    }
 
    for (rapidjson::SizeType i = 0; i < map_value.Size(); i++) {
      if (!map_value[i].IsArray()) {
        RCLCPP_WARN(get_logger(), "sensor_fault_map rows are not arrays");
        continue;
      }
 
      if (map_value[i].Size() < 2) {
        RCLCPP_WARN(get_logger(), "sensor_fault_map rows do not contain more than 1 element");
        continue;
      }
 
      std::vector<SensorBooleanStatus> statuses; 
      statuses.reserve(map_value[i].Size()-1);
      for (rapidjson::SizeType j = 0; j < map_value[i].Size() - 1; j++) {
        if (!map_value[i][j].IsInt()) {
          RCLCPP_WARN_STREAM(get_logger(), "sensor_fault_map row " << i << " element " << j << " is not an int");
          continue;
        }
 
        statuses.emplace_back(static_cast<SensorBooleanStatus>(map_value[i][j].GetInt()));
      }
 
      rapidjson::Value& alert = map_value[i][map_value[i].Size() - 1];
 
      if (!alert.IsInt()) {
        RCLCPP_WARN_STREAM(get_logger(), "sensor_fault_map alert type is not an int in row " << i);
        continue;
      }
 
      map[statuses] = static_cast<SensorAlertStatus>(alert.GetInt());
    }
 
    return map;
  }

References process_bag::i.

Referenced by LocalizationControllerNode(), and handle_on_configure().

Here is the caller graph for this function:

Member Data Documentation

config_

LocalizationControllerConfig subsystem_controllers::LocalizationControllerNode::config_

protected

Definition at line 59 of file localization_controller.hpp.

Referenced by LocalizationControllerNode(), handle_on_configure(), and on_system_alert().

sensor_status_

std::unordered_map<std::string, SensorBooleanStatus> subsystem_controllers::LocalizationControllerNode::sensor_status_

protected

Definition at line 58 of file localization_controller.hpp.

Referenced by handle_on_configure(), and on_system_alert().

---

The documentation for this class was generated from the following files:

• subsystem_controllers/include/subsystem_controllers/localization_controller/localization_controller.hpp
• subsystem_controllers/src/localization_controller/localization_controller.cpp