#include <drivers_controller_node.hpp>

Inheritance diagram for subsystem_controllers::DriversControllerNode:

Collaboration diagram for subsystem_controllers::DriversControllerNode:

Public Member Functions
	DriversControllerNode ()=delete

	~DriversControllerNode ()=default

	DriversControllerNode (const rclcpp::NodeOptions &options)
	Constructor. Set explicitly to support node composition. More...

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)

Private Member Functions
void	driver_discovery_cb (const carma_driver_msgs::msg::DriverStatus::SharedPtr msg)

void	critical_drivers_check_callback ()
	Timer callback function to check status of required ros1 drivers. More...

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

carma_ros2_utils::CallbackReturn	handle_on_activate (const rclcpp_lifecycle::State &prev_state)

Private Attributes
std::shared_ptr< SSCDriverManager >	ssc_driver_manager_

DriversControllerConfig	config_
	Config for user provided parameters. More...

carma_ros2_utils::SubPtr< carma_driver_msgs::msg::DriverStatus >	driver_status_sub_
	ROS handles. More...

std::vector< std::string >	ros1_ssc_driver_name_
	ROS parameters. More...

long	start_up_timestamp_

rclcpp::TimerBase::SharedPtr	ssc_status_check_timer_

boost::optional< carma_msgs::msg::SystemAlert >	prev_alert

Additional Inherited Members
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 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 33 of file drivers_controller_node.hpp.

Constructor & Destructor Documentation

◆ DriversControllerNode() [1/2]

subsystem_controllers::DriversControllerNode::DriversControllerNode ( )

delete

◆ ~DriversControllerNode()

subsystem_controllers::DriversControllerNode::~DriversControllerNode ( )

default

◆ DriversControllerNode() [2/2]

subsystem_controllers::DriversControllerNode::DriversControllerNode ( 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 22 of file drivers_controller_node.cpp.

: BaseSubsystemController(options)
{
  // Don't automatically trigger state transitions from base class on configure
  // In this class the managed nodes list first needs to be modified then the transition will be
  // triggered manually
  trigger_managed_nodes_configure_from_base_class_ = false;
 
  config_.startup_duration_ = declare_parameter<int>("startup_duration", config_.startup_duration_);
  config_.driver_timeout_ =
    declare_parameter<double>("ros1_ssc_driver_timeout ", config_.driver_timeout_);
 
  // carma-config parameters
  config_.ros1_ssc_driver_name_ =
    declare_parameter<std::string>("ros1_ssc_driver_name", config_.ros1_ssc_driver_name_);
  config_.excluded_namespace_nodes_ = declare_parameter<std::vector<std::string>>(
    "excluded_namespace_nodes", config_.excluded_namespace_nodes_);
}

References config_, subsystem_controllers::DriversControllerConfig::driver_timeout_, subsystem_controllers::DriversControllerConfig::excluded_namespace_nodes_, subsystem_controllers::DriversControllerConfig::ros1_ssc_driver_name_, subsystem_controllers::DriversControllerConfig::startup_duration_, and subsystem_controllers::BaseSubsystemController::trigger_managed_nodes_configure_from_base_class_.

Member Function Documentation

◆ critical_drivers_check_callback()

void subsystem_controllers::DriversControllerNode::critical_drivers_check_callback ( )

private

Timer callback function to check status of required ros1 drivers.

Definition at line 129 of file drivers_controller_node.cpp.

{
  if (get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
    return;  // Exit early if node is not active
  }
 
  long time_now = this->now().nanoseconds() / 1e6;
  long start_duration = rclcpp::Duration(config_.startup_duration_, 0).nanoseconds() / 1e6;
 
  auto sys_alert_msg_from_ssc =
    ssc_driver_manager_->get_latest_system_alert(time_now, start_up_timestamp_, start_duration);
 
  // Only publish if it is first time publishing
  // or if its type or description changed
  if (
    !prev_alert || prev_alert->type != sys_alert_msg_from_ssc.type ||
    prev_alert->description != sys_alert_msg_from_ssc.description) {
    prev_alert = sys_alert_msg_from_ssc;
    publish_system_alert(sys_alert_msg_from_ssc);
  } else {
    RCLCPP_DEBUG_STREAM(get_logger(), "No change to alert status");
  }
}

References config_, prev_alert, ssc_driver_manager_, start_up_timestamp_, and subsystem_controllers::DriversControllerConfig::startup_duration_.

Referenced by handle_on_configure().

Here is the caller graph for this function:

◆ driver_discovery_cb()

void subsystem_controllers::DriversControllerNode::driver_discovery_cb ( const carma_driver_msgs::msg::DriverStatus::SharedPtr msg )

private

Definition at line 153 of file drivers_controller_node.cpp.

{
  // Driver discovery only published by ros1 drivers
  ssc_driver_manager_->update_driver_status(msg, this->now().nanoseconds() / 1e6);
}

References ssc_driver_manager_.

Referenced by handle_on_configure().

Here is the caller graph for this function:

◆ handle_on_activate()

carma_ros2_utils::CallbackReturn subsystem_controllers::DriversControllerNode::handle_on_activate ( const rclcpp_lifecycle::State & prev_state )

privatevirtual

Reimplemented from subsystem_controllers::BaseSubsystemController.

Definition at line 107 of file drivers_controller_node.cpp.

{
  // Reset to automatically trigger state transitions from base class
  trigger_managed_nodes_configure_from_base_class_ = true;
 
  // return only either SUCCESS or FAILURE
  auto base_return = BaseSubsystemController::handle_on_activate(
    prev_state);  // This will activate all base_managed_nodes
 
  if (base_return == CallbackReturn::SUCCESS) {
    RCLCPP_INFO(get_logger(), "Driver Controller activated!");
  } else if (base_return == CallbackReturn::FAILURE) {
    RCLCPP_ERROR(
      get_logger(),
      "Driver Controller encountered FAILURE when trying to activate the subsystems... please "
      "check which driver failed to activate...");
  }
 
  return base_return;
}

References subsystem_controllers::BaseSubsystemController::handle_on_activate(), and subsystem_controllers::BaseSubsystemController::trigger_managed_nodes_configure_from_base_class_.

Here is the call graph for this function:

◆ handle_on_configure()

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

privatevirtual

Reimplemented from subsystem_controllers::BaseSubsystemController.

Definition at line 41 of file drivers_controller_node.cpp.

{
  auto base_return = BaseSubsystemController::handle_on_configure(prev_state);
 
  if (base_return != carma_ros2_utils::CallbackReturn::SUCCESS) {
    RCLCPP_ERROR(get_logger(), "Drivers Controller could not configure");
    return base_return;
  }
 
  config_ = DriversControllerConfig();
 
  // Load required plugins and default enabled plugins
  get_parameter<std::string>("ros1_ssc_driver_name", config_.ros1_ssc_driver_name_);
  get_parameter<int>("startup_duration", config_.startup_duration_);
  get_parameter<double>("ros1_ssc_driver_timeout ", config_.driver_timeout_);
  get_parameter<std::vector<std::string>>(
    "excluded_namespace_nodes", config_.excluded_namespace_nodes_);
 
  RCLCPP_INFO_STREAM(get_logger(), "Config: " << config_);
 
  // Handle fact that parameter vectors cannot be empty
  if (
    config_.excluded_namespace_nodes_.size() == 1 && config_.excluded_namespace_nodes_[0].empty()) {
    config_.excluded_namespace_nodes_.clear();
  }
 
  auto base_managed_nodes = lifecycle_mgr_.get_managed_nodes();
  // Update managed nodes
  // Collect namespace nodes not managed by other subsystem controllers - manually specified in
  // carma-config
  auto updated_managed_nodes =
    get_non_intersecting_set(base_managed_nodes, config_.excluded_namespace_nodes_);
 
  lifecycle_mgr_.set_managed_nodes(updated_managed_nodes);
 
  ssc_driver_manager_ =
    std::make_shared<SSCDriverManager>(config_.ros1_ssc_driver_name_, config_.driver_timeout_);
 
  // record starup time
  start_up_timestamp_ = this->now().nanoseconds() / 1e6;
 
  driver_status_sub_ = create_subscription<carma_driver_msgs::msg::DriverStatus>(
    "driver_discovery", 20,
    std::bind(&DriversControllerNode::driver_discovery_cb, this, std::placeholders::_1));
 
  ssc_status_check_timer_ = create_timer(
    get_clock(), std::chrono::milliseconds(1000),
    std::bind(&DriversControllerNode::critical_drivers_check_callback, this));
 
  // Configure our drivers
  bool success = lifecycle_mgr_
                   .configure(
                     std::chrono::milliseconds(base_config_.service_timeout_ms),
                     std::chrono::milliseconds(base_config_.call_timeout_ms))
                   .empty();
 
  if (success) {
    RCLCPP_INFO_STREAM(get_logger(), "Subsystem able to configure");
    return CallbackReturn::SUCCESS;
  } else {
    RCLCPP_INFO_STREAM(get_logger(), "Subsystem unable to configure");
    return CallbackReturn::FAILURE;
  }
}

Here is the call graph for this function:

Member Data Documentation

◆ config_

DriversControllerConfig subsystem_controllers::DriversControllerNode::config_

private

Config for user provided parameters.

Definition at line 52 of file drivers_controller_node.hpp.

Referenced by DriversControllerNode(), critical_drivers_check_callback(), and handle_on_configure().

◆ driver_status_sub_

carma_ros2_utils::SubPtr<carma_driver_msgs::msg::DriverStatus> subsystem_controllers::DriversControllerNode::driver_status_sub_

private

ROS handles.

Definition at line 55 of file drivers_controller_node.hpp.

Referenced by handle_on_configure().

◆ prev_alert

boost::optional<carma_msgs::msg::SystemAlert> subsystem_controllers::DriversControllerNode::prev_alert

private

Definition at line 75 of file drivers_controller_node.hpp.

Referenced by critical_drivers_check_callback().

◆ ros1_ssc_driver_name_

std::vector<std::string> subsystem_controllers::DriversControllerNode::ros1_ssc_driver_name_

private

ROS parameters.

Definition at line 67 of file drivers_controller_node.hpp.

◆ ssc_driver_manager_

std::shared_ptr<SSCDriverManager> subsystem_controllers::DriversControllerNode::ssc_driver_manager_

private

Definition at line 49 of file drivers_controller_node.hpp.

Referenced by critical_drivers_check_callback(), driver_discovery_cb(), and handle_on_configure().

◆ ssc_status_check_timer_

rclcpp::TimerBase::SharedPtr subsystem_controllers::DriversControllerNode::ssc_status_check_timer_

private

Definition at line 72 of file drivers_controller_node.hpp.

Referenced by handle_on_configure().

◆ start_up_timestamp_

long subsystem_controllers::DriversControllerNode::start_up_timestamp_

private

Definition at line 70 of file drivers_controller_node.hpp.

Referenced by critical_drivers_check_callback(), and handle_on_configure().

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

subsystem_controllers/include/subsystem_controllers/drivers_controller/drivers_controller_node.hpp
subsystem_controllers/src/drivers_controller/drivers_controller_node.cpp

Public Member Functions

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ DriversControllerNode() [1/2]

◆ ~DriversControllerNode()

◆ DriversControllerNode() [2/2]

Member Function Documentation

◆ critical_drivers_check_callback()

◆ driver_discovery_cb()

◆ handle_on_activate()

◆ handle_on_configure()

Member Data Documentation

◆ config_

◆ driver_status_sub_

◆ prev_alert

◆ ros1_ssc_driver_name_

◆ ssc_driver_manager_

◆ ssc_status_check_timer_

◆ start_up_timestamp_