yield_plugin::YieldPluginNode Class Reference

ROS node for the YieldPlugin. More...

#include <yield_plugin_node.hpp>

Inheritance diagram for yield_plugin::YieldPluginNode:

Collaboration diagram for yield_plugin::YieldPluginNode:

Public Member Functions
	YieldPluginNode (const rclcpp::NodeOptions &)
	Node constructor. More...
carma_ros2_utils::CallbackReturn	on_configure_plugin ()
	Method which is triggered when this plugin is moved from the UNCONFIGURED to INACTIVE states. This method should be used to load parameters and is required to be implemented. More...
bool	get_availability () override
	Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes. More...
std::string	get_version_id () override final
	Returns the version id of this plugin. More...
void	plan_trajectory_callback (std::shared_ptr< rmw_request_id_t > srv_header, carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) override
	Extending class provided callback which should return a planned trajectory based on the provided trajectory planning request. More...
Public Member Functions inherited from carma_guidance_plugins::TacticalPlugin
	TacticalPlugin (const rclcpp::NodeOptions &)
	TacticalPlugin constructor. More...
virtual	~TacticalPlugin ()=default
	Virtual destructor for safe deletion. More...
virtual void	plan_trajectory_callback (std::shared_ptr< rmw_request_id_t > srv_header, carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp)=0
	Extending class provided callback which should return a planned trajectory based on the provided trajectory planning request. More...
std::string	get_capability () override
	Get the capability string representing this plugins capabilities Method must be overriden by extending classes. Expectation is that abstract plugin type parent classes will provide a default implementation. More...
uint8_t	get_type () override final
	Returns the type of this plugin according to the carma_planning_msgs::Plugin type enum. Extending classes for the specific type should override this method. More...
carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &) override final
carma_ros2_utils::CallbackReturn	handle_on_activate (const rclcpp_lifecycle::State &) override final
carma_ros2_utils::CallbackReturn	handle_on_deactivate (const rclcpp_lifecycle::State &) override final
carma_ros2_utils::CallbackReturn	handle_on_cleanup (const rclcpp_lifecycle::State &) override final
carma_ros2_utils::CallbackReturn	handle_on_shutdown (const rclcpp_lifecycle::State &) override final
carma_ros2_utils::CallbackReturn	handle_on_error (const rclcpp_lifecycle::State &, const std::string &exception_string) override final
Public Member Functions inherited from carma_guidance_plugins::PluginBaseNode
	PluginBaseNode (const rclcpp::NodeOptions &)
	PluginBaseNode constructor. More...
virtual	~PluginBaseNode ()=default
	Virtual destructor for safe deletion. More...
virtual std::shared_ptr< carma_wm::WMListener >	get_world_model_listener () final
	Method to return the default world model listener provided as a convience by this base class If this method or get_world_model() are not called then the world model remains uninitialized and will not create unnecessary subscriptions. More...
virtual carma_wm::WorldModelConstPtr	get_world_model () final
	Method to return the default world model provided as a convience by this base class If this method or get_world_model_listener() are not called then the world model remains uninitialized and will not create unnecessary subscriptions. More...
virtual bool	get_activation_status () final
	Returns the activation status of this plugin. The plugins API callbacks will only be triggered when this method returns true. More...
virtual uint8_t	get_type ()
	Returns the type of this plugin according to the carma_planning_msgs::Plugin type enum. Extending classes for the specific type should override this method. More...
std::string	get_plugin_name_and_ns () const
	Return the name of this plugin with namespace. NOTE: If only the name of the plugin is required, use get_plugin_name() More...
std::string	get_plugin_name () const
	Return the name of this plugin. More...
virtual bool	get_availability ()=0
	Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes. More...
virtual std::string	get_capability ()=0
	Get the capability string representing this plugins capabilities Method must be overriden by extending classes. Expectation is that abstract plugin type parent classes will provide a default implementation. More...
virtual std::string	get_version_id ()=0
	Returns the version id of this plugin. More...
virtual carma_ros2_utils::CallbackReturn	on_configure_plugin ()=0
	Method which is triggered when this plugin is moved from the UNCONFIGURED to INACTIVE states. This method should be used to load parameters and is required to be implemented. More...
virtual carma_ros2_utils::CallbackReturn	on_activate_plugin ()
	Method which is triggered when this plugin is moved from the INACTIVE to ACTIVE states. This method should be used to prepare for future callbacks for plugin's capabilites. More...
virtual carma_ros2_utils::CallbackReturn	on_deactivate_plugin ()
	Method which is triggered when this plugin is moved from the ACTIVE to INACTIVE states. This method should be used to disable any functionality which should cease execution when plugin is inactive. More...
virtual carma_ros2_utils::CallbackReturn	on_cleanup_plugin ()
	Method which is triggered when this plugin is moved from the INACTIVE to UNCONFIGURED states. This method should be used to fully reset the plugin such that a future call to on_configure_plugin would leave the plugin in a fresh state as though just launched. More...
virtual carma_ros2_utils::CallbackReturn	on_shutdown_plugin ()
	Method which is triggered when this plugin is moved from any state to FINALIZED This method should be used to generate any shutdown logs or final cleanup. More...
virtual carma_ros2_utils::CallbackReturn	on_error_plugin (const std::string &exception_string)
	Method which is triggered when an unhandled exception occurs in this plugin This method should be used to cleanup such that the plugin could be moved to UNCONFIGURED state if possible. More...
carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &) override
carma_ros2_utils::CallbackReturn	handle_on_activate (const rclcpp_lifecycle::State &) override
carma_ros2_utils::CallbackReturn	handle_on_deactivate (const rclcpp_lifecycle::State &) override
carma_ros2_utils::CallbackReturn	handle_on_cleanup (const rclcpp_lifecycle::State &) override
carma_ros2_utils::CallbackReturn	handle_on_shutdown (const rclcpp_lifecycle::State &) override
carma_ros2_utils::CallbackReturn	handle_on_error (const rclcpp_lifecycle::State &, const std::string &exception_string) override
	FRIEND_TEST (carma_guidance_plugins_test, connections_test)

Private Attributes
YieldPluginConfig	config_
std::shared_ptr< YieldPlugin >	worker_
std::string	version_id_
rclcpp_lifecycle::LifecyclePublisher< carma_v2x_msgs::msg::MobilityResponse >::SharedPtr	mob_resp_pub_
rclcpp_lifecycle::LifecyclePublisher< carma_planning_msgs::msg::LaneChangeStatus >::SharedPtr	lc_status_pub_
rclcpp::Subscription< carma_v2x_msgs::msg::MobilityRequest >::SharedPtr	mob_request_sub_
rclcpp::Subscription< carma_v2x_msgs::msg::BSM >::SharedPtr	bsm_sub_
rclcpp::Subscription< carma_perception_msgs::msg::ExternalObjectList >::SharedPtr	external_objects_sub_
rclcpp::Subscription< std_msgs::msg::String >::SharedPtr	georeference_sub_

Detailed Description

ROS node for the YieldPlugin.

Definition at line 37 of file yield_plugin_node.hpp.

Constructor & Destructor Documentation

YieldPluginNode()

yield_plugin::YieldPluginNode::YieldPluginNode ( const rclcpp::NodeOptions &  options )

explicit

Node constructor.

Definition at line 23 of file yield_plugin_node.cpp.

      : carma_guidance_plugins::TacticalPlugin(options),
        version_id_("v1.0"),
        config_(YieldPluginConfig())
  {
    // Declare parameters
    config_.acceleration_adjustment_factor = declare_parameter<double>("acceleration_adjustment_factor", config_.acceleration_adjustment_factor);
    config_.obstacle_zero_speed_threshold_in_ms = declare_parameter<double>("obstacle_zero_speed_threshold_in_ms", config_.obstacle_zero_speed_threshold_in_ms);
    config_.on_route_vehicle_collision_horizon_in_s = declare_parameter<double>("on_route_vehicle_collision_horizon_in_s", config_.on_route_vehicle_collision_horizon_in_s);
    config_.collision_check_radius_in_m = declare_parameter<double>("collision_check_radius_in_m", config_.collision_check_radius_in_m);
    config_.yield_max_deceleration_in_ms2 = declare_parameter<double>("yield_max_deceleration_in_ms2", config_.yield_max_deceleration_in_ms2);
    config_.min_obj_avoidance_plan_time_in_s = declare_parameter<double>("min_obj_avoidance_plan_time_in_s", config_.min_obj_avoidance_plan_time_in_s);
    config_.minimum_safety_gap_in_meters = declare_parameter<double>("minimum_safety_gap_in_meters", config_.minimum_safety_gap_in_meters);
    config_.max_stop_speed_in_ms= declare_parameter<double>("max_stop_speed_in_ms", config_.max_stop_speed_in_ms);
    config_.enable_cooperative_behavior = declare_parameter< bool>("enable_cooperative_behavior", config_.enable_cooperative_behavior);
    config_.always_accept_mobility_request = declare_parameter< bool>("always_accept_mobility_request", config_.always_accept_mobility_request);
    config_.acceptable_passed_timesteps = declare_parameter<int>("acceptable_passed_timesteps", config_.acceptable_passed_timesteps);
    config_.consecutive_clearance_count_for_obstacles_threshold = declare_parameter<int>("consecutive_clearance_count_for_obstacles_threshold",
      config_.consecutive_clearance_count_for_obstacles_threshold);
    config_.intervehicle_collision_distance_in_m = declare_parameter<double>("intervehicle_collision_distance_in_m", config_.intervehicle_collision_distance_in_m);
    config_.safety_collision_time_gap_in_s = declare_parameter<double>("safety_collision_time_gap_in_s", config_.safety_collision_time_gap_in_s);
    config_.enable_adjustable_gap = declare_parameter<bool>("enable_adjustable_gap", config_.enable_adjustable_gap);
    config_.acceptable_urgency = declare_parameter<int>("acceptable_urgency", config_.acceptable_urgency);
    config_.speed_moving_average_window_size = declare_parameter<double>("speed_moving_average_window_size", config_.speed_moving_average_window_size);
    config_.vehicle_length = declare_parameter<double>("vehicle_length", config_.vehicle_length);
    config_.vehicle_height = declare_parameter<double>("vehicle_height", config_.vehicle_height);
    config_.vehicle_width = declare_parameter<double>("vehicle_width", config_.vehicle_width);
    config_.vehicle_id = declare_parameter<std::string>("vehicle_id", config_.vehicle_id);
 
  }

References YieldPluginConfig::acceleration_adjustment_factor, YieldPluginConfig::acceptable_passed_timesteps, YieldPluginConfig::acceptable_urgency, YieldPluginConfig::always_accept_mobility_request, YieldPluginConfig::collision_check_radius_in_m, config_, YieldPluginConfig::consecutive_clearance_count_for_obstacles_threshold, YieldPluginConfig::enable_adjustable_gap, YieldPluginConfig::enable_cooperative_behavior, YieldPluginConfig::intervehicle_collision_distance_in_m, YieldPluginConfig::max_stop_speed_in_ms, YieldPluginConfig::min_obj_avoidance_plan_time_in_s, YieldPluginConfig::minimum_safety_gap_in_meters, YieldPluginConfig::obstacle_zero_speed_threshold_in_ms, YieldPluginConfig::on_route_vehicle_collision_horizon_in_s, YieldPluginConfig::safety_collision_time_gap_in_s, YieldPluginConfig::speed_moving_average_window_size, YieldPluginConfig::vehicle_height, YieldPluginConfig::vehicle_id, YieldPluginConfig::vehicle_length, YieldPluginConfig::vehicle_width, and YieldPluginConfig::yield_max_deceleration_in_ms2.

Member Function Documentation

get_availability()

bool yield_plugin::YieldPluginNode::get_availability ( )

overridevirtual

Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes.

Returns

This method should return true if the plugin's current understanding of the world means it would be capable of planning or executing its capability.

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 115 of file yield_plugin_node.cpp.

  {
    return true;
  }

get_version_id()

std::string yield_plugin::YieldPluginNode::get_version_id ( )

finaloverridevirtual

Returns the version id of this plugin.

Returns

The version id represented as a string

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 120 of file yield_plugin_node.cpp.

  {
    return version_id_;
  }

References version_id_.

on_configure_plugin()

carma_ros2_utils::CallbackReturn yield_plugin::YieldPluginNode::on_configure_plugin ( )

virtual

Method which is triggered when this plugin is moved from the UNCONFIGURED to INACTIVE states. This method should be used to load parameters and is required to be implemented.

Returns

SUCCESS, FAILURE, or ERROR. Transition to INACTIVE will only complete on SUCCESS.

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 54 of file yield_plugin_node.cpp.

  {
    config_ = YieldPluginConfig();
 
    get_parameter<double>("acceleration_adjustment_factor", config_.acceleration_adjustment_factor);
    get_parameter<double>("obstacle_zero_speed_threshold_in_ms", config_.obstacle_zero_speed_threshold_in_ms);
    get_parameter<double>("on_route_vehicle_collision_horizon_in_s", config_.on_route_vehicle_collision_horizon_in_s);
    get_parameter<double>("collision_check_radius_in_m", config_.collision_check_radius_in_m);
    get_parameter<double>("yield_max_deceleration_in_ms2", config_.yield_max_deceleration_in_ms2);
    get_parameter<double>("minimum_safety_gap_in_meters", config_.minimum_safety_gap_in_meters);
    get_parameter<double>("max_stop_speed_in_ms", config_.max_stop_speed_in_ms);
    get_parameter<bool>("enable_cooperative_behavior", config_.enable_cooperative_behavior);
    get_parameter<bool>("always_accept_mobility_request", config_.always_accept_mobility_request);
    get_parameter<int>("acceptable_passed_timesteps", config_.acceptable_passed_timesteps);
    get_parameter<int>("consecutive_clearance_count_for_obstacles_threshold", config_.consecutive_clearance_count_for_obstacles_threshold);
    get_parameter<double>("intervehicle_collision_distance_in_m", config_.intervehicle_collision_distance_in_m);
    get_parameter<double>("safety_collision_time_gap_in_s", config_.safety_collision_time_gap_in_s);
    get_parameter<bool>("enable_adjustable_gap", config_.enable_adjustable_gap);
    get_parameter<int>("acceptable_urgency", config_.acceptable_urgency);
    get_parameter<double>("speed_moving_average_window_size", config_.speed_moving_average_window_size);
    get_parameter<double>("min_obj_avoidance_plan_time_in_s", config_.min_obj_avoidance_plan_time_in_s);
    get_parameter<double>("vehicle_length", config_.vehicle_length);
    get_parameter<double>("vehicle_height", config_.vehicle_height);
    get_parameter<double>("vehicle_width", config_.vehicle_width);
    get_parameter<std::string>("vehicle_id", config_.vehicle_id);
 
    RCLCPP_INFO_STREAM(get_logger(), "YieldPlugin Params: " << config_);
 
    worker_ = std::make_shared<YieldPlugin>(shared_from_this(), get_world_model(), config_,
                                                          [this](auto msg) { mob_resp_pub_->publish(msg); },
                                                          [this](auto msg) { lc_status_pub_->publish(msg); });
 
     // Publisher
    mob_resp_pub_ = create_publisher<carma_v2x_msgs::msg::MobilityResponse>("outgoing_mobility_response", 1);
    lc_status_pub_ = create_publisher<carma_planning_msgs::msg::LaneChangeStatus>("cooperative_lane_change_status", 10);
 
    // Subscriber
    mob_request_sub_ = create_subscription<carma_v2x_msgs::msg::MobilityRequest>("incoming_mobility_request", 10, std::bind(&YieldPlugin::mobilityrequest_cb,worker_.get(),std_ph::_1));
    bsm_sub_ = create_subscription<carma_v2x_msgs::msg::BSM>("bsm_outbound", 1, std::bind(&YieldPlugin::bsm_cb,worker_.get(),std_ph::_1));
    georeference_sub_ = create_subscription<std_msgs::msg::String>("georeference", 10,
                                                                  [this](const std_msgs::msg::String::SharedPtr msg) {
                                                                    worker_->set_georeference_string(msg->data);
                                                                  });
 
    // Return success if everything initialized successfully
    external_objects_sub_ = create_subscription<carma_perception_msgs::msg::ExternalObjectList>("external_object_predictions", 20,
                                                                                                [this](const carma_perception_msgs::msg::ExternalObjectList::SharedPtr msg) {
                                                                                                  worker_->set_external_objects(msg->objects);
                                                                                                });
    // Return success if everything initialized successfully
    return CallbackReturn::SUCCESS;
  }

References YieldPluginConfig::acceleration_adjustment_factor, YieldPluginConfig::acceptable_passed_timesteps, YieldPluginConfig::acceptable_urgency, YieldPluginConfig::always_accept_mobility_request, yield_plugin::YieldPlugin::bsm_cb(), bsm_sub_, YieldPluginConfig::collision_check_radius_in_m, config_, YieldPluginConfig::consecutive_clearance_count_for_obstacles_threshold, YieldPluginConfig::enable_adjustable_gap, YieldPluginConfig::enable_cooperative_behavior, external_objects_sub_, georeference_sub_, carma_guidance_plugins::PluginBaseNode::get_world_model(), YieldPluginConfig::intervehicle_collision_distance_in_m, lc_status_pub_, YieldPluginConfig::max_stop_speed_in_ms, YieldPluginConfig::min_obj_avoidance_plan_time_in_s, YieldPluginConfig::minimum_safety_gap_in_meters, mob_request_sub_, mob_resp_pub_, yield_plugin::YieldPlugin::mobilityrequest_cb(), YieldPluginConfig::obstacle_zero_speed_threshold_in_ms, YieldPluginConfig::on_route_vehicle_collision_horizon_in_s, YieldPluginConfig::safety_collision_time_gap_in_s, YieldPluginConfig::speed_moving_average_window_size, YieldPluginConfig::vehicle_height, YieldPluginConfig::vehicle_id, YieldPluginConfig::vehicle_length, YieldPluginConfig::vehicle_width, worker_, and YieldPluginConfig::yield_max_deceleration_in_ms2.

Here is the call graph for this function:

plan_trajectory_callback()

void yield_plugin::YieldPluginNode::plan_trajectory_callback ( std::shared_ptr< rmw_request_id_t >  srv_header, carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr  req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr  resp  )

overridevirtual

Extending class provided callback which should return a planned trajectory based on the provided trajectory planning request.

Parameters

srv_header	RCL header for services calls. Can usually be ignored by implementers.
req	The service request containing the maneuvers to plan trajectories for and current vehicle state
resp	The response containing the planned trajectory

Implements carma_guidance_plugins::TacticalPlugin.

Definition at line 107 of file yield_plugin_node.cpp.

  {
    worker_->plan_trajectory_callback(req, resp);
  }

References worker_.

Member Data Documentation

bsm_sub_

rclcpp::Subscription<carma_v2x_msgs::msg::BSM>::SharedPtr yield_plugin::YieldPluginNode::bsm_sub_

private

Definition at line 76 of file yield_plugin_node.hpp.

Referenced by on_configure_plugin().

config_

YieldPluginConfig yield_plugin::YieldPluginNode::config_

private

Definition at line 63 of file yield_plugin_node.hpp.

Referenced by YieldPluginNode(), and on_configure_plugin().

external_objects_sub_

rclcpp::Subscription<carma_perception_msgs::msg::ExternalObjectList>::SharedPtr yield_plugin::YieldPluginNode::external_objects_sub_

private

Definition at line 77 of file yield_plugin_node.hpp.

Referenced by on_configure_plugin().

georeference_sub_

rclcpp::Subscription<std_msgs::msg::String>::SharedPtr yield_plugin::YieldPluginNode::georeference_sub_

private

Definition at line 78 of file yield_plugin_node.hpp.

Referenced by on_configure_plugin().

lc_status_pub_

rclcpp_lifecycle::LifecyclePublisher<carma_planning_msgs::msg::LaneChangeStatus>::SharedPtr yield_plugin::YieldPluginNode::lc_status_pub_

private

Definition at line 72 of file yield_plugin_node.hpp.

Referenced by on_configure_plugin().

mob_request_sub_

rclcpp::Subscription<carma_v2x_msgs::msg::MobilityRequest>::SharedPtr yield_plugin::YieldPluginNode::mob_request_sub_

private

Definition at line 75 of file yield_plugin_node.hpp.

Referenced by on_configure_plugin().

mob_resp_pub_

rclcpp_lifecycle::LifecyclePublisher<carma_v2x_msgs::msg::MobilityResponse>::SharedPtr yield_plugin::YieldPluginNode::mob_resp_pub_

private

Definition at line 71 of file yield_plugin_node.hpp.

Referenced by on_configure_plugin().

version_id_

std::string yield_plugin::YieldPluginNode::version_id_

private

Definition at line 68 of file yield_plugin_node.hpp.

Referenced by get_version_id().

worker_

std::shared_ptr<YieldPlugin> yield_plugin::YieldPluginNode::worker_

private

Definition at line 66 of file yield_plugin_node.hpp.

Referenced by on_configure_plugin(), and plan_trajectory_callback().

---

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

• yield_plugin/include/yield_plugin/yield_plugin_node.hpp
• yield_plugin/src/yield_plugin_node.cpp