21#define GET_MANEUVER_PROPERTY(mvr, property) \
22 (((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? \
23 (mvr).intersection_transit_left_turn_maneuver.property : \
24 ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? \
25 (mvr).intersection_transit_right_turn_maneuver.property : \
26 ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? \
27 (mvr).intersection_transit_straight_maneuver.property : \
28 ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_CHANGE ? (mvr).lane_change_maneuver.property : \
29 (mvr).type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property : \
30 throw new std::invalid_argument("GET_MANEUVER_" \
40 namespace std_ph = std::placeholders;
43 :
carma_guidance_plugins::StrategicPlugin(options), tf2_buffer_(std::make_shared<tf2_ros::Buffer>(this->get_clock()))
67 declare_parameter<long>(
"enable_carma_wm_spat_processing",
111 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
112 "LCI Strategic Plugin updated green_light_time_buffer based on vehicle_response_lag to "
120 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Done loading parameters: " <<
config_);
125 mob_operation_sub_ = create_subscription<carma_v2x_msgs::msg::MobilityOperation>(
"incoming_mobility_operation", 1,
129 bsm_sub_ = create_subscription<carma_v2x_msgs::msg::BSM>(
"bsm_outbound", 1,
141 mobility_operation_pub_ = create_publisher<carma_v2x_msgs::msg::MobilityOperation>(
"outgoing_mobility_operation", 1);
142 case_pub_ = create_publisher<std_msgs::msg::Int8>(
"lci_strategic_plugin/ts_case_num", 1);
143 tf_distance_pub_ = create_publisher<std_msgs::msg::Float64>(
"lci_strategic_plugin/distance_remaining_to_tf", 1);
144 earliest_et_pub_ = create_publisher<std_msgs::msg::Float64>(
"lci_strategic_plugin/earliest_entry_time", 1);
145 et_pub_ = create_publisher<std_msgs::msg::Float64>(
"lci_strategic_plugin/scheduled_entry_time", 1);
148 return CallbackReturn::SUCCESS;
153 auto error_double = update_params<double>({
173 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
174 "LCI Strategic Plugin updated green_light_time_buffer based on vehicle_response_lag to "
176 rcl_interfaces::msg::SetParametersResult result;
178 result.successful = !error_double;
185 std_msgs::msg::Int8 case_num_msg;
186 std_msgs::msg::Float64 tf_distance;
187 std_msgs::msg::Float64 earliest_et;
188 std_msgs::msg::Float64 scheduled_et;
198 et_pub_->publish(scheduled_et);
208 std::chrono::duration<double>(0.5),
211 return CallbackReturn::SUCCESS;
220 case lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN:
221 case lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE:
222 case lanelet::CarmaTrafficSignalState::PERMISSIVE_MOVEMENT_ALLOWED:
223 case lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED:
228 case lanelet::CarmaTrafficSignalState::PERMISSIVE_CLEARANCE:
231 case lanelet::CarmaTrafficSignalState::DARK:
232 case lanelet::CarmaTrafficSignalState::STOP_THEN_PROCEED:
234 case lanelet::CarmaTrafficSignalState::PRE_MOVEMENT:
236 case lanelet::CarmaTrafficSignalState::CAUTION_CONFLICTING_TRAFFIC:
248 geometry_msgs::msg::TransformStamped tf2 =
tf2_buffer_->lookupTransform(
"base_link",
"vehicle_front", rclcpp::Time(0), rclcpp::Duration::from_nanoseconds(20.0 * 1e9));
250 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"length_to_front_bumper_: " <<
length_to_front_bumper_);
253 catch (
const tf2::TransformException &ex)
255 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"), ex.what());
262 if (!req->prior_plan.maneuvers.empty())
264 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Provided with initial plan...");
272 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"No initial plan provided...");
274 state.
stamp = rclcpp::Time(req->header.stamp, RCL_SYSTEM_TIME);
276 state.
speed = req->veh_logitudinal_velocity;
277 state.
lane_id = stoi(req->veh_lane_id);
279 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"extractInitialState >>>> state.stamp: " <<
std::to_string(state.
stamp.seconds()));
280 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"extractInitialState >>>> state.downtrack : " << state.
downtrack );
281 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"extractInitialState >>>> state.speed: " << state.
speed);
282 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"extractInitialState >>>> state.lane_id: " << state.
lane_id);
289 lanelet::Optional<carma_wm::TrafficRulesConstPtr> traffic_rules =
wm_->getTrafficRules();
292 return (*traffic_rules)->speedLimit(llt).speedLimit.value();
296 throw std::invalid_argument(
"Valid traffic rules object could not be built");
301 const rclcpp::Time& source_time)
const
305 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Traffic light data not available for source_time " <<
std::to_string(source_time.seconds()));
309 lanelet::CarmaTrafficSignalState light_state = optional_state.get().second;
313 RCLCPP_ERROR_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"LCIStrategic Plugin asked to handle CarmaTrafficSignalState: " << light_state
314 <<
" which is not supported.");
323 rclcpp::Time early_arrival_time_by_algo =
326 rclcpp::Time late_arrival_time_by_algo =
329 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"light_arrival_time_by_algo: " <<
std::to_string(light_arrival_time_by_algo.seconds()));
330 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"early_arrival_time_by_algo: " <<
std::to_string(early_arrival_time_by_algo.seconds()));
331 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"late_arrival_time_by_algo: " <<
std::to_string(late_arrival_time_by_algo.seconds()));
333 auto early_arrival_state_by_algo_optional = traffic_light->predictState(lanelet::time::timeFromSec(early_arrival_time_by_algo.seconds()));
335 if (!
validLightState(early_arrival_state_by_algo_optional, early_arrival_time_by_algo))
338 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"early_arrival_state_by_algo: " << early_arrival_state_by_algo_optional.get().second);
340 auto late_arrival_state_by_algo_optional = traffic_light->predictState(lanelet::time::timeFromSec(late_arrival_time_by_algo.seconds()));
342 if (!
validLightState(late_arrival_state_by_algo_optional, late_arrival_time_by_algo))
345 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"late_arrival_state_by_algo: " << late_arrival_state_by_algo_optional.get().second);
347 bool can_make_early_arrival =
true;
348 bool can_make_late_arrival =
true;
351 can_make_early_arrival =
isStateAllowedGreen(early_arrival_state_by_algo_optional.get().second);
354 can_make_late_arrival =
isStateAllowedGreen(late_arrival_state_by_algo_optional.get().second);
357 if (can_make_early_arrival && can_make_late_arrival)
365 return state == lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED ||
366 state == lanelet::CarmaTrafficSignalState::PERMISSIVE_MOVEMENT_ALLOWED;
370 double end_downtrack,
371 bool shortest_path_only,
372 bool bounds_inclusive)
const
374 std::vector<lanelet::ConstLanelet> crossed_lanelets =
375 wm_->getLaneletsBetween(start_downtrack, end_downtrack, shortest_path_only, bounds_inclusive);
377 if (crossed_lanelets.size() == 0)
379 throw std::invalid_argument(
"getLaneletsBetweenWithException called but inputs do not cross any lanelets going "
384 return crossed_lanelets;
388 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
390 const lanelet::CarmaTrafficSignalPtr& traffic_light,
391 const lanelet::ConstLanelet& entry_lanelet,
const lanelet::ConstLanelet& exit_lanelet,
const lanelet::ConstLanelet& current_lanelet,
392 double traffic_light_down_track,
395 double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.
downtrack;
398 std::vector<lanelet::ConstLanelet> crossed_lanelets =
413 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"HANDLE_STOPPING: Planning stop and wait maneuver at decel_rate: " << decel_rate);
416 current_state.
downtrack, traffic_light_down_track, current_state.
speed, crossed_lanelets.front().id(),
417 crossed_lanelets.back().id(), current_state.
stamp,
423 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
425 double current_state_speed,
427 double remaining_time,
428 lanelet::Id exit_lanelet_id,
429 const lanelet::CarmaTrafficSignalPtr& traffic_light,
432 double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.
downtrack;
435 std::vector<lanelet::ConstLanelet> crossed_lanelets =
440 if (incomplete_traj_params.is_algorithm_successful ==
false)
442 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Failed to generate maneuver for edge cases...");
447 current_state_speed, incomplete_traj_params.modified_departure_speed, current_state.
stamp, current_state.
stamp + rclcpp::Duration::from_nanoseconds(incomplete_traj_params.modified_remaining_time * 1e9), incomplete_traj_params));
451 rclcpp::Time intersection_exit_time =
452 current_state.
stamp + rclcpp::Duration::from_nanoseconds(incomplete_traj_params.modified_remaining_time * 1e9) + rclcpp::Duration::from_nanoseconds(intersection_length / incomplete_traj_params.modified_departure_speed * 1e9);
456 incomplete_traj_params.modified_departure_speed, current_state.
stamp + rclcpp::Duration::from_nanoseconds(incomplete_traj_params.modified_remaining_time * 1e9), intersection_exit_time, crossed_lanelets.back().id(), crossed_lanelets.back().id()));
462 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
464 double current_state_speed,
465 const lanelet::CarmaTrafficSignalPtr& traffic_light,
470 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"handleCruisingUntilStop is called but it is not case_8");
474 auto new_ts_params = ts_params;
476 double decel_rate = std::fabs(ts_params.
a3_);
478 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"CASE_8: Planning cruise and stop with decel_rate: " << decel_rate);
480 new_ts_params.t3_ = new_ts_params.t2_;
481 new_ts_params.x3_ = new_ts_params.x2_;
482 new_ts_params.v3_ = new_ts_params.v2_;
483 new_ts_params.a3_ = new_ts_params.a2_;
486 std::vector<lanelet::ConstLanelet> lane_follow_crossed_lanelets =
490 current_state_speed, new_ts_params.v2_, current_state.
stamp, rclcpp::Time(new_ts_params.t2_ * 1e9), new_ts_params));
493 std::vector<lanelet::ConstLanelet> case_8_crossed_lanelets =
497 new_ts_params.x2_, traffic_light_down_track, new_ts_params.v2_, case_8_crossed_lanelets.front().id(),
498 case_8_crossed_lanelets.back().id(), rclcpp::Time(new_ts_params.t2_ * 1e9),
505 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
507 double current_state_speed,
508 const lanelet::CarmaTrafficSignalPtr& traffic_light,
509 const lanelet::ConstLanelet& entry_lanelet,
const lanelet::ConstLanelet& exit_lanelet,
510 double traffic_light_down_track,
const TrajectoryParams& ts_params,
bool is_certainty_check_optional)
517 rclcpp::Time light_arrival_time_by_algo = rclcpp::Time(ts_params.
t3_ * 1e9);
518 double remaining_time = light_arrival_time_by_algo.seconds() - rclcpp::Time(req->header.stamp, RCL_SYSTEM_TIME).seconds();
519 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Algo initially successful: New light_arrival_time_by_algo: " <<
std::to_string(light_arrival_time_by_algo.seconds()) <<
", with remaining_time: " <<
std::to_string(remaining_time));
523 std::vector<lanelet::ConstLanelet> crossed_lanelets =
527 if (!can_make_green_optional)
530 if (can_make_green_optional.get() || is_certainty_check_optional)
532 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"HANDLE_SUCCESSFULL: Algorithm successful, and able to make it at green with certainty. Planning traj smooth and intersection transit maneuvers");
535 current_state_speed, ts_params.
v3_, current_state.
stamp, light_arrival_time_by_algo, ts_params));
539 rclcpp::Time intersection_exit_time =
540 light_arrival_time_by_algo + rclcpp::Duration::from_nanoseconds(intersection_length /
intersection_speed_.get() * 1e9);
544 intersection_speed_.get(), light_arrival_time_by_algo, intersection_exit_time, crossed_lanelets.back().id(), crossed_lanelets.back().id()));
557 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"HANDLE_LAST_RESORT_CASE: Starting...");
558 double starting_downtrack = traffic_light_downtrack - remaining_downtrack;
559 double modified_remaining_time_upper;
560 double modified_remaining_time_lower;
561 double modified_departure_speed_upper;
562 double modified_departure_speed_lower;
563 bool calculation_success_upper =
true;
567 traj_upper.
t0_ = current_time;
568 traj_upper.
v0_ = starting_speed;
569 traj_upper.
x0_ = starting_downtrack;
573 if (departure_speed >= starting_speed)
575 if ((pow(departure_speed,2) - pow(starting_speed,2))/(2*
max_comfort_accel_) >= remaining_downtrack)
577 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"HandleFailureCase -> Upper Trajectory -> Current Speed <= Desired Departure Speed, Actual Departure Speed < Desired Departure Speed");
579 modified_departure_speed_upper = sqrt(pow(starting_speed, 2) + (2 *
max_comfort_accel_ * remaining_downtrack));
580 modified_remaining_time_upper = (modified_departure_speed_upper - starting_speed) /
max_comfort_accel_;
582 traj_upper.
t1_ = current_time + modified_remaining_time_upper;
583 traj_upper.
v1_ = modified_departure_speed_upper;
585 traj_upper.
x1_ = traffic_light_downtrack;
587 traj_upper.
t2_ = traj_upper.
t1_;
588 traj_upper.
v2_ = traj_upper.
v1_;
589 traj_upper.
a2_ = traj_upper.
a1_;
590 traj_upper.
x2_ = traj_upper.
x1_;
597 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"HandleFailureCase -> Upper Trajectory -> Current Speed < Desired Departure Speed, Actual Departure Speed = Desired Departure Speed");
599 double cruising_distance = remaining_downtrack - (pow(departure_speed, 2) - pow(starting_speed, 2))/ ( 2 *
max_comfort_accel_);
600 if (cruising_distance < -
EPSILON)
602 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Detected calculation failure in upper case 2");
603 calculation_success_upper =
false;
605 modified_remaining_time_upper = ((departure_speed - starting_speed) /
max_comfort_accel_) + (cruising_distance / departure_speed);
608 traj_upper.
v1_ = departure_speed;
610 traj_upper.
x1_ = starting_downtrack + (pow(departure_speed, 2) - pow(starting_speed, 2)) / (2 *
max_comfort_accel_);
612 traj_upper.
t2_ = current_time + modified_remaining_time_upper;
613 traj_upper.
v2_ = departure_speed;
615 traj_upper.
x2_ = traffic_light_downtrack;
622 if (departure_speed < starting_speed)
624 if ((pow(departure_speed,2) - pow(starting_speed,2))/(2*
max_comfort_decel_) >= remaining_downtrack)
626 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"HandleFailureCase -> Upper Trajectory -> Current Speed > Desired Departure Speed, Actual Departure Speed > Desired Departure Speed");
628 modified_departure_speed_upper = sqrt(pow(starting_speed, 2) + (2 *
max_comfort_decel_ * remaining_downtrack));
629 modified_remaining_time_upper = (modified_departure_speed_upper - starting_speed) /
max_comfort_decel_;
631 traj_upper.
t1_ = current_time + modified_remaining_time_upper;
632 traj_upper.
v1_ = modified_departure_speed_upper;
634 traj_upper.
x1_ = traffic_light_downtrack;
636 traj_upper.
t2_ = traj_upper.
t1_;
637 traj_upper.
v2_ = traj_upper.
v1_;
638 traj_upper.
a2_ = traj_upper.
a1_;
639 traj_upper.
x2_ = traj_upper.
x1_;
646 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"HandleFailureCase -> Upper Trajectory -> Current Speed > Desired Departure Speed, Actual Departure Speed = Desired Departure Speed");
648 double cruising_distance = remaining_downtrack - (pow(departure_speed, 2) - pow(starting_speed, 2))/ ( 2 *
max_comfort_decel_);
650 if (cruising_distance < -
EPSILON)
652 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Detected calculation failure in upper case 4");
653 calculation_success_upper =
false;
656 modified_remaining_time_upper = cruising_distance / starting_speed + (departure_speed - starting_speed) /
max_comfort_decel_ ;
658 traj_upper.
t1_ = current_time + cruising_distance / starting_speed;
659 traj_upper.
v1_ = starting_speed;
660 traj_upper.
a1_ = 0.0;
661 traj_upper.
x1_ = starting_downtrack + cruising_distance;
663 traj_upper.
t2_ = current_time + modified_remaining_time_upper;
664 traj_upper.
v2_ = departure_speed;
666 traj_upper.
x2_ = traffic_light_downtrack;
673 traj_upper.
t3_ = traj_upper.
t2_;
674 traj_upper.
v3_ = traj_upper.
v2_;
675 traj_upper.
a3_ = traj_upper.
a2_;
676 traj_upper.
x3_ = traj_upper.
x2_;
680 modified_departure_speed_lower = sqrt(pow(starting_speed, 2) + (2 *
max_comfort_decel_ * remaining_downtrack));
681 modified_remaining_time_lower = (modified_departure_speed_lower - starting_speed) /
max_comfort_decel_;
683 traj_lower.
t0_ = current_time;
684 traj_lower.
v0_ = starting_speed;
685 traj_lower.
x0_ = starting_downtrack;
689 traj_lower.
t1_ = current_time + modified_remaining_time_lower;
690 traj_lower.
v1_ = modified_departure_speed_lower;
692 traj_lower.
x1_ = traffic_light_downtrack;
694 traj_lower.
t2_ = traj_lower.
t1_;
695 traj_lower.
v2_ = traj_lower.
v1_;
696 traj_lower.
a2_ = traj_lower.
a1_;
697 traj_lower.
x2_ = traj_lower.
x1_;
699 traj_lower.
t3_ = traj_lower.
t2_;
700 traj_lower.
v3_ = traj_lower.
v2_;
701 traj_lower.
a3_ = traj_lower.
a2_;
702 traj_lower.
x3_ = traj_lower.
x2_;
708 bool is_return_params_found =
false;
710 if (calculation_success_upper)
712 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Checking this time!: " << current_time + modified_remaining_time_upper);
714 auto upper_optional = traffic_light->predictState(lanelet::time::timeFromSec(current_time + modified_remaining_time_upper));
716 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Checking this time! state: " << upper_optional.get().second);
718 if (!
validLightState(upper_optional, rclcpp::Time((current_time + modified_remaining_time_upper) * 1e9)))
720 RCLCPP_ERROR_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Unable to resolve give signal for modified_remaining_time_upper: " <<
std::to_string(current_time + modified_remaining_time_upper));
726 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Detected Upper GREEN case");
727 return_params = traj_upper;
728 is_return_params_found =
true;
733 if (!is_return_params_found)
735 auto lower_optional = traffic_light->predictState(lanelet::time::timeFromSec(current_time + modified_remaining_time_lower));
737 if (!
validLightState(lower_optional, rclcpp::Time((current_time + modified_remaining_time_lower) * 1e9)))
739 RCLCPP_ERROR_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Unable to resolve give signal for modified_remaining_time_lower:" <<
std::to_string(current_time + modified_remaining_time_lower));
745 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Detected Lower GREEN case");
746 return_params = traj_lower;
747 is_return_params_found =
true;
756 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Updated the speed, and using modified_departure_speed: " << return_params.
modified_departure_speed);
758 return return_params;
762 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Unable to handle edge case gracefully");
765 return return_params;
771 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
const VehicleState& current_state,
772 const lanelet::CarmaTrafficSignalPtr& traffic_light,
const lanelet::ConstLanelet& entry_lanelet,
const lanelet::ConstLanelet& exit_lanelet,
const lanelet::ConstLanelet& current_lanelet)
782 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"No lights found along route. Returning maneuver plan unchanged");
789 auto stop_line = traffic_light->getStopLine(entry_lanelet);
793 throw std::invalid_argument(
"Given entry lanelet doesn't have stop_line...");
796 double traffic_light_down_track =
797 wm_->routeTrackPos(stop_line.get().front().basicPoint2d()).downtrack;
799 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"traffic_light_down_track: " << traffic_light_down_track);
801 double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.
downtrack;
803 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"distance_remaining_to_traffic_light: " << distance_remaining_to_traffic_light <<
804 ", current_state.downtrack: " << current_state.
downtrack);
810 current_state_speed = std::min(speed_limit, current_state_speed);
812 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"speed_limit (free flow speed): " << speed_limit);
819 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Stopping distance: " << stopping_dist);
825 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"plugin_activation_distance: " << plugin_activation_distance);
827 if (distance_remaining_to_traffic_light <= plugin_activation_distance)
829 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Within intersection range");
835 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Not within intersection range");
841 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
const VehicleState& current_state,
842 const lanelet::CarmaTrafficSignalPtr& traffic_light,
const lanelet::ConstLanelet& entry_lanelet,
const lanelet::ConstLanelet& exit_lanelet,
const lanelet::ConstLanelet& current_lanelet)
855 auto stop_line = traffic_light->getStopLine(entry_lanelet);
859 throw std::invalid_argument(
"Given entry lanelet doesn't have stop_line...");
862 double traffic_light_down_track =
863 wm_->routeTrackPos(stop_line.get().front().basicPoint2d()).downtrack;
865 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"traffic_light_down_track: " << traffic_light_down_track);
867 double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.
downtrack;
871 if (distance_remaining_to_traffic_light < 0)
873 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Crossed the bar distance_remaining_to_traffic_light: " << distance_remaining_to_traffic_light);
880 lanelet::ConstLanelet intersection_lanelet;
882 auto route =
wm_->getRoute()->shortestPath();
883 auto entry_llt_iter = std::find(
route.begin(),
route.end(), entry_lanelet);
884 if (entry_llt_iter !=
route.end())
886 intersection_lanelet = *(entry_llt_iter + 1);
889 if (intersection_lanelet.id() != lanelet::InvalId)
903 current_state_speed = std::min(current_state_speed, speed_limit - 5 *
epsilon_);
905 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"current_state_speed: " << current_state_speed);
906 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"intersection_speed_: " <<
intersection_speed_.get());
907 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"distance_remaining_to_traffic_light: " << distance_remaining_to_traffic_light);
910 wm_->routeTrackPos(exit_lanelet.centerline2d().front().basicPoint2d()).downtrack;
912 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"intersection_end_downtrack_: " <<
intersection_end_downtrack_.get());
915 constexpr double HALF_MPH_IN_MPS = 0.22352;
916 if (current_state.
speed < HALF_MPH_IN_MPS &&
920 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"CARMA has detected that the vehicle stopped at the stop bar. Transitioning to WAITING STATE");
930 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"earliest_entry_time: " <<
std::to_string(earliest_entry_time.seconds()) <<
", with : " <<
std::to_string((earliest_entry_time - current_state.
stamp).seconds()) <<
" left at: " <<
std::to_string(current_state.
stamp.seconds()));
934 if (nearest_green_entry_time == rclcpp::Time(0))
937 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Final nearest_green_entry_time: " <<
std::to_string(nearest_green_entry_time.seconds()));
939 auto et_state = traffic_light->predictState(lanelet::time::timeFromSec(nearest_green_entry_time.seconds()));
940 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Signal at ET: " << et_state.get().second);
944 double remaining_time = nearest_green_entry_time.seconds() - current_state.
stamp.seconds();
945 double remaining_time_earliest_entry = earliest_entry_time.seconds() - current_state.
stamp.seconds();
952 auto boundary_traj_params =
get_boundary_traj_params(rclcpp::Time(req->header.stamp, RCL_SYSTEM_TIME).seconds(), current_state_speed,
intersection_speed_.get(), speed_limit,
config_.
algo_minimum_speed,
max_comfort_accel_,
max_comfort_decel_, current_state.
downtrack, traffic_light_down_track, distance_remaining_to_traffic_light, boundary_distances);
954 TrajectoryParams ts_params =
get_ts_case(rclcpp::Time(req->header.stamp, RCL_SYSTEM_TIME).seconds(), nearest_green_entry_time.seconds(), current_state_speed,
intersection_speed_.get(), speed_limit,
config_.
algo_minimum_speed,
max_comfort_accel_,
max_comfort_decel_, current_state.
downtrack, traffic_light_down_track, distance_remaining_to_traffic_light, boundary_distances, boundary_traj_params);
957 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"SPEED PROFILE CASE:" << ts_params.
case_num);
961 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"emergency_distance_to_stop at emergency_decel_norm_ (with stopping_location_buffer/2): " << emergency_distance_to_stop <<
", emergency_decel_norm_: " <<
emergency_decel_norm_);
964 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"safe_distance_to_stop at max_comfort_decel (with stopping_location_buffer/2): " << safe_distance_to_stop <<
", max_comfort_decel_norm_: " <<
max_comfort_decel_norm_);
973 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"new emergency_distance_to_stop: " << emergency_distance_to_stop);
974 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"new safe_distance_to_stop: " << safe_distance_to_stop);
975 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"new desired_distance_to_stop: " << desired_distance_to_stop);
977 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"distance_remaining_to_traffic_light: " << distance_remaining_to_traffic_light <<
", current_state.speed: " << current_state.
speed);
984 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"ET is still in TBD despite the vehicle being in desired distance to start stopping. Trying to handle this edge case gracefully...");
989 rclcpp::Time stopping_arrival_time =
990 current_state.
stamp + rclcpp::Duration::from_nanoseconds(stopping_time * 1e9);
992 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"stopping_arrival_time: " <<
std::to_string(stopping_arrival_time.seconds()));
994 auto stopping_arrival_state_optional = traffic_light->predictState(lanelet::time::timeFromSec(stopping_arrival_time.seconds()));
996 if (!
validLightState(stopping_arrival_state_optional, stopping_arrival_time))
998 RCLCPP_ERROR_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Unable to resolve give signal for stopping_arrival_state_optional: " <<
std::to_string(stopping_arrival_time.seconds()));
1004 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Detected possible RED light violation! Stopping!");
1005 handleStopping(req,resp, current_state, traffic_light, entry_lanelet, exit_lanelet, current_lanelet, traffic_light_down_track,
true);
1014 (distance_remaining_to_traffic_light >= desired_distance_to_stop || !in_tbd) &&
1015 is_entry_time_within_green_or_tbd)
1017 handleGreenSignalScenario(req, resp, current_state, current_state_speed, traffic_light, entry_lanelet, exit_lanelet, traffic_light_down_track, ts_params, in_tbd);
1019 if (!resp->new_plan.maneuvers.empty())
1025 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Not able to make it with certainty: TSCase: " << ts_params.
case_num <<
", changing it to 8");
1026 ts_params = boundary_traj_params[7];
1032 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Not able to make it with certainty: NEW TSCase: " << ts_params.
case_num);
1037 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Way too early to stop. Cruising at CASE8");
1038 handleCruisingUntilStop(req, resp, current_state, current_state_speed, traffic_light, traffic_light_down_track, ts_params);
1040 if (!resp->new_plan.maneuvers.empty())
1047 if ((safe_distance_to_stop <= distance_remaining_to_traffic_light && desired_distance_to_stop >= distance_remaining_to_traffic_light) ||
1050 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Planning stopping now. last case:" <<
static_cast<int>(
last_case_num_));
1051 handleStopping(req,resp, current_state, traffic_light, entry_lanelet, exit_lanelet, current_lanelet, traffic_light_down_track);
1055 if (safe_distance_to_stop > distance_remaining_to_traffic_light)
1057 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"No longer within safe distance to stop! Decide to continue stopping or continue into intersection");
1062 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"HANDLE_LAST_RESORT: The vehicle is not able to stop at red/yellow light nor is able to reach target speed at green. Attempting its best to pass through at green!");
1064 handleFailureCase(req, resp, current_state, current_state_speed, speed_limit, remaining_time,
1065 exit_lanelet.id(), traffic_light, traffic_light_down_track, ts_params);
1067 if (resp->new_plan.maneuvers.empty())
1069 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"HANDLE_SAFETY: Planning forced slow-down... last case:" <<
static_cast<int>(
last_case_num_));
1070 handleStopping(req,resp, current_state, traffic_light, entry_lanelet, exit_lanelet, current_lanelet, traffic_light_down_track,
true);
1075 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"HANDLE_SAFETY: Planning to keep stopping now. last case:" <<
static_cast<int>(
last_case_num_));
1076 handleStopping(req,resp, current_state, traffic_light, entry_lanelet, exit_lanelet, current_lanelet, traffic_light_down_track);
1084 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
const VehicleState& current_state,
1085 const lanelet::CarmaTrafficSignalPtr& traffic_light,
const lanelet::ConstLanelet& entry_lanelet,
const lanelet::ConstLanelet& exit_lanelet,
const lanelet::ConstLanelet& current_lanelet)
1091 throw std::invalid_argument(
"While in WAITING state, the traffic lights disappeared.");
1094 auto stop_line = traffic_light->getStopLine(entry_lanelet);
1098 throw std::invalid_argument(
"Given entry lanelet doesn't have stop_line...");
1101 double traffic_light_down_track =
1102 wm_->routeTrackPos(stop_line.get().front().basicPoint2d()).downtrack;
1104 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"traffic_light_down_track: "<< traffic_light_down_track);
1106 double entering_time = current_state.
stamp.seconds();
1108 auto current_light_state_optional = traffic_light->predictState(lanelet::time::timeFromSec(entering_time));
1110 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"WAITING STATE: requested time to rclcpp::Time(req->header.stamp, RCL_SYSTEM_TIME) check: " <<
std::to_string(rclcpp::Time(req->header.stamp, RCL_SYSTEM_TIME).seconds()));
1111 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"WAITING STATE: requested time to CURRENT STATE check: " <<
std::to_string(entering_time));
1113 if (!
validLightState(current_light_state_optional, rclcpp::Time(entering_time * 1e9)))
1118 if (!bool_optional_late_certainty)
1120 RCLCPP_ERROR_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Unable to resolve green light...");
1124 bool should_enter =
true;
1127 should_enter =
false;
1129 if (
isStateAllowedGreen(current_light_state_optional.get().second) && bool_optional_late_certainty.get() && should_enter)
1138 constexpr double stop_maneuver_buffer = 10.0;
1144 current_state.
downtrack - stop_maneuver_buffer, traffic_light_down_track, current_state.
speed,
1145 current_state.
lane_id, current_state.
lane_id, rclcpp::Time(entering_time * 1e9),
1150 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
const VehicleState& current_state,
1151 double intersection_end_downtrack,
double intersection_speed_limit)
1155 if (current_state.
downtrack > intersection_end_downtrack)
1162 rclcpp::Time intersection_exit_time =
1163 current_state.
stamp +
1164 rclcpp::Duration::from_nanoseconds(2.0 * (intersection_end_downtrack - current_state.
downtrack) / (current_state.
speed + intersection_speed_limit) * 1e9);
1167 std::vector<lanelet::ConstLanelet> crossed_lanelets =
1172 current_state.
downtrack, intersection_end_downtrack, current_state.
speed, intersection_speed_limit,
1173 current_state.
stamp, intersection_exit_time, crossed_lanelets.back().id(), crossed_lanelets.back().id()));
1182 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Received Schedule message with id: " << msg->m_header.plan_id);
1189 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"street_msg_timestamp_: " <<
street_msg_timestamp_);
1199 std::vector<uint8_t> bsm_id_vec = msg->core_data.id;
1200 bsm_id_ = BSMHelper::BSMHelper::bsmIDtoString(bsm_id_vec);
1208 std::string params = strategy_params;
1209 std::vector<std::string> inputsParams;
1212 std::vector<std::string> et_parsed;
1214 auto new_scheduled_enter_time = std::stoull(et_parsed[1]);
1220 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"scheduled_enter_time_: " <<
scheduled_enter_time_);
1226 carma_v2x_msgs::msg::MobilityOperation mo_;
1227 mo_.m_header.timestamp = now().nanoseconds()/1000000;
1230 mo_.m_header.sender_bsm_id =
bsm_id_;
1236 std::string intersection_turn_direction =
"straight";
1240 mo_.strategy_params =
"access: 1, max_accel: " +
std::to_string(vehicle_acceleration_limit_) +
1252 for (
auto l:lanelets_list)
1254 if(l.hasAttribute(
"turn_direction")) {
1255 std::string direction_attribute = l.attribute(
"turn_direction").value();
1256 if (direction_attribute ==
"right")
1261 else if (direction_attribute ==
"left")
1275 return turn_direction;
1290 std::shared_ptr<rmw_request_id_t> srv_header,
1291 carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
1292 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp)
1294 std::chrono::system_clock::time_point execution_start_time = std::chrono::system_clock::now();
1296 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"<<<<<<<<<<<<<<<<< STARTING LCI_STRATEGIC_PLAN!!!!!!!!! >>>>>>>>>>>>>>>>");
1298 if (!
wm_->getRoute())
1300 RCLCPP_ERROR_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Could not plan maneuvers as route was not available");
1308 resp->new_plan.maneuvers = {};
1309 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Not approaching light-controlled intersection so no maneuvers");
1314 if (is_empty_schedule_msg)
1316 resp->new_plan.maneuvers = {};
1317 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Receiving empty schedule message");
1322 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Finding car information");
1328 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"State is NOT UNAVAILABLE AND Maneuvers in request is NOT empty");
1332 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"\n\nFinding traffic_light information");
1334 auto inter_list =
wm_->getSignalizedIntersectionsAlongRoute({ req->veh_x, req->veh_y });
1337 auto traffic_list =
wm_->getSignalsAlongRoute({ req->veh_x, req->veh_y });
1339 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Found traffic lights of size: " << traffic_list.size());
1341 auto current_lanelets =
wm_->getLaneletsFromPoint({ req->veh_x, req->veh_y});
1342 lanelet::ConstLanelet current_lanelet;
1344 if (current_lanelets.empty())
1346 RCLCPP_ERROR_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Given vehicle position is not on the road! Returning...");
1351 auto llt_on_route_optional =
wm_->getFirstLaneletOnShortestPath(current_lanelets);
1353 if (llt_on_route_optional){
1354 current_lanelet = llt_on_route_optional.value();
1357 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"When identifying the corresponding lanelet for requested maneuever's state, x: "
1358 << req->veh_x <<
", y: " << req->veh_y <<
", no possible lanelet was found to be on the shortest path."
1359 <<
"Picking arbitrary lanelet: " << current_lanelets[0].id() <<
", instead");
1360 current_lanelet = current_lanelets[0];
1363 lanelet::CarmaTrafficSignalPtr nearest_traffic_signal =
nullptr;
1365 lanelet::ConstLanelet entry_lanelet;
1366 lanelet::ConstLanelet exit_lanelet;
1368 for (
auto signal : traffic_list)
1370 auto optional_entry_exit =
wm_->getEntryExitOfSignalAlongRoute(signal);
1372 if (!optional_entry_exit)
1374 RCLCPP_ERROR_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Did not find entry.exit along the route");
1378 entry_lanelet = optional_entry_exit.get().first;
1379 exit_lanelet = optional_entry_exit.get().second;
1381 nearest_traffic_signal = signal;
1392 resp->new_plan = carma_planning_msgs::msg::ManeuverPlan();
1398 boost::optional<std::pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState>> current_light_state_optional = boost::none;
1399 if (nearest_traffic_signal)
1401 current_light_state_optional = nearest_traffic_signal->predictState(lanelet::time::timeFromSec(current_state.
stamp.seconds()));
1402 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Real-time current signal: " << current_light_state_optional.get().second <<
", for Id: " << nearest_traffic_signal->id());
1405 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Signal state not available returning..." );
1412 planWhenUNAVAILABLE(req, resp, current_state, nearest_traffic_signal, entry_lanelet, exit_lanelet, current_lanelet);
1416 planWhenAPPROACHING(req, resp, current_state, nearest_traffic_signal, entry_lanelet, exit_lanelet, current_lanelet);
1420 planWhenWAITING(req, resp, current_state, nearest_traffic_signal, entry_lanelet, exit_lanelet, current_lanelet);
1427 throw std::invalid_argument(
"State is DEPARTING but the intersection variables are not available");
1433 throw std::invalid_argument(
"LCIStrategic Strategic Plugin entered unknown state");
1439 std::chrono::system_clock::time_point execution_end_time = std::chrono::system_clock::now();
1441 auto execution_duration = execution_end_time - execution_start_time;
1442 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"ExecutionTime lci_strategic_plugin: " << std::chrono::duration<double>(execution_duration).count());
1449 double target_speed, rclcpp::Time start_time, rclcpp::Time end_time,
1452 carma_planning_msgs::msg::Maneuver maneuver_msg;
1453 maneuver_msg.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING;
1454 maneuver_msg.lane_following_maneuver.parameters.negotiation_type =
1455 carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
1456 maneuver_msg.lane_following_maneuver.parameters.presence_vector =
1457 carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN | carma_planning_msgs::msg::ManeuverParameters::HAS_FLOAT_META_DATA | carma_planning_msgs::msg::ManeuverParameters::HAS_INT_META_DATA;
1458 maneuver_msg.lane_following_maneuver.parameters.planning_tactical_plugin =
1460 maneuver_msg.lane_following_maneuver.parameters.planning_strategic_plugin =
1462 maneuver_msg.lane_following_maneuver.start_dist = start_dist;
1463 maneuver_msg.lane_following_maneuver.start_speed = start_speed;
1464 maneuver_msg.lane_following_maneuver.start_time = start_time;
1465 maneuver_msg.lane_following_maneuver.end_dist = end_dist;
1466 maneuver_msg.lane_following_maneuver.end_speed = target_speed;
1467 maneuver_msg.lane_following_maneuver.end_time = end_time;
1471 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.
a1_);
1472 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.
v1_);
1473 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.
x1_);
1475 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.
a2_);
1476 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.
v2_);
1477 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.
x2_);
1479 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.
a3_);
1480 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.
v3_);
1481 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.
x3_);
1483 maneuver_msg.lane_following_maneuver.parameters.int_valued_meta_data.push_back(
static_cast<int>(tsp.
case_num));
1484 maneuver_msg.lane_following_maneuver.parameters.int_valued_meta_data.push_back(
static_cast<int>(tsp.
is_algorithm_successful));
1486 for (
auto llt : crossed_lanelets)
1488 maneuver_msg.lane_following_maneuver.lane_ids.push_back(
std::to_string(llt.id()));
1492 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Creating TrajectorySmoothingManeuver start dist: " << start_dist <<
" end dist: " << end_dist
1496 return maneuver_msg;
1501 const lanelet::Id& starting_lane_id,
1502 const lanelet::Id& ending_lane_id,
1503 rclcpp::Time start_time, rclcpp::Time end_time,
double stopping_accel)
const
1505 carma_planning_msgs::msg::Maneuver maneuver_msg;
1506 maneuver_msg.type = carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT;
1507 maneuver_msg.stop_and_wait_maneuver.parameters.negotiation_type = carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
1508 maneuver_msg.stop_and_wait_maneuver.parameters.presence_vector =
1509 carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN | carma_planning_msgs::msg::ManeuverParameters::HAS_FLOAT_META_DATA;
1512 maneuver_msg.stop_and_wait_maneuver.start_dist = current_dist;
1513 maneuver_msg.stop_and_wait_maneuver.end_dist = end_dist;
1514 maneuver_msg.stop_and_wait_maneuver.start_speed = start_speed;
1515 maneuver_msg.stop_and_wait_maneuver.start_time = start_time;
1516 maneuver_msg.stop_and_wait_maneuver.end_time = end_time;
1517 maneuver_msg.stop_and_wait_maneuver.starting_lane_id =
std::to_string(starting_lane_id);
1518 maneuver_msg.stop_and_wait_maneuver.ending_lane_id =
std::to_string(ending_lane_id);
1522 maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(stopping_accel);
1524 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Creating stop and wait start dist: " << current_dist <<
" end dist: " << end_dist
1527 <<
" stopping_accel: " << stopping_accel);
1529 return maneuver_msg;
1533 double start_speed,
double target_speed,
1534 rclcpp::Time start_time, rclcpp::Time end_time,
1535 const lanelet::Id& starting_lane_id,
1536 const lanelet::Id& ending_lane_id)
const
1538 carma_planning_msgs::msg::Maneuver maneuver_msg;
1539 maneuver_msg.type = carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT;
1540 maneuver_msg.intersection_transit_straight_maneuver.parameters.negotiation_type =
1541 carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
1542 maneuver_msg.intersection_transit_straight_maneuver.parameters.presence_vector =
1543 carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN;
1544 maneuver_msg.intersection_transit_straight_maneuver.parameters.planning_tactical_plugin =
1546 maneuver_msg.intersection_transit_straight_maneuver.parameters.planning_strategic_plugin =
1548 maneuver_msg.intersection_transit_straight_maneuver.start_dist = start_dist;
1549 maneuver_msg.intersection_transit_straight_maneuver.start_speed = start_speed;
1550 maneuver_msg.intersection_transit_straight_maneuver.start_time = start_time;
1551 maneuver_msg.intersection_transit_straight_maneuver.end_dist = end_dist;
1552 maneuver_msg.intersection_transit_straight_maneuver.end_speed = target_speed;
1553 maneuver_msg.intersection_transit_straight_maneuver.end_time = end_time;
1554 maneuver_msg.intersection_transit_straight_maneuver.starting_lane_id =
std::to_string(starting_lane_id);
1555 maneuver_msg.intersection_transit_straight_maneuver.ending_lane_id =
std::to_string(ending_lane_id);
1559 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Creating IntersectionTransitManeuver start dist: " << start_dist <<
" end dist: " << end_dist
1560 <<
" From lanelet: " << starting_lane_id
1561 <<
" to lanelet: " << ending_lane_id
1565 return maneuver_msg;
1581#include "rclcpp_components/register_node_macro.hpp"
virtual carma_wm::WorldModelConstPtr get_world_model() final
Method to return the default world model provided as a convience by this base class If this method or...
virtual std::shared_ptr< carma_wm::WMListener > get_world_model_listener() final
Method to return the default world model listener provided as a convience by this base class If this ...
void handleStopping(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp, const VehicleState ¤t_state, const lanelet::CarmaTrafficSignalPtr &traffic_light, const lanelet::ConstLanelet &entry_lanelet, const lanelet::ConstLanelet &exit_lanelet, const lanelet::ConstLanelet ¤t_lanelet, double traffic_light_down_track, bool is_emergency=false)
This function returns stopping maneuver if the vehicle is able to stop at red and in safe stopping di...
void handleGreenSignalScenario(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp, const VehicleState ¤t_state, double current_state_speed, const lanelet::CarmaTrafficSignalPtr &traffic_light, const lanelet::ConstLanelet &entry_lanelet, const lanelet::ConstLanelet &exit_lanelet, double traffic_light_down_track, const TrajectoryParams &ts_params, bool is_certainty_check_optional)
This function returns valid maneuvers if the vehicle is able to utilize trajectory smoothing paramete...
void lookupFrontBumperTransform()
Lookup transfrom from front bumper to base link.
LCIStrategicPluginConfig config_
Config containing configurable algorithm parameters.
boost::optional< bool > canArriveAtGreenWithCertainty(const rclcpp::Time &light_arrival_time_by_algo, const lanelet::CarmaTrafficSignalPtr &traffic_light, bool check_late, bool check_early) const
Return true if the car can arrive at given arrival time within green light time buffer.
BoundaryDistances get_delta_x(double v0, double v1, double v_max, double v_min, double a_max, double a_min)
Get boundary distances used to compare against current state in order to create trajectory smoothing ...
carma_ros2_utils::CallbackReturn on_activate_plugin()
Method which is triggered when this plugin is moved from the INACTIVE to ACTIVE states....
bool validLightState(const boost::optional< std::pair< boost::posix_time::ptime, lanelet::CarmaTrafficSignalState > > &optional_state, const rclcpp::Time &source_time) const
Helper method that checks both if the input optional light state is set and if the state it contains ...
TrajectoryParams handleFailureCaseHelper(const lanelet::CarmaTrafficSignalPtr &traffic_light, double current_time, double starting_speed, double departure_speed, double speed_limit, double remaining_downtrack, double traffic_light_downtrack)
Helper function to handleFailureCase that modifies ts_params to desired new parameters....
VehicleState extractInitialState(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req) const
Helper method to extract the initial vehicle state from the planning request method based on if the p...
boost::optional< rclcpp::Time > nearest_green_entry_time_cached_
carma_wm::WorldModelConstPtr wm_
World Model pointer.
void mobilityOperationCb(carma_v2x_msgs::msg::MobilityOperation::UniquePtr msg)
callback function for mobility operation
carma_ros2_utils::PubPtr< std_msgs::msg::Float64 > tf_distance_pub_
double emergency_decel_norm_
unsigned long long scheduled_enter_time_
double findSpeedLimit(const lanelet::ConstLanelet &llt) const
Given a Lanelet, find it's associated Speed Limit.
carma_planning_msgs::msg::Maneuver composeTrajectorySmoothingManeuverMessage(double start_dist, double end_dist, const std::vector< lanelet::ConstLanelet > &crossed_lanelets, double start_speed, double target_speed, rclcpp::Time start_time, rclcpp::Time end_time, const TrajectoryParams &tsp) const
Compose a trajectory smoothing maneuver msg (sent as transit maneuver message)
rclcpp::Duration get_earliest_entry_time(double remaining_distance, double free_flow_speed, double current_speed, double departure_speed, double max_accel, double max_decel) const
Gets the earliest entry time into the intersection that is kinematically possible for the vehicle....
double max_comfort_decel_norm_
void planWhenDEPARTING(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp, const VehicleState ¤t_state, double intersection_end_downtrack, double intersection_speed_limit)
Method for performing maneuver planning when the current plugin state is TransitState::DEPARTING Ther...
carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::MobilityOperation > mob_operation_sub_
carma_ros2_utils::CallbackReturn on_configure_plugin()
Method which is triggered when this plugin is moved from the UNCONFIGURED to INACTIVE states....
carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::BSM > bsm_sub_
carma_ros2_utils::PubPtr< std_msgs::msg::Int8 > case_pub_
bool get_availability()
Get the availability status of this plugin based on the current operating environment....
void handleFailureCase(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp, const VehicleState ¤t_state, double current_state_speed, double speed_limit, double remaining_time, lanelet::Id exit_lanelet_id, const lanelet::CarmaTrafficSignalPtr &traffic_light, double traffic_light_down_track, const TrajectoryParams &ts_params)
When the vehicle is not able to successfully run the algorithm or not able to stop,...
std::tuple< rclcpp::Time, bool, bool > get_final_entry_time_and_conditions(const VehicleState ¤t_state, const rclcpp::Time &earliest_entry_time, lanelet::CarmaTrafficSignalPtr traffic_light)
Get the final entry time from either vehicle's own internal ET calculation (TSMO UC2) or from carma-s...
unsigned long long street_msg_timestamp_
double length_to_front_bumper_
double max_comfort_accel_
LCIStrategicPlugin(const rclcpp::NodeOptions &)
Default constructor for RouteFollowingPlugin class.
std::vector< lanelet::ConstLanelet > getLaneletsBetweenWithException(double start_downtrack, double end_downtrack, bool shortest_path_only=false, bool bounds_inclusive=true) const
Helper method which calls carma_wm::WorldModel::getLaneletsBetween(start_downtrack,...
void planWhenWAITING(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp, const VehicleState ¤t_state, const lanelet::CarmaTrafficSignalPtr &traffic_light, const lanelet::ConstLanelet &entry_lanelet, const lanelet::ConstLanelet &exit_lanelet, const lanelet::ConstLanelet ¤t_lanelet)
Method for performing maneuver planning when the current plugin state is TransitState::WAITING Theref...
void BSMCb(carma_v2x_msgs::msg::BSM::UniquePtr msg)
BSM callback function.
double distance_remaining_to_tf_
void print_params(TrajectoryParams params)
Helper method to print TrajectoryParams.
void parseStrategyParams(const std::string &strategy_params)
parse strategy parameters
carma_v2x_msgs::msg::MobilityOperation generateMobilityOperation()
Generates Mobility Operation messages.
carma_ros2_utils::PubPtr< std_msgs::msg::Float64 > earliest_et_pub_
TrajectoryParams get_ts_case(double t, double et, double v0, double v1, double v_max, double v_min, double a_max, double a_min, double x0, double x_end, double dx, BoundaryDistances boundary_distances, std::vector< TrajectoryParams > params)
std::shared_ptr< tf2_ros::Buffer > tf2_buffer_
void planWhenUNAVAILABLE(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp, const VehicleState ¤t_state, const lanelet::CarmaTrafficSignalPtr &traffic_light, const lanelet::ConstLanelet &entry_lanelet, const lanelet::ConstLanelet &exit_lanelet, const lanelet::ConstLanelet ¤t_lanelet)
Method for performing maneuver planning when the current plugin state is TransitState::UNAVAILABLE Th...
double max_comfort_decel_
void publishTrajectorySmoothingInfo()
Publish trajectory smoothing info at a fixed rate.
carma_ros2_utils::PubPtr< carma_v2x_msgs::msg::MobilityOperation > mobility_operation_pub_
TurnDirection intersection_turn_direction_
std::string previous_strategy_params_
bool approaching_light_controlled_intersection_
carma_planning_msgs::msg::Maneuver composeStopAndWaitManeuverMessage(double current_dist, double end_dist, double start_speed, const lanelet::Id &starting_lane_id, const lanelet::Id &ending_lane_id, rclcpp::Time start_time, rclcpp::Time end_time, double stopping_accel) const
Compose a stop and wait maneuver message based on input params.
std::string light_controlled_intersection_strategy_
std::string get_version_id()
Returns the version id of this plugin.
void handleCruisingUntilStop(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp, const VehicleState ¤t_state, double current_state_speed, const lanelet::CarmaTrafficSignalPtr &traffic_light, double traffic_light_down_track, const TrajectoryParams &ts_params)
This function returns valid maneuvers if the vehicle is able to utilize trajectory smoothing paramete...
boost::optional< double > intersection_end_downtrack_
double estimate_distance_to_stop(double v, double a) const
Helper method to use basic kinematics to compute an estimated stopping distance from from the inputs.
double earliest_entry_time_
bool isStateAllowedGreen(const lanelet::CarmaTrafficSignalState &state) const
Method to check if the state is an allowed green movement state. Currently Permissive and Protected g...
TurnDirection getTurnDirectionAtIntersection(std::vector< lanelet::ConstLanelet > lanelets_list)
Determine the turn direction at intersection.
void plan_maneuvers_callback(std::shared_ptr< rmw_request_id_t > srv_header, carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp)
Service callback for arbitrator maneuver planning.
double scheduled_entry_time_
void planWhenAPPROACHING(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp, const VehicleState ¤t_state, const lanelet::CarmaTrafficSignalPtr &traffic_light, const lanelet::ConstLanelet &entry_lanelet, const lanelet::ConstLanelet &exit_lanelet, const lanelet::ConstLanelet ¤t_lanelet)
Method for performing maneuver planning when the current plugin state is TransitState::APPROACHING Th...
LCIStrategicStateTransitionTable transition_table_
State Machine Transition table.
std::vector< TrajectoryParams > get_boundary_traj_params(double t, double v0, double v1, double v_max, double v_min, double a_max, double a_min, double x0, double x_end, double dx, BoundaryDistances boundary_distances)
Get all possible trajectory smoothing parameters for each segments. Later this will be used to genera...
TSCase last_case_num_
Useful metrics for LCI Plugin.
void print_boundary_distances(BoundaryDistances delta_xs)
Helper method to print TrajectoryParams.
std::shared_ptr< tf2_ros::TransformListener > tf2_listener_
boost::optional< double > intersection_speed_
Cache variables for storing the current intersection state between state machine transitions.
void publishMobilityOperation()
Publish mobility operation at a fixed rate.
rcl_interfaces::msg::SetParametersResult parameter_update_callback(const std::vector< rclcpp::Parameter > ¶meters)
Callback for dynamic parameter updates.
bool supportedLightState(lanelet::CarmaTrafficSignalState state) const
Helper method to evaluate if the given traffic light state is supported by this plugin.
rclcpp::TimerBase::SharedPtr ts_info_pub_timer_
carma_ros2_utils::PubPtr< std_msgs::msg::Float64 > et_pub_
rclcpp::TimerBase::SharedPtr mob_op_pub_timer_
carma_planning_msgs::msg::Maneuver composeIntersectionTransitMessage(double start_dist, double end_dist, double start_speed, double target_speed, rclcpp::Time start_time, rclcpp::Time end_time, const lanelet::Id &starting_lane_id, const lanelet::Id &ending_lane_id) const
Compose a intersection transit maneuver message based on input params.
TransitState getState() const
Returns the current state.
void signal(TransitEvent signal)
Trigger signal for the transition table.
#define GET_MANEUVER_PROPERTY(mvr, property)
auto to_string(const UtmZone &zone) -> std::string
TransitState
Enum describing the possible states of the LCIStrategic Strategic Plugin.
Struct to store the configuration settings for the LCIStrategicPlugin class.
std::string intersection_transit_plugin_name
bool enable_carma_streets_connection
Bool: Enable carma streets connection.
std::string vehicle_id
License plate of the vehicle.
double trajectory_smoothing_activation_distance
Downtrack distance until nearest intersection where the Trajectory Smoothing algorithm should activat...
double algo_minimum_speed
Minimum allowable speed TS algorithm in m/s.
double vehicle_accel_limit_multiplier
A multiplier to apply to the maximum allowable vehicle acceleration limit so we plan under our capabi...
double min_maneuver_planning_period
The minimum period in seconds which a maneuver plan must cover if the plugin wishes to control the wh...
double green_light_time_buffer
A buffer in seconds around the green phase which will reduce the phase length such that vehicle still...
std::string stop_and_wait_plugin_name
The name of the plugin to use for stop and wait trajectory planning.
double stopping_location_buffer
A buffer infront of the stopping location which will still be considered a valid stop....
double vehicle_response_lag
An approximation of the delay (sec) between sent vehicle commands and the vehicle begining a meaningf...
double reaction_time
Double: Vehicle reaction time to a received schedule in seconds (approximate value,...
std::string lane_following_plugin_name
double vehicle_decel_limit
The maximum allowable vehicle deceleration limit in m/s.
double vehicle_decel_limit_multiplier
A multiplier to apply to the maximum allowable vehicle deceleration limit so we plan under our capabi...
double min_approach_distance
The minimum distance in meters that the vehicle can be at before requiring a transition to the APPROA...
int enable_carma_wm_spat_processing
Int: 0 to turn off and 1 to turn on.
double vehicle_accel_limit
The maximum allowable vehicle acceleration limit in m/s.
std::string strategic_plugin_name
The name to use for this plugin during comminications with the arbitrator.
double deceleration_fraction
double mobility_rate
Double: Mobility operation rate.
double desired_distance_to_stop_buffer
Double: Desired distance to stop buffer in meters.
double min_gap
Double: Minimum inter-vehicle gap.
Struct representing a vehicle state for the purposes of planning.
bool is_algorithm_successful
double modified_remaining_time
double modified_departure_speed