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.hpp
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 */
16
17#pragma once
18
19#include <rclcpp/rclcpp.hpp>
20#include <functional>
21#include <boost/uuid/uuid_generators.hpp>
22#include <boost/uuid/uuid_io.hpp>
23#include <boost/geometry.hpp>
24#include <boost/shared_ptr.hpp>
25#include <lanelet2_core/geometry/Point.h>
26#include <lanelet2_core/primitives/Lanelet.h>
27#include <lanelet2_core/geometry/LineString.h>
28#include <carma_planning_msgs/msg/trajectory_plan_point.hpp>
29#include <carma_planning_msgs/msg/trajectory_plan.hpp>
30#include <carma_planning_msgs/srv/plan_trajectory.hpp>
31#include <carma_planning_msgs/msg/maneuver.hpp>
32#include <autoware_msgs/msg/lane.hpp>
33#include <carma_debug_ros2_msgs/msg/trajectory_curvature_speeds.hpp>
36
38
62#define GET_MANEUVER_PROPERTY(mvr, property)\
63 (((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property :\
64 ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property :\
65 ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property :\
66 ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property :\
67 throw std::invalid_argument("GET_MANEUVER_PROPERTY (property) called on maneuver with invalid type id " + std::to_string((mvr).type)))))))
68
70{
74 using DebugPublisher = std::function<void(const carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds&)>;
75
76 enum TSCase {
77 CASE_1 = 1,
78 CASE_2 = 2,
79 CASE_3 = 3,
80 CASE_4 = 4,
81 CASE_5 = 5,
82 CASE_6 = 6,
83 CASE_7 = 7,
84 CASE_8 = 8,
85 };
86
88 double a1_ = 0;
89 double v1_ = 0;
90 double x1_ = 0;
91
92 double a2_ = 0;
93 double v2_ = 0;
94 double x2_ = 0;
95
96 double a3_ = 0;
97 double v3_ = 0;
98 double x3_ = 0;
99 };
100
106 {
107
108 private:
109 // World Model object
111
112 // Config for this object
114
115 std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode> nh_;
116
117 // CARMA Streets Variables
118 // timestamp for msg received from carma streets
119 uint32_t street_msg_timestamp_ = 0.0;
120 // scheduled stop time
121 uint32_t scheduled_stop_time_ = 0.0;
122 // scheduled enter time
123 uint32_t scheduled_enter_time_ = 0.0;
124 // scheduled depart time
126 // scheduled latest depart time
128 // flag to show if the vehicle is allowed in intersection
129 bool is_allowed_int_ = false;
130
131 double speed_limit_ = 11.176; // Approximate speed limit; 25 mph by default
132 boost::optional<TSCase> last_case_;
133 boost::optional<bool> is_last_case_successful_;
134 carma_planning_msgs::msg::TrajectoryPlan last_trajectory_time_unbound_;
135 carma_ros2_utils::ClientPtr<carma_planning_msgs::srv::PlanTrajectory> yield_client_;
136 const std::string LCI_TACTICAL_LOGGER = "light_controlled_intersection_tactical_plugin";
137 carma_planning_msgs::msg::VehicleState ending_state_before_buffer_; //state before applying extra points for curvature calculation that are removed later
139 double epsilon_ = 0.001; //Small constant to compare (double) 0.0 with
140
141 // downtrack of host vehicle
142 double current_downtrack_ = 0.0;
143
144 double last_successful_ending_downtrack_; // if algorithm was successful, this is traffic_light_downtrack
145 double last_successful_scheduled_entry_time_; // if algorithm was successful, this is also scheduled entry time (ET in TSMO UC2 Algo)
146
147 std::string plugin_name_;
148 DebugPublisher debug_publisher_; // Publishes the debug message that includes many useful data used to generate the trajectory
149 carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds debug_msg_;
150 std::vector<double> last_speeds_time_unbound_;
151
152 std::string light_controlled_intersection_strategy_ = "Carma/signalized_intersection"; // Strategy carma-streets is sending. Could be more verbose but needs to be changed on both ends
153
166 void applyTrajectorySmoothingAlgorithm(const carma_wm::WorldModelConstPtr& wm, std::vector<PointSpeedPair>& points_and_target_speeds, double start_dist, double remaining_dist,
167 double starting_speed, double departure_speed, TrajectoryParams tsp);
168
178 void applyOptimizedTargetSpeedProfile(const carma_planning_msgs::msg::Maneuver& maneuver, const double starting_speed, std::vector<PointSpeedPair>& points_and_target_speeds);
179
193 std::vector<PointSpeedPair> createGeometryProfile(const std::vector<carma_planning_msgs::msg::Maneuver> &maneuvers, double max_starting_downtrack,const carma_wm::WorldModelConstPtr &wm,
194 carma_planning_msgs::msg::VehicleState &ending_state_before_buffer,const carma_planning_msgs::msg::VehicleState& state,
195 const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config);
196
207 double findSpeedLimit(const lanelet::ConstLanelet& llt, const carma_wm::WorldModelConstPtr &wm) const;
208
216
229 bool shouldUseLastTrajectory(TSCase new_case, bool is_new_case_successful,
230 const rclcpp::Time& current_time);
231
242 carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req,
243 carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp);
244
257 carma_planning_msgs::msg::TrajectoryPlan generateNewTrajectory(
258 const std::vector<carma_planning_msgs::msg::Maneuver>& maneuver_plan,
259 const carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr& req,
260 std::vector<double>& final_speeds);
261
275 bool isLastTrajectoryValid(const rclcpp::Time& current_time,
276 double min_remaining_time_seconds = 0.0) const;
277
280 FRIEND_TEST(LCITacticalPluginTest, planTrajectorySmoothing);
281 FRIEND_TEST(LCITacticalPluginTest, createGeometryProfile);
282 FRIEND_TEST(LCITacticalPluginTest, planTrajectoryCB);
283 FRIEND_TEST(LCITacticalPluginTest, setConfig);
284
285 public:
286
291 const Config& config,
292 const DebugPublisher& debug_publisher,
293 const std::string& plugin_name,
294 std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode> nh);
295
301 void planTrajectoryCB(
302 carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req,
303 carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp);
304
310 void set_yield_client(carma_ros2_utils::ClientPtr<carma_planning_msgs::srv::PlanTrajectory> client);
311
316 void setConfig(const Config& config);
317 };
318
319} // light_controlled_intersection_tactical_plugin
Class containing primary business logic for the Light Controlled Intersection Tactical Plugin.
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,...
FRIEND_TEST(LCITacticalPluginTest, applyOptimizedTargetSpeedProfile)
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.
FRIEND_TEST(LCITacticalPluginTest, applyTrajectorySmoothingAlgorithm)
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 ...
std::shared_ptr< const WorldModel > WorldModelConstPtr
Definition: WorldModel.hpp:454
basic_autonomy::waypoint_generation::PointSpeedPair PointSpeedPair
std::function< void(const carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds &)> DebugPublisher
basic_autonomy::waypoint_generation::DetailedTrajConfig DetailedTrajConfig
basic_autonomy::waypoint_generation::GeneralTrajConfig GeneralTrajConfig
Stuct containing the algorithm configuration values for light_controlled_intersection_tactical_plugin...