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