approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin Class Reference

Class that implements the Approaching Emergency Vehicle Plugin (ERV) strategic plugin. This class is primarily responsible for handling ROS topic subscriptions, publications, and service callbacks for processing incoming BSMs from ERV's, generating manuever plans that account for a nearby ERV, and publishing warning message(s) to an approaching ERV when the ego vehicle is in its path. More...

#include <approaching_emergency_vehicle_plugin_node.hpp>

Inheritance diagram for approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin:

Collaboration diagram for approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin:

Public Member Functions
	ApproachingEmergencyVehiclePlugin (const rclcpp::NodeOptions &)
	ApproachingEmergencyVehiclePlugin constructor. More...
rcl_interfaces::msg::SetParametersResult	parameter_update_callback (const std::vector< rclcpp::Parameter > &parameters)
	Callback for dynamic parameter updates. More...
void	incomingBsmCallback (carma_v2x_msgs::msg::BSM::UniquePtr msg)
	Incoming BSM subscription callback, which determines whether a BSM should be processed, and extracts ERV information from the BSM through a call to getErvInformationFromBsm(). This function is responsible for updating this plugin's tracked_erv_ object and setting has_tracked_erv_ to true when a new ERV becomes tracked. More...
void	routeStateCallback (carma_planning_msgs::msg::RouteState::UniquePtr msg)
	Route State subscription callback, with is used to update this plugin's latest_route_state_ object. More...
void	routeCallback (carma_planning_msgs::msg::Route::UniquePtr msg)
	Route subscription callback, with is used to update this plugin's future_route_lanelet_ids_ object. More...
void	georeferenceCallback (const std_msgs::msg::String::UniquePtr msg)
	Subscription callback for the georeference. More...
void	incomingEmergencyVehicleAckCallback (const carma_v2x_msgs::msg::EmergencyVehicleAck::UniquePtr msg)
	Subscription callback for incoming EmergencyVehicleAck messages. If the message is from the currently tracked ERV and is intended for the ego vehicle, then the plugin will reset num_warnings_broadcasted_ to zero and will reset the should_broadcast_warnings_ flag to false. More...
void	twistCallback (geometry_msgs::msg::TwistStamped::UniquePtr msg)
	Subscription callback for the twist subscriber, which will store latest current velocity of the ego vehicle. More...
void	guidanceStateCallback (const carma_planning_msgs::msg::GuidanceState::UniquePtr msg)
	Subscription callback to process the latest guidance state and update the is_guidance_engaged_ flag accordingly. More...
void	plan_maneuvers_callback (std::shared_ptr< rmw_request_id_t >, carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp) override
	Extending class provided callback which should return a planned trajectory based on the provided trajectory planning request. More...
bool	get_availability () override
	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 () override
	Returns the version id of this plugin. More...
carma_ros2_utils::CallbackReturn	on_configure_plugin ()
	This method is used to load parameters and will be called on the configure state transition. More...
carma_ros2_utils::CallbackReturn	on_activate_plugin ()
	This method is used to create a timer and will be called on the activate transition. 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
carma_wm::WorldModelConstPtr	wm_

Private Member Functions
boost::optional< lanelet::BasicPoint2d >	getErvPositionInMap (const double &current_latitude, const double &current_longitude)
	Helper function to obtain an ERV's position in the map frame from its current latitude and longitude. More...
lanelet::Optional< lanelet::routing::Route >	generateErvRoute (double current_latitude, double current_longitude, std::vector< carma_v2x_msgs::msg::Position3D > erv_destination_points)
	Helper function to generate an ERV's route based on its current position and its future route destination points. More...
boost::optional< lanelet::ConstLanelet >	getRouteIntersectingLanelet (const lanelet::routing::Route &erv_future_route)
	Helper function to obtain the earliest lanelet that exists on both an ERV's future route and the ego vehicle's future shortest path. Accesses the ego vehicle's future shortest path using wm_ object. More...
boost::optional< double >	getSecondsUntilPassing (lanelet::Optional< lanelet::routing::Route > &erv_future_route, const lanelet::BasicPoint2d &erv_position_in_map, const double &erv_current_speed, lanelet::ConstLanelet &intersecting_lanelet)
	Helper function to calculate the estimated seconds until an ERV will pass the ego vehicle. This is an estimate since it assumes both the ERV and the ego vehicle will continue travelling at their current speed. More...
boost::optional< ErvInformation >	getErvInformationFromBsm (carma_v2x_msgs::msg::BSM::UniquePtr msg)
	Through internal logic and calls to separate helper functions, this function processes a received BSM to obtain all necessary information for an ERV to be tracked. This "necessary information" includes all fields contained within an ErvInformation struct. More...
void	checkForErvTimeout ()
	This is a callback function for the erv_timeout_timer_ timer, and is called to determine whether a timeout has occurred for the currently tracked ERV by comparing the duration since the latest ERV BSM was received with config_.timeout_duration. If a timeout has occurred, an ERV_UPDATE_TIMEOUT event will be triggered for this plugin's transition_table_ object, and the has_tracked_erv_ boolean flag will be set to false. More...
void	broadcastWarningToErv ()
	This is a callback function for the warning_broadcast_timer_, and is called to broadcast an EmergencyVehicleResponse warning message to the currently tracked ERV when the ego vehicle is in the ERV's path, but is unable to change lanes because the ERV is estimated to pass the ego vehicle in under config_.passing_threshold. It increases the num_warnings_broadcasted_ counter, and resets it to 0 when it has reached config_.max_warning_broadcasts. Additionally, when config_.max_warning_broadcasts is reached, this method sets should_broadcast_warnings_ to false. More...
void	publishApproachingErvStatus ()
	This is a callback function for the approaching_emergency_vehicle_status_timer_. It makes a call to generateApproachingErvStatusMessage() and publishes the generated message to approaching_erv_status_pub_. More...
void	publishHazardLightStatus ()
	This is a callback function for publishing turn ON/OFF (true/false) hazard light command to the ssc driver. More...
carma_msgs::msg::UIInstructions	generateApproachingErvStatusMessage ()
	Function to generate a carma_msgs::msg::UIInstructions message that describes whether there is currently an approaching ERV that is being tracked by this plugin. The generated message will follow the format described for this plugin's APPROACHING_ERV_STATUS_PARAMS string object. More...
boost::optional< lanelet::ConstLanelet >	getLaneletOnEgoRouteFromMapPosition (const double &x_position, const double &y_position)
	Helper function to convert a map x,y coordinate pair to a lanelet on the ego vehicle's route. More...
double	getLaneletSpeedLimit (const lanelet::ConstLanelet &lanelet)
	Helper function to extract the speed limit (m/s) from a provided lanelet. More...
rclcpp::Duration	getManeuverDuration (const carma_planning_msgs::msg::Maneuver &maneuver, double epsilon) const
	Helper function to obtain the (seconds) of a provided maneuver. More...
carma_planning_msgs::msg::Maneuver	composeLaneFollowingManeuverMessage (double start_dist, double end_dist, double start_speed, double target_speed, int lanelet_id, rclcpp::Time &start_time) const
	Function to compose a lane following maneuver message based on the provided maneuver parameters. More...
carma_planning_msgs::msg::Maneuver	composeLaneChangeManeuverMessage (double start_dist, double end_dist, double start_speed, double target_speed, lanelet::Id starting_lane_id, lanelet::Id ending_lane_id, rclcpp::Time &start_time) const
	Function to compose a lane change maneuver message based on the provided maneuver parameters. More...
carma_planning_msgs::msg::Maneuver	composeStopAndWaitManeuverMessage (double start_dist, double end_dist, double start_speed, lanelet::Id starting_lane_id, lanelet::Id ending_lane_id, double stopping_deceleration, rclcpp::Time &start_time) const
	Function to compose a stop and wait maneuver message based on the provided maneuver parameters. More...
void	addStopAndWaitToEndOfPlan (carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp, double downtrack_progress, double stop_maneuver_beginning_downtrack, double end_of_route_downtrack, double stopping_entry_speed, double stopping_deceleration, double current_lanelet_ending_downtrack, lanelet::ConstLanelet current_lanelet, rclcpp::Time time_progress)
	Function to add a stop and wait maneuver to the end of the maneuver plan contained in the provided 'resp' object. If there is signficant distance between the provided downtrack_progress and stop_maneuver_beginning_downtrack, a lane follow maneuver will be added first to fill this distance. Otherwise, the stop and wait maneuver will at the downtrack provided by downtrack_progress. More...
void	generateReducedSpeedLaneFollowingeManeuverPlan (carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp, lanelet::ConstLanelet current_lanelet, double downtrack_progress, double current_lanelet_ending_downtrack, double speed_progress, double target_speed, rclcpp::Time time_progress, bool is_maintaining_non_reduced_speed)
	Function to generate a maneuver plan when the ego vehicle must remain in its lane due to being in the SLOWING_DOWN_FOR_ERV state. The target speed of lane follow maneuvers in the generated plan will have a reduced value. More...
void	generateMoveOverManeuverPlan (carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp, lanelet::ConstLanelet current_lanelet, double downtrack_progress, double current_lanelet_ending_downtrack, double speed_progress, double target_speed, rclcpp::Time time_progress, int ego_lane_index, int erv_lane_index)
	Function to generate a maneuver plan when the ego vehicle must change lanes due to being in the MOVING_OVER_FOR_APPROACHING_ERV state. If both the ego vehicle and the ERV are in the rightmost lane, a left lane change will be contained in the plan; otherwise a right lane change will be planned. More...
std::vector< lanelet::BasicPoint2d >	filter_points_ahead (const lanelet::BasicPoint2d &reference_point, const std::vector< lanelet::BasicPoint2d > &original_points) const
	Helper function that return points ahead of the given reference point accounting for the list of point's direction (idx 0 is the beginning). Used for getting route destination points ahead of the ERV. More...
	FRIEND_TEST (Testapproaching_emergency_vehicle_plugin, testStateMachineTransitions)
	FRIEND_TEST (Testapproaching_emergency_vehicle_plugin, testBSMProcessing)
	FRIEND_TEST (Testapproaching_emergency_vehicle_plugin, testRouteConflict)
	FRIEND_TEST (Testapproaching_emergency_vehicle_plugin, testManeuverPlanWhenSlowingDownForErv)
	FRIEND_TEST (Testapproaching_emergency_vehicle_plugin, testManeuverPlanWhenMovingOverForErv)
	FRIEND_TEST (Testapproaching_emergency_vehicle_plugin, testWarningBroadcast)
	FRIEND_TEST (Testapproaching_emergency_vehicle_plugin, testApproachingErvStatusMessage)
	FRIEND_TEST (Testapproaching_emergency_vehicle_plugin, filter_points_ahead)

Private Attributes
carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::BSM >	incoming_bsm_sub_
carma_ros2_utils::SubPtr< std_msgs::msg::String >	georeference_sub_
carma_ros2_utils::SubPtr< carma_planning_msgs::msg::RouteState >	route_state_sub_
carma_ros2_utils::SubPtr< geometry_msgs::msg::TwistStamped >	twist_sub_
carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::EmergencyVehicleAck >	incoming_emergency_vehicle_ack_sub_
carma_ros2_utils::SubPtr< carma_planning_msgs::msg::GuidanceState >	guidance_state_sub_
carma_ros2_utils::SubPtr< carma_planning_msgs::msg::Route >	route_sub_
carma_ros2_utils::PubPtr< carma_v2x_msgs::msg::EmergencyVehicleResponse >	outgoing_emergency_vehicle_response_pub_
carma_ros2_utils::PubPtr< carma_msgs::msg::UIInstructions >	approaching_erv_status_pub_
carma_ros2_utils::PubPtr< std_msgs::msg::Bool >	hazard_light_cmd_pub_
Config	config_
ApproachingEmergencyVehicleTransitionTable	transition_table_
ErvInformation	tracked_erv_
bool	has_tracked_erv_ = false
int	ego_lane_index_
carma_planning_msgs::msg::ManeuverPlan	latest_maneuver_plan_
const std::string	APPROACHING_ERV_STATUS_PARAMS = "HAS_APPROACHING_ERV:%1%,TIME_UNTIL_PASSING:%2$.1f,EGO_VEHICLE_ACTION:%3%"
rclcpp::TimerBase::SharedPtr	erv_timeout_timer_
bool	has_broadcasted_warning_messages_ = false
bool	should_broadcast_warnings_ = false
int	num_warnings_broadcasted_ = 0
rclcpp::TimerBase::SharedPtr	warning_broadcast_timer_
rclcpp::TimerBase::SharedPtr	approaching_emergency_vehicle_status_timer_
rclcpp::TimerBase::SharedPtr	hazard_light_timer_
UpcomingLaneChangeParameters	upcoming_lc_params_
bool	has_planned_upcoming_lc_ = false
bool	hazard_light_cmd_ = false
const double	MAINTAIN_SPEED_THRESHOLD = 8.0
bool	is_maintaining_non_reduced_speed_ = false
double	non_reduced_speed_to_maintain_ = 4.4704
bool	is_guidance_engaged_ = false
std::unordered_map< std::string, bool >	is_same_direction_
std::unordered_map< std::string, rclcpp::Time >	latest_erv_update_times_
boost::optional< std::string >	map_projector_
carma_planning_msgs::msg::RouteState	latest_route_state_
double	current_speed_
bool	has_received_route_state_ = false
std::vector< int >	future_route_lanelet_ids_
std::string	logger_name = "approaching_emergency_vehicle_plugin"
std::string	strategic_plugin_name_ = "approaching_emergency_vehicle_plugin"
std::shared_ptr< carma_wm::CARMAWorldModel >	erv_world_model_
double	epsilon_ = 0.0001

Detailed Description

Class that implements the Approaching Emergency Vehicle Plugin (ERV) strategic plugin. This class is primarily responsible for handling ROS topic subscriptions, publications, and service callbacks for processing incoming BSMs from ERV's, generating manuever plans that account for a nearby ERV, and publishing warning message(s) to an approaching ERV when the ego vehicle is in its path.

Definition at line 110 of file approaching_emergency_vehicle_plugin_node.hpp.

Constructor & Destructor Documentation

ApproachingEmergencyVehiclePlugin()

approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::ApproachingEmergencyVehiclePlugin ( const rclcpp::NodeOptions &  options )

explicit

ApproachingEmergencyVehiclePlugin constructor.

Definition at line 41 of file approaching_emergency_vehicle_plugin_node.cpp.

      : carma_guidance_plugins::StrategicPlugin(options)
  {
    // Create initial config
    config_ = Config();
 
    // Declare parameters
    config_.passing_threshold = declare_parameter<double>("passing_threshold", config_.passing_threshold);
    config_.approaching_threshold = declare_parameter<double>("approaching_threshold", config_.approaching_threshold);
    config_.finished_passing_threshold = declare_parameter<double>("finished_passing_threshold", config_.finished_passing_threshold);
    config_.min_lane_following_duration_before_lane_change = declare_parameter<double>("min_lane_following_duration_before_lane_change", config_.min_lane_following_duration_before_lane_change);
    config_.bsm_processing_frequency = declare_parameter<double>("bsm_processing_frequency", config_.bsm_processing_frequency);
    config_.speed_limit_reduction_during_passing = declare_parameter<double>("speed_limit_reduction_during_passing", config_.speed_limit_reduction_during_passing);
    config_.minimum_reduced_speed_limit = declare_parameter<double>("minimum_reduced_speed_limit", config_.minimum_reduced_speed_limit);
    config_.default_speed_limit = declare_parameter<double>("default_speed_limit", config_.default_speed_limit);
    config_.reduced_speed_buffer = declare_parameter<double>("reduced_speed_buffer", config_.reduced_speed_buffer);
    config_.timeout_check_frequency = declare_parameter<double>("timeout_check_frequency", config_.timeout_check_frequency);
    config_.timeout_duration = declare_parameter<double>("timeout_duration", config_.timeout_duration);
    config_.minimal_plan_duration = declare_parameter<double>("minimal_plan_duration", config_.minimal_plan_duration);
    config_.buffer_distance_before_stopping = declare_parameter<double>("buffer_distance_before_stopping", config_.buffer_distance_before_stopping);
    config_.stopping_accel_limit_multiplier = declare_parameter<double>("stopping_accel_limit_multiplier", config_.stopping_accel_limit_multiplier);
    config_.vehicle_acceleration_limit = declare_parameter<double>("vehicle_acceleration_limit", config_.vehicle_acceleration_limit);
    config_.route_end_point_buffer = declare_parameter<double>("route_end_point_buffer", config_.route_end_point_buffer);
    config_.approaching_erv_status_publication_frequency = declare_parameter<double>("approaching_erv_status_publication_frequency", config_.approaching_erv_status_publication_frequency);
    config_.warning_broadcast_frequency = declare_parameter<double>("warning_broadcast_frequency", config_.warning_broadcast_frequency);
    config_.max_warning_broadcasts = declare_parameter<int>("max_warning_broadcasts", config_.max_warning_broadcasts);
    config_.vehicle_length = declare_parameter<double>("vehicle_length", config_.vehicle_length);
    config_.lane_following_plugin = declare_parameter<std::string>("lane_following_plugin", config_.lane_following_plugin);
    config_.lane_change_plugin = declare_parameter<std::string>("lane_change_plugin", config_.lane_change_plugin);
    config_.vehicle_id = declare_parameter<std::string>("vehicle_id", config_.vehicle_id);
 
    erv_world_model_.reset(new carma_wm::CARMAWorldModel);
  }

References approaching_emergency_vehicle_plugin::Config::approaching_erv_status_publication_frequency, approaching_emergency_vehicle_plugin::Config::approaching_threshold, approaching_emergency_vehicle_plugin::Config::bsm_processing_frequency, approaching_emergency_vehicle_plugin::Config::buffer_distance_before_stopping, config_, approaching_emergency_vehicle_plugin::Config::default_speed_limit, erv_world_model_, approaching_emergency_vehicle_plugin::Config::finished_passing_threshold, approaching_emergency_vehicle_plugin::Config::lane_change_plugin, approaching_emergency_vehicle_plugin::Config::lane_following_plugin, approaching_emergency_vehicle_plugin::Config::max_warning_broadcasts, approaching_emergency_vehicle_plugin::Config::min_lane_following_duration_before_lane_change, approaching_emergency_vehicle_plugin::Config::minimal_plan_duration, approaching_emergency_vehicle_plugin::Config::minimum_reduced_speed_limit, approaching_emergency_vehicle_plugin::Config::passing_threshold, approaching_emergency_vehicle_plugin::Config::reduced_speed_buffer, approaching_emergency_vehicle_plugin::Config::route_end_point_buffer, approaching_emergency_vehicle_plugin::Config::speed_limit_reduction_during_passing, approaching_emergency_vehicle_plugin::Config::stopping_accel_limit_multiplier, approaching_emergency_vehicle_plugin::Config::timeout_check_frequency, approaching_emergency_vehicle_plugin::Config::timeout_duration, approaching_emergency_vehicle_plugin::Config::vehicle_acceleration_limit, approaching_emergency_vehicle_plugin::Config::vehicle_id, approaching_emergency_vehicle_plugin::Config::vehicle_length, and approaching_emergency_vehicle_plugin::Config::warning_broadcast_frequency.

Member Function Documentation

addStopAndWaitToEndOfPlan()

void approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::addStopAndWaitToEndOfPlan ( carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr  resp, double  downtrack_progress, double  stop_maneuver_beginning_downtrack, double  end_of_route_downtrack, double  stopping_entry_speed, double  stopping_deceleration, double  current_lanelet_ending_downtrack, lanelet::ConstLanelet  current_lanelet, rclcpp::Time  time_progress  )

private

Function to add a stop and wait maneuver to the end of the maneuver plan contained in the provided 'resp' object. If there is signficant distance between the provided downtrack_progress and stop_maneuver_beginning_downtrack, a lane follow maneuver will be added first to fill this distance. Otherwise, the stop and wait maneuver will at the downtrack provided by downtrack_progress.

Parameters

resp	The service response for the Plan Maneuvers service, which will be updated by this function.
downtrack_progress	The current downtrack that the first maneuver generated by this function will begin at.
stop_maneuver_beginning_downtrack	The downtrack that the stop and wait maneuver must begin by.
end_of_route_downtrack	The ending downtrack of the ego vehicle's route.
stopping_entry_speed	The estimated speed (m/s) that the vehicle will begin the stop and wait maneuver at. If a lane follow is created before the stop and wait maneuver, the lane follow maneuver will include a start_speed and target_speed equal to this speed.
current_lanelet_ending_downtrack	The ending downtrack of the current lanelet that the first maneuver generated by this function will use.
stopping_deceleration	The stopping deceleration rate for the planned stop and wait maneuver.
current_lanelet	The current lanelet that the first maneuver generated by this function will use.
time_progress	The time that the first maneuver generated by this function will begin at.

Returns

None; but function updates the provided 'resp', which is the service response for the Plan Maneuvers service. The maneuver plan in this response will be updated to have a stop and wait maneuver added at its end.

Definition at line 1150 of file approaching_emergency_vehicle_plugin_node.cpp.

  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Maneuver plan is reaching the end of the route");
 
    // Add a lane follow maneuver before the stop and wait maneuver if the distance before stop_maneuver_beginning_downtrack is too large
    if((stop_maneuver_beginning_downtrack - downtrack_progress) >= config_.buffer_distance_before_stopping){
      // Compose lane follow maneuver and update downtrack_progress
      resp->new_plan.maneuvers.push_back(composeLaneFollowingManeuverMessage(downtrack_progress, stop_maneuver_beginning_downtrack,  
                            stopping_entry_speed, stopping_entry_speed, current_lanelet.id(), time_progress));
      
      downtrack_progress = stop_maneuver_beginning_downtrack;
      time_progress += getManeuverDuration(resp->new_plan.maneuvers.back(), epsilon_);
    }
 
    // Get the starting lanelet ID for the stop and wait maneuver
    lanelet::Id start_lane_id = current_lanelet.id();
 
    // Identify the ending lanelet for the stop and wait maneuver
    while(current_lanelet_ending_downtrack < end_of_route_downtrack){
      // Get the next lanelet in the current lane if it exists
      if(!wm_->getMapRoutingGraph()->following(current_lanelet, false).empty()){
        current_lanelet = wm_->getMapRoutingGraph()->following(current_lanelet, false).front();
      }
      else{
        RCLCPP_WARN_STREAM(rclcpp::get_logger(logger_name), "A following lanelet in the current lane could not be found; returning empty plan");
        resp->new_plan.maneuvers = {};
        return;
      }          
 
      // Check whether the next lanelet is on the route
      if(!wm_->getRoute()->contains(current_lanelet)){
        RCLCPP_WARN_STREAM(rclcpp::get_logger(logger_name), "The next lanelet in the current lane is not on the route; returning empty plan");
        resp->new_plan.maneuvers = {};
        return;
      }
 
      // Break from loop if lanelet's ending downtrack is after the route end downtrack; this is the maneuver's ending lanelet 
      current_lanelet_ending_downtrack = wm_->routeTrackPos(current_lanelet.centerline2d().back()).downtrack;
      if(current_lanelet_ending_downtrack >= end_of_route_downtrack){
        break;
      }
    }
 
    // Add stop and wait maneuver to maneuver plan
    resp->new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage(downtrack_progress, end_of_route_downtrack,
                  stopping_entry_speed, start_lane_id, current_lanelet.id(), stopping_deceleration, time_progress));
  }

References approaching_emergency_vehicle_plugin::Config::buffer_distance_before_stopping, composeLaneFollowingManeuverMessage(), composeStopAndWaitManeuverMessage(), config_, epsilon_, getManeuverDuration(), logger_name, and wm_.

Referenced by generateMoveOverManeuverPlan(), and generateReducedSpeedLaneFollowingeManeuverPlan().

Here is the call graph for this function:

Here is the caller graph for this function:

broadcastWarningToErv()

void approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::broadcastWarningToErv ( )

private

This is a callback function for the warning_broadcast_timer_, and is called to broadcast an EmergencyVehicleResponse warning message to the currently tracked ERV when the ego vehicle is in the ERV's path, but is unable to change lanes because the ERV is estimated to pass the ego vehicle in under config_.passing_threshold. It increases the num_warnings_broadcasted_ counter, and resets it to 0 when it has reached config_.max_warning_broadcasts. Additionally, when config_.max_warning_broadcasts is reached, this method sets should_broadcast_warnings_ to false.

Definition at line 249 of file approaching_emergency_vehicle_plugin_node.cpp.

                                                               {
    if(has_tracked_erv_ && should_broadcast_warnings_){
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Sending a warning message to " << tracked_erv_.vehicle_id);
 
      carma_v2x_msgs::msg::EmergencyVehicleResponse msg;
      msg.m_header.sender_id = config_.vehicle_id;
      msg.m_header.recipient_id = tracked_erv_.vehicle_id;
 
      msg.can_change_lanes = false;
      msg.reason_exists = true;
      msg.reason = "Vehicle " + config_.vehicle_id + " is unable to change lanes.";
 
      num_warnings_broadcasted_++;
 
      outgoing_emergency_vehicle_response_pub_->publish(msg);
 
      // Reset counter and boolean flag if the maximum number of warnings have been broadcasted
      if(num_warnings_broadcasted_ >= config_.max_warning_broadcasts){
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Maximum number of warning messages sent to " << tracked_erv_.vehicle_id);
        num_warnings_broadcasted_ = 0;
        should_broadcast_warnings_ = false;
      }
    }
  }

References config_, has_tracked_erv_, logger_name, approaching_emergency_vehicle_plugin::Config::max_warning_broadcasts, num_warnings_broadcasted_, outgoing_emergency_vehicle_response_pub_, should_broadcast_warnings_, tracked_erv_, approaching_emergency_vehicle_plugin::Config::vehicle_id, and approaching_emergency_vehicle_plugin::ErvInformation::vehicle_id.

Referenced by on_activate_plugin().

Here is the caller graph for this function:

checkForErvTimeout()

void approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::checkForErvTimeout ( )

private

This is a callback function for the erv_timeout_timer_ timer, and is called to determine whether a timeout has occurred for the currently tracked ERV by comparing the duration since the latest ERV BSM was received with config_.timeout_duration. If a timeout has occurred, an ERV_UPDATE_TIMEOUT event will be triggered for this plugin's transition_table_ object, and the has_tracked_erv_ boolean flag will be set to false.

Definition at line 235 of file approaching_emergency_vehicle_plugin_node.cpp.

                                                            {
    // Trigger timeout event if a timeout has occurred for the currently tracked ERV
    if(has_tracked_erv_){
      double seconds_since_prev_update = (this->get_clock()->now() - tracked_erv_.latest_update_time).seconds();
 
      if(seconds_since_prev_update >= config_.timeout_duration){
        RCLCPP_ERROR_STREAM(rclcpp::get_logger(logger_name), "Timeout occurred for ERV " << tracked_erv_.vehicle_id);
        has_tracked_erv_ = false;
        has_planned_upcoming_lc_ = false;
        transition_table_.event(ApproachingEmergencyVehicleEvent::ERV_UPDATE_TIMEOUT);
      }
    }
  }

References config_, approaching_emergency_vehicle_plugin::ERV_UPDATE_TIMEOUT, approaching_emergency_vehicle_plugin::ApproachingEmergencyVehicleTransitionTable::event(), has_planned_upcoming_lc_, has_tracked_erv_, approaching_emergency_vehicle_plugin::ErvInformation::latest_update_time, logger_name, approaching_emergency_vehicle_plugin::Config::timeout_duration, tracked_erv_, transition_table_, and approaching_emergency_vehicle_plugin::ErvInformation::vehicle_id.

Referenced by on_activate_plugin().

Here is the call graph for this function:

Here is the caller graph for this function:

composeLaneChangeManeuverMessage()

carma_planning_msgs::msg::Maneuver approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::composeLaneChangeManeuverMessage ( double  start_dist, double  end_dist, double  start_speed, double  target_speed, lanelet::Id  starting_lane_id, lanelet::Id  ending_lane_id, rclcpp::Time &  start_time  ) const

private

Function to compose a lane change maneuver message based on the provided maneuver parameters.

Parameters

start_dist	The starting downtrack for the maneuver.
end_dist	The ending downtrack for the maneuver.
start_speed	The start speed (m/s) for the maneuver.
target_speed	The target speed (m/s) for the maneuver.
starting_lane_id	The ID of the starting lanelet for this lane change maneuver.
ending_lane_id	The ID of the ending lanelet for this lane change maneuver.
start_time	The estimated time that the ego vehicle will start this maneuver.

Returns

A lane change maneuver message composed based on the provided maneuver parameters.

Definition at line 1080 of file approaching_emergency_vehicle_plugin_node.cpp.

  {
    carma_planning_msgs::msg::Maneuver maneuver_msg;
    maneuver_msg.type = carma_planning_msgs::msg::Maneuver::LANE_CHANGE;
    maneuver_msg.lane_change_maneuver.parameters.negotiation_type = carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
    maneuver_msg.lane_change_maneuver.parameters.presence_vector = carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN;
    maneuver_msg.lane_change_maneuver.parameters.planning_tactical_plugin = config_.lane_change_plugin;
    maneuver_msg.lane_change_maneuver.parameters.planning_strategic_plugin = strategic_plugin_name_;
    maneuver_msg.lane_change_maneuver.start_dist = start_dist;
    maneuver_msg.lane_change_maneuver.start_speed = start_speed;
    maneuver_msg.lane_change_maneuver.start_time = start_time;
    maneuver_msg.lane_change_maneuver.end_dist = end_dist;
    maneuver_msg.lane_change_maneuver.end_speed = target_speed;
    maneuver_msg.lane_change_maneuver.starting_lane_id = std::to_string(starting_lane_id);
    maneuver_msg.lane_change_maneuver.ending_lane_id = std::to_string(ending_lane_id);
 
    rclcpp::Duration maneuver_duration = getManeuverDuration(maneuver_msg, epsilon_);
    maneuver_msg.lane_change_maneuver.end_time = start_time + maneuver_duration;
 
    // Preserve lane change maneuver ID from previous plan to maintain identical trajectory generation across separate maneuver plans
    if(has_planned_upcoming_lc_){
      maneuver_msg.lane_change_maneuver.parameters.maneuver_id = upcoming_lc_params_.maneuver_id;
    }
    else{
      static auto gen = boost::uuids::random_generator(); // Initialize uuid generator
      maneuver_msg.lane_change_maneuver.parameters.maneuver_id = boost::lexical_cast<std::string>(gen()); // generate uuid and convert to string
    }
 
    RCLCPP_DEBUG_STREAM(get_logger(),"Creating lane change id: "  << maneuver_msg.lane_change_maneuver.parameters.maneuver_id << "start dist: " << start_dist << " end dist: " << end_dist << " Starting llt: " << starting_lane_id << " Ending llt: " << ending_lane_id);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "start speed: " << start_speed << ", end speed: " << target_speed << ", duration: " << maneuver_duration.seconds());
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "start dist: " << start_dist << ", end dist: " << end_dist);
 
    return maneuver_msg;
  }

References config_, epsilon_, getManeuverDuration(), has_planned_upcoming_lc_, approaching_emergency_vehicle_plugin::Config::lane_change_plugin, logger_name, approaching_emergency_vehicle_plugin::UpcomingLaneChangeParameters::maneuver_id, strategic_plugin_name_, carma_cooperative_perception::to_string(), and upcoming_lc_params_.

Referenced by generateMoveOverManeuverPlan().

Here is the call graph for this function:

Here is the caller graph for this function:

composeLaneFollowingManeuverMessage()

carma_planning_msgs::msg::Maneuver approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::composeLaneFollowingManeuverMessage ( double  start_dist, double  end_dist, double  start_speed, double  target_speed, int  lanelet_id, rclcpp::Time &  start_time  ) const

private

Function to compose a lane following maneuver message based on the provided maneuver parameters.

Parameters

start_dist	The starting downtrack for the maneuver.
end_dist	The ending downtrack for the maneuver.
start_speed	The start speed (m/s) for the maneuver.
target_speed	The target speed (m/s) for the maneuver.
lanelet_id	The ID of the lanelet that this maneuver is for.
start_time	The estimated time that the ego vehicle will start this maneuver.

Returns

A lane following maneuver message composed based on the provided maneuver parameters.

Definition at line 1053 of file approaching_emergency_vehicle_plugin_node.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;
    maneuver_msg.lane_following_maneuver.parameters.planning_tactical_plugin = config_.lane_following_plugin;
    maneuver_msg.lane_following_maneuver.parameters.planning_strategic_plugin = 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.lane_ids = { std::to_string(lanelet_id) };
 
    rclcpp::Duration maneuver_duration = getManeuverDuration(maneuver_msg, epsilon_);
    maneuver_msg.lane_following_maneuver.end_time = start_time + maneuver_duration;
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Composed lane follow maneuver for lanelet ID:" << lanelet_id << " with duration " << maneuver_duration.seconds());
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "start speed: " << start_speed << ", end speed: " << target_speed);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "start dist: " << start_dist << ", end dist: " << end_dist);
 
    return maneuver_msg;
  }

References config_, epsilon_, getManeuverDuration(), approaching_emergency_vehicle_plugin::Config::lane_following_plugin, logger_name, strategic_plugin_name_, and carma_cooperative_perception::to_string().

Referenced by addStopAndWaitToEndOfPlan(), generateMoveOverManeuverPlan(), and generateReducedSpeedLaneFollowingeManeuverPlan().

Here is the call graph for this function:

Here is the caller graph for this function:

composeStopAndWaitManeuverMessage()

carma_planning_msgs::msg::Maneuver approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::composeStopAndWaitManeuverMessage ( double  start_dist, double  end_dist, double  start_speed, lanelet::Id  starting_lane_id, lanelet::Id  ending_lane_id, double  stopping_deceleration, rclcpp::Time &  start_time  ) const

private

Function to compose a stop and wait maneuver message based on the provided maneuver parameters.

Parameters

start_dist	The starting downtrack for the maneuver.
end_dist	The ending downtrack for the maneuver.
start_speed	The start speed (m/s) for the maneuver.
starting_lane_id	The ID of the starting lanelet for this stop and wait maneuver.
ending_lane_id	The ID of the ending lanelet for this stop and wait maneuver.
stopping_deceleration	The stopping deceleration rate for the planned stop and wait maneuver.
start_time	The estimated time that the ego vehicle will start this maneuver.

Returns

A lane change maneuver message composed based on the provided maneuver parameters.

Definition at line 1116 of file approaching_emergency_vehicle_plugin_node.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;
    maneuver_msg.stop_and_wait_maneuver.parameters.planning_strategic_plugin = strategic_plugin_name_;
    maneuver_msg.stop_and_wait_maneuver.start_time = start_time;
    maneuver_msg.stop_and_wait_maneuver.start_dist = start_dist;
    maneuver_msg.stop_and_wait_maneuver.start_speed = start_speed;
    maneuver_msg.stop_and_wait_maneuver.end_dist = end_dist;
    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);
 
    rclcpp::Duration maneuver_duration = getManeuverDuration(maneuver_msg, epsilon_);
    maneuver_msg.stop_and_wait_maneuver.end_time = start_time + maneuver_duration;
 
    // Set the meta-data for the StopAndWait Maneuver to define the buffer in the route end point stopping location
    maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(config_.route_end_point_buffer);
    maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(stopping_deceleration);
 
    static auto gen = boost::uuids::random_generator(); // Initialize uuid generator
    maneuver_msg.stop_and_wait_maneuver.parameters.maneuver_id = boost::lexical_cast<std::string>(gen()); // generate uuid and convert to string
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Composed stop and wait maneuver for with start lanelet:" << starting_lane_id << ", end lanelet: " << ending_lane_id << " with duration " << maneuver_duration.seconds());
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "start speed: " << start_speed);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "start dist: " << start_dist << ", end dist: " << end_dist);
 
    return maneuver_msg;
  }

References config_, epsilon_, getManeuverDuration(), logger_name, approaching_emergency_vehicle_plugin::Config::route_end_point_buffer, approaching_emergency_vehicle_plugin::Config::stop_and_wait_plugin, strategic_plugin_name_, and carma_cooperative_perception::to_string().

Referenced by addStopAndWaitToEndOfPlan().

Here is the call graph for this function:

Here is the caller graph for this function:

filter_points_ahead()

std::vector< lanelet::BasicPoint2d > approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::filter_points_ahead ( const lanelet::BasicPoint2d &  reference_point, const std::vector< lanelet::BasicPoint2d > &  original_points  ) const

private

Helper function that return points ahead of the given reference point accounting for the list of point's direction (idx 0 is the beginning). Used for getting route destination points ahead of the ERV.

Parameters

reference_point	Reference point to check against the list of points
original_points	List of points.

Returns

Points ahead of the reference_point. Empty if the point is determined to have passed the destination points. NOTE: if the list only has 1 point, it returns it as it is not possible to determine direction.

Definition at line 737 of file approaching_emergency_vehicle_plugin_node.cpp.

  {
    if (original_points.size() <= 1)
    {
      return original_points;
    }
    
    // extend the list by extrapolating last two points
    auto extended_points = original_points;
    double last_dx = (original_points.end() - 1 ) ->x() - (original_points.end() - 2 ) ->x();
    double last_dy = (original_points.end() - 1 ) ->y() - (original_points.end() - 2 ) ->y();
    lanelet::BasicPoint2d extended_point = {(original_points.end() - 1 ) ->x() + last_dx, (original_points.end() - 1 ) ->y() + last_dy};
    extended_points.push_back(extended_point);
 
    size_t i = 0;
    size_t closest_idx;
    double closest_dist = DBL_MAX;
    while (i < extended_points.size() - 1)
    {
      // Calculate vectors
      double v1x = reference_point.x() - extended_points[i].x();
 
      double v1y = reference_point.y() - extended_points[i].y();
      double v2x = extended_points[i+1].x() - extended_points[i].x();
      double v2y = extended_points[i+1].y() - extended_points[i].y();
 
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "reference_point x: " << reference_point.x() << ", y: " << reference_point.y());
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "extended_points x: " << extended_points[i].x() << ", y: " << extended_points[i].y());
 
 
      // Calculate dot product
      double dotProduct = v1x * v2x + v1y * v2y;
 
      // Calculate magnitudes
      double v1Mag = sqrt(v1x * v1x + v1y * v1y);
      double v2Mag = sqrt(v2x * v2x + v2y * v2y);
 
      // Calculate angle in radians
      double angleRad = acos(dotProduct / (v1Mag * v2Mag));
 
      // Angle between the vectors above 90 degrees means the point i is ahead
      if (angleRad >= M_PI / 2) 
      {
        double dx = reference_point.x() - extended_points[i].x(); 
        double dy = reference_point.y() - extended_points[i].y();
        double distance = sqrt (dx * dx + dy * dy);
        if (distance < closest_dist) // get closest point to the reference point from the multiple possible points 
        {
          closest_dist = distance;
          closest_idx = i;
          RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "closest_dist: " << closest_dist << ", closest_idx" << closest_idx);
 
        }
      }
      i ++;
    }
 
    double dx = reference_point.x() - original_points.back().x(); 
    double dy = reference_point.y() - original_points.back().y();
    double distance = sqrt (dx * dx + dy * dy);
 
    // last point is still closer to the reference point, all points have passed
    // note that if the optimal point is the last one, this check fails as intended
    if (distance < closest_dist) 
    {
      RCLCPP_ERROR_STREAM(rclcpp::get_logger(logger_name), "Returning empty here");
      
      return {};
    }
 
    return std::vector<lanelet::BasicPoint2d>(original_points.begin() + closest_idx, original_points.end());
  }

References process_bag::i, logger_name, process_traj_logs::x, and process_traj_logs::y.

Referenced by generateErvRoute().

Here is the caller graph for this function:

FRIEND_TEST() [1/8]

approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::FRIEND_TEST ( Testapproaching_emergency_vehicle_plugin  , filter_points_ahead    )

private

FRIEND_TEST() [2/8]

approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::FRIEND_TEST ( Testapproaching_emergency_vehicle_plugin  , testApproachingErvStatusMessage    )

private

FRIEND_TEST() [3/8]

approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::FRIEND_TEST ( Testapproaching_emergency_vehicle_plugin  , testBSMProcessing    )

private

FRIEND_TEST() [4/8]

approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::FRIEND_TEST ( Testapproaching_emergency_vehicle_plugin  , testManeuverPlanWhenMovingOverForErv    )

private

FRIEND_TEST() [5/8]

approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::FRIEND_TEST ( Testapproaching_emergency_vehicle_plugin  , testManeuverPlanWhenSlowingDownForErv    )

private

FRIEND_TEST() [6/8]

approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::FRIEND_TEST ( Testapproaching_emergency_vehicle_plugin  , testRouteConflict    )

private

FRIEND_TEST() [7/8]

approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::FRIEND_TEST ( Testapproaching_emergency_vehicle_plugin  , testStateMachineTransitions    )

private

FRIEND_TEST() [8/8]

approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::FRIEND_TEST ( Testapproaching_emergency_vehicle_plugin  , testWarningBroadcast    )

private

generateApproachingErvStatusMessage()

carma_msgs::msg::UIInstructions approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::generateApproachingErvStatusMessage ( )

private

Function to generate a carma_msgs::msg::UIInstructions message that describes whether there is currently an approaching ERV that is being tracked by this plugin. The generated message will follow the format described for this plugin's APPROACHING_ERV_STATUS_PARAMS string object.

Returns

A carma_msgs::msg::UIInstructions message with a 'msg' string field that follows the format described for this plugin's APPROACHING_ERV_STATUS_PARAMS string object.

Note: APPROACHING_ERV_STATUS_PARAMS format: "HAS_APPROACHING_ERV:%1%,TIME_UNTIL_PASSING:%2$.1f,EGO_VEHICLE_ACTION:%3%" |-----—0-------------------—1-------------------—2-----------—| Index 0: (Boolean) Value indicating whether the ego vehicle is tracking an approaching ERV Index 1: (Double; rounded to first decimal place) If an approaching ERV exists, this indicates the estimated seconds until the ERV passes the ego vehicle. Index 2: (String) If an approaching ERV exists, this describes the current action of the ego vehicle in response to the approaching ERV. NOTE: The values of indexes 1 and 2 can be ignored if index 0 indicates that no approaching ERV is being tracked.

Definition at line 280 of file approaching_emergency_vehicle_plugin_node.cpp.

                                                                                                    {
    // Initialize the message that will be returned by this function
    carma_msgs::msg::UIInstructions status_msg;
    status_msg.type = carma_msgs::msg::UIInstructions::INFO;
 
    boost::format fmter(APPROACHING_ERV_STATUS_PARAMS);
    
    // Index 0 of formatted string; indicates whether an ERV that is approaching the ego vehicle is being tracked
    if(has_tracked_erv_ && tracked_erv_.seconds_until_passing <= config_.approaching_threshold){
      fmter %true;
    }
    else{
      fmter %false;
    }
 
    // Index 1 of formatted string; indicates estimated time until tracked ERV passes the ego vehicle
    fmter %tracked_erv_.seconds_until_passing;
 
    // Add Index 2 of formatted string based on this plugin's current ApproachingEmergencyVehicleState
    switch (transition_table_.getState())
    {
      case ApproachingEmergencyVehicleState::NO_APPROACHING_ERV:
        fmter %"No action.";
        break;
 
      case ApproachingEmergencyVehicleState::MOVING_OVER_FOR_APPROACHING_ERV:
        // Add ego vehicle action based on the direction of the upcoming lane change
        if(upcoming_lc_params_.is_right_lane_change){
          fmter %"Approaching ERV is in our lane. Attempting to change lanes to the right.";
        }
        else{
          fmter %"Approaching ERV is in our lane. Attempting to change lanes to the left.";
        }
 
        break;
 
      case ApproachingEmergencyVehicleState::SLOWING_DOWN_FOR_ERV:
        // Add ego vehicle action based on whether the current speed is near the reduced target speed of the latest maneuver plan's first maneuver
        if(!latest_maneuver_plan_.maneuvers.empty()){
          // Extract the target speed in m/s from the latest maneuver plan's first maneuver and convert it to mph
          double target_speed_ms = getManeuverEndSpeed(latest_maneuver_plan_.maneuvers[0]);
          int target_speed_mph = std::round(target_speed_ms * METERS_PER_SEC_TO_MILES_PER_HOUR);
 
          if(ego_lane_index_ == tracked_erv_.lane_index){
            if(!is_maintaining_non_reduced_speed_){
              if(abs(current_speed_ - target_speed_ms) <= config_.reduced_speed_buffer){
                fmter %("Approaching ERV is in our lane and a lane change is not possible. Remaining in the current lane at a reduced speed of " + std::to_string(target_speed_mph) + " mph.");
              }
              else{
                fmter %("Approaching ERV is in our lane and a lane change is not possible. Remaining in the current lane and slowing down to a reduced speed of " + std::to_string(target_speed_mph) + " mph.");
              }
            }
            else{
              int non_reduced_speed_to_maintain_mph = std::round(non_reduced_speed_to_maintain_ * METERS_PER_SEC_TO_MILES_PER_HOUR);
              fmter %("Approaching ERV is in our lane and a lane change is not possible. Remaining in the current lane and maintaining a speed of " + std::to_string(non_reduced_speed_to_maintain_mph) + " mph.");
            }
          }
          else{
            if(abs(current_speed_ - target_speed_ms) <= config_.reduced_speed_buffer){
              fmter %("Approaching ERV is in adjacent lane. Remaining in the current lane at a reduced speed of " + std::to_string(target_speed_mph) + " mph.");
            }
            else{
              fmter %("Approaching ERV is in adjacent lane. Remaining in the current lane and slowing down to a reduced speed of " + std::to_string(target_speed_mph) + " mph.");
            }
          }
        }
        else{
          throw std::invalid_argument("State is SLOWING_DOWN_FOR_ERV but latest maneuver plan is empty!");
        }
        break;
 
      default:
        throw std::invalid_argument("Transition table in unsupported state");
    }
 
    // Add formatted string to status message
    std::string str = fmter.str();
    status_msg.msg = str;
 
    return status_msg;
  }

References APPROACHING_ERV_STATUS_PARAMS, approaching_emergency_vehicle_plugin::Config::approaching_threshold, config_, current_speed_, ego_lane_index_, approaching_emergency_vehicle_plugin::anonymous_namespace{approaching_emergency_vehicle_plugin_node.cpp}::getManeuverEndSpeed(), approaching_emergency_vehicle_plugin::ApproachingEmergencyVehicleTransitionTable::getState(), has_tracked_erv_, is_maintaining_non_reduced_speed_, approaching_emergency_vehicle_plugin::UpcomingLaneChangeParameters::is_right_lane_change, approaching_emergency_vehicle_plugin::ErvInformation::lane_index, latest_maneuver_plan_, approaching_emergency_vehicle_plugin::METERS_PER_SEC_TO_MILES_PER_HOUR, approaching_emergency_vehicle_plugin::MOVING_OVER_FOR_APPROACHING_ERV, approaching_emergency_vehicle_plugin::NO_APPROACHING_ERV, non_reduced_speed_to_maintain_, approaching_emergency_vehicle_plugin::Config::reduced_speed_buffer, approaching_emergency_vehicle_plugin::ErvInformation::seconds_until_passing, approaching_emergency_vehicle_plugin::SLOWING_DOWN_FOR_ERV, carma_cooperative_perception::to_string(), tracked_erv_, transition_table_, and upcoming_lc_params_.

Referenced by publishApproachingErvStatus().

Here is the call graph for this function:

Here is the caller graph for this function:

generateErvRoute()

lanelet::Optional< lanelet::routing::Route > approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::generateErvRoute ( double  current_latitude, double  current_longitude, std::vector< carma_v2x_msgs::msg::Position3D >  erv_destination_points  )

private

Helper function to generate an ERV's route based on its current position and its future route destination points.

Parameters

current_latitude	The current latitude of the ERV.
current_longitude	The current longitude of the ERV.
erv_destination_points	The ERV's future destination points.

Returns

An optional lanelet::routing::Route object, which is empty if the generation of the ERV's route was not successful.

Definition at line 633 of file approaching_emergency_vehicle_plugin_node.cpp.

                                                                                                                                           {
    // Check if the map projection is available
    if (!map_projector_) {
      throw std::invalid_argument("Attempting to generate an ERV's route before map projection was set");
    }
      
    // Build map projector
    lanelet::projection::LocalFrameProjector projector(map_projector_.get().c_str());
 
    // Create vector to hold ERV's destination points
    std::vector<lanelet::BasicPoint3d> erv_destination_points_projected;
 
    // Add ERV's current location to the beginning of erv_destination_points
    lanelet::GPSPoint current_erv_location;
 
    current_erv_location.lat = current_latitude;
    current_erv_location.lon = current_longitude;
 
   
 
    // Add ERV's future destination points to erv_destination_points
    if(erv_destination_points.size() > 0){
      for(size_t i = 0; i < erv_destination_points.size(); ++i){
        carma_v2x_msgs::msg::Position3D position_3d_point = erv_destination_points[i];
        
        lanelet::GPSPoint erv_destination_point;
        erv_destination_point.lon = position_3d_point.longitude;
        erv_destination_point.lat = position_3d_point.latitude;
 
        if(position_3d_point.elevation_exists){
          erv_destination_point.ele = position_3d_point.elevation;
        }
 
        erv_destination_points_projected.emplace_back(projector.forward(erv_destination_point));
      }
    }
 
    // Convert ERV destination points to map frame
    auto erv_destination_points_in_map = lanelet::utils::transform(erv_destination_points_projected, [](auto a) { return lanelet::traits::to2D(a); });
 
    auto cmv_location = lanelet::traits::to2D(projector.forward(current_erv_location));
    auto shortened_erv_destination_points_in_map = filter_points_ahead(cmv_location, erv_destination_points_in_map);
 
    if(shortened_erv_destination_points_in_map.empty())
    {
        RCLCPP_ERROR_STREAM(rclcpp::get_logger(logger_name), "ERV has passed all the destination points!");
 
        // Return empty route
        return lanelet::Optional<lanelet::routing::Route>();
    }
 
    // Verify that ERV destination points are geometrically in the map
    for(size_t i = 0; i < shortened_erv_destination_points_in_map.size(); ++i){
      auto pt = shortened_erv_destination_points_in_map[i];
 
      // Return empty route if a destination point is not contained within the map
      if((wm_->getLaneletsFromPoint(shortened_erv_destination_points_in_map[i])).empty()){
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "ERV destination point " << i 
                << " is not contained in a lanelet map; x: " << pt.x() << " y: " << pt.y());
 
        return lanelet::Optional<lanelet::routing::Route>();
      }
    }
 
    // Obtain ERV's starting lanelet
    auto starting_lanelet_vector = lanelet::geometry::findNearest(wm_->getMap()->laneletLayer, cmv_location, 1);
    if(starting_lanelet_vector.empty())
    {
        RCLCPP_ERROR_STREAM(rclcpp::get_logger(logger_name), "Found no lanelets in the map. ERV routing cannot be completed.");
 
        // Return empty route
        return lanelet::Optional<lanelet::routing::Route>();
    }
    auto starting_lanelet = lanelet::ConstLanelet(starting_lanelet_vector[0].second.constData());
 
    // Obtain ERV's ending lanelet
    auto ending_lanelet_vector = lanelet::geometry::findNearest(wm_->getMap()->laneletLayer, shortened_erv_destination_points_in_map.back(), 1);
    auto ending_lanelet = lanelet::ConstLanelet(ending_lanelet_vector[0].second.constData());
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "ending_lanelet: " << ending_lanelet.id());
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "starting_lanelet: " << starting_lanelet.id());
 
    // Obtain ERV's via lanelets
    std::vector<lanelet::BasicPoint2d> via = std::vector<lanelet::BasicPoint2d>(shortened_erv_destination_points_in_map.begin(), shortened_erv_destination_points_in_map.end() - 1);
    lanelet::ConstLanelets via_lanelets_vector;
    for(const auto& point : via){
      auto lanelet_vector = lanelet::geometry::findNearest(wm_->getMap()->laneletLayer, point, 1);
      auto chosen_lanelet_to_emplace = lanelet::ConstLanelet(lanelet_vector[0].second.constData());
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "chosen_lanelet_to_emplace: " << chosen_lanelet_to_emplace.id());
 
      if (chosen_lanelet_to_emplace.id() != starting_lanelet.id())  // if id is same, it fails to route
        via_lanelets_vector.emplace_back(chosen_lanelet_to_emplace);
      else
      {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "======> id was found same: " << chosen_lanelet_to_emplace.id());
      }
    }
 
    // Generate the ERV's route
    auto erv_route = wm_->getMapRoutingGraph()->getRouteVia(starting_lanelet, via_lanelets_vector, ending_lanelet);
 
    return erv_route;
  }

References filter_points_ahead(), process_bag::i, logger_name, map_projector_, process_traj_logs::point, and wm_.

Referenced by getErvInformationFromBsm().

Here is the call graph for this function:

Here is the caller graph for this function:

generateMoveOverManeuverPlan()

void approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::generateMoveOverManeuverPlan ( carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr  resp, lanelet::ConstLanelet  current_lanelet, double  downtrack_progress, double  current_lanelet_ending_downtrack, double  speed_progress, double  target_speed, rclcpp::Time  time_progress, int  ego_lane_index, int  erv_lane_index  )

private

Function to generate a maneuver plan when the ego vehicle must change lanes due to being in the MOVING_OVER_FOR_APPROACHING_ERV state. If both the ego vehicle and the ERV are in the rightmost lane, a left lane change will be contained in the plan; otherwise a right lane change will be planned.

Parameters

resp	The service response for the Plan Maneuvers service, which will be updated by this function.
current_lanelet	The current lanelet that the first maneuver generated by this function will use.
downtrack_progress	The current downtrack that the first maneuver generated by this function will begin at.
current_lanelet_ending_downtrack	The ending downtrack of the current lanelet that the first maneuver generated by this function will use.
speed_progress	The current speed that the first maneuver generated by this function will begin at.
target_speed	The target speed of the initial lane follow maneuver generated by this function.
time_progress	The time that the first maneuver generated by this function will begin at.
ego_lane_index	The ego vehicle's current lane index (NOTE: For 'lane index', 0 is rightmost lane, 1 is second rightmost, etc.; Only the current travel direction is considered)
erv_lane_index	The ERV's current lane index (NOTE: For 'lane index', 0 is rightmost lane, 1 is second rightmost, etc.; Only the current travel direction is considered)

Returns

None; but function updates the provided 'resp', which is the service response for the Plan Maneuvers service. The maneuver plan in this response will be updated to contain lane following maneuvers along with one lane change maneuver. A stop and wait maneuver will be included at the end if the maneuver plan reaches the end of the route.

Definition at line 1295 of file approaching_emergency_vehicle_plugin_node.cpp.

  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Generating move-over maneuver plan");
 
    double maneuver_plan_ending_downtrack = downtrack_progress + config_.minimal_plan_duration * target_speed;
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Ending downtrack based on plan duration: " << maneuver_plan_ending_downtrack);
 
    maneuver_plan_ending_downtrack = std::min(maneuver_plan_ending_downtrack, wm_->getRouteEndTrackPos().downtrack);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Ending downtrack based on end of route: " << wm_->getRouteEndTrackPos().downtrack);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Selected ending downtrack: " << maneuver_plan_ending_downtrack);
 
    // Compute target deceleration for stopping
    double stopping_deceleration = config_.vehicle_acceleration_limit * config_.stopping_accel_limit_multiplier;
 
    // Compute stopping distance where v_f = 0
    // (v_f^2 - v_i^2) / (2*a) = d 
    double stopping_distance = (target_speed * target_speed) / (2.0 * stopping_deceleration);
    double begin_stopping_downtrack = wm_->getRouteEndTrackPos().downtrack - stopping_distance;
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Stop and wait maneuver must begin by downtrack " << begin_stopping_downtrack);
 
    // Generate maneuver plan when there is already a planned upcoming lane change
    if(has_planned_upcoming_lc_){
      while(downtrack_progress < maneuver_plan_ending_downtrack){
        if(begin_stopping_downtrack <= current_lanelet_ending_downtrack){
          // Complete maneuver plan with a stop and wait maneuver if the current lanelet intercepts the stopping downtrack
          addStopAndWaitToEndOfPlan(resp, downtrack_progress, begin_stopping_downtrack, wm_->getRouteEndTrackPos().downtrack, 
                          speed_progress, stopping_deceleration, current_lanelet_ending_downtrack, current_lanelet, time_progress);
          return;
        }
        else{
          // Plan lane change maneuver if current downtrack progress is between the starting and ending downtracks of the planned upcoming lane change, otherwise plan lane follow maneuver
          if(((upcoming_lc_params_.start_dist - epsilon_) <= downtrack_progress) && (downtrack_progress < (upcoming_lc_params_.end_dist - epsilon_))){
            resp->new_plan.maneuvers.push_back(composeLaneChangeManeuverMessage(upcoming_lc_params_.start_dist, upcoming_lc_params_.end_dist,  
                                      upcoming_lc_params_.start_speed, upcoming_lc_params_.end_speed, upcoming_lc_params_.starting_lanelet.id(), upcoming_lc_params_.ending_lanelet.id(), time_progress));
            
            current_lanelet = upcoming_lc_params_.ending_lanelet;
          }
          else{
            resp->new_plan.maneuvers.push_back(composeLaneFollowingManeuverMessage(downtrack_progress, current_lanelet_ending_downtrack,  
                                      speed_progress, target_speed, current_lanelet.id(), time_progress));
          }          
        
          // Get the next lanelet in the current lane if it exists
          if(!wm_->getMapRoutingGraph()->following(current_lanelet, false).empty())
          {
            current_lanelet = wm_->getMapRoutingGraph()->following(current_lanelet, false).front();
          }
          else
          {
            RCLCPP_WARN_STREAM(rclcpp::get_logger(logger_name), "A following lanelet in the current lane could not be found; returning empty plan");
            resp->new_plan.maneuvers = {};
            return;
          }
 
          // Check whether the next lanelet is on the route
          if(!wm_->getRoute()->contains(current_lanelet)){
            RCLCPP_WARN_STREAM(rclcpp::get_logger(logger_name), "The next lanelet in the current lane is not on the route; returning empty plan");
            resp->new_plan.maneuvers = {};
            return;
          }
 
          // Update the maneuver parameters for the next maneuver in the plan
          downtrack_progress = wm_->routeTrackPos(current_lanelet.centerline2d().front()).downtrack;
          current_lanelet_ending_downtrack = wm_->routeTrackPos(current_lanelet.centerline2d().back()).downtrack;
          speed_progress = getManeuverEndSpeed(resp->new_plan.maneuvers.back());
          target_speed = getLaneletSpeedLimit(current_lanelet);
          time_progress += getManeuverDuration(resp->new_plan.maneuvers.back(), epsilon_);
 
          RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Next maneuver starting downtrack is " << downtrack_progress << ", end of plan is at " << maneuver_plan_ending_downtrack);
        }
      }
    }
    else{
      bool completed_initial_lane_following = false; // Flag to indicate whether sufficient lane following has been planned before the lane change
      double initial_lane_following_duration = 0.0; // Object to store the duration of lane following planned before the lane change
 
      while(downtrack_progress < maneuver_plan_ending_downtrack){
 
        if(begin_stopping_downtrack <= current_lanelet_ending_downtrack){
          // Complete maneuver plan with a stop and wait maneuver if the current lanelet intercepts the stopping downtrack
          addStopAndWaitToEndOfPlan(resp, downtrack_progress, begin_stopping_downtrack, wm_->getRouteEndTrackPos().downtrack, 
                          speed_progress, stopping_deceleration, current_lanelet_ending_downtrack, current_lanelet, time_progress);
          return;
        }
        else{
          // First maneuver(s) are lane-following to enable sufficient time to activate turn signals before conducting lane change
          if(!completed_initial_lane_following){
            resp->new_plan.maneuvers.push_back(composeLaneFollowingManeuverMessage(downtrack_progress, current_lanelet_ending_downtrack,  
                                    speed_progress, target_speed, current_lanelet.id(), time_progress));
 
            initial_lane_following_duration += getManeuverDuration(resp->new_plan.maneuvers.back(), epsilon_).seconds();
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Seconds of lane following before lane change: " << initial_lane_following_duration);
 
            if(initial_lane_following_duration >= config_.min_lane_following_duration_before_lane_change){
              RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Minimum lane following duration before lane change satisfied.");
              completed_initial_lane_following = true;
            }
          }
          else{
 
            if(!has_planned_upcoming_lc_){
              // Initialize UpcomingLaneChangeParameters object to store information regarding the planned lane change maneuver
              UpcomingLaneChangeParameters new_lc_params;
 
              // Determine if left lane change or right lane change required, and get the target lanelet
              lanelet::Optional<lanelet::ConstLanelet> target_lanelet;
              if(ego_lane_index != erv_lane_index){
                RCLCPP_WARN_STREAM(rclcpp::get_logger(logger_name), "Planning a move-over maneuver plan when the ego vehicle is not in the ERV's path! Returning an empty maneuver plan");
                resp->new_plan.maneuvers = {};
                return;              
              }
              else if((ego_lane_index == 0) && (erv_lane_index == 0)){
                // Ego vehicle and ERV are both in the rightmost lane so a left lane change is required
                new_lc_params.is_right_lane_change = false;
                target_lanelet = wm_->getMapRoutingGraph()->left(current_lanelet);   
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Composing a left lane change maneuver from lanelet " << current_lanelet.id());
              }
              else{
                // A right lane change is required
                new_lc_params.is_right_lane_change = true;
                target_lanelet = wm_->getMapRoutingGraph()->right(current_lanelet);
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Composing a right lane change maneuver from lanelet " << current_lanelet.id());
              }
 
              // Only compose lane change maneuver if the target lanelet exists, otherwise compose lane follow maneuver
              if(target_lanelet){
                resp->new_plan.maneuvers.push_back(composeLaneChangeManeuverMessage(downtrack_progress, current_lanelet_ending_downtrack,  
                                        speed_progress, target_speed, current_lanelet.id(), target_lanelet.get().id(), time_progress));
                
                has_planned_upcoming_lc_ = true;
                
                new_lc_params.starting_lanelet = current_lanelet;
                new_lc_params.ending_lanelet = target_lanelet.get();
                new_lc_params.start_dist = downtrack_progress;
                new_lc_params.end_dist = current_lanelet_ending_downtrack;
                new_lc_params.start_speed = speed_progress;
                new_lc_params.end_speed = target_speed;
                new_lc_params.maneuver_id = resp->new_plan.maneuvers.back().lane_change_maneuver.parameters.maneuver_id;
                upcoming_lc_params_ = new_lc_params;
 
                current_lanelet = *target_lanelet;
              }
              else{
                RCLCPP_WARN_STREAM(rclcpp::get_logger(logger_name), "A lane change maneuver from " << current_lanelet.id() << " could not be composed since no target lanelet was found!");
                resp->new_plan.maneuvers.push_back(composeLaneFollowingManeuverMessage(downtrack_progress, current_lanelet_ending_downtrack,  
                                        speed_progress, target_speed, current_lanelet.id(), time_progress));
              }
            }
            else{
              resp->new_plan.maneuvers.push_back(composeLaneFollowingManeuverMessage(downtrack_progress, current_lanelet_ending_downtrack,  
                                        speed_progress, target_speed, current_lanelet.id(), time_progress));
            }
          }
 
          // Get the next lanelet in the current lane if it exists
          if(!wm_->getMapRoutingGraph()->following(current_lanelet, false).empty())
          {
            current_lanelet = wm_->getMapRoutingGraph()->following(current_lanelet, false).front();
          }
          else
          {
            RCLCPP_WARN_STREAM(rclcpp::get_logger(logger_name), "A following lanelet in the current lane could not be found; returning empty plan");
            resp->new_plan.maneuvers = {};
            return;
          }
 
          // Check whether the next lanelet is on the route
          if(!wm_->getRoute()->contains(current_lanelet)){
            RCLCPP_WARN_STREAM(rclcpp::get_logger(logger_name), "The next lanelet in the current lane is not on the route; returning empty plan");
            resp->new_plan.maneuvers = {};
            return;
          }
 
          // Update the maneuver parameters for the next maneuver in the plan
          downtrack_progress = wm_->routeTrackPos(current_lanelet.centerline2d().front()).downtrack;
          current_lanelet_ending_downtrack = wm_->routeTrackPos(current_lanelet.centerline2d().back()).downtrack;
          speed_progress = getManeuverEndSpeed(resp->new_plan.maneuvers.back());
          target_speed = getLaneletSpeedLimit(current_lanelet);
          time_progress += getManeuverDuration(resp->new_plan.maneuvers.back(), epsilon_);
 
          RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Next maneuver starting downtrack is " << downtrack_progress << ", end of plan is at " << maneuver_plan_ending_downtrack);
        }
      }    
    }
  }

References addStopAndWaitToEndOfPlan(), composeLaneChangeManeuverMessage(), composeLaneFollowingManeuverMessage(), config_, approaching_emergency_vehicle_plugin::UpcomingLaneChangeParameters::end_dist, approaching_emergency_vehicle_plugin::UpcomingLaneChangeParameters::end_speed, approaching_emergency_vehicle_plugin::UpcomingLaneChangeParameters::ending_lanelet, epsilon_, getLaneletSpeedLimit(), getManeuverDuration(), approaching_emergency_vehicle_plugin::anonymous_namespace{approaching_emergency_vehicle_plugin_node.cpp}::getManeuverEndSpeed(), has_planned_upcoming_lc_, approaching_emergency_vehicle_plugin::UpcomingLaneChangeParameters::is_right_lane_change, logger_name, approaching_emergency_vehicle_plugin::UpcomingLaneChangeParameters::maneuver_id, approaching_emergency_vehicle_plugin::Config::min_lane_following_duration_before_lane_change, approaching_emergency_vehicle_plugin::Config::minimal_plan_duration, approaching_emergency_vehicle_plugin::UpcomingLaneChangeParameters::start_dist, approaching_emergency_vehicle_plugin::UpcomingLaneChangeParameters::start_speed, approaching_emergency_vehicle_plugin::UpcomingLaneChangeParameters::starting_lanelet, approaching_emergency_vehicle_plugin::Config::stopping_accel_limit_multiplier, upcoming_lc_params_, approaching_emergency_vehicle_plugin::Config::vehicle_acceleration_limit, and wm_.

Referenced by plan_maneuvers_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

generateReducedSpeedLaneFollowingeManeuverPlan()

void approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::generateReducedSpeedLaneFollowingeManeuverPlan ( carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr  resp, lanelet::ConstLanelet  current_lanelet, double  downtrack_progress, double  current_lanelet_ending_downtrack, double  speed_progress, double  target_speed, rclcpp::Time  time_progress, bool  is_maintaining_non_reduced_speed  )

private

Function to generate a maneuver plan when the ego vehicle must remain in its lane due to being in the SLOWING_DOWN_FOR_ERV state. The target speed of lane follow maneuvers in the generated plan will have a reduced value.

Parameters

resp	The service response for the Plan Maneuvers service, which will be updated by this function.
current_lanelet	The current lanelet that the first maneuver generated by this function will use.
downtrack_progress	The current downtrack that the first maneuver generated by this function will begin at.
current_lanelet_ending_downtrack	The ending downtrack of the current lanelet that the first maneuver generated by this function will use.
speed_progress	The current speed that the first maneuver generated by this function will begin at.
target_speed	The target speed of the initial lane follow maneuver generated by this function.
time_progress	The time that the first maneuver generated by this function will begin at.
is_maintaining_non_reduced_speed	Flag to indicate whether the ego vehicle should maintain its current speed since it is in the same lane as the approaching ERV, and the time until the ERV will pass the ego vehicle is so slow that if the ego vehicle slows down, it could cause a safety hazard.

Returns

None; but function updates the provided 'resp', which is the service response for the Plan Maneuvers service. The maneuver plan in this response will be updated to contain all lane following maneuvers. A stop and wait maneuver will be included at the end if the maneuver plan reaches the end of the route.

Definition at line 1201 of file approaching_emergency_vehicle_plugin_node.cpp.

  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Generating remain-in-lane maneuver plan");
 
    // Reset planned lane change flag since plugin will not be planning for an upcoming lane change
    has_planned_upcoming_lc_ = false;
 
    // Update maneuver target speed to be a speed being maintained or a reduced speed below the speed limit
    if(is_maintaining_non_reduced_speed){
      // ERV is close enough that if the ego vehicle slows down it could cause a safety hazard; set target speed to maintain a non-reduced speed
      target_speed = non_reduced_speed_to_maintain_;
    }
    else{
      // Reduce target_speed since this method is triggered when an ERV is approaching the ego vehicle
      target_speed = std::max((target_speed - config_.speed_limit_reduction_during_passing), config_.minimum_reduced_speed_limit);
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Maneuver target speed reduced to " << target_speed);
    }
 
    double maneuver_plan_ending_downtrack = downtrack_progress + config_.minimal_plan_duration * target_speed;
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Ending downtrack based on plan duration: " << maneuver_plan_ending_downtrack);
 
    maneuver_plan_ending_downtrack = std::min(maneuver_plan_ending_downtrack, wm_->getRouteEndTrackPos().downtrack);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Ending downtrack based on end of route: " << wm_->getRouteEndTrackPos().downtrack);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Selected ending downtrack: " << maneuver_plan_ending_downtrack);
 
    // Compute target deceleration for stopping
    double stopping_deceleration = config_.vehicle_acceleration_limit * config_.stopping_accel_limit_multiplier;
 
    // Compute stopping distance where v_f = 0
    // (v_f^2 - v_i^2) / (2*a) = d 
    double stopping_distance = (target_speed * target_speed) / (2.0 * stopping_deceleration);
    double begin_stopping_downtrack = wm_->getRouteEndTrackPos().downtrack - stopping_distance;
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Stop and wait maneuver must begin by downtrack " << begin_stopping_downtrack);
 
    // Generate maneuver plan
    while(downtrack_progress < maneuver_plan_ending_downtrack){
 
      if(begin_stopping_downtrack <= current_lanelet_ending_downtrack){
        // Complete maneuver plan with a stop and wait maneuver if the current lanelet intercepts the stopping downtrack
        addStopAndWaitToEndOfPlan(resp, downtrack_progress, begin_stopping_downtrack, wm_->getRouteEndTrackPos().downtrack, 
                        speed_progress, stopping_deceleration, current_lanelet_ending_downtrack, current_lanelet, time_progress);
        return;
      }
      else{
        // Compose lane following maneuver and add it to the response's maneuver plan
        resp->new_plan.maneuvers.push_back(composeLaneFollowingManeuverMessage(downtrack_progress, current_lanelet_ending_downtrack,  
                                speed_progress, target_speed, current_lanelet.id(), time_progress));
 
        // Get the next lanelet in the current lane if it exists
        if(!wm_->getMapRoutingGraph()->following(current_lanelet, false).empty())
        {
          current_lanelet = wm_->getMapRoutingGraph()->following(current_lanelet, false).front();
        }
        else
        {
          RCLCPP_WARN_STREAM(rclcpp::get_logger(logger_name), "A following lanelet in the current lane could not be found; returning empty plan");
          resp->new_plan.maneuvers = {};
          return;
        }
 
        // Check whether the next lanelet is on the route
        if(!wm_->getRoute()->contains(current_lanelet)){
          RCLCPP_WARN_STREAM(rclcpp::get_logger(logger_name), "The next lanelet in the current lane is not on the route; returning empty plan");
          resp->new_plan.maneuvers = {};
          return;
        }
 
        // Update the lane follow maneuver parameters for the next maneuver in the plan
        downtrack_progress = wm_->routeTrackPos(current_lanelet.centerline2d().front()).downtrack;
        current_lanelet_ending_downtrack = wm_->routeTrackPos(current_lanelet.centerline2d().back()).downtrack;
        speed_progress = getManeuverEndSpeed(resp->new_plan.maneuvers.back());
        target_speed = getLaneletSpeedLimit(current_lanelet);
 
        // Update maneuver target speed to be a speed being maintained or a reduced speed below the speed limit
        if(is_maintaining_non_reduced_speed){
          // ERV is close enough that if the ego vehicle slows down it could cause a safety hazard; set target speed to maintain a non-reduced speed
          target_speed = non_reduced_speed_to_maintain_;
        }
        else{
          // Reduce target_speed if an ERV is actively passing the ego vehicle
          target_speed = std::max((target_speed - config_.speed_limit_reduction_during_passing), config_.minimum_reduced_speed_limit);
          RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Maneuver target speed reduced to " << target_speed);
        }
 
        time_progress += getManeuverDuration(resp->new_plan.maneuvers.back(), epsilon_);
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Next maneuver starting downtrack is " << downtrack_progress << ", end of plan is at " << maneuver_plan_ending_downtrack);
      }
    }
  }

References addStopAndWaitToEndOfPlan(), composeLaneFollowingManeuverMessage(), config_, epsilon_, getLaneletSpeedLimit(), getManeuverDuration(), approaching_emergency_vehicle_plugin::anonymous_namespace{approaching_emergency_vehicle_plugin_node.cpp}::getManeuverEndSpeed(), has_planned_upcoming_lc_, logger_name, approaching_emergency_vehicle_plugin::Config::minimal_plan_duration, approaching_emergency_vehicle_plugin::Config::minimum_reduced_speed_limit, non_reduced_speed_to_maintain_, approaching_emergency_vehicle_plugin::Config::speed_limit_reduction_during_passing, approaching_emergency_vehicle_plugin::Config::stopping_accel_limit_multiplier, approaching_emergency_vehicle_plugin::Config::vehicle_acceleration_limit, and wm_.

Referenced by plan_maneuvers_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

georeferenceCallback()

void approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::georeferenceCallback

(

const std_msgs::msg::String::UniquePtr

msg

)

Subscription callback for the georeference.

Parameters

msg	The latest georeference message.

Definition at line 614 of file approaching_emergency_vehicle_plugin_node.cpp.

  {
    // Build projector from proj string
    map_projector_ = msg->data;
  }

References map_projector_.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

get_availability()

bool approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::get_availability ( )

overridevirtual

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 1635 of file approaching_emergency_vehicle_plugin_node.cpp.

                                                           {
    return true;
  }

get_version_id()

std::string approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::get_version_id ( )

overridevirtual

Returns the version id of this plugin.

Returns

The version id represented as a string

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 1639 of file approaching_emergency_vehicle_plugin_node.cpp.

                                                              {
    return "v1.0";
  }

getErvInformationFromBsm()

boost::optional< ErvInformation > approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::getErvInformationFromBsm ( carma_v2x_msgs::msg::BSM::UniquePtr  msg )

private

Through internal logic and calls to separate helper functions, this function processes a received BSM to obtain all necessary information for an ERV to be tracked. This "necessary information" includes all fields contained within an ErvInformation struct.

Parameters

msg	The BSM message to be processed. It may or may not be from an active ERV.

Returns

An optional ErvInformation object, which is empty if BSM processing could not extract all necessary information required for an ERV to be tracked.

Definition at line 401 of file approaching_emergency_vehicle_plugin_node.cpp.

                                                                                                                            {
    // Initialize ErvInformation object, which will be populated with information from this BSM if it is from an ERV
    ErvInformation erv_information;
 
    // Get vehicle_id from the BSM
    std::stringstream ss;
    for(size_t i = 0; i < msg->core_data.id.size(); ++i){
      ss << std::setfill('0') << std::setw(2) << std::hex << (unsigned)msg->core_data.id.at(i);
    }
    erv_information.vehicle_id = ss.str();
 
    // Check whether vehicle's lights and sirens are active
    bool has_active_lights_and_sirens = false;
    if(msg->presence_vector & carma_v2x_msgs::msg::BSM::HAS_PART_II){
 
      // Parse BSM Part II Content to determine whether the vehicle's lights and sirens are active
      if(msg->part_ii.size() > 0){
        for(size_t i = 0; i < msg->part_ii.size(); ++i){
 
          // special_vehicle_extensions contain the status of the vehicle's lights and sirens
          if(msg->part_ii[i].part_ii_id == carma_v2x_msgs::msg::BSMPartIIExtension::SPECIAL_VEHICLE_EXT){
            carma_v2x_msgs::msg::SpecialVehicleExtensions special_vehicle_ext = msg->part_ii[i].special_vehicle_extensions;
 
            if(special_vehicle_ext.presence_vector & carma_v2x_msgs::msg::SpecialVehicleExtensions::HAS_VEHICLE_ALERTS){
              if(special_vehicle_ext.vehicle_alerts.siren_use.siren_in_use == j2735_v2x_msgs::msg::SirenInUse::IN_USE
                && special_vehicle_ext.vehicle_alerts.lights_use.lightbar_in_use == j2735_v2x_msgs::msg::LightbarInUse::IN_USE){
                  has_active_lights_and_sirens = true;
              }
            }
          }
        }
      }
    }
 
    if(!has_active_lights_and_sirens){
      // BSM is not a valid ERV BSM since the lights and sirens are not both active; return an empty object
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "BSM is not a valid ERV BSM since the lights and sirens are not both active; return an empty object");
 
      return boost::optional<ErvInformation>();
    }
 
    // Get vehicle's current speed from the BSM
    if(msg->core_data.presence_vector & carma_v2x_msgs::msg::BSMCoreData::SPEED_AVAILABLE){
      erv_information.current_speed = msg->core_data.speed;
    }
    else{
      // BSM is not a valid ERV BSM since current speed is not included; return an empty object
      RCLCPP_ERROR_STREAM(rclcpp::get_logger(logger_name), "BSM is not a valid ERV BSM since current speed is not included; return an empty object");
 
      return boost::optional<ErvInformation>();
    }
 
    if(erv_information.current_speed <= current_speed_){
      RCLCPP_ERROR_STREAM(rclcpp::get_logger(logger_name), "Ignoring received BSM since ERV speed of " << erv_information.current_speed << " is less than ego speed of " << current_speed_);
      return boost::optional<ErvInformation>();
    }
 
    // Get vehicle's current latitude from the BSM
    if(msg->core_data.presence_vector & carma_v2x_msgs::msg::BSMCoreData::LATITUDE_AVAILABLE){
      erv_information.current_latitude = msg->core_data.latitude;
    }
    else{
      RCLCPP_ERROR_STREAM(rclcpp::get_logger(logger_name), "No latitude available");
 
      // BSM is not a valid ERV BSM since current latitude is not included; return an empty object
      return boost::optional<ErvInformation>();
    }
 
    // Get vehicle's current longitude from the BSM
    if(msg->core_data.presence_vector & carma_v2x_msgs::msg::BSMCoreData::LONGITUDE_AVAILABLE){
 
      erv_information.current_longitude = msg->core_data.longitude;
    }
    else{
      // BSM is not a valid ERV BSM since current longitude is not included; return an empty object
      RCLCPP_ERROR_STREAM(rclcpp::get_logger(logger_name), "No longitude available");
 
      return boost::optional<ErvInformation>();
    }
 
    // Get vehicle's current position in the map frame from the BSM
    boost::optional<lanelet::BasicPoint2d> erv_position_in_map = getErvPositionInMap(erv_information.current_latitude, erv_information.current_longitude);
    
    if(erv_position_in_map){
      erv_information.current_position_in_map = *erv_position_in_map;
    }
 
    // Get vehicle's route destination points from the BSM
    std::vector<carma_v2x_msgs::msg::Position3D> erv_destination_points;
    if(msg->presence_vector & carma_v2x_msgs::msg::BSM::HAS_REGIONAL){
 
      // Parse BSM Regional Extensions to determine whether BSM includes the ERV's route destination points
      if(msg->regional.size() > 0){
        for(size_t i = 0; i < msg->regional.size(); ++i){
          if(msg->regional[i].regional_extension_id == carma_v2x_msgs::msg::BSMRegionalExtension::ROUTE_DESTINATIONS){
            erv_destination_points = msg->regional[i].route_destination_points;
            break;
          }
        }
      }
    }
    
    if(erv_destination_points.empty()){
      RCLCPP_ERROR_STREAM(rclcpp::get_logger(logger_name), "ERV's BSM does not contain any future destination points");
      
      // BSM is not a valid ERV BSM since it does not include destination points; return an empty object
      return boost::optional<ErvInformation>();
    }
 
    // Update the latest processing time of this ERV
    latest_erv_update_times_[erv_information.vehicle_id] = this->now();
 
    // Generate ERV's route based on its current position and its destination points
    lanelet::Optional<lanelet::routing::Route> erv_future_route = generateErvRoute(erv_information.current_latitude, erv_information.current_longitude, erv_destination_points);
 
    if(!erv_future_route){
      // ERV cannot be tracked since its route could not be generated; return an empty object
      RCLCPP_ERROR_STREAM(rclcpp::get_logger(logger_name), "The ERV's route could not be generated");
 
      return boost::optional<ErvInformation>();
    }
    
    // Determine the ERV's current lane index
    // Note: For 'lane index', 0 is rightmost lane, 1 is second rightmost, etc.; Only the current travel direction is considered
    if(!erv_future_route.get().shortestPath().empty()){
      lanelet::ConstLanelet erv_current_lanelet = erv_future_route.get().shortestPath()[0];
 
      // NOTE: this logic checks if the ERV and CMV are on a same direction or not. 
      // Currently this check is sufficient to happen only once due to the use case scenarios
      if (is_same_direction_.find(erv_information.vehicle_id) == is_same_direction_.end()) 
      {
        is_same_direction_[erv_information.vehicle_id] = false;
        for (auto llt: erv_future_route.get().shortestPath()) // checks if ERV is on the same path assuming CMV got all of its planned route when detected
        {
          if (wm_->getRoute()->contains(llt))
          {
            is_same_direction_[erv_information.vehicle_id] = true;
 
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Detected that ERV: " << erv_information.vehicle_id << " and CMV are travelling in the SAME direction");
            break;
          }
        }
      }
 
      if (!is_same_direction_[erv_information.vehicle_id])  // opposite direction
      {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Detected that ERV and CMV are travelling in DIFFERENT directions");
        return boost::optional<ErvInformation>(); // if opposite direction, do not track
      }
 
      // Get ERV's lane index
      int lane_index = wm_->getMapRoutingGraph()->rights(erv_current_lanelet).size();
 
      // A currently-tracked ERV must report the same lane index twice in a row before it is assigned the new lane index
      if(has_tracked_erv_){
        if(lane_index == tracked_erv_.previous_lane_index){
          erv_information.lane_index = lane_index;
        }
        else{
          erv_information.lane_index = tracked_erv_.previous_lane_index;
          RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Detected first new lane index of " << lane_index << ", ERV's lane index will remain " << tracked_erv_.previous_lane_index);
        }
 
        erv_information.previous_lane_index = lane_index;
      }
      else{
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "First time detecting this ERV, it's lane index will be " << lane_index);
        erv_information.lane_index = lane_index;
        erv_information.previous_lane_index = lane_index;
      }
 
     
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "ERV's lane index is " << erv_information.lane_index);
 
    }
    else{
      RCLCPP_ERROR_STREAM(rclcpp::get_logger(logger_name), "ERV's shortest path is empty!");
    }
 
    // Get intersecting lanelet between ERV's future route and ego vehicle's future shortest path
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Calling getRouteIntersectingLanelet"); 
    boost::optional<lanelet::ConstLanelet> intersecting_lanelet = getRouteIntersectingLanelet(erv_future_route.get());
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Done calling getRouteIntersectingLanelet"); 
 
    if(intersecting_lanelet){
      erv_information.intersecting_lanelet = *intersecting_lanelet;
    }
    else{
      // No intersecting lanelet between ERV and ego vehicle was found; return an empty object
      RCLCPP_ERROR_STREAM(rclcpp::get_logger(logger_name), "No intersecting lanelet between the ERV's future route and the ego vehicle's future route was found.");
 
      return boost::optional<ErvInformation>();
    }
 
    // Get the time (seconds) until the ERV passes the ego vehicle
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Calling getSecondsUntilPassing()"); 
    boost::optional<double> seconds_until_passing = getSecondsUntilPassing(erv_future_route, erv_information.current_position_in_map, erv_information.current_speed, erv_information.intersecting_lanelet);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Done calling getSecondsUntilPassing()"); 
 
    if(seconds_until_passing){
      erv_information.seconds_until_passing = seconds_until_passing.get();
      RCLCPP_INFO_STREAM(rclcpp::get_logger(logger_name), "Detected approaching ERV that is passing ego vehicle in " << seconds_until_passing.get() << " seconds");
    }
    else{
      RCLCPP_ERROR_STREAM(rclcpp::get_logger(logger_name), "Detected ERV is not approaching the ego vehicle");
 
      // ERV will not be tracked since it is not considered to be approaching the ego vehicle; return an empty object
      return boost::optional<ErvInformation>();
    }
 
    return erv_information;
  }

References approaching_emergency_vehicle_plugin::ErvInformation::current_latitude, approaching_emergency_vehicle_plugin::ErvInformation::current_longitude, approaching_emergency_vehicle_plugin::ErvInformation::current_position_in_map, approaching_emergency_vehicle_plugin::ErvInformation::current_speed, current_speed_, generateErvRoute(), getErvPositionInMap(), getRouteIntersectingLanelet(), getSecondsUntilPassing(), has_tracked_erv_, process_bag::i, approaching_emergency_vehicle_plugin::ErvInformation::intersecting_lanelet, is_same_direction_, approaching_emergency_vehicle_plugin::ErvInformation::lane_index, latest_erv_update_times_, logger_name, approaching_emergency_vehicle_plugin::ErvInformation::previous_lane_index, approaching_emergency_vehicle_plugin::ErvInformation::seconds_until_passing, tracked_erv_, approaching_emergency_vehicle_plugin::ErvInformation::vehicle_id, and wm_.

Referenced by incomingBsmCallback().

Here is the call graph for this function:

Here is the caller graph for this function:

getErvPositionInMap()

boost::optional< lanelet::BasicPoint2d > approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::getErvPositionInMap ( const double &  current_latitude, const double &  current_longitude  )

private

Helper function to obtain an ERV's position in the map frame from its current latitude and longitude.

Parameters

current_latitude	The current latitude of the ERV.
current_longitude	The current longitude of the ERV.

Returns

An optional lanelet::BasicPoint2d object, which is empty if the projection of the ERV's current position into the map frame was not successful.

Definition at line 371 of file approaching_emergency_vehicle_plugin_node.cpp.

                                                                                                                                                          {
    // Check if the map projection is available
    if (!map_projector_) {
      throw std::invalid_argument("Attempting to get ERV's current position in map before map projection was set");
    }
      
    // Build map projector
    lanelet::projection::LocalFrameProjector projector(map_projector_.get().c_str());
 
    // Create vector to hold ERV's projected current position
    std::vector<lanelet::BasicPoint3d> erv_current_position_projected_vec;
 
    // Add ERV's current location to the beginning of erv_current_position_projected_vec
    lanelet::GPSPoint current_erv_location;
    current_erv_location.lat = current_latitude;
    current_erv_location.lon = current_longitude;
 
    // Convert ERV's projected position to its position in the map frame
    erv_current_position_projected_vec.emplace_back(projector.forward(current_erv_location));
    auto erv_current_position_in_map_vec = lanelet::utils::transform(erv_current_position_projected_vec, [](auto a) { return lanelet::traits::to2D(a); });
 
    // Conduct size check since only the first element is being returned
    if(!erv_current_position_in_map_vec.empty()){
      return erv_current_position_in_map_vec[0];
    }
    else{
      return boost::optional<lanelet::BasicPoint2d>();
    }
  }

References map_projector_.

Referenced by getErvInformationFromBsm().

Here is the caller graph for this function:

getLaneletOnEgoRouteFromMapPosition()

boost::optional< lanelet::ConstLanelet > approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::getLaneletOnEgoRouteFromMapPosition ( const double &  x_position, const double &  y_position  )

private

Helper function to convert a map x,y coordinate pair to a lanelet on the ego vehicle's route.

Parameters

x_position	A map x-coordinate.
y_position	A map y-coordinate.

Returns

An optional lanelet::ConstLanelet object corresponding to the lanelet on the ego vehicle's route that the map x,y coordinates are positioned within. If empty, the map x,y coordinates are not positioned within a lanelet on the ego vehicle's route.

Definition at line 1484 of file approaching_emergency_vehicle_plugin_node.cpp.

  {
    // Get lanelet(s) that the provided map coordinates are contained within
    std::vector<lanelet::ConstLanelet> ego_route_lanelets = wm_->getLaneletsFromPoint({x_position, y_position});
    boost::optional<lanelet::ConstLanelet> ego_route_lanelet;
    
    if (ego_route_lanelets.empty())
    {
      RCLCPP_WARN_STREAM(rclcpp::get_logger(logger_name), "Given vehicle position is not on the road.");
      return boost::optional<lanelet::ConstLanelet>();
    }
 
    // Get the lanelet that is on the ego vehicle's route in case vehicle is located on overlapping lanelets
    for(const auto& lanelet : ego_route_lanelets)
    {
      if(wm_->getRoute()->contains(lanelet)){
        ego_route_lanelet = lanelet;
        break;
      }
    }
 
    return ego_route_lanelet;
  }

References logger_name, and wm_.

Referenced by plan_maneuvers_callback().

Here is the caller graph for this function:

getLaneletSpeedLimit()

double approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::getLaneletSpeedLimit ( const lanelet::ConstLanelet &  lanelet )

private

Helper function to extract the speed limit (m/s) from a provided lanelet.

Parameters

lanelet

A lanelet in the loaded vector map used by the CARMA System.

Returns

A double containing the speed limit (m/s) for the provided lanelet.

Definition at line 1015 of file approaching_emergency_vehicle_plugin_node.cpp.

  {
    double speed_limit = config_.default_speed_limit;
 
    lanelet::Optional<carma_wm::TrafficRulesConstPtr> traffic_rules = wm_->getTrafficRules();
    if (traffic_rules)
    {
      speed_limit =(*traffic_rules)->speedLimit(lanelet).speedLimit.value();
    }
    else
    {
      throw std::invalid_argument("No speed limit could be found since valid traffic rules object could not be built");
    }
    
    return speed_limit;
  }

References config_, approaching_emergency_vehicle_plugin::Config::default_speed_limit, and wm_.

Referenced by generateMoveOverManeuverPlan(), generateReducedSpeedLaneFollowingeManeuverPlan(), and plan_maneuvers_callback().

Here is the caller graph for this function:

getManeuverDuration()

rclcpp::Duration approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::getManeuverDuration ( const carma_planning_msgs::msg::Maneuver &  maneuver, double  epsilon  ) const

private

Helper function to obtain the (seconds) of a provided maneuver.

Parameters

maneuver	The maneuver from which that duration is desired.
epsilon	A double used for comparisons to zero.

Returns

The total duration (seconds) of the provided maneuver.

Definition at line 1032 of file approaching_emergency_vehicle_plugin_node.cpp.

  {
    double maneuver_start_speed = GET_MANEUVER_PROPERTY(maneuver, start_speed);
    double maneuver_end_speed = getManeuverEndSpeed(maneuver);
    double sum_start_and_end_speed = maneuver_start_speed + maneuver_end_speed;
 
    if(sum_start_and_end_speed < epsilon){
        throw std::invalid_argument("Maneuver start and ending speed is zero");
    }
 
    rclcpp::Duration maneuver_duration{0,0};
    double maneuver_start_dist = GET_MANEUVER_PROPERTY(maneuver, start_dist);
    double maneuver_end_dist = GET_MANEUVER_PROPERTY(maneuver, end_dist);
 
    RCLCPP_ERROR_STREAM(rclcpp::get_logger(logger_name),"maneuver_end_dist: " << maneuver_end_dist << ", maneuver_start_dist: " << maneuver_start_dist << ", sum_start_and_end_speed: " << sum_start_and_end_speed);
 
    maneuver_duration = rclcpp::Duration((maneuver_end_dist - maneuver_start_dist) / (0.5 * sum_start_and_end_speed) * 1e9);
 
    return maneuver_duration;
  }

References GET_MANEUVER_PROPERTY, approaching_emergency_vehicle_plugin::anonymous_namespace{approaching_emergency_vehicle_plugin_node.cpp}::getManeuverEndSpeed(), and logger_name.

Referenced by addStopAndWaitToEndOfPlan(), composeLaneChangeManeuverMessage(), composeLaneFollowingManeuverMessage(), composeStopAndWaitManeuverMessage(), generateMoveOverManeuverPlan(), and generateReducedSpeedLaneFollowingeManeuverPlan().

Here is the call graph for this function:

Here is the caller graph for this function:

getRouteIntersectingLanelet()

boost::optional< lanelet::ConstLanelet > approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::getRouteIntersectingLanelet ( const lanelet::routing::Route &  erv_future_route )

private

Helper function to obtain the earliest lanelet that exists on both an ERV's future route and the ego vehicle's future shortest path. Accesses the ego vehicle's future shortest path using wm_ object.

Parameters

erv_future_route

The ERV's future route

Returns

An optional lanelet::ConstLanelet object, which is empty if no intersecting lanelet was found.

Definition at line 944 of file approaching_emergency_vehicle_plugin_node.cpp.

                                                                                                                                              {
 
    if(future_route_lanelet_ids_.empty()){
      RCLCPP_WARN_STREAM(rclcpp::get_logger(logger_name), "Remaining route lanelets for the ego vehicle not found; plugin cannot compute the intersecting lanelet."); 
      return boost::optional<lanelet::ConstLanelet>();
    }
 
    // Get current downtrack from latest_route_state_
    double current_downtrack = latest_route_state_.down_track;
 
    // Find first successful intersecting lanelet between ERV route and ego route with a lanelet starting downtrack greater than current downtrack.
    // Additionally, remove lanelets from future_route_lanelet_ids_ that the ego vehicle has passed.
    for(auto it = future_route_lanelet_ids_.begin(); it != future_route_lanelet_ids_.end();){
      // Get lanelet
      lanelet::Id ego_route_lanelet_id = *it;
      lanelet::ConstLanelet ego_route_lanelet = wm_->getMap()->laneletLayer.get(ego_route_lanelet_id);
 
      // Get lanelet's centerline end point downtrack
      lanelet::BasicPoint2d ego_route_lanelet_centerline_end = lanelet::utils::to2D(ego_route_lanelet.centerline()).back();
      double ego_route_lanelet_centerline_end_downtrack = wm_->routeTrackPos(ego_route_lanelet_centerline_end).downtrack;
 
      if(current_downtrack > ego_route_lanelet_centerline_end_downtrack){
        // If current downtrack is greater than lanelet ending downtrack, remove lanelet from future route lanelet and continue
        // NOTE: Iterator is not updated since an element is being erased
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Removing passed lanelet " << ego_route_lanelet_id); 
        future_route_lanelet_ids_.erase(it);
      }
      else{
        // If lanelet exists in both, return it as the intersecting lanelet
        if(erv_future_route.contains(ego_route_lanelet)){
          RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Found intersecting lanelet " << ego_route_lanelet_id); 
          return boost::optional<lanelet::ConstLanelet>(ego_route_lanelet);
        }
        else{
          // Increase iterator
          ++it;
        }
      }
    }
  }

References future_route_lanelet_ids_, latest_route_state_, logger_name, and wm_.

Referenced by getErvInformationFromBsm().

Here is the caller graph for this function:

getSecondsUntilPassing()

boost::optional< double > approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::getSecondsUntilPassing ( lanelet::Optional< lanelet::routing::Route > &  erv_future_route, const lanelet::BasicPoint2d &  erv_position_in_map, const double &  erv_current_speed, lanelet::ConstLanelet &  intersecting_lanelet  )

private

Helper function to calculate the estimated seconds until an ERV will pass the ego vehicle. This is an estimate since it assumes both the ERV and the ego vehicle will continue travelling at their current speed.

Parameters

erv_future_route	The ERV's future route
erv_position_in_map	The ERV's current position in the map frame
erv_current_speed	The ERV's current speed (m/s)
intersecting_lanelet	The earliest lanelet that exists on both the ERV's future route and the ego vehicle's future shortest path.

Returns

An optional double, which includes the estimated seconds until the ERV will pass the ego vehicle. If empty, the ERV is behind the ego vehicle and travelling slower than the ego vehicle, or the ERV is in front of the ego vehicle while not actively passing the ego vehicle.

Definition at line 883 of file approaching_emergency_vehicle_plugin_node.cpp.

                                                                                                                                              {
 
    // Obtain ego vehicle and ERV distances to the end of the intersecting lanelet so neither vehicle will currently be past that point
    lanelet::ConstLineString2d intersecting_centerline = lanelet::utils::to2D(intersecting_lanelet.centerline());
    lanelet::BasicPoint2d intersecting_end_point = intersecting_centerline.back();
 
    // Get ego vehicle's (its rear bumper) distance to the intersecting lanelet's centerline endpoint
    double ego_dist_to_lanelet = wm_->routeTrackPos(intersecting_end_point).downtrack - (latest_route_state_.down_track - config_.vehicle_length);
 
    // Set erv_world_model_ route to the erv_future_route
    lanelet::routing::Route route = std::move(*erv_future_route);
    carma_wm::LaneletRoutePtr erv_future_route_ptr = std::make_shared<lanelet::routing::Route>(std::move(route));
    erv_world_model_->setRoute(erv_future_route_ptr);
 
    // Get ERV's distance to the intersecting lanelet's centerline endpoint
    double erv_dist_to_lanelet = erv_world_model_->routeTrackPos(intersecting_end_point).downtrack - erv_world_model_->routeTrackPos(erv_position_in_map).downtrack;
 
    if(erv_dist_to_lanelet < ego_dist_to_lanelet){
      // When ERV is in front of the ego vehicle, only process further if the ERV is actively passing the ego vehicle
 
      if(transition_table_.getState() != ApproachingEmergencyVehicleState::SLOWING_DOWN_FOR_ERV){
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Detected ERV is in front of the ego vehicle");
        return boost::optional<double>();
      }
      else{
        // This ERV is actively passing the ego vehicle until its distance in front of the ego vehicle is at least config_.finished_passing_threshold
        if((ego_dist_to_lanelet - erv_dist_to_lanelet) < config_.finished_passing_threshold){
          // Return zero to indicate that the ERV is currently passing the ego vehicle
          return 0.0;
        }
        else{
          RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "ERV has passed the ego vehicle");
          has_tracked_erv_ = false;
          has_planned_upcoming_lc_ = false;
          transition_table_.event(ApproachingEmergencyVehicleEvent::ERV_PASSED);
          return boost::optional<double>();
        }
      }
    }
    else{
      // When ERV is behind the ego vehicle, only process further if the ERV is travelling faster than the ego vehicle
 
      if(erv_current_speed < current_speed_){
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Detected ERV is travelling slower than the ego vehicle, and will not pass the ego vehicle");
        return boost::optional<double>();
      }
      else{
        // Calculate seconds_until_passing and protect against division by zero
        double delta_speed = erv_current_speed - current_speed_;
 
        if(delta_speed == 0.0){
          delta_speed = epsilon_;
        }
        double seconds_until_passing = (erv_dist_to_lanelet - ego_dist_to_lanelet) / delta_speed;
 
        return seconds_until_passing;
      }
    }
  }

References config_, current_speed_, epsilon_, approaching_emergency_vehicle_plugin::ERV_PASSED, erv_world_model_, approaching_emergency_vehicle_plugin::ApproachingEmergencyVehicleTransitionTable::event(), approaching_emergency_vehicle_plugin::Config::finished_passing_threshold, approaching_emergency_vehicle_plugin::ApproachingEmergencyVehicleTransitionTable::getState(), has_planned_upcoming_lc_, has_tracked_erv_, latest_route_state_, logger_name, approaching_emergency_vehicle_plugin::SLOWING_DOWN_FOR_ERV, transition_table_, approaching_emergency_vehicle_plugin::Config::vehicle_length, and wm_.

Referenced by getErvInformationFromBsm().

Here is the call graph for this function:

Here is the caller graph for this function:

guidanceStateCallback()

void approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::guidanceStateCallback

(

const carma_planning_msgs::msg::GuidanceState::UniquePtr

msg

)

Subscription callback to process the latest guidance state and update the is_guidance_engaged_ flag accordingly.

Definition at line 994 of file approaching_emergency_vehicle_plugin_node.cpp.

                                                                                                               {
    if(msg->state == carma_planning_msgs::msg::GuidanceState::ENGAGED){
      is_guidance_engaged_ = true;
    }
    else{
      is_guidance_engaged_ = false;
    }
  }

References lightbar_manager::ENGAGED, and is_guidance_engaged_.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

incomingBsmCallback()

void approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::incomingBsmCallback

(

carma_v2x_msgs::msg::BSM::UniquePtr

msg

)

Incoming BSM subscription callback, which determines whether a BSM should be processed, and extracts ERV information from the BSM through a call to getErvInformationFromBsm(). This function is responsible for updating this plugin's tracked_erv_ object and setting has_tracked_erv_ to true when a new ERV becomes tracked.

Parameters

msg	The incoming BSM message.

Definition at line 809 of file approaching_emergency_vehicle_plugin_node.cpp.

  {
    // Only process incoming BSMs if guidance is currently engaged
    if(!is_guidance_engaged_){
      return;
    }
 
    // Get the vehicle ID associated with the received BSM
    std::stringstream ss;
    for(size_t i = 0; i < msg->core_data.id.size(); ++i){
      ss << std::setfill('0') << std::setw(2) << std::hex << (unsigned)msg->core_data.id.at(i);
    }
    std::string erv_vehicle_id = ss.str();
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Received a BSM from " << erv_vehicle_id);
 
    if(has_tracked_erv_){
      // If there is already an ERV approaching the ego vehicle, only process this BSM further if it is from that ERV and enough time has passed since the previously processed BSM
 
      if(erv_vehicle_id == tracked_erv_.vehicle_id){
        double seconds_since_prev_processed_bsm = (this->now() - tracked_erv_.latest_update_time).seconds();
 
        if(seconds_since_prev_processed_bsm < (1.0 / config_.bsm_processing_frequency)){
          // Do not process ERV's BSM further since not enough time has passed since its previously processed BSM
          RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Ignoring BSM from tracked ERV " << erv_vehicle_id << " since a BSM from it was processed " << seconds_since_prev_processed_bsm << " seconds ago");
          return;
        }
      }
      else{
        // Do not process BSM further since it is not for the currently tracked ERV
        return;
      }
    }
    else{
 
      // If BSM is from a detected active ERV, only process it further if enough time has passed since the previously processed BSM from this ERV
      if (latest_erv_update_times_.find(erv_vehicle_id) != latest_erv_update_times_.end()){
        double seconds_since_prev_processed_bsm = (this->now() - latest_erv_update_times_[erv_vehicle_id]).seconds();
 
        if(seconds_since_prev_processed_bsm < (1.0 / config_.bsm_processing_frequency)){
          // Do not process ERV's BSM further since not enough time has passed since its previously processed BSM
          RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Ignoring BSM from non tracked ERV " << erv_vehicle_id << " since a BSM from it was processed " << seconds_since_prev_processed_bsm << " seconds ago");
          return;
        }
      }
    }
 
    // Get ErvInformation object with information from the BSM if it is from an active ERV that is approaching the ego vehicle
    boost::optional<ErvInformation> erv_information = getErvInformationFromBsm(std::move(msg));
    if(!erv_information){
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "BSM is not from an active ERV that is approaching the ego vehicle.");
 
        if(has_tracked_erv_){
          RCLCPP_INFO_STREAM(rclcpp::get_logger(logger_name), "BSM was from the currently tracked approaching ERV! ERV is no longer approaching.");
          has_tracked_erv_ = false;
          has_planned_upcoming_lc_ = false;
          transition_table_.event(ApproachingEmergencyVehicleEvent::NO_APPROACHING_ERV);
        }
 
      // BSM is not from an active ERV that is approaching the ego vehicle
      return;
    }
    else{
      // Update tracked ERV information since this is an active ERV that is approaching the ego vehicle
      tracked_erv_ = *erv_information;
      tracked_erv_.latest_update_time = this->now();
 
      if(!has_tracked_erv_){
        has_tracked_erv_ = true;
      }
    }
 
    return;
  }

References approaching_emergency_vehicle_plugin::Config::bsm_processing_frequency, config_, approaching_emergency_vehicle_plugin::ApproachingEmergencyVehicleTransitionTable::event(), getErvInformationFromBsm(), has_planned_upcoming_lc_, has_tracked_erv_, process_bag::i, is_guidance_engaged_, latest_erv_update_times_, approaching_emergency_vehicle_plugin::ErvInformation::latest_update_time, logger_name, approaching_emergency_vehicle_plugin::NO_APPROACHING_ERV, tracked_erv_, transition_table_, and approaching_emergency_vehicle_plugin::ErvInformation::vehicle_id.

Referenced by on_configure_plugin().

Here is the call graph for this function:

Here is the caller graph for this function:

incomingEmergencyVehicleAckCallback()

void approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::incomingEmergencyVehicleAckCallback

(

const carma_v2x_msgs::msg::EmergencyVehicleAck::UniquePtr

msg

)

Subscription callback for incoming EmergencyVehicleAck messages. If the message is from the currently tracked ERV and is intended for the ego vehicle, then the plugin will reset num_warnings_broadcasted_ to zero and will reset the should_broadcast_warnings_ flag to false.

Definition at line 620 of file approaching_emergency_vehicle_plugin_node.cpp.

  {
    // Only process message if it is from the currently-tracked ERV and it is intended for the ego vehicle
    if(has_tracked_erv_ && (msg->m_header.sender_id == tracked_erv_.vehicle_id) && (msg->m_header.recipient_id == config_.vehicle_id)){
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "EmergencyVehicleAck received from ERV " << tracked_erv_.vehicle_id);
 
      // Reset counter and boolean flag to stop warning messages from being broadcasted
      num_warnings_broadcasted_ = 0;
      should_broadcast_warnings_ = false;
    }
  }

References config_, has_tracked_erv_, logger_name, num_warnings_broadcasted_, should_broadcast_warnings_, tracked_erv_, approaching_emergency_vehicle_plugin::Config::vehicle_id, and approaching_emergency_vehicle_plugin::ErvInformation::vehicle_id.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

on_activate_plugin()

carma_ros2_utils::CallbackReturn approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::on_activate_plugin ( )

virtual

This method is used to create a timer and will be called on the activate transition.

Reimplemented from carma_guidance_plugins::PluginBaseNode.

Definition at line 189 of file approaching_emergency_vehicle_plugin_node.cpp.

  {
    // Timer setup for generating a BSM
    int erv_timeout_check_period_ms = (1 / config_.timeout_check_frequency) * 1000; // Conversion from frequency (Hz) to milliseconds time period
    erv_timeout_timer_ = create_timer(get_clock(),
                          std::chrono::milliseconds(erv_timeout_check_period_ms),
                          std::bind(&ApproachingEmergencyVehiclePlugin::checkForErvTimeout, this));
 
    // Timer setup for broadcasting EmergencyVehicleResponse warning message to an approaching ERV when the ego vehicle cannot change lanes out of the ERV's path
    int emergency_vehicle_response_period_ms = (1 / config_.warning_broadcast_frequency) * 1000; // Conversion from frequency (Hz) to milliseconds time period
    warning_broadcast_timer_ = create_timer(get_clock(),
                          std::chrono::milliseconds(emergency_vehicle_response_period_ms),
                          std::bind(&ApproachingEmergencyVehiclePlugin::broadcastWarningToErv, this));
 
    // Timer setup for publishing approaching ERV status update to the Web UI
    int approaching_erv_status_period_ms = (1 / config_.approaching_erv_status_publication_frequency) * 1000; // Conversion from frequency (Hz) to milliseconds time period
    approaching_emergency_vehicle_status_timer_ = create_timer(get_clock(),
                          std::chrono::milliseconds(approaching_erv_status_period_ms),
                          std::bind(&ApproachingEmergencyVehiclePlugin::publishApproachingErvStatus, this));
 
    // Timer setup for publishing hazard light ON/OFF status boolean
    int hazard_light_status_ms = (1 / 30) * 1000; // Conversion from frequency 30(Hz) to milliseconds time period
    hazard_light_timer_ = create_timer(get_clock(),
                          std::chrono::milliseconds(hazard_light_status_ms),
                          std::bind(&ApproachingEmergencyVehiclePlugin::publishHazardLightStatus, this));
 
    return CallbackReturn::SUCCESS;
  }

References approaching_emergency_vehicle_status_timer_, approaching_emergency_vehicle_plugin::Config::approaching_erv_status_publication_frequency, broadcastWarningToErv(), checkForErvTimeout(), config_, erv_timeout_timer_, hazard_light_timer_, publishApproachingErvStatus(), publishHazardLightStatus(), approaching_emergency_vehicle_plugin::Config::timeout_check_frequency, approaching_emergency_vehicle_plugin::Config::warning_broadcast_frequency, and warning_broadcast_timer_.

Here is the call graph for this function:

on_configure_plugin()

carma_ros2_utils::CallbackReturn approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::on_configure_plugin ( )

virtual

This method is used to load parameters and will be called on the configure state transition.

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 117 of file approaching_emergency_vehicle_plugin_node.cpp.

  {
    RCLCPP_INFO_STREAM(rclcpp::get_logger(logger_name), "ApproachingEmergencyVehiclePlugin trying to configure");
 
    // Reset config
    config_ = Config();
 
    // Load parameters
    get_parameter<double>("passing_threshold", config_.passing_threshold);
    get_parameter<double>("approaching_threshold", config_.approaching_threshold);
    get_parameter<double>("finished_passing_threshold", config_.finished_passing_threshold);
    get_parameter<double>("min_lane_following_duration_before_lane_change", config_.min_lane_following_duration_before_lane_change);
    get_parameter<double>("bsm_processing_frequency", config_.bsm_processing_frequency);
    get_parameter<double>("speed_limit_reduction_during_passing", config_.speed_limit_reduction_during_passing);
    get_parameter<double>("minimum_reduced_speed_limit", config_.minimum_reduced_speed_limit);
    get_parameter<double>("default_speed_limit", config_.default_speed_limit);
    get_parameter<double>("reduced_speed_buffer", config_.reduced_speed_buffer);
    get_parameter<double>("timeout_check_frequency", config_.timeout_check_frequency);
    get_parameter<double>("timeout_duration", config_.timeout_duration);
    get_parameter<double>("minimal_plan_duration", config_.minimal_plan_duration);
    get_parameter<double>("buffer_distance_before_stopping", config_.buffer_distance_before_stopping);
    get_parameter<double>("stopping_accel_limit_multiplier", config_.stopping_accel_limit_multiplier);
    get_parameter<double>("vehicle_acceleration_limit", config_.vehicle_acceleration_limit);
    get_parameter<double>("route_end_point_buffer", config_.route_end_point_buffer);
    get_parameter<double>("approaching_erv_status_publication_frequency", config_.approaching_erv_status_publication_frequency);
    get_parameter<double>("warning_broadcast_frequency", config_.warning_broadcast_frequency);
    get_parameter<double>("vehicle_length", config_.vehicle_length);
    get_parameter<int>("max_warning_broadcasts", config_.max_warning_broadcasts);
    get_parameter<std::string>("lane_following_plugin", config_.lane_following_plugin);
    get_parameter<std::string>("lane_change_plugin", config_.lane_change_plugin);
    get_parameter<std::string>("vehicle_id", config_.vehicle_id);
 
    RCLCPP_INFO_STREAM(rclcpp::get_logger(logger_name), "ApproachingEmergencyVehiclePlugin Config: " << config_);
 
    // Register runtime parameter update callback
    add_on_set_parameters_callback(std::bind(&ApproachingEmergencyVehiclePlugin::parameter_update_callback, this, std_ph::_1));
 
    // Setup subscribers
    incoming_bsm_sub_ = create_subscription<carma_v2x_msgs::msg::BSM>("incoming_bsm", 1,
                                            std::bind(&ApproachingEmergencyVehiclePlugin::incomingBsmCallback, this, std_ph::_1));
 
    georeference_sub_ = create_subscription<std_msgs::msg::String>("georeference", 10,
                                            std::bind(&ApproachingEmergencyVehiclePlugin::georeferenceCallback, this, std_ph::_1));
 
    route_state_sub_ = create_subscription<carma_planning_msgs::msg::RouteState>("route_state", 10,
                                          std::bind(&ApproachingEmergencyVehiclePlugin::routeStateCallback, this, std_ph::_1));
 
    twist_sub_ = create_subscription<geometry_msgs::msg::TwistStamped>("current_velocity", 10,
                                          std::bind(&ApproachingEmergencyVehiclePlugin::twistCallback, this, std_ph::_1));
 
    incoming_emergency_vehicle_ack_sub_ = create_subscription<carma_v2x_msgs::msg::EmergencyVehicleAck>("incoming_emergency_vehicle_ack", 10,
                                                              std::bind(&ApproachingEmergencyVehiclePlugin::incomingEmergencyVehicleAckCallback, this, std_ph::_1));
 
    guidance_state_sub_ = create_subscription<carma_planning_msgs::msg::GuidanceState>("state", 1,
                                          std::bind(&ApproachingEmergencyVehiclePlugin::guidanceStateCallback, this, std_ph::_1));
 
    route_sub_ = create_subscription<carma_planning_msgs::msg::Route>("route", 1,
                                          std::bind(&ApproachingEmergencyVehiclePlugin::routeCallback, this, std_ph::_1));
 
    // Setup publishers
    outgoing_emergency_vehicle_response_pub_ = create_publisher<carma_v2x_msgs::msg::EmergencyVehicleResponse>("outgoing_emergency_vehicle_response", 10);
 
    approaching_erv_status_pub_ = create_publisher<carma_msgs::msg::UIInstructions>("approaching_erv_status", 10);
 
    hazard_light_cmd_pub_ = create_publisher<std_msgs::msg::Bool>("hazard_light_status", 10);
 
    wm_ = get_world_model();
 
    // Return success if everything initialized successfully
    return CallbackReturn::SUCCESS;
  }

References approaching_erv_status_pub_, approaching_emergency_vehicle_plugin::Config::approaching_erv_status_publication_frequency, approaching_emergency_vehicle_plugin::Config::approaching_threshold, approaching_emergency_vehicle_plugin::Config::bsm_processing_frequency, approaching_emergency_vehicle_plugin::Config::buffer_distance_before_stopping, config_, approaching_emergency_vehicle_plugin::Config::default_speed_limit, approaching_emergency_vehicle_plugin::Config::finished_passing_threshold, georeference_sub_, georeferenceCallback(), carma_guidance_plugins::PluginBaseNode::get_world_model(), guidance_state_sub_, guidanceStateCallback(), hazard_light_cmd_pub_, incoming_bsm_sub_, incoming_emergency_vehicle_ack_sub_, incomingBsmCallback(), incomingEmergencyVehicleAckCallback(), approaching_emergency_vehicle_plugin::Config::lane_change_plugin, approaching_emergency_vehicle_plugin::Config::lane_following_plugin, logger_name, approaching_emergency_vehicle_plugin::Config::max_warning_broadcasts, approaching_emergency_vehicle_plugin::Config::min_lane_following_duration_before_lane_change, approaching_emergency_vehicle_plugin::Config::minimal_plan_duration, approaching_emergency_vehicle_plugin::Config::minimum_reduced_speed_limit, outgoing_emergency_vehicle_response_pub_, parameter_update_callback(), approaching_emergency_vehicle_plugin::Config::passing_threshold, approaching_emergency_vehicle_plugin::Config::reduced_speed_buffer, approaching_emergency_vehicle_plugin::Config::route_end_point_buffer, route_state_sub_, route_sub_, routeCallback(), routeStateCallback(), approaching_emergency_vehicle_plugin::Config::speed_limit_reduction_during_passing, approaching_emergency_vehicle_plugin::Config::stopping_accel_limit_multiplier, approaching_emergency_vehicle_plugin::Config::timeout_check_frequency, approaching_emergency_vehicle_plugin::Config::timeout_duration, twist_sub_, twistCallback(), approaching_emergency_vehicle_plugin::Config::vehicle_acceleration_limit, approaching_emergency_vehicle_plugin::Config::vehicle_id, approaching_emergency_vehicle_plugin::Config::vehicle_length, approaching_emergency_vehicle_plugin::Config::warning_broadcast_frequency, and wm_.

Here is the call graph for this function:

parameter_update_callback()

rcl_interfaces::msg::SetParametersResult approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::parameter_update_callback

(

const std::vector< rclcpp::Parameter > &

parameters

)

Callback for dynamic parameter updates.

Definition at line 75 of file approaching_emergency_vehicle_plugin_node.cpp.

  {
    auto error_1 = update_params<double>({
        {"passing_threshold", config_.passing_threshold},
        {"approaching_threshold", config_.approaching_threshold},
        {"finished_passing_threshold", config_.finished_passing_threshold},
        {"min_lane_following_duration_before_lane_change", config_.min_lane_following_duration_before_lane_change},
        {"bsm_processing_frequency", config_.bsm_processing_frequency},
        {"speed_limit_reduction_during_passing", config_.speed_limit_reduction_during_passing},
        {"minimum_reduced_speed_limit", config_.minimum_reduced_speed_limit},
        {"default_speed_limit", config_.default_speed_limit},
        {"reduced_speed_buffer", config_.reduced_speed_buffer},
        {"timeout_check_frequency", config_.timeout_check_frequency},
        {"timeout_duration", config_.timeout_duration},
        {"minimal_plan_duration", config_.minimal_plan_duration},
        {"buffer_distance_before_stopping", config_.buffer_distance_before_stopping},
        {"stopping_accel_limit_multiplier", config_.stopping_accel_limit_multiplier},
        {"vehicle_acceleration_limit", config_.vehicle_acceleration_limit},
        {"route_end_point_buffer", config_.route_end_point_buffer},
        {"approaching_erv_status_publication_frequency", config_.approaching_erv_status_publication_frequency},
        {"warning_broadcast_frequency", config_.warning_broadcast_frequency},
        {"vehicle_length", config_.vehicle_length}
    }, parameters);
 
    auto error_2 = update_params<int>({
        {"max_warning_broadcasts", config_.max_warning_broadcasts}
    }, parameters);
 
    auto error_3 = update_params<std::string>({
        {"lane_following_plugin", config_.lane_following_plugin},
        {"lane_change_plugin", config_.lane_change_plugin},
        {"stop_and_wait_plugin", config_.stop_and_wait_plugin},
        {"vehicle_id", config_.vehicle_id}
    }, parameters);
 
    rcl_interfaces::msg::SetParametersResult result;
 
    result.successful = !error_1 && !error_2 && !error_3;
 
    return result;
  }

References approaching_emergency_vehicle_plugin::Config::approaching_erv_status_publication_frequency, approaching_emergency_vehicle_plugin::Config::approaching_threshold, approaching_emergency_vehicle_plugin::Config::bsm_processing_frequency, approaching_emergency_vehicle_plugin::Config::buffer_distance_before_stopping, config_, approaching_emergency_vehicle_plugin::Config::default_speed_limit, approaching_emergency_vehicle_plugin::Config::finished_passing_threshold, approaching_emergency_vehicle_plugin::Config::lane_change_plugin, approaching_emergency_vehicle_plugin::Config::lane_following_plugin, approaching_emergency_vehicle_plugin::Config::max_warning_broadcasts, approaching_emergency_vehicle_plugin::Config::min_lane_following_duration_before_lane_change, approaching_emergency_vehicle_plugin::Config::minimal_plan_duration, approaching_emergency_vehicle_plugin::Config::minimum_reduced_speed_limit, approaching_emergency_vehicle_plugin::Config::passing_threshold, approaching_emergency_vehicle_plugin::Config::reduced_speed_buffer, approaching_emergency_vehicle_plugin::Config::route_end_point_buffer, approaching_emergency_vehicle_plugin::Config::speed_limit_reduction_during_passing, approaching_emergency_vehicle_plugin::Config::stop_and_wait_plugin, approaching_emergency_vehicle_plugin::Config::stopping_accel_limit_multiplier, approaching_emergency_vehicle_plugin::Config::timeout_check_frequency, approaching_emergency_vehicle_plugin::Config::timeout_duration, approaching_emergency_vehicle_plugin::Config::vehicle_acceleration_limit, approaching_emergency_vehicle_plugin::Config::vehicle_id, approaching_emergency_vehicle_plugin::Config::vehicle_length, and approaching_emergency_vehicle_plugin::Config::warning_broadcast_frequency.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

plan_maneuvers_callback()

void approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::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  )

overridevirtual

Extending class provided callback which should return a planned trajectory based on the provided trajectory planning request.

Parameters

srv_header	RCL header for services calls. Can usually be ignored by implementers.
req	The service request containing the previously planned maneuvers and vehicle state
resp	The response containing the additional maneuvers generated by this plugin

Implements carma_guidance_plugins::StrategicPlugin.

Definition at line 1508 of file approaching_emergency_vehicle_plugin_node.cpp.

  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Received request to plan maneuvers");
 
    // Check whether ego vehicle is currently in the middle of a lane change, or whether its planned lane change has been completed
    bool is_currently_lane_changing = false;
    if(has_planned_upcoming_lc_){
      if((upcoming_lc_params_.start_dist < req->veh_downtrack) && (req->veh_downtrack < upcoming_lc_params_.end_dist)){
        is_currently_lane_changing = true;
      }
      else if(req->veh_downtrack > upcoming_lc_params_.end_dist){
        // Update flag since the planned lane change has been completed
        has_planned_upcoming_lc_ = false;
      }
    }
 
    boost::optional<lanelet::ConstLanelet> ego_current_lanelet_optional = getLaneletOnEgoRouteFromMapPosition(req->veh_x, req->veh_y);
 
    if(ego_current_lanelet_optional){
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "starting lanelet for maneuver plan is " << ego_current_lanelet_optional.get().id());
    }
    else{
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Given vehicle position is not within a lanelet on the route. Returning empty maneuver plan");
      return;
    }
 
    // Get ego vehicle's current lane index
    // Note: For 'lane index', 0 is rightmost lane, 1 is second rightmost, etc.; Only the current travel direction is considered
    ego_lane_index_ = wm_->getMapRoutingGraph()->rights(ego_current_lanelet_optional.get()).size();
 
    // Update state machine if there is currently an ERV being tracked and if ego vehicle is not currently in the middle of a lane change
    if(has_tracked_erv_ && !is_currently_lane_changing){
      if(ego_lane_index_ == tracked_erv_.lane_index){
        // Trigger state machine transition for case in which the ego vehicle is in the ERV's path
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Ego vehicle and ERV are both in lane index " << ego_lane_index_);
 
        if(tracked_erv_.seconds_until_passing >= config_.approaching_threshold){
          transition_table_.event(ApproachingEmergencyVehicleEvent::NO_APPROACHING_ERV);
          has_planned_upcoming_lc_ = false;
        }
        else if((config_.passing_threshold <= tracked_erv_.seconds_until_passing) && (tracked_erv_.seconds_until_passing < config_.approaching_threshold)){
          transition_table_.event(ApproachingEmergencyVehicleEvent::APPROACHING_ERV_IN_PATH);
        }
        else if((0.0 <= tracked_erv_.seconds_until_passing) && (tracked_erv_.seconds_until_passing < config_.passing_threshold)){
          transition_table_.event(ApproachingEmergencyVehicleEvent::ERV_PASSING_IN_PATH);
          
          if(tracked_erv_.seconds_until_passing >= MAINTAIN_SPEED_THRESHOLD){
            is_maintaining_non_reduced_speed_ = false;
          }
          else{
            // ERV is close enough (in the same lane) that if the ego vehicle slows down it could cause a safety hazard; set internal members for ego vehicle to maintain its current speed
            if(!is_maintaining_non_reduced_speed_){
              is_maintaining_non_reduced_speed_ = true;
              non_reduced_speed_to_maintain_ = req->veh_logitudinal_velocity;
            }
          }
 
          // Set flag to broadcast EmergencyVehicleResponse warning messages to ERV if they have not already been broadcasted
          if(!has_broadcasted_warning_messages_){
            should_broadcast_warnings_ = true;
            has_broadcasted_warning_messages_ = true;
          }
        }
        else{
          RCLCPP_WARN_STREAM(rclcpp::get_logger(logger_name), "Unsupported seconds_until_passing of " << tracked_erv_.seconds_until_passing << ", returning empty maneuver plan");
          resp->new_plan.maneuvers = {};
          return;
        }
      }
      else{
        // Trigger state machine transition for case in which the ego vehicle is not in the ERV's path
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Ego vehicle is not in ERV's path");
 
        if((tracked_erv_.seconds_until_passing >= config_.approaching_threshold)){
          transition_table_.event(ApproachingEmergencyVehicleEvent::NO_APPROACHING_ERV);
          has_planned_upcoming_lc_ = false;
        }
        else if((0.0 <= tracked_erv_.seconds_until_passing) && (tracked_erv_.seconds_until_passing <= config_.approaching_threshold)){
          transition_table_.event(ApproachingEmergencyVehicleEvent::APPROACHING_ERV_NOT_IN_PATH);
        }
        else{
          RCLCPP_WARN_STREAM(rclcpp::get_logger(logger_name), "Unsupported seconds_until_passing of " << tracked_erv_.seconds_until_passing << ", returning empty maneuver plan");
          resp->new_plan.maneuvers = {};
          return;
        }
      }
    }
 
    // Set initial maneuver plan parameters based on the ego vehicle's initial state
    double downtrack_progress = req->veh_downtrack;
    double current_lanelet_ending_downtrack = wm_->routeTrackPos(ego_current_lanelet_optional.get().centerline2d().back()).downtrack;
    double speed_progress = req->veh_logitudinal_velocity;
    double target_speed = getLaneletSpeedLimit(ego_current_lanelet_optional.get()); 
    rclcpp::Time time_progress = rclcpp::Time(req->header.stamp);
 
    // Generate maneuver plan based on the current state
    switch (transition_table_.getState())
    {
      case ApproachingEmergencyVehicleState::NO_APPROACHING_ERV:
        resp->new_plan.maneuvers = {};
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "No approaching ERV. Returning empty maneuver plan");
        is_maintaining_non_reduced_speed_ = false; // Reset flag since ego vehicle does not need to maintain a non-reduced speed
        break;
 
      case ApproachingEmergencyVehicleState::MOVING_OVER_FOR_APPROACHING_ERV:
        is_maintaining_non_reduced_speed_ = false; // Reset flag since ego vehicle does not need to maintain a non-reduced speed
 
        generateMoveOverManeuverPlan(resp, ego_current_lanelet_optional.get(), downtrack_progress, current_lanelet_ending_downtrack, speed_progress,
                                        target_speed, time_progress, ego_lane_index_, tracked_erv_.lane_index);
        break;
 
      case ApproachingEmergencyVehicleState::SLOWING_DOWN_FOR_ERV:
        // Generate a maneuver plan consisting of only lane-following maneuvers at a reduced speed since the ERV is actively passing the ego vehicle
        generateReducedSpeedLaneFollowingeManeuverPlan(resp, ego_current_lanelet_optional.get(), downtrack_progress, current_lanelet_ending_downtrack, speed_progress,
                                        target_speed, time_progress, is_maintaining_non_reduced_speed_);
        break;
 
      default:
        throw std::invalid_argument("Transition table in unsupported state");
    }
 
    latest_maneuver_plan_ = resp->new_plan;
  }

References approaching_emergency_vehicle_plugin::APPROACHING_ERV_IN_PATH, approaching_emergency_vehicle_plugin::APPROACHING_ERV_NOT_IN_PATH, approaching_emergency_vehicle_plugin::Config::approaching_threshold, config_, ego_lane_index_, approaching_emergency_vehicle_plugin::UpcomingLaneChangeParameters::end_dist, approaching_emergency_vehicle_plugin::ERV_PASSING_IN_PATH, approaching_emergency_vehicle_plugin::ApproachingEmergencyVehicleTransitionTable::event(), generateMoveOverManeuverPlan(), generateReducedSpeedLaneFollowingeManeuverPlan(), getLaneletOnEgoRouteFromMapPosition(), getLaneletSpeedLimit(), approaching_emergency_vehicle_plugin::ApproachingEmergencyVehicleTransitionTable::getState(), has_broadcasted_warning_messages_, has_planned_upcoming_lc_, has_tracked_erv_, is_maintaining_non_reduced_speed_, approaching_emergency_vehicle_plugin::ErvInformation::lane_index, latest_maneuver_plan_, logger_name, MAINTAIN_SPEED_THRESHOLD, approaching_emergency_vehicle_plugin::MOVING_OVER_FOR_APPROACHING_ERV, approaching_emergency_vehicle_plugin::NO_APPROACHING_ERV, non_reduced_speed_to_maintain_, approaching_emergency_vehicle_plugin::Config::passing_threshold, approaching_emergency_vehicle_plugin::ErvInformation::seconds_until_passing, should_broadcast_warnings_, approaching_emergency_vehicle_plugin::SLOWING_DOWN_FOR_ERV, approaching_emergency_vehicle_plugin::UpcomingLaneChangeParameters::start_dist, tracked_erv_, transition_table_, upcoming_lc_params_, and wm_.

Here is the call graph for this function:

publishApproachingErvStatus()

void approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::publishApproachingErvStatus ( )

private

This is a callback function for the approaching_emergency_vehicle_status_timer_. It makes a call to generateApproachingErvStatusMessage() and publishes the generated message to approaching_erv_status_pub_.

Definition at line 274 of file approaching_emergency_vehicle_plugin_node.cpp.

                                                                     {
    // Generate the status message and publish it to the applicable ROS topic
    carma_msgs::msg::UIInstructions status_msg = generateApproachingErvStatusMessage();
    approaching_erv_status_pub_->publish(status_msg);
  }

References approaching_erv_status_pub_, and generateApproachingErvStatusMessage().

Referenced by on_activate_plugin().

Here is the call graph for this function:

Here is the caller graph for this function:

publishHazardLightStatus()

void approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::publishHazardLightStatus ( )

private

This is a callback function for publishing turn ON/OFF (true/false) hazard light command to the ssc driver.

Definition at line 218 of file approaching_emergency_vehicle_plugin_node.cpp.

  {
    if (transition_table_.getState() == ApproachingEmergencyVehicleState::SLOWING_DOWN_FOR_ERV &&
    has_tracked_erv_ &&
    tracked_erv_.lane_index == ego_lane_index_)
    {
      hazard_light_cmd_ = true;
    }
    else
    {
      hazard_light_cmd_ = false;
    }
    std_msgs::msg::Bool msg;
    msg.data = hazard_light_cmd_;
    hazard_light_cmd_pub_->publish(msg);
  }

References ego_lane_index_, approaching_emergency_vehicle_plugin::ApproachingEmergencyVehicleTransitionTable::getState(), has_tracked_erv_, hazard_light_cmd_, hazard_light_cmd_pub_, approaching_emergency_vehicle_plugin::ErvInformation::lane_index, approaching_emergency_vehicle_plugin::SLOWING_DOWN_FOR_ERV, tracked_erv_, and transition_table_.

Referenced by on_activate_plugin().

Here is the call graph for this function:

Here is the caller graph for this function:

routeCallback()

void approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::routeCallback

(

carma_planning_msgs::msg::Route::UniquePtr

msg

)

Route subscription callback, with is used to update this plugin's future_route_lanelet_ids_ object.

Parameters

msg	The latest Route message.

Definition at line 1003 of file approaching_emergency_vehicle_plugin_node.cpp.

                                                                                               {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Received route callback"); 
    std::vector<int> new_future_route_lanelet_ids;
 
    for(size_t i = 0; i < msg->route_path_lanelet_ids.size(); ++i){
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "New route lanelet: " << msg->route_path_lanelet_ids[i]); 
      new_future_route_lanelet_ids.push_back(msg->route_path_lanelet_ids[i]);
    }
    
    future_route_lanelet_ids_ = new_future_route_lanelet_ids;
  }

References future_route_lanelet_ids_, process_bag::i, and logger_name.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

routeStateCallback()

void approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::routeStateCallback

(

carma_planning_msgs::msg::RouteState::UniquePtr

msg

)

Route State subscription callback, with is used to update this plugin's latest_route_state_ object.

Parameters

msg	The latest Route State message.

Definition at line 985 of file approaching_emergency_vehicle_plugin_node.cpp.

  {
    latest_route_state_ = *msg;
  }

References latest_route_state_.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

twistCallback()

void approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::twistCallback

(

geometry_msgs::msg::TwistStamped::UniquePtr

msg

)

Subscription callback for the twist subscriber, which will store latest current velocity of the ego vehicle.

Parameters

msg	Latest twist message

Definition at line 990 of file approaching_emergency_vehicle_plugin_node.cpp.

                                                                                                {
    current_speed_ = msg->twist.linear.x;
  }

References current_speed_.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

Member Data Documentation

approaching_emergency_vehicle_status_timer_

rclcpp::TimerBase::SharedPtr approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::approaching_emergency_vehicle_status_timer_

private

Definition at line 409 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by on_activate_plugin().

APPROACHING_ERV_STATUS_PARAMS

const std::string approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::APPROACHING_ERV_STATUS_PARAMS = "HAS_APPROACHING_ERV:%1%,TIME_UNTIL_PASSING:%2$.1f,EGO_VEHICLE_ACTION:%3%"

private

Formatted string for conveying the status of this plugin and the ego vehicle's current action in response to an approaching ERV.

• Index 0: (Boolean) Value indicating whether the ego vehicle is tracking an approaching ERV
• Index 1: (Double; rounded to first decimal place) If an approaching ERV exists, this indicates the estimated seconds until the ERV passes the ego vehicle.
• Index 2: (String) If an approaching ERV exists, this describes the current action of the ego vehicle in response to the approaching ERV.
• NOTE: The values of indexes 1 and 2 can be ignored if index 0 indicates that no approaching ERV is being tracked.

Definition at line 390 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by generateApproachingErvStatusMessage().

approaching_erv_status_pub_

carma_ros2_utils::PubPtr<carma_msgs::msg::UIInstructions> approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::approaching_erv_status_pub_

private

Definition at line 132 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by on_configure_plugin(), and publishApproachingErvStatus().

config_

Config approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::config_

private

Definition at line 364 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by ApproachingEmergencyVehiclePlugin(), addStopAndWaitToEndOfPlan(), broadcastWarningToErv(), checkForErvTimeout(), composeLaneChangeManeuverMessage(), composeLaneFollowingManeuverMessage(), composeStopAndWaitManeuverMessage(), generateApproachingErvStatusMessage(), generateMoveOverManeuverPlan(), generateReducedSpeedLaneFollowingeManeuverPlan(), getLaneletSpeedLimit(), getSecondsUntilPassing(), incomingBsmCallback(), incomingEmergencyVehicleAckCallback(), on_activate_plugin(), on_configure_plugin(), parameter_update_callback(), and plan_maneuvers_callback().

current_speed_

double approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::current_speed_

private

Definition at line 456 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by generateApproachingErvStatusMessage(), getErvInformationFromBsm(), getSecondsUntilPassing(), and twistCallback().

ego_lane_index_

int approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::ego_lane_index_

private

Definition at line 378 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by generateApproachingErvStatusMessage(), plan_maneuvers_callback(), and publishHazardLightStatus().

epsilon_

double approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::epsilon_ = 0.0001

private

Definition at line 473 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by addStopAndWaitToEndOfPlan(), composeLaneChangeManeuverMessage(), composeLaneFollowingManeuverMessage(), composeStopAndWaitManeuverMessage(), generateMoveOverManeuverPlan(), generateReducedSpeedLaneFollowingeManeuverPlan(), and getSecondsUntilPassing().

erv_timeout_timer_

rclcpp::TimerBase::SharedPtr approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::erv_timeout_timer_

private

Definition at line 393 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by on_activate_plugin().

erv_world_model_

std::shared_ptr<carma_wm::CARMAWorldModel> approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::erv_world_model_

private

Definition at line 470 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by ApproachingEmergencyVehiclePlugin(), and getSecondsUntilPassing().

future_route_lanelet_ids_

std::vector<int> approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::future_route_lanelet_ids_

private

Definition at line 461 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by getRouteIntersectingLanelet(), and routeCallback().

georeference_sub_

carma_ros2_utils::SubPtr<std_msgs::msg::String> approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::georeference_sub_

private

Definition at line 117 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by on_configure_plugin().

guidance_state_sub_

carma_ros2_utils::SubPtr<carma_planning_msgs::msg::GuidanceState> approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::guidance_state_sub_

private

Definition at line 125 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by on_configure_plugin().

has_broadcasted_warning_messages_

bool approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::has_broadcasted_warning_messages_ = false

private

Definition at line 396 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by plan_maneuvers_callback().

has_planned_upcoming_lc_

bool approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::has_planned_upcoming_lc_ = false

private

Definition at line 420 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by checkForErvTimeout(), composeLaneChangeManeuverMessage(), generateMoveOverManeuverPlan(), generateReducedSpeedLaneFollowingeManeuverPlan(), getSecondsUntilPassing(), incomingBsmCallback(), and plan_maneuvers_callback().

has_received_route_state_

bool approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::has_received_route_state_ = false

private

Definition at line 459 of file approaching_emergency_vehicle_plugin_node.hpp.

has_tracked_erv_

bool approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::has_tracked_erv_ = false

private

Definition at line 375 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by broadcastWarningToErv(), checkForErvTimeout(), generateApproachingErvStatusMessage(), getErvInformationFromBsm(), getSecondsUntilPassing(), incomingBsmCallback(), incomingEmergencyVehicleAckCallback(), plan_maneuvers_callback(), and publishHazardLightStatus().

hazard_light_cmd_

bool approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::hazard_light_cmd_ = false

private

Definition at line 423 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by publishHazardLightStatus().

hazard_light_cmd_pub_

carma_ros2_utils::PubPtr<std_msgs::msg::Bool> approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::hazard_light_cmd_pub_

private

Definition at line 134 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by on_configure_plugin(), and publishHazardLightStatus().

hazard_light_timer_

rclcpp::TimerBase::SharedPtr approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::hazard_light_timer_

private

Definition at line 412 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by on_activate_plugin().

incoming_bsm_sub_

carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::BSM> approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::incoming_bsm_sub_

private

Definition at line 115 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by on_configure_plugin().

incoming_emergency_vehicle_ack_sub_

carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::EmergencyVehicleAck> approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::incoming_emergency_vehicle_ack_sub_

private

Definition at line 123 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by on_configure_plugin().

is_guidance_engaged_

bool approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::is_guidance_engaged_ = false

private

Definition at line 441 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by guidanceStateCallback(), and incomingBsmCallback().

is_maintaining_non_reduced_speed_

bool approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::is_maintaining_non_reduced_speed_ = false

private

Definition at line 433 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by generateApproachingErvStatusMessage(), and plan_maneuvers_callback().

is_same_direction_

std::unordered_map<std::string, bool> approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::is_same_direction_

private

Definition at line 444 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by getErvInformationFromBsm().

latest_erv_update_times_

std::unordered_map<std::string, rclcpp::Time> approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::latest_erv_update_times_

private

Definition at line 447 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by getErvInformationFromBsm(), and incomingBsmCallback().

latest_maneuver_plan_

carma_planning_msgs::msg::ManeuverPlan approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::latest_maneuver_plan_

private

Definition at line 381 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by generateApproachingErvStatusMessage(), and plan_maneuvers_callback().

latest_route_state_

carma_planning_msgs::msg::RouteState approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::latest_route_state_

private

Definition at line 453 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by getRouteIntersectingLanelet(), getSecondsUntilPassing(), and routeStateCallback().

logger_name

std::string approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::logger_name = "approaching_emergency_vehicle_plugin"

private

Definition at line 464 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by addStopAndWaitToEndOfPlan(), broadcastWarningToErv(), checkForErvTimeout(), composeLaneChangeManeuverMessage(), composeLaneFollowingManeuverMessage(), composeStopAndWaitManeuverMessage(), filter_points_ahead(), generateErvRoute(), generateMoveOverManeuverPlan(), generateReducedSpeedLaneFollowingeManeuverPlan(), getErvInformationFromBsm(), getLaneletOnEgoRouteFromMapPosition(), getManeuverDuration(), getRouteIntersectingLanelet(), getSecondsUntilPassing(), incomingBsmCallback(), incomingEmergencyVehicleAckCallback(), on_configure_plugin(), plan_maneuvers_callback(), and routeCallback().

MAINTAIN_SPEED_THRESHOLD

const double approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::MAINTAIN_SPEED_THRESHOLD = 8.0

private

Definition at line 429 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by plan_maneuvers_callback().

map_projector_

boost::optional<std::string> approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::map_projector_

private

Definition at line 450 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by generateErvRoute(), georeferenceCallback(), and getErvPositionInMap().

non_reduced_speed_to_maintain_

double approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::non_reduced_speed_to_maintain_ = 4.4704

private

Definition at line 438 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by generateApproachingErvStatusMessage(), generateReducedSpeedLaneFollowingeManeuverPlan(), and plan_maneuvers_callback().

num_warnings_broadcasted_

int approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::num_warnings_broadcasted_ = 0

private

Definition at line 403 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by broadcastWarningToErv(), and incomingEmergencyVehicleAckCallback().

outgoing_emergency_vehicle_response_pub_

carma_ros2_utils::PubPtr<carma_v2x_msgs::msg::EmergencyVehicleResponse> approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::outgoing_emergency_vehicle_response_pub_

private

Definition at line 130 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by broadcastWarningToErv(), and on_configure_plugin().

route_state_sub_

carma_ros2_utils::SubPtr<carma_planning_msgs::msg::RouteState> approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::route_state_sub_

private

Definition at line 119 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by on_configure_plugin().

route_sub_

carma_ros2_utils::SubPtr<carma_planning_msgs::msg::Route> approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::route_sub_

private

Definition at line 127 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by on_configure_plugin().

should_broadcast_warnings_

bool approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::should_broadcast_warnings_ = false

private

Definition at line 400 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by broadcastWarningToErv(), incomingEmergencyVehicleAckCallback(), and plan_maneuvers_callback().

strategic_plugin_name_

std::string approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::strategic_plugin_name_ = "approaching_emergency_vehicle_plugin"

private

Definition at line 467 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by composeLaneChangeManeuverMessage(), composeLaneFollowingManeuverMessage(), and composeStopAndWaitManeuverMessage().

tracked_erv_

ErvInformation approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::tracked_erv_

private

Definition at line 370 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by broadcastWarningToErv(), checkForErvTimeout(), generateApproachingErvStatusMessage(), getErvInformationFromBsm(), incomingBsmCallback(), incomingEmergencyVehicleAckCallback(), plan_maneuvers_callback(), and publishHazardLightStatus().

transition_table_

ApproachingEmergencyVehicleTransitionTable approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::transition_table_

private

Definition at line 367 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by checkForErvTimeout(), generateApproachingErvStatusMessage(), getSecondsUntilPassing(), incomingBsmCallback(), plan_maneuvers_callback(), and publishHazardLightStatus().

twist_sub_

carma_ros2_utils::SubPtr<geometry_msgs::msg::TwistStamped> approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::twist_sub_

private

Definition at line 121 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by on_configure_plugin().

upcoming_lc_params_

UpcomingLaneChangeParameters approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::upcoming_lc_params_

private

Definition at line 416 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by composeLaneChangeManeuverMessage(), generateApproachingErvStatusMessage(), generateMoveOverManeuverPlan(), and plan_maneuvers_callback().

warning_broadcast_timer_

rclcpp::TimerBase::SharedPtr approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::warning_broadcast_timer_

private

Definition at line 406 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by on_activate_plugin().

wm_

carma_wm::WorldModelConstPtr approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::wm_

Definition at line 564 of file approaching_emergency_vehicle_plugin_node.hpp.

Referenced by addStopAndWaitToEndOfPlan(), generateErvRoute(), generateMoveOverManeuverPlan(), generateReducedSpeedLaneFollowingeManeuverPlan(), getErvInformationFromBsm(), getLaneletOnEgoRouteFromMapPosition(), getLaneletSpeedLimit(), getRouteIntersectingLanelet(), getSecondsUntilPassing(), on_configure_plugin(), and plan_maneuvers_callback().

---

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

• approaching_emergency_vehicle_plugin/include/approaching_emergency_vehicle_plugin/approaching_emergency_vehicle_plugin_node.hpp
• approaching_emergency_vehicle_plugin/src/approaching_emergency_vehicle_plugin_node.cpp