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;
109 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Done loading parameters: " <<
config_);
114 mob_operation_sub_ = create_subscription<carma_v2x_msgs::msg::MobilityOperation>(
"incoming_mobility_operation", 1,
118 bsm_sub_ = create_subscription<carma_v2x_msgs::msg::BSM>(
"bsm_outbound", 1,
125 mobility_operation_pub_ = create_publisher<carma_v2x_msgs::msg::MobilityOperation>(
"outgoing_mobility_operation", 1);
126 case_pub_ = create_publisher<std_msgs::msg::Int8>(
"lci_strategic_plugin/ts_case_num", 1);
127 tf_distance_pub_ = create_publisher<std_msgs::msg::Float64>(
"lci_strategic_plugin/distance_remaining_to_tf", 1);
128 earliest_et_pub_ = create_publisher<std_msgs::msg::Float64>(
"lci_strategic_plugin/earliest_entry_time", 1);
129 et_pub_ = create_publisher<std_msgs::msg::Float64>(
"lci_strategic_plugin/scheduled_entry_time", 1);
132 return CallbackReturn::SUCCESS;
137 auto error_double = update_params<double>({
153 rcl_interfaces::msg::SetParametersResult result;
155 result.successful = !error_double;
162 std_msgs::msg::Int8 case_num_msg;
163 std_msgs::msg::Float64 tf_distance;
164 std_msgs::msg::Float64 earliest_et;
165 std_msgs::msg::Float64 scheduled_et;
175 et_pub_->publish(scheduled_et);
185 std::chrono::duration<double>(0.5),
188 return CallbackReturn::SUCCESS;
197 case lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN:
198 case lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE:
199 case lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED:
204 case lanelet::CarmaTrafficSignalState::PERMISSIVE_MOVEMENT_ALLOWED:
205 case lanelet::CarmaTrafficSignalState::PERMISSIVE_CLEARANCE:
208 case lanelet::CarmaTrafficSignalState::DARK:
209 case lanelet::CarmaTrafficSignalState::STOP_THEN_PROCEED:
211 case lanelet::CarmaTrafficSignalState::PRE_MOVEMENT:
213 case lanelet::CarmaTrafficSignalState::CAUTION_CONFLICTING_TRAFFIC:
225 geometry_msgs::msg::TransformStamped
tf2 =
tf2_buffer_.lookupTransform(
"base_link",
"vehicle_front", rclcpp::Time(0), rclcpp::Duration(20.0 * 1e9));
227 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"length_to_front_bumper_: " <<
length_to_front_bumper_);
230 catch (
const tf2::TransformException &ex)
232 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"), ex.what());
239 if (!req->prior_plan.maneuvers.empty())
241 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Provided with initial plan...");
249 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"No initial plan provided...");
251 state.
stamp = rclcpp::Time(req->header.stamp, RCL_SYSTEM_TIME);
253 state.
speed = req->veh_logitudinal_velocity;
254 state.
lane_id = stoi(req->veh_lane_id);
256 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"extractInitialState >>>> state.stamp: " <<
std::to_string(state.
stamp.seconds()));
257 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"extractInitialState >>>> state.downtrack : " << state.
downtrack );
258 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"extractInitialState >>>> state.speed: " << state.
speed);
259 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"extractInitialState >>>> state.lane_id: " << state.
lane_id);
266 lanelet::Optional<carma_wm::TrafficRulesConstPtr> traffic_rules =
wm_->getTrafficRules();
269 return (*traffic_rules)->speedLimit(llt).speedLimit.value();
273 throw std::invalid_argument(
"Valid traffic rules object could not be built");
278 const rclcpp::Time& source_time)
const
282 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Traffic light data not available for source_time " <<
std::to_string(source_time.seconds()));
286 lanelet::CarmaTrafficSignalState light_state = optional_state.get().second;
290 RCLCPP_ERROR_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"LCIStrategic Plugin asked to handle CarmaTrafficSignalState: " << light_state
291 <<
" which is not supported.");
300 rclcpp::Time early_arrival_time_by_algo =
303 rclcpp::Time late_arrival_time_by_algo =
306 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"light_arrival_time_by_algo: " <<
std::to_string(light_arrival_time_by_algo.seconds()));
307 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"early_arrival_time_by_algo: " <<
std::to_string(early_arrival_time_by_algo.seconds()));
308 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"late_arrival_time_by_algo: " <<
std::to_string(late_arrival_time_by_algo.seconds()));
310 auto early_arrival_state_by_algo_optional = traffic_light->predictState(lanelet::time::timeFromSec(early_arrival_time_by_algo.seconds()));
312 if (!
validLightState(early_arrival_state_by_algo_optional, early_arrival_time_by_algo))
315 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"early_arrival_state_by_algo: " << early_arrival_state_by_algo_optional.get().second);
317 auto late_arrival_state_by_algo_optional = traffic_light->predictState(lanelet::time::timeFromSec(late_arrival_time_by_algo.seconds()));
319 if (!
validLightState(late_arrival_state_by_algo_optional, late_arrival_time_by_algo))
322 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"late_arrival_state_by_algo: " << late_arrival_state_by_algo_optional.get().second);
324 bool can_make_early_arrival =
true;
325 bool can_make_late_arrival =
true;
328 can_make_early_arrival = (early_arrival_state_by_algo_optional.get().second == lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED);
331 can_make_late_arrival = (late_arrival_state_by_algo_optional.get().second == lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED);
334 if (can_make_early_arrival && can_make_late_arrival)
342 double end_downtrack,
343 bool shortest_path_only,
344 bool bounds_inclusive)
const
346 std::vector<lanelet::ConstLanelet> crossed_lanelets =
347 wm_->getLaneletsBetween(start_downtrack, end_downtrack, shortest_path_only, bounds_inclusive);
349 if (crossed_lanelets.size() == 0)
351 throw std::invalid_argument(
"getLaneletsBetweenWithException called but inputs do not cross any lanelets going "
356 return crossed_lanelets;
360 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
362 const lanelet::CarmaTrafficSignalPtr& traffic_light,
363 const lanelet::ConstLanelet& entry_lanelet,
const lanelet::ConstLanelet& exit_lanelet,
const lanelet::ConstLanelet& current_lanelet,
364 double traffic_light_down_track,
367 double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.
downtrack;
370 std::vector<lanelet::ConstLanelet> crossed_lanelets =
385 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"HANDLE_STOPPING: Planning stop and wait maneuver at decel_rate: " << decel_rate);
388 current_state.
downtrack, traffic_light_down_track, current_state.
speed, crossed_lanelets.front().id(),
389 crossed_lanelets.back().id(), current_state.
stamp,
395 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
397 double current_state_speed,
399 double remaining_time,
400 lanelet::Id exit_lanelet_id,
401 const lanelet::CarmaTrafficSignalPtr& traffic_light,
404 double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.
downtrack;
407 std::vector<lanelet::ConstLanelet> crossed_lanelets =
412 if (incomplete_traj_params.is_algorithm_successful ==
false)
414 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Failed to generate maneuver for edge cases...");
419 current_state_speed, incomplete_traj_params.modified_departure_speed, current_state.
stamp, current_state.
stamp + rclcpp::Duration(incomplete_traj_params.modified_remaining_time * 1e9), incomplete_traj_params));
423 rclcpp::Time intersection_exit_time =
424 current_state.
stamp + rclcpp::Duration(incomplete_traj_params.modified_remaining_time * 1e9) + rclcpp::Duration(intersection_length / incomplete_traj_params.modified_departure_speed * 1e9);
428 incomplete_traj_params.modified_departure_speed, current_state.
stamp + rclcpp::Duration(incomplete_traj_params.modified_remaining_time * 1e9), intersection_exit_time, crossed_lanelets.back().id(), crossed_lanelets.back().id()));
434 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
436 double current_state_speed,
437 const lanelet::CarmaTrafficSignalPtr& traffic_light,
442 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"handleCruisingUntilStop is called but it is not case_8");
446 auto new_ts_params = ts_params;
448 double decel_rate = std::fabs(ts_params.
a3_);
450 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"CASE_8: Planning cruise and stop with decel_rate: " << decel_rate);
452 new_ts_params.t3_ = new_ts_params.t2_;
453 new_ts_params.x3_ = new_ts_params.x2_;
454 new_ts_params.v3_ = new_ts_params.v2_;
455 new_ts_params.a3_ = new_ts_params.a2_;
458 std::vector<lanelet::ConstLanelet> lane_follow_crossed_lanelets =
462 current_state_speed, new_ts_params.v2_, current_state.
stamp, rclcpp::Time(new_ts_params.t2_ * 1e9), new_ts_params));
465 std::vector<lanelet::ConstLanelet> case_8_crossed_lanelets =
469 new_ts_params.x2_, traffic_light_down_track, new_ts_params.v2_, case_8_crossed_lanelets.front().id(),
470 case_8_crossed_lanelets.back().id(), rclcpp::Time(new_ts_params.t2_ * 1e9),
477 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
479 double current_state_speed,
480 const lanelet::CarmaTrafficSignalPtr& traffic_light,
481 const lanelet::ConstLanelet& entry_lanelet,
const lanelet::ConstLanelet& exit_lanelet,
482 double traffic_light_down_track,
const TrajectoryParams& ts_params,
bool is_certainty_check_optional)
489 rclcpp::Time light_arrival_time_by_algo = rclcpp::Time(ts_params.
t3_ * 1e9);
490 double remaining_time = light_arrival_time_by_algo.seconds() - rclcpp::Time(req->header.stamp, RCL_SYSTEM_TIME).seconds();
491 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));
495 std::vector<lanelet::ConstLanelet> crossed_lanelets =
499 if (!can_make_green_optional)
502 if (can_make_green_optional.get() || is_certainty_check_optional)
504 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");
507 current_state_speed, ts_params.
v3_, current_state.
stamp, light_arrival_time_by_algo, ts_params));
511 rclcpp::Time intersection_exit_time =
512 light_arrival_time_by_algo + rclcpp::Duration(intersection_length /
intersection_speed_.get() * 1e9);
516 intersection_speed_.get(), light_arrival_time_by_algo, intersection_exit_time, crossed_lanelets.back().id(), crossed_lanelets.back().id()));
529 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"HANDLE_LAST_RESORT_CASE: Starting...");
530 double starting_downtrack = traffic_light_downtrack - remaining_downtrack;
531 double modified_remaining_time_upper;
532 double modified_remaining_time_lower;
533 double modified_departure_speed_upper;
534 double modified_departure_speed_lower;
535 bool calculation_success_upper =
true;
539 traj_upper.
t0_ = current_time;
540 traj_upper.
v0_ = starting_speed;
541 traj_upper.
x0_ = starting_downtrack;
545 if (departure_speed >= starting_speed)
547 if ((pow(departure_speed,2) - pow(starting_speed,2))/(2*
max_comfort_accel_) >= remaining_downtrack)
549 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"HandleFailureCase -> Upper Trajectory -> Current Speed <= Desired Departure Speed, Actual Departure Speed < Desired Departure Speed");
551 modified_departure_speed_upper = sqrt(pow(starting_speed, 2) + (2 *
max_comfort_accel_ * remaining_downtrack));
552 modified_remaining_time_upper = (modified_departure_speed_upper - starting_speed) /
max_comfort_accel_;
554 traj_upper.
t1_ = current_time + modified_remaining_time_upper;
555 traj_upper.
v1_ = modified_departure_speed_upper;
557 traj_upper.
x1_ = traffic_light_downtrack;
559 traj_upper.
t2_ = traj_upper.
t1_;
560 traj_upper.
v2_ = traj_upper.
v1_;
561 traj_upper.
a2_ = traj_upper.
a1_;
562 traj_upper.
x2_ = traj_upper.
x1_;
569 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"HandleFailureCase -> Upper Trajectory -> Current Speed < Desired Departure Speed, Actual Departure Speed = Desired Departure Speed");
571 double cruising_distance = remaining_downtrack - (pow(departure_speed, 2) - pow(starting_speed, 2))/ ( 2 *
max_comfort_accel_);
572 if (cruising_distance < -
EPSILON)
574 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Detected calculation failure in upper case 2");
575 calculation_success_upper =
false;
577 modified_remaining_time_upper = ((departure_speed - starting_speed) /
max_comfort_accel_) + (cruising_distance / departure_speed);
580 traj_upper.
v1_ = departure_speed;
582 traj_upper.
x1_ = starting_downtrack + (pow(departure_speed, 2) - pow(starting_speed, 2)) / (2 *
max_comfort_accel_);
584 traj_upper.
t2_ = current_time + modified_remaining_time_upper;
585 traj_upper.
v2_ = departure_speed;
587 traj_upper.
x2_ = traffic_light_downtrack;
594 if (departure_speed < starting_speed)
596 if ((pow(departure_speed,2) - pow(starting_speed,2))/(2*
max_comfort_decel_) >= remaining_downtrack)
598 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"HandleFailureCase -> Upper Trajectory -> Current Speed > Desired Departure Speed, Actual Departure Speed > Desired Departure Speed");
600 modified_departure_speed_upper = sqrt(pow(starting_speed, 2) + (2 *
max_comfort_decel_ * remaining_downtrack));
601 modified_remaining_time_upper = (modified_departure_speed_upper - starting_speed) /
max_comfort_decel_;
603 traj_upper.
t1_ = current_time + modified_remaining_time_upper;
604 traj_upper.
v1_ = modified_departure_speed_upper;
606 traj_upper.
x1_ = traffic_light_downtrack;
608 traj_upper.
t2_ = traj_upper.
t1_;
609 traj_upper.
v2_ = traj_upper.
v1_;
610 traj_upper.
a2_ = traj_upper.
a1_;
611 traj_upper.
x2_ = traj_upper.
x1_;
618 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"HandleFailureCase -> Upper Trajectory -> Current Speed > Desired Departure Speed, Actual Departure Speed = Desired Departure Speed");
620 double cruising_distance = remaining_downtrack - (pow(departure_speed, 2) - pow(starting_speed, 2))/ ( 2 *
max_comfort_decel_);
622 if (cruising_distance < -
EPSILON)
624 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Detected calculation failure in upper case 4");
625 calculation_success_upper =
false;
628 modified_remaining_time_upper = cruising_distance / starting_speed + (departure_speed - starting_speed) /
max_comfort_decel_ ;
630 traj_upper.
t1_ = current_time + cruising_distance / starting_speed;
631 traj_upper.
v1_ = starting_speed;
632 traj_upper.
a1_ = 0.0;
633 traj_upper.
x1_ = starting_downtrack + cruising_distance;
635 traj_upper.
t2_ = current_time + modified_remaining_time_upper;
636 traj_upper.
v2_ = departure_speed;
638 traj_upper.
x2_ = traffic_light_downtrack;
645 traj_upper.
t3_ = traj_upper.
t2_;
646 traj_upper.
v3_ = traj_upper.
v2_;
647 traj_upper.
a3_ = traj_upper.
a2_;
648 traj_upper.
x3_ = traj_upper.
x2_;
652 modified_departure_speed_lower = sqrt(pow(starting_speed, 2) + (2 *
max_comfort_decel_ * remaining_downtrack));
653 modified_remaining_time_lower = (modified_departure_speed_lower - starting_speed) /
max_comfort_decel_;
655 traj_lower.
t0_ = current_time;
656 traj_lower.
v0_ = starting_speed;
657 traj_lower.
x0_ = starting_downtrack;
661 traj_lower.
t1_ = current_time + modified_remaining_time_lower;
662 traj_lower.
v1_ = modified_departure_speed_lower;
664 traj_lower.
x1_ = traffic_light_downtrack;
666 traj_lower.
t2_ = traj_lower.
t1_;
667 traj_lower.
v2_ = traj_lower.
v1_;
668 traj_lower.
a2_ = traj_lower.
a1_;
669 traj_lower.
x2_ = traj_lower.
x1_;
671 traj_lower.
t3_ = traj_lower.
t2_;
672 traj_lower.
v3_ = traj_lower.
v2_;
673 traj_lower.
a3_ = traj_lower.
a2_;
674 traj_lower.
x3_ = traj_lower.
x2_;
680 bool is_return_params_found =
false;
682 if (calculation_success_upper)
684 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Checking this time!: " << current_time + modified_remaining_time_upper);
686 auto upper_optional = traffic_light->predictState(lanelet::time::timeFromSec(current_time + modified_remaining_time_upper));
688 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Checking this time! state: " << upper_optional.get().second);
690 if (!
validLightState(upper_optional, rclcpp::Time((current_time + modified_remaining_time_upper) * 1e9)))
692 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));
696 if (upper_optional.get().second == lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED)
698 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Detected Upper GREEN case");
699 return_params = traj_upper;
700 is_return_params_found =
true;
705 if (!is_return_params_found)
707 auto lower_optional = traffic_light->predictState(lanelet::time::timeFromSec(current_time + modified_remaining_time_lower));
709 if (!
validLightState(lower_optional, rclcpp::Time((current_time + modified_remaining_time_lower) * 1e9)))
711 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));
715 if (lower_optional.get().second == lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED)
717 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Detected Lower GREEN case");
718 return_params = traj_lower;
719 is_return_params_found =
true;
728 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Updated the speed, and using modified_departure_speed: " << return_params.
modified_departure_speed);
730 return return_params;
734 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Unable to handle edge case gracefully");
737 return return_params;
743 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
const VehicleState& current_state,
744 const lanelet::CarmaTrafficSignalPtr& traffic_light,
const lanelet::ConstLanelet& entry_lanelet,
const lanelet::ConstLanelet& exit_lanelet,
const lanelet::ConstLanelet& current_lanelet)
754 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"No lights found along route. Returning maneuver plan unchanged");
761 auto stop_line = traffic_light->getStopLine(entry_lanelet);
765 throw std::invalid_argument(
"Given entry lanelet doesn't have stop_line...");
768 double traffic_light_down_track =
769 wm_->routeTrackPos(stop_line.get().front().basicPoint2d()).downtrack;
771 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"traffic_light_down_track: " << traffic_light_down_track);
773 double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.
downtrack;
775 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"distance_remaining_to_traffic_light: " << distance_remaining_to_traffic_light <<
776 ", current_state.downtrack: " << current_state.
downtrack);
782 current_state_speed = std::min(speed_limit, current_state_speed);
784 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"speed_limit (free flow speed): " << speed_limit);
791 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Stopping distance: " << stopping_dist);
797 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"plugin_activation_distance: " << plugin_activation_distance);
799 if (distance_remaining_to_traffic_light <= plugin_activation_distance)
801 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Within intersection range");
807 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Not within intersection range");
813 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
const VehicleState& current_state,
814 const lanelet::CarmaTrafficSignalPtr& traffic_light,
const lanelet::ConstLanelet& entry_lanelet,
const lanelet::ConstLanelet& exit_lanelet,
const lanelet::ConstLanelet& current_lanelet)
827 auto stop_line = traffic_light->getStopLine(entry_lanelet);
831 throw std::invalid_argument(
"Given entry lanelet doesn't have stop_line...");
834 double traffic_light_down_track =
835 wm_->routeTrackPos(stop_line.get().front().basicPoint2d()).downtrack;
837 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"traffic_light_down_track: " << traffic_light_down_track);
839 double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.
downtrack;
843 if (distance_remaining_to_traffic_light < 0)
845 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Crossed the bar distance_remaining_to_traffic_light: " << distance_remaining_to_traffic_light);
852 lanelet::ConstLanelet intersection_lanelet;
854 auto route =
wm_->getRoute()->shortestPath();
855 auto entry_llt_iter = std::find(
route.begin(),
route.end(), entry_lanelet);
856 if (entry_llt_iter !=
route.end())
858 intersection_lanelet = *(entry_llt_iter + 1);
861 if (intersection_lanelet.id() != lanelet::InvalId)
875 current_state_speed = std::min(current_state_speed, speed_limit - 5 *
epsilon_);
877 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"current_state_speed: " << current_state_speed);
878 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"intersection_speed_: " <<
intersection_speed_.get());
879 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"distance_remaining_to_traffic_light: " << distance_remaining_to_traffic_light);
882 wm_->routeTrackPos(exit_lanelet.centerline2d().front().basicPoint2d()).downtrack;
884 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"intersection_end_downtrack_: " <<
intersection_end_downtrack_.get());
887 constexpr double HALF_MPH_IN_MPS = 0.22352;
888 if (current_state.
speed < HALF_MPH_IN_MPS &&
892 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"CARMA has detected that the vehicle stopped at the stop bar. Transitioning to WAITING STATE");
902 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()));
906 if (nearest_green_entry_time == rclcpp::Time(0))
909 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Final nearest_green_entry_time: " <<
std::to_string(nearest_green_entry_time.seconds()));
911 auto et_state = traffic_light->predictState(lanelet::time::timeFromSec(nearest_green_entry_time.seconds()));
912 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Signal at ET: " << et_state.get().second);
916 double remaining_time = nearest_green_entry_time.seconds() - current_state.
stamp.seconds();
917 double remaining_time_earliest_entry = earliest_entry_time.seconds() - current_state.
stamp.seconds();
924 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);
926 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);
929 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"SPEED PROFILE CASE:" << ts_params.
case_num);
933 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_);
936 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_);
945 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"new emergency_distance_to_stop: " << emergency_distance_to_stop);
946 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"new safe_distance_to_stop: " << safe_distance_to_stop);
947 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"new desired_distance_to_stop: " << desired_distance_to_stop);
949 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);
956 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...");
961 rclcpp::Time stopping_arrival_time =
962 current_state.
stamp + rclcpp::Duration(stopping_time * 1e9);
964 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"stopping_arrival_time: " <<
std::to_string(stopping_arrival_time.seconds()));
966 auto stopping_arrival_state_optional = traffic_light->predictState(lanelet::time::timeFromSec(stopping_arrival_time.seconds()));
968 if (!
validLightState(stopping_arrival_state_optional, stopping_arrival_time))
970 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()));
976 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Detected possible RED light violation! Stopping!");
977 handleStopping(req,resp, current_state, traffic_light, entry_lanelet, exit_lanelet, current_lanelet, traffic_light_down_track,
true);
986 (distance_remaining_to_traffic_light >= desired_distance_to_stop || !in_tbd) &&
987 is_entry_time_within_green_or_tbd)
989 handleGreenSignalScenario(req, resp, current_state, current_state_speed, traffic_light, entry_lanelet, exit_lanelet, traffic_light_down_track, ts_params, in_tbd);
991 if (!resp->new_plan.maneuvers.empty())
997 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");
998 ts_params = boundary_traj_params[7];
1004 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Not able to make it with certainty: NEW TSCase: " << ts_params.
case_num);
1009 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Way too early to stop. Cruising at CASE8");
1010 handleCruisingUntilStop(req, resp, current_state, current_state_speed, traffic_light, traffic_light_down_track, ts_params);
1012 if (!resp->new_plan.maneuvers.empty())
1019 if ((safe_distance_to_stop <= distance_remaining_to_traffic_light && desired_distance_to_stop >= distance_remaining_to_traffic_light) ||
1022 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Planning stopping now. last case:" <<
static_cast<int>(
last_case_num_));
1023 handleStopping(req,resp, current_state, traffic_light, entry_lanelet, exit_lanelet, current_lanelet, traffic_light_down_track);
1027 if (safe_distance_to_stop > distance_remaining_to_traffic_light)
1029 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"No longer within safe distance to stop! Decide to continue stopping or continue into intersection");
1034 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!");
1036 handleFailureCase(req, resp, current_state, current_state_speed, speed_limit, remaining_time,
1037 exit_lanelet.id(), traffic_light, traffic_light_down_track, ts_params);
1039 if (resp->new_plan.maneuvers.empty())
1041 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"HANDLE_SAFETY: Planning forced slow-down... last case:" <<
static_cast<int>(
last_case_num_));
1042 handleStopping(req,resp, current_state, traffic_light, entry_lanelet, exit_lanelet, current_lanelet, traffic_light_down_track,
true);
1047 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"HANDLE_SAFETY: Planning to keep stopping now. last case:" <<
static_cast<int>(
last_case_num_));
1048 handleStopping(req,resp, current_state, traffic_light, entry_lanelet, exit_lanelet, current_lanelet, traffic_light_down_track);
1056 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
const VehicleState& current_state,
1057 const lanelet::CarmaTrafficSignalPtr& traffic_light,
const lanelet::ConstLanelet& entry_lanelet,
const lanelet::ConstLanelet& exit_lanelet,
const lanelet::ConstLanelet& current_lanelet)
1063 throw std::invalid_argument(
"While in WAITING state, the traffic lights disappeared.");
1066 auto stop_line = traffic_light->getStopLine(entry_lanelet);
1070 throw std::invalid_argument(
"Given entry lanelet doesn't have stop_line...");
1073 double traffic_light_down_track =
1074 wm_->routeTrackPos(stop_line.get().front().basicPoint2d()).downtrack;
1076 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"traffic_light_down_track: "<< traffic_light_down_track);
1078 double entering_time = current_state.
stamp.seconds();
1080 auto current_light_state_optional = traffic_light->predictState(lanelet::time::timeFromSec(entering_time));
1082 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()));
1083 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"WAITING STATE: requested time to CURRENT STATE check: " <<
std::to_string(entering_time));
1085 if (!
validLightState(current_light_state_optional, rclcpp::Time(entering_time * 1e9)))
1090 if (!bool_optional_late_certainty)
1092 RCLCPP_ERROR_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Unable to resolve green light...");
1096 bool should_enter =
true;
1099 should_enter =
false;
1101 if (current_light_state_optional.get().second == lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED &&
1102 bool_optional_late_certainty.get() && should_enter)
1111 constexpr double stop_maneuver_buffer = 10.0;
1117 current_state.
downtrack - stop_maneuver_buffer, traffic_light_down_track, current_state.
speed,
1118 current_state.
lane_id, current_state.
lane_id, rclcpp::Time(entering_time * 1e9),
1123 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
const VehicleState& current_state,
1124 double intersection_end_downtrack,
double intersection_speed_limit)
1128 if (current_state.
downtrack > intersection_end_downtrack)
1135 rclcpp::Time intersection_exit_time =
1136 current_state.
stamp +
1137 rclcpp::Duration(2.0 * (intersection_end_downtrack - current_state.
downtrack) / (current_state.
speed + intersection_speed_limit) * 1e9);
1140 std::vector<lanelet::ConstLanelet> crossed_lanelets =
1145 current_state.
downtrack, intersection_end_downtrack, current_state.
speed, intersection_speed_limit,
1146 current_state.
stamp, intersection_exit_time, crossed_lanelets.back().id(), crossed_lanelets.back().id()));
1155 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Received Schedule message with id: " << msg->m_header.plan_id);
1162 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"street_msg_timestamp_: " <<
street_msg_timestamp_);
1172 std::vector<uint8_t> bsm_id_vec = msg->core_data.id;
1173 bsm_id_ = BSMHelper::BSMHelper::bsmIDtoString(bsm_id_vec);
1181 std::string params = strategy_params;
1182 std::vector<std::string> inputsParams;
1185 std::vector<std::string> et_parsed;
1187 auto new_scheduled_enter_time = std::stoull(et_parsed[1]);
1193 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"scheduled_enter_time_: " <<
scheduled_enter_time_);
1199 carma_v2x_msgs::msg::MobilityOperation mo_;
1200 mo_.m_header.timestamp = now().nanoseconds()/1000000;
1203 mo_.m_header.sender_bsm_id =
bsm_id_;
1209 std::string intersection_turn_direction =
"straight";
1213 mo_.strategy_params =
"access: 1, max_accel: " +
std::to_string(vehicle_acceleration_limit_) +
1225 for (
auto l:lanelets_list)
1227 if(l.hasAttribute(
"turn_direction")) {
1228 std::string direction_attribute = l.attribute(
"turn_direction").value();
1229 if (direction_attribute ==
"right")
1234 else if (direction_attribute ==
"left")
1248 return turn_direction;
1263 std::shared_ptr<rmw_request_id_t> srv_header,
1264 carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
1265 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp)
1267 std::chrono::system_clock::time_point execution_start_time = std::chrono::system_clock::now();
1269 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"<<<<<<<<<<<<<<<<< STARTING LCI_STRATEGIC_PLAN!!!!!!!!! >>>>>>>>>>>>>>>>");
1271 if (!
wm_->getRoute())
1273 RCLCPP_ERROR_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Could not plan maneuvers as route was not available");
1281 resp->new_plan.maneuvers = {};
1282 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Not approaching light-controlled intersection so no maneuvers");
1287 if (is_empty_schedule_msg)
1289 resp->new_plan.maneuvers = {};
1290 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Receiving empty schedule message");
1295 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Finding car information");
1301 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"State is NOT UNAVAILABLE AND Maneuvers in request is NOT empty");
1305 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"\n\nFinding traffic_light information");
1307 auto inter_list =
wm_->getSignalizedIntersectionsAlongRoute({ req->veh_x, req->veh_y });
1310 auto traffic_list =
wm_->getSignalsAlongRoute({ req->veh_x, req->veh_y });
1312 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Found traffic lights of size: " << traffic_list.size());
1314 auto current_lanelets =
wm_->getLaneletsFromPoint({ req->veh_x, req->veh_y});
1315 lanelet::ConstLanelet current_lanelet;
1317 if (current_lanelets.empty())
1319 RCLCPP_ERROR_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Given vehicle position is not on the road! Returning...");
1324 auto llt_on_route_optional =
wm_->getFirstLaneletOnShortestPath(current_lanelets);
1326 if (llt_on_route_optional){
1327 current_lanelet = llt_on_route_optional.value();
1330 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"When identifying the corresponding lanelet for requested maneuever's state, x: "
1331 << req->veh_x <<
", y: " << req->veh_y <<
", no possible lanelet was found to be on the shortest path."
1332 <<
"Picking arbitrary lanelet: " << current_lanelets[0].id() <<
", instead");
1333 current_lanelet = current_lanelets[0];
1336 lanelet::CarmaTrafficSignalPtr nearest_traffic_signal =
nullptr;
1338 lanelet::ConstLanelet entry_lanelet;
1339 lanelet::ConstLanelet exit_lanelet;
1341 for (
auto signal : traffic_list)
1343 auto optional_entry_exit =
wm_->getEntryExitOfSignalAlongRoute(signal);
1345 if (!optional_entry_exit)
1347 RCLCPP_ERROR_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Did not find entry.exit along the route");
1351 entry_lanelet = optional_entry_exit.get().first;
1352 exit_lanelet = optional_entry_exit.get().second;
1354 nearest_traffic_signal = signal;
1365 resp->new_plan = carma_planning_msgs::msg::ManeuverPlan();
1371 boost::optional<std::pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState>> current_light_state_optional = boost::none;
1372 if (nearest_traffic_signal)
1374 current_light_state_optional = nearest_traffic_signal->predictState(lanelet::time::timeFromSec(current_state.
stamp.seconds()));
1375 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());
1378 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Signal state not available returning..." );
1385 planWhenUNAVAILABLE(req, resp, current_state, nearest_traffic_signal, entry_lanelet, exit_lanelet, current_lanelet);
1389 planWhenAPPROACHING(req, resp, current_state, nearest_traffic_signal, entry_lanelet, exit_lanelet, current_lanelet);
1393 planWhenWAITING(req, resp, current_state, nearest_traffic_signal, entry_lanelet, exit_lanelet, current_lanelet);
1400 throw std::invalid_argument(
"State is DEPARTING but the intersection variables are not available");
1406 throw std::invalid_argument(
"LCIStrategic Strategic Plugin entered unknown state");
1412 std::chrono::system_clock::time_point execution_end_time = std::chrono::system_clock::now();
1414 auto execution_duration = execution_end_time - execution_start_time;
1415 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"ExecutionTime lci_strategic_plugin: " << std::chrono::duration<double>(execution_duration).count());
1422 double target_speed, rclcpp::Time start_time, rclcpp::Time end_time,
1425 carma_planning_msgs::msg::Maneuver maneuver_msg;
1426 maneuver_msg.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING;
1427 maneuver_msg.lane_following_maneuver.parameters.negotiation_type =
1428 carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
1429 maneuver_msg.lane_following_maneuver.parameters.presence_vector =
1430 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;
1431 maneuver_msg.lane_following_maneuver.parameters.planning_tactical_plugin =
1433 maneuver_msg.lane_following_maneuver.parameters.planning_strategic_plugin =
1435 maneuver_msg.lane_following_maneuver.start_dist = start_dist;
1436 maneuver_msg.lane_following_maneuver.start_speed = start_speed;
1437 maneuver_msg.lane_following_maneuver.start_time = start_time;
1438 maneuver_msg.lane_following_maneuver.end_dist = end_dist;
1439 maneuver_msg.lane_following_maneuver.end_speed = target_speed;
1440 maneuver_msg.lane_following_maneuver.end_time = end_time;
1444 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.
a1_);
1445 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.
v1_);
1446 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.
x1_);
1448 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.
a2_);
1449 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.
v2_);
1450 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.
x2_);
1452 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.
a3_);
1453 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.
v3_);
1454 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.
x3_);
1456 maneuver_msg.lane_following_maneuver.parameters.int_valued_meta_data.push_back(
static_cast<int>(tsp.
case_num));
1457 maneuver_msg.lane_following_maneuver.parameters.int_valued_meta_data.push_back(
static_cast<int>(tsp.
is_algorithm_successful));
1459 for (
auto llt : crossed_lanelets)
1461 maneuver_msg.lane_following_maneuver.lane_ids.push_back(
std::to_string(llt.id()));
1465 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Creating TrajectorySmoothingManeuver start dist: " << start_dist <<
" end dist: " << end_dist
1469 return maneuver_msg;
1474 const lanelet::Id& starting_lane_id,
1475 const lanelet::Id& ending_lane_id,
1476 rclcpp::Time start_time, rclcpp::Time end_time,
double stopping_accel)
const
1478 carma_planning_msgs::msg::Maneuver maneuver_msg;
1479 maneuver_msg.type = carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT;
1480 maneuver_msg.stop_and_wait_maneuver.parameters.negotiation_type = carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
1481 maneuver_msg.stop_and_wait_maneuver.parameters.presence_vector =
1482 carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN | carma_planning_msgs::msg::ManeuverParameters::HAS_FLOAT_META_DATA;
1485 maneuver_msg.stop_and_wait_maneuver.start_dist = current_dist;
1486 maneuver_msg.stop_and_wait_maneuver.end_dist = end_dist;
1487 maneuver_msg.stop_and_wait_maneuver.start_speed = start_speed;
1488 maneuver_msg.stop_and_wait_maneuver.start_time = start_time;
1489 maneuver_msg.stop_and_wait_maneuver.end_time = end_time;
1490 maneuver_msg.stop_and_wait_maneuver.starting_lane_id =
std::to_string(starting_lane_id);
1491 maneuver_msg.stop_and_wait_maneuver.ending_lane_id =
std::to_string(ending_lane_id);
1495 maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(stopping_accel);
1497 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Creating stop and wait start dist: " << current_dist <<
" end dist: " << end_dist
1500 <<
" stopping_accel: " << stopping_accel);
1502 return maneuver_msg;
1506 double start_speed,
double target_speed,
1507 rclcpp::Time start_time, rclcpp::Time end_time,
1508 const lanelet::Id& starting_lane_id,
1509 const lanelet::Id& ending_lane_id)
const
1511 carma_planning_msgs::msg::Maneuver maneuver_msg;
1512 maneuver_msg.type = carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT;
1513 maneuver_msg.intersection_transit_straight_maneuver.parameters.negotiation_type =
1514 carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
1515 maneuver_msg.intersection_transit_straight_maneuver.parameters.presence_vector =
1516 carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN;
1517 maneuver_msg.intersection_transit_straight_maneuver.parameters.planning_tactical_plugin =
1519 maneuver_msg.intersection_transit_straight_maneuver.parameters.planning_strategic_plugin =
1521 maneuver_msg.intersection_transit_straight_maneuver.start_dist = start_dist;
1522 maneuver_msg.intersection_transit_straight_maneuver.start_speed = start_speed;
1523 maneuver_msg.intersection_transit_straight_maneuver.start_time = start_time;
1524 maneuver_msg.intersection_transit_straight_maneuver.end_dist = end_dist;
1525 maneuver_msg.intersection_transit_straight_maneuver.end_speed = target_speed;
1526 maneuver_msg.intersection_transit_straight_maneuver.end_time = end_time;
1527 maneuver_msg.intersection_transit_straight_maneuver.starting_lane_id =
std::to_string(starting_lane_id);
1528 maneuver_msg.intersection_transit_straight_maneuver.ending_lane_id =
std::to_string(ending_lane_id);
1532 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"lci_strategic_plugin"),
"Creating IntersectionTransitManeuver start dist: " << start_dist <<
" end dist: " << end_dist
1533 <<
" From lanelet: " << starting_lane_id
1534 <<
" to lanelet: " << ending_lane_id
1538 return maneuver_msg;
1554#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...
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_
tf2_ros::Buffer tf2_buffer_
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)
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_
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.
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.
std::unique_ptr< tf2_ros::TransformListener > tf2_listener_
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 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...
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