WMBroadcaster.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 <functional>
#include <mutex>
#include <lanelet2_core/LaneletMap.h>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/date_time/date_defs.hpp>
#include <boost/icl/interval_set.hpp>
#include <unordered_set>
#include <rclcpp/rclcpp.hpp>
#include <lanelet2_core/geometry/Lanelet.h>
#include <lanelet2_core/primitives/Lanelet.h>
#include <autoware_lanelet2_msgs/msg/map_bin.h>
#include <autoware_lanelet2_ros2_interface/utility/message_conversion.hpp>
#include <lanelet2_extension/projection/local_frame_projector.h>
#include <carma_wm_ctrl/GeofenceScheduler.hpp>
#include <lanelet2_core/geometry/BoundingBox.h>
#include <lanelet2_core/primitives/BoundingBox.h>
#include <carma_wm/WMListener.hpp>
#include <carma_planning_msgs/msg/route.hpp>
#include <carma_v2x_msgs/msg/traffic_control_request.hpp>
#include <carma_v2x_msgs/msg/traffic_control_bounds.hpp>
#include <lanelet2_routing/RoutingGraph.h>
#include <geometry_msgs/msg/pose_stamped.h>
 
#include <carma_wm/MapConformer.hpp>
 
#include <lanelet2_extension/traffic_rules/CarmaUSTrafficRules.h>
#include <lanelet2_core/utility/Units.h>
#include <carma_perception_msgs/msg/check_active_geofence.hpp>
#include <carma_wm/TrafficControl.hpp>
#include <std_msgs/msg/string.hpp>
#include <unordered_set>
#include <visualization_msgs/msg/marker_array.hpp>
#include <carma_v2x_msgs/msg/traffic_control_request_polygon.hpp>
#include <carma_wm/WorldModelUtils.hpp>
#include <std_msgs/msg/int32_multi_array.hpp>
#include <carma_v2x_msgs/msg/map_data.hpp>
#include <carma_wm/SignalizedIntersectionManager.hpp>
#include <carma_v2x_msgs/msg/mobility_operation.hpp>
 
 
namespace carma_wm_ctrl
{
  const int WORKZONE_TCM_REQUIRED_SIZE = 4;
  struct WorkZoneSection { static const uint8_t OPEN; static const uint8_t CLOSED; static const uint8_t TAPERLEFT; static const uint8_t TAPERRIGHT; static const uint8_t OPENLEFT; static const uint8_t OPENRIGHT; static const uint8_t REVERSE;}; 
 
class WMBroadcaster
{
public:
  using PublishMapCallback = std::function<void(const autoware_lanelet2_msgs::msg::MapBin&)>;
  using PublishMapUpdateCallback = std::function<void(const autoware_lanelet2_msgs::msg::MapBin&)>;
  using PublishCtrlRequestCallback = std::function<void(const carma_v2x_msgs::msg::TrafficControlRequest&)>;
  using PublishActiveGeofCallback = std::function<void(const carma_perception_msgs::msg::CheckActiveGeofence&)>;
  using PublishMobilityOperationCallback  = std::function<void(const carma_v2x_msgs::msg::MobilityOperation&)>;
  
 
  WMBroadcaster(const PublishMapCallback& map_pub, const PublishMapUpdateCallback& map_update_pub, const PublishCtrlRequestCallback& control_msg_pub,
  const PublishActiveGeofCallback& active_pub, std::shared_ptr<carma_ros2_utils::timers::TimerFactory> timer_factory, const PublishMobilityOperationCallback& tcm_ack_pub);
 
  void baseMapCallback(autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg);
 
  void geoReferenceCallback(std_msgs::msg::String::UniquePtr geo_ref);
 
  void geofenceCallback(carma_v2x_msgs::msg::TrafficControlMessage::UniquePtr geofence_msg);
 
  void externalMapMsgCallback(carma_v2x_msgs::msg::MapData::UniquePtr map_msg);
 
  void addGeofence(std::shared_ptr<Geofence> gf_ptr);
 
  void removeGeofence(std::shared_ptr<Geofence> gf_ptr);
  
  void routeCallbackMessage(carma_planning_msgs::msg::Route::UniquePtr route_msg);
 
  visualization_msgs::msg::Marker composeTCMMarkerVisualizer(const std::vector<lanelet::Point3d>& input);
 
  carma_v2x_msgs::msg::TrafficControlRequestPolygon composeTCRStatus(const lanelet::BasicPoint3d& localPoint, const carma_v2x_msgs::msg::TrafficControlBounds& cB, const lanelet::projection::LocalFrameProjector& local_projector);
 
  carma_v2x_msgs::msg::TrafficControlRequest controlRequestFromRoute(const carma_planning_msgs::msg::Route& route_msg, std::shared_ptr<j2735_v2x_msgs::msg::Id64b> req_id_for_testing = NULL);
 
  lanelet::Points3d getPointsInLocalFrame(const carma_v2x_msgs::msg::TrafficControlMessageV01& geofence_msg);
  
  lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& gf_pts);
 
  void setMaxLaneWidth(double max_lane_width);
 
  void setIntersectionCoordCorrection(const std::vector<int64_t>& intersection_ids_for_correction, const std::vector<double>& intersection_correction);
 
  void setConfigSpeedLimit(double cL);
 
  void setVehicleParticipationType(std::string participant);
 
  std::string getVehicleParticipationType();
 
  void geofenceFromMsg(std::shared_ptr<Geofence> gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01& geofence_msg);
 
  std::vector<std::shared_ptr<Geofence>> geofenceFromMapMsg(std::shared_ptr<Geofence> gf_ptr, const carma_v2x_msgs::msg::MapData& map_msg);
 
  double distToNearestActiveGeofence(const lanelet::BasicPoint2d& curr_pos);
 
 
  void currentLocationCallback(geometry_msgs::msg::PoseStamped::UniquePtr current_pos);
  carma_perception_msgs::msg::CheckActiveGeofence checkActiveGeofenceLogic(const geometry_msgs::msg::PoseStamped& current_pos);
  void addRegionAccessRule(std::shared_ptr<Geofence> gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01, const std::vector<lanelet::Lanelet>& affected_llts) const;
  void addRegionMinimumGap(std::shared_ptr<Geofence> gf_ptr,  const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01, double min_gap, const std::vector<lanelet::Lanelet>& affected_llts, const std::vector<lanelet::Area>& affected_areas) const;
 
  std::vector<std::string> participantsChecker(const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01) const;
 
  std::vector<std::string> invertParticipants(const std::vector<std::string>& input_participants) const;
 
  std::vector<std::string> combineParticipantsToVehicle(const std::vector<std::string>& input_participants) const;
 
  carma_planning_msgs::msg::Route getRoute();
 
  std::shared_ptr<Geofence> createWorkzoneGeofence(std::unordered_map<uint8_t, std::shared_ptr<Geofence>> work_zone_geofence_cache);
 
  void preprocessWorkzoneGeometry(std::unordered_map<uint8_t, std::shared_ptr<Geofence>> work_zone_geofence_cache, std::shared_ptr<std::vector<lanelet::Lanelet>> parallel_llts, 
                                                    std::shared_ptr<std::vector<lanelet::Lanelet>> opposite_llts);
 
  std::shared_ptr<Geofence> createWorkzoneGeometry(std::unordered_map<uint8_t, std::shared_ptr<Geofence>> work_zone_geofence_cache, lanelet::Lanelet parallel_llt_front,  lanelet::Lanelet parallel_llt_back, 
                                                    std::shared_ptr<std::vector<lanelet::Lanelet>> middle_opposite_lanelets);
 
  std::vector<lanelet::Lanelet> splitLaneletWithPoint(const std::vector<lanelet::BasicPoint2d>& input_pts, const lanelet::Lanelet& input_llt, double error_distance);
 
  lanelet::Lanelets splitOppositeLaneletWithPoint(std::shared_ptr<std::vector<lanelet::Lanelet>> opposite_llts, const lanelet::BasicPoint2d& input_pt, const lanelet::Lanelet& input_llt, double error_distance);
 
  std::vector<lanelet::Lanelet> splitLaneletWithRatio(std::vector<double> ratios, lanelet::Lanelet input_lanelet, double error_distance) const;
  
  uint32_t generate32BitId(const std::string& label);
 
  bool convertLightIdToInterGroupId(unsigned& intersection_id, unsigned& group_id, const lanelet::Id& lanelet_id);
 
  void setErrorDistance (double error_distance);
  
  void publishLightId();
  
  void setConfigVehicleId(const std::string& vehicle_id);
 
  void setConfigACKPubTimes(int ack_pub_times);
 
  void pubTCMACK(j2735_v2x_msgs::msg::Id64b tcm_req_id, uint16_t msgnum, int ack_status, const std::string& ack_reason);
 
 
  void updateUpcomingSGIntersectionIds();
 
  visualization_msgs::msg::MarkerArray tcm_marker_array_;
  carma_v2x_msgs::msg::TrafficControlRequestPolygon tcr_polygon_;
  std_msgs::msg::Int32MultiArray upcoming_intersection_ids_;
 
private:
  double error_distance_ = 5; //meters
  lanelet::ConstLanelets route_path_;
  std::unordered_set<lanelet::Id> active_geofence_llt_ids_; 
  std::unordered_map<uint8_t, std::shared_ptr<Geofence>> work_zone_geofence_cache_;
  std::unordered_map<uint32_t, lanelet::Id> traffic_light_id_lookup_;
  void addRegulatoryComponent(std::shared_ptr<Geofence> gf_ptr) const;
  void addBackRegulatoryComponent(std::shared_ptr<Geofence> gf_ptr) const;
  void removeGeofenceHelper(std::shared_ptr<Geofence> gf_ptr) const;
  void addGeofenceHelper(std::shared_ptr<Geofence> gf_ptr);
  bool shouldChangeControlLine(const lanelet::ConstLaneletOrArea& el,const lanelet::RegulatoryElementConstPtr& regem, std::shared_ptr<Geofence> gf_ptr) const;
  bool shouldChangeTrafficSignal(const lanelet::ConstLaneletOrArea& el,const lanelet::RegulatoryElementConstPtr& regem, std::shared_ptr<carma_wm::SignalizedIntersectionManager> sim) const;
  void addPassingControlLineFromMsg(std::shared_ptr<Geofence> gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01, const std::vector<lanelet::Lanelet>& affected_llts) const; 
  void addScheduleFromMsg(std::shared_ptr<Geofence> gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01);
  void scheduleGeofence(std::shared_ptr<carma_wm_ctrl::Geofence> gf_ptr_list);
  lanelet::LineString3d createLinearInterpolatingLinestring(const lanelet::Point3d& front_pt, const lanelet::Point3d& back_pt, double increment_distance = 0.25);
  lanelet::Lanelet  createLinearInterpolatingLanelet(const lanelet::Point3d& left_front_pt, const lanelet::Point3d& right_front_pt, 
                                                      const lanelet::Point3d& left_back_pt, const lanelet::Point3d& right_back_pt, double increment_distance = 0.25);
  std::unordered_set<lanelet::Lanelet> filterSuccessorLanelets(const std::unordered_set<lanelet::Lanelet>& possible_lanelets, const std::unordered_set<lanelet::Lanelet>& root_lanelets);
  
  lanelet::LaneletMapPtr base_map_;
  lanelet::LaneletMapPtr current_map_;
  lanelet::routing::RoutingGraphPtr current_routing_graph_; // Current map routing graph
  lanelet::Velocity config_limit;
  std::string participant_ = lanelet::Participants::VehicleCar;//Default participant type
  std::unordered_set<std::string>  checked_geofence_ids_;
  std::unordered_set<std::string>  generated_geofence_reqids_;
  std::vector<lanelet::LaneletMapPtr> cached_maps_;
  std::mutex map_mutex_;
  PublishMapCallback map_pub_;
  PublishMapUpdateCallback map_update_pub_;
  PublishCtrlRequestCallback control_msg_pub_;
  PublishActiveGeofCallback active_pub_;
  GeofenceScheduler scheduler_;
  PublishMobilityOperationCallback tcm_ack_pub_;
  std::string base_map_georef_;
  double max_lane_width_;
  std::vector<carma_v2x_msgs::msg::TrafficControlMessageV01> workzone_remaining_msgs_;
  bool workzone_geometry_published_ = false;
  /* Version ID of the current_map_ variable. Monotonically increasing value
   * NOTE: This parameter needs to be incremented any time a new map is ready to be published. 
   * It should not be incremented for updates that do not require a full map publication.
   */
  size_t current_map_version_ = 0;
 
  carma_planning_msgs::msg::Route current_route; // Most recently received route message
  std::vector<autoware_lanelet2_msgs::msg::MapBin> map_update_message_queue_; 
 
  size_t update_count_ = -1; // Records the total number of sent map updates. Used as the set value for update.seq_id
 
  std::shared_ptr<carma_wm::SignalizedIntersectionManager> sim_;
 
  enum class AcknowledgementStatus {
    ACKNOWLEDGED = 1,
    REJECTED = 2
  };
  const std::string geofence_ack_strategy_ = "carma3/Geofence_Acknowledgement";
  int ack_pub_times_ = 1;
  std::string vehicle_id_;
};
 
 
}  // namespace carma_wm_ctrl