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.
light_controlled_intersection_tactical_plugin.cpp
Go to the documentation of this file.
1/*
2 * Copyright (C) 2022 LEIDOS.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
5 * use this file except in compliance with the License. You may obtain a copy of
6 * the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
12 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
13 * License for the specific language governing permissions and limitations under
14 * the License.
15 */
17#include <valarray>
18
20{
21
22
25 const Config& config,
26 const DebugPublisher& debug_publisher,
27 const std::string& plugin_name,
28 std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode> nh)
29 :wm_(wm), config_(config), nh_(nh), plugin_name_(plugin_name),
30 debug_publisher_(debug_publisher)
31 {
32 }
33
35 const rclcpp::Time& current_time,
36 double min_remaining_time_seconds) const
37 {
38 // Check if we have at least 2 points in the trajectory
39 if (last_trajectory_time_unbound_.trajectory_points.size() < 2)
40 {
41 return false;
42 }
43
44 // Check if the last point's time is sufficiently in the future
45 auto last_point_time = rclcpp::Time(last_trajectory_time_unbound_.trajectory_points.back().target_time);
46
47
48 if (rclcpp::Duration min_time_remaining =
49 rclcpp::Duration::from_seconds(min_remaining_time_seconds);
50 last_point_time <= current_time + min_time_remaining)
51 {
52 return false;
53 }
54
55 // Check if we have case information from previous planning
56 if (is_last_case_successful_ == boost::none || last_case_ == boost::none)
57 {
58 return false;
59 }
60
61 // Ensure we have consistent speed data
62 if (last_speeds_time_unbound_.size() != last_trajectory_time_unbound_.trajectory_points.size())
63 {
64 return false;
65 }
66
67 return true;
68 }
69
71 TSCase new_case, bool is_new_case_successful, const rclcpp::Time& current_time)
72 {
73 // Validate trajectory with 1 second minimum remaining time or vehicle_response_lag seconds
74 // This is because the vehicle executes speed command of vehicle_response_lag secs ahead
75 // Therefore, the trajectory should be enough to cover the vehicle_response_lag
76 if (!isLastTrajectoryValid(current_time, std::max(config_.vehicle_response_lag, 1.0)))
77 {
78 return false;
79 }
80
81 // New case is successful and is same as the last case
82 if (last_case_.get() == new_case && is_new_case_successful == true)
83 {
84 return true;
85 }
86
87 // Edge case - TS Alorithm successful to unsuccessful transition (Case (1-7) to 8)
88 // near the intersection. The vehicle should "lock in" to the last trajectory as it is
89 // expected for the TS algorithm to be unsuccessful near the intersection.
90 if (is_last_case_successful_.get() == true &&
91 is_new_case_successful == false &&
94 last_successful_scheduled_entry_time_ - current_time.seconds() <
96 {
97 return true;
98 }
99
100 // Returning false here means that it should use the new trajectory because:
101 // New case is not same as last case and not within the "lock in" distance
102 return false;
103 }
104
105 carma_planning_msgs::msg::TrajectoryPlan LightControlledIntersectionTacticalPlugin::
107 const std::vector<carma_planning_msgs::msg::Maneuver>& maneuver_plan,
108 const carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr& req,
109 std::vector<double>& final_speeds)
110 {
111 DetailedTrajConfig wpg_detail_config;
112 GeneralTrajConfig wpg_general_config;
113
115 "intersection_transit",
118
120 99.0, // trajectory time length in duration (seconds) is arbitrarily selected
121 // high to generate all at once
128
129 // Create trajectory with raw speed limits from maneuver
130 auto points_and_target_speeds = createGeometryProfile(
131 maneuver_plan, std::max((double)0, current_downtrack_ - config_.back_distance),
132 wm_, ending_state_before_buffer_, req->vehicle_state,
133 wpg_general_config, wpg_detail_config);
134
135 // Apply optimized speed profile
136 applyOptimizedTargetSpeedProfile(maneuver_plan.front(), req->vehicle_state.longitudinal_vel,
137 points_and_target_speeds);
138
139 // Create new trajectory
140 carma_planning_msgs::msg::TrajectoryPlan new_trajectory;
141 new_trajectory.header.frame_id = "map";
142 new_trajectory.header.stamp = req->header.stamp;
143 new_trajectory.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()());
144
145 // Generate points
146 new_trajectory.trajectory_points =
148 points_and_target_speeds,
149 req->vehicle_state, req->header.stamp, wm_, ending_state_before_buffer_, debug_msg_,
150 wpg_detail_config);
151
152 // Save final speeds
153 final_speeds = debug_msg_.velocity_profile;
154
155 return new_trajectory;
156 }
157
159 {
160 if (is_last_case_successful_ != boost::none && last_case_ != boost::none)
161 {
162 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
163 "all variables are set!");
164 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
165 "is_last_case_successful_.get(): " << (int)is_last_case_successful_.get());
166 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
167 "evaluation distance: " << last_successful_ending_downtrack_ - current_downtrack_);
168 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
171 }
172 else
173 {
174 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
175 "Not all variables are set...");
176 }
177
178 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
179 "traj points size: " << last_trajectory_time_unbound_.trajectory_points.size() <<
180 ", last_speeds_time_unbound_ size: " << last_speeds_time_unbound_.size());
181 }
182
184 carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req,
185 carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp)
186 {
187 // Validate request
188 if(req->maneuver_index_to_plan >= req->maneuver_plan.maneuvers.size())
189 {
190 throw std::invalid_argument(
191 "Light Control Intersection Tactical Plugin was asked to plan invalid "
192 "maneuver index: " + std::to_string(req->maneuver_index_to_plan)
193 + " for plan of size: " + std::to_string(req->maneuver_plan.maneuvers.size()));
194 }
195
196 // Extract maneuver plan
197 std::vector<carma_planning_msgs::msg::Maneuver> maneuver_plan;
198 if(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan].type ==
199 carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING
200 && GET_MANEUVER_PROPERTY(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan],
201 parameters.string_valued_meta_data.front()) == light_controlled_intersection_strategy_)
202 {
203 maneuver_plan.push_back(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan]);
204 resp->related_maneuvers.push_back(req->maneuver_index_to_plan);
205 }
206 else
207 {
208 throw std::invalid_argument("Light Control Intersection Tactical Plugin "
209 "was asked to plan unsupported maneuver");
210 }
211
212 // Get vehicle position and update tracking variables
213 lanelet::BasicPoint2d veh_pos(req->vehicle_state.x_pos_global,
214 req->vehicle_state.y_pos_global);
215 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
216 "Planning state x:" << req->vehicle_state.x_pos_global
217 << " , y: " << req->vehicle_state.y_pos_global);
218
219 current_downtrack_ = wm_->routeTrackPos(veh_pos).downtrack;
220 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
221 "Current_downtrack: "<< current_downtrack_);
222
223 // Find current lanelet
224 auto current_lanelets = wm_->getLaneletsFromPoint({req->vehicle_state.x_pos_global,
225 req->vehicle_state.y_pos_global});
226
227 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER), "size: "
228 << current_lanelets.size());
229
230 lanelet::ConstLanelet current_lanelet;
231
232 if (current_lanelets.empty())
233 {
234 RCLCPP_ERROR_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
235 "Given vehicle position is not on the road! Returning...");
236 return;
237 }
238
239 // Get the lanelet that is on the route in case overlapping ones found
240 if (auto llt_on_route_optional = wm_->getFirstLaneletOnShortestPath(current_lanelets);
241 llt_on_route_optional)
242 {
243 current_lanelet = llt_on_route_optional.value();
244 }
245 else
246 {
247 RCLCPP_WARN_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
248 "When identifying the corresponding lanelet for requested trajectory plan's state, "
249 << "x: " << req->vehicle_state.x_pos_global
250 << ", y: " << req->vehicle_state.y_pos_global
251 << ", no possible lanelet was found to be on the shortest path."
252 << "Picking arbitrary lanelet: " << current_lanelets[0].id() << ", instead");
253
254 current_lanelet = current_lanelets[0];
255 }
256
257 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
258 "Current_lanelet: " << current_lanelet.id());
259
260 speed_limit_ = findSpeedLimit(current_lanelet, wm_);
261 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
262 "speed_limit_: " << speed_limit_);
263
264 // Get new case parameters
265 bool is_new_case_successful =
266 GET_MANEUVER_PROPERTY(maneuver_plan.front(), parameters.int_valued_meta_data[1]);
267
268 auto new_case =
269 static_cast<TSCase>GET_MANEUVER_PROPERTY(
270 maneuver_plan.front(), parameters.int_valued_meta_data[0]);
271
272 // Log debug info about previous trajectory
274
275 // Find closest point in last trajectory to current vehicle position
276 size_t idx_to_start_new_traj =
278 last_trajectory_time_unbound_.trajectory_points,
279 veh_pos);
280
281 // Update last trajectory to start from closest point (remove passed points)
282 if (!last_trajectory_time_unbound_.trajectory_points.empty()) {
283 last_speeds_time_unbound_ = std::vector<double>(
284 last_speeds_time_unbound_.begin() + idx_to_start_new_traj,
286 last_trajectory_time_unbound_.trajectory_points =
287 std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint>
288 (last_trajectory_time_unbound_.trajectory_points.begin() + idx_to_start_new_traj,
289 last_trajectory_time_unbound_.trajectory_points.end());
290 }
291
292 // Check if we should use the last trajectory completely
293 auto current_time = rclcpp::Time(req->header.stamp);
294
295
296 auto last_trajectory_time_bound =
299 if (shouldUseLastTrajectory(new_case, is_new_case_successful, current_time))
300 {
301 resp->trajectory_plan = last_trajectory_time_unbound_;
302 resp->trajectory_plan.trajectory_points = last_trajectory_time_bound;
303
304 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
305 "USING LAST TRAJ WITH CASE: " << (int)last_case_.get());
306
307 resp->trajectory_plan.initial_longitudinal_velocity = last_speeds_time_unbound_.front();
308
309 // Set the planning plugin field name
310 for (auto& p : resp->trajectory_plan.trajectory_points) {
311 p.planner_plugin_name = plugin_name_;
312 }
313
314 debug_msg_.trajectory_plan = resp->trajectory_plan;
315 debug_msg_.velocity_profile = last_speeds_time_unbound_;
317 return;
318 }
319
320 // Reaching here means the plugin needs to generate a new trajectory
321 std::vector<double> new_final_speeds;
322 if (carma_planning_msgs::msg::TrajectoryPlan new_trajectory =
323 generateNewTrajectory(maneuver_plan, req, new_final_speeds);
324 new_trajectory.trajectory_points.size() >= 2)
325 {
326 // New trajectory, by default, extends for the whole maneuver duration, so time bound it
327 auto new_trajectory_time_bound =
329 new_trajectory.trajectory_points, config_.trajectory_time_length);
330
331 resp->trajectory_plan = new_trajectory;
332 resp->trajectory_plan.trajectory_points = new_trajectory_time_bound;
333
334 // Update stored trajectories
335 last_trajectory_time_unbound_ = new_trajectory;
336 last_speeds_time_unbound_ = new_final_speeds;
337
338 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
339 "USING NEW TRAJECTORY for case: " << (int)new_case);
340 }
341 // Fall back to last trajectory if new is invalid but last is valid
342 else if (last_trajectory_time_unbound_.trajectory_points.size() >= 2 &&
343 rclcpp::Time(last_trajectory_time_unbound_.trajectory_points.back().target_time) > current_time)
344 {
345 resp->trajectory_plan = last_trajectory_time_unbound_;
346 resp->trajectory_plan.trajectory_points = last_trajectory_time_bound;
347
348 RCLCPP_WARN_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
349 "Failed to generate a new trajectory, so using last valid trajectory!");
350 }
351 // Last resort - return the invalid new trajectory
352 else
353 {
354 resp->trajectory_plan = new_trajectory;
355 RCLCPP_WARN_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
356 "Failed to generate a new trajectory or use old valid trajectory, "
357 "so returning empty/invalid trajectory!");
358 }
359
360 // Update stored case information
361
362 last_case_ = new_case;
363 is_last_case_successful_ = is_new_case_successful;
364
365 // Update variables for next evaluation
366 if (is_new_case_successful) {
367 // LCI Tactical plugin receives only single maneuver that can span multiple lanelets
368 // end of the maneuver is expected to be the start of the intersection
370 GET_MANEUVER_PROPERTY(maneuver_plan.front(), end_dist);
371
372 // Entry time to the intersection
374 rclcpp::Time(GET_MANEUVER_PROPERTY(maneuver_plan.front(), end_time)).seconds();
375
376 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
377 "last_successful_ending_downtrack_:" << last_successful_ending_downtrack_ <<
378 ", last_successful_scheduled_entry_time_: " <<
380 }
381
382 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
383 "Debug: new case:" << (int) new_case << ", is_new_case_successful: "
384 << is_new_case_successful);
385
386 resp->maneuver_status.push_back(
387 carma_planning_msgs::srv::PlanTrajectory::Response::MANEUVER_IN_PROGRESS);
388 resp->trajectory_plan.initial_longitudinal_velocity = last_speeds_time_unbound_.front();
389
390 // Set the planning plugin field name
391 for (auto& p : resp->trajectory_plan.trajectory_points) {
392 p.planner_plugin_name = plugin_name_;
393 }
394
395 debug_msg_.trajectory_plan = resp->trajectory_plan;
396 debug_msg_.velocity_profile = last_speeds_time_unbound_;
398 }
399
401 carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req,
402 carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp)
403 {
404 std::chrono::system_clock::time_point start_time = std::chrono::system_clock::now();
405
406 latest_traj_request_header_stamp_ = rclcpp::Time(req->header.stamp); //for debugging
407
408 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
409 "Starting light controlled intersection trajectory planning");
410
411 // Call the function to plan trajectory without yield
412 planTrajectorySmoothing(req, resp);
413
414 // Yield for potential obstacles in the road
415 if (config_.enable_object_avoidance && resp->trajectory_plan.trajectory_points.size() >= 2)
416 {
419 }
420 else
421 {
422 RCLCPP_DEBUG(rclcpp::get_logger(LCI_TACTICAL_LOGGER), "Ignored Object Avoidance");
423 }
424
425 std::chrono::system_clock::time_point end_time = std::chrono::system_clock::now();
426 auto duration = end_time - start_time;
427
428 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
429 "ExecutionTime: " << std::chrono::duration<double>(duration).count());
430 }
431
432 void LightControlledIntersectionTacticalPlugin::applyTrajectorySmoothingAlgorithm(const carma_wm::WorldModelConstPtr& wm, std::vector<PointSpeedPair>& points_and_target_speeds, double start_dist, double remaining_dist,
433 double starting_speed, double departure_speed, TrajectoryParams tsp)
434 {
435 if (points_and_target_speeds.empty())
436 {
437 throw std::invalid_argument("Point and target speed list is empty! Unable to apply case one speed profile...");
438 }
439
440 // Checking route geometry start against start_dist and adjust profile
441 double planning_downtrack_start = wm->routeTrackPos(points_and_target_speeds[0].point).downtrack; // this can include buffered points earlier than maneuver start_dist
442
443 //Check calculated total dist against maneuver limits
444 double total_distance_needed = remaining_dist;
445 double dist1 = tsp.x1_ - start_dist;
446 double dist2 = tsp.x2_ - start_dist;
447 double dist3 = tsp.x3_ - start_dist;
448
449 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER), "total_distance_needed: " << total_distance_needed << "\n" <<
450 "dist1: " << dist1 << "\n" <<
451 "dist2: " << dist2 << "\n" <<
452 "dist3: " << dist3);
453 double algo_min_speed = std::min({tsp.v1_,tsp.v2_,tsp.v3_});
454 double algo_max_speed = std::max({tsp.v1_,tsp.v2_,tsp.v3_});
455
456 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER), "found algo_minimum_speed: " << algo_min_speed << "\n" <<
457 "algo_max_speed: " << algo_max_speed);
458
459 double total_dist_planned = 0; //Starting dist for maneuver treated as 0.0
460
461 if (planning_downtrack_start < start_dist)
462 {
463 //Account for the buffer distance that is technically not part of this maneuver
464
465 total_dist_planned = planning_downtrack_start - start_dist;
466 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER), "buffered section is present. Adjusted total_dist_planned to: " << total_dist_planned);
467 }
468
469 double prev_speed = starting_speed;
470 auto prev_point = points_and_target_speeds.front();
471
472 for(auto& p : points_and_target_speeds)
473 {
474 double delta_d = lanelet::geometry::distance2d(prev_point.point, p.point);
475 total_dist_planned += delta_d;
476
477 //Apply the speed from algorithm at dist covered
478 //Kinematic: v_f = sqrt(v_o^2 + 2*a*d)
479 double speed_i;
480 if (total_dist_planned <= epsilon_)
481 {
482 //Keep target speed same for buffer distance portion
483 speed_i = starting_speed;
484 }
485 else if(total_dist_planned <= dist1 + epsilon_){
486 //First segment
487 speed_i = sqrt(pow(starting_speed, 2) + 2 * tsp.a1_ * total_dist_planned);
488 }
489 else if(total_dist_planned > dist1 && total_dist_planned <= dist2 + epsilon_){
490 //Second segment
491 speed_i = sqrt(std::max(pow(tsp.v1_, 2) + 2 * tsp.a2_ * (total_dist_planned - dist1), 0.0)); //std::max to ensure negative value is not sqrt
492 }
493 else if (total_dist_planned > dist2 && total_dist_planned <= dist3 + epsilon_)
494 {
495 //Third segment
496 speed_i = sqrt(std::max(pow(tsp.v2_, 2) + 2 * tsp.a3_ * (total_dist_planned - dist2), 0.0)); //std::max to ensure negative value is not sqrt
497 }
498 else
499 {
500 //buffer points that will be cut
501 speed_i = prev_speed;
502 }
503
504 if (isnan(speed_i))
505 {
506 speed_i = std::max(config_.minimum_speed, algo_min_speed);
507 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER), "Detected nan number from equations. Set to " << speed_i);
508 }
509
510 p.speed = std::max({speed_i, config_.minimum_speed, algo_min_speed});
511 p.speed = std::min({p.speed, speed_limit_, algo_max_speed});
512 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER), "Applied speed: " << p.speed << ", at dist: " << total_dist_planned);
513
514 prev_point = p;
515 prev_speed = p.speed;
516 }
517 }
518
519 void LightControlledIntersectionTacticalPlugin::applyOptimizedTargetSpeedProfile(const carma_planning_msgs::msg::Maneuver& maneuver, const double starting_speed, std::vector<PointSpeedPair>& points_and_target_speeds)
520 {
521 if(GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data).size() < 9 ||
522 GET_MANEUVER_PROPERTY(maneuver, parameters.int_valued_meta_data).size() < 2 ){
523 throw std::invalid_argument("There must be 9 float_valued_meta_data and 2 int_valued_meta_data to apply algorithm's parameters.");
524 }
525
527
528 tsp.a1_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[0]);
529 tsp.v1_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[1]);
530 tsp.x1_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[2]);
531
532 tsp.a2_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[3]);
533 tsp.v2_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[4]);
534 tsp.x2_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[5]);
535
536 tsp.a3_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[6]);
537 tsp.v3_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[7]);
538 tsp.x3_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[8]);
539
540 double starting_downtrack = GET_MANEUVER_PROPERTY(maneuver, start_dist);
541 double ending_downtrack = GET_MANEUVER_PROPERTY(maneuver, end_dist);
542 double departure_speed = GET_MANEUVER_PROPERTY(maneuver, end_speed);
543 double scheduled_entry_time = rclcpp::Time(GET_MANEUVER_PROPERTY(maneuver, end_time)).seconds();
544 double entry_dist = ending_downtrack - starting_downtrack;
545
546 // change speed profile depending on algorithm case starting from maneuver start_dist
547 applyTrajectorySmoothingAlgorithm(wm_, points_and_target_speeds, starting_downtrack,
548 entry_dist, starting_speed, departure_speed, tsp);
549 }
550
551 double LightControlledIntersectionTacticalPlugin::findSpeedLimit(const lanelet::ConstLanelet& llt, const carma_wm::WorldModelConstPtr &wm) const
552 {
553 lanelet::Optional<carma_wm::TrafficRulesConstPtr> traffic_rules = wm->getTrafficRules();
554 if (traffic_rules)
555 {
556 return (*traffic_rules)->speedLimit(llt).speedLimit.value();
557 }
558 else
559 {
560 throw std::invalid_argument("Valid traffic rules object could not be built");
561 }
562 }
563
564 std::vector<PointSpeedPair> LightControlledIntersectionTacticalPlugin::createGeometryProfile(const std::vector<carma_planning_msgs::msg::Maneuver> &maneuvers, double max_starting_downtrack,const carma_wm::WorldModelConstPtr &wm,
565 carma_planning_msgs::msg::VehicleState &ending_state_before_buffer,const carma_planning_msgs::msg::VehicleState& state,
566 const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config)
567 {
568 std::vector<PointSpeedPair> points_and_target_speeds;
569
570 bool first = true;
571 std::unordered_set<lanelet::Id> visited_lanelets;
572 std::vector<carma_planning_msgs::msg::Maneuver> processed_maneuvers;
573 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER), "VehDowntrack: "<<max_starting_downtrack);
574
575 // Only one maneuver is expected in the received maneuver plan
576 if(maneuvers.size() == 1)
577 {
578 auto maneuver = maneuvers.front();
579
580 double starting_downtrack = GET_MANEUVER_PROPERTY(maneuver, start_dist);
581
582 starting_downtrack = std::min(starting_downtrack, max_starting_downtrack);
583
584 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER), "Used downtrack: " << starting_downtrack);
585
586 // check if required parameter from strategic planner is present
587 if(GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data).empty())
588 {
589 throw std::invalid_argument("No time_to_schedule_entry is provided in float_valued_meta_data");
590 }
591
592 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER), "Creating Lane Follow Geometry");
593 std::vector<PointSpeedPair> lane_follow_points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, starting_downtrack, wm, general_config, detailed_config, visited_lanelets);
594 points_and_target_speeds.insert(points_and_target_speeds.end(), lane_follow_points.begin(), lane_follow_points.end());
595 processed_maneuvers.push_back(maneuver);
596 }
597 else
598 {
599 throw std::invalid_argument("Light Control Intersection Tactical Plugin currently can"
600 " only create a geometry profile for one maneuver");
601 }
602
603 //Add buffer ending to lane follow points at the end of maneuver(s) end dist
604 if(!processed_maneuvers.empty() && processed_maneuvers.back().type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING){
605 points_and_target_speeds = add_lanefollow_buffer(wm, points_and_target_speeds, processed_maneuvers, ending_state_before_buffer, detailed_config);
606 }
607
608 return points_and_target_speeds;
609 }
610
612 {
613 config_ = config;
614 }
615
616 void LightControlledIntersectionTacticalPlugin::set_yield_client(carma_ros2_utils::ClientPtr<carma_planning_msgs::srv::PlanTrajectory> client)
617 {
618 yield_client_ = client;
619 }
620
621
622} // light_controlled_intersection_tactical_plugin
#define GET_MANEUVER_PROPERTY(mvr, property)
Macro definition to enable easier access to fields shared across the maneuver types.
void planTrajectoryCB(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp)
Function to process the light controlled intersection tactical plugin service call for trajectory pla...
void applyTrajectorySmoothingAlgorithm(const carma_wm::WorldModelConstPtr &wm, std::vector< PointSpeedPair > &points_and_target_speeds, double start_dist, double remaining_dist, double starting_speed, double departure_speed, TrajectoryParams tsp)
Creates a speed profile according to case one or two of the light controlled intersection,...
bool isLastTrajectoryValid(const rclcpp::Time &current_time, double min_remaining_time_seconds=0.0) const
Checks if the last trajectory plan remains valid based on the current time.
void applyOptimizedTargetSpeedProfile(const carma_planning_msgs::msg::Maneuver &maneuver, const double starting_speed, std::vector< PointSpeedPair > &points_and_target_speeds)
Apply optimized target speeds to the trajectory determined for fixed-time and actuated signals....
void planTrajectorySmoothing(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp)
Smooths the trajectory as part of the trajectory planning process.
carma_planning_msgs::msg::TrajectoryPlan generateNewTrajectory(const std::vector< carma_planning_msgs::msg::Maneuver > &maneuver_plan, const carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr &req, std::vector< double > &final_speeds)
Generates a new trajectory plan based on the provided maneuver plan and request. NOTE: This function ...
LightControlledIntersectionTacticalPlugin(carma_wm::WorldModelConstPtr wm, const Config &config, const DebugPublisher &debug_publisher, const std::string &plugin_name, std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode > nh)
LightControlledIntersectionTacticalPlugin constructor.
double findSpeedLimit(const lanelet::ConstLanelet &llt, const carma_wm::WorldModelConstPtr &wm) const
Given a Lanelet, find its associated Speed Limit.
void set_yield_client(carma_ros2_utils::ClientPtr< carma_planning_msgs::srv::PlanTrajectory > client)
set the yield service
bool shouldUseLastTrajectory(TSCase new_case, bool is_new_case_successful, const rclcpp::Time &current_time)
Determines whether the last trajectory should be reused based on the planning case....
std::vector< PointSpeedPair > createGeometryProfile(const std::vector< carma_planning_msgs::msg::Maneuver > &maneuvers, double max_starting_downtrack, const carma_wm::WorldModelConstPtr &wm, carma_planning_msgs::msg::VehicleState &ending_state_before_buffer, const carma_planning_msgs::msg::VehicleState &state, const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config)
Creates geometry profile to return a point speed pair struct for INTERSECTION_TRANSIT maneuver types ...
GeneralTrajConfig compose_general_trajectory_config(const std::string &trajectory_type, int default_downsample_ratio, int turn_downsample_ratio)
carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr modify_trajectory_to_yield_to_obstacles(const std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode > &node_handler, const carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr &req, const carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr &resp, const carma_ros2_utils::ClientPtr< carma_planning_msgs::srv::PlanTrajectory > &yield_client, int yield_plugin_service_call_timeout)
Applies a yield trajectory to the original trajectory set in response.
DetailedTrajConfig compose_detailed_trajectory_config(double trajectory_time_length, double curve_resample_step_size, double minimum_speed, double max_accel, double lateral_accel_limit, int speed_moving_average_window_size, int curvature_moving_average_window_size, double back_distance, double buffer_ending_downtrack, std::string desired_controller_plugin="default")
int get_nearest_point_index(const std::vector< lanelet::BasicPoint2d > &points, const carma_planning_msgs::msg::VehicleState &state)
Returns the nearest point (in terms of cartesian 2d distance) to the provided vehicle pose in the pro...
std::vector< PointSpeedPair > create_lanefollow_geometry(const carma_planning_msgs::msg::Maneuver &maneuver, double max_starting_downtrack, const carma_wm::WorldModelConstPtr &wm, const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config, std::unordered_set< lanelet::Id > &visited_lanelets)
Converts a set of requested LANE_FOLLOWING maneuvers to point speed limit pairs.
std::vector< PointSpeedPair > add_lanefollow_buffer(const carma_wm::WorldModelConstPtr &wm, std::vector< PointSpeedPair > &points_and_target_speeds, const std::vector< carma_planning_msgs::msg::Maneuver > &maneuvers, carma_planning_msgs::msg::VehicleState &ending_state_before_buffer, const DetailedTrajConfig &detailed_config)
Adds extra centerline points beyond required message length to lane follow maneuver points so that th...
std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > compose_lanefollow_trajectory_from_path(const std::vector< PointSpeedPair > &points, const carma_planning_msgs::msg::VehicleState &state, const rclcpp::Time &state_time, const carma_wm::WorldModelConstPtr &wm, const carma_planning_msgs::msg::VehicleState &ending_state_before_buffer, carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds &debug_msg, const DetailedTrajConfig &detailed_config)
Method converts a list of lanelet centerline points and current vehicle state into a usable list of t...
std::vector< PointSpeedPair > constrain_to_time_boundary(const std::vector< PointSpeedPair > &points, double time_span)
Reduces the input points to only those points that fit within the provided time boundary.
auto to_string(const UtmZone &zone) -> std::string
Definition: utm_zone.cpp:21
std::shared_ptr< const WorldModel > WorldModelConstPtr
Definition: WorldModel.hpp:454
std::function< void(const carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds &)> DebugPublisher
Stuct containing the algorithm configuration values for light_controlled_intersection_tactical_plugin...