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.hpp
Go to the documentation of this file.
1#pragma once
2
3/*
4 * Copyright (C)2023 LEIDOS.
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
7 * use this file except in compliance with the License. You may obtain a copy of
8 * the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
14 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
15 * License for the specific language governing permissions and limitations under
16 * the License.
17 */
18
19#include <rclcpp/rclcpp.hpp>
20#include <string>
21#include <carma_planning_msgs/srv/plan_maneuvers.hpp>
22#include <carma_v2x_msgs/msg/mobility_operation.hpp>
23#include <carma_v2x_msgs/msg/bsm.hpp>
26
27#include <bsm_helper/bsm_helper.h>
28#include <carma_wm/Geometry.hpp>
29#include <lanelet2_core/Forward.h>
30#include <gtest/gtest_prod.h>
31
32#include <lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h>
36
37#include <tf2_ros/transform_listener.h>
38#include <tf2_ros/buffer.h>
39#include <tf2/LinearMath/Transform.h>
40#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
41#include <std_msgs/msg/int8.hpp>
42#include <std_msgs/msg/float64.hpp>
43#include <math.h>
44#include <algorithm>
46#include <carma_ros2_utils/carma_lifecycle_node.hpp>
47#include <optional>
48
50{
51
52enum class TurnDirection {
53 Straight,
54 Right,
55 Left
56};
57
62enum TSCase {
63 CASE_1 = 1,
64 CASE_2 = 2,
65 CASE_3 = 3,
66 CASE_4 = 4,
67 CASE_5 = 5,
68 CASE_6 = 6,
69 CASE_7 = 7,
70 CASE_8 = 8,
74 DEGRADED_TSCASE=12 // when not performing trajectory smoothing, but making through GREEN
75};
76
78{
79 double t0_ = 0;
80 double v0_ = 0;
81 double x0_ = 0;
82
83 double a1_ = 0;
84 double t1_ = 0;
85 double v1_ = 0;
86 double x1_ = 0;
87
88 double a2_ = 0;
89 double t2_ = 0;
90 double v2_ = 0;
91 double x2_ = 0;
92
93 double a3_ = 0;
94 double t3_ = 0;
95 double v3_ = 0;
96 double x3_ = 0;
97
100 double modified_departure_speed = -1.0; // modified departure speed if algorithm failed
101 double modified_remaining_time = -1.0; // modified departure time if algorithm failed
102};
103
105{
106 double dx1 = 0.0;
107 double dx2 = 0.0;
108 double dx3 = 0.0;
109 double dx4 = 0.0;
110 double dx5 = 0.0;
111};
112
114{
115public:
116
120 explicit LCIStrategicPlugin(const rclcpp::NodeOptions &);
121
129 std::shared_ptr<rmw_request_id_t> srv_header,
130 carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
131 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp);
132
136 rcl_interfaces::msg::SetParametersResult
137 parameter_update_callback(const std::vector<rclcpp::Parameter> &parameters);
138
140 carma_ros2_utils::CallbackReturn on_configure_plugin();
141 carma_ros2_utils::CallbackReturn on_activate_plugin();
142
143 bool get_availability();
144 std::string get_version_id();
145
150
156 void mobilityOperationCb(carma_v2x_msgs::msg::MobilityOperation::UniquePtr msg);
157
161 void BSMCb(carma_v2x_msgs::msg::BSM::UniquePtr msg);
162
168 void parseStrategyParams(const std::string& strategy_params);
169
175 carma_v2x_msgs::msg::MobilityOperation generateMobilityOperation();
176
185 TurnDirection getTurnDirectionAtIntersection(std::vector<lanelet::ConstLanelet> lanelets_list);
186
191
196
197private:
198
200 // CARMA Streets Variables
201 // timestamp for msg received from carma streets
202 unsigned long long street_msg_timestamp_ = 0;
203 // scheduled enter time
204 unsigned long long scheduled_enter_time_ = 0;
206 std::string upcoming_id_ = "";
207
208 //BSM
209 std::string bsm_id_ = "default_bsm_id";
210 uint8_t bsm_msg_count_ = 0;
211 uint16_t bsm_sec_mark_ = 0;
212
213 // TS planning related variables
214 double max_comfort_accel_ = 2.0; // acceleration rates after applying miltiplier
215 double max_comfort_decel_ = -2.0;
218
219 boost::optional<rclcpp::Time> nearest_green_entry_time_cached_;
232
235
238
241
243 boost::optional<double> intersection_speed_;
244 boost::optional<double> intersection_end_downtrack_;
245 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
246
247 // TF listenser
248 std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
249 std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
250 tf2::Stamped<tf2::Transform> frontbumper_transform_;
252
253 double epsilon_ = 0.001; //Small constant to compare (double) 0.0 with
254 double accel_epsilon_ = 0.0001; //Small constant to compare (double) 0.0 with
255
258
259 // Timers
260 rclcpp::TimerBase::SharedPtr mob_op_pub_timer_;
261 rclcpp::TimerBase::SharedPtr ts_info_pub_timer_;
262
263 // Create subscribers
264 carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::MobilityOperation> mob_operation_sub_;
265 carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::BSM> bsm_sub_;
266
267 // Create publishers
268 carma_ros2_utils::PubPtr<carma_v2x_msgs::msg::MobilityOperation> mobility_operation_pub_;
269 carma_ros2_utils::PubPtr<std_msgs::msg::Int8> case_pub_;
270 carma_ros2_utils::PubPtr<std_msgs::msg::Float64> tf_distance_pub_;
271 carma_ros2_utils::PubPtr<std_msgs::msg::Float64> earliest_et_pub_;
272 carma_ros2_utils::PubPtr<std_msgs::msg::Float64> et_pub_;
273
278 {
279 rclcpp::Time stamp; // Timestamp of this state data
280 double downtrack; // The downtrack of the vehicle along the route at time stamp
281 double speed; // The speed of the vehicle at time stamp
282 lanelet::Id lane_id; // The current lane id of the vehicle at time stamp
283 };
284
290 bool isStateAllowedGreen(const lanelet::CarmaTrafficSignalState& state) const;
291
297 boost::posix_time::time_duration getMovementAllowedDuration(lanelet::CarmaTrafficSignalPtr traffic_light);
298
313 void planWhenUNAVAILABLE(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
314 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
315 const VehicleState& current_state,
316 const lanelet::CarmaTrafficSignalPtr& traffic_light,
317 const lanelet::ConstLanelet& entry_lanelet,
318 const lanelet::ConstLanelet& exit_lanelet,
319 const lanelet::ConstLanelet& current_lanelet);
320
336 void planWhenAPPROACHING(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
337 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
338 const VehicleState& current_state,
339 const lanelet::CarmaTrafficSignalPtr& traffic_light,
340 const lanelet::ConstLanelet& entry_lanelet,
341 const lanelet::ConstLanelet& exit_lanelet,
342 const lanelet::ConstLanelet& current_lanelet);
343
357 void planWhenWAITING(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
358 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
359 const VehicleState& current_state,
360 const lanelet::CarmaTrafficSignalPtr& traffic_light,
361 const lanelet::ConstLanelet& entry_lanelet,
362 const lanelet::ConstLanelet& exit_lanelet,
363 const lanelet::ConstLanelet& current_lanelet);
364
376 void planWhenDEPARTING(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
377 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
378 const VehicleState& current_state, double intersection_end_downtrack,
379 double intersection_speed_limit);
380
392 boost::optional<bool> canArriveAtGreenWithCertainty(const rclcpp::Time& light_arrival_time_by_algo, const lanelet::CarmaTrafficSignalPtr& traffic_light, bool check_late, bool check_early) const;
393
407 carma_planning_msgs::msg::Maneuver composeTrajectorySmoothingManeuverMessage(double start_dist, double end_dist, const std::vector<lanelet::ConstLanelet>& crossed_lanelets, double start_speed,
408 double target_speed, rclcpp::Time start_time, rclcpp::Time end_time,
409 const TrajectoryParams& tsp) const;
410
425 carma_planning_msgs::msg::Maneuver composeStopAndWaitManeuverMessage(double current_dist, double end_dist, double start_speed,
426 const lanelet::Id& starting_lane_id,
427 const lanelet::Id& ending_lane_id, rclcpp::Time start_time,
428 rclcpp::Time end_time, double stopping_accel) const;
429
444 carma_planning_msgs::msg::Maneuver composeIntersectionTransitMessage(double start_dist, double end_dist, double start_speed,
445 double target_speed, rclcpp::Time start_time, rclcpp::Time end_time,
446 const lanelet::Id& starting_lane_id,
447 const lanelet::Id& ending_lane_id) const;
448
449
465 void handleStopping(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
466 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
467 const VehicleState& current_state,
468 const lanelet::CarmaTrafficSignalPtr& traffic_light,
469 const lanelet::ConstLanelet& entry_lanelet, const lanelet::ConstLanelet& exit_lanelet, const lanelet::ConstLanelet& current_lanelet,
470 double traffic_light_down_track,
471 bool is_emergency = false);
472
488 void handleGreenSignalScenario(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
489 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
490 const VehicleState& current_state,
491 double current_state_speed,
492 const lanelet::CarmaTrafficSignalPtr& traffic_light,
493 const lanelet::ConstLanelet& entry_lanelet, const lanelet::ConstLanelet& exit_lanelet,
494 double traffic_light_down_track, const TrajectoryParams& ts_params, bool is_certainty_check_optional);
508 void handleCruisingUntilStop(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
509 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
510 const VehicleState& current_state,
511 double current_state_speed,
512 const lanelet::CarmaTrafficSignalPtr& traffic_light,
513 double traffic_light_down_track, const TrajectoryParams& ts_params);
514
532 void handleFailureCase(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
533 carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
534 const VehicleState& current_state,
535 double current_state_speed,
536 double speed_limit,
537 double remaining_time,
538 lanelet::Id exit_lanelet_id,
539 const lanelet::CarmaTrafficSignalPtr& traffic_light,
540 double traffic_light_down_track, const TrajectoryParams& ts_params);
541
552 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);
553
561 bool supportedLightState(lanelet::CarmaTrafficSignalState state) const;
562
572 bool validLightState(const boost::optional<std::pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState>>& optional_state,
573 const rclcpp::Time& source_time) const;
574
583 VehicleState extractInitialState(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req) const;
584
590 std::vector<lanelet::ConstLanelet> getLaneletsBetweenWithException(double start_downtrack, double end_downtrack,
591 bool shortest_path_only = false,
592 bool bounds_inclusive = true) const;
593
604 std::tuple<rclcpp::Time, bool, bool> get_final_entry_time_and_conditions(const VehicleState& current_state,
605 const rclcpp::Time& earliest_entry_time,
606 lanelet::CarmaTrafficSignalPtr traffic_light);
607
622 std::optional<rclcpp::Time> get_nearest_green_entry_time(const rclcpp::Time& current_time, const rclcpp::Time& earliest_entry_time, const lanelet::CarmaTrafficSignalPtr& signal, double minimum_required_green_time = 0.0) const;
623
633 rclcpp::Time get_eet_or_tbd(const rclcpp::Time& earliest_entry_time, const lanelet::CarmaTrafficSignalPtr& signal) const;
634
650 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;
651
667 double get_trajectory_smoothing_activation_distance(double time_remaining_at_free_flow, double full_cycle_duration, double current_speed, double speed_limit, double departure_speed, double max_accel, double max_decel) const;
668
681 double get_distance_to_accel_or_decel_twice (double free_flow_speed, double current_speed, double departure_speed, double max_accel, double max_decel) const;
682
698 double get_inflection_speed_value (double x, double x1, double x2, double free_flow_speed, double current_speed, double departure_speed, double max_accel, double max_decel) const;
699
711 double get_distance_to_accel_or_decel_once (double current_speed, double departure_speed, double max_accel, double max_decel) const;
712
721 double estimate_distance_to_stop(double v, double a) const;
722
731 double estimate_time_to_stop(double d, double v) const;
732
742 double findSpeedLimit(const lanelet::ConstLanelet& llt) const;
743
755 double calc_estimated_entry_time_left(double entry_dist, double current_speed, double departure_speed) const;
756
768 BoundaryDistances get_delta_x(double v0, double v1, double v_max, double v_min, double a_max, double a_min);
769
785 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);
786
790 void print_params(TrajectoryParams params);
791
796
800 TrajectoryParams ts_case1(double t, double et, double v0, double v1, double v_max, double a_max, double a_min, double x0, double x_end, double dx);
801 TrajectoryParams ts_case2(double t, double et, double v0, double v1, double a_max, double a_min, double x0, double x_end, double dx);
802 TrajectoryParams ts_case3(double t, double et, double v0, double v1, double a_max, double a_min, double x0, double x_end, double dx);
803 TrajectoryParams ts_case4(double t, double et, double v0, double v1, double v_min, double a_max, double a_min, double x0, double x_end, double dx);
804 TrajectoryParams ts_case5(double t, double et, double v0, double a_max, double a_min, double x0, double x_end, double dx);
805 TrajectoryParams ts_case6(double t, double et, double v0, double v_min, double a_min, double x0, double x_end, double dx, double dx3, TrajectoryParams traj6);
806 TrajectoryParams ts_case7(double t, double et, double v0, double v_min, double a_min, double x0, double x_end, double dx);
807 TrajectoryParams ts_case8(double dx, double dx5, TrajectoryParams traj8);
808
809 TrajectoryParams boundary_accel_or_decel_incomplete_upper(double t, double v0, double v1, double a_max, double a_min, double x0, double x_end, double dx);
810 TrajectoryParams boundary_accel_nocruise_notmaxspeed_decel(double t, double v0, double v1, double a_max, double a_min, double x0, double x_end, double dx);
811 TrajectoryParams boundary_accel_cruise_maxspeed_decel(double t, double v0, double v1, double v_max, double a_max, double a_min, double x0, double x_end, double dx);
812 TrajectoryParams boundary_accel_nocruise_maxspeed_decel(double t, double v0, double v1, double v_max, double a_max, double a_min, double x0, double x_end, double dx);
813 TrajectoryParams boundary_accel_or_decel_complete_upper(double t, double v0, double v1, double x0, double x_end, double dx);
814 TrajectoryParams boundary_decel_nocruise_notminspeed_accel(double t, double v0, double v1, double v_min, double a_max, double a_min, double x0, double x_end, double dx);
815 TrajectoryParams boundary_decel_nocruise_minspeed_accel_incomplete(double t, double v0, double v_min, double a_max, double a_min, double x0, double x_end, double dx);
816 TrajectoryParams boundary_decel_nocruise_minspeed_accel_complete(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);
817 TrajectoryParams boundary_decel_cruise_minspeed_accel(double t, double v0, double v1, double v_min, double a_max, double a_min, double x0, double x_end, double dx);
818 TrajectoryParams boundary_decel_cruise_minspeed(double t, double v0, double v_min, double a_min, double x0, double x_end, double dx);
819 TrajectoryParams boundary_decel_incomplete_lower(double t, double v0, double a_min, double x0, double x_end, double dx);
820 TrajectoryParams boundary_decel_cruise_minspeed_decel(double t, double v0, double v_min, double a_min, double x0, double x_end, double dx);
821
822 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);
823
824 //Unit Tests
825 FRIEND_TEST(LCIStrategicTestFixture, getDiscoveryMsg);
826 FRIEND_TEST(LCIStrategicTestFixture, supportedLightState);
827 FRIEND_TEST(LCIStrategicTestFixture, estimate_distance_to_stop);
828 FRIEND_TEST(LCIStrategicTestFixture, estimate_time_to_stop);
829 FRIEND_TEST(LCIStrategicTestFixture, extractInitialState);
830 FRIEND_TEST(LCIStrategicTestFixture, DISABLED_extractInitialState);
831
832 FRIEND_TEST(LCIStrategicTestFixture, validLightState);
837 FRIEND_TEST(LCIStrategicTestFixture, handleFailureCaseHelper);
838 FRIEND_TEST(LCIStrategicTestFixture, planWhenETInTBD);
839
840 // Algo Unit Tests
844 FRIEND_TEST(LCIStrategicTestFixture, get_nearest_green_entry_time);
845 FRIEND_TEST(LCIStrategicTestFixture, get_eet_or_tbd);
846 FRIEND_TEST(LCIStrategicTestFixture, get_earliest_entry_time);
847 FRIEND_TEST(LCIStrategicTestFixture, handleFailureCase);
848 FRIEND_TEST(LCIStrategicTestFixture, handleStopping);
849 FRIEND_TEST(LCIStrategicTestFixture, planManeuverCb);
850 FRIEND_TEST(LCIStrategicTestFixture, DISABLED_planManeuverCb);
851
852 FRIEND_TEST(LCIStrategicPluginTest, parseStrategyParamstest);
853 FRIEND_TEST(LCIStrategicPluginTest, moboperationcbtest);
854 FRIEND_TEST(LCIStrategicTestFixture, findSpeedLimit);
855 FRIEND_TEST(LCIStrategicTestFixture, DISABLED_planWhenETInTBD);
856
857
858
859
860
861};
862} // namespace lci_strategic_plugin
StrategicPlugin base class which can be extended by user provided plugins which wish to implement the...
tf2::Stamped< tf2::Transform > frontbumper_transform_
TrajectoryParams boundary_decel_nocruise_notminspeed_accel(double t, double v0, double v1, double v_min, double a_max, double a_min, double x0, double x_end, double dx)
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...
FRIEND_TEST(LCIStrategicTestFixture, get_nearest_green_entry_time)
TrajectoryParams boundary_decel_cruise_minspeed_decel(double t, double v0, double v_min, double a_min, double x0, double x_end, double dx)
FRIEND_TEST(LCIStrategicTestFixture, get_distance_to_accel_or_decel_once)
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 ...
double get_distance_to_accel_or_decel_twice(double free_flow_speed, double current_speed, double departure_speed, double max_accel, double max_decel) const
Get required distance to accel then decel, or vise versa - each at least once - to reach departure_sp...
rclcpp::Time get_eet_or_tbd(const rclcpp::Time &earliest_entry_time, const lanelet::CarmaTrafficSignalPtr &signal) const
Returns the later of earliest_entry_time or the end of the available signal states in the traffic_sig...
double estimate_time_to_stop(double d, double v) const
Helper method to use basic kinematics to compute an estimated stopping time from from the inputs.
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 ...
FRIEND_TEST(LCIStrategicTestFixture, estimate_time_to_stop)
FRIEND_TEST(LCIStrategicTestFixture, handleFailureCase)
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....
TrajectoryParams ts_case7(double t, double et, double v0, double v_min, double a_min, double x0, double x_end, double dx)
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_
TrajectoryParams boundary_accel_nocruise_maxspeed_decel(double t, double v0, double v1, double v_max, double a_max, double a_min, double x0, double x_end, double dx)
carma_wm::WorldModelConstPtr wm_
World Model pointer.
TrajectoryParams ts_case1(double t, double et, double v0, double v1, double v_max, double a_max, double a_min, double x0, double x_end, double dx)
Trajectory Smoothing Algorithm 8 cases. TSMO UC2, UC3 algorithm equations.
TrajectoryParams boundary_accel_or_decel_complete_upper(double t, double v0, double v1, double x0, double x_end, double dx)
std::optional< rclcpp::Time > get_nearest_green_entry_time(const rclcpp::Time &current_time, const rclcpp::Time &earliest_entry_time, const lanelet::CarmaTrafficSignalPtr &signal, double minimum_required_green_time=0.0) const
Provides the scheduled entry time for the vehicle in the future. This scheduled time is the earliest ...
FRIEND_TEST(LCIStrategicPluginTest, moboperationcbtest)
TrajectoryParams boundary_accel_or_decel_incomplete_upper(double t, double v0, double v1, double a_max, double a_min, double x0, double x_end, double dx)
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_
FRIEND_TEST(LCIStrategicTestFixture, findSpeedLimit)
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)
FRIEND_TEST(LCIStrategicTestFixture, get_distance_to_accel_or_decel_twice)
double calc_estimated_entry_time_left(double entry_dist, double current_speed, double departure_speed) const
calculate the time vehicle will enter to intersection with optimal deceleration
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....
FRIEND_TEST(LCIStrategicTestFixture, validLightState)
FRIEND_TEST(LCIStrategicTestFixture, planManeuverCb)
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_
boost::posix_time::time_duration getMovementAllowedDuration(lanelet::CarmaTrafficSignalPtr traffic_light)
Returns the duration for the allowed movements.
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_
TrajectoryParams boundary_accel_nocruise_notmaxspeed_decel(double t, double v0, double v1, double a_max, double a_min, double x0, double x_end, double dx)
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....
TrajectoryParams boundary_decel_nocruise_minspeed_accel_incomplete(double t, double v0, double v_min, double a_max, double a_min, double x0, double x_end, double dx)
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,...
FRIEND_TEST(LCIStrategicTestFixture, composeIntersectionTransitMessage)
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...
FRIEND_TEST(LCIStrategicTestFixture, handleFailureCaseHelper)
FRIEND_TEST(LCIStrategicTestFixture, DISABLED_planWhenETInTBD)
FRIEND_TEST(LCIStrategicTestFixture, composeStopAndWaitManeuverMessage)
LCIStrategicPlugin(const rclcpp::NodeOptions &)
Default constructor for RouteFollowingPlugin class.
TrajectoryParams boundary_accel_cruise_maxspeed_decel(double t, double v0, double v1, double v_max, double a_max, double a_min, double x0, double x_end, double dx)
TrajectoryParams boundary_decel_incomplete_lower(double t, double v0, double a_min, double x0, double x_end, double dx)
FRIEND_TEST(LCIStrategicTestFixture, DISABLED_extractInitialState)
TrajectoryParams ts_case6(double t, double et, double v0, double v_min, double a_min, double x0, double x_end, double dx, double dx3, TrajectoryParams traj6)
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,...
TrajectoryParams ts_case4(double t, double et, double v0, double v1, double v_min, double a_max, double a_min, double x0, double x_end, double dx)
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.
FRIEND_TEST(LCIStrategicTestFixture, composeTrajectorySmoothingManeuverMessage)
TrajectoryParams boundary_decel_cruise_minspeed_accel(double t, double v0, double v1, double v_min, double a_max, double a_min, double x0, double x_end, double dx)
FRIEND_TEST(LCIStrategicTestFixture, calc_estimated_entry_time_left)
FRIEND_TEST(LCIStrategicTestFixture, extractInitialState)
FRIEND_TEST(LCIStrategicTestFixture, get_eet_or_tbd)
void print_params(TrajectoryParams params)
Helper method to print TrajectoryParams.
void parseStrategyParams(const std::string &strategy_params)
parse strategy parameters
FRIEND_TEST(LCIStrategicTestFixture, estimate_distance_to_stop)
carma_v2x_msgs::msg::MobilityOperation generateMobilityOperation()
Generates Mobility Operation messages.
TrajectoryParams ts_case2(double t, double et, double v0, double v1, double a_max, double a_min, double x0, double x_end, double dx)
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...
TrajectoryParams boundary_decel_nocruise_minspeed_accel_complete(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)
double get_distance_to_accel_or_decel_once(double current_speed, double departure_speed, double max_accel, double max_decel) const
Get required distance to accel or decel to reach departure_speed with given speed and acceleration pa...
TrajectoryParams boundary_decel_cruise_minspeed(double t, double v0, double v_min, double a_min, double x0, double x_end, double dx)
double get_inflection_speed_value(double x, double x1, double x2, double free_flow_speed, double current_speed, double departure_speed, double max_accel, double max_decel) const
Get the speed between the acceleration and deceleration pieces.
double get_trajectory_smoothing_activation_distance(double time_remaining_at_free_flow, double full_cycle_duration, double current_speed, double speed_limit, double departure_speed, double max_accel, double max_decel) const
Gets maximum distance (nearest downtrack) the trajectory smoothing algorithm makes difference than si...
void publishTrajectorySmoothingInfo()
Publish trajectory smoothing info at a fixed rate.
carma_ros2_utils::PubPtr< carma_v2x_msgs::msg::MobilityOperation > mobility_operation_pub_
FRIEND_TEST(LCIStrategicTestFixture, DISABLED_planManeuverCb)
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.
FRIEND_TEST(LCIStrategicTestFixture, planWhenETInTBD)
TrajectoryParams ts_case8(double dx, double dx5, TrajectoryParams traj8)
FRIEND_TEST(LCIStrategicTestFixture, getLaneletsBetweenWithException)
std::string get_version_id()
Returns the version id of this plugin.
FRIEND_TEST(LCIStrategicPluginTest, parseStrategyParamstest)
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...
FRIEND_TEST(LCIStrategicTestFixture, handleStopping)
boost::optional< double > intersection_end_downtrack_
TrajectoryParams ts_case3(double t, double et, double v0, double v1, double a_max, double a_min, double x0, double x_end, double dx)
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.
FRIEND_TEST(LCIStrategicTestFixture, supportedLightState)
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.
TrajectoryParams ts_case5(double t, double et, double v0, double a_max, double a_min, double x0, double x_end, double dx)
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.
FRIEND_TEST(LCIStrategicTestFixture, getDiscoveryMsg)
rclcpp::TimerBase::SharedPtr ts_info_pub_timer_
carma_ros2_utils::PubPtr< std_msgs::msg::Float64 > et_pub_
FRIEND_TEST(LCIStrategicTestFixture, get_earliest_entry_time)
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.
Class defining the state transition table behavior for the LCIStrategic Strategic Plugin.
std::shared_ptr< const WorldModel > WorldModelConstPtr
Definition: WorldModel.hpp:454
TSCase
Struct representing trajectory smoothing algorithm parameters using distance and acceleration Based o...
Struct to store the configuration settings for the LCIStrategicPlugin class.
Struct representing a vehicle state for the purposes of planning.