lci_strategic_plugin.hpp

Go to the documentation of this file.

#pragma once
 
/*
 * Copyright (C)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.
 */
 
#include <rclcpp/rclcpp.hpp>
#include <string>
#include <carma_planning_msgs/srv/plan_maneuvers.hpp>
#include <carma_v2x_msgs/msg/mobility_operation.hpp>
#include <carma_v2x_msgs/msg/bsm.hpp>
#include <carma_wm/WMListener.hpp>
#include <carma_wm/WorldModel.hpp>
 
#include <bsm_helper/bsm_helper.h>
#include <carma_wm/Geometry.hpp>
#include <lanelet2_core/Forward.h>
#include <gtest/gtest_prod.h>
 
#include <lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h>
#include "lci_strategic_plugin/lci_state_transition_table.hpp"
#include "lci_strategic_plugin/lci_strategic_plugin_config.hpp"
#include "lci_strategic_plugin/lci_states.hpp"
 
#include <tf2_ros/transform_listener.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <std_msgs/msg/int8.hpp>
#include <std_msgs/msg/float64.hpp>
#include <math.h>
#include <algorithm>
#include <carma_guidance_plugins/strategic_plugin.hpp>
#include <carma_ros2_utils/carma_lifecycle_node.hpp>
#include <optional>
 
namespace lci_strategic_plugin
{
 
enum class TurnDirection {
  Straight,
  Right,
  Left
};
 
enum TSCase {
  CASE_1 = 1,
  CASE_2 = 2,
  CASE_3 = 3,
  CASE_4 = 4,
  CASE_5 = 5,
  CASE_6 = 6,
  CASE_7 = 7,
  CASE_8 = 8,
  STOPPING=9,
  UNAVAILABLE = 10,
  EMERGENCY_STOPPING=11,
  DEGRADED_TSCASE=12  // when not performing trajectory smoothing, but making through GREEN
};
 
struct TrajectoryParams
{
  double t0_ = 0;
  double v0_ = 0;
  double x0_ = 0;
 
  double a1_ = 0;
  double t1_ = 0;
  double v1_ = 0;
  double x1_ = 0;
 
  double a2_ = 0;
  double t2_ = 0;
  double v2_ = 0;
  double x2_ = 0;
 
  double a3_ = 0;
  double t3_ = 0;
  double v3_ = 0;
  double x3_ = 0;
 
  bool is_algorithm_successful = true;
  TSCase case_num;
  double modified_departure_speed = -1.0;  // modified departure speed if algorithm failed
  double modified_remaining_time = -1.0;  // modified departure time if algorithm failed
};
 
struct BoundaryDistances
{
  double dx1 = 0.0;
  double dx2 = 0.0;
  double dx3 = 0.0;
  double dx4 = 0.0;
  double dx5 = 0.0;
};
 
class LCIStrategicPlugin : public carma_guidance_plugins::StrategicPlugin
{
public:
 
  explicit LCIStrategicPlugin(const rclcpp::NodeOptions &);
 
  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);
 
  rcl_interfaces::msg::SetParametersResult
  parameter_update_callback(const std::vector<rclcpp::Parameter> &parameters);
 
  carma_ros2_utils::CallbackReturn on_configure_plugin();
  carma_ros2_utils::CallbackReturn on_activate_plugin();
 
  bool get_availability();
  std::string get_version_id();
 
  void lookupFrontBumperTransform();
 
  void mobilityOperationCb(carma_v2x_msgs::msg::MobilityOperation::UniquePtr msg);
 
  void BSMCb(carma_v2x_msgs::msg::BSM::UniquePtr msg);
 
  void parseStrategyParams(const std::string& strategy_params);
 
  carma_v2x_msgs::msg::MobilityOperation generateMobilityOperation();
 
  TurnDirection getTurnDirectionAtIntersection(std::vector<lanelet::ConstLanelet> lanelets_list);
 
  void publishMobilityOperation();
 
  void publishTrajectorySmoothingInfo();
 
private:
 
  // CARMA Streets Variables
  // timestamp for msg received from carma streets
  unsigned long long street_msg_timestamp_ = 0;
  // scheduled enter time
  unsigned long long scheduled_enter_time_ = 0;
  std::string previous_strategy_params_ = "";
  std::string upcoming_id_ = "";
 
  //BSM
  std::string bsm_id_ = "default_bsm_id";
  uint8_t bsm_msg_count_ = 0;
  uint16_t bsm_sec_mark_ = 0;
 
  // TS planning related variables
  double max_comfort_accel_ = 2.0;  // acceleration rates after applying miltiplier
  double max_comfort_decel_ = -2.0;
  double max_comfort_decel_norm_ = -1 * max_comfort_decel_;
  double emergency_decel_norm_ = -2 * max_comfort_decel_;
 
  boost::optional<rclcpp::Time> nearest_green_entry_time_cached_;
  TSCase last_case_num_ = TSCase::UNAVAILABLE;
  double distance_remaining_to_tf_ = 0.0;
  double earliest_entry_time_ = 0.0;
  double scheduled_entry_time_ = 0.0;
 
  TurnDirection intersection_turn_direction_ = TurnDirection::Straight;
  bool approaching_light_controlled_intersection_ = false;
 
  LCIStrategicPluginConfig config_;
 
  LCIStrategicStateTransitionTable transition_table_;
 
  boost::optional<double> intersection_speed_;
  boost::optional<double> intersection_end_downtrack_;
  std::string light_controlled_intersection_strategy_ = "Carma/signalized_intersection"; // Strategy carma-streets is sending. Could be more verbose but needs to be changed on both ends
 
  // TF listenser
  tf2_ros::Buffer tf2_buffer_;
  std::unique_ptr<tf2_ros::TransformListener> tf2_listener_;
  tf2::Stamped<tf2::Transform> frontbumper_transform_;
  double length_to_front_bumper_ = 3.0;
 
  double epsilon_ = 0.001; //Small constant to compare (double) 0.0 with
  double accel_epsilon_ = 0.0001; //Small constant to compare (double) 0.0 with
 
  carma_wm::WorldModelConstPtr wm_;
 
  // Timers
  rclcpp::TimerBase::SharedPtr mob_op_pub_timer_;
  rclcpp::TimerBase::SharedPtr ts_info_pub_timer_;
 
  // Create subscribers
  carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::MobilityOperation> mob_operation_sub_;
  carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::BSM> bsm_sub_;
 
  // Create publishers
  carma_ros2_utils::PubPtr<carma_v2x_msgs::msg::MobilityOperation> mobility_operation_pub_;
  carma_ros2_utils::PubPtr<std_msgs::msg::Int8> case_pub_;
  carma_ros2_utils::PubPtr<std_msgs::msg::Float64> tf_distance_pub_;
  carma_ros2_utils::PubPtr<std_msgs::msg::Float64> earliest_et_pub_;
  carma_ros2_utils::PubPtr<std_msgs::msg::Float64> et_pub_;
 
  struct VehicleState
  {
    rclcpp::Time stamp;      // Timestamp of this state data
    double downtrack;     // The downtrack of the vehicle along the route at time stamp
    double speed;         // The speed of the vehicle at time stamp
    lanelet::Id lane_id;  // The current lane id of the vehicle at time stamp
  };
 
  void planWhenUNAVAILABLE(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
                           carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
                           const VehicleState& current_state,
                           const lanelet::CarmaTrafficSignalPtr& traffic_light,
                           const lanelet::ConstLanelet& entry_lanelet,
                           const lanelet::ConstLanelet& exit_lanelet,
                           const lanelet::ConstLanelet& current_lanelet);
 
  void planWhenAPPROACHING(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
                           carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
                           const VehicleState& current_state,
                           const lanelet::CarmaTrafficSignalPtr& traffic_light,
                           const lanelet::ConstLanelet& entry_lanelet,
                           const lanelet::ConstLanelet& exit_lanelet,
                           const lanelet::ConstLanelet& current_lanelet);
 
  void planWhenWAITING(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
                       carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
                       const VehicleState& current_state,
                       const lanelet::CarmaTrafficSignalPtr& traffic_light,
                       const lanelet::ConstLanelet& entry_lanelet,
                       const lanelet::ConstLanelet& exit_lanelet,
                       const lanelet::ConstLanelet& current_lanelet);
 
  void planWhenDEPARTING(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
                         carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
                         const VehicleState& current_state, double intersection_end_downtrack,
                         double intersection_speed_limit);
 
  boost::optional<bool> canArriveAtGreenWithCertainty(const rclcpp::Time& light_arrival_time_by_algo, const lanelet::CarmaTrafficSignalPtr& traffic_light, bool check_late, bool check_early) const;
 
  carma_planning_msgs::msg::Maneuver composeTrajectorySmoothingManeuverMessage(double start_dist, double end_dist, const std::vector<lanelet::ConstLanelet>& crossed_lanelets, double start_speed,
                                                       double target_speed, rclcpp::Time start_time, rclcpp::Time end_time,
                                                       const TrajectoryParams& tsp) const;
 
  carma_planning_msgs::msg::Maneuver composeStopAndWaitManeuverMessage(double current_dist, double end_dist, double start_speed,
                                                       const lanelet::Id& starting_lane_id,
                                                       const lanelet::Id& ending_lane_id, rclcpp::Time start_time,
                                                       rclcpp::Time end_time, double stopping_accel) const;
 
  carma_planning_msgs::msg::Maneuver composeIntersectionTransitMessage(double start_dist, double end_dist, double start_speed,
                                                       double target_speed, rclcpp::Time start_time, rclcpp::Time end_time,
                                                       const lanelet::Id& starting_lane_id,
                                                       const lanelet::Id& ending_lane_id) const;
 
 
  void handleStopping(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
  carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
                                        const VehicleState& current_state,
                                        const lanelet::CarmaTrafficSignalPtr& traffic_light,
                                        const lanelet::ConstLanelet& entry_lanelet, const lanelet::ConstLanelet& exit_lanelet, const lanelet::ConstLanelet& current_lanelet,
                                        double traffic_light_down_track,
                                        bool is_emergency = false);
 
  void handleGreenSignalScenario(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
  carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
                                        const VehicleState& current_state,
                                        double current_state_speed,
                                        const lanelet::CarmaTrafficSignalPtr& traffic_light,
                                        const lanelet::ConstLanelet& entry_lanelet, const lanelet::ConstLanelet& exit_lanelet,
                                        double traffic_light_down_track, const TrajectoryParams& ts_params, bool is_certainty_check_optional);
  void handleCruisingUntilStop(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
  carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
                                        const VehicleState& current_state,
                                        double current_state_speed,
                                        const lanelet::CarmaTrafficSignalPtr& traffic_light,
                                        double traffic_light_down_track, const TrajectoryParams& ts_params);
 
  void handleFailureCase(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
  carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp,
                                        const VehicleState& current_state,
                                        double current_state_speed,
                                        double speed_limit,
                                        double remaining_time,
                                        lanelet::Id exit_lanelet_id,
                                        const lanelet::CarmaTrafficSignalPtr& traffic_light,
                                        double traffic_light_down_track, const TrajectoryParams& ts_params);
 
  TrajectoryParams handleFailureCaseHelper(const lanelet::CarmaTrafficSignalPtr& traffic_light, double current_time, double starting_speed, double departure_speed, double speed_limit, double remaining_downtrack, double traffic_light_downtrack);
 
  bool supportedLightState(lanelet::CarmaTrafficSignalState state) const;
 
  bool validLightState(const boost::optional<std::pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState>>& optional_state,
                       const rclcpp::Time& source_time) const;
 
  VehicleState extractInitialState(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req) const;
 
  std::vector<lanelet::ConstLanelet> getLaneletsBetweenWithException(double start_downtrack, double end_downtrack,
                                                                     bool shortest_path_only = false,
                                                                     bool bounds_inclusive = true) const;
 
  std::tuple<rclcpp::Time, bool, bool> get_final_entry_time_and_conditions(const VehicleState& current_state,
                                                const rclcpp::Time& earliest_entry_time,
                                                lanelet::CarmaTrafficSignalPtr traffic_light);
 
  std::optional<rclcpp::Time> get_nearest_green_entry_time(const rclcpp::Time& current_time, const rclcpp::Time& earliest_entry_time, const lanelet::CarmaTrafficSignalPtr& signal, double minimum_required_green_time = 0.0) const;
 
  rclcpp::Time get_eet_or_tbd(const rclcpp::Time& earliest_entry_time, const lanelet::CarmaTrafficSignalPtr& signal) const;
 
  rclcpp::Duration get_earliest_entry_time(double remaining_distance, double free_flow_speed, double current_speed, double departure_speed, double max_accel, double max_decel) const;
 
  double get_trajectory_smoothing_activation_distance(double time_remaining_at_free_flow, double full_cycle_duration, double current_speed, double speed_limit, double departure_speed, double max_accel, double max_decel) const;
 
  double get_distance_to_accel_or_decel_twice (double free_flow_speed, double current_speed, double departure_speed, double max_accel, double max_decel) const;
 
  double get_inflection_speed_value (double x, double x1, double x2, double free_flow_speed, double current_speed, double departure_speed, double max_accel, double max_decel) const;
 
  double get_distance_to_accel_or_decel_once (double current_speed, double departure_speed, double max_accel, double max_decel) const;
 
  double estimate_distance_to_stop(double v, double a) const;
 
  double estimate_time_to_stop(double d, double v) const;
 
  double findSpeedLimit(const lanelet::ConstLanelet& llt) const;
 
  double calc_estimated_entry_time_left(double entry_dist, double current_speed, double departure_speed) const;
 
  BoundaryDistances get_delta_x(double v0, double v1, double v_max, double v_min, double a_max, double a_min);
 
  std::vector<TrajectoryParams> get_boundary_traj_params(double t, double v0, double v1, double v_max, double v_min, double a_max, double a_min, double x0, double x_end, double dx, BoundaryDistances boundary_distances);
 
  void print_params(TrajectoryParams params);
 
  void print_boundary_distances(BoundaryDistances delta_xs);
 
  TrajectoryParams ts_case1(double t, double et, double v0, double v1, double v_max, double a_max, double a_min, double x0, double x_end, double dx);
  TrajectoryParams ts_case2(double t, double et, double v0, double v1, double a_max, double a_min, double x0, double x_end, double dx);
  TrajectoryParams ts_case3(double t, double et, double v0, double v1, double a_max, double a_min, double x0, double x_end, double dx);
  TrajectoryParams ts_case4(double t, double et, double v0, double v1, double v_min, double a_max, double a_min, double x0, double x_end, double dx);
  TrajectoryParams ts_case5(double t, double et, double v0, double a_max, double a_min, double x0, double x_end, double dx);
  TrajectoryParams ts_case6(double t, double et, double v0, double v_min, double a_min, double x0, double x_end, double dx, double dx3, TrajectoryParams traj6);
  TrajectoryParams ts_case7(double t, double et, double v0, double v_min, double a_min, double x0, double x_end, double dx);
  TrajectoryParams ts_case8(double dx, double dx5, TrajectoryParams traj8);
 
  TrajectoryParams boundary_accel_or_decel_incomplete_upper(double t, double v0, double v1, double a_max, double a_min, double x0, double x_end, double dx);
  TrajectoryParams boundary_accel_nocruise_notmaxspeed_decel(double t, double v0, double v1, double a_max, double a_min, double x0, double x_end, double dx);
  TrajectoryParams boundary_accel_cruise_maxspeed_decel(double t, double v0, double v1, double v_max, double a_max, double a_min, double x0, double x_end, double dx);
  TrajectoryParams boundary_accel_nocruise_maxspeed_decel(double t, double v0, double v1, double v_max, double a_max, double a_min, double x0, double x_end, double dx);
  TrajectoryParams boundary_accel_or_decel_complete_upper(double t, double v0, double v1, double x0, double x_end, double dx);
  TrajectoryParams boundary_decel_nocruise_notminspeed_accel(double t, double v0, double v1, double v_min, double a_max, double a_min, double x0, double x_end, double dx);
  TrajectoryParams boundary_decel_nocruise_minspeed_accel_incomplete(double t, double v0, double v_min, double a_max, double a_min, double x0, double x_end, double dx);
  TrajectoryParams boundary_decel_nocruise_minspeed_accel_complete(double t, double v0, double v1, double v_max, double v_min, double a_max, double a_min, double x0, double x_end, double dx);
  TrajectoryParams boundary_decel_cruise_minspeed_accel(double t, double v0, double v1, double v_min, double a_max, double a_min, double x0, double x_end, double dx);
  TrajectoryParams boundary_decel_cruise_minspeed(double t, double v0, double v_min, double a_min, double x0, double x_end, double dx);
  TrajectoryParams boundary_decel_incomplete_lower(double t, double v0, double a_min, double x0, double x_end, double dx);
  TrajectoryParams boundary_decel_cruise_minspeed_decel(double t, double v0, double v_min, double a_min, double x0, double x_end, double dx);
 
  TrajectoryParams get_ts_case(double t, double et, double v0, double v1, double v_max, double v_min, double a_max, double a_min, double x0, double x_end, double dx, BoundaryDistances boundary_distances, std::vector<TrajectoryParams> params);
 
  //Unit Tests
  FRIEND_TEST(LCIStrategicTestFixture, getDiscoveryMsg);
  FRIEND_TEST(LCIStrategicTestFixture, supportedLightState);
  FRIEND_TEST(LCIStrategicTestFixture, estimate_distance_to_stop);
  FRIEND_TEST(LCIStrategicTestFixture, estimate_time_to_stop);
  FRIEND_TEST(LCIStrategicTestFixture, extractInitialState);
  FRIEND_TEST(LCIStrategicTestFixture, DISABLED_extractInitialState);
 
  FRIEND_TEST(LCIStrategicTestFixture, validLightState);
  FRIEND_TEST(LCIStrategicTestFixture, getLaneletsBetweenWithException);
  FRIEND_TEST(LCIStrategicTestFixture, composeStopAndWaitManeuverMessage);
  FRIEND_TEST(LCIStrategicTestFixture, composeIntersectionTransitMessage);
  FRIEND_TEST(LCIStrategicTestFixture, composeTrajectorySmoothingManeuverMessage);
  FRIEND_TEST(LCIStrategicTestFixture, handleFailureCaseHelper);
  FRIEND_TEST(LCIStrategicTestFixture, planWhenETInTBD);
 
  // Algo Unit Tests
  FRIEND_TEST(LCIStrategicTestFixture, calc_estimated_entry_time_left);
  FRIEND_TEST(LCIStrategicTestFixture, get_distance_to_accel_or_decel_twice);
  FRIEND_TEST(LCIStrategicTestFixture, get_distance_to_accel_or_decel_once);
  FRIEND_TEST(LCIStrategicTestFixture, get_nearest_green_entry_time);
  FRIEND_TEST(LCIStrategicTestFixture, get_eet_or_tbd);
  FRIEND_TEST(LCIStrategicTestFixture, get_earliest_entry_time);
  FRIEND_TEST(LCIStrategicTestFixture, handleFailureCase);
  FRIEND_TEST(LCIStrategicTestFixture, handleStopping);
  FRIEND_TEST(LCIStrategicTestFixture, planManeuverCb);
  FRIEND_TEST(LCIStrategicTestFixture, DISABLED_planManeuverCb);
 
  FRIEND_TEST(LCIStrategicPluginTest, parseStrategyParamstest);
  FRIEND_TEST(LCIStrategicPluginTest, moboperationcbtest);
  FRIEND_TEST(LCIStrategicTestFixture, findSpeedLimit);
  FRIEND_TEST(LCIStrategicTestFixture, DISABLED_planWhenETInTBD);
 
 
 
 
 
};
}  // namespace lci_strategic_plugin