subsystem_controllers::GuidanceControllerNode Class Reference

#include <guidance_controller.hpp>

Inheritance diagram for subsystem_controllers::GuidanceControllerNode:

Collaboration diagram for subsystem_controllers::GuidanceControllerNode:

Public Member Functions
	GuidanceControllerNode ()=delete
	~GuidanceControllerNode ()=default
	GuidanceControllerNode (const rclcpp::NodeOptions &options)
	Constructor. Set explicitly to support node composition. More...
cr2::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &)
cr2::CallbackReturn	handle_on_activate (const rclcpp_lifecycle::State &)
cr2::CallbackReturn	handle_on_deactivate (const rclcpp_lifecycle::State &)
cr2::CallbackReturn	handle_on_cleanup (const rclcpp_lifecycle::State &)
cr2::CallbackReturn	handle_on_shutdown (const rclcpp_lifecycle::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)

Private Attributes
std::shared_ptr< PluginManager >	plugin_manager_
	Plugin manager to handle all the plugin specific discovery and reporting. More...
GuidanceControllerConfig	config_
	Config for user provided parameters. More...
cr2::SubPtr< carma_planning_msgs::msg::Plugin >	plugin_discovery_sub_
	ROS handles. More...
cr2::ServicePtr< carma_planning_msgs::srv::PluginList >	get_registered_plugins_server_
cr2::ServicePtr< carma_planning_msgs::srv::PluginList >	get_active_plugins_server_
cr2::ServicePtr< carma_planning_msgs::srv::PluginActivation >	activate_plugin_server_
cr2::ServicePtr< carma_planning_msgs::srv::GetPluginApi >	get_strategic_plugins_by_capability_server_
cr2::ServicePtr< carma_planning_msgs::srv::GetPluginApi >	get_tactical_plugins_by_capability_server_
cr2::ServicePtr< carma_planning_msgs::srv::GetPluginApi >	get_control_plugins_by_capability_server_

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 38 of file guidance_controller.hpp.

Constructor & Destructor Documentation

GuidanceControllerNode() [1/2]

subsystem_controllers::GuidanceControllerNode::GuidanceControllerNode ( )

delete

~GuidanceControllerNode()

subsystem_controllers::GuidanceControllerNode::~GuidanceControllerNode ( )

default

GuidanceControllerNode() [2/2]

subsystem_controllers::GuidanceControllerNode::GuidanceControllerNode ( 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 25 of file guidance_controller.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_.required_plugins = declare_parameter<std::vector<std::string>>("required_plugins", config_.required_plugins);
      config_.auto_activated_plugins = declare_parameter<std::vector<std::string>>("auto_activated_plugins", config_.auto_activated_plugins);
  }

References subsystem_controllers::GuidanceControllerConfig::auto_activated_plugins, config_, subsystem_controllers::GuidanceControllerConfig::required_plugins, and subsystem_controllers::BaseSubsystemController::trigger_managed_nodes_configure_from_base_class_.

Member Function Documentation

handle_on_activate()

cr2::CallbackReturn subsystem_controllers::GuidanceControllerNode::handle_on_activate ( const rclcpp_lifecycle::State &  prev_state )

virtual

Reimplemented from subsystem_controllers::BaseSubsystemController.

Definition at line 137 of file guidance_controller.cpp.

  {
    auto base_return = BaseSubsystemController::handle_on_activate(prev_state); // This will activate all nodes in the namespace TODO what about the plugins?
 
    if (base_return != cr2::CallbackReturn::SUCCESS) {
      RCLCPP_ERROR(get_logger(), "Guidance Controller could not activate");
      return base_return;
    }
 
    bool success = true;
    try {
      plugin_manager_->activate(); // Only checking required nodes. Other node failure tracked by activity status
    } catch(const std::runtime_error& e) {
      RCLCPP_ERROR_STREAM(get_logger(), "Error activating plugin manager " << e.what());
      success = false;
    }
 
    if (success)
    {
 
      RCLCPP_INFO_STREAM(get_logger(), "Subsystem able to activate");
      return CallbackReturn::SUCCESS;
    }
    else
    {
 
      RCLCPP_INFO_STREAM(get_logger(), "Subsystem unable to activate");
      return CallbackReturn::FAILURE;
    }
 
  }

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

Here is the call graph for this function:

handle_on_cleanup()

cr2::CallbackReturn subsystem_controllers::GuidanceControllerNode::handle_on_cleanup ( const rclcpp_lifecycle::State &  prev_state )

virtual

Reimplemented from subsystem_controllers::BaseSubsystemController.

Definition at line 200 of file guidance_controller.cpp.

  {
    auto base_return = BaseSubsystemController::handle_on_cleanup(prev_state);
 
    if (base_return != cr2::CallbackReturn::SUCCESS) {
      RCLCPP_ERROR(get_logger(), "Guidance Controller could not cleanup");
      return base_return;
    }
 
    bool success = true;
    try {
      plugin_manager_->cleanup(); // Only checking required nodes. Other node failure tracked by activity status
    } catch(const std::runtime_error& e) {
      RCLCPP_ERROR_STREAM(get_logger(), "Error cleaning up plugin manager " << e.what());
      success = false;
    }
 
    if (success)
    {
 
      RCLCPP_INFO_STREAM(get_logger(), "Subsystem able to cleanup");
      return CallbackReturn::SUCCESS;
    }
    else
    {
 
      RCLCPP_INFO_STREAM(get_logger(), "Subsystem unable to cleanup");
      return CallbackReturn::FAILURE;
    }
  }

References subsystem_controllers::BaseSubsystemController::handle_on_cleanup(), and plugin_manager_.

Here is the call graph for this function:

handle_on_configure()

cr2::CallbackReturn subsystem_controllers::GuidanceControllerNode::handle_on_configure ( const rclcpp_lifecycle::State &  prev_state )

virtual

Reimplemented from subsystem_controllers::BaseSubsystemController.

Definition at line 35 of file guidance_controller.cpp.

                                                                                                       {
    auto base_return = BaseSubsystemController::handle_on_configure(prev_state);
 
    if (base_return != cr2::CallbackReturn::SUCCESS) {
      RCLCPP_ERROR(get_logger(), "Guidance Controller could not configure");
      return base_return;
    }
 
    config_ = GuidanceControllerConfig();
 
    auto base_managed_nodes = lifecycle_mgr_.get_managed_nodes();
 
    std::string plugin_namespace = base_config_.subsystem_namespace + "/plugins/";
 
 
    std::vector<std::string> guidance_plugin_nodes;
    // Extract the nodes under the plugin namespaces (ie. /guidance/plugins/)
    std::copy_if(base_managed_nodes.begin(), base_managed_nodes.end(),
      std::back_inserter(guidance_plugin_nodes),
      [plugin_namespace](const std::string& s) { return s.rfind(plugin_namespace, 0) == 0; });
 
    std::vector<std::string> non_plugin_guidance_nodes = get_non_intersecting_set(base_managed_nodes, guidance_plugin_nodes);
 
    lifecycle_mgr_.set_managed_nodes(non_plugin_guidance_nodes);
 
    // Load required plugins and default enabled plugins
    get_parameter<std::vector<std::string>>("required_plugins", config_.required_plugins);
    get_parameter<std::vector<std::string>>("auto_activated_plugins", config_.auto_activated_plugins);
 
    RCLCPP_INFO_STREAM(get_logger(), "Config: " << config_);
 
    // The core need is that plugins need to be managed separately from guidance nodes
    auto plugin_lifecycle_manager = std::make_shared<ros2_lifecycle_manager::Ros2LifecycleManager>(
      get_node_base_interface(), get_node_graph_interface(), get_node_logging_interface(), get_node_services_interface());
 
    plugin_manager_ = std::make_shared<PluginManager>(
      config_.required_plugins,
      config_.auto_activated_plugins,
      plugin_lifecycle_manager,
      [this](){ return get_current_state().id(); },
      [this](auto node, auto ns){ return get_service_names_and_types_by_node(node, ns); },
      std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)
    );
 
    plugin_discovery_sub_ = create_subscription<carma_planning_msgs::msg::Plugin>(
      "plugin_discovery", 50,
      std::bind(&PluginManager::update_plugin_status, plugin_manager_, std::placeholders::_1));
 
    get_registered_plugins_server_ = create_service<carma_planning_msgs::srv::PluginList>(
        "plugins/get_registered_plugins",
        std::bind(&PluginManager::get_registered_plugins, plugin_manager_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
    get_active_plugins_server_ = create_service<carma_planning_msgs::srv::PluginList>(
        "plugins/get_active_plugins",
        std::bind(&PluginManager::get_active_plugins, plugin_manager_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
    activate_plugin_server_ = create_service<carma_planning_msgs::srv::PluginActivation>(
        "plugins/activate_plugin", // TODO check topic name
        std::bind(&PluginManager::activate_plugin, plugin_manager_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
    get_strategic_plugins_by_capability_server_ = create_service<carma_planning_msgs::srv::GetPluginApi>(
        "plugins/get_strategic_plugins_by_capability",
        std::bind(&PluginManager::get_strategic_plugins_by_capability, plugin_manager_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
    get_tactical_plugins_by_capability_server_ = create_service<carma_planning_msgs::srv::GetPluginApi>(
        "plugins/get_tactical_plugins_by_capability",
        std::bind(&PluginManager::get_tactical_plugins_by_capability, plugin_manager_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
    get_control_plugins_by_capability_server_ = create_service<carma_planning_msgs::srv::GetPluginApi>(
        "plugins/get_control_plugins_by_capability",
        std::bind(&PluginManager::get_control_plugins_by_capability, plugin_manager_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
 
 
    // With all of our non-plugin managed nodes now being tracked we can execute their configure operations
    bool success = lifecycle_mgr_.configure(std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)).empty();
 
    // Configure our plugins
    try {
      plugin_manager_->configure(); // Only checking required nodes. Other node failure tracked by activity status
    } catch(const std::runtime_error& e) {
      RCLCPP_ERROR_STREAM(get_logger(), "Error configuring plugin manager " << e.what());
      success = false;
    }
 
 
 
    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::PluginManager::activate_plugin(), activate_plugin_server_, subsystem_controllers::GuidanceControllerConfig::auto_activated_plugins, subsystem_controllers::BaseSubsystemController::base_config_, subsystem_controllers::BaseSubSystemControllerConfig::call_timeout_ms, config_, subsystem_controllers::PluginManager::get_active_plugins(), get_active_plugins_server_, subsystem_controllers::PluginManager::get_control_plugins_by_capability(), get_control_plugins_by_capability_server_, subsystem_controllers::BaseSubsystemController::get_non_intersecting_set(), subsystem_controllers::PluginManager::get_registered_plugins(), get_registered_plugins_server_, subsystem_controllers::PluginManager::get_strategic_plugins_by_capability(), get_strategic_plugins_by_capability_server_, subsystem_controllers::PluginManager::get_tactical_plugins_by_capability(), get_tactical_plugins_by_capability_server_, subsystem_controllers::BaseSubsystemController::handle_on_configure(), subsystem_controllers::BaseSubsystemController::lifecycle_mgr_, plugin_discovery_sub_, plugin_manager_, subsystem_controllers::GuidanceControllerConfig::required_plugins, subsystem_controllers::BaseSubSystemControllerConfig::service_timeout_ms, subsystem_controllers::BaseSubSystemControllerConfig::subsystem_namespace, and subsystem_controllers::PluginManager::update_plugin_status().

Here is the call graph for this function:

handle_on_deactivate()

cr2::CallbackReturn subsystem_controllers::GuidanceControllerNode::handle_on_deactivate ( const rclcpp_lifecycle::State &  prev_state )

virtual

Reimplemented from subsystem_controllers::BaseSubsystemController.

Definition at line 169 of file guidance_controller.cpp.

  {
    auto base_return = BaseSubsystemController::handle_on_deactivate(prev_state);
 
    if (base_return != cr2::CallbackReturn::SUCCESS) {
      RCLCPP_ERROR(get_logger(), "Guidance Controller could not deactivate");
      return base_return;
    }
 
    bool success = true;
    try {
      plugin_manager_->deactivate(); // Only checking required nodes. Other node failure tracked by activity status
    } catch(const std::runtime_error& e) {
      RCLCPP_ERROR_STREAM(get_logger(), "Error deactivating plugin manager " << e.what());
      success = false;
    }
 
    if (success)
    {
 
      RCLCPP_INFO_STREAM(get_logger(), "Subsystem able to deactivate");
      return CallbackReturn::SUCCESS;
    }
    else
    {
 
      RCLCPP_INFO_STREAM(get_logger(), "Subsystem unable to deactivate");
      return CallbackReturn::FAILURE;
    }
  }

References subsystem_controllers::BaseSubsystemController::handle_on_deactivate(), and plugin_manager_.

Here is the call graph for this function:

handle_on_shutdown()

cr2::CallbackReturn subsystem_controllers::GuidanceControllerNode::handle_on_shutdown ( const rclcpp_lifecycle::State &  prev_state )

virtual

Reimplemented from subsystem_controllers::BaseSubsystemController.

Definition at line 231 of file guidance_controller.cpp.

  {
    auto base_return = BaseSubsystemController::handle_on_shutdown(prev_state);
 
    if (base_return != cr2::CallbackReturn::SUCCESS) {
      RCLCPP_ERROR(get_logger(), "Guidance Controller could not shutdown");
      return base_return;
    }
 
    bool success = plugin_manager_->shutdown();
 
    if (success)
    {
 
      RCLCPP_INFO_STREAM(get_logger(), "Subsystem able to shutdown cleanly");
      return CallbackReturn::SUCCESS;
    }
    else
    {
 
      RCLCPP_INFO_STREAM(get_logger(), "Subsystem unable to shutdown cleanly");
      return CallbackReturn::FAILURE;
    }
  }

References subsystem_controllers::BaseSubsystemController::handle_on_shutdown(), and plugin_manager_.

Here is the call graph for this function:

Member Data Documentation

activate_plugin_server_

cr2::ServicePtr<carma_planning_msgs::srv::PluginActivation> subsystem_controllers::GuidanceControllerNode::activate_plugin_server_

private

Definition at line 78 of file guidance_controller.hpp.

Referenced by handle_on_configure().

config_

GuidanceControllerConfig subsystem_controllers::GuidanceControllerNode::config_

private

Config for user provided parameters.

Definition at line 68 of file guidance_controller.hpp.

Referenced by GuidanceControllerNode(), and handle_on_configure().

get_active_plugins_server_

cr2::ServicePtr<carma_planning_msgs::srv::PluginList> subsystem_controllers::GuidanceControllerNode::get_active_plugins_server_

private

Definition at line 76 of file guidance_controller.hpp.

Referenced by handle_on_configure().

get_control_plugins_by_capability_server_

cr2::ServicePtr<carma_planning_msgs::srv::GetPluginApi> subsystem_controllers::GuidanceControllerNode::get_control_plugins_by_capability_server_

private

Definition at line 84 of file guidance_controller.hpp.

Referenced by handle_on_configure().

get_registered_plugins_server_

cr2::ServicePtr<carma_planning_msgs::srv::PluginList> subsystem_controllers::GuidanceControllerNode::get_registered_plugins_server_

private

Definition at line 74 of file guidance_controller.hpp.

Referenced by handle_on_configure().

get_strategic_plugins_by_capability_server_

cr2::ServicePtr<carma_planning_msgs::srv::GetPluginApi> subsystem_controllers::GuidanceControllerNode::get_strategic_plugins_by_capability_server_

private

Definition at line 80 of file guidance_controller.hpp.

Referenced by handle_on_configure().

get_tactical_plugins_by_capability_server_

cr2::ServicePtr<carma_planning_msgs::srv::GetPluginApi> subsystem_controllers::GuidanceControllerNode::get_tactical_plugins_by_capability_server_

private

Definition at line 82 of file guidance_controller.hpp.

Referenced by handle_on_configure().

plugin_discovery_sub_

cr2::SubPtr<carma_planning_msgs::msg::Plugin> subsystem_controllers::GuidanceControllerNode::plugin_discovery_sub_

private

ROS handles.

Definition at line 72 of file guidance_controller.hpp.

Referenced by handle_on_configure().

plugin_manager_

std::shared_ptr<PluginManager> subsystem_controllers::GuidanceControllerNode::plugin_manager_

private

Plugin manager to handle all the plugin specific discovery and reporting.

Definition at line 65 of file guidance_controller.hpp.

Referenced by handle_on_activate(), handle_on_cleanup(), handle_on_configure(), handle_on_deactivate(), and handle_on_shutdown().

---

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

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