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::LCIStrategicPlugin Class Reference

#include <lci_strategic_plugin.hpp>

Inheritance diagram for lci_strategic_plugin::LCIStrategicPlugin:
Inheritance graph
Collaboration diagram for lci_strategic_plugin::LCIStrategicPlugin:
Collaboration graph

Classes

struct  VehicleState
 Struct representing a vehicle state for the purposes of planning. More...
 

Public Member Functions

 LCIStrategicPlugin (const rclcpp::NodeOptions &)
 Default constructor for RouteFollowingPlugin class. More...
 
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. More...
 
rcl_interfaces::msg::SetParametersResult parameter_update_callback (const std::vector< rclcpp::Parameter > &parameters)
 Callback for dynamic parameter updates. More...
 
carma_ros2_utils::CallbackReturn on_configure_plugin ()
 Method which is triggered when this plugin is moved from the UNCONFIGURED to INACTIVE states. This method should be used to load parameters and is required to be implemented. More...
 
carma_ros2_utils::CallbackReturn on_activate_plugin ()
 Method which is triggered when this plugin is moved from the INACTIVE to ACTIVE states. This method should be used to prepare for future callbacks for plugin's capabilites. More...
 
bool get_availability ()
 Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes. More...
 
std::string get_version_id ()
 Returns the version id of this plugin. More...
 
void lookupFrontBumperTransform ()
 Lookup transfrom from front bumper to base link. More...
 
void mobilityOperationCb (carma_v2x_msgs::msg::MobilityOperation::UniquePtr msg)
 callback function for mobility operation More...
 
void BSMCb (carma_v2x_msgs::msg::BSM::UniquePtr msg)
 BSM callback function. More...
 
void parseStrategyParams (const std::string &strategy_params)
 parse strategy parameters More...
 
carma_v2x_msgs::msg::MobilityOperation generateMobilityOperation ()
 Generates Mobility Operation messages. More...
 
TurnDirection getTurnDirectionAtIntersection (std::vector< lanelet::ConstLanelet > lanelets_list)
 Determine the turn direction at intersection. More...
 
void publishMobilityOperation ()
 Publish mobility operation at a fixed rate. More...
 
void publishTrajectorySmoothingInfo ()
 Publish trajectory smoothing info at a fixed rate. More...
 
- Public Member Functions inherited from carma_guidance_plugins::StrategicPlugin
 StrategicPlugin (const rclcpp::NodeOptions &)
 StrategicPlugin constructor. More...
 
virtual ~StrategicPlugin ()=default
 Virtual destructor for safe deletion. More...
 
virtual 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)=0
 Extending class provided callback which should return a planned trajectory based on the provided trajectory planning request. More...
 
std::string get_capability () override
 Get the capability string representing this plugins capabilities Method must be overriden by extending classes. Expectation is that abstract plugin type parent classes will provide a default implementation. More...
 
uint8_t get_type () override final
 Returns the type of this plugin according to the carma_planning_msgs::Plugin type enum. Extending classes for the specific type should override this method. More...
 
carma_ros2_utils::CallbackReturn handle_on_configure (const rclcpp_lifecycle::State &) override final
 
carma_ros2_utils::CallbackReturn handle_on_activate (const rclcpp_lifecycle::State &) override final
 
carma_ros2_utils::CallbackReturn handle_on_deactivate (const rclcpp_lifecycle::State &) override final
 
carma_ros2_utils::CallbackReturn handle_on_cleanup (const rclcpp_lifecycle::State &) override final
 
carma_ros2_utils::CallbackReturn handle_on_shutdown (const rclcpp_lifecycle::State &) override final
 
carma_ros2_utils::CallbackReturn handle_on_error (const rclcpp_lifecycle::State &, const std::string &exception_string) override final
 
- Public Member Functions inherited from carma_guidance_plugins::PluginBaseNode
 PluginBaseNode (const rclcpp::NodeOptions &)
 PluginBaseNode constructor. More...
 
virtual ~PluginBaseNode ()=default
 Virtual destructor for safe deletion. More...
 
virtual std::shared_ptr< carma_wm::WMListenerget_world_model_listener () final
 Method to return the default world model listener provided as a convience by this base class If this method or get_world_model() are not called then the world model remains uninitialized and will not create unnecessary subscriptions. More...
 
virtual carma_wm::WorldModelConstPtr get_world_model () final
 Method to return the default world model provided as a convience by this base class If this method or get_world_model_listener() are not called then the world model remains uninitialized and will not create unnecessary subscriptions. More...
 
virtual bool get_activation_status () final
 Returns the activation status of this plugin. The plugins API callbacks will only be triggered when this method returns true. More...
 
virtual uint8_t get_type ()
 Returns the type of this plugin according to the carma_planning_msgs::Plugin type enum. Extending classes for the specific type should override this method. More...
 
std::string get_plugin_name_and_ns () const
 Return the name of this plugin with namespace. NOTE: If only the name of the plugin is required, use get_plugin_name() More...
 
std::string get_plugin_name () const
 Return the name of this plugin. More...
 
virtual bool get_availability ()=0
 Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes. More...
 
virtual std::string get_capability ()=0
 Get the capability string representing this plugins capabilities Method must be overriden by extending classes. Expectation is that abstract plugin type parent classes will provide a default implementation. More...
 
virtual std::string get_version_id ()=0
 Returns the version id of this plugin. More...
 
virtual carma_ros2_utils::CallbackReturn on_configure_plugin ()=0
 Method which is triggered when this plugin is moved from the UNCONFIGURED to INACTIVE states. This method should be used to load parameters and is required to be implemented. More...
 
virtual carma_ros2_utils::CallbackReturn on_activate_plugin ()
 Method which is triggered when this plugin is moved from the INACTIVE to ACTIVE states. This method should be used to prepare for future callbacks for plugin's capabilites. More...
 
virtual carma_ros2_utils::CallbackReturn on_deactivate_plugin ()
 Method which is triggered when this plugin is moved from the ACTIVE to INACTIVE states. This method should be used to disable any functionality which should cease execution when plugin is inactive. More...
 
virtual carma_ros2_utils::CallbackReturn on_cleanup_plugin ()
 Method which is triggered when this plugin is moved from the INACTIVE to UNCONFIGURED states. This method should be used to fully reset the plugin such that a future call to on_configure_plugin would leave the plugin in a fresh state as though just launched. More...
 
virtual carma_ros2_utils::CallbackReturn on_shutdown_plugin ()
 Method which is triggered when this plugin is moved from any state to FINALIZED This method should be used to generate any shutdown logs or final cleanup. More...
 
virtual carma_ros2_utils::CallbackReturn on_error_plugin (const std::string &exception_string)
 Method which is triggered when an unhandled exception occurs in this plugin This method should be used to cleanup such that the plugin could be moved to UNCONFIGURED state if possible. More...
 
carma_ros2_utils::CallbackReturn handle_on_configure (const rclcpp_lifecycle::State &) override
 
carma_ros2_utils::CallbackReturn handle_on_activate (const rclcpp_lifecycle::State &) override
 
carma_ros2_utils::CallbackReturn handle_on_deactivate (const rclcpp_lifecycle::State &) override
 
carma_ros2_utils::CallbackReturn handle_on_cleanup (const rclcpp_lifecycle::State &) override
 
carma_ros2_utils::CallbackReturn handle_on_shutdown (const rclcpp_lifecycle::State &) override
 
carma_ros2_utils::CallbackReturn handle_on_error (const rclcpp_lifecycle::State &, const std::string &exception_string) override
 
 FRIEND_TEST (carma_guidance_plugins_test, connections_test)
 

Private Member Functions

bool isStateAllowedGreen (const lanelet::CarmaTrafficSignalState &state) const
 Method to check if the state is an allowed green movement state. Currently Permissive and Protected green are supported. More...
 
boost::posix_time::time_duration getMovementAllowedDuration (lanelet::CarmaTrafficSignalPtr traffic_light)
 Returns the duration for the allowed movements. More...
 
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 Therefor no maneuvers are planned but the system checks for the precense of traffic lights ahead of it. More...
 
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 Therefore the planned maneuvers deal with approaching a traffic light. More...
 
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 Therefor the planned maneuvers deal with waiting at a red light. More...
 
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 Therefor the planned maneuvers deal with the transit of the intersection once the stop bar has been crossed. More...
 
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. More...
 
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) More...
 
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. More...
 
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. More...
 
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 distance. Given the initial speed, ending speed (which is 0), and remaining_downtrack, this function checks if calculated remaining_time falls on RED. More...
 
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 parameters to go through the intersection with certainty It utilizes canMakeWithCertainty function to determine if it is able to make it. Corresponds to TSCase 1-7. More...
 
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 parameters to go through the intersection with certainty It utilizes canMakeWithCertainty function to determine if it is able to make it. Corresponds to TSCase 1-7. More...
 
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, this function modifies departure speed as close as possible to the original using max comfortable accelerations with given distance. More...
 
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. See handleFailureCase. More...
 
bool supportedLightState (lanelet::CarmaTrafficSignalState state) const
 Helper method to evaluate if the given traffic light state is supported by this plugin. More...
 
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 is supported via supportedLightState. More...
 
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 prior_plan was set or not. More...
 
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, end_downtrack, shortest_path_only, bounds_inclusive) and throws and exception if the returned list of lanelets is empty. See the referenced method for additional details on parameters. More...
 
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-street (TSMO UC3) This function also applies necessary green_buffer to adjust the ET. More...
 
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 possible entry time that is during green phase and after timestamps both that is kinematically possible and required times for vehicle in front to pass through first Returns std::nullopt if no valid green entry time found. More...
 
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_signal. More...
 
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. Designed to get it for motion that has maximum one acceleration, deceleration, or cruise segments until destination. More...
 
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 simple lane following within half of one full fixed cycle time boundary compared to free flow arrival time. More...
 
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_speed with given speed and acceleration parameters. More...
 
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. More...
 
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 parameters. More...
 
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. More...
 
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. More...
 
double findSpeedLimit (const lanelet::ConstLanelet &llt) const
 Given a Lanelet, find it's associated Speed Limit. More...
 
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 More...
 
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 parameters. More...
 
std::vector< TrajectoryParamsget_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 generate a single trajectory. More...
 
void print_params (TrajectoryParams params)
 Helper method to print TrajectoryParams. More...
 
void print_boundary_distances (BoundaryDistances delta_xs)
 Helper method to print TrajectoryParams. More...
 
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. More...
 
TrajectoryParams ts_case2 (double t, double et, double v0, double v1, double a_max, double a_min, double x0, double x_end, double dx)
 
TrajectoryParams ts_case3 (double t, double et, double v0, double v1, double a_max, double a_min, double x0, double x_end, double dx)
 
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)
 
TrajectoryParams ts_case5 (double t, double et, double v0, double a_max, double a_min, double x0, double x_end, double dx)
 
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)
 
TrajectoryParams ts_case7 (double t, double et, double v0, double v_min, double a_min, double x0, double x_end, double dx)
 
TrajectoryParams ts_case8 (double dx, double dx5, TrajectoryParams traj8)
 
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)
 
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)
 
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_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)
 
TrajectoryParams boundary_accel_or_decel_complete_upper (double t, double v0, double v1, double x0, double x_end, double dx)
 
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)
 
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)
 
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)
 
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)
 
TrajectoryParams boundary_decel_cruise_minspeed (double t, double v0, double v_min, 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)
 
TrajectoryParams boundary_decel_cruise_minspeed_decel (double t, double v0, double v_min, double a_min, double x0, double x_end, double dx)
 
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)
 
 FRIEND_TEST (LCIStrategicTestFixture, getDiscoveryMsg)
 
 FRIEND_TEST (LCIStrategicTestFixture, supportedLightState)
 
 FRIEND_TEST (LCIStrategicTestFixture, estimate_distance_to_stop)
 
 FRIEND_TEST (LCIStrategicTestFixture, estimate_time_to_stop)
 
 FRIEND_TEST (LCIStrategicTestFixture, extractInitialState)
 
 FRIEND_TEST (LCIStrategicTestFixture, DISABLED_extractInitialState)
 
 FRIEND_TEST (LCIStrategicTestFixture, validLightState)
 
 FRIEND_TEST (LCIStrategicTestFixture, getLaneletsBetweenWithException)
 
 FRIEND_TEST (LCIStrategicTestFixture, composeStopAndWaitManeuverMessage)
 
 FRIEND_TEST (LCIStrategicTestFixture, composeIntersectionTransitMessage)
 
 FRIEND_TEST (LCIStrategicTestFixture, composeTrajectorySmoothingManeuverMessage)
 
 FRIEND_TEST (LCIStrategicTestFixture, handleFailureCaseHelper)
 
 FRIEND_TEST (LCIStrategicTestFixture, planWhenETInTBD)
 
 FRIEND_TEST (LCIStrategicTestFixture, calc_estimated_entry_time_left)
 
 FRIEND_TEST (LCIStrategicTestFixture, get_distance_to_accel_or_decel_twice)
 
 FRIEND_TEST (LCIStrategicTestFixture, get_distance_to_accel_or_decel_once)
 
 FRIEND_TEST (LCIStrategicTestFixture, get_nearest_green_entry_time)
 
 FRIEND_TEST (LCIStrategicTestFixture, get_eet_or_tbd)
 
 FRIEND_TEST (LCIStrategicTestFixture, get_earliest_entry_time)
 
 FRIEND_TEST (LCIStrategicTestFixture, handleFailureCase)
 
 FRIEND_TEST (LCIStrategicTestFixture, handleStopping)
 
 FRIEND_TEST (LCIStrategicTestFixture, planManeuverCb)
 
 FRIEND_TEST (LCIStrategicTestFixture, DISABLED_planManeuverCb)
 
 FRIEND_TEST (LCIStrategicPluginTest, parseStrategyParamstest)
 
 FRIEND_TEST (LCIStrategicPluginTest, moboperationcbtest)
 
 FRIEND_TEST (LCIStrategicTestFixture, findSpeedLimit)
 
 FRIEND_TEST (LCIStrategicTestFixture, DISABLED_planWhenETInTBD)
 

Private Attributes

unsigned long long street_msg_timestamp_ = 0
 
unsigned long long scheduled_enter_time_ = 0
 
std::string previous_strategy_params_ = ""
 
std::string upcoming_id_ = ""
 
std::string bsm_id_ = "default_bsm_id"
 
uint8_t bsm_msg_count_ = 0
 
uint16_t bsm_sec_mark_ = 0
 
double max_comfort_accel_ = 2.0
 
double max_comfort_decel_ = -2.0
 
double max_comfort_decel_norm_ = -1 * max_comfort_decel_
 
double emergency_decel_norm_ = -2 * max_comfort_decel_
 
boost::optional< rclcpp::Time > nearest_green_entry_time_cached_
 
TSCase last_case_num_ = TSCase::UNAVAILABLE
 Useful metrics for LCI Plugin. More...
 
double distance_remaining_to_tf_ = 0.0
 
double earliest_entry_time_ = 0.0
 
double scheduled_entry_time_ = 0.0
 
TurnDirection intersection_turn_direction_ = TurnDirection::Straight
 
bool approaching_light_controlled_intersection_ = false
 
LCIStrategicPluginConfig config_
 Config containing configurable algorithm parameters. More...
 
LCIStrategicStateTransitionTable transition_table_
 State Machine Transition table. More...
 
boost::optional< double > intersection_speed_
 Cache variables for storing the current intersection state between state machine transitions. More...
 
boost::optional< double > intersection_end_downtrack_
 
std::string light_controlled_intersection_strategy_ = "Carma/signalized_intersection"
 
std::shared_ptr< tf2_ros::Buffer > tf2_buffer_
 
std::shared_ptr< tf2_ros::TransformListener > tf2_listener_
 
tf2::Stamped< tf2::Transform > frontbumper_transform_
 
double length_to_front_bumper_ = 3.0
 
double epsilon_ = 0.001
 
double accel_epsilon_ = 0.0001
 
carma_wm::WorldModelConstPtr wm_
 World Model pointer. More...
 
rclcpp::TimerBase::SharedPtr mob_op_pub_timer_
 
rclcpp::TimerBase::SharedPtr ts_info_pub_timer_
 
carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::MobilityOperation > mob_operation_sub_
 
carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::BSM > bsm_sub_
 
carma_ros2_utils::PubPtr< carma_v2x_msgs::msg::MobilityOperation > mobility_operation_pub_
 
carma_ros2_utils::PubPtr< std_msgs::msg::Int8 > case_pub_
 
carma_ros2_utils::PubPtr< std_msgs::msg::Float64 > tf_distance_pub_
 
carma_ros2_utils::PubPtr< std_msgs::msg::Float64 > earliest_et_pub_
 
carma_ros2_utils::PubPtr< std_msgs::msg::Float64 > et_pub_
 

Detailed Description

Definition at line 113 of file lci_strategic_plugin.hpp.

Constructor & Destructor Documentation

◆ LCIStrategicPlugin()

lci_strategic_plugin::LCIStrategicPlugin::LCIStrategicPlugin ( const rclcpp::NodeOptions &  options)
explicit

Default constructor for RouteFollowingPlugin class.

Definition at line 42 of file lci_strategic_plugin.cpp.

43 : carma_guidance_plugins::StrategicPlugin(options), tf2_buffer_(std::make_shared<tf2_ros::Buffer>(this->get_clock()))
44{
45 config_ = LCIStrategicPluginConfig();
46
47 config_.vehicle_accel_limit = declare_parameter<double>("vehicle_acceleration_limit", config_.vehicle_accel_limit);
48 config_.vehicle_decel_limit = declare_parameter<double>("vehicle_deceleration_limit", config_.vehicle_decel_limit);
49 config_.vehicle_decel_limit_multiplier = declare_parameter<double>("vehicle_decel_limit_multiplier", config_.vehicle_decel_limit_multiplier);
50 config_.vehicle_accel_limit_multiplier = declare_parameter<double>("vehicle_accel_limit_multiplier", config_.vehicle_accel_limit_multiplier);
51 config_.min_approach_distance = declare_parameter<double>("min_approach_distance", config_.min_approach_distance);
52 config_.trajectory_smoothing_activation_distance = declare_parameter<double>("trajectory_smoothing_activation_distance", config_.trajectory_smoothing_activation_distance);
53 config_.stopping_location_buffer = declare_parameter<double>("stopping_location_buffer", config_.stopping_location_buffer);
54 config_.green_light_time_buffer = declare_parameter<double>("green_light_time_buffer", config_.green_light_time_buffer);
55 config_.vehicle_response_lag = declare_parameter<double>("vehicle_response_lag", config_.vehicle_response_lag);
56 config_.algo_minimum_speed = declare_parameter<double>("algo_minimum_speed", config_.algo_minimum_speed);
57 config_.deceleration_fraction = declare_parameter<double>("deceleration_fraction", config_.deceleration_fraction);
58 config_.desired_distance_to_stop_buffer = declare_parameter<double>("desired_distance_to_stop_buffer", config_.desired_distance_to_stop_buffer);
59 config_.min_maneuver_planning_period = declare_parameter<double>("min_maneuver_planning_period", config_.min_maneuver_planning_period);
60 config_.strategic_plugin_name = declare_parameter<std::string>("strategic_plugin_name", config_.strategic_plugin_name);
61 config_.lane_following_plugin_name = declare_parameter<std::string>("lane_following_plugin_name", config_.lane_following_plugin_name);
62 config_.stop_and_wait_plugin_name = declare_parameter<std::string>("stop_and_wait_plugin_name", config_.stop_and_wait_plugin_name);
63 config_.intersection_transit_plugin_name = declare_parameter<std::string>("intersection_transit_plugin_name", config_.intersection_transit_plugin_name);
64 config_.enable_carma_streets_connection = declare_parameter<bool>("enable_carma_streets_connection",config_.enable_carma_streets_connection);
65 config_.mobility_rate = declare_parameter<double>("mobility_rate", config_.mobility_rate);
67 declare_parameter<long>("enable_carma_wm_spat_processing",
69 config_.vehicle_id = declare_parameter<std::string>("vehicle_id", config_.vehicle_id);
70
75};
StrategicPlugin base class which can be extended by user provided plugins which wish to implement the...
LCIStrategicPluginConfig config_
Config containing configurable algorithm parameters.
std::shared_ptr< tf2_ros::Buffer > tf2_buffer_
bool enable_carma_streets_connection
Bool: Enable carma streets connection.
std::string vehicle_id
License plate of the vehicle.
double trajectory_smoothing_activation_distance
Downtrack distance until nearest intersection where the Trajectory Smoothing algorithm should activat...
double algo_minimum_speed
Minimum allowable speed TS algorithm in m/s.
double vehicle_accel_limit_multiplier
A multiplier to apply to the maximum allowable vehicle acceleration limit so we plan under our capabi...
double min_maneuver_planning_period
The minimum period in seconds which a maneuver plan must cover if the plugin wishes to control the wh...
double green_light_time_buffer
A buffer in seconds around the green phase which will reduce the phase length such that vehicle still...
std::string stop_and_wait_plugin_name
The name of the plugin to use for stop and wait trajectory planning.
double stopping_location_buffer
A buffer infront of the stopping location which will still be considered a valid stop....
double vehicle_response_lag
An approximation of the delay (sec) between sent vehicle commands and the vehicle begining a meaningf...
double vehicle_decel_limit
The maximum allowable vehicle deceleration limit in m/s.
double vehicle_decel_limit_multiplier
A multiplier to apply to the maximum allowable vehicle deceleration limit so we plan under our capabi...
double min_approach_distance
The minimum distance in meters that the vehicle can be at before requiring a transition to the APPROA...
int enable_carma_wm_spat_processing
Int: 0 to turn off and 1 to turn on.
double vehicle_accel_limit
The maximum allowable vehicle acceleration limit in m/s.
std::string strategic_plugin_name
The name to use for this plugin during comminications with the arbitrator.
double mobility_rate
Double: Mobility operation rate.
double desired_distance_to_stop_buffer
Double: Desired distance to stop buffer in meters.

References lci_strategic_plugin::LCIStrategicPluginConfig::algo_minimum_speed, config_, lci_strategic_plugin::LCIStrategicPluginConfig::deceleration_fraction, lci_strategic_plugin::LCIStrategicPluginConfig::desired_distance_to_stop_buffer, emergency_decel_norm_, lci_strategic_plugin::LCIStrategicPluginConfig::enable_carma_streets_connection, lci_strategic_plugin::LCIStrategicPluginConfig::enable_carma_wm_spat_processing, lci_strategic_plugin::LCIStrategicPluginConfig::green_light_time_buffer, lci_strategic_plugin::LCIStrategicPluginConfig::intersection_transit_plugin_name, lci_strategic_plugin::LCIStrategicPluginConfig::lane_following_plugin_name, max_comfort_accel_, max_comfort_decel_, max_comfort_decel_norm_, lci_strategic_plugin::LCIStrategicPluginConfig::min_approach_distance, lci_strategic_plugin::LCIStrategicPluginConfig::min_maneuver_planning_period, lci_strategic_plugin::LCIStrategicPluginConfig::mobility_rate, lci_strategic_plugin::LCIStrategicPluginConfig::stop_and_wait_plugin_name, lci_strategic_plugin::LCIStrategicPluginConfig::stopping_location_buffer, lci_strategic_plugin::LCIStrategicPluginConfig::strategic_plugin_name, lci_strategic_plugin::LCIStrategicPluginConfig::trajectory_smoothing_activation_distance, lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_accel_limit, lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_accel_limit_multiplier, lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_decel_limit, lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_decel_limit_multiplier, lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_id, and lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_response_lag.

Member Function Documentation

◆ boundary_accel_cruise_maxspeed_decel()

TrajectoryParams lci_strategic_plugin::LCIStrategicPlugin::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 
)
private

Definition at line 1059 of file lci_strategic_plugin_algo.cpp.

1060{
1061 double t_end = t + (dx / v_max) + (pow(v_max - v0, 2) / (2 * a_max * v_max)) - (pow(v1 - v_max, 2) / (2 * a_min * v_max));
1062
1063 TrajectoryParams traj;
1064
1065 traj.t0_ = t;
1066 traj.v0_ = v0;
1067 traj.x0_ = x0;
1068
1069 traj.v1_ = v_max;
1070 traj.a1_ = a_max;
1071 traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
1072 traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
1073
1074 traj.v2_ = v_max;
1075 traj.a2_ = 0;
1076 traj.t2_ = t_end - ((v1 - v_max) / a_min);
1077 traj.x2_ = x_end - ((pow(v1, 2) - pow(v_max, 2)) / (2 * a_min));
1078
1079 traj.t3_ = t_end;
1080 traj.a3_ = a_min;
1081 traj.v3_ = v1;
1082 traj.x3_ = x_end;
1083
1084 return traj;
1085}

References lci_strategic_plugin::TrajectoryParams::a1_, lci_strategic_plugin::TrajectoryParams::a2_, lci_strategic_plugin::TrajectoryParams::a3_, lci_strategic_plugin::TrajectoryParams::t0_, lci_strategic_plugin::TrajectoryParams::t1_, lci_strategic_plugin::TrajectoryParams::t2_, lci_strategic_plugin::TrajectoryParams::t3_, lci_strategic_plugin::TrajectoryParams::v0_, lci_strategic_plugin::TrajectoryParams::v1_, lci_strategic_plugin::TrajectoryParams::v2_, lci_strategic_plugin::TrajectoryParams::v3_, lci_strategic_plugin::TrajectoryParams::x0_, lci_strategic_plugin::TrajectoryParams::x1_, lci_strategic_plugin::TrajectoryParams::x2_, and lci_strategic_plugin::TrajectoryParams::x3_.

Referenced by get_boundary_traj_params().

Here is the caller graph for this function:

◆ boundary_accel_nocruise_maxspeed_decel()

TrajectoryParams lci_strategic_plugin::LCIStrategicPlugin::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 
)
private

Definition at line 1087 of file lci_strategic_plugin_algo.cpp.

1088{
1089 double nom = (v_max - v0) + ((a_max / a_min) * (v1 - v_max));
1090 double den = (pow(v_max, 2) - pow(v0, 2)) + ((a_max / a_min) * (pow(v1, 2) - pow(v_max, 2)));
1091
1092 if (den <= epsilon_ && den >= -epsilon_)
1093 throw std::invalid_argument("boundary_accel_nocruise_maxspeed_decel: Received den near zero..." + std::to_string(den));
1094
1095 double t_end = t + (2 * dx * nom / den);
1096
1097 TrajectoryParams traj;
1098
1099 traj.t0_ = t;
1100 traj.v0_ = v0;
1101 traj.x0_ = x0;
1102
1103 double dt = t_end - t;
1104 double tc = 0;
1105
1106 traj.v1_ = v_max;
1107
1108 if (dt - tc <= epsilon_ && dt - tc >= -epsilon_)
1109 throw std::invalid_argument("boundary_accel_nocruise_maxspeed_decel: Received dt - tc near zero..." + std::to_string(dt - tc));
1110
1111 traj.a1_ = (((1 - (a_max / a_min)) * v_max) + ((a_max / a_min) * v1) - v0) / (dt - tc);
1112
1113 if (traj.a1_ <= accel_epsilon_ && traj.a1_ >= -accel_epsilon_)
1114 {
1115 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "boundary_accel_nocruise_maxspeed_decel: Received traj.a1_ near zero...");
1116 traj.t1_ = traj.t0_ + (dt * (a_max / (a_min + a_max)));
1117 traj.x1_ = traj.x0_ + (v_max * (traj.t1_ - traj.t0_));
1118 }
1119 else
1120 {
1121 traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
1122 traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
1123 }
1124
1125 traj.t2_ = t_end;
1126 traj.a2_ = ((((a_min / a_max) - 1) * v_max) + v1 - ((a_min / a_max) * v0)) / (dt - tc);
1127 traj.v2_ = v1;
1128 traj.x2_ = x_end;
1129
1130 traj.t3_ = traj.t2_;
1131 traj.a3_ = 0;
1132 traj.v3_ = traj.v2_;
1133 traj.x3_ = traj.x2_;
1134
1135 return traj;
1136}
auto to_string(const UtmZone &zone) -> std::string
Definition: utm_zone.cpp:21

References accel_epsilon_, epsilon_, lci_strategic_plugin::TrajectoryParams::t0_, and carma_cooperative_perception::to_string().

Referenced by get_boundary_traj_params().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ boundary_accel_nocruise_notmaxspeed_decel()

TrajectoryParams lci_strategic_plugin::LCIStrategicPlugin::boundary_accel_nocruise_notmaxspeed_decel ( double  t,
double  v0,
double  v1,
double  a_max,
double  a_min,
double  x0,
double  x_end,
double  dx 
)
private

Definition at line 1027 of file lci_strategic_plugin_algo.cpp.

1028{
1029
1030 double v_hat = sqrt(((2 * dx * a_max * a_min) + (a_min * pow(v0, 2)) - (a_max * pow(v1, 2))) / (a_min - a_max));
1031 double t_end = t + ((v_hat * (a_min - a_max)) - (v0 * a_min) + (v1 * a_max)) / (a_max * a_min);
1032
1033 TrajectoryParams traj;
1034
1035 traj.t0_ = t;
1036 traj.v0_ = v0;
1037 traj.x0_ = x0;
1038
1039 traj.v1_ = v_hat;
1040 traj.a1_ = a_max;
1041 traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
1042
1043 traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
1044
1045 traj.t2_ = t_end;
1046 traj.a2_ = a_min;
1047 traj.v2_ = v1;
1048 traj.x2_ = x_end;
1049
1050 traj.t3_ = traj.t2_;
1051 traj.a3_ = 0;
1052 traj.v3_ = traj.v2_;
1053 traj.x3_ = traj.x2_;
1054
1055 return traj;
1056
1057}

References lci_strategic_plugin::TrajectoryParams::a1_, lci_strategic_plugin::TrajectoryParams::a2_, lci_strategic_plugin::TrajectoryParams::a3_, lci_strategic_plugin::TrajectoryParams::t0_, lci_strategic_plugin::TrajectoryParams::t1_, lci_strategic_plugin::TrajectoryParams::t2_, lci_strategic_plugin::TrajectoryParams::t3_, lci_strategic_plugin::TrajectoryParams::v0_, lci_strategic_plugin::TrajectoryParams::v1_, lci_strategic_plugin::TrajectoryParams::v2_, lci_strategic_plugin::TrajectoryParams::v3_, lci_strategic_plugin::TrajectoryParams::x0_, lci_strategic_plugin::TrajectoryParams::x1_, lci_strategic_plugin::TrajectoryParams::x2_, and lci_strategic_plugin::TrajectoryParams::x3_.

Referenced by get_boundary_traj_params().

Here is the caller graph for this function:

◆ boundary_accel_or_decel_complete_upper()

TrajectoryParams lci_strategic_plugin::LCIStrategicPlugin::boundary_accel_or_decel_complete_upper ( double  t,
double  v0,
double  v1,
double  x0,
double  x_end,
double  dx 
)
private

Definition at line 1138 of file lci_strategic_plugin_algo.cpp.

1139{
1140 if (v0 + v1 <= epsilon_ && v0 + v1 >= -epsilon_)
1141 throw std::invalid_argument("boundary_accel_or_decel_complete_upper: Received v0 + v1 near zero..." + std::to_string(v0 + v1));
1142
1143 double t_end = t + ((2 * dx) / (v0 + v1));
1144
1145 TrajectoryParams traj;
1146
1147 traj.t0_ = t;
1148 traj.v0_ = v0;
1149 traj.x0_ = x0;
1150
1151 traj.t1_ = t_end;
1152
1153 if (dx <= epsilon_ && dx >= -epsilon_)
1154 throw std::invalid_argument("boundary_accel_or_decel_complete_upper: Received dx near zero..." + std::to_string(dx));
1155
1156 traj.a1_ = (pow(v1, 2) - pow(v0, 2)) / (2 * dx);
1157 traj.v1_ = v1;
1158 traj.x1_ = x_end;
1159
1160 traj.t2_ = traj.t1_;
1161 traj.a2_ = 0;
1162 traj.v2_ = traj.v1_;
1163 traj.x2_ = traj.x1_;
1164
1165 traj.t3_ = traj.t1_;
1166 traj.a3_ = 0;
1167 traj.v3_ = traj.v1_;
1168 traj.x3_ = traj.x1_;
1169
1170 return traj;
1171}

References epsilon_, lci_strategic_plugin::TrajectoryParams::t0_, and carma_cooperative_perception::to_string().

Referenced by get_boundary_traj_params().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ boundary_accel_or_decel_incomplete_upper()

TrajectoryParams lci_strategic_plugin::LCIStrategicPlugin::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 
)
private

Definition at line 984 of file lci_strategic_plugin_algo.cpp.

985{
986 double t_end;
987
988 if (v0 <= v1 + epsilon_)
989 t_end = t + (sqrt(pow(v0, 2) + (2 * a_max * dx)) - v0)/a_max;
990 else
991 t_end = t + (sqrt(pow(v0, 2) + (2 * a_min * dx)) - v0) / a_min;
992
993 TrajectoryParams traj;
994
995 traj.t0_ = t;
996 traj.v0_ = v0;
997 traj.x0_ = x0;
998
999 traj.t1_ = t_end;
1000
1001 if (v0 <= v1 + epsilon_)
1002 {
1003 traj.a1_ = a_max;
1004 traj.v1_ = sqrt(pow(v0, 2) + (2 * a_max * dx));
1005 }
1006 else
1007 {
1008 traj.a1_ = a_min;
1009 traj.v1_ = sqrt(pow(v0, 2) + (2 * a_min * dx));
1010 }
1011
1012 traj.x1_ = x_end;
1013
1014 traj.t2_ = traj.t1_;
1015 traj.a2_ = 0;
1016 traj.v2_ = traj.v1_;
1017 traj.x2_ = traj.x1_;
1018
1019 traj.t3_ = traj.t1_;
1020 traj.a3_ = 0;
1021 traj.v3_ = traj.v1_;
1022 traj.x3_ = traj.x1_;
1023
1024 return traj;
1025}

References epsilon_, and lci_strategic_plugin::TrajectoryParams::t0_.

Referenced by get_boundary_traj_params().

Here is the caller graph for this function:

◆ boundary_decel_cruise_minspeed()

TrajectoryParams lci_strategic_plugin::LCIStrategicPlugin::boundary_decel_cruise_minspeed ( double  t,
double  v0,
double  v_min,
double  a_min,
double  x0,
double  x_end,
double  dx 
)
private

Definition at line 1299 of file lci_strategic_plugin_algo.cpp.

1300{
1301 double t_end = t + (dx / v_min) + (pow(v_min - v0, 2) / (2 * a_min * v_min));
1302
1303 TrajectoryParams traj;
1304
1305 traj.t0_ = t;
1306 traj.v0_ = v0;
1307 traj.x0_ = x0;
1308
1309 traj.v1_ = v_min;
1310 traj.a1_ = a_min;
1311 traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
1312 traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
1313
1314 traj.v2_ = v_min;
1315 traj.a2_ = 0;
1316 traj.t2_ = t_end;
1317 traj.x2_ = x_end;
1318
1319 traj.t3_ = traj.t2_;
1320 traj.a3_ = traj.a2_;
1321 traj.v3_ = traj.v2_;
1322 traj.x3_ = traj.x2_;
1323
1324 return traj;
1325}

References lci_strategic_plugin::TrajectoryParams::a1_, lci_strategic_plugin::TrajectoryParams::a2_, lci_strategic_plugin::TrajectoryParams::a3_, lci_strategic_plugin::TrajectoryParams::t0_, lci_strategic_plugin::TrajectoryParams::t1_, lci_strategic_plugin::TrajectoryParams::t2_, lci_strategic_plugin::TrajectoryParams::t3_, lci_strategic_plugin::TrajectoryParams::v0_, lci_strategic_plugin::TrajectoryParams::v1_, lci_strategic_plugin::TrajectoryParams::v2_, lci_strategic_plugin::TrajectoryParams::v3_, lci_strategic_plugin::TrajectoryParams::x0_, lci_strategic_plugin::TrajectoryParams::x1_, lci_strategic_plugin::TrajectoryParams::x2_, and lci_strategic_plugin::TrajectoryParams::x3_.

Referenced by get_boundary_traj_params().

Here is the caller graph for this function:

◆ boundary_decel_cruise_minspeed_accel()

TrajectoryParams lci_strategic_plugin::LCIStrategicPlugin::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 
)
private

Definition at line 1271 of file lci_strategic_plugin_algo.cpp.

1272{
1273 double t_end = t + (dx / v_min) + ((pow(v_min - v0, 2)) / (2 * a_min * v_min)) - ((pow(v1 - v_min, 2)) / (2 * a_max * v_min));
1274
1275 TrajectoryParams traj;
1276
1277 traj.t0_ = t;
1278 traj.v0_ = v0;
1279 traj.x0_ = x0;
1280
1281 traj.v1_ = v_min;
1282 traj.a1_ = a_min;
1283 traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
1284 traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
1285
1286 traj.v2_ = v_min;
1287 traj.a2_ = 0;
1288 traj.t2_ = t_end - ((v1 - v_min) / a_max);
1289 traj.x2_ = x_end - ((pow(v1, 2) - pow(v_min, 2)) / (2 * a_max));
1290
1291 traj.t3_ = t_end;
1292 traj.a3_ = a_max;
1293 traj.v3_ = v1;
1294 traj.x3_ = x_end;
1295
1296 return traj;
1297}

References lci_strategic_plugin::TrajectoryParams::a1_, lci_strategic_plugin::TrajectoryParams::a2_, lci_strategic_plugin::TrajectoryParams::a3_, lci_strategic_plugin::TrajectoryParams::t0_, lci_strategic_plugin::TrajectoryParams::t1_, lci_strategic_plugin::TrajectoryParams::t2_, lci_strategic_plugin::TrajectoryParams::t3_, lci_strategic_plugin::TrajectoryParams::v0_, lci_strategic_plugin::TrajectoryParams::v1_, lci_strategic_plugin::TrajectoryParams::v2_, lci_strategic_plugin::TrajectoryParams::v3_, lci_strategic_plugin::TrajectoryParams::x0_, lci_strategic_plugin::TrajectoryParams::x1_, lci_strategic_plugin::TrajectoryParams::x2_, and lci_strategic_plugin::TrajectoryParams::x3_.

Referenced by get_boundary_traj_params().

Here is the caller graph for this function:

◆ boundary_decel_cruise_minspeed_decel()

TrajectoryParams lci_strategic_plugin::LCIStrategicPlugin::boundary_decel_cruise_minspeed_decel ( double  t,
double  v0,
double  v_min,
double  a_min,
double  x0,
double  x_end,
double  dx 
)
private

Definition at line 1353 of file lci_strategic_plugin_algo.cpp.

1354{
1355 double t_end = t + (dx / v_min) + (v0 * (v0 - (2 * v_min)) / (2 * a_min * v_min));
1356
1357 TrajectoryParams traj;
1358
1359 traj.t0_ = t;
1360 traj.v0_ = v0;
1361 traj.x0_ = x0;
1362
1363 traj.v1_ = v_min;
1364 traj.a1_ = a_min;
1365 traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
1366 traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
1367
1368 traj.a2_ = 0;
1369 traj.v2_ = v_min;
1370 traj.t2_ = t_end - ((0 - traj.v2_) / a_min);
1371 traj.x2_ = x_end - ((0 - pow(traj.v2_, 2)) / (2 * a_min));
1372
1373 traj.t3_ = t_end;
1374 traj.a3_ = a_min;
1375 traj.v3_ = 0;
1376 traj.x3_ = x_end;
1377
1378 return traj;
1379}

References lci_strategic_plugin::TrajectoryParams::a1_, lci_strategic_plugin::TrajectoryParams::a2_, lci_strategic_plugin::TrajectoryParams::a3_, lci_strategic_plugin::TrajectoryParams::t0_, lci_strategic_plugin::TrajectoryParams::t1_, lci_strategic_plugin::TrajectoryParams::t2_, lci_strategic_plugin::TrajectoryParams::t3_, lci_strategic_plugin::TrajectoryParams::v0_, lci_strategic_plugin::TrajectoryParams::v1_, lci_strategic_plugin::TrajectoryParams::v2_, lci_strategic_plugin::TrajectoryParams::v3_, lci_strategic_plugin::TrajectoryParams::x0_, lci_strategic_plugin::TrajectoryParams::x1_, lci_strategic_plugin::TrajectoryParams::x2_, and lci_strategic_plugin::TrajectoryParams::x3_.

Referenced by get_boundary_traj_params().

Here is the caller graph for this function:

◆ boundary_decel_incomplete_lower()

TrajectoryParams lci_strategic_plugin::LCIStrategicPlugin::boundary_decel_incomplete_lower ( double  t,
double  v0,
double  a_min,
double  x0,
double  x_end,
double  dx 
)
private

Definition at line 1326 of file lci_strategic_plugin_algo.cpp.

1327{
1328 double t_end = t + (sqrt(pow(v0, 2) + (2 * a_min * dx)) - v0) / a_min;
1329
1330 TrajectoryParams traj;
1331
1332 traj.t0_ = t;
1333 traj.v0_ = v0;
1334 traj.x0_ = x0;
1335
1336 traj.t1_ = t_end;
1337 traj.v1_ = sqrt(pow(v0, 2) + (2 * a_min * dx));
1338 traj.a1_ = a_min;
1339 traj.x1_ = x_end;
1340
1341 traj.t2_ = traj.t1_;
1342 traj.a2_ = 0;
1343 traj.v2_ = traj.v1_;
1344 traj.x2_ = traj.x1_;
1345
1346 traj.t3_ = traj.t1_;
1347 traj.a3_ = 0;
1348 traj.v3_ = traj.v1_;
1349 traj.x3_ = traj.x1_;
1350
1351 return traj;
1352}

References lci_strategic_plugin::TrajectoryParams::t0_.

Referenced by get_boundary_traj_params().

Here is the caller graph for this function:

◆ boundary_decel_nocruise_minspeed_accel_complete()

TrajectoryParams lci_strategic_plugin::LCIStrategicPlugin::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 
)
private

Definition at line 1232 of file lci_strategic_plugin_algo.cpp.

1233{
1234 double nom = (v1 - v_min) + ((a_max / a_min) * (v_min - v0));
1235 double den = (pow(v1, 2) - pow(v_min, 2)) + ((a_max / a_min) * (pow(v_min, 2) - pow(v0, 2)));
1236
1237 if (den <= epsilon_ && den >= -epsilon_)
1238 throw std::invalid_argument("boundary_decel_nocruise_minspeed_accel_complete: Received den near zero..." + std::to_string(den));
1239
1240 double t_end = t + (2 * dx * nom / den);
1241 TrajectoryParams traj;
1242
1243 traj.t0_ = t;
1244 traj.v0_ = v0;
1245 traj.x0_ = x0;
1246
1247 double dt = t_end - t;
1248 double tc = 0;
1249
1250 traj.v1_ = v_min;
1251
1252 if (dt - tc <= epsilon_ && dt - tc >= -epsilon_)
1253 throw std::invalid_argument("boundary_decel_nocruise_minspeed_accel_complete: Received dt - tc near zero..." + std::to_string(dt - tc));
1254
1255 traj.a1_ = (((1 - (a_min / a_max)) * v_min) + ((a_min / a_max) * v1) - v0) / (dt - tc);
1256 traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
1257 traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
1258
1259 traj.t2_ = t_end;
1260 traj.a2_ = ((((a_max / a_min) - 1) * v_min) + v1 - ((a_max / a_min) * v0)) / (dt - tc);
1261 traj.v2_ = v1;
1262 traj.x2_ = x_end;
1263
1264 traj.t3_ = traj.t2_;
1265 traj.a3_ = 0;
1266 traj.v3_ = traj.v2_;
1267 traj.x3_ = traj.x2_;
1268
1269 return traj;
1270}

References epsilon_, lci_strategic_plugin::TrajectoryParams::t0_, and carma_cooperative_perception::to_string().

Referenced by get_boundary_traj_params().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ boundary_decel_nocruise_minspeed_accel_incomplete()

TrajectoryParams lci_strategic_plugin::LCIStrategicPlugin::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 
)
private

Definition at line 1202 of file lci_strategic_plugin_algo.cpp.

1203{
1204 double sqr = sqrt((2 * a_max * dx) - ((pow(v_min, 2) - pow(v0, 2)) * (a_max / a_min)) + pow(v_min, 2));
1205
1206 double t_end = t + ((sqr - v_min) / a_max) + ((v_min - v0) / a_min);
1207
1208 TrajectoryParams traj;
1209
1210 traj.t0_ = t;
1211 traj.v0_ = v0;
1212 traj.x0_ = x0;
1213
1214 traj.v1_ = v_min;
1215 traj.a1_ = a_min;
1216 traj.t1_ = traj.t0_ + (traj.v1_ - traj.v0_) / a_min;
1217 traj.x1_ = traj.x0_ + (pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * a_min);
1218
1219 traj.t2_ = t_end;
1220 traj.a2_ = a_max;
1221 traj.v2_ = (traj.a2_ * (traj.t2_ - traj.t1_)) + traj.v1_;
1222 traj.x2_ = x_end;
1223
1224 traj.t3_ = traj.t2_;
1225 traj.a3_ = 0;
1226 traj.v3_ = traj.v2_;
1227 traj.x3_ = traj.x2_;
1228
1229 return traj;
1230}

References lci_strategic_plugin::TrajectoryParams::a1_, lci_strategic_plugin::TrajectoryParams::a2_, lci_strategic_plugin::TrajectoryParams::a3_, lci_strategic_plugin::TrajectoryParams::t0_, lci_strategic_plugin::TrajectoryParams::t1_, lci_strategic_plugin::TrajectoryParams::t2_, lci_strategic_plugin::TrajectoryParams::t3_, lci_strategic_plugin::TrajectoryParams::v0_, lci_strategic_plugin::TrajectoryParams::v1_, lci_strategic_plugin::TrajectoryParams::v2_, lci_strategic_plugin::TrajectoryParams::v3_, lci_strategic_plugin::TrajectoryParams::x0_, lci_strategic_plugin::TrajectoryParams::x1_, lci_strategic_plugin::TrajectoryParams::x2_, and lci_strategic_plugin::TrajectoryParams::x3_.

Referenced by get_boundary_traj_params().

Here is the caller graph for this function:

◆ boundary_decel_nocruise_notminspeed_accel()

TrajectoryParams lci_strategic_plugin::LCIStrategicPlugin::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 
)
private

Definition at line 1173 of file lci_strategic_plugin_algo.cpp.

1174{
1175 double v_hat = sqrt(((2 * dx * a_max * a_min) + (a_max * pow(v0, 2)) - (a_min * pow(v1, 2))) / (a_max - a_min));
1176 double t_end = t + ((v_hat * (a_max - a_min)) - (v0 * a_max) + (v1 * a_min)) / (a_max * a_min);
1177
1178 TrajectoryParams traj;
1179
1180 traj.t0_ = t;
1181 traj.v0_ = v0;
1182 traj.x0_ = x0;
1183
1184 traj.v1_ = v_hat;
1185 traj.a1_ = a_min;
1186 traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
1187 traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
1188
1189 traj.t2_ = t_end;
1190 traj.a2_ = a_max;
1191 traj.v2_ = v1;
1192 traj.x2_ = x_end;
1193
1194 traj.t3_ = traj.t2_;
1195 traj.a3_ = 0;
1196 traj.v3_ = traj.v2_;
1197 traj.x3_ = traj.x2_;
1198
1199 return traj;
1200}

References lci_strategic_plugin::TrajectoryParams::a1_, lci_strategic_plugin::TrajectoryParams::a2_, lci_strategic_plugin::TrajectoryParams::a3_, lci_strategic_plugin::TrajectoryParams::t0_, lci_strategic_plugin::TrajectoryParams::t1_, lci_strategic_plugin::TrajectoryParams::t2_, lci_strategic_plugin::TrajectoryParams::t3_, lci_strategic_plugin::TrajectoryParams::v0_, lci_strategic_plugin::TrajectoryParams::v1_, lci_strategic_plugin::TrajectoryParams::v2_, lci_strategic_plugin::TrajectoryParams::v3_, lci_strategic_plugin::TrajectoryParams::x0_, lci_strategic_plugin::TrajectoryParams::x1_, lci_strategic_plugin::TrajectoryParams::x2_, and lci_strategic_plugin::TrajectoryParams::x3_.

Referenced by get_boundary_traj_params().

Here is the caller graph for this function:

◆ BSMCb()

void lci_strategic_plugin::LCIStrategicPlugin::BSMCb ( carma_v2x_msgs::msg::BSM::UniquePtr  msg)

BSM callback function.

Definition at line 1197 of file lci_strategic_plugin.cpp.

1198{
1199 std::vector<uint8_t> bsm_id_vec = msg->core_data.id;
1200 bsm_id_ = BSMHelper::BSMHelper::bsmIDtoString(bsm_id_vec);
1201 bsm_msg_count_ = msg->core_data.msg_count;
1202 bsm_sec_mark_ = msg->core_data.sec_mark;
1203}

References bsm_id_, bsm_msg_count_, and bsm_sec_mark_.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

◆ calc_estimated_entry_time_left()

double lci_strategic_plugin::LCIStrategicPlugin::calc_estimated_entry_time_left ( double  entry_dist,
double  current_speed,
double  departure_speed 
) const
private

calculate the time vehicle will enter to intersection with optimal deceleration

Parameters
entry_distdistance to stop line
current_speedcurrent speed of vehicle
departure_speedspeed to get into the intersection
Returns
the time vehicle will stop with optimal decelarion

Definition at line 452 of file lci_strategic_plugin_algo.cpp.

453{
454 double t_entry = 0;
455 // t = 2 * d / (v_i + v_f)
456 // from TSMO USE CASE 2 Algorithm Doc - Figure 4. Equation: Estimation of t*_nt
457 t_entry = 2*entry_dist/(current_speed + departure_speed);
458 return t_entry;
459}

◆ canArriveAtGreenWithCertainty()

boost::optional< bool > lci_strategic_plugin::LCIStrategicPlugin::canArriveAtGreenWithCertainty ( const rclcpp::Time &  light_arrival_time_by_algo,
const lanelet::CarmaTrafficSignalPtr &  traffic_light,
bool  check_late = true,
bool  check_early = true 
) const
private

Return true if the car can arrive at given arrival time within green light time buffer.

Parameters
light_arrival_time_by_algoROS time at green where vehicle arrives
traffic_lightTraffic signal to check time against
check_latecheck late arrival, default true
check_earlycheck early arrival, default true
Returns
true (bool optional) if can make it at both bounds of error time.

NOTE: internally uses config_.green_light_time_buffer NOTE: boost::none optional if any of the light state was invalid

Definition at line 321 of file lci_strategic_plugin.cpp.

322{
323 rclcpp::Time early_arrival_time_by_algo =
324 light_arrival_time_by_algo - rclcpp::Duration::from_nanoseconds(config_.green_light_time_buffer * 1e9);
325
326 rclcpp::Time late_arrival_time_by_algo =
327 light_arrival_time_by_algo + rclcpp::Duration::from_nanoseconds(config_.green_light_time_buffer * 1e9);
328
329 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "light_arrival_time_by_algo: " << std::to_string(light_arrival_time_by_algo.seconds()));
330 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "early_arrival_time_by_algo: " << std::to_string(early_arrival_time_by_algo.seconds()));
331 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "late_arrival_time_by_algo: " << std::to_string(late_arrival_time_by_algo.seconds()));
332
333 auto early_arrival_state_by_algo_optional = traffic_light->predictState(lanelet::time::timeFromSec(early_arrival_time_by_algo.seconds()));
334
335 if (!validLightState(early_arrival_state_by_algo_optional, early_arrival_time_by_algo))
336 return boost::none;
337
338 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "early_arrival_state_by_algo: " << early_arrival_state_by_algo_optional.get().second);
339
340 auto late_arrival_state_by_algo_optional = traffic_light->predictState(lanelet::time::timeFromSec(late_arrival_time_by_algo.seconds()));
341
342 if (!validLightState(late_arrival_state_by_algo_optional, late_arrival_time_by_algo))
343 return boost::none;
344
345 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "late_arrival_state_by_algo: " << late_arrival_state_by_algo_optional.get().second);
346
347 bool can_make_early_arrival = true;
348 bool can_make_late_arrival = true;
349
350 if (check_early)
351 can_make_early_arrival = isStateAllowedGreen(early_arrival_state_by_algo_optional.get().second);
352
353 if (check_late)
354 can_make_late_arrival = isStateAllowedGreen(late_arrival_state_by_algo_optional.get().second);
355
356 // We will cross the light on the green phase even if we arrive early or late
357 if (can_make_early_arrival && can_make_late_arrival) // Green light
358 return true;
359 else
360 return false;
361
362}
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 ...
bool isStateAllowedGreen(const lanelet::CarmaTrafficSignalState &state) const
Method to check if the state is an allowed green movement state. Currently Permissive and Protected g...

References config_, lci_strategic_plugin::LCIStrategicPluginConfig::green_light_time_buffer, isStateAllowedGreen(), carma_cooperative_perception::to_string(), and validLightState().

Referenced by handleGreenSignalScenario(), and planWhenWAITING().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ composeIntersectionTransitMessage()

carma_planning_msgs::msg::Maneuver lci_strategic_plugin::LCIStrategicPlugin::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
private

Compose a intersection transit maneuver message based on input params.

Parameters
start_distStart downtrack distance of the current maneuver
end_distEnd downtrack distance of the current maneuver
start_speedStart speed of the current maneuver
target_speedTarget speed pf the current maneuver, usually it is the lanelet speed limit
start_timeThe starting time of the maneuver
end_timeThe ending time of the maneuver
starting_lane_idThe starting lanelet id of this maneuver
ending_lane_idThe ending lanelet id of this maneuver
Returns
A intersection transit maneuver maneuver message which is ready to be published

Definition at line 1532 of file lci_strategic_plugin.cpp.

1537{
1538 carma_planning_msgs::msg::Maneuver maneuver_msg;
1539 maneuver_msg.type = carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT;
1540 maneuver_msg.intersection_transit_straight_maneuver.parameters.negotiation_type =
1541 carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
1542 maneuver_msg.intersection_transit_straight_maneuver.parameters.presence_vector =
1543 carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN;
1544 maneuver_msg.intersection_transit_straight_maneuver.parameters.planning_tactical_plugin =
1546 maneuver_msg.intersection_transit_straight_maneuver.parameters.planning_strategic_plugin =
1548 maneuver_msg.intersection_transit_straight_maneuver.start_dist = start_dist;
1549 maneuver_msg.intersection_transit_straight_maneuver.start_speed = start_speed;
1550 maneuver_msg.intersection_transit_straight_maneuver.start_time = start_time;
1551 maneuver_msg.intersection_transit_straight_maneuver.end_dist = end_dist;
1552 maneuver_msg.intersection_transit_straight_maneuver.end_speed = target_speed;
1553 maneuver_msg.intersection_transit_straight_maneuver.end_time = end_time;
1554 maneuver_msg.intersection_transit_straight_maneuver.starting_lane_id = std::to_string(starting_lane_id);
1555 maneuver_msg.intersection_transit_straight_maneuver.ending_lane_id = std::to_string(ending_lane_id);
1556
1557 // Start time and end time for maneuver are assigned in updateTimeProgress
1558
1559 RCLCPP_INFO_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Creating IntersectionTransitManeuver start dist: " << start_dist << " end dist: " << end_dist
1560 << " From lanelet: " << starting_lane_id
1561 << " to lanelet: " << ending_lane_id
1562 << " From start_time: " << std::to_string(start_time.seconds())
1563 << " to end_time: " << std::to_string(end_time.seconds()));
1564
1565 return maneuver_msg;
1566}

References config_, lci_strategic_plugin::LCIStrategicPluginConfig::intersection_transit_plugin_name, lci_strategic_plugin::LCIStrategicPluginConfig::strategic_plugin_name, and carma_cooperative_perception::to_string().

Referenced by handleFailureCase(), handleGreenSignalScenario(), and planWhenDEPARTING().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ composeStopAndWaitManeuverMessage()

carma_planning_msgs::msg::Maneuver lci_strategic_plugin::LCIStrategicPlugin::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
private

Compose a stop and wait maneuver message based on input params.

Parameters
current_distStart downtrack distance of the current maneuver
end_distEnd downtrack distance of the current maneuver
start_speedStart speed of the current maneuver
starting_lane_idThe starting lanelet id of this maneuver
ending_lane_idThe ending lanelet id of this maneuver
start_timeThe starting time of the maneuver
end_timeThe ending time of the maneuver
stopping_accelAcceleration used for calculating the stopping distance
Returns
A stop and wait maneuver message which is ready to be published

Definition at line 1499 of file lci_strategic_plugin.cpp.

1504{
1505 carma_planning_msgs::msg::Maneuver maneuver_msg;
1506 maneuver_msg.type = carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT;
1507 maneuver_msg.stop_and_wait_maneuver.parameters.negotiation_type = carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
1508 maneuver_msg.stop_and_wait_maneuver.parameters.presence_vector =
1509 carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN | carma_planning_msgs::msg::ManeuverParameters::HAS_FLOAT_META_DATA;
1510 maneuver_msg.stop_and_wait_maneuver.parameters.planning_tactical_plugin = config_.stop_and_wait_plugin_name;
1511 maneuver_msg.stop_and_wait_maneuver.parameters.planning_strategic_plugin = config_.strategic_plugin_name;
1512 maneuver_msg.stop_and_wait_maneuver.start_dist = current_dist;
1513 maneuver_msg.stop_and_wait_maneuver.end_dist = end_dist;
1514 maneuver_msg.stop_and_wait_maneuver.start_speed = start_speed;
1515 maneuver_msg.stop_and_wait_maneuver.start_time = start_time;
1516 maneuver_msg.stop_and_wait_maneuver.end_time = end_time;
1517 maneuver_msg.stop_and_wait_maneuver.starting_lane_id = std::to_string(starting_lane_id);
1518 maneuver_msg.stop_and_wait_maneuver.ending_lane_id = std::to_string(ending_lane_id);
1519
1520 // Set the meta data for the stop location buffer
1521 maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(config_.stopping_location_buffer);
1522 maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(stopping_accel);
1523
1524 RCLCPP_INFO_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Creating stop and wait start dist: " << current_dist << " end dist: " << end_dist
1525 << " start_time: " << std::to_string(start_time.seconds())
1526 << " end_time: " << std::to_string(end_time.seconds())
1527 << " stopping_accel: " << stopping_accel);
1528
1529 return maneuver_msg;
1530}

References config_, lci_strategic_plugin::LCIStrategicPluginConfig::stop_and_wait_plugin_name, lci_strategic_plugin::LCIStrategicPluginConfig::stopping_location_buffer, lci_strategic_plugin::LCIStrategicPluginConfig::strategic_plugin_name, and carma_cooperative_perception::to_string().

Referenced by handleCruisingUntilStop(), handleStopping(), and planWhenWAITING().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ composeTrajectorySmoothingManeuverMessage()

carma_planning_msgs::msg::Maneuver lci_strategic_plugin::LCIStrategicPlugin::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
private

Compose a trajectory smoothing maneuver msg (sent as transit maneuver message)

Parameters
start_distStart downtrack distance of the current maneuver
end_distEnd downtrack distance of the current maneuver
start_speedStart speed of the current maneuver
target_speedTarget speed pf the current maneuver, usually it is the lanelet speed limit
start_timeThe starting time of the maneuver
end_timeThe ending time of the maneuver
tspTrajectory Smoothing parameters needed to modify speed profile
Returns
A transift maneuver message specifically designed for light controlled intersection tactical plugin

Definition at line 1448 of file lci_strategic_plugin.cpp.

1451{
1452 carma_planning_msgs::msg::Maneuver maneuver_msg;
1453 maneuver_msg.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING;
1454 maneuver_msg.lane_following_maneuver.parameters.negotiation_type =
1455 carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
1456 maneuver_msg.lane_following_maneuver.parameters.presence_vector =
1457 carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN | carma_planning_msgs::msg::ManeuverParameters::HAS_FLOAT_META_DATA | carma_planning_msgs::msg::ManeuverParameters::HAS_INT_META_DATA;
1458 maneuver_msg.lane_following_maneuver.parameters.planning_tactical_plugin =
1460 maneuver_msg.lane_following_maneuver.parameters.planning_strategic_plugin =
1462 maneuver_msg.lane_following_maneuver.start_dist = start_dist;
1463 maneuver_msg.lane_following_maneuver.start_speed = start_speed;
1464 maneuver_msg.lane_following_maneuver.start_time = start_time;
1465 maneuver_msg.lane_following_maneuver.end_dist = end_dist;
1466 maneuver_msg.lane_following_maneuver.end_speed = target_speed;
1467 maneuver_msg.lane_following_maneuver.end_time = end_time;
1468
1469 maneuver_msg.lane_following_maneuver.parameters.string_valued_meta_data.push_back(light_controlled_intersection_strategy_);
1470
1471 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.a1_);
1472 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.v1_);
1473 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.x1_);
1474
1475 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.a2_);
1476 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.v2_);
1477 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.x2_);
1478
1479 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.a3_);
1480 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.v3_);
1481 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.x3_);
1482
1483 maneuver_msg.lane_following_maneuver.parameters.int_valued_meta_data.push_back(static_cast<int>(tsp.case_num));
1484 maneuver_msg.lane_following_maneuver.parameters.int_valued_meta_data.push_back(static_cast<int>(tsp.is_algorithm_successful));
1485
1486 for (auto llt : crossed_lanelets)
1487 {
1488 maneuver_msg.lane_following_maneuver.lane_ids.push_back(std::to_string(llt.id()));
1489 }
1490 // Start time and end time for maneuver are assigned in updateTimeProgress
1491
1492 RCLCPP_INFO_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Creating TrajectorySmoothingManeuver start dist: " << start_dist << " end dist: " << end_dist
1493 << " start_time: " << std::to_string(start_time.seconds())
1494 << " end_time: " << std::to_string(end_time.seconds()));
1495
1496 return maneuver_msg;
1497}

References lci_strategic_plugin::TrajectoryParams::a1_, lci_strategic_plugin::TrajectoryParams::a2_, lci_strategic_plugin::TrajectoryParams::a3_, lci_strategic_plugin::TrajectoryParams::case_num, config_, lci_strategic_plugin::TrajectoryParams::is_algorithm_successful, lci_strategic_plugin::LCIStrategicPluginConfig::lane_following_plugin_name, light_controlled_intersection_strategy_, lci_strategic_plugin::LCIStrategicPluginConfig::strategic_plugin_name, carma_cooperative_perception::to_string(), lci_strategic_plugin::TrajectoryParams::v1_, lci_strategic_plugin::TrajectoryParams::v2_, lci_strategic_plugin::TrajectoryParams::v3_, lci_strategic_plugin::TrajectoryParams::x1_, lci_strategic_plugin::TrajectoryParams::x2_, and lci_strategic_plugin::TrajectoryParams::x3_.

Referenced by handleCruisingUntilStop(), handleFailureCase(), and handleGreenSignalScenario().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ estimate_distance_to_stop()

double lci_strategic_plugin::LCIStrategicPlugin::estimate_distance_to_stop ( double  v,
double  a 
) const
private

Helper method to use basic kinematics to compute an estimated stopping distance from from the inputs.

Parameters
vThe initial velocity in m/s
aThe deceleration in m/s^2
Returns
The estimated stopping distance in meters

Definition at line 24 of file lci_strategic_plugin_algo.cpp.

25{
26 return (v * v) / (2.0 * a);
27}

Referenced by planWhenUNAVAILABLE().

Here is the caller graph for this function:

◆ estimate_time_to_stop()

double lci_strategic_plugin::LCIStrategicPlugin::estimate_time_to_stop ( double  d,
double  v 
) const
private

Helper method to use basic kinematics to compute an estimated stopping time from from the inputs.

Parameters
dThe distance to travel in meters
vThe initial velocity in m/s
Returns
The estimated stopping time in seconds

Definition at line 29 of file lci_strategic_plugin_algo.cpp.

30{
31 return 2.0 * d / v;
32};

◆ extractInitialState()

LCIStrategicPlugin::VehicleState lci_strategic_plugin::LCIStrategicPlugin::extractInitialState ( carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr  req) const
private

Helper method to extract the initial vehicle state from the planning request method based on if the prior_plan was set or not.

Parameters
reqThe maneuver planning request to extract the vehicle state from
Returns
The extracted VehicleState

Definition at line 259 of file lci_strategic_plugin.cpp.

260{
261 VehicleState state;
262 if (!req->prior_plan.maneuvers.empty())
263 {
264 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Provided with initial plan...");
265 state.stamp = GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_time);
266 state.downtrack = GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_dist);
267 state.speed = GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_speed);
268 state.lane_id = getLaneletsBetweenWithException(state.downtrack, state.downtrack, true).front().id();
269 }
270 else
271 {
272 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "No initial plan provided...");
273
274 state.stamp = rclcpp::Time(req->header.stamp, RCL_SYSTEM_TIME);
275 state.downtrack = req->veh_downtrack;
276 state.speed = req->veh_logitudinal_velocity;
277 state.lane_id = stoi(req->veh_lane_id);
278 }
279 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "extractInitialState >>>> state.stamp: " << std::to_string(state.stamp.seconds()));
280 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "extractInitialState >>>> state.downtrack : " << state.downtrack );
281 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "extractInitialState >>>> state.speed: " << state.speed);
282 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "extractInitialState >>>> state.lane_id: " << state.lane_id);
283
284 return state;
285}
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,...
#define GET_MANEUVER_PROPERTY(mvr, property)

References lci_strategic_plugin::LCIStrategicPlugin::VehicleState::downtrack, GET_MANEUVER_PROPERTY, getLaneletsBetweenWithException(), lci_strategic_plugin::LCIStrategicPlugin::VehicleState::lane_id, lci_strategic_plugin::LCIStrategicPlugin::VehicleState::speed, lci_strategic_plugin::LCIStrategicPlugin::VehicleState::stamp, and carma_cooperative_perception::to_string().

Referenced by plan_maneuvers_callback().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ findSpeedLimit()

double lci_strategic_plugin::LCIStrategicPlugin::findSpeedLimit ( const lanelet::ConstLanelet &  llt) const
private

Given a Lanelet, find it's associated Speed Limit.

Parameters
lltConstant Lanelet object
Exceptions
std::invalid_argumentif the speed limit could not be retrieved
Returns
value of speed limit in mps

Definition at line 287 of file lci_strategic_plugin.cpp.

288{
289 lanelet::Optional<carma_wm::TrafficRulesConstPtr> traffic_rules = wm_->getTrafficRules();
290 if (traffic_rules)
291 {
292 return (*traffic_rules)->speedLimit(llt).speedLimit.value();
293 }
294 else
295 {
296 throw std::invalid_argument("Valid traffic rules object could not be built");
297 }
298}
carma_wm::WorldModelConstPtr wm_
World Model pointer.

References wm_.

Referenced by planWhenAPPROACHING(), and planWhenUNAVAILABLE().

Here is the caller graph for this function:

◆ FRIEND_TEST() [1/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicPluginTest  ,
moboperationcbtest   
)
private

◆ FRIEND_TEST() [2/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicPluginTest  ,
parseStrategyParamstest   
)
private

◆ FRIEND_TEST() [3/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
calc_estimated_entry_time_left   
)
private

◆ FRIEND_TEST() [4/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
composeIntersectionTransitMessage   
)
private

◆ FRIEND_TEST() [5/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
composeStopAndWaitManeuverMessage   
)
private

◆ FRIEND_TEST() [6/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
composeTrajectorySmoothingManeuverMessage   
)
private

◆ FRIEND_TEST() [7/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
DISABLED_extractInitialState   
)
private

◆ FRIEND_TEST() [8/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
DISABLED_planManeuverCb   
)
private

◆ FRIEND_TEST() [9/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
DISABLED_planWhenETInTBD   
)
private

◆ FRIEND_TEST() [10/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
estimate_distance_to_stop   
)
private

◆ FRIEND_TEST() [11/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
estimate_time_to_stop   
)
private

◆ FRIEND_TEST() [12/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
extractInitialState   
)
private

◆ FRIEND_TEST() [13/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
findSpeedLimit   
)
private

◆ FRIEND_TEST() [14/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
get_distance_to_accel_or_decel_once   
)
private

◆ FRIEND_TEST() [15/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
get_distance_to_accel_or_decel_twice   
)
private

◆ FRIEND_TEST() [16/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
get_earliest_entry_time   
)
private

◆ FRIEND_TEST() [17/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
get_eet_or_tbd   
)
private

◆ FRIEND_TEST() [18/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
get_nearest_green_entry_time   
)
private

◆ FRIEND_TEST() [19/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
getDiscoveryMsg   
)
private

◆ FRIEND_TEST() [20/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
getLaneletsBetweenWithException   
)
private

◆ FRIEND_TEST() [21/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
handleFailureCase   
)
private

◆ FRIEND_TEST() [22/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
handleFailureCaseHelper   
)
private

◆ FRIEND_TEST() [23/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
handleStopping   
)
private

◆ FRIEND_TEST() [24/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
planManeuverCb   
)
private

◆ FRIEND_TEST() [25/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
planWhenETInTBD   
)
private

◆ FRIEND_TEST() [26/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
supportedLightState   
)
private

◆ FRIEND_TEST() [27/27]

lci_strategic_plugin::LCIStrategicPlugin::FRIEND_TEST ( LCIStrategicTestFixture  ,
validLightState   
)
private

◆ generateMobilityOperation()

carma_v2x_msgs::msg::MobilityOperation lci_strategic_plugin::LCIStrategicPlugin::generateMobilityOperation ( )

Generates Mobility Operation messages.

Returns
mobility operation msg for status and intent

Definition at line 1224 of file lci_strategic_plugin.cpp.

1225{
1226 carma_v2x_msgs::msg::MobilityOperation mo_;
1227 mo_.m_header.timestamp = now().nanoseconds()/1000000;
1228 mo_.m_header.sender_id = config_.vehicle_id;
1229 mo_.m_header.recipient_id = upcoming_id_;
1230 mo_.m_header.sender_bsm_id = bsm_id_;
1232
1233 double vehicle_acceleration_limit_ = config_.vehicle_accel_limit * config_.vehicle_accel_limit_multiplier;
1234 double vehicle_deceleration_limit_ = -1 * config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier;
1235
1236 std::string intersection_turn_direction = "straight";
1237 if (intersection_turn_direction_ == TurnDirection::Right) intersection_turn_direction = "right";
1238 if (intersection_turn_direction_ == TurnDirection::Left) intersection_turn_direction = "left";
1239
1240 mo_.strategy_params = "access: 1, max_accel: " + std::to_string(vehicle_acceleration_limit_) + // NOTE: Access currently set to 1 at all times since its not specified by streets
1241 ", max_decel: " + std::to_string(vehicle_deceleration_limit_) + ", react_time: " + std::to_string(config_.reaction_time) +
1242 ", min_gap: " + std::to_string(config_.min_gap) + ", depart_pos: 0" + // NOTE: Departure position set to 0 at all times since it's not specified by streets
1243 ", turn_direction: " + intersection_turn_direction + ", msg_count: " + std::to_string(bsm_msg_count_) + ", sec_mark: " + std::to_string(bsm_sec_mark_);
1244
1245
1246 return mo_;
1247}
double reaction_time
Double: Vehicle reaction time to a received schedule in seconds (approximate value,...
double min_gap
Double: Minimum inter-vehicle gap.

References bsm_id_, bsm_msg_count_, bsm_sec_mark_, config_, intersection_turn_direction_, lci_strategic_plugin::Left, light_controlled_intersection_strategy_, lci_strategic_plugin::LCIStrategicPluginConfig::min_gap, lci_strategic_plugin::LCIStrategicPluginConfig::reaction_time, lci_strategic_plugin::Right, carma_cooperative_perception::to_string(), upcoming_id_, lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_accel_limit, lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_accel_limit_multiplier, lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_decel_limit, lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_decel_limit_multiplier, and lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_id.

Referenced by publishMobilityOperation().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ get_availability()

bool lci_strategic_plugin::LCIStrategicPlugin::get_availability ( )
virtual

Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes.

Returns
This method should return true if the plugin's current understanding of the world means it would be capable of planning or executing its capability.

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 1568 of file lci_strategic_plugin.cpp.

1569{
1570 return true;
1571}

◆ get_boundary_traj_params()

std::vector< TrajectoryParams > lci_strategic_plugin::LCIStrategicPlugin::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 
)
private

Get all possible trajectory smoothing parameters for each segments. Later this will be used to generate a single trajectory.

Parameters
tcurrent time in seconds
v0starting_speed
v1departure_speed
v_maxspeed_limit
v_minminimum speed
a_maxmax_comfort_acceleration limit
a_minmax_comfort_deceleration limit
x0current_downtrack
x_endtraffic_light_down_track
dxdistance_remaining_to_traffic_light
boundary_distancesboundary_distances to compare against current state
Returns
all possible trajectory smoothing parameters to later generate single trajectory

Definition at line 514 of file lci_strategic_plugin_algo.cpp.

515{
516 double dx1 = boundary_distances.dx1;
517 double dx2 = boundary_distances.dx2;
518 double dx3 = boundary_distances.dx3;
519 double dx4 = boundary_distances.dx4;
520 double dx5 = boundary_distances.dx5;
521 TrajectoryParams traj1, traj2, traj3, traj4, traj5, traj6, traj7, traj8;
522
523 // t1, t2, t3
524 if (dx < dx2)
525 {
526 traj1 = boundary_accel_or_decel_incomplete_upper(t, v0, v1, a_max, a_min, x0, x_end, dx);
527 traj2 = traj1;
528 traj3 = traj1;
529 }
530 else if (dx < dx1)
531 {
532 traj1 = boundary_accel_nocruise_notmaxspeed_decel(t, v0, v1, a_max, a_min, x0, x_end, dx);
533 traj2 = traj1;
534 traj3 = boundary_accel_or_decel_complete_upper(t, v0, v1, x0, x_end, dx);
535 }
536 else
537 {
538 traj1 = boundary_accel_cruise_maxspeed_decel(t, v0, v1, v_max, a_max, a_min, x0, x_end, dx);
539 traj2 = boundary_accel_nocruise_maxspeed_decel(t, v0, v1, v_max, a_max, a_min, x0, x_end, dx);
540 traj3 = boundary_accel_or_decel_complete_upper(t, v0, v1, x0, x_end, dx);
541 }
542 // t4, t5, t6, t7
543 if (dx < dx4)
544 {
545 traj4 = traj1;
546 traj5 = traj1;
547 traj6 = boundary_decel_incomplete_lower(t, v0, a_min, x0, x_end, dx);
548 traj7 = traj6;
549 }
550 else if (dx < dx3)
551 {
552 if (dx < dx2)
553 {
554 traj4 = traj1;
555 }
556 else
557 {
558 traj4 = boundary_decel_nocruise_notminspeed_accel(t, v0, v1, v_min, a_max, a_min, x0, x_end, dx);
559 }
560 traj5 = traj4;
561 traj6 = boundary_decel_nocruise_minspeed_accel_incomplete(t, v0, v_min, a_max, a_min, x0, x_end, dx);
562 traj7 = boundary_decel_cruise_minspeed(t, v0, v_min, a_min, x0, x_end, dx);
563
564 }
565 else
566 {
567 traj4 = boundary_decel_nocruise_minspeed_accel_complete(t, v0, v1, v_max, v_min, a_max, a_min, x0, x_end, dx);
568 traj5 = boundary_decel_cruise_minspeed_accel(t, v0, v1, v_min, a_max, a_min, x0, x_end, dx);
569 traj6 = traj5;
570 traj7 = boundary_decel_cruise_minspeed(t, v0, v_min, a_min, x0, x_end, dx);
571 }
572 // t8
573 if (dx < dx4)
574 {
575 traj8 = traj6;
576 }
577 else if (dx < dx5)
578 {
579 traj8 = boundary_decel_incomplete_lower(t, v0, a_min, x0, x_end, dx);
580 }
581 else
582 {
583 traj8 = boundary_decel_cruise_minspeed_decel(t, v0, v_min, a_min, x0, x_end, dx);
584 }
585
586 return {traj1, traj2, traj3, traj4, traj5, traj6, traj7, traj8};
587}
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)
TrajectoryParams boundary_decel_cruise_minspeed_decel(double t, double v0, double v_min, double a_min, double x0, double x_end, double dx)
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)
TrajectoryParams boundary_accel_or_decel_complete_upper(double t, double v0, double v1, double x0, double x_end, double dx)
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)
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)
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)
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)
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)
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)
TrajectoryParams boundary_decel_cruise_minspeed(double t, double v0, double v_min, double a_min, double x0, double x_end, double dx)

References boundary_accel_cruise_maxspeed_decel(), boundary_accel_nocruise_maxspeed_decel(), boundary_accel_nocruise_notmaxspeed_decel(), boundary_accel_or_decel_complete_upper(), boundary_accel_or_decel_incomplete_upper(), boundary_decel_cruise_minspeed(), boundary_decel_cruise_minspeed_accel(), boundary_decel_cruise_minspeed_decel(), boundary_decel_incomplete_lower(), boundary_decel_nocruise_minspeed_accel_complete(), boundary_decel_nocruise_minspeed_accel_incomplete(), boundary_decel_nocruise_notminspeed_accel(), lci_strategic_plugin::BoundaryDistances::dx1, lci_strategic_plugin::BoundaryDistances::dx2, lci_strategic_plugin::BoundaryDistances::dx3, lci_strategic_plugin::BoundaryDistances::dx4, and lci_strategic_plugin::BoundaryDistances::dx5.

Referenced by planWhenAPPROACHING().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ get_delta_x()

BoundaryDistances lci_strategic_plugin::LCIStrategicPlugin::get_delta_x ( double  v0,
double  v1,
double  v_max,
double  v_min,
double  a_max,
double  a_min 
)
private

Get boundary distances used to compare against current state in order to create trajectory smoothing parameters.

Parameters
v0starting_speed
v1departure_speed
v_maxspeed_limit
v_minminimum speed
a_maxmax_comfort_acceleration limit
a_minmax_comfort_deceleration limit
Returns
boundary distances used to generate trajectory smoothing segments

Definition at line 462 of file lci_strategic_plugin_algo.cpp.

463{
464 BoundaryDistances distances;
465
466 distances.dx1 = ((pow(v_max, 2) - pow(v0, 2)) / (2 * a_max)) + ((pow(v1, 2) - pow(v_max, 2)) / (2 * a_min));
467 if (v1 > v0)
468 distances.dx2 = ((pow(v1, 2) - pow(v0, 2)) / (2 * a_max));
469 else
470 distances.dx2 = ((pow(v1, 2) - pow(v0, 2)) / (2 * a_min));
471
472 distances.dx3 = ((pow(v_min, 2) - pow(v0, 2)) / (2 * a_min)) + ((pow(v1, 2) - pow(v_min, 2)) / (2 * a_max));
473 distances.dx4 = ((pow(v_min, 2) - pow(v0, 2)) / (2 * a_min));
474 distances.dx5 = - pow(v0, 2) / (2 * a_min);
475
476 return distances;
477}

References lci_strategic_plugin::BoundaryDistances::dx1, lci_strategic_plugin::BoundaryDistances::dx2, lci_strategic_plugin::BoundaryDistances::dx3, lci_strategic_plugin::BoundaryDistances::dx4, and lci_strategic_plugin::BoundaryDistances::dx5.

Referenced by planWhenAPPROACHING().

Here is the caller graph for this function:

◆ get_distance_to_accel_or_decel_once()

double lci_strategic_plugin::LCIStrategicPlugin::get_distance_to_accel_or_decel_once ( double  current_speed,
double  departure_speed,
double  max_accel,
double  max_decel 
) const
private

Get required distance to accel or decel to reach departure_speed with given speed and acceleration parameters.

Parameters
current_speedCurrent speed in m/s
departure_speedSpeed to enter into the intersection in m/s
max_accelAcceleration rate during acceleration segment in m/s^2
max_decelDeceleration rate during acceleration segment in m/s^2
Returns
estimated distance to accel or decel in meters

Definition at line 40 of file lci_strategic_plugin_algo.cpp.

41{
42 if (current_speed <= departure_speed + epsilon_)
43 {
44 return (std::pow(departure_speed, 2) - std::pow(current_speed, 2))/(2 * max_accel);
45 }
46 else
47 {
48 return (std::pow(departure_speed, 2) - std::pow(current_speed, 2))/(2 * max_decel);
49 }
50}

References epsilon_.

Referenced by get_earliest_entry_time().

Here is the caller graph for this function:

◆ get_distance_to_accel_or_decel_twice()

double lci_strategic_plugin::LCIStrategicPlugin::get_distance_to_accel_or_decel_twice ( double  free_flow_speed,
double  current_speed,
double  departure_speed,
double  max_accel,
double  max_decel 
) const
private

Get required distance to accel then decel, or vise versa - each at least once - to reach departure_speed with given speed and acceleration parameters.

Parameters
free_flow_speedFree flow speed in m/s (usually the speed limit)
current_speedCurrent speed in m/s
departure_speedSpeed to enter into the intersection in m/s
max_accelAcceleration rate during acceleration segment in m/s^2
max_decelDeceleration rate during acceleration segment in m/s^2
Returns
estimated distance to accel/decel or vice versa exactly twice in meters

Definition at line 34 of file lci_strategic_plugin_algo.cpp.

35{
36 // (v_e^2 - v^2)/ 2a_a + (v_d^2 - v_e^2)/ 2a_d
37 return (std::pow(free_flow_speed, 2) - std::pow(current_speed, 2))/(2 * max_accel) + (std::pow(departure_speed, 2) - std::pow(free_flow_speed, 2))/(2* max_decel);
38}

Referenced by get_earliest_entry_time().

Here is the caller graph for this function:

◆ get_earliest_entry_time()

rclcpp::Duration lci_strategic_plugin::LCIStrategicPlugin::get_earliest_entry_time ( double  remaining_distance,
double  free_flow_speed,
double  current_speed,
double  departure_speed,
double  max_accel,
double  max_decel 
) const
private

Gets the earliest entry time into the intersection that is kinematically possible for the vehicle. Designed to get it for motion that has maximum one acceleration, deceleration, or cruise segments until destination.

Parameters
remaining_distanceDistance to the intersection in meters
free_flow_speedFree flow speed along the route until the intersection (usually speed_limit) in m/s
current_speedCurrent speed in m/s
departure_speedSpeed into the intersection in m/s. A.k.a target speed at the intersection
max_accelThe acceleration in m/s^2 of the acceleration segment
max_decelThe deceleration in m/s^2 of the deceleration segment
Returns
The estimated stopping distance in meters NOTE: TSMO UC2: Algorithm 1. Earliest entering time estimation algorithm for EVs in all cooperation classes (defined for C-ADS-equipped vehicles)

Definition at line 191 of file lci_strategic_plugin_algo.cpp.

192{
193 double x = remaining_distance;
194 double x2 = get_distance_to_accel_or_decel_once(current_speed, departure_speed, max_accel, max_decel);
195 double x1 = get_distance_to_accel_or_decel_twice(free_flow_speed, current_speed, departure_speed, max_accel, max_decel);
196 double v_hat = get_inflection_speed_value(x, x1, x2, free_flow_speed, current_speed, departure_speed, max_accel, max_decel);
197
198 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "x: " << x << ", x2: " << x2 << ", x1: " << x1 << ", v_hat: " << v_hat);
199
200 if (v_hat <= config_.algo_minimum_speed - epsilon_ || isnan(v_hat))
201 {
202 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Detected that v_hat is smaller than allowed!!!: " << v_hat);
204 }
205
206 if (v_hat >= free_flow_speed + epsilon_)
207 {
208 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Detected that v_hat is Bigger than allowed!!!: " << v_hat);
209 v_hat = free_flow_speed;
210 }
211
212 rclcpp::Duration t_accel(0,0);
213 if ( x < x2 && current_speed > departure_speed)
214 {
215 t_accel = rclcpp::Duration::from_nanoseconds(0.0);
216 }
217 else
218 {
219 t_accel = rclcpp::Duration::from_nanoseconds(std::max((v_hat - current_speed) / max_accel, 0.0) * 1e9);
220 }
221 rclcpp::Duration t_decel(0,0);
222 if ( x < x2 && current_speed < departure_speed)
223 {
224 t_decel = rclcpp::Duration::from_nanoseconds(0.0);
225 }
226 else
227 {
228 if (x < x2)
229 {
230 t_decel = rclcpp::Duration::from_nanoseconds(std::max((v_hat - current_speed) / max_decel, 0.0) * 1e9);
231
232 }
233 else
234 {
235 t_decel = rclcpp::Duration::from_nanoseconds(std::max((departure_speed - v_hat) / max_decel, 0.0) * 1e9);
236 }
237 }
238
239 rclcpp::Duration t_cruise(0,0);
240 if (x1 <= x)
241 {
242 t_cruise = rclcpp::Duration::from_nanoseconds(std::max((x - x1)/v_hat, 0.0) * 1e9);
243 }
244 else
245 {
246 t_cruise = rclcpp::Duration::from_nanoseconds(0.0);
247 }
248 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "t_accel: " << t_accel.seconds() << ", t_cruise: " << t_cruise.seconds() << ", t_decel: " << t_decel.seconds());
249 return t_accel + t_cruise + t_decel;
250
251}
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...
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...
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.

References lci_strategic_plugin::LCIStrategicPluginConfig::algo_minimum_speed, config_, epsilon_, get_distance_to_accel_or_decel_once(), get_distance_to_accel_or_decel_twice(), get_inflection_speed_value(), and process_traj_logs::x.

Referenced by planWhenAPPROACHING().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ get_eet_or_tbd()

rclcpp::Time lci_strategic_plugin::LCIStrategicPlugin::get_eet_or_tbd ( const rclcpp::Time &  earliest_entry_time,
const lanelet::CarmaTrafficSignalPtr &  signal 
) const
private

Returns the later of earliest_entry_time or the end of the available signal states in the traffic_signal.

Parameters
earliest_entry_timeEarliest ROS entry time into the intersection kinematically possible
signalCARMATrafficSignal that is responsible for the intersection along route
Returns
Returns the later of earliest_entry_time or the end of the available signal states in the traffic_signal

Definition at line 52 of file lci_strategic_plugin_algo.cpp.

53{
54 // If no green signal found after earliest entry time
55 auto start_of_tbd_time = rclcpp::Time((lanelet::time::toSec(signal->recorded_time_stamps.back().first) + EPSILON) * 1e9);
56 if (earliest_entry_time > start_of_tbd_time)
57 {
58 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Not enough signals are available to check for EET, so returning EET at: " << std::to_string(earliest_entry_time.seconds()));
59 return earliest_entry_time; //return TBD or EET red if no green is found...
60 }
61
62 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "No valid green entry time found, so returning start of TBD at: " << std::to_string(start_of_tbd_time.seconds()));
63 return start_of_tbd_time;
64}
#define EPSILON

References EPSILON, and carma_cooperative_perception::to_string().

Referenced by get_final_entry_time_and_conditions().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ get_final_entry_time_and_conditions()

std::tuple< rclcpp::Time, bool, bool > lci_strategic_plugin::LCIStrategicPlugin::get_final_entry_time_and_conditions ( const VehicleState current_state,
const rclcpp::Time &  earliest_entry_time,
lanelet::CarmaTrafficSignalPtr  traffic_light 
)
private

Get the final entry time from either vehicle's own internal ET calculation (TSMO UC2) or from carma-street (TSMO UC3) This function also applies necessary green_buffer to adjust the ET.

Parameters
current_stateCurrent state of the vehicle
earliest_entry_timeEarliest entry time calculated by the vehicle
traffic_lighttraffic signal the vehicle is using
Returns
tuple of <final entry time the vehicle uses to enter the intersection, is_entry_time_within_green_or_tbd: true if ET is in green or TBD (related to UC3, always set to true in UC2), in_tbd: true if ET is in TBD (false if in UC2)>

Definition at line 264 of file lci_strategic_plugin_algo.cpp.

265{
266 rclcpp::Time nearest_green_entry_time = rclcpp::Time(0);
267 bool is_entry_time_within_green_or_tbd = false;
268 bool in_tbd = true;
269
271 {
272 auto nearest_green_entry_time_optional = get_nearest_green_entry_time(current_state.stamp, earliest_entry_time, traffic_light);
273 is_entry_time_within_green_or_tbd = true;
274
275 if (!nearest_green_entry_time_optional)
276 {
277 nearest_green_entry_time = get_eet_or_tbd(earliest_entry_time, traffic_light);
278 }
279 else
280 {
281 in_tbd = false;
282 nearest_green_entry_time = nearest_green_entry_time_optional.value() + rclcpp::Duration::from_nanoseconds(EPSILON * 1e9); //0.01sec more buffer since green_light algorithm's timestamp picks the previous signal - Vehicle Estimation
283 }
284 }
286 {
287 nearest_green_entry_time = rclcpp::Time(std::max(earliest_entry_time.seconds(), (scheduled_enter_time_)/1000.0) * 1e9) + rclcpp::Duration::from_nanoseconds(EPSILON * 1e9); //Carma Street
288
289 // check if scheduled_enter_time_ is inside the available states interval
290 size_t i = 0;
291
292
293 for (auto pair : traffic_light->recorded_time_stamps)
294 {
295 if (lanelet::time::timeFromSec(nearest_green_entry_time.seconds()) < pair.first)
296 {
297 if (isStateAllowedGreen(pair.second))
298 {
299 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "ET is inside the GREEN phase! where starting time: " << std::to_string(lanelet::time::toSec(traffic_light->recorded_start_time_stamps[i]))
300 << ", ending time of that green signal is: " << std::to_string(lanelet::time::toSec(pair.first)));
301 is_entry_time_within_green_or_tbd = true;
302 }
303 else
304 {
305 RCLCPP_ERROR_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Vehicle should plan cruise and stop as ET is inside the RED or YELLOW phase! where starting time: " << std::to_string(lanelet::time::toSec(traffic_light->recorded_start_time_stamps[i]))
306 << ", ending time of that green signal is: " << std::to_string(lanelet::time::toSec(pair.first)));
307 is_entry_time_within_green_or_tbd = false;
308 }
309
310 in_tbd = false;
311 break;
312 }
313 i++;
314 }
315
316 if (in_tbd)
317 {
318 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "ET is inside TBD phase! where starting time: " << std::to_string(lanelet::time::toSec(traffic_light->recorded_time_stamps.back().first)));
319 is_entry_time_within_green_or_tbd = true;
320 }
321
322 }
323 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "nearest_green_entry_time: " << nearest_green_entry_time.get_clock_type());
324 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "current_state.stamp: " << current_state.stamp.get_clock_type());
325
326 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "nearest_green_entry_time: " << std::to_string(nearest_green_entry_time.seconds()) << ", with : " << std::to_string((nearest_green_entry_time - current_state.stamp).seconds()) << " seconds left at: " << std::to_string(current_state.stamp.seconds()));
327
329 { // always pick later of buffered green entry time, or earliest entry time
330 nearest_green_entry_time = rclcpp::Time(std::max(nearest_green_entry_time.seconds(), nearest_green_entry_time_cached_.get().seconds()) * 1e9);
331 }
332
333 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "After accounting for cached - nearest_green_entry_time: " << std::to_string(nearest_green_entry_time.seconds()) << ", with : " << std::to_string((nearest_green_entry_time - current_state.stamp).seconds()) << " seconds left at: " << std::to_string(current_state.stamp.seconds()));
334
335 if (!nearest_green_entry_time_cached_ && is_entry_time_within_green_or_tbd)
336 {
337 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Applying green_light_buffer for the first time and caching! nearest_green_entry_time (without buffer):" << std::to_string(nearest_green_entry_time.seconds()) << ", and earliest_entry_time: " << std::to_string(earliest_entry_time.seconds()));
338 // save first calculated nearest_green_entry_time + buffer to compare against in the future as nearest_green_entry_time changes with earliest_entry_time
339
340 // check if it needs buffer below:
341 rclcpp::Time early_arrival_time_green_et =
342 nearest_green_entry_time - rclcpp::Duration::from_nanoseconds(config_.green_light_time_buffer * 1e9);
343
344 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "early_arrival_time_green_et: " << std::to_string(early_arrival_time_green_et.seconds()));
345
346 auto early_arrival_state_green_et_optional = traffic_light->predictState(lanelet::time::timeFromSec(early_arrival_time_green_et.seconds()));
347
348 if (!validLightState(early_arrival_state_green_et_optional, early_arrival_time_green_et))
349 {
350 RCLCPP_ERROR_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Unable to resolve give signal...");
351 return std::make_tuple(rclcpp::Time(0), is_entry_time_within_green_or_tbd, in_tbd);
352 }
353
354 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "early_arrival_state_green_et: " << early_arrival_state_green_et_optional.get().second);
355
356 bool can_make_early_arrival = isStateAllowedGreen(early_arrival_state_green_et_optional.get().second);
357
358 // nearest_green_entry_time is by definition on green, so only check early_arrival
359 if (can_make_early_arrival) // Green light with Certainty
360 {
361 nearest_green_entry_time_cached_ = nearest_green_entry_time; //don't apply buffer if ET is in green
362 }
363 else //buffer is needed
364 {
365 // below logic stores correct buffered timestamp into nearest_green_entry_time_cached_ to be used later
366
367 rclcpp::Time nearest_green_signal_start_time = rclcpp::Time(0);
368 if (traffic_light->fixed_cycle_duration.total_milliseconds()/1000.0 > 1.0) // UC2
369 {
370 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "UC2 Handling");
371 auto normal_arrival_state_green_et_optional = traffic_light->predictState(lanelet::time::timeFromSec(nearest_green_entry_time.seconds()));
372
373 if (!validLightState(normal_arrival_state_green_et_optional, nearest_green_entry_time))
374 {
375 RCLCPP_ERROR_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Unable to resolve give signal...");
376 return std::make_tuple(rclcpp::Time(0), is_entry_time_within_green_or_tbd, in_tbd);
377 }
378
379 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "normal_arrival_signal_end_time: " << std::to_string(lanelet::time::toSec(normal_arrival_state_green_et_optional.get().first)));
380
381 // nearest_green_signal_start_time = normal_arrival_signal_end_time (green guaranteed) - green_signal_duration
382 nearest_green_signal_start_time = rclcpp::Time(lanelet::time::toSec(getMovementAllowedDuration(traffic_light)));
383 }
384 else // UC3
385 {
386 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "UC3 Handling");
387
388 for (size_t i = 0; i < traffic_light->recorded_start_time_stamps.size(); i++)
389 {
390 if (isStateAllowedGreen(traffic_light->recorded_time_stamps[i].second) &&
391 lanelet::time::timeFromSec(nearest_green_entry_time.seconds()) < traffic_light->recorded_time_stamps[i].first ) // Make sure it is in correct GREEN phase there are multiple
392 {
393 nearest_green_signal_start_time = rclcpp::Time(lanelet::time::toSec(traffic_light->recorded_start_time_stamps[i]) * 1e9);
394 break;
395 }
396 }
397
398 if (nearest_green_signal_start_time == rclcpp::Time(0)) //in tdb
399 {
400 nearest_green_signal_start_time = rclcpp::Time(lanelet::time::toSec(traffic_light->recorded_time_stamps.back().first) * 1e9);
401 }
402 }
403
404 // If ET is within green or TBD, it should always aim for at least minimum of "start_time of green or tdb + green_buffer" for safety
405
406 nearest_green_entry_time_cached_ = nearest_green_signal_start_time + rclcpp::Duration::from_nanoseconds((config_.green_light_time_buffer + EPSILON) * 1e9);
407
408 // EPSILON=0.01 is there because if predictState's input exactly falls on ending_time it picks the previous state.
409 //For example, if 0 - 10s is GREEN, and 10 - 12s is YELLOW, checking exactly 10.0s will return GREEN,
410 //but 10.01s will return YELLOW. This 0.01 convention is used throughout the file, so thought it is better
411 //to keep it consistent and probably too detailed for the user to think about, which is why it is not included in the buffer.
412 //Actually including in the buffer doesn't work because it uses that same buffer to check early and late. If buffer is 2s and
413 //green starts at 10s, it will check +/-2s from 12s. If the buffer was 2.01s and green starts at 10s again, it checks +/-2.01
414 //from 12.01, so both checks 10s.
415
416 }
417
418 nearest_green_entry_time = rclcpp::Time(std::max(nearest_green_entry_time.seconds(), nearest_green_entry_time_cached_.get().seconds()) * 1e9);
419 }
420
421 if (nearest_green_entry_time_cached_ && nearest_green_entry_time > nearest_green_entry_time_cached_.get())
422 {
423 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Earliest entry time... has gone past the cashed entering time. nearest_green_entry_time_cached_ (which can also be TBD):" << std::to_string(nearest_green_entry_time_cached_.get().seconds()) << ", and earliest_entry_time: " << std::to_string(earliest_entry_time.seconds()));
424 }
425 return std::make_tuple(nearest_green_entry_time, is_entry_time_within_green_or_tbd, in_tbd);
426}
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...
boost::optional< rclcpp::Time > nearest_green_entry_time_cached_
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 ...
boost::posix_time::time_duration getMovementAllowedDuration(lanelet::CarmaTrafficSignalPtr traffic_light)
Returns the duration for the allowed movements.

References config_, lci_strategic_plugin::LCIStrategicPluginConfig::enable_carma_streets_connection, EPSILON, get_eet_or_tbd(), get_nearest_green_entry_time(), getMovementAllowedDuration(), lci_strategic_plugin::LCIStrategicPluginConfig::green_light_time_buffer, process_bag::i, isStateAllowedGreen(), nearest_green_entry_time_cached_, scheduled_enter_time_, lci_strategic_plugin::LCIStrategicPlugin::VehicleState::stamp, carma_cooperative_perception::to_string(), and validLightState().

Referenced by planWhenAPPROACHING().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ get_inflection_speed_value()

double lci_strategic_plugin::LCIStrategicPlugin::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
private

Get the speed between the acceleration and deceleration pieces.

Parameters
xDistance remaining in meters
x1Required distance to accel then decel, or vise versa-each at least once-to reach departure_speed with given speed and acceleration parameters
x2required distance to accel or decel to reach departure_speed with given speed and acceleration parameters
free_flow_speedFree flow speed in m/s (usually the speed limit)
current_speedCurrent speed in m/s
departure_speedSpeed to enter into the intersection in m/s
max_accelAcceleration rate during acceleration segment in m/s^2
max_decelDeceleration rate during acceleration segment in m/s^2 NOTE: note that this returns departure speed if x < x2 since vehicle's trajectory corresponding to its EET would contain only one acceleration or deceleration piece
Returns
The estimated stopping distance in meters

Definition at line 428 of file lci_strategic_plugin_algo.cpp.

429{
430 if (x >= x1)
431 {
432 return free_flow_speed;
433 }
434 else if (x1 > x && x >= x2)
435 {
436 return std::sqrt((2 * x * max_accel * max_decel + max_decel * std::pow(current_speed, 2) - max_accel * (std::pow(departure_speed, 2)))/(max_decel - max_accel));
437 }
438 else if (x2 > x)
439 {
440 if (current_speed <= departure_speed)
441 {
442 return std::sqrt(2 * x * max_accel + std::pow(current_speed, 2));
443 }
444 else
445 {
446 return std::sqrt(2 * x * max_decel + std::pow(current_speed, 2));
447 }
448 }
449
450}

References process_traj_logs::x.

Referenced by get_earliest_entry_time().

Here is the caller graph for this function:

◆ get_nearest_green_entry_time()

std::optional< rclcpp::Time > lci_strategic_plugin::LCIStrategicPlugin::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
private

Provides the scheduled entry time for the vehicle in the future. This scheduled time is the earliest possible entry time that is during green phase and after timestamps both that is kinematically possible and required times for vehicle in front to pass through first Returns std::nullopt if no valid green entry time found.

Parameters
current_timeCurrent state's ROS time
earliest_entry_timeEarliest ROS entry time into the intersection kinematically possible
signalCARMATrafficSignal that is responsible for the intersection along route
minimum_required_green_timeMinimum seconds required by other vehicles in front to pass through the intersection first
Returns
Nearest ROS entry time during green phase if it exists, std::nullopt if no valid green entry time found NOTE: TSMO UC2: Algorithm 2. Entering time estimation algorithm for EVs in cooperation Class A when the SPaT plan is fixed-time (defined for C-ADS-equipped vehicles)

Definition at line 66 of file lci_strategic_plugin_algo.cpp.

67{
68 boost::posix_time::time_duration g = lanelet::time::durationFromSec(minimum_required_green_time); // provided by considering min headways of vehicles in front
69 boost::posix_time::ptime t = lanelet::time::timeFromSec(current_time.seconds()); // time variable
70 boost::posix_time::ptime eet = lanelet::time::timeFromSec(earliest_entry_time.seconds()); // earliest entry time
71
72 // check if the signal even has a green signal
73 bool has_green_signal = false;
74 for (auto pair : signal->recorded_time_stamps)
75 {
76 if (isStateAllowedGreen(pair.second) && t <= pair.first )
77 {
78 has_green_signal = true;
79 break;
80 }
81 }
82
83 if (!has_green_signal)
84 {
85 return std::nullopt;
86 }
87
88 auto curr_pair = signal->predictState(t);
89 if (!curr_pair)
90 throw std::invalid_argument("Traffic signal with id:" + std::to_string(signal->id()) + ", does not have any recorded time stamps!");
91
92 boost::posix_time::time_duration theta = curr_pair.get().first - t; // remaining time left in this state
93 auto p = curr_pair.get().second;
94 while ( 0.0 < g.total_milliseconds() || (!isStateAllowedGreen(p))) //green
95 {
97 {
98 if (g < theta)
99 {
100 t = t + g;
101 theta = theta - g;
102 g = boost::posix_time::seconds(0);
103 }
104 else
105 {
106 t = t + theta;
107 g = g - theta;
108 curr_pair = signal->predictState(t + boost::posix_time::milliseconds(20)); // select next phase
109 p = curr_pair.get().second;
110 theta = curr_pair.get().first - t;
111 }
112 }
113 else
114 {
115 t = t + theta;
116 curr_pair = signal->predictState(t + boost::posix_time::milliseconds(20)); // select next phase
117 p = curr_pair.get().second;
118 theta = curr_pair.get().first - t;
119 }
120 }
121
122 if (t <= eet)
123 {
124 double cycle_duration = signal->fixed_cycle_duration.total_milliseconds()/1000.0;
125 if (cycle_duration < 0.001) //if it is a dynamic traffic signal not fixed
126 cycle_duration = lanelet::time::toSec(signal->recorded_time_stamps.back().first) - lanelet::time::toSec(signal->recorded_start_time_stamps.front());
127
128 t = t + lanelet::time::durationFromSec(std::floor((eet - t).total_milliseconds()/1000.0/cycle_duration) * cycle_duration); //fancy logic was needed to compile
129 curr_pair = signal->predictState(t + boost::posix_time::milliseconds(20)); // select next phase
130 p = curr_pair.get().second;
131 theta = curr_pair.get().first - t;
132 while ( t < eet || !isStateAllowedGreen(p))
133 {
134 if ( (isStateAllowedGreen(p)) && eet - t < theta)
135 {
136 t = eet;
137 theta = theta - (eet - t);
138 }
139 else
140 {
141 t = t + theta;
142 curr_pair = signal->predictState(t + boost::posix_time::milliseconds(20)); // select next phase
143 p = curr_pair.get().second;
144 theta = curr_pair.get().first - t;
145 }
146
147 if (t != lanelet::time::timeFromSec(lanelet::time::INFINITY_END_TIME_FOR_NOT_ENOUGH_STATES))
148 continue;
149
150 // if no green signal left in the signal states, return
151 return std::nullopt;
152 }
153 }
154 return rclcpp::Time(lanelet::time::toSec(t) * 1e9);
155}

References isStateAllowedGreen(), and carma_cooperative_perception::to_string().

Referenced by get_final_entry_time_and_conditions().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ get_trajectory_smoothing_activation_distance()

double lci_strategic_plugin::LCIStrategicPlugin::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
private

Gets maximum distance (nearest downtrack) the trajectory smoothing algorithm makes difference than simple lane following within half of one full fixed cycle time boundary compared to free flow arrival time.

Parameters
time_remaining_at_free_flowFree flow arrival at the intersection
full_cycle_durationOne fixed cycle of the signal
current_speedCurrent speed in m/s
speed_limitSpeed limit speed in m/s
departure_speedSpeed into the intersection in m/s. A.k.a target speed at the intersection
max_accelThe acceleration in m/s^2 of the acceleration segment
max_decelThe deceleration in m/s^2 of the deceleration segment
Returns
the max distance the plugin should activate to support all speed profile cases of the trajectory smoothing algorithm -1 if it is within the distance already

Definition at line 157 of file lci_strategic_plugin_algo.cpp.

158{
159 // TSMO USE CASE 2: Figure 7. Trajectory smoothing solution Case 2. Subsituted a+ as max_accel and solved for inflection_speed
160 double accel_ratio = max_accel / max_decel;
161 double remaining_time = time_remaining_at_free_flow - full_cycle_duration / 2;
162 double inflection_speed = (max_accel * remaining_time - accel_ratio * departure_speed + current_speed)/ (1 - accel_ratio);
163 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "ENTER TRAJ CALC: time_remaining_at_free_flow: " << time_remaining_at_free_flow << ", full_cycle_duration: " << full_cycle_duration << ", inflection_speed: " << inflection_speed);
164
165 if (remaining_time < 0)
166 return -1;
167
168 if (inflection_speed > 0 && inflection_speed <= speed_limit + epsilon_ && inflection_speed >= departure_speed - epsilon_)
169 {
170 // kinematic equation to find distance of acceleration + deceleration
171 // (vf^2 - vi^2)/2a = d
172 double d = (std::pow(inflection_speed, 2) - std::pow (current_speed, 2)) / (2 * max_accel) + (std::pow(departure_speed, 2) - std::pow(inflection_speed, 2)) / (2 * max_decel);
173 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "calculated distance WITHOUT cruising: " << d);
174 return d;
175 }
176 else //there must be cruising
177 {
178 // acceleration and deceleration parts must reach maximum speed
179 // kinematic equation: t = (vf - vi)/ a where vf = 0
180 double decel_time = (current_speed - speed_limit) / max_decel;
181 double accel_time = (speed_limit - current_speed) / max_accel;
182 double cruising_time = remaining_time - decel_time - accel_time;
183 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "decel_time: " << decel_time << ", accel_time: " << accel_time << ", cruising_time: " << cruising_time);
184 double d = (std::pow(speed_limit, 2) - std::pow (current_speed, 2)) / (2 * max_accel) + (std::pow(departure_speed, 2) - std::pow(speed_limit, 2)) / (2 * max_decel) + cruising_time * speed_limit;
185 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "calculated distance with cruising: " << d << ", accel_seg: " << (std::pow(speed_limit, 2) - std::pow (current_speed, 2)) / (2 * max_accel) <<
186 ", cruising: " << + cruising_time * speed_limit << ", decel_seg:" << (std::pow(departure_speed, 2) - std::pow(speed_limit, 2)) / (2 * max_decel));
187 return d;
188 }
189}

References epsilon_.

◆ get_ts_case()

TrajectoryParams lci_strategic_plugin::LCIStrategicPlugin::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 
)
private

Definition at line 589 of file lci_strategic_plugin_algo.cpp.

590{
591 double dx1 = boundary_distances.dx1;
592 double dx2 = boundary_distances.dx2;
593 double dx3 = boundary_distances.dx3;
594 double dx4 = boundary_distances.dx4;
595 double dx5 = boundary_distances.dx5;
596
597 if (params.size() != 8)
598 {
599 throw std::invalid_argument("Not enough trajectory paras given! Given size: " + std::to_string(params.size()));
600 }
601
602 TrajectoryParams traj1 = params[0];
603 TrajectoryParams traj2 = params[1];
604 TrajectoryParams traj3 = params[2];
605 TrajectoryParams traj4 = params[3];
606 TrajectoryParams traj5 = params[4];
607 TrajectoryParams traj6 = params[5];
608 TrajectoryParams traj7 = params[6];
609 TrajectoryParams traj8 = params[7];
610 TrajectoryParams veh_traj;
611 veh_traj.is_algorithm_successful = true;
612
613 if (traj1.t3_ <= et && et < traj2.t3_)
614 {
615 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CALCULATED: case 1");
616 veh_traj = ts_case1(t, et, v0, v1, v_max, a_max, a_min, x0, x_end, dx);
617 veh_traj.case_num = CASE_1;
618 }
619 else if (traj2.t3_ <= et && et < traj3.t3_)
620 {
621 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CALCULATED: case 2");
622 veh_traj = ts_case2(t, et, v0, v1, a_max, a_min, x0, x_end, dx);
623 veh_traj.case_num = CASE_2;
624 }
625 else if (traj3.t3_ <= et && et < traj4.t3_)
626 {
627 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CALCULATED: case 3");
628 veh_traj = ts_case3(t, et, v0, v1, a_max, a_min, x0, x_end, dx);
629 veh_traj.case_num = CASE_3;
630 }
631 else if (traj4.t3_ <= et && et < traj5.t3_)
632 {
633 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CALCULATED: case 4");
634 veh_traj = ts_case4(t, et, v0, v1, v_min, a_max, a_min, x0, x_end, dx);
635 veh_traj.case_num = CASE_4;
636 }
637 else if (traj5.t3_ <= et && et < traj6.t3_)
638 {
639 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CALCULATED: case 5");
640 veh_traj = ts_case5(t, et, v0, a_max, a_min, x0, x_end, dx);
641 veh_traj.case_num = CASE_5;
642 }
643 else if (traj6.t3_ <= et && et < traj7.t3_)
644 {
645 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CALCULATED: case 6");
646 veh_traj = ts_case6(t, et, v0, v_min, a_min, x0, x_end, dx, dx3, traj6);
647 veh_traj.case_num = CASE_6;
648 }
649 else if (traj7.t3_ <= et && et <= traj8.t3_)
650 {
651 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CALCULATED: case 7");
652 veh_traj = ts_case7(t, et, v0, v_min, a_min, x0, x_end, dx);
653 veh_traj.case_num = CASE_7;
654 }
655 else
656 {
657 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CALCULATED: case 8");
658 veh_traj = ts_case8(dx, dx5, traj8);
659 veh_traj.case_num = CASE_8;
660 }
661
662 return veh_traj;
663}
TrajectoryParams ts_case7(double t, double et, double v0, double v_min, double a_min, double x0, double x_end, double dx)
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 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)
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)
TrajectoryParams ts_case2(double t, double et, double v0, double v1, double a_max, double a_min, double x0, double x_end, double dx)
TrajectoryParams ts_case8(double dx, double dx5, TrajectoryParams traj8)
TrajectoryParams ts_case3(double t, double et, double v0, double v1, double a_max, double a_min, double x0, double x_end, double dx)
TrajectoryParams ts_case5(double t, double et, double v0, double a_max, double a_min, double x0, double x_end, double dx)

References lci_strategic_plugin::CASE_1, lci_strategic_plugin::CASE_2, lci_strategic_plugin::CASE_3, lci_strategic_plugin::CASE_4, lci_strategic_plugin::CASE_5, lci_strategic_plugin::CASE_6, lci_strategic_plugin::CASE_7, lci_strategic_plugin::CASE_8, lci_strategic_plugin::TrajectoryParams::case_num, lci_strategic_plugin::BoundaryDistances::dx1, lci_strategic_plugin::BoundaryDistances::dx2, lci_strategic_plugin::BoundaryDistances::dx3, lci_strategic_plugin::BoundaryDistances::dx4, lci_strategic_plugin::BoundaryDistances::dx5, lci_strategic_plugin::TrajectoryParams::is_algorithm_successful, lci_strategic_plugin::TrajectoryParams::t3_, carma_cooperative_perception::to_string(), ts_case1(), ts_case2(), ts_case3(), ts_case4(), ts_case5(), ts_case6(), ts_case7(), and ts_case8().

Referenced by planWhenAPPROACHING().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ get_version_id()

std::string lci_strategic_plugin::LCIStrategicPlugin::get_version_id ( )
virtual

Returns the version id of this plugin.

Returns
The version id represented as a string

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 1573 of file lci_strategic_plugin.cpp.

1574{
1575 return "v1.0";
1576}

◆ getLaneletsBetweenWithException()

std::vector< lanelet::ConstLanelet > lci_strategic_plugin::LCIStrategicPlugin::getLaneletsBetweenWithException ( double  start_downtrack,
double  end_downtrack,
bool  shortest_path_only = false,
bool  bounds_inclusive = true 
) const
private

Helper method which calls carma_wm::WorldModel::getLaneletsBetween(start_downtrack, end_downtrack, shortest_path_only, bounds_inclusive) and throws and exception if the returned list of lanelets is empty. See the referenced method for additional details on parameters.

Definition at line 369 of file lci_strategic_plugin.cpp.

373{
374 std::vector<lanelet::ConstLanelet> crossed_lanelets =
375 wm_->getLaneletsBetween(start_downtrack, end_downtrack, shortest_path_only, bounds_inclusive);
376
377 if (crossed_lanelets.size() == 0)
378 {
379 throw std::invalid_argument("getLaneletsBetweenWithException called but inputs do not cross any lanelets going "
380 "from: " +
381 std::to_string(start_downtrack) + " to: " + std::to_string(end_downtrack));
382 }
383
384 return crossed_lanelets;
385}

References carma_cooperative_perception::to_string(), and wm_.

Referenced by extractInitialState(), handleCruisingUntilStop(), handleFailureCase(), handleGreenSignalScenario(), handleStopping(), and planWhenDEPARTING().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ getMovementAllowedDuration()

boost::posix_time::time_duration lci_strategic_plugin::LCIStrategicPlugin::getMovementAllowedDuration ( lanelet::CarmaTrafficSignalPtr  traffic_light)
private

Returns the duration for the allowed movements.

Parameters
traffic_lightThe traffic light object to get the green duration for
Returns
int value of the duration for the allowed movement

Definition at line 253 of file lci_strategic_plugin_algo.cpp.

254{
255 if(traffic_light->signal_durations.find(lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED) != traffic_light->signal_durations.end())
256 {
257 return traffic_light->signal_durations[lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED];
258 }
259
260 return traffic_light->signal_durations[lanelet::CarmaTrafficSignalState::PERMISSIVE_MOVEMENT_ALLOWED];
261
262}

Referenced by get_final_entry_time_and_conditions().

Here is the caller graph for this function:

◆ getTurnDirectionAtIntersection()

TurnDirection lci_strategic_plugin::LCIStrategicPlugin::getTurnDirectionAtIntersection ( std::vector< lanelet::ConstLanelet >  lanelets_list)

Determine the turn direction at intersection.

Parameters
lanelets_listList of lanelets crossed around the intersection area
Returns
turn direction in format of straight, left, right

Definition at line 1249 of file lci_strategic_plugin.cpp.

1250{
1251 TurnDirection turn_direction = TurnDirection::Straight;
1252 for (auto l:lanelets_list)
1253 {
1254 if(l.hasAttribute("turn_direction")) {
1255 std::string direction_attribute = l.attribute("turn_direction").value();
1256 if (direction_attribute == "right")
1257 {
1258 turn_direction = TurnDirection::Right;
1259 break;
1260 }
1261 else if (direction_attribute == "left")
1262 {
1263 turn_direction = TurnDirection::Left;
1264 break;
1265 }
1266 else turn_direction = TurnDirection::Straight;
1267 }
1268 else
1269 {
1270 // if there is no attribute, assumption is straight
1271 turn_direction = TurnDirection::Straight;
1272 }
1273
1274 }
1275 return turn_direction;
1276}

References lci_strategic_plugin::Left, lci_strategic_plugin::Right, and lci_strategic_plugin::Straight.

Referenced by planWhenAPPROACHING().

Here is the caller graph for this function:

◆ handleCruisingUntilStop()

void lci_strategic_plugin::LCIStrategicPlugin::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 
)
private

This function returns valid maneuvers if the vehicle is able to utilize trajectory smoothing parameters to go through the intersection with certainty It utilizes canMakeWithCertainty function to determine if it is able to make it. Corresponds to TSCase 1-7.

Parameters
reqPlan maneuver request
[out]respPlan maneuver response with a list of maneuver plan
current_stateCurrent state of the vehicle
current_state_speedThe current speed to use when planning
traffic_lightCarmaTrafficSignalPtr of the relevant signal
traffic_light_down_trackDowntrack to the given traffic_light
ts_paramstrajectory smoothing params
Returns
void. if successful, resp is non-empty. Resp is empty if not able to make it with certainty or ts_params.is_successful is false

Definition at line 461 of file lci_strategic_plugin.cpp.

467{
468 if (!ts_params.is_algorithm_successful || ts_params.case_num != TSCase::CASE_8)
469 {
470 RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"),"handleCruisingUntilStop is called but it is not case_8");
471 return;
472 }
473
474 auto new_ts_params = ts_params;
475
476 double decel_rate = std::fabs(ts_params.a3_);
477
478 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CASE_8: Planning cruise and stop with decel_rate: " << decel_rate);
479
480 new_ts_params.t3_ = new_ts_params.t2_;
481 new_ts_params.x3_ = new_ts_params.x2_;
482 new_ts_params.v3_ = new_ts_params.v2_;
483 new_ts_params.a3_ = new_ts_params.a2_;
484
485 // Identify the lanelets which will be crossed by approach maneuvers stopping part
486 std::vector<lanelet::ConstLanelet> lane_follow_crossed_lanelets =
487 getLaneletsBetweenWithException(new_ts_params.x1_, new_ts_params.x2_, true, true);
488
489 resp->new_plan.maneuvers.push_back(composeTrajectorySmoothingManeuverMessage(current_state.downtrack, new_ts_params.x2_, lane_follow_crossed_lanelets,
490 current_state_speed, new_ts_params.v2_, current_state.stamp, rclcpp::Time(new_ts_params.t2_ * 1e9), new_ts_params));
491
492 // Identify the lanelets which will be crossed by approach maneuvers stopping part
493 std::vector<lanelet::ConstLanelet> case_8_crossed_lanelets =
494 getLaneletsBetweenWithException(new_ts_params.x2_, traffic_light_down_track, true, true);
495
496 resp->new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage(
497 new_ts_params.x2_, traffic_light_down_track, new_ts_params.v2_, case_8_crossed_lanelets.front().id(),
498 case_8_crossed_lanelets.back().id(), rclcpp::Time(new_ts_params.t2_ * 1e9),
499 rclcpp::Time(new_ts_params.t2_ * 1e9) + rclcpp::Duration::from_nanoseconds(config_.min_maneuver_planning_period * 1e9), decel_rate));
500
501 return;
502}
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)
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.

References lci_strategic_plugin::TrajectoryParams::a3_, lci_strategic_plugin::CASE_8, lci_strategic_plugin::TrajectoryParams::case_num, composeStopAndWaitManeuverMessage(), composeTrajectorySmoothingManeuverMessage(), config_, lci_strategic_plugin::LCIStrategicPlugin::VehicleState::downtrack, getLaneletsBetweenWithException(), lci_strategic_plugin::TrajectoryParams::is_algorithm_successful, lci_strategic_plugin::LCIStrategicPluginConfig::min_maneuver_planning_period, and lci_strategic_plugin::LCIStrategicPlugin::VehicleState::stamp.

Referenced by planWhenAPPROACHING().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ handleFailureCase()

void lci_strategic_plugin::LCIStrategicPlugin::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 
)
private

When the vehicle is not able to successfully run the algorithm or not able to stop, this function modifies departure speed as close as possible to the original using max comfortable accelerations with given distance.

Parameters
reqPlan maneuver request
[out]respPlan maneuver response with a list of maneuver plan
current_stateCurrent state of the vehicle
current_state_speedThe current speed to use when planning
speed_limitSpeed limit to cap the speed
remaining_timeRemaining time until scheduled entry
exit_lanelet_idId of the exit lanelet
traffic_lightCarmaTrafficSignalPtr of the relevant signal
traffic_light_down_trackDowntrack to the given traffic_light
ts_paramstrajectory smoothing params
Returns
void. if successful, resp is non-empty. Resp is empty if not able to make it with certainty or ts_params.is_successful is false

Definition at line 422 of file lci_strategic_plugin.cpp.

431{
432 double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.downtrack;
433
434 // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver
435 std::vector<lanelet::ConstLanelet> crossed_lanelets =
436 getLaneletsBetweenWithException(current_state.downtrack, traffic_light_down_track, true, true);
437
438 auto incomplete_traj_params = handleFailureCaseHelper(traffic_light, current_state.stamp.seconds(), current_state_speed, intersection_speed_.get(), speed_limit, distance_remaining_to_traffic_light, traffic_light_down_track);
439
440 if (incomplete_traj_params.is_algorithm_successful == false)
441 {
442 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Failed to generate maneuver for edge cases...");
443 return;
444 }
445
446 resp->new_plan.maneuvers.push_back(composeTrajectorySmoothingManeuverMessage(current_state.downtrack, traffic_light_down_track, crossed_lanelets,
447 current_state_speed, incomplete_traj_params.modified_departure_speed, current_state.stamp, current_state.stamp + rclcpp::Duration::from_nanoseconds(incomplete_traj_params.modified_remaining_time * 1e9), incomplete_traj_params));
448
449 double intersection_length = intersection_end_downtrack_.get() - traffic_light_down_track;
450
451 rclcpp::Time intersection_exit_time =
452 current_state.stamp + rclcpp::Duration::from_nanoseconds(incomplete_traj_params.modified_remaining_time * 1e9) + rclcpp::Duration::from_nanoseconds(intersection_length / incomplete_traj_params.modified_departure_speed * 1e9);
453
454 resp->new_plan.maneuvers.push_back(composeIntersectionTransitMessage(
455 traffic_light_down_track, intersection_end_downtrack_.get(), intersection_speed_.get(),
456 incomplete_traj_params.modified_departure_speed, current_state.stamp + rclcpp::Duration::from_nanoseconds(incomplete_traj_params.modified_remaining_time * 1e9), intersection_exit_time, crossed_lanelets.back().id(), crossed_lanelets.back().id()));
457
459}
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....
boost::optional< double > intersection_end_downtrack_
TSCase last_case_num_
Useful metrics for LCI Plugin.
boost::optional< double > intersection_speed_
Cache variables for storing the current intersection state between state machine transitions.
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.

References composeIntersectionTransitMessage(), composeTrajectorySmoothingManeuverMessage(), lci_strategic_plugin::DEGRADED_TSCASE, lci_strategic_plugin::LCIStrategicPlugin::VehicleState::downtrack, getLaneletsBetweenWithException(), handleFailureCaseHelper(), intersection_end_downtrack_, intersection_speed_, last_case_num_, and lci_strategic_plugin::LCIStrategicPlugin::VehicleState::stamp.

Referenced by planWhenAPPROACHING().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ handleFailureCaseHelper()

TrajectoryParams lci_strategic_plugin::LCIStrategicPlugin::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 
)
private

Helper function to handleFailureCase that modifies ts_params to desired new parameters. See handleFailureCase.

Parameters
starting_speedstarting speed
departure_speeddeparture_speed originally planned
speed_limitspeed_limit
remaining_downtrackremaining_downtrack until the intersection
traffic_light_downtracktraffic_light_downtrack when vehicle is scheduled to enter
Returns
TSP with parameters that is best available to pass the intersection. Either cruising with starting_speed or sacrifice departure speed to meet time and distance

Definition at line 549 of file lci_strategic_plugin.cpp.

550{
551 //Requested maneuver needs to be modified to meet remaining_dist req
552 //by trying to get close to the target_speed and remaining_time as much as possible
553 TrajectoryParams return_params;
554 TrajectoryParams traj_upper;
555 TrajectoryParams traj_lower;
556
557 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "HANDLE_LAST_RESORT_CASE: Starting...");
558 double starting_downtrack = traffic_light_downtrack - remaining_downtrack;
559 double modified_remaining_time_upper; // upper meaning downtrack vs time trajectory is curved upwards
560 double modified_remaining_time_lower; // lower meaning downtrack vs time trajectory is curved lower
561 double modified_departure_speed_upper;
562 double modified_departure_speed_lower;
563 bool calculation_success_upper = true; // identifies places in codes where calculation can be invalid such as negative distance
564
565 // the upper ET
566 // accel case
567 traj_upper.t0_ = current_time;
568 traj_upper.v0_ = starting_speed;
569 traj_upper.x0_ = starting_downtrack;
570 traj_upper.is_algorithm_successful = true;
571 traj_upper.case_num = CASE_1;
572
573 if (departure_speed >= starting_speed)
574 {
575 if ((pow(departure_speed,2) - pow(starting_speed,2))/(2*max_comfort_accel_) >= remaining_downtrack)
576 {
577 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "HandleFailureCase -> Upper Trajectory -> Current Speed <= Desired Departure Speed, Actual Departure Speed < Desired Departure Speed");
578
579 modified_departure_speed_upper = sqrt(pow(starting_speed, 2) + (2 * max_comfort_accel_ * remaining_downtrack));
580 modified_remaining_time_upper = (modified_departure_speed_upper - starting_speed) / max_comfort_accel_;
581
582 traj_upper.t1_ = current_time + modified_remaining_time_upper;
583 traj_upper.v1_ = modified_departure_speed_upper;
584 traj_upper.a1_ = max_comfort_accel_;
585 traj_upper.x1_ = traffic_light_downtrack;
586
587 traj_upper.t2_ = traj_upper.t1_;
588 traj_upper.v2_ = traj_upper.v1_;
589 traj_upper.a2_ = traj_upper.a1_;
590 traj_upper.x2_ = traj_upper.x1_;
591
592 traj_upper.modified_departure_speed = modified_departure_speed_upper;
593 traj_upper.modified_remaining_time = modified_remaining_time_upper;
594 }
595 else // NOTE: most likely will not happen as it would have happened at trajectory smoothing part
596 {
597 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "HandleFailureCase -> Upper Trajectory -> Current Speed < Desired Departure Speed, Actual Departure Speed = Desired Departure Speed");
598
599 double cruising_distance = remaining_downtrack - (pow(departure_speed, 2) - pow(starting_speed, 2))/ ( 2 * max_comfort_accel_);
600 if (cruising_distance < -EPSILON)
601 {
602 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Detected calculation failure in upper case 2");
603 calculation_success_upper = false;
604 }
605 modified_remaining_time_upper = ((departure_speed - starting_speed) / max_comfort_accel_) + (cruising_distance / departure_speed);
606
607 traj_upper.t1_ = current_time + ((departure_speed - starting_speed) / max_comfort_accel_);
608 traj_upper.v1_ = departure_speed;
609 traj_upper.a1_ = max_comfort_accel_;
610 traj_upper.x1_ = starting_downtrack + (pow(departure_speed, 2) - pow(starting_speed, 2)) / (2 * max_comfort_accel_);
611
612 traj_upper.t2_ = current_time + modified_remaining_time_upper;
613 traj_upper.v2_ = departure_speed;
614 traj_upper.a2_ = 0;
615 traj_upper.x2_ = traffic_light_downtrack;
616
617 traj_upper.modified_departure_speed = departure_speed;
618 traj_upper.modified_remaining_time = modified_remaining_time_upper;
619 }
620 }
621
622 if (departure_speed < starting_speed) // NOTE: most cases will not run due to departure speed being equal to speed limit
623 {
624 if ((pow(departure_speed,2) - pow(starting_speed,2))/(2*max_comfort_decel_) >= remaining_downtrack)
625 {
626 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "HandleFailureCase -> Upper Trajectory -> Current Speed > Desired Departure Speed, Actual Departure Speed > Desired Departure Speed");
627
628 modified_departure_speed_upper = sqrt(pow(starting_speed, 2) + (2 * max_comfort_decel_ * remaining_downtrack));
629 modified_remaining_time_upper = (modified_departure_speed_upper - starting_speed) / max_comfort_decel_;
630
631 traj_upper.t1_ = current_time + modified_remaining_time_upper;
632 traj_upper.v1_ = modified_departure_speed_upper;
633 traj_upper.a1_ = max_comfort_decel_;
634 traj_upper.x1_ = traffic_light_downtrack;
635
636 traj_upper.t2_ = traj_upper.t1_;
637 traj_upper.v2_ = traj_upper.v1_;
638 traj_upper.a2_ = traj_upper.a1_;
639 traj_upper.x2_ = traj_upper.x1_;
640
641 traj_upper.modified_departure_speed = modified_departure_speed_upper;
642 traj_upper.modified_remaining_time = modified_remaining_time_upper;
643 }
644 else // NOTE: most likely will not happen as it would have happened at trajectory smoothing part
645 {
646 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "HandleFailureCase -> Upper Trajectory -> Current Speed > Desired Departure Speed, Actual Departure Speed = Desired Departure Speed");
647
648 double cruising_distance = remaining_downtrack - (pow(departure_speed, 2) - pow(starting_speed, 2))/ ( 2 * max_comfort_decel_);
649
650 if (cruising_distance < -EPSILON)
651 {
652 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Detected calculation failure in upper case 4");
653 calculation_success_upper = false;
654 }
655
656 modified_remaining_time_upper = cruising_distance / starting_speed + (departure_speed - starting_speed) / max_comfort_decel_ ;
657
658 traj_upper.t1_ = current_time + cruising_distance / starting_speed;
659 traj_upper.v1_ = starting_speed;
660 traj_upper.a1_ = 0.0;
661 traj_upper.x1_ = starting_downtrack + cruising_distance;
662
663 traj_upper.t2_ = current_time + modified_remaining_time_upper;
664 traj_upper.v2_ = departure_speed;
665 traj_upper.a2_ = max_comfort_decel_;
666 traj_upper.x2_ = traffic_light_downtrack;
667
668 traj_upper.modified_departure_speed = departure_speed;
669 traj_upper.modified_remaining_time = modified_remaining_time_upper;
670 }
671 }
672
673 traj_upper.t3_ = traj_upper.t2_;
674 traj_upper.v3_ = traj_upper.v2_;
675 traj_upper.a3_ = traj_upper.a2_;
676 traj_upper.x3_ = traj_upper.x2_;
677
678 // The lower ET
679 // (note that the distance is definitely not enough for deceleration to zero speed, therefore, modified_departure_speed will be greater than zero for sure!).
680 modified_departure_speed_lower = sqrt(pow(starting_speed, 2) + (2 * max_comfort_decel_ * remaining_downtrack));
681 modified_remaining_time_lower = (modified_departure_speed_lower - starting_speed) / max_comfort_decel_;
682
683 traj_lower.t0_ = current_time;
684 traj_lower.v0_ = starting_speed;
685 traj_lower.x0_ = starting_downtrack;
686 traj_lower.is_algorithm_successful = true;
687 traj_lower.case_num = CASE_1;
688
689 traj_lower.t1_ = current_time + modified_remaining_time_lower;
690 traj_lower.v1_ = modified_departure_speed_lower;
691 traj_lower.a1_ = max_comfort_decel_;
692 traj_lower.x1_ = traffic_light_downtrack;
693
694 traj_lower.t2_ = traj_lower.t1_;
695 traj_lower.v2_ = traj_lower.v1_;
696 traj_lower.a2_ = traj_lower.a1_;
697 traj_lower.x2_ = traj_lower.x1_;
698
699 traj_lower.t3_ = traj_lower.t2_;
700 traj_lower.v3_ = traj_lower.v2_;
701 traj_lower.a3_ = traj_lower.a2_;
702 traj_lower.x3_ = traj_lower.x2_;
703
704 traj_lower.modified_departure_speed = modified_departure_speed_lower;
705 traj_lower.modified_remaining_time = modified_remaining_time_upper;
706
707 // Pick UPPER or LOWER trajectory based on light
708 bool is_return_params_found = false;
709
710 if (calculation_success_upper)
711 {
712 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Checking this time!: " << current_time + modified_remaining_time_upper);
713
714 auto upper_optional = traffic_light->predictState(lanelet::time::timeFromSec(current_time + modified_remaining_time_upper));
715
716 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Checking this time! state: " << upper_optional.get().second);
717
718 if (!validLightState(upper_optional, rclcpp::Time((current_time + modified_remaining_time_upper) * 1e9)))
719 {
720 RCLCPP_ERROR_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Unable to resolve give signal for modified_remaining_time_upper: " << std::to_string(current_time + modified_remaining_time_upper));
721 }
722 else
723 {
724 if (isStateAllowedGreen(upper_optional.get().second))
725 {
726 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Detected Upper GREEN case");
727 return_params = traj_upper;
728 is_return_params_found = true;
729 }
730 }
731 }
732
733 if (!is_return_params_found)
734 {
735 auto lower_optional = traffic_light->predictState(lanelet::time::timeFromSec(current_time + modified_remaining_time_lower));
736
737 if (!validLightState(lower_optional, rclcpp::Time((current_time + modified_remaining_time_lower) * 1e9)))
738 {
739 RCLCPP_ERROR_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Unable to resolve give signal for modified_remaining_time_lower:" << std::to_string(current_time + modified_remaining_time_lower));
740 }
741 else
742 {
743 if (isStateAllowedGreen(lower_optional.get().second))
744 {
745 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Detected Lower GREEN case");
746 return_params = traj_lower;
747 is_return_params_found = true;
748 }
749 }
750 }
751
752 // Handle hard failure case such as nan or invalid states
753 if (is_return_params_found && !isnan(return_params.modified_departure_speed) && return_params.modified_departure_speed > epsilon_ &&
754 return_params.modified_departure_speed < speed_limit ) //80_mph
755 {
756 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Updated the speed, and using modified_departure_speed: " << return_params.modified_departure_speed);
757 print_params(return_params);
758 return return_params;
759 }
760 else
761 {
762 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Unable to handle edge case gracefully");
763 return_params = TrajectoryParams(); //reset
764 return_params.is_algorithm_successful = false;
765 return return_params;
766 }
767}
void print_params(TrajectoryParams params)
Helper method to print TrajectoryParams.
#define EPSILON

References lci_strategic_plugin::TrajectoryParams::a1_, lci_strategic_plugin::TrajectoryParams::a2_, lci_strategic_plugin::TrajectoryParams::a3_, lci_strategic_plugin::CASE_1, lci_strategic_plugin::TrajectoryParams::case_num, EPSILON, epsilon_, lci_strategic_plugin::TrajectoryParams::is_algorithm_successful, isStateAllowedGreen(), max_comfort_accel_, max_comfort_decel_, lci_strategic_plugin::TrajectoryParams::modified_departure_speed, lci_strategic_plugin::TrajectoryParams::modified_remaining_time, print_params(), lci_strategic_plugin::TrajectoryParams::t0_, lci_strategic_plugin::TrajectoryParams::t1_, lci_strategic_plugin::TrajectoryParams::t2_, lci_strategic_plugin::TrajectoryParams::t3_, carma_cooperative_perception::to_string(), lci_strategic_plugin::TrajectoryParams::v0_, lci_strategic_plugin::TrajectoryParams::v1_, lci_strategic_plugin::TrajectoryParams::v2_, lci_strategic_plugin::TrajectoryParams::v3_, validLightState(), lci_strategic_plugin::TrajectoryParams::x0_, lci_strategic_plugin::TrajectoryParams::x1_, lci_strategic_plugin::TrajectoryParams::x2_, and lci_strategic_plugin::TrajectoryParams::x3_.

Referenced by handleFailureCase().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ handleGreenSignalScenario()

void lci_strategic_plugin::LCIStrategicPlugin::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 
)
private

This function returns valid maneuvers if the vehicle is able to utilize trajectory smoothing parameters to go through the intersection with certainty It utilizes canMakeWithCertainty function to determine if it is able to make it. Corresponds to TSCase 1-7.

Parameters
reqPlan maneuver request
[out]respPlan maneuver response with a list of maneuver plan
current_stateCurrent state of the vehicle
current_state_speedThe current speed to use when planning
traffic_lightCarmaTrafficSignalPtr of the relevant signal
entry_laneletEntry lanelet in to the intersection along route
exit_laneletExit lanelet in to the intersection along route
traffic_light_down_trackDowntrack to the given traffic_light
ts_paramstrajectory smoothing params
Returns
void. if successful, resp is non-empty. Resp is empty if not able to make it with certainty or ts_params.is_successful is false

Definition at line 504 of file lci_strategic_plugin.cpp.

511{
512 if (!ts_params.is_algorithm_successful || ts_params.case_num == TSCase::CASE_8)
513 {
514 return;
515 }
516
517 rclcpp::Time light_arrival_time_by_algo = rclcpp::Time(ts_params.t3_ * 1e9);
518 double remaining_time = light_arrival_time_by_algo.seconds() - rclcpp::Time(req->header.stamp, RCL_SYSTEM_TIME).seconds();
519 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Algo initially successful: New light_arrival_time_by_algo: " << std::to_string(light_arrival_time_by_algo.seconds()) << ", with remaining_time: " << std::to_string(remaining_time));
520 auto can_make_green_optional = canArriveAtGreenWithCertainty(light_arrival_time_by_algo, traffic_light);
521
522 // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver
523 std::vector<lanelet::ConstLanelet> crossed_lanelets =
524 getLaneletsBetweenWithException(current_state.downtrack, traffic_light_down_track, true, true);
525
526 // no change for maneuver if invalid light states
527 if (!can_make_green_optional)
528 return;
529
530 if (can_make_green_optional.get() || is_certainty_check_optional)
531 {
532 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "HANDLE_SUCCESSFULL: Algorithm successful, and able to make it at green with certainty. Planning traj smooth and intersection transit maneuvers");
533
534 resp->new_plan.maneuvers.push_back(composeTrajectorySmoothingManeuverMessage(current_state.downtrack, traffic_light_down_track, crossed_lanelets,
535 current_state_speed, ts_params.v3_, current_state.stamp, light_arrival_time_by_algo, ts_params));
536
537 double intersection_length = intersection_end_downtrack_.get() - traffic_light_down_track;
538
539 rclcpp::Time intersection_exit_time =
540 light_arrival_time_by_algo + rclcpp::Duration::from_nanoseconds(intersection_length / intersection_speed_.get() * 1e9);
541
542 resp->new_plan.maneuvers.push_back(composeIntersectionTransitMessage(
543 traffic_light_down_track, intersection_end_downtrack_.get(), intersection_speed_.get(),
544 intersection_speed_.get(), light_arrival_time_by_algo, intersection_exit_time, crossed_lanelets.back().id(), crossed_lanelets.back().id()));
545 }
546}
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.

References canArriveAtGreenWithCertainty(), lci_strategic_plugin::CASE_8, lci_strategic_plugin::TrajectoryParams::case_num, composeIntersectionTransitMessage(), composeTrajectorySmoothingManeuverMessage(), lci_strategic_plugin::LCIStrategicPlugin::VehicleState::downtrack, getLaneletsBetweenWithException(), intersection_end_downtrack_, intersection_speed_, lci_strategic_plugin::TrajectoryParams::is_algorithm_successful, lci_strategic_plugin::LCIStrategicPlugin::VehicleState::stamp, lci_strategic_plugin::TrajectoryParams::t3_, carma_cooperative_perception::to_string(), and lci_strategic_plugin::TrajectoryParams::v3_.

Referenced by planWhenAPPROACHING().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ handleStopping()

void lci_strategic_plugin::LCIStrategicPlugin::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 
)
private

This function returns stopping maneuver if the vehicle is able to stop at red and in safe stopping distance. Given the initial speed, ending speed (which is 0), and remaining_downtrack, this function checks if calculated remaining_time falls on RED.

Parameters
reqPlan maneuver request
[out]respPlan maneuver response with a list of maneuver plan
current_stateCurrent state of the vehicle
traffic_lightCarmaTrafficSignalPtr of the relevant signal
entry_laneletEntry lanelet in to the intersection along route
exit_laneletExit lanelet in to the intersection along route
current_laneletCurrent lanelet
traffic_light_down_trackDowntrack to the given traffic_light
is_emergencyif enabled, double the deceleration rate of max_comfort_accel is used
Returns
void. if successful, resp is non-empty

Definition at line 387 of file lci_strategic_plugin.cpp.

394{
395 double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.downtrack;
396
397 // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver
398 std::vector<lanelet::ConstLanelet> crossed_lanelets =
399 getLaneletsBetweenWithException(current_state.downtrack, traffic_light_down_track, true, true);
400
401 double decel_rate = max_comfort_decel_norm_; // Kinematic |(v_f - v_i) / t = a|
402
403 if (is_emergency)
404 {
405 decel_rate = emergency_decel_norm_;
407 }
408 else
409 {
411 }
412
413 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "HANDLE_STOPPING: Planning stop and wait maneuver at decel_rate: " << decel_rate);
414
415 resp->new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage(
416 current_state.downtrack, traffic_light_down_track, current_state.speed, crossed_lanelets.front().id(),
417 crossed_lanelets.back().id(), current_state.stamp,
418 current_state.stamp + rclcpp::Duration::from_nanoseconds(config_.min_maneuver_planning_period * 1e9), decel_rate));
419}

References composeStopAndWaitManeuverMessage(), config_, lci_strategic_plugin::LCIStrategicPlugin::VehicleState::downtrack, emergency_decel_norm_, lci_strategic_plugin::EMERGENCY_STOPPING, getLaneletsBetweenWithException(), last_case_num_, max_comfort_decel_norm_, lci_strategic_plugin::LCIStrategicPluginConfig::min_maneuver_planning_period, lci_strategic_plugin::LCIStrategicPlugin::VehicleState::speed, lci_strategic_plugin::LCIStrategicPlugin::VehicleState::stamp, and lci_strategic_plugin::STOPPING.

Referenced by planWhenAPPROACHING().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ isStateAllowedGreen()

bool lci_strategic_plugin::LCIStrategicPlugin::isStateAllowedGreen ( const lanelet::CarmaTrafficSignalState &  state) const
private

Method to check if the state is an allowed green movement state. Currently Permissive and Protected green are supported.

Parameters
stateThe traffic signal state to check for
Returns
bool value - True if state is PERMISSIVE_MOVEMENT_ALLOWED or PROTECTED_MOVEMENT_ALLOWED

Definition at line 364 of file lci_strategic_plugin.cpp.

364 {
365 return state == lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED ||
366 state == lanelet::CarmaTrafficSignalState::PERMISSIVE_MOVEMENT_ALLOWED;
367}

Referenced by canArriveAtGreenWithCertainty(), get_final_entry_time_and_conditions(), get_nearest_green_entry_time(), handleFailureCaseHelper(), and planWhenWAITING().

Here is the caller graph for this function:

◆ lookupFrontBumperTransform()

void lci_strategic_plugin::LCIStrategicPlugin::lookupFrontBumperTransform ( )

Lookup transfrom from front bumper to base link.

Definition at line 242 of file lci_strategic_plugin.cpp.

243{
244 tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);
245 tf2_buffer_->setUsingDedicatedThread(true);
246 try
247 {
248 geometry_msgs::msg::TransformStamped tf2 = tf2_buffer_->lookupTransform("base_link", "vehicle_front", rclcpp::Time(0), rclcpp::Duration::from_nanoseconds(20.0 * 1e9)); //save to local copy of transform 20 sec timeout
249 length_to_front_bumper_ = tf2.transform.translation.x;
250 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "length_to_front_bumper_: " << length_to_front_bumper_);
251
252 }
253 catch (const tf2::TransformException &ex)
254 {
255 RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"), ex.what());
256 }
257}
std::shared_ptr< tf2_ros::TransformListener > tf2_listener_

References length_to_front_bumper_, tf2_buffer_, and tf2_listener_.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

◆ mobilityOperationCb()

void lci_strategic_plugin::LCIStrategicPlugin::mobilityOperationCb ( carma_v2x_msgs::msg::MobilityOperation::UniquePtr  msg)

callback function for mobility operation

Parameters
msginput mobility operation msg

Definition at line 1178 of file lci_strategic_plugin.cpp.

1179{
1180 if (msg->strategy == light_controlled_intersection_strategy_)
1181 {
1182 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Received Schedule message with id: " << msg->m_header.plan_id);
1184 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Approaching light Controlled Intersection: " << approaching_light_controlled_intersection_);
1185
1186 if (msg->m_header.recipient_id == config_.vehicle_id)
1187 {
1188 street_msg_timestamp_ = msg->m_header.timestamp;
1189 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "street_msg_timestamp_: " << street_msg_timestamp_);
1190 parseStrategyParams(msg->strategy_params);
1191 previous_strategy_params_ = msg->strategy_params;
1192 }
1193 }
1194
1195}
void parseStrategyParams(const std::string &strategy_params)
parse strategy parameters

References approaching_light_controlled_intersection_, config_, light_controlled_intersection_strategy_, parseStrategyParams(), previous_strategy_params_, street_msg_timestamp_, and lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_id.

Referenced by on_configure_plugin().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ on_activate_plugin()

carma_ros2_utils::CallbackReturn lci_strategic_plugin::LCIStrategicPlugin::on_activate_plugin ( )
virtual

Method which is triggered when this plugin is moved from the INACTIVE to ACTIVE states. This method should be used to prepare for future callbacks for plugin's capabilites.

Returns
SUCCESS, FAILURE, or ERROR. Transition to ACTIVE will only complete on SUCCESS.

Reimplemented from carma_guidance_plugins::PluginBaseNode.

Definition at line 201 of file lci_strategic_plugin.cpp.

202{
203 mob_op_pub_timer_ = create_timer(get_clock(),
204 std::chrono::duration<double>(1/config_.mobility_rate),
206
207 ts_info_pub_timer_ = create_timer(get_clock(),
208 std::chrono::duration<double>(0.5),
210
211 return CallbackReturn::SUCCESS;
212}
void publishTrajectorySmoothingInfo()
Publish trajectory smoothing info at a fixed rate.
void publishMobilityOperation()
Publish mobility operation at a fixed rate.
rclcpp::TimerBase::SharedPtr ts_info_pub_timer_
rclcpp::TimerBase::SharedPtr mob_op_pub_timer_

References config_, mob_op_pub_timer_, lci_strategic_plugin::LCIStrategicPluginConfig::mobility_rate, publishMobilityOperation(), publishTrajectorySmoothingInfo(), and ts_info_pub_timer_.

Here is the call graph for this function:

◆ on_configure_plugin()

carma_ros2_utils::CallbackReturn lci_strategic_plugin::LCIStrategicPlugin::on_configure_plugin ( )
virtual

Method which is triggered when this plugin is moved from the UNCONFIGURED to INACTIVE states. This method should be used to load parameters and is required to be implemented.

Returns
SUCCESS, FAILURE, or ERROR. Transition to INACTIVE will only complete on SUCCESS.

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 77 of file lci_strategic_plugin.cpp.

78{
79 // reset config
80 config_ = LCIStrategicPluginConfig();
81
82 // clang-format off
83 get_parameter<double>("vehicle_acceleration_limit", config_.vehicle_accel_limit);
84 get_parameter<double>("vehicle_deceleration_limit", config_.vehicle_decel_limit);
85 get_parameter<double>("vehicle_decel_limit_multiplier", config_.vehicle_decel_limit_multiplier);
86 get_parameter<double>("vehicle_accel_limit_multiplier", config_.vehicle_accel_limit_multiplier);
87 get_parameter<double>("min_approach_distance", config_.min_approach_distance);
88 get_parameter<double>("trajectory_smoothing_activation_distance", config_.trajectory_smoothing_activation_distance);
89 get_parameter<double>("stopping_location_buffer", config_.stopping_location_buffer);
90 get_parameter<double>("green_light_time_buffer", config_.green_light_time_buffer);
91 get_parameter<double>("vehicle_response_lag", config_.vehicle_response_lag);
92 get_parameter<double>("algo_minimum_speed", config_.algo_minimum_speed);
93 get_parameter<double>("deceleration_fraction", config_.deceleration_fraction);
94 get_parameter<double>("desired_distance_to_stop_buffer", config_.desired_distance_to_stop_buffer);
95 get_parameter<double>("min_maneuver_planning_period", config_.min_maneuver_planning_period);
96 get_parameter<std::string>("strategic_plugin_name", config_.strategic_plugin_name);
97 get_parameter<std::string>("lane_following_plugin_name", config_.lane_following_plugin_name);
98 get_parameter<std::string>("stop_and_wait_plugin_name", config_.stop_and_wait_plugin_name);
99 get_parameter<std::string>("intersection_transit_plugin_name", config_.intersection_transit_plugin_name);
100 get_parameter<bool>("enable_carma_streets_connection", config_.enable_carma_streets_connection);
101 get_parameter<int>("enable_carma_wm_spat_processing", config_.enable_carma_wm_spat_processing);
102 get_parameter<double>("mobility_rate", config_.mobility_rate);
103 get_parameter<std::string>("vehicle_id", config_.vehicle_id);
104
111 RCLCPP_INFO_STREAM(rclcpp::get_logger("lci_strategic_plugin"),
112 "LCI Strategic Plugin updated green_light_time_buffer based on vehicle_response_lag to "
114
115 // clang-format on
116
117 // Register runtime parameter update callback
118 add_on_set_parameters_callback(std::bind(&LCIStrategicPlugin::parameter_update_callback, this, std_ph::_1));
119
120 RCLCPP_INFO_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Done loading parameters: " << config_);
121
123
124 // Mobility Operation Subscriber
125 mob_operation_sub_ = create_subscription<carma_v2x_msgs::msg::MobilityOperation>("incoming_mobility_operation", 1,
126 std::bind(&LCIStrategicPlugin::mobilityOperationCb,this,std_ph::_1));
127
128 // BSM subscriber
129 bsm_sub_ = create_subscription<carma_v2x_msgs::msg::BSM>("bsm_outbound", 1,
130 std::bind(&LCIStrategicPlugin::BSMCb,this,std_ph::_1));
131
132 // set world model point from wm listener
134
135 // Activate SPAT processor, which is turned off by default,
136 // with OFF (0), ON (1)
137 get_world_model_listener()->setWMSpatProcessingState(
139
140 // Setup publishers
141 mobility_operation_pub_ = create_publisher<carma_v2x_msgs::msg::MobilityOperation>("outgoing_mobility_operation", 1);
142 case_pub_ = create_publisher<std_msgs::msg::Int8>("lci_strategic_plugin/ts_case_num", 1);
143 tf_distance_pub_ = create_publisher<std_msgs::msg::Float64>("lci_strategic_plugin/distance_remaining_to_tf", 1);
144 earliest_et_pub_ = create_publisher<std_msgs::msg::Float64>("lci_strategic_plugin/earliest_entry_time", 1);
145 et_pub_ = create_publisher<std_msgs::msg::Float64>("lci_strategic_plugin/scheduled_entry_time", 1);
146
147 // Return success if everything initialized successfully
148 return CallbackReturn::SUCCESS;
149}
virtual carma_wm::WorldModelConstPtr get_world_model() final
Method to return the default world model provided as a convience by this base class If this method or...
virtual std::shared_ptr< carma_wm::WMListener > get_world_model_listener() final
Method to return the default world model listener provided as a convience by this base class If this ...
void lookupFrontBumperTransform()
Lookup transfrom from front bumper to base link.
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_
carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::MobilityOperation > mob_operation_sub_
carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::BSM > bsm_sub_
carma_ros2_utils::PubPtr< std_msgs::msg::Int8 > case_pub_
void BSMCb(carma_v2x_msgs::msg::BSM::UniquePtr msg)
BSM callback function.
carma_ros2_utils::PubPtr< std_msgs::msg::Float64 > earliest_et_pub_
carma_ros2_utils::PubPtr< carma_v2x_msgs::msg::MobilityOperation > mobility_operation_pub_
rcl_interfaces::msg::SetParametersResult parameter_update_callback(const std::vector< rclcpp::Parameter > &parameters)
Callback for dynamic parameter updates.
carma_ros2_utils::PubPtr< std_msgs::msg::Float64 > et_pub_

References lci_strategic_plugin::LCIStrategicPluginConfig::algo_minimum_speed, bsm_sub_, BSMCb(), case_pub_, config_, lci_strategic_plugin::LCIStrategicPluginConfig::deceleration_fraction, lci_strategic_plugin::LCIStrategicPluginConfig::desired_distance_to_stop_buffer, earliest_et_pub_, emergency_decel_norm_, lci_strategic_plugin::LCIStrategicPluginConfig::enable_carma_streets_connection, lci_strategic_plugin::LCIStrategicPluginConfig::enable_carma_wm_spat_processing, et_pub_, carma_guidance_plugins::PluginBaseNode::get_world_model(), carma_guidance_plugins::PluginBaseNode::get_world_model_listener(), lci_strategic_plugin::LCIStrategicPluginConfig::green_light_time_buffer, lci_strategic_plugin::LCIStrategicPluginConfig::intersection_transit_plugin_name, lci_strategic_plugin::LCIStrategicPluginConfig::lane_following_plugin_name, lookupFrontBumperTransform(), max_comfort_accel_, max_comfort_decel_, max_comfort_decel_norm_, lci_strategic_plugin::LCIStrategicPluginConfig::min_approach_distance, lci_strategic_plugin::LCIStrategicPluginConfig::min_maneuver_planning_period, mob_operation_sub_, mobility_operation_pub_, lci_strategic_plugin::LCIStrategicPluginConfig::mobility_rate, mobilityOperationCb(), parameter_update_callback(), lci_strategic_plugin::LCIStrategicPluginConfig::stop_and_wait_plugin_name, lci_strategic_plugin::LCIStrategicPluginConfig::stopping_location_buffer, lci_strategic_plugin::LCIStrategicPluginConfig::strategic_plugin_name, tf_distance_pub_, lci_strategic_plugin::LCIStrategicPluginConfig::trajectory_smoothing_activation_distance, lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_accel_limit, lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_accel_limit_multiplier, lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_decel_limit, lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_decel_limit_multiplier, lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_id, lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_response_lag, and wm_.

Here is the call graph for this function:

◆ parameter_update_callback()

rcl_interfaces::msg::SetParametersResult lci_strategic_plugin::LCIStrategicPlugin::parameter_update_callback ( const std::vector< rclcpp::Parameter > &  parameters)

Callback for dynamic parameter updates.

Definition at line 151 of file lci_strategic_plugin.cpp.

152{
153 auto error_double = update_params<double>({
154 {"vehicle_acceleration_limit", config_.vehicle_accel_limit},
155 {"vehicle_deceleration_limit", config_.vehicle_decel_limit},
156 {"vehicle_decel_limit_multiplier", config_.vehicle_decel_limit_multiplier},
157 {"vehicle_accel_limit_multiplier", config_.vehicle_accel_limit_multiplier},
158 {"min_approach_distance", config_.min_approach_distance},
159 {"trajectory_smoothing_activation_distance", config_.trajectory_smoothing_activation_distance},
160 {"stopping_location_buffer", config_.stopping_location_buffer},
161 {"green_light_time_buffer", config_.green_light_time_buffer},
162 {"vehicle_response_lag", config_.vehicle_response_lag},
163 {"algo_minimum_speed", config_.algo_minimum_speed},
164 {"deceleration_fraction", config_.deceleration_fraction},
165 {"desired_distance_to_stop_buffer", config_.desired_distance_to_stop_buffer},
166 {"min_maneuver_planning_period", config_.min_maneuver_planning_period},
167 {"mobility_rate", config_.mobility_rate},
168 }, parameters);
169
172
173 RCLCPP_INFO_STREAM(rclcpp::get_logger("lci_strategic_plugin"),
174 "LCI Strategic Plugin updated green_light_time_buffer based on vehicle_response_lag to "
176 rcl_interfaces::msg::SetParametersResult result;
177
178 result.successful = !error_double;
179
180 return result;
181}

References lci_strategic_plugin::LCIStrategicPluginConfig::algo_minimum_speed, config_, lci_strategic_plugin::LCIStrategicPluginConfig::deceleration_fraction, lci_strategic_plugin::LCIStrategicPluginConfig::desired_distance_to_stop_buffer, lci_strategic_plugin::LCIStrategicPluginConfig::green_light_time_buffer, lci_strategic_plugin::LCIStrategicPluginConfig::min_approach_distance, lci_strategic_plugin::LCIStrategicPluginConfig::min_maneuver_planning_period, lci_strategic_plugin::LCIStrategicPluginConfig::mobility_rate, lci_strategic_plugin::LCIStrategicPluginConfig::stopping_location_buffer, lci_strategic_plugin::LCIStrategicPluginConfig::trajectory_smoothing_activation_distance, lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_accel_limit, lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_accel_limit_multiplier, lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_decel_limit, lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_decel_limit_multiplier, and lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_response_lag.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

◆ parseStrategyParams()

void lci_strategic_plugin::LCIStrategicPlugin::parseStrategyParams ( const std::string &  strategy_params)

parse strategy parameters

Parameters
strategy_paramsinput string of strategy params from mob op msg

Definition at line 1205 of file lci_strategic_plugin.cpp.

1206{
1207 // sample strategy_params: "et:1634067059"
1208 std::string params = strategy_params;
1209 std::vector<std::string> inputsParams;
1210 boost::algorithm::split(inputsParams, params, boost::is_any_of(","));
1211
1212 std::vector<std::string> et_parsed;
1213 boost::algorithm::split(et_parsed, inputsParams[0], boost::is_any_of(":"));
1214 auto new_scheduled_enter_time = std::stoull(et_parsed[1]);
1215
1216 if (scheduled_enter_time_ != new_scheduled_enter_time) //reset green buffer cache so it can be re-evaluated
1218
1219 scheduled_enter_time_ = new_scheduled_enter_time;
1220 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "scheduled_enter_time_: " << scheduled_enter_time_);
1221
1222}

References nearest_green_entry_time_cached_, scheduled_enter_time_, and process_traj_logs::split.

Referenced by mobilityOperationCb().

Here is the caller graph for this function:

◆ plan_maneuvers_callback()

void lci_strategic_plugin::LCIStrategicPlugin::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 
)
virtual

Service callback for arbitrator maneuver planning.

Parameters
reqPlan maneuver request
respPlan maneuver response with a list of maneuver plan
Returns
If service call successed

Implements carma_guidance_plugins::StrategicPlugin.

Definition at line 1289 of file lci_strategic_plugin.cpp.

1293{
1294 std::chrono::system_clock::time_point execution_start_time = std::chrono::system_clock::now(); // Start timing the execution time for planning so it can be logged
1295
1296 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "<<<<<<<<<<<<<<<<< STARTING LCI_STRATEGIC_PLAN!!!!!!!!! >>>>>>>>>>>>>>>>");
1297
1298 if (!wm_->getRoute())
1299 {
1300 RCLCPP_ERROR_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Could not plan maneuvers as route was not available");
1301 return;
1302 }
1303
1305 {
1307 {
1308 resp->new_plan.maneuvers = {};
1309 RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"),"Not approaching light-controlled intersection so no maneuvers");
1310 return;
1311 }
1312
1313 bool is_empty_schedule_msg = (scheduled_enter_time_ == 0);
1314 if (is_empty_schedule_msg)
1315 {
1316 resp->new_plan.maneuvers = {};
1317 RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"),"Receiving empty schedule message");
1318 return;
1319 }
1320 }
1321
1322 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Finding car information");
1323
1324 // Extract vehicle data from request
1325 VehicleState current_state = extractInitialState(req);
1326 if (transition_table_.getState() != TransitState::UNAVAILABLE && !req->prior_plan.maneuvers.empty())
1327 {
1328 RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"),"State is NOT UNAVAILABLE AND Maneuvers in request is NOT empty");
1329 return;
1330 }
1331 // Get current traffic light information
1332 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "\n\nFinding traffic_light information");
1333
1334 auto inter_list = wm_->getSignalizedIntersectionsAlongRoute({ req->veh_x, req->veh_y });
1335 auto upcoming_id_= inter_list.front()->id();
1336
1337 auto traffic_list = wm_->getSignalsAlongRoute({ req->veh_x, req->veh_y });
1338
1339 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Found traffic lights of size: " << traffic_list.size());
1340
1341 auto current_lanelets = wm_->getLaneletsFromPoint({ req->veh_x, req->veh_y});
1342 lanelet::ConstLanelet current_lanelet;
1343
1344 if (current_lanelets.empty())
1345 {
1346 RCLCPP_ERROR_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Given vehicle position is not on the road! Returning...");
1347 return;
1348 }
1349
1350 // get the lanelet that is on the route in case overlapping ones found
1351 auto llt_on_route_optional = wm_->getFirstLaneletOnShortestPath(current_lanelets);
1352
1353 if (llt_on_route_optional){
1354 current_lanelet = llt_on_route_optional.value();
1355 }
1356 else{
1357 RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "When identifying the corresponding lanelet for requested maneuever's state, x: "
1358 << req->veh_x << ", y: " << req->veh_y << ", no possible lanelet was found to be on the shortest path."
1359 << "Picking arbitrary lanelet: " << current_lanelets[0].id() << ", instead");
1360 current_lanelet = current_lanelets[0];
1361 }
1362
1363 lanelet::CarmaTrafficSignalPtr nearest_traffic_signal = nullptr;
1364
1365 lanelet::ConstLanelet entry_lanelet;
1366 lanelet::ConstLanelet exit_lanelet;
1367
1368 for (auto signal : traffic_list)
1369 {
1370 auto optional_entry_exit = wm_->getEntryExitOfSignalAlongRoute(signal);
1371 // if signal is not matching our destination skip
1372 if (!optional_entry_exit)
1373 {
1374 RCLCPP_ERROR_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Did not find entry.exit along the route");
1375 continue;
1376 }
1377
1378 entry_lanelet = optional_entry_exit.get().first;
1379 exit_lanelet = optional_entry_exit.get().second;
1380
1381 nearest_traffic_signal = signal;
1382 break;
1383 }
1384
1385
1386 TransitState prev_state;
1387
1388 do
1389 {
1390 // Clear previous maneuvers planned by this plugin as guard against state change since each state generates an
1391 // independent set of maneuvers
1392 resp->new_plan = carma_planning_msgs::msg::ManeuverPlan();
1393
1394 prev_state = transition_table_.getState(); // Cache previous state to check if state has changed after 1 iteration
1395
1396 RCLCPP_INFO_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Planning in state: " << transition_table_.getState());
1397
1398 boost::optional<std::pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState>> current_light_state_optional = boost::none;
1399 if (nearest_traffic_signal)
1400 {
1401 current_light_state_optional = nearest_traffic_signal->predictState(lanelet::time::timeFromSec(current_state.stamp.seconds()));
1402 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Real-time current signal: " << current_light_state_optional.get().second << ", for Id: " << nearest_traffic_signal->id());
1403 if (current_light_state_optional.get().second == lanelet::CarmaTrafficSignalState::UNAVAILABLE || !current_light_state_optional)
1404 {
1405 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Signal state not available returning..." );
1406 return;
1407 }
1408 }
1409 switch (transition_table_.getState())
1410 {
1412 planWhenUNAVAILABLE(req, resp, current_state, nearest_traffic_signal, entry_lanelet, exit_lanelet, current_lanelet);
1413 break;
1414
1416 planWhenAPPROACHING(req, resp, current_state, nearest_traffic_signal, entry_lanelet, exit_lanelet, current_lanelet);
1417 break;
1418
1420 planWhenWAITING(req, resp, current_state, nearest_traffic_signal, entry_lanelet, exit_lanelet, current_lanelet);
1421 break;
1422
1424 // Reset intersection state since we are no longer in an intersection
1426 {
1427 throw std::invalid_argument("State is DEPARTING but the intersection variables are not available");
1428 }
1429 planWhenDEPARTING(req, resp, current_state, intersection_end_downtrack_.get(), intersection_speed_.get());
1430 break;
1431
1432 default:
1433 throw std::invalid_argument("LCIStrategic Strategic Plugin entered unknown state");
1434 break;
1435 }
1436
1437 } while (transition_table_.getState() != prev_state); // If the state has changed then continue looping
1438
1439 std::chrono::system_clock::time_point execution_end_time = std::chrono::system_clock::now(); // Planning complete
1440
1441 auto execution_duration = execution_end_time - execution_start_time;
1442 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "ExecutionTime lci_strategic_plugin: " << std::chrono::duration<double>(execution_duration).count());
1443
1444 return;
1445 // We need to evaluate the events so the state transitions can be triggered
1446}
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...
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...
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 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...
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.
TransitState getState() const
Returns the current state.
TransitState
Enum describing the possible states of the LCIStrategic Strategic Plugin.
Definition: lci_states.hpp:25

References lci_strategic_plugin::APPROACHING, approaching_light_controlled_intersection_, config_, lci_strategic_plugin::DEPARTING, lci_strategic_plugin::LCIStrategicPluginConfig::enable_carma_streets_connection, extractInitialState(), lci_strategic_plugin::LCIStrategicStateTransitionTable::getState(), intersection_end_downtrack_, intersection_speed_, planWhenAPPROACHING(), planWhenDEPARTING(), planWhenUNAVAILABLE(), planWhenWAITING(), scheduled_enter_time_, lci_strategic_plugin::LCIStrategicPlugin::VehicleState::stamp, transition_table_, lci_strategic_plugin::UNAVAILABLE, upcoming_id_, lci_strategic_plugin::WAITING, and wm_.

Here is the call graph for this function:

◆ planWhenAPPROACHING()

void lci_strategic_plugin::LCIStrategicPlugin::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 
)
private

Method for performing maneuver planning when the current plugin state is TransitState::APPROACHING Therefore the planned maneuvers deal with approaching a traffic light.

Parameters
reqPlan maneuver request
[out]respPlan maneuver response with a list of maneuver plan
current_stateThe current state of the vehicle at the start of planning
traffic_lightThe single traffic light along the vehicle's route in the intersection that is relevant and in front of the vehicle
entry_laneletThe entry lanelet into the intersection
exit_laneletThe exit lanelet into the intersection
current_laneletThe current lanelet the vehicle's state is on
Exceptions
ifgiven entry_lanelet doesn't have stop_line or speed profile case number was failed to be calculated NOTE: if there is empty traffic_light, it signals that the vehicle has crossed the bar, or if invalid light state is given

Definition at line 840 of file lci_strategic_plugin.cpp.

843{
845
846 if (!traffic_light) // If we are in the approaching state and there is no longer any lights ahead of us then
847 // the vehicle must have crossed the stop bar
848 {
850 return;
851 }
852
853 double current_state_speed = std::max(current_state.speed, config_.algo_minimum_speed * 1.001);
854
855 auto stop_line = traffic_light->getStopLine(entry_lanelet);
856
857 if (!stop_line)
858 {
859 throw std::invalid_argument("Given entry lanelet doesn't have stop_line...");
860 }
861
862 double traffic_light_down_track =
863 wm_->routeTrackPos(stop_line.get().front().basicPoint2d()).downtrack;
864
865 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "traffic_light_down_track: " << traffic_light_down_track);
866
867 double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.downtrack;
868
869 distance_remaining_to_tf_ = distance_remaining_to_traffic_light;
870
871 if (distance_remaining_to_traffic_light < 0) // there is small discrepancy between signal's routeTrackPos as well as current
872 { // downtrack calculated using veh_x,y from state. Therefore, it may have crossed it
873 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Crossed the bar distance_remaining_to_traffic_light: " << distance_remaining_to_traffic_light);
875 return;
876 }
877
878 // At this point we know the vehicle is within the activation distance and we know the current and next light phases
879 // All we need to now determine is if we should stop or if we should continue
880 lanelet::ConstLanelet intersection_lanelet;
881
882 auto route = wm_->getRoute()->shortestPath();
883 auto entry_llt_iter = std::find(route.begin(), route.end(), entry_lanelet);
884 if (entry_llt_iter != route.end())
885 {
886 intersection_lanelet = *(entry_llt_iter + 1);
887 }
888
889 if (intersection_lanelet.id() != lanelet::InvalId)
890 {
891 intersection_speed_ = findSpeedLimit(intersection_lanelet);
893 }
894 else
895 {
896 intersection_speed_ = findSpeedLimit(exit_lanelet);
897 }
898
899 intersection_speed_ = intersection_speed_.get() * 0.999; // so that speed_limit is not exactly same as departure or current speed
900
901 double speed_limit = findSpeedLimit(current_lanelet);
902
903 current_state_speed = std::min(current_state_speed, speed_limit - 5 * epsilon_); // so that current_speed is not exactly same as departure or speed limit
904
905 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "current_state_speed: " << current_state_speed);
906 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "intersection_speed_: " << intersection_speed_.get());
907 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "distance_remaining_to_traffic_light: " << distance_remaining_to_traffic_light);
908
910 wm_->routeTrackPos(exit_lanelet.centerline2d().front().basicPoint2d()).downtrack;
911
912 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "intersection_end_downtrack_: " << intersection_end_downtrack_.get());
913
914 // If the vehicle is at a stop trigger the stopped state
915 constexpr double HALF_MPH_IN_MPS = 0.22352;
916 if (current_state.speed < HALF_MPH_IN_MPS &&
917 fabs(distance_remaining_to_traffic_light) < config_.stopping_location_buffer)
918 {
919 transition_table_.signal(TransitEvent::STOPPED); // The vehicle has come to a stop at the light
920 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CARMA has detected that the vehicle stopped at the stop bar. Transitioning to WAITING STATE");
921
922 return;
923 }
924
926
927 rclcpp::Time earliest_entry_time = current_state.stamp + get_earliest_entry_time(distance_remaining_to_traffic_light, speed_limit,
928 current_state_speed, intersection_speed_.get(), max_comfort_accel_, max_comfort_decel_);
929
930 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "earliest_entry_time: " << std::to_string(earliest_entry_time.seconds()) << ", with : " << std::to_string((earliest_entry_time - current_state.stamp).seconds()) << " left at: " << std::to_string(current_state.stamp.seconds()));
931
932 auto [nearest_green_entry_time, is_entry_time_within_green_or_tbd, in_tbd] = get_final_entry_time_and_conditions(current_state, earliest_entry_time, traffic_light);
933
934 if (nearest_green_entry_time == rclcpp::Time(0))
935 return;
936
937 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Final nearest_green_entry_time: " << std::to_string(nearest_green_entry_time.seconds()));
938
939 auto et_state = traffic_light->predictState(lanelet::time::timeFromSec(nearest_green_entry_time.seconds()));
940 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Signal at ET: " << et_state.get().second);
941
943
944 double remaining_time = nearest_green_entry_time.seconds() - current_state.stamp.seconds();
945 double remaining_time_earliest_entry = earliest_entry_time.seconds() - current_state.stamp.seconds();
946 scheduled_entry_time_ = remaining_time; // performance metric
947 earliest_entry_time_ = remaining_time_earliest_entry; // performance metric
948
949 auto boundary_distances = get_delta_x(current_state_speed, intersection_speed_.get(), speed_limit, config_.algo_minimum_speed, max_comfort_accel_, max_comfort_decel_);
950 print_boundary_distances(boundary_distances); //debug
951
952 auto boundary_traj_params = get_boundary_traj_params(rclcpp::Time(req->header.stamp, RCL_SYSTEM_TIME).seconds(), current_state_speed, intersection_speed_.get(), speed_limit, config_.algo_minimum_speed, max_comfort_accel_, max_comfort_decel_, current_state.downtrack, traffic_light_down_track, distance_remaining_to_traffic_light, boundary_distances);
953
954 TrajectoryParams ts_params = get_ts_case(rclcpp::Time(req->header.stamp, RCL_SYSTEM_TIME).seconds(), nearest_green_entry_time.seconds(), current_state_speed, intersection_speed_.get(), speed_limit, config_.algo_minimum_speed, max_comfort_accel_, max_comfort_decel_, current_state.downtrack, traffic_light_down_track, distance_remaining_to_traffic_light, boundary_distances, boundary_traj_params);
955 print_params(ts_params);
956
957 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "SPEED PROFILE CASE:" << ts_params.case_num);
958
960 double emergency_distance_to_stop = pow(current_state.speed, 2)/(2 * emergency_decel_norm_) + config_.stopping_location_buffer / 2; //Idea is to aim the middle part of stopping buffer
961 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "emergency_distance_to_stop at emergency_decel_norm_ (with stopping_location_buffer/2): " << emergency_distance_to_stop << ", emergency_decel_norm_: " << emergency_decel_norm_);
962
963 double safe_distance_to_stop = pow(current_state.speed, 2)/(2 * max_comfort_decel_norm_) + config_.stopping_location_buffer / 2; //Idea is to aim the middle part of stopping buffer
964 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "safe_distance_to_stop at max_comfort_decel (with stopping_location_buffer/2): " << safe_distance_to_stop << ", max_comfort_decel_norm_: " << max_comfort_decel_norm_);
965
966 double desired_distance_to_stop = pow(current_state.speed, 2)/(2 * max_comfort_decel_norm_ * config_.deceleration_fraction) + config_.desired_distance_to_stop_buffer;
967 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "desired_distance_to_stop at: " << desired_distance_to_stop << ", where effective deceleration rate is: " << max_comfort_decel_norm_ * config_.deceleration_fraction);
968
969 emergency_distance_to_stop = std::max(emergency_distance_to_stop, config_.stopping_location_buffer);
970 safe_distance_to_stop = std::max(safe_distance_to_stop, config_.stopping_location_buffer);
971 desired_distance_to_stop = std::max(desired_distance_to_stop, config_.stopping_location_buffer);
972
973 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "new emergency_distance_to_stop: " << emergency_distance_to_stop);
974 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "new safe_distance_to_stop: " << safe_distance_to_stop);
975 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "new desired_distance_to_stop: " << desired_distance_to_stop);
976
977 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "distance_remaining_to_traffic_light: " << distance_remaining_to_traffic_light << ", current_state.speed: " << current_state.speed);
978
979 // Basic RED signal violation check
980 if (distance_remaining_to_traffic_light <= emergency_distance_to_stop || last_case_num_ == TSCase::EMERGENCY_STOPPING)
981 {
982 if (in_tbd) // Given ET is in TBD, but vehicle is too close to intersection
983 {
984 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "ET is still in TBD despite the vehicle being in desired distance to start stopping. Trying to handle this edge case gracefully...");
985 }
986
987 double stopping_time = current_state.speed / 1.5 / max_comfort_decel_norm_; //one half the acceleration (twice the acceleration to stop) to account for emergency case, see emergency_decel_norm_
988
989 rclcpp::Time stopping_arrival_time =
990 current_state.stamp + rclcpp::Duration::from_nanoseconds(stopping_time * 1e9);
991
992 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "stopping_arrival_time: " << std::to_string(stopping_arrival_time.seconds()));
993
994 auto stopping_arrival_state_optional = traffic_light->predictState(lanelet::time::timeFromSec(stopping_arrival_time.seconds()));
995
996 if (!validLightState(stopping_arrival_state_optional, stopping_arrival_time))
997 {
998 RCLCPP_ERROR_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Unable to resolve give signal for stopping_arrival_state_optional: " << std::to_string(stopping_arrival_time.seconds()));
999 return;
1000 }
1001
1002 if (stopping_arrival_state_optional.get().second == lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN || last_case_num_ == TSCase::EMERGENCY_STOPPING) // if once started emergency stopped, keep doing it to avoid jerkiness
1003 {
1004 RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"),"Detected possible RED light violation! Stopping!");
1005 handleStopping(req,resp, current_state, traffic_light, entry_lanelet, exit_lanelet, current_lanelet, traffic_light_down_track, true); //case_11
1007
1008 return;
1009 }
1010 }
1011
1012 // Check if the vehicle can arrive with certainty (Case 1-7)
1013 if (ts_params.is_algorithm_successful && ts_params.case_num != TSCase::CASE_8 &&
1014 (distance_remaining_to_traffic_light >= desired_distance_to_stop || !in_tbd) &&
1015 is_entry_time_within_green_or_tbd) // ET cannot be explicitly inside RED or YELLOW in available future states, which is ERROR case
1016 {
1017 handleGreenSignalScenario(req, resp, current_state, current_state_speed, traffic_light, entry_lanelet, exit_lanelet, traffic_light_down_track, ts_params, in_tbd); //in_tbd means optional to check certainty arrival at green
1018
1019 if (!resp->new_plan.maneuvers.empty()) // able to pass at green
1020 {
1021 last_case_num_ = ts_params.case_num;
1022 return;
1023 }
1024
1025 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Not able to make it with certainty: TSCase: " << ts_params.case_num << ", changing it to 8");
1026 ts_params = boundary_traj_params[7];
1027 ts_params.is_algorithm_successful = true; //false correspond to cases when vehicle is beyond safe_distance to stop for case8
1028 ts_params.case_num = CASE_8;
1029 print_params(ts_params);
1030 }
1031
1032 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Not able to make it with certainty: NEW TSCase: " << ts_params.case_num);
1033 // if algorithm is NOT successful or if the vehicle cannot make the green light with certainty
1034
1035 if (desired_distance_to_stop < distance_remaining_to_traffic_light && last_case_num_ != TSCase::STOPPING) // do not switch from STOPPING to case8 again
1036 {
1037 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Way too early to stop. Cruising at CASE8");
1038 handleCruisingUntilStop(req, resp, current_state, current_state_speed, traffic_light, traffic_light_down_track, ts_params);
1039
1040 if (!resp->new_plan.maneuvers.empty())
1041 {
1043 return;
1044 }
1045 }
1046
1047 if ((safe_distance_to_stop <= distance_remaining_to_traffic_light && desired_distance_to_stop >= distance_remaining_to_traffic_light) ||
1048 last_case_num_ == TSCase::STOPPING) // if stopping continue stopping until transition to planwhenWAITING
1049 {
1050 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Planning stopping now. last case:" << static_cast<int>(last_case_num_));
1051 handleStopping(req,resp, current_state, traffic_light, entry_lanelet, exit_lanelet, current_lanelet, traffic_light_down_track); //case_9
1052 return;
1053 }
1054
1055 if (safe_distance_to_stop > distance_remaining_to_traffic_light)
1056 {
1057 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "No longer within safe distance to stop! Decide to continue stopping or continue into intersection");
1058
1059 if (last_case_num_ != TSCase::STOPPING && last_case_num_ != TSCase::UNAVAILABLE && last_case_num_ != TSCase::CASE_8) //case 1-7 or emergency stop or handlefailure
1060 {
1061 // 3. if not able to stop nor reach target speed at green, attempt its best to reach the target parameters at the intersection
1062 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "HANDLE_LAST_RESORT: The vehicle is not able to stop at red/yellow light nor is able to reach target speed at green. Attempting its best to pass through at green!");
1063
1064 handleFailureCase(req, resp, current_state, current_state_speed, speed_limit, remaining_time,
1065 exit_lanelet.id(), traffic_light, traffic_light_down_track, ts_params);
1066
1067 if (resp->new_plan.maneuvers.empty())
1068 {
1069 RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"),"HANDLE_SAFETY: Planning forced slow-down... last case:" << static_cast<int>(last_case_num_));
1070 handleStopping(req,resp, current_state, traffic_light, entry_lanelet, exit_lanelet, current_lanelet, traffic_light_down_track, true); //case_11 emergency case with twice the normal deceleration
1071 }
1072 }
1073 else
1074 {
1075 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "HANDLE_SAFETY: Planning to keep stopping now. last case:" << static_cast<int>(last_case_num_));
1076 handleStopping(req,resp, current_state, traffic_light, entry_lanelet, exit_lanelet, current_lanelet, traffic_light_down_track); //case_9
1077 }
1078 }
1079
1080
1081}
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...
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...
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 findSpeedLimit(const lanelet::ConstLanelet &llt) const
Given a Lanelet, find it's associated Speed Limit.
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....
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,...
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...
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)
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...
TurnDirection getTurnDirectionAtIntersection(std::vector< lanelet::ConstLanelet > lanelets_list)
Determine the turn direction at intersection.
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...
void print_boundary_distances(BoundaryDistances delta_xs)
Helper method to print TrajectoryParams.
void signal(TransitEvent signal)
Trigger signal for the transition table.

References lci_strategic_plugin::LCIStrategicPluginConfig::algo_minimum_speed, lci_strategic_plugin::CASE_8, lci_strategic_plugin::TrajectoryParams::case_num, config_, lci_strategic_plugin::CROSSED_STOP_BAR, lci_strategic_plugin::LCIStrategicPluginConfig::deceleration_fraction, lci_strategic_plugin::LCIStrategicPluginConfig::desired_distance_to_stop_buffer, distance_remaining_to_tf_, lci_strategic_plugin::LCIStrategicPlugin::VehicleState::downtrack, earliest_entry_time_, emergency_decel_norm_, lci_strategic_plugin::EMERGENCY_STOPPING, epsilon_, findSpeedLimit(), get_boundary_traj_params(), get_delta_x(), get_earliest_entry_time(), get_final_entry_time_and_conditions(), get_ts_case(), getTurnDirectionAtIntersection(), handleCruisingUntilStop(), handleFailureCase(), handleGreenSignalScenario(), handleStopping(), intersection_end_downtrack_, intersection_speed_, intersection_turn_direction_, lci_strategic_plugin::TrajectoryParams::is_algorithm_successful, last_case_num_, max_comfort_accel_, max_comfort_decel_, max_comfort_decel_norm_, print_boundary_distances(), print_params(), scheduled_entry_time_, lci_strategic_plugin::LCIStrategicStateTransitionTable::signal(), lci_strategic_plugin::LCIStrategicPlugin::VehicleState::speed, lci_strategic_plugin::LCIStrategicPlugin::VehicleState::stamp, lci_strategic_plugin::STOPPED, lci_strategic_plugin::STOPPING, lci_strategic_plugin::LCIStrategicPluginConfig::stopping_location_buffer, carma_cooperative_perception::to_string(), transition_table_, lci_strategic_plugin::UNAVAILABLE, validLightState(), and wm_.

Referenced by plan_maneuvers_callback().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ planWhenDEPARTING()

void lci_strategic_plugin::LCIStrategicPlugin::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 
)
private

Method for performing maneuver planning when the current plugin state is TransitState::DEPARTING Therefor the planned maneuvers deal with the transit of the intersection once the stop bar has been crossed.

Parameters
reqPlan maneuver request
[out]respPlan maneuver response with a list of maneuver plan
current_stateThe current state of the vehicle at the start of planning
intersection_end_downtrackThe ending downtrack in meters of the current intersection
intersection_speed_limitThe speed limit of the current intersection

Definition at line 1149 of file lci_strategic_plugin.cpp.

1152{
1154
1155 if (current_state.downtrack > intersection_end_downtrack)
1156 {
1157 transition_table_.signal(TransitEvent::INTERSECTION_EXIT); // If we are past the most recent
1158 return;
1159 }
1160
1161 // Calculate exit time assuming constant acceleration
1162 rclcpp::Time intersection_exit_time =
1163 current_state.stamp +
1164 rclcpp::Duration::from_nanoseconds(2.0 * (intersection_end_downtrack - current_state.downtrack) / (current_state.speed + intersection_speed_limit) * 1e9);
1165
1166 // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver
1167 std::vector<lanelet::ConstLanelet> crossed_lanelets =
1168 getLaneletsBetweenWithException(current_state.downtrack, intersection_end_downtrack, true, false);
1169
1170 // Compose intersection transit maneuver
1171 resp->new_plan.maneuvers.push_back(composeIntersectionTransitMessage(
1172 current_state.downtrack, intersection_end_downtrack, current_state.speed, intersection_speed_limit,
1173 current_state.stamp, intersection_exit_time, crossed_lanelets.back().id(), crossed_lanelets.back().id()));
1174}

References composeIntersectionTransitMessage(), lci_strategic_plugin::LCIStrategicPlugin::VehicleState::downtrack, getLaneletsBetweenWithException(), lci_strategic_plugin::INTERSECTION_EXIT, last_case_num_, lci_strategic_plugin::LCIStrategicStateTransitionTable::signal(), lci_strategic_plugin::LCIStrategicPlugin::VehicleState::speed, lci_strategic_plugin::LCIStrategicPlugin::VehicleState::stamp, transition_table_, and lci_strategic_plugin::UNAVAILABLE.

Referenced by plan_maneuvers_callback().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ planWhenUNAVAILABLE()

void lci_strategic_plugin::LCIStrategicPlugin::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 
)
private

Method for performing maneuver planning when the current plugin state is TransitState::UNAVAILABLE Therefor no maneuvers are planned but the system checks for the precense of traffic lights ahead of it.

Parameters
reqPlan maneuver request
[out]respPlan maneuver response with a list of maneuver plan
current_stateThe current state of the vehicle at the start of planning
traffic_lightThe single traffic light along the vehicle's route in the intersection that is relevant and in front of the vehicle
entry_laneletThe entry lanelet into the intersection
exit_laneletThe exit lanelet into the intersection
current_laneletThe current lanelet the vehicle's state is on
Exceptions
ifgiven entry_lanelet doesn't have stop_line NOTE: returns empty if the given traffic light is empty

Definition at line 770 of file lci_strategic_plugin.cpp.

773{
774 // Reset intersection state since in this state we are not yet known to be in or approaching an intersection
775 intersection_speed_ = boost::none;
776 intersection_end_downtrack_ = boost::none;
777 double current_state_speed = std::max(current_state.speed, config_.algo_minimum_speed * 1.001);
778
779
780 if (!traffic_light)
781 {
782 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "No lights found along route. Returning maneuver plan unchanged");
783 return;
784 }
785
787 double max_comfort_decel = -1.0 * config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier;
788
789 auto stop_line = traffic_light->getStopLine(entry_lanelet);
790
791 if (!stop_line)
792 {
793 throw std::invalid_argument("Given entry lanelet doesn't have stop_line...");
794 }
795
796 double traffic_light_down_track =
797 wm_->routeTrackPos(stop_line.get().front().basicPoint2d()).downtrack;
798
799 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "traffic_light_down_track: " << traffic_light_down_track);
800
801 double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.downtrack;
802
803 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "distance_remaining_to_traffic_light: " << distance_remaining_to_traffic_light <<
804 ", current_state.downtrack: " << current_state.downtrack);
805
806 distance_remaining_to_tf_ = distance_remaining_to_traffic_light; // performance metric
807
808 auto speed_limit = findSpeedLimit(current_lanelet);
809
810 current_state_speed = std::min(speed_limit, current_state_speed);
811
812 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "speed_limit (free flow speed): " << speed_limit);
813
814 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "trajectory_smoothing_activation_distance: " << config_.trajectory_smoothing_activation_distance);
815
816 double stopping_dist = estimate_distance_to_stop(current_state_speed, config_.vehicle_decel_limit_multiplier *
817 config_.vehicle_decel_limit); //accepts positive decel
818
819 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Stopping distance: " << stopping_dist);
820
821 double plugin_activation_distance = std::max(stopping_dist, config_.min_approach_distance);
822
823 plugin_activation_distance = std::max(plugin_activation_distance, config_.trajectory_smoothing_activation_distance);
824
825 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "plugin_activation_distance: " << plugin_activation_distance);
826
827 if (distance_remaining_to_traffic_light <= plugin_activation_distance)
828 {
829 RCLCPP_INFO_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Within intersection range");
830 transition_table_.signal(TransitEvent::IN_STOPPING_RANGE); // Evaluate Signal and Run Trajectory Smoothing Algorithm
831 }
832 else
833 {
835 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Not within intersection range");
836 }
837
838}
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.

References lci_strategic_plugin::LCIStrategicPluginConfig::algo_minimum_speed, config_, distance_remaining_to_tf_, lci_strategic_plugin::LCIStrategicPlugin::VehicleState::downtrack, estimate_distance_to_stop(), findSpeedLimit(), lci_strategic_plugin::IN_STOPPING_RANGE, intersection_end_downtrack_, intersection_speed_, last_case_num_, lci_strategic_plugin::LCIStrategicPluginConfig::min_approach_distance, lci_strategic_plugin::LCIStrategicStateTransitionTable::signal(), lci_strategic_plugin::LCIStrategicPlugin::VehicleState::speed, lci_strategic_plugin::LCIStrategicPluginConfig::trajectory_smoothing_activation_distance, transition_table_, lci_strategic_plugin::UNAVAILABLE, lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_accel_limit, lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_accel_limit_multiplier, lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_decel_limit, lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_decel_limit_multiplier, and wm_.

Referenced by plan_maneuvers_callback().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ planWhenWAITING()

void lci_strategic_plugin::LCIStrategicPlugin::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 
)
private

Method for performing maneuver planning when the current plugin state is TransitState::WAITING Therefor the planned maneuvers deal with waiting at a red light.

Parameters
reqPlan maneuver request
[out]respPlan maneuver response with a list of maneuver plan
current_stateThe current state of the vehicle at the start of planning
traffic_lightThe single traffic light along the vehicle's route in the intersection that is relevant and in front of the vehicle
entry_laneletThe entry lanelet into the intersection
exit_laneletThe exit lanelet into the intersection
current_laneletThe current lanelet the vehicle's state is on NOTE: returns empty if the given traffic light is empty

Definition at line 1083 of file lci_strategic_plugin.cpp.

1086{
1088
1089 if (!traffic_light)
1090 {
1091 throw std::invalid_argument("While in WAITING state, the traffic lights disappeared.");
1092 }
1093
1094 auto stop_line = traffic_light->getStopLine(entry_lanelet);
1095
1096 if (!stop_line)
1097 {
1098 throw std::invalid_argument("Given entry lanelet doesn't have stop_line...");
1099 }
1100
1101 double traffic_light_down_track =
1102 wm_->routeTrackPos(stop_line.get().front().basicPoint2d()).downtrack;
1103
1104 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "traffic_light_down_track: "<< traffic_light_down_track);
1105
1106 double entering_time = current_state.stamp.seconds();
1107
1108 auto current_light_state_optional = traffic_light->predictState(lanelet::time::timeFromSec(entering_time));
1109
1110 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "WAITING STATE: requested time to rclcpp::Time(req->header.stamp, RCL_SYSTEM_TIME) check: " << std::to_string(rclcpp::Time(req->header.stamp, RCL_SYSTEM_TIME).seconds()));
1111 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "WAITING STATE: requested time to CURRENT STATE check: " << std::to_string(entering_time));
1112
1113 if (!validLightState(current_light_state_optional, rclcpp::Time(entering_time * 1e9)))
1114 return;
1115
1116 auto bool_optional_late_certainty = canArriveAtGreenWithCertainty(rclcpp::Time(entering_time * 1e9), traffic_light, true, false);
1117
1118 if (!bool_optional_late_certainty)
1119 {
1120 RCLCPP_ERROR_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Unable to resolve green light...");
1121 return;
1122 }
1123
1124 bool should_enter = true; //uc2
1125
1126 if (config_.enable_carma_streets_connection && entering_time > current_state.stamp.seconds()) //uc3
1127 should_enter = false;
1128
1129 if (isStateAllowedGreen(current_light_state_optional.get().second) && bool_optional_late_certainty.get() && should_enter) // if can make with certainty
1130 {
1131 transition_table_.signal(TransitEvent::RED_TO_GREEN_LIGHT); // If the light is green send the light transition
1132 // signal
1133 return;
1134 }
1135
1136 // A fixed buffer to add to stopping maneuvers once the vehicle is already stopped to ensure that they can have their
1137 // trajectory planned
1138 constexpr double stop_maneuver_buffer = 10.0;
1139
1140 // If the light is not green then continue waiting by creating a stop and wait maneuver on top of the vehicle
1142
1143 resp->new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage(
1144 current_state.downtrack - stop_maneuver_buffer, traffic_light_down_track, current_state.speed,
1145 current_state.lane_id, current_state.lane_id, rclcpp::Time(entering_time * 1e9),
1146 rclcpp::Time(entering_time * 1e9) + rclcpp::Duration::from_nanoseconds(config_.min_maneuver_planning_period * 1e9), stopping_accel));
1147}

References canArriveAtGreenWithCertainty(), composeStopAndWaitManeuverMessage(), config_, lci_strategic_plugin::LCIStrategicPlugin::VehicleState::downtrack, lci_strategic_plugin::LCIStrategicPluginConfig::enable_carma_streets_connection, isStateAllowedGreen(), lci_strategic_plugin::LCIStrategicPlugin::VehicleState::lane_id, last_case_num_, lci_strategic_plugin::LCIStrategicPluginConfig::min_maneuver_planning_period, lci_strategic_plugin::RED_TO_GREEN_LIGHT, lci_strategic_plugin::LCIStrategicStateTransitionTable::signal(), lci_strategic_plugin::LCIStrategicPlugin::VehicleState::speed, lci_strategic_plugin::LCIStrategicPlugin::VehicleState::stamp, lci_strategic_plugin::STOPPING, carma_cooperative_perception::to_string(), transition_table_, validLightState(), lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_decel_limit, lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_decel_limit_multiplier, and wm_.

Referenced by plan_maneuvers_callback().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ print_boundary_distances()

void lci_strategic_plugin::LCIStrategicPlugin::print_boundary_distances ( BoundaryDistances  delta_xs)
private

Helper method to print TrajectoryParams.

Definition at line 504 of file lci_strategic_plugin_algo.cpp.

505{
506 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "\n");
507 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "dx1: " << delta_xs.dx1);
508 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "dx2: " << delta_xs.dx2);
509 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "dx3: " << delta_xs.dx3);
510 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "dx4: " << delta_xs.dx4);
511 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "dx5: " << delta_xs.dx5 << "\n");
512}

References lci_strategic_plugin::BoundaryDistances::dx1, lci_strategic_plugin::BoundaryDistances::dx2, lci_strategic_plugin::BoundaryDistances::dx3, lci_strategic_plugin::BoundaryDistances::dx4, and lci_strategic_plugin::BoundaryDistances::dx5.

Referenced by planWhenAPPROACHING().

Here is the caller graph for this function:

◆ print_params()

void lci_strategic_plugin::LCIStrategicPlugin::print_params ( TrajectoryParams  params)
private

Helper method to print TrajectoryParams.

Definition at line 479 of file lci_strategic_plugin_algo.cpp.

480{
481 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "\n");
482 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "t0: " << std::to_string(params.t0_));
483 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "v0: " << params.v0_);
484 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "x0: " << params.x0_);
485
486 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "t1: " << std::to_string(params.t1_));
487 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "v1: " << params.v1_);
488 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "x1: " << params.x1_);
489 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "a1: " << params.a1_);
490
491 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "t2: " << std::to_string(params.t2_));
492 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "v2: " << params.v2_);
493 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "x2: " << params.x2_);
494 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "a2: " << params.a2_);
495
496 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "t3: " << std::to_string(params.t3_));
497 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "v3: " << params.v3_);
498 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "x3: " << params.x3_);
499 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "a3: " << params.a3_);
500
501 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "\n");
502}

References lci_strategic_plugin::TrajectoryParams::a1_, lci_strategic_plugin::TrajectoryParams::a2_, lci_strategic_plugin::TrajectoryParams::a3_, lci_strategic_plugin::TrajectoryParams::t0_, lci_strategic_plugin::TrajectoryParams::t1_, lci_strategic_plugin::TrajectoryParams::t2_, lci_strategic_plugin::TrajectoryParams::t3_, carma_cooperative_perception::to_string(), lci_strategic_plugin::TrajectoryParams::v0_, lci_strategic_plugin::TrajectoryParams::v1_, lci_strategic_plugin::TrajectoryParams::v2_, lci_strategic_plugin::TrajectoryParams::v3_, lci_strategic_plugin::TrajectoryParams::x0_, lci_strategic_plugin::TrajectoryParams::x1_, lci_strategic_plugin::TrajectoryParams::x2_, and lci_strategic_plugin::TrajectoryParams::x3_.

Referenced by handleFailureCaseHelper(), and planWhenAPPROACHING().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ publishMobilityOperation()

void lci_strategic_plugin::LCIStrategicPlugin::publishMobilityOperation ( )

Publish mobility operation at a fixed rate.

Definition at line 1278 of file lci_strategic_plugin.cpp.

1279{
1281 {
1282 carma_v2x_msgs::msg::MobilityOperation status_msg = generateMobilityOperation();
1283 mobility_operation_pub_->publish(status_msg);
1284 }
1285}
carma_v2x_msgs::msg::MobilityOperation generateMobilityOperation()
Generates Mobility Operation messages.

References approaching_light_controlled_intersection_, generateMobilityOperation(), and mobility_operation_pub_.

Referenced by on_activate_plugin().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ publishTrajectorySmoothingInfo()

void lci_strategic_plugin::LCIStrategicPlugin::publishTrajectorySmoothingInfo ( )

Publish trajectory smoothing info at a fixed rate.

Definition at line 183 of file lci_strategic_plugin.cpp.

184{
185 std_msgs::msg::Int8 case_num_msg;
186 std_msgs::msg::Float64 tf_distance;
187 std_msgs::msg::Float64 earliest_et;
188 std_msgs::msg::Float64 scheduled_et;
189
190 case_num_msg.data = static_cast<int>(last_case_num_);
191 tf_distance.data = distance_remaining_to_tf_;
192 earliest_et.data = earliest_entry_time_;
193 scheduled_et.data = scheduled_entry_time_;
194
195 case_pub_->publish(case_num_msg);
196 tf_distance_pub_->publish(tf_distance);
197 earliest_et_pub_->publish(earliest_et);
198 et_pub_->publish(scheduled_et);
199}

References case_pub_, distance_remaining_to_tf_, earliest_entry_time_, earliest_et_pub_, et_pub_, last_case_num_, scheduled_entry_time_, and tf_distance_pub_.

Referenced by on_activate_plugin().

Here is the caller graph for this function:

◆ supportedLightState()

bool lci_strategic_plugin::LCIStrategicPlugin::supportedLightState ( lanelet::CarmaTrafficSignalState  state) const
private

Helper method to evaluate if the given traffic light state is supported by this plugin.

Parameters
stateThe state to evaluate
Returns
true if the state is supported, flase otherwise

Definition at line 214 of file lci_strategic_plugin.cpp.

215{
216 switch (state)
217 {
218 // NOTE: Following cases are intentional fall through.
219 // Supported light states
220 case lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN: // Solid Red
221 case lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE: // Yellow Solid no chance of conflicting traffic
222 case lanelet::CarmaTrafficSignalState::PERMISSIVE_MOVEMENT_ALLOWED: // Solid Green there could be conflict traffic
223 case lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED: // Solid Green no chance of conflict traffic
224 // (normally used with arrows)
225 return true;
226
227 // Unsupported light states
228 case lanelet::CarmaTrafficSignalState::PERMISSIVE_CLEARANCE: // Yellow Solid there is a chance of conflicting
229 // traffic
230 case lanelet::CarmaTrafficSignalState::UNAVAILABLE: // No data available
231 case lanelet::CarmaTrafficSignalState::DARK: // Light is non-functional
232 case lanelet::CarmaTrafficSignalState::STOP_THEN_PROCEED: // Flashing Red
233
234 case lanelet::CarmaTrafficSignalState::PRE_MOVEMENT: // Yellow Red transition (Found only in the EU)
235 // (normally used with arrows)
236 case lanelet::CarmaTrafficSignalState::CAUTION_CONFLICTING_TRAFFIC: // Yellow Flashing
237 default:
238 return false;
239 }
240}

References lci_strategic_plugin::UNAVAILABLE.

Referenced by validLightState().

Here is the caller graph for this function:

◆ ts_case1()

TrajectoryParams lci_strategic_plugin::LCIStrategicPlugin::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 
)
private

Trajectory Smoothing Algorithm 8 cases. TSMO UC2, UC3 algorithm equations.

Definition at line 666 of file lci_strategic_plugin_algo.cpp.

667{
668 TrajectoryParams traj;
669
670 traj.t0_ = t;
671 traj.v0_ = v0;
672 traj.x0_ = x0;
673
674 double dt = et - t;
675 double nom1 = 2 * dx * (((1 - (a_max / a_min)) * v_max) + ((a_max / a_min) * v1) - v0);
676 double nom2 = dt * (((1 - (a_max / a_min)) * pow(v_max, 2)) + ((a_max / a_min) * pow(v1, 2)) - pow(v0, 2));
677 double den = pow(v_max - v0, 2) - ((a_max / a_min) * pow(v_max - v1, 2));
678
679 if (den <= epsilon_ && den >= -epsilon_)
680 throw std::invalid_argument("CASE1: Received den near zero..." + std::to_string(den));
681
682 double tc = (nom1 - nom2) / den;
683
684 traj.v1_ = v_max;
685
686 if (dt - tc <= epsilon_ && dt - tc >= -epsilon_)
687 throw std::invalid_argument("CASE1: Received dt - tc near zero..." + std::to_string(dt - tc));
688
689 traj.a1_ = (((1 - (a_max / a_min)) * v_max) + ((a_max / a_min) * v1) - v0) / (dt - tc);
690
691 if (traj.a1_ <= accel_epsilon_ && traj.a1_ >= -accel_epsilon_)
692 {
693 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CASE1: Received traj.a1_ near zero...");
694 traj.t1_ = traj.t0_ + ((dt - tc) * (a_max / (a_min + a_max)));
695 traj.x1_ = traj.x0_ + (v_max * (traj.t1_ - traj.t0_));
696 }
697 else
698 {
699 traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
700 traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
701 }
702 traj.a2_ = 0;
703 traj.v2_ = v_max;
704 traj.t2_ = traj.t1_ + tc;
705 traj.x2_ = traj.x1_ + (v_max * tc);
706
707 traj.t3_ = et;
708 traj.a3_ = traj.a1_ * (a_min / a_max);
709 traj.v3_ = v1;
710 traj.x3_ = x_end;
711
712 return traj;
713
714}

References lci_strategic_plugin::TrajectoryParams::a1_, lci_strategic_plugin::TrajectoryParams::a2_, lci_strategic_plugin::TrajectoryParams::a3_, accel_epsilon_, epsilon_, lci_strategic_plugin::TrajectoryParams::t0_, lci_strategic_plugin::TrajectoryParams::t1_, lci_strategic_plugin::TrajectoryParams::t2_, lci_strategic_plugin::TrajectoryParams::t3_, carma_cooperative_perception::to_string(), lci_strategic_plugin::TrajectoryParams::v0_, lci_strategic_plugin::TrajectoryParams::v1_, lci_strategic_plugin::TrajectoryParams::v2_, lci_strategic_plugin::TrajectoryParams::v3_, lci_strategic_plugin::TrajectoryParams::x0_, lci_strategic_plugin::TrajectoryParams::x1_, lci_strategic_plugin::TrajectoryParams::x2_, and lci_strategic_plugin::TrajectoryParams::x3_.

Referenced by get_ts_case().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ ts_case2()

TrajectoryParams lci_strategic_plugin::LCIStrategicPlugin::ts_case2 ( double  t,
double  et,
double  v0,
double  v1,
double  a_max,
double  a_min,
double  x0,
double  x_end,
double  dx 
)
private

Definition at line 715 of file lci_strategic_plugin_algo.cpp.

716{
717 TrajectoryParams traj;
718
719 traj.t0_ = t;
720 traj.v0_ = v0;
721 traj.x0_ = x0;
722
723 double dt = et - t;
724
725 if (dt <= epsilon_ && dt >= -epsilon_)
726 throw std::invalid_argument("CASE2: Received dt near zero..." + std::to_string(dt));
727
728 double sqr1 = pow(1 - (a_max / a_min), 2) * pow(dx / dt, 2);
729 double sqr2 = (1 - (a_max / a_min)) * (((a_max / a_min) * v1 * (v1 - (2 * dx / dt))) + (v0 * ((2 * dx / dt) - v0)));
730 double v_hat = (dx / dt) + (sqrt(sqr1 - sqr2) / (1 - (a_max / a_min)));
731
732 traj.v1_ = v_hat;
733 traj.a1_ = (((1 - (a_max / a_min)) * v_hat) + ((a_max / a_min) * v1) - v0) / dt;
734
735 if (traj.a1_ <= accel_epsilon_ && traj.a1_ >= -accel_epsilon_)
736 {
737 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CASE2: Received traj.a1_ near zero...");
738 traj.t1_ = traj.t0_ + (dt * (a_max / (a_min + a_max)));
739 traj.x1_ = traj.x0_ + (v_hat * (traj.t1_ - traj.t0_));
740 }
741 else
742 {
743 traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
744 traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
745 }
746
747 traj.v2_ = v1;
748 traj.a2_ = traj.a1_ * a_min / a_max;
749
750 if (traj.a2_ <= accel_epsilon_ && traj.a2_ >= -accel_epsilon_)
751 {
752 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CASE2: Received traj.a2_ near zero...");
753 traj.t2_ = traj.t1_ + (dt * (a_min / (a_min + a_max)));
754 traj.x2_ = traj.x1_ + (v_hat * (traj.t2_ - traj.t1_));
755 }
756 else
757 {
758 traj.t2_ = traj.t1_ + ((traj.v2_ - traj.v1_) / traj.a2_);
759 traj.x2_ = x_end;
760 }
761
762 traj.t3_ = traj.t2_;
763 traj.a3_ = 0;
764 traj.v3_ = traj.v2_;
765 traj.x3_ = traj.x2_;
766
767 return traj;
768}

References lci_strategic_plugin::TrajectoryParams::a1_, lci_strategic_plugin::TrajectoryParams::a2_, lci_strategic_plugin::TrajectoryParams::a3_, accel_epsilon_, epsilon_, lci_strategic_plugin::TrajectoryParams::t0_, lci_strategic_plugin::TrajectoryParams::t1_, lci_strategic_plugin::TrajectoryParams::t2_, lci_strategic_plugin::TrajectoryParams::t3_, carma_cooperative_perception::to_string(), lci_strategic_plugin::TrajectoryParams::v0_, lci_strategic_plugin::TrajectoryParams::v1_, lci_strategic_plugin::TrajectoryParams::v2_, lci_strategic_plugin::TrajectoryParams::v3_, lci_strategic_plugin::TrajectoryParams::x0_, lci_strategic_plugin::TrajectoryParams::x1_, lci_strategic_plugin::TrajectoryParams::x2_, and lci_strategic_plugin::TrajectoryParams::x3_.

Referenced by get_ts_case().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ ts_case3()

TrajectoryParams lci_strategic_plugin::LCIStrategicPlugin::ts_case3 ( double  t,
double  et,
double  v0,
double  v1,
double  a_max,
double  a_min,
double  x0,
double  x_end,
double  dx 
)
private

Definition at line 769 of file lci_strategic_plugin_algo.cpp.

770{
771 TrajectoryParams traj;
772
773 traj.t0_ = t;
774 traj.v0_ = v0;
775 traj.x0_ = x0;
776
777 double dt = et - t;
778
779 if (dt <= epsilon_ && dt >= -epsilon_)
780 throw std::invalid_argument("CASE3: Received dt near zero..." + std::to_string(dt));
781
782 double sqr1 = pow((a_max / a_min) - 1, 2) * pow(dx / dt, 2);
783 double sqr2 = ((a_max / a_min) - 1) * ((v1 * (v1 - (2 * dx / dt))) + ((a_max / a_min) * v0 * ((2 * dx / dt) - v0)));
784 double v_hat = (dx / dt) + (sqrt(sqr1 - sqr2) / ((a_max / a_min) - 1));
785
786 traj.v1_ = v_hat;
787 traj.a1_ = (((1 - (a_min / a_max)) * v_hat) + ((a_min / a_max) * v1) - v0) / dt;
788
789 if (traj.a1_ <= accel_epsilon_ && traj.a1_ >= -accel_epsilon_)
790 {
791 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CASE3: Received traj.a1_ near zero...");
792 traj.t1_ = traj.t0_ + (dt * (a_max / (a_min + a_max)));
793 traj.x1_ = traj.x0_ + (v_hat * (traj.t1_ - traj.t0_));
794 }
795 else
796 {
797 traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
798 traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
799 }
800
801 traj.v2_ = v1;
802 traj.a2_ = traj.a1_ * a_max / a_min;
803 traj.t2_ = traj.t1_ + ((traj.v2_ - traj.v1_) / traj.a2_);
804 traj.x2_ = x_end;
805
806 traj.t3_ = traj.t2_;
807 traj.a3_ = 0;
808 traj.v3_ = traj.v2_;
809 traj.x3_ = traj.x2_;
810
811 return traj;
812}

References lci_strategic_plugin::TrajectoryParams::a1_, lci_strategic_plugin::TrajectoryParams::a2_, lci_strategic_plugin::TrajectoryParams::a3_, accel_epsilon_, epsilon_, lci_strategic_plugin::TrajectoryParams::t0_, lci_strategic_plugin::TrajectoryParams::t1_, lci_strategic_plugin::TrajectoryParams::t2_, lci_strategic_plugin::TrajectoryParams::t3_, carma_cooperative_perception::to_string(), lci_strategic_plugin::TrajectoryParams::v0_, lci_strategic_plugin::TrajectoryParams::v1_, lci_strategic_plugin::TrajectoryParams::v2_, lci_strategic_plugin::TrajectoryParams::v3_, lci_strategic_plugin::TrajectoryParams::x0_, lci_strategic_plugin::TrajectoryParams::x1_, lci_strategic_plugin::TrajectoryParams::x2_, and lci_strategic_plugin::TrajectoryParams::x3_.

Referenced by get_ts_case().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ ts_case4()

TrajectoryParams lci_strategic_plugin::LCIStrategicPlugin::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 
)
private

Definition at line 813 of file lci_strategic_plugin_algo.cpp.

814{
815 TrajectoryParams traj;
816
817 traj.t0_ = t;
818 traj.v0_ = v0;
819 traj.x0_ = x0;
820
821 double dt = et - t;
822 double nom1 = 2 * dx * ((((a_max / a_min) - 1) * v_min) + v1 - ((a_max / a_min) * v0));
823 double nom2 = dt * ((((a_max / a_min) - 1) * pow(v_min, 2)) + pow(v1, 2) - ((a_max / a_min) * pow(v0, 2)));
824 double den = ((a_max / a_min) * pow(v_min - v0, 2)) - pow(v_min - v1, 2);
825
826 if (den <= epsilon_ && den >= -epsilon_)
827 throw std::invalid_argument("CASE4: Received den near zero..." + std::to_string(den));
828
829 double tc = (nom1 - nom2) / den;
830
831 traj.v1_ = v_min;
832
833 if (dt - tc <= epsilon_ && dt - tc >= -epsilon_)
834 throw std::invalid_argument("CASE4: Received dt - tc near zero..." + std::to_string(dt - tc));
835
836 traj.a1_ = (((1 - (a_min / a_max)) * v_min) + ((a_min / a_max) * v1) - v0) / (dt - tc);
837
838 if (traj.a1_ <= accel_epsilon_ && traj.a1_ >= -accel_epsilon_)
839 {
840 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CASE4: Received traj.a1_ near zero...");
841 traj.t1_ = traj.t0_ + ((dt - tc) * (a_min / (a_min + a_max)));
842 traj.x1_ = traj.x0_ + (v_min * (traj.t1_ - traj.t0_));
843 }
844 else
845 {
846 traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
847 traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
848 }
849
850 traj.v2_ = v_min;
851 traj.a2_ = 0;
852 traj.t2_ = traj.t1_ + tc;
853 traj.x2_ = traj.x1_ + (v_min * tc);
854
855 traj.t3_ = et;
856 traj.a3_ = traj.a1_ * a_max / a_min;
857 traj.v3_ = v1;
858 traj.x3_ = x_end;
859
860 return traj;
861}

References lci_strategic_plugin::TrajectoryParams::a1_, lci_strategic_plugin::TrajectoryParams::a2_, lci_strategic_plugin::TrajectoryParams::a3_, accel_epsilon_, epsilon_, lci_strategic_plugin::TrajectoryParams::t0_, lci_strategic_plugin::TrajectoryParams::t1_, lci_strategic_plugin::TrajectoryParams::t2_, lci_strategic_plugin::TrajectoryParams::t3_, carma_cooperative_perception::to_string(), lci_strategic_plugin::TrajectoryParams::v0_, lci_strategic_plugin::TrajectoryParams::v1_, lci_strategic_plugin::TrajectoryParams::v2_, lci_strategic_plugin::TrajectoryParams::v3_, lci_strategic_plugin::TrajectoryParams::x0_, lci_strategic_plugin::TrajectoryParams::x1_, lci_strategic_plugin::TrajectoryParams::x2_, and lci_strategic_plugin::TrajectoryParams::x3_.

Referenced by get_ts_case().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ ts_case5()

TrajectoryParams lci_strategic_plugin::LCIStrategicPlugin::ts_case5 ( double  t,
double  et,
double  v0,
double  a_max,
double  a_min,
double  x0,
double  x_end,
double  dx 
)
private

Definition at line 863 of file lci_strategic_plugin_algo.cpp.

864{
865 TrajectoryParams traj;
866
867 traj.t0_ = t;
868 traj.v0_ = v0;
869 traj.x0_ = x0;
870
871 double dt = et - t;
872 double sqr = ((a_max / a_min) - 1) * ((2 * a_min * (dx - (v0 * dt))) - pow(a_min * dt, 2));
873 double v_hat = (v0 + (a_min * dt)) - (sqrt(sqr) / ((a_max / a_min) - 1));
874 double v_p = ((1 - (a_max / a_min)) * v_hat) + ((a_max / a_min) * v0) + (a_max * dt);
875
876 traj.v1_ = v_hat;
877 traj.a1_ = a_min;
878
879 traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
880 traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
881
882 traj.v2_ = v_p;
883 traj.a2_ = a_max;
884 traj.t2_ = traj.t1_ + ((traj.v2_ - traj.v1_) / traj.a2_);
885 traj.x2_ = x_end;
886
887 traj.t3_ = traj.t2_;
888 traj.a3_ = 0;
889 traj.v3_ = traj.v2_;
890 traj.x3_ = traj.x2_;
891
892 return traj;
893}

References lci_strategic_plugin::TrajectoryParams::a1_, lci_strategic_plugin::TrajectoryParams::a2_, lci_strategic_plugin::TrajectoryParams::a3_, lci_strategic_plugin::TrajectoryParams::t0_, lci_strategic_plugin::TrajectoryParams::t1_, lci_strategic_plugin::TrajectoryParams::t2_, lci_strategic_plugin::TrajectoryParams::t3_, lci_strategic_plugin::TrajectoryParams::v0_, lci_strategic_plugin::TrajectoryParams::v1_, lci_strategic_plugin::TrajectoryParams::v2_, lci_strategic_plugin::TrajectoryParams::v3_, lci_strategic_plugin::TrajectoryParams::x0_, lci_strategic_plugin::TrajectoryParams::x1_, lci_strategic_plugin::TrajectoryParams::x2_, and lci_strategic_plugin::TrajectoryParams::x3_.

Referenced by get_ts_case().

Here is the caller graph for this function:

◆ ts_case6()

TrajectoryParams lci_strategic_plugin::LCIStrategicPlugin::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 
)
private

Definition at line 895 of file lci_strategic_plugin_algo.cpp.

896{
897 TrajectoryParams traj;
898
899 traj.t0_ = t;
900 traj.v0_ = v0;
901 traj.x0_ = x0;
902
903 double dt = et - t;
904
905 traj.v1_ = v_min;
906 traj.a1_ = a_min;
907 traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
908 traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
909
910 double tc;
911
912 if (dx <= dx3)
913 tc = 0;
914 else
915 tc = traj6.t2_ - traj6.t1_;
916
917 traj.v2_ = v_min;
918 traj.a2_ = 0;
919 traj.t2_ = traj.t1_ + tc;
920 traj.x2_ = traj.x1_ + (v_min * tc);
921
922 double dt_p = dt - (traj.t1_ - traj.t0_) - tc;
923
924 if (dt_p <= epsilon_ && dt_p >= -epsilon_)
925 throw std::invalid_argument("CASE6: Received dt_p near zero..." + std::to_string(dt_p));
926
927 double v_p = ((2 * a_min * (dx - (v_min * tc))) - (pow(v_min, 2) - pow(v0, 2)) - (v_min * dt_p * a_min)) / (dt_p * a_min);
928
929 traj.v3_ = v_p;
930 traj.a3_ = (v_p - v_min) / dt_p;
931 traj.t3_ = et;
932 traj.x3_ = x_end;
933
934 return traj;
935}

References lci_strategic_plugin::TrajectoryParams::a1_, lci_strategic_plugin::TrajectoryParams::a2_, lci_strategic_plugin::TrajectoryParams::a3_, epsilon_, lci_strategic_plugin::TrajectoryParams::t0_, lci_strategic_plugin::TrajectoryParams::t1_, lci_strategic_plugin::TrajectoryParams::t2_, lci_strategic_plugin::TrajectoryParams::t3_, carma_cooperative_perception::to_string(), lci_strategic_plugin::TrajectoryParams::v0_, lci_strategic_plugin::TrajectoryParams::v1_, lci_strategic_plugin::TrajectoryParams::v2_, lci_strategic_plugin::TrajectoryParams::v3_, lci_strategic_plugin::TrajectoryParams::x0_, lci_strategic_plugin::TrajectoryParams::x1_, lci_strategic_plugin::TrajectoryParams::x2_, and lci_strategic_plugin::TrajectoryParams::x3_.

Referenced by get_ts_case().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ ts_case7()

TrajectoryParams lci_strategic_plugin::LCIStrategicPlugin::ts_case7 ( double  t,
double  et,
double  v0,
double  v_min,
double  a_min,
double  x0,
double  x_end,
double  dx 
)
private

Definition at line 937 of file lci_strategic_plugin_algo.cpp.

938{
939 TrajectoryParams traj;
940
941 traj.t0_ = t;
942 traj.v0_ = v0;
943 traj.x0_ = x0;
944
945 traj.v1_ = v_min;
946 traj.a1_ = a_min;
947 traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
948 traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
949
950 double dt = et - t;
951 double v_p = v_min - sqrt(pow(v_min - v0, 2) - (2 * a_min * ((v_min * dt) - dx)));
952 double dt_p = (v_p - v_min) / a_min;
953
954 if (dt_p <= epsilon_ && dt_p >= -epsilon_)
955 throw std::invalid_argument("CASE7: Received dt_p near zero..." + std::to_string(dt_p));
956
957 double tc = dt - ((v_p - v0) / a_min);
958
959 traj.v2_ = v_min;
960 traj.a2_ = 0;
961 traj.t2_ = traj.t1_ + tc;
962 traj.x2_ = traj.x1_ + (v_min * tc);
963
964 traj.v3_ = v_p;
965 traj.a3_ = (v_p - v_min) / dt_p;
966 traj.t3_ = et;
967 traj.x3_ = x_end;
968
969 return traj;
970}

References lci_strategic_plugin::TrajectoryParams::a1_, lci_strategic_plugin::TrajectoryParams::a2_, lci_strategic_plugin::TrajectoryParams::a3_, epsilon_, lci_strategic_plugin::TrajectoryParams::t0_, lci_strategic_plugin::TrajectoryParams::t1_, lci_strategic_plugin::TrajectoryParams::t2_, lci_strategic_plugin::TrajectoryParams::t3_, carma_cooperative_perception::to_string(), lci_strategic_plugin::TrajectoryParams::v0_, lci_strategic_plugin::TrajectoryParams::v1_, lci_strategic_plugin::TrajectoryParams::v2_, lci_strategic_plugin::TrajectoryParams::v3_, lci_strategic_plugin::TrajectoryParams::x0_, lci_strategic_plugin::TrajectoryParams::x1_, lci_strategic_plugin::TrajectoryParams::x2_, and lci_strategic_plugin::TrajectoryParams::x3_.

Referenced by get_ts_case().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ ts_case8()

TrajectoryParams lci_strategic_plugin::LCIStrategicPlugin::ts_case8 ( double  dx,
double  dx5,
TrajectoryParams  traj8 
)
private

Definition at line 972 of file lci_strategic_plugin_algo.cpp.

973{
974 TrajectoryParams traj = traj8;
975 if (dx < dx5)
976 {
977 traj.is_algorithm_successful = false;
978 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CASE8: Not within safe stopping distance originally planned!");
979 }
980 return traj;
981}

References lci_strategic_plugin::TrajectoryParams::is_algorithm_successful.

Referenced by get_ts_case().

Here is the caller graph for this function:

◆ validLightState()

bool lci_strategic_plugin::LCIStrategicPlugin::validLightState ( const boost::optional< std::pair< boost::posix_time::ptime, lanelet::CarmaTrafficSignalState > > &  optional_state,
const rclcpp::Time &  source_time 
) const
private

Helper method that checks both if the input optional light state is set and if the state it contains is supported via supportedLightState.

Parameters
optional_stateAn optional light state and its min_end_time pair. If this is unset the method will return false
source_timeThe time used to optain the optional light state. This is used for logging only
Returns
True if the optional is set and the contained state signal is supported. False otherwise

Definition at line 300 of file lci_strategic_plugin.cpp.

302{
303 if (!optional_state)
304 {
305 RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"),"Traffic light data not available for source_time " << std::to_string(source_time.seconds()));
306 return false;
307 }
308
309 lanelet::CarmaTrafficSignalState light_state = optional_state.get().second;
310
311 if (!supportedLightState(light_state))
312 {
313 RCLCPP_ERROR_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "LCIStrategic Plugin asked to handle CarmaTrafficSignalState: " << light_state
314 << " which is not supported.");
315 return false;
316 }
317
318 return true;
319}
bool supportedLightState(lanelet::CarmaTrafficSignalState state) const
Helper method to evaluate if the given traffic light state is supported by this plugin.

References supportedLightState(), and carma_cooperative_perception::to_string().

Referenced by canArriveAtGreenWithCertainty(), get_final_entry_time_and_conditions(), handleFailureCaseHelper(), planWhenAPPROACHING(), and planWhenWAITING().

Here is the call graph for this function:
Here is the caller graph for this function:

Member Data Documentation

◆ accel_epsilon_

double lci_strategic_plugin::LCIStrategicPlugin::accel_epsilon_ = 0.0001
private

◆ approaching_light_controlled_intersection_

bool lci_strategic_plugin::LCIStrategicPlugin::approaching_light_controlled_intersection_ = false
private

◆ bsm_id_

std::string lci_strategic_plugin::LCIStrategicPlugin::bsm_id_ = "default_bsm_id"
private

Definition at line 209 of file lci_strategic_plugin.hpp.

Referenced by BSMCb(), and generateMobilityOperation().

◆ bsm_msg_count_

uint8_t lci_strategic_plugin::LCIStrategicPlugin::bsm_msg_count_ = 0
private

Definition at line 210 of file lci_strategic_plugin.hpp.

Referenced by BSMCb(), and generateMobilityOperation().

◆ bsm_sec_mark_

uint16_t lci_strategic_plugin::LCIStrategicPlugin::bsm_sec_mark_ = 0
private

Definition at line 211 of file lci_strategic_plugin.hpp.

Referenced by BSMCb(), and generateMobilityOperation().

◆ bsm_sub_

carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::BSM> lci_strategic_plugin::LCIStrategicPlugin::bsm_sub_
private

Definition at line 265 of file lci_strategic_plugin.hpp.

Referenced by on_configure_plugin().

◆ case_pub_

carma_ros2_utils::PubPtr<std_msgs::msg::Int8> lci_strategic_plugin::LCIStrategicPlugin::case_pub_
private

◆ config_

◆ distance_remaining_to_tf_

double lci_strategic_plugin::LCIStrategicPlugin::distance_remaining_to_tf_ = 0.0
private

◆ earliest_entry_time_

double lci_strategic_plugin::LCIStrategicPlugin::earliest_entry_time_ = 0.0
private

◆ earliest_et_pub_

carma_ros2_utils::PubPtr<std_msgs::msg::Float64> lci_strategic_plugin::LCIStrategicPlugin::earliest_et_pub_
private

◆ emergency_decel_norm_

double lci_strategic_plugin::LCIStrategicPlugin::emergency_decel_norm_ = -2 * max_comfort_decel_
private

◆ epsilon_

◆ et_pub_

carma_ros2_utils::PubPtr<std_msgs::msg::Float64> lci_strategic_plugin::LCIStrategicPlugin::et_pub_
private

◆ frontbumper_transform_

tf2::Stamped<tf2::Transform> lci_strategic_plugin::LCIStrategicPlugin::frontbumper_transform_
private

Definition at line 250 of file lci_strategic_plugin.hpp.

◆ intersection_end_downtrack_

boost::optional<double> lci_strategic_plugin::LCIStrategicPlugin::intersection_end_downtrack_
private

◆ intersection_speed_

boost::optional<double> lci_strategic_plugin::LCIStrategicPlugin::intersection_speed_
private

Cache variables for storing the current intersection state between state machine transitions.

Definition at line 243 of file lci_strategic_plugin.hpp.

Referenced by handleFailureCase(), handleGreenSignalScenario(), plan_maneuvers_callback(), planWhenAPPROACHING(), and planWhenUNAVAILABLE().

◆ intersection_turn_direction_

TurnDirection lci_strategic_plugin::LCIStrategicPlugin::intersection_turn_direction_ = TurnDirection::Straight
private

Definition at line 233 of file lci_strategic_plugin.hpp.

Referenced by generateMobilityOperation(), and planWhenAPPROACHING().

◆ last_case_num_

TSCase lci_strategic_plugin::LCIStrategicPlugin::last_case_num_ = TSCase::UNAVAILABLE
private

Useful metrics for LCI Plugin.

Parameters
last_case_num_Current speed profile case generated
distance_remaining_to_tf_distance_remaining_to_traffic signal meters
earliest_entry_time_earliest_entry_time in sec
scheduled_entry_time_scheduled_entry_time in sec

Definition at line 228 of file lci_strategic_plugin.hpp.

Referenced by handleFailureCase(), handleStopping(), planWhenAPPROACHING(), planWhenDEPARTING(), planWhenUNAVAILABLE(), planWhenWAITING(), and publishTrajectorySmoothingInfo().

◆ length_to_front_bumper_

double lci_strategic_plugin::LCIStrategicPlugin::length_to_front_bumper_ = 3.0
private

Definition at line 251 of file lci_strategic_plugin.hpp.

Referenced by lookupFrontBumperTransform().

◆ light_controlled_intersection_strategy_

std::string lci_strategic_plugin::LCIStrategicPlugin::light_controlled_intersection_strategy_ = "Carma/signalized_intersection"
private

◆ max_comfort_accel_

double lci_strategic_plugin::LCIStrategicPlugin::max_comfort_accel_ = 2.0
private

◆ max_comfort_decel_

double lci_strategic_plugin::LCIStrategicPlugin::max_comfort_decel_ = -2.0
private

◆ max_comfort_decel_norm_

double lci_strategic_plugin::LCIStrategicPlugin::max_comfort_decel_norm_ = -1 * max_comfort_decel_
private

◆ mob_op_pub_timer_

rclcpp::TimerBase::SharedPtr lci_strategic_plugin::LCIStrategicPlugin::mob_op_pub_timer_
private

Definition at line 260 of file lci_strategic_plugin.hpp.

Referenced by on_activate_plugin().

◆ mob_operation_sub_

carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::MobilityOperation> lci_strategic_plugin::LCIStrategicPlugin::mob_operation_sub_
private

Definition at line 264 of file lci_strategic_plugin.hpp.

Referenced by on_configure_plugin().

◆ mobility_operation_pub_

carma_ros2_utils::PubPtr<carma_v2x_msgs::msg::MobilityOperation> lci_strategic_plugin::LCIStrategicPlugin::mobility_operation_pub_
private

Definition at line 268 of file lci_strategic_plugin.hpp.

Referenced by on_configure_plugin(), and publishMobilityOperation().

◆ nearest_green_entry_time_cached_

boost::optional<rclcpp::Time> lci_strategic_plugin::LCIStrategicPlugin::nearest_green_entry_time_cached_
private

◆ previous_strategy_params_

std::string lci_strategic_plugin::LCIStrategicPlugin::previous_strategy_params_ = ""
private

Definition at line 205 of file lci_strategic_plugin.hpp.

Referenced by mobilityOperationCb().

◆ scheduled_enter_time_

unsigned long long lci_strategic_plugin::LCIStrategicPlugin::scheduled_enter_time_ = 0
private

◆ scheduled_entry_time_

double lci_strategic_plugin::LCIStrategicPlugin::scheduled_entry_time_ = 0.0
private

◆ street_msg_timestamp_

unsigned long long lci_strategic_plugin::LCIStrategicPlugin::street_msg_timestamp_ = 0
private

Definition at line 202 of file lci_strategic_plugin.hpp.

Referenced by mobilityOperationCb().

◆ tf2_buffer_

std::shared_ptr<tf2_ros::Buffer> lci_strategic_plugin::LCIStrategicPlugin::tf2_buffer_
private

Definition at line 248 of file lci_strategic_plugin.hpp.

Referenced by lookupFrontBumperTransform().

◆ tf2_listener_

std::shared_ptr<tf2_ros::TransformListener> lci_strategic_plugin::LCIStrategicPlugin::tf2_listener_
private

Definition at line 249 of file lci_strategic_plugin.hpp.

Referenced by lookupFrontBumperTransform().

◆ tf_distance_pub_

carma_ros2_utils::PubPtr<std_msgs::msg::Float64> lci_strategic_plugin::LCIStrategicPlugin::tf_distance_pub_
private

◆ transition_table_

LCIStrategicStateTransitionTable lci_strategic_plugin::LCIStrategicPlugin::transition_table_
private

State Machine Transition table.

Definition at line 240 of file lci_strategic_plugin.hpp.

Referenced by plan_maneuvers_callback(), planWhenAPPROACHING(), planWhenDEPARTING(), planWhenUNAVAILABLE(), and planWhenWAITING().

◆ ts_info_pub_timer_

rclcpp::TimerBase::SharedPtr lci_strategic_plugin::LCIStrategicPlugin::ts_info_pub_timer_
private

Definition at line 261 of file lci_strategic_plugin.hpp.

Referenced by on_activate_plugin().

◆ upcoming_id_

std::string lci_strategic_plugin::LCIStrategicPlugin::upcoming_id_ = ""
private

Definition at line 206 of file lci_strategic_plugin.hpp.

Referenced by generateMobilityOperation(), and plan_maneuvers_callback().

◆ wm_


The documentation for this class was generated from the following files: