17#include <rclcpp/rclcpp.hpp>
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>
28#include <carma_ros2_utils/carma_lifecycle_node.hpp>
30#include <Eigen/Geometry>
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>
40using oss = std::ostringstream;
48 : nh_(nh), wm_(wm), config_(config),mobility_response_publisher_(mobility_response_publisher), lc_status_publisher_(lc_status_publisher)
55 return rclcpp::Time(trajectory.trajectory_points.back().target_time).seconds();
60 return rclcpp::Time(trajectory.trajectory_points.front().target_time).seconds();
70 return (rclcpp::Time(trajectory.back().header.stamp) - rclcpp::Time(trajectory.front().header.stamp)).seconds();
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)
79 boost::geometry::append(self_traj, tpp);
82 for (
size_t i=0;
i<incoming_trajectory.size();
i++)
84 double res = boost::geometry::distance(incoming_trajectory.at(
i), self_traj);
88 intersection_points.push_back(std::make_pair(
i, incoming_trajectory.at(
i)));
91 return intersection_points;
96 carma_planning_msgs::msg::TrajectoryPlan trajectory_plan;
97 std::vector<lanelet::BasicPoint2d> map_points;
102 auto curr_point = ecef_trajectory.location;
104 for (
size_t i = 0;
i<ecef_trajectory.offsets.size();
i++)
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;
113 map_points.push_back(offset_point);
123 throw std::invalid_argument(
"No map projector available for ecef conversion");
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);
128 return lanelet::traits::to2D(map_point);
135 carma_v2x_msgs::msg::MobilityResponse out_mobility_response;
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;
145 out_mobility_response.is_accepted =
true;
147 else out_mobility_response.is_accepted =
false;
149 return out_mobility_response;
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")
160 RCLCPP_ERROR(
nh_->get_logger(),
"Cannot process mobility request as map projection is not yet set!");
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)
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";
171 RCLCPP_DEBUG(
nh_->get_logger(),
"CLC Request correctly received");
175 std::string req_sender_id = incoming_request.m_header.sender_id;
176 std::string req_plan_id = incoming_request.m_header.plan_id;
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;
182 RCLCPP_DEBUG_STREAM(
nh_->get_logger(),
"received urgency: " <<
clc_urgency_);
185 using boost::property_tree::ptree;
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);
198 std::vector<lanelet::BasicPoint2d> req_traj_plan = {};
202 double req_expiration_sec =
static_cast<double>(incoming_request.expiration);
203 double current_time_sec =
nh_->now().seconds();
205 bool response_to_clc_req =
false;
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;
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");
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");
227 carma_v2x_msgs::msg::MobilityResponse outgoing_response =
compose_mobility_response(req_sender_id, req_plan_id, response_to_clc_req);
229 lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::RESPONSE_SENT;
230 RCLCPP_DEBUG(
nh_->get_logger(),
"response sent");
248 carma_v2x_msgs::msg::BSMCoreData bsm_core_ = msg->core_data;
253 carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req,
254 carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp)
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");
260 rclcpp::Clock system_clock(RCL_SYSTEM_TIME);
261 rclcpp::Time start_time = system_clock.now();
263 carma_planning_msgs::msg::TrajectoryPlan original_trajectory = req->initial_trajectory_plan;
264 carma_planning_msgs::msg::TrajectoryPlan yield_trajectory;
272 double initial_velocity = req->vehicle_state.longitudinal_vel;
275 if (initial_velocity <
EPSILON)
277 initial_velocity = original_trajectory.initial_longitudinal_velocity;
283 RCLCPP_DEBUG(
nh_->get_logger(),
"Only consider high urgency clc");
286 RCLCPP_DEBUG(
nh_->get_logger(),
"Yield for CLC. We haven't received an updated negotiation this timestep");
292 RCLCPP_DEBUG(
nh_->get_logger(),
"unreliable CLC communication, switching to object avoidance");
298 RCLCPP_DEBUG(
nh_->get_logger(),
"Yield for object avoidance");
305 resp->trajectory_plan = original_trajectory;
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;
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;
321 rclcpp::Time end_time = system_clock.now();
323 auto duration = end_time - start_time;
324 RCLCPP_DEBUG_STREAM(
nh_->get_logger(),
"ExecutionTime: " <<
std::to_string(duration.seconds()));
329 carma_planning_msgs::msg::TrajectoryPlan cooperative_trajectory;
331 double initial_pos = 0;
333 double initial_velocity = current_speed;
337 std::vector<lanelet::BasicPoint2d> host_traj_points = {};
338 for (
size_t i=0;
i<original_tp.trajectory_points.size();
i++)
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);
347 if (!intersection_points.empty())
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();
354 RCLCPP_DEBUG_STREAM(
nh_->get_logger(),
"digital_gap: " << digital_gap);
356 RCLCPP_DEBUG_STREAM(
nh_->get_logger(),
"Goal position (goal_pos): " << goal_pos);
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);
366 double incoming_trajectory_speed = sqrt(dx2*dx2 + dy2*dy2)/(intersection_points[0].first *
ecef_traj_timestep_);
368 goal_velocity = std::min(goal_velocity, incoming_trajectory_speed);
371 RCLCPP_DEBUG_STREAM(
nh_->get_logger(),
"goal_velocity: " << goal_velocity);
372 RCLCPP_DEBUG_STREAM(
nh_->get_logger(),
"incoming_trajectory_speed: " << incoming_trajectory_speed);
374 if (planning_time > min_time)
378 cooperative_trajectory =
generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, planning_time, original_max_speed);
383 RCLCPP_DEBUG(
nh_->get_logger(),
"The incoming requested trajectory is rejected, due to insufficient gap");
384 cooperative_trajectory = original_tp;
391 RCLCPP_DEBUG(
nh_->get_logger(),
"The incoming requested trajectory does not overlap with host vehicle's trajectory");
392 cooperative_trajectory = original_tp;
395 return cooperative_trajectory;
400 double smallest_time_step = std::numeric_limits<double>::infinity();
401 for (
size_t i = 0;
i < original_tp.trajectory_points.size() - 1;
i ++)
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());
407 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"yield_plugin"),
"smallest_time_step: " << smallest_time_step);
409 return smallest_time_step;
413 double initial_velocity,
double goal_velocity,
double planning_time,
double original_max_speed)
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]);
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);
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);
445 std::vector<double> polynomial_coefficients = quintic_coefficient_calculator::quintic_coefficient_calculator(initial_pos,
453 RCLCPP_DEBUG_STREAM(
nh_->get_logger(),
"Used original_max_speed: " << original_max_speed);
455 while (new_traj_accumulated_downtrack < goal_pos -
EPSILON && original_traj_idx < original_traj_relative_downtracks.size())
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);
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);
466 if (velocity_at_target_time < 0.0)
472 velocity_at_target_time = std::clamp(velocity_at_target_time, 0.0, original_max_speed);
475 if (downtrack_at_target_time >= original_traj_accumulated_downtrack)
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);
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 ++;
485 new_traj_accumulated_downtrack = downtrack_at_target_time;
492 std::fill_n(std::back_inserter(calculated_speeds),
493 std::size(original_traj_relative_downtracks) - std::size(calculated_speeds),
498 RCLCPP_DEBUG_STREAM(
nh_->get_logger(),
"filtered_speeds size: " << filtered_speeds.size());
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()));
504 for(
size_t i = 1;
i < original_tp.trajectory_points.size();
i++)
506 carma_planning_msgs::msg::TrajectoryPlanPoint jmt_tpp = original_tp.trajectory_points.at(
i);
510 double current_speed = goal_velocity;
512 if (
i < filtered_speeds.size())
514 current_speed = filtered_speeds.at(
i);
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);
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);
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);
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;
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;
561 const std::vector<carma_perception_msgs::msg::PredictedState>& trajectory2,
double collision_radius,
double trajectory1_max_speed)
565 RCLCPP_DEBUG_STREAM(
nh_->get_logger(),
"Starting a new collision detection, trajectory size: "
566 << trajectory1.trajectory_points.size() <<
". prediction size: " << trajectory2.size());
569 bool on_route =
false;
570 int on_route_idx = 0;
573 const auto traj2_speed{std::hypot(trajectory2.front().predicted_velocity.linear.x,
574 trajectory2.front().predicted_velocity.linear.y)};
577 if (trajectory2.size() < 2)
579 throw std::invalid_argument(
"Object on ther road doesn't have enough predicted states! Please check motion_computation is correctly applying predicted states");
581 const double predict_step_duration = (rclcpp::Time(trajectory2.at(1).header.stamp) - rclcpp::Time(trajectory2.front().header.stamp)).seconds();
584 if (predict_step_duration < 0.0)
586 throw std::invalid_argument(
"Predicted states of the object is malformed. Detected trajectory going backwards in time!");
596 int iteration_stride = std::max(1,
static_cast<int>(iteration_stride_max_time_s / predict_step_duration));
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);
604 for (
size_t j = 0; j < trajectory2.size(); j += iteration_stride)
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;
610 auto corresponding_lanelets =
wm_->getLaneletsFromPoint(curr_point, 8);
612 for (
const auto& llt: corresponding_lanelets)
614 RCLCPP_DEBUG_STREAM(
nh_->get_logger(),
"Checking llt: " << llt.id());
623 if (on_route || traj2_has_zero_speed)
629 RCLCPP_DEBUG(
nh_->get_logger(),
"Predicted states are not on the route! ignoring");
633 double smallest_dist = std::numeric_limits<double>::infinity();
634 for (
size_t i = 0;
i < trajectory1.trajectory_points.size() - 1; ++
i)
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)
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();
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);
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);
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;
664 const auto distance{std::hypot(x1 - x2, y1 - y2)};
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
676 if (previous_distance_between_predictions < distance)
678 RCLCPP_DEBUG_STREAM(
nh_->get_logger(),
"Stopping search here because the distance between predictions started to increase");
681 previous_distance_between_predictions = distance;
685 RCLCPP_DEBUG(
nh_->get_logger(),
"Too far away" );
689 if (distance > collision_radius)
696 collision_result.
point1 = lanelet::BasicPoint2d(x1,y1);
697 collision_result.
point2 = lanelet::BasicPoint2d(x2,y2);
699 return collision_result;
715 RCLCPP_INFO_STREAM(
nh_->get_logger(),
"Detected an object nearby might be behind the vehicle at timestamp: " <<
std::to_string(collision_time.seconds()) <<
734 const carma_perception_msgs::msg::ExternalObject& curr_obstacle,
double original_tp_max_speed)
738 RCLCPP_DEBUG_STREAM(
nh_->get_logger(),
"Object's back time: " <<
std::to_string(rclcpp::Time(curr_obstacle.predictions.back().header.stamp).seconds())
742 if (rclcpp::Time(curr_obstacle.predictions.back().header.stamp).seconds() <= plan_start_time)
747 std::vector<carma_perception_msgs::msg::PredictedState> new_list;
748 carma_perception_msgs::msg::PredictedState curr_state;
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;
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());
763 if (!collision_result)
771 const double vehicle_downtrack =
wm_->routeTrackPos(collision_result.value().point1).downtrack;
772 const double object_downtrack =
wm_->routeTrackPos(collision_result.value().point2).downtrack;
774 if (
is_object_behind_vehicle(curr_obstacle.id, collision_result.value().collision_time, vehicle_downtrack, object_downtrack))
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()));
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()
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);
790 return collision_result.value().collision_time;
794 const std::vector<carma_perception_msgs::msg::ExternalObject>& external_objects,
double original_tp_max_speed)
797 std::unordered_map<uint32_t, std::future<std::optional<rclcpp::Time>>> futures;
798 std::unordered_map<uint32_t, rclcpp::Time> 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]{
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();
814 return collision_times;
818 const std::vector<carma_perception_msgs::msg::ExternalObject>& external_objects)
820 RCLCPP_DEBUG_STREAM(
nh_->get_logger(),
"ExternalObjects size: " << external_objects.size());
822 if (!
wm_->getRoute())
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");
829 for (
const auto& llt:
wm_->getRoute()->shortestPath())
835 RCLCPP_DEBUG_STREAM(
nh_->get_logger(),
"External Object List (external_objects) size: " << external_objects.size());
839 if (collision_times.empty()) {
return std::nullopt; }
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};
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; })};
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());
856 const carma_planning_msgs::msg::TrajectoryPlan& original_tp,
double timestamp_in_sec_to_predict)
858 RCLCPP_DEBUG_STREAM(
nh_->get_logger(),
"timestamp_in_sec_to_predict: " <<
std::to_string(timestamp_in_sec_to_predict) <<
861 double point_b_time = 0.0;
862 carma_planning_msgs::msg::TrajectoryPlanPoint point_a;
863 carma_planning_msgs::msg::TrajectoryPlanPoint point_b;
867 for (
size_t i = 0;
i < original_tp.trajectory_points.size() - 1; ++
i)
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)
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);
882 RCLCPP_DEBUG_STREAM(
nh_->get_logger(),
"timestamp_in_sec_to_predict: " <<
std::to_string(timestamp_in_sec_to_predict)
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);
888 if (trajectory_direction.length() < 0.001)
893 const tf2::Vector3 object_direction(object_velocity_in_map_frame.linear.x, object_velocity_in_map_frame.linear.y, 0);
895 return tf2::tf2Dot(object_direction, trajectory_direction) / trajectory_direction.length();
899 const std::vector<carma_perception_msgs::msg::ExternalObject>& external_objects,
double initial_velocity)
901 if (original_tp.trajectory_points.size() < 2)
903 RCLCPP_WARN(
nh_->get_logger(),
"Yield plugin received less than 2 points in update_traj_for_object, returning unchanged...");
910 if (!earliest_collision_obj_pair)
912 RCLCPP_DEBUG(
nh_->get_logger(),
"No collision detected, so trajectory not modified.");
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;
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;
925 RCLCPP_DEBUG_STREAM(
nh_->get_logger(),
"vehicle_downtrack: " << vehicle_downtrack);
927 RCLCPP_WARN_STREAM(
nh_->get_logger(),
"Collision Detected!");
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;
932 RCLCPP_DEBUG_STREAM(
nh_->get_logger(),
"object_downtrack: " << object_downtrack);
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);
939 RCLCPP_DEBUG_STREAM(
nh_->get_logger(),
"object's speed along trajectory at collision: " << goal_velocity);
945 RCLCPP_WARN_STREAM(
nh_->get_logger(),
"The obstacle is not moving, goal velocity is set to 0 from: " << goal_velocity);
951 if (!std::isnormal(safety_gap))
953 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"yield_plugin"),
"Detected non-normal (nan, inf, etc.) safety_gap."
962 RCLCPP_DEBUG_STREAM(
nh_->get_logger(),
"externally_commanded_safety_gap: " << externally_commanded_safety_gap);
964 safety_gap = std::max(safety_gap, externally_commanded_safety_gap);
968 const double initial_pos = 0.0;
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);
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);
980 RCLCPP_DEBUG_STREAM(
nh_->get_logger(),
"Object avoidance planning time: " << planning_time_in_s);
982 return generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, planning_time_in_s, original_max_speed);
988 std::vector<double> downtracks;
989 downtracks.reserve(trajectory_plan.trajectory_points.size());
991 downtracks.push_back(0.0);
992 for (
size_t i=1;
i < trajectory_plan.trajectory_points.size();
i++){
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));
1004 for (
size_t i = 0;
i < coeff.size();
i++)
1006 double value = coeff.at(
i) * pow(
x,
static_cast<int>(coeff.size() - 1 -
i));
1007 result = result + value;
1015 for (
size_t i = 0;
i < coeff.size()-1;
i++)
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;
1025 double max_speed = 0;
1026 for(
size_t i = 0;
i < trajectory_points.size() - 2;
i++ )
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());
1037 if (rclcpp::Time(trajectory_points.at(
i + 1).target_time).seconds() >= timestamp_in_sec_to_search_until)
1048 double desired_gap = 0;
1050 for (
size_t i = 0;
i < original_tp.trajectory_points.size();
i++)
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);
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);
1062 auto digital_min_gap = llts[0].regulatoryElementsAs<lanelet::DigitalMinimumGap>();
1063 if (!digital_min_gap.empty())
1065 double digital_gap = digital_min_gap[0]->getMinimumGap();
1066 RCLCPP_DEBUG_STREAM(
nh_->get_logger(),
"Digital Gap found with value: " << digital_gap);
1067 desired_gap = std::max(desired_gap, digital_gap);
1078 map_projector_ = std::make_shared<lanelet::projection::LocalFrameProjector>(georeference.c_str());
std::string georeference_
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
double ecef_traj_timestep_
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
double req_target_plan_time_
bool cooperative_request_acceptable_
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...
int timesteps_since_last_req_
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::vector< double > moving_average_filter(const std::vector< double > input, int window_size, bool ignore_first_point=true)
Extremely simplie moving average filter.
auto to_string(const UtmZone &zone) -> std::string
std::shared_ptr< const WorldModel > WorldModelConstPtr
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
bool enable_adjustable_gap
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
int acceptable_passed_timesteps
double safety_collision_time_gap_in_s
Convenience class for saving collision results.
lanelet::BasicPoint2d point1
rclcpp::Time collision_time
lanelet::BasicPoint2d point2