reference_line.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 <string>
#include <utility>
#include <vector>
 
#include "modules/common_msgs/basic_msgs/pnc_point.pb.h"
#include "modules/common_msgs/map_msgs/map.pb.h"
#include "modules/common_msgs/map_msgs/map_geometry.pb.h"
#include "modules/common_msgs/planning_msgs/sl_boundary.pb.h"
#include "modules/common_msgs/routing_msgs/routing.pb.h"
 
#include "modules/common/math/vec2d.h"
#include "modules/map/pnc_map/path.h"
#include "modules/planning/planning_base/reference_line/reference_point.h"
 
namespace apollo {
namespace planning {
 
class ReferenceLine {
 public:
  ReferenceLine() = default;
  explicit ReferenceLine(const ReferenceLine& reference_line) = default;
  template <typename Iterator>
  ReferenceLine(const Iterator begin, const Iterator end)
      : reference_points_(begin, end),
        map_path_(std::move(std::vector<hdmap::MapPathPoint>(begin, end))) {}
  explicit ReferenceLine(const std::vector<ReferencePoint>& reference_points);
  explicit ReferenceLine(const hdmap::Path& hdmap_path);
 
  bool Stitch(const ReferenceLine& other);
 
  bool Segment(const common::math::Vec2d& point, const double distance_backward,
               const double distance_forward);
 
  bool Segment(const double s, const double distance_backward,
               const double distance_forward);
 
  const hdmap::Path& map_path() const;
  const std::vector<ReferencePoint>& reference_points() const;
 
  ReferencePoint GetReferencePoint(const double s) const;
 
  common::FrenetFramePoint GetFrenetPoint(
      const common::PathPoint& path_point) const;
 
  std::pair<std::array<double, 3>, std::array<double, 3>> ToFrenetFrame(
      const common::TrajectoryPoint& traj_point) const;
 
  std::vector<ReferencePoint> GetReferencePoints(double start_s,
                                                 double end_s) const;
 
  size_t GetNearestReferenceIndex(const double s) const;
 
  ReferencePoint GetNearestReferencePoint(const common::math::Vec2d& xy) const;
 
  std::vector<hdmap::LaneSegment> GetLaneSegments(const double start_s,
                                                  const double end_s) const;
 
  ReferencePoint GetNearestReferencePoint(const double s) const;
 
  ReferencePoint GetReferencePoint(const double x, const double y) const;
 
  bool GetApproximateSLBoundary(const common::math::Box2d& box,
                                const double start_s, const double end_s,
                                SLBoundary* const sl_boundary) const;
  bool GetSLBoundary(const common::math::Box2d& box,
                     SLBoundary* const sl_boundary,
                     double warm_start_s = -1.0) const;
  bool GetSLBoundary(const common::math::Polygon2d& polygon,
                     SLBoundary* const sl_boundary,
                     double warm_start_s = -1.0) const;
  bool GetSLBoundary(const std::vector<common::math::Vec2d>& corners,
                     SLBoundary* const sl_boundary, double warm_start_s) const;
  bool GetSLBoundary(const hdmap::Polygon& polygon,
                     SLBoundary* const sl_boundary) const;
 
  bool SLToXY(const common::SLPoint& sl_point,
              common::math::Vec2d* const xy_point) const;
  bool XYToSL(const common::math::Vec2d& xy_point,
              common::SLPoint* const sl_point,
              double warm_start_s = -1.0) const;
 
  bool XYToSL(const double heading, const common::math::Vec2d& xy_point,
              common::SLPoint* const sl_point,
              double warm_start_s = -1.0) const;
  bool XYToSL(const common::math::Vec2d& xy_point,
              common::SLPoint* const sl_point, double hueristic_start_s,
              double hueristic_end_s) const;
 
  template <class XYPoint>
  bool XYToSL(const XYPoint& xy, common::SLPoint* const sl_point) const {
    return XYToSL(common::math::Vec2d(xy.x(), xy.y()), sl_point);
  }
 
  bool GetLaneWidth(const double s, double* const lane_left_width,
                    double* const lane_right_width) const;
 
  bool GetOffsetToMap(const double s, double* l_offset) const;
 
  bool GetRoadWidth(const double s, double* const road_left_width,
                    double* const road_right_width) const;
 
  hdmap::Road::Type GetRoadType(const double s) const;
  void GetLaneBoundaryType(
      const double s, hdmap::LaneBoundaryType::Type* const left_boundary_type,
      hdmap::LaneBoundaryType::Type* const right_boundary_type) const;
 
  void GetLaneFromS(const double s,
                    std::vector<hdmap::LaneInfoConstPtr>* lanes) const;
 
  double GetDrivingWidth(const SLBoundary& sl_boundary) const;
 
  bool IsOnLane(const common::SLPoint& sl_point) const;
  bool IsOnLane(const common::math::Vec2d& vec2d_point) const;
  template <class XYPoint>
  bool IsOnLane(const XYPoint& xy) const {
    return IsOnLane(common::math::Vec2d(xy.x(), xy.y()));
  }
  bool IsOnLane(const SLBoundary& sl_boundary) const;
 
  bool IsOnRoad(const common::SLPoint& sl_point) const;
  bool IsOnRoad(const common::math::Vec2d& vec2d_point) const;
  bool IsOnRoad(const SLBoundary& sl_boundary) const;
 
  bool IsBlockRoad(const common::math::Box2d& box2d, double gap) const;
 
  bool HasOverlap(const common::math::Box2d& box) const;
 
  double Length() const { return map_path_.length(); }
 
  std::string DebugString() const;
 
  double GetSpeedLimitFromS(const double s) const;
 
  void AddSpeedLimit(double start_s, double end_s, double speed_limit);
 
  uint32_t GetPriority() const { return priority_; }
 
  void SetPriority(uint32_t priority) { priority_ = priority; }
 
  const hdmap::Path& GetMapPath() const { return map_path_; }
 
  void SetEgoPosition(common::math::Vec2d ego_pos) { ego_position_ = ego_pos; }
 
 private:
  static ReferencePoint Interpolate(const ReferencePoint& p0, const double s0,
                                    const ReferencePoint& p1, const double s1,
                                    const double s);
  ReferencePoint InterpolateWithMatchedIndex(
      const ReferencePoint& p0, const double s0, const ReferencePoint& p1,
      const double s1, const hdmap::InterpolatedIndex& index) const;
 
  static double FindMinDistancePoint(const ReferencePoint& p0, const double s0,
                                     const ReferencePoint& p1, const double s1,
                                     const double x, const double y);
 
 private:
  struct SpeedLimit {
    double start_s = 0.0;
    double end_s = 0.0;
    double speed_limit = 0.0;  // unit m/s
    SpeedLimit() = default;
    SpeedLimit(double _start_s, double _end_s, double _speed_limit)
        : start_s(_start_s), end_s(_end_s), speed_limit(_speed_limit) {}
  };
  std::vector<SpeedLimit> speed_limit_;
  std::vector<ReferencePoint> reference_points_;
  hdmap::Path map_path_;
  uint32_t priority_ = 0;
  common::math::Vec2d ego_position_;
};
 
}  // namespace planning
}  // namespace apollo