#include <lci_strategic_plugin.hpp>

Inheritance diagram for lci_strategic_plugin::LCIStrategicPlugin:

Collaboration diagram for lci_strategic_plugin::LCIStrategicPlugin:

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::WMListener >	get_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< 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 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"

tf2_ros::Buffer	tf2_buffer_

std::unique_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 112 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.

  : carma_guidance_plugins::StrategicPlugin(options), tf2_buffer_(this->get_clock())
{
  config_ = LCIStrategicPluginConfig();
 
  config_.vehicle_accel_limit = declare_parameter<double>("vehicle_acceleration_limit", config_.vehicle_accel_limit);
  config_.vehicle_decel_limit = declare_parameter<double>("vehicle_deceleration_limit", config_.vehicle_decel_limit);
  config_.vehicle_decel_limit_multiplier = declare_parameter<double>("vehicle_decel_limit_multiplier", config_.vehicle_decel_limit_multiplier);
  config_.vehicle_accel_limit_multiplier = declare_parameter<double>("vehicle_accel_limit_multiplier",   config_.vehicle_accel_limit_multiplier);
  config_.min_approach_distance = declare_parameter<double>("min_approach_distance", config_.min_approach_distance);
  config_.trajectory_smoothing_activation_distance = declare_parameter<double>("trajectory_smoothing_activation_distance", config_.trajectory_smoothing_activation_distance);
  config_.stopping_location_buffer = declare_parameter<double>("stopping_location_buffer", config_.stopping_location_buffer);
  config_.green_light_time_buffer = declare_parameter<double>("green_light_time_buffer", config_.green_light_time_buffer);
  config_.algo_minimum_speed = declare_parameter<double>("algo_minimum_speed", config_.algo_minimum_speed);
  config_.deceleration_fraction = declare_parameter<double>("deceleration_fraction",  config_.deceleration_fraction);
  config_.desired_distance_to_stop_buffer = declare_parameter<double>("desired_distance_to_stop_buffer", config_.desired_distance_to_stop_buffer);
  config_.min_maneuver_planning_period = declare_parameter<double>("min_maneuver_planning_period", config_.min_maneuver_planning_period);
  config_.strategic_plugin_name = declare_parameter<std::string>("strategic_plugin_name",   config_.strategic_plugin_name);
  config_.lane_following_plugin_name = declare_parameter<std::string>("lane_following_plugin_name",  config_.lane_following_plugin_name);
  config_.stop_and_wait_plugin_name = declare_parameter<std::string>("stop_and_wait_plugin_name",  config_.stop_and_wait_plugin_name);
  config_.intersection_transit_plugin_name = declare_parameter<std::string>("intersection_transit_plugin_name", config_.intersection_transit_plugin_name);
  config_.enable_carma_streets_connection = declare_parameter<bool>("enable_carma_streets_connection",config_.enable_carma_streets_connection);
  config_.mobility_rate = declare_parameter<double>("mobility_rate", config_.mobility_rate);
  config_.vehicle_id = declare_parameter<std::string>("vehicle_id", config_.vehicle_id);
 
  max_comfort_accel_ = config_.vehicle_accel_limit * config_.vehicle_accel_limit_multiplier;
  max_comfort_decel_ = -1 * config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier;
  max_comfort_decel_norm_ = config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier;
  emergency_decel_norm_ = 2 * config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier;
};

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.

{
  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));
 
  TrajectoryParams traj;
 
  traj.t0_ = t;
  traj.v0_ = v0;
  traj.x0_ = x0;
 
  traj.v1_ = v_max;
  traj.a1_ = a_max;
  traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
  traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
 
  traj.v2_ = v_max;
  traj.a2_ = 0;
  traj.t2_ = t_end - ((v1 - v_max) / a_min);
  traj.x2_ = x_end - ((pow(v1, 2) - pow(v_max, 2)) / (2 * a_min));
 
  traj.t3_ = t_end;
  traj.a3_ = a_min;
  traj.v3_ = v1;
  traj.x3_ = x_end;
 
  return traj;
}

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.

{
  double nom = (v_max - v0) + ((a_max / a_min) * (v1 - v_max));
  double den = (pow(v_max, 2) - pow(v0, 2)) + ((a_max / a_min) * (pow(v1, 2) - pow(v_max, 2)));
 
  if (den <= epsilon_ && den >= -epsilon_)
    throw std::invalid_argument("boundary_accel_nocruise_maxspeed_decel: Received den near zero..." + std::to_string(den));
 
  double t_end = t + (2 * dx * nom / den);
 
  TrajectoryParams traj;
 
  traj.t0_ = t;
  traj.v0_ = v0;
  traj.x0_ = x0;
 
  double dt = t_end - t;
  double tc = 0;
 
  traj.v1_ = v_max;
 
  if (dt - tc <= epsilon_ && dt - tc >= -epsilon_)
    throw std::invalid_argument("boundary_accel_nocruise_maxspeed_decel: Received dt - tc near zero..." + std::to_string(dt - tc));
 
  traj.a1_ = (((1 - (a_max / a_min)) * v_max) + ((a_max / a_min) * v1) - v0) / (dt - tc);
 
  if (traj.a1_ <= accel_epsilon_ && traj.a1_ >= -accel_epsilon_)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "boundary_accel_nocruise_maxspeed_decel: Received traj.a1_ near zero...");
    traj.t1_ = traj.t0_ + (dt * (a_max / (a_min + a_max)));
    traj.x1_ = traj.x0_ + (v_max * (traj.t1_ - traj.t0_));
  }
  else
  {
    traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
    traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
  }
 
  traj.t2_ = t_end;
  traj.a2_ = ((((a_min / a_max) - 1) * v_max) + v1 - ((a_min / a_max) * v0)) / (dt - tc);
  traj.v2_ = v1;
  traj.x2_ = x_end;
 
  traj.t3_ = traj.t2_;
  traj.a3_ = 0;
  traj.v3_ = traj.v2_;
  traj.x3_ = traj.x2_;
 
  return traj;
}

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.

{
 
  double v_hat = sqrt(((2 * dx * a_max * a_min) + (a_min * pow(v0, 2)) - (a_max * pow(v1, 2))) / (a_min - a_max));
  double t_end = t + ((v_hat * (a_min - a_max)) - (v0 * a_min) + (v1 * a_max)) / (a_max * a_min);
 
  TrajectoryParams traj;
 
  traj.t0_ = t;
  traj.v0_ = v0;
  traj.x0_ = x0;
 
  traj.v1_ = v_hat;
  traj.a1_ = a_max;
  traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
 
  traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
 
  traj.t2_ = t_end;
  traj.a2_ = a_min;
  traj.v2_ = v1;
  traj.x2_ = x_end;
 
  traj.t3_ = traj.t2_;
  traj.a3_ = 0;
  traj.v3_ = traj.v2_;
  traj.x3_ = traj.x2_;
 
  return traj;
 
}

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.

{
  if (v0 + v1 <= epsilon_ && v0 + v1 >= -epsilon_)
    throw std::invalid_argument("boundary_accel_or_decel_complete_upper: Received v0 + v1 near zero..." + std::to_string(v0 + v1));
 
  double t_end = t + ((2 * dx) / (v0 + v1));
 
  TrajectoryParams traj;
 
  traj.t0_ = t;
  traj.v0_ = v0;
  traj.x0_ = x0;
 
  traj.t1_ = t_end;
 
  if (dx <= epsilon_ && dx >= -epsilon_)
    throw std::invalid_argument("boundary_accel_or_decel_complete_upper: Received dx near zero..." + std::to_string(dx));
 
  traj.a1_ = (pow(v1, 2) - pow(v0, 2)) / (2 * dx);
  traj.v1_ = v1;
  traj.x1_ = x_end;
 
  traj.t2_ = traj.t1_;
  traj.a2_ = 0;
  traj.v2_ = traj.v1_;
  traj.x2_ = traj.x1_;
 
  traj.t3_ = traj.t1_;
  traj.a3_ = 0;
  traj.v3_ = traj.v1_;
  traj.x3_ = traj.x1_;
 
  return traj;
}

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.

{
  double t_end;
 
  if (v0 <= v1 + epsilon_)
    t_end = t + (sqrt(pow(v0, 2) + (2 * a_max * dx)) - v0)/a_max;
  else
    t_end = t + (sqrt(pow(v0, 2) + (2 * a_min * dx)) - v0) / a_min;
 
  TrajectoryParams traj;
 
  traj.t0_ = t;
  traj.v0_ = v0;
  traj.x0_ = x0;
 
  traj.t1_ = t_end;
 
  if (v0 <= v1 + epsilon_)
  {
    traj.a1_ = a_max;
    traj.v1_ = sqrt(pow(v0, 2) + (2 * a_max * dx));
  }
  else
  {
    traj.a1_ = a_min;
    traj.v1_ = sqrt(pow(v0, 2) + (2 * a_min * dx));
  }
 
  traj.x1_ = x_end;
 
  traj.t2_ = traj.t1_;
  traj.a2_ = 0;
  traj.v2_ = traj.v1_;
  traj.x2_ = traj.x1_;
 
  traj.t3_ = traj.t1_;
  traj.a3_ = 0;
  traj.v3_ = traj.v1_;
  traj.x3_ = traj.x1_;
 
  return traj;
}

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.

{
  double t_end = t + (dx / v_min) + (pow(v_min - v0, 2) / (2 * a_min * v_min));
 
  TrajectoryParams traj;
 
  traj.t0_ = t;
  traj.v0_ = v0;
  traj.x0_ = x0;
 
  traj.v1_ = v_min;
  traj.a1_ = a_min;
  traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
  traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
 
  traj.v2_ = v_min;
  traj.a2_ = 0;
  traj.t2_ = t_end;
  traj.x2_ = x_end;
 
  traj.t3_ = traj.t2_;
  traj.a3_ = traj.a2_;
  traj.v3_ = traj.v2_;
  traj.x3_ = traj.x2_;
 
  return traj;
}

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.

{
  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));
 
  TrajectoryParams traj;
 
  traj.t0_ = t;
  traj.v0_ = v0;
  traj.x0_ = x0;
 
  traj.v1_ = v_min;
  traj.a1_ = a_min;
  traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
  traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
 
  traj.v2_ = v_min;
  traj.a2_ = 0;
  traj.t2_ = t_end - ((v1 - v_min) / a_max);
  traj.x2_ = x_end - ((pow(v1, 2) - pow(v_min, 2)) / (2 * a_max));
 
  traj.t3_ = t_end;
  traj.a3_ = a_max;
  traj.v3_ = v1;
  traj.x3_ = x_end;
 
  return traj;
}

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.

{
  double t_end = t + (dx / v_min) + (v0 * (v0 - (2 * v_min)) / (2 * a_min * v_min));
 
  TrajectoryParams traj;
 
  traj.t0_ = t;
  traj.v0_ = v0;
  traj.x0_ = x0;
 
  traj.v1_ = v_min;
  traj.a1_ = a_min;
  traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
  traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
 
  traj.a2_ = 0;
  traj.v2_ = v_min;
  traj.t2_ = t_end - ((0 - traj.v2_) / a_min);
  traj.x2_ = x_end - ((0 - pow(traj.v2_, 2)) / (2 * a_min));
 
  traj.t3_ = t_end;
  traj.a3_ = a_min;
  traj.v3_ = 0;
  traj.x3_ = x_end;
 
  return traj;
}

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.

{
  double t_end = t + (sqrt(pow(v0, 2) + (2 * a_min * dx)) - v0) / a_min;
 
  TrajectoryParams traj;
 
  traj.t0_ = t;
  traj.v0_ = v0;
  traj.x0_ = x0;
 
  traj.t1_ = t_end;
  traj.v1_ = sqrt(pow(v0, 2) + (2 * a_min * dx));
  traj.a1_ = a_min;
  traj.x1_ = x_end;
 
  traj.t2_ = traj.t1_;
  traj.a2_ = 0;
  traj.v2_ = traj.v1_;
  traj.x2_ = traj.x1_;
 
  traj.t3_ = traj.t1_;
  traj.a3_ = 0;
  traj.v3_ = traj.v1_;
  traj.x3_ = traj.x1_;
 
  return traj;
}

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.

{
  double nom = (v1 - v_min) + ((a_max / a_min) * (v_min - v0));
  double den = (pow(v1, 2) - pow(v_min, 2)) + ((a_max / a_min) * (pow(v_min, 2) - pow(v0, 2)));
 
  if (den <= epsilon_ && den >= -epsilon_)
    throw std::invalid_argument("boundary_decel_nocruise_minspeed_accel_complete: Received den near zero..." + std::to_string(den));
 
  double t_end = t + (2 * dx * nom / den);
  TrajectoryParams traj;
 
  traj.t0_ = t;
  traj.v0_ = v0;
  traj.x0_ = x0;
 
  double dt = t_end - t;
  double tc = 0;
 
  traj.v1_ = v_min;
 
  if (dt - tc <= epsilon_ && dt - tc >= -epsilon_)
    throw std::invalid_argument("boundary_decel_nocruise_minspeed_accel_complete: Received dt - tc near zero..." + std::to_string(dt - tc));
 
  traj.a1_ = (((1 - (a_min / a_max)) * v_min) + ((a_min / a_max) * v1) - v0) / (dt - tc);
  traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
  traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
 
  traj.t2_ = t_end;
  traj.a2_ = ((((a_max / a_min) - 1) * v_min) + v1 - ((a_max / a_min) * v0)) / (dt - tc);
  traj.v2_ = v1;
  traj.x2_ = x_end;
 
  traj.t3_ = traj.t2_;
  traj.a3_ = 0;
  traj.v3_ = traj.v2_;
  traj.x3_ = traj.x2_;
 
  return traj;
}

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.

{
  double sqr = sqrt((2 * a_max * dx) - ((pow(v_min, 2) - pow(v0, 2)) * (a_max / a_min)) + pow(v_min, 2));
 
  double t_end = t + ((sqr - v_min) / a_max) + ((v_min - v0) / a_min);
 
  TrajectoryParams traj;
 
  traj.t0_ = t;
  traj.v0_ = v0;
  traj.x0_ = x0;
 
  traj.v1_ = v_min;
  traj.a1_ = a_min;
  traj.t1_ = traj.t0_ + (traj.v1_ - traj.v0_) / a_min;
  traj.x1_ = traj.x0_ + (pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * a_min);
 
  traj.t2_ = t_end;
  traj.a2_ = a_max;
  traj.v2_ = (traj.a2_ * (traj.t2_ - traj.t1_)) + traj.v1_;
  traj.x2_ = x_end;
 
  traj.t3_ = traj.t2_;
  traj.a3_ = 0;
  traj.v3_ = traj.v2_;
  traj.x3_ = traj.x2_;
 
  return traj;
}

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.

{
  double v_hat = sqrt(((2 * dx * a_max * a_min) + (a_max * pow(v0, 2)) - (a_min * pow(v1, 2))) / (a_max - a_min));
  double t_end = t + ((v_hat * (a_max - a_min)) - (v0 * a_max) + (v1 * a_min)) / (a_max * a_min);
 
  TrajectoryParams traj;
 
  traj.t0_ = t;
  traj.v0_ = v0;
  traj.x0_ = x0;
 
  traj.v1_ = v_hat;
  traj.a1_ = a_min;
  traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
  traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
 
  traj.t2_ = t_end;
  traj.a2_ = a_max;
  traj.v2_ = v1;
  traj.x2_ = x_end;
 
  traj.t3_ = traj.t2_;
  traj.a3_ = 0;
  traj.v3_ = traj.v2_;
  traj.x3_ = traj.x2_;
 
  return traj;
}

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

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

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_dist	distance to stop line
current_speed	current speed of vehicle
departure_speed	speed 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.

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

◆ 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_algo	ROS time at green where vehicle arrives
traffic_light	Traffic signal to check time against
check_late	check late arrival, default true
check_early	check 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 298 of file lci_strategic_plugin.cpp.

{
    rclcpp::Time early_arrival_time_by_algo =
        light_arrival_time_by_algo - rclcpp::Duration(config_.green_light_time_buffer * 1e9);
 
    rclcpp::Time late_arrival_time_by_algo =
        light_arrival_time_by_algo + rclcpp::Duration(config_.green_light_time_buffer * 1e9);
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "light_arrival_time_by_algo: " << std::to_string(light_arrival_time_by_algo.seconds()));
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "early_arrival_time_by_algo: " << std::to_string(early_arrival_time_by_algo.seconds()));
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "late_arrival_time_by_algo: " << std::to_string(late_arrival_time_by_algo.seconds()));
 
    auto early_arrival_state_by_algo_optional = traffic_light->predictState(lanelet::time::timeFromSec(early_arrival_time_by_algo.seconds()));
 
    if (!validLightState(early_arrival_state_by_algo_optional, early_arrival_time_by_algo))
      return boost::none;
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "early_arrival_state_by_algo: " << early_arrival_state_by_algo_optional.get().second);
 
    auto late_arrival_state_by_algo_optional = traffic_light->predictState(lanelet::time::timeFromSec(late_arrival_time_by_algo.seconds()));
 
    if (!validLightState(late_arrival_state_by_algo_optional, late_arrival_time_by_algo))
      return boost::none;
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "late_arrival_state_by_algo: " << late_arrival_state_by_algo_optional.get().second);
 
    bool can_make_early_arrival = true;
    bool can_make_late_arrival = true;
 
    if (check_early)
      can_make_early_arrival = isStateAllowedGreen(early_arrival_state_by_algo_optional.get().second);
 
    if (check_late)
      can_make_late_arrival = isStateAllowedGreen(late_arrival_state_by_algo_optional.get().second);
 
    // We will cross the light on the green phase even if we arrive early or late
    if (can_make_early_arrival && can_make_late_arrival)  // Green light
      return true;
    else
      return false;
 
}

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_dist	Start downtrack distance of the current maneuver
end_dist	End downtrack distance of the current maneuver
start_speed	Start speed of the current maneuver
target_speed	Target speed pf the current maneuver, usually it is the lanelet speed limit
start_time	The starting time of the maneuver
end_time	The ending time of the maneuver
starting_lane_id	The starting lanelet id of this maneuver
ending_lane_id	The ending lanelet id of this maneuver

Returns: A intersection transit maneuver maneuver message which is ready to be published

Definition at line 1509 of file lci_strategic_plugin.cpp.

{
  carma_planning_msgs::msg::Maneuver maneuver_msg;
  maneuver_msg.type = carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT;
  maneuver_msg.intersection_transit_straight_maneuver.parameters.negotiation_type =
      carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
  maneuver_msg.intersection_transit_straight_maneuver.parameters.presence_vector =
      carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN;
  maneuver_msg.intersection_transit_straight_maneuver.parameters.planning_tactical_plugin =
      config_.intersection_transit_plugin_name;
  maneuver_msg.intersection_transit_straight_maneuver.parameters.planning_strategic_plugin =
      config_.strategic_plugin_name;
  maneuver_msg.intersection_transit_straight_maneuver.start_dist = start_dist;
  maneuver_msg.intersection_transit_straight_maneuver.start_speed = start_speed;
  maneuver_msg.intersection_transit_straight_maneuver.start_time = start_time;
  maneuver_msg.intersection_transit_straight_maneuver.end_dist = end_dist;
  maneuver_msg.intersection_transit_straight_maneuver.end_speed = target_speed;
  maneuver_msg.intersection_transit_straight_maneuver.end_time = end_time;
  maneuver_msg.intersection_transit_straight_maneuver.starting_lane_id = std::to_string(starting_lane_id);
  maneuver_msg.intersection_transit_straight_maneuver.ending_lane_id = std::to_string(ending_lane_id);
 
  // Start time and end time for maneuver are assigned in updateTimeProgress
 
  RCLCPP_INFO_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Creating IntersectionTransitManeuver start dist: " << start_dist << " end dist: " << end_dist
                                                                      << " From lanelet: " << starting_lane_id
                                                                      << " to lanelet: " << ending_lane_id
                                                                      << " From start_time: " << std::to_string(start_time.seconds())
                                                                      << " to end_time: " << std::to_string(end_time.seconds()));
 
  return maneuver_msg;
}

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_dist	Start downtrack distance of the current maneuver
end_dist	End downtrack distance of the current maneuver
start_speed	Start speed of the current maneuver
starting_lane_id	The starting lanelet id of this maneuver
ending_lane_id	The ending lanelet id of this maneuver
start_time	The starting time of the maneuver
end_time	The ending time of the maneuver
stopping_accel	Acceleration used for calculating the stopping distance

Returns: A stop and wait maneuver message which is ready to be published

Definition at line 1476 of file lci_strategic_plugin.cpp.

{
  carma_planning_msgs::msg::Maneuver maneuver_msg;
  maneuver_msg.type = carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT;
  maneuver_msg.stop_and_wait_maneuver.parameters.negotiation_type = carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
  maneuver_msg.stop_and_wait_maneuver.parameters.presence_vector =
      carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN | carma_planning_msgs::msg::ManeuverParameters::HAS_FLOAT_META_DATA;
  maneuver_msg.stop_and_wait_maneuver.parameters.planning_tactical_plugin = config_.stop_and_wait_plugin_name;
  maneuver_msg.stop_and_wait_maneuver.parameters.planning_strategic_plugin = config_.strategic_plugin_name;
  maneuver_msg.stop_and_wait_maneuver.start_dist = current_dist;
  maneuver_msg.stop_and_wait_maneuver.end_dist = end_dist;
  maneuver_msg.stop_and_wait_maneuver.start_speed = start_speed;
  maneuver_msg.stop_and_wait_maneuver.start_time = start_time;
  maneuver_msg.stop_and_wait_maneuver.end_time = end_time;
  maneuver_msg.stop_and_wait_maneuver.starting_lane_id = std::to_string(starting_lane_id);
  maneuver_msg.stop_and_wait_maneuver.ending_lane_id = std::to_string(ending_lane_id);
 
  // Set the meta data for the stop location buffer
  maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(config_.stopping_location_buffer);
  maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(stopping_accel);
 
  RCLCPP_INFO_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Creating stop and wait start dist: " << current_dist << " end dist: " << end_dist
                                                                        << " start_time: " << std::to_string(start_time.seconds())
                                                                        << " end_time: " << std::to_string(end_time.seconds())
                                                                        << " stopping_accel: " << stopping_accel);
 
  return maneuver_msg;
}

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_dist	Start downtrack distance of the current maneuver
end_dist	End downtrack distance of the current maneuver
start_speed	Start speed of the current maneuver
target_speed	Target speed pf the current maneuver, usually it is the lanelet speed limit
start_time	The starting time of the maneuver
end_time	The ending time of the maneuver
tsp	Trajectory Smoothing parameters needed to modify speed profile

Returns: A transift maneuver message specifically designed for light controlled intersection tactical plugin

Definition at line 1425 of file lci_strategic_plugin.cpp.

{
  carma_planning_msgs::msg::Maneuver maneuver_msg;
  maneuver_msg.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING;
  maneuver_msg.lane_following_maneuver.parameters.negotiation_type =
      carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
  maneuver_msg.lane_following_maneuver.parameters.presence_vector =
      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;
  maneuver_msg.lane_following_maneuver.parameters.planning_tactical_plugin =
      config_.lane_following_plugin_name;
  maneuver_msg.lane_following_maneuver.parameters.planning_strategic_plugin =
      config_.strategic_plugin_name;
  maneuver_msg.lane_following_maneuver.start_dist = start_dist;
  maneuver_msg.lane_following_maneuver.start_speed = start_speed;
  maneuver_msg.lane_following_maneuver.start_time = start_time;
  maneuver_msg.lane_following_maneuver.end_dist = end_dist;
  maneuver_msg.lane_following_maneuver.end_speed = target_speed;
  maneuver_msg.lane_following_maneuver.end_time = end_time;
 
  maneuver_msg.lane_following_maneuver.parameters.string_valued_meta_data.push_back(light_controlled_intersection_strategy_);
 
  maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.a1_);
  maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.v1_);
  maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.x1_);
 
  maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.a2_);
  maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.v2_);
  maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.x2_);
 
  maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.a3_);
  maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.v3_);
  maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.x3_);
 
  maneuver_msg.lane_following_maneuver.parameters.int_valued_meta_data.push_back(static_cast<int>(tsp.case_num));
  maneuver_msg.lane_following_maneuver.parameters.int_valued_meta_data.push_back(static_cast<int>(tsp.is_algorithm_successful));
 
  for (auto llt : crossed_lanelets)
  {
    maneuver_msg.lane_following_maneuver.lane_ids.push_back(std::to_string(llt.id()));
  }
  // Start time and end time for maneuver are assigned in updateTimeProgress
 
  RCLCPP_INFO_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Creating TrajectorySmoothingManeuver start dist: " << start_dist << " end dist: " << end_dist
                                                                      << " start_time: " << std::to_string(start_time.seconds())
                                                                      << " end_time: " << std::to_string(end_time.seconds()));
 
  return maneuver_msg;
}

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

v	The initial velocity in m/s
a	The deceleration in m/s^2

Returns: The estimated stopping distance in meters

Definition at line 24 of file lci_strategic_plugin_algo.cpp.

{
  return (v * v) / (2.0 * a);
}

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

d	The distance to travel in meters
v	The initial velocity in m/s

Returns: The estimated stopping time in seconds

Definition at line 29 of file lci_strategic_plugin_algo.cpp.

{
  return 2.0 * d / v;
};

◆ 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

req	The maneuver planning request to extract the vehicle state from

Returns: The extracted VehicleState

Definition at line 236 of file lci_strategic_plugin.cpp.

{
  VehicleState state;
  if (!req->prior_plan.maneuvers.empty())
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Provided with initial plan...");
    state.stamp = GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_time);
    state.downtrack = GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_dist);
    state.speed = GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_speed);
    state.lane_id = getLaneletsBetweenWithException(state.downtrack, state.downtrack, true).front().id();
  }
  else
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "No initial plan provided...");
 
    state.stamp = rclcpp::Time(req->header.stamp, RCL_SYSTEM_TIME);
    state.downtrack = req->veh_downtrack;
    state.speed = req->veh_logitudinal_velocity;
    state.lane_id = stoi(req->veh_lane_id);
  }
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "extractInitialState >>>> state.stamp: " << std::to_string(state.stamp.seconds()));
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "extractInitialState >>>> state.downtrack : " << state.downtrack );
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "extractInitialState >>>> state.speed: " << state.speed);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "extractInitialState >>>> state.lane_id: " << state.lane_id);
 
  return state;
}

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

llt	Constant Lanelet object

Exceptions

std::invalid_argument if the speed limit could not be retrieved

Returns: value of speed limit in mps

Definition at line 264 of file lci_strategic_plugin.cpp.

{
  lanelet::Optional<carma_wm::TrafficRulesConstPtr> traffic_rules = wm_->getTrafficRules();
  if (traffic_rules)
  {
    return (*traffic_rules)->speedLimit(llt).speedLimit.value();
  }
  else
  {
    throw std::invalid_argument("Valid traffic rules object could not be built");
  }
}

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

{
    carma_v2x_msgs::msg::MobilityOperation mo_;
    mo_.m_header.timestamp = now().nanoseconds()/1000000;
    mo_.m_header.sender_id = config_.vehicle_id;
    mo_.m_header.recipient_id = upcoming_id_;
    mo_.m_header.sender_bsm_id = bsm_id_;
    mo_.strategy = light_controlled_intersection_strategy_;
 
    double vehicle_acceleration_limit_ = config_.vehicle_accel_limit * config_.vehicle_accel_limit_multiplier;
    double vehicle_deceleration_limit_ = -1 * config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier;
 
    std::string intersection_turn_direction = "straight";
    if (intersection_turn_direction_ == TurnDirection::Right) intersection_turn_direction = "right";
    if (intersection_turn_direction_ == TurnDirection::Left) intersection_turn_direction = "left";
 
    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
                        ", max_decel: " + std::to_string(vehicle_deceleration_limit_) + ", react_time: " + std::to_string(config_.reaction_time) +
                        ", 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
                        ", turn_direction: " + intersection_turn_direction + ", msg_count: " + std::to_string(bsm_msg_count_) + ", sec_mark: " + std::to_string(bsm_sec_mark_);
 
 
    return mo_;
}

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

{
  return true;
}

◆ 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

t	current time in seconds
v0	starting_speed
v1	departure_speed
v_max	speed_limit
v_min	minimum speed
a_max	max_comfort_acceleration limit
a_min	max_comfort_deceleration limit
x0	current_downtrack
x_end	traffic_light_down_track
dx	distance_remaining_to_traffic_light
boundary_distances	boundary_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.

{
  double dx1 = boundary_distances.dx1;
  double dx2 = boundary_distances.dx2;
  double dx3 = boundary_distances.dx3;
  double dx4 = boundary_distances.dx4;
  double dx5 = boundary_distances.dx5;
  TrajectoryParams traj1, traj2, traj3, traj4, traj5, traj6, traj7, traj8;
 
  // t1, t2, t3
  if (dx < dx2)
  {
    traj1 = boundary_accel_or_decel_incomplete_upper(t, v0, v1, a_max, a_min, x0, x_end, dx);
    traj2 = traj1;
    traj3 = traj1;
  }
  else if (dx < dx1)
  {
    traj1 = boundary_accel_nocruise_notmaxspeed_decel(t, v0, v1, a_max, a_min, x0, x_end, dx);
    traj2 = traj1;
    traj3 = boundary_accel_or_decel_complete_upper(t, v0, v1, x0, x_end, dx);
  }
  else
  {
    traj1 = boundary_accel_cruise_maxspeed_decel(t, v0, v1, v_max, a_max, a_min, x0, x_end, dx);
    traj2 = boundary_accel_nocruise_maxspeed_decel(t, v0, v1, v_max, a_max, a_min, x0, x_end, dx);
    traj3 = boundary_accel_or_decel_complete_upper(t, v0, v1, x0, x_end, dx);
  }
  // t4, t5, t6, t7
  if (dx < dx4)
  {
    traj4 = traj1;
    traj5 = traj1;
    traj6 = boundary_decel_incomplete_lower(t, v0, a_min, x0, x_end, dx);
    traj7 = traj6;
  }
  else if (dx < dx3)
  {
    if (dx < dx2)
    {
      traj4 = traj1;
    }
    else
    {
      traj4 = boundary_decel_nocruise_notminspeed_accel(t, v0, v1, v_min, a_max, a_min, x0, x_end, dx);
    }
    traj5 = traj4;
    traj6 = boundary_decel_nocruise_minspeed_accel_incomplete(t, v0, v_min, a_max, a_min, x0, x_end, dx);
    traj7 = boundary_decel_cruise_minspeed(t, v0, v_min, a_min, x0, x_end, dx);
 
  }
  else
  {
    traj4 = boundary_decel_nocruise_minspeed_accel_complete(t, v0, v1, v_max, v_min, a_max, a_min, x0, x_end, dx);
    traj5 = boundary_decel_cruise_minspeed_accel(t, v0, v1, v_min, a_max, a_min, x0, x_end, dx);
    traj6 = traj5;
    traj7 = boundary_decel_cruise_minspeed(t, v0, v_min, a_min, x0, x_end, dx);
  }
  // t8
  if (dx < dx4)
  {
    traj8 = traj6;
  }
  else if (dx < dx5)
  {
    traj8 = boundary_decel_incomplete_lower(t, v0, a_min, x0, x_end, dx);
  }
  else
  {
    traj8 = boundary_decel_cruise_minspeed_decel(t, v0, v_min, a_min, x0, x_end, dx);
  }
 
  return {traj1, traj2, traj3, traj4, traj5, traj6, traj7, traj8};
}

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

v0	starting_speed
v1	departure_speed
v_max	speed_limit
v_min	minimum speed
a_max	max_comfort_acceleration limit
a_min	max_comfort_deceleration limit

Returns: boundary distances used to generate trajectory smoothing segments

Definition at line 462 of file lci_strategic_plugin_algo.cpp.

{
  BoundaryDistances distances;
 
  distances.dx1 = ((pow(v_max, 2) - pow(v0, 2)) / (2 * a_max)) + ((pow(v1, 2) - pow(v_max, 2)) / (2 * a_min));
  if (v1 > v0)
    distances.dx2 = ((pow(v1, 2) - pow(v0, 2)) / (2 * a_max));
  else
    distances.dx2 = ((pow(v1, 2) - pow(v0, 2)) / (2 * a_min));
 
  distances.dx3 = ((pow(v_min, 2) - pow(v0, 2)) / (2 * a_min)) + ((pow(v1, 2) - pow(v_min, 2)) / (2 * a_max));
  distances.dx4 = ((pow(v_min, 2) - pow(v0, 2)) / (2 * a_min));
  distances.dx5 = - pow(v0, 2) / (2 * a_min);
 
  return distances;
}

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_speed	Current speed in m/s
departure_speed	Speed to enter into the intersection in m/s
max_accel	Acceleration rate during acceleration segment in m/s^2
max_decel	Deceleration 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.

{
  if (current_speed <= departure_speed + epsilon_)
  {
    return (std::pow(departure_speed, 2) - std::pow(current_speed, 2))/(2 * max_accel);
  }
  else
  {
    return (std::pow(departure_speed, 2) - std::pow(current_speed, 2))/(2 * max_decel);
  }
}

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_speed	Free flow speed in m/s (usually the speed limit)
current_speed	Current speed in m/s
departure_speed	Speed to enter into the intersection in m/s
max_accel	Acceleration rate during acceleration segment in m/s^2
max_decel	Deceleration 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.

{
  // (v_e^2 - v^2)/ 2a_a + (v_d^2 - v_e^2)/ 2a_d
  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);
}

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_distance	Distance to the intersection in meters
free_flow_speed	Free flow speed along the route until the intersection (usually speed_limit) in m/s
current_speed	Current speed in m/s
departure_speed	Speed into the intersection in m/s. A.k.a target speed at the intersection
max_accel	The acceleration in m/s^2 of the acceleration segment
max_decel	The 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.

{
  double x = remaining_distance;
  double x2 = get_distance_to_accel_or_decel_once(current_speed, departure_speed, max_accel, max_decel);
  double x1 = get_distance_to_accel_or_decel_twice(free_flow_speed, current_speed, departure_speed, max_accel, max_decel);
  double v_hat = get_inflection_speed_value(x, x1, x2, free_flow_speed, current_speed, departure_speed, max_accel, max_decel);
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "x: " << x << ", x2: " << x2 << ", x1: " << x1 << ", v_hat: " << v_hat);
 
  if (v_hat <= config_.algo_minimum_speed - epsilon_ || isnan(v_hat))
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Detected that v_hat is smaller than allowed!!!: " << v_hat);
    v_hat = config_.algo_minimum_speed;
  }
 
  if (v_hat >= free_flow_speed + epsilon_)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Detected that v_hat is Bigger than allowed!!!: " << v_hat);
    v_hat = free_flow_speed;
  }
 
  rclcpp::Duration t_accel(0,0);
  if ( x < x2 && current_speed > departure_speed)
  {
    t_accel = rclcpp::Duration(0.0);
  }
  else
  {
    t_accel = rclcpp::Duration(std::max((v_hat - current_speed) / max_accel, 0.0) * 1e9);
  }
  rclcpp::Duration t_decel(0,0);
  if ( x < x2 && current_speed < departure_speed)
  {
    t_decel = rclcpp::Duration(0.0);
  }
  else
  {
    if (x < x2)
    {
      t_decel = rclcpp::Duration(std::max((v_hat - current_speed) / max_decel, 0.0) * 1e9);
 
    }
    else
    {
      t_decel = rclcpp::Duration(std::max((departure_speed - v_hat) / max_decel, 0.0) * 1e9);
    }
  }
 
  rclcpp::Duration t_cruise(0,0);
  if (x1 <= x)
  {
    t_cruise = rclcpp::Duration(std::max((x - x1)/v_hat, 0.0) * 1e9);
  }
  else
  {
    t_cruise = rclcpp::Duration(0.0);
  }
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "t_accel: " <<  t_accel.seconds() << ", t_cruise: " << t_cruise.seconds() << ", t_decel: " << t_decel.seconds());
  return t_accel + t_cruise + t_decel;
 
}

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_time	Earliest ROS entry time into the intersection kinematically possible
signal	CARMATrafficSignal 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.

{
  // If no green signal found after earliest entry time
  auto start_of_tbd_time = rclcpp::Time((lanelet::time::toSec(signal->recorded_time_stamps.back().first) + EPSILON) * 1e9);
  if (earliest_entry_time > start_of_tbd_time)
  {
    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()));
    return earliest_entry_time; //return TBD or EET red if no green is found...
  }
 
  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()));
  return start_of_tbd_time;
}

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_state	Current state of the vehicle
earliest_entry_time	Earliest entry time calculated by the vehicle
traffic_light	traffic 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.

{
  rclcpp::Time nearest_green_entry_time = rclcpp::Time(0);
  bool is_entry_time_within_green_or_tbd = false;
  bool in_tbd = true;
 
  if (config_.enable_carma_streets_connection ==false || scheduled_enter_time_ == 0) //UC2
  {
    auto nearest_green_entry_time_optional = get_nearest_green_entry_time(current_state.stamp, earliest_entry_time, traffic_light);
    is_entry_time_within_green_or_tbd = true;
 
    if (!nearest_green_entry_time_optional)
    {
      nearest_green_entry_time = get_eet_or_tbd(earliest_entry_time, traffic_light);
    }
    else
    {
      in_tbd = false;
      nearest_green_entry_time = nearest_green_entry_time_optional.value() + rclcpp::Duration(EPSILON * 1e9); //0.01sec more buffer since green_light algorithm's timestamp picks the previous signal - Vehicle Estimation
    }
  }
  else if(config_.enable_carma_streets_connection ==true && scheduled_enter_time_ != 0 ) // UC3
  {
    nearest_green_entry_time = rclcpp::Time(std::max(earliest_entry_time.seconds(), (scheduled_enter_time_)/1000.0) * 1e9) + rclcpp::Duration(EPSILON * 1e9); //Carma Street
 
    // check if scheduled_enter_time_ is inside the available states interval
    size_t i = 0;
 
 
    for (auto pair : traffic_light->recorded_time_stamps)
    {
      if (lanelet::time::timeFromSec(nearest_green_entry_time.seconds()) < pair.first)
      {
        if (isStateAllowedGreen(pair.second))
        {
          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]))
            << ", ending time of that green signal is: " << std::to_string(lanelet::time::toSec(pair.first)));
          is_entry_time_within_green_or_tbd = true;
        }
        else
        {
          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]))
            << ", ending time of that green signal is: " << std::to_string(lanelet::time::toSec(pair.first)));
          is_entry_time_within_green_or_tbd = false;
        }
 
        in_tbd = false;
        break;
      }
      i++;
    }
 
    if (in_tbd)
    {
      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)));
      is_entry_time_within_green_or_tbd = true;
    }
 
  }
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "nearest_green_entry_time: " << nearest_green_entry_time.get_clock_type());
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "current_state.stamp: " << current_state.stamp.get_clock_type());
 
  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()));
 
  if (nearest_green_entry_time_cached_)
  { // always pick later of buffered green entry time, or earliest entry time
    nearest_green_entry_time = rclcpp::Time(std::max(nearest_green_entry_time.seconds(), nearest_green_entry_time_cached_.get().seconds()) * 1e9);
  }
 
  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()));
 
  if (!nearest_green_entry_time_cached_ && is_entry_time_within_green_or_tbd)
  {
    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()));
    // save first calculated nearest_green_entry_time + buffer to compare against in the future as nearest_green_entry_time changes with earliest_entry_time
 
    // check if it needs buffer below:
    rclcpp::Time early_arrival_time_green_et =
        nearest_green_entry_time - rclcpp::Duration(config_.green_light_time_buffer * 1e9);
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "early_arrival_time_green_et: " << std::to_string(early_arrival_time_green_et.seconds()));
 
    auto early_arrival_state_green_et_optional = traffic_light->predictState(lanelet::time::timeFromSec(early_arrival_time_green_et.seconds()));
 
    if (!validLightState(early_arrival_state_green_et_optional, early_arrival_time_green_et))
    {
      RCLCPP_ERROR_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Unable to resolve give signal...");
      return std::make_tuple(rclcpp::Time(0), is_entry_time_within_green_or_tbd, in_tbd);
    }
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "early_arrival_state_green_et: " << early_arrival_state_green_et_optional.get().second);
 
    bool can_make_early_arrival  = isStateAllowedGreen(early_arrival_state_green_et_optional.get().second);
 
    // nearest_green_entry_time is by definition on green, so only check early_arrival
    if (can_make_early_arrival)  // Green light with Certainty
    {
      nearest_green_entry_time_cached_ = nearest_green_entry_time;  //don't apply buffer if ET is in green
    }
    else //buffer is needed
    {
      // below logic stores correct buffered timestamp into nearest_green_entry_time_cached_ to be used later
 
      rclcpp::Time nearest_green_signal_start_time = rclcpp::Time(0);
      if (traffic_light->fixed_cycle_duration.total_milliseconds()/1000.0 > 1.0) // UC2
      {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "UC2 Handling");
        auto normal_arrival_state_green_et_optional = traffic_light->predictState(lanelet::time::timeFromSec(nearest_green_entry_time.seconds()));
 
        if (!validLightState(normal_arrival_state_green_et_optional, nearest_green_entry_time))
        {
          RCLCPP_ERROR_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Unable to resolve give signal...");
          return std::make_tuple(rclcpp::Time(0), is_entry_time_within_green_or_tbd, in_tbd);
        }
 
        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)));
 
        // nearest_green_signal_start_time = normal_arrival_signal_end_time (green guaranteed) - green_signal_duration
        nearest_green_signal_start_time = rclcpp::Time(lanelet::time::toSec(getMovementAllowedDuration(traffic_light)));
      }
      else  // UC3
      {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "UC3 Handling");
 
        for (size_t i = 0; i < traffic_light->recorded_start_time_stamps.size(); i++)
        {
          if (isStateAllowedGreen(traffic_light->recorded_time_stamps[i].second) &&
            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
          {
            nearest_green_signal_start_time = rclcpp::Time(lanelet::time::toSec(traffic_light->recorded_start_time_stamps[i]) * 1e9);
            break;
          }
        }
 
        if (nearest_green_signal_start_time == rclcpp::Time(0)) //in tdb
        {
          nearest_green_signal_start_time = rclcpp::Time(lanelet::time::toSec(traffic_light->recorded_time_stamps.back().first) * 1e9);
        }
      }
 
      // 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
 
      nearest_green_entry_time_cached_ = nearest_green_signal_start_time + rclcpp::Duration((config_.green_light_time_buffer + EPSILON) * 1e9);
 
      // EPSILON=0.01 is there because if predictState's input exactly falls on ending_time it picks the previous state.
      //For example, if 0 - 10s is GREEN, and 10 - 12s is YELLOW, checking exactly 10.0s will return GREEN,
      //but 10.01s will return YELLOW. This 0.01 convention is used throughout the file, so thought it is better
      //to keep it consistent and probably too detailed for the user to think about, which is why it is not included in the buffer.
      //Actually including in the buffer doesn't work because it uses that same buffer to check early and late. If buffer is 2s and
      //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
      //from 12.01, so both checks 10s.
 
    }
 
    nearest_green_entry_time = rclcpp::Time(std::max(nearest_green_entry_time.seconds(), nearest_green_entry_time_cached_.get().seconds()) * 1e9);
  }
 
  if (nearest_green_entry_time_cached_ && nearest_green_entry_time > nearest_green_entry_time_cached_.get())
  {
    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()));
  }
  return std::make_tuple(nearest_green_entry_time, is_entry_time_within_green_or_tbd, in_tbd);
}

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

x	Distance remaining in meters
x1	Required distance to accel then decel, or vise versa-each at least once-to reach departure_speed with given speed and acceleration parameters
x2	required distance to accel or decel to reach departure_speed with given speed and acceleration parameters
free_flow_speed	Free flow speed in m/s (usually the speed limit)
current_speed	Current speed in m/s
departure_speed	Speed to enter into the intersection in m/s
max_accel	Acceleration rate during acceleration segment in m/s^2
max_decel	Deceleration 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.

{
  if (x >= x1)
  {
    return free_flow_speed;
  }
  else if (x1 > x && x >= x2)
  {
    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));
  }
  else if (x2 > x)
  {
    if (current_speed <= departure_speed)
    {
      return std::sqrt(2 * x * max_accel + std::pow(current_speed, 2));
    }
    else
    {
      return std::sqrt(2 * x * max_decel + std::pow(current_speed, 2));
    }
  }
 
}

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_time	Current state's ROS time
earliest_entry_time	Earliest ROS entry time into the intersection kinematically possible
signal	CARMATrafficSignal that is responsible for the intersection along route
minimum_required_green_time	Minimum 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.

{
  boost::posix_time::time_duration g =  lanelet::time::durationFromSec(minimum_required_green_time);         // provided by considering min headways of vehicles in front
  boost::posix_time::ptime t = lanelet::time::timeFromSec(current_time.seconds());                        // time variable
  boost::posix_time::ptime eet = lanelet::time::timeFromSec(earliest_entry_time.seconds());                        // earliest entry time
 
  // check if the signal even has a green signal
  bool has_green_signal = false;
  for (auto pair : signal->recorded_time_stamps)
  {
    if (isStateAllowedGreen(pair.second) && t <= pair.first )
    {
      has_green_signal = true;
      break;
    }
  }
 
  if (!has_green_signal)
  {
    return std::nullopt;
  }
 
  auto curr_pair = signal->predictState(t);
  if (!curr_pair)
    throw std::invalid_argument("Traffic signal with id:" + std::to_string(signal->id()) + ", does not have any recorded time stamps!");
 
  boost::posix_time::time_duration theta =  curr_pair.get().first - t;   // remaining time left in this state
  auto p = curr_pair.get().second;
  while ( 0.0 < g.total_milliseconds() || (!isStateAllowedGreen(p))) //green
  {
    if (isStateAllowedGreen(p))
    {
      if (g < theta)
      {
        t = t + g;
        theta = theta - g;
        g = boost::posix_time::seconds(0);
      }
      else
      {
        t = t + theta;
        g = g - theta;
        curr_pair = signal->predictState(t + boost::posix_time::milliseconds(20)); // select next phase
        p = curr_pair.get().second;
        theta = curr_pair.get().first - t;
      }
    }
    else
    {
      t = t + theta;
      curr_pair = signal->predictState(t + boost::posix_time::milliseconds(20)); // select next phase
      p = curr_pair.get().second;
      theta = curr_pair.get().first - t;
    }
  }
 
  if (t <= eet)
  {
    double cycle_duration = signal->fixed_cycle_duration.total_milliseconds()/1000.0;
    if (cycle_duration < 0.001) //if it is a dynamic traffic signal not fixed
      cycle_duration = lanelet::time::toSec(signal->recorded_time_stamps.back().first) - lanelet::time::toSec(signal->recorded_start_time_stamps.front());
 
    t = t + lanelet::time::durationFromSec(std::floor((eet - t).total_milliseconds()/1000.0/cycle_duration) * cycle_duration); //fancy logic was needed to compile
    curr_pair = signal->predictState(t + boost::posix_time::milliseconds(20)); // select next phase
    p = curr_pair.get().second;
    theta = curr_pair.get().first - t;
    while ( t < eet || !isStateAllowedGreen(p))
    {
      if ( (isStateAllowedGreen(p)) && eet - t < theta)
      {
        t = eet;
        theta = theta - (eet - t);
      }
      else
      {
        t = t + theta;
        curr_pair = signal->predictState(t + boost::posix_time::milliseconds(20)); // select next phase
        p = curr_pair.get().second;
        theta = curr_pair.get().first - t;
      }
 
      if (t != lanelet::time::timeFromSec(lanelet::time::INFINITY_END_TIME_FOR_NOT_ENOUGH_STATES))
        continue;
 
      // if no green signal left in the signal states, return
      return std::nullopt;
    }
  }
  return rclcpp::Time(lanelet::time::toSec(t) * 1e9);
}

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_flow	Free flow arrival at the intersection
full_cycle_duration	One fixed cycle of the signal
current_speed	Current speed in m/s
speed_limit	Speed limit speed in m/s
departure_speed	Speed into the intersection in m/s. A.k.a target speed at the intersection
max_accel	The acceleration in m/s^2 of the acceleration segment
max_decel	The 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.

{
  // TSMO USE CASE 2: Figure 7. Trajectory smoothing solution Case 2. Subsituted a+ as max_accel and solved for inflection_speed
  double accel_ratio = max_accel / max_decel;
  double remaining_time = time_remaining_at_free_flow - full_cycle_duration / 2;
  double inflection_speed = (max_accel * remaining_time - accel_ratio * departure_speed + current_speed)/ (1 - accel_ratio);
  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);
 
  if (remaining_time < 0)
    return -1;
 
  if (inflection_speed > 0 && inflection_speed <= speed_limit + epsilon_ && inflection_speed >= departure_speed - epsilon_)
  {
    // kinematic equation to find distance of acceleration + deceleration
    // (vf^2 - vi^2)/2a = d
    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);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "calculated distance WITHOUT cruising: " << d);
    return d;
  }
  else //there must be cruising
  {
    // acceleration and deceleration parts must reach maximum speed
    // kinematic equation: t = (vf - vi)/ a where vf = 0
    double decel_time = (current_speed - speed_limit) / max_decel;
    double accel_time = (speed_limit - current_speed) / max_accel;
    double cruising_time = remaining_time - decel_time - accel_time;
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "decel_time: " << decel_time << ", accel_time: " << accel_time << ", cruising_time: " << cruising_time);
    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;
    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) <<
                      ", cruising: " << + cruising_time * speed_limit << ", decel_seg:" << (std::pow(departure_speed, 2) - std::pow(speed_limit, 2)) / (2 * max_decel));
    return d;
  }
}

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.

{
  double dx1 = boundary_distances.dx1;
  double dx2 = boundary_distances.dx2;
  double dx3 = boundary_distances.dx3;
  double dx4 = boundary_distances.dx4;
  double dx5 = boundary_distances.dx5;
 
  if (params.size() != 8)
  {
    throw std::invalid_argument("Not enough trajectory paras given! Given size: " + std::to_string(params.size()));
  }
 
  TrajectoryParams traj1 = params[0];
  TrajectoryParams traj2 = params[1];
  TrajectoryParams traj3 = params[2];
  TrajectoryParams traj4 = params[3];
  TrajectoryParams traj5 = params[4];
  TrajectoryParams traj6 = params[5];
  TrajectoryParams traj7 = params[6];
  TrajectoryParams traj8 = params[7];
  TrajectoryParams veh_traj;
  veh_traj.is_algorithm_successful = true;
 
  if (traj1.t3_ <= et && et < traj2.t3_)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CALCULATED: case 1");
    veh_traj = ts_case1(t, et, v0, v1, v_max, a_max, a_min, x0, x_end, dx);
    veh_traj.case_num = CASE_1;
  }
  else if (traj2.t3_ <= et && et < traj3.t3_)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CALCULATED: case 2");
    veh_traj = ts_case2(t, et, v0, v1, a_max, a_min, x0, x_end, dx);
    veh_traj.case_num = CASE_2;
  }
  else if (traj3.t3_ <= et && et < traj4.t3_)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CALCULATED: case 3");
    veh_traj = ts_case3(t, et, v0, v1, a_max, a_min, x0, x_end, dx);
    veh_traj.case_num = CASE_3;
  }
  else if (traj4.t3_ <= et && et < traj5.t3_)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CALCULATED: case 4");
    veh_traj = ts_case4(t, et, v0, v1, v_min, a_max, a_min, x0, x_end, dx);
    veh_traj.case_num = CASE_4;
  }
  else if (traj5.t3_ <= et && et < traj6.t3_)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CALCULATED: case 5");
    veh_traj = ts_case5(t, et, v0, a_max, a_min, x0, x_end, dx);
    veh_traj.case_num = CASE_5;
  }
  else if (traj6.t3_ <= et && et < traj7.t3_)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CALCULATED: case 6");
    veh_traj = ts_case6(t, et, v0, v_min, a_min, x0, x_end, dx, dx3, traj6);
    veh_traj.case_num = CASE_6;
  }
  else if (traj7.t3_ <= et && et <= traj8.t3_)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CALCULATED: case 7");
    veh_traj = ts_case7(t, et, v0, v_min, a_min, x0, x_end, dx);
    veh_traj.case_num = CASE_7;
  }
  else
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CALCULATED: case 8");
    veh_traj = ts_case8(dx, dx5, traj8);
    veh_traj.case_num = CASE_8;
  }
 
  return veh_traj;
}

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

{
  return "v1.0";
}

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

{
  std::vector<lanelet::ConstLanelet> crossed_lanelets =
      wm_->getLaneletsBetween(start_downtrack, end_downtrack, shortest_path_only, bounds_inclusive);
 
  if (crossed_lanelets.size() == 0)
  {
    throw std::invalid_argument("getLaneletsBetweenWithException called but inputs do not cross any lanelets going "
                                "from: " +
                                std::to_string(start_downtrack) + " to: " + std::to_string(end_downtrack));
  }
 
  return crossed_lanelets;
}

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_light The 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.

{
  if(traffic_light->signal_durations.find(lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED) != traffic_light->signal_durations.end())
  {
    return traffic_light->signal_durations[lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED];
  }
 
  return traffic_light->signal_durations[lanelet::CarmaTrafficSignalState::PERMISSIVE_MOVEMENT_ALLOWED];
 
}

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_list List of lanelets crossed around the intersection area

Returns: turn direction in format of straight, left, right

Definition at line 1226 of file lci_strategic_plugin.cpp.

{
  TurnDirection turn_direction = TurnDirection::Straight;
  for (auto l:lanelets_list)
  {
    if(l.hasAttribute("turn_direction")) {
      std::string direction_attribute = l.attribute("turn_direction").value();
      if (direction_attribute == "right")
      {
        turn_direction = TurnDirection::Right;
        break;
      }
      else if (direction_attribute == "left")
      {
        turn_direction = TurnDirection::Left;
        break;
      }
      else turn_direction = TurnDirection::Straight;
    }
    else
    {
      // if there is no attribute, assumption is straight
      turn_direction = TurnDirection::Straight;
    }
 
  }
  return turn_direction;
}

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

	req	Plan maneuver request
[out]	resp	Plan maneuver response with a list of maneuver plan
	current_state	Current state of the vehicle
	current_state_speed	The current speed to use when planning
	traffic_light	CarmaTrafficSignalPtr of the relevant signal
	traffic_light_down_track	Downtrack to the given traffic_light
	ts_params	trajectory 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 438 of file lci_strategic_plugin.cpp.

{
  if (!ts_params.is_algorithm_successful || ts_params.case_num != TSCase::CASE_8)
  {
    RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"),"handleCruisingUntilStop is called but it is not case_8");
    return;
  }
 
  auto new_ts_params = ts_params;
 
  double decel_rate = std::fabs(ts_params.a3_);
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CASE_8: Planning cruise and stop with decel_rate: " << decel_rate);
 
  new_ts_params.t3_ = new_ts_params.t2_;
  new_ts_params.x3_ = new_ts_params.x2_;
  new_ts_params.v3_ = new_ts_params.v2_;
  new_ts_params.a3_ = new_ts_params.a2_;
 
  // Identify the lanelets which will be crossed by approach maneuvers stopping part
  std::vector<lanelet::ConstLanelet> lane_follow_crossed_lanelets =
      getLaneletsBetweenWithException(new_ts_params.x1_, new_ts_params.x2_, true, true);
 
  resp->new_plan.maneuvers.push_back(composeTrajectorySmoothingManeuverMessage(current_state.downtrack, new_ts_params.x2_, lane_follow_crossed_lanelets,
                                          current_state_speed, new_ts_params.v2_, current_state.stamp, rclcpp::Time(new_ts_params.t2_ * 1e9), new_ts_params));
 
  // Identify the lanelets which will be crossed by approach maneuvers stopping part
  std::vector<lanelet::ConstLanelet> case_8_crossed_lanelets =
      getLaneletsBetweenWithException(new_ts_params.x2_, traffic_light_down_track, true, true);
 
  resp->new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage(
    new_ts_params.x2_, traffic_light_down_track, new_ts_params.v2_, case_8_crossed_lanelets.front().id(),
    case_8_crossed_lanelets.back().id(), rclcpp::Time(new_ts_params.t2_ * 1e9),
    rclcpp::Time(new_ts_params.t2_ * 1e9) + rclcpp::Duration(config_.min_maneuver_planning_period * 1e9), decel_rate));
 
  return;
}

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

	req	Plan maneuver request
[out]	resp	Plan maneuver response with a list of maneuver plan
	current_state	Current state of the vehicle
	current_state_speed	The current speed to use when planning
	speed_limit	Speed limit to cap the speed
	remaining_time	Remaining time until scheduled entry
	exit_lanelet_id	Id of the exit lanelet
	traffic_light	CarmaTrafficSignalPtr of the relevant signal
	traffic_light_down_track	Downtrack to the given traffic_light
	ts_params	trajectory 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 399 of file lci_strategic_plugin.cpp.

{
  double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.downtrack;
 
  // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver
  std::vector<lanelet::ConstLanelet> crossed_lanelets =
        getLaneletsBetweenWithException(current_state.downtrack, traffic_light_down_track, true, true);
 
  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);
 
  if (incomplete_traj_params.is_algorithm_successful == false)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Failed to generate maneuver for edge cases...");
    return;
  }
 
  resp->new_plan.maneuvers.push_back(composeTrajectorySmoothingManeuverMessage(current_state.downtrack, traffic_light_down_track, crossed_lanelets,
                                          current_state_speed, incomplete_traj_params.modified_departure_speed, current_state.stamp, current_state.stamp + rclcpp::Duration(incomplete_traj_params.modified_remaining_time * 1e9), incomplete_traj_params));
 
  double intersection_length = intersection_end_downtrack_.get() - traffic_light_down_track;
 
  rclcpp::Time intersection_exit_time =
      current_state.stamp + rclcpp::Duration(incomplete_traj_params.modified_remaining_time * 1e9) + rclcpp::Duration(intersection_length / incomplete_traj_params.modified_departure_speed * 1e9);
 
  resp->new_plan.maneuvers.push_back(composeIntersectionTransitMessage(
      traffic_light_down_track, intersection_end_downtrack_.get(), intersection_speed_.get(),
      incomplete_traj_params.modified_departure_speed, current_state.stamp + rclcpp::Duration(incomplete_traj_params.modified_remaining_time * 1e9), intersection_exit_time, crossed_lanelets.back().id(), crossed_lanelets.back().id()));
 
  last_case_num_ = TSCase::DEGRADED_TSCASE;
}

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_speed	starting speed
departure_speed	departure_speed originally planned
speed_limit	speed_limit
remaining_downtrack	remaining_downtrack until the intersection
traffic_light_downtrack	traffic_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 526 of file lci_strategic_plugin.cpp.

{
  //Requested maneuver needs to be modified to meet remaining_dist req
  //by trying to get close to the target_speed and remaining_time as much as possible
  TrajectoryParams return_params;
  TrajectoryParams traj_upper;
  TrajectoryParams traj_lower;
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "HANDLE_LAST_RESORT_CASE: Starting...");
  double starting_downtrack = traffic_light_downtrack - remaining_downtrack;
  double modified_remaining_time_upper; // upper meaning downtrack vs time trajectory is curved upwards
  double modified_remaining_time_lower; // lower meaning downtrack vs time trajectory is curved lower
  double modified_departure_speed_upper;
  double modified_departure_speed_lower;
  bool calculation_success_upper = true; // identifies places in codes where calculation can be invalid such as negative distance
 
  // the upper ET
  // accel case
  traj_upper.t0_ = current_time;
  traj_upper.v0_ = starting_speed;
  traj_upper.x0_ = starting_downtrack;
  traj_upper.is_algorithm_successful = true;
  traj_upper.case_num = CASE_1;
 
  if (departure_speed >= starting_speed)
  {
    if ((pow(departure_speed,2) - pow(starting_speed,2))/(2*max_comfort_accel_) >= remaining_downtrack)
    {
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "HandleFailureCase -> Upper Trajectory -> Current Speed <= Desired Departure Speed, Actual Departure Speed < Desired Departure Speed");
 
      modified_departure_speed_upper = sqrt(pow(starting_speed, 2) + (2 * max_comfort_accel_ * remaining_downtrack));
      modified_remaining_time_upper = (modified_departure_speed_upper - starting_speed) / max_comfort_accel_;
 
      traj_upper.t1_ = current_time + modified_remaining_time_upper;
      traj_upper.v1_ = modified_departure_speed_upper;
      traj_upper.a1_ = max_comfort_accel_;
      traj_upper.x1_ = traffic_light_downtrack;
 
      traj_upper.t2_ = traj_upper.t1_;
      traj_upper.v2_ = traj_upper.v1_;
      traj_upper.a2_ = traj_upper.a1_;
      traj_upper.x2_ = traj_upper.x1_;
 
      traj_upper.modified_departure_speed = modified_departure_speed_upper;
      traj_upper.modified_remaining_time = modified_remaining_time_upper;
    }
    else // NOTE: most likely will not happen as it would have happened at trajectory smoothing part
    {
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "HandleFailureCase -> Upper Trajectory -> Current Speed < Desired Departure Speed, Actual Departure Speed = Desired Departure Speed");
 
      double cruising_distance = remaining_downtrack - (pow(departure_speed, 2) - pow(starting_speed, 2))/ ( 2 * max_comfort_accel_);
      if (cruising_distance < -EPSILON)
      {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Detected calculation failure in upper case 2");
        calculation_success_upper = false;
      }
      modified_remaining_time_upper = ((departure_speed - starting_speed) / max_comfort_accel_) + (cruising_distance / departure_speed);
 
      traj_upper.t1_ = current_time + ((departure_speed - starting_speed) / max_comfort_accel_);
      traj_upper.v1_ = departure_speed;
      traj_upper.a1_ = max_comfort_accel_;
      traj_upper.x1_ = starting_downtrack + (pow(departure_speed, 2) - pow(starting_speed, 2)) / (2 * max_comfort_accel_);
 
      traj_upper.t2_ = current_time + modified_remaining_time_upper;
      traj_upper.v2_ = departure_speed;
      traj_upper.a2_ = 0;
      traj_upper.x2_ = traffic_light_downtrack;
 
      traj_upper.modified_departure_speed = departure_speed;
      traj_upper.modified_remaining_time = modified_remaining_time_upper;
    }
  }
 
  if (departure_speed < starting_speed) // NOTE: most cases will not run due to departure speed being equal to speed limit
  {
    if ((pow(departure_speed,2) - pow(starting_speed,2))/(2*max_comfort_decel_) >= remaining_downtrack)
    {
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "HandleFailureCase -> Upper Trajectory -> Current Speed > Desired Departure Speed, Actual Departure Speed > Desired Departure Speed");
 
      modified_departure_speed_upper = sqrt(pow(starting_speed, 2) + (2 * max_comfort_decel_ * remaining_downtrack));
      modified_remaining_time_upper = (modified_departure_speed_upper - starting_speed) / max_comfort_decel_;
 
      traj_upper.t1_ = current_time + modified_remaining_time_upper;
      traj_upper.v1_ = modified_departure_speed_upper;
      traj_upper.a1_ = max_comfort_decel_;
      traj_upper.x1_ = traffic_light_downtrack;
 
      traj_upper.t2_ = traj_upper.t1_;
      traj_upper.v2_ = traj_upper.v1_;
      traj_upper.a2_ = traj_upper.a1_;
      traj_upper.x2_ = traj_upper.x1_;
 
      traj_upper.modified_departure_speed = modified_departure_speed_upper;
      traj_upper.modified_remaining_time = modified_remaining_time_upper;
    }
    else  // NOTE: most likely will not happen as it would have happened at trajectory smoothing part
    {
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "HandleFailureCase -> Upper Trajectory -> Current Speed > Desired Departure Speed, Actual Departure Speed = Desired Departure Speed");
 
      double cruising_distance = remaining_downtrack - (pow(departure_speed, 2) - pow(starting_speed, 2))/ ( 2 * max_comfort_decel_);
 
      if (cruising_distance < -EPSILON)
      {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Detected calculation failure in upper case 4");
        calculation_success_upper = false;
      }
 
      modified_remaining_time_upper = cruising_distance / starting_speed + (departure_speed - starting_speed) / max_comfort_decel_ ;
 
      traj_upper.t1_ = current_time + cruising_distance / starting_speed;
      traj_upper.v1_ = starting_speed;
      traj_upper.a1_ = 0.0;
      traj_upper.x1_ = starting_downtrack + cruising_distance;
 
      traj_upper.t2_ = current_time + modified_remaining_time_upper;
      traj_upper.v2_ = departure_speed;
      traj_upper.a2_ = max_comfort_decel_;
      traj_upper.x2_ = traffic_light_downtrack;
 
      traj_upper.modified_departure_speed = departure_speed;
      traj_upper.modified_remaining_time = modified_remaining_time_upper;
    }
  }
 
  traj_upper.t3_ = traj_upper.t2_;
  traj_upper.v3_ = traj_upper.v2_;
  traj_upper.a3_ = traj_upper.a2_;
  traj_upper.x3_ = traj_upper.x2_;
 
  // The lower ET
  // (note that the distance is definitely not enough for deceleration to zero speed, therefore, modified_departure_speed will be greater than zero for sure!).
  modified_departure_speed_lower = sqrt(pow(starting_speed, 2) + (2 * max_comfort_decel_ * remaining_downtrack));
  modified_remaining_time_lower = (modified_departure_speed_lower - starting_speed) / max_comfort_decel_;
 
  traj_lower.t0_ = current_time;
  traj_lower.v0_ = starting_speed;
  traj_lower.x0_ = starting_downtrack;
  traj_lower.is_algorithm_successful = true;
  traj_lower.case_num = CASE_1;
 
  traj_lower.t1_ = current_time + modified_remaining_time_lower;
  traj_lower.v1_ = modified_departure_speed_lower;
  traj_lower.a1_ = max_comfort_decel_;
  traj_lower.x1_ = traffic_light_downtrack;
 
  traj_lower.t2_ = traj_lower.t1_;
  traj_lower.v2_ = traj_lower.v1_;
  traj_lower.a2_ = traj_lower.a1_;
  traj_lower.x2_ = traj_lower.x1_;
 
  traj_lower.t3_ = traj_lower.t2_;
  traj_lower.v3_ = traj_lower.v2_;
  traj_lower.a3_ = traj_lower.a2_;
  traj_lower.x3_ = traj_lower.x2_;
 
  traj_lower.modified_departure_speed = modified_departure_speed_lower;
  traj_lower.modified_remaining_time = modified_remaining_time_upper;
 
  // Pick UPPER or LOWER trajectory based on light
  bool is_return_params_found = false;
 
  if (calculation_success_upper)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Checking this time!: " << current_time + modified_remaining_time_upper);
 
    auto upper_optional = traffic_light->predictState(lanelet::time::timeFromSec(current_time + modified_remaining_time_upper));
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Checking this time! state: " << upper_optional.get().second);
 
    if (!validLightState(upper_optional, rclcpp::Time((current_time + modified_remaining_time_upper) * 1e9)))
    {
      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));
    }
    else
    {
      if (isStateAllowedGreen(upper_optional.get().second))
      {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Detected Upper GREEN case");
        return_params = traj_upper;
        is_return_params_found = true;
      }
    }
  }
 
  if (!is_return_params_found)
  {
    auto lower_optional = traffic_light->predictState(lanelet::time::timeFromSec(current_time + modified_remaining_time_lower));
 
    if (!validLightState(lower_optional, rclcpp::Time((current_time + modified_remaining_time_lower) * 1e9)))
    {
      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));
    }
    else
    {
      if (isStateAllowedGreen(lower_optional.get().second))
      {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Detected Lower GREEN case");
        return_params = traj_lower;
        is_return_params_found = true;
      }
    }
  }
 
  // Handle hard failure case such as nan or invalid states
  if (is_return_params_found && !isnan(return_params.modified_departure_speed) && return_params.modified_departure_speed > epsilon_ &&
      return_params.modified_departure_speed < speed_limit ) //80_mph
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Updated the speed, and using modified_departure_speed: " << return_params.modified_departure_speed);
    print_params(return_params);
    return return_params;
  }
  else
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Unable to handle edge case gracefully");
    return_params = TrajectoryParams(); //reset
    return_params.is_algorithm_successful = false;
    return return_params;
  }
}

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

	req	Plan maneuver request
[out]	resp	Plan maneuver response with a list of maneuver plan
	current_state	Current state of the vehicle
	current_state_speed	The current speed to use when planning
	traffic_light	CarmaTrafficSignalPtr of the relevant signal
	entry_lanelet	Entry lanelet in to the intersection along route
	exit_lanelet	Exit lanelet in to the intersection along route
	traffic_light_down_track	Downtrack to the given traffic_light
	ts_params	trajectory 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 481 of file lci_strategic_plugin.cpp.

{
  if (!ts_params.is_algorithm_successful || ts_params.case_num == TSCase::CASE_8)
  {
    return;
  }
 
  rclcpp::Time light_arrival_time_by_algo = rclcpp::Time(ts_params.t3_ * 1e9);
  double remaining_time = light_arrival_time_by_algo.seconds() - rclcpp::Time(req->header.stamp, RCL_SYSTEM_TIME).seconds();
  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));
  auto can_make_green_optional = canArriveAtGreenWithCertainty(light_arrival_time_by_algo, traffic_light);
 
    // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver
  std::vector<lanelet::ConstLanelet> crossed_lanelets =
        getLaneletsBetweenWithException(current_state.downtrack, traffic_light_down_track, true, true);
 
  // no change for maneuver if invalid light states
  if (!can_make_green_optional)
    return;
 
  if (can_make_green_optional.get() || is_certainty_check_optional)
  {
    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");
 
    resp->new_plan.maneuvers.push_back(composeTrajectorySmoothingManeuverMessage(current_state.downtrack, traffic_light_down_track, crossed_lanelets,
                                          current_state_speed, ts_params.v3_, current_state.stamp, light_arrival_time_by_algo, ts_params));
 
    double intersection_length = intersection_end_downtrack_.get() - traffic_light_down_track;
 
    rclcpp::Time intersection_exit_time =
        light_arrival_time_by_algo + rclcpp::Duration(intersection_length / intersection_speed_.get() * 1e9);
 
    resp->new_plan.maneuvers.push_back(composeIntersectionTransitMessage(
        traffic_light_down_track, intersection_end_downtrack_.get(), intersection_speed_.get(),
        intersection_speed_.get(), light_arrival_time_by_algo, intersection_exit_time, crossed_lanelets.back().id(), crossed_lanelets.back().id()));
  }
}

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

	req	Plan maneuver request
[out]	resp	Plan maneuver response with a list of maneuver plan
	current_state	Current state of the vehicle
	traffic_light	CarmaTrafficSignalPtr of the relevant signal
	entry_lanelet	Entry lanelet in to the intersection along route
	exit_lanelet	Exit lanelet in to the intersection along route
	current_lanelet	Current lanelet
	traffic_light_down_track	Downtrack to the given traffic_light
	is_emergency	if enabled, double the deceleration rate of max_comfort_accel is used

Returns: void. if successful, resp is non-empty

Definition at line 364 of file lci_strategic_plugin.cpp.

{
  double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.downtrack;
 
  // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver
  std::vector<lanelet::ConstLanelet> crossed_lanelets =
        getLaneletsBetweenWithException(current_state.downtrack, traffic_light_down_track, true, true);
 
  double decel_rate =  max_comfort_decel_norm_; // Kinematic |(v_f - v_i) / t = a|
 
  if (is_emergency)
  {
    decel_rate = emergency_decel_norm_;
    last_case_num_ = TSCase::EMERGENCY_STOPPING;
  }
  else
  {
    last_case_num_ = TSCase::STOPPING;
  }
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "HANDLE_STOPPING: Planning stop and wait maneuver at decel_rate: " << decel_rate);
 
  resp->new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage(
    current_state.downtrack, traffic_light_down_track, current_state.speed, crossed_lanelets.front().id(),
    crossed_lanelets.back().id(), current_state.stamp,
    current_state.stamp + rclcpp::Duration(config_.min_maneuver_planning_period * 1e9), decel_rate));
}

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

state The traffic signal state to check for

Returns: bool value - True if state is PERMISSIVE_MOVEMENT_ALLOWED or PROTECTED_MOVEMENT_ALLOWED

Definition at line 341 of file lci_strategic_plugin.cpp.

                                                                                             {
    return state == lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED ||
           state == lanelet::CarmaTrafficSignalState::PERMISSIVE_MOVEMENT_ALLOWED;
}

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

{
    tf2_listener_.reset(new tf2_ros::TransformListener(tf2_buffer_));
    tf2_buffer_.setUsingDedicatedThread(true);
    try
    {
        geometry_msgs::msg::TransformStamped tf2 = tf2_buffer_.lookupTransform("base_link", "vehicle_front", rclcpp::Time(0), rclcpp::Duration(20.0 * 1e9)); //save to local copy of transform 20 sec timeout
        length_to_front_bumper_ = tf2.transform.translation.x;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "length_to_front_bumper_: " << length_to_front_bumper_);
 
    }
    catch (const tf2::TransformException &ex)
    {
        RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"), ex.what());
    }
}

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

msg	input mobility operation msg

Definition at line 1155 of file lci_strategic_plugin.cpp.

{
  if (msg->strategy == light_controlled_intersection_strategy_)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Received Schedule message with id: " << msg->m_header.plan_id);
    approaching_light_controlled_intersection_ = true;
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Approaching light Controlled Intersection: " << approaching_light_controlled_intersection_);
 
    if (msg->m_header.recipient_id == config_.vehicle_id)
    {
      street_msg_timestamp_ = msg->m_header.timestamp;
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "street_msg_timestamp_: " << street_msg_timestamp_);
      parseStrategyParams(msg->strategy_params);
      previous_strategy_params_ = msg->strategy_params;
    }
  }
 
}

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

{
  mob_op_pub_timer_ = create_timer(get_clock(),
    std::chrono::duration<double>(1/config_.mobility_rate),
    std::bind(&LCIStrategicPlugin::publishMobilityOperation, this));
 
  ts_info_pub_timer_ = create_timer(get_clock(),
    std::chrono::duration<double>(0.5),
    std::bind(&LCIStrategicPlugin::publishTrajectorySmoothingInfo, this));
 
  return CallbackReturn::SUCCESS;
}

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

{
   // reset config
  config_ = LCIStrategicPluginConfig();
 
  // clang-format off
  get_parameter<double>("vehicle_acceleration_limit", config_.vehicle_accel_limit);
  get_parameter<double>("vehicle_deceleration_limit", config_.vehicle_decel_limit);
  get_parameter<double>("vehicle_decel_limit_multiplier", config_.vehicle_decel_limit_multiplier);
  get_parameter<double>("vehicle_accel_limit_multiplier", config_.vehicle_accel_limit_multiplier);
  get_parameter<double>("min_approach_distance", config_.min_approach_distance);
  get_parameter<double>("trajectory_smoothing_activation_distance", config_.trajectory_smoothing_activation_distance);
  get_parameter<double>("stopping_location_buffer", config_.stopping_location_buffer);
  get_parameter<double>("green_light_time_buffer", config_.green_light_time_buffer);
  get_parameter<double>("algo_minimum_speed", config_.algo_minimum_speed);
  get_parameter<double>("deceleration_fraction", config_.deceleration_fraction);
  get_parameter<double>("desired_distance_to_stop_buffer", config_.desired_distance_to_stop_buffer);
  get_parameter<double>("min_maneuver_planning_period", config_.min_maneuver_planning_period);
  get_parameter<std::string>("strategic_plugin_name", config_.strategic_plugin_name);
  get_parameter<std::string>("lane_following_plugin_name", config_.lane_following_plugin_name);
  get_parameter<std::string>("stop_and_wait_plugin_name", config_.stop_and_wait_plugin_name);
  get_parameter<std::string>("intersection_transit_plugin_name", config_.intersection_transit_plugin_name);
  get_parameter<bool>("enable_carma_streets_connection", config_.enable_carma_streets_connection);
  get_parameter<double>("mobility_rate", config_.mobility_rate);
  get_parameter<std::string>("vehicle_id", config_.vehicle_id);
 
  max_comfort_accel_ = config_.vehicle_accel_limit * config_.vehicle_accel_limit_multiplier;
  max_comfort_decel_ = -1 * config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier;
  max_comfort_decel_norm_ = config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier;
  emergency_decel_norm_ = 2 * config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier;
 
  // clang-format on
 
  // Register runtime parameter update callback
  add_on_set_parameters_callback(std::bind(&LCIStrategicPlugin::parameter_update_callback, this, std_ph::_1));
 
  RCLCPP_INFO_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Done loading parameters: " << config_);
 
  lookupFrontBumperTransform();
 
   // Mobility Operation Subscriber
  mob_operation_sub_ = create_subscription<carma_v2x_msgs::msg::MobilityOperation>("incoming_mobility_operation", 1,
    std::bind(&LCIStrategicPlugin::mobilityOperationCb,this,std_ph::_1));
 
  // BSM subscriber
  bsm_sub_ = create_subscription<carma_v2x_msgs::msg::BSM>("bsm_outbound", 1,
    std::bind(&LCIStrategicPlugin::BSMCb,this,std_ph::_1));
 
  // set world model point form wm listener
  wm_ = get_world_model();
 
  // Setup publishers
  mobility_operation_pub_ = create_publisher<carma_v2x_msgs::msg::MobilityOperation>("outgoing_mobility_operation", 1);
  case_pub_ = create_publisher<std_msgs::msg::Int8>("lci_strategic_plugin/ts_case_num", 1);
  tf_distance_pub_ = create_publisher<std_msgs::msg::Float64>("lci_strategic_plugin/distance_remaining_to_tf", 1);
  earliest_et_pub_ = create_publisher<std_msgs::msg::Float64>("lci_strategic_plugin/earliest_entry_time", 1);
  et_pub_ = create_publisher<std_msgs::msg::Float64>("lci_strategic_plugin/scheduled_entry_time", 1);
 
  // Return success if everything initialized successfully
  return CallbackReturn::SUCCESS;
}

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

{
  auto error_double = update_params<double>({
    {"vehicle_acceleration_limit", config_.vehicle_accel_limit},
    {"vehicle_deceleration_limit", config_.vehicle_decel_limit},
    {"vehicle_decel_limit_multiplier", config_.vehicle_decel_limit_multiplier},
    {"vehicle_accel_limit_multiplier", config_.vehicle_accel_limit_multiplier},
    {"min_approach_distance", config_.min_approach_distance},
    {"trajectory_smoothing_activation_distance", config_.trajectory_smoothing_activation_distance},
    {"stopping_location_buffer", config_.stopping_location_buffer},
    {"green_light_time_buffer", config_.green_light_time_buffer},
    {"algo_minimum_speed", config_.algo_minimum_speed},
    {"deceleration_fraction", config_.deceleration_fraction},
    {"desired_distance_to_stop_buffer", config_.desired_distance_to_stop_buffer},
    {"min_maneuver_planning_period", config_.min_maneuver_planning_period},
    {"mobility_rate", config_.mobility_rate},
  }, parameters);
 
  rcl_interfaces::msg::SetParametersResult result;
 
  result.successful = !error_double;
 
  return result;
}

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_params input string of strategy params from mob op msg

Definition at line 1182 of file lci_strategic_plugin.cpp.

{
  // sample strategy_params: "et:1634067059"
  std::string params = strategy_params;
  std::vector<std::string> inputsParams;
  boost::algorithm::split(inputsParams, params, boost::is_any_of(","));
 
  std::vector<std::string> et_parsed;
  boost::algorithm::split(et_parsed, inputsParams[0], boost::is_any_of(":"));
  auto new_scheduled_enter_time = std::stoull(et_parsed[1]);
 
  if (scheduled_enter_time_ != new_scheduled_enter_time) //reset green buffer cache so it can be re-evaluated
    nearest_green_entry_time_cached_ = boost::none;
 
  scheduled_enter_time_ = new_scheduled_enter_time;
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "scheduled_enter_time_: " << scheduled_enter_time_);
 
}

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

req	Plan maneuver request
resp	Plan maneuver response with a list of maneuver plan

Returns: If service call successed

Implements carma_guidance_plugins::StrategicPlugin.

Definition at line 1266 of file lci_strategic_plugin.cpp.

{
  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
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "<<<<<<<<<<<<<<<<< STARTING LCI_STRATEGIC_PLAN!!!!!!!!! >>>>>>>>>>>>>>>>");
 
  if (!wm_->getRoute())
  {
    RCLCPP_ERROR_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Could not plan maneuvers as route was not available");
    return;
  }
 
  if(config_.enable_carma_streets_connection ==true)
  {
  if (!approaching_light_controlled_intersection_)
  {
    resp->new_plan.maneuvers = {};
    RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"),"Not approaching light-controlled intersection so no maneuvers");
    return;
  }
 
  bool is_empty_schedule_msg = (scheduled_enter_time_ == 0);
  if (is_empty_schedule_msg)
  {
    resp->new_plan.maneuvers = {};
    RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"),"Receiving empty schedule message");
    return;
  }
  }
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Finding car information");
 
  // Extract vehicle data from request
  VehicleState current_state = extractInitialState(req);
  if (transition_table_.getState() != TransitState::UNAVAILABLE && !req->prior_plan.maneuvers.empty())
  {
    RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"),"State is NOT UNAVAILABLE AND Maneuvers in request is NOT empty");
    return;
  }
  // Get current traffic light information
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "\n\nFinding traffic_light information");
 
  auto inter_list = wm_->getSignalizedIntersectionsAlongRoute({ req->veh_x, req->veh_y });
  auto upcoming_id_= inter_list.front()->id();
 
  auto traffic_list = wm_->getSignalsAlongRoute({ req->veh_x, req->veh_y });
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Found traffic lights of size: " << traffic_list.size());
 
  auto current_lanelets = wm_->getLaneletsFromPoint({ req->veh_x, req->veh_y});
  lanelet::ConstLanelet current_lanelet;
 
  if (current_lanelets.empty())
  {
    RCLCPP_ERROR_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Given vehicle position is not on the road! Returning...");
    return;
  }
 
  // get the lanelet that is on the route in case overlapping ones found
  auto llt_on_route_optional = wm_->getFirstLaneletOnShortestPath(current_lanelets);
 
  if (llt_on_route_optional){
    current_lanelet = llt_on_route_optional.value();
  }
  else{
    RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "When identifying the corresponding lanelet for requested maneuever's state, x: " 
      << req->veh_x << ", y: " << req->veh_y << ", no possible lanelet was found to be on the shortest path."
      << "Picking arbitrary lanelet: " << current_lanelets[0].id() << ", instead");
    current_lanelet = current_lanelets[0];
  }
 
  lanelet::CarmaTrafficSignalPtr nearest_traffic_signal = nullptr;
 
  lanelet::ConstLanelet entry_lanelet;
  lanelet::ConstLanelet exit_lanelet;
 
  for (auto signal : traffic_list)
  {
    auto optional_entry_exit = wm_->getEntryExitOfSignalAlongRoute(signal);
    // if signal is not matching our destination skip
    if (!optional_entry_exit)
    {
      RCLCPP_ERROR_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Did not find entry.exit along the route");
      continue;
    }
 
    entry_lanelet = optional_entry_exit.get().first;
    exit_lanelet = optional_entry_exit.get().second;
 
    nearest_traffic_signal = signal;
    break;
  }
 
 
  TransitState prev_state;
 
  do
  {
    // Clear previous maneuvers planned by this plugin as guard against state change since each state generates an
    // independent set of maneuvers
    resp->new_plan = carma_planning_msgs::msg::ManeuverPlan();
 
    prev_state = transition_table_.getState();  // Cache previous state to check if state has changed after 1 iteration
 
    RCLCPP_INFO_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Planning in state: " << transition_table_.getState());
 
    boost::optional<std::pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState>> current_light_state_optional = boost::none;
    if (nearest_traffic_signal)
    {
      current_light_state_optional = nearest_traffic_signal->predictState(lanelet::time::timeFromSec(current_state.stamp.seconds()));
      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());
      if (current_light_state_optional.get().second == lanelet::CarmaTrafficSignalState::UNAVAILABLE || !current_light_state_optional)
      {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Signal state not available returning..." );
        return;
      }
    }
    switch (transition_table_.getState())
    {
      case TransitState::UNAVAILABLE:
        planWhenUNAVAILABLE(req, resp, current_state, nearest_traffic_signal, entry_lanelet, exit_lanelet, current_lanelet);
        break;
 
      case TransitState::APPROACHING:
        planWhenAPPROACHING(req, resp, current_state, nearest_traffic_signal, entry_lanelet, exit_lanelet, current_lanelet);
        break;
 
      case TransitState::WAITING:
        planWhenWAITING(req, resp, current_state, nearest_traffic_signal, entry_lanelet, exit_lanelet, current_lanelet);
        break;
 
      case TransitState::DEPARTING:
        // Reset intersection state since we are no longer in an intersection
        if (!intersection_end_downtrack_ || !intersection_speed_)
        {
          throw std::invalid_argument("State is DEPARTING but the intersection variables are not available");
        }
        planWhenDEPARTING(req, resp, current_state, intersection_end_downtrack_.get(), intersection_speed_.get());
        break;
 
      default:
        throw std::invalid_argument("LCIStrategic Strategic Plugin entered unknown state");
        break;
    }
 
  } while (transition_table_.getState() != prev_state);  // If the state has changed then continue looping
 
  std::chrono::system_clock::time_point execution_end_time = std::chrono::system_clock::now();  // Planning complete
 
  auto execution_duration = execution_end_time - execution_start_time;
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "ExecutionTime lci_strategic_plugin: " << std::chrono::duration<double>(execution_duration).count());
 
  return;
  // We need to evaluate the events so the state transitions can be triggered
}

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

	req	Plan maneuver request
[out]	resp	Plan maneuver response with a list of maneuver plan
	current_state	The current state of the vehicle at the start of planning
	traffic_light	The single traffic light along the vehicle's route in the intersection that is relevant and in front of the vehicle
	entry_lanelet	The entry lanelet into the intersection
	exit_lanelet	The exit lanelet into the intersection
	current_lanelet	The current lanelet the vehicle's state is on

Exceptions

if	given 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 817 of file lci_strategic_plugin.cpp.

{
 
  if (!traffic_light)  // If we are in the approaching state and there is no longer any lights ahead of us then
                       // the vehicle must have crossed the stop bar
  {
    transition_table_.signal(TransitEvent::CROSSED_STOP_BAR);
    return;
  }
 
  double current_state_speed = std::max(current_state.speed, config_.algo_minimum_speed * 1.001);
 
  auto stop_line = traffic_light->getStopLine(entry_lanelet);
 
  if (!stop_line)
  {
    throw std::invalid_argument("Given entry lanelet doesn't have stop_line...");
  }
 
  double traffic_light_down_track =
      wm_->routeTrackPos(stop_line.get().front().basicPoint2d()).downtrack;
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "traffic_light_down_track: " << traffic_light_down_track);
 
  double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.downtrack;
 
  distance_remaining_to_tf_ = distance_remaining_to_traffic_light;
 
  if (distance_remaining_to_traffic_light < 0) // there is small discrepancy between signal's routeTrackPos as well as current
  {                                            // downtrack calculated using veh_x,y from state. Therefore, it may have crossed it
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Crossed the bar distance_remaining_to_traffic_light: " << distance_remaining_to_traffic_light);
    transition_table_.signal(TransitEvent::CROSSED_STOP_BAR);
    return;
  }
 
  // At this point we know the vehicle is within the activation distance and we know the current and next light phases
  // All we need to now determine is if we should stop or if we should continue
  lanelet::ConstLanelet intersection_lanelet;
 
  auto route = wm_->getRoute()->shortestPath();
  auto entry_llt_iter = std::find(route.begin(), route.end(), entry_lanelet);
  if (entry_llt_iter != route.end())
  {
    intersection_lanelet = *(entry_llt_iter + 1);
  }
 
  if (intersection_lanelet.id() != lanelet::InvalId)
  {
    intersection_speed_ = findSpeedLimit(intersection_lanelet);
    intersection_turn_direction_ = getTurnDirectionAtIntersection({intersection_lanelet});
  }
  else
  {
    intersection_speed_ = findSpeedLimit(exit_lanelet);
  }
 
  intersection_speed_ = intersection_speed_.get() * 0.999; // so that speed_limit is not exactly same as departure or current speed
 
  double speed_limit = findSpeedLimit(current_lanelet);
 
  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
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "current_state_speed: " << current_state_speed);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "intersection_speed_: " << intersection_speed_.get());
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "distance_remaining_to_traffic_light: " << distance_remaining_to_traffic_light);
 
  intersection_end_downtrack_ =
      wm_->routeTrackPos(exit_lanelet.centerline2d().front().basicPoint2d()).downtrack;
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "intersection_end_downtrack_: " << intersection_end_downtrack_.get());
 
  // If the vehicle is at a stop trigger the stopped state
  constexpr double HALF_MPH_IN_MPS = 0.22352;
  if (current_state.speed < HALF_MPH_IN_MPS &&
      fabs(distance_remaining_to_traffic_light) < config_.stopping_location_buffer)
  {
    transition_table_.signal(TransitEvent::STOPPED);  // The vehicle has come to a stop at the light
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CARMA has detected that the vehicle stopped at the stop bar. Transitioning to WAITING STATE");
 
    return;
  }
 
 
  rclcpp::Time earliest_entry_time = current_state.stamp + get_earliest_entry_time(distance_remaining_to_traffic_light, speed_limit,
                                                  current_state_speed, intersection_speed_.get(), max_comfort_accel_, max_comfort_decel_);
 
  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()));
 
  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);
 
  if (nearest_green_entry_time == rclcpp::Time(0))
    return;
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Final nearest_green_entry_time: " << std::to_string(nearest_green_entry_time.seconds()));
 
  auto et_state = traffic_light->predictState(lanelet::time::timeFromSec(nearest_green_entry_time.seconds()));
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Signal at ET: " << et_state.get().second);
 
 
  double remaining_time = nearest_green_entry_time.seconds() - current_state.stamp.seconds();
  double remaining_time_earliest_entry = earliest_entry_time.seconds() - current_state.stamp.seconds();
  scheduled_entry_time_ = remaining_time; // performance metric
  earliest_entry_time_ = remaining_time_earliest_entry; // performance metric
 
  auto boundary_distances = get_delta_x(current_state_speed, intersection_speed_.get(), speed_limit, config_.algo_minimum_speed, max_comfort_accel_, max_comfort_decel_);
  print_boundary_distances(boundary_distances); //debug
 
  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);
 
  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);
  print_params(ts_params);
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "SPEED PROFILE CASE:" << ts_params.case_num);
 
  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
  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_);
 
  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
  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_);
 
  double desired_distance_to_stop = pow(current_state.speed, 2)/(2 * max_comfort_decel_norm_ * config_.deceleration_fraction) + config_.desired_distance_to_stop_buffer;
  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);
 
  emergency_distance_to_stop = std::max(emergency_distance_to_stop, config_.stopping_location_buffer);
  safe_distance_to_stop = std::max(safe_distance_to_stop, config_.stopping_location_buffer);
  desired_distance_to_stop = std::max(desired_distance_to_stop, config_.stopping_location_buffer);
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "new emergency_distance_to_stop: " << emergency_distance_to_stop);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "new safe_distance_to_stop: " << safe_distance_to_stop);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "new desired_distance_to_stop: " << desired_distance_to_stop);
 
  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);
 
  // Basic RED signal violation check
  if (distance_remaining_to_traffic_light <= emergency_distance_to_stop || last_case_num_ == TSCase::EMERGENCY_STOPPING)
  {
    if (in_tbd) // Given ET is in TBD, but vehicle is too close to intersection
    {
      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...");
    }
 
    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_
 
    rclcpp::Time stopping_arrival_time =
          current_state.stamp + rclcpp::Duration(stopping_time * 1e9);
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "stopping_arrival_time: " << std::to_string(stopping_arrival_time.seconds()));
 
    auto stopping_arrival_state_optional = traffic_light->predictState(lanelet::time::timeFromSec(stopping_arrival_time.seconds()));
 
    if (!validLightState(stopping_arrival_state_optional, stopping_arrival_time))
    {
      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()));
      return;
    }
 
    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
    {
      RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"),"Detected possible RED light violation! Stopping!");
      handleStopping(req,resp, current_state, traffic_light, entry_lanelet, exit_lanelet, current_lanelet, traffic_light_down_track, true); //case_11
      last_case_num_ = TSCase::EMERGENCY_STOPPING;
 
      return;
    }
  }
 
  // Check if the vehicle can arrive with certainty (Case 1-7)
  if (ts_params.is_algorithm_successful && ts_params.case_num != TSCase::CASE_8 &&
    (distance_remaining_to_traffic_light >= desired_distance_to_stop || !in_tbd) &&
    is_entry_time_within_green_or_tbd) // ET cannot be explicitly inside RED or YELLOW in available future states, which is ERROR case
  {
    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
 
    if (!resp->new_plan.maneuvers.empty()) // able to pass at green
    {
      last_case_num_ = ts_params.case_num;
      return;
    }
 
    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");
    ts_params = boundary_traj_params[7];
    ts_params.is_algorithm_successful = true; //false correspond to cases when vehicle is beyond safe_distance to stop for case8
    ts_params.case_num = CASE_8;
    print_params(ts_params);
  }
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Not able to make it with certainty: NEW TSCase: " << ts_params.case_num);
  // if algorithm is NOT successful or if the vehicle cannot make the green light with certainty
 
  if (desired_distance_to_stop < distance_remaining_to_traffic_light && last_case_num_ != TSCase::STOPPING) // do not switch from STOPPING to case8 again
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Way too early to stop. Cruising at CASE8");
    handleCruisingUntilStop(req, resp, current_state, current_state_speed, traffic_light, traffic_light_down_track, ts_params);
 
    if (!resp->new_plan.maneuvers.empty())
    {
      last_case_num_ = TSCase::CASE_8;
      return;
    }
  }
 
  if ((safe_distance_to_stop <= distance_remaining_to_traffic_light && desired_distance_to_stop >= distance_remaining_to_traffic_light) ||
    last_case_num_ == TSCase::STOPPING) // if stopping continue stopping until transition to planwhenWAITING
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Planning stopping now. last case:" << static_cast<int>(last_case_num_));
    handleStopping(req,resp, current_state, traffic_light, entry_lanelet, exit_lanelet, current_lanelet, traffic_light_down_track); //case_9
    return;
  }
 
  if (safe_distance_to_stop > distance_remaining_to_traffic_light)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "No longer within safe distance to stop! Decide to continue stopping or continue into intersection");
 
    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
    {
      // 3. if not able to stop nor reach target speed at green, attempt its best to reach the target parameters at the intersection
      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!");
 
      handleFailureCase(req, resp, current_state, current_state_speed, speed_limit, remaining_time,
                                    exit_lanelet.id(), traffic_light, traffic_light_down_track, ts_params);
 
      if (resp->new_plan.maneuvers.empty())
      {
        RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"),"HANDLE_SAFETY: Planning forced slow-down... last case:" << static_cast<int>(last_case_num_));
        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
      }
    }
    else
    {
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "HANDLE_SAFETY: Planning to keep stopping now. last case:" << static_cast<int>(last_case_num_));
      handleStopping(req,resp, current_state, traffic_light, entry_lanelet, exit_lanelet, current_lanelet, traffic_light_down_track); //case_9
    }
  }
 
 
}

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

	req	Plan maneuver request
[out]	resp	Plan maneuver response with a list of maneuver plan
	current_state	The current state of the vehicle at the start of planning
	intersection_end_downtrack	The ending downtrack in meters of the current intersection
	intersection_speed_limit	The speed limit of the current intersection

Definition at line 1126 of file lci_strategic_plugin.cpp.

{
  last_case_num_ = TSCase::UNAVAILABLE;
 
  if (current_state.downtrack > intersection_end_downtrack)
  {
    transition_table_.signal(TransitEvent::INTERSECTION_EXIT);  // If we are past the most recent
    return;
  }
 
  // Calculate exit time assuming constant acceleration
  rclcpp::Time intersection_exit_time =
      current_state.stamp +
      rclcpp::Duration(2.0 * (intersection_end_downtrack - current_state.downtrack) / (current_state.speed + intersection_speed_limit) * 1e9);
 
  // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver
  std::vector<lanelet::ConstLanelet> crossed_lanelets =
      getLaneletsBetweenWithException(current_state.downtrack, intersection_end_downtrack, true, false);
 
  // Compose intersection transit maneuver
  resp->new_plan.maneuvers.push_back(composeIntersectionTransitMessage(
      current_state.downtrack, intersection_end_downtrack, current_state.speed, intersection_speed_limit,
      current_state.stamp, intersection_exit_time, crossed_lanelets.back().id(), crossed_lanelets.back().id()));
}

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

	req	Plan maneuver request
[out]	resp	Plan maneuver response with a list of maneuver plan
	current_state	The current state of the vehicle at the start of planning
	traffic_light	The single traffic light along the vehicle's route in the intersection that is relevant and in front of the vehicle
	entry_lanelet	The entry lanelet into the intersection
	exit_lanelet	The exit lanelet into the intersection
	current_lanelet	The current lanelet the vehicle's state is on

Exceptions

if	given entry_lanelet doesn't have stop_line NOTE: returns empty if the given traffic light is empty

Definition at line 747 of file lci_strategic_plugin.cpp.

{
  // Reset intersection state since in this state we are not yet known to be in or approaching an intersection
  intersection_speed_ = boost::none;
  intersection_end_downtrack_ = boost::none;
  double current_state_speed = std::max(current_state.speed, config_.algo_minimum_speed * 1.001);
 
 
  if (!traffic_light)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "No lights found along route. Returning maneuver plan unchanged");
    return;
  }
 
  double max_comfort_accel = config_.vehicle_accel_limit * config_.vehicle_accel_limit_multiplier;
  double max_comfort_decel = -1.0 * config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier;
 
  auto stop_line = traffic_light->getStopLine(entry_lanelet);
 
  if (!stop_line)
  {
    throw std::invalid_argument("Given entry lanelet doesn't have stop_line...");
  }
 
  double traffic_light_down_track =
      wm_->routeTrackPos(stop_line.get().front().basicPoint2d()).downtrack;
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "traffic_light_down_track: " << traffic_light_down_track);
 
  double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.downtrack;
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "distance_remaining_to_traffic_light: " << distance_remaining_to_traffic_light <<
                    ", current_state.downtrack: " << current_state.downtrack);
 
  distance_remaining_to_tf_ = distance_remaining_to_traffic_light; // performance metric
 
  auto speed_limit = findSpeedLimit(current_lanelet);
 
  current_state_speed = std::min(speed_limit, current_state_speed);
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "speed_limit (free flow speed): " << speed_limit);
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "trajectory_smoothing_activation_distance: " << config_.trajectory_smoothing_activation_distance);
 
  double stopping_dist = estimate_distance_to_stop(current_state_speed, config_.vehicle_decel_limit_multiplier  *
                                                                            config_.vehicle_decel_limit); //accepts positive decel
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Stopping distance: " << stopping_dist);
 
  double plugin_activation_distance = std::max(stopping_dist, config_.min_approach_distance);
 
  plugin_activation_distance = std::max(plugin_activation_distance, config_.trajectory_smoothing_activation_distance);
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "plugin_activation_distance: " << plugin_activation_distance);
 
  if (distance_remaining_to_traffic_light <= plugin_activation_distance)
  {
    RCLCPP_INFO_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Within intersection range");
    transition_table_.signal(TransitEvent::IN_STOPPING_RANGE);  // Evaluate Signal and Run Trajectory Smoothing Algorithm
  }
  else
  {
    last_case_num_ = TSCase::UNAVAILABLE;
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Not within intersection range");
  }
 
}

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

	req	Plan maneuver request
[out]	resp	Plan maneuver response with a list of maneuver plan
	current_state	The current state of the vehicle at the start of planning
	traffic_light	The single traffic light along the vehicle's route in the intersection that is relevant and in front of the vehicle
	entry_lanelet	The entry lanelet into the intersection
	exit_lanelet	The exit lanelet into the intersection
	current_lanelet	The current lanelet the vehicle's state is on NOTE: returns empty if the given traffic light is empty

Definition at line 1060 of file lci_strategic_plugin.cpp.

{
  last_case_num_ = TSCase::STOPPING;
 
  if (!traffic_light)
  {
    throw std::invalid_argument("While in WAITING state, the traffic lights disappeared.");
  }
 
  auto stop_line = traffic_light->getStopLine(entry_lanelet);
 
  if (!stop_line)
  {
    throw std::invalid_argument("Given entry lanelet doesn't have stop_line...");
  }
 
  double traffic_light_down_track =
      wm_->routeTrackPos(stop_line.get().front().basicPoint2d()).downtrack;
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "traffic_light_down_track: "<<  traffic_light_down_track);
 
  double entering_time = current_state.stamp.seconds();
 
  auto current_light_state_optional = traffic_light->predictState(lanelet::time::timeFromSec(entering_time));
 
  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()));
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "WAITING STATE: requested time to CURRENT STATE check: " << std::to_string(entering_time));
 
  if (!validLightState(current_light_state_optional, rclcpp::Time(entering_time * 1e9)))
    return;
 
  auto bool_optional_late_certainty = canArriveAtGreenWithCertainty(rclcpp::Time(entering_time * 1e9), traffic_light, true, false);
 
  if (!bool_optional_late_certainty)
  {
    RCLCPP_ERROR_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Unable to resolve green light...");
    return;
  }
 
  bool should_enter = true; //uc2
 
  if (config_.enable_carma_streets_connection && entering_time > current_state.stamp.seconds()) //uc3
    should_enter = false;
 
  if (isStateAllowedGreen(current_light_state_optional.get().second) && bool_optional_late_certainty.get() && should_enter) // if can make with certainty
  {
    transition_table_.signal(TransitEvent::RED_TO_GREEN_LIGHT);  // If the light is green send the light transition
                                                                 // signal
    return;
  }
 
  // A fixed buffer to add to stopping maneuvers once the vehicle is already stopped to ensure that they can have their
  // trajectory planned
  constexpr double stop_maneuver_buffer = 10.0;
 
  // If the light is not green then continue waiting by creating a stop and wait maneuver on top of the vehicle
  double stopping_accel = config_.vehicle_decel_limit_multiplier * config_.vehicle_decel_limit;
 
  resp->new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage(
      current_state.downtrack - stop_maneuver_buffer, traffic_light_down_track, current_state.speed,
      current_state.lane_id, current_state.lane_id, rclcpp::Time(entering_time * 1e9),
      rclcpp::Time(entering_time * 1e9) + rclcpp::Duration(config_.min_maneuver_planning_period * 1e9), stopping_accel));
}

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.

{
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "\n");
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "dx1: " << delta_xs.dx1);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "dx2: " << delta_xs.dx2);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "dx3: " << delta_xs.dx3);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "dx4: " << delta_xs.dx4);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "dx5: " << delta_xs.dx5 << "\n");
}

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.

{
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "\n");
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "t0: " << std::to_string(params.t0_));
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "v0: " << params.v0_);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "x0: " << params.x0_);
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "t1: " << std::to_string(params.t1_));
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "v1: " << params.v1_);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "x1: " << params.x1_);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "a1: " << params.a1_);
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "t2: " << std::to_string(params.t2_));
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "v2: " << params.v2_);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "x2: " << params.x2_);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "a2: " << params.a2_);
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "t3: " << std::to_string(params.t3_));
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "v3: " << params.v3_);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "x3: " << params.x3_);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "a3: " << params.a3_);
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "\n");
}

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

{
  if (approaching_light_controlled_intersection_)
  {
    carma_v2x_msgs::msg::MobilityOperation status_msg = generateMobilityOperation();
    mobility_operation_pub_->publish(status_msg);
  }
}

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

{
  std_msgs::msg::Int8 case_num_msg;
  std_msgs::msg::Float64 tf_distance;
  std_msgs::msg::Float64 earliest_et;
  std_msgs::msg::Float64 scheduled_et;
 
  case_num_msg.data = static_cast<int>(last_case_num_);
  tf_distance.data = distance_remaining_to_tf_;
  earliest_et.data = earliest_entry_time_;
  scheduled_et.data = scheduled_entry_time_;
 
  case_pub_->publish(case_num_msg);
  tf_distance_pub_->publish(tf_distance);
  earliest_et_pub_->publish(earliest_et);
  et_pub_->publish(scheduled_et);
}

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

state The state to evaluate

Returns: true if the state is supported, flase otherwise

Definition at line 191 of file lci_strategic_plugin.cpp.

{
  switch (state)
  {
    // NOTE: Following cases are intentional fall through.
    // Supported light states
    case lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN:              // Solid Red
    case lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE:          // Yellow Solid no chance of conflicting traffic
    case lanelet::CarmaTrafficSignalState::PERMISSIVE_MOVEMENT_ALLOWED:  // Solid Green there could be conflict traffic
    case lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED:   // Solid Green no chance of conflict traffic
                                                                        // (normally used with arrows)
      return true;
 
    // Unsupported light states
    case lanelet::CarmaTrafficSignalState::PERMISSIVE_CLEARANCE:         // Yellow Solid there is a chance of conflicting
                                                                        // traffic
    case lanelet::CarmaTrafficSignalState::UNAVAILABLE:                  // No data available
    case lanelet::CarmaTrafficSignalState::DARK:                         // Light is non-functional
    case lanelet::CarmaTrafficSignalState::STOP_THEN_PROCEED:            // Flashing Red
 
    case lanelet::CarmaTrafficSignalState::PRE_MOVEMENT:                 // Yellow Red transition (Found only in the EU)
                                                                        // (normally used with arrows)
    case lanelet::CarmaTrafficSignalState::CAUTION_CONFLICTING_TRAFFIC:  // Yellow Flashing
    default:
      return false;
  }
}

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.

{
  TrajectoryParams traj;
 
  traj.t0_ = t;
  traj.v0_ = v0;
  traj.x0_ = x0;
 
  double dt = et - t;
  double nom1 = 2 * dx * (((1 - (a_max / a_min)) * v_max) + ((a_max / a_min) * v1) - v0);
  double nom2 = dt * (((1 - (a_max / a_min)) * pow(v_max, 2)) + ((a_max / a_min) * pow(v1, 2)) - pow(v0, 2));
  double den = pow(v_max - v0, 2) - ((a_max / a_min) * pow(v_max - v1, 2));
 
  if (den <= epsilon_ && den >= -epsilon_)
    throw std::invalid_argument("CASE1: Received den near zero..." + std::to_string(den));
 
  double tc = (nom1 - nom2) / den;
 
  traj.v1_ = v_max;
 
  if (dt - tc <= epsilon_ && dt - tc >= -epsilon_)
    throw std::invalid_argument("CASE1: Received dt - tc near zero..." + std::to_string(dt - tc));
 
  traj.a1_ = (((1 - (a_max / a_min)) * v_max) + ((a_max / a_min) * v1) - v0) / (dt - tc);
 
  if (traj.a1_ <= accel_epsilon_ && traj.a1_ >= -accel_epsilon_)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CASE1: Received traj.a1_ near zero...");
    traj.t1_ = traj.t0_ + ((dt - tc) * (a_max / (a_min + a_max)));
    traj.x1_ = traj.x0_ + (v_max * (traj.t1_ - traj.t0_));
  }
  else
  {
    traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
    traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
  }
  traj.a2_ = 0;
  traj.v2_ = v_max;
  traj.t2_ = traj.t1_ + tc;
  traj.x2_ = traj.x1_ + (v_max * tc);
 
  traj.t3_ = et;
  traj.a3_ = traj.a1_ * (a_min / a_max);
  traj.v3_ = v1;
  traj.x3_ = x_end;
 
  return traj;
 
}

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.

{
  TrajectoryParams traj;
 
  traj.t0_ = t;
  traj.v0_ = v0;
  traj.x0_ = x0;
 
  double dt = et - t;
 
  if (dt <= epsilon_ && dt >= -epsilon_)
    throw std::invalid_argument("CASE2: Received dt near zero..." + std::to_string(dt));
 
  double sqr1 = pow(1 - (a_max / a_min), 2) * pow(dx / dt, 2);
  double sqr2 = (1 - (a_max / a_min)) * (((a_max / a_min) * v1 * (v1 - (2 * dx / dt))) + (v0 * ((2 * dx / dt) - v0)));
  double v_hat = (dx / dt) + (sqrt(sqr1 - sqr2) / (1 - (a_max / a_min)));
 
  traj.v1_ = v_hat;
  traj.a1_ = (((1 - (a_max / a_min)) * v_hat) + ((a_max / a_min) * v1) - v0) / dt;
 
  if (traj.a1_ <= accel_epsilon_ && traj.a1_ >= -accel_epsilon_)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CASE2: Received traj.a1_ near zero...");
    traj.t1_ = traj.t0_ + (dt * (a_max / (a_min + a_max)));
    traj.x1_ = traj.x0_ + (v_hat * (traj.t1_ - traj.t0_));
  }
  else
  {
    traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
    traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
  }
 
  traj.v2_ = v1;
  traj.a2_ = traj.a1_ * a_min / a_max;
 
  if (traj.a2_ <= accel_epsilon_ && traj.a2_ >= -accel_epsilon_)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CASE2: Received traj.a2_ near zero...");
    traj.t2_ = traj.t1_ + (dt * (a_min / (a_min + a_max)));
    traj.x2_ = traj.x1_ + (v_hat * (traj.t2_ - traj.t1_));
  }
  else
  {
    traj.t2_ = traj.t1_ + ((traj.v2_ - traj.v1_) / traj.a2_);
    traj.x2_ = x_end;
  }
 
  traj.t3_ = traj.t2_;
  traj.a3_ = 0;
  traj.v3_ = traj.v2_;
  traj.x3_ = traj.x2_;
 
  return traj;
}

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.

{
  TrajectoryParams traj;
 
  traj.t0_ = t;
  traj.v0_ = v0;
  traj.x0_ = x0;
 
  double dt = et - t;
 
  if (dt <= epsilon_ && dt >= -epsilon_)
    throw std::invalid_argument("CASE3: Received dt near zero..." + std::to_string(dt));
 
  double sqr1 = pow((a_max / a_min) - 1, 2) * pow(dx / dt, 2);
  double sqr2 = ((a_max / a_min) - 1) * ((v1 * (v1 - (2 * dx / dt))) + ((a_max / a_min) * v0 * ((2 * dx / dt) - v0)));
  double v_hat = (dx / dt) + (sqrt(sqr1 - sqr2) / ((a_max / a_min) - 1));
 
  traj.v1_ = v_hat;
  traj.a1_ = (((1 - (a_min / a_max)) * v_hat) + ((a_min / a_max) * v1) - v0) / dt;
 
  if (traj.a1_ <= accel_epsilon_ && traj.a1_ >= -accel_epsilon_)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CASE3: Received traj.a1_ near zero...");
    traj.t1_ = traj.t0_ + (dt * (a_max / (a_min + a_max)));
    traj.x1_ = traj.x0_ + (v_hat * (traj.t1_ - traj.t0_));
  }
  else
  {
    traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
    traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
  }
 
  traj.v2_ = v1;
  traj.a2_ = traj.a1_ * a_max / a_min;
  traj.t2_ = traj.t1_ + ((traj.v2_ - traj.v1_) / traj.a2_);
  traj.x2_ = x_end;
 
  traj.t3_ = traj.t2_;
  traj.a3_ = 0;
  traj.v3_ = traj.v2_;
  traj.x3_ = traj.x2_;
 
  return traj;
}

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.

{
  TrajectoryParams traj;
 
  traj.t0_ = t;
  traj.v0_ = v0;
  traj.x0_ = x0;
 
  double dt = et - t;
  double nom1 = 2 * dx * ((((a_max / a_min) - 1) * v_min) + v1 - ((a_max / a_min) * v0));
  double nom2 = dt * ((((a_max / a_min) - 1) * pow(v_min, 2)) + pow(v1, 2) - ((a_max / a_min) * pow(v0, 2)));
  double den = ((a_max / a_min) * pow(v_min - v0, 2)) - pow(v_min - v1, 2);
 
  if (den <= epsilon_ && den >= -epsilon_)
    throw std::invalid_argument("CASE4: Received den near zero..." + std::to_string(den));
 
  double tc = (nom1 - nom2) / den;
 
  traj.v1_ = v_min;
 
  if (dt - tc <= epsilon_ && dt - tc >= -epsilon_)
    throw std::invalid_argument("CASE4: Received dt - tc near zero..." + std::to_string(dt - tc));
 
  traj.a1_ = (((1 - (a_min / a_max)) * v_min) + ((a_min / a_max) * v1) - v0) / (dt - tc);
 
  if (traj.a1_ <= accel_epsilon_ && traj.a1_ >= -accel_epsilon_)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CASE4: Received traj.a1_ near zero...");
    traj.t1_ = traj.t0_ + ((dt - tc) * (a_min / (a_min + a_max)));
    traj.x1_ = traj.x0_ + (v_min * (traj.t1_ - traj.t0_));
  }
  else
  {
    traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
    traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
  }
 
  traj.v2_ = v_min;
  traj.a2_ = 0;
  traj.t2_ = traj.t1_ + tc;
  traj.x2_ = traj.x1_ + (v_min * tc);
 
  traj.t3_ = et;
  traj.a3_ = traj.a1_ * a_max / a_min;
  traj.v3_ = v1;
  traj.x3_ = x_end;
 
  return traj;
}

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.

{
  TrajectoryParams traj;
 
  traj.t0_ = t;
  traj.v0_ = v0;
  traj.x0_ = x0;
 
  double dt = et - t;
  double sqr = ((a_max / a_min) - 1) * ((2 * a_min * (dx - (v0 * dt))) - pow(a_min * dt, 2));
  double v_hat = (v0 + (a_min * dt)) - (sqrt(sqr) / ((a_max / a_min) - 1));
  double v_p = ((1 - (a_max / a_min)) * v_hat) + ((a_max / a_min) * v0) + (a_max * dt);
 
  traj.v1_ = v_hat;
  traj.a1_ = a_min;
 
  traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
  traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
 
  traj.v2_ = v_p;
  traj.a2_ = a_max;
  traj.t2_ = traj.t1_ + ((traj.v2_ - traj.v1_) / traj.a2_);
  traj.x2_ = x_end;
 
  traj.t3_ = traj.t2_;
  traj.a3_ = 0;
  traj.v3_ = traj.v2_;
  traj.x3_ = traj.x2_;
 
  return traj;
}

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.

{
  TrajectoryParams traj;
 
  traj.t0_ = t;
  traj.v0_ = v0;
  traj.x0_ = x0;
 
  double dt = et - t;
 
  traj.v1_ = v_min;
  traj.a1_ = a_min;
  traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
  traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
 
  double tc;
 
  if (dx <= dx3)
    tc = 0;
  else
    tc = traj6.t2_ - traj6.t1_;
 
  traj.v2_ = v_min;
  traj.a2_ = 0;
  traj.t2_ = traj.t1_ + tc;
  traj.x2_ = traj.x1_ + (v_min * tc);
 
  double dt_p = dt - (traj.t1_ - traj.t0_) - tc;
 
  if (dt_p <= epsilon_ && dt_p >= -epsilon_)
    throw std::invalid_argument("CASE6: Received dt_p near zero..." + std::to_string(dt_p));
 
  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);
 
  traj.v3_ = v_p;
  traj.a3_ = (v_p - v_min) / dt_p;
  traj.t3_ = et;
  traj.x3_ = x_end;
 
  return traj;
}

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.

{
  TrajectoryParams traj;
 
  traj.t0_ = t;
  traj.v0_ = v0;
  traj.x0_ = x0;
 
  traj.v1_ = v_min;
  traj.a1_ = a_min;
  traj.t1_ = traj.t0_ + ((traj.v1_ - traj.v0_) / traj.a1_);
  traj.x1_ = traj.x0_ + ((pow(traj.v1_, 2) - pow(traj.v0_, 2)) / (2 * traj.a1_));
 
  double dt = et - t;
  double v_p = v_min - sqrt(pow(v_min - v0, 2) - (2 * a_min * ((v_min * dt) - dx)));
  double dt_p = (v_p - v_min) / a_min;
 
  if (dt_p <= epsilon_ && dt_p >= -epsilon_)
    throw std::invalid_argument("CASE7: Received dt_p near zero..." + std::to_string(dt_p));
 
  double tc = dt - ((v_p - v0) / a_min);
 
  traj.v2_ = v_min;
  traj.a2_ = 0;
  traj.t2_ = traj.t1_ + tc;
  traj.x2_ = traj.x1_ + (v_min * tc);
 
  traj.v3_ = v_p;
  traj.a3_ = (v_p - v_min) / dt_p;
  traj.t3_ = et;
  traj.x3_ = x_end;
 
  return traj;
}

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.

{
  TrajectoryParams traj = traj8;
  if (dx < dx5)
  {
    traj.is_algorithm_successful = false;
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "CASE8: Not within safe stopping distance originally planned!");
  }
  return traj;
}

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_state	An optional light state and its min_end_time pair. If this is unset the method will return false
source_time	The 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 277 of file lci_strategic_plugin.cpp.

{
  if (!optional_state)
  {
    RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"),"Traffic light data not available for source_time " << std::to_string(source_time.seconds()));
    return false;
  }
 
  lanelet::CarmaTrafficSignalState light_state = optional_state.get().second;
 
  if (!supportedLightState(light_state))
  {
    RCLCPP_ERROR_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "LCIStrategic Plugin asked to handle CarmaTrafficSignalState: " << light_state
                                                                                << " which is not supported.");
    return false;
  }
 
  return true;
}

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

Definition at line 253 of file lci_strategic_plugin.hpp.

Referenced by boundary_accel_nocruise_maxspeed_decel(), ts_case1(), ts_case2(), ts_case3(), and ts_case4().

◆ approaching_light_controlled_intersection_

bool lci_strategic_plugin::LCIStrategicPlugin::approaching_light_controlled_intersection_ = false

private

Definition at line 233 of file lci_strategic_plugin.hpp.

Referenced by mobilityOperationCb(), plan_maneuvers_callback(), and publishMobilityOperation().

◆ bsm_id_

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

private

Definition at line 208 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 209 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 210 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 264 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

Definition at line 268 of file lci_strategic_plugin.hpp.

Referenced by on_configure_plugin(), and publishTrajectorySmoothingInfo().

◆ config_

LCIStrategicPluginConfig lci_strategic_plugin::LCIStrategicPlugin::config_

private

Config containing configurable algorithm parameters.

Definition at line 236 of file lci_strategic_plugin.hpp.

Referenced by LCIStrategicPlugin(), canArriveAtGreenWithCertainty(), composeIntersectionTransitMessage(), composeStopAndWaitManeuverMessage(), composeTrajectorySmoothingManeuverMessage(), generateMobilityOperation(), get_earliest_entry_time(), get_final_entry_time_and_conditions(), handleCruisingUntilStop(), handleStopping(), mobilityOperationCb(), on_activate_plugin(), on_configure_plugin(), parameter_update_callback(), plan_maneuvers_callback(), planWhenAPPROACHING(), planWhenUNAVAILABLE(), and planWhenWAITING().

◆ distance_remaining_to_tf_

double lci_strategic_plugin::LCIStrategicPlugin::distance_remaining_to_tf_ = 0.0

private

Definition at line 228 of file lci_strategic_plugin.hpp.

Referenced by planWhenAPPROACHING(), planWhenUNAVAILABLE(), and publishTrajectorySmoothingInfo().

◆ earliest_entry_time_

double lci_strategic_plugin::LCIStrategicPlugin::earliest_entry_time_ = 0.0

private

Definition at line 229 of file lci_strategic_plugin.hpp.

Referenced by planWhenAPPROACHING(), and publishTrajectorySmoothingInfo().

◆ earliest_et_pub_

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

private

Definition at line 270 of file lci_strategic_plugin.hpp.

Referenced by on_configure_plugin(), and publishTrajectorySmoothingInfo().

◆ emergency_decel_norm_

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

private

Definition at line 216 of file lci_strategic_plugin.hpp.

Referenced by LCIStrategicPlugin(), handleStopping(), on_configure_plugin(), and planWhenAPPROACHING().

◆ epsilon_

double lci_strategic_plugin::LCIStrategicPlugin::epsilon_ = 0.001

private

Definition at line 252 of file lci_strategic_plugin.hpp.

Referenced by boundary_accel_nocruise_maxspeed_decel(), boundary_accel_or_decel_complete_upper(), boundary_accel_or_decel_incomplete_upper(), boundary_decel_nocruise_minspeed_accel_complete(), get_distance_to_accel_or_decel_once(), get_earliest_entry_time(), get_trajectory_smoothing_activation_distance(), handleFailureCaseHelper(), planWhenAPPROACHING(), ts_case1(), ts_case2(), ts_case3(), ts_case4(), ts_case6(), and ts_case7().

◆ et_pub_

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

private

Definition at line 271 of file lci_strategic_plugin.hpp.

Referenced by on_configure_plugin(), and publishTrajectorySmoothingInfo().

◆ frontbumper_transform_

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

private

Definition at line 249 of file lci_strategic_plugin.hpp.

◆ intersection_end_downtrack_

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

private

Definition at line 243 of file lci_strategic_plugin.hpp.

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

◆ 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 242 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 232 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 227 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 250 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

Definition at line 244 of file lci_strategic_plugin.hpp.

Referenced by composeTrajectorySmoothingManeuverMessage(), generateMobilityOperation(), and mobilityOperationCb().

◆ max_comfort_accel_

double lci_strategic_plugin::LCIStrategicPlugin::max_comfort_accel_ = 2.0

private

Definition at line 213 of file lci_strategic_plugin.hpp.

Referenced by LCIStrategicPlugin(), handleFailureCaseHelper(), on_configure_plugin(), and planWhenAPPROACHING().

◆ max_comfort_decel_

double lci_strategic_plugin::LCIStrategicPlugin::max_comfort_decel_ = -2.0

private

Definition at line 214 of file lci_strategic_plugin.hpp.

Referenced by LCIStrategicPlugin(), handleFailureCaseHelper(), on_configure_plugin(), and planWhenAPPROACHING().

◆ max_comfort_decel_norm_

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

private

Definition at line 215 of file lci_strategic_plugin.hpp.

Referenced by LCIStrategicPlugin(), handleStopping(), on_configure_plugin(), and planWhenAPPROACHING().

◆ mob_op_pub_timer_

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

private

Definition at line 259 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 263 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 267 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

Definition at line 218 of file lci_strategic_plugin.hpp.

Referenced by get_final_entry_time_and_conditions(), and parseStrategyParams().

◆ previous_strategy_params_

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

private

Definition at line 204 of file lci_strategic_plugin.hpp.

Referenced by mobilityOperationCb().

◆ scheduled_enter_time_

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

private

Definition at line 203 of file lci_strategic_plugin.hpp.

Referenced by get_final_entry_time_and_conditions(), parseStrategyParams(), and plan_maneuvers_callback().

◆ scheduled_entry_time_

double lci_strategic_plugin::LCIStrategicPlugin::scheduled_entry_time_ = 0.0

private

Definition at line 230 of file lci_strategic_plugin.hpp.

Referenced by planWhenAPPROACHING(), and publishTrajectorySmoothingInfo().

◆ street_msg_timestamp_

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

private

Definition at line 201 of file lci_strategic_plugin.hpp.

Referenced by mobilityOperationCb().

◆ tf2_buffer_

tf2_ros::Buffer lci_strategic_plugin::LCIStrategicPlugin::tf2_buffer_

private

Definition at line 247 of file lci_strategic_plugin.hpp.

Referenced by lookupFrontBumperTransform().

◆ tf2_listener_

std::unique_ptr<tf2_ros::TransformListener> lci_strategic_plugin::LCIStrategicPlugin::tf2_listener_

private

Definition at line 248 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

Definition at line 269 of file lci_strategic_plugin.hpp.

Referenced by on_configure_plugin(), and publishTrajectorySmoothingInfo().

◆ transition_table_

LCIStrategicStateTransitionTable lci_strategic_plugin::LCIStrategicPlugin::transition_table_

private

State Machine Transition table.

Definition at line 239 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 260 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 205 of file lci_strategic_plugin.hpp.

Referenced by generateMobilityOperation(), and plan_maneuvers_callback().

◆ wm_

carma_wm::WorldModelConstPtr lci_strategic_plugin::LCIStrategicPlugin::wm_

private

World Model pointer.

Definition at line 256 of file lci_strategic_plugin.hpp.

Referenced by findSpeedLimit(), getLaneletsBetweenWithException(), on_configure_plugin(), plan_maneuvers_callback(), planWhenAPPROACHING(), planWhenUNAVAILABLE(), and planWhenWAITING().

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

lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.hpp
lci_strategic_plugin/src/lci_strategic_plugin.cpp
lci_strategic_plugin/src/lci_strategic_plugin_algo.cpp

Classes

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ LCIStrategicPlugin()

Member Function Documentation

◆ 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()

◆ BSMCb()

◆ calc_estimated_entry_time_left()

◆ canArriveAtGreenWithCertainty()

◆ composeIntersectionTransitMessage()

◆ composeStopAndWaitManeuverMessage()

◆ composeTrajectorySmoothingManeuverMessage()

◆ estimate_distance_to_stop()

◆ estimate_time_to_stop()

◆ extractInitialState()

◆ findSpeedLimit()

◆ FRIEND_TEST() [1/27]

◆ FRIEND_TEST() [2/27]

◆ FRIEND_TEST() [3/27]

◆ FRIEND_TEST() [4/27]

◆ FRIEND_TEST() [5/27]

◆ FRIEND_TEST() [6/27]

◆ FRIEND_TEST() [7/27]

◆ FRIEND_TEST() [8/27]

◆ FRIEND_TEST() [9/27]

◆ FRIEND_TEST() [10/27]

◆ FRIEND_TEST() [11/27]

◆ FRIEND_TEST() [12/27]

◆ FRIEND_TEST() [13/27]

◆ FRIEND_TEST() [14/27]

◆ FRIEND_TEST() [15/27]

◆ FRIEND_TEST() [16/27]

◆ FRIEND_TEST() [17/27]

◆ FRIEND_TEST() [18/27]

◆ FRIEND_TEST() [19/27]

◆ FRIEND_TEST() [20/27]

◆ FRIEND_TEST() [21/27]

◆ FRIEND_TEST() [22/27]

◆ FRIEND_TEST() [23/27]

◆ FRIEND_TEST() [24/27]

◆ FRIEND_TEST() [25/27]

◆ FRIEND_TEST() [26/27]

◆ FRIEND_TEST() [27/27]

◆ generateMobilityOperation()

◆ get_availability()

◆ get_boundary_traj_params()

◆ get_delta_x()

◆ get_distance_to_accel_or_decel_once()

◆ get_distance_to_accel_or_decel_twice()

◆ get_earliest_entry_time()

◆ get_eet_or_tbd()

◆ get_final_entry_time_and_conditions()

◆ get_inflection_speed_value()

◆ get_nearest_green_entry_time()

◆ get_trajectory_smoothing_activation_distance()

◆ get_ts_case()

◆ get_version_id()

◆ getLaneletsBetweenWithException()

◆ getMovementAllowedDuration()

◆ getTurnDirectionAtIntersection()

◆ handleCruisingUntilStop()

◆ handleFailureCase()

◆ handleFailureCaseHelper()

◆ handleGreenSignalScenario()

◆ handleStopping()

◆ isStateAllowedGreen()