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_.algo_minimum_speed = declare_parameter<double>("algo_minimum_speed", config_.algo_minimum_speed);
56 config_.deceleration_fraction = declare_parameter<double>("deceleration_fraction", config_.deceleration_fraction);
57 config_.desired_distance_to_stop_buffer = declare_parameter<double>("desired_distance_to_stop_buffer", config_.desired_distance_to_stop_buffer);
58 config_.min_maneuver_planning_period = declare_parameter<double>("min_maneuver_planning_period", config_.min_maneuver_planning_period);
59 config_.strategic_plugin_name = declare_parameter<std::string>("strategic_plugin_name", config_.strategic_plugin_name);
60 config_.lane_following_plugin_name = declare_parameter<std::string>("lane_following_plugin_name", config_.lane_following_plugin_name);
61 config_.stop_and_wait_plugin_name = declare_parameter<std::string>("stop_and_wait_plugin_name", config_.stop_and_wait_plugin_name);
62 config_.intersection_transit_plugin_name = declare_parameter<std::string>("intersection_transit_plugin_name", config_.intersection_transit_plugin_name);
63 config_.enable_carma_streets_connection = declare_parameter<bool>("enable_carma_streets_connection",config_.enable_carma_streets_connection);
64 config_.mobility_rate = declare_parameter<double>("mobility_rate", config_.mobility_rate);
66 declare_parameter<long>("enable_carma_wm_spat_processing",
68 config_.vehicle_id = declare_parameter<std::string>("vehicle_id", config_.vehicle_id);
69
74};
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_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, and lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_id.

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 1183 of file lci_strategic_plugin.cpp.

1184{
1185 std::vector<uint8_t> bsm_id_vec = msg->core_data.id;
1186 bsm_id_ = BSMHelper::BSMHelper::bsmIDtoString(bsm_id_vec);
1187 bsm_msg_count_ = msg->core_data.msg_count;
1188 bsm_sec_mark_ = msg->core_data.sec_mark;
1189}

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 307 of file lci_strategic_plugin.cpp.

308{
309 rclcpp::Time early_arrival_time_by_algo =
310 light_arrival_time_by_algo - rclcpp::Duration::from_nanoseconds(config_.green_light_time_buffer * 1e9);
311
312 rclcpp::Time late_arrival_time_by_algo =
313 light_arrival_time_by_algo + rclcpp::Duration::from_nanoseconds(config_.green_light_time_buffer * 1e9);
314
315 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "light_arrival_time_by_algo: " << std::to_string(light_arrival_time_by_algo.seconds()));
316 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "early_arrival_time_by_algo: " << std::to_string(early_arrival_time_by_algo.seconds()));
317 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "late_arrival_time_by_algo: " << std::to_string(late_arrival_time_by_algo.seconds()));
318
319 auto early_arrival_state_by_algo_optional = traffic_light->predictState(lanelet::time::timeFromSec(early_arrival_time_by_algo.seconds()));
320
321 if (!validLightState(early_arrival_state_by_algo_optional, early_arrival_time_by_algo))
322 return boost::none;
323
324 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "early_arrival_state_by_algo: " << early_arrival_state_by_algo_optional.get().second);
325
326 auto late_arrival_state_by_algo_optional = traffic_light->predictState(lanelet::time::timeFromSec(late_arrival_time_by_algo.seconds()));
327
328 if (!validLightState(late_arrival_state_by_algo_optional, late_arrival_time_by_algo))
329 return boost::none;
330
331 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "late_arrival_state_by_algo: " << late_arrival_state_by_algo_optional.get().second);
332
333 bool can_make_early_arrival = true;
334 bool can_make_late_arrival = true;
335
336 if (check_early)
337 can_make_early_arrival = isStateAllowedGreen(early_arrival_state_by_algo_optional.get().second);
338
339 if (check_late)
340 can_make_late_arrival = isStateAllowedGreen(late_arrival_state_by_algo_optional.get().second);
341
342 // We will cross the light on the green phase even if we arrive early or late
343 if (can_make_early_arrival && can_make_late_arrival) // Green light
344 return true;
345 else
346 return false;
347
348}
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 1518 of file lci_strategic_plugin.cpp.

1523{
1524 carma_planning_msgs::msg::Maneuver maneuver_msg;
1525 maneuver_msg.type = carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT;
1526 maneuver_msg.intersection_transit_straight_maneuver.parameters.negotiation_type =
1527 carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
1528 maneuver_msg.intersection_transit_straight_maneuver.parameters.presence_vector =
1529 carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN;
1530 maneuver_msg.intersection_transit_straight_maneuver.parameters.planning_tactical_plugin =
1532 maneuver_msg.intersection_transit_straight_maneuver.parameters.planning_strategic_plugin =
1534 maneuver_msg.intersection_transit_straight_maneuver.start_dist = start_dist;
1535 maneuver_msg.intersection_transit_straight_maneuver.start_speed = start_speed;
1536 maneuver_msg.intersection_transit_straight_maneuver.start_time = start_time;
1537 maneuver_msg.intersection_transit_straight_maneuver.end_dist = end_dist;
1538 maneuver_msg.intersection_transit_straight_maneuver.end_speed = target_speed;
1539 maneuver_msg.intersection_transit_straight_maneuver.end_time = end_time;
1540 maneuver_msg.intersection_transit_straight_maneuver.starting_lane_id = std::to_string(starting_lane_id);
1541 maneuver_msg.intersection_transit_straight_maneuver.ending_lane_id = std::to_string(ending_lane_id);
1542
1543 // Start time and end time for maneuver are assigned in updateTimeProgress
1544
1545 RCLCPP_INFO_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Creating IntersectionTransitManeuver start dist: " << start_dist << " end dist: " << end_dist
1546 << " From lanelet: " << starting_lane_id
1547 << " to lanelet: " << ending_lane_id
1548 << " From start_time: " << std::to_string(start_time.seconds())
1549 << " to end_time: " << std::to_string(end_time.seconds()));
1550
1551 return maneuver_msg;
1552}

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 1485 of file lci_strategic_plugin.cpp.

1490{
1491 carma_planning_msgs::msg::Maneuver maneuver_msg;
1492 maneuver_msg.type = carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT;
1493 maneuver_msg.stop_and_wait_maneuver.parameters.negotiation_type = carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
1494 maneuver_msg.stop_and_wait_maneuver.parameters.presence_vector =
1495 carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN | carma_planning_msgs::msg::ManeuverParameters::HAS_FLOAT_META_DATA;
1496 maneuver_msg.stop_and_wait_maneuver.parameters.planning_tactical_plugin = config_.stop_and_wait_plugin_name;
1497 maneuver_msg.stop_and_wait_maneuver.parameters.planning_strategic_plugin = config_.strategic_plugin_name;
1498 maneuver_msg.stop_and_wait_maneuver.start_dist = current_dist;
1499 maneuver_msg.stop_and_wait_maneuver.end_dist = end_dist;
1500 maneuver_msg.stop_and_wait_maneuver.start_speed = start_speed;
1501 maneuver_msg.stop_and_wait_maneuver.start_time = start_time;
1502 maneuver_msg.stop_and_wait_maneuver.end_time = end_time;
1503 maneuver_msg.stop_and_wait_maneuver.starting_lane_id = std::to_string(starting_lane_id);
1504 maneuver_msg.stop_and_wait_maneuver.ending_lane_id = std::to_string(ending_lane_id);
1505
1506 // Set the meta data for the stop location buffer
1507 maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(config_.stopping_location_buffer);
1508 maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(stopping_accel);
1509
1510 RCLCPP_INFO_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Creating stop and wait start dist: " << current_dist << " end dist: " << end_dist
1511 << " start_time: " << std::to_string(start_time.seconds())
1512 << " end_time: " << std::to_string(end_time.seconds())
1513 << " stopping_accel: " << stopping_accel);
1514
1515 return maneuver_msg;
1516}

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 1434 of file lci_strategic_plugin.cpp.

1437{
1438 carma_planning_msgs::msg::Maneuver maneuver_msg;
1439 maneuver_msg.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING;
1440 maneuver_msg.lane_following_maneuver.parameters.negotiation_type =
1441 carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
1442 maneuver_msg.lane_following_maneuver.parameters.presence_vector =
1443 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;
1444 maneuver_msg.lane_following_maneuver.parameters.planning_tactical_plugin =
1446 maneuver_msg.lane_following_maneuver.parameters.planning_strategic_plugin =
1448 maneuver_msg.lane_following_maneuver.start_dist = start_dist;
1449 maneuver_msg.lane_following_maneuver.start_speed = start_speed;
1450 maneuver_msg.lane_following_maneuver.start_time = start_time;
1451 maneuver_msg.lane_following_maneuver.end_dist = end_dist;
1452 maneuver_msg.lane_following_maneuver.end_speed = target_speed;
1453 maneuver_msg.lane_following_maneuver.end_time = end_time;
1454
1455 maneuver_msg.lane_following_maneuver.parameters.string_valued_meta_data.push_back(light_controlled_intersection_strategy_);
1456
1457 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.a1_);
1458 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.v1_);
1459 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.x1_);
1460
1461 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.a2_);
1462 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.v2_);
1463 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.x2_);
1464
1465 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.a3_);
1466 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.v3_);
1467 maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.x3_);
1468
1469 maneuver_msg.lane_following_maneuver.parameters.int_valued_meta_data.push_back(static_cast<int>(tsp.case_num));
1470 maneuver_msg.lane_following_maneuver.parameters.int_valued_meta_data.push_back(static_cast<int>(tsp.is_algorithm_successful));
1471
1472 for (auto llt : crossed_lanelets)
1473 {
1474 maneuver_msg.lane_following_maneuver.lane_ids.push_back(std::to_string(llt.id()));
1475 }
1476 // Start time and end time for maneuver are assigned in updateTimeProgress
1477
1478 RCLCPP_INFO_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Creating TrajectorySmoothingManeuver start dist: " << start_dist << " end dist: " << end_dist
1479 << " start_time: " << std::to_string(start_time.seconds())
1480 << " end_time: " << std::to_string(end_time.seconds()));
1481
1482 return maneuver_msg;
1483}

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 245 of file lci_strategic_plugin.cpp.

246{
247 VehicleState state;
248 if (!req->prior_plan.maneuvers.empty())
249 {
250 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Provided with initial plan...");
251 state.stamp = GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_time);
252 state.downtrack = GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_dist);
253 state.speed = GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_speed);
254 state.lane_id = getLaneletsBetweenWithException(state.downtrack, state.downtrack, true).front().id();
255 }
256 else
257 {
258 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "No initial plan provided...");
259
260 state.stamp = rclcpp::Time(req->header.stamp, RCL_SYSTEM_TIME);
261 state.downtrack = req->veh_downtrack;
262 state.speed = req->veh_logitudinal_velocity;
263 state.lane_id = stoi(req->veh_lane_id);
264 }
265 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "extractInitialState >>>> state.stamp: " << std::to_string(state.stamp.seconds()));
266 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "extractInitialState >>>> state.downtrack : " << state.downtrack );
267 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "extractInitialState >>>> state.speed: " << state.speed);
268 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "extractInitialState >>>> state.lane_id: " << state.lane_id);
269
270 return state;
271}
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 273 of file lci_strategic_plugin.cpp.

274{
275 lanelet::Optional<carma_wm::TrafficRulesConstPtr> traffic_rules = wm_->getTrafficRules();
276 if (traffic_rules)
277 {
278 return (*traffic_rules)->speedLimit(llt).speedLimit.value();
279 }
280 else
281 {
282 throw std::invalid_argument("Valid traffic rules object could not be built");
283 }
284}
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 1210 of file lci_strategic_plugin.cpp.

1211{
1212 carma_v2x_msgs::msg::MobilityOperation mo_;
1213 mo_.m_header.timestamp = now().nanoseconds()/1000000;
1214 mo_.m_header.sender_id = config_.vehicle_id;
1215 mo_.m_header.recipient_id = upcoming_id_;
1216 mo_.m_header.sender_bsm_id = bsm_id_;
1218
1219 double vehicle_acceleration_limit_ = config_.vehicle_accel_limit * config_.vehicle_accel_limit_multiplier;
1220 double vehicle_deceleration_limit_ = -1 * config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier;
1221
1222 std::string intersection_turn_direction = "straight";
1223 if (intersection_turn_direction_ == TurnDirection::Right) intersection_turn_direction = "right";
1224 if (intersection_turn_direction_ == TurnDirection::Left) intersection_turn_direction = "left";
1225
1226 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
1227 ", max_decel: " + std::to_string(vehicle_deceleration_limit_) + ", react_time: " + std::to_string(config_.reaction_time) +
1228 ", 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
1229 ", turn_direction: " + intersection_turn_direction + ", msg_count: " + std::to_string(bsm_msg_count_) + ", sec_mark: " + std::to_string(bsm_sec_mark_);
1230
1231
1232 return mo_;
1233}
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 1554 of file lci_strategic_plugin.cpp.

1555{
1556 return true;
1557}

◆ 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 1559 of file lci_strategic_plugin.cpp.

1560{
1561 return "v1.0";
1562}

◆ 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 355 of file lci_strategic_plugin.cpp.

359{
360 std::vector<lanelet::ConstLanelet> crossed_lanelets =
361 wm_->getLaneletsBetween(start_downtrack, end_downtrack, shortest_path_only, bounds_inclusive);
362
363 if (crossed_lanelets.size() == 0)
364 {
365 throw std::invalid_argument("getLaneletsBetweenWithException called but inputs do not cross any lanelets going "
366 "from: " +
367 std::to_string(start_downtrack) + " to: " + std::to_string(end_downtrack));
368 }
369
370 return crossed_lanelets;
371}

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 1235 of file lci_strategic_plugin.cpp.

1236{
1237 TurnDirection turn_direction = TurnDirection::Straight;
1238 for (auto l:lanelets_list)
1239 {
1240 if(l.hasAttribute("turn_direction")) {
1241 std::string direction_attribute = l.attribute("turn_direction").value();
1242 if (direction_attribute == "right")
1243 {
1244 turn_direction = TurnDirection::Right;
1245 break;
1246 }
1247 else if (direction_attribute == "left")
1248 {
1249 turn_direction = TurnDirection::Left;
1250 break;
1251 }
1252 else turn_direction = TurnDirection::Straight;
1253 }
1254 else
1255 {
1256 // if there is no attribute, assumption is straight
1257 turn_direction = TurnDirection::Straight;
1258 }
1259
1260 }
1261 return turn_direction;
1262}

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 447 of file lci_strategic_plugin.cpp.

453{
454 if (!ts_params.is_algorithm_successful || ts_params.case_num != TSCase::CASE_8)
455 {
456 RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"),"handleCruisingUntilStop is called but it is not case_8");
457 return;
458 }
459
460 auto new_ts_params = ts_params;
461
462 double decel_rate = std::fabs(ts_params.a3_);
463
464 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CASE_8: Planning cruise and stop with decel_rate: " << decel_rate);
465
466 new_ts_params.t3_ = new_ts_params.t2_;
467 new_ts_params.x3_ = new_ts_params.x2_;
468 new_ts_params.v3_ = new_ts_params.v2_;
469 new_ts_params.a3_ = new_ts_params.a2_;
470
471 // Identify the lanelets which will be crossed by approach maneuvers stopping part
472 std::vector<lanelet::ConstLanelet> lane_follow_crossed_lanelets =
473 getLaneletsBetweenWithException(new_ts_params.x1_, new_ts_params.x2_, true, true);
474
475 resp->new_plan.maneuvers.push_back(composeTrajectorySmoothingManeuverMessage(current_state.downtrack, new_ts_params.x2_, lane_follow_crossed_lanelets,
476 current_state_speed, new_ts_params.v2_, current_state.stamp, rclcpp::Time(new_ts_params.t2_ * 1e9), new_ts_params));
477
478 // Identify the lanelets which will be crossed by approach maneuvers stopping part
479 std::vector<lanelet::ConstLanelet> case_8_crossed_lanelets =
480 getLaneletsBetweenWithException(new_ts_params.x2_, traffic_light_down_track, true, true);
481
482 resp->new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage(
483 new_ts_params.x2_, traffic_light_down_track, new_ts_params.v2_, case_8_crossed_lanelets.front().id(),
484 case_8_crossed_lanelets.back().id(), rclcpp::Time(new_ts_params.t2_ * 1e9),
485 rclcpp::Time(new_ts_params.t2_ * 1e9) + rclcpp::Duration::from_nanoseconds(config_.min_maneuver_planning_period * 1e9), decel_rate));
486
487 return;
488}
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 408 of file lci_strategic_plugin.cpp.

417{
418 double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.downtrack;
419
420 // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver
421 std::vector<lanelet::ConstLanelet> crossed_lanelets =
422 getLaneletsBetweenWithException(current_state.downtrack, traffic_light_down_track, true, true);
423
424 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);
425
426 if (incomplete_traj_params.is_algorithm_successful == false)
427 {
428 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Failed to generate maneuver for edge cases...");
429 return;
430 }
431
432 resp->new_plan.maneuvers.push_back(composeTrajectorySmoothingManeuverMessage(current_state.downtrack, traffic_light_down_track, crossed_lanelets,
433 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));
434
435 double intersection_length = intersection_end_downtrack_.get() - traffic_light_down_track;
436
437 rclcpp::Time intersection_exit_time =
438 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);
439
440 resp->new_plan.maneuvers.push_back(composeIntersectionTransitMessage(
441 traffic_light_down_track, intersection_end_downtrack_.get(), intersection_speed_.get(),
442 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()));
443
445}
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 535 of file lci_strategic_plugin.cpp.

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

497{
498 if (!ts_params.is_algorithm_successful || ts_params.case_num == TSCase::CASE_8)
499 {
500 return;
501 }
502
503 rclcpp::Time light_arrival_time_by_algo = rclcpp::Time(ts_params.t3_ * 1e9);
504 double remaining_time = light_arrival_time_by_algo.seconds() - rclcpp::Time(req->header.stamp, RCL_SYSTEM_TIME).seconds();
505 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));
506 auto can_make_green_optional = canArriveAtGreenWithCertainty(light_arrival_time_by_algo, traffic_light);
507
508 // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver
509 std::vector<lanelet::ConstLanelet> crossed_lanelets =
510 getLaneletsBetweenWithException(current_state.downtrack, traffic_light_down_track, true, true);
511
512 // no change for maneuver if invalid light states
513 if (!can_make_green_optional)
514 return;
515
516 if (can_make_green_optional.get() || is_certainty_check_optional)
517 {
518 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");
519
520 resp->new_plan.maneuvers.push_back(composeTrajectorySmoothingManeuverMessage(current_state.downtrack, traffic_light_down_track, crossed_lanelets,
521 current_state_speed, ts_params.v3_, current_state.stamp, light_arrival_time_by_algo, ts_params));
522
523 double intersection_length = intersection_end_downtrack_.get() - traffic_light_down_track;
524
525 rclcpp::Time intersection_exit_time =
526 light_arrival_time_by_algo + rclcpp::Duration::from_nanoseconds(intersection_length / intersection_speed_.get() * 1e9);
527
528 resp->new_plan.maneuvers.push_back(composeIntersectionTransitMessage(
529 traffic_light_down_track, intersection_end_downtrack_.get(), intersection_speed_.get(),
530 intersection_speed_.get(), light_arrival_time_by_algo, intersection_exit_time, crossed_lanelets.back().id(), crossed_lanelets.back().id()));
531 }
532}
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 373 of file lci_strategic_plugin.cpp.

380{
381 double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.downtrack;
382
383 // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver
384 std::vector<lanelet::ConstLanelet> crossed_lanelets =
385 getLaneletsBetweenWithException(current_state.downtrack, traffic_light_down_track, true, true);
386
387 double decel_rate = max_comfort_decel_norm_; // Kinematic |(v_f - v_i) / t = a|
388
389 if (is_emergency)
390 {
391 decel_rate = emergency_decel_norm_;
393 }
394 else
395 {
397 }
398
399 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "HANDLE_STOPPING: Planning stop and wait maneuver at decel_rate: " << decel_rate);
400
401 resp->new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage(
402 current_state.downtrack, traffic_light_down_track, current_state.speed, crossed_lanelets.front().id(),
403 crossed_lanelets.back().id(), current_state.stamp,
404 current_state.stamp + rclcpp::Duration::from_nanoseconds(config_.min_maneuver_planning_period * 1e9), decel_rate));
405}

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 350 of file lci_strategic_plugin.cpp.

350 {
351 return state == lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED ||
352 state == lanelet::CarmaTrafficSignalState::PERMISSIVE_MOVEMENT_ALLOWED;
353}

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 228 of file lci_strategic_plugin.cpp.

229{
230 tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);
231 tf2_buffer_->setUsingDedicatedThread(true);
232 try
233 {
234 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
235 length_to_front_bumper_ = tf2.transform.translation.x;
236 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "length_to_front_bumper_: " << length_to_front_bumper_);
237
238 }
239 catch (const tf2::TransformException &ex)
240 {
241 RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"), ex.what());
242 }
243}
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 1164 of file lci_strategic_plugin.cpp.

1165{
1166 if (msg->strategy == light_controlled_intersection_strategy_)
1167 {
1168 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Received Schedule message with id: " << msg->m_header.plan_id);
1170 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Approaching light Controlled Intersection: " << approaching_light_controlled_intersection_);
1171
1172 if (msg->m_header.recipient_id == config_.vehicle_id)
1173 {
1174 street_msg_timestamp_ = msg->m_header.timestamp;
1175 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "street_msg_timestamp_: " << street_msg_timestamp_);
1176 parseStrategyParams(msg->strategy_params);
1177 previous_strategy_params_ = msg->strategy_params;
1178 }
1179 }
1180
1181}
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 187 of file lci_strategic_plugin.cpp.

188{
189 mob_op_pub_timer_ = create_timer(get_clock(),
190 std::chrono::duration<double>(1/config_.mobility_rate),
192
193 ts_info_pub_timer_ = create_timer(get_clock(),
194 std::chrono::duration<double>(0.5),
196
197 return CallbackReturn::SUCCESS;
198}
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 76 of file lci_strategic_plugin.cpp.

77{
78 // reset config
79 config_ = LCIStrategicPluginConfig();
80
81 // clang-format off
82 get_parameter<double>("vehicle_acceleration_limit", config_.vehicle_accel_limit);
83 get_parameter<double>("vehicle_deceleration_limit", config_.vehicle_decel_limit);
84 get_parameter<double>("vehicle_decel_limit_multiplier", config_.vehicle_decel_limit_multiplier);
85 get_parameter<double>("vehicle_accel_limit_multiplier", config_.vehicle_accel_limit_multiplier);
86 get_parameter<double>("min_approach_distance", config_.min_approach_distance);
87 get_parameter<double>("trajectory_smoothing_activation_distance", config_.trajectory_smoothing_activation_distance);
88 get_parameter<double>("stopping_location_buffer", config_.stopping_location_buffer);
89 get_parameter<double>("green_light_time_buffer", config_.green_light_time_buffer);
90 get_parameter<double>("algo_minimum_speed", config_.algo_minimum_speed);
91 get_parameter<double>("deceleration_fraction", config_.deceleration_fraction);
92 get_parameter<double>("desired_distance_to_stop_buffer", config_.desired_distance_to_stop_buffer);
93 get_parameter<double>("min_maneuver_planning_period", config_.min_maneuver_planning_period);
94 get_parameter<std::string>("strategic_plugin_name", config_.strategic_plugin_name);
95 get_parameter<std::string>("lane_following_plugin_name", config_.lane_following_plugin_name);
96 get_parameter<std::string>("stop_and_wait_plugin_name", config_.stop_and_wait_plugin_name);
97 get_parameter<std::string>("intersection_transit_plugin_name", config_.intersection_transit_plugin_name);
98 get_parameter<bool>("enable_carma_streets_connection", config_.enable_carma_streets_connection);
99 get_parameter<int>("enable_carma_wm_spat_processing", config_.enable_carma_wm_spat_processing);
100 get_parameter<double>("mobility_rate", config_.mobility_rate);
101 get_parameter<std::string>("vehicle_id", config_.vehicle_id);
102
107
108 // clang-format on
109
110 // Register runtime parameter update callback
111 add_on_set_parameters_callback(std::bind(&LCIStrategicPlugin::parameter_update_callback, this, std_ph::_1));
112
113 RCLCPP_INFO_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Done loading parameters: " << config_);
114
116
117 // Mobility Operation Subscriber
118 mob_operation_sub_ = create_subscription<carma_v2x_msgs::msg::MobilityOperation>("incoming_mobility_operation", 1,
119 std::bind(&LCIStrategicPlugin::mobilityOperationCb,this,std_ph::_1));
120
121 // BSM subscriber
122 bsm_sub_ = create_subscription<carma_v2x_msgs::msg::BSM>("bsm_outbound", 1,
123 std::bind(&LCIStrategicPlugin::BSMCb,this,std_ph::_1));
124
125 // set world model point from wm listener
127
128 // Activate SPAT processor, which is turned off by default,
129 // with OFF (0), ON (1)
130 get_world_model_listener()->setWMSpatProcessingState(
132
133 // Setup publishers
134 mobility_operation_pub_ = create_publisher<carma_v2x_msgs::msg::MobilityOperation>("outgoing_mobility_operation", 1);
135 case_pub_ = create_publisher<std_msgs::msg::Int8>("lci_strategic_plugin/ts_case_num", 1);
136 tf_distance_pub_ = create_publisher<std_msgs::msg::Float64>("lci_strategic_plugin/distance_remaining_to_tf", 1);
137 earliest_et_pub_ = create_publisher<std_msgs::msg::Float64>("lci_strategic_plugin/earliest_entry_time", 1);
138 et_pub_ = create_publisher<std_msgs::msg::Float64>("lci_strategic_plugin/scheduled_entry_time", 1);
139
140 // Return success if everything initialized successfully
141 return CallbackReturn::SUCCESS;
142}
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, 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 144 of file lci_strategic_plugin.cpp.

145{
146 auto error_double = update_params<double>({
147 {"vehicle_acceleration_limit", config_.vehicle_accel_limit},
148 {"vehicle_deceleration_limit", config_.vehicle_decel_limit},
149 {"vehicle_decel_limit_multiplier", config_.vehicle_decel_limit_multiplier},
150 {"vehicle_accel_limit_multiplier", config_.vehicle_accel_limit_multiplier},
151 {"min_approach_distance", config_.min_approach_distance},
152 {"trajectory_smoothing_activation_distance", config_.trajectory_smoothing_activation_distance},
153 {"stopping_location_buffer", config_.stopping_location_buffer},
154 {"green_light_time_buffer", config_.green_light_time_buffer},
155 {"algo_minimum_speed", config_.algo_minimum_speed},
156 {"deceleration_fraction", config_.deceleration_fraction},
157 {"desired_distance_to_stop_buffer", config_.desired_distance_to_stop_buffer},
158 {"min_maneuver_planning_period", config_.min_maneuver_planning_period},
159 {"mobility_rate", config_.mobility_rate},
160 }, parameters);
161
162 rcl_interfaces::msg::SetParametersResult result;
163
164 result.successful = !error_double;
165
166 return result;
167}

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, and lci_strategic_plugin::LCIStrategicPluginConfig::vehicle_decel_limit_multiplier.

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 1191 of file lci_strategic_plugin.cpp.

1192{
1193 // sample strategy_params: "et:1634067059"
1194 std::string params = strategy_params;
1195 std::vector<std::string> inputsParams;
1196 boost::algorithm::split(inputsParams, params, boost::is_any_of(","));
1197
1198 std::vector<std::string> et_parsed;
1199 boost::algorithm::split(et_parsed, inputsParams[0], boost::is_any_of(":"));
1200 auto new_scheduled_enter_time = std::stoull(et_parsed[1]);
1201
1202 if (scheduled_enter_time_ != new_scheduled_enter_time) //reset green buffer cache so it can be re-evaluated
1204
1205 scheduled_enter_time_ = new_scheduled_enter_time;
1206 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "scheduled_enter_time_: " << scheduled_enter_time_);
1207
1208}

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 1275 of file lci_strategic_plugin.cpp.

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

829{
831
832 if (!traffic_light) // If we are in the approaching state and there is no longer any lights ahead of us then
833 // the vehicle must have crossed the stop bar
834 {
836 return;
837 }
838
839 double current_state_speed = std::max(current_state.speed, config_.algo_minimum_speed * 1.001);
840
841 auto stop_line = traffic_light->getStopLine(entry_lanelet);
842
843 if (!stop_line)
844 {
845 throw std::invalid_argument("Given entry lanelet doesn't have stop_line...");
846 }
847
848 double traffic_light_down_track =
849 wm_->routeTrackPos(stop_line.get().front().basicPoint2d()).downtrack;
850
851 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "traffic_light_down_track: " << traffic_light_down_track);
852
853 double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.downtrack;
854
855 distance_remaining_to_tf_ = distance_remaining_to_traffic_light;
856
857 if (distance_remaining_to_traffic_light < 0) // there is small discrepancy between signal's routeTrackPos as well as current
858 { // downtrack calculated using veh_x,y from state. Therefore, it may have crossed it
859 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Crossed the bar distance_remaining_to_traffic_light: " << distance_remaining_to_traffic_light);
861 return;
862 }
863
864 // At this point we know the vehicle is within the activation distance and we know the current and next light phases
865 // All we need to now determine is if we should stop or if we should continue
866 lanelet::ConstLanelet intersection_lanelet;
867
868 auto route = wm_->getRoute()->shortestPath();
869 auto entry_llt_iter = std::find(route.begin(), route.end(), entry_lanelet);
870 if (entry_llt_iter != route.end())
871 {
872 intersection_lanelet = *(entry_llt_iter + 1);
873 }
874
875 if (intersection_lanelet.id() != lanelet::InvalId)
876 {
877 intersection_speed_ = findSpeedLimit(intersection_lanelet);
879 }
880 else
881 {
882 intersection_speed_ = findSpeedLimit(exit_lanelet);
883 }
884
885 intersection_speed_ = intersection_speed_.get() * 0.999; // so that speed_limit is not exactly same as departure or current speed
886
887 double speed_limit = findSpeedLimit(current_lanelet);
888
889 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
890
891 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "current_state_speed: " << current_state_speed);
892 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "intersection_speed_: " << intersection_speed_.get());
893 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "distance_remaining_to_traffic_light: " << distance_remaining_to_traffic_light);
894
896 wm_->routeTrackPos(exit_lanelet.centerline2d().front().basicPoint2d()).downtrack;
897
898 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "intersection_end_downtrack_: " << intersection_end_downtrack_.get());
899
900 // If the vehicle is at a stop trigger the stopped state
901 constexpr double HALF_MPH_IN_MPS = 0.22352;
902 if (current_state.speed < HALF_MPH_IN_MPS &&
903 fabs(distance_remaining_to_traffic_light) < config_.stopping_location_buffer)
904 {
905 transition_table_.signal(TransitEvent::STOPPED); // The vehicle has come to a stop at the light
906 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CARMA has detected that the vehicle stopped at the stop bar. Transitioning to WAITING STATE");
907
908 return;
909 }
910
912
913 rclcpp::Time earliest_entry_time = current_state.stamp + get_earliest_entry_time(distance_remaining_to_traffic_light, speed_limit,
914 current_state_speed, intersection_speed_.get(), max_comfort_accel_, max_comfort_decel_);
915
916 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()));
917
918 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);
919
920 if (nearest_green_entry_time == rclcpp::Time(0))
921 return;
922
923 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Final nearest_green_entry_time: " << std::to_string(nearest_green_entry_time.seconds()));
924
925 auto et_state = traffic_light->predictState(lanelet::time::timeFromSec(nearest_green_entry_time.seconds()));
926 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Signal at ET: " << et_state.get().second);
927
929
930 double remaining_time = nearest_green_entry_time.seconds() - current_state.stamp.seconds();
931 double remaining_time_earliest_entry = earliest_entry_time.seconds() - current_state.stamp.seconds();
932 scheduled_entry_time_ = remaining_time; // performance metric
933 earliest_entry_time_ = remaining_time_earliest_entry; // performance metric
934
935 auto boundary_distances = get_delta_x(current_state_speed, intersection_speed_.get(), speed_limit, config_.algo_minimum_speed, max_comfort_accel_, max_comfort_decel_);
936 print_boundary_distances(boundary_distances); //debug
937
938 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);
939
940 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);
941 print_params(ts_params);
942
943 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "SPEED PROFILE CASE:" << ts_params.case_num);
944
946 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
947 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_);
948
949 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
950 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_);
951
952 double desired_distance_to_stop = pow(current_state.speed, 2)/(2 * max_comfort_decel_norm_ * config_.deceleration_fraction) + config_.desired_distance_to_stop_buffer;
953 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);
954
955 emergency_distance_to_stop = std::max(emergency_distance_to_stop, config_.stopping_location_buffer);
956 safe_distance_to_stop = std::max(safe_distance_to_stop, config_.stopping_location_buffer);
957 desired_distance_to_stop = std::max(desired_distance_to_stop, config_.stopping_location_buffer);
958
959 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "new emergency_distance_to_stop: " << emergency_distance_to_stop);
960 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "new safe_distance_to_stop: " << safe_distance_to_stop);
961 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "new desired_distance_to_stop: " << desired_distance_to_stop);
962
963 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);
964
965 // Basic RED signal violation check
966 if (distance_remaining_to_traffic_light <= emergency_distance_to_stop || last_case_num_ == TSCase::EMERGENCY_STOPPING)
967 {
968 if (in_tbd) // Given ET is in TBD, but vehicle is too close to intersection
969 {
970 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...");
971 }
972
973 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_
974
975 rclcpp::Time stopping_arrival_time =
976 current_state.stamp + rclcpp::Duration::from_nanoseconds(stopping_time * 1e9);
977
978 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "stopping_arrival_time: " << std::to_string(stopping_arrival_time.seconds()));
979
980 auto stopping_arrival_state_optional = traffic_light->predictState(lanelet::time::timeFromSec(stopping_arrival_time.seconds()));
981
982 if (!validLightState(stopping_arrival_state_optional, stopping_arrival_time))
983 {
984 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()));
985 return;
986 }
987
988 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
989 {
990 RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"),"Detected possible RED light violation! Stopping!");
991 handleStopping(req,resp, current_state, traffic_light, entry_lanelet, exit_lanelet, current_lanelet, traffic_light_down_track, true); //case_11
993
994 return;
995 }
996 }
997
998 // Check if the vehicle can arrive with certainty (Case 1-7)
999 if (ts_params.is_algorithm_successful && ts_params.case_num != TSCase::CASE_8 &&
1000 (distance_remaining_to_traffic_light >= desired_distance_to_stop || !in_tbd) &&
1001 is_entry_time_within_green_or_tbd) // ET cannot be explicitly inside RED or YELLOW in available future states, which is ERROR case
1002 {
1003 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
1004
1005 if (!resp->new_plan.maneuvers.empty()) // able to pass at green
1006 {
1007 last_case_num_ = ts_params.case_num;
1008 return;
1009 }
1010
1011 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");
1012 ts_params = boundary_traj_params[7];
1013 ts_params.is_algorithm_successful = true; //false correspond to cases when vehicle is beyond safe_distance to stop for case8
1014 ts_params.case_num = CASE_8;
1015 print_params(ts_params);
1016 }
1017
1018 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Not able to make it with certainty: NEW TSCase: " << ts_params.case_num);
1019 // if algorithm is NOT successful or if the vehicle cannot make the green light with certainty
1020
1021 if (desired_distance_to_stop < distance_remaining_to_traffic_light && last_case_num_ != TSCase::STOPPING) // do not switch from STOPPING to case8 again
1022 {
1023 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Way too early to stop. Cruising at CASE8");
1024 handleCruisingUntilStop(req, resp, current_state, current_state_speed, traffic_light, traffic_light_down_track, ts_params);
1025
1026 if (!resp->new_plan.maneuvers.empty())
1027 {
1029 return;
1030 }
1031 }
1032
1033 if ((safe_distance_to_stop <= distance_remaining_to_traffic_light && desired_distance_to_stop >= distance_remaining_to_traffic_light) ||
1034 last_case_num_ == TSCase::STOPPING) // if stopping continue stopping until transition to planwhenWAITING
1035 {
1036 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Planning stopping now. last case:" << static_cast<int>(last_case_num_));
1037 handleStopping(req,resp, current_state, traffic_light, entry_lanelet, exit_lanelet, current_lanelet, traffic_light_down_track); //case_9
1038 return;
1039 }
1040
1041 if (safe_distance_to_stop > distance_remaining_to_traffic_light)
1042 {
1043 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "No longer within safe distance to stop! Decide to continue stopping or continue into intersection");
1044
1045 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
1046 {
1047 // 3. if not able to stop nor reach target speed at green, attempt its best to reach the target parameters at the intersection
1048 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!");
1049
1050 handleFailureCase(req, resp, current_state, current_state_speed, speed_limit, remaining_time,
1051 exit_lanelet.id(), traffic_light, traffic_light_down_track, ts_params);
1052
1053 if (resp->new_plan.maneuvers.empty())
1054 {
1055 RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"),"HANDLE_SAFETY: Planning forced slow-down... last case:" << static_cast<int>(last_case_num_));
1056 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
1057 }
1058 }
1059 else
1060 {
1061 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "HANDLE_SAFETY: Planning to keep stopping now. last case:" << static_cast<int>(last_case_num_));
1062 handleStopping(req,resp, current_state, traffic_light, entry_lanelet, exit_lanelet, current_lanelet, traffic_light_down_track); //case_9
1063 }
1064 }
1065
1066
1067}
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 1135 of file lci_strategic_plugin.cpp.

1138{
1140
1141 if (current_state.downtrack > intersection_end_downtrack)
1142 {
1143 transition_table_.signal(TransitEvent::INTERSECTION_EXIT); // If we are past the most recent
1144 return;
1145 }
1146
1147 // Calculate exit time assuming constant acceleration
1148 rclcpp::Time intersection_exit_time =
1149 current_state.stamp +
1150 rclcpp::Duration::from_nanoseconds(2.0 * (intersection_end_downtrack - current_state.downtrack) / (current_state.speed + intersection_speed_limit) * 1e9);
1151
1152 // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver
1153 std::vector<lanelet::ConstLanelet> crossed_lanelets =
1154 getLaneletsBetweenWithException(current_state.downtrack, intersection_end_downtrack, true, false);
1155
1156 // Compose intersection transit maneuver
1157 resp->new_plan.maneuvers.push_back(composeIntersectionTransitMessage(
1158 current_state.downtrack, intersection_end_downtrack, current_state.speed, intersection_speed_limit,
1159 current_state.stamp, intersection_exit_time, crossed_lanelets.back().id(), crossed_lanelets.back().id()));
1160}

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 756 of file lci_strategic_plugin.cpp.

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

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

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 1264 of file lci_strategic_plugin.cpp.

1265{
1267 {
1268 carma_v2x_msgs::msg::MobilityOperation status_msg = generateMobilityOperation();
1269 mobility_operation_pub_->publish(status_msg);
1270 }
1271}
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 169 of file lci_strategic_plugin.cpp.

170{
171 std_msgs::msg::Int8 case_num_msg;
172 std_msgs::msg::Float64 tf_distance;
173 std_msgs::msg::Float64 earliest_et;
174 std_msgs::msg::Float64 scheduled_et;
175
176 case_num_msg.data = static_cast<int>(last_case_num_);
177 tf_distance.data = distance_remaining_to_tf_;
178 earliest_et.data = earliest_entry_time_;
179 scheduled_et.data = scheduled_entry_time_;
180
181 case_pub_->publish(case_num_msg);
182 tf_distance_pub_->publish(tf_distance);
183 earliest_et_pub_->publish(earliest_et);
184 et_pub_->publish(scheduled_et);
185}

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 200 of file lci_strategic_plugin.cpp.

201{
202 switch (state)
203 {
204 // NOTE: Following cases are intentional fall through.
205 // Supported light states
206 case lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN: // Solid Red
207 case lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE: // Yellow Solid no chance of conflicting traffic
208 case lanelet::CarmaTrafficSignalState::PERMISSIVE_MOVEMENT_ALLOWED: // Solid Green there could be conflict traffic
209 case lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED: // Solid Green no chance of conflict traffic
210 // (normally used with arrows)
211 return true;
212
213 // Unsupported light states
214 case lanelet::CarmaTrafficSignalState::PERMISSIVE_CLEARANCE: // Yellow Solid there is a chance of conflicting
215 // traffic
216 case lanelet::CarmaTrafficSignalState::UNAVAILABLE: // No data available
217 case lanelet::CarmaTrafficSignalState::DARK: // Light is non-functional
218 case lanelet::CarmaTrafficSignalState::STOP_THEN_PROCEED: // Flashing Red
219
220 case lanelet::CarmaTrafficSignalState::PRE_MOVEMENT: // Yellow Red transition (Found only in the EU)
221 // (normally used with arrows)
222 case lanelet::CarmaTrafficSignalState::CAUTION_CONFLICTING_TRAFFIC: // Yellow Flashing
223 default:
224 return false;
225 }
226}

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 286 of file lci_strategic_plugin.cpp.

288{
289 if (!optional_state)
290 {
291 RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"),"Traffic light data not available for source_time " << std::to_string(source_time.seconds()));
292 return false;
293 }
294
295 lanelet::CarmaTrafficSignalState light_state = optional_state.get().second;
296
297 if (!supportedLightState(light_state))
298 {
299 RCLCPP_ERROR_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "LCIStrategic Plugin asked to handle CarmaTrafficSignalState: " << light_state
300 << " which is not supported.");
301 return false;
302 }
303
304 return true;
305}
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: