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.cpp
Go to the documentation of this file.
1/*
2 * Copyright (C) 2022 LEIDOS.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
5 * use this file except in compliance with the License. You may obtain a copy of
6 * the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
12 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
13 * License for the specific language governing permissions and limitations under
14 * the License.
15 */
16
17#include <rclcpp/rclcpp.hpp>
18#include <string>
19#include <algorithm>
20#include <memory>
21#include <limits>
22#include <boost/uuid/uuid_generators.hpp>
23#include <boost/uuid/uuid_io.hpp>
24#include <lanelet2_core/geometry/Point.h>
25#include <trajectory_utils/trajectory_utils.hpp>
26#include <trajectory_utils/conversions/conversions.hpp>
27#include <sstream>
28#include <carma_ros2_utils/carma_lifecycle_node.hpp>
29#include <Eigen/Core>
30#include <Eigen/Geometry>
31#include <Eigen/LU>
32#include <Eigen/SVD>
34#include <carma_v2x_msgs/msg/location_ecef.hpp>
35#include <carma_v2x_msgs/msg/trajectory.hpp>
36#include <carma_v2x_msgs/msg/plan_type.hpp>
38#include <future>
40
41using oss = std::ostringstream;
42constexpr auto EPSILON {0.01}; //small value to compare doubles
43
44namespace yield_plugin
45{
46 YieldPlugin::YieldPlugin(std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode> nh, carma_wm::WorldModelConstPtr wm, YieldPluginConfig config,
47 MobilityResponseCB mobility_response_publisher,
48 LaneChangeStatusCB lc_status_publisher)
49 : nh_(nh), wm_(wm), config_(config),mobility_response_publisher_(mobility_response_publisher), lc_status_publisher_(lc_status_publisher)
50 {
51
52 }
53
54 double get_trajectory_end_time(const carma_planning_msgs::msg::TrajectoryPlan& trajectory)
55 {
56 return rclcpp::Time(trajectory.trajectory_points.back().target_time).seconds();
57 }
58
59 double get_trajectory_start_time(const carma_planning_msgs::msg::TrajectoryPlan& trajectory)
60 {
61 return rclcpp::Time(trajectory.trajectory_points.front().target_time).seconds();
62 }
63
64 double get_trajectory_duration(const carma_planning_msgs::msg::TrajectoryPlan& trajectory)
65 {
66 return fabs(get_trajectory_end_time(trajectory) - get_trajectory_start_time(trajectory));
67 }
68
69 double get_trajectory_duration(const std::vector<carma_perception_msgs::msg::PredictedState>& trajectory)
70 {
71 return (rclcpp::Time(trajectory.back().header.stamp) - rclcpp::Time(trajectory.front().header.stamp)).seconds();
72 }
73
74 std::vector<std::pair<int, lanelet::BasicPoint2d>> YieldPlugin::detect_trajectories_intersection(std::vector<lanelet::BasicPoint2d> self_trajectory, std::vector<lanelet::BasicPoint2d> incoming_trajectory) const
75 {
76 std::vector<std::pair<int, lanelet::BasicPoint2d>> intersection_points;
77 boost::geometry::model::linestring<lanelet::BasicPoint2d> self_traj;
78 for (auto tpp:self_trajectory)
79 {
80 boost::geometry::append(self_traj, tpp);
81 }
82 // distance to consider trajectories colliding (chosen based on lane width and vehicle size)
83 for (size_t i=0; i<incoming_trajectory.size(); i++)
84 {
85 double res = boost::geometry::distance(incoming_trajectory.at(i), self_traj);
86
88 {
89 intersection_points.push_back(std::make_pair(i, incoming_trajectory.at(i)));
90 }
91 }
92 return intersection_points;
93 }
94
95 std::vector<lanelet::BasicPoint2d> YieldPlugin::convert_eceftrajectory_to_mappoints(const carma_v2x_msgs::msg::Trajectory& ecef_trajectory) const
96 {
97 carma_planning_msgs::msg::TrajectoryPlan trajectory_plan;
98 std::vector<lanelet::BasicPoint2d> map_points;
99
100 lanelet::BasicPoint2d first_point = ecef_to_map_point(ecef_trajectory.location);
101
102 map_points.push_back(first_point);
103 auto curr_point = ecef_trajectory.location;
104
105 for (size_t i = 0; i<ecef_trajectory.offsets.size(); i++)
106 {
107 lanelet::BasicPoint2d offset_point;
108 curr_point.ecef_x += ecef_trajectory.offsets.at(i).offset_x;
109 curr_point.ecef_y += ecef_trajectory.offsets.at(i).offset_y;
110 curr_point.ecef_z += ecef_trajectory.offsets.at(i).offset_z;
111
112 offset_point = ecef_to_map_point(curr_point);
113
114 map_points.push_back(offset_point);
115 }
116
117 return map_points;
118 }
119
120 lanelet::BasicPoint2d YieldPlugin::ecef_to_map_point(const carma_v2x_msgs::msg::LocationECEF& ecef_point) const
121 {
122
123 if (!map_projector_) {
124 throw std::invalid_argument("No map projector available for ecef conversion");
125 }
126
127 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);
128
129 return lanelet::traits::to2D(map_point);
130 }
131
132
133
134 carma_v2x_msgs::msg::MobilityResponse YieldPlugin::compose_mobility_response(const std::string& resp_recipient_id, const std::string& req_plan_id, bool response) const
135 {
136 carma_v2x_msgs::msg::MobilityResponse out_mobility_response;
137 out_mobility_response.m_header.sender_id = config_.vehicle_id;
138 out_mobility_response.m_header.recipient_id = resp_recipient_id;
139 out_mobility_response.m_header.sender_bsm_id = host_bsm_id_;
140 out_mobility_response.m_header.plan_id = req_plan_id;
141 out_mobility_response.m_header.timestamp = nh_->now().seconds()*1000;
142
143
145 {
146 out_mobility_response.is_accepted = true;
147 }
148 else out_mobility_response.is_accepted = false;
149
150 return out_mobility_response;
151 }
152
153
154 void YieldPlugin::mobilityrequest_cb(const carma_v2x_msgs::msg::MobilityRequest::UniquePtr msg)
155 {
156 carma_v2x_msgs::msg::MobilityRequest incoming_request = *msg;
157 carma_planning_msgs::msg::LaneChangeStatus lc_status_msg;
158 if (incoming_request.strategy == "carma/cooperative-lane-change")
159 {
160 if (!map_projector_) {
161 RCLCPP_ERROR(nh_->get_logger(),"Cannot process mobility request as map projection is not yet set!");
162 return;
163 }
164 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)
165 {
166 RCLCPP_DEBUG(nh_->get_logger(),"Cooperative Lane Change Request Received");
167 lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REQUEST_RECEIVED;
168 lc_status_msg.description = "Received lane merge request";
169
170 if (incoming_request.m_header.recipient_id == config_.vehicle_id)
171 {
172 RCLCPP_DEBUG(nh_->get_logger(),"CLC Request correctly received");
173 }
174
175 // extract mobility header
176 std::string req_sender_id = incoming_request.m_header.sender_id;
177 std::string req_plan_id = incoming_request.m_header.plan_id;
178 // extract mobility request
179 carma_v2x_msgs::msg::LocationECEF ecef_location = incoming_request.location;
180 carma_v2x_msgs::msg::Trajectory incoming_trajectory = incoming_request.trajectory;
181 std::string req_strategy_params = incoming_request.strategy_params;
182 clc_urgency_ = incoming_request.urgency;
183 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"received urgency: " << clc_urgency_);
184
185 // Parse strategy parameters
186 using boost::property_tree::ptree;
187 ptree pt;
188 std::istringstream strstream(req_strategy_params);
189 boost::property_tree::json_parser::read_json(strstream, pt);
190 int req_traj_speed_full = pt.get<int>("s");
191 int req_traj_fractional = pt.get<int>("f");
192 int start_lanelet_id = pt.get<int>("sl");
193 int end_lanelet_id = pt.get<int>("el");
194 double req_traj_speed = static_cast<double>(req_traj_speed_full) + static_cast<double>(req_traj_fractional)/10.0;
195 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"req_traj_speed" << req_traj_speed);
196 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"start_lanelet_id" << start_lanelet_id);
197 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"end_lanelet_id" << end_lanelet_id);
198
199 std::vector<lanelet::BasicPoint2d> req_traj_plan = {};
200
201 req_traj_plan = convert_eceftrajectory_to_mappoints(incoming_trajectory);
202
203 double req_expiration_sec = static_cast<double>(incoming_request.expiration);
204 double current_time_sec = nh_->now().seconds();
205
206 bool response_to_clc_req = false;
207 // ensure there is enough time for the yield
208 double req_plan_time = req_expiration_sec - current_time_sec;
209 double req_timestamp = static_cast<double>(incoming_request.m_header.timestamp) / 1000.0 - current_time_sec;
210 set_incoming_request_info(req_traj_plan, req_traj_speed, req_plan_time, req_timestamp);
211
212
213 if (req_expiration_sec - current_time_sec >= config_.min_obj_avoidance_plan_time_in_s && cooperative_request_acceptable_)
214 {
216 lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REQUEST_ACCEPTED;
217 lc_status_msg.description = "Accepted lane merge request";
218 response_to_clc_req = true;
219 RCLCPP_DEBUG(nh_->get_logger(),"CLC accepted");
220 }
221 else
222 {
223 lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REQUEST_REJECTED;
224 lc_status_msg.description = "Rejected lane merge request";
225 response_to_clc_req = false;
226 RCLCPP_DEBUG(nh_->get_logger(),"CLC rejected");
227 }
228 carma_v2x_msgs::msg::MobilityResponse outgoing_response = compose_mobility_response(req_sender_id, req_plan_id, response_to_clc_req);
229 mobility_response_publisher_(outgoing_response);
230 lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::RESPONSE_SENT;
231 RCLCPP_DEBUG(nh_->get_logger(),"response sent");
232 }
233 }
234 lc_status_publisher_(lc_status_msg);
235
236 }
237
238 void YieldPlugin::set_incoming_request_info(std::vector <lanelet::BasicPoint2d> req_trajectory, double req_speed, double req_planning_time, double req_timestamp)
239 {
240 req_trajectory_points_ = req_trajectory;
241 req_target_speed_ = req_speed;
242 req_target_plan_time_ = req_planning_time;
243 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"req_target_plan_time_" << req_target_plan_time_);
244 req_timestamp_ = req_timestamp;
245 }
246
247 void YieldPlugin::bsm_cb(const carma_v2x_msgs::msg::BSM::UniquePtr msg)
248 {
249 carma_v2x_msgs::msg::BSMCoreData bsm_core_ = msg->core_data;
250 host_bsm_id_ = bsmIDtoString(bsm_core_);
251 }
252
254 carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req,
255 carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp)
256{
257 RCLCPP_DEBUG(nh_->get_logger(),"Yield_plugin was called!");
258 if (req->initial_trajectory_plan.trajectory_points.size() < 2){
259 throw std::invalid_argument("Empty Trajectory received by Yield");
260 }
261 rclcpp::Clock system_clock(RCL_SYSTEM_TIME);
262 rclcpp::Time start_time = system_clock.now(); // Start timing the execution time for planning so it can be logged
263
264 carma_planning_msgs::msg::TrajectoryPlan original_trajectory = req->initial_trajectory_plan;
265 carma_planning_msgs::msg::TrajectoryPlan yield_trajectory;
266
267 // if ego is not stopped and we committed to stopping, use the last committed trajectory
268 if (req->vehicle_state.longitudinal_vel > EPSILON &&
270 {
271 RCLCPP_DEBUG(nh_->get_logger(), "Using last committed trajectory to stopping");
272 lanelet::BasicPoint2d veh_pos(req->vehicle_state.x_pos_global,
273 req->vehicle_state.y_pos_global);
274 auto updated_trajectory = last_traj_plan_committed_to_stopping_.value();
275
276 // Find closest point in last trajectory to current vehicle position
277 size_t idx_to_start_new_traj =
279 updated_trajectory.trajectory_points,
280 veh_pos);
281
282 // Update last trajectory to start from closest point (remove passed points)
283 if (!updated_trajectory.trajectory_points.empty()) {
284 updated_trajectory.trajectory_points =
285 std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint>
286 (updated_trajectory.trajectory_points.begin() + idx_to_start_new_traj,
287 updated_trajectory.trajectory_points.end());
288 }
289 last_traj_plan_committed_to_stopping_ = updated_trajectory;
290 resp->trajectory_plan = last_traj_plan_committed_to_stopping_.value();
291
292 rclcpp::Time end_time = system_clock.now(); // Planning complete
293
294 auto duration = end_time - start_time;
295 RCLCPP_DEBUG_STREAM(nh_->get_logger(),
296 "ExecutionTime: " << std::to_string(duration.seconds()));
297 return;
298 }
299
300 // Otherwise, we are planning a new trajectory by checking collision
301 try
302 {
303 // NOTE: Wrapping entire plan_trajectory logic with try catch because there is intermittent
304 // open issue of which cause is uncertain:
305 // https://github.com/usdot-fhwa-stol/carma-platform/issues/2501
306
307 double initial_velocity = req->vehicle_state.longitudinal_vel;
308 // If vehicle_state is stopped, non-zero velocity from the trajectory
309 // should be used. Otherwise, vehicle will not move.
310 if (initial_velocity < EPSILON)
311 {
312 initial_velocity = original_trajectory.initial_longitudinal_velocity;
313 // Record the time when vehicle was stopped first due to collision avoidance
316 {
318 RCLCPP_DEBUG(nh_->get_logger(), "First time stopped to prevent collision: %f",
320 }
321 }
322
323 // seperating cooperative yield with regular object detection for better performance.
325 {
326 RCLCPP_DEBUG(nh_->get_logger(),"Only consider high urgency clc");
328 {
329 RCLCPP_DEBUG(nh_->get_logger(),"Yield for CLC. We haven't received an updated negotiation this timestep");
330 yield_trajectory = update_traj_for_cooperative_behavior(original_trajectory, initial_velocity);
332 }
333 else
334 {
335 RCLCPP_DEBUG(nh_->get_logger(),"unreliable CLC communication, switching to object avoidance");
336 yield_trajectory = update_traj_for_object(original_trajectory, external_objects_, initial_velocity); // Compute the trajectory
337 }
338 }
339 else
340 {
341 RCLCPP_DEBUG(nh_->get_logger(),"Yield for object avoidance");
342 yield_trajectory = update_traj_for_object(original_trajectory, external_objects_, initial_velocity); // Compute the trajectory
343 }
344
345
346 // return original trajectory if no difference in trajectory points a.k.a no collision
347 if (fabs(get_trajectory_end_time(original_trajectory) - get_trajectory_end_time(yield_trajectory)) < EPSILON)
348 {
349
350 resp->trajectory_plan = original_trajectory;
351 // Reset the collision prevention variables
354
355 }
356 else
357 {
358 yield_trajectory.header.frame_id = "map";
359 yield_trajectory.header.stamp = nh_->now();
360 yield_trajectory.trajectory_id = original_trajectory.trajectory_id;
361 resp->trajectory_plan = yield_trajectory;
362 }
363 }
364 catch(const std::runtime_error& e) {
365 RCLCPP_WARN_STREAM(nh_->get_logger(), "Yield Plugin failed to plan trajectory due to known negative time issue: " << e.what());
366 RCLCPP_WARN_STREAM(nh_->get_logger(), "Returning the original trajectory, and retrying at the next call.");
367 resp->trajectory_plan = original_trajectory;
368 }
369
370 rclcpp::Time end_time = system_clock.now(); // Planning complete
371
372 auto duration = end_time - start_time;
373 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "ExecutionTime: " << std::to_string(duration.seconds()));
374 }
375
376 carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::update_traj_for_cooperative_behavior(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double current_speed)
377 {
378 carma_planning_msgs::msg::TrajectoryPlan cooperative_trajectory;
379
380 double initial_pos = 0;
381 double goal_pos;
382 double initial_velocity = current_speed;
383 double goal_velocity = req_target_speed_;
384 double planning_time = req_target_plan_time_;
385
386 std::vector<lanelet::BasicPoint2d> host_traj_points = {};
387 for (size_t i=0; i<original_tp.trajectory_points.size(); i++)
388 {
389 lanelet::BasicPoint2d traj_point;
390 traj_point.x() = original_tp.trajectory_points.at(i).x;
391 traj_point.y() = original_tp.trajectory_points.at(i).y;
392 host_traj_points.push_back(traj_point);
393 }
394
395 std::vector<std::pair<int, lanelet::BasicPoint2d>> intersection_points = detect_trajectories_intersection(host_traj_points, req_trajectory_points_);
396 if (!intersection_points.empty())
397 {
398 lanelet::BasicPoint2d intersection_point = intersection_points[0].second;
399 double dx = original_tp.trajectory_points[0].x - intersection_point.x();
400 double dy = original_tp.trajectory_points[0].y - intersection_point.y();
401 // check if a digital_gap is available
402 double digital_gap = check_traj_for_digital_min_gap(original_tp);
403 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"digital_gap: " << digital_gap);
404 goal_pos = sqrt(dx*dx + dy*dy) - std::max(config_.minimum_safety_gap_in_meters, digital_gap);
405 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Goal position (goal_pos): " << goal_pos);
406 double collision_time = req_timestamp_ + (intersection_points[0].first * ecef_traj_timestep_) - config_.safety_collision_time_gap_in_s;
407 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"req time stamp: " << req_timestamp_);
408 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Collision time: " << collision_time);
409 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"intersection num: " << intersection_points[0].first);
410 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Planning time: " << planning_time);
411 // calculate distance traveled from beginning of trajectory to collision point
412 double dx2 = intersection_point.x() - req_trajectory_points_[0].x();
413 double dy2 = intersection_point.y() - req_trajectory_points_[0].y();
414 // calculate incoming trajectory speed from time and distance between trajectory points
415 double incoming_trajectory_speed = sqrt(dx2*dx2 + dy2*dy2)/(intersection_points[0].first * ecef_traj_timestep_);
416 // calculate goal velocity from request trajectory
417 goal_velocity = std::min(goal_velocity, incoming_trajectory_speed);
418 double min_time = (initial_velocity - goal_velocity)/config_.yield_max_deceleration_in_ms2;
419
420 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"goal_velocity: " << goal_velocity);
421 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"incoming_trajectory_speed: " << incoming_trajectory_speed);
422
423 if (planning_time > min_time)
424 {
426 double original_max_speed = max_trajectory_speed(original_tp.trajectory_points, get_trajectory_end_time(original_tp));
427 cooperative_trajectory = generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, planning_time, original_max_speed);
428 }
429 else
430 {
432 RCLCPP_DEBUG(nh_->get_logger(),"The incoming requested trajectory is rejected, due to insufficient gap");
433 cooperative_trajectory = original_tp;
434 }
435
436 }
437 else
438 {
440 RCLCPP_DEBUG(nh_->get_logger(),"The incoming requested trajectory does not overlap with host vehicle's trajectory");
441 cooperative_trajectory = original_tp;
442 }
443
444 return cooperative_trajectory;
445 }
446
447 double get_smallest_time_step_of_traj(const carma_planning_msgs::msg::TrajectoryPlan& original_tp)
448 {
449 double smallest_time_step = std::numeric_limits<double>::infinity();
450 for (size_t i = 0; i < original_tp.trajectory_points.size() - 1; i ++)
451 {
452 smallest_time_step = std::min(smallest_time_step,
453 (rclcpp::Time(original_tp.trajectory_points.at(i + 1).target_time)
454 - rclcpp::Time(original_tp.trajectory_points.at(i).target_time)).seconds());
455 }
456 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),"smallest_time_step: " << smallest_time_step);
457
458 return smallest_time_step;
459 }
460
461 carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::generate_JMT_trajectory(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double initial_pos, double goal_pos,
462 double initial_velocity, double goal_velocity, double planning_time, double original_max_speed)
463 {
464 carma_planning_msgs::msg::TrajectoryPlan jmt_trajectory;
465 std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint> jmt_trajectory_points;
466 jmt_trajectory_points.push_back(original_tp.trajectory_points[0]);
467
468 std::vector<double> original_traj_relative_downtracks = get_relative_downtracks(original_tp);
469 std::vector<double> calculated_speeds = {};
470 std::vector<double> new_relative_downtracks = {};
471 new_relative_downtracks.push_back(0.0);
472 calculated_speeds.push_back(initial_velocity);
473 double new_traj_accumulated_downtrack = 0.0;
474 double original_traj_accumulated_downtrack = original_traj_relative_downtracks.at(1);
475
476 // Up until goal_pos (which also can be until end of the entire original trajectory), generate new speeds at
477 // or near original trajectory points by generating them at a fixed time interval using the JMT polynomial equation
478 const double initial_time = 0;
479 double initial_accel = 0;
481 {
482 initial_accel = (initial_velocity - last_speed_.value()) /
483 (nh_->now() - last_speed_time_.value()).seconds();
484
485 if (!std::isnormal(initial_accel))
486 {
487 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),"Detecting nan initial_accel set to 0");
488 initial_accel = 0.0;
489 }
490
491 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),"Detecting initial_accel: " << initial_accel
492 << ", initial_velocity:" << initial_velocity
493 << ", last_speed_: " << last_speed_.value()
494 << ", nh_->now(): " << nh_->now().seconds()
495 << ", last_speed_time_.get(): " << last_speed_time_.value().seconds());
496 }
497
498 const double goal_accel = 0;
499 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),"Following parameters used for JMT: "
500 "\ninitial_pos: " << initial_pos <<
501 "\ngoal_pos: " << goal_pos <<
502 "\ninitial_velocity: " << initial_velocity <<
503 "\ngoal_velocity: " << goal_velocity <<
504 "\ninitial_accel: " << initial_accel <<
505 "\ngoal_accel: " << goal_accel <<
506 "\nplanning_time: " << planning_time <<
507 "\noriginal_max_speed: " << original_max_speed);
508
509 // Get the polynomial solutions used to generate the trajectory
510 std::vector<double> polynomial_coefficients = quintic_coefficient_calculator::quintic_coefficient_calculator(initial_pos,
511 goal_pos,
512 initial_velocity,
513 goal_velocity,
514 initial_accel,
515 goal_accel,
516 initial_time,
517 planning_time);
518 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),"Used original_max_speed: " << original_max_speed);
519 for (size_t i = 0; i < polynomial_coefficients.size(); i++) {
520 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),"Coefficient " << i << ": " << polynomial_coefficients[i]);
521 }
522 const auto smallest_time_step = get_smallest_time_step_of_traj(original_tp);
523 int new_traj_idx = 1;
524 int original_traj_idx = 1;
525 while (new_traj_accumulated_downtrack < goal_pos - EPSILON && original_traj_idx < original_traj_relative_downtracks.size())
526 {
527 const double target_time = new_traj_idx * smallest_time_step;
528 const double downtrack_at_target_time = polynomial_calc(polynomial_coefficients, target_time);
529 double velocity_at_target_time = polynomial_calc_d(polynomial_coefficients, target_time);
530
531 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "Calculated speed velocity_at_target_time: " << velocity_at_target_time
532 << ", downtrack_at_target_time: "<< downtrack_at_target_time << ", target_time: " << target_time);
533
534 // if the speed becomes negative, the downtrack starts reversing to negative as well
535 // which will never reach the goal_pos, so break here.
536 if (velocity_at_target_time < 0.0)
537 {
538 break;
539 }
540
541 // Cannot have a negative speed or have a higher speed than that of the original trajectory
542 velocity_at_target_time = std::clamp(velocity_at_target_time, 0.0, original_max_speed);
543
544 // Pick the speed if it matches with the original downtracks
545 if (downtrack_at_target_time >= original_traj_accumulated_downtrack)
546 {
547 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "Picked calculated speed velocity_at_target_time: " << velocity_at_target_time
548 << ", downtrack_at_target_time: "<< downtrack_at_target_time << ", target_time: " << target_time);
549 // velocity_at_target_time doesn't exactly correspond to original_traj_accumulated_downtrack but does for new_traj_accumulated_downtrack.
550 // however, the logic is assuming they are close enough that the speed is usable
551 calculated_speeds.push_back(velocity_at_target_time);
552 original_traj_accumulated_downtrack += original_traj_relative_downtracks.at(original_traj_idx);
553 original_traj_idx ++;
554 }
555 new_traj_accumulated_downtrack = downtrack_at_target_time;
556 new_traj_idx++;
557
558 }
559
560 // if the loop above finished prematurely due to negative speed, fill with 0.0 speeds
561 // since the speed crossed 0.0 and algorithm indicates stopping
562 std::fill_n(std::back_inserter(calculated_speeds),
563 std::size(original_traj_relative_downtracks) - std::size(calculated_speeds),
564 0.0);
565
566 // Moving average filter to smoothen the speeds
567 std::vector<double> filtered_speeds = basic_autonomy::smoothing::moving_average_filter(calculated_speeds, config_.speed_moving_average_window_size);
568 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "filtered_speeds size: " << filtered_speeds.size());
569 for (const auto& speed : filtered_speeds)
570 {
571 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "filtered speed: " << speed);
572 }
573 // Replace the original trajectory's associated timestamps based on the newly calculated speeds
574 double prev_speed = filtered_speeds.at(0);
575 last_speed_ = prev_speed;
576 last_speed_time_ = nh_->now();
577 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()));
578
579 for(size_t i = 1; i < original_tp.trajectory_points.size(); i++)
580 {
581 carma_planning_msgs::msg::TrajectoryPlanPoint jmt_tpp = original_tp.trajectory_points.at(i);
582
583 // In case only subset of original trajectory needs modification,
584 // the rest of the points should keep the last speed to cruise
585 double current_speed = goal_velocity;
586
587 if (i < filtered_speeds.size())
588 {
589 current_speed = filtered_speeds.at(i);
590 }
591
592 //Force the speed to 0 if below configured value for more control over stopping behavior
593 if (current_speed < config_.max_stop_speed_in_ms)
594 {
595 current_speed = 0;
596 }
597
598 // Derived from constant accelaration kinematic equation: (vi + vf) / 2 * dt = d_dist
599 // This also handles a case correctly when current_speed is 0, but prev_speed is not 0 yet
600 const double dt = (2 * original_traj_relative_downtracks.at(i)) / (current_speed + prev_speed);
601 jmt_tpp.target_time = rclcpp::Time(jmt_trajectory_points.back().target_time) + rclcpp::Duration::from_nanoseconds(dt*1e9);
602
603 if (prev_speed < EPSILON) // Handle a special case if prev_speed (thus current_speed too) is 0
604 {
605 // NOTE: Assigning arbitrary 100 mins dt between points where normally dt is only 1 sec to model a stopping behavior.
606 // Another way to model it is to keep the trajectory point at a same location and increment time slightly. However,
607 // if the vehicle goes past the point, it may cruise toward undesirable location (for example into the intersection).
608 // Keeping the points help the controller steer the vehicle toward direction of travel even when stopping.
609 // Only downside is the trajectory plan is huge where only 15 sec is expected, but since this is stopping case, it shouldn't matter.
610 jmt_tpp.target_time = rclcpp::Time(jmt_trajectory_points.back().target_time) + rclcpp::Duration::from_nanoseconds(6000 * 1e9);
611 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "Zero speed = x: " << jmt_tpp.x << ", y:" << jmt_tpp.y
612 << ", t:" << std::to_string(rclcpp::Time(jmt_tpp.target_time).seconds())
613 << ", prev_speed: " << prev_speed << ", current_speed: " << current_speed);
614 }
615 else
616 {
617 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "Non-zero speed = x: " << jmt_tpp.x << ", y:" << jmt_tpp.y
618 << ", t:" << std::to_string(rclcpp::Time(jmt_tpp.target_time).seconds())
619 << ", prev_speed: " << prev_speed << ", current_speed: " << current_speed);
620 }
621
622 jmt_trajectory_points.push_back(jmt_tpp);
623 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());
624 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "insta_decel: " << insta_decel );
625 prev_speed = current_speed;
626 }
627
628 jmt_trajectory.header = original_tp.header;
629 jmt_trajectory.trajectory_id = original_tp.trajectory_id;
630 jmt_trajectory.trajectory_points = jmt_trajectory_points;
631 jmt_trajectory.initial_longitudinal_velocity = initial_velocity;
632 return jmt_trajectory;
633 }
634
635 std::optional<GetCollisionResult> YieldPlugin::get_collision(const carma_planning_msgs::msg::TrajectoryPlan& trajectory1,
636 const std::vector<carma_perception_msgs::msg::PredictedState>& trajectory2, double collision_radius, double trajectory1_max_speed)
637 {
638
639 // Iterate through each pair of consecutive points in the trajectories
640 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Starting a new collision detection, trajectory size: "
641 << trajectory1.trajectory_points.size() << ". prediction size: " << trajectory2.size());
642
643 // Iterate through the object to check if it's on the route
644 bool on_route = false;
645 int on_route_idx = 0;
646
647 // A flag to stop searching more than one lanelet if the object has no velocity
648 const auto traj2_speed{std::hypot(trajectory2.front().predicted_velocity.linear.x,
649 trajectory2.front().predicted_velocity.linear.y)};
650 bool traj2_has_zero_speed = traj2_speed < config_.obstacle_zero_speed_threshold_in_ms;
651
652 if (trajectory2.size() < 2)
653 {
654 throw std::invalid_argument("Object on ther road doesn't have enough predicted states! Please check motion_computation is correctly applying predicted states");
655 }
656 const double predict_step_duration = (rclcpp::Time(trajectory2.at(1).header.stamp) - rclcpp::Time(trajectory2.front().header.stamp)).seconds();
657 const double predict_total_duration = get_trajectory_duration(trajectory2);
658
659 if (predict_step_duration < 0.0)
660 {
661 throw std::invalid_argument("Predicted states of the object is malformed. Detected trajectory going backwards in time!");
662 }
663
664 // In order to optimize the for loops for comparing two trajectories, following logic skips every iteration_stride-th points of the traj2.
665 // Since skipping number of points from the traj2 may result in ignoring potential collisions, its value is dependent on two
666 // trajectories' speeds and intervehicle_collision_distance_in_m radius.
667 // Therefore, the derivation first calculates the max time, t, that both actors can move while still being in collision radius:
668 // sqrt( (v1 * t / 2)^2 + (v2 * t / 2)^2 ) = collision_radius. Here v1 and v2 are assumed to be perpendicular to each other and
669 // intersecting at t/2 to get max possible collision_radius. Solving for t gives following:
670 double iteration_stride_max_time_s = 2 * config_.intervehicle_collision_distance_in_m / sqrt(pow(traj2_speed, 2) + pow(trajectory1_max_speed, 2));
671 int iteration_stride = std::max(1, static_cast<int>(iteration_stride_max_time_s / predict_step_duration));
672
673 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Determined iteration_stride: " << iteration_stride
674 << ", with traj2_speed: " << traj2_speed
675 << ", with trajectory1_max_speed: " << trajectory1_max_speed
676 << ", with predict_step_duration: " << predict_step_duration
677 << ", iteration_stride_max_time_s: " << iteration_stride_max_time_s);
678
679 for (size_t j = 0; j < trajectory2.size(); j += iteration_stride) // Saving computation time aiming for 1.5 meter interval
680 {
681 lanelet::BasicPoint2d curr_point;
682 curr_point.x() = trajectory2.at(j).predicted_position.position.x;
683 curr_point.y() = trajectory2.at(j).predicted_position.position.y;
684
685 auto corresponding_lanelets = wm_->getLaneletsFromPoint(curr_point, 8); // some intersection can have 8 overlapping lanelets
686
687 for (const auto& llt: corresponding_lanelets)
688 {
689 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Checking llt: " << llt.id());
690
691 if (route_llt_ids_.find(llt.id()) != route_llt_ids_.end())
692 {
693 on_route = true;
694 on_route_idx = j;
695 break;
696 }
697 }
698 if (on_route || traj2_has_zero_speed)
699 break;
700 }
701
702 if (!on_route)
703 {
704 RCLCPP_DEBUG(nh_->get_logger(), "Predicted states are not on the route! ignoring");
705 return std::nullopt;
706 }
707
708 double smallest_dist = std::numeric_limits<double>::infinity();
709 for (size_t i = 0; i < trajectory1.trajectory_points.size() - 1; ++i)
710 {
711 auto p1a = trajectory1.trajectory_points.at(i);
712 auto p1b = trajectory1.trajectory_points.at(i + 1);
713 double previous_distance_between_predictions = std::numeric_limits<double>::infinity();
714 for (size_t j = on_route_idx; j < trajectory2.size() - 1; j += iteration_stride)
715 {
716 auto p2a = trajectory2.at(j);
717 auto p2b = trajectory2.at(j + 1);
718 double p1a_t = rclcpp::Time(p1a.target_time).seconds();
719 double p1b_t = rclcpp::Time(p1b.target_time).seconds();
720 double p2a_t = rclcpp::Time(p2a.header.stamp).seconds();
721 double p2b_t = rclcpp::Time(p2b.header.stamp).seconds();
722
723 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p1a.target_time: " << std::to_string(p1a_t) << ", p1b.target_time: " << std::to_string(p1b_t));
724 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p2a.target_time: " << std::to_string(p2a_t) << ", p2b.target_time: " << std::to_string(p2b_t));
725 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p1a.x: " << p1a.x << ", p1a.y: " << p1a.y);
726 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p1b.x: " << p1b.x << ", p1b.y: " << p1b.y);
727
728 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p2a.x: " << p2a.predicted_position.position.x << ", p2a.y: " << p2a.predicted_position.position.y);
729 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p2b.x: " << p2b.predicted_position.position.x << ", p2b.y: " << p2b.predicted_position.position.y);
730
731 // Linearly interpolate positions at a common timestamp for both trajectories
732 double dt = (p2a_t - p1a_t) / (p1b_t - p1a_t);
733 double x1 = p1a.x + dt * (p1b.x - p1a.x);
734 double y1 = p1a.y + dt * (p1b.y - p1a.y);
735 double x2 = p2a.predicted_position.position.x;
736 double y2 = p2a.predicted_position.position.y;
737
738 // Calculate the distance between the two interpolated points
739 const auto distance{std::hypot(x1 - x2, y1 - y2)};
740
741 smallest_dist = std::min(distance, smallest_dist);
742 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Smallest_dist: " << smallest_dist << ", distance: " << distance << ", dt: " << dt
743 << ", x1: " << x1 << ", y1: " << y1
744 << ", x2: " << x2 << ", y2: " << y2
745 << ", p2a_t:" << std::to_string(p2a_t));
746
747 // Following "if logic" assumes the traj2 is a simple cv model, aka, traj2 point is a straight line over time.
748 // And current traj1 point is fixed in this iteration.
749 // Then once the distance between the two start to increase over traj2 iteration,
750 // the distance will always increase and it's unnecessary to continue the logic to find the smallest_dist
751 if (previous_distance_between_predictions < distance)
752 {
753 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Stopping search here because the distance between predictions started to increase");
754 break;
755 }
756 previous_distance_between_predictions = distance;
757
758 if (i == 0 && j == 0 && distance > config_.collision_check_radius_in_m)
759 {
760 RCLCPP_DEBUG(nh_->get_logger(), "Too far away" );
761 return std::nullopt;
762 }
763
764 if (distance > collision_radius)
765 {
766 // continue searching for collision
767 continue;
768 }
769
770 GetCollisionResult collision_result;
771 collision_result.point1 = lanelet::BasicPoint2d(x1,y1);
772 collision_result.point2 = lanelet::BasicPoint2d(x2,y2);
773 collision_result.collision_time = rclcpp::Time(p2a.header.stamp);
774 return collision_result;
775 }
776 }
777
778 // No collision detected
779 return std::nullopt;
780 }
781
782 //TODO: Revisit this logic. Further investigation required for this logic since it currently seems like it will never return true
783 bool YieldPlugin::is_object_behind_vehicle(uint32_t object_id, const rclcpp::Time& collision_time, double vehicle_downtrack, double object_downtrack)
784 {
785 const auto previous_clearance_count = consecutive_clearance_count_for_obstacles_[object_id];
786 // if the object's location is half a length of the vehicle past its rear-axle, it is considered behind
787 // half a length of the vehicle to conservatively estimate the rear axle to rear bumper length
788 if (object_downtrack < vehicle_downtrack - config_.vehicle_length / 2)
789 {
791 RCLCPP_INFO_STREAM(nh_->get_logger(), "Detected an object nearby might be behind the vehicle at timestamp: " << std::to_string(collision_time.seconds()) <<
792 ", and consecutive_clearance_count_for obstacle: " << object_id << ", is: " << consecutive_clearance_count_for_obstacles_[object_id]);
793 }
794 // confirmed false positive for a collision
796 {
797 return true;
798 }
799 // if the clearance counter didn't increase by this point, true collision was detected
800 // therefore reset the consecutive clearance counter as it is no longer consecutive
801 if (consecutive_clearance_count_for_obstacles_[object_id] == previous_clearance_count)
802 {
804 }
805
806 return false;
807 }
808
809 std::optional<rclcpp::Time> YieldPlugin::get_collision_time(const carma_planning_msgs::msg::TrajectoryPlan& original_tp,
810 const carma_perception_msgs::msg::ExternalObject& curr_obstacle, double original_tp_max_speed)
811 {
812 auto plan_start_time = get_trajectory_start_time(original_tp);
813
814 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Object's back time: " << std::to_string(rclcpp::Time(curr_obstacle.predictions.back().header.stamp).seconds())
815 << ", plan_start_time: " << std::to_string(plan_start_time));
816
817 // do not process outdated objects
818 if (rclcpp::Time(curr_obstacle.predictions.back().header.stamp).seconds() <= plan_start_time)
819 {
820 return std::nullopt;
821 }
822
823 std::vector<carma_perception_msgs::msg::PredictedState> new_list;
824 carma_perception_msgs::msg::PredictedState curr_state;
825 // artificially include current position as one of the predicted states
826 curr_state.header.stamp = curr_obstacle.header.stamp;
827 curr_state.predicted_position.position.x = curr_obstacle.pose.pose.position.x;
828 curr_state.predicted_position.position.y = curr_obstacle.pose.pose.position.y;
829 // NOTE: predicted_velocity is not used for collision calculation, but timestamps
830 curr_state.predicted_velocity.linear.x = curr_obstacle.velocity.twist.linear.x;
831 curr_state.predicted_velocity.linear.y = curr_obstacle.velocity.twist.linear.y;
832 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Object: " << curr_obstacle.id <<", type: " << static_cast<int>(curr_obstacle.object_type)
833 << ", speed_x: " << curr_obstacle.velocity.twist.linear.x << ", speed_y: " << curr_obstacle.velocity.twist.linear.y);
834 new_list.push_back(curr_state);
835 new_list.insert(new_list.end(), curr_obstacle.predictions.cbegin(), curr_obstacle.predictions.cend());
836
837 const auto collision_result = get_collision(original_tp, new_list, config_.intervehicle_collision_distance_in_m, original_tp_max_speed);
838
839 if (!collision_result)
840 {
841 // reset the consecutive clearance counter because no collision was detected at this iteration
842 consecutive_clearance_count_for_obstacles_[curr_obstacle.id] = 0;
843 return std::nullopt;
844 }
845
846 // if within collision radius, it is not a collision if obstacle is behind the vehicle despite being in collision radius
847 const double vehicle_downtrack = wm_->routeTrackPos(collision_result.value().point1).downtrack;
848 const double object_downtrack = wm_->routeTrackPos(collision_result.value().point2).downtrack;
849
850 if (is_object_behind_vehicle(curr_obstacle.id, collision_result.value().collision_time, vehicle_downtrack, object_downtrack))
851 {
852 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()));
853 return std::nullopt;
854 }
855
856 const auto distance{std::hypot(
857 collision_result.value().point1.x() - collision_result.value().point2.x(),
858 collision_result.value().point1.y() - collision_result.value().point2.y()
859 )}; //for debug
860
861 RCLCPP_WARN_STREAM(nh_->get_logger(), "Collision detected for object: " << curr_obstacle.id << ", at timestamp " << std::to_string(collision_result.value().collision_time.seconds()) <<
862 ", x: " << collision_result.value().point1.x() << ", y: " << collision_result.value().point1.y() <<
863 ", within actual downtrack distance: " << object_downtrack - vehicle_downtrack <<
864 ", and collision distance: " << distance);
865
866 return collision_result.value().collision_time;
867 }
868
869 std::unordered_map<uint32_t, rclcpp::Time> YieldPlugin::get_collision_times_concurrently(const carma_planning_msgs::msg::TrajectoryPlan& original_tp,
870 const std::vector<carma_perception_msgs::msg::ExternalObject>& external_objects, double original_tp_max_speed)
871 {
872
873 std::unordered_map<uint32_t, std::future<std::optional<rclcpp::Time>>> futures;
874 std::unordered_map<uint32_t, rclcpp::Time> collision_times;
875
876 // Launch asynchronous tasks to check for collision times
877 for (const auto& object : external_objects) {
878 futures[object.id] = std::async(std::launch::async,[this, &original_tp, &object, &original_tp_max_speed]{
879 return get_collision_time(original_tp, object, original_tp_max_speed);
880 });
881 }
882
883 // Collect results from futures and update collision_times
884 for (const auto& object : external_objects) {
885 if (const auto collision_time{futures.at(object.id).get()}) {
886 collision_times[object.id] = collision_time.value();
887 }
888 }
889
890 return collision_times;
891 }
892
893 std::optional<std::pair<carma_perception_msgs::msg::ExternalObject, double>> YieldPlugin::get_earliest_collision_object_and_time(const carma_planning_msgs::msg::TrajectoryPlan& original_tp,
894 const std::vector<carma_perception_msgs::msg::ExternalObject>& external_objects)
895 {
896 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "ExternalObjects size: " << external_objects.size());
897
898 if (!wm_->getRoute())
899 {
900 RCLCPP_WARN(nh_->get_logger(), "Yield plugin was not able to analyze collision since route is not available! Please check if route is set");
901 return std::nullopt;
902 }
903
904 // save route Ids for faster access
905 for (const auto& llt: wm_->getRoute()->shortestPath())
906 {
907 // TODO: Enhancement https://github.com/usdot-fhwa-stol/carma-platform/issues/2316
908 route_llt_ids_.insert(llt.id());
909 }
910
911 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"External Object List (external_objects) size: " << external_objects.size());
912 const double original_max_speed = max_trajectory_speed(original_tp.trajectory_points, get_trajectory_end_time(original_tp));
913 std::unordered_map<uint32_t, rclcpp::Time> collision_times = get_collision_times_concurrently(original_tp,external_objects, original_max_speed);
914
915 if (collision_times.empty()) { return std::nullopt; }
916
917 const auto earliest_colliding_object_id{std::min_element(
918 std::cbegin(collision_times), std::cend(collision_times),
919 [](const auto & a, const auto & b){ return a.second < b.second; })->first};
920
921 const auto earliest_colliding_object{std::find_if(
922 std::cbegin(external_objects), std::cend(external_objects),
923 [&earliest_colliding_object_id](const auto & object) { return object.id == earliest_colliding_object_id; })};
924
925 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"earliest object x: " << earliest_colliding_object->velocity.twist.linear.x
926 << ", y: " << earliest_colliding_object->velocity.twist.linear.y);
927 return std::make_pair(*earliest_colliding_object, collision_times.at(earliest_colliding_object_id).seconds());
928
929 }
930
931 double YieldPlugin::get_predicted_velocity_at_time(const geometry_msgs::msg::Twist& object_velocity_in_map_frame,
932 const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double timestamp_in_sec_to_predict)
933 {
934 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "timestamp_in_sec_to_predict: " << std::to_string(timestamp_in_sec_to_predict) <<
935 ", trajectory_end_time: " << std::to_string(get_trajectory_end_time(original_tp)));
936
937 double point_b_time = 0.0;
938 carma_planning_msgs::msg::TrajectoryPlanPoint point_a;
939 carma_planning_msgs::msg::TrajectoryPlanPoint point_b;
940
941 // trajectory points' time is guaranteed to be increasing
942 // then find the corresponding point at timestamp_in_sec_to_predict
943 for (size_t i = 0; i < original_tp.trajectory_points.size() - 1; ++i)
944 {
945 point_a = original_tp.trajectory_points.at(i);
946 point_b = original_tp.trajectory_points.at(i + 1);
947 point_b_time = rclcpp::Time(point_b.target_time).seconds();
948 if (point_b_time >= timestamp_in_sec_to_predict)
949 {
950 break;
951 }
952 }
953
954 auto dx = point_b.x - point_a.x;
955 auto dy = point_b.y - point_a.y;
956 const tf2::Vector3 trajectory_direction(dx, dy, 0);
957
958 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "timestamp_in_sec_to_predict: " << std::to_string(timestamp_in_sec_to_predict)
959 << ", point_b_time: " << std::to_string(point_b_time)
960 << ", dx: " << dx << ", dy: " << dy << ", "
961 << ", object_velocity_in_map_frame.x: " << object_velocity_in_map_frame.linear.x
962 << ", object_velocity_in_map_frame.y: " << object_velocity_in_map_frame.linear.y);
963
964 if (trajectory_direction.length() < 0.001) //EPSILON
965 {
966 return 0.0;
967 }
968
969 const tf2::Vector3 object_direction(object_velocity_in_map_frame.linear.x, object_velocity_in_map_frame.linear.y, 0);
970
971 return tf2::tf2Dot(object_direction, trajectory_direction) / trajectory_direction.length();
972 }
973
974 carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::update_traj_for_object(const carma_planning_msgs::msg::TrajectoryPlan& original_tp,
975 const std::vector<carma_perception_msgs::msg::ExternalObject>& external_objects, double initial_velocity)
976 {
977 if (original_tp.trajectory_points.size() < 2)
978 {
979 RCLCPP_WARN(nh_->get_logger(), "Yield plugin received less than 2 points in update_traj_for_object, returning unchanged...");
980 return original_tp;
981 }
982
983 // Get earliest collision object
984 const auto earliest_collision_obj_pair = get_earliest_collision_object_and_time(original_tp, external_objects);
985
986 if (!earliest_collision_obj_pair)
987 {
988 RCLCPP_DEBUG(nh_->get_logger(),"No collision detected, so trajectory not modified.");
989 return original_tp;
990 }
991
992 carma_perception_msgs::msg::ExternalObject earliest_collision_obj = earliest_collision_obj_pair.value().first;
993 double earliest_collision_time_in_seconds = earliest_collision_obj_pair.value().second;
994
995 // 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,
996 // it is able to plan yielding much earlier and smoother using on_route_vehicle_collision_horizon_in_s.
997
998 const lanelet::BasicPoint2d vehicle_point(original_tp.trajectory_points[0].x,original_tp.trajectory_points[0].y);
999 const double vehicle_downtrack = wm_->routeTrackPos(vehicle_point).downtrack;
1000
1001 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"vehicle_downtrack: " << vehicle_downtrack);
1002
1003 RCLCPP_WARN_STREAM(nh_->get_logger(),"Collision Detected!");
1004
1005 const lanelet::BasicPoint2d object_point(earliest_collision_obj.pose.pose.position.x, earliest_collision_obj.pose.pose.position.y);
1006 const double object_downtrack = wm_->routeTrackPos(object_point).downtrack;
1007
1008 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object_downtrack: " << object_downtrack);
1009
1010 const double object_downtrack_lead = std::max(0.0, object_downtrack - vehicle_downtrack);
1011 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object_downtrack_lead: " << object_downtrack_lead);
1012
1013 // The vehicle's goal velocity of the yielding behavior is to match the velocity of the object along the trajectory.
1014 double goal_velocity = get_predicted_velocity_at_time(earliest_collision_obj.velocity.twist, original_tp, earliest_collision_time_in_seconds);
1015 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object's speed along trajectory at collision: " << goal_velocity);
1016
1017 // roadway object position
1018 const double gap_time_until_min_gap_distance = std::max(0.0, object_downtrack_lead - config_.minimum_safety_gap_in_meters)/initial_velocity;
1019
1020 if (goal_velocity <= config_.obstacle_zero_speed_threshold_in_ms){
1021 RCLCPP_WARN_STREAM(nh_->get_logger(),"The obstacle is not moving, goal velocity is set to 0 from: " << goal_velocity);
1022 goal_velocity = 0.0;
1023 }
1024
1025 // determine the safety inter-vehicle gap based on speed
1026 double safety_gap = std::max(goal_velocity * gap_time_until_min_gap_distance, config_.minimum_safety_gap_in_meters);
1027 if (!std::isnormal(safety_gap))
1028 {
1029 RCLCPP_WARN_STREAM(rclcpp::get_logger("yield_plugin"),"Detected non-normal (nan, inf, etc.) safety_gap."
1030 "Making it desired safety gap configured at config_.minimum_safety_gap_in_meters: " << config_.minimum_safety_gap_in_meters);
1032 }
1034 {
1035 // externally_commanded_safety_gap is desired distance gap commanded from external sources
1036 // such as different plugin, map, or infrastructure depending on the use case
1037 double externally_commanded_safety_gap = check_traj_for_digital_min_gap(original_tp);
1038 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"externally_commanded_safety_gap: " << externally_commanded_safety_gap);
1039 // if a digital gap is available, it is replaced as safety gap
1040 safety_gap = std::max(safety_gap, externally_commanded_safety_gap);
1041 }
1042
1043 const double goal_pos = std::max(0.0, object_downtrack_lead - safety_gap - config_.vehicle_length);
1044 const double initial_pos = 0.0; //relative initial position (first trajectory point)
1045 const double original_max_speed = max_trajectory_speed(original_tp.trajectory_points, earliest_collision_time_in_seconds);
1046 const double delta_v_max = fabs(goal_velocity - original_max_speed);
1047 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"delta_v_max: " << delta_v_max << ", safety_gap: " << safety_gap);
1048
1049 const double time_required_for_comfortable_decel_in_s = config_.acceleration_adjustment_factor * 2 * goal_pos / delta_v_max;
1050 const double min_time_required_for_comfortable_decel_in_s = delta_v_max / config_.yield_max_deceleration_in_ms2;
1051
1052 // planning time for object avoidance
1053 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});
1054 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);
1055
1056 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Object avoidance planning time: " << planning_time_in_s);
1057
1058 auto jmt_trajectory = generate_JMT_trajectory(original_tp,
1059 initial_pos, goal_pos, initial_velocity, goal_velocity,
1060 planning_time_in_s, original_max_speed);
1061
1062 // If expected to stop to prevent collision, we should save this trajectory to commit to it
1063 if (!last_traj_plan_committed_to_stopping_.has_value() &&
1064 goal_velocity < EPSILON &&
1065 earliest_collision_time_in_seconds - nh_->now().seconds()
1067 {
1069 }
1070 return jmt_trajectory;
1071 }
1072
1073
1074 std::vector<double> YieldPlugin::get_relative_downtracks(const carma_planning_msgs::msg::TrajectoryPlan& trajectory_plan) const
1075 {
1076 std::vector<double> downtracks;
1077 downtracks.reserve(trajectory_plan.trajectory_points.size());
1078 // relative downtrack distance of the fist Point is 0.0
1079 downtracks.push_back(0.0);
1080 for (size_t i=1; i < trajectory_plan.trajectory_points.size(); i++){
1081
1082 double dx = trajectory_plan.trajectory_points.at(i).x - trajectory_plan.trajectory_points.at(i-1).x;
1083 double dy = trajectory_plan.trajectory_points.at(i).y - trajectory_plan.trajectory_points.at(i-1).y;
1084 downtracks.push_back(sqrt(dx*dx + dy*dy));
1085 }
1086 return downtracks;
1087 }
1088
1089 double YieldPlugin::polynomial_calc(std::vector<double> coeff, double x) const
1090 {
1091 double result = 0;
1092 for (size_t i = 0; i < coeff.size(); i++)
1093 {
1094 double value = coeff.at(i) * pow(x, static_cast<int>(coeff.size() - 1 - i));
1095 result = result + value;
1096 }
1097 return result;
1098 }
1099
1100 double YieldPlugin::polynomial_calc_d(std::vector<double> coeff, double x) const
1101 {
1102 double result = 0;
1103 for (size_t i = 0; i < coeff.size()-1; i++)
1104 {
1105 double value = static_cast<int>(coeff.size() - 1 - i) * coeff.at(i) * pow(x, static_cast<int>(coeff.size() - 2 - i));
1106 result = result + value;
1107 }
1108 return result;
1109 }
1110
1111 double YieldPlugin::max_trajectory_speed(const std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint>& trajectory_points, double timestamp_in_sec_to_search_until) const
1112 {
1113 double max_speed = 0;
1114 for(size_t i = 0; i < trajectory_points.size() - 2; i++ )
1115 {
1116 double dx = trajectory_points.at(i + 1).x - trajectory_points.at(i).x;
1117 double dy = trajectory_points.at(i + 1).y - trajectory_points.at(i).y;
1118 double d = sqrt(dx*dx + dy*dy);
1119 double t = (rclcpp::Time(trajectory_points.at(i + 1).target_time).seconds() - rclcpp::Time(trajectory_points.at(i).target_time).seconds());
1120 double v = d/t;
1121 if(v > max_speed)
1122 {
1123 max_speed = v;
1124 }
1125 if (rclcpp::Time(trajectory_points.at(i + 1).target_time).seconds() >= timestamp_in_sec_to_search_until)
1126 {
1127 break;
1128 }
1129
1130 }
1131 return max_speed;
1132 }
1133
1134 double YieldPlugin::check_traj_for_digital_min_gap(const carma_planning_msgs::msg::TrajectoryPlan& original_tp) const
1135 {
1136 double desired_gap = 0;
1137
1138 for (size_t i = 0; i < original_tp.trajectory_points.size(); i++)
1139 {
1140 lanelet::BasicPoint2d veh_pos(original_tp.trajectory_points.at(i).x, original_tp.trajectory_points.at(i).y);
1141 auto llts = wm_->getLaneletsFromPoint(veh_pos, 1);
1142 if (llts.empty())
1143 {
1144 // This should technically never happen
1145 // However, trajectory generation currently may fail due to osm map issue https://github.com/usdot-fhwa-stol/carma-platform/issues/2503
1146 RCLCPP_WARN_STREAM(nh_->get_logger(),"Trajectory point: x= " << original_tp.trajectory_points.at(i).x << "y="<< original_tp.trajectory_points.at(i).y);
1147 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);
1148 return desired_gap;
1149 }
1150 auto digital_min_gap = llts[0].regulatoryElementsAs<lanelet::DigitalMinimumGap>(); //Returns a list of these elements)
1151 if (!digital_min_gap.empty())
1152 {
1153 double digital_gap = digital_min_gap[0]->getMinimumGap(); // Provided gap is in meters
1154 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Digital Gap found with value: " << digital_gap);
1155 desired_gap = std::max(desired_gap, digital_gap);
1156 }
1157 }
1158 return desired_gap;
1159 }
1160
1161 void YieldPlugin::set_georeference_string(const std::string& georeference)
1162 {
1163 if (georeference_ != georeference)
1164 {
1165 georeference_ = georeference;
1166 map_projector_ = std::make_shared<lanelet::projection::LocalFrameProjector>(georeference.c_str()); // Build projector from proj string
1167 }
1168 }
1169
1170 void YieldPlugin::set_external_objects(const std::vector<carma_perception_msgs::msg::ExternalObject>& object_list)
1171 {
1172 external_objects_ = object_list;
1173 }
1174
1175} // namespace yield_plugin
std::set< lanelet::Id > route_llt_ids_
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.
std::vector< double > get_relative_downtracks(const carma_planning_msgs::msg::TrajectoryPlan &trajectory_plan) const
calculates distance between trajectory points in a plan
LaneChangeStatusCB lc_status_publisher_
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 > req_trajectory_points_
void mobilityrequest_cb(const carma_v2x_msgs::msg::MobilityRequest::UniquePtr msg)
callback for mobility request
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
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::shared_ptr< lanelet::projection::LocalFrameProjector > map_projector_
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...
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...
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.
std::optional< rclcpp::Time > last_speed_time_
void bsm_cb(const carma_v2x_msgs::msg::BSM::UniquePtr msg)
callback for bsm message
MobilityResponseCB mobility_response_publisher_
std::string bsmIDtoString(carma_v2x_msgs::msg::BSMCoreData bsm_core)
void set_external_objects(const std::vector< carma_perception_msgs::msg::ExternalObject > &object_list)
Setter for external objects with predictions in the environment.
YieldPlugin(std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode > nh, carma_wm::WorldModelConstPtr wm, YieldPluginConfig config, MobilityResponseCB mobility_response_publisher, LaneChangeStatusCB lc_status_publisher)
Constructor.
std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode > nh_
void set_georeference_string(const std::string &georeference)
Setter for map projection string to define lat/lon -> map conversion.
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...
YieldPluginConfig config_
std::optional< rclcpp::Time > first_time_stopped_to_prevent_collision_
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::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
std::optional< carma_planning_msgs::msg::TrajectoryPlan > last_traj_plan_committed_to_stopping_
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)
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 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
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
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::unordered_map< uint32_t, int > consecutive_clearance_count_for_obstacles_
std::vector< carma_perception_msgs::msg::ExternalObject > external_objects_
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 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 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
carma_wm::WorldModelConstPtr wm_
std::ostringstream oss
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
int get_nearest_point_index(const std::vector< lanelet::BasicPoint2d > &points, const carma_planning_msgs::msg::VehicleState &state)
Returns the nearest point (in terms of cartesian 2d distance) to the provided vehicle pose in the pro...
auto to_string(const UtmZone &zone) -> std::string
Definition: utm_zone.cpp:21
std::shared_ptr< const WorldModel > WorldModelConstPtr
Definition: WorldModel.hpp:454
list first_point
Definition: process_bag.py:52
std::function< void(const carma_planning_msgs::msg::LaneChangeStatus &)> LaneChangeStatusCB
double get_smallest_time_step_of_traj(const carma_planning_msgs::msg::TrajectoryPlan &original_tp)
double get_trajectory_start_time(const carma_planning_msgs::msg::TrajectoryPlan &trajectory)
double get_trajectory_end_time(const carma_planning_msgs::msg::TrajectoryPlan &trajectory)
std::function< void(const carma_v2x_msgs::msg::MobilityResponse &)> MobilityResponseCB
double get_trajectory_duration(const carma_planning_msgs::msg::TrajectoryPlan &trajectory)
Stuct containing the algorithm configuration values for the YieldPluginConfig.
bool always_accept_mobility_request
double max_stop_speed_in_ms
double collision_check_radius_in_m
std::string vehicle_id
double speed_moving_average_window_size
int consecutive_clearance_count_for_passed_obstacles_threshold
double acceleration_adjustment_factor
double yield_max_deceleration_in_ms2
double time_horizon_until_collision_to_commit_to_stop_in_s
double intervehicle_collision_distance_in_m
double obstacle_zero_speed_threshold_in_ms
bool enable_cooperative_behavior
double min_obj_avoidance_plan_time_in_s
double minimum_safety_gap_in_meters
double safety_collision_time_gap_in_s
Convenience class for saving collision results.
lanelet::BasicPoint2d point1
lanelet::BasicPoint2d point2
constexpr auto EPSILON