CARMAWorldModel.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 "carma_wm/WorldModel.hpp"
#include <lanelet2_extension/traffic_rules/CarmaUSTrafficRules.h>
#include <lanelet2_core/primitives/BasicRegulatoryElements.h>
#include <lanelet2_core/primitives/LineString.h>
#include "carma_wm/IndexedDistanceMap.hpp"
#include <carma_perception_msgs/msg/roadway_obstacle.hpp>
#include <carma_perception_msgs/msg/roadway_obstacle_list.hpp>
#include <carma_perception_msgs/msg/external_object.hpp>
#include <carma_perception_msgs/msg/external_object_list.hpp>
#include <carma_v2x_msgs/msg/spat.hpp>
#include "carma_wm/TrackPos.hpp"
#include "carma_wm/WorldModelUtils.hpp"
#include <lanelet2_extension/time/TimeConversion.h>
#include "boost/date_time/posix_time/posix_time.hpp"
#include "carma_wm/SignalizedIntersectionManager.hpp"
#include <rosgraph_msgs/msg/clock.hpp>
#include <gtest/gtest_prod.h>
namespace carma_wm
{
class CARMAWorldModel : public WorldModel
{
public:
  CARMAWorldModel() = default;
 
  ~CARMAWorldModel() = default;
 
  void setMap(lanelet::LaneletMapPtr map, size_t map_version = 0, bool recompute_routing_graph = true);
 
  void setRoutingGraph(LaneletRoutingGraphPtr graph);
 
  void setRoute(LaneletRoutePtr route);
 
  void setTrafficLightIds(uint32_t id, lanelet::Id lanelet_id);
 
  lanelet::LaneletMapPtr getMutableMap() const;
 
  void setRoadwayObjects(const std::vector<carma_perception_msgs::msg::RoadwayObstacle>& rw_objs);
 
  void processSpatFromMsg(const carma_v2x_msgs::msg::SPAT& spat_msg, bool use_sim_time = false, bool is_spat_wall_time=true);
 
  lanelet::Optional<std::tuple<TrackPos,carma_perception_msgs::msg::RoadwayObstacle>> getNearestObjInLane(const lanelet::BasicPoint2d& object_center, const LaneSection& section = LANE_AHEAD) const;
 
  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 is_spat_wall_time=false);
 
  void setConfigSpeedLimit(double config_lim);
 
  void setVehicleParticipationType(const std::string& participant);
 
  std::string getVehicleParticipationType();
 
  void setRouteEndPoint(const lanelet::BasicPoint3d& end_point);
 
  void setRouteName(const std::string& route_name);
 
  void setRos1Clock(const rclcpp::Time& time_now);
 
  void setSimulationClock(const rclcpp::Time& time_now);
 
  lanelet::Id getTrafficSignalId(uint16_t intersection_id,uint8_t signal_id);
 
  lanelet::CarmaTrafficSignalPtr getTrafficSignal(const lanelet::Id& id) const;
 
  std::vector<lanelet::Lanelet> getLaneletsFromPoint(const lanelet::BasicPoint2d& point, const unsigned int n);
 
  std::vector<lanelet::Lanelet> nonConnectedAdjacentLeft(const lanelet::BasicPoint2d& input_point, const unsigned int n = 10);
 
  // Overrides
  std::pair<TrackPos, TrackPos> routeTrackPos(const lanelet::ConstArea& area) const override;
 
  TrackPos routeTrackPos(const lanelet::ConstLanelet& lanelet) const override;
 
  TrackPos routeTrackPos(const lanelet::BasicPoint2d& point) const override;
 
  std::vector<lanelet::ConstLanelet> getLaneletsBetween(double start, double end, bool shortest_path_only = false,  bool bounds_inclusive = true) const override;
 
  std::vector<lanelet::BasicPoint2d> sampleRoutePoints(double start_downtrack, double end_downtrack, double step_size) const override;
 
  boost::optional<lanelet::BasicPoint2d> pointFromRouteTrackPos(const TrackPos& route_pos) const override;
 
  lanelet::LaneletMapConstPtr getMap() const override;
 
  LaneletRouteConstPtr getRoute() const override;
 
  std::string getRouteName() const override;
 
  TrackPos getRouteEndTrackPos() const override;
 
  LaneletRoutingGraphConstPtr getMapRoutingGraph() const override;
 
  lanelet::Optional<TrafficRulesConstPtr>
  getTrafficRules(const std::string& participant) const override;
 
  lanelet::Optional<TrafficRulesConstPtr>
  getTrafficRules() const override;
 
  std::vector<carma_perception_msgs::msg::RoadwayObstacle> getRoadwayObjects() const override;
 
  std::vector<carma_perception_msgs::msg::RoadwayObstacle> getInLaneObjects(const lanelet::ConstLanelet& lanelet, const LaneSection& section = LANE_AHEAD) const override;
 
  lanelet::Optional<lanelet::Lanelet> getIntersectingLanelet (const carma_perception_msgs::msg::ExternalObject& object) const override;
 
  lanelet::Optional<carma_perception_msgs::msg::RoadwayObstacle> toRoadwayObstacle(const carma_perception_msgs::msg::ExternalObject& object) const override;
 
  lanelet::Optional<double> distToNearestObjInLane(const lanelet::BasicPoint2d& object_center) const override;
 
  lanelet::Optional<std::tuple<TrackPos,carma_perception_msgs::msg::RoadwayObstacle>> nearestObjectAheadInLane(const lanelet::BasicPoint2d& object_center) const override;
 
  lanelet::Optional<std::tuple<TrackPos,carma_perception_msgs::msg::RoadwayObstacle>> nearestObjectBehindInLane(const lanelet::BasicPoint2d& object_center) const override;
 
  std::vector<lanelet::ConstLanelet> getLane(const lanelet::ConstLanelet& lanelet, const LaneSection& section = LANE_AHEAD) const override;
 
  size_t getMapVersion() const override;
 
  std::vector<lanelet::ConstLanelet> getLaneletsFromPoint(const lanelet::BasicPoint2d& point, const unsigned int n = 10) const override;
 
  std::vector<lanelet::ConstLanelet> nonConnectedAdjacentLeft(const lanelet::BasicPoint2d& input_point, const unsigned int n = 10) const override;
 
  std::vector<lanelet::CarmaTrafficSignalPtr> getSignalsAlongRoute(const lanelet::BasicPoint2d& loc) const override;
 
  std::vector<lanelet::BusStopRulePtr> getBusStopsAlongRoute(const lanelet::BasicPoint2d& loc) const override;
 
  boost::optional<std::pair<lanelet::ConstLanelet, lanelet::ConstLanelet>> getEntryExitOfSignalAlongRoute(const lanelet::CarmaTrafficSignalPtr& traffic_signal) const override;
 
  std::vector<std::shared_ptr<lanelet::AllWayStop>> getIntersectionsAlongRoute(const lanelet::BasicPoint2d& loc) const override;
 
  std::vector<lanelet::SignalizedIntersectionPtr> getSignalizedIntersectionsAlongRoute(const lanelet::BasicPoint2d &loc) const;
 
  std::optional<lanelet::ConstLanelet> getFirstLaneletOnShortestPath(const std::vector<lanelet::ConstLanelet>& lanelets_to_filter) const;
 
  std::unordered_map<uint32_t, lanelet::Id> traffic_light_ids_;
 
  carma_wm::SignalizedIntersectionManager sim_; // records SPAT/MAP lane ids to lanelet ids
 
private:
 
  double config_speed_limit_;
 
  std::string participant_type_ = lanelet::Participants::Vehicle;
 
  void computeDowntrackReferenceLine();
 
  lanelet::LineString3d copyConstructLineString(const lanelet::ConstLineString3d& line) const;
 
  std::optional<rclcpp::Time> ros1_clock_ = std::nullopt;
  std::optional<rclcpp::Time> simulation_clock_ = std::nullopt;
 
  std::shared_ptr<lanelet::LaneletMap> semantic_map_;
  LaneletRoutePtr route_;
  LaneletRoutingGraphPtr map_routing_graph_;
  double route_length_ = 0;
  lanelet::LaneletSubmapConstUPtr shortest_path_view_;  // Map containing only lanelets along the shortest path of the
                                                     // route
  std::vector<lanelet::LineString3d> shortest_path_centerlines_;  // List of disjoint centerlines seperated by lane
                                                                  // changes along the shortest path
  IndexedDistanceMap shortest_path_distance_map_;
  lanelet::LaneletMapUPtr shortest_path_filtered_centerline_view_;  // Lanelet map view of shortest path center lines
                                                                    // only
  std::vector<carma_perception_msgs::msg::RoadwayObstacle> roadway_objects_; //
 
  size_t map_version_ = 0; // The current map version. This is cached from calls to setMap();
 
  std::string route_name_; // The current route name. This is set from calls to setRouteName();
 
  // The following constants are default timining plans for recieved traffic lights.
  // The light is assumed to use these values until otherwise known
  // TODO can these be optional parameters?
  static constexpr double RED_LIGHT_DURATION = 20.0; //in sec
  static constexpr double YELLOW_LIGHT_DURATION = 3.0; //in sec
  static constexpr double GREEN_LIGHT_DURATION = 20.0; //in sec
 
  FRIEND_TEST(CARMAWorldModelTest, getFirstLaneletOnShortestPath);
};
}  // namespace carma_wm