#include <system_controller_node.hpp>

Inheritance diagram for system_controller::SystemControllerNode:

Collaboration diagram for system_controller::SystemControllerNode:

Public Member Functions
	SystemControllerNode ()=delete

	~SystemControllerNode ()=default

	SystemControllerNode (const rclcpp::NodeOptions &options, bool auto_init=true)
	Constructor. Set explicitly to support node composition. More...

void	initialize ()
	Initialize this node by loading parameters from the ROS Network. More...

void	set_config (SystemControllerConfig config)
	Reset the configurations of this node. This is mean to support testing or other non-standard launch mechanisms. More...

Protected Member Functions
void	on_system_alert (const carma_msgs::msg::SystemAlert::UniquePtr msg)
	Callback for system alert messages used to evaluate system fault handling. More...

void	startup_delay_callback ()
	Callback to be triggered when the startup delay has passed. More...

void	on_error (const std::exception &e)
	Exception handling method for processing all internal exceptions;. More...

void	publish_system_alert (carma_msgs::msg::SystemAlert msg)
	Publishes a SystemAlert message to the rest of the carma-platform system. NOTE: This callback will automatically populate the msg.source_node field based on this node name. More...

Protected Attributes
const std::string	system_alert_topic_ {"/system_alert"}
	The default topic name for the system alert topic. More...

rclcpp::Subscription< carma_msgs::msg::SystemAlert >::SharedPtr	system_alert_sub_
	The subscriber for the system alert topic. More...

std::shared_ptr< rclcpp::Publisher< carma_msgs::msg::SystemAlert > >	system_alert_pub_
	System alert publisher. More...

ros2_lifecycle_manager::Ros2LifecycleManager	lifecycle_mgr_
	Lifecycle Manager which will track the managed nodes and call their lifecycle services on request. More...

rclcpp::TimerBase::SharedPtr	startup_timer_
	Timer which triggers when the startup delay has passed. More...

SystemControllerConfig	config_
	The configuration of this node. More...

Detailed Description

Definition at line 30 of file system_controller_node.hpp.

Constructor & Destructor Documentation

◆ SystemControllerNode() [1/2]

system_controller::SystemControllerNode::SystemControllerNode ( )

delete

◆ ~SystemControllerNode()

system_controller::SystemControllerNode::~SystemControllerNode ( )

default

◆ SystemControllerNode() [2/2]

system_controller::SystemControllerNode::SystemControllerNode	(	const rclcpp::NodeOptions &	options,
		bool	auto_init = `true`
	)

explicit

Constructor. Set explicitly to support node composition.

Parameters

options	The node options to use for configuring this node
auto_init	If true this node will automatically call its initialize method. If false the call will wait for the user. This is meant to support unit testing

Definition at line 25 of file system_controller_node.cpp.

      : rclcpp::Node("system_controller", options), 
      lifecycle_mgr_(get_node_base_interface(), get_node_graph_interface(), get_node_logging_interface(), get_node_services_interface())
  {
 
    if (auto_init)
    {
      initialize();
    }
  }

References initialize().

Here is the call graph for this function:

Member Function Documentation

◆ initialize()

void system_controller::SystemControllerNode::initialize ( )

Initialize this node by loading parameters from the ROS Network.

Definition at line 36 of file system_controller_node.cpp.

  {
    try
    {
 
      RCLCPP_INFO(get_logger(), "Initializing SystemControllerNode");
 
      // Create subscriptions. History size of 100 is used here to ensure no missed alerts
      system_alert_sub_ = create_subscription<carma_msgs::msg::SystemAlert>(
          system_alert_topic_, 100,
          std::bind(&SystemControllerNode::on_system_alert, this, std::placeholders::_1));
 
 
      // NOTE: Currently, intra-process comms must be disabled for a publisher that is transient_local: https://github.com/ros2/rclcpp/issues/1753
      rclcpp::PublisherOptions intra_proc_disabled; 
      intra_proc_disabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable;
      
      // Create a publisher that will send all previously published messages to late-joining subscribers ONLY If the subscriber is transient_local too
      auto pub_qos_transient_local = rclcpp::QoS(rclcpp::KeepLast(5)); // A publisher with this QoS will store the last 5 messages that it has sent on the topic
      pub_qos_transient_local.transient_local();
 
      system_alert_pub_ = create_publisher<carma_msgs::msg::SystemAlert>(
        system_alert_topic_, pub_qos_transient_local, intra_proc_disabled);
 
      config_.signal_configure_delay = this->declare_parameter<double>("signal_configure_delay", config_.signal_configure_delay);
      config_.service_timeout_ms = this->declare_parameter<int64_t>("service_timeout_ms", config_.service_timeout_ms);
      config_.call_timeout_ms = this->declare_parameter<int64_t>("call_timeout_ms", config_.call_timeout_ms);
      config_.required_subsystem_nodes = this->declare_parameter<std::vector<std::string>>("required_subsystem_nodes", config_.required_subsystem_nodes);
 
      RCLCPP_INFO_STREAM(get_logger(), "Config: " << config_);
 
      lifecycle_mgr_.set_managed_nodes(config_.required_subsystem_nodes);
 
      // Create startup timer
      startup_timer_ = rclcpp::create_timer(
          this,
          get_clock(),
          std::chrono::milliseconds(static_cast<long>(config_.signal_configure_delay * 1000)),
          std::bind(&SystemControllerNode::startup_delay_callback, this));
 
 
    }
    catch (const std::exception &e)
    {
      on_error(e);
    }
  }

References SystemControllerConfig::call_timeout_ms, config_, lifecycle_mgr_, on_error(), on_system_alert(), SystemControllerConfig::required_subsystem_nodes, SystemControllerConfig::service_timeout_ms, SystemControllerConfig::signal_configure_delay, startup_delay_callback(), startup_timer_, system_alert_pub_, system_alert_sub_, and system_alert_topic_.

Referenced by SystemControllerNode().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ on_error()

void system_controller::SystemControllerNode::on_error ( const std::exception & e )

protected

Exception handling method for processing all internal exceptions;.

Definition at line 89 of file system_controller_node.cpp.

  {
 
    if (startup_timer_)
        startup_timer_->cancel(); // Cancel the timer to ensure we are not interrupted 
    
    std::string reason = "Uncaught exception: " + std::string(e.what());
 
    RCLCPP_ERROR_STREAM(
        get_logger(), reason); // Log error
 
    lifecycle_mgr_.shutdown(std_msec(config_.service_timeout_ms), std_msec(config_.call_timeout_ms), false); // Trigger shutdown when internal error occurs
 
    rclcpp::shutdown(nullptr, reason); // Fully shutdown this node
  }

References SystemControllerConfig::call_timeout_ms, config_, lifecycle_mgr_, SystemControllerConfig::service_timeout_ms, and startup_timer_.

Referenced by initialize(), on_system_alert(), and startup_delay_callback().

Here is the caller graph for this function:

◆ on_system_alert()

void system_controller::SystemControllerNode::on_system_alert ( const carma_msgs::msg::SystemAlert::UniquePtr msg )

protected

Callback for system alert messages used to evaluate system fault handling.

Parameters

msg	The message which was received

Definition at line 158 of file system_controller_node.cpp.

  {
 
    try
    {
 
      RCLCPP_INFO_STREAM(
          get_logger(), "Received SystemAlert message of type: " << static_cast<int>(msg->type) << " with message: " << msg->description);
 
      
      if (msg->type == carma_msgs::msg::SystemAlert::SHUTDOWN // If a SHUTDOWN signal was received
          || ( msg->type == carma_msgs::msg::SystemAlert::FATAL // OR if a required node notified FATAL
              && std::find(config_.required_subsystem_nodes.begin(), config_.required_subsystem_nodes.end(), msg->source_node)
                != config_.required_subsystem_nodes.end() ) )
      { 
        // TODO might make more sense for external shutdown to be a service call and remove SHUTDOWN from system alert entirely
       RCLCPP_INFO_STREAM( get_logger(), "Shutting down");
 
       lifecycle_mgr_.shutdown(std_msec(config_.service_timeout_ms), std_msec(config_.call_timeout_ms), false);
 
       carma_msgs::msg::SystemAlert shutdown_msg; // Notify ros1 nodes of shutdown
 
       shutdown_msg.type = carma_msgs::msg::SystemAlert::SHUTDOWN;
       shutdown_msg.description = "SHUTDOWN " + msg->description;
 
       publish_system_alert(shutdown_msg);
 
       std::this_thread::sleep_for(std::chrono::milliseconds(5)); // Add a bit of sleep to allow message publication
 
       rclcpp::shutdown(nullptr, "System Alert requested shutdown"); // Fully shutdown this node
      }
    }
    catch (const std::exception &e)
    {
      on_error(e);
    }
  }

References SystemControllerConfig::call_timeout_ms, config_, lifecycle_mgr_, on_error(), publish_system_alert(), SystemControllerConfig::required_subsystem_nodes, SystemControllerConfig::service_timeout_ms, and arbitrator::SHUTDOWN.

Referenced by initialize().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ publish_system_alert()

void system_controller::SystemControllerNode::publish_system_alert ( carma_msgs::msg::SystemAlert msg )

protected

Publishes a SystemAlert message to the rest of the carma-platform system. NOTE: This callback will automatically populate the msg.source_node field based on this node name.

Parameters

msg	The message to publish

Definition at line 147 of file system_controller_node.cpp.

  {
 
    if (!get_node_base_interface())
      throw std::runtime_error("get_node_base_interface() returned null pointer. May indicate ROS2 bug.");
 
    msg.source_node = get_node_base_interface()->get_fully_qualified_name(); // The the source name for the message
 
    system_alert_pub_->publish(msg); 
  }

References system_alert_pub_.

Referenced by on_system_alert().

Here is the caller graph for this function:

◆ set_config()

void system_controller::SystemControllerNode::set_config ( SystemControllerConfig config )

Reset the configurations of this node. This is mean to support testing or other non-standard launch mechanisms.

Parameters

config The config to set

Definition at line 84 of file system_controller_node.cpp.

  {
    config_ = config;
  }

References config_.

◆ startup_delay_callback()

void system_controller::SystemControllerNode::startup_delay_callback ( )

protected

Callback to be triggered when the startup delay has passed.

Definition at line 105 of file system_controller_node.cpp.

  {
    try
    {
      RCLCPP_INFO(get_logger(), "Attempting to configure system...");
      
      if (startup_timer_)
        startup_timer_->cancel(); // Cancel the timer as this callback should only trigger once
 
      // Walk the managed nodes through their lifecycle
      // First we configure the nodes
      if (!lifecycle_mgr_.configure(std_msec(config_.service_timeout_ms), std_msec(config_.call_timeout_ms)).empty())
      {
        // If some nodes failed to configure then we will shutdown the system
        RCLCPP_ERROR_STREAM(
            get_logger(), "System could not be configured on startup. Shutting down.");
 
        lifecycle_mgr_.shutdown(std_msec(config_.service_timeout_ms), std_msec(config_.call_timeout_ms), false);
        rclcpp::shutdown(nullptr, "System could not be configured on startup. Shutting down.");
        return;
      }
 
      // Second we activate the nodes
      if (!lifecycle_mgr_.activate(std_msec(config_.service_timeout_ms), std_msec(config_.call_timeout_ms)).empty())
      {
 
        RCLCPP_ERROR_STREAM(
            get_logger(), "System could not be activated. Shutting down.");
 
        lifecycle_mgr_.shutdown(std_msec(config_.service_timeout_ms), std_msec(config_.call_timeout_ms), false);
        rclcpp::shutdown(nullptr, "System could not be activated. Shutting down.");
        return;
      }
 
      RCLCPP_INFO(get_logger(), "System configured successfully");
    }
    catch (const std::exception &e)
    {
      on_error(e);
    }
  }

References SystemControllerConfig::call_timeout_ms, config_, lifecycle_mgr_, on_error(), SystemControllerConfig::service_timeout_ms, and startup_timer_.

Referenced by initialize().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ config_

SystemControllerConfig system_controller::SystemControllerNode::config_

protected

The configuration of this node.

Definition at line 99 of file system_controller_node.hpp.

Referenced by initialize(), on_error(), on_system_alert(), set_config(), and startup_delay_callback().

◆ lifecycle_mgr_

ros2_lifecycle_manager::Ros2LifecycleManager system_controller::SystemControllerNode::lifecycle_mgr_

protected

Lifecycle Manager which will track the managed nodes and call their lifecycle services on request.

Definition at line 93 of file system_controller_node.hpp.

Referenced by initialize(), on_error(), on_system_alert(), and startup_delay_callback().

◆ startup_timer_

rclcpp::TimerBase::SharedPtr system_controller::SystemControllerNode::startup_timer_

protected

Timer which triggers when the startup delay has passed.

Definition at line 96 of file system_controller_node.hpp.

Referenced by initialize(), on_error(), and startup_delay_callback().

◆ system_alert_pub_

std::shared_ptr<rclcpp::Publisher<carma_msgs::msg::SystemAlert> > system_controller::SystemControllerNode::system_alert_pub_

protected

System alert publisher.

Definition at line 90 of file system_controller_node.hpp.

Referenced by initialize(), and publish_system_alert().

◆ system_alert_sub_

rclcpp::Subscription<carma_msgs::msg::SystemAlert>::SharedPtr system_controller::SystemControllerNode::system_alert_sub_

protected

The subscriber for the system alert topic.

Definition at line 87 of file system_controller_node.hpp.

Referenced by initialize().

◆ system_alert_topic_

const std::string system_controller::SystemControllerNode::system_alert_topic_ {"/system_alert"}

protected

The default topic name for the system alert topic.

Definition at line 84 of file system_controller_node.hpp.

Referenced by initialize().

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

system_controller/include/system_controller/system_controller_node.hpp
system_controller/src/system_controller_node.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ SystemControllerNode() [1/2]

◆ ~SystemControllerNode()

◆ SystemControllerNode() [2/2]

Member Function Documentation

◆ initialize()

◆ on_error()

◆ on_system_alert()

◆ publish_system_alert()

◆ set_config()

◆ startup_delay_callback()

Member Data Documentation

◆ config_

◆ lifecycle_mgr_

◆ startup_timer_

◆ system_alert_pub_

◆ system_alert_sub_

◆ system_alert_topic_