#include <sci_strategic_plugin.hpp>

Inheritance diagram for sci_strategic_plugin::SCIStrategicPlugin:

Collaboration diagram for sci_strategic_plugin::SCIStrategicPlugin:

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

Public Member Functions
	SCIStrategicPlugin (const rclcpp::NodeOptions &)
	Default constructor for RouteFollowingPlugin class. More...

void	publishMobilityOperation ()
	Method to publish mobility operation msgs at a fixed rate of 10hz if approaching intersection. 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...

void	mobilityOperationCb (carma_v2x_msgs::msg::MobilityOperation::UniquePtr msg)
	callback function for mobility operation More...

void	currentPoseCb (geometry_msgs::msg::PoseStamped::UniquePtr msg)
	callback function for current pose More...

carma_planning_msgs::msg::Maneuver	composeLaneFollowingManeuverMessage (int case_num, double start_dist, double end_dist, double start_speed, double target_speed, rclcpp::Time start_time, double time_to_stop, std::vector< lanelet::Id > lane_ids)
	Compose a lane keeping maneuver message based on input params. 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, double stopping_accel, rclcpp::Time start_time, rclcpp::Time end_time) const

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, TurnDirection turn_direction, const lanelet::Id &starting_lane_id, const lanelet::Id &ending_lane_id) const

VehicleState	extractInitialState (const carma_planning_msgs::srv::PlanManeuvers::Request &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...

double	findSpeedLimit (const lanelet::ConstLanelet &llt) const
	Given a Lanelet, find it's associated Speed Limit. More...

int	determine_speed_profile_case (double stop_dist, double current_speed, double schedule_stop_time, double speed_limit)
	Determine the speed profile case fpr approaching an intersection. More...

double	calc_speed_before_decel (double stop_time, double stop_dist, double current_speed) const
	calculate the speed, right before the car starts to decelerate for stopping More...

void	parseStrategyParams (const std::string &strategy_params)
	parse strategy parameters More...

double	calcEstimatedStopTime (double stop_dist, double current_speed) const
	calculate the time vehicle will stop with optimal decelarion More...

void	caseOneSpeedProfile (double speed_before_decel, double current_speed, double stop_time, std::vector< double > *float_metadata_list) const
	Determine the desired speed profile parameters for Case 1. More...

void	caseTwoSpeedProfile (double stop_dist, double speed_before_decel, double current_speed, double stop_time, double speed_limit, std::vector< double > *float_metadata_list) const
	Determine the desired speed profile parameters for Case 2. More...

double	caseThreeSpeedProfile (double stop_dist, double current_speed, double stop_time) const
	Determine the desired speed profile parameters for Case 3. More...

carma_v2x_msgs::msg::MobilityOperation	generateMobilityOperation ()
	Generates Mobility Operation messages. More...

void	BSMCb (carma_v2x_msgs::msg::BSM::UniquePtr msg)
	BSM callback function. More...

void	guidance_state_cb (const carma_planning_msgs::msg::GuidanceState::UniquePtr msg)
	Callback for the Guidance State. More...

TurnDirection	getTurnDirectionAtIntersection (std::vector< lanelet::ConstLanelet > lanelets_list)
	Determine the turn direction at intersection. 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...

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)

Public Attributes
unsigned long long	street_msg_timestamp_ = 0

unsigned long long	scheduled_stop_time_ = 0

unsigned long long	scheduled_enter_time_ = 0

unsigned long long	scheduled_depart_time_ = 0

uint32_t	scheduled_departure_position_ = std::numeric_limits<uint32_t>::max()

bool	is_allowed_int_ = false

TurnDirection	intersection_turn_direction_ = TurnDirection::Straight

bool	vehicle_engaged_ = false

double	speed_limit_ = 100.0

double	current_downtrack_ = 0.0

bool	approaching_stop_controlled_interction_ = false

std::string	bsm_id_ = "default_bsm_id"

uint8_t	bsm_msg_count_ = 0

uint16_t	bsm_sec_mark_ = 0

Private Member Functions
carma_wm::WorldModelConstPtr	get_wm ()

void	set_wm (carma_wm::WorldModelConstPtr new_wm)

	FRIEND_TEST (SCIStrategicPluginTest, findSpeedLimit)

	FRIEND_TEST (SCIStrategicPluginTest, moboperationcbtest)

	FRIEND_TEST (SCIStrategicPluginTest, parseStrategyParamstest)

	FRIEND_TEST (SCIStrategicPluginTest, calcEstimatedStopTimetest)

	FRIEND_TEST (SCIStrategicPluginTest, calc_speed_before_deceltest)

	FRIEND_TEST (SCIStrategicPluginTest, determine_speed_profile_casetest)

	FRIEND_TEST (SCIStrategicPluginTest, caseOneSpeedProfiletest)

	FRIEND_TEST (SCIStrategicPluginTest, caseTwoSpeedProfiletest)

	FRIEND_TEST (SCIStrategicPluginTest, caseThreeSpeedProfiletest)

	FRIEND_TEST (SCIStrategicPluginTest, testIntersectionturndirection)

	FRIEND_TEST (SCIStrategicPluginTest, DISABLED_maneuvercbtest)

	FRIEND_TEST (SCIStrategicPluginTest, maneuvercbtest)

Private Attributes
carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::MobilityOperation >	mob_operation_sub_

carma_ros2_utils::SubPtr< geometry_msgs::msg::PoseStamped >	current_pose_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::SubPtr< carma_planning_msgs::msg::GuidanceState >	guidance_state_sub_

bool	guidance_engaged_ = false

rclcpp::TimerBase::SharedPtr	mob_op_pub_timer_

carma_wm::WorldModelConstPtr	wm_
	World Model pointer. More...

SCIStrategicPluginConfig	config_
	Config containing configurable algorithm parameters. 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	stop_controlled_intersection_strategy_ = "Carma/stop_controlled_intersection"

std::string	previous_strategy_params_ = ""

Detailed Description

Definition at line 48 of file sci_strategic_plugin.hpp.

Constructor & Destructor Documentation

◆ SCIStrategicPlugin()

sci_strategic_plugin::SCIStrategicPlugin::SCIStrategicPlugin ( const rclcpp::NodeOptions & options )

explicit

Default constructor for RouteFollowingPlugin class.

Definition at line 70 of file sci_strategic_plugin.cpp.

  : carma_guidance_plugins::StrategicPlugin(options), config_(SCIStrategicPluginConfig())
{
  // Declare parameters
  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_.stop_line_buffer = declare_parameter<double>("stop_line_buffer",   config_.stop_line_buffer);
  config_.delta_t = declare_parameter<double>("delta_t",   config_.delta_t);
  config_.min_gap = declare_parameter<double>("min_gap",   config_.min_gap);
  config_.reaction_time = declare_parameter<double>("reaction_time",   config_.reaction_time);
  config_.intersection_exit_zone_length = declare_parameter<double>("intersection_exit_zone_length",   config_.intersection_exit_zone_length);
  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_.intersection_transit_plugin_name = declare_parameter<std::string>("intersection_transit_plugin_name", config_.intersection_transit_plugin_name);
  config_.vehicle_id = declare_parameter<std::string>("vehicle_id", config_.vehicle_id);
  config_.veh_length = declare_parameter<double>("vehicle_length", config_.veh_length);
  config_.vehicle_decel_limit = declare_parameter<double>("vehicle_deceleration_limit", config_.vehicle_decel_limit);
  config_.vehicle_accel_limit = declare_parameter<double>("vehicle_acceleration_limit", config_.vehicle_accel_limit);
 
};

Member Function Documentation

◆ BSMCb()

void sci_strategic_plugin::SCIStrategicPlugin::BSMCb ( carma_v2x_msgs::msg::BSM::UniquePtr msg )

BSM callback function.

Definition at line 218 of file sci_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_speed_before_decel()

double sci_strategic_plugin::SCIStrategicPlugin::calc_speed_before_decel	(	double	stop_time,
		double	stop_dist,
		double	current_speed
	)		const

calculate the speed, right before the car starts to decelerate for stopping

Parameters

stop_time	time the vehicle must stop
stop_dist	distance to stop line
current_speed	current speed of vehicle

Returns: speed value

Definition at line 315 of file sci_strategic_plugin.cpp.

{
  double speed_before_decel = 0;
 
  double desired_acceleration = config_.vehicle_accel_limit * config_.vehicle_accel_limit_multiplier;
  double desired_deceleration = -1 * config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier;
 
  double sqr_term = sqrt(pow(1 - (desired_acceleration/desired_deceleration), 2) * pow(stop_dist/stop_time, 2) 
                        + (1 - (desired_acceleration/desired_deceleration))*(current_speed*current_speed - 2*current_speed*stop_dist/stop_time));
 
  speed_before_decel = (stop_dist/stop_time) + sqr_term/(1 - (desired_acceleration/desired_deceleration));
 
  return speed_before_decel;
}

References config_, sci_strategic_plugin::SCIStrategicPluginConfig::vehicle_accel_limit, sci_strategic_plugin::SCIStrategicPluginConfig::vehicle_accel_limit_multiplier, sci_strategic_plugin::SCIStrategicPluginConfig::vehicle_decel_limit, and sci_strategic_plugin::SCIStrategicPluginConfig::vehicle_decel_limit_multiplier.

Referenced by composeLaneFollowingManeuverMessage(), and determine_speed_profile_case().

Here is the caller graph for this function:

◆ calcEstimatedStopTime()

double sci_strategic_plugin::SCIStrategicPlugin::calcEstimatedStopTime	(	double	stop_dist,
		double	current_speed
	)		const

calculate the time vehicle will stop with optimal decelarion

Parameters

stop_dist	distance to stop line
current_speed	current speed of vehicle

Returns: the time vehicle will stop with optimal decelarion

Definition at line 307 of file sci_strategic_plugin.cpp.

{
  
  double t_stop = 0;
  t_stop = 2*stop_dist/current_speed;
  return t_stop;
}

Referenced by determine_speed_profile_case().

Here is the caller graph for this function:

◆ caseOneSpeedProfile()

void sci_strategic_plugin::SCIStrategicPlugin::caseOneSpeedProfile	(	double	speed_before_decel,
		double	current_speed,
		double	stop_time,
		std::vector< double > *	float_metadata_list
	)		const

Determine the desired speed profile parameters for Case 1.

Parameters

speed_before_decel	vehicle speed before it starts to decelerate
current_speed	current speed of vehicle
stop_time	time duration to stop in s
float_metadata_list	metadata vector for storing speed profile parameters

Definition at line 652 of file sci_strategic_plugin.cpp.

{
  double desired_acceleration = config_.vehicle_accel_limit * config_.vehicle_accel_limit_multiplier;
  double desired_deceleration = -1 * config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier;  
 
  // Equations obtained from TSMO UC 1 Algorithm draft doc
  double a_acc = ((1 - desired_acceleration/desired_deceleration)*speed_before_decel - current_speed)/stop_time;
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Case one a_acc: " << a_acc);
  a_acc = std::min(a_acc, desired_acceleration);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Used Case one a_acc: " << a_acc);
  
  double a_dec = ((desired_deceleration - desired_acceleration)*speed_before_decel - desired_deceleration * current_speed)/(desired_acceleration * stop_time);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Case one a_dec: " << a_dec);
  a_dec = std::max(a_dec, desired_deceleration);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Used Case one a_dec: " << a_dec);
 
  double t_acc = (speed_before_decel - current_speed)/a_acc;
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Case one t_acc: " << t_acc);
  double t_dec = -speed_before_decel/a_dec; // a_dec is negative so a - is used to make the t_dec positive. 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Case one t_dec: " << t_dec);
  float_metadata_list->push_back(a_acc);
  float_metadata_list->push_back(a_dec);
  float_metadata_list->push_back(t_acc);
  float_metadata_list->push_back(t_dec);
  float_metadata_list->push_back(speed_before_decel);
}

References config_, sci_strategic_plugin::SCIStrategicPluginConfig::vehicle_accel_limit, sci_strategic_plugin::SCIStrategicPluginConfig::vehicle_accel_limit_multiplier, sci_strategic_plugin::SCIStrategicPluginConfig::vehicle_decel_limit, and sci_strategic_plugin::SCIStrategicPluginConfig::vehicle_decel_limit_multiplier.

Referenced by composeLaneFollowingManeuverMessage().

Here is the caller graph for this function:

◆ caseThreeSpeedProfile()

double sci_strategic_plugin::SCIStrategicPlugin::caseThreeSpeedProfile	(	double	stop_dist,
		double	current_speed,
		double	stop_time
	)		const

Determine the desired speed profile parameters for Case 3.

Parameters

stop_dist	distance to stop line
current_speed	current speed of vehicle
stop_time	time duration to stop in s

Returns: deceleration value for case three

Definition at line 724 of file sci_strategic_plugin.cpp.

{
  double a_dec = (2*stop_dist - current_speed*(stop_time + config_.delta_t))/(stop_time * config_.delta_t);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Case three a_dec: " << a_dec);
  return a_dec;
}

References config_, and sci_strategic_plugin::SCIStrategicPluginConfig::delta_t.

Referenced by plan_maneuvers_callback().

Here is the caller graph for this function:

◆ caseTwoSpeedProfile()

void sci_strategic_plugin::SCIStrategicPlugin::caseTwoSpeedProfile	(	double	stop_dist,
		double	speed_before_decel,
		double	current_speed,
		double	stop_time,
		double	speed_limit,
		std::vector< double > *	float_metadata_list
	)		const

Determine the desired speed profile parameters for Case 2.

Parameters

stop_dist	distance to stop line
speed_before_decel	vehicle speed before it starts to decelerate
current_speed	current speed of vehicle
stop_time	time duration to stop in s
float_metadata_list	metadata vector for storing speed profile parameters

Definition at line 680 of file sci_strategic_plugin.cpp.

{
  double desired_acceleration = config_.vehicle_accel_limit * config_.vehicle_accel_limit_multiplier;
  double desired_deceleration = -1 * config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier;
 
  if (speed_before_decel > speed_limit)
  {
    speed_before_decel = speed_limit;
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Case two speed_before_decel: " << speed_before_decel);
  }
  
  double t_c_nom = 2*stop_dist * ((1 - desired_acceleration/desired_deceleration)*speed_limit - current_speed) - 
                  stop_time * ((1 - desired_acceleration/desired_deceleration)*pow(speed_limit,2) - pow(current_speed, 2));
  double t_c_den = pow(speed_limit - current_speed, 2) - (desired_acceleration/desired_deceleration) * pow(speed_limit, 2);
  double t_cruise = t_c_nom / t_c_den;
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Case two t_cruise: " << t_cruise);
 
  // Equations obtained from TSMO UC 1 Algorithm draft doc
  double a_acc = ((1 - desired_acceleration/desired_deceleration)*speed_limit - current_speed)/(stop_time - t_cruise);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Case two a_acc: " << a_acc);
  a_acc = std::min(desired_acceleration, std::abs(a_acc));
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Used Case two a_acc: " << a_acc);
 
  double a_dec = ((desired_deceleration - desired_acceleration)*speed_limit - desired_deceleration * current_speed)/(desired_acceleration*(stop_time - t_cruise));
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Case two a_dec: " << a_dec);
  a_dec = -1*std::min(desired_deceleration, std::abs(a_dec));
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Used Case two a_dec: " << a_dec);
 
  double t_acc = (speed_limit - current_speed)/a_acc;
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Case two t_acc: " << t_acc);
  double t_dec = -speed_limit/a_dec; // a_dec is negative so a - is used to make the t_dec positive. 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Case two t_dec: " << t_dec);
 
  float_metadata_list->push_back(a_acc);
  float_metadata_list->push_back(a_dec);
  float_metadata_list->push_back(t_acc);
  float_metadata_list->push_back(t_dec);
  float_metadata_list->push_back(t_cruise);
  float_metadata_list->push_back(speed_before_decel);
 
}

References config_, sci_strategic_plugin::SCIStrategicPluginConfig::vehicle_accel_limit, sci_strategic_plugin::SCIStrategicPluginConfig::vehicle_accel_limit_multiplier, sci_strategic_plugin::SCIStrategicPluginConfig::vehicle_decel_limit, and sci_strategic_plugin::SCIStrategicPluginConfig::vehicle_decel_limit_multiplier.

Referenced by composeLaneFollowingManeuverMessage().

Here is the caller graph for this function:

◆ composeIntersectionTransitMessage()

carma_planning_msgs::msg::Maneuver sci_strategic_plugin::SCIStrategicPlugin::composeIntersectionTransitMessage	(	double	start_dist,
		double	end_dist,
		double	start_speed,
		double	target_speed,
		rclcpp::Time	start_time,
		rclcpp::Time	end_time,
		TurnDirection	turn_direction,
		const lanelet::Id &	starting_lane_id,
		const lanelet::Id &	ending_lane_id
	)		const

Definition at line 858 of file sci_strategic_plugin.cpp.

{
  carma_planning_msgs::msg::Maneuver maneuver_msg;
  if (turn_direction == TurnDirection::Left)
  {
    maneuver_msg.type = carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN;
    maneuver_msg.intersection_transit_left_turn_maneuver.parameters.planning_strategic_plugin =
        config_.strategic_plugin_name;
    maneuver_msg.intersection_transit_left_turn_maneuver.parameters.planning_tactical_plugin =
        config_.intersection_transit_plugin_name;
    maneuver_msg.intersection_transit_left_turn_maneuver.parameters.presence_vector =
        carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN;
    maneuver_msg.intersection_transit_left_turn_maneuver.parameters.negotiation_type =
        carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
    maneuver_msg.intersection_transit_left_turn_maneuver.start_dist = start_dist;
    maneuver_msg.intersection_transit_left_turn_maneuver.end_dist = end_dist;
    maneuver_msg.intersection_transit_left_turn_maneuver.start_speed = start_speed;
    maneuver_msg.intersection_transit_left_turn_maneuver.end_speed = target_speed;
    maneuver_msg.intersection_transit_left_turn_maneuver.start_time = start_time;
    maneuver_msg.intersection_transit_left_turn_maneuver.end_time = end_time;
    maneuver_msg.intersection_transit_left_turn_maneuver.starting_lane_id = std::to_string(starting_lane_id);
    maneuver_msg.intersection_transit_left_turn_maneuver.ending_lane_id = std::to_string(ending_lane_id);
 
  }
  else if (turn_direction == TurnDirection::Right)
  {
    maneuver_msg.type = carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN;
    maneuver_msg.intersection_transit_right_turn_maneuver.parameters.planning_strategic_plugin =
        config_.strategic_plugin_name;
    maneuver_msg.intersection_transit_right_turn_maneuver.parameters.planning_tactical_plugin =
        config_.intersection_transit_plugin_name;
    maneuver_msg.intersection_transit_right_turn_maneuver.parameters.presence_vector =
        carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN;
    maneuver_msg.intersection_transit_right_turn_maneuver.parameters.negotiation_type =
        carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
    maneuver_msg.intersection_transit_right_turn_maneuver.start_dist = start_dist;
    maneuver_msg.intersection_transit_right_turn_maneuver.end_dist = end_dist;
    maneuver_msg.intersection_transit_right_turn_maneuver.start_speed = start_speed;
    maneuver_msg.intersection_transit_right_turn_maneuver.end_speed = target_speed;
    maneuver_msg.intersection_transit_right_turn_maneuver.start_time = start_time;
    maneuver_msg.intersection_transit_right_turn_maneuver.end_time = end_time;
    maneuver_msg.intersection_transit_right_turn_maneuver.starting_lane_id = std::to_string(starting_lane_id);
    maneuver_msg.intersection_transit_right_turn_maneuver.ending_lane_id = std::to_string(ending_lane_id);
 
  }
  else
  {
    maneuver_msg.type = carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT;
    maneuver_msg.intersection_transit_straight_maneuver.parameters.planning_strategic_plugin =
        config_.strategic_plugin_name;
    maneuver_msg.intersection_transit_straight_maneuver.parameters.planning_tactical_plugin =
        config_.intersection_transit_plugin_name;
    maneuver_msg.intersection_transit_straight_maneuver.parameters.presence_vector =
        carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN;
    maneuver_msg.intersection_transit_straight_maneuver.parameters.negotiation_type =
        carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
    maneuver_msg.intersection_transit_straight_maneuver.start_dist = start_dist;
    maneuver_msg.intersection_transit_straight_maneuver.end_dist = end_dist;
    maneuver_msg.intersection_transit_straight_maneuver.start_speed = start_speed;
    maneuver_msg.intersection_transit_straight_maneuver.end_speed = target_speed;
    maneuver_msg.intersection_transit_straight_maneuver.start_time = start_time;
    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("sci_strategic_plugin"), "Creating IntersectionTransitManeuver start dist: " << start_dist << " end dist: " << end_dist
                                                                      << " From lanelet: " << starting_lane_id
                                                                      << " to lanelet: " << ending_lane_id);
 
  return maneuver_msg;
}

References config_, sci_strategic_plugin::SCIStrategicPluginConfig::intersection_transit_plugin_name, sci_strategic_plugin::Left, sci_strategic_plugin::Right, sci_strategic_plugin::SCIStrategicPluginConfig::strategic_plugin_name, 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:

◆ composeLaneFollowingManeuverMessage()

carma_planning_msgs::msg::Maneuver sci_strategic_plugin::SCIStrategicPlugin::composeLaneFollowingManeuverMessage	(	int	case_num,
		double	start_dist,
		double	end_dist,
		double	start_speed,
		double	target_speed,
		rclcpp::Time	start_time,
		double	time_to_stop,
		std::vector< lanelet::Id >	lane_ids
	)

Compose a lane keeping 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
lane_ids	List of lanelet IDs that the current maneuver traverses. Message expects these to be contiguous and end to end

Returns: A lane keeping maneuver message which is ready to be published

Definition at line 731 of file sci_strategic_plugin.cpp.

{
  carma_planning_msgs::msg::Maneuver maneuver_msg;
  carma_planning_msgs::msg::Maneuver empty_msg;
  maneuver_msg.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING;
  maneuver_msg.lane_following_maneuver.parameters.planning_strategic_plugin = config_.strategic_plugin_name;
  maneuver_msg.lane_following_maneuver.parameters.presence_vector = 
  carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN | carma_planning_msgs::msg::ManeuverParameters::HAS_INT_META_DATA | 
  carma_planning_msgs::msg::ManeuverParameters::HAS_FLOAT_META_DATA | carma_planning_msgs::msg::ManeuverParameters::HAS_STRING_META_DATA;
  maneuver_msg.lane_following_maneuver.parameters.negotiation_type = carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
  maneuver_msg.lane_following_maneuver.parameters.planning_tactical_plugin = config_.lane_following_plugin_name;
  maneuver_msg.lane_following_maneuver.start_dist = start_dist;
  maneuver_msg.lane_following_maneuver.end_dist = end_dist;
  maneuver_msg.lane_following_maneuver.start_speed = start_speed;
  maneuver_msg.lane_following_maneuver.end_speed = target_speed;
  maneuver_msg.lane_following_maneuver.start_time = start_time;
  maneuver_msg.lane_following_maneuver.end_time =  start_time + rclcpp::Duration(time_to_stop*1e9);
  maneuver_msg.lane_following_maneuver.lane_ids =
      lanelet::utils::transform(lane_ids, [](auto id) { return std::to_string(id); });
 
  RCLCPP_INFO_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Creating lane follow start dist: " << start_dist << " end dist: " << end_dist);
 
  std::vector<double> float_metadata_list;
 
  double speed_before_decel = calc_speed_before_decel(time_to_stop, end_dist-start_dist, start_speed);
 
  switch(case_num)
  {
    case 1:
      caseOneSpeedProfile(speed_before_decel, start_speed, time_to_stop, &float_metadata_list);
      break;
    case 2:
      caseTwoSpeedProfile(end_dist-start_dist, speed_before_decel, start_speed, time_to_stop, speed_limit_, &float_metadata_list);
      break;
    default:
      return empty_msg;
  }
  
  
  maneuver_msg.lane_following_maneuver.parameters.int_valued_meta_data.push_back(case_num);
  maneuver_msg.lane_following_maneuver.parameters.string_valued_meta_data.push_back(stop_controlled_intersection_strategy_);
  maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data = float_metadata_list;
  
  return maneuver_msg;
}

References calc_speed_before_decel(), caseOneSpeedProfile(), caseTwoSpeedProfile(), config_, sci_strategic_plugin::SCIStrategicPluginConfig::lane_following_plugin_name, speed_limit_, stop_controlled_intersection_strategy_, sci_strategic_plugin::SCIStrategicPluginConfig::strategic_plugin_name, 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:

◆ composeStopAndWaitManeuverMessage()

carma_planning_msgs::msg::Maneuver sci_strategic_plugin::SCIStrategicPlugin::composeStopAndWaitManeuverMessage	(	double	current_dist,
		double	end_dist,
		double	start_speed,
		const lanelet::Id &	starting_lane_id,
		const lanelet::Id &	ending_lane_id,
		double	stopping_accel,
		rclcpp::Time	start_time,
		rclcpp::Time	end_time
	)		const

Definition at line 829 of file sci_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.planning_strategic_plugin = config_.strategic_plugin_name;
  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.negotiation_type = carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
  maneuver_msg.stop_and_wait_maneuver.parameters.planning_tactical_plugin = config_.stop_and_wait_plugin_name;
  maneuver_msg.stop_and_wait_maneuver.start_speed = start_speed;
  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_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_.stop_line_buffer);
  maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(stopping_accel);
 
  RCLCPP_INFO_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Creating stop and wait start dist: " << current_dist << " end dist: " << end_dist);
  
  return maneuver_msg;
}

References config_, sci_strategic_plugin::SCIStrategicPluginConfig::stop_and_wait_plugin_name, sci_strategic_plugin::SCIStrategicPluginConfig::stop_line_buffer, sci_strategic_plugin::SCIStrategicPluginConfig::strategic_plugin_name, 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:

◆ currentPoseCb()

void sci_strategic_plugin::SCIStrategicPlugin::currentPoseCb ( geometry_msgs::msg::PoseStamped::UniquePtr msg )

callback function for current pose

Parameters

msg	input pose stamed msg

Definition at line 231 of file sci_strategic_plugin.cpp.

{
  geometry_msgs::msg::PoseStamped pose_msg = geometry_msgs::msg::PoseStamped(*msg.get());
  if (vehicle_engaged_)
  {
    lanelet::BasicPoint2d current_loc(pose_msg.pose.position.x, pose_msg.pose.position.y);
    current_downtrack_ = wm_->routeTrackPos(current_loc).downtrack;
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Downtrack from current pose: " << current_downtrack_);
  }
  
  
}

References current_downtrack_, vehicle_engaged_, and wm_.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

◆ determine_speed_profile_case()

int sci_strategic_plugin::SCIStrategicPlugin::determine_speed_profile_case	(	double	stop_dist,
		double	current_speed,
		double	schedule_stop_time,
		double	speed_limit
	)

Determine the speed profile case fpr approaching an intersection.

Parameters

stop_dist	distance to stop line
current_speed	current speed of vehicle
schedule_stop_time	scheduled stop time
speed_limit	speed limit

Returns: integer case number

Definition at line 280 of file sci_strategic_plugin.cpp.

{
  int case_num = 0;
  double estimated_stop_time = calcEstimatedStopTime(stop_dist, current_speed);
  
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "estimated_stop_time: " << estimated_stop_time);
  if (estimated_stop_time < schedule_stop_time)
  {
    case_num = 3;
  }
  else
  {
    double speed_before_stop = calc_speed_before_decel(estimated_stop_time, stop_dist, current_speed);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "speed_before_stop: " << speed_before_stop);
    if (speed_before_stop < speed_limit)
    {
      case_num = 1;
    }
    else
    {
      case_num = 2;
    }
  }
  
  return case_num;
}

References calc_speed_before_decel(), and calcEstimatedStopTime().

Referenced by plan_maneuvers_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ extractInitialState()

SCIStrategicPlugin::VehicleState sci_strategic_plugin::SCIStrategicPlugin::extractInitialState ( const carma_planning_msgs::srv::PlanManeuvers::Request & 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.

Parameters

req	The maneuver planning request to extract the vehicle state from

Returns: The extracted VehicleState

Definition at line 170 of file sci_strategic_plugin.cpp.

{
  VehicleState state;
  if (!req.prior_plan.maneuvers.empty())
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Provided with initial plan...");
    state.stamp = GET_MANEUVER_PROPERTY(req.prior_plan.maneuvers.back(), end_time);
    state.speed = getManeuverEndSpeed(req.prior_plan.maneuvers.back());
    state.downtrack = GET_MANEUVER_PROPERTY(req.prior_plan.maneuvers.back(), end_dist);
    state.lane_id = getLaneletsBetweenWithException(state.downtrack, state.downtrack, true).front().id();
  }
  else
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "No initial plan provided...");
    
    state.stamp = req.header.stamp;
    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("sci_strategic_plugin"), "state.stamp: " << std::to_string(state.stamp.seconds()));
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "state.downtrack : " << state.downtrack );
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "state.speed: " << state.speed);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "state.lane_id: " << state.lane_id);
 
  return state;
}

References sci_strategic_plugin::SCIStrategicPlugin::VehicleState::downtrack, GET_MANEUVER_PROPERTY, getLaneletsBetweenWithException(), sci_strategic_plugin::anonymous_namespace{sci_strategic_plugin.cpp}::getManeuverEndSpeed(), sci_strategic_plugin::SCIStrategicPlugin::VehicleState::lane_id, sci_strategic_plugin::SCIStrategicPlugin::VehicleState::speed, sci_strategic_plugin::SCIStrategicPlugin::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 sci_strategic_plugin::SCIStrategicPlugin::findSpeedLimit ( const lanelet::ConstLanelet & llt ) const

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 781 of file sci_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 plan_maneuvers_callback().

Here is the caller graph for this function:

◆ FRIEND_TEST() [1/12]

sci_strategic_plugin::SCIStrategicPlugin::FRIEND_TEST	(	SCIStrategicPluginTest	,
		calc_speed_before_deceltest
	)

private

◆ FRIEND_TEST() [2/12]

sci_strategic_plugin::SCIStrategicPlugin::FRIEND_TEST	(	SCIStrategicPluginTest	,
		calcEstimatedStopTimetest
	)

private

◆ FRIEND_TEST() [3/12]

sci_strategic_plugin::SCIStrategicPlugin::FRIEND_TEST	(	SCIStrategicPluginTest	,
		caseOneSpeedProfiletest
	)

private

◆ FRIEND_TEST() [4/12]

sci_strategic_plugin::SCIStrategicPlugin::FRIEND_TEST	(	SCIStrategicPluginTest	,
		caseThreeSpeedProfiletest
	)

private

◆ FRIEND_TEST() [5/12]

sci_strategic_plugin::SCIStrategicPlugin::FRIEND_TEST	(	SCIStrategicPluginTest	,
		caseTwoSpeedProfiletest
	)

private

◆ FRIEND_TEST() [6/12]

sci_strategic_plugin::SCIStrategicPlugin::FRIEND_TEST	(	SCIStrategicPluginTest	,
		determine_speed_profile_casetest
	)

private

◆ FRIEND_TEST() [7/12]

sci_strategic_plugin::SCIStrategicPlugin::FRIEND_TEST	(	SCIStrategicPluginTest	,
		DISABLED_maneuvercbtest
	)

private

◆ FRIEND_TEST() [8/12]

sci_strategic_plugin::SCIStrategicPlugin::FRIEND_TEST	(	SCIStrategicPluginTest	,
		findSpeedLimit
	)

private

◆ FRIEND_TEST() [9/12]

sci_strategic_plugin::SCIStrategicPlugin::FRIEND_TEST	(	SCIStrategicPluginTest	,
		maneuvercbtest
	)

private

◆ FRIEND_TEST() [10/12]

sci_strategic_plugin::SCIStrategicPlugin::FRIEND_TEST	(	SCIStrategicPluginTest	,
		moboperationcbtest
	)

private

◆ FRIEND_TEST() [11/12]

sci_strategic_plugin::SCIStrategicPlugin::FRIEND_TEST	(	SCIStrategicPluginTest	,
		parseStrategyParamstest
	)

private

◆ FRIEND_TEST() [12/12]

sci_strategic_plugin::SCIStrategicPlugin::FRIEND_TEST	(	SCIStrategicPluginTest	,
		testIntersectionturndirection
	)

private

◆ generateMobilityOperation()

carma_v2x_msgs::msg::MobilityOperation sci_strategic_plugin::SCIStrategicPlugin::generateMobilityOperation ( )

Generates Mobility Operation messages.

Returns: mobility operation msg for status and intent

Definition at line 794 of file sci_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.sender_bsm_id = bsm_id_;
    mo_.strategy = stop_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: " +  std::to_string(is_allowed_int_) + ", max_accel: " + std::to_string(vehicle_acceleration_limit_) + 
                        ", 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: " + std::to_string(scheduled_departure_position_) + 
                        ", 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 sci_strategic_plugin::SCIStrategicPlugin::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 941 of file sci_strategic_plugin.cpp.

{
  return true;
}

◆ get_version_id()

std::string sci_strategic_plugin::SCIStrategicPlugin::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 946 of file sci_strategic_plugin.cpp.

{
  return "v1.0";
}

◆ get_wm()

carma_wm::WorldModelConstPtr sci_strategic_plugin::SCIStrategicPlugin::get_wm ( )

inlineprivate

Definition at line 347 of file sci_strategic_plugin.hpp.

347{ return wm_; }

References wm_.

◆ getLaneletsBetweenWithException()

std::vector< lanelet::ConstLanelet > sci_strategic_plugin::SCIStrategicPlugin::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.

Definition at line 331 of file sci_strategic_plugin.cpp.

{
  std::vector<lanelet::ConstLanelet> crossed_lanelets =
      wm_->getLaneletsBetween(start_downtrack, end_downtrack, shortest_path_only, bounds_inclusive);
 
  if (crossed_lanelets.empty())
  {
    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(), and plan_maneuvers_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getTurnDirectionAtIntersection()

TurnDirection sci_strategic_plugin::SCIStrategicPlugin::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 622 of file sci_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;
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "intersection crossed lanelet direction is: " << turn_direction);
    }
    else
    {
      // if there is no attribute, assumption is straight
      turn_direction = TurnDirection::Straight;
    }
 
  }
  return turn_direction;
}

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

Referenced by plan_maneuvers_callback().

Here is the caller graph for this function:

◆ guidance_state_cb()

void sci_strategic_plugin::SCIStrategicPlugin::guidance_state_cb ( const carma_planning_msgs::msg::GuidanceState::UniquePtr msg )

Callback for the Guidance State.

Parameters

msg	Latest GuidanceState message

Definition at line 226 of file sci_strategic_plugin.cpp.

{
  guidance_engaged_ = (msg->state == carma_planning_msgs::msg::GuidanceState::ENGAGED);
}

References lightbar_manager::ENGAGED, and guidance_engaged_.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

◆ mobilityOperationCb()

void sci_strategic_plugin::SCIStrategicPlugin::mobilityOperationCb ( carma_v2x_msgs::msg::MobilityOperation::UniquePtr msg )

callback function for mobility operation

Parameters

msg	input mobility operation msg

Definition at line 199 of file sci_strategic_plugin.cpp.

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

References approaching_stop_controlled_interction_, config_, parseStrategyParams(), previous_strategy_params_, stop_controlled_intersection_strategy_, street_msg_timestamp_, and sci_strategic_plugin::SCIStrategicPluginConfig::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 sci_strategic_plugin::SCIStrategicPlugin::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 161 of file sci_strategic_plugin.cpp.

{
  mob_op_pub_timer_ = create_timer(get_clock(),
    std::chrono::duration<double>(0.1),
    std::bind(&SCIStrategicPlugin::publishMobilityOperation, this));
 
  return CallbackReturn::SUCCESS;
}

References mob_op_pub_timer_, and publishMobilityOperation().

Here is the call graph for this function:

◆ on_configure_plugin()

carma_ros2_utils::CallbackReturn sci_strategic_plugin::SCIStrategicPlugin::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 91 of file sci_strategic_plugin.cpp.

{
  // reset config
  config_ = SCIStrategicPluginConfig();
  // Declare parameters
  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>("stop_line_buffer",   config_.stop_line_buffer);
  get_parameter<double>("delta_t",   config_.delta_t);
  get_parameter<double>("min_gap",   config_.min_gap);
  get_parameter<double>("reaction_time",   config_.reaction_time);
  get_parameter<double>("intersection_exit_zone_length",   config_.intersection_exit_zone_length);
  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>("intersection_transit_plugin_name", config_.intersection_transit_plugin_name);
  get_parameter<std::string>("vehicle_id", config_.vehicle_id);
  get_parameter<double>("vehicle_length", config_.veh_length);
  get_parameter<double>("vehicle_deceleration_limit", config_.vehicle_decel_limit);
  get_parameter<double>("vehicle_acceleration_limit", config_.vehicle_accel_limit);
  
   // Register runtime parameter update callback
  add_on_set_parameters_callback(std::bind(&SCIStrategicPlugin::parameter_update_callback, this, std_ph::_1));
 
  RCLCPP_INFO_STREAM(rclcpp::get_logger("sci_strategic_plugin"),"Done loading parameters: " << config_);
 
  // Mobility Operation Subscriber
  mob_operation_sub_ = create_subscription<carma_v2x_msgs::msg::MobilityOperation>("incoming_mobility_operation", 1, 
    std::bind(&SCIStrategicPlugin::mobilityOperationCb,this,std_ph::_1));
  
  // Current Pose Subscriber
  current_pose_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>("current_pose", 1, 
    std::bind(&SCIStrategicPlugin::currentPoseCb,this,std_ph::_1));
 
  // BSM subscriber
  bsm_sub_ = create_subscription<carma_v2x_msgs::msg::BSM>("bsm_outbound", 1, 
    std::bind(&SCIStrategicPlugin::BSMCb,this,std_ph::_1));
 
  // Guidance State subscriber
  guidance_state_sub_ = create_subscription<carma_planning_msgs::msg::GuidanceState>("guidance_state", 5, 
    std::bind(&SCIStrategicPlugin::guidance_state_cb, this, std::placeholders::_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);
 
  // Return success if everthing initialized successfully
  return CallbackReturn::SUCCESS;
}

Here is the call graph for this function:

◆ parameter_update_callback()

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

Callback for dynamic parameter updates.

Definition at line 142 of file sci_strategic_plugin.cpp.

{
  auto error_double = update_params<double>({
    {"vehicle_decel_limit_multiplier", config_.vehicle_decel_limit_multiplier},
    {"vehicle_accel_limit_multiplier", config_.vehicle_accel_limit_multiplier},
    {"stop_line_buffer", config_.stop_line_buffer},
    {"delta_t", config_.delta_t},
    {"min_gap", config_.min_gap},
    {"reaction_time", config_.reaction_time},
    {"intersection_exit_zone_length", config_.intersection_exit_zone_length}
  }, parameters); // vehicle_acceleration_limit not updated as it's global param
 
  rcl_interfaces::msg::SetParametersResult result;
 
  result.successful = !error_double;
 
  return result;
}

References config_, sci_strategic_plugin::SCIStrategicPluginConfig::delta_t, sci_strategic_plugin::SCIStrategicPluginConfig::intersection_exit_zone_length, sci_strategic_plugin::SCIStrategicPluginConfig::min_gap, sci_strategic_plugin::SCIStrategicPluginConfig::reaction_time, sci_strategic_plugin::SCIStrategicPluginConfig::stop_line_buffer, sci_strategic_plugin::SCIStrategicPluginConfig::vehicle_accel_limit_multiplier, and sci_strategic_plugin::SCIStrategicPluginConfig::vehicle_decel_limit_multiplier.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

◆ parseStrategyParams()

void sci_strategic_plugin::SCIStrategicPlugin::parseStrategyParams ( const std::string & strategy_params )

parse strategy parameters

Parameters

strategy_params input string of strategy params from mob op msg

Definition at line 244 of file sci_strategic_plugin.cpp.

{
  // sample strategy_params: "st:1634067044,et:1634067059, dt:1634067062.3256602,dp:2,,access: 0"
  std::string params = strategy_params;
  std::vector<std::string> inputsParams;
  boost::algorithm::split(inputsParams, params, boost::is_any_of(","));
 
  std::vector<std::string> st_parsed;
  boost::algorithm::split(st_parsed, inputsParams[0], boost::is_any_of(":"));
  scheduled_stop_time_ = std::stoull(st_parsed[1]);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "scheduled_stop_time_: " << scheduled_stop_time_);
 
  std::vector<std::string> et_parsed;
  boost::algorithm::split(et_parsed, inputsParams[1], boost::is_any_of(":"));
  scheduled_enter_time_ = std::stoull(et_parsed[1]);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "scheduled_enter_time_: " << scheduled_enter_time_);
 
  std::vector<std::string> dt_parsed;
  boost::algorithm::split(dt_parsed, inputsParams[2], boost::is_any_of(":"));
  scheduled_depart_time_ = std::stoull(dt_parsed[1]);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "scheduled_depart_time_: " << scheduled_depart_time_);
 
 
  std::vector<std::string> dp_parsed;
  boost::algorithm::split(dp_parsed, inputsParams[3], boost::is_any_of(":"));
  scheduled_departure_position_ = std::stoi(dp_parsed[1]);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "scheduled_departure_position_: " << scheduled_departure_position_);
 
  std::vector<std::string> access_parsed;
  boost::algorithm::split(access_parsed, inputsParams[4], boost::is_any_of(":"));
  int access = std::stoi(access_parsed[1]);
  is_allowed_int_ = (access == 1);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "is_allowed_int_: " << is_allowed_int_);
 
}

References is_allowed_int_, scheduled_depart_time_, scheduled_departure_position_, scheduled_enter_time_, scheduled_stop_time_, and process_traj_logs::split.

Referenced by mobilityOperationCb().

Here is the caller graph for this function:

◆ plan_maneuvers_callback()

void sci_strategic_plugin::SCIStrategicPlugin::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 351 of file sci_strategic_plugin.cpp.

{
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "In Maneuver callback...");
  vehicle_engaged_ = true;
 
  if (!wm_->getRoute())
  {
    RCLCPP_ERROR_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Could not plan maneuvers as route was not available");
    return;
  }
 
  if (!approaching_stop_controlled_interction_)
  {
    resp->new_plan.maneuvers = {};
    RCLCPP_WARN_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Not approaching stop-controlled intersection so no maneuvers");
    return;
  }
 
  bool is_empty_schedule_msg = (scheduled_depart_time_ == 0 && scheduled_stop_time_ == 0 && scheduled_enter_time_ == 0);
  if (is_empty_schedule_msg)
  {
    resp->new_plan.maneuvers = {};
    RCLCPP_WARN_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Receiving empty schedule message");
    return;
  }
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Planning for intersection...");
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Finding car information");
  // Extract vehicle data from request
  VehicleState current_state = extractInitialState(*req);
 
  // Get current traffic light information
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "\n\nFinding intersection information");
 
  auto stop_intersection_list = wm_->getIntersectionsAlongRoute({ req->veh_x, req->veh_y });
  bool no_intersections = stop_intersection_list.empty();
  if (no_intersections)
  {
    resp->new_plan.maneuvers = {};
    RCLCPP_WARN_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "There are no stop controlled intersections in the map");
    return;
  }
 
  auto nearest_stop_intersection = stop_intersection_list.front();
 
  bool no_stop_lines = nearest_stop_intersection->stopLines().empty();
  if (no_stop_lines)
  {
    resp->new_plan.maneuvers = {};
    RCLCPP_WARN_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "There are no stop lines on the closest stop-controlled intersections in the map");
    return;
  }
  
 
  // extract the intersection stop line information
  std::vector<double> stop_lines;
  for (auto l : nearest_stop_intersection->stopLines())
  {
    //TODO: temp use veh_length here until planning stack is updated with front bumper pos
    double stop_bar_dtd = wm_->routeTrackPos(l.front().basicPoint2d()).downtrack - config_.veh_length;
    stop_lines.push_back(stop_bar_dtd);
  }
  std::sort(stop_lines.begin(), stop_lines.end());
  
  double stop_intersection_down_track = stop_lines.front();
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "stop_intersection_down_track  " << stop_intersection_down_track);
  
  
  double distance_to_stopline = stop_intersection_down_track - current_downtrack_ - config_.stop_line_buffer;
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "distance_to_stopline  " << distance_to_stopline);
 
  if (distance_to_stopline < -config_.intersection_exit_zone_length)
  {
    resp->new_plan.maneuvers = {};
    RCLCPP_WARN_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Already passed intersection, sending empty maneuvers");
    return;
  }
 
 
  double intersection_end_downtrack = stop_lines.back();
  // Identify the lanelets of the intersection
  std::vector<lanelet::ConstLanelet> intersection_lanelets =
          getLaneletsBetweenWithException(stop_intersection_down_track, intersection_end_downtrack, true, true);
 
  // find the turn direction at intersection:
 
  intersection_turn_direction_ = getTurnDirectionAtIntersection(intersection_lanelets);
 
  uint32_t base_time = street_msg_timestamp_;
  
  bool time_to_approach_int = int((scheduled_stop_time_) - (street_msg_timestamp_))>0;
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "time_to_approach_int  " << time_to_approach_int);
  
  carma_planning_msgs::msg::Maneuver maneuver_planned;
 
  
  if (time_to_approach_int)
  {   
      auto tmp = (scheduled_stop_time_) - (street_msg_timestamp_);
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "tmp  " << tmp);
      double time_to_schedule_stop = (tmp)/1000.0;
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "time_to_schedule_stop  " << time_to_schedule_stop);
      // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver
      std::vector<lanelet::ConstLanelet> crossed_lanelets =
          getLaneletsBetweenWithException(current_downtrack_, stop_intersection_down_track, true, true);
 
      // approaching stop line
      speed_limit_ = findSpeedLimit(crossed_lanelets.front());
 
      // lane following to intersection
      
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "time_to_schedule_stop  " << time_to_schedule_stop);
 
      double desired_deceleration = config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier;
 
 
      double safe_distance = pow(current_state.speed, 2)/(2*desired_deceleration);
       RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "safe_distance:  " << safe_distance);
 
      if (distance_to_stopline - safe_distance > config_.stop_line_buffer)
      {
        int case_num = determine_speed_profile_case(distance_to_stopline , current_state.speed, time_to_schedule_stop, speed_limit_);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "case_num:  " << case_num);
 
        if (case_num < 3)
        {
          maneuver_planned = composeLaneFollowingManeuverMessage(
          case_num, current_state.downtrack, stop_intersection_down_track, current_state.speed, 0.0,
          current_state.stamp, time_to_schedule_stop,
          lanelet::utils::transform(crossed_lanelets, [](const auto& ll) { return ll.id(); }));
          resp->new_plan.maneuvers.push_back(maneuver_planned);
        }
        else
        {
          RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Decelerating to stop at the intersection");
 
          // at the stop line or decelerating to stop line
          // stop and wait maneuver
        
          std::vector<lanelet::ConstLanelet> crossed_lanelets =
              getLaneletsBetweenWithException(current_downtrack_, stop_intersection_down_track, true, true);
          
 
          double stopping_accel = caseThreeSpeedProfile(distance_to_stopline, current_state.speed, time_to_schedule_stop);
          stopping_accel = std::max(-stopping_accel, desired_deceleration);
          RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "used deceleration for case three: " << stopping_accel);
 
          maneuver_planned = composeStopAndWaitManeuverMessage(
          current_state.downtrack, stop_intersection_down_track, current_state.speed, crossed_lanelets[0].id(),
          crossed_lanelets[0].id(), stopping_accel, current_state.stamp,
          current_state.stamp + rclcpp::Duration(time_to_schedule_stop*1e9));
 
          resp->new_plan.maneuvers.push_back(maneuver_planned);
 
        }
      }
      else
      {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Too close to the intersection, constant deceleration to stop");
 
        maneuver_planned = composeStopAndWaitManeuverMessage(
          current_state.downtrack, stop_intersection_down_track, current_state.speed, crossed_lanelets[0].id(),
          crossed_lanelets[0].id(), desired_deceleration, current_state.stamp,
          current_state.stamp + rclcpp::Duration(time_to_schedule_stop*1e9));
      }
  }
 
  bool time_to_reach_stopline = int((street_msg_timestamp_) - (scheduled_stop_time_)) >= 0;
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "time_to_reach_stopline: " << time_to_reach_stopline);
  bool not_time_to_intersection_traverse = int((street_msg_timestamp_) - (scheduled_enter_time_)) < 0;
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "not_time_to_intersection_traverse: " << not_time_to_intersection_traverse);
  bool time_to_stop_at_stopline = (time_to_reach_stopline && not_time_to_intersection_traverse);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "time_to_stop_at_stopline: " << time_to_stop_at_stopline);
 
  if (time_to_stop_at_stopline)
  {
    base_time = std::max(scheduled_stop_time_, street_msg_timestamp_);
    double stop_duration = int(scheduled_enter_time_ - base_time)/1000;
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "stop_duration:  " << stop_duration);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Planning stop and wait maneuver");
    double stopping_accel = config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier;
    auto stop_line_lanelet = nearest_stop_intersection->lanelets().front();
 
    maneuver_planned = composeStopAndWaitManeuverMessage(
          current_state.downtrack, stop_intersection_down_track, current_state.speed, stop_line_lanelet.id(),
          stop_line_lanelet.id(), stopping_accel, current_state.stamp,
          current_state.stamp + rclcpp::Duration(stop_duration*1e9));
  }
 
  if (!is_allowed_int_)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Vehicle at stop line, waiting for access to intersection");
    // vehicle is at the intersection but it is not allowed access, so it must wait
    // arbitrary parameters
    double stop_acc = config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier;
    double stop_duration = 999;//std::numeric_limits<double>::max();
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "current_downtrack_: " << current_downtrack_);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "current state dtd: " << current_state.downtrack);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "stop_intersection_down_track dtd: " << stop_intersection_down_track);
 
      maneuver_planned = composeStopAndWaitManeuverMessage(
          current_state.downtrack, stop_intersection_down_track, current_state.speed, current_state.lane_id,
          current_state.lane_id, stop_acc, current_state.stamp,
          current_state.stamp + rclcpp::Duration(stop_duration*1e9));
 
  }
 
  bool time_for_crossing = int((street_msg_timestamp_) - (scheduled_enter_time_)) >= 0;
  // time to cross intersection
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "time for crossing? " << time_for_crossing);
 
  if (time_for_crossing && is_allowed_int_)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "street_msg_timestamp_ - scheduled_enter_time_ = " << street_msg_timestamp_ - scheduled_enter_time_);
 
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Vehicle is crossing the intersection");
    // Passing the stop line
    
    // Compose intersection transit maneuver
    base_time = std::max(scheduled_enter_time_, street_msg_timestamp_);
    double intersection_transit_time = (scheduled_depart_time_ - base_time)/1000;
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "intersection_transit_time: " << intersection_transit_time);
    
    intersection_transit_time = config_.min_maneuver_planning_period;//std::max(intersection_transit_time, config_.min_maneuver_planning_period);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "used intersection_transit_time: " << intersection_transit_time);
    // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver
    std::vector<lanelet::ConstLanelet> crossed_lanelets =
          getLaneletsBetweenWithException(current_downtrack_, intersection_end_downtrack, true, true);
 
    
    // find the turn direction at intersection:
 
    intersection_turn_direction_ = getTurnDirectionAtIntersection(crossed_lanelets);
    
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "turn direction at the intersection is: " << intersection_turn_direction_);
 
    double intersection_speed_limit = findSpeedLimit(nearest_stop_intersection->lanelets().front());
 
    // when passing intersection, set the flag to false
    double end_of_intersection = std::max(config_.intersection_exit_zone_length, intersection_end_downtrack - stop_intersection_down_track);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Actual length of intersection: " << intersection_end_downtrack - stop_intersection_down_track);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Used length of intersection: " << end_of_intersection);
 
      maneuver_planned = composeIntersectionTransitMessage(
      current_state.downtrack, current_state.downtrack + end_of_intersection, current_state.speed, intersection_speed_limit,
      current_state.stamp, rclcpp::Time(req->header.stamp) + rclcpp::Duration(intersection_transit_time*1e9), intersection_turn_direction_, crossed_lanelets.front().id(), crossed_lanelets.back().id());
    
    
    if (distance_to_stopline < -end_of_intersection)
    {
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Vehicle is out of intersection, stop planning...");
      // once the vehicle crosses the intersection, reset the flag to stop planning and publishing status/intent
      approaching_stop_controlled_interction_ = false;
      // once the intersection is crossed, reset turn direction
      intersection_turn_direction_ = TurnDirection::Straight;
    }
 
  }
  resp->new_plan.maneuvers.push_back(maneuver_planned);
  
  return;
}

Here is the call graph for this function:

◆ publishMobilityOperation()

void sci_strategic_plugin::SCIStrategicPlugin::publishMobilityOperation ( )

Method to publish mobility operation msgs at a fixed rate of 10hz if approaching intersection.

Definition at line 819 of file sci_strategic_plugin.cpp.

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

References approaching_stop_controlled_interction_, generateMobilityOperation(), guidance_engaged_, 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:

◆ set_wm()

void sci_strategic_plugin::SCIStrategicPlugin::set_wm ( carma_wm::WorldModelConstPtr new_wm )

inlineprivate

Definition at line 348 of file sci_strategic_plugin.hpp.

348{ wm_ = new_wm; }

References wm_.

Member Data Documentation

◆ approaching_stop_controlled_interction_

bool sci_strategic_plugin::SCIStrategicPlugin::approaching_stop_controlled_interction_ = false

Definition at line 313 of file sci_strategic_plugin.hpp.

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

◆ bsm_id_

std::string sci_strategic_plugin::SCIStrategicPlugin::bsm_id_ = "default_bsm_id"

Definition at line 315 of file sci_strategic_plugin.hpp.

Referenced by BSMCb(), and generateMobilityOperation().

◆ bsm_msg_count_

uint8_t sci_strategic_plugin::SCIStrategicPlugin::bsm_msg_count_ = 0

Definition at line 316 of file sci_strategic_plugin.hpp.

Referenced by BSMCb(), and generateMobilityOperation().

◆ bsm_sec_mark_

uint16_t sci_strategic_plugin::SCIStrategicPlugin::bsm_sec_mark_ = 0

Definition at line 317 of file sci_strategic_plugin.hpp.

Referenced by BSMCb(), and generateMobilityOperation().

◆ bsm_sub_

carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::BSM> sci_strategic_plugin::SCIStrategicPlugin::bsm_sub_

private

Definition at line 323 of file sci_strategic_plugin.hpp.

Referenced by on_configure_plugin().

◆ config_

SCIStrategicPluginConfig sci_strategic_plugin::SCIStrategicPlugin::config_

private

Config containing configurable algorithm parameters.

Definition at line 336 of file sci_strategic_plugin.hpp.

Referenced by SCIStrategicPlugin(), calc_speed_before_decel(), caseOneSpeedProfile(), caseThreeSpeedProfile(), caseTwoSpeedProfile(), composeIntersectionTransitMessage(), composeLaneFollowingManeuverMessage(), composeStopAndWaitManeuverMessage(), generateMobilityOperation(), mobilityOperationCb(), on_configure_plugin(), parameter_update_callback(), and plan_maneuvers_callback().

◆ current_downtrack_

double sci_strategic_plugin::SCIStrategicPlugin::current_downtrack_ = 0.0

Definition at line 311 of file sci_strategic_plugin.hpp.

Referenced by currentPoseCb(), and plan_maneuvers_callback().

◆ current_pose_sub_

carma_ros2_utils::SubPtr<geometry_msgs::msg::PoseStamped> sci_strategic_plugin::SCIStrategicPlugin::current_pose_sub_

private

Definition at line 322 of file sci_strategic_plugin.hpp.

Referenced by on_configure_plugin().

◆ guidance_engaged_

bool sci_strategic_plugin::SCIStrategicPlugin::guidance_engaged_ = false

private

Definition at line 327 of file sci_strategic_plugin.hpp.

Referenced by guidance_state_cb(), and publishMobilityOperation().

◆ guidance_state_sub_

carma_ros2_utils::SubPtr<carma_planning_msgs::msg::GuidanceState> sci_strategic_plugin::SCIStrategicPlugin::guidance_state_sub_

private

Definition at line 325 of file sci_strategic_plugin.hpp.

Referenced by on_configure_plugin().

◆ intersection_end_downtrack_

boost::optional<double> sci_strategic_plugin::SCIStrategicPlugin::intersection_end_downtrack_

private

Definition at line 340 of file sci_strategic_plugin.hpp.

◆ intersection_speed_

boost::optional<double> sci_strategic_plugin::SCIStrategicPlugin::intersection_speed_

private

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

Definition at line 339 of file sci_strategic_plugin.hpp.

◆ intersection_turn_direction_

TurnDirection sci_strategic_plugin::SCIStrategicPlugin::intersection_turn_direction_ = TurnDirection::Straight

Definition at line 304 of file sci_strategic_plugin.hpp.

Referenced by generateMobilityOperation(), and plan_maneuvers_callback().

◆ is_allowed_int_

bool sci_strategic_plugin::SCIStrategicPlugin::is_allowed_int_ = false

Definition at line 302 of file sci_strategic_plugin.hpp.

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

◆ mob_op_pub_timer_

rclcpp::TimerBase::SharedPtr sci_strategic_plugin::SCIStrategicPlugin::mob_op_pub_timer_

private

Definition at line 330 of file sci_strategic_plugin.hpp.

Referenced by on_activate_plugin().

◆ mob_operation_sub_

carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::MobilityOperation> sci_strategic_plugin::SCIStrategicPlugin::mob_operation_sub_

private

Definition at line 321 of file sci_strategic_plugin.hpp.

Referenced by on_configure_plugin().

◆ mobility_operation_pub_

carma_ros2_utils::PubPtr<carma_v2x_msgs::msg::MobilityOperation> sci_strategic_plugin::SCIStrategicPlugin::mobility_operation_pub_

private

Definition at line 324 of file sci_strategic_plugin.hpp.

Referenced by on_configure_plugin(), and publishMobilityOperation().

◆ previous_strategy_params_

std::string sci_strategic_plugin::SCIStrategicPlugin::previous_strategy_params_ = ""

private

Definition at line 344 of file sci_strategic_plugin.hpp.

Referenced by mobilityOperationCb().

◆ scheduled_depart_time_

unsigned long long sci_strategic_plugin::SCIStrategicPlugin::scheduled_depart_time_ = 0

Definition at line 298 of file sci_strategic_plugin.hpp.

Referenced by parseStrategyParams(), and plan_maneuvers_callback().

◆ scheduled_departure_position_

uint32_t sci_strategic_plugin::SCIStrategicPlugin::scheduled_departure_position_ = std::numeric_limits<uint32_t>::max()

Definition at line 300 of file sci_strategic_plugin.hpp.

Referenced by generateMobilityOperation(), and parseStrategyParams().

◆ scheduled_enter_time_

unsigned long long sci_strategic_plugin::SCIStrategicPlugin::scheduled_enter_time_ = 0

Definition at line 296 of file sci_strategic_plugin.hpp.

Referenced by parseStrategyParams(), and plan_maneuvers_callback().

◆ scheduled_stop_time_

unsigned long long sci_strategic_plugin::SCIStrategicPlugin::scheduled_stop_time_ = 0

Definition at line 294 of file sci_strategic_plugin.hpp.

Referenced by parseStrategyParams(), and plan_maneuvers_callback().

◆ speed_limit_

double sci_strategic_plugin::SCIStrategicPlugin::speed_limit_ = 100.0

Definition at line 308 of file sci_strategic_plugin.hpp.

Referenced by composeLaneFollowingManeuverMessage(), and plan_maneuvers_callback().

◆ stop_controlled_intersection_strategy_

std::string sci_strategic_plugin::SCIStrategicPlugin::stop_controlled_intersection_strategy_ = "Carma/stop_controlled_intersection"

private

Definition at line 343 of file sci_strategic_plugin.hpp.

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

◆ street_msg_timestamp_

unsigned long long sci_strategic_plugin::SCIStrategicPlugin::street_msg_timestamp_ = 0

Definition at line 292 of file sci_strategic_plugin.hpp.

Referenced by mobilityOperationCb(), and plan_maneuvers_callback().

◆ vehicle_engaged_

bool sci_strategic_plugin::SCIStrategicPlugin::vehicle_engaged_ = false

Definition at line 305 of file sci_strategic_plugin.hpp.

Referenced by currentPoseCb(), and plan_maneuvers_callback().

◆ wm_

carma_wm::WorldModelConstPtr sci_strategic_plugin::SCIStrategicPlugin::wm_

private

World Model pointer.

Definition at line 333 of file sci_strategic_plugin.hpp.

Referenced by currentPoseCb(), findSpeedLimit(), get_wm(), getLaneletsBetweenWithException(), on_configure_plugin(), plan_maneuvers_callback(), and set_wm().

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

sci_strategic_plugin/include/sci_strategic_plugin.hpp
sci_strategic_plugin/src/sci_strategic_plugin.cpp

Classes

Public Member Functions

Public Attributes

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ SCIStrategicPlugin()

Member Function Documentation

◆ BSMCb()

◆ calc_speed_before_decel()

◆ calcEstimatedStopTime()

◆ caseOneSpeedProfile()

◆ caseThreeSpeedProfile()

◆ caseTwoSpeedProfile()

◆ composeIntersectionTransitMessage()

◆ composeLaneFollowingManeuverMessage()

◆ composeStopAndWaitManeuverMessage()

◆ currentPoseCb()

◆ determine_speed_profile_case()

◆ extractInitialState()

◆ findSpeedLimit()

◆ FRIEND_TEST() [1/12]

◆ FRIEND_TEST() [2/12]

◆ FRIEND_TEST() [3/12]

◆ FRIEND_TEST() [4/12]

◆ FRIEND_TEST() [5/12]

◆ FRIEND_TEST() [6/12]

◆ FRIEND_TEST() [7/12]

◆ FRIEND_TEST() [8/12]

◆ FRIEND_TEST() [9/12]

◆ FRIEND_TEST() [10/12]

◆ FRIEND_TEST() [11/12]

◆ FRIEND_TEST() [12/12]

◆ generateMobilityOperation()

◆ get_availability()

◆ get_version_id()

◆ get_wm()

◆ getLaneletsBetweenWithException()

◆ getTurnDirectionAtIntersection()

◆ guidance_state_cb()

◆ mobilityOperationCb()

◆ on_activate_plugin()

◆ on_configure_plugin()

◆ parameter_update_callback()

◆ parseStrategyParams()

◆ plan_maneuvers_callback()

◆ publishMobilityOperation()

◆ set_wm()

Member Data Documentation

◆ approaching_stop_controlled_interction_

◆ bsm_id_

◆ bsm_msg_count_

◆ bsm_sec_mark_

◆ bsm_sub_

◆ config_

◆ current_downtrack_

◆ current_pose_sub_

◆ guidance_engaged_

◆ guidance_state_sub_

◆ intersection_end_downtrack_

◆ intersection_speed_

◆ intersection_turn_direction_

◆ is_allowed_int_

◆ mob_op_pub_timer_

◆ mob_operation_sub_

◆ mobility_operation_pub_

◆ previous_strategy_params_

◆ scheduled_depart_time_

◆ scheduled_departure_position_

◆ scheduled_enter_time_

◆ scheduled_stop_time_

◆ speed_limit_

◆ stop_controlled_intersection_strategy_

◆ street_msg_timestamp_

◆ vehicle_engaged_

◆ wm_