#include <trajectory_executor_node.hpp>

Inheritance diagram for trajectory_executor::TrajectoryExecutor:

Collaboration diagram for trajectory_executor::TrajectoryExecutor:

Public Member Functions
	TrajectoryExecutor (const rclcpp::NodeOptions &)
	Constructor for TrajectoryExecutor. More...

rcl_interfaces::msg::SetParametersResult	parameter_update_callback (const std::vector< rclcpp::Parameter > &parameters)
	Example callback for dynamic parameter updates. More...

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

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

Protected Member Functions
std::map< std::string, std::string >	queryControlPlugins ()
	Helper function to query control plugin registration system. More...

void	onNewTrajectoryPlan (carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg)
	Callback to be invoked when a new trajectory plan is received on our inbound plan topic. More...

void	guidanceStateMonitor (carma_planning_msgs::msg::GuidanceState::UniquePtr msg)
	Monitor the guidance state and set the current trajector as null_ptr. More...

void	onTrajEmitTick ()
	Timer callback to be invoked at our output tickrate. Outputs current trajectory plan to the first control plugin in it's point list. If this is our second or later timestep on the same trajectory, consumes the first point in the point list before transmission. More...

Private Attributes
carma_ros2_utils::SubPtr< carma_planning_msgs::msg::TrajectoryPlan >	plan_sub_

carma_ros2_utils::SubPtr< carma_planning_msgs::msg::GuidanceState >	state_sub_

std::map< std::string, carma_ros2_utils::PubPtr< carma_planning_msgs::msg::TrajectoryPlan > >	traj_publisher_map_

rclcpp::TimerBase::SharedPtr	timer_

Config	config_

std::unique_ptr< carma_planning_msgs::msg::TrajectoryPlan >	cur_traj_

int	timesteps_since_last_traj_ {0}

Detailed Description

Trajectory Executor package primary worker class

Handles subscribing to inbound plans from Plan Delegator component. control plugin registration querying, and then coordination of execution of that plan amongst multiple control plugins.

Definition at line 38 of file trajectory_executor_node.hpp.

Constructor & Destructor Documentation

◆ TrajectoryExecutor()

trajectory_executor::TrajectoryExecutor::TrajectoryExecutor ( const rclcpp::NodeOptions & options )

explicit

Constructor for TrajectoryExecutor.

Definition at line 22 of file trajectory_executor_node.cpp.

      : carma_ros2_utils::CarmaLifecycleNode(options)
  {
    // Create initial config
    config_ = Config();
 
    // Declare parameters
    config_.trajectory_publish_rate = declare_parameter<double>("trajectory_publish_rate", config_.trajectory_publish_rate);
    config_.default_control_plugin = declare_parameter<std::string>("default_control_plugin", config_.default_control_plugin);
    config_.default_control_plugin_topic = declare_parameter<std::string>("default_control_plugin_topic", config_.default_control_plugin_topic);
  }

References config_, trajectory_executor::Config::default_control_plugin, trajectory_executor::Config::default_control_plugin_topic, and trajectory_executor::Config::trajectory_publish_rate.

Member Function Documentation

◆ guidanceStateMonitor()

void trajectory_executor::TrajectoryExecutor::guidanceStateMonitor ( carma_planning_msgs::msg::GuidanceState::UniquePtr msg )

protected

Monitor the guidance state and set the current trajector as null_ptr.

Definition at line 165 of file trajectory_executor_node.cpp.

  {
    // TODO need to handle control handover once alernative planner system is finished
    if(msg->state != carma_planning_msgs::msg::GuidanceState::ENGAGED)
    {
        cur_traj_= nullptr;
    }
  }

References cur_traj_, and lightbar_manager::ENGAGED.

Referenced by handle_on_configure().

Here is the caller graph for this function:

◆ handle_on_activate()

carma_ros2_utils::CallbackReturn trajectory_executor::TrajectoryExecutor::handle_on_activate ( const rclcpp_lifecycle::State & )

Definition at line 108 of file trajectory_executor_node.cpp.

  {
    // Create timer for publishing outbound trajectories to the control plugins
    int timer_period_ms = (1 / config_.trajectory_publish_rate) * 1000; // Conversion from frequency (Hz) to milliseconds time period
 
    timer_ = create_timer(
        get_clock(),
        std::chrono::milliseconds(timer_period_ms),
        std::bind(&TrajectoryExecutor::onTrajEmitTick, this));
 
    return CallbackReturn::SUCCESS;
  }

References config_, onTrajEmitTick(), timer_, and trajectory_executor::Config::trajectory_publish_rate.

Here is the call graph for this function:

◆ handle_on_configure()

carma_ros2_utils::CallbackReturn trajectory_executor::TrajectoryExecutor::handle_on_configure ( const rclcpp_lifecycle::State & )

Definition at line 65 of file trajectory_executor_node.cpp.

  {
    RCLCPP_INFO_STREAM(get_logger(), "TrajectoryExecutor trying to configure");
 
    // Reset config
    config_ = Config();
 
    // Load parameters
    get_parameter<double>("trajectory_publish_rate", config_.trajectory_publish_rate);
    get_parameter<std::string>("default_control_plugin", config_.default_control_plugin);
    get_parameter<std::string>("default_control_plugin_topic", config_.default_control_plugin_topic);
 
    RCLCPP_INFO_STREAM(get_logger(), "Loaded params: " << config_);
 
    // Register runtime parameter update callback
    add_on_set_parameters_callback(std::bind(&TrajectoryExecutor::parameter_update_callback, this, std_ph::_1));
 
    // Setup subscribers
    plan_sub_ = create_subscription<carma_planning_msgs::msg::TrajectoryPlan>("trajectory", 5,
                                                              std::bind(&TrajectoryExecutor::onNewTrajectoryPlan, this, std_ph::_1));
    state_sub_ = create_subscription<carma_planning_msgs::msg::GuidanceState>("state", 5,
                                                              std::bind(&TrajectoryExecutor::guidanceStateMonitor, this, std_ph::_1));
 
    cur_traj_ = std::unique_ptr<carma_planning_msgs::msg::TrajectoryPlan>();
 
    RCLCPP_DEBUG_STREAM(get_logger(), "Setting up publishers for control plugin topics...");
 
    std::map<std::string, carma_ros2_utils::PubPtr<carma_planning_msgs::msg::TrajectoryPlan>> control_plugin_topics;
    auto discovered_control_plugins = queryControlPlugins();
 
    for (auto it = discovered_control_plugins.begin(); it != discovered_control_plugins.end(); it++)
    {
      RCLCPP_DEBUG_STREAM(get_logger(), "Trajectory executor discovered control plugin " << it->first.c_str() << " listening on topic " <<  it->second.c_str());
      carma_ros2_utils::PubPtr<carma_planning_msgs::msg::TrajectoryPlan> control_plugin_pub = create_publisher<carma_planning_msgs::msg::TrajectoryPlan>(it->second, 1000);
      control_plugin_topics.insert(std::make_pair(it->first, control_plugin_pub));
    }
 
    traj_publisher_map_ = control_plugin_topics;
    RCLCPP_DEBUG_STREAM(get_logger(), "TrajectoryExecutor component initialized succesfully!");
 
    // Return success if everthing initialized successfully
    return CallbackReturn::SUCCESS;
  }

References config_, cur_traj_, trajectory_executor::Config::default_control_plugin, trajectory_executor::Config::default_control_plugin_topic, guidanceStateMonitor(), onNewTrajectoryPlan(), parameter_update_callback(), plan_sub_, queryControlPlugins(), state_sub_, traj_publisher_map_, and trajectory_executor::Config::trajectory_publish_rate.

Here is the call graph for this function:

◆ onNewTrajectoryPlan()

void trajectory_executor::TrajectoryExecutor::onNewTrajectoryPlan ( carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg )

protected

Callback to be invoked when a new trajectory plan is received on our inbound plan topic.

Parameters

msg	The new TrajectoryPlan message

Definition at line 154 of file trajectory_executor_node.cpp.

  {
    RCLCPP_DEBUG_STREAM(get_logger(), "Received new trajectory plan!");
    RCLCPP_DEBUG_STREAM(get_logger(), "New Trajectory plan ID: " << msg->trajectory_id);
    RCLCPP_DEBUG_STREAM(get_logger(), "New plan contains " << msg->trajectory_points.size() << " points");
 
    cur_traj_ = std::unique_ptr<carma_planning_msgs::msg::TrajectoryPlan>(move(msg));
    timesteps_since_last_traj_ = 0;
    RCLCPP_INFO_STREAM(get_logger(), "Successfully swapped trajectories!");
  }

References cur_traj_, and timesteps_since_last_traj_.

Referenced by handle_on_configure().

Here is the caller graph for this function:

◆ onTrajEmitTick()

void trajectory_executor::TrajectoryExecutor::onTrajEmitTick ( )

protected

Timer callback to be invoked at our output tickrate. Outputs current trajectory plan to the first control plugin in it's point list. If this is our second or later timestep on the same trajectory, consumes the first point in the point list before transmission.

Definition at line 121 of file trajectory_executor_node.cpp.

  {
    RCLCPP_DEBUG_STREAM(get_logger(), "TrajectoryExecutor tick start!");
 
    if (cur_traj_ != nullptr) {
      // Determine the relevant control plugin for the current timestep
      std::string control_plugin = cur_traj_->trajectory_points[0].controller_plugin_name;
      // if it instructed to use default control_plugin
      if (control_plugin == "default" || control_plugin =="") {
        control_plugin = config_.default_control_plugin;
      }
 
      std::map<std::string, carma_ros2_utils::PubPtr<carma_planning_msgs::msg::TrajectoryPlan>>::iterator it = traj_publisher_map_.find(control_plugin);
      if (it != traj_publisher_map_.end()) {
        RCLCPP_DEBUG_STREAM(get_logger(), "Found match for control plugin " << control_plugin.c_str() << " at point " << timesteps_since_last_traj_ << " in current trajectory!");
        it->second->publish(*cur_traj_);
      } else {
        std::ostringstream description_builder;
              description_builder << "No match found for control plugin "
              << control_plugin << " at point "
              << timesteps_since_last_traj_ << " in current trajectory!";
 
        throw std::invalid_argument(description_builder.str());
      }
      timesteps_since_last_traj_++;
    } else {
      RCLCPP_DEBUG_STREAM(get_logger(), "Awaiting initial trajectory publication...");
    }
 
    RCLCPP_DEBUG_STREAM(get_logger(), "TrajectoryExecutor tick completed succesfully!");
 
  }

References config_, cur_traj_, trajectory_executor::Config::default_control_plugin, timesteps_since_last_traj_, and traj_publisher_map_.

Referenced by handle_on_activate().

Here is the caller graph for this function:

◆ parameter_update_callback()

rcl_interfaces::msg::SetParametersResult trajectory_executor::TrajectoryExecutor::parameter_update_callback ( const std::vector< rclcpp::Parameter > & parameters )

Example callback for dynamic parameter updates.

Definition at line 34 of file trajectory_executor_node.cpp.

  {
    auto error = update_params<double>({{"trajectory_publish_rate", config_.trajectory_publish_rate}}, parameters);
    auto error_2 = update_params<std::string>({
        {"default_control_plugin", config_.default_control_plugin},
        {"default_control_plugin_topic", config_.default_control_plugin_topic}}, parameters);
 
    rcl_interfaces::msg::SetParametersResult result;
 
    result.successful = !error && !error_2;
 
    return result;
  }

References config_, trajectory_executor::Config::default_control_plugin, trajectory_executor::Config::default_control_plugin_topic, and trajectory_executor::Config::trajectory_publish_rate.

Referenced by handle_on_configure().

Here is the caller graph for this function:

◆ queryControlPlugins()

std::map< std::string, std::string > trajectory_executor::TrajectoryExecutor::queryControlPlugins ( )

protected

Helper function to query control plugin registration system.

Returns: A map of control plugin name -> control plugin input topics for all discovered control plugins

Definition at line 48 of file trajectory_executor_node.cpp.

  {
      // Hard coded stub for MVP since plugin manager won't be developed yet
      // TODO: Query plugin manager to receive actual list of plugins and their corresponding topics
      RCLCPP_DEBUG_STREAM(get_logger(), "Executing stub behavior for plugin discovery MVP...");
      std::map<std::string, std::string> out;
 
      out[config_.default_control_plugin] = config_.default_control_plugin_topic;
 
      //Hardcoding platoon control plugins
      std::string control_plugin2 = "platooning_control";
      std::string control_plugin_topic2 = "/guidance/plugins/platooning_control/plan_trajectory";
      out[control_plugin2] = control_plugin_topic2;
 
      return out;
  }

References config_, trajectory_executor::Config::default_control_plugin, and trajectory_executor::Config::default_control_plugin_topic.

Referenced by handle_on_configure().

Here is the caller graph for this function:

Member Data Documentation

◆ config_

Config trajectory_executor::TrajectoryExecutor::config_

private

Definition at line 52 of file trajectory_executor_node.hpp.

Referenced by TrajectoryExecutor(), handle_on_activate(), handle_on_configure(), onTrajEmitTick(), parameter_update_callback(), and queryControlPlugins().

◆ cur_traj_

std::unique_ptr<carma_planning_msgs::msg::TrajectoryPlan> trajectory_executor::TrajectoryExecutor::cur_traj_

private

Definition at line 55 of file trajectory_executor_node.hpp.

Referenced by guidanceStateMonitor(), handle_on_configure(), onNewTrajectoryPlan(), and onTrajEmitTick().

◆ plan_sub_

carma_ros2_utils::SubPtr<carma_planning_msgs::msg::TrajectoryPlan> trajectory_executor::TrajectoryExecutor::plan_sub_

private

Definition at line 43 of file trajectory_executor_node.hpp.

Referenced by handle_on_configure().

◆ state_sub_

carma_ros2_utils::SubPtr<carma_planning_msgs::msg::GuidanceState> trajectory_executor::TrajectoryExecutor::state_sub_

private

Definition at line 44 of file trajectory_executor_node.hpp.

Referenced by handle_on_configure().

◆ timer_

rclcpp::TimerBase::SharedPtr trajectory_executor::TrajectoryExecutor::timer_

private

Definition at line 49 of file trajectory_executor_node.hpp.

Referenced by handle_on_activate().

◆ timesteps_since_last_traj_

int trajectory_executor::TrajectoryExecutor::timesteps_since_last_traj_ {0}

private

Definition at line 56 of file trajectory_executor_node.hpp.

Referenced by onNewTrajectoryPlan(), and onTrajEmitTick().

◆ traj_publisher_map_

std::map<std::string, carma_ros2_utils::PubPtr<carma_planning_msgs::msg::TrajectoryPlan> > trajectory_executor::TrajectoryExecutor::traj_publisher_map_

private

Definition at line 46 of file trajectory_executor_node.hpp.

Referenced by handle_on_configure(), and onTrajEmitTick().

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

trajectory_executor/include/trajectory_executor/trajectory_executor_node.hpp
trajectory_executor/src/trajectory_executor_node.cpp

Public Member Functions

Protected Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ TrajectoryExecutor()

Member Function Documentation

◆ guidanceStateMonitor()

◆ handle_on_activate()

◆ handle_on_configure()

◆ onNewTrajectoryPlan()

◆ onTrajEmitTick()

◆ parameter_update_callback()

◆ queryControlPlugins()

Member Data Documentation

◆ config_

◆ cur_traj_

◆ plan_sub_

◆ state_sub_

◆ timer_

◆ timesteps_since_last_traj_

◆ traj_publisher_map_