approaching_emergency_vehicle_plugin_node.hpp

Go to the documentation of this file.

/*
 * Copyright (C) 2022-2023 LEIDOS.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */
 
#pragma once
 
#include <rclcpp/rclcpp.hpp>
#include <functional>
#include <std_msgs/msg/string.hpp>
#include <std_srvs/srv/empty.hpp>
#include <carma_planning_msgs/msg/plugin.hpp>
#include <carma_planning_msgs/msg/upcoming_lane_change_status.hpp>
#include <carma_planning_msgs/msg/route_state.hpp>
#include <carma_planning_msgs/msg/guidance_state.hpp>
#include <carma_v2x_msgs/msg/bsm.hpp>
#include <carma_v2x_msgs/msg/emergency_vehicle_response.hpp>
#include <carma_v2x_msgs/msg/bsm.hpp>
#include <carma_v2x_msgs/msg/emergency_vehicle_ack.hpp>
#include <carma_msgs/msg/ui_instructions.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <carma_wm/WMListener.hpp>
#include <carma_wm/WorldModel.hpp>
#include <lanelet2_core/geometry/Lanelet.h>
#include <lanelet2_extension/projection/local_frame_projector.h>
#include <lanelet2_extension/io/autoware_osm_parser.h>
#include <carma_wm/CARMAWorldModel.hpp>
#include <boost/format.hpp>
#include <std_msgs/msg/bool.hpp>
#include <unordered_map>
 
#include <carma_guidance_plugins/strategic_plugin.hpp>
#include "approaching_emergency_vehicle_plugin/approaching_emergency_vehicle_plugin_config.hpp"
#include "approaching_emergency_vehicle_plugin/approaching_emergency_vehicle_transition_table.hpp"
#include "approaching_emergency_vehicle_plugin/approaching_emergency_vehicle_states.hpp"
 
#define GET_MANEUVER_PROPERTY(mvr, property)\
        (((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property :\
            ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property :\
                ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property :\
                    ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_CHANGE ? (mvr).lane_change_maneuver.property :\
                        ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property :\
                            ((mvr).type == carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT ? (mvr).stop_and_wait_maneuver.property :\
                                throw std::invalid_argument("GET_MANEUVER_PROPERTY (property) called on maneuver with invalid type id " + std::to_string((mvr).type)))))))))
 
namespace approaching_emergency_vehicle_plugin
{
 
  struct ErvInformation{
    std::string vehicle_id;            // The vehicle ID asssociated with an ERV 
    double current_speed;              // The current speed (m/s) of an ERV
    double current_latitude;           // The current latitude of an ERV
    double current_longitude;          // The current longitude of an ERV
    lanelet::BasicPoint2d current_position_in_map; // The current position of the ERV in the map frame
    lanelet::ConstLanelet intersecting_lanelet;    // The first intersecting lanelet between ERV's future route and CMV's future shortest path
    double seconds_until_passing;      // The estimated duration (seconds) until the ERV will pass the ego vehicle
                                       //     based on their current positions and current speeds
    int previous_lane_index;           // The lane index of the 
    int lane_index = 0;                // The ERV's current lane index 
                                       //     NOTE 1: For 'lane index', 0 is rightmost lane, 1 is second rightmost, etc.; Only the current travel direction is considered
                                       //     NOTE 2: To prevent "noisy" lane index calculations from the ERV, this value is only updated when the lane index of the ERV (according to a processed BSM) matches
                                       //                previous_lane_index, which was obtained from the previously processed BSM.
    rclcpp::Time latest_update_time;   // The timestamp (from this node's clock) associated with the last update of this object
  };
 
  struct UpcomingLaneChangeParameters{
    lanelet::ConstLanelet starting_lanelet; // The starting lanelet of the upcoming lane change
    lanelet::ConstLanelet ending_lanelet;   // The ending lanelet of the upcoming lane change
    bool is_right_lane_change;              // Flag to indicate lane change direction; true if a right lane change, false if a left lane change
    double start_dist;                      // The starting downtrack of the upcoming lane change
    double end_dist;                        // The ending downtrack of the upcoming lane change
    double start_speed;                     // The start speed of the upcoming lane change
    double end_speed;                       // The end speed of the upcoming lane change
    std::string maneuver_id;                // The maneuver ID of the upcoming lane change
  };
 
  // Constant for converting from meters per second to miles per hour
  constexpr double METERS_PER_SEC_TO_MILES_PER_HOUR = 2.23694;
 
  class ApproachingEmergencyVehiclePlugin : public carma_guidance_plugins::StrategicPlugin
  {
 
  private:
    // Subscribers
    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_;
 
    // Publishers
    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_;
 
    boost::optional<lanelet::BasicPoint2d> getErvPositionInMap(const double& current_latitude, const double& current_longitude);
 
    lanelet::Optional<lanelet::routing::Route> generateErvRoute(double current_latitude, double current_longitude, 
                                                                             std::vector<carma_v2x_msgs::msg::Position3D> erv_destination_points);
 
    boost::optional<lanelet::ConstLanelet> getRouteIntersectingLanelet(const lanelet::routing::Route& erv_future_route);
 
    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);
 
    boost::optional<ErvInformation> getErvInformationFromBsm(carma_v2x_msgs::msg::BSM::UniquePtr msg);
 
    void checkForErvTimeout();
 
    void broadcastWarningToErv();
 
    void publishApproachingErvStatus();
 
    void publishHazardLightStatus();
 
    carma_msgs::msg::UIInstructions generateApproachingErvStatusMessage();
 
    boost::optional<lanelet::ConstLanelet> getLaneletOnEgoRouteFromMapPosition(const double& x_position, const double& y_position);
 
    double getLaneletSpeedLimit(const lanelet::ConstLanelet& lanelet);
 
    rclcpp::Duration getManeuverDuration(const carma_planning_msgs::msg::Maneuver &maneuver, double epsilon) const;
 
    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;
 
    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;
 
    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;
 
    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);
 
    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);
 
    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);
 
    std::vector<lanelet::BasicPoint2d> filter_points_ahead(const lanelet::BasicPoint2d& reference_point, const std::vector<lanelet::BasicPoint2d>& original_points) const;
 
    // ApproachingEmergencyVehiclePlugin configuration
    Config config_;
 
    // State Machine Transition table
    ApproachingEmergencyVehicleTransitionTable transition_table_;
 
    // ErvInformation object, which includes information on the currently-tracked ERV
    ErvInformation tracked_erv_;
 
    // Boolean flag to indicate whether an ERV is being actively tracked by this plugin. When true, the
    //         tracked_erv_ object contains the latest received information pertaining to the ERV.
    // NOTE: An ERV is only actively tracked if it is considered to be approaching or passing the ego vehicle.
    bool has_tracked_erv_ = false;
 
    // 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)
    int ego_lane_index_;
 
    // The latest maneuver plan generated by this plugin
    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%";
 
    // Timer used to check whether a timeout has occurred with the currently-tracked ERV
    rclcpp::TimerBase::SharedPtr erv_timeout_timer_;
 
    // Boolean flag to indicate whether the ego vehicle has broadcasted EmergencyVehicleResponse warning message(s) to an approaching ERV
    bool has_broadcasted_warning_messages_ = false;
 
    // Boolean flag to indicate that EmergencyVehicleResponse warning message(s) should be broadcasted to the currently-tracked ERV
    // Note: These are broadcasted when the ego vehicle is in the ERV's path but is unable to change lanes
    bool should_broadcast_warnings_ = false;
 
    // Counter for the number of EmergencyVehicleResponse warning messages broadcasted to the currently-tracked ERV
    int num_warnings_broadcasted_ = 0;
 
    // Timer used to trigger the broadcast of an EmergencyVehicleResponse warning message to the currently-tracked ERV
    rclcpp::TimerBase::SharedPtr warning_broadcast_timer_;
 
    // Timer used to trigger the publication of a message describing the status of this plugin and the ego vehicle's current action in response to an approaching ERV
    rclcpp::TimerBase::SharedPtr approaching_emergency_vehicle_status_timer_;
 
    // Timer used to command the hazard lights to turn ON/OFF (true/false) 
    rclcpp::TimerBase::SharedPtr hazard_light_timer_;
 
    // Object to store the parameters of an upcoming lane change maneuver so that the same parameters are used when the
    //        maneuver plan is regenerated
    UpcomingLaneChangeParameters upcoming_lc_params_;
 
    // Boolean flag to indicate that this plugin has planned an upcoming lane change, and those same lane change maneuver
    //        parameters should be used for the next generated maneuver plan as well
    bool has_planned_upcoming_lc_ = false;
 
    // Boolean flag to command turning ON/OFF (true/false) the hazard lights
    bool hazard_light_cmd_ = false;
 
    // (Seconds) A threshold; if the estimated duration until an ERV passes the ego vehicle is below this and the
    //           ego vehicle is in the same lane as the ERV, then the ego vehicle will not reduce its speed, because
    //           doing so could cause a safety hazard.
    // NOTE: This value is not configurable because it is considered to be a safety threshold.
    const double MAINTAIN_SPEED_THRESHOLD = 8.0;
 
    // Boolean flag to indicate that transition_table_ is in the 'SLOWING_DOWN_FOR_ERV' state, but the ego vehicle will maintain a non-reduced speed since the
    // ERV is in the same lane as the ego vehicle and is estimated to pass the ego vehicle in less than the 'MAINTAIN_SPEED_THRESHOLD' time threshold.
    bool is_maintaining_non_reduced_speed_ = false;
 
    // (m/s) The target speed that the ego vehicle shall maintain when the 'is_maintaining_non_reduced_speed_' flag is true. Set to the latest ego vehicle
    //       speed at the moment transition_table_ most recently received the 'ERV_PASSING_IN_PATH' event and the ERV was estimated to pass the ego 
    //       vehicle in less than the 'MAINTAIN_SPEED_THERSHOLD' time threshold.
    double non_reduced_speed_to_maintain_ = 4.4704; // Default is 4.4704 m/s or 10 mph 
 
    // Boolean flag to indicate whether guidance is currently engaged
    bool is_guidance_engaged_ = false;
 
    // Boolean flag to indicate whether if each ERV and CMV are going on same direction
    std::unordered_map<std::string, bool> is_same_direction_;
 
    // Unordered map to store the latest time a BSM was processed for a given active ERV
    std::unordered_map<std::string, rclcpp::Time> latest_erv_update_times_;
 
    // Pointer for map projector
    boost::optional<std::string> map_projector_;
 
    // Latest route state
    carma_planning_msgs::msg::RouteState latest_route_state_;
 
    // Ego vehicle's current speed
    double current_speed_;
 
    // Boolean flag to indicate whether this node has received a Route State message in routeStateCallback()
    bool has_received_route_state_ = false;
 
    std::vector<int> future_route_lanelet_ids_;
    
    // Logger name for this plugin
    std::string logger_name = "approaching_emergency_vehicle_plugin";
 
    // The name of this strategic plugin
    std::string strategic_plugin_name_ = "approaching_emergency_vehicle_plugin";
 
    // Separate world model object for storing the ERV's route and enabling downtrack calculations on its route
    std::shared_ptr<carma_wm::CARMAWorldModel> erv_world_model_;
 
    // Parameter for comparisons to 0
    double epsilon_ = 0.0001;
 
    // Unit Test Accessors
    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);
 
  public:
    explicit ApproachingEmergencyVehiclePlugin(const rclcpp::NodeOptions &);
 
    rcl_interfaces::msg::SetParametersResult 
    parameter_update_callback(const std::vector<rclcpp::Parameter> &parameters);
 
    void incomingBsmCallback(carma_v2x_msgs::msg::BSM::UniquePtr msg);
 
    void routeStateCallback(carma_planning_msgs::msg::RouteState::UniquePtr msg);
 
    void routeCallback(carma_planning_msgs::msg::Route::UniquePtr msg);
 
    void georeferenceCallback(const std_msgs::msg::String::UniquePtr msg);
 
    void incomingEmergencyVehicleAckCallback(const carma_v2x_msgs::msg::EmergencyVehicleAck::UniquePtr msg);
 
    void twistCallback(geometry_msgs::msg::TwistStamped::UniquePtr msg);
 
    void guidanceStateCallback(const carma_planning_msgs::msg::GuidanceState::UniquePtr msg);
 
    // Overrides
    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;
 
    bool get_availability() override;
 
    std::string get_version_id() override;
    
    carma_ros2_utils::CallbackReturn on_configure_plugin();
 
    carma_ros2_utils::CallbackReturn on_activate_plugin();
 
    // World Model pointer
    carma_wm::WorldModelConstPtr wm_;
 
  };
 
} // approaching_emergency_vehicle_plugin