Carma-platform v4.2.0
CARMA Platform is built on robot operating system (ROS) and utilizes open source software (OSS) that enables Cooperative Driving Automation (CDA) features to allow Automated Driving Systems to interact and cooperate with infrastructure and other vehicles through communication.
lci_strategic_plugin.cpp
Go to the documentation of this file.
1/*
2 * Copyright (C) 2023 LEIDOS.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
5 * use this file except in compliance with the License. You may obtain a copy of
6 * the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
12 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
13 * License for the specific language governing permissions and limitations under
14 * the License.
15 */
18
19#define EPSILON 0.01
20
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_" \
31 "PROPERTY " \
32 "(property) " \
33 "called on " \
34 "maneuver with " \
35 "invalid type " \
36 "id"))))))
37
39{
40 namespace std_ph = std::placeholders;
41
42LCIStrategicPlugin::LCIStrategicPlugin(const rclcpp::NodeOptions &options)
43 : carma_guidance_plugins::StrategicPlugin(options), tf2_buffer_(std::make_shared<tf2_ros::Buffer>(this->get_clock()))
44{
46
47 config_.vehicle_accel_limit = declare_parameter<double>("vehicle_acceleration_limit", config_.vehicle_accel_limit);
48 config_.vehicle_decel_limit = declare_parameter<double>("vehicle_deceleration_limit", config_.vehicle_decel_limit);
49 config_.vehicle_decel_limit_multiplier = declare_parameter<double>("vehicle_decel_limit_multiplier", config_.vehicle_decel_limit_multiplier);
50 config_.vehicle_accel_limit_multiplier = declare_parameter<double>("vehicle_accel_limit_multiplier", config_.vehicle_accel_limit_multiplier);
51 config_.min_approach_distance = declare_parameter<double>("min_approach_distance", config_.min_approach_distance);
52 config_.trajectory_smoothing_activation_distance = declare_parameter<double>("trajectory_smoothing_activation_distance", config_.trajectory_smoothing_activation_distance);
53 config_.stopping_location_buffer = declare_parameter<double>("stopping_location_buffer", config_.stopping_location_buffer);
54 config_.green_light_time_buffer = declare_parameter<double>("green_light_time_buffer", config_.green_light_time_buffer);
55 config_.vehicle_response_lag = declare_parameter<double>("vehicle_response_lag", config_.vehicle_response_lag);
56 config_.algo_minimum_speed = declare_parameter<double>("algo_minimum_speed", config_.algo_minimum_speed);
57 config_.deceleration_fraction = declare_parameter<double>("deceleration_fraction", config_.deceleration_fraction);
58 config_.desired_distance_to_stop_buffer = declare_parameter<double>("desired_distance_to_stop_buffer", config_.desired_distance_to_stop_buffer);
59 config_.min_maneuver_planning_period = declare_parameter<double>("min_maneuver_planning_period", config_.min_maneuver_planning_period);
60 config_.strategic_plugin_name = declare_parameter<std::string>("strategic_plugin_name", config_.strategic_plugin_name);
61 config_.lane_following_plugin_name = declare_parameter<std::string>("lane_following_plugin_name", config_.lane_following_plugin_name);
62 config_.stop_and_wait_plugin_name = declare_parameter<std::string>("stop_and_wait_plugin_name", config_.stop_and_wait_plugin_name);
63 config_.intersection_transit_plugin_name = declare_parameter<std::string>("intersection_transit_plugin_name", config_.intersection_transit_plugin_name);
64 config_.enable_carma_streets_connection = declare_parameter<bool>("enable_carma_streets_connection",config_.enable_carma_streets_connection);
65 config_.mobility_rate = declare_parameter<double>("mobility_rate", config_.mobility_rate);
67 declare_parameter<long>("enable_carma_wm_spat_processing",
69 config_.vehicle_id = declare_parameter<std::string>("vehicle_id", config_.vehicle_id);
70
75};
76
77carma_ros2_utils::CallbackReturn LCIStrategicPlugin::on_configure_plugin()
78{
79 // reset config
81
82 // clang-format off
83 get_parameter<double>("vehicle_acceleration_limit", config_.vehicle_accel_limit);
84 get_parameter<double>("vehicle_deceleration_limit", config_.vehicle_decel_limit);
85 get_parameter<double>("vehicle_decel_limit_multiplier", config_.vehicle_decel_limit_multiplier);
86 get_parameter<double>("vehicle_accel_limit_multiplier", config_.vehicle_accel_limit_multiplier);
87 get_parameter<double>("min_approach_distance", config_.min_approach_distance);
88 get_parameter<double>("trajectory_smoothing_activation_distance", config_.trajectory_smoothing_activation_distance);
89 get_parameter<double>("stopping_location_buffer", config_.stopping_location_buffer);
90 get_parameter<double>("green_light_time_buffer", config_.green_light_time_buffer);
91 get_parameter<double>("vehicle_response_lag", config_.vehicle_response_lag);
92 get_parameter<double>("algo_minimum_speed", config_.algo_minimum_speed);
93 get_parameter<double>("deceleration_fraction", config_.deceleration_fraction);
94 get_parameter<double>("desired_distance_to_stop_buffer", config_.desired_distance_to_stop_buffer);
95 get_parameter<double>("min_maneuver_planning_period", config_.min_maneuver_planning_period);
96 get_parameter<std::string>("strategic_plugin_name", config_.strategic_plugin_name);
97 get_parameter<std::string>("lane_following_plugin_name", config_.lane_following_plugin_name);
98 get_parameter<std::string>("stop_and_wait_plugin_name", config_.stop_and_wait_plugin_name);
99 get_parameter<std::string>("intersection_transit_plugin_name", config_.intersection_transit_plugin_name);
100 get_parameter<bool>("enable_carma_streets_connection", config_.enable_carma_streets_connection);
101 get_parameter<int>("enable_carma_wm_spat_processing", config_.enable_carma_wm_spat_processing);
102 get_parameter<double>("mobility_rate", config_.mobility_rate);
103 get_parameter<std::string>("vehicle_id", config_.vehicle_id);
104
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 "
114
115 // clang-format on
116
117 // Register runtime parameter update callback
118 add_on_set_parameters_callback(std::bind(&LCIStrategicPlugin::parameter_update_callback, this, std_ph::_1));
119
120 RCLCPP_INFO_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Done loading parameters: " << config_);
121
123
124 // Mobility Operation Subscriber
125 mob_operation_sub_ = create_subscription<carma_v2x_msgs::msg::MobilityOperation>("incoming_mobility_operation", 1,
126 std::bind(&LCIStrategicPlugin::mobilityOperationCb,this,std_ph::_1));
127
128 // BSM subscriber
129 bsm_sub_ = create_subscription<carma_v2x_msgs::msg::BSM>("bsm_outbound", 1,
130 std::bind(&LCIStrategicPlugin::BSMCb,this,std_ph::_1));
131
132 // set world model point from wm listener
134
135 // Activate SPAT processor, which is turned off by default,
136 // with OFF (0), ON (1)
137 get_world_model_listener()->setWMSpatProcessingState(
139
140 // Setup publishers
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);
146
147 // Return success if everything initialized successfully
148 return CallbackReturn::SUCCESS;
149}
150
151rcl_interfaces::msg::SetParametersResult LCIStrategicPlugin::parameter_update_callback(const std::vector<rclcpp::Parameter> &parameters)
152{
153 auto error_double = update_params<double>({
154 {"vehicle_acceleration_limit", config_.vehicle_accel_limit},
155 {"vehicle_deceleration_limit", config_.vehicle_decel_limit},
156 {"vehicle_decel_limit_multiplier", config_.vehicle_decel_limit_multiplier},
157 {"vehicle_accel_limit_multiplier", config_.vehicle_accel_limit_multiplier},
158 {"min_approach_distance", config_.min_approach_distance},
159 {"trajectory_smoothing_activation_distance", config_.trajectory_smoothing_activation_distance},
160 {"stopping_location_buffer", config_.stopping_location_buffer},
161 {"green_light_time_buffer", config_.green_light_time_buffer},
162 {"vehicle_response_lag", config_.vehicle_response_lag},
163 {"algo_minimum_speed", config_.algo_minimum_speed},
164 {"deceleration_fraction", config_.deceleration_fraction},
165 {"desired_distance_to_stop_buffer", config_.desired_distance_to_stop_buffer},
166 {"min_maneuver_planning_period", config_.min_maneuver_planning_period},
167 {"mobility_rate", config_.mobility_rate},
168 }, parameters);
169
172
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;
177
178 result.successful = !error_double;
179
180 return result;
181}
182
184{
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;
189
190 case_num_msg.data = static_cast<int>(last_case_num_);
191 tf_distance.data = distance_remaining_to_tf_;
192 earliest_et.data = earliest_entry_time_;
193 scheduled_et.data = scheduled_entry_time_;
194
195 case_pub_->publish(case_num_msg);
196 tf_distance_pub_->publish(tf_distance);
197 earliest_et_pub_->publish(earliest_et);
198 et_pub_->publish(scheduled_et);
199}
200
201carma_ros2_utils::CallbackReturn LCIStrategicPlugin::on_activate_plugin()
202{
203 mob_op_pub_timer_ = create_timer(get_clock(),
204 std::chrono::duration<double>(1/config_.mobility_rate),
206
207 ts_info_pub_timer_ = create_timer(get_clock(),
208 std::chrono::duration<double>(0.5),
210
211 return CallbackReturn::SUCCESS;
212}
213
214bool LCIStrategicPlugin::supportedLightState(lanelet::CarmaTrafficSignalState state) const
215{
216 switch (state)
217 {
218 // NOTE: Following cases are intentional fall through.
219 // Supported light states
220 case lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN: // Solid Red
221 case lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE: // Yellow Solid no chance of conflicting traffic
222 case lanelet::CarmaTrafficSignalState::PERMISSIVE_MOVEMENT_ALLOWED: // Solid Green there could be conflict traffic
223 case lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED: // Solid Green no chance of conflict traffic
224 // (normally used with arrows)
225 return true;
226
227 // Unsupported light states
228 case lanelet::CarmaTrafficSignalState::PERMISSIVE_CLEARANCE: // Yellow Solid there is a chance of conflicting
229 // traffic
230 case lanelet::CarmaTrafficSignalState::UNAVAILABLE: // No data available
231 case lanelet::CarmaTrafficSignalState::DARK: // Light is non-functional
232 case lanelet::CarmaTrafficSignalState::STOP_THEN_PROCEED: // Flashing Red
233
234 case lanelet::CarmaTrafficSignalState::PRE_MOVEMENT: // Yellow Red transition (Found only in the EU)
235 // (normally used with arrows)
236 case lanelet::CarmaTrafficSignalState::CAUTION_CONFLICTING_TRAFFIC: // Yellow Flashing
237 default:
238 return false;
239 }
240}
241
243{
244 tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);
245 tf2_buffer_->setUsingDedicatedThread(true);
246 try
247 {
248 geometry_msgs::msg::TransformStamped tf2 = tf2_buffer_->lookupTransform("base_link", "vehicle_front", rclcpp::Time(0), rclcpp::Duration::from_nanoseconds(20.0 * 1e9)); //save to local copy of transform 20 sec timeout
249 length_to_front_bumper_ = tf2.transform.translation.x;
250 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "length_to_front_bumper_: " << length_to_front_bumper_);
251
252 }
253 catch (const tf2::TransformException &ex)
254 {
255 RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"), ex.what());
256 }
257}
258
259LCIStrategicPlugin::VehicleState LCIStrategicPlugin::extractInitialState(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req) const
260{
261 VehicleState state;
262 if (!req->prior_plan.maneuvers.empty())
263 {
264 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Provided with initial plan...");
265 state.stamp = GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_time);
266 state.downtrack = GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_dist);
267 state.speed = GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_speed);
268 state.lane_id = getLaneletsBetweenWithException(state.downtrack, state.downtrack, true).front().id();
269 }
270 else
271 {
272 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "No initial plan provided...");
273
274 state.stamp = rclcpp::Time(req->header.stamp, RCL_SYSTEM_TIME);
275 state.downtrack = req->veh_downtrack;
276 state.speed = req->veh_logitudinal_velocity;
277 state.lane_id = stoi(req->veh_lane_id);
278 }
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);
283
284 return state;
285}
286
287double LCIStrategicPlugin::findSpeedLimit(const lanelet::ConstLanelet& llt) const
288{
289 lanelet::Optional<carma_wm::TrafficRulesConstPtr> traffic_rules = wm_->getTrafficRules();
290 if (traffic_rules)
291 {
292 return (*traffic_rules)->speedLimit(llt).speedLimit.value();
293 }
294 else
295 {
296 throw std::invalid_argument("Valid traffic rules object could not be built");
297 }
298}
299
300bool LCIStrategicPlugin::validLightState(const boost::optional<std::pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState>>& optional_state,
301 const rclcpp::Time& source_time) const
302{
303 if (!optional_state)
304 {
305 RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"),"Traffic light data not available for source_time " << std::to_string(source_time.seconds()));
306 return false;
307 }
308
309 lanelet::CarmaTrafficSignalState light_state = optional_state.get().second;
310
311 if (!supportedLightState(light_state))
312 {
313 RCLCPP_ERROR_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "LCIStrategic Plugin asked to handle CarmaTrafficSignalState: " << light_state
314 << " which is not supported.");
315 return false;
316 }
317
318 return true;
319}
320
321boost::optional<bool> LCIStrategicPlugin::canArriveAtGreenWithCertainty(const rclcpp::Time& light_arrival_time_by_algo, const lanelet::CarmaTrafficSignalPtr& traffic_light, bool check_late = true, bool check_early = true) const
322{
323 rclcpp::Time early_arrival_time_by_algo =
324 light_arrival_time_by_algo - rclcpp::Duration::from_nanoseconds(config_.green_light_time_buffer * 1e9);
325
326 rclcpp::Time late_arrival_time_by_algo =
327 light_arrival_time_by_algo + rclcpp::Duration::from_nanoseconds(config_.green_light_time_buffer * 1e9);
328
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()));
332
333 auto early_arrival_state_by_algo_optional = traffic_light->predictState(lanelet::time::timeFromSec(early_arrival_time_by_algo.seconds()));
334
335 if (!validLightState(early_arrival_state_by_algo_optional, early_arrival_time_by_algo))
336 return boost::none;
337
338 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "early_arrival_state_by_algo: " << early_arrival_state_by_algo_optional.get().second);
339
340 auto late_arrival_state_by_algo_optional = traffic_light->predictState(lanelet::time::timeFromSec(late_arrival_time_by_algo.seconds()));
341
342 if (!validLightState(late_arrival_state_by_algo_optional, late_arrival_time_by_algo))
343 return boost::none;
344
345 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "late_arrival_state_by_algo: " << late_arrival_state_by_algo_optional.get().second);
346
347 bool can_make_early_arrival = true;
348 bool can_make_late_arrival = true;
349
350 if (check_early)
351 can_make_early_arrival = isStateAllowedGreen(early_arrival_state_by_algo_optional.get().second);
352
353 if (check_late)
354 can_make_late_arrival = isStateAllowedGreen(late_arrival_state_by_algo_optional.get().second);
355
356 // We will cross the light on the green phase even if we arrive early or late
357 if (can_make_early_arrival && can_make_late_arrival) // Green light
358 return true;
359 else
360 return false;
361
362}
363
364bool LCIStrategicPlugin::isStateAllowedGreen(const lanelet::CarmaTrafficSignalState& state) const{
365 return state == lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED ||
366 state == lanelet::CarmaTrafficSignalState::PERMISSIVE_MOVEMENT_ALLOWED;
367}
368
369std::vector<lanelet::ConstLanelet> LCIStrategicPlugin::getLaneletsBetweenWithException(double start_downtrack,
370 double end_downtrack,
371 bool shortest_path_only,
372 bool bounds_inclusive) const
373{
374 std::vector<lanelet::ConstLanelet> crossed_lanelets =
375 wm_->getLaneletsBetween(start_downtrack, end_downtrack, shortest_path_only, bounds_inclusive);
376
377 if (crossed_lanelets.size() == 0)
378 {
379 throw std::invalid_argument("getLaneletsBetweenWithException called but inputs do not cross any lanelets going "
380 "from: " +
381 std::to_string(start_downtrack) + " to: " + std::to_string(end_downtrack));
382 }
383
384 return crossed_lanelets;
385}
386
387void LCIStrategicPlugin::handleStopping(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
388 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
389 const VehicleState& current_state,
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,
393 bool is_emergency)
394{
395 double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.downtrack;
396
397 // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver
398 std::vector<lanelet::ConstLanelet> crossed_lanelets =
399 getLaneletsBetweenWithException(current_state.downtrack, traffic_light_down_track, true, true);
400
401 double decel_rate = max_comfort_decel_norm_; // Kinematic |(v_f - v_i) / t = a|
402
403 if (is_emergency)
404 {
405 decel_rate = emergency_decel_norm_;
407 }
408 else
409 {
411 }
412
413 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "HANDLE_STOPPING: Planning stop and wait maneuver at decel_rate: " << decel_rate);
414
415 resp->new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage(
416 current_state.downtrack, traffic_light_down_track, current_state.speed, crossed_lanelets.front().id(),
417 crossed_lanelets.back().id(), current_state.stamp,
418 current_state.stamp + rclcpp::Duration::from_nanoseconds(config_.min_maneuver_planning_period * 1e9), decel_rate));
419}
420
421
422void LCIStrategicPlugin::handleFailureCase(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
423 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
424 const VehicleState& current_state,
425 double current_state_speed,
426 double speed_limit,
427 double remaining_time,
428 lanelet::Id exit_lanelet_id,
429 const lanelet::CarmaTrafficSignalPtr& traffic_light,
430 double traffic_light_down_track, const TrajectoryParams& ts_params)
431{
432 double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.downtrack;
433
434 // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver
435 std::vector<lanelet::ConstLanelet> crossed_lanelets =
436 getLaneletsBetweenWithException(current_state.downtrack, traffic_light_down_track, true, true);
437
438 auto incomplete_traj_params = handleFailureCaseHelper(traffic_light, current_state.stamp.seconds(), current_state_speed, intersection_speed_.get(), speed_limit, distance_remaining_to_traffic_light, traffic_light_down_track);
439
440 if (incomplete_traj_params.is_algorithm_successful == false)
441 {
442 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Failed to generate maneuver for edge cases...");
443 return;
444 }
445
446 resp->new_plan.maneuvers.push_back(composeTrajectorySmoothingManeuverMessage(current_state.downtrack, traffic_light_down_track, crossed_lanelets,
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));
448
449 double intersection_length = intersection_end_downtrack_.get() - traffic_light_down_track;
450
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);
453
454 resp->new_plan.maneuvers.push_back(composeIntersectionTransitMessage(
455 traffic_light_down_track, intersection_end_downtrack_.get(), intersection_speed_.get(),
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()));
457
459}
460
461void LCIStrategicPlugin::handleCruisingUntilStop(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
462 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
463 const VehicleState& current_state,
464 double current_state_speed,
465 const lanelet::CarmaTrafficSignalPtr& traffic_light,
466 double traffic_light_down_track, const TrajectoryParams& ts_params)
467{
468 if (!ts_params.is_algorithm_successful || ts_params.case_num != TSCase::CASE_8)
469 {
470 RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"),"handleCruisingUntilStop is called but it is not case_8");
471 return;
472 }
473
474 auto new_ts_params = ts_params;
475
476 double decel_rate = std::fabs(ts_params.a3_);
477
478 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CASE_8: Planning cruise and stop with decel_rate: " << decel_rate);
479
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_;
484
485 // Identify the lanelets which will be crossed by approach maneuvers stopping part
486 std::vector<lanelet::ConstLanelet> lane_follow_crossed_lanelets =
487 getLaneletsBetweenWithException(new_ts_params.x1_, new_ts_params.x2_, true, true);
488
489 resp->new_plan.maneuvers.push_back(composeTrajectorySmoothingManeuverMessage(current_state.downtrack, new_ts_params.x2_, 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));
491
492 // Identify the lanelets which will be crossed by approach maneuvers stopping part
493 std::vector<lanelet::ConstLanelet> case_8_crossed_lanelets =
494 getLaneletsBetweenWithException(new_ts_params.x2_, traffic_light_down_track, true, true);
495
496 resp->new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage(
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),
499 rclcpp::Time(new_ts_params.t2_ * 1e9) + rclcpp::Duration::from_nanoseconds(config_.min_maneuver_planning_period * 1e9), decel_rate));
500
501 return;
502}
503
504void LCIStrategicPlugin::handleGreenSignalScenario(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
505 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
506 const VehicleState& current_state,
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)
511{
512 if (!ts_params.is_algorithm_successful || ts_params.case_num == TSCase::CASE_8)
513 {
514 return;
515 }
516
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));
520 auto can_make_green_optional = canArriveAtGreenWithCertainty(light_arrival_time_by_algo, traffic_light);
521
522 // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver
523 std::vector<lanelet::ConstLanelet> crossed_lanelets =
524 getLaneletsBetweenWithException(current_state.downtrack, traffic_light_down_track, true, true);
525
526 // no change for maneuver if invalid light states
527 if (!can_make_green_optional)
528 return;
529
530 if (can_make_green_optional.get() || is_certainty_check_optional)
531 {
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");
533
534 resp->new_plan.maneuvers.push_back(composeTrajectorySmoothingManeuverMessage(current_state.downtrack, traffic_light_down_track, crossed_lanelets,
535 current_state_speed, ts_params.v3_, current_state.stamp, light_arrival_time_by_algo, ts_params));
536
537 double intersection_length = intersection_end_downtrack_.get() - traffic_light_down_track;
538
539 rclcpp::Time intersection_exit_time =
540 light_arrival_time_by_algo + rclcpp::Duration::from_nanoseconds(intersection_length / intersection_speed_.get() * 1e9);
541
542 resp->new_plan.maneuvers.push_back(composeIntersectionTransitMessage(
543 traffic_light_down_track, intersection_end_downtrack_.get(), intersection_speed_.get(),
544 intersection_speed_.get(), light_arrival_time_by_algo, intersection_exit_time, crossed_lanelets.back().id(), crossed_lanelets.back().id()));
545 }
546}
547
548
549TrajectoryParams LCIStrategicPlugin::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)
550{
551 //Requested maneuver needs to be modified to meet remaining_dist req
552 //by trying to get close to the target_speed and remaining_time as much as possible
553 TrajectoryParams return_params;
554 TrajectoryParams traj_upper;
555 TrajectoryParams traj_lower;
556
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; // upper meaning downtrack vs time trajectory is curved upwards
560 double modified_remaining_time_lower; // lower meaning downtrack vs time trajectory is curved lower
561 double modified_departure_speed_upper;
562 double modified_departure_speed_lower;
563 bool calculation_success_upper = true; // identifies places in codes where calculation can be invalid such as negative distance
564
565 // the upper ET
566 // accel case
567 traj_upper.t0_ = current_time;
568 traj_upper.v0_ = starting_speed;
569 traj_upper.x0_ = starting_downtrack;
570 traj_upper.is_algorithm_successful = true;
571 traj_upper.case_num = CASE_1;
572
573 if (departure_speed >= starting_speed)
574 {
575 if ((pow(departure_speed,2) - pow(starting_speed,2))/(2*max_comfort_accel_) >= remaining_downtrack)
576 {
577 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "HandleFailureCase -> Upper Trajectory -> Current Speed <= Desired Departure Speed, Actual Departure Speed < Desired Departure Speed");
578
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_;
581
582 traj_upper.t1_ = current_time + modified_remaining_time_upper;
583 traj_upper.v1_ = modified_departure_speed_upper;
584 traj_upper.a1_ = max_comfort_accel_;
585 traj_upper.x1_ = traffic_light_downtrack;
586
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_;
591
592 traj_upper.modified_departure_speed = modified_departure_speed_upper;
593 traj_upper.modified_remaining_time = modified_remaining_time_upper;
594 }
595 else // NOTE: most likely will not happen as it would have happened at trajectory smoothing part
596 {
597 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "HandleFailureCase -> Upper Trajectory -> Current Speed < Desired Departure Speed, Actual Departure Speed = Desired Departure Speed");
598
599 double cruising_distance = remaining_downtrack - (pow(departure_speed, 2) - pow(starting_speed, 2))/ ( 2 * max_comfort_accel_);
600 if (cruising_distance < -EPSILON)
601 {
602 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Detected calculation failure in upper case 2");
603 calculation_success_upper = false;
604 }
605 modified_remaining_time_upper = ((departure_speed - starting_speed) / max_comfort_accel_) + (cruising_distance / departure_speed);
606
607 traj_upper.t1_ = current_time + ((departure_speed - starting_speed) / max_comfort_accel_);
608 traj_upper.v1_ = departure_speed;
609 traj_upper.a1_ = max_comfort_accel_;
610 traj_upper.x1_ = starting_downtrack + (pow(departure_speed, 2) - pow(starting_speed, 2)) / (2 * max_comfort_accel_);
611
612 traj_upper.t2_ = current_time + modified_remaining_time_upper;
613 traj_upper.v2_ = departure_speed;
614 traj_upper.a2_ = 0;
615 traj_upper.x2_ = traffic_light_downtrack;
616
617 traj_upper.modified_departure_speed = departure_speed;
618 traj_upper.modified_remaining_time = modified_remaining_time_upper;
619 }
620 }
621
622 if (departure_speed < starting_speed) // NOTE: most cases will not run due to departure speed being equal to speed limit
623 {
624 if ((pow(departure_speed,2) - pow(starting_speed,2))/(2*max_comfort_decel_) >= remaining_downtrack)
625 {
626 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "HandleFailureCase -> Upper Trajectory -> Current Speed > Desired Departure Speed, Actual Departure Speed > Desired Departure Speed");
627
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_;
630
631 traj_upper.t1_ = current_time + modified_remaining_time_upper;
632 traj_upper.v1_ = modified_departure_speed_upper;
633 traj_upper.a1_ = max_comfort_decel_;
634 traj_upper.x1_ = traffic_light_downtrack;
635
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_;
640
641 traj_upper.modified_departure_speed = modified_departure_speed_upper;
642 traj_upper.modified_remaining_time = modified_remaining_time_upper;
643 }
644 else // NOTE: most likely will not happen as it would have happened at trajectory smoothing part
645 {
646 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "HandleFailureCase -> Upper Trajectory -> Current Speed > Desired Departure Speed, Actual Departure Speed = Desired Departure Speed");
647
648 double cruising_distance = remaining_downtrack - (pow(departure_speed, 2) - pow(starting_speed, 2))/ ( 2 * max_comfort_decel_);
649
650 if (cruising_distance < -EPSILON)
651 {
652 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Detected calculation failure in upper case 4");
653 calculation_success_upper = false;
654 }
655
656 modified_remaining_time_upper = cruising_distance / starting_speed + (departure_speed - starting_speed) / max_comfort_decel_ ;
657
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;
662
663 traj_upper.t2_ = current_time + modified_remaining_time_upper;
664 traj_upper.v2_ = departure_speed;
665 traj_upper.a2_ = max_comfort_decel_;
666 traj_upper.x2_ = traffic_light_downtrack;
667
668 traj_upper.modified_departure_speed = departure_speed;
669 traj_upper.modified_remaining_time = modified_remaining_time_upper;
670 }
671 }
672
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_;
677
678 // The lower ET
679 // (note that the distance is definitely not enough for deceleration to zero speed, therefore, modified_departure_speed will be greater than zero for sure!).
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_;
682
683 traj_lower.t0_ = current_time;
684 traj_lower.v0_ = starting_speed;
685 traj_lower.x0_ = starting_downtrack;
686 traj_lower.is_algorithm_successful = true;
687 traj_lower.case_num = CASE_1;
688
689 traj_lower.t1_ = current_time + modified_remaining_time_lower;
690 traj_lower.v1_ = modified_departure_speed_lower;
691 traj_lower.a1_ = max_comfort_decel_;
692 traj_lower.x1_ = traffic_light_downtrack;
693
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_;
698
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_;
703
704 traj_lower.modified_departure_speed = modified_departure_speed_lower;
705 traj_lower.modified_remaining_time = modified_remaining_time_upper;
706
707 // Pick UPPER or LOWER trajectory based on light
708 bool is_return_params_found = false;
709
710 if (calculation_success_upper)
711 {
712 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Checking this time!: " << current_time + modified_remaining_time_upper);
713
714 auto upper_optional = traffic_light->predictState(lanelet::time::timeFromSec(current_time + modified_remaining_time_upper));
715
716 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Checking this time! state: " << upper_optional.get().second);
717
718 if (!validLightState(upper_optional, rclcpp::Time((current_time + modified_remaining_time_upper) * 1e9)))
719 {
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));
721 }
722 else
723 {
724 if (isStateAllowedGreen(upper_optional.get().second))
725 {
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;
729 }
730 }
731 }
732
733 if (!is_return_params_found)
734 {
735 auto lower_optional = traffic_light->predictState(lanelet::time::timeFromSec(current_time + modified_remaining_time_lower));
736
737 if (!validLightState(lower_optional, rclcpp::Time((current_time + modified_remaining_time_lower) * 1e9)))
738 {
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));
740 }
741 else
742 {
743 if (isStateAllowedGreen(lower_optional.get().second))
744 {
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;
748 }
749 }
750 }
751
752 // Handle hard failure case such as nan or invalid states
753 if (is_return_params_found && !isnan(return_params.modified_departure_speed) && return_params.modified_departure_speed > epsilon_ &&
754 return_params.modified_departure_speed < speed_limit ) //80_mph
755 {
756 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Updated the speed, and using modified_departure_speed: " << return_params.modified_departure_speed);
757 print_params(return_params);
758 return return_params;
759 }
760 else
761 {
762 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Unable to handle edge case gracefully");
763 return_params = TrajectoryParams(); //reset
764 return_params.is_algorithm_successful = false;
765 return return_params;
766 }
767}
768
769
770void LCIStrategicPlugin::planWhenUNAVAILABLE(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
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)
773{
774 // Reset intersection state since in this state we are not yet known to be in or approaching an intersection
775 intersection_speed_ = boost::none;
776 intersection_end_downtrack_ = boost::none;
777 double current_state_speed = std::max(current_state.speed, config_.algo_minimum_speed * 1.001);
778
779
780 if (!traffic_light)
781 {
782 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "No lights found along route. Returning maneuver plan unchanged");
783 return;
784 }
785
787 double max_comfort_decel = -1.0 * config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier;
788
789 auto stop_line = traffic_light->getStopLine(entry_lanelet);
790
791 if (!stop_line)
792 {
793 throw std::invalid_argument("Given entry lanelet doesn't have stop_line...");
794 }
795
796 double traffic_light_down_track =
797 wm_->routeTrackPos(stop_line.get().front().basicPoint2d()).downtrack;
798
799 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "traffic_light_down_track: " << traffic_light_down_track);
800
801 double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.downtrack;
802
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);
805
806 distance_remaining_to_tf_ = distance_remaining_to_traffic_light; // performance metric
807
808 auto speed_limit = findSpeedLimit(current_lanelet);
809
810 current_state_speed = std::min(speed_limit, current_state_speed);
811
812 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "speed_limit (free flow speed): " << speed_limit);
813
814 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "trajectory_smoothing_activation_distance: " << config_.trajectory_smoothing_activation_distance);
815
816 double stopping_dist = estimate_distance_to_stop(current_state_speed, config_.vehicle_decel_limit_multiplier *
817 config_.vehicle_decel_limit); //accepts positive decel
818
819 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Stopping distance: " << stopping_dist);
820
821 double plugin_activation_distance = std::max(stopping_dist, config_.min_approach_distance);
822
823 plugin_activation_distance = std::max(plugin_activation_distance, config_.trajectory_smoothing_activation_distance);
824
825 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "plugin_activation_distance: " << plugin_activation_distance);
826
827 if (distance_remaining_to_traffic_light <= plugin_activation_distance)
828 {
829 RCLCPP_INFO_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Within intersection range");
830 transition_table_.signal(TransitEvent::IN_STOPPING_RANGE); // Evaluate Signal and Run Trajectory Smoothing Algorithm
831 }
832 else
833 {
835 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Not within intersection range");
836 }
837
838}
839
840void LCIStrategicPlugin::planWhenAPPROACHING(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
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)
843{
845
846 if (!traffic_light) // If we are in the approaching state and there is no longer any lights ahead of us then
847 // the vehicle must have crossed the stop bar
848 {
850 return;
851 }
852
853 double current_state_speed = std::max(current_state.speed, config_.algo_minimum_speed * 1.001);
854
855 auto stop_line = traffic_light->getStopLine(entry_lanelet);
856
857 if (!stop_line)
858 {
859 throw std::invalid_argument("Given entry lanelet doesn't have stop_line...");
860 }
861
862 double traffic_light_down_track =
863 wm_->routeTrackPos(stop_line.get().front().basicPoint2d()).downtrack;
864
865 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "traffic_light_down_track: " << traffic_light_down_track);
866
867 double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.downtrack;
868
869 distance_remaining_to_tf_ = distance_remaining_to_traffic_light;
870
871 if (distance_remaining_to_traffic_light < 0) // there is small discrepancy between signal's routeTrackPos as well as current
872 { // downtrack calculated using veh_x,y from state. Therefore, it may have crossed it
873 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Crossed the bar distance_remaining_to_traffic_light: " << distance_remaining_to_traffic_light);
875 return;
876 }
877
878 // At this point we know the vehicle is within the activation distance and we know the current and next light phases
879 // All we need to now determine is if we should stop or if we should continue
880 lanelet::ConstLanelet intersection_lanelet;
881
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())
885 {
886 intersection_lanelet = *(entry_llt_iter + 1);
887 }
888
889 if (intersection_lanelet.id() != lanelet::InvalId)
890 {
891 intersection_speed_ = findSpeedLimit(intersection_lanelet);
893 }
894 else
895 {
896 intersection_speed_ = findSpeedLimit(exit_lanelet);
897 }
898
899 intersection_speed_ = intersection_speed_.get() * 0.999; // so that speed_limit is not exactly same as departure or current speed
900
901 double speed_limit = findSpeedLimit(current_lanelet);
902
903 current_state_speed = std::min(current_state_speed, speed_limit - 5 * epsilon_); // so that current_speed is not exactly same as departure or speed limit
904
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);
908
910 wm_->routeTrackPos(exit_lanelet.centerline2d().front().basicPoint2d()).downtrack;
911
912 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "intersection_end_downtrack_: " << intersection_end_downtrack_.get());
913
914 // If the vehicle is at a stop trigger the stopped state
915 constexpr double HALF_MPH_IN_MPS = 0.22352;
916 if (current_state.speed < HALF_MPH_IN_MPS &&
917 fabs(distance_remaining_to_traffic_light) < config_.stopping_location_buffer)
918 {
919 transition_table_.signal(TransitEvent::STOPPED); // The vehicle has come to a stop at the light
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");
921
922 return;
923 }
924
926
927 rclcpp::Time earliest_entry_time = current_state.stamp + get_earliest_entry_time(distance_remaining_to_traffic_light, speed_limit,
928 current_state_speed, intersection_speed_.get(), max_comfort_accel_, max_comfort_decel_);
929
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()));
931
932 auto [nearest_green_entry_time, is_entry_time_within_green_or_tbd, in_tbd] = get_final_entry_time_and_conditions(current_state, earliest_entry_time, traffic_light);
933
934 if (nearest_green_entry_time == rclcpp::Time(0))
935 return;
936
937 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Final nearest_green_entry_time: " << std::to_string(nearest_green_entry_time.seconds()));
938
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);
941
943
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();
946 scheduled_entry_time_ = remaining_time; // performance metric
947 earliest_entry_time_ = remaining_time_earliest_entry; // performance metric
948
949 auto boundary_distances = get_delta_x(current_state_speed, intersection_speed_.get(), speed_limit, config_.algo_minimum_speed, max_comfort_accel_, max_comfort_decel_);
950 print_boundary_distances(boundary_distances); //debug
951
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);
953
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);
955 print_params(ts_params);
956
957 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "SPEED PROFILE CASE:" << ts_params.case_num);
958
960 double emergency_distance_to_stop = pow(current_state.speed, 2)/(2 * emergency_decel_norm_) + config_.stopping_location_buffer / 2; //Idea is to aim the middle part of stopping buffer
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_);
962
963 double safe_distance_to_stop = pow(current_state.speed, 2)/(2 * max_comfort_decel_norm_) + config_.stopping_location_buffer / 2; //Idea is to aim the middle part of stopping buffer
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_);
965
966 double desired_distance_to_stop = pow(current_state.speed, 2)/(2 * max_comfort_decel_norm_ * config_.deceleration_fraction) + config_.desired_distance_to_stop_buffer;
967 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "desired_distance_to_stop at: " << desired_distance_to_stop << ", where effective deceleration rate is: " << max_comfort_decel_norm_ * config_.deceleration_fraction);
968
969 emergency_distance_to_stop = std::max(emergency_distance_to_stop, config_.stopping_location_buffer);
970 safe_distance_to_stop = std::max(safe_distance_to_stop, config_.stopping_location_buffer);
971 desired_distance_to_stop = std::max(desired_distance_to_stop, config_.stopping_location_buffer);
972
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);
976
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);
978
979 // Basic RED signal violation check
980 if (distance_remaining_to_traffic_light <= emergency_distance_to_stop || last_case_num_ == TSCase::EMERGENCY_STOPPING)
981 {
982 if (in_tbd) // Given ET is in TBD, but vehicle is too close to intersection
983 {
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...");
985 }
986
987 double stopping_time = current_state.speed / 1.5 / max_comfort_decel_norm_; //one half the acceleration (twice the acceleration to stop) to account for emergency case, see emergency_decel_norm_
988
989 rclcpp::Time stopping_arrival_time =
990 current_state.stamp + rclcpp::Duration::from_nanoseconds(stopping_time * 1e9);
991
992 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "stopping_arrival_time: " << std::to_string(stopping_arrival_time.seconds()));
993
994 auto stopping_arrival_state_optional = traffic_light->predictState(lanelet::time::timeFromSec(stopping_arrival_time.seconds()));
995
996 if (!validLightState(stopping_arrival_state_optional, stopping_arrival_time))
997 {
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()));
999 return;
1000 }
1001
1002 if (stopping_arrival_state_optional.get().second == lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN || last_case_num_ == TSCase::EMERGENCY_STOPPING) // if once started emergency stopped, keep doing it to avoid jerkiness
1003 {
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); //case_11
1007
1008 return;
1009 }
1010 }
1011
1012 // Check if the vehicle can arrive with certainty (Case 1-7)
1013 if (ts_params.is_algorithm_successful && ts_params.case_num != TSCase::CASE_8 &&
1014 (distance_remaining_to_traffic_light >= desired_distance_to_stop || !in_tbd) &&
1015 is_entry_time_within_green_or_tbd) // ET cannot be explicitly inside RED or YELLOW in available future states, which is ERROR case
1016 {
1017 handleGreenSignalScenario(req, resp, current_state, current_state_speed, traffic_light, entry_lanelet, exit_lanelet, traffic_light_down_track, ts_params, in_tbd); //in_tbd means optional to check certainty arrival at green
1018
1019 if (!resp->new_plan.maneuvers.empty()) // able to pass at green
1020 {
1021 last_case_num_ = ts_params.case_num;
1022 return;
1023 }
1024
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];
1027 ts_params.is_algorithm_successful = true; //false correspond to cases when vehicle is beyond safe_distance to stop for case8
1028 ts_params.case_num = CASE_8;
1029 print_params(ts_params);
1030 }
1031
1032 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Not able to make it with certainty: NEW TSCase: " << ts_params.case_num);
1033 // if algorithm is NOT successful or if the vehicle cannot make the green light with certainty
1034
1035 if (desired_distance_to_stop < distance_remaining_to_traffic_light && last_case_num_ != TSCase::STOPPING) // do not switch from STOPPING to case8 again
1036 {
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);
1039
1040 if (!resp->new_plan.maneuvers.empty())
1041 {
1043 return;
1044 }
1045 }
1046
1047 if ((safe_distance_to_stop <= distance_remaining_to_traffic_light && desired_distance_to_stop >= distance_remaining_to_traffic_light) ||
1048 last_case_num_ == TSCase::STOPPING) // if stopping continue stopping until transition to planwhenWAITING
1049 {
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); //case_9
1052 return;
1053 }
1054
1055 if (safe_distance_to_stop > distance_remaining_to_traffic_light)
1056 {
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");
1058
1059 if (last_case_num_ != TSCase::STOPPING && last_case_num_ != TSCase::UNAVAILABLE && last_case_num_ != TSCase::CASE_8) //case 1-7 or emergency stop or handlefailure
1060 {
1061 // 3. if not able to stop nor reach target speed at green, attempt its best to reach the target parameters at the 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!");
1063
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);
1066
1067 if (resp->new_plan.maneuvers.empty())
1068 {
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); //case_11 emergency case with twice the normal deceleration
1071 }
1072 }
1073 else
1074 {
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); //case_9
1077 }
1078 }
1079
1080
1081}
1082
1083void LCIStrategicPlugin::planWhenWAITING(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
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)
1086{
1088
1089 if (!traffic_light)
1090 {
1091 throw std::invalid_argument("While in WAITING state, the traffic lights disappeared.");
1092 }
1093
1094 auto stop_line = traffic_light->getStopLine(entry_lanelet);
1095
1096 if (!stop_line)
1097 {
1098 throw std::invalid_argument("Given entry lanelet doesn't have stop_line...");
1099 }
1100
1101 double traffic_light_down_track =
1102 wm_->routeTrackPos(stop_line.get().front().basicPoint2d()).downtrack;
1103
1104 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "traffic_light_down_track: "<< traffic_light_down_track);
1105
1106 double entering_time = current_state.stamp.seconds();
1107
1108 auto current_light_state_optional = traffic_light->predictState(lanelet::time::timeFromSec(entering_time));
1109
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));
1112
1113 if (!validLightState(current_light_state_optional, rclcpp::Time(entering_time * 1e9)))
1114 return;
1115
1116 auto bool_optional_late_certainty = canArriveAtGreenWithCertainty(rclcpp::Time(entering_time * 1e9), traffic_light, true, false);
1117
1118 if (!bool_optional_late_certainty)
1119 {
1120 RCLCPP_ERROR_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Unable to resolve green light...");
1121 return;
1122 }
1123
1124 bool should_enter = true; //uc2
1125
1126 if (config_.enable_carma_streets_connection && entering_time > current_state.stamp.seconds()) //uc3
1127 should_enter = false;
1128
1129 if (isStateAllowedGreen(current_light_state_optional.get().second) && bool_optional_late_certainty.get() && should_enter) // if can make with certainty
1130 {
1131 transition_table_.signal(TransitEvent::RED_TO_GREEN_LIGHT); // If the light is green send the light transition
1132 // signal
1133 return;
1134 }
1135
1136 // A fixed buffer to add to stopping maneuvers once the vehicle is already stopped to ensure that they can have their
1137 // trajectory planned
1138 constexpr double stop_maneuver_buffer = 10.0;
1139
1140 // If the light is not green then continue waiting by creating a stop and wait maneuver on top of the vehicle
1142
1143 resp->new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage(
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),
1146 rclcpp::Time(entering_time * 1e9) + rclcpp::Duration::from_nanoseconds(config_.min_maneuver_planning_period * 1e9), stopping_accel));
1147}
1148
1149void LCIStrategicPlugin::planWhenDEPARTING(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
1150 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp, const VehicleState& current_state,
1151 double intersection_end_downtrack, double intersection_speed_limit)
1152{
1154
1155 if (current_state.downtrack > intersection_end_downtrack)
1156 {
1157 transition_table_.signal(TransitEvent::INTERSECTION_EXIT); // If we are past the most recent
1158 return;
1159 }
1160
1161 // Calculate exit time assuming constant acceleration
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);
1165
1166 // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver
1167 std::vector<lanelet::ConstLanelet> crossed_lanelets =
1168 getLaneletsBetweenWithException(current_state.downtrack, intersection_end_downtrack, true, false);
1169
1170 // Compose intersection transit maneuver
1171 resp->new_plan.maneuvers.push_back(composeIntersectionTransitMessage(
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()));
1174}
1175
1177
1178void LCIStrategicPlugin::mobilityOperationCb(carma_v2x_msgs::msg::MobilityOperation::UniquePtr msg)
1179{
1180 if (msg->strategy == light_controlled_intersection_strategy_)
1181 {
1182 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Received Schedule message with id: " << msg->m_header.plan_id);
1184 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Approaching light Controlled Intersection: " << approaching_light_controlled_intersection_);
1185
1186 if (msg->m_header.recipient_id == config_.vehicle_id)
1187 {
1188 street_msg_timestamp_ = msg->m_header.timestamp;
1189 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "street_msg_timestamp_: " << street_msg_timestamp_);
1190 parseStrategyParams(msg->strategy_params);
1191 previous_strategy_params_ = msg->strategy_params;
1192 }
1193 }
1194
1195}
1196
1197void LCIStrategicPlugin::BSMCb(carma_v2x_msgs::msg::BSM::UniquePtr msg)
1198{
1199 std::vector<uint8_t> bsm_id_vec = msg->core_data.id;
1200 bsm_id_ = BSMHelper::BSMHelper::bsmIDtoString(bsm_id_vec);
1201 bsm_msg_count_ = msg->core_data.msg_count;
1202 bsm_sec_mark_ = msg->core_data.sec_mark;
1203}
1204
1205void LCIStrategicPlugin::parseStrategyParams(const std::string& strategy_params)
1206{
1207 // sample strategy_params: "et:1634067059"
1208 std::string params = strategy_params;
1209 std::vector<std::string> inputsParams;
1210 boost::algorithm::split(inputsParams, params, boost::is_any_of(","));
1211
1212 std::vector<std::string> et_parsed;
1213 boost::algorithm::split(et_parsed, inputsParams[0], boost::is_any_of(":"));
1214 auto new_scheduled_enter_time = std::stoull(et_parsed[1]);
1215
1216 if (scheduled_enter_time_ != new_scheduled_enter_time) //reset green buffer cache so it can be re-evaluated
1218
1219 scheduled_enter_time_ = new_scheduled_enter_time;
1220 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "scheduled_enter_time_: " << scheduled_enter_time_);
1221
1222}
1223
1224carma_v2x_msgs::msg::MobilityOperation LCIStrategicPlugin::generateMobilityOperation()
1225{
1226 carma_v2x_msgs::msg::MobilityOperation mo_;
1227 mo_.m_header.timestamp = now().nanoseconds()/1000000;
1228 mo_.m_header.sender_id = config_.vehicle_id;
1229 mo_.m_header.recipient_id = upcoming_id_;
1230 mo_.m_header.sender_bsm_id = bsm_id_;
1232
1233 double vehicle_acceleration_limit_ = config_.vehicle_accel_limit * config_.vehicle_accel_limit_multiplier;
1234 double vehicle_deceleration_limit_ = -1 * config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier;
1235
1236 std::string intersection_turn_direction = "straight";
1237 if (intersection_turn_direction_ == TurnDirection::Right) intersection_turn_direction = "right";
1238 if (intersection_turn_direction_ == TurnDirection::Left) intersection_turn_direction = "left";
1239
1240 mo_.strategy_params = "access: 1, max_accel: " + std::to_string(vehicle_acceleration_limit_) + // NOTE: Access currently set to 1 at all times since its not specified by streets
1241 ", max_decel: " + std::to_string(vehicle_deceleration_limit_) + ", react_time: " + std::to_string(config_.reaction_time) +
1242 ", min_gap: " + std::to_string(config_.min_gap) + ", depart_pos: 0" + // NOTE: Departure position set to 0 at all times since it's not specified by streets
1243 ", turn_direction: " + intersection_turn_direction + ", msg_count: " + std::to_string(bsm_msg_count_) + ", sec_mark: " + std::to_string(bsm_sec_mark_);
1244
1245
1246 return mo_;
1247}
1248
1249TurnDirection LCIStrategicPlugin::getTurnDirectionAtIntersection(std::vector<lanelet::ConstLanelet> lanelets_list)
1250{
1251 TurnDirection turn_direction = TurnDirection::Straight;
1252 for (auto l:lanelets_list)
1253 {
1254 if(l.hasAttribute("turn_direction")) {
1255 std::string direction_attribute = l.attribute("turn_direction").value();
1256 if (direction_attribute == "right")
1257 {
1258 turn_direction = TurnDirection::Right;
1259 break;
1260 }
1261 else if (direction_attribute == "left")
1262 {
1263 turn_direction = TurnDirection::Left;
1264 break;
1265 }
1266 else turn_direction = TurnDirection::Straight;
1267 }
1268 else
1269 {
1270 // if there is no attribute, assumption is straight
1271 turn_direction = TurnDirection::Straight;
1272 }
1273
1274 }
1275 return turn_direction;
1276}
1277
1279{
1281 {
1282 carma_v2x_msgs::msg::MobilityOperation status_msg = generateMobilityOperation();
1283 mobility_operation_pub_->publish(status_msg);
1284 }
1285}
1286
1288
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)
1293{
1294 std::chrono::system_clock::time_point execution_start_time = std::chrono::system_clock::now(); // Start timing the execution time for planning so it can be logged
1295
1296 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "<<<<<<<<<<<<<<<<< STARTING LCI_STRATEGIC_PLAN!!!!!!!!! >>>>>>>>>>>>>>>>");
1297
1298 if (!wm_->getRoute())
1299 {
1300 RCLCPP_ERROR_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Could not plan maneuvers as route was not available");
1301 return;
1302 }
1303
1305 {
1307 {
1308 resp->new_plan.maneuvers = {};
1309 RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"),"Not approaching light-controlled intersection so no maneuvers");
1310 return;
1311 }
1312
1313 bool is_empty_schedule_msg = (scheduled_enter_time_ == 0);
1314 if (is_empty_schedule_msg)
1315 {
1316 resp->new_plan.maneuvers = {};
1317 RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"),"Receiving empty schedule message");
1318 return;
1319 }
1320 }
1321
1322 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Finding car information");
1323
1324 // Extract vehicle data from request
1325 VehicleState current_state = extractInitialState(req);
1326 if (transition_table_.getState() != TransitState::UNAVAILABLE && !req->prior_plan.maneuvers.empty())
1327 {
1328 RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"),"State is NOT UNAVAILABLE AND Maneuvers in request is NOT empty");
1329 return;
1330 }
1331 // Get current traffic light information
1332 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "\n\nFinding traffic_light information");
1333
1334 auto inter_list = wm_->getSignalizedIntersectionsAlongRoute({ req->veh_x, req->veh_y });
1335 auto upcoming_id_= inter_list.front()->id();
1336
1337 auto traffic_list = wm_->getSignalsAlongRoute({ req->veh_x, req->veh_y });
1338
1339 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Found traffic lights of size: " << traffic_list.size());
1340
1341 auto current_lanelets = wm_->getLaneletsFromPoint({ req->veh_x, req->veh_y});
1342 lanelet::ConstLanelet current_lanelet;
1343
1344 if (current_lanelets.empty())
1345 {
1346 RCLCPP_ERROR_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Given vehicle position is not on the road! Returning...");
1347 return;
1348 }
1349
1350 // get the lanelet that is on the route in case overlapping ones found
1351 auto llt_on_route_optional = wm_->getFirstLaneletOnShortestPath(current_lanelets);
1352
1353 if (llt_on_route_optional){
1354 current_lanelet = llt_on_route_optional.value();
1355 }
1356 else{
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];
1361 }
1362
1363 lanelet::CarmaTrafficSignalPtr nearest_traffic_signal = nullptr;
1364
1365 lanelet::ConstLanelet entry_lanelet;
1366 lanelet::ConstLanelet exit_lanelet;
1367
1368 for (auto signal : traffic_list)
1369 {
1370 auto optional_entry_exit = wm_->getEntryExitOfSignalAlongRoute(signal);
1371 // if signal is not matching our destination skip
1372 if (!optional_entry_exit)
1373 {
1374 RCLCPP_ERROR_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Did not find entry.exit along the route");
1375 continue;
1376 }
1377
1378 entry_lanelet = optional_entry_exit.get().first;
1379 exit_lanelet = optional_entry_exit.get().second;
1380
1381 nearest_traffic_signal = signal;
1382 break;
1383 }
1384
1385
1386 TransitState prev_state;
1387
1388 do
1389 {
1390 // Clear previous maneuvers planned by this plugin as guard against state change since each state generates an
1391 // independent set of maneuvers
1392 resp->new_plan = carma_planning_msgs::msg::ManeuverPlan();
1393
1394 prev_state = transition_table_.getState(); // Cache previous state to check if state has changed after 1 iteration
1395
1396 RCLCPP_INFO_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Planning in state: " << transition_table_.getState());
1397
1398 boost::optional<std::pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState>> current_light_state_optional = boost::none;
1399 if (nearest_traffic_signal)
1400 {
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());
1403 if (current_light_state_optional.get().second == lanelet::CarmaTrafficSignalState::UNAVAILABLE || !current_light_state_optional)
1404 {
1405 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Signal state not available returning..." );
1406 return;
1407 }
1408 }
1409 switch (transition_table_.getState())
1410 {
1412 planWhenUNAVAILABLE(req, resp, current_state, nearest_traffic_signal, entry_lanelet, exit_lanelet, current_lanelet);
1413 break;
1414
1416 planWhenAPPROACHING(req, resp, current_state, nearest_traffic_signal, entry_lanelet, exit_lanelet, current_lanelet);
1417 break;
1418
1420 planWhenWAITING(req, resp, current_state, nearest_traffic_signal, entry_lanelet, exit_lanelet, current_lanelet);
1421 break;
1422
1424 // Reset intersection state since we are no longer in an intersection
1426 {
1427 throw std::invalid_argument("State is DEPARTING but the intersection variables are not available");
1428 }
1429 planWhenDEPARTING(req, resp, current_state, intersection_end_downtrack_.get(), intersection_speed_.get());
1430 break;
1431
1432 default:
1433 throw std::invalid_argument("LCIStrategic Strategic Plugin entered unknown state");
1434 break;
1435 }
1436
1437 } while (transition_table_.getState() != prev_state); // If the state has changed then continue looping
1438
1439 std::chrono::system_clock::time_point execution_end_time = std::chrono::system_clock::now(); // Planning complete
1440
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());
1443
1444 return;
1445 // We need to evaluate the events so the state transitions can be triggered
1446}
1447
1448carma_planning_msgs::msg::Maneuver LCIStrategicPlugin::composeTrajectorySmoothingManeuverMessage(double start_dist, double end_dist, const std::vector<lanelet::ConstLanelet>& crossed_lanelets, double start_speed,
1449 double target_speed, rclcpp::Time start_time, rclcpp::Time end_time,
1450 const TrajectoryParams& tsp) const
1451{
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;
1468
1469 maneuver_msg.lane_following_maneuver.parameters.string_valued_meta_data.push_back(light_controlled_intersection_strategy_);
1470
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_);
1474
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_);
1478
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_);
1482
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));
1485
1486 for (auto llt : crossed_lanelets)
1487 {
1488 maneuver_msg.lane_following_maneuver.lane_ids.push_back(std::to_string(llt.id()));
1489 }
1490 // Start time and end time for maneuver are assigned in updateTimeProgress
1491
1492 RCLCPP_INFO_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Creating TrajectorySmoothingManeuver start dist: " << start_dist << " end dist: " << end_dist
1493 << " start_time: " << std::to_string(start_time.seconds())
1494 << " end_time: " << std::to_string(end_time.seconds()));
1495
1496 return maneuver_msg;
1497}
1498
1499carma_planning_msgs::msg::Maneuver LCIStrategicPlugin::composeStopAndWaitManeuverMessage(double current_dist, double end_dist,
1500 double start_speed,
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
1504{
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;
1510 maneuver_msg.stop_and_wait_maneuver.parameters.planning_tactical_plugin = config_.stop_and_wait_plugin_name;
1511 maneuver_msg.stop_and_wait_maneuver.parameters.planning_strategic_plugin = config_.strategic_plugin_name;
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);
1519
1520 // Set the meta data for the stop location buffer
1521 maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(config_.stopping_location_buffer);
1522 maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(stopping_accel);
1523
1524 RCLCPP_INFO_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Creating stop and wait start dist: " << current_dist << " end dist: " << end_dist
1525 << " start_time: " << std::to_string(start_time.seconds())
1526 << " end_time: " << std::to_string(end_time.seconds())
1527 << " stopping_accel: " << stopping_accel);
1528
1529 return maneuver_msg;
1530}
1531
1532carma_planning_msgs::msg::Maneuver LCIStrategicPlugin::composeIntersectionTransitMessage(double start_dist, double end_dist,
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
1537{
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);
1556
1557 // Start time and end time for maneuver are assigned in updateTimeProgress
1558
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
1562 << " From start_time: " << std::to_string(start_time.seconds())
1563 << " to end_time: " << std::to_string(end_time.seconds()));
1564
1565 return maneuver_msg;
1566}
1567
1569{
1570 return true;
1571}
1572
1574{
1575 return "v1.0";
1576}
1577
1578} // namespace lci_strategic_plugin
1579
1580
1581#include "rclcpp_components/register_node_macro.hpp"
1582
1583// Register the component with class_loader
1584RCLCPP_COMPONENTS_REGISTER_NODE(lci_strategic_plugin::LCIStrategicPlugin)
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 &current_state, const lanelet::CarmaTrafficSignalPtr &traffic_light, const lanelet::ConstLanelet &entry_lanelet, const lanelet::ConstLanelet &exit_lanelet, const lanelet::ConstLanelet &current_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 &current_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 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....
void planWhenDEPARTING(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp, const VehicleState &current_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 &current_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 &current_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...
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 &current_state, const lanelet::CarmaTrafficSignalPtr &traffic_light, const lanelet::ConstLanelet &entry_lanelet, const lanelet::ConstLanelet &exit_lanelet, const lanelet::ConstLanelet &current_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.
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 &current_state, const lanelet::CarmaTrafficSignalPtr &traffic_light, const lanelet::ConstLanelet &entry_lanelet, const lanelet::ConstLanelet &exit_lanelet, const lanelet::ConstLanelet &current_lanelet)
Method for performing maneuver planning when the current plugin state is TransitState::UNAVAILABLE Th...
void publishTrajectorySmoothingInfo()
Publish trajectory smoothing info at a fixed rate.
carma_ros2_utils::PubPtr< carma_v2x_msgs::msg::MobilityOperation > mobility_operation_pub_
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 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 &current_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.
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.
void planWhenAPPROACHING(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp, const VehicleState &current_state, const lanelet::CarmaTrafficSignalPtr &traffic_light, const lanelet::ConstLanelet &entry_lanelet, const lanelet::ConstLanelet &exit_lanelet, const lanelet::ConstLanelet &current_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 > &parameters)
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 EPSILON
#define GET_MANEUVER_PROPERTY(mvr, property)
auto to_string(const UtmZone &zone) -> std::string
Definition: utm_zone.cpp:21
TransitState
Enum describing the possible states of the LCIStrategic Strategic Plugin.
Definition: lci_states.hpp:25
Struct to store the configuration settings for the LCIStrategicPlugin class.
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,...
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 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.