light_controlled_intersection_tactical_plugin.hpp

Go to the documentation of this file.

/*
 * Copyright (C) 2022 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 <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>
#include <boost/geometry.hpp>
#include <boost/shared_ptr.hpp>
#include <lanelet2_core/geometry/Point.h>
#include <lanelet2_core/primitives/Lanelet.h>
#include <lanelet2_core/geometry/LineString.h>
#include <carma_planning_msgs/msg/trajectory_plan_point.hpp>
#include <carma_planning_msgs/msg/trajectory_plan.hpp>
#include <carma_planning_msgs/srv/plan_trajectory.hpp>
#include <carma_planning_msgs/msg/maneuver.hpp>
#include <autoware_msgs/msg/lane.hpp>
#include <carma_debug_ros2_msgs/msg/trajectory_curvature_speeds.hpp>
#include <basic_autonomy/basic_autonomy.hpp>
#include <basic_autonomy/helper_functions.hpp>
 
#include "light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.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_FOLLOWING ? (mvr).lane_following_maneuver.property :\
                                throw std::invalid_argument("GET_MANEUVER_PROPERTY (property) called on maneuver with invalid type id " + std::to_string((mvr).type)))))))
 
namespace light_controlled_intersection_tactical_plugin
{
  using PointSpeedPair = basic_autonomy::waypoint_generation::PointSpeedPair;
  using GeneralTrajConfig = basic_autonomy::waypoint_generation::GeneralTrajConfig;
  using DetailedTrajConfig = basic_autonomy::waypoint_generation::DetailedTrajConfig;
  using DebugPublisher = std::function<void(const carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds&)>;
 
  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,
  };
 
  struct TrajectoryParams {
    double a1_ = 0;
    double v1_ = 0;
    double x1_ = 0;
 
    double a2_ = 0;
    double v2_ = 0;
    double x2_ = 0;
 
    double a3_ = 0;
    double v3_ = 0;
    double x3_ = 0;
  };
 
  class LightControlledIntersectionTacticalPlugin
  {
 
  private:
    // World Model object
    carma_wm::WorldModelConstPtr wm_;
 
    // Config for this object
    Config config_;
 
    std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode> nh_;
 
    // CARMA Streets Variables
    // timestamp for msg received from carma streets
    uint32_t street_msg_timestamp_ = 0.0;
    // scheduled stop time
    uint32_t scheduled_stop_time_ = 0.0;
    // scheduled enter time
    uint32_t scheduled_enter_time_ = 0.0;
    // scheduled depart time
    uint32_t scheduled_depart_time_ = 0.0;
    // scheduled latest depart time
    uint32_t scheduled_latest_depart_time_ = 0.0;
    // flag to show if the vehicle is allowed in intersection
    bool is_allowed_int_ = false;
 
    double speed_limit_ = 11.176; // Approximate speed limit; 25 mph by default
    boost::optional<TSCase> last_case_;
    boost::optional<bool> is_last_case_successful_;
    carma_planning_msgs::msg::TrajectoryPlan last_trajectory_time_unbound_;
    const std::string LCI_TACTICAL_LOGGER = "light_controlled_intersection_tactical_plugin";
    carma_planning_msgs::msg::VehicleState ending_state_before_buffer_; //state before applying extra points for curvature calculation that are removed later
    rclcpp::Time latest_traj_request_header_stamp_;
    double epsilon_ = 0.001; //Small constant to compare (double) 0.0 with
 
    // downtrack of host vehicle
    double current_downtrack_ = 0.0;
 
    double last_successful_ending_downtrack_;         // if algorithm was successful, this is traffic_light_downtrack
    double last_successful_scheduled_entry_time_;     // if algorithm was successful, this is also scheduled entry time (ET in TSMO UC2 Algo)
 
    std::string plugin_name_;
    DebugPublisher debug_publisher_;  // Publishes the debug message that includes many useful data used to generate the trajectory
    carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds debug_msg_;
    std::vector<double> last_speeds_time_unbound_;
 
    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
 
    void applyTrajectorySmoothingAlgorithm(const carma_wm::WorldModelConstPtr& wm, std::vector<PointSpeedPair>& points_and_target_speeds, double start_dist, double remaining_dist,
                                            double starting_speed, double departure_speed, TrajectoryParams tsp);
 
    void applyOptimizedTargetSpeedProfile(const carma_planning_msgs::msg::Maneuver& maneuver, const double starting_speed, std::vector<PointSpeedPair>& points_and_target_speeds);
 
    std::vector<PointSpeedPair> createGeometryProfile(const std::vector<carma_planning_msgs::msg::Maneuver> &maneuvers, double max_starting_downtrack,const carma_wm::WorldModelConstPtr &wm,
                                                                        carma_planning_msgs::msg::VehicleState &ending_state_before_buffer,const carma_planning_msgs::msg::VehicleState& state,
                                                                        const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config);
 
    double findSpeedLimit(const lanelet::ConstLanelet& llt, const carma_wm::WorldModelConstPtr &wm) const;
 
    void logDebugInfoAboutPreviousTrajectory();
 
    bool shouldUseLastTrajectory(TSCase new_case, bool is_new_case_successful,
                                  const rclcpp::Time& current_time);
 
    void planTrajectorySmoothing(
      carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req,
      carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp);
 
    carma_planning_msgs::msg::TrajectoryPlan generateNewTrajectory(
        const std::vector<carma_planning_msgs::msg::Maneuver>& maneuver_plan,
        const carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr& req,
        std::vector<double>& final_speeds);
 
    bool isLastTrajectoryValid(const rclcpp::Time& current_time,
      double min_remaining_time_seconds = 0.0) const;
 
    FRIEND_TEST(LCITacticalPluginTest, applyTrajectorySmoothingAlgorithm);
    FRIEND_TEST(LCITacticalPluginTest, applyOptimizedTargetSpeedProfile);
    FRIEND_TEST(LCITacticalPluginTest, planTrajectorySmoothing);
    FRIEND_TEST(LCITacticalPluginTest, createGeometryProfile);
    FRIEND_TEST(LCITacticalPluginTest, planTrajectoryCB);
    FRIEND_TEST(LCITacticalPluginTest, setConfig);
 
  public:
 
    LightControlledIntersectionTacticalPlugin(carma_wm::WorldModelConstPtr wm,
      const Config& config,
      const DebugPublisher& debug_publisher,
      const std::string& plugin_name,
      std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode> nh);
 
    void planTrajectoryCB(
      carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req,
      carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp);
 
    void setConfig(const Config& config);
  };
 
} // light_controlled_intersection_tactical_plugin