lane_follow_map.h

浏览该文件的文档.

/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * 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 <list>
#include <memory>
#include <string>
#include <unordered_set>
#include <vector>
 
#include "modules/common/vehicle_state/proto/vehicle_state.pb.h"
#include "modules/common_msgs/routing_msgs/routing.pb.h"
 
#include "cyber/plugin_manager/plugin_manager.h"
#include "modules/map/hdmap/hdmap.h"
#include "modules/map/pnc_map/path.h"
#include "modules/map/pnc_map/pnc_map_base.h"
#include "modules/map/pnc_map/route_segments.h"
 
namespace apollo {
namespace planning {
 
class LaneFollowMap : public PncMapBase {
 public:
  LaneFollowMap();
 
  virtual ~LaneFollowMap() = default;
 
  bool CanProcess(const planning::PlanningCommand &command) const override;
 
  bool UpdatePlanningCommand(const planning::PlanningCommand &command) override;
  bool GetRouteSegments(
      const common::VehicleState &vehicle_state,
      std::list<apollo::hdmap::RouteSegments> *const route_segments) override;
 
  bool ExtendSegments(
      const apollo::hdmap::RouteSegments &segments, double start_s,
      double end_s,
      apollo::hdmap::RouteSegments *const truncated_segments) const override;
 
  std::vector<routing::LaneWaypoint> FutureRouteWaypoints() const override;
  void GetEndLaneWayPoint(
      std::shared_ptr<routing::LaneWaypoint> &end_point) const override;
  hdmap::LaneInfoConstPtr GetLaneById(const hdmap::Id &id) const override;
 
  bool GetNearestPointFromRouting(
      const common::VehicleState &state,
      apollo::hdmap::LaneWaypoint *waypoint) const override;
  
  double GetDistanceToDestination() const override;
  apollo::hdmap::LaneWaypoint GetAdcWaypoint() const override;
 
 private:
  bool IsValid(const planning::PlanningCommand &command) const override;
 
  bool GetRouteSegments(
      const common::VehicleState &vehicle_state, const double backward_length,
      const double forward_length,
      std::list<apollo::hdmap::RouteSegments> *const route_segments);
 
  bool ExtendSegments(const apollo::hdmap::RouteSegments &segments,
                      const common::PointENU &point, double look_forward,
                      double look_backward,
                      apollo::hdmap::RouteSegments *extended_segments);
 
  bool UpdateVehicleState(const common::VehicleState &vehicle_state);
  int GetWaypointIndex(const apollo::hdmap::LaneWaypoint &waypoint) const;
 
  bool PassageToSegments(routing::Passage passage,
                         apollo::hdmap::RouteSegments *segments) const;
 
  bool ProjectToSegments(const common::PointENU &point_enu,
                         const apollo::hdmap::RouteSegments &segments,
                         apollo::hdmap::LaneWaypoint *waypoint) const;
 
  static void AppendLaneToPoints(
      apollo::hdmap::LaneInfoConstPtr lane, const double start_s,
      const double end_s,
      std::vector<apollo::hdmap::MapPathPoint> *const points);
 
  apollo::hdmap::LaneInfoConstPtr GetRoutePredecessor(
      apollo::hdmap::LaneInfoConstPtr lane) const;
  apollo::hdmap::LaneInfoConstPtr GetRouteSuccessor(
      apollo::hdmap::LaneInfoConstPtr lane) const;
 
  std::vector<int> GetNeighborPassages(const routing::RoadSegment &road,
                                       int start_passage) const;
 
  apollo::hdmap::LaneWaypoint ToLaneWaypoint(
      const routing::LaneWaypoint &waypoint) const;
 
  apollo::hdmap::LaneSegment ToLaneSegment(
      const routing::LaneSegment &segment) const;
 
  void UpdateNextRoutingWaypointIndex(int cur_index);
 
  int SearchForwardWaypointIndex(
      int start, const apollo::hdmap::LaneWaypoint &waypoint) const;
  int SearchBackwardWaypointIndex(
      int start, const apollo::hdmap::LaneWaypoint &waypoint) const;
 
  void UpdateRoutingRange(int adc_index);
  void UpdateRouteSegmentsLaneIds(
      const std::list<hdmap::RouteSegments> *route_segments);
 
 private:
  struct RouteIndex {
    apollo::hdmap::LaneSegment segment;
    std::array<int, 3> index;
  };
  std::vector<RouteIndex> route_indices_;
  int range_start_ = 0;
  int range_end_ = 0;
  // routing ids in range
  std::unordered_set<std::string> range_lane_ids_;
  std::unordered_set<std::string> all_lane_ids_;
  std::unordered_set<std::string> route_segments_lane_ids_;
  routing::LaneSegment dest_lane_segment_;
 
  struct WaypointIndex {
    apollo::hdmap::LaneWaypoint waypoint;
    int index;
    WaypointIndex(const apollo::hdmap::LaneWaypoint &waypoint, int index)
        : waypoint(waypoint), index(index) {}
  };
 
  // return the segment of an index
  int NextWaypointIndex(int index) const;
 
  std::vector<WaypointIndex> routing_waypoint_index_;
  std::size_t next_routing_waypoint_index_ = 0;
 
  const hdmap::HDMap *hdmap_ = nullptr;
  bool is_same_routing_ = false;
 
  common::VehicleState adc_state_;
  int adc_route_index_ = -1;
  bool stop_for_destination_ = false;
};
 
CYBER_PLUGIN_MANAGER_REGISTER_PLUGIN(apollo::planning::LaneFollowMap,
                                     PncMapBase)
 
}  // namespace planning
}  // namespace apollo