SignalizedIntersectionManager.hpp

Go to the documentation of this file.

#pragma once
 
/*
 * 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.
 */
 
#include <rclcpp/rclcpp.hpp>
#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_io.hpp>
#include <boost/uuid/uuid_generators.hpp>
#include <lanelet2_core/LaneletMap.h>
#include <lanelet2_core/primitives/Point.h>
#include <lanelet2_extension/regulatory_elements/StopRule.h>
#include <lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h>
#include <lanelet2_core/primitives/LaneletOrArea.h>
#include <lanelet2_extension/projection/local_frame_projector.h>
#include <carma_wm/WorldModelUtils.hpp>
#include <carma_v2x_msgs/msg/map_data.hpp>
#include <carma_v2x_msgs/msg/spat.hpp>
#include <lanelet2_core/Forward.h>
#include <lanelet2_extension/regulatory_elements/SignalizedIntersection.h>
#include <lanelet2_core/geometry/LaneletMap.h>
#include <carma_ros2_utils/carma_lifecycle_node.hpp>
 
 
namespace carma_wm
{
 
struct LANE_DIRECTION
{
  static const uint8_t INGRESS = 2;
  static const uint8_t EGRESS = 1;
};
 
enum class SIGNAL_PHASE_PROCESSING : uint8_t
{
  OFF = 0,
  ON = 1,
};
 
using namespace lanelet::units::literals;
 
class SignalizedIntersectionManager
{
public:
 
  SignalizedIntersectionManager(){}
 
  SignalizedIntersectionManager(const SignalizedIntersectionManager& other);
 
  SignalizedIntersectionManager& operator=(SignalizedIntersectionManager other);
 
  bool operator==(const SignalizedIntersectionManager& rhs);
 
  void createIntersectionFromMapMsg(std::vector<lanelet::SignalizedIntersectionPtr>& intersections, std::vector<lanelet::CarmaTrafficSignalPtr>& traffic_signals, const carma_v2x_msgs::msg::MapData& map_msg,
                                    const std::shared_ptr<lanelet::LaneletMap>& map, std::shared_ptr<const lanelet::routing::RoutingGraph> routing_graph);
 
  void convertLaneToLaneletId(std::unordered_map<uint8_t, lanelet::Id>& entry, std::unordered_map<uint8_t, lanelet::Id>& exit, const carma_v2x_msgs::msg::IntersectionGeometry& intersection,
                              const std::shared_ptr<lanelet::LaneletMap>& map, std::shared_ptr<const lanelet::routing::RoutingGraph> current_routing_graph);
 
  void setMaxLaneWidth(double max_lane_width);
 
  lanelet::Id matchSignalizedIntersection(const lanelet::Lanelets& entry_llts, const lanelet::Lanelets& exit_llts);
 
  void setTargetFrame(const std::string& target_frame);
 
  std::shared_ptr<lanelet::CarmaTrafficSignal> createTrafficSignalUsingSGID(uint8_t signal_group_id, const lanelet::Lanelets& entry_lanelets, const lanelet::Lanelets& exit_lanelets);
 
  lanelet::Lanelets identifyInteriorLanelets(const lanelet::Lanelets& entry_llts, const std::shared_ptr<lanelet::LaneletMap>& map);
 
  void processSpatFromMsg(const carma_v2x_msgs::msg::SPAT& spat_msg,
    const std::shared_ptr<lanelet::LaneletMap>& semantic_map);
 
  std::tuple<std::vector<std::pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState>>,
    std::vector<boost::posix_time::ptime>>
    extract_signal_states_from_movement_state(
      const carma_v2x_msgs::msg::IntersectionState& curr_intersection,
      const carma_v2x_msgs::msg::MovementState& current_movement_state);
 
  boost::posix_time::ptime min_end_time_converter_minute_of_year(
    boost::posix_time::ptime min_end_time,bool moy_exists,uint32_t moy=0,
    bool is_simulation = true, bool use_real_time_spat_in_sim=false);
 
  lanelet::Id getTrafficSignalId(uint16_t intersection_id,uint8_t signal_id);
 
  lanelet::CarmaTrafficSignalPtr getTrafficSignal(const lanelet::Id& id,
    const std::shared_ptr<lanelet::LaneletMap>& semantic_map) const;
 
  // SignalizedIntersection's geometry points from MAP Msg
  std::unordered_map<uint16_t, std::vector<lanelet::Point3d>> intersection_nodes_;
 
  // SignalizedIntersection's reference point correction pair of (x, y) for each intersection_id
  std::unordered_map<uint16_t, std::pair<double, double>> intersection_coord_correction_;
 
  // SignalizedIntersection quick id lookup
  std::unordered_map<uint16_t, lanelet::Id> intersection_id_to_regem_id_;
 
  // SignalizedIntersection reverse quick id lookup
  std::unordered_map<lanelet::Id, uint16_t> regem_id_to_intersection_id_;
 
  // CarmaTrafficSignal quick id lookup
  std::unordered_map<uint8_t, lanelet::Id> signal_group_to_traffic_light_id_;
 
  // CarmaTrafficSignal exit lanelets ids quick lookup
  std::unordered_map<uint8_t, std::unordered_set<lanelet::Id>> signal_group_to_exit_lanelet_ids_;
 
  // CarmaTrafficSignal entry lanelets ids quick lookup
  std::unordered_map<uint8_t, std::unordered_set<lanelet::Id>> signal_group_to_entry_lanelet_ids_;
 
  std::optional<rclcpp::Time> ros1_clock_ = std::nullopt;
  std::optional<rclcpp::Time> simulation_clock_ = std::nullopt;
 
  bool use_sim_time_;
  bool use_real_time_spat_in_sim_;
  SIGNAL_PHASE_PROCESSING spat_processor_state_ = SIGNAL_PHASE_PROCESSING::OFF;
 
private:
  // PROJ string of current map
  std::string target_frame_ = "";
 
  // Max width of lane in meters
  double max_lane_width_ = 4;
 
};
 
}  // namespace carma_wm