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>
39
40using oss = std::ostringstream;
41constexpr auto EPSILON {0.01}; //small value to compare doubles
42
43namespace yield_plugin
44{
45 YieldPlugin::YieldPlugin(std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode> nh, carma_wm::WorldModelConstPtr wm, YieldPluginConfig config,
46 MobilityResponseCB mobility_response_publisher,
47 LaneChangeStatusCB lc_status_publisher)
48 : nh_(nh), wm_(wm), config_(config),mobility_response_publisher_(mobility_response_publisher), lc_status_publisher_(lc_status_publisher)
49 {
50
51 }
52
53 double get_trajectory_end_time(const carma_planning_msgs::msg::TrajectoryPlan& trajectory)
54 {
55 return rclcpp::Time(trajectory.trajectory_points.back().target_time).seconds();
56 }
57
58 double get_trajectory_start_time(const carma_planning_msgs::msg::TrajectoryPlan& trajectory)
59 {
60 return rclcpp::Time(trajectory.trajectory_points.front().target_time).seconds();
61 }
62
63 double get_trajectory_duration(const carma_planning_msgs::msg::TrajectoryPlan& trajectory)
64 {
65 return fabs(get_trajectory_end_time(trajectory) - get_trajectory_start_time(trajectory));
66 }
67
68 double get_trajectory_duration(const std::vector<carma_perception_msgs::msg::PredictedState>& trajectory)
69 {
70 return (rclcpp::Time(trajectory.back().header.stamp) - rclcpp::Time(trajectory.front().header.stamp)).seconds();
71 }
72
73 std::vector<std::pair<int, lanelet::BasicPoint2d>> YieldPlugin::detect_trajectories_intersection(std::vector<lanelet::BasicPoint2d> self_trajectory, std::vector<lanelet::BasicPoint2d> incoming_trajectory) const
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 }
93
94 std::vector<lanelet::BasicPoint2d> YieldPlugin::convert_eceftrajectory_to_mappoints(const carma_v2x_msgs::msg::Trajectory& ecef_trajectory) const
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 }
118
119 lanelet::BasicPoint2d YieldPlugin::ecef_to_map_point(const carma_v2x_msgs::msg::LocationECEF& ecef_point) const
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 }
130
131
132
133 carma_v2x_msgs::msg::MobilityResponse YieldPlugin::compose_mobility_response(const std::string& resp_recipient_id, const std::string& req_plan_id, bool response) const
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 }
151
152
153 void YieldPlugin::mobilityrequest_cb(const carma_v2x_msgs::msg::MobilityRequest::UniquePtr msg)
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 }
236
237 void YieldPlugin::set_incoming_request_info(std::vector <lanelet::BasicPoint2d> req_trajectory, double req_speed, double req_planning_time, double req_timestamp)
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 }
245
246 void YieldPlugin::bsm_cb(const carma_v2x_msgs::msg::BSM::UniquePtr msg)
247 {
248 carma_v2x_msgs::msg::BSMCoreData bsm_core_ = msg->core_data;
249 host_bsm_id_ = bsmIDtoString(bsm_core_);
250 }
251
253 carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req,
254 carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp)
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 }
326
327 carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::update_traj_for_cooperative_behavior(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double current_speed)
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 }
397
398 double get_smallest_time_step_of_traj(const carma_planning_msgs::msg::TrajectoryPlan& original_tp)
399 {
400 double smallest_time_step = std::numeric_limits<double>::infinity();
401 for (size_t i = 0; i < original_tp.trajectory_points.size() - 1; i ++)
402 {
403 smallest_time_step = std::min(smallest_time_step,
404 (rclcpp::Time(original_tp.trajectory_points.at(i + 1).target_time)
405 - rclcpp::Time(original_tp.trajectory_points.at(i).target_time)).seconds());
406 }
407 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),"smallest_time_step: " << smallest_time_step);
408
409 return smallest_time_step;
410 }
411
412 carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::generate_JMT_trajectory(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double initial_pos, double goal_pos,
413 double initial_velocity, double goal_velocity, double planning_time, double original_max_speed)
414 {
415 carma_planning_msgs::msg::TrajectoryPlan jmt_trajectory;
416 std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint> jmt_trajectory_points;
417 jmt_trajectory_points.push_back(original_tp.trajectory_points[0]);
418
419 std::vector<double> original_traj_relative_downtracks = get_relative_downtracks(original_tp);
420 std::vector<double> calculated_speeds = {};
421 std::vector<double> new_relative_downtracks = {};
422 new_relative_downtracks.push_back(0.0);
423 calculated_speeds.push_back(initial_velocity);
424 double new_traj_accumulated_downtrack = 0.0;
425 double original_traj_accumulated_downtrack = original_traj_relative_downtracks.at(1);
426
427 // Up until goal_pos (which also can be until end of the entire original trajectory), generate new speeds at
428 // or near original trajectory points by generating them at a fixed time interval using the JMT polynomial equation
429 const double initial_time = 0;
430 double initial_accel = 0;
432 {
433 initial_accel = (initial_velocity - last_speed_.value()) /
434 (nh_->now() - last_speed_time_.value()).seconds();
435
436 if (!std::isnormal(initial_accel))
437 {
438 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),"Detecting nan initial_accel set to 0");
439 initial_accel = 0.0;
440 }
441
442 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),"Detecting initial_accel: " << initial_accel
443 << ", initial_velocity:" << initial_velocity
444 << ", last_speed_: " << last_speed_.value()
445 << ", nh_->now(): " << nh_->now().seconds()
446 << ", last_speed_time_.get(): " << last_speed_time_.value().seconds());
447 }
448
449 const double goal_accel = 0;
450 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),"Following parameters used for JMT: "
451 "\ninitial_pos: " << initial_pos <<
452 "\ngoal_pos: " << goal_pos <<
453 "\ninitial_velocity: " << initial_velocity <<
454 "\ngoal_velocity: " << goal_velocity <<
455 "\ninitial_accel: " << initial_accel <<
456 "\ngoal_accel: " << goal_accel <<
457 "\nplanning_time: " << planning_time <<
458 "\noriginal_max_speed: " << original_max_speed);
459
460 // Get the polynomial solutions used to generate the trajectory
461 std::vector<double> polynomial_coefficients = quintic_coefficient_calculator::quintic_coefficient_calculator(initial_pos,
462 goal_pos,
463 initial_velocity,
464 goal_velocity,
465 initial_accel,
466 goal_accel,
467 initial_time,
468 planning_time);
469 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),"Used original_max_speed: " << original_max_speed);
470 for (size_t i = 0; i < polynomial_coefficients.size(); i++) {
471 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),"Coefficient " << i << ": " << polynomial_coefficients[i]);
472 }
473 const auto smallest_time_step = get_smallest_time_step_of_traj(original_tp);
474 int new_traj_idx = 1;
475 int original_traj_idx = 1;
476 while (new_traj_accumulated_downtrack < goal_pos - EPSILON && original_traj_idx < original_traj_relative_downtracks.size())
477 {
478 const double target_time = new_traj_idx * smallest_time_step;
479 const double downtrack_at_target_time = polynomial_calc(polynomial_coefficients, target_time);
480 double velocity_at_target_time = polynomial_calc_d(polynomial_coefficients, target_time);
481
482 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "Calculated speed velocity_at_target_time: " << velocity_at_target_time
483 << ", downtrack_at_target_time: "<< downtrack_at_target_time << ", target_time: " << target_time);
484
485 // if the speed becomes negative, the downtrack starts reversing to negative as well
486 // which will never reach the goal_pos, so break here.
487 if (velocity_at_target_time < 0.0)
488 {
489 break;
490 }
491
492 // Cannot have a negative speed or have a higher speed than that of the original trajectory
493 velocity_at_target_time = std::clamp(velocity_at_target_time, 0.0, original_max_speed);
494
495 // Pick the speed if it matches with the original downtracks
496 if (downtrack_at_target_time >= original_traj_accumulated_downtrack)
497 {
498 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "Picked calculated speed velocity_at_target_time: " << velocity_at_target_time
499 << ", downtrack_at_target_time: "<< downtrack_at_target_time << ", target_time: " << target_time);
500 // velocity_at_target_time doesn't exactly correspond to original_traj_accumulated_downtrack but does for new_traj_accumulated_downtrack.
501 // however, the logic is assuming they are close enough that the speed is usable
502 calculated_speeds.push_back(velocity_at_target_time);
503 original_traj_accumulated_downtrack += original_traj_relative_downtracks.at(original_traj_idx);
504 original_traj_idx ++;
505 }
506 new_traj_accumulated_downtrack = downtrack_at_target_time;
507 new_traj_idx++;
508
509 }
510
511 // if the loop above finished prematurely due to negative speed, fill with 0.0 speeds
512 // since the speed crossed 0.0 and algorithm indicates stopping
513 std::fill_n(std::back_inserter(calculated_speeds),
514 std::size(original_traj_relative_downtracks) - std::size(calculated_speeds),
515 0.0);
516
517 // Moving average filter to smoothen the speeds
518 std::vector<double> filtered_speeds = basic_autonomy::smoothing::moving_average_filter(calculated_speeds, config_.speed_moving_average_window_size);
519 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "filtered_speeds size: " << filtered_speeds.size());
520 for (const auto& speed : filtered_speeds)
521 {
522 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "filtered speed: " << speed);
523 }
524 // Replace the original trajectory's associated timestamps based on the newly calculated speeds
525 double prev_speed = filtered_speeds.at(0);
526 last_speed_ = prev_speed;
527 last_speed_time_ = nh_->now();
528 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "start speed: " << prev_speed << ", target_time: " << std::to_string(rclcpp::Time(original_tp.trajectory_points[0].target_time).seconds()));
529
530 for(size_t i = 1; i < original_tp.trajectory_points.size(); i++)
531 {
532 carma_planning_msgs::msg::TrajectoryPlanPoint jmt_tpp = original_tp.trajectory_points.at(i);
533
534 // In case only subset of original trajectory needs modification,
535 // the rest of the points should keep the last speed to cruise
536 double current_speed = goal_velocity;
537
538 if (i < filtered_speeds.size())
539 {
540 current_speed = filtered_speeds.at(i);
541 }
542
543 //Force the speed to 0 if below configured value for more control over stopping behavior
544 if (current_speed < config_.max_stop_speed_in_ms)
545 {
546 current_speed = 0;
547 }
548
549 // Derived from constant accelaration kinematic equation: (vi + vf) / 2 * dt = d_dist
550 // This also handles a case correctly when current_speed is 0, but prev_speed is not 0 yet
551 const double dt = (2 * original_traj_relative_downtracks.at(i)) / (current_speed + prev_speed);
552 jmt_tpp.target_time = rclcpp::Time(jmt_trajectory_points.back().target_time) + rclcpp::Duration::from_nanoseconds(dt*1e9);
553
554 if (prev_speed < EPSILON) // Handle a special case if prev_speed (thus current_speed too) is 0
555 {
556 // NOTE: Assigning arbitrary 100 mins dt between points where normally dt is only 1 sec to model a stopping behavior.
557 // Another way to model it is to keep the trajectory point at a same location and increment time slightly. However,
558 // if the vehicle goes past the point, it may cruise toward undesirable location (for example into the intersection).
559 // Keeping the points help the controller steer the vehicle toward direction of travel even when stopping.
560 // Only downside is the trajectory plan is huge where only 15 sec is expected, but since this is stopping case, it shouldn't matter.
561 jmt_tpp.target_time = rclcpp::Time(jmt_trajectory_points.back().target_time) + rclcpp::Duration::from_nanoseconds(6000 * 1e9);
562 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "Zero speed = x: " << jmt_tpp.x << ", y:" << jmt_tpp.y
563 << ", t:" << std::to_string(rclcpp::Time(jmt_tpp.target_time).seconds())
564 << ", prev_speed: " << prev_speed << ", current_speed: " << current_speed);
565 }
566 else
567 {
568 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "Non-zero speed = x: " << jmt_tpp.x << ", y:" << jmt_tpp.y
569 << ", t:" << std::to_string(rclcpp::Time(jmt_tpp.target_time).seconds())
570 << ", prev_speed: " << prev_speed << ", current_speed: " << current_speed);
571 }
572
573 jmt_trajectory_points.push_back(jmt_tpp);
574 double insta_decel = (current_speed - prev_speed) / (rclcpp::Time(jmt_trajectory_points.at(i).target_time).seconds() - rclcpp::Time(jmt_trajectory_points.at(i - 1).target_time).seconds());
575 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "insta_decel: " << insta_decel );
576 prev_speed = current_speed;
577 }
578
579 jmt_trajectory.header = original_tp.header;
580 jmt_trajectory.trajectory_id = original_tp.trajectory_id;
581 jmt_trajectory.trajectory_points = jmt_trajectory_points;
582 jmt_trajectory.initial_longitudinal_velocity = initial_velocity;
583 return jmt_trajectory;
584 }
585
586 std::optional<GetCollisionResult> YieldPlugin::get_collision(const carma_planning_msgs::msg::TrajectoryPlan& trajectory1,
587 const std::vector<carma_perception_msgs::msg::PredictedState>& trajectory2, double collision_radius, double trajectory1_max_speed)
588 {
589
590 // Iterate through each pair of consecutive points in the trajectories
591 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Starting a new collision detection, trajectory size: "
592 << trajectory1.trajectory_points.size() << ". prediction size: " << trajectory2.size());
593
594 // Iterate through the object to check if it's on the route
595 bool on_route = false;
596 int on_route_idx = 0;
597
598 // A flag to stop searching more than one lanelet if the object has no velocity
599 const auto traj2_speed{std::hypot(trajectory2.front().predicted_velocity.linear.x,
600 trajectory2.front().predicted_velocity.linear.y)};
601 bool traj2_has_zero_speed = traj2_speed < config_.obstacle_zero_speed_threshold_in_ms;
602
603 if (trajectory2.size() < 2)
604 {
605 throw std::invalid_argument("Object on ther road doesn't have enough predicted states! Please check motion_computation is correctly applying predicted states");
606 }
607 const double predict_step_duration = (rclcpp::Time(trajectory2.at(1).header.stamp) - rclcpp::Time(trajectory2.front().header.stamp)).seconds();
608 const double predict_total_duration = get_trajectory_duration(trajectory2);
609
610 if (predict_step_duration < 0.0)
611 {
612 throw std::invalid_argument("Predicted states of the object is malformed. Detected trajectory going backwards in time!");
613 }
614
615 // In order to optimize the for loops for comparing two trajectories, following logic skips every iteration_stride-th points of the traj2.
616 // Since skipping number of points from the traj2 may result in ignoring potential collisions, its value is dependent on two
617 // trajectories' speeds and intervehicle_collision_distance_in_m radius.
618 // Therefore, the derivation first calculates the max time, t, that both actors can move while still being in collision radius:
619 // sqrt( (v1 * t / 2)^2 + (v2 * t / 2)^2 ) = collision_radius. Here v1 and v2 are assumed to be perpendicular to each other and
620 // intersecting at t/2 to get max possible collision_radius. Solving for t gives following:
621 double iteration_stride_max_time_s = 2 * config_.intervehicle_collision_distance_in_m / sqrt(pow(traj2_speed, 2) + pow(trajectory1_max_speed, 2));
622 int iteration_stride = std::max(1, static_cast<int>(iteration_stride_max_time_s / predict_step_duration));
623
624 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Determined iteration_stride: " << iteration_stride
625 << ", with traj2_speed: " << traj2_speed
626 << ", with trajectory1_max_speed: " << trajectory1_max_speed
627 << ", with predict_step_duration: " << predict_step_duration
628 << ", iteration_stride_max_time_s: " << iteration_stride_max_time_s);
629
630 for (size_t j = 0; j < trajectory2.size(); j += iteration_stride) // Saving computation time aiming for 1.5 meter interval
631 {
632 lanelet::BasicPoint2d curr_point;
633 curr_point.x() = trajectory2.at(j).predicted_position.position.x;
634 curr_point.y() = trajectory2.at(j).predicted_position.position.y;
635
636 auto corresponding_lanelets = wm_->getLaneletsFromPoint(curr_point, 8); // some intersection can have 8 overlapping lanelets
637
638 for (const auto& llt: corresponding_lanelets)
639 {
640 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Checking llt: " << llt.id());
641
642 if (route_llt_ids_.find(llt.id()) != route_llt_ids_.end())
643 {
644 on_route = true;
645 on_route_idx = j;
646 break;
647 }
648 }
649 if (on_route || traj2_has_zero_speed)
650 break;
651 }
652
653 if (!on_route)
654 {
655 RCLCPP_DEBUG(nh_->get_logger(), "Predicted states are not on the route! ignoring");
656 return std::nullopt;
657 }
658
659 double smallest_dist = std::numeric_limits<double>::infinity();
660 for (size_t i = 0; i < trajectory1.trajectory_points.size() - 1; ++i)
661 {
662 auto p1a = trajectory1.trajectory_points.at(i);
663 auto p1b = trajectory1.trajectory_points.at(i + 1);
664 double previous_distance_between_predictions = std::numeric_limits<double>::infinity();
665 for (size_t j = on_route_idx; j < trajectory2.size() - 1; j += iteration_stride)
666 {
667 auto p2a = trajectory2.at(j);
668 auto p2b = trajectory2.at(j + 1);
669 double p1a_t = rclcpp::Time(p1a.target_time).seconds();
670 double p1b_t = rclcpp::Time(p1b.target_time).seconds();
671 double p2a_t = rclcpp::Time(p2a.header.stamp).seconds();
672 double p2b_t = rclcpp::Time(p2b.header.stamp).seconds();
673
674 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p1a.target_time: " << std::to_string(p1a_t) << ", p1b.target_time: " << std::to_string(p1b_t));
675 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p2a.target_time: " << std::to_string(p2a_t) << ", p2b.target_time: " << std::to_string(p2b_t));
676 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p1a.x: " << p1a.x << ", p1a.y: " << p1a.y);
677 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p1b.x: " << p1b.x << ", p1b.y: " << p1b.y);
678
679 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p2a.x: " << p2a.predicted_position.position.x << ", p2a.y: " << p2a.predicted_position.position.y);
680 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p2b.x: " << p2b.predicted_position.position.x << ", p2b.y: " << p2b.predicted_position.position.y);
681
682 // Linearly interpolate positions at a common timestamp for both trajectories
683 double dt = (p2a_t - p1a_t) / (p1b_t - p1a_t);
684 double x1 = p1a.x + dt * (p1b.x - p1a.x);
685 double y1 = p1a.y + dt * (p1b.y - p1a.y);
686 double x2 = p2a.predicted_position.position.x;
687 double y2 = p2a.predicted_position.position.y;
688
689 // Calculate the distance between the two interpolated points
690 const auto distance{std::hypot(x1 - x2, y1 - y2)};
691
692 smallest_dist = std::min(distance, smallest_dist);
693 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Smallest_dist: " << smallest_dist << ", distance: " << distance << ", dt: " << dt
694 << ", x1: " << x1 << ", y1: " << y1
695 << ", x2: " << x2 << ", y2: " << y2
696 << ", p2a_t:" << std::to_string(p2a_t));
697
698 // Following "if logic" assumes the traj2 is a simple cv model, aka, traj2 point is a straight line over time.
699 // And current traj1 point is fixed in this iteration.
700 // Then once the distance between the two start to increase over traj2 iteration,
701 // the distance will always increase and it's unnecessary to continue the logic to find the smallest_dist
702 if (previous_distance_between_predictions < distance)
703 {
704 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Stopping search here because the distance between predictions started to increase");
705 break;
706 }
707 previous_distance_between_predictions = distance;
708
709 if (i == 0 && j == 0 && distance > config_.collision_check_radius_in_m)
710 {
711 RCLCPP_DEBUG(nh_->get_logger(), "Too far away" );
712 return std::nullopt;
713 }
714
715 if (distance > collision_radius)
716 {
717 // continue searching for collision
718 continue;
719 }
720
721 GetCollisionResult collision_result;
722 collision_result.point1 = lanelet::BasicPoint2d(x1,y1);
723 collision_result.point2 = lanelet::BasicPoint2d(x2,y2);
724 collision_result.collision_time = rclcpp::Time(p2a.header.stamp);
725 return collision_result;
726 }
727 }
728
729 // No collision detected
730 return std::nullopt;
731 }
732
733 bool YieldPlugin::is_object_behind_vehicle(uint32_t object_id, const rclcpp::Time& collision_time, double vehicle_downtrack, double object_downtrack)
734 {
735 const auto previous_clearance_count = consecutive_clearance_count_for_obstacles_[object_id];
736 // if the object's location is half a length of the vehicle past its rear-axle, it is considered behind
737 // half a length of the vehicle to conservatively estimate the rear axle to rear bumper length
738 if (object_downtrack < vehicle_downtrack - config_.vehicle_length / 2)
739 {
741 RCLCPP_INFO_STREAM(nh_->get_logger(), "Detected an object nearby might be behind the vehicle at timestamp: " << std::to_string(collision_time.seconds()) <<
742 ", and consecutive_clearance_count_for obstacle: " << object_id << ", is: " << consecutive_clearance_count_for_obstacles_[object_id]);
743 }
744 // confirmed false positive for a collision
746 {
747 return true;
748 }
749 // if the clearance counter didn't increase by this point, true collision was detected
750 // therefore reset the consecutive clearance counter as it is no longer consecutive
751 if (consecutive_clearance_count_for_obstacles_[object_id] == previous_clearance_count)
752 {
754 }
755
756 return false;
757 }
758
759 std::optional<rclcpp::Time> YieldPlugin::get_collision_time(const carma_planning_msgs::msg::TrajectoryPlan& original_tp,
760 const carma_perception_msgs::msg::ExternalObject& curr_obstacle, double original_tp_max_speed)
761 {
762 auto plan_start_time = get_trajectory_start_time(original_tp);
763
764 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Object's back time: " << std::to_string(rclcpp::Time(curr_obstacle.predictions.back().header.stamp).seconds())
765 << ", plan_start_time: " << std::to_string(plan_start_time));
766
767 // do not process outdated objects
768 if (rclcpp::Time(curr_obstacle.predictions.back().header.stamp).seconds() <= plan_start_time)
769 {
770 return std::nullopt;
771 }
772
773 std::vector<carma_perception_msgs::msg::PredictedState> new_list;
774 carma_perception_msgs::msg::PredictedState curr_state;
775 // artificially include current position as one of the predicted states
776 curr_state.header.stamp = curr_obstacle.header.stamp;
777 curr_state.predicted_position.position.x = curr_obstacle.pose.pose.position.x;
778 curr_state.predicted_position.position.y = curr_obstacle.pose.pose.position.y;
779 // NOTE: predicted_velocity is not used for collision calculation, but timestamps
780 curr_state.predicted_velocity.linear.x = curr_obstacle.velocity.twist.linear.x;
781 curr_state.predicted_velocity.linear.y = curr_obstacle.velocity.twist.linear.y;
782 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Object: " << curr_obstacle.id <<", type: " << static_cast<int>(curr_obstacle.object_type)
783 << ", speed_x: " << curr_obstacle.velocity.twist.linear.x << ", speed_y: " << curr_obstacle.velocity.twist.linear.y);
784 new_list.push_back(curr_state);
785 new_list.insert(new_list.end(), curr_obstacle.predictions.cbegin(), curr_obstacle.predictions.cend());
786
787 const auto collision_result = get_collision(original_tp, new_list, config_.intervehicle_collision_distance_in_m, original_tp_max_speed);
788
789 if (!collision_result)
790 {
791 // reset the consecutive clearance counter because no collision was detected at this iteration
792 consecutive_clearance_count_for_obstacles_[curr_obstacle.id] = 0;
793 return std::nullopt;
794 }
795
796 // if within collision radius, it is not a collision if obstacle is behind the vehicle despite being in collision radius
797 const double vehicle_downtrack = wm_->routeTrackPos(collision_result.value().point1).downtrack;
798 const double object_downtrack = wm_->routeTrackPos(collision_result.value().point2).downtrack;
799
800 if (is_object_behind_vehicle(curr_obstacle.id, collision_result.value().collision_time, vehicle_downtrack, object_downtrack))
801 {
802 RCLCPP_INFO_STREAM(nh_->get_logger(), "Confirmed that the object: " << curr_obstacle.id << " is behind the vehicle at timestamp " << std::to_string(collision_result.value().collision_time.seconds()));
803 return std::nullopt;
804 }
805
806 const auto distance{std::hypot(
807 collision_result.value().point1.x() - collision_result.value().point2.x(),
808 collision_result.value().point1.y() - collision_result.value().point2.y()
809 )}; //for debug
810
811 RCLCPP_WARN_STREAM(nh_->get_logger(), "Collision detected for object: " << curr_obstacle.id << ", at timestamp " << std::to_string(collision_result.value().collision_time.seconds()) <<
812 ", x: " << collision_result.value().point1.x() << ", y: " << collision_result.value().point1.y() <<
813 ", within actual downtrack distance: " << object_downtrack - vehicle_downtrack <<
814 ", and collision distance: " << distance);
815
816 return collision_result.value().collision_time;
817 }
818
819 std::unordered_map<uint32_t, rclcpp::Time> YieldPlugin::get_collision_times_concurrently(const carma_planning_msgs::msg::TrajectoryPlan& original_tp,
820 const std::vector<carma_perception_msgs::msg::ExternalObject>& external_objects, double original_tp_max_speed)
821 {
822
823 std::unordered_map<uint32_t, std::future<std::optional<rclcpp::Time>>> futures;
824 std::unordered_map<uint32_t, rclcpp::Time> collision_times;
825
826 // Launch asynchronous tasks to check for collision times
827 for (const auto& object : external_objects) {
828 futures[object.id] = std::async(std::launch::async,[this, &original_tp, &object, &original_tp_max_speed]{
829 return get_collision_time(original_tp, object, original_tp_max_speed);
830 });
831 }
832
833 // Collect results from futures and update collision_times
834 for (const auto& object : external_objects) {
835 if (const auto collision_time{futures.at(object.id).get()}) {
836 collision_times[object.id] = collision_time.value();
837 }
838 }
839
840 return collision_times;
841 }
842
843 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,
844 const std::vector<carma_perception_msgs::msg::ExternalObject>& external_objects)
845 {
846 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "ExternalObjects size: " << external_objects.size());
847
848 if (!wm_->getRoute())
849 {
850 RCLCPP_WARN(nh_->get_logger(), "Yield plugin was not able to analyze collision since route is not available! Please check if route is set");
851 return std::nullopt;
852 }
853
854 // save route Ids for faster access
855 for (const auto& llt: wm_->getRoute()->shortestPath())
856 {
857 // TODO: Enhancement https://github.com/usdot-fhwa-stol/carma-platform/issues/2316
858 route_llt_ids_.insert(llt.id());
859 }
860
861 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"External Object List (external_objects) size: " << external_objects.size());
862 const double original_max_speed = max_trajectory_speed(original_tp.trajectory_points, get_trajectory_end_time(original_tp));
863 std::unordered_map<uint32_t, rclcpp::Time> collision_times = get_collision_times_concurrently(original_tp,external_objects, original_max_speed);
864
865 if (collision_times.empty()) { return std::nullopt; }
866
867 const auto earliest_colliding_object_id{std::min_element(
868 std::cbegin(collision_times), std::cend(collision_times),
869 [](const auto & a, const auto & b){ return a.second < b.second; })->first};
870
871 const auto earliest_colliding_object{std::find_if(
872 std::cbegin(external_objects), std::cend(external_objects),
873 [&earliest_colliding_object_id](const auto & object) { return object.id == earliest_colliding_object_id; })};
874
875 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"earliest object x: " << earliest_colliding_object->velocity.twist.linear.x
876 << ", y: " << earliest_colliding_object->velocity.twist.linear.y);
877 return std::make_pair(*earliest_colliding_object, collision_times.at(earliest_colliding_object_id).seconds());
878
879 }
880
881 double YieldPlugin::get_predicted_velocity_at_time(const geometry_msgs::msg::Twist& object_velocity_in_map_frame,
882 const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double timestamp_in_sec_to_predict)
883 {
884 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "timestamp_in_sec_to_predict: " << std::to_string(timestamp_in_sec_to_predict) <<
885 ", trajectory_end_time: " << std::to_string(get_trajectory_end_time(original_tp)));
886
887 double point_b_time = 0.0;
888 carma_planning_msgs::msg::TrajectoryPlanPoint point_a;
889 carma_planning_msgs::msg::TrajectoryPlanPoint point_b;
890
891 // trajectory points' time is guaranteed to be increasing
892 // then find the corresponding point at timestamp_in_sec_to_predict
893 for (size_t i = 0; i < original_tp.trajectory_points.size() - 1; ++i)
894 {
895 point_a = original_tp.trajectory_points.at(i);
896 point_b = original_tp.trajectory_points.at(i + 1);
897 point_b_time = rclcpp::Time(point_b.target_time).seconds();
898 if (point_b_time >= timestamp_in_sec_to_predict)
899 {
900 break;
901 }
902 }
903
904 auto dx = point_b.x - point_a.x;
905 auto dy = point_b.y - point_a.y;
906 const tf2::Vector3 trajectory_direction(dx, dy, 0);
907
908 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "timestamp_in_sec_to_predict: " << std::to_string(timestamp_in_sec_to_predict)
909 << ", point_b_time: " << std::to_string(point_b_time)
910 << ", dx: " << dx << ", dy: " << dy << ", "
911 << ", object_velocity_in_map_frame.x: " << object_velocity_in_map_frame.linear.x
912 << ", object_velocity_in_map_frame.y: " << object_velocity_in_map_frame.linear.y);
913
914 if (trajectory_direction.length() < 0.001) //EPSILON
915 {
916 return 0.0;
917 }
918
919 const tf2::Vector3 object_direction(object_velocity_in_map_frame.linear.x, object_velocity_in_map_frame.linear.y, 0);
920
921 return tf2::tf2Dot(object_direction, trajectory_direction) / trajectory_direction.length();
922 }
923
924 carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::update_traj_for_object(const carma_planning_msgs::msg::TrajectoryPlan& original_tp,
925 const std::vector<carma_perception_msgs::msg::ExternalObject>& external_objects, double initial_velocity)
926 {
927 if (original_tp.trajectory_points.size() < 2)
928 {
929 RCLCPP_WARN(nh_->get_logger(), "Yield plugin received less than 2 points in update_traj_for_object, returning unchanged...");
930 return original_tp;
931 }
932
933 // Get earliest collision object
934 const auto earliest_collision_obj_pair = get_earliest_collision_object_and_time(original_tp, external_objects);
935
936 if (!earliest_collision_obj_pair)
937 {
938 RCLCPP_DEBUG(nh_->get_logger(),"No collision detected, so trajectory not modified.");
939 return original_tp;
940 }
941
942 carma_perception_msgs::msg::ExternalObject earliest_collision_obj = earliest_collision_obj_pair.value().first;
943 double earliest_collision_time_in_seconds = earliest_collision_obj_pair.value().second;
944
945 // Issue (https://github.com/usdot-fhwa-stol/carma-platform/issues/2155): If the yield_plugin can detect if the roadway object is moving along the route,
946 // it is able to plan yielding much earlier and smoother using on_route_vehicle_collision_horizon_in_s.
947
948 const lanelet::BasicPoint2d vehicle_point(original_tp.trajectory_points[0].x,original_tp.trajectory_points[0].y);
949 const double vehicle_downtrack = wm_->routeTrackPos(vehicle_point).downtrack;
950
951 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"vehicle_downtrack: " << vehicle_downtrack);
952
953 RCLCPP_WARN_STREAM(nh_->get_logger(),"Collision Detected!");
954
955 const lanelet::BasicPoint2d object_point(earliest_collision_obj.pose.pose.position.x, earliest_collision_obj.pose.pose.position.y);
956 const double object_downtrack = wm_->routeTrackPos(object_point).downtrack;
957
958 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object_downtrack: " << object_downtrack);
959
960 const double object_downtrack_lead = std::max(0.0, object_downtrack - vehicle_downtrack);
961 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object_downtrack_lead: " << object_downtrack_lead);
962
963 // The vehicle's goal velocity of the yielding behavior is to match the velocity of the object along the trajectory.
964 double goal_velocity = get_predicted_velocity_at_time(earliest_collision_obj.velocity.twist, original_tp, earliest_collision_time_in_seconds);
965 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object's speed along trajectory at collision: " << goal_velocity);
966
967 // roadway object position
968 const double gap_time_until_min_gap_distance = std::max(0.0, object_downtrack_lead - config_.minimum_safety_gap_in_meters)/initial_velocity;
969
970 if (goal_velocity <= config_.obstacle_zero_speed_threshold_in_ms){
971 RCLCPP_WARN_STREAM(nh_->get_logger(),"The obstacle is not moving, goal velocity is set to 0 from: " << goal_velocity);
972 goal_velocity = 0.0;
973 }
974
975 // determine the safety inter-vehicle gap based on speed
976 double safety_gap = std::max(goal_velocity * gap_time_until_min_gap_distance, config_.minimum_safety_gap_in_meters);
977 if (!std::isnormal(safety_gap))
978 {
979 RCLCPP_WARN_STREAM(rclcpp::get_logger("yield_plugin"),"Detected non-normal (nan, inf, etc.) safety_gap."
980 "Making it desired safety gap configured at config_.minimum_safety_gap_in_meters: " << config_.minimum_safety_gap_in_meters);
982 }
984 {
985 // externally_commanded_safety_gap is desired distance gap commanded from external sources
986 // such as different plugin, map, or infrastructure depending on the use case
987 double externally_commanded_safety_gap = check_traj_for_digital_min_gap(original_tp);
988 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"externally_commanded_safety_gap: " << externally_commanded_safety_gap);
989 // if a digital gap is available, it is replaced as safety gap
990 safety_gap = std::max(safety_gap, externally_commanded_safety_gap);
991 }
992
993 const double goal_pos = std::max(0.0, object_downtrack_lead - safety_gap - config_.vehicle_length);
994 const double initial_pos = 0.0; //relative initial position (first trajectory point)
995 const double original_max_speed = max_trajectory_speed(original_tp.trajectory_points, earliest_collision_time_in_seconds);
996 const double delta_v_max = fabs(goal_velocity - original_max_speed);
997 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"delta_v_max: " << delta_v_max << ", safety_gap: " << safety_gap);
998
999 const double time_required_for_comfortable_decel_in_s = config_.acceleration_adjustment_factor * 2 * goal_pos / delta_v_max;
1000 const double min_time_required_for_comfortable_decel_in_s = delta_v_max / config_.yield_max_deceleration_in_ms2;
1001
1002 // planning time for object avoidance
1003 double planning_time_in_s = std::max({config_.min_obj_avoidance_plan_time_in_s, time_required_for_comfortable_decel_in_s, min_time_required_for_comfortable_decel_in_s});
1004 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"time_required_for_comfortable_decel_in_s: " << time_required_for_comfortable_decel_in_s << ", min_time_required_for_comfortable_decel_in_s: " << min_time_required_for_comfortable_decel_in_s);
1005
1006 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Object avoidance planning time: " << planning_time_in_s);
1007
1008 return generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, planning_time_in_s, original_max_speed);
1009 }
1010
1011
1012 std::vector<double> YieldPlugin::get_relative_downtracks(const carma_planning_msgs::msg::TrajectoryPlan& trajectory_plan) const
1013 {
1014 std::vector<double> downtracks;
1015 downtracks.reserve(trajectory_plan.trajectory_points.size());
1016 // relative downtrack distance of the fist Point is 0.0
1017 downtracks.push_back(0.0);
1018 for (size_t i=1; i < trajectory_plan.trajectory_points.size(); i++){
1019
1020 double dx = trajectory_plan.trajectory_points.at(i).x - trajectory_plan.trajectory_points.at(i-1).x;
1021 double dy = trajectory_plan.trajectory_points.at(i).y - trajectory_plan.trajectory_points.at(i-1).y;
1022 downtracks.push_back(sqrt(dx*dx + dy*dy));
1023 }
1024 return downtracks;
1025 }
1026
1027 double YieldPlugin::polynomial_calc(std::vector<double> coeff, double x) const
1028 {
1029 double result = 0;
1030 for (size_t i = 0; i < coeff.size(); i++)
1031 {
1032 double value = coeff.at(i) * pow(x, static_cast<int>(coeff.size() - 1 - i));
1033 result = result + value;
1034 }
1035 return result;
1036 }
1037
1038 double YieldPlugin::polynomial_calc_d(std::vector<double> coeff, double x) const
1039 {
1040 double result = 0;
1041 for (size_t i = 0; i < coeff.size()-1; i++)
1042 {
1043 double value = static_cast<int>(coeff.size() - 1 - i) * coeff.at(i) * pow(x, static_cast<int>(coeff.size() - 2 - i));
1044 result = result + value;
1045 }
1046 return result;
1047 }
1048
1049 double YieldPlugin::max_trajectory_speed(const std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint>& trajectory_points, double timestamp_in_sec_to_search_until) const
1050 {
1051 double max_speed = 0;
1052 for(size_t i = 0; i < trajectory_points.size() - 2; i++ )
1053 {
1054 double dx = trajectory_points.at(i + 1).x - trajectory_points.at(i).x;
1055 double dy = trajectory_points.at(i + 1).y - trajectory_points.at(i).y;
1056 double d = sqrt(dx*dx + dy*dy);
1057 double t = (rclcpp::Time(trajectory_points.at(i + 1).target_time).seconds() - rclcpp::Time(trajectory_points.at(i).target_time).seconds());
1058 double v = d/t;
1059 if(v > max_speed)
1060 {
1061 max_speed = v;
1062 }
1063 if (rclcpp::Time(trajectory_points.at(i + 1).target_time).seconds() >= timestamp_in_sec_to_search_until)
1064 {
1065 break;
1066 }
1067
1068 }
1069 return max_speed;
1070 }
1071
1072 double YieldPlugin::check_traj_for_digital_min_gap(const carma_planning_msgs::msg::TrajectoryPlan& original_tp) const
1073 {
1074 double desired_gap = 0;
1075
1076 for (size_t i = 0; i < original_tp.trajectory_points.size(); i++)
1077 {
1078 lanelet::BasicPoint2d veh_pos(original_tp.trajectory_points.at(i).x, original_tp.trajectory_points.at(i).y);
1079 auto llts = wm_->getLaneletsFromPoint(veh_pos, 1);
1080 if (llts.empty())
1081 {
1082 // This should technically never happen
1083 // However, trajectory generation currently may fail due to osm map issue https://github.com/usdot-fhwa-stol/carma-platform/issues/2503
1084 RCLCPP_WARN_STREAM(nh_->get_logger(),"Trajectory point: x= " << original_tp.trajectory_points.at(i).x << "y="<< original_tp.trajectory_points.at(i).y);
1085 RCLCPP_WARN_STREAM(nh_->get_logger(),"Trajectory is not on the road, so was unable to get the digital minimum gap. Returning default minimum_safety_gap_in_meters: " << config_.minimum_safety_gap_in_meters);
1086 return desired_gap;
1087 }
1088 auto digital_min_gap = llts[0].regulatoryElementsAs<lanelet::DigitalMinimumGap>(); //Returns a list of these elements)
1089 if (!digital_min_gap.empty())
1090 {
1091 double digital_gap = digital_min_gap[0]->getMinimumGap(); // Provided gap is in meters
1092 RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Digital Gap found with value: " << digital_gap);
1093 desired_gap = std::max(desired_gap, digital_gap);
1094 }
1095 }
1096 return desired_gap;
1097 }
1098
1099 void YieldPlugin::set_georeference_string(const std::string& georeference)
1100 {
1101 if (georeference_ != georeference)
1102 {
1103 georeference_ = georeference;
1104 map_projector_ = std::make_shared<lanelet::projection::LocalFrameProjector>(georeference.c_str()); // Build projector from proj string
1105 }
1106 }
1107
1108 void YieldPlugin::set_external_objects(const std::vector<carma_perception_msgs::msg::ExternalObject>& object_list)
1109 {
1110 external_objects_ = object_list;
1111 }
1112
1113} // 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< 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
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
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
double acceleration_adjustment_factor
int consecutive_clearance_count_for_obstacles_threshold
double yield_max_deceleration_in_ms2
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