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
24 std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode> nh)
25 :wm_(wm), config_(config), plugin_name_(plugin_name), nh_(nh)
26 {
27 }
28
30 carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req,
31 carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp)
32 {
33 std::chrono::system_clock::time_point start_time = std::chrono::system_clock::now(); // Start timing the execution time for planning so it can be logged
34
35 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Starting light controlled intersection trajectory planning");
36
37 if(req->maneuver_index_to_plan >= req->maneuver_plan.maneuvers.size())
38 {
39 throw std::invalid_argument(
40 "Light Control Intersection Plugin asked to plan invalid maneuver index: " + std::to_string(req->maneuver_index_to_plan) +
41 " for plan of size: " + std::to_string(req->maneuver_plan.maneuvers.size()));
42 }
43
44 std::vector<carma_planning_msgs::msg::Maneuver> maneuver_plan;
45 // Expecting only one maneuver for an intersection, which corresponds to the maneuver_index_to_plan value provided in the request
46 if(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan].type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING
47 && GET_MANEUVER_PROPERTY(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan], parameters.string_valued_meta_data.front()) == light_controlled_intersection_strategy_)
48 {
49 maneuver_plan.push_back(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan]);
50 resp->related_maneuvers.push_back(req->maneuver_index_to_plan);
51 }
52 else
53 {
54 throw std::invalid_argument("Light Control Intersection Plugin asked to plan unsupported maneuver");
55 }
56
57 lanelet::BasicPoint2d veh_pos(req->vehicle_state.x_pos_global, req->vehicle_state.y_pos_global);
58 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Planning state x:" << req->vehicle_state.x_pos_global << " , y: " << req->vehicle_state.y_pos_global);
59
60 current_downtrack_ = wm_->routeTrackPos(veh_pos).downtrack;
61
62 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Current_downtrack: "<< current_downtrack_);
63
64 auto current_lanelets = wm_->getLaneletsFromPoint({req->vehicle_state.x_pos_global, req->vehicle_state.y_pos_global});
65 lanelet::ConstLanelet current_lanelet;
66
67 if (current_lanelets.empty())
68 {
69 RCLCPP_ERROR_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Given vehicle position is not on the road! Returning...");
70 return;
71 }
72
73 // get the lanelet that is on the route in case overlapping ones found
74 auto llt_on_route_optional = wm_->getFirstLaneletOnShortestPath(current_lanelets);
75
76 if (llt_on_route_optional){
77 current_lanelet = llt_on_route_optional.value();
78 }
79 else{
80 RCLCPP_WARN_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "When identifying the corresponding lanelet for requested trajectory plan's state, x: "
81 << req->vehicle_state.x_pos_global << ", y: " << req->vehicle_state.y_pos_global << ", no possible lanelet was found to be on the shortest path."
82 << "Picking arbitrary lanelet: " << current_lanelets[0].id() << ", instead");
83 current_lanelet = current_lanelets[0];
84 }
85
86 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Current_lanelet: " << current_lanelet.id());
87
88 speed_limit_ = findSpeedLimit(current_lanelet, wm_);
89
90 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "speed_limit_: " << speed_limit_);
91
92 DetailedTrajConfig wpg_detail_config;
93 GeneralTrajConfig wpg_general_config;
94
95 wpg_general_config = basic_autonomy:: waypoint_generation::compose_general_trajectory_config("intersection_transit",
98
106
107 // CHECK IF LAST TRAJECTORY WILL BE USED
108
109 bool is_new_case_successful = GET_MANEUVER_PROPERTY(maneuver_plan.front(), parameters.int_valued_meta_data[1]);
110 TSCase new_case = static_cast<TSCase>GET_MANEUVER_PROPERTY(maneuver_plan.front(), parameters.int_valued_meta_data[0]);
111
112 if (is_last_case_successful_ != boost::none && last_case_ != boost::none)
113 {
114 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "all variables are set!");
115 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "is_last_case_successful_.get(): " << (int)is_last_case_successful_.get());
116 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "evaluation distance: " << last_successful_ending_downtrack_ - current_downtrack_);
117
118 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "evaluation time: " << std::to_string(last_successful_scheduled_entry_time_ - rclcpp::Time(req->header.stamp).seconds()));
119 }
120 else
121 {
122 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Not all variables are set...");
123 }
124
125
126
127 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "traj points size: " << last_trajectory_.trajectory_points.size() << ", last_final_speeds_ size: " <<
128 last_final_speeds_.size() );
129
130 size_t idx_to_start_new_traj = 0;
131 for (size_t i = 0; i < last_trajectory_.trajectory_points.size(); i++)
132 {
133 auto dist = sqrt(pow(veh_pos.x() - last_trajectory_.trajectory_points.at(i).x, 2) + std::pow(veh_pos.y() - last_trajectory_.trajectory_points.at(i).y, 2));
134
135 if (dist < 2.0) // reduce until if 2 meters away
136 {
137 idx_to_start_new_traj = i;
138 break;
139 }
140 }
141
142 last_final_speeds_ = std::vector<double>(last_final_speeds_.begin() + idx_to_start_new_traj, last_final_speeds_.end());
143 last_trajectory_.trajectory_points = std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint>(last_trajectory_.trajectory_points.begin() + idx_to_start_new_traj, last_trajectory_.trajectory_points.end());
144
145 if (is_last_case_successful_ != boost::none && last_case_ != boost::none
146 && last_case_.get() == new_case
147 && is_new_case_successful == true
148 && last_trajectory_.trajectory_points.size() >= 2
149 && rclcpp::Time(last_trajectory_.trajectory_points.back().target_time) > rclcpp::Time(req->header.stamp) + rclcpp::Duration(1 * 1e9)) // Duration is in nanoseconds
150 {
151 resp->trajectory_plan = last_trajectory_;
152 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Last TRAJ's target time: " << std::to_string(rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds()) << ", and stamp:" << std::to_string(rclcpp::Time(req->header.stamp).seconds()));
153 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "USING LAST TRAJ: " << (int)last_case_.get());
154 }
155 else if (is_last_case_successful_ != boost::none && last_case_ != boost::none
156 && is_last_case_successful_.get() == true
157 && is_new_case_successful == false
159 && last_successful_scheduled_entry_time_ - rclcpp::Time(req->header.stamp).seconds() < config_.algorithm_evaluation_period
160 && last_trajectory_.trajectory_points.size() >= 2
161 && rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds() > rclcpp::Time(req->header.stamp).seconds())
162 {
163 resp->trajectory_plan = last_trajectory_;
164 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Last Traj's target time: " << std::to_string(rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds()) << ", and stamp:" << std::to_string(rclcpp::Time(req->header.stamp).seconds()) << ", and scheduled: " << std::to_string(last_successful_scheduled_entry_time_));
165 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "EDGE CASE: USING LAST TRAJ: " << (int)last_case_.get());
166 }
167
168 if (!resp->trajectory_plan.trajectory_points.empty()) // if has valid trajectory saved from before return
169 {
170 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Debug: new case:" << (int) new_case << ", is_new_case_successful: " << is_new_case_successful);
171
172 resp->trajectory_plan.initial_longitudinal_velocity = last_final_speeds_.front();
173
174 resp->maneuver_status.push_back(carma_planning_msgs::srv::PlanTrajectory::Response::MANEUVER_IN_PROGRESS);
175
176 std::chrono::system_clock::time_point end_time = std::chrono::system_clock::now(); // Planning complete
177
178 auto duration = end_time - start_time;
179 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "ExecutionTime Using New: " << std::chrono::duration<double>(duration).count());
180
181 return;
182 }
183
184 // IF NOT USING LAST TRAJECTORY PLAN NEW TRAJECTORY
185
186 // Create curve-fitting compatible trajectories (with extra back and front attached points) with raw speed limits from maneuver
187 auto points_and_target_speeds = createGeometryProfile(maneuver_plan, std::max((double)0, current_downtrack_ - config_.back_distance),
188 wm_, ending_state_before_buffer_, req->vehicle_state, wpg_general_config, wpg_detail_config);
189
190 // Change raw speed limit values to target speeds specified by the algorithm
191 applyOptimizedTargetSpeedProfile(maneuver_plan.front(), req->vehicle_state.longitudinal_vel, points_and_target_speeds);
192
193 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "points_and_target_speeds: " << points_and_target_speeds.size());
194
195 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "PlanTrajectory");
196
197 //Trajectory Plan
198 carma_planning_msgs::msg::TrajectoryPlan trajectory;
199 trajectory.header.frame_id = "map";
200 trajectory.header.stamp = req->header.stamp;
201 trajectory.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()());
202
203 // Compose smooth trajectory/speed by resampling
204 trajectory.trajectory_points = basic_autonomy::waypoint_generation::compose_lanefollow_trajectory_from_path(points_and_target_speeds,
205 req->vehicle_state, req->header.stamp, wm_, ending_state_before_buffer_, debug_msg_,
206 wpg_detail_config); // Compute the trajectory
207
208 // Set the planning plugin field name
209 for (auto& p : trajectory.trajectory_points) {
210 p.planner_plugin_name = plugin_name_;
211 }
212
213 if (trajectory.trajectory_points.size () < 2)
214 {
215 if (last_trajectory_.trajectory_points.size() >= 2
216 && rclcpp::Time(last_trajectory_.trajectory_points.back().target_time) > rclcpp::Time(req->header.stamp))
217 {
218 resp->trajectory_plan = last_trajectory_;
219 RCLCPP_WARN_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Failed to generate new trajectory, so using last valid trajectory!");
220 }
221 else
222 {
223 resp->trajectory_plan = trajectory;
224 RCLCPP_WARN_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Failed to generate new trajectory or use old valid trajectory, so returning empty/invalid trajectory!");
225 }
226 }
227 else
228 {
229 last_trajectory_ = trajectory;
230 resp->trajectory_plan = trajectory;
231 last_case_ = new_case;
232 last_final_speeds_ = debug_msg_.velocity_profile;
233 is_last_case_successful_ = is_new_case_successful;
234 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "USING NEW: Target time: " << std::to_string(rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds()) << ", and stamp:" << std::to_string(rclcpp::Time(req->header.stamp).seconds()));
235 if (is_new_case_successful)
236 {
237 last_successful_ending_downtrack_ = GET_MANEUVER_PROPERTY(maneuver_plan.front(), end_dist); // if algorithm was successful, this is traffic_light_downtrack
238 last_successful_scheduled_entry_time_ = rclcpp::Time(GET_MANEUVER_PROPERTY(maneuver_plan.front(), end_time)).seconds(); // if algorithm was successful, this is also scheduled entry time (ET in TSMO UC2 Algo)
239 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "last_successful_ending_downtrack_:" << last_successful_ending_downtrack_ << ", last_successful_scheduled_entry_time_: " << std::to_string(last_successful_scheduled_entry_time_));
240 }
241 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "USING NEW CASE!!! : " << (int)last_case_.get());
242
243 }
244 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Debug: new case:" << (int) new_case << ", is_new_case_successful: " << is_new_case_successful);
245
246 resp->trajectory_plan.initial_longitudinal_velocity = last_final_speeds_.front();
247
248 // Yield for potential obstacles in the road
249 // Aside from the flag, yield_plugin should not be called on invalid trajectories
250 if (config_.enable_object_avoidance && resp->trajectory_plan.trajectory_points.size() >= 2)
251 {
253 }
254 else
255 {
256 RCLCPP_DEBUG(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Ignored Object Avoidance");
257 }
258
259 resp->maneuver_status.push_back(carma_planning_msgs::srv::PlanTrajectory::Response::MANEUVER_IN_PROGRESS);
260
261 std::chrono::system_clock::time_point end_time = std::chrono::system_clock::now(); // Planning complete
262
263 auto duration = end_time - start_time;
264 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "ExecutionTime Using Last: " << std::chrono::duration<double>(duration).count());
265
266 return;
267 }
268
269 void LightControlledIntersectionTacticalPlugin::applyTrajectorySmoothingAlgorithm(const carma_wm::WorldModelConstPtr& wm, std::vector<PointSpeedPair>& points_and_target_speeds, double start_dist, double remaining_dist,
270 double starting_speed, double departure_speed, TrajectoryParams tsp)
271 {
272 if (points_and_target_speeds.empty())
273 {
274 throw std::invalid_argument("Point and target speed list is empty! Unable to apply case one speed profile...");
275 }
276
277 // Checking route geometry start against start_dist and adjust profile
278 double planning_downtrack_start = wm->routeTrackPos(points_and_target_speeds[0].point).downtrack; // this can include buffered points earlier than maneuver start_dist
279
280 //Check calculated total dist against maneuver limits
281 double total_distance_needed = remaining_dist;
282 double dist1 = tsp.x1_ - start_dist;
283 double dist2 = tsp.x2_ - start_dist;
284 double dist3 = tsp.x3_ - start_dist;
285
286 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "total_distance_needed: " << total_distance_needed << "\n" <<
287 "dist1: " << dist1 << "\n" <<
288 "dist2: " << dist2 << "\n" <<
289 "dist3: " << dist3);
290 double algo_min_speed = std::min({tsp.v1_,tsp.v2_,tsp.v3_});
291 double algo_max_speed = std::max({tsp.v1_,tsp.v2_,tsp.v3_});
292
293 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "found algo_minimum_speed: " << algo_min_speed << "\n" <<
294 "algo_max_speed: " << algo_max_speed);
295
296 double total_dist_planned = 0; //Starting dist for maneuver treated as 0.0
297
298 if (planning_downtrack_start < start_dist)
299 {
300 //Account for the buffer distance that is technically not part of this maneuver
301
302 total_dist_planned = planning_downtrack_start - start_dist;
303 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "buffered section is present. Adjusted total_dist_planned to: " << total_dist_planned);
304 }
305
306 double prev_speed = starting_speed;
307 auto prev_point = points_and_target_speeds.front();
308
309 for(auto& p : points_and_target_speeds)
310 {
311 double delta_d = lanelet::geometry::distance2d(prev_point.point, p.point);
312 total_dist_planned += delta_d;
313
314 //Apply the speed from algorithm at dist covered
315 //Kinematic: v_f = sqrt(v_o^2 + 2*a*d)
316 double speed_i;
317 if (total_dist_planned <= epsilon_)
318 {
319 //Keep target speed same for buffer distance portion
320 speed_i = starting_speed;
321 }
322 else if(total_dist_planned <= dist1 + epsilon_){
323 //First segment
324 speed_i = sqrt(pow(starting_speed, 2) + 2 * tsp.a1_ * total_dist_planned);
325 }
326 else if(total_dist_planned > dist1 && total_dist_planned <= dist2 + epsilon_){
327 //Second segment
328 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
329 }
330 else if (total_dist_planned > dist2 && total_dist_planned <= dist3 + epsilon_)
331 {
332 //Third segment
333 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
334 }
335 else
336 {
337 //buffer points that will be cut
338 speed_i = prev_speed;
339 }
340
341 if (isnan(speed_i))
342 {
343 speed_i = std::max(config_.minimum_speed, algo_min_speed);
344 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Detected nan number from equations. Set to " << speed_i);
345 }
346
347 p.speed = std::max({speed_i, config_.minimum_speed, algo_min_speed});
348 p.speed = std::min({p.speed, speed_limit_, algo_max_speed});
349 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Applied speed: " << p.speed << ", at dist: " << total_dist_planned);
350
351 prev_point = p;
352 prev_speed = p.speed;
353 }
354 }
355
356 void LightControlledIntersectionTacticalPlugin::applyOptimizedTargetSpeedProfile(const carma_planning_msgs::msg::Maneuver& maneuver, const double starting_speed, std::vector<PointSpeedPair>& points_and_target_speeds)
357 {
358 if(GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data).size() < 9 ||
359 GET_MANEUVER_PROPERTY(maneuver, parameters.int_valued_meta_data).size() < 2 ){
360 throw std::invalid_argument("There must be 9 float_valued_meta_data and 2 int_valued_meta_data to apply algorithm's parameters.");
361 }
362
364
365 tsp.a1_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[0]);
366 tsp.v1_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[1]);
367 tsp.x1_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[2]);
368
369 tsp.a2_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[3]);
370 tsp.v2_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[4]);
371 tsp.x2_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[5]);
372
373 tsp.a3_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[6]);
374 tsp.v3_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[7]);
375 tsp.x3_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[8]);
376
377 double starting_downtrack = GET_MANEUVER_PROPERTY(maneuver, start_dist);
378 double ending_downtrack = GET_MANEUVER_PROPERTY(maneuver, end_dist);
379 double departure_speed = GET_MANEUVER_PROPERTY(maneuver, end_speed);
380 double scheduled_entry_time = rclcpp::Time(GET_MANEUVER_PROPERTY(maneuver, end_time)).seconds();
381 double entry_dist = ending_downtrack - starting_downtrack;
382
383 // change speed profile depending on algorithm case starting from maneuver start_dist
384 applyTrajectorySmoothingAlgorithm(wm_, points_and_target_speeds, starting_downtrack, entry_dist, starting_speed,
385 departure_speed, tsp);
386 }
387
388 double LightControlledIntersectionTacticalPlugin::findSpeedLimit(const lanelet::ConstLanelet& llt, const carma_wm::WorldModelConstPtr &wm) const
389 {
390 lanelet::Optional<carma_wm::TrafficRulesConstPtr> traffic_rules = wm->getTrafficRules();
391 if (traffic_rules)
392 {
393 return (*traffic_rules)->speedLimit(llt).speedLimit.value();
394 }
395 else
396 {
397 throw std::invalid_argument("Valid traffic rules object could not be built");
398 }
399 }
400
401 std::vector<PointSpeedPair> LightControlledIntersectionTacticalPlugin::createGeometryProfile(const std::vector<carma_planning_msgs::msg::Maneuver> &maneuvers, double max_starting_downtrack,const carma_wm::WorldModelConstPtr &wm,
402 carma_planning_msgs::msg::VehicleState &ending_state_before_buffer,const carma_planning_msgs::msg::VehicleState& state,
403 const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config)
404 {
405 std::vector<PointSpeedPair> points_and_target_speeds;
406
407 bool first = true;
408 std::unordered_set<lanelet::Id> visited_lanelets;
409 std::vector<carma_planning_msgs::msg::Maneuver> processed_maneuvers;
410 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "VehDowntrack: "<<max_starting_downtrack);
411
412 // Only one maneuver is expected in the received maneuver plan
413 if(maneuvers.size() == 1)
414 {
415 auto maneuver = maneuvers.front();
416
417 double starting_downtrack = GET_MANEUVER_PROPERTY(maneuver, start_dist);
418
419 starting_downtrack = std::min(starting_downtrack, max_starting_downtrack);
420
421 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Used downtrack: " << starting_downtrack);
422
423 // check if required parameter from strategic planner is present
424 if(GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data).empty())
425 {
426 throw std::invalid_argument("No time_to_schedule_entry is provided in float_valued_meta_data");
427 }
428
429 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Creating Lane Follow Geometry");
430 std::vector<PointSpeedPair> lane_follow_points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, starting_downtrack, wm, general_config, detailed_config, visited_lanelets);
431 points_and_target_speeds.insert(points_and_target_speeds.end(), lane_follow_points.begin(), lane_follow_points.end());
432 processed_maneuvers.push_back(maneuver);
433 }
434 else
435 {
436 throw std::invalid_argument("Light Control Intersection Plugin can only create a geometry profile for one maneuver");
437 }
438
439 //Add buffer ending to lane follow points at the end of maneuver(s) end dist
440 if(!processed_maneuvers.empty() && processed_maneuvers.back().type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING){
441 points_and_target_speeds = add_lanefollow_buffer(wm, points_and_target_speeds, processed_maneuvers, ending_state_before_buffer, detailed_config);
442 }
443
444 return points_and_target_speeds;
445 }
446
448 {
449 config_ = config;
450 }
451
452 void LightControlledIntersectionTacticalPlugin::set_yield_client(carma_ros2_utils::ClientPtr<carma_planning_msgs::srv::PlanTrajectory> client)
453 {
454 yield_client_ = client;
455 }
456
457
458} // 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,...
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....
double findSpeedLimit(const lanelet::ConstLanelet &llt, const carma_wm::WorldModelConstPtr &wm) const
Given a Lanelet, find it's associated Speed Limit.
void set_yield_client(carma_ros2_utils::ClientPtr< carma_planning_msgs::srv::PlanTrajectory > client)
set the yield service
LightControlledIntersectionTacticalPlugin(carma_wm::WorldModelConstPtr wm, const Config &config, const std::string &plugin_name, std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode > nh)
LightControlledIntersectionTacticalPlugin constructor.
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")
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...
auto to_string(const UtmZone &zone) -> std::string
Definition: utm_zone.cpp:21
std::shared_ptr< const WorldModel > WorldModelConstPtr
Definition: WorldModel.hpp:452
Stuct containing the algorithm configuration values for light_controlled_intersection_tactical_plugin...