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
 
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 342 of file yield_plugin.hpp.

343 {
344 std::string res = "";
345 for (size_t i=0; i<bsm_core.id.size(); i++)
346 {
347 res+=std::to_string(bsm_core.id[i]);
348 }
349 return res;
350 }
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 1046 of file yield_plugin.cpp.

1047 {
1048 double desired_gap = 0;
1049
1050 for (size_t i = 0; i < original_tp.trajectory_points.size(); i++)
1051 {
1052 lanelet::BasicPoint2d veh_pos(original_tp.trajectory_points.at(i).x, original_tp.trajectory_points.at(i).y);
1053 auto llts = wm_->getLaneletsFromPoint(veh_pos, 1);
1054 if (llts.empty())
1055 {
1056 // This should technically never happen
1057 // However, trajectory generation currently may fail due to osm map issue https://github.com/usdot-fhwa-stol/carma-platform/issues/2503
1058 RCLCPP_WARN_STREAM(nh_->get_logger(),"Trajectory point: x= " << original_tp.trajectory_points.at(i).x << "y="<< original_tp.trajectory_points.at(i).y);
1059 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);
1060 return desired_gap;
1061 }
1062 auto digital_min_gap = llts[0].regulatoryElementsAs<lanelet::DigitalMinimumGap>(); //Returns a list of these elements)
1063 if (!digital_min_gap.empty())
1064 {
1065 double digital_gap = digital_min_gap[0]->getMinimumGap(); // Provided gap is in meters
1066 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Digital Gap found with value: " << digital_gap);
1067 desired_gap = std::max(desired_gap, digital_gap);
1068 }
1069 }
1070 return desired_gap;
1071 }
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 const double initial_accel = 0;
431 const double goal_accel = 0;
432 int new_traj_idx = 1;
433 int original_traj_idx = 1;
434 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Following parameters used for JMT: "
435 "\ninitial_pos: " << initial_pos <<
436 "\ngoal_pos: " << goal_pos <<
437 "\ninitial_velocity: " << initial_velocity <<
438 "\ngoal_velocity: " << goal_velocity <<
439 "\ninitial_accel: " << initial_accel <<
440 "\ngoal_accel: " << goal_accel <<
441 "\nplanning_time: " << planning_time <<
442 "\noriginal_max_speed: " << original_max_speed);
443
444 // Get the polynomial solutions used to generate the trajectory
445 std::vector<double> polynomial_coefficients = quintic_coefficient_calculator::quintic_coefficient_calculator(initial_pos,
446 goal_pos,
447 initial_velocity,
448 goal_velocity,
449 initial_accel,
450 goal_accel,
451 initial_time,
452 planning_time);
453 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Used original_max_speed: " << original_max_speed);
454 const auto smallest_time_step = get_smallest_time_step_of_traj(original_tp);
455 while (new_traj_accumulated_downtrack < goal_pos - EPSILON && original_traj_idx < original_traj_relative_downtracks.size())
456 {
457 const double target_time = new_traj_idx * smallest_time_step;
458 const double downtrack_at_target_time = polynomial_calc(polynomial_coefficients, target_time);
459 double velocity_at_target_time = polynomial_calc_d(polynomial_coefficients, target_time);
460
461 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Calculated speed velocity_at_target_time: " << velocity_at_target_time
462 << ", downtrack_at_target_time: "<< downtrack_at_target_time << ", target_time: " << target_time);
463
464 // if the speed becomes negative, the downtrack starts reversing to negative as well
465 // which will never reach the goal_pos, so break here.
466 if (velocity_at_target_time < 0.0)
467 {
468 break;
469 }
470
471 // Cannot have a negative speed or have a higher speed than that of the original trajectory
472 velocity_at_target_time = std::clamp(velocity_at_target_time, 0.0, original_max_speed);
473
474 // Pick the speed if it matches with the original downtracks
475 if (downtrack_at_target_time >= original_traj_accumulated_downtrack)
476 {
477 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Picked calculated speed velocity_at_target_time: " << velocity_at_target_time
478 << ", downtrack_at_target_time: "<< downtrack_at_target_time << ", target_time: " << target_time);
479 // velocity_at_target_time doesn't exactly correspond to original_traj_accumulated_downtrack but does for new_traj_accumulated_downtrack.
480 // however, the logic is assuming they are close enough that the speed is usable
481 calculated_speeds.push_back(velocity_at_target_time);
482 original_traj_accumulated_downtrack += original_traj_relative_downtracks.at(original_traj_idx);
483 original_traj_idx ++;
484 }
485 new_traj_accumulated_downtrack = downtrack_at_target_time;
486 new_traj_idx++;
487
488 }
489
490 // if the loop above finished prematurely due to negative speed, fill with 0.0 speeds
491 // since the speed crossed 0.0 and algorithm indicates stopping
492 std::fill_n(std::back_inserter(calculated_speeds),
493 std::size(original_traj_relative_downtracks) - std::size(calculated_speeds),
494 0.0);
495
496 // Moving average filter to smoothen the speeds
497 std::vector<double> filtered_speeds = basic_autonomy::smoothing::moving_average_filter(calculated_speeds, config_.speed_moving_average_window_size);
498 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "filtered_speeds size: " << filtered_speeds.size());
499
500 // Replace the original trajectory's associated timestamps based on the newly calculated speeds
501 double prev_speed = filtered_speeds.at(0);
502 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "start speed: " << prev_speed << ", target_time: " << std::to_string(rclcpp::Time(original_tp.trajectory_points[0].target_time).seconds()));
503
504 for(size_t i = 1; i < original_tp.trajectory_points.size(); i++)
505 {
506 carma_planning_msgs::msg::TrajectoryPlanPoint jmt_tpp = original_tp.trajectory_points.at(i);
507
508 // In case only subset of original trajectory needs modification,
509 // the rest of the points should keep the last speed to cruise
510 double current_speed = goal_velocity;
511
512 if (i < filtered_speeds.size())
513 {
514 current_speed = filtered_speeds.at(i);
515 }
516
517 //Force the speed to 0 if below configured value for more control over stopping behavior
518 if (current_speed < config_.max_stop_speed_in_ms)
519 {
520 current_speed = 0;
521 }
522
523 // Derived from constant accelaration kinematic equation: (vi + vf) / 2 * dt = d_dist
524 // This also handles a case correctly when current_speed is 0, but prev_speed is not 0 yet
525 const double dt = (2 * original_traj_relative_downtracks.at(i)) / (current_speed + prev_speed);
526 jmt_tpp.target_time = rclcpp::Time(jmt_trajectory_points.back().target_time) + rclcpp::Duration::from_nanoseconds(dt*1e9);
527
528 if (prev_speed < EPSILON) // Handle a special case if prev_speed (thus current_speed too) is 0
529 {
530 // NOTE: Assigning arbitrary 100 mins dt between points where normally dt is only 1 sec to model a stopping behavior.
531 // Another way to model it is to keep the trajectory point at a same location and increment time slightly. However,
532 // if the vehicle goes past the point, it may cruise toward undesirable location (for example into the intersection).
533 // Keeping the points help the controller steer the vehicle toward direction of travel even when stopping.
534 // Only downside is the trajectory plan is huge where only 15 sec is expected, but since this is stopping case, it shouldn't matter.
535 jmt_tpp.target_time = rclcpp::Time(jmt_trajectory_points.back().target_time) + rclcpp::Duration::from_nanoseconds(6000 * 1e9);
536 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Zero speed = x: " << jmt_tpp.x << ", y:" << jmt_tpp.y
537 << ", t:" << std::to_string(rclcpp::Time(jmt_tpp.target_time).seconds())
538 << ", prev_speed: " << prev_speed << ", current_speed: " << current_speed);
539 }
540 else
541 {
542 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Non-zero speed = x: " << jmt_tpp.x << ", y:" << jmt_tpp.y
543 << ", t:" << std::to_string(rclcpp::Time(jmt_tpp.target_time).seconds())
544 << ", prev_speed: " << prev_speed << ", current_speed: " << current_speed);
545 }
546
547 jmt_trajectory_points.push_back(jmt_tpp);
548 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());
549 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "insta_decel: " << insta_decel );
550 prev_speed = current_speed;
551 }
552
553 jmt_trajectory.header = original_tp.header;
554 jmt_trajectory.trajectory_id = original_tp.trajectory_id;
555 jmt_trajectory.trajectory_points = jmt_trajectory_points;
556 jmt_trajectory.initial_longitudinal_velocity = initial_velocity;
557 return jmt_trajectory;
558 }
std::vector< double > get_relative_downtracks(const carma_planning_msgs::msg::TrajectoryPlan &trajectory_plan) const
calculates distance between trajectory points in a plan
double polynomial_calc(std::vector< double > coeff, double x) const
calculate quintic polynomial equation for a given x
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, 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 560 of file yield_plugin.cpp.

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

735 {
736 auto plan_start_time = get_trajectory_start_time(original_tp);
737
738 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Object's back time: " << std::to_string(rclcpp::Time(curr_obstacle.predictions.back().header.stamp).seconds())
739 << ", plan_start_time: " << std::to_string(plan_start_time));
740
741 // do not process outdated objects
742 if (rclcpp::Time(curr_obstacle.predictions.back().header.stamp).seconds() <= plan_start_time)
743 {
744 return std::nullopt;
745 }
746
747 std::vector<carma_perception_msgs::msg::PredictedState> new_list;
748 carma_perception_msgs::msg::PredictedState curr_state;
749 // artificially include current position as one of the predicted states
750 curr_state.header.stamp = curr_obstacle.header.stamp;
751 curr_state.predicted_position.position.x = curr_obstacle.pose.pose.position.x;
752 curr_state.predicted_position.position.y = curr_obstacle.pose.pose.position.y;
753 // NOTE: predicted_velocity is not used for collision calculation, but timestamps
754 curr_state.predicted_velocity.linear.x = curr_obstacle.velocity.twist.linear.x;
755 curr_state.predicted_velocity.linear.y = curr_obstacle.velocity.twist.linear.y;
756 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Object: " << curr_obstacle.id <<", type: " << static_cast<int>(curr_obstacle.object_type)
757 << ", speed_x: " << curr_obstacle.velocity.twist.linear.x << ", speed_y: " << curr_obstacle.velocity.twist.linear.y);
758 new_list.push_back(curr_state);
759 new_list.insert(new_list.end(), curr_obstacle.predictions.cbegin(), curr_obstacle.predictions.cend());
760
761 const auto collision_result = get_collision(original_tp, new_list, config_.intervehicle_collision_distance_in_m, original_tp_max_speed);
762
763 if (!collision_result)
764 {
765 // reset the consecutive clearance counter because no collision was detected at this iteration
766 consecutive_clearance_count_for_obstacles_[curr_obstacle.id] = 0;
767 return std::nullopt;
768 }
769
770 // if within collision radius, it is not a collision if obstacle is behind the vehicle despite being in collision radius
771 const double vehicle_downtrack = wm_->routeTrackPos(collision_result.value().point1).downtrack;
772 const double object_downtrack = wm_->routeTrackPos(collision_result.value().point2).downtrack;
773
774 if (is_object_behind_vehicle(curr_obstacle.id, collision_result.value().collision_time, vehicle_downtrack, object_downtrack))
775 {
776 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()));
777 return std::nullopt;
778 }
779
780 const auto distance{std::hypot(
781 collision_result.value().point1.x() - collision_result.value().point2.x(),
782 collision_result.value().point1.y() - collision_result.value().point2.y()
783 )}; //for debug
784
785 RCLCPP_WARN_STREAM(nh_->get_logger(), "Collision detected for object: " << curr_obstacle.id << ", at timestamp " << std::to_string(collision_result.value().collision_time.seconds()) <<
786 ", x: " << collision_result.value().point1.x() << ", y: " << collision_result.value().point1.y() <<
787 ", within actual downtrack distance: " << object_downtrack - vehicle_downtrack <<
788 ", and collision distance: " << distance);
789
790 return collision_result.value().collision_time;
791 }
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 793 of file yield_plugin.cpp.

795 {
796
797 std::unordered_map<uint32_t, std::future<std::optional<rclcpp::Time>>> futures;
798 std::unordered_map<uint32_t, rclcpp::Time> collision_times;
799
800 // Launch asynchronous tasks to check for collision times
801 for (const auto& object : external_objects) {
802 futures[object.id] = std::async(std::launch::async,[this, &original_tp, &object, &original_tp_max_speed]{
803 return get_collision_time(original_tp, object, original_tp_max_speed);
804 });
805 }
806
807 // Collect results from futures and update collision_times
808 for (const auto& object : external_objects) {
809 if (const auto collision_time{futures.at(object.id).get()}) {
810 collision_times[object.id] = collision_time.value();
811 }
812 }
813
814 return collision_times;
815 }
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 817 of file yield_plugin.cpp.

819 {
820 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "ExternalObjects size: " << external_objects.size());
821
822 if (!wm_->getRoute())
823 {
824 RCLCPP_WARN(nh_->get_logger(), "Yield plugin was not able to analyze collision since route is not available! Please check if route is set");
825 return std::nullopt;
826 }
827
828 // save route Ids for faster access
829 for (const auto& llt: wm_->getRoute()->shortestPath())
830 {
831 // TODO: Enhancement https://github.com/usdot-fhwa-stol/carma-platform/issues/2316
832 route_llt_ids_.insert(llt.id());
833 }
834
835 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"External Object List (external_objects) size: " << external_objects.size());
836 const double original_max_speed = max_trajectory_speed(original_tp.trajectory_points, get_trajectory_end_time(original_tp));
837 std::unordered_map<uint32_t, rclcpp::Time> collision_times = get_collision_times_concurrently(original_tp,external_objects, original_max_speed);
838
839 if (collision_times.empty()) { return std::nullopt; }
840
841 const auto earliest_colliding_object_id{std::min_element(
842 std::cbegin(collision_times), std::cend(collision_times),
843 [](const auto & a, const auto & b){ return a.second < b.second; })->first};
844
845 const auto earliest_colliding_object{std::find_if(
846 std::cbegin(external_objects), std::cend(external_objects),
847 [&earliest_colliding_object_id](const auto & object) { return object.id == earliest_colliding_object_id; })};
848
849 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"earliest object x: " << earliest_colliding_object->velocity.twist.linear.x
850 << ", y: " << earliest_colliding_object->velocity.twist.linear.y);
851 return std::make_pair(*earliest_colliding_object, collision_times.at(earliest_colliding_object_id).seconds());
852
853 }
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 855 of file yield_plugin.cpp.

857 {
858 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "timestamp_in_sec_to_predict: " << std::to_string(timestamp_in_sec_to_predict) <<
859 ", trajectory_end_time: " << std::to_string(get_trajectory_end_time(original_tp)));
860
861 double point_b_time = 0.0;
862 carma_planning_msgs::msg::TrajectoryPlanPoint point_a;
863 carma_planning_msgs::msg::TrajectoryPlanPoint point_b;
864
865 // trajectory points' time is guaranteed to be increasing
866 // then find the corresponding point at timestamp_in_sec_to_predict
867 for (size_t i = 0; i < original_tp.trajectory_points.size() - 1; ++i)
868 {
869 point_a = original_tp.trajectory_points.at(i);
870 point_b = original_tp.trajectory_points.at(i + 1);
871 point_b_time = rclcpp::Time(point_b.target_time).seconds();
872 if (point_b_time >= timestamp_in_sec_to_predict)
873 {
874 break;
875 }
876 }
877
878 auto dx = point_b.x - point_a.x;
879 auto dy = point_b.y - point_a.y;
880 const tf2::Vector3 trajectory_direction(dx, dy, 0);
881
882 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "timestamp_in_sec_to_predict: " << std::to_string(timestamp_in_sec_to_predict)
883 << ", point_b_time: " << std::to_string(point_b_time)
884 << ", dx: " << dx << ", dy: " << dy << ", "
885 << ", object_velocity_in_map_frame.x: " << object_velocity_in_map_frame.linear.x
886 << ", object_velocity_in_map_frame.y: " << object_velocity_in_map_frame.linear.y);
887
888 if (trajectory_direction.length() < 0.001) //EPSILON
889 {
890 return 0.0;
891 }
892
893 const tf2::Vector3 object_direction(object_velocity_in_map_frame.linear.x, object_velocity_in_map_frame.linear.y, 0);
894
895 return tf2::tf2Dot(object_direction, trajectory_direction) / trajectory_direction.length();
896 }

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 986 of file yield_plugin.cpp.

987 {
988 std::vector<double> downtracks;
989 downtracks.reserve(trajectory_plan.trajectory_points.size());
990 // relative downtrack distance of the fist Point is 0.0
991 downtracks.push_back(0.0);
992 for (size_t i=1; i < trajectory_plan.trajectory_points.size(); i++){
993
994 double dx = trajectory_plan.trajectory_points.at(i).x - trajectory_plan.trajectory_points.at(i-1).x;
995 double dy = trajectory_plan.trajectory_points.at(i).y - trajectory_plan.trajectory_points.at(i-1).y;
996 downtracks.push_back(sqrt(dx*dx + dy*dy));
997 }
998 return downtracks;
999 }

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 707 of file yield_plugin.cpp.

708 {
709 const auto previous_clearance_count = consecutive_clearance_count_for_obstacles_[object_id];
710 // if the object's location is half a length of the vehicle past its rear-axle, it is considered behind
711 // half a length of the vehicle to conservatively estimate the rear axle to rear bumper length
712 if (object_downtrack < vehicle_downtrack - config_.vehicle_length / 2)
713 {
715 RCLCPP_INFO_STREAM(nh_->get_logger(), "Detected an object nearby might be behind the vehicle at timestamp: " << std::to_string(collision_time.seconds()) <<
716 ", and consecutive_clearance_count_for obstacle: " << object_id << ", is: " << consecutive_clearance_count_for_obstacles_[object_id]);
717 }
718 // confirmed false positive for a collision
720 {
721 return true;
722 }
723 // if the clearance counter didn't increase by this point, true collision was detected
724 // therefore reset the consecutive clearance counter as it is no longer consecutive
725 if (consecutive_clearance_count_for_obstacles_[object_id] == previous_clearance_count)
726 {
728 }
729
730 return false;
731 }
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 1023 of file yield_plugin.cpp.

1024 {
1025 double max_speed = 0;
1026 for(size_t i = 0; i < trajectory_points.size() - 2; i++ )
1027 {
1028 double dx = trajectory_points.at(i + 1).x - trajectory_points.at(i).x;
1029 double dy = trajectory_points.at(i + 1).y - trajectory_points.at(i).y;
1030 double d = sqrt(dx*dx + dy*dy);
1031 double t = (rclcpp::Time(trajectory_points.at(i + 1).target_time).seconds() - rclcpp::Time(trajectory_points.at(i).target_time).seconds());
1032 double v = d/t;
1033 if(v > max_speed)
1034 {
1035 max_speed = v;
1036 }
1037 if (rclcpp::Time(trajectory_points.at(i + 1).target_time).seconds() >= timestamp_in_sec_to_search_until)
1038 {
1039 break;
1040 }
1041
1042 }
1043 return max_speed;
1044 }

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 1001 of file yield_plugin.cpp.

1002 {
1003 double result = 0;
1004 for (size_t i = 0; i < coeff.size(); i++)
1005 {
1006 double value = coeff.at(i) * pow(x, static_cast<int>(coeff.size() - 1 - i));
1007 result = result + value;
1008 }
1009 return result;
1010 }

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 1012 of file yield_plugin.cpp.

1013 {
1014 double result = 0;
1015 for (size_t i = 0; i < coeff.size()-1; i++)
1016 {
1017 double value = static_cast<int>(coeff.size() - 1 - i) * coeff.at(i) * pow(x, static_cast<int>(coeff.size() - 2 - i));
1018 result = result + value;
1019 }
1020 return result;
1021 }

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 1082 of file yield_plugin.cpp.

1083 {
1084 external_objects_ = object_list;
1085 }

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 1073 of file yield_plugin.cpp.

1074 {
1075 if (georeference_ != georeference)
1076 {
1077 georeference_ = georeference;
1078 map_projector_ = std::make_shared<lanelet::projection::LocalFrameProjector>(georeference.c_str()); // Build projector from proj string
1079 }
1080 }

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 898 of file yield_plugin.cpp.

900 {
901 if (original_tp.trajectory_points.size() < 2)
902 {
903 RCLCPP_WARN(nh_->get_logger(), "Yield plugin received less than 2 points in update_traj_for_object, returning unchanged...");
904 return original_tp;
905 }
906
907 // Get earliest collision object
908 const auto earliest_collision_obj_pair = get_earliest_collision_object_and_time(original_tp, external_objects);
909
910 if (!earliest_collision_obj_pair)
911 {
912 RCLCPP_DEBUG(nh_->get_logger(),"No collision detected, so trajectory not modified.");
913 return original_tp;
914 }
915
916 carma_perception_msgs::msg::ExternalObject earliest_collision_obj = earliest_collision_obj_pair.value().first;
917 double earliest_collision_time_in_seconds = earliest_collision_obj_pair.value().second;
918
919 // 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,
920 // it is able to plan yielding much earlier and smoother using on_route_vehicle_collision_horizon_in_s.
921
922 const lanelet::BasicPoint2d vehicle_point(original_tp.trajectory_points[0].x,original_tp.trajectory_points[0].y);
923 const double vehicle_downtrack = wm_->routeTrackPos(vehicle_point).downtrack;
924
925 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"vehicle_downtrack: " << vehicle_downtrack);
926
927 RCLCPP_WARN_STREAM(nh_->get_logger(),"Collision Detected!");
928
929 const lanelet::BasicPoint2d object_point(earliest_collision_obj.pose.pose.position.x, earliest_collision_obj.pose.pose.position.y);
930 const double object_downtrack = wm_->routeTrackPos(object_point).downtrack;
931
932 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object_downtrack: " << object_downtrack);
933
934 const double object_downtrack_lead = std::max(0.0, object_downtrack - vehicle_downtrack);
935 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object_downtrack_lead: " << object_downtrack_lead);
936
937 // The vehicle's goal velocity of the yielding behavior is to match the velocity of the object along the trajectory.
938 double goal_velocity = get_predicted_velocity_at_time(earliest_collision_obj.velocity.twist, original_tp, earliest_collision_time_in_seconds);
939 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object's speed along trajectory at collision: " << goal_velocity);
940
941 // roadway object position
942 const double gap_time_until_min_gap_distance = std::max(0.0, object_downtrack_lead - config_.minimum_safety_gap_in_meters)/initial_velocity;
943
944 if (goal_velocity <= config_.obstacle_zero_speed_threshold_in_ms){
945 RCLCPP_WARN_STREAM(nh_->get_logger(),"The obstacle is not moving, goal velocity is set to 0 from: " << goal_velocity);
946 goal_velocity = 0.0;
947 }
948
949 // determine the safety inter-vehicle gap based on speed
950 double safety_gap = std::max(goal_velocity * gap_time_until_min_gap_distance, config_.minimum_safety_gap_in_meters);
951 if (!std::isnormal(safety_gap))
952 {
953 RCLCPP_WARN_STREAM(rclcpp::get_logger("yield_plugin"),"Detected non-normal (nan, inf, etc.) safety_gap."
954 "Making it desired safety gap configured at config_.minimum_safety_gap_in_meters: " << config_.minimum_safety_gap_in_meters);
956 }
958 {
959 // externally_commanded_safety_gap is desired distance gap commanded from external sources
960 // such as different plugin, map, or infrastructure depending on the use case
961 double externally_commanded_safety_gap = check_traj_for_digital_min_gap(original_tp);
962 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"externally_commanded_safety_gap: " << externally_commanded_safety_gap);
963 // if a digital gap is available, it is replaced as safety gap
964 safety_gap = std::max(safety_gap, externally_commanded_safety_gap);
965 }
966
967 const double goal_pos = std::max(0.0, object_downtrack_lead - safety_gap - config_.vehicle_length);
968 const double initial_pos = 0.0; //relative initial position (first trajectory point)
969 const double original_max_speed = max_trajectory_speed(original_tp.trajectory_points, earliest_collision_time_in_seconds);
970 const double delta_v_max = fabs(goal_velocity - original_max_speed);
971 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"delta_v_max: " << delta_v_max << ", safety_gap: " << safety_gap);
972
973 const double time_required_for_comfortable_decel_in_s = config_.acceleration_adjustment_factor * 2 * goal_pos / delta_v_max;
974 const double min_time_required_for_comfortable_decel_in_s = delta_v_max / config_.yield_max_deceleration_in_ms2;
975
976 // planning time for object avoidance
977 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});
978 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);
979
980 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Object avoidance planning time: " << planning_time_in_s);
981
982 return generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, planning_time_in_s, original_max_speed);
983 }
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 335 of file yield_plugin.hpp.

◆ ecef_traj_timestep_

double yield_plugin::YieldPlugin::ecef_traj_timestep_ = 0.1
private

Definition at line 332 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 339 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 337 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 334 of file yield_plugin.hpp.

◆ 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: