Carma-platform v4.2.0
CARMA Platform is built on robot operating system (ROS) and utilizes open source software (OSS) that enables Cooperative Driving Automation (CDA) features to allow Automated Driving Systems to interact and cooperate with infrastructure and other vehicles through communication.
yield_plugin_node.cpp
Go to the documentation of this file.
1/*
2 * Copyright (C) 2022 LEIDOS.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
5 * use this file except in compliance with the License. You may obtain a copy of
6 * the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
12 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
13 * License for the specific language governing permissions and limitations under
14 * the License.
15 */
16
18
19namespace yield_plugin
20{
21 namespace std_ph = std::placeholders;
22
23 YieldPluginNode::YieldPluginNode(const rclcpp::NodeOptions &options)
24 : carma_guidance_plugins::TacticalPlugin(options),
25 version_id_("v1.0"),
26 config_(YieldPluginConfig())
27 {
28 // Declare parameters
29 config_.acceleration_adjustment_factor = declare_parameter<double>("acceleration_adjustment_factor", config_.acceleration_adjustment_factor);
30 config_.time_horizon_until_collision_to_commit_to_stop_in_s = declare_parameter<double>("time_horizon_until_collision_to_commit_to_stop_in_s", config_.time_horizon_until_collision_to_commit_to_stop_in_s);
31 config_.obstacle_zero_speed_threshold_in_ms = declare_parameter<double>("obstacle_zero_speed_threshold_in_ms", config_.obstacle_zero_speed_threshold_in_ms);
32 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);
33 config_.collision_check_radius_in_m = declare_parameter<double>("collision_check_radius_in_m", config_.collision_check_radius_in_m);
34 config_.yield_max_deceleration_in_ms2 = declare_parameter<double>("yield_max_deceleration_in_ms2", config_.yield_max_deceleration_in_ms2);
35 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);
36 config_.minimum_safety_gap_in_meters = declare_parameter<double>("minimum_safety_gap_in_meters", config_.minimum_safety_gap_in_meters);
37 config_.max_stop_speed_in_ms= declare_parameter<double>("max_stop_speed_in_ms", config_.max_stop_speed_in_ms);
38 config_.enable_cooperative_behavior = declare_parameter< bool>("enable_cooperative_behavior", config_.enable_cooperative_behavior);
39 config_.always_accept_mobility_request = declare_parameter< bool>("always_accept_mobility_request", config_.always_accept_mobility_request);
40 config_.acceptable_passed_timesteps = declare_parameter<int>("acceptable_passed_timesteps", config_.acceptable_passed_timesteps);
41 config_.consecutive_clearance_count_for_passed_obstacles_threshold = declare_parameter<int>("consecutive_clearance_count_for_passed_obstacles_threshold",
43 config_.intervehicle_collision_distance_in_m = declare_parameter<double>("intervehicle_collision_distance_in_m", config_.intervehicle_collision_distance_in_m);
44 config_.safety_collision_time_gap_in_s = declare_parameter<double>("safety_collision_time_gap_in_s", config_.safety_collision_time_gap_in_s);
45 config_.enable_adjustable_gap = declare_parameter<bool>("enable_adjustable_gap", config_.enable_adjustable_gap);
46 config_.acceptable_urgency = declare_parameter<int>("acceptable_urgency", config_.acceptable_urgency);
47 config_.speed_moving_average_window_size = declare_parameter<double>("speed_moving_average_window_size", config_.speed_moving_average_window_size);
48 config_.vehicle_length = declare_parameter<double>("vehicle_length", config_.vehicle_length);
49 config_.vehicle_height = declare_parameter<double>("vehicle_height", config_.vehicle_height);
50 config_.vehicle_width = declare_parameter<double>("vehicle_width", config_.vehicle_width);
51 config_.vehicle_id = declare_parameter<std::string>("vehicle_id", config_.vehicle_id);
52
53 }
54
55 carma_ros2_utils::CallbackReturn YieldPluginNode::on_configure_plugin()
56 {
58
59 get_parameter<double>("acceleration_adjustment_factor", config_.acceleration_adjustment_factor);
60 get_parameter<double>("time_horizon_until_collision_to_commit_to_stop_in_s", config_.time_horizon_until_collision_to_commit_to_stop_in_s);
61 get_parameter<double>("obstacle_zero_speed_threshold_in_ms", config_.obstacle_zero_speed_threshold_in_ms);
62 get_parameter<double>("on_route_vehicle_collision_horizon_in_s", config_.on_route_vehicle_collision_horizon_in_s);
63 get_parameter<double>("collision_check_radius_in_m", config_.collision_check_radius_in_m);
64 get_parameter<double>("yield_max_deceleration_in_ms2", config_.yield_max_deceleration_in_ms2);
65 get_parameter<double>("minimum_safety_gap_in_meters", config_.minimum_safety_gap_in_meters);
66 get_parameter<double>("max_stop_speed_in_ms", config_.max_stop_speed_in_ms);
67 get_parameter<bool>("enable_cooperative_behavior", config_.enable_cooperative_behavior);
68 get_parameter<bool>("always_accept_mobility_request", config_.always_accept_mobility_request);
69 get_parameter<int>("acceptable_passed_timesteps", config_.acceptable_passed_timesteps);
70 get_parameter<int>("consecutive_clearance_count_for_passed_obstacles_threshold", config_.consecutive_clearance_count_for_passed_obstacles_threshold);
71 get_parameter<double>("intervehicle_collision_distance_in_m", config_.intervehicle_collision_distance_in_m);
72 get_parameter<double>("safety_collision_time_gap_in_s", config_.safety_collision_time_gap_in_s);
73 get_parameter<bool>("enable_adjustable_gap", config_.enable_adjustable_gap);
74 get_parameter<int>("acceptable_urgency", config_.acceptable_urgency);
75 get_parameter<double>("speed_moving_average_window_size", config_.speed_moving_average_window_size);
76 get_parameter<double>("min_obj_avoidance_plan_time_in_s", config_.min_obj_avoidance_plan_time_in_s);
77 get_parameter<double>("vehicle_length", config_.vehicle_length);
78 get_parameter<double>("vehicle_height", config_.vehicle_height);
79 get_parameter<double>("vehicle_width", config_.vehicle_width);
80 get_parameter<std::string>("vehicle_id", config_.vehicle_id);
81
82 RCLCPP_INFO_STREAM(get_logger(), "YieldPlugin Params: " << config_);
83
84 worker_ = std::make_shared<YieldPlugin>(shared_from_this(), get_world_model(), config_,
85 [this](auto msg) { mob_resp_pub_->publish(msg); },
86 [this](auto msg) { lc_status_pub_->publish(msg); });
87
88 // Publisher
89 mob_resp_pub_ = create_publisher<carma_v2x_msgs::msg::MobilityResponse>("outgoing_mobility_response", 1);
90 lc_status_pub_ = create_publisher<carma_planning_msgs::msg::LaneChangeStatus>("cooperative_lane_change_status", 10);
91
92 // Subscriber
93 mob_request_sub_ = create_subscription<carma_v2x_msgs::msg::MobilityRequest>("incoming_mobility_request", 10, std::bind(&YieldPlugin::mobilityrequest_cb,worker_.get(),std_ph::_1));
94 bsm_sub_ = create_subscription<carma_v2x_msgs::msg::BSM>("bsm_outbound", 1, std::bind(&YieldPlugin::bsm_cb,worker_.get(),std_ph::_1));
95 georeference_sub_ = create_subscription<std_msgs::msg::String>("georeference", 10,
96 [this](const std_msgs::msg::String::SharedPtr msg) {
97 worker_->set_georeference_string(msg->data);
98 });
99
100 // Return success if everything initialized successfully
101 external_objects_sub_ = create_subscription<carma_perception_msgs::msg::ExternalObjectList>("external_object_predictions", 20,
102 [this](const carma_perception_msgs::msg::ExternalObjectList::SharedPtr msg) {
103 worker_->set_external_objects(msg->objects);
104 });
105 // Return success if everything initialized successfully
106 return CallbackReturn::SUCCESS;
107 }
108
110 std::shared_ptr<rmw_request_id_t> srv_header,
111 carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req,
112 carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp)
113 {
114 worker_->plan_trajectory_callback(req, resp);
115 }
116
118 {
119 return true;
120 }
121
123 {
124 return version_id_;
125 }
126
127} // yield_plugin
128
129#include "rclcpp_components/register_node_macro.hpp"
130
131// Register the component with class_loader
132RCLCPP_COMPONENTS_REGISTER_NODE(yield_plugin::YieldPluginNode)
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...
ROS node for the YieldPlugin.
YieldPluginNode(const rclcpp::NodeOptions &)
Node constructor.
rclcpp_lifecycle::LifecyclePublisher< carma_planning_msgs::msg::LaneChangeStatus >::SharedPtr lc_status_pub_
rclcpp::Subscription< carma_v2x_msgs::msg::MobilityRequest >::SharedPtr mob_request_sub_
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 traj...
rclcpp::Subscription< carma_v2x_msgs::msg::BSM >::SharedPtr bsm_sub_
std::shared_ptr< YieldPlugin > worker_
rclcpp::Subscription< carma_perception_msgs::msg::ExternalObjectList >::SharedPtr external_objects_sub_
std::string get_version_id() override final
Returns the version id of this plugin.
bool get_availability() override
Get the availability status of this plugin based on the current operating environment....
carma_ros2_utils::CallbackReturn on_configure_plugin()
Method which is triggered when this plugin is moved from the UNCONFIGURED to INACTIVE states....
rclcpp_lifecycle::LifecyclePublisher< carma_v2x_msgs::msg::MobilityResponse >::SharedPtr mob_resp_pub_
rclcpp::Subscription< std_msgs::msg::String >::SharedPtr georeference_sub_
void mobilityrequest_cb(const carma_v2x_msgs::msg::MobilityRequest::UniquePtr msg)
callback for mobility request
void bsm_cb(const carma_v2x_msgs::msg::BSM::UniquePtr msg)
callback for bsm message
Stuct containing the algorithm configuration values for the YieldPluginConfig.
bool always_accept_mobility_request
double max_stop_speed_in_ms
double collision_check_radius_in_m
std::string vehicle_id
double speed_moving_average_window_size
int consecutive_clearance_count_for_passed_obstacles_threshold
double acceleration_adjustment_factor
double yield_max_deceleration_in_ms2
double time_horizon_until_collision_to_commit_to_stop_in_s
double intervehicle_collision_distance_in_m
double obstacle_zero_speed_threshold_in_ms
double on_route_vehicle_collision_horizon_in_s
bool enable_cooperative_behavior
double min_obj_avoidance_plan_time_in_s
double minimum_safety_gap_in_meters
double safety_collision_time_gap_in_s