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::YieldPlugin Class Reference

Class containing primary business logic for the In-Lane Cruising Plugin. More...

#include <yield_plugin.hpp>

Collaboration diagram for yield_plugin::YieldPlugin:
Collaboration graph

Public Member Functions

 YieldPlugin (std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode > nh, carma_wm::WorldModelConstPtr wm, YieldPluginConfig config, MobilityResponseCB mobility_response_publisher, LaneChangeStatusCB lc_status_publisher)
 Constructor. More...
 
void plan_trajectory_callback (carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp)
 Service callback for trajectory planning. More...
 
carma_planning_msgs::msg::TrajectoryPlan update_traj_for_object (const carma_planning_msgs::msg::TrajectoryPlan &original_tp, const std::vector< carma_perception_msgs::msg::ExternalObject > &external_objects, double initial_velocity)
 trajectory is modified to safely avoid obstacles on the road More...
 
double polynomial_calc (std::vector< double > coeff, double x) const
 calculate quintic polynomial equation for a given x More...
 
double polynomial_calc_d (std::vector< double > coeff, double x) const
 calculate derivative of quintic polynomial equation for a given x More...
 
double max_trajectory_speed (const std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > &trajectory_points, double timestamp_in_sec_to_search_until) const
 calculates the maximum speed in a set of tajectory points More...
 
std::vector< double > get_relative_downtracks (const carma_planning_msgs::msg::TrajectoryPlan &trajectory_plan) const
 calculates distance between trajectory points in a plan More...
 
void mobilityrequest_cb (const carma_v2x_msgs::msg::MobilityRequest::UniquePtr msg)
 callback for mobility request More...
 
void bsm_cb (const carma_v2x_msgs::msg::BSM::UniquePtr msg)
 callback for bsm message More...
 
std::vector< lanelet::BasicPoint2d > convert_eceftrajectory_to_mappoints (const carma_v2x_msgs::msg::Trajectory &ecef_trajectory) const
 convert a carma trajectory from ecef frame to map frame ecef trajectory consists of the point and a set of offsets with reference to the point More...
 
lanelet::BasicPoint2d ecef_to_map_point (const carma_v2x_msgs::msg::LocationECEF &ecef_point) const
 convert a point in ecef frame (in cm) into map frame (in meters) More...
 
carma_v2x_msgs::msg::MobilityResponse compose_mobility_response (const std::string &resp_recipient_id, const std::string &req_plan_id, bool response) const
 compose a mobility response message More...
 
carma_planning_msgs::msg::TrajectoryPlan generate_JMT_trajectory (const carma_planning_msgs::msg::TrajectoryPlan &original_tp, double initial_pos, double goal_pos, double initial_velocity, double goal_velocity, double planning_time, double original_max_speed)
 generate a Jerk Minimizing Trajectory(JMT) with the provided start and end conditions More...
 
carma_planning_msgs::msg::TrajectoryPlan update_traj_for_cooperative_behavior (const carma_planning_msgs::msg::TrajectoryPlan &original_tp, double current_speed)
 update trajectory for yielding to an incoming cooperative behavior More...
 
std::vector< std::pair< int, lanelet::BasicPoint2d > > detect_trajectories_intersection (std::vector< lanelet::BasicPoint2d > self_trajectory, std::vector< lanelet::BasicPoint2d > incoming_trajectory) const
 detect intersection point(s) of two trajectories More...
 
void set_incoming_request_info (std::vector< lanelet::BasicPoint2d > req_trajectory, double req_speed, double req_planning_time, double req_timestamp)
 set values for member variables related to cooperative behavior More...
 
void lookup_ecef_to_map_transform ()
 Looks up the transform between map and earth frames, and sets the member variable. More...
 
double check_traj_for_digital_min_gap (const carma_planning_msgs::msg::TrajectoryPlan &original_tp) const
 checks trajectory for minimum gap associated with it from the road More...
 
void set_georeference_string (const std::string &georeference)
 Setter for map projection string to define lat/lon -> map conversion. More...
 
void set_external_objects (const std::vector< carma_perception_msgs::msg::ExternalObject > &object_list)
 Setter for external objects with predictions in the environment. More...
 
std::optional< GetCollisionResultget_collision (const carma_planning_msgs::msg::TrajectoryPlan &trajectory1, const std::vector< carma_perception_msgs::msg::PredictedState > &trajectory2, double collision_radius, double trajectory1_max_speed)
 Return naive collision time and locations based on collision radius given two trajectories with one being obstacle's predicted steps. More...
 
std::optional< rclcpp::Time > get_collision_time (const carma_planning_msgs::msg::TrajectoryPlan &original_tp, const carma_perception_msgs::msg::ExternalObject &curr_obstacle, double original_tp_max_speed)
 Return collision time given two trajectories with one being external object with predicted steps. More...
 
bool is_object_behind_vehicle (uint32_t object_id, const rclcpp::Time &collision_time, double vehicle_point, double object_downtrack)
 Check if object location is behind the vehicle using estimates of the vehicle's length and route downtracks. More...
 
std::optional< std::pair< carma_perception_msgs::msg::ExternalObject, double > > get_earliest_collision_object_and_time (const carma_planning_msgs::msg::TrajectoryPlan &original_tp, const std::vector< carma_perception_msgs::msg::ExternalObject > &external_objects)
 Return the earliest collision object and time of collision pair from the given trajectory and list of external objects with predicted states. Function first filters obstacles based on whether if their any of predicted state will be on the route. Only then, the logic compares trajectory and predicted states. More...
 
std::unordered_map< uint32_t, rclcpp::Time > get_collision_times_concurrently (const carma_planning_msgs::msg::TrajectoryPlan &original_tp, const std::vector< carma_perception_msgs::msg::ExternalObject > &external_objects, double original_tp_max_speed)
 Given the list of objects with predicted states, get all collision times concurrently using multi-threading. More...
 
double get_predicted_velocity_at_time (const geometry_msgs::msg::Twist &object_velocity_in_map_frame, const carma_planning_msgs::msg::TrajectoryPlan &original_tp, double timestamp_in_sec_to_predict)
 Given the object velocity in map frame with x,y components, this function returns the projected velocity along the trajectory at given time. More...
 

Private Member Functions

std::string bsmIDtoString (carma_v2x_msgs::msg::BSMCoreData bsm_core)
 

Private Attributes

carma_wm::WorldModelConstPtr wm_
 
YieldPluginConfig config_
 
MobilityResponseCB mobility_response_publisher_
 
LaneChangeStatusCB lc_status_publisher_
 
std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode > nh_
 
std::set< lanelet::Id > route_llt_ids_
 
lanelet::Id previous_llt_id_
 
std::vector< carma_perception_msgs::msg::ExternalObject > external_objects_
 
std::unordered_map< uint32_t, int > consecutive_clearance_count_for_obstacles_
 
bool cooperative_request_acceptable_ = false
 
std::vector< lanelet::BasicPoint2d > req_trajectory_points_
 
double req_target_speed_ = 0
 
double req_timestamp_ = 0
 
double req_target_plan_time_ = 0
 
int timesteps_since_last_req_ = 0
 
int clc_urgency_ = 0
 
std::optional< double > last_speed_ = std::nullopt
 
std::optional< rclcpp::Time > last_speed_time_ = std::nullopt
 
double ecef_traj_timestep_ = 0.1
 
geometry_msgs::msg::Vector3 host_vehicle_size
 
double current_speed_
 
std::string host_bsm_id_
 
std::string georeference_ {""}
 
std::shared_ptr< lanelet::projection::LocalFrameProjector > map_projector_
 

Detailed Description

Class containing primary business logic for the In-Lane Cruising Plugin.

Definition at line 77 of file yield_plugin.hpp.

Constructor & Destructor Documentation

◆ YieldPlugin()

yield_plugin::YieldPlugin::YieldPlugin ( std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode >  nh,
carma_wm::WorldModelConstPtr  wm,
YieldPluginConfig  config,
MobilityResponseCB  mobility_response_publisher,
LaneChangeStatusCB  lc_status_publisher 
)

Constructor.

Parameters
wmPointer to intialized instance of the carma world model for accessing semantic map data
configThe configuration to be used for this object
mobility_response_publisherCallback which will publish the mobility response
lc_status_publisherCallback which will publish the cooperative lane change status

Definition at line 45 of file yield_plugin.cpp.

48 : nh_(nh), wm_(wm), config_(config),mobility_response_publisher_(mobility_response_publisher), lc_status_publisher_(lc_status_publisher)
49 {
50
51 }
LaneChangeStatusCB lc_status_publisher_
MobilityResponseCB mobility_response_publisher_
std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode > nh_
YieldPluginConfig config_
carma_wm::WorldModelConstPtr wm_

Member Function Documentation

◆ bsm_cb()

void yield_plugin::YieldPlugin::bsm_cb ( const carma_v2x_msgs::msg::BSM::UniquePtr  msg)

callback for bsm message

Parameters
msgmobility bsm message

Definition at line 246 of file yield_plugin.cpp.

247 {
248 carma_v2x_msgs::msg::BSMCoreData bsm_core_ = msg->core_data;
249 host_bsm_id_ = bsmIDtoString(bsm_core_);
250 }
std::string bsmIDtoString(carma_v2x_msgs::msg::BSMCoreData bsm_core)

References bsmIDtoString(), and host_bsm_id_.

Referenced by yield_plugin::YieldPluginNode::on_configure_plugin().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ bsmIDtoString()

std::string yield_plugin::YieldPlugin::bsmIDtoString ( carma_v2x_msgs::msg::BSMCoreData  bsm_core)
inlineprivate

Definition at line 343 of file yield_plugin.hpp.

344 {
345 std::string res = "";
346 for (size_t i=0; i<bsm_core.id.size(); i++)
347 {
348 res+=std::to_string(bsm_core.id[i]);
349 }
350 return res;
351 }
auto to_string(const UtmZone &zone) -> std::string
Definition: utm_zone.cpp:21

References process_bag::i, and carma_cooperative_perception::to_string().

Referenced by bsm_cb().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ check_traj_for_digital_min_gap()

double yield_plugin::YieldPlugin::check_traj_for_digital_min_gap ( const carma_planning_msgs::msg::TrajectoryPlan &  original_tp) const

checks trajectory for minimum gap associated with it from the road

Parameters
original_tporiginal trajectory plan
Returns
minumum required min_gap from the road, if none exists, return default minimum_safety_gap_in_meters

Definition at line 1072 of file yield_plugin.cpp.

1073 {
1074 double desired_gap = 0;
1075
1076 for (size_t i = 0; i < original_tp.trajectory_points.size(); i++)
1077 {
1078 lanelet::BasicPoint2d veh_pos(original_tp.trajectory_points.at(i).x, original_tp.trajectory_points.at(i).y);
1079 auto llts = wm_->getLaneletsFromPoint(veh_pos, 1);
1080 if (llts.empty())
1081 {
1082 // This should technically never happen
1083 // However, trajectory generation currently may fail due to osm map issue https://github.com/usdot-fhwa-stol/carma-platform/issues/2503
1084 RCLCPP_WARN_STREAM(nh_->get_logger(),"Trajectory point: x= " << original_tp.trajectory_points.at(i).x << "y="<< original_tp.trajectory_points.at(i).y);
1085 RCLCPP_WARN_STREAM(nh_->get_logger(),"Trajectory is not on the road, so was unable to get the digital minimum gap. Returning default minimum_safety_gap_in_meters: " << config_.minimum_safety_gap_in_meters);
1086 return desired_gap;
1087 }
1088 auto digital_min_gap = llts[0].regulatoryElementsAs<lanelet::DigitalMinimumGap>(); //Returns a list of these elements)
1089 if (!digital_min_gap.empty())
1090 {
1091 double digital_gap = digital_min_gap[0]->getMinimumGap(); // Provided gap is in meters
1092 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Digital Gap found with value: " << digital_gap);
1093 desired_gap = std::max(desired_gap, digital_gap);
1094 }
1095 }
1096 return desired_gap;
1097 }
double minimum_safety_gap_in_meters

References config_, process_bag::i, YieldPluginConfig::minimum_safety_gap_in_meters, nh_, and wm_.

Referenced by update_traj_for_cooperative_behavior(), and update_traj_for_object().

Here is the caller graph for this function:

◆ compose_mobility_response()

carma_v2x_msgs::msg::MobilityResponse yield_plugin::YieldPlugin::compose_mobility_response ( const std::string &  resp_recipient_id,
const std::string &  req_plan_id,
bool  response 
) const

compose a mobility response message

Parameters
resp_recipient_idvehicle id of the recipient of the message
req_plan_idplan id from the requested message
responseaccept/reject to the response based on conditions
Returns
filled mobility response

Definition at line 133 of file yield_plugin.cpp.

134 {
135 carma_v2x_msgs::msg::MobilityResponse out_mobility_response;
136 out_mobility_response.m_header.sender_id = config_.vehicle_id;
137 out_mobility_response.m_header.recipient_id = resp_recipient_id;
138 out_mobility_response.m_header.sender_bsm_id = host_bsm_id_;
139 out_mobility_response.m_header.plan_id = req_plan_id;
140 out_mobility_response.m_header.timestamp = nh_->now().seconds()*1000;
141
142
144 {
145 out_mobility_response.is_accepted = true;
146 }
147 else out_mobility_response.is_accepted = false;
148
149 return out_mobility_response;
150 }
bool always_accept_mobility_request
std::string vehicle_id

References YieldPluginConfig::always_accept_mobility_request, config_, host_bsm_id_, nh_, and YieldPluginConfig::vehicle_id.

Referenced by mobilityrequest_cb().

Here is the caller graph for this function:

◆ convert_eceftrajectory_to_mappoints()

std::vector< lanelet::BasicPoint2d > yield_plugin::YieldPlugin::convert_eceftrajectory_to_mappoints ( const carma_v2x_msgs::msg::Trajectory &  ecef_trajectory) const

convert a carma trajectory from ecef frame to map frame ecef trajectory consists of the point and a set of offsets with reference to the point

Parameters
ecef_trajectorycarma trajectory (ecef frame)
Returns
vector of 2d points in map frame

Definition at line 94 of file yield_plugin.cpp.

95 {
96 carma_planning_msgs::msg::TrajectoryPlan trajectory_plan;
97 std::vector<lanelet::BasicPoint2d> map_points;
98
99 lanelet::BasicPoint2d first_point = ecef_to_map_point(ecef_trajectory.location);
100
101 map_points.push_back(first_point);
102 auto curr_point = ecef_trajectory.location;
103
104 for (size_t i = 0; i<ecef_trajectory.offsets.size(); i++)
105 {
106 lanelet::BasicPoint2d offset_point;
107 curr_point.ecef_x += ecef_trajectory.offsets.at(i).offset_x;
108 curr_point.ecef_y += ecef_trajectory.offsets.at(i).offset_y;
109 curr_point.ecef_z += ecef_trajectory.offsets.at(i).offset_z;
110
111 offset_point = ecef_to_map_point(curr_point);
112
113 map_points.push_back(offset_point);
114 }
115
116 return map_points;
117 }
lanelet::BasicPoint2d ecef_to_map_point(const carma_v2x_msgs::msg::LocationECEF &ecef_point) const
convert a point in ecef frame (in cm) into map frame (in meters)
list first_point
Definition: process_bag.py:52

References ecef_to_map_point(), process_bag::first_point, and process_bag::i.

Referenced by mobilityrequest_cb().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ detect_trajectories_intersection()

std::vector< std::pair< int, lanelet::BasicPoint2d > > yield_plugin::YieldPlugin::detect_trajectories_intersection ( std::vector< lanelet::BasicPoint2d >  self_trajectory,
std::vector< lanelet::BasicPoint2d >  incoming_trajectory 
) const

detect intersection point(s) of two trajectories

Parameters
trajectory1vector of 2d trajectory points
trajectory2vector of 2d trajectory points
Returns
vector of pairs of 2d intersection points and index of the point in trajectory array

Definition at line 73 of file yield_plugin.cpp.

74 {
75 std::vector<std::pair<int, lanelet::BasicPoint2d>> intersection_points;
76 boost::geometry::model::linestring<lanelet::BasicPoint2d> self_traj;
77 for (auto tpp:self_trajectory)
78 {
79 boost::geometry::append(self_traj, tpp);
80 }
81 // distance to consider trajectories colliding (chosen based on lane width and vehicle size)
82 for (size_t i=0; i<incoming_trajectory.size(); i++)
83 {
84 double res = boost::geometry::distance(incoming_trajectory.at(i), self_traj);
85
87 {
88 intersection_points.push_back(std::make_pair(i, incoming_trajectory.at(i)));
89 }
90 }
91 return intersection_points;
92 }
double intervehicle_collision_distance_in_m

References config_, process_bag::i, and YieldPluginConfig::intervehicle_collision_distance_in_m.

Referenced by update_traj_for_cooperative_behavior().

Here is the caller graph for this function:

◆ ecef_to_map_point()

lanelet::BasicPoint2d yield_plugin::YieldPlugin::ecef_to_map_point ( const carma_v2x_msgs::msg::LocationECEF &  ecef_point) const

convert a point in ecef frame (in cm) into map frame (in meters)

Parameters
ecef_pointcarma point ecef frame in cm
map_in_earthtranslate frame
Returns
2d point in map frame

Definition at line 119 of file yield_plugin.cpp.

120 {
121
122 if (!map_projector_) {
123 throw std::invalid_argument("No map projector available for ecef conversion");
124 }
125
126 lanelet::BasicPoint3d map_point = map_projector_->projectECEF( { static_cast<double>(ecef_point.ecef_x)/100.0, static_cast<double>(ecef_point.ecef_y)/100.0, static_cast<double>(ecef_point.ecef_z)/100.0 } , 1);
127
128 return lanelet::traits::to2D(map_point);
129 }
std::shared_ptr< lanelet::projection::LocalFrameProjector > map_projector_

References map_projector_.

Referenced by convert_eceftrajectory_to_mappoints().

Here is the caller graph for this function:

◆ generate_JMT_trajectory()

carma_planning_msgs::msg::TrajectoryPlan yield_plugin::YieldPlugin::generate_JMT_trajectory ( const carma_planning_msgs::msg::TrajectoryPlan &  original_tp,
double  initial_pos,
double  goal_pos,
double  initial_velocity,
double  goal_velocity,
double  planning_time,
double  original_max_speed 
)

generate a Jerk Minimizing Trajectory(JMT) with the provided start and end conditions

Parameters
original_tporiginal trajectory plan
intial_posstart position
goal_posfinal position
initial_velocitystart velocity
goal_velocityend velocity
planning_timetime duration of the planning
originaloriginal_max_speed from original_tp up until the goal_pos portion NOTE: the function would generate trajectory duration arbitrarily high if stopping motion is needed. This is to keep the original trajectory's shape in terms of location so that the vehicle steers toward the direction of travel even when stopping.
Returns
updated JMT trajectory

Definition at line 412 of file yield_plugin.cpp.

414 {
415 carma_planning_msgs::msg::TrajectoryPlan jmt_trajectory;
416 std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint> jmt_trajectory_points;
417 jmt_trajectory_points.push_back(original_tp.trajectory_points[0]);
418
419 std::vector<double> original_traj_relative_downtracks = get_relative_downtracks(original_tp);
420 std::vector<double> calculated_speeds = {};
421 std::vector<double> new_relative_downtracks = {};
422 new_relative_downtracks.push_back(0.0);
423 calculated_speeds.push_back(initial_velocity);
424 double new_traj_accumulated_downtrack = 0.0;
425 double original_traj_accumulated_downtrack = original_traj_relative_downtracks.at(1);
426
427 // Up until goal_pos (which also can be until end of the entire original trajectory), generate new speeds at
428 // or near original trajectory points by generating them at a fixed time interval using the JMT polynomial equation
429 const double initial_time = 0;
430 double initial_accel = 0;
432 {
433 initial_accel = (initial_velocity - last_speed_.value()) /
434 (nh_->now() - last_speed_time_.value()).seconds();
435
436 if (!std::isnormal(initial_accel))
437 {
438 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),"Detecting nan initial_accel set to 0");
439 initial_accel = 0.0;
440 }
441
442 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),"Detecting initial_accel: " << initial_accel
443 << ", initial_velocity:" << initial_velocity
444 << ", last_speed_: " << last_speed_.value()
445 << ", nh_->now(): " << nh_->now().seconds()
446 << ", last_speed_time_.get(): " << last_speed_time_.value().seconds());
447 }
448
449 const double goal_accel = 0;
450 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),"Following parameters used for JMT: "
451 "\ninitial_pos: " << initial_pos <<
452 "\ngoal_pos: " << goal_pos <<
453 "\ninitial_velocity: " << initial_velocity <<
454 "\ngoal_velocity: " << goal_velocity <<
455 "\ninitial_accel: " << initial_accel <<
456 "\ngoal_accel: " << goal_accel <<
457 "\nplanning_time: " << planning_time <<
458 "\noriginal_max_speed: " << original_max_speed);
459
460 // Get the polynomial solutions used to generate the trajectory
461 std::vector<double> polynomial_coefficients = quintic_coefficient_calculator::quintic_coefficient_calculator(initial_pos,
462 goal_pos,
463 initial_velocity,
464 goal_velocity,
465 initial_accel,
466 goal_accel,
467 initial_time,
468 planning_time);
469 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),"Used original_max_speed: " << original_max_speed);
470 for (size_t i = 0; i < polynomial_coefficients.size(); i++) {
471 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),"Coefficient " << i << ": " << polynomial_coefficients[i]);
472 }
473 const auto smallest_time_step = get_smallest_time_step_of_traj(original_tp);
474 int new_traj_idx = 1;
475 int original_traj_idx = 1;
476 while (new_traj_accumulated_downtrack < goal_pos - EPSILON && original_traj_idx < original_traj_relative_downtracks.size())
477 {
478 const double target_time = new_traj_idx * smallest_time_step;
479 const double downtrack_at_target_time = polynomial_calc(polynomial_coefficients, target_time);
480 double velocity_at_target_time = polynomial_calc_d(polynomial_coefficients, target_time);
481
482 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "Calculated speed velocity_at_target_time: " << velocity_at_target_time
483 << ", downtrack_at_target_time: "<< downtrack_at_target_time << ", target_time: " << target_time);
484
485 // if the speed becomes negative, the downtrack starts reversing to negative as well
486 // which will never reach the goal_pos, so break here.
487 if (velocity_at_target_time < 0.0)
488 {
489 break;
490 }
491
492 // Cannot have a negative speed or have a higher speed than that of the original trajectory
493 velocity_at_target_time = std::clamp(velocity_at_target_time, 0.0, original_max_speed);
494
495 // Pick the speed if it matches with the original downtracks
496 if (downtrack_at_target_time >= original_traj_accumulated_downtrack)
497 {
498 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "Picked calculated speed velocity_at_target_time: " << velocity_at_target_time
499 << ", downtrack_at_target_time: "<< downtrack_at_target_time << ", target_time: " << target_time);
500 // velocity_at_target_time doesn't exactly correspond to original_traj_accumulated_downtrack but does for new_traj_accumulated_downtrack.
501 // however, the logic is assuming they are close enough that the speed is usable
502 calculated_speeds.push_back(velocity_at_target_time);
503 original_traj_accumulated_downtrack += original_traj_relative_downtracks.at(original_traj_idx);
504 original_traj_idx ++;
505 }
506 new_traj_accumulated_downtrack = downtrack_at_target_time;
507 new_traj_idx++;
508
509 }
510
511 // if the loop above finished prematurely due to negative speed, fill with 0.0 speeds
512 // since the speed crossed 0.0 and algorithm indicates stopping
513 std::fill_n(std::back_inserter(calculated_speeds),
514 std::size(original_traj_relative_downtracks) - std::size(calculated_speeds),
515 0.0);
516
517 // Moving average filter to smoothen the speeds
518 std::vector<double> filtered_speeds = basic_autonomy::smoothing::moving_average_filter(calculated_speeds, config_.speed_moving_average_window_size);
519 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "filtered_speeds size: " << filtered_speeds.size());
520 for (const auto& speed : filtered_speeds)
521 {
522 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "filtered speed: " << speed);
523 }
524 // Replace the original trajectory's associated timestamps based on the newly calculated speeds
525 double prev_speed = filtered_speeds.at(0);
526 last_speed_ = prev_speed;
527 last_speed_time_ = nh_->now();
528 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "start speed: " << prev_speed << ", target_time: " << std::to_string(rclcpp::Time(original_tp.trajectory_points[0].target_time).seconds()));
529
530 for(size_t i = 1; i < original_tp.trajectory_points.size(); i++)
531 {
532 carma_planning_msgs::msg::TrajectoryPlanPoint jmt_tpp = original_tp.trajectory_points.at(i);
533
534 // In case only subset of original trajectory needs modification,
535 // the rest of the points should keep the last speed to cruise
536 double current_speed = goal_velocity;
537
538 if (i < filtered_speeds.size())
539 {
540 current_speed = filtered_speeds.at(i);
541 }
542
543 //Force the speed to 0 if below configured value for more control over stopping behavior
544 if (current_speed < config_.max_stop_speed_in_ms)
545 {
546 current_speed = 0;
547 }
548
549 // Derived from constant accelaration kinematic equation: (vi + vf) / 2 * dt = d_dist
550 // This also handles a case correctly when current_speed is 0, but prev_speed is not 0 yet
551 const double dt = (2 * original_traj_relative_downtracks.at(i)) / (current_speed + prev_speed);
552 jmt_tpp.target_time = rclcpp::Time(jmt_trajectory_points.back().target_time) + rclcpp::Duration::from_nanoseconds(dt*1e9);
553
554 if (prev_speed < EPSILON) // Handle a special case if prev_speed (thus current_speed too) is 0
555 {
556 // NOTE: Assigning arbitrary 100 mins dt between points where normally dt is only 1 sec to model a stopping behavior.
557 // Another way to model it is to keep the trajectory point at a same location and increment time slightly. However,
558 // if the vehicle goes past the point, it may cruise toward undesirable location (for example into the intersection).
559 // Keeping the points help the controller steer the vehicle toward direction of travel even when stopping.
560 // Only downside is the trajectory plan is huge where only 15 sec is expected, but since this is stopping case, it shouldn't matter.
561 jmt_tpp.target_time = rclcpp::Time(jmt_trajectory_points.back().target_time) + rclcpp::Duration::from_nanoseconds(6000 * 1e9);
562 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "Zero speed = x: " << jmt_tpp.x << ", y:" << jmt_tpp.y
563 << ", t:" << std::to_string(rclcpp::Time(jmt_tpp.target_time).seconds())
564 << ", prev_speed: " << prev_speed << ", current_speed: " << current_speed);
565 }
566 else
567 {
568 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "Non-zero speed = x: " << jmt_tpp.x << ", y:" << jmt_tpp.y
569 << ", t:" << std::to_string(rclcpp::Time(jmt_tpp.target_time).seconds())
570 << ", prev_speed: " << prev_speed << ", current_speed: " << current_speed);
571 }
572
573 jmt_trajectory_points.push_back(jmt_tpp);
574 double insta_decel = (current_speed - prev_speed) / (rclcpp::Time(jmt_trajectory_points.at(i).target_time).seconds() - rclcpp::Time(jmt_trajectory_points.at(i - 1).target_time).seconds());
575 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "insta_decel: " << insta_decel );
576 prev_speed = current_speed;
577 }
578
579 jmt_trajectory.header = original_tp.header;
580 jmt_trajectory.trajectory_id = original_tp.trajectory_id;
581 jmt_trajectory.trajectory_points = jmt_trajectory_points;
582 jmt_trajectory.initial_longitudinal_velocity = initial_velocity;
583 return jmt_trajectory;
584 }
std::vector< double > get_relative_downtracks(const carma_planning_msgs::msg::TrajectoryPlan &trajectory_plan) const
calculates distance between trajectory points in a plan
std::optional< rclcpp::Time > last_speed_time_
double polynomial_calc(std::vector< double > coeff, double x) const
calculate quintic polynomial equation for a given x
std::optional< double > last_speed_
double polynomial_calc_d(std::vector< double > coeff, double x) const
calculate derivative of quintic polynomial equation for a given x
std::vector< double > moving_average_filter(const std::vector< double > input, int window_size, bool ignore_first_point=true)
Extremely simplie moving average filter.
Definition: filters.cpp:24
double get_smallest_time_step_of_traj(const carma_planning_msgs::msg::TrajectoryPlan &original_tp)
double max_stop_speed_in_ms
double speed_moving_average_window_size
constexpr auto EPSILON

References config_, EPSILON, get_relative_downtracks(), yield_plugin::get_smallest_time_step_of_traj(), process_bag::i, last_speed_, last_speed_time_, YieldPluginConfig::max_stop_speed_in_ms, basic_autonomy::smoothing::moving_average_filter(), nh_, polynomial_calc(), polynomial_calc_d(), YieldPluginConfig::speed_moving_average_window_size, and carma_cooperative_perception::to_string().

Referenced by update_traj_for_cooperative_behavior(), and update_traj_for_object().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ get_collision()

std::optional< GetCollisionResult > yield_plugin::YieldPlugin::get_collision ( const carma_planning_msgs::msg::TrajectoryPlan &  trajectory1,
const std::vector< carma_perception_msgs::msg::PredictedState > &  trajectory2,
double  collision_radius,
double  trajectory1_max_speed 
)

Return naive collision time and locations based on collision radius given two trajectories with one being obstacle's predicted steps.

Parameters
trajectory1trajectory of the ego vehicle
trajectory2trajectory of predicted steps
collision_radiusa distance to check between two trajectory points at a same timestamp that is considered a collision
trajectory1_max_speedmax speed of the trajectory1 to efficiently traverse through possible collision combination of the two trajectories NOTE: Currently Traj2 is assumed to be a simple cv model to save computational performance NOTE: Collisions are based on only collision radius at the same predicted time even if ego vehicle maybe past the obstacle. To filter these cases, see is_object_behind_vehicle()
Returns
data of time of collision if detected, otherwise, std::nullopt

Definition at line 586 of file yield_plugin.cpp.

588 {
589
590 // Iterate through each pair of consecutive points in the trajectories
591 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Starting a new collision detection, trajectory size: "
592 << trajectory1.trajectory_points.size() << ". prediction size: " << trajectory2.size());
593
594 // Iterate through the object to check if it's on the route
595 bool on_route = false;
596 int on_route_idx = 0;
597
598 // A flag to stop searching more than one lanelet if the object has no velocity
599 const auto traj2_speed{std::hypot(trajectory2.front().predicted_velocity.linear.x,
600 trajectory2.front().predicted_velocity.linear.y)};
601 bool traj2_has_zero_speed = traj2_speed < config_.obstacle_zero_speed_threshold_in_ms;
602
603 if (trajectory2.size() < 2)
604 {
605 throw std::invalid_argument("Object on ther road doesn't have enough predicted states! Please check motion_computation is correctly applying predicted states");
606 }
607 const double predict_step_duration = (rclcpp::Time(trajectory2.at(1).header.stamp) - rclcpp::Time(trajectory2.front().header.stamp)).seconds();
608 const double predict_total_duration = get_trajectory_duration(trajectory2);
609
610 if (predict_step_duration < 0.0)
611 {
612 throw std::invalid_argument("Predicted states of the object is malformed. Detected trajectory going backwards in time!");
613 }
614
615 // In order to optimize the for loops for comparing two trajectories, following logic skips every iteration_stride-th points of the traj2.
616 // Since skipping number of points from the traj2 may result in ignoring potential collisions, its value is dependent on two
617 // trajectories' speeds and intervehicle_collision_distance_in_m radius.
618 // Therefore, the derivation first calculates the max time, t, that both actors can move while still being in collision radius:
619 // sqrt( (v1 * t / 2)^2 + (v2 * t / 2)^2 ) = collision_radius. Here v1 and v2 are assumed to be perpendicular to each other and
620 // intersecting at t/2 to get max possible collision_radius. Solving for t gives following:
621 double iteration_stride_max_time_s = 2 * config_.intervehicle_collision_distance_in_m / sqrt(pow(traj2_speed, 2) + pow(trajectory1_max_speed, 2));
622 int iteration_stride = std::max(1, static_cast<int>(iteration_stride_max_time_s / predict_step_duration));
623
624 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Determined iteration_stride: " << iteration_stride
625 << ", with traj2_speed: " << traj2_speed
626 << ", with trajectory1_max_speed: " << trajectory1_max_speed
627 << ", with predict_step_duration: " << predict_step_duration
628 << ", iteration_stride_max_time_s: " << iteration_stride_max_time_s);
629
630 for (size_t j = 0; j < trajectory2.size(); j += iteration_stride) // Saving computation time aiming for 1.5 meter interval
631 {
632 lanelet::BasicPoint2d curr_point;
633 curr_point.x() = trajectory2.at(j).predicted_position.position.x;
634 curr_point.y() = trajectory2.at(j).predicted_position.position.y;
635
636 auto corresponding_lanelets = wm_->getLaneletsFromPoint(curr_point, 8); // some intersection can have 8 overlapping lanelets
637
638 for (const auto& llt: corresponding_lanelets)
639 {
640 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Checking llt: " << llt.id());
641
642 if (route_llt_ids_.find(llt.id()) != route_llt_ids_.end())
643 {
644 on_route = true;
645 on_route_idx = j;
646 break;
647 }
648 }
649 if (on_route || traj2_has_zero_speed)
650 break;
651 }
652
653 if (!on_route)
654 {
655 RCLCPP_DEBUG(nh_->get_logger(), "Predicted states are not on the route! ignoring");
656 return std::nullopt;
657 }
658
659 double smallest_dist = std::numeric_limits<double>::infinity();
660 for (size_t i = 0; i < trajectory1.trajectory_points.size() - 1; ++i)
661 {
662 auto p1a = trajectory1.trajectory_points.at(i);
663 auto p1b = trajectory1.trajectory_points.at(i + 1);
664 double previous_distance_between_predictions = std::numeric_limits<double>::infinity();
665 for (size_t j = on_route_idx; j < trajectory2.size() - 1; j += iteration_stride)
666 {
667 auto p2a = trajectory2.at(j);
668 auto p2b = trajectory2.at(j + 1);
669 double p1a_t = rclcpp::Time(p1a.target_time).seconds();
670 double p1b_t = rclcpp::Time(p1b.target_time).seconds();
671 double p2a_t = rclcpp::Time(p2a.header.stamp).seconds();
672 double p2b_t = rclcpp::Time(p2b.header.stamp).seconds();
673
674 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p1a.target_time: " << std::to_string(p1a_t) << ", p1b.target_time: " << std::to_string(p1b_t));
675 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p2a.target_time: " << std::to_string(p2a_t) << ", p2b.target_time: " << std::to_string(p2b_t));
676 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p1a.x: " << p1a.x << ", p1a.y: " << p1a.y);
677 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p1b.x: " << p1b.x << ", p1b.y: " << p1b.y);
678
679 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p2a.x: " << p2a.predicted_position.position.x << ", p2a.y: " << p2a.predicted_position.position.y);
680 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p2b.x: " << p2b.predicted_position.position.x << ", p2b.y: " << p2b.predicted_position.position.y);
681
682 // Linearly interpolate positions at a common timestamp for both trajectories
683 double dt = (p2a_t - p1a_t) / (p1b_t - p1a_t);
684 double x1 = p1a.x + dt * (p1b.x - p1a.x);
685 double y1 = p1a.y + dt * (p1b.y - p1a.y);
686 double x2 = p2a.predicted_position.position.x;
687 double y2 = p2a.predicted_position.position.y;
688
689 // Calculate the distance between the two interpolated points
690 const auto distance{std::hypot(x1 - x2, y1 - y2)};
691
692 smallest_dist = std::min(distance, smallest_dist);
693 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Smallest_dist: " << smallest_dist << ", distance: " << distance << ", dt: " << dt
694 << ", x1: " << x1 << ", y1: " << y1
695 << ", x2: " << x2 << ", y2: " << y2
696 << ", p2a_t:" << std::to_string(p2a_t));
697
698 // Following "if logic" assumes the traj2 is a simple cv model, aka, traj2 point is a straight line over time.
699 // And current traj1 point is fixed in this iteration.
700 // Then once the distance between the two start to increase over traj2 iteration,
701 // the distance will always increase and it's unnecessary to continue the logic to find the smallest_dist
702 if (previous_distance_between_predictions < distance)
703 {
704 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Stopping search here because the distance between predictions started to increase");
705 break;
706 }
707 previous_distance_between_predictions = distance;
708
709 if (i == 0 && j == 0 && distance > config_.collision_check_radius_in_m)
710 {
711 RCLCPP_DEBUG(nh_->get_logger(), "Too far away" );
712 return std::nullopt;
713 }
714
715 if (distance > collision_radius)
716 {
717 // continue searching for collision
718 continue;
719 }
720
721 GetCollisionResult collision_result;
722 collision_result.point1 = lanelet::BasicPoint2d(x1,y1);
723 collision_result.point2 = lanelet::BasicPoint2d(x2,y2);
724 collision_result.collision_time = rclcpp::Time(p2a.header.stamp);
725 return collision_result;
726 }
727 }
728
729 // No collision detected
730 return std::nullopt;
731 }
std::set< lanelet::Id > route_llt_ids_
double get_trajectory_duration(const carma_planning_msgs::msg::TrajectoryPlan &trajectory)
double collision_check_radius_in_m
double obstacle_zero_speed_threshold_in_ms

References YieldPluginConfig::collision_check_radius_in_m, yield_plugin::GetCollisionResult::collision_time, config_, yield_plugin::get_trajectory_duration(), process_bag::i, YieldPluginConfig::intervehicle_collision_distance_in_m, nh_, YieldPluginConfig::obstacle_zero_speed_threshold_in_ms, yield_plugin::GetCollisionResult::point1, yield_plugin::GetCollisionResult::point2, route_llt_ids_, carma_cooperative_perception::to_string(), and wm_.

Referenced by get_collision_time().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ get_collision_time()

std::optional< rclcpp::Time > yield_plugin::YieldPlugin::get_collision_time ( const carma_planning_msgs::msg::TrajectoryPlan &  original_tp,
const carma_perception_msgs::msg::ExternalObject &  curr_obstacle,
double  original_tp_max_speed 
)

Return collision time given two trajectories with one being external object with predicted steps.

Parameters
trajectory1trajectory of the ego vehicle
trajectory2trajectory of the obstacle
original_tp_max_speedmax speed of the original_tp to efficiently traverse through possible collision combination of the two trajectories NOTE: Currently curr_obstacle is assumed to be using a simple cv model to save computational performance
Returns
time_of_collision if collision detected, otherwise, std::nullopt

Definition at line 759 of file yield_plugin.cpp.

761 {
762 auto plan_start_time = get_trajectory_start_time(original_tp);
763
764 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Object's back time: " << std::to_string(rclcpp::Time(curr_obstacle.predictions.back().header.stamp).seconds())
765 << ", plan_start_time: " << std::to_string(plan_start_time));
766
767 // do not process outdated objects
768 if (rclcpp::Time(curr_obstacle.predictions.back().header.stamp).seconds() <= plan_start_time)
769 {
770 return std::nullopt;
771 }
772
773 std::vector<carma_perception_msgs::msg::PredictedState> new_list;
774 carma_perception_msgs::msg::PredictedState curr_state;
775 // artificially include current position as one of the predicted states
776 curr_state.header.stamp = curr_obstacle.header.stamp;
777 curr_state.predicted_position.position.x = curr_obstacle.pose.pose.position.x;
778 curr_state.predicted_position.position.y = curr_obstacle.pose.pose.position.y;
779 // NOTE: predicted_velocity is not used for collision calculation, but timestamps
780 curr_state.predicted_velocity.linear.x = curr_obstacle.velocity.twist.linear.x;
781 curr_state.predicted_velocity.linear.y = curr_obstacle.velocity.twist.linear.y;
782 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Object: " << curr_obstacle.id <<", type: " << static_cast<int>(curr_obstacle.object_type)
783 << ", speed_x: " << curr_obstacle.velocity.twist.linear.x << ", speed_y: " << curr_obstacle.velocity.twist.linear.y);
784 new_list.push_back(curr_state);
785 new_list.insert(new_list.end(), curr_obstacle.predictions.cbegin(), curr_obstacle.predictions.cend());
786
787 const auto collision_result = get_collision(original_tp, new_list, config_.intervehicle_collision_distance_in_m, original_tp_max_speed);
788
789 if (!collision_result)
790 {
791 // reset the consecutive clearance counter because no collision was detected at this iteration
792 consecutive_clearance_count_for_obstacles_[curr_obstacle.id] = 0;
793 return std::nullopt;
794 }
795
796 // if within collision radius, it is not a collision if obstacle is behind the vehicle despite being in collision radius
797 const double vehicle_downtrack = wm_->routeTrackPos(collision_result.value().point1).downtrack;
798 const double object_downtrack = wm_->routeTrackPos(collision_result.value().point2).downtrack;
799
800 if (is_object_behind_vehicle(curr_obstacle.id, collision_result.value().collision_time, vehicle_downtrack, object_downtrack))
801 {
802 RCLCPP_INFO_STREAM(nh_->get_logger(), "Confirmed that the object: " << curr_obstacle.id << " is behind the vehicle at timestamp " << std::to_string(collision_result.value().collision_time.seconds()));
803 return std::nullopt;
804 }
805
806 const auto distance{std::hypot(
807 collision_result.value().point1.x() - collision_result.value().point2.x(),
808 collision_result.value().point1.y() - collision_result.value().point2.y()
809 )}; //for debug
810
811 RCLCPP_WARN_STREAM(nh_->get_logger(), "Collision detected for object: " << curr_obstacle.id << ", at timestamp " << std::to_string(collision_result.value().collision_time.seconds()) <<
812 ", x: " << collision_result.value().point1.x() << ", y: " << collision_result.value().point1.y() <<
813 ", within actual downtrack distance: " << object_downtrack - vehicle_downtrack <<
814 ", and collision distance: " << distance);
815
816 return collision_result.value().collision_time;
817 }
std::optional< GetCollisionResult > get_collision(const carma_planning_msgs::msg::TrajectoryPlan &trajectory1, const std::vector< carma_perception_msgs::msg::PredictedState > &trajectory2, double collision_radius, double trajectory1_max_speed)
Return naive collision time and locations based on collision radius given two trajectories with one b...
std::unordered_map< uint32_t, int > consecutive_clearance_count_for_obstacles_
bool is_object_behind_vehicle(uint32_t object_id, const rclcpp::Time &collision_time, double vehicle_point, double object_downtrack)
Check if object location is behind the vehicle using estimates of the vehicle's length and route down...
double get_trajectory_start_time(const carma_planning_msgs::msg::TrajectoryPlan &trajectory)

References config_, consecutive_clearance_count_for_obstacles_, get_collision(), yield_plugin::get_trajectory_start_time(), YieldPluginConfig::intervehicle_collision_distance_in_m, is_object_behind_vehicle(), nh_, carma_cooperative_perception::to_string(), and wm_.

Referenced by get_collision_times_concurrently().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ get_collision_times_concurrently()

std::unordered_map< uint32_t, rclcpp::Time > yield_plugin::YieldPlugin::get_collision_times_concurrently ( const carma_planning_msgs::msg::TrajectoryPlan &  original_tp,
const std::vector< carma_perception_msgs::msg::ExternalObject > &  external_objects,
double  original_tp_max_speed 
)

Given the list of objects with predicted states, get all collision times concurrently using multi-threading.

Parameters
original_tptrajectory of the ego vehicle
external_objectslist of external objects with predicted states
original_tp_max_speedmax speed of the original_tp to efficiently traverse through possible collision combination of the two trajectories
Returns
mapping of objects' ids and their corresponding collision times (non-colliding objects are omitted)

Definition at line 819 of file yield_plugin.cpp.

821 {
822
823 std::unordered_map<uint32_t, std::future<std::optional<rclcpp::Time>>> futures;
824 std::unordered_map<uint32_t, rclcpp::Time> collision_times;
825
826 // Launch asynchronous tasks to check for collision times
827 for (const auto& object : external_objects) {
828 futures[object.id] = std::async(std::launch::async,[this, &original_tp, &object, &original_tp_max_speed]{
829 return get_collision_time(original_tp, object, original_tp_max_speed);
830 });
831 }
832
833 // Collect results from futures and update collision_times
834 for (const auto& object : external_objects) {
835 if (const auto collision_time{futures.at(object.id).get()}) {
836 collision_times[object.id] = collision_time.value();
837 }
838 }
839
840 return collision_times;
841 }
std::optional< rclcpp::Time > get_collision_time(const carma_planning_msgs::msg::TrajectoryPlan &original_tp, const carma_perception_msgs::msg::ExternalObject &curr_obstacle, double original_tp_max_speed)
Return collision time given two trajectories with one being external object with predicted steps.

References get_collision_time().

Referenced by get_earliest_collision_object_and_time().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ get_earliest_collision_object_and_time()

std::optional< std::pair< carma_perception_msgs::msg::ExternalObject, double > > yield_plugin::YieldPlugin::get_earliest_collision_object_and_time ( const carma_planning_msgs::msg::TrajectoryPlan &  original_tp,
const std::vector< carma_perception_msgs::msg::ExternalObject > &  external_objects 
)

Return the earliest collision object and time of collision pair from the given trajectory and list of external objects with predicted states. Function first filters obstacles based on whether if their any of predicted state will be on the route. Only then, the logic compares trajectory and predicted states.

Parameters
original_tptrajectory of the ego vehicle
external_objectslist of external objects with predicted states
Returns
earliest collision object and its collision time if collision detected. std::nullopt if no collision is detected or if route is not available.

Definition at line 843 of file yield_plugin.cpp.

845 {
846 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "ExternalObjects size: " << external_objects.size());
847
848 if (!wm_->getRoute())
849 {
850 RCLCPP_WARN(nh_->get_logger(), "Yield plugin was not able to analyze collision since route is not available! Please check if route is set");
851 return std::nullopt;
852 }
853
854 // save route Ids for faster access
855 for (const auto& llt: wm_->getRoute()->shortestPath())
856 {
857 // TODO: Enhancement https://github.com/usdot-fhwa-stol/carma-platform/issues/2316
858 route_llt_ids_.insert(llt.id());
859 }
860
861 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"External Object List (external_objects) size: " << external_objects.size());
862 const double original_max_speed = max_trajectory_speed(original_tp.trajectory_points, get_trajectory_end_time(original_tp));
863 std::unordered_map<uint32_t, rclcpp::Time> collision_times = get_collision_times_concurrently(original_tp,external_objects, original_max_speed);
864
865 if (collision_times.empty()) { return std::nullopt; }
866
867 const auto earliest_colliding_object_id{std::min_element(
868 std::cbegin(collision_times), std::cend(collision_times),
869 [](const auto & a, const auto & b){ return a.second < b.second; })->first};
870
871 const auto earliest_colliding_object{std::find_if(
872 std::cbegin(external_objects), std::cend(external_objects),
873 [&earliest_colliding_object_id](const auto & object) { return object.id == earliest_colliding_object_id; })};
874
875 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"earliest object x: " << earliest_colliding_object->velocity.twist.linear.x
876 << ", y: " << earliest_colliding_object->velocity.twist.linear.y);
877 return std::make_pair(*earliest_colliding_object, collision_times.at(earliest_colliding_object_id).seconds());
878
879 }
std::unordered_map< uint32_t, rclcpp::Time > get_collision_times_concurrently(const carma_planning_msgs::msg::TrajectoryPlan &original_tp, const std::vector< carma_perception_msgs::msg::ExternalObject > &external_objects, double original_tp_max_speed)
Given the list of objects with predicted states, get all collision times concurrently using multi-thr...
double max_trajectory_speed(const std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > &trajectory_points, double timestamp_in_sec_to_search_until) const
calculates the maximum speed in a set of tajectory points
double get_trajectory_end_time(const carma_planning_msgs::msg::TrajectoryPlan &trajectory)

References get_collision_times_concurrently(), yield_plugin::get_trajectory_end_time(), max_trajectory_speed(), nh_, route_llt_ids_, and wm_.

Referenced by update_traj_for_object().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ get_predicted_velocity_at_time()

double yield_plugin::YieldPlugin::get_predicted_velocity_at_time ( const geometry_msgs::msg::Twist &  object_velocity_in_map_frame,
const carma_planning_msgs::msg::TrajectoryPlan &  original_tp,
double  timestamp_in_sec_to_predict 
)

Given the object velocity in map frame with x,y components, this function returns the projected velocity along the trajectory at given time.

Parameters
object_velocity_in_map_frametrajectory of the ego vehicle
original_tptrajectory of the ego vehicle
timestamp_in_sec_to_predicttimestamp in seconds along the trajectory to return the projected velocity NOTE: returns the last point's speed if the specified time is past the trajectory's planning time
Returns
get_predicted_velocity_at_time

Definition at line 881 of file yield_plugin.cpp.

883 {
884 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "timestamp_in_sec_to_predict: " << std::to_string(timestamp_in_sec_to_predict) <<
885 ", trajectory_end_time: " << std::to_string(get_trajectory_end_time(original_tp)));
886
887 double point_b_time = 0.0;
888 carma_planning_msgs::msg::TrajectoryPlanPoint point_a;
889 carma_planning_msgs::msg::TrajectoryPlanPoint point_b;
890
891 // trajectory points' time is guaranteed to be increasing
892 // then find the corresponding point at timestamp_in_sec_to_predict
893 for (size_t i = 0; i < original_tp.trajectory_points.size() - 1; ++i)
894 {
895 point_a = original_tp.trajectory_points.at(i);
896 point_b = original_tp.trajectory_points.at(i + 1);
897 point_b_time = rclcpp::Time(point_b.target_time).seconds();
898 if (point_b_time >= timestamp_in_sec_to_predict)
899 {
900 break;
901 }
902 }
903
904 auto dx = point_b.x - point_a.x;
905 auto dy = point_b.y - point_a.y;
906 const tf2::Vector3 trajectory_direction(dx, dy, 0);
907
908 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "timestamp_in_sec_to_predict: " << std::to_string(timestamp_in_sec_to_predict)
909 << ", point_b_time: " << std::to_string(point_b_time)
910 << ", dx: " << dx << ", dy: " << dy << ", "
911 << ", object_velocity_in_map_frame.x: " << object_velocity_in_map_frame.linear.x
912 << ", object_velocity_in_map_frame.y: " << object_velocity_in_map_frame.linear.y);
913
914 if (trajectory_direction.length() < 0.001) //EPSILON
915 {
916 return 0.0;
917 }
918
919 const tf2::Vector3 object_direction(object_velocity_in_map_frame.linear.x, object_velocity_in_map_frame.linear.y, 0);
920
921 return tf2::tf2Dot(object_direction, trajectory_direction) / trajectory_direction.length();
922 }

References yield_plugin::get_trajectory_end_time(), process_bag::i, nh_, and carma_cooperative_perception::to_string().

Referenced by update_traj_for_object().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ get_relative_downtracks()

std::vector< double > yield_plugin::YieldPlugin::get_relative_downtracks ( const carma_planning_msgs::msg::TrajectoryPlan &  trajectory_plan) const

calculates distance between trajectory points in a plan

Parameters
trajectory_planinput trajectory plan
Returns
vector of relative distances between trajectory points

Definition at line 1012 of file yield_plugin.cpp.

1013 {
1014 std::vector<double> downtracks;
1015 downtracks.reserve(trajectory_plan.trajectory_points.size());
1016 // relative downtrack distance of the fist Point is 0.0
1017 downtracks.push_back(0.0);
1018 for (size_t i=1; i < trajectory_plan.trajectory_points.size(); i++){
1019
1020 double dx = trajectory_plan.trajectory_points.at(i).x - trajectory_plan.trajectory_points.at(i-1).x;
1021 double dy = trajectory_plan.trajectory_points.at(i).y - trajectory_plan.trajectory_points.at(i-1).y;
1022 downtracks.push_back(sqrt(dx*dx + dy*dy));
1023 }
1024 return downtracks;
1025 }

References process_bag::i.

Referenced by generate_JMT_trajectory().

Here is the caller graph for this function:

◆ is_object_behind_vehicle()

bool yield_plugin::YieldPlugin::is_object_behind_vehicle ( uint32_t  object_id,
const rclcpp::Time &  collision_time,
double  vehicle_point,
double  object_downtrack 
)

Check if object location is behind the vehicle using estimates of the vehicle's length and route downtracks.

Parameters
object_idobject id to use for the consecutive_clearance_count_for_obstacles_
collision_timepredicted time of collision
vehicle_downtrackat the time of collision
object_downtrackat the time of collision NOTE: Uses internal counter low pass filter to confirm the object is behind only if it counted continuously above config_.consecutive_clearance_count_for_obstacles_threshold
Returns
return true if object is behind the vehicle

Definition at line 733 of file yield_plugin.cpp.

734 {
735 const auto previous_clearance_count = consecutive_clearance_count_for_obstacles_[object_id];
736 // if the object's location is half a length of the vehicle past its rear-axle, it is considered behind
737 // half a length of the vehicle to conservatively estimate the rear axle to rear bumper length
738 if (object_downtrack < vehicle_downtrack - config_.vehicle_length / 2)
739 {
741 RCLCPP_INFO_STREAM(nh_->get_logger(), "Detected an object nearby might be behind the vehicle at timestamp: " << std::to_string(collision_time.seconds()) <<
742 ", and consecutive_clearance_count_for obstacle: " << object_id << ", is: " << consecutive_clearance_count_for_obstacles_[object_id]);
743 }
744 // confirmed false positive for a collision
746 {
747 return true;
748 }
749 // if the clearance counter didn't increase by this point, true collision was detected
750 // therefore reset the consecutive clearance counter as it is no longer consecutive
751 if (consecutive_clearance_count_for_obstacles_[object_id] == previous_clearance_count)
752 {
754 }
755
756 return false;
757 }
int consecutive_clearance_count_for_obstacles_threshold

References config_, consecutive_clearance_count_for_obstacles_, YieldPluginConfig::consecutive_clearance_count_for_obstacles_threshold, nh_, carma_cooperative_perception::to_string(), and YieldPluginConfig::vehicle_length.

Referenced by get_collision_time().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ lookup_ecef_to_map_transform()

void yield_plugin::YieldPlugin::lookup_ecef_to_map_transform ( )

Looks up the transform between map and earth frames, and sets the member variable.

◆ max_trajectory_speed()

double yield_plugin::YieldPlugin::max_trajectory_speed ( const std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > &  trajectory_points,
double  timestamp_in_sec_to_search_until 
) const

calculates the maximum speed in a set of tajectory points

Parameters
trajectory_pointstrajectory points

\param timestamp_in_sec_to_search_until before which to look for max trajectory speed

Returns
maximum speed

Definition at line 1049 of file yield_plugin.cpp.

1050 {
1051 double max_speed = 0;
1052 for(size_t i = 0; i < trajectory_points.size() - 2; i++ )
1053 {
1054 double dx = trajectory_points.at(i + 1).x - trajectory_points.at(i).x;
1055 double dy = trajectory_points.at(i + 1).y - trajectory_points.at(i).y;
1056 double d = sqrt(dx*dx + dy*dy);
1057 double t = (rclcpp::Time(trajectory_points.at(i + 1).target_time).seconds() - rclcpp::Time(trajectory_points.at(i).target_time).seconds());
1058 double v = d/t;
1059 if(v > max_speed)
1060 {
1061 max_speed = v;
1062 }
1063 if (rclcpp::Time(trajectory_points.at(i + 1).target_time).seconds() >= timestamp_in_sec_to_search_until)
1064 {
1065 break;
1066 }
1067
1068 }
1069 return max_speed;
1070 }

References process_bag::i.

Referenced by get_earliest_collision_object_and_time(), update_traj_for_cooperative_behavior(), and update_traj_for_object().

Here is the caller graph for this function:

◆ mobilityrequest_cb()

void yield_plugin::YieldPlugin::mobilityrequest_cb ( const carma_v2x_msgs::msg::MobilityRequest::UniquePtr  msg)

callback for mobility request

Parameters
msgmobility request message

Definition at line 153 of file yield_plugin.cpp.

154 {
155 carma_v2x_msgs::msg::MobilityRequest incoming_request = *msg;
156 carma_planning_msgs::msg::LaneChangeStatus lc_status_msg;
157 if (incoming_request.strategy == "carma/cooperative-lane-change")
158 {
159 if (!map_projector_) {
160 RCLCPP_ERROR(nh_->get_logger(),"Cannot process mobility request as map projection is not yet set!");
161 return;
162 }
163 if (incoming_request.plan_type.type == carma_v2x_msgs::msg::PlanType::CHANGE_LANE_LEFT || incoming_request.plan_type.type == carma_v2x_msgs::msg::PlanType::CHANGE_LANE_RIGHT)
164 {
165 RCLCPP_DEBUG(nh_->get_logger(),"Cooperative Lane Change Request Received");
166 lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REQUEST_RECEIVED;
167 lc_status_msg.description = "Received lane merge request";
168
169 if (incoming_request.m_header.recipient_id == config_.vehicle_id)
170 {
171 RCLCPP_DEBUG(nh_->get_logger(),"CLC Request correctly received");
172 }
173
174 // extract mobility header
175 std::string req_sender_id = incoming_request.m_header.sender_id;
176 std::string req_plan_id = incoming_request.m_header.plan_id;
177 // extract mobility request
178 carma_v2x_msgs::msg::LocationECEF ecef_location = incoming_request.location;
179 carma_v2x_msgs::msg::Trajectory incoming_trajectory = incoming_request.trajectory;
180 std::string req_strategy_params = incoming_request.strategy_params;
181 clc_urgency_ = incoming_request.urgency;
182 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"received urgency: " << clc_urgency_);
183
184 // Parse strategy parameters
185 using boost::property_tree::ptree;
186 ptree pt;
187 std::istringstream strstream(req_strategy_params);
188 boost::property_tree::json_parser::read_json(strstream, pt);
189 int req_traj_speed_full = pt.get<int>("s");
190 int req_traj_fractional = pt.get<int>("f");
191 int start_lanelet_id = pt.get<int>("sl");
192 int end_lanelet_id = pt.get<int>("el");
193 double req_traj_speed = static_cast<double>(req_traj_speed_full) + static_cast<double>(req_traj_fractional)/10.0;
194 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"req_traj_speed" << req_traj_speed);
195 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"start_lanelet_id" << start_lanelet_id);
196 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"end_lanelet_id" << end_lanelet_id);
197
198 std::vector<lanelet::BasicPoint2d> req_traj_plan = {};
199
200 req_traj_plan = convert_eceftrajectory_to_mappoints(incoming_trajectory);
201
202 double req_expiration_sec = static_cast<double>(incoming_request.expiration);
203 double current_time_sec = nh_->now().seconds();
204
205 bool response_to_clc_req = false;
206 // ensure there is enough time for the yield
207 double req_plan_time = req_expiration_sec - current_time_sec;
208 double req_timestamp = static_cast<double>(incoming_request.m_header.timestamp) / 1000.0 - current_time_sec;
209 set_incoming_request_info(req_traj_plan, req_traj_speed, req_plan_time, req_timestamp);
210
211
212 if (req_expiration_sec - current_time_sec >= config_.min_obj_avoidance_plan_time_in_s && cooperative_request_acceptable_)
213 {
215 lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REQUEST_ACCEPTED;
216 lc_status_msg.description = "Accepted lane merge request";
217 response_to_clc_req = true;
218 RCLCPP_DEBUG(nh_->get_logger(),"CLC accepted");
219 }
220 else
221 {
222 lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REQUEST_REJECTED;
223 lc_status_msg.description = "Rejected lane merge request";
224 response_to_clc_req = false;
225 RCLCPP_DEBUG(nh_->get_logger(),"CLC rejected");
226 }
227 carma_v2x_msgs::msg::MobilityResponse outgoing_response = compose_mobility_response(req_sender_id, req_plan_id, response_to_clc_req);
228 mobility_response_publisher_(outgoing_response);
229 lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::RESPONSE_SENT;
230 RCLCPP_DEBUG(nh_->get_logger(),"response sent");
231 }
232 }
233 lc_status_publisher_(lc_status_msg);
234
235 }
void set_incoming_request_info(std::vector< lanelet::BasicPoint2d > req_trajectory, double req_speed, double req_planning_time, double req_timestamp)
set values for member variables related to cooperative behavior
std::vector< lanelet::BasicPoint2d > convert_eceftrajectory_to_mappoints(const carma_v2x_msgs::msg::Trajectory &ecef_trajectory) const
convert a carma trajectory from ecef frame to map frame ecef trajectory consists of the point and a s...
carma_v2x_msgs::msg::MobilityResponse compose_mobility_response(const std::string &resp_recipient_id, const std::string &req_plan_id, bool response) const
compose a mobility response message
double min_obj_avoidance_plan_time_in_s

References clc_urgency_, compose_mobility_response(), config_, convert_eceftrajectory_to_mappoints(), cooperative_request_acceptable_, lc_status_publisher_, map_projector_, YieldPluginConfig::min_obj_avoidance_plan_time_in_s, mobility_response_publisher_, nh_, set_incoming_request_info(), timesteps_since_last_req_, and YieldPluginConfig::vehicle_id.

Referenced by yield_plugin::YieldPluginNode::on_configure_plugin().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ plan_trajectory_callback()

void yield_plugin::YieldPlugin::plan_trajectory_callback ( carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr  req,
carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr  resp 
)

Service callback for trajectory planning.

Parameters
srv_headerheader
reqThe service request
respThe service response

Definition at line 252 of file yield_plugin.cpp.

255{
256 RCLCPP_DEBUG(nh_->get_logger(),"Yield_plugin was called!");
257 if (req->initial_trajectory_plan.trajectory_points.size() < 2){
258 throw std::invalid_argument("Empty Trajectory received by Yield");
259 }
260 rclcpp::Clock system_clock(RCL_SYSTEM_TIME);
261 rclcpp::Time start_time = system_clock.now(); // Start timing the execution time for planning so it can be logged
262
263 carma_planning_msgs::msg::TrajectoryPlan original_trajectory = req->initial_trajectory_plan;
264 carma_planning_msgs::msg::TrajectoryPlan yield_trajectory;
265
266 try
267 {
268 // NOTE: Wrapping entire plan_trajectory logic with try catch because there is intermittent
269 // open issue of which cause is uncertain:
270 // https://github.com/usdot-fhwa-stol/carma-platform/issues/2501
271
272 double initial_velocity = req->vehicle_state.longitudinal_vel;
273 // If vehicle_state is stopped, non-zero velocity from the trajectory
274 // should be used. Otherwise, vehicle will not move.
275 if (initial_velocity < EPSILON)
276 {
277 initial_velocity = original_trajectory.initial_longitudinal_velocity;
278 }
279
280 // seperating cooperative yield with regular object detection for better performance.
282 {
283 RCLCPP_DEBUG(nh_->get_logger(),"Only consider high urgency clc");
285 {
286 RCLCPP_DEBUG(nh_->get_logger(),"Yield for CLC. We haven't received an updated negotiation this timestep");
287 yield_trajectory = update_traj_for_cooperative_behavior(original_trajectory, initial_velocity);
289 }
290 else
291 {
292 RCLCPP_DEBUG(nh_->get_logger(),"unreliable CLC communication, switching to object avoidance");
293 yield_trajectory = update_traj_for_object(original_trajectory, external_objects_, initial_velocity); // Compute the trajectory
294 }
295 }
296 else
297 {
298 RCLCPP_DEBUG(nh_->get_logger(),"Yield for object avoidance");
299 yield_trajectory = update_traj_for_object(original_trajectory, external_objects_, initial_velocity); // Compute the trajectory
300 }
301
302 // return original trajectory if no difference in trajectory points a.k.a no collision
303 if (fabs(get_trajectory_end_time(original_trajectory) - get_trajectory_end_time(yield_trajectory)) < EPSILON)
304 {
305 resp->trajectory_plan = original_trajectory;
306 }
307 else
308 {
309 yield_trajectory.header.frame_id = "map";
310 yield_trajectory.header.stamp = nh_->now();
311 yield_trajectory.trajectory_id = original_trajectory.trajectory_id;
312 resp->trajectory_plan = yield_trajectory;
313 }
314 }
315 catch(const std::runtime_error& e) {
316 RCLCPP_WARN_STREAM(nh_->get_logger(), "Yield Plugin failed to plan trajectory due to known negative time issue: " << e.what());
317 RCLCPP_WARN_STREAM(nh_->get_logger(), "Returning the original trajectory, and retrying at the next call.");
318 resp->trajectory_plan = original_trajectory;
319 }
320
321 rclcpp::Time end_time = system_clock.now(); // Planning complete
322
323 auto duration = end_time - start_time;
324 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "ExecutionTime: " << std::to_string(duration.seconds()));
325 }
carma_planning_msgs::msg::TrajectoryPlan update_traj_for_object(const carma_planning_msgs::msg::TrajectoryPlan &original_tp, const std::vector< carma_perception_msgs::msg::ExternalObject > &external_objects, double initial_velocity)
trajectory is modified to safely avoid obstacles on the road
carma_planning_msgs::msg::TrajectoryPlan update_traj_for_cooperative_behavior(const carma_planning_msgs::msg::TrajectoryPlan &original_tp, double current_speed)
update trajectory for yielding to an incoming cooperative behavior
std::vector< carma_perception_msgs::msg::ExternalObject > external_objects_
bool enable_cooperative_behavior

References YieldPluginConfig::acceptable_passed_timesteps, YieldPluginConfig::acceptable_urgency, clc_urgency_, config_, YieldPluginConfig::enable_cooperative_behavior, EPSILON, external_objects_, yield_plugin::get_trajectory_end_time(), nh_, timesteps_since_last_req_, carma_cooperative_perception::to_string(), update_traj_for_cooperative_behavior(), and update_traj_for_object().

Here is the call graph for this function:

◆ polynomial_calc()

double yield_plugin::YieldPlugin::polynomial_calc ( std::vector< double >  coeff,
double  x 
) const

calculate quintic polynomial equation for a given x

Parameters
coeffvector including polynomial coefficiencrs
xinput variable to the polynomial
Returns
value of polynomial for given input

Definition at line 1027 of file yield_plugin.cpp.

1028 {
1029 double result = 0;
1030 for (size_t i = 0; i < coeff.size(); i++)
1031 {
1032 double value = coeff.at(i) * pow(x, static_cast<int>(coeff.size() - 1 - i));
1033 result = result + value;
1034 }
1035 return result;
1036 }

References process_bag::i, and process_traj_logs::x.

Referenced by generate_JMT_trajectory().

Here is the caller graph for this function:

◆ polynomial_calc_d()

double yield_plugin::YieldPlugin::polynomial_calc_d ( std::vector< double >  coeff,
double  x 
) const

calculate derivative of quintic polynomial equation for a given x

Parameters
coeffvector including polynomial coefficiencrs
xinput variable to the polynomial
Returns
value of derivative polynomial for given input

Definition at line 1038 of file yield_plugin.cpp.

1039 {
1040 double result = 0;
1041 for (size_t i = 0; i < coeff.size()-1; i++)
1042 {
1043 double value = static_cast<int>(coeff.size() - 1 - i) * coeff.at(i) * pow(x, static_cast<int>(coeff.size() - 2 - i));
1044 result = result + value;
1045 }
1046 return result;
1047 }

References process_bag::i, and process_traj_logs::x.

Referenced by generate_JMT_trajectory().

Here is the caller graph for this function:

◆ set_external_objects()

void yield_plugin::YieldPlugin::set_external_objects ( const std::vector< carma_perception_msgs::msg::ExternalObject > &  object_list)

Setter for external objects with predictions in the environment.

Parameters
object_listThe object list.

Definition at line 1108 of file yield_plugin.cpp.

1109 {
1110 external_objects_ = object_list;
1111 }

References external_objects_.

◆ set_georeference_string()

void yield_plugin::YieldPlugin::set_georeference_string ( const std::string &  georeference)

Setter for map projection string to define lat/lon -> map conversion.

Parameters
georeferenceThe proj string defining the projection.

Definition at line 1099 of file yield_plugin.cpp.

1100 {
1101 if (georeference_ != georeference)
1102 {
1103 georeference_ = georeference;
1104 map_projector_ = std::make_shared<lanelet::projection::LocalFrameProjector>(georeference.c_str()); // Build projector from proj string
1105 }
1106 }

References georeference_, and map_projector_.

◆ set_incoming_request_info()

void yield_plugin::YieldPlugin::set_incoming_request_info ( std::vector< lanelet::BasicPoint2d >  req_trajectory,
double  req_speed,
double  req_planning_time,
double  req_timestamp 
)

set values for member variables related to cooperative behavior

Parameters
req_trajectoryrequested trajectory
req_speedspeed of requested cooperative behavior
req_planning_timeplanning time for the requested cooperative behavior
req_timestampthe mobility request time stamp

Definition at line 237 of file yield_plugin.cpp.

238 {
239 req_trajectory_points_ = req_trajectory;
240 req_target_speed_ = req_speed;
241 req_target_plan_time_ = req_planning_time;
242 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"req_target_plan_time_" << req_target_plan_time_);
243 req_timestamp_ = req_timestamp;
244 }
std::vector< lanelet::BasicPoint2d > req_trajectory_points_

References nh_, req_target_plan_time_, req_target_speed_, req_timestamp_, and req_trajectory_points_.

Referenced by mobilityrequest_cb().

Here is the caller graph for this function:

◆ update_traj_for_cooperative_behavior()

carma_planning_msgs::msg::TrajectoryPlan yield_plugin::YieldPlugin::update_traj_for_cooperative_behavior ( const carma_planning_msgs::msg::TrajectoryPlan &  original_tp,
double  current_speed 
)

update trajectory for yielding to an incoming cooperative behavior

Parameters
original_tporiginal trajectory plan
current_speedcurrent speed of the vehicle
Returns
updated trajectory for cooperative behavior

Definition at line 327 of file yield_plugin.cpp.

328 {
329 carma_planning_msgs::msg::TrajectoryPlan cooperative_trajectory;
330
331 double initial_pos = 0;
332 double goal_pos;
333 double initial_velocity = current_speed;
334 double goal_velocity = req_target_speed_;
335 double planning_time = req_target_plan_time_;
336
337 std::vector<lanelet::BasicPoint2d> host_traj_points = {};
338 for (size_t i=0; i<original_tp.trajectory_points.size(); i++)
339 {
340 lanelet::BasicPoint2d traj_point;
341 traj_point.x() = original_tp.trajectory_points.at(i).x;
342 traj_point.y() = original_tp.trajectory_points.at(i).y;
343 host_traj_points.push_back(traj_point);
344 }
345
346 std::vector<std::pair<int, lanelet::BasicPoint2d>> intersection_points = detect_trajectories_intersection(host_traj_points, req_trajectory_points_);
347 if (!intersection_points.empty())
348 {
349 lanelet::BasicPoint2d intersection_point = intersection_points[0].second;
350 double dx = original_tp.trajectory_points[0].x - intersection_point.x();
351 double dy = original_tp.trajectory_points[0].y - intersection_point.y();
352 // check if a digital_gap is available
353 double digital_gap = check_traj_for_digital_min_gap(original_tp);
354 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"digital_gap: " << digital_gap);
355 goal_pos = sqrt(dx*dx + dy*dy) - std::max(config_.minimum_safety_gap_in_meters, digital_gap);
356 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Goal position (goal_pos): " << goal_pos);
357 double collision_time = req_timestamp_ + (intersection_points[0].first * ecef_traj_timestep_) - config_.safety_collision_time_gap_in_s;
358 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"req time stamp: " << req_timestamp_);
359 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Collision time: " << collision_time);
360 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"intersection num: " << intersection_points[0].first);
361 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Planning time: " << planning_time);
362 // calculate distance traveled from beginning of trajectory to collision point
363 double dx2 = intersection_point.x() - req_trajectory_points_[0].x();
364 double dy2 = intersection_point.y() - req_trajectory_points_[0].y();
365 // calculate incoming trajectory speed from time and distance between trajectory points
366 double incoming_trajectory_speed = sqrt(dx2*dx2 + dy2*dy2)/(intersection_points[0].first * ecef_traj_timestep_);
367 // calculate goal velocity from request trajectory
368 goal_velocity = std::min(goal_velocity, incoming_trajectory_speed);
369 double min_time = (initial_velocity - goal_velocity)/config_.yield_max_deceleration_in_ms2;
370
371 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"goal_velocity: " << goal_velocity);
372 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"incoming_trajectory_speed: " << incoming_trajectory_speed);
373
374 if (planning_time > min_time)
375 {
377 double original_max_speed = max_trajectory_speed(original_tp.trajectory_points, get_trajectory_end_time(original_tp));
378 cooperative_trajectory = generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, planning_time, original_max_speed);
379 }
380 else
381 {
383 RCLCPP_DEBUG(nh_->get_logger(),"The incoming requested trajectory is rejected, due to insufficient gap");
384 cooperative_trajectory = original_tp;
385 }
386
387 }
388 else
389 {
391 RCLCPP_DEBUG(nh_->get_logger(),"The incoming requested trajectory does not overlap with host vehicle's trajectory");
392 cooperative_trajectory = original_tp;
393 }
394
395 return cooperative_trajectory;
396 }
double check_traj_for_digital_min_gap(const carma_planning_msgs::msg::TrajectoryPlan &original_tp) const
checks trajectory for minimum gap associated with it from the road
std::vector< std::pair< int, lanelet::BasicPoint2d > > detect_trajectories_intersection(std::vector< lanelet::BasicPoint2d > self_trajectory, std::vector< lanelet::BasicPoint2d > incoming_trajectory) const
detect intersection point(s) of two trajectories
carma_planning_msgs::msg::TrajectoryPlan generate_JMT_trajectory(const carma_planning_msgs::msg::TrajectoryPlan &original_tp, double initial_pos, double goal_pos, double initial_velocity, double goal_velocity, double planning_time, double original_max_speed)
generate a Jerk Minimizing Trajectory(JMT) with the provided start and end conditions
double yield_max_deceleration_in_ms2
double safety_collision_time_gap_in_s

References check_traj_for_digital_min_gap(), config_, cooperative_request_acceptable_, detect_trajectories_intersection(), ecef_traj_timestep_, generate_JMT_trajectory(), yield_plugin::get_trajectory_end_time(), process_bag::i, max_trajectory_speed(), YieldPluginConfig::minimum_safety_gap_in_meters, nh_, req_target_plan_time_, req_target_speed_, req_timestamp_, req_trajectory_points_, YieldPluginConfig::safety_collision_time_gap_in_s, and YieldPluginConfig::yield_max_deceleration_in_ms2.

Referenced by plan_trajectory_callback().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ update_traj_for_object()

carma_planning_msgs::msg::TrajectoryPlan yield_plugin::YieldPlugin::update_traj_for_object ( const carma_planning_msgs::msg::TrajectoryPlan &  original_tp,
const std::vector< carma_perception_msgs::msg::ExternalObject > &  external_objects,
double  initial_velocity 
)

trajectory is modified to safely avoid obstacles on the road

Parameters
original_tporiginal trajectory plan without object avoidance
current_speed_current speed of the vehicle

return modified trajectory plan

Definition at line 924 of file yield_plugin.cpp.

926 {
927 if (original_tp.trajectory_points.size() < 2)
928 {
929 RCLCPP_WARN(nh_->get_logger(), "Yield plugin received less than 2 points in update_traj_for_object, returning unchanged...");
930 return original_tp;
931 }
932
933 // Get earliest collision object
934 const auto earliest_collision_obj_pair = get_earliest_collision_object_and_time(original_tp, external_objects);
935
936 if (!earliest_collision_obj_pair)
937 {
938 RCLCPP_DEBUG(nh_->get_logger(),"No collision detected, so trajectory not modified.");
939 return original_tp;
940 }
941
942 carma_perception_msgs::msg::ExternalObject earliest_collision_obj = earliest_collision_obj_pair.value().first;
943 double earliest_collision_time_in_seconds = earliest_collision_obj_pair.value().second;
944
945 // Issue (https://github.com/usdot-fhwa-stol/carma-platform/issues/2155): If the yield_plugin can detect if the roadway object is moving along the route,
946 // it is able to plan yielding much earlier and smoother using on_route_vehicle_collision_horizon_in_s.
947
948 const lanelet::BasicPoint2d vehicle_point(original_tp.trajectory_points[0].x,original_tp.trajectory_points[0].y);
949 const double vehicle_downtrack = wm_->routeTrackPos(vehicle_point).downtrack;
950
951 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"vehicle_downtrack: " << vehicle_downtrack);
952
953 RCLCPP_WARN_STREAM(nh_->get_logger(),"Collision Detected!");
954
955 const lanelet::BasicPoint2d object_point(earliest_collision_obj.pose.pose.position.x, earliest_collision_obj.pose.pose.position.y);
956 const double object_downtrack = wm_->routeTrackPos(object_point).downtrack;
957
958 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object_downtrack: " << object_downtrack);
959
960 const double object_downtrack_lead = std::max(0.0, object_downtrack - vehicle_downtrack);
961 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object_downtrack_lead: " << object_downtrack_lead);
962
963 // The vehicle's goal velocity of the yielding behavior is to match the velocity of the object along the trajectory.
964 double goal_velocity = get_predicted_velocity_at_time(earliest_collision_obj.velocity.twist, original_tp, earliest_collision_time_in_seconds);
965 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object's speed along trajectory at collision: " << goal_velocity);
966
967 // roadway object position
968 const double gap_time_until_min_gap_distance = std::max(0.0, object_downtrack_lead - config_.minimum_safety_gap_in_meters)/initial_velocity;
969
970 if (goal_velocity <= config_.obstacle_zero_speed_threshold_in_ms){
971 RCLCPP_WARN_STREAM(nh_->get_logger(),"The obstacle is not moving, goal velocity is set to 0 from: " << goal_velocity);
972 goal_velocity = 0.0;
973 }
974
975 // determine the safety inter-vehicle gap based on speed
976 double safety_gap = std::max(goal_velocity * gap_time_until_min_gap_distance, config_.minimum_safety_gap_in_meters);
977 if (!std::isnormal(safety_gap))
978 {
979 RCLCPP_WARN_STREAM(rclcpp::get_logger("yield_plugin"),"Detected non-normal (nan, inf, etc.) safety_gap."
980 "Making it desired safety gap configured at config_.minimum_safety_gap_in_meters: " << config_.minimum_safety_gap_in_meters);
982 }
984 {
985 // externally_commanded_safety_gap is desired distance gap commanded from external sources
986 // such as different plugin, map, or infrastructure depending on the use case
987 double externally_commanded_safety_gap = check_traj_for_digital_min_gap(original_tp);
988 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"externally_commanded_safety_gap: " << externally_commanded_safety_gap);
989 // if a digital gap is available, it is replaced as safety gap
990 safety_gap = std::max(safety_gap, externally_commanded_safety_gap);
991 }
992
993 const double goal_pos = std::max(0.0, object_downtrack_lead - safety_gap - config_.vehicle_length);
994 const double initial_pos = 0.0; //relative initial position (first trajectory point)
995 const double original_max_speed = max_trajectory_speed(original_tp.trajectory_points, earliest_collision_time_in_seconds);
996 const double delta_v_max = fabs(goal_velocity - original_max_speed);
997 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"delta_v_max: " << delta_v_max << ", safety_gap: " << safety_gap);
998
999 const double time_required_for_comfortable_decel_in_s = config_.acceleration_adjustment_factor * 2 * goal_pos / delta_v_max;
1000 const double min_time_required_for_comfortable_decel_in_s = delta_v_max / config_.yield_max_deceleration_in_ms2;
1001
1002 // planning time for object avoidance
1003 double planning_time_in_s = std::max({config_.min_obj_avoidance_plan_time_in_s, time_required_for_comfortable_decel_in_s, min_time_required_for_comfortable_decel_in_s});
1004 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"time_required_for_comfortable_decel_in_s: " << time_required_for_comfortable_decel_in_s << ", min_time_required_for_comfortable_decel_in_s: " << min_time_required_for_comfortable_decel_in_s);
1005
1006 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Object avoidance planning time: " << planning_time_in_s);
1007
1008 return generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, planning_time_in_s, original_max_speed);
1009 }
double get_predicted_velocity_at_time(const geometry_msgs::msg::Twist &object_velocity_in_map_frame, const carma_planning_msgs::msg::TrajectoryPlan &original_tp, double timestamp_in_sec_to_predict)
Given the object velocity in map frame with x,y components, this function returns the projected veloc...
std::optional< std::pair< carma_perception_msgs::msg::ExternalObject, double > > get_earliest_collision_object_and_time(const carma_planning_msgs::msg::TrajectoryPlan &original_tp, const std::vector< carma_perception_msgs::msg::ExternalObject > &external_objects)
Return the earliest collision object and time of collision pair from the given trajectory and list of...
double acceleration_adjustment_factor

References YieldPluginConfig::acceleration_adjustment_factor, check_traj_for_digital_min_gap(), config_, YieldPluginConfig::enable_adjustable_gap, generate_JMT_trajectory(), get_earliest_collision_object_and_time(), get_predicted_velocity_at_time(), max_trajectory_speed(), YieldPluginConfig::min_obj_avoidance_plan_time_in_s, YieldPluginConfig::minimum_safety_gap_in_meters, nh_, YieldPluginConfig::obstacle_zero_speed_threshold_in_ms, YieldPluginConfig::vehicle_length, wm_, and YieldPluginConfig::yield_max_deceleration_in_ms2.

Referenced by plan_trajectory_callback().

Here is the call graph for this function:
Here is the caller graph for this function:

Member Data Documentation

◆ clc_urgency_

int yield_plugin::YieldPlugin::clc_urgency_ = 0
private

Definition at line 329 of file yield_plugin.hpp.

Referenced by mobilityrequest_cb(), and plan_trajectory_callback().

◆ config_

◆ consecutive_clearance_count_for_obstacles_

std::unordered_map<uint32_t, int> yield_plugin::YieldPlugin::consecutive_clearance_count_for_obstacles_
private

Definition at line 319 of file yield_plugin.hpp.

Referenced by get_collision_time(), and is_object_behind_vehicle().

◆ cooperative_request_acceptable_

bool yield_plugin::YieldPlugin::cooperative_request_acceptable_ = false
private

Definition at line 321 of file yield_plugin.hpp.

Referenced by mobilityrequest_cb(), and update_traj_for_cooperative_behavior().

◆ current_speed_

double yield_plugin::YieldPlugin::current_speed_
private

Definition at line 336 of file yield_plugin.hpp.

◆ ecef_traj_timestep_

double yield_plugin::YieldPlugin::ecef_traj_timestep_ = 0.1
private

Definition at line 333 of file yield_plugin.hpp.

Referenced by update_traj_for_cooperative_behavior().

◆ external_objects_

std::vector<carma_perception_msgs::msg::ExternalObject> yield_plugin::YieldPlugin::external_objects_
private

Definition at line 318 of file yield_plugin.hpp.

Referenced by plan_trajectory_callback(), and set_external_objects().

◆ georeference_

std::string yield_plugin::YieldPlugin::georeference_ {""}
private

Definition at line 340 of file yield_plugin.hpp.

Referenced by set_georeference_string().

◆ host_bsm_id_

std::string yield_plugin::YieldPlugin::host_bsm_id_
private

Definition at line 338 of file yield_plugin.hpp.

Referenced by bsm_cb(), and compose_mobility_response().

◆ host_vehicle_size

geometry_msgs::msg::Vector3 yield_plugin::YieldPlugin::host_vehicle_size
private

Definition at line 335 of file yield_plugin.hpp.

◆ last_speed_

std::optional<double> yield_plugin::YieldPlugin::last_speed_ = std::nullopt
private

Definition at line 330 of file yield_plugin.hpp.

Referenced by generate_JMT_trajectory().

◆ last_speed_time_

std::optional<rclcpp::Time> yield_plugin::YieldPlugin::last_speed_time_ = std::nullopt
private

Definition at line 331 of file yield_plugin.hpp.

Referenced by generate_JMT_trajectory().

◆ lc_status_publisher_

LaneChangeStatusCB yield_plugin::YieldPlugin::lc_status_publisher_
private

Definition at line 314 of file yield_plugin.hpp.

Referenced by mobilityrequest_cb().

◆ map_projector_

std::shared_ptr<lanelet::projection::LocalFrameProjector> yield_plugin::YieldPlugin::map_projector_
private

◆ mobility_response_publisher_

MobilityResponseCB yield_plugin::YieldPlugin::mobility_response_publisher_
private

Definition at line 313 of file yield_plugin.hpp.

Referenced by mobilityrequest_cb().

◆ nh_

◆ previous_llt_id_

lanelet::Id yield_plugin::YieldPlugin::previous_llt_id_
private

Definition at line 317 of file yield_plugin.hpp.

◆ req_target_plan_time_

double yield_plugin::YieldPlugin::req_target_plan_time_ = 0
private

◆ req_target_speed_

double yield_plugin::YieldPlugin::req_target_speed_ = 0
private

◆ req_timestamp_

double yield_plugin::YieldPlugin::req_timestamp_ = 0
private

◆ req_trajectory_points_

std::vector<lanelet::BasicPoint2d> yield_plugin::YieldPlugin::req_trajectory_points_
private

◆ route_llt_ids_

std::set<lanelet::Id> yield_plugin::YieldPlugin::route_llt_ids_
private

Definition at line 316 of file yield_plugin.hpp.

Referenced by get_collision(), and get_earliest_collision_object_and_time().

◆ timesteps_since_last_req_

int yield_plugin::YieldPlugin::timesteps_since_last_req_ = 0
private

Definition at line 328 of file yield_plugin.hpp.

Referenced by mobilityrequest_cb(), and plan_trajectory_callback().

◆ wm_


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