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)
35 const rclcpp::Time& current_time,
36 double min_remaining_time_seconds)
const
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)
71 TSCase new_case,
bool is_new_case_successful,
const rclcpp::Time& current_time)
82 if (
last_case_.get() == new_case && is_new_case_successful ==
true)
91 is_new_case_successful ==
false &&
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)
115 "intersection_transit",
133 wpg_general_config, wpg_detail_config);
137 points_and_target_speeds);
140 carma_planning_msgs::msg::TrajectoryPlan new_trajectory;
141 new_trajectory.header.frame_id =
"map";
142 new_trajectory.header.stamp = req->header.stamp;
146 new_trajectory.trajectory_points =
148 points_and_target_speeds,
155 return new_trajectory;
163 "all variables are set!");
175 "Not all variables are set...");
184 carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req,
185 carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp)
188 if(req->maneuver_index_to_plan >= req->maneuver_plan.maneuvers.size())
190 throw std::invalid_argument(
191 "Light Control Intersection Tactical Plugin was asked to plan invalid "
193 +
" for plan of size: " +
std::to_string(req->maneuver_plan.maneuvers.size()));
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
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);
208 throw std::invalid_argument(
"Light Control Intersection Tactical Plugin "
209 "was asked to plan unsupported maneuver");
213 lanelet::BasicPoint2d veh_pos(req->vehicle_state.x_pos_global,
214 req->vehicle_state.y_pos_global);
216 "Planning state x:" << req->vehicle_state.x_pos_global
217 <<
" , y: " << req->vehicle_state.y_pos_global);
224 auto current_lanelets =
wm_->getLaneletsFromPoint({req->vehicle_state.x_pos_global,
225 req->vehicle_state.y_pos_global});
228 << current_lanelets.size());
230 lanelet::ConstLanelet current_lanelet;
232 if (current_lanelets.empty())
235 "Given vehicle position is not on the road! Returning...");
240 if (
auto llt_on_route_optional =
wm_->getFirstLaneletOnShortestPath(current_lanelets);
241 llt_on_route_optional)
243 current_lanelet = llt_on_route_optional.value();
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");
254 current_lanelet = current_lanelets[0];
258 "Current_lanelet: " << current_lanelet.id());
265 bool is_new_case_successful =
270 maneuver_plan.front(), parameters.int_valued_meta_data[0]);
276 size_t idx_to_start_new_traj =
287 std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint>
293 auto current_time = rclcpp::Time(req->header.stamp);
296 auto last_trajectory_time_bound =
302 resp->trajectory_plan.trajectory_points = last_trajectory_time_bound;
305 "USING LAST TRAJ WITH CASE: " << (
int)
last_case_.get());
310 for (
auto& p : resp->trajectory_plan.trajectory_points) {
314 debug_msg_.trajectory_plan = resp->trajectory_plan;
321 std::vector<double> new_final_speeds;
322 if (carma_planning_msgs::msg::TrajectoryPlan new_trajectory =
324 new_trajectory.trajectory_points.size() >= 2)
327 auto new_trajectory_time_bound =
331 resp->trajectory_plan = new_trajectory;
332 resp->trajectory_plan.trajectory_points = new_trajectory_time_bound;
339 "USING NEW TRAJECTORY for case: " << (
int)new_case);
346 resp->trajectory_plan.trajectory_points = last_trajectory_time_bound;
349 "Failed to generate a new trajectory, so using last valid trajectory!");
354 resp->trajectory_plan = new_trajectory;
356 "Failed to generate a new trajectory or use old valid trajectory, "
357 "so returning empty/invalid trajectory!");
366 if (is_new_case_successful) {
378 ", last_successful_scheduled_entry_time_: " <<
383 "Debug: new case:" << (
int) new_case <<
", is_new_case_successful: "
384 << is_new_case_successful);
386 resp->maneuver_status.push_back(
387 carma_planning_msgs::srv::PlanTrajectory::Response::MANEUVER_IN_PROGRESS);
391 for (
auto& p : resp->trajectory_plan.trajectory_points) {
395 debug_msg_.trajectory_plan = resp->trajectory_plan;
401 carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req,
402 carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp)
404 std::chrono::system_clock::time_point start_time = std::chrono::system_clock::now();
409 "Starting light controlled intersection trajectory planning");
425 std::chrono::system_clock::time_point end_time = std::chrono::system_clock::now();
426 auto duration = end_time - start_time;
429 "ExecutionTime: " << std::chrono::duration<double>(duration).count());
435 if (points_and_target_speeds.empty())
437 throw std::invalid_argument(
"Point and target speed list is empty! Unable to apply case one speed profile...");
441 double planning_downtrack_start = wm->routeTrackPos(points_and_target_speeds[0].
point).downtrack;
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;
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" <<
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_});
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);
459 double total_dist_planned = 0;
461 if (planning_downtrack_start < start_dist)
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);
469 double prev_speed = starting_speed;
470 auto prev_point = points_and_target_speeds.front();
472 for(
auto& p : points_and_target_speeds)
474 double delta_d = lanelet::geometry::distance2d(prev_point.point, p.point);
475 total_dist_planned += delta_d;
483 speed_i = starting_speed;
485 else if(total_dist_planned <= dist1 +
epsilon_){
487 speed_i = sqrt(pow(starting_speed, 2) + 2 * tsp.
a1_ * total_dist_planned);
489 else if(total_dist_planned > dist1 && total_dist_planned <= dist2 +
epsilon_){
491 speed_i = sqrt(std::max(pow(tsp.
v1_, 2) + 2 * tsp.
a2_ * (total_dist_planned - dist1), 0.0));
493 else if (total_dist_planned > dist2 && total_dist_planned <= dist3 +
epsilon_)
496 speed_i = sqrt(std::max(pow(tsp.
v2_, 2) + 2 * tsp.
a3_ * (total_dist_planned - dist2), 0.0));
501 speed_i = prev_speed;
507 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
LCI_TACTICAL_LOGGER),
"Detected nan number from equations. Set to " << speed_i);
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);
515 prev_speed = p.speed;
523 throw std::invalid_argument(
"There must be 9 float_valued_meta_data and 2 int_valued_meta_data to apply algorithm's parameters.");
544 double entry_dist = ending_downtrack - starting_downtrack;
548 entry_dist, starting_speed, departure_speed, tsp);
553 lanelet::Optional<carma_wm::TrafficRulesConstPtr> traffic_rules = wm->getTrafficRules();
556 return (*traffic_rules)->speedLimit(llt).speedLimit.value();
560 throw std::invalid_argument(
"Valid traffic rules object could not be built");
565 carma_planning_msgs::msg::VehicleState &ending_state_before_buffer,
const carma_planning_msgs::msg::VehicleState& state,
568 std::vector<PointSpeedPair> points_and_target_speeds;
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);
576 if(maneuvers.size() == 1)
578 auto maneuver = maneuvers.front();
582 starting_downtrack = std::min(starting_downtrack, max_starting_downtrack);
584 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
LCI_TACTICAL_LOGGER),
"Used downtrack: " << starting_downtrack);
589 throw std::invalid_argument(
"No time_to_schedule_entry is provided in float_valued_meta_data");
592 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
LCI_TACTICAL_LOGGER),
"Creating Lane Follow Geometry");
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);
599 throw std::invalid_argument(
"Light Control Intersection Tactical Plugin currently can"
600 " only create a geometry profile for one maneuver");
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);
608 return points_and_target_speeds;
#define GET_MANEUVER_PROPERTY(mvr, property)
Macro definition to enable easier access to fields shared across the maneuver types.
carma_planning_msgs::msg::TrajectoryPlan last_trajectory_time_unbound_
carma_planning_msgs::msg::VehicleState ending_state_before_buffer_
carma_ros2_utils::ClientPtr< carma_planning_msgs::srv::PlanTrajectory > yield_client_
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...
carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds debug_msg_
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,...
std::string light_controlled_intersection_strategy_
rclcpp::Time latest_traj_request_header_stamp_
double last_successful_scheduled_entry_time_
double current_downtrack_
boost::optional< bool > is_last_case_successful_
std::vector< double > last_speeds_time_unbound_
DebugPublisher debug_publisher_
bool isLastTrajectoryValid(const rclcpp::Time ¤t_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 ...
void setConfig(const Config &config)
Setter function to set a new config for this object.
void logDebugInfoAboutPreviousTrajectory()
Logs debug information about the previously planned trajectory.
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.
const std::string LCI_TACTICAL_LOGGER
double findSpeedLimit(const lanelet::ConstLanelet &llt, const carma_wm::WorldModelConstPtr &wm) const
Given a Lanelet, find its associated Speed Limit.
std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode > nh_
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 ¤t_time)
Determines whether the last trajectory should be reused based on the planning case....
carma_wm::WorldModelConstPtr wm_
boost::optional< TSCase > last_case_
double last_successful_ending_downtrack_
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
std::shared_ptr< const WorldModel > WorldModelConstPtr
std::function< void(const carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds &)> DebugPublisher
Stuct containing the algorithm configuration values for light_controlled_intersection_tactical_plugin...
int curvature_moving_average_window_size
double period_before_intersection_to_force_last_traj
int turn_downsample_ratio
double lateral_accel_limit
double buffer_ending_downtrack
double curve_resample_step_size
bool enable_object_avoidance
int default_downsample_ratio
double trajectory_time_length
double dist_before_intersection_to_force_last_traj
int speed_moving_average_window_size
double vehicle_accel_limit
double vehicle_response_lag
int tactical_plugin_service_call_timeout