subsystem_controllers::DriversControllerNode Class Reference

#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	timer_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< DriverManager >	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_required_drivers_
	ROS parameters. More...
std::vector< std::string >	ros1_camera_drivers_
long	start_up_timestamp_
rclcpp::TimerBase::SharedPtr	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 35 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>("required_driver_timeout", config_.driver_timeout_);
 
    // carma-config parameters
    config_.ros1_required_drivers_ = declare_parameter<std::vector<std::string>>("ros1_required_drivers", config_.ros1_required_drivers_);
    config_.ros1_camera_drivers_ = declare_parameter<std::vector<std::string>>("ros1_camera_drivers", config_.ros1_camera_drivers_);
    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_camera_drivers_, subsystem_controllers::DriversControllerConfig::ros1_required_drivers_, subsystem_controllers::DriversControllerConfig::startup_duration_, and subsystem_controllers::BaseSubsystemController::trigger_managed_nodes_configure_from_base_class_.

Member Function Documentation

driver_discovery_cb()

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

private

Definition at line 154 of file drivers_controller_node.cpp.

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

References 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 106 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 39 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::vector<std::string>>("ros1_required_drivers", config_.ros1_required_drivers_);
    get_parameter<std::vector<std::string>>("ros1_camera_drivers", config_.ros1_camera_drivers_);
    get_parameter<int>("startup_duration", config_.startup_duration_);
    get_parameter<double>("required_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_.ros1_required_drivers_.size() == 1 && config_.ros1_required_drivers_[0].empty()) {
      config_.ros1_required_drivers_.clear();
    }
    if (config_.ros1_camera_drivers_.size() == 1 && config_.ros1_camera_drivers_[0].empty()) {
      config_.ros1_camera_drivers_.clear();
    }
    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);
 
    driver_manager_ = std::make_shared<DriverManager>(
      config_.ros1_required_drivers_,
      config_.ros1_camera_drivers_,
      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));
 
    timer_ = create_timer(get_clock(), std::chrono::milliseconds(1000), std::bind(&DriversControllerNode::timer_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;
    }
 
  }

References subsystem_controllers::BaseSubsystemController::base_config_, subsystem_controllers::BaseSubSystemControllerConfig::call_timeout_ms, config_, driver_discovery_cb(), driver_manager_, driver_status_sub_, subsystem_controllers::DriversControllerConfig::driver_timeout_, subsystem_controllers::DriversControllerConfig::excluded_namespace_nodes_, subsystem_controllers::BaseSubsystemController::get_non_intersecting_set(), subsystem_controllers::BaseSubsystemController::handle_on_configure(), subsystem_controllers::BaseSubsystemController::lifecycle_mgr_, subsystem_controllers::DriversControllerConfig::ros1_camera_drivers_, subsystem_controllers::DriversControllerConfig::ros1_required_drivers_, subsystem_controllers::BaseSubSystemControllerConfig::service_timeout_ms, start_up_timestamp_, subsystem_controllers::DriversControllerConfig::startup_duration_, timer_, and timer_callback().

Here is the call graph for this function:

timer_callback()

void subsystem_controllers::DriversControllerNode::timer_callback ( )

private

Timer callback function to check status of required ros1 drivers.

Definition at line 127 of file drivers_controller_node.cpp.

  {
 
    long time_now = this->now().nanoseconds() / 1e6;
    rclcpp::Duration sd(config_.startup_duration_, 0);
    long start_duration = sd.nanoseconds()/1e6;
 
    auto dm = driver_manager_->handle_spin(time_now, start_up_timestamp_, start_duration);
 
    //Wait for node to be activated
    if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE){
      if (!prev_alert) {
        prev_alert = dm;
        publish_system_alert(dm);
      }
      else if ( prev_alert->type == dm.type && prev_alert->description.compare(dm.description) == 0) { // Do not publish duplicate alerts
        RCLCPP_DEBUG_STREAM(get_logger(), "No change to alert status");
      }
      else{
        prev_alert = dm;
        publish_system_alert(dm);
      }
    }
 
  }

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

Referenced by handle_on_configure().

Here is the caller graph for this function:

Member Data Documentation

config_

DriversControllerConfig subsystem_controllers::DriversControllerNode::config_

private

Config for user provided parameters.

Definition at line 57 of file drivers_controller_node.hpp.

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

driver_manager_

std::shared_ptr<DriverManager> subsystem_controllers::DriversControllerNode::driver_manager_

private

Definition at line 54 of file drivers_controller_node.hpp.

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

driver_status_sub_

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

private

ROS handles.

Definition at line 60 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 81 of file drivers_controller_node.hpp.

Referenced by timer_callback().

ros1_camera_drivers_

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

private

Definition at line 73 of file drivers_controller_node.hpp.

ros1_required_drivers_

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

private

ROS parameters.

Definition at line 72 of file drivers_controller_node.hpp.

start_up_timestamp_

long subsystem_controllers::DriversControllerNode::start_up_timestamp_

private

Definition at line 76 of file drivers_controller_node.hpp.

Referenced by handle_on_configure(), and timer_callback().

timer_

rclcpp::TimerBase::SharedPtr subsystem_controllers::DriversControllerNode::timer_

private

Definition at line 78 of file drivers_controller_node.hpp.

Referenced by 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