The route package provides the following functionality: More...

#include <route_node.hpp>

Inheritance diagram for route::Route:

Collaboration diagram for route::Route:

Public Member Functions
	Route (const rclcpp::NodeOptions &)
	Route constructor. More...

rcl_interfaces::msg::SetParametersResult	parameter_update_callback (const std::vector< rclcpp::Parameter > &parameters)
	Function 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 &)

Private Attributes
carma_ros2_utils::SubPtr< geometry_msgs::msg::TwistStamped >	twist_sub_

carma_ros2_utils::SubPtr< std_msgs::msg::String >	geo_sub_

carma_ros2_utils::PubPtr< carma_planning_msgs::msg::Route >	route_pub_

carma_ros2_utils::PubPtr< carma_planning_msgs::msg::RouteState >	route_state_pub_

carma_ros2_utils::PubPtr< carma_planning_msgs::msg::RouteEvent >	route_event_pub_

carma_ros2_utils::PubPtr< visualization_msgs::msg::Marker >	route_marker_pub_

carma_ros2_utils::ServicePtr< carma_planning_msgs::srv::GetAvailableRoutes >	get_available_route_srv_

carma_ros2_utils::ServicePtr< carma_planning_msgs::srv::SetActiveRoute >	set_active_route_srv_

carma_ros2_utils::ServicePtr< carma_planning_msgs::srv::AbortActiveRoute >	abort_active_route_srv_

rclcpp::TimerBase::SharedPtr	spin_timer_

Config	config_

tf2_ros::Buffer	tf2_buffer_

carma_wm::WMListener	wml_

carma_wm::WorldModelConstPtr	wm_

RouteGeneratorWorker	rg_worker_

Detailed Description

The route package provides the following functionality:

Route generation which provides the list of available routes and provides vehicle travel route description and management.
Route state management which provides the current state of the route following, including tracking vehicle cross track and down track distances along the active route

Definition at line 50 of file route_node.hpp.

Constructor & Destructor Documentation

◆ Route()

route::Route::Route ( const rclcpp::NodeOptions & options )

explicit

Route constructor.

Definition at line 22 of file route_node.cpp.

      : carma_ros2_utils::CarmaLifecycleNode(options),
        tf2_buffer_(this->get_clock()),
        wml_(this->get_node_base_interface(), this->get_node_logging_interface(),
             this->get_node_topics_interface(), this->get_node_parameters_interface()),
        rg_worker_(tf2_buffer_)
  {
    // Create initial config
    config_ = Config();
 
    // Declare parameters
    config_.max_crosstrack_error = declare_parameter<double>("max_crosstrack_error", config_.max_crosstrack_error);
    config_.destination_downtrack_range = declare_parameter<double>("destination_downtrack_range", config_.destination_downtrack_range);
    config_.route_spin_rate = declare_parameter<double>("route_spin_rate", config_.route_spin_rate);
    config_.cte_max_count = declare_parameter<int>("cte_max_count", config_.cte_max_count);
    config_.route_file_path = declare_parameter<std::string>("route_file_path", config_.route_file_path);
  }

References config_, route::Config::cte_max_count, route::Config::destination_downtrack_range, route::Config::max_crosstrack_error, route::Config::route_file_path, and route::Config::route_spin_rate.

Member Function Documentation

◆ handle_on_activate()

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

Definition at line 124 of file route_node.cpp.

  {
    rg_worker_.setRouteFilePath(config_.route_file_path);
 
    // Timer for route generator worker's spin callback
    auto rg_worker_spin_period_ms = int ((1 / config_.route_spin_rate) * 1000); // Conversion from frequency (Hz) to milliseconds time period
    spin_timer_ = create_timer(get_clock(),
                          std::chrono::milliseconds(rg_worker_spin_period_ms),
                          std::bind(&RouteGeneratorWorker::spinCallback, &rg_worker_));
 
    return CallbackReturn::SUCCESS;
  }

References config_, rg_worker_, route::Config::route_file_path, route::Config::route_spin_rate, route::RouteGeneratorWorker::setRouteFilePath(), spin_timer_, and route::RouteGeneratorWorker::spinCallback().

Here is the call graph for this function:

◆ handle_on_configure()

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

Definition at line 56 of file route_node.cpp.

  {
    RCLCPP_INFO_STREAM(get_logger(), "Route trying to configure");
 
    // Reset config
    config_ = Config();
 
    // Load parameters
    get_parameter<double>("max_crosstrack_error", config_.max_crosstrack_error);
    get_parameter<double>("destination_downtrack_range", config_.destination_downtrack_range);
    get_parameter<double>("route_spin_rate", config_.route_spin_rate);
    get_parameter<int>("cte_max_count", config_.cte_max_count);
    get_parameter<std::string>("route_file_path", config_.route_file_path);
 
    RCLCPP_INFO_STREAM(get_logger(), "Loaded params: " << config_);
 
    // Register runtime parameter update callback
    add_on_set_parameters_callback(std::bind(&Route::parameter_update_callback, this, std_ph::_1));
 
    // Setup subscribers
    twist_sub_ = create_subscription<geometry_msgs::msg::TwistStamped>("current_velocity", 1,
                                                              std::bind(&RouteGeneratorWorker::twistCb, &rg_worker_, std_ph::_1));
    geo_sub_ = create_subscription<std_msgs::msg::String>("georeference", 1,
                                                              std::bind(&RouteGeneratorWorker::georeferenceCb, &rg_worker_, std_ph::_1));
 
    // Setup publishers
    route_pub_ = create_publisher<carma_planning_msgs::msg::Route>("route", 1);
    route_state_pub_ = create_publisher<carma_planning_msgs::msg::RouteState>("route_state", 1);
 
    // NOTE: Currently, intra-process comms must be disabled for the following two publishers that are transient_local: https://github.com/ros2/rclcpp/issues/1753
    rclcpp::PublisherOptions intra_proc_disabled; 
    intra_proc_disabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; // Disable intra-process comms for this PublisherOptions object
 
    // 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::KeepAll()); // A publisher with this QoS will store all messages that it has sent on the topic
    pub_qos_transient_local.transient_local();  // A publisher with this QoS will re-send all (when KeepAll is used) messages to all late-joining subscribers 
                                          // NOTE: The subscriber's QoS must be set to transient_local() as well for earlier messages to be resent to the later-joiner.
  
    route_event_pub_ = create_publisher<carma_planning_msgs::msg::RouteEvent>("route_event", pub_qos_transient_local, intra_proc_disabled);
    route_marker_pub_ = create_publisher<visualization_msgs::msg::Marker>("route_marker", 1);
 
    // Setup service servers
    get_available_route_srv_ = create_service<carma_planning_msgs::srv::GetAvailableRoutes>("get_available_routes",
                                                            std::bind(&RouteGeneratorWorker::getAvailableRouteCb, &rg_worker_, std_ph::_1, std_ph::_2, std_ph::_3));
    set_active_route_srv_ = create_service<carma_planning_msgs::srv::SetActiveRoute>("set_active_route",
                                                            std::bind(&RouteGeneratorWorker::setActiveRouteCb, &rg_worker_, std_ph::_1, std_ph::_2, std_ph::_3));
    abort_active_route_srv_ = create_service<carma_planning_msgs::srv::AbortActiveRoute>("abort_active_route",
                                                            std::bind(&RouteGeneratorWorker::abortActiveRouteCb, &rg_worker_, std_ph::_1, std_ph::_2, std_ph::_3));
 
    // Set world model pointer from wm listener
    wm_ = wml_.getWorldModel();
    wml_.enableUpdatesWithoutRouteWL();
 
    // Configure route generator worker parameters
    rg_worker_.setClock(get_clock());
    rg_worker_.setLoggerInterface(get_node_logging_interface());
    rg_worker_.setWorldModelPtr(wm_);
    rg_worker_.setReroutingChecker(std::bind(&carma_wm::WMListener::checkIfReRoutingNeededWL, &wml_));
    rg_worker_.setDowntrackDestinationRange(config_.destination_downtrack_range);
    rg_worker_.setCrosstrackErrorDistance(config_.max_crosstrack_error);
    rg_worker_.setCrosstrackErrorCountMax(config_.cte_max_count);
    rg_worker_.setPublishers(route_event_pub_, route_state_pub_, route_pub_, route_marker_pub_);
    rg_worker_.initializeBumperTransformLookup();
 
    // Return success if everthing initialized successfully
    return CallbackReturn::SUCCESS;
  }

Here is the call graph for this function:

◆ parameter_update_callback()

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

Function callback for dynamic parameter updates.

Definition at line 40 of file route_node.cpp.

  {
    auto error = update_params<double>({
      {"max_crosstrack_error", config_.max_crosstrack_error},
      {"destination_downtrack_range", config_.destination_downtrack_range}, 
      {"route_spin_rate", config_.route_spin_rate}}, parameters);
    auto error_2 = update_params<int>({{"cte_max_count", config_.cte_max_count}}, parameters);
    auto error_3 = update_params<std::string>({{"route_file_path", config_.route_file_path}}, parameters);
 
    rcl_interfaces::msg::SetParametersResult result;
 
    result.successful = !error && !error_2 && !error_3;
 
    return result;
  }