WorldModel.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 <exception>
#include <memory>
#include <tuple>
#include <optional>
#include <lanelet2_core/LaneletMap.h>
#include <lanelet2_core/primitives/Area.h>
#include <lanelet2_core/primitives/Lanelet.h>
#include <lanelet2_core/primitives/Point.h>
#include <lanelet2_routing/Route.h>
#include <lanelet2_routing/RoutingGraph.h>
#include <lanelet2_traffic_rules/TrafficRules.h>
#include <lanelet2_core/utility/Optional.h>
#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 <lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h>
#include <lanelet2_extension/regulatory_elements/SignalizedIntersection.h>
#include <lanelet2_core/primitives/BasicRegulatoryElements.h>
#include "carma_wm/TrackPos.hpp"
#include <lanelet2_extension/regulatory_elements/BusStopRule.h>
 
 
 
namespace carma_wm
{
 
// Helpful using declarations which are not defined by lanelet::routing module
using LaneletRoutePtr = std::shared_ptr<lanelet::routing::Route>;
using LaneletRouteConstPtr = std::shared_ptr<const lanelet::routing::Route>;
using LaneletRouteUPtr = std::unique_ptr<lanelet::routing::Route>;
using LaneletRouteUConstPtr = std::unique_ptr<const lanelet::routing::Route>;
 
using LaneletRoutingGraphPtr = std::shared_ptr<lanelet::routing::RoutingGraph>;
using LaneletRoutingGraphConstPtr = std::shared_ptr<const lanelet::routing::RoutingGraph>;
using LaneletRoutingGraphUPtr = std::unique_ptr<lanelet::routing::RoutingGraph>;
using LaneletRoutingGraphConstUPtr = std::unique_ptr<const lanelet::routing::RoutingGraph>;
 
using TrafficRulesConstPtr = std::shared_ptr<const lanelet::traffic_rules::TrafficRules>;
using TrafficRulesUConstPtr = std::unique_ptr<const lanelet::traffic_rules::TrafficRules>;
 
// Helpful enums for dividing lane into sections of interest.
enum LaneSection
{
  LANE_AHEAD,
  LANE_BEHIND,
  LANE_FULL
};
 
class WorldModel
{
  public:
    virtual ~WorldModel() {};
 
    virtual std::pair<TrackPos, TrackPos> routeTrackPos(const lanelet::ConstArea& area) const = 0;
 
 
    virtual TrackPos routeTrackPos(const lanelet::ConstLanelet& lanelet) const = 0;
 
    virtual TrackPos routeTrackPos(const lanelet::BasicPoint2d& point) const = 0;
 
    virtual std::vector<lanelet::ConstLanelet> getLaneletsBetween(double start, double end, bool shortest_path_only = false,
                                                                bool bounds_inclusive = true) const = 0;
 
    virtual std::vector<lanelet::BasicPoint2d> sampleRoutePoints(double start_downtrack, double end_downtrack,
                                                               double step_size) const = 0;
 
 
    virtual boost::optional<lanelet::BasicPoint2d> pointFromRouteTrackPos(const TrackPos& route_pos) const = 0;
 
    virtual lanelet::LaneletMapConstPtr getMap() const = 0;
 
    virtual LaneletRouteConstPtr getRoute() const = 0;
 
    virtual std::string getRouteName() const = 0;
 
    virtual TrackPos getRouteEndTrackPos() const = 0;
 
    virtual LaneletRoutingGraphConstPtr getMapRoutingGraph() const = 0;
 
    virtual std::vector<carma_perception_msgs::msg::RoadwayObstacle> getRoadwayObjects() const = 0;
 
    virtual lanelet::Optional<TrafficRulesConstPtr>
    getTrafficRules(const std::string& participant) const = 0;
 
    virtual lanelet::Optional<TrafficRulesConstPtr>
    getTrafficRules() const = 0;
 
    virtual lanelet::Optional<carma_perception_msgs::msg::RoadwayObstacle>
    toRoadwayObstacle(const carma_perception_msgs::msg::ExternalObject& object) const = 0;
 
    virtual lanelet::Optional<lanelet::Lanelet> getIntersectingLanelet(const carma_perception_msgs::msg::ExternalObject& object) const = 0;
 
    virtual std::vector<lanelet::BusStopRulePtr> getBusStopsAlongRoute(const lanelet::BasicPoint2d& loc) const = 0;
 
    virtual std::vector<carma_perception_msgs::msg::RoadwayObstacle> getInLaneObjects(const lanelet::ConstLanelet& lanelet,
                                                                  const LaneSection& section = LANE_AHEAD) const = 0;
 
    virtual lanelet::Optional<double> distToNearestObjInLane(const lanelet::BasicPoint2d& object_center) const = 0;
 
    virtual lanelet::Optional<std::tuple<TrackPos, carma_perception_msgs::msg::RoadwayObstacle>>
    nearestObjectAheadInLane(const lanelet::BasicPoint2d& object_center) const = 0;
 
    virtual lanelet::Optional<std::tuple<TrackPos, carma_perception_msgs::msg::RoadwayObstacle>>
    nearestObjectBehindInLane(const lanelet::BasicPoint2d& object_center) const = 0;
 
    virtual std::vector<lanelet::ConstLanelet> getLane(const lanelet::ConstLanelet& lanelet,
                                                     const LaneSection& section = LANE_AHEAD) const = 0;
 
    virtual size_t getMapVersion() const = 0;
 
    virtual std::vector<lanelet::ConstLanelet> getLaneletsFromPoint(const lanelet::BasicPoint2d& point, const unsigned int n = 10) const = 0;
 
    virtual std::vector<lanelet::CarmaTrafficSignalPtr> getSignalsAlongRoute(const lanelet::BasicPoint2d& loc) const = 0;
 
  virtual boost::optional<std::pair<lanelet::ConstLanelet, lanelet::ConstLanelet>> getEntryExitOfSignalAlongRoute(const lanelet::CarmaTrafficSignalPtr& traffic_signal) const = 0;
 
    virtual std::vector<std::shared_ptr<lanelet::AllWayStop>> getIntersectionsAlongRoute(const lanelet::BasicPoint2d& loc) const = 0;
 
    virtual std::vector<lanelet::SignalizedIntersectionPtr> getSignalizedIntersectionsAlongRoute(const lanelet::BasicPoint2d &loc) const = 0;
 
    virtual std::vector<lanelet::ConstLanelet> nonConnectedAdjacentLeft(const lanelet::BasicPoint2d& input_point, const unsigned int n = 10) const = 0;
 
    virtual std::optional<lanelet::ConstLanelet> getFirstLaneletOnShortestPath(const std::vector<lanelet::ConstLanelet>& lanelets_to_filter) const = 0;
 
};
// Helpful using declarations for carma_wm classes
using WorldModelConstPtr = std::shared_ptr<const WorldModel>;
}  // namespace carma_wm