path.h

Go to the documentation of this file.

/******************************************************************************
 * 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/map/proto/map_lane.pb.h"

#include "cyber/common/log.h"
#include "modules/common/math/box2d.h"
#include "modules/common/math/line_segment2d.h"
#include "modules/common/math/vec2d.h"
#include "modules/map/hdmap/hdmap.h"
#include "modules/map/hdmap/hdmap_common.h"
#include "modules/map/hdmap/hdmap_util.h"

namespace apollo {
namespace hdmap {

// class LaneInfoConstPtr;
// class OverlapInfoConstPtr;

struct LaneWaypoint {
  LaneWaypoint() = default;
  LaneWaypoint(LaneInfoConstPtr lane, const double s)
      : lane(CHECK_NOTNULL(lane)), s(s) {}
  LaneWaypoint(LaneInfoConstPtr lane, const double s, const double l)
      : lane(CHECK_NOTNULL(lane)), s(s), l(l) {}
  LaneInfoConstPtr lane = nullptr;
  double s = 0.0;
  double l = 0.0;

  std::string DebugString() const;
};

LaneBoundaryType::Type LeftBoundaryType(const LaneWaypoint& waypoint);

LaneBoundaryType::Type RightBoundaryType(const LaneWaypoint& waypoint);

LaneWaypoint LeftNeighborWaypoint(const LaneWaypoint& waypoint);

LaneWaypoint RightNeighborWaypoint(const LaneWaypoint& waypoint);

struct LaneSegment {
  LaneSegment() = default;
  LaneSegment(LaneInfoConstPtr lane, const double start_s, const double end_s)
      : lane(CHECK_NOTNULL(lane)), start_s(start_s), end_s(end_s) {}
  LaneInfoConstPtr lane = nullptr;
  double start_s = 0.0;
  double end_s = 0.0;
  double Length() const { return end_s - start_s; }

  static void Join(std::vector<LaneSegment>* segments);

  std::string DebugString() const;
};

struct PathOverlap {
  PathOverlap() = default;
  PathOverlap(std::string object_id, const double start_s, const double end_s)
      : object_id(std::move(object_id)), start_s(start_s), end_s(end_s) {}

  std::string object_id;
  double start_s = 0.0;
  double end_s = 0.0;

  std::string DebugString() const;
};

class MapPathPoint : public common::math::Vec2d {
 public:
  MapPathPoint() = default;
  MapPathPoint(const common::math::Vec2d& point, double heading)
      : Vec2d(point.x(), point.y()), heading_(heading) {}
  MapPathPoint(const common::math::Vec2d& point, double heading,
               LaneWaypoint lane_waypoint)
      : Vec2d(point.x(), point.y()), heading_(heading) {
    lane_waypoints_.emplace_back(std::move(lane_waypoint));
  }
  MapPathPoint(const common::math::Vec2d& point, double heading,
               std::vector<LaneWaypoint> lane_waypoints)
      : Vec2d(point.x(), point.y()),
        heading_(heading),
        lane_waypoints_(std::move(lane_waypoints)) {}

  double heading() const { return heading_; }
  void set_heading(const double heading) { heading_ = heading; }

  const std::vector<LaneWaypoint>& lane_waypoints() const {
    return lane_waypoints_;
  }

  void add_lane_waypoint(LaneWaypoint lane_waypoint) {
    lane_waypoints_.emplace_back(std::move(lane_waypoint));
  }
  void add_lane_waypoints(const std::vector<LaneWaypoint>& lane_waypoints) {
    lane_waypoints_.insert(lane_waypoints_.end(), lane_waypoints.begin(),
                           lane_waypoints.end());
  }

  void clear_lane_waypoints() { lane_waypoints_.clear(); }

  static void RemoveDuplicates(std::vector<MapPathPoint>* points);

  static std::vector<MapPathPoint> GetPointsFromSegment(
      const LaneSegment& segment);

  static std::vector<MapPathPoint> GetPointsFromLane(LaneInfoConstPtr lane,
                                                     const double start_s,
                                                     const double end_s);

  std::string DebugString() const;

 protected:
  double heading_ = 0.0;
  std::vector<LaneWaypoint> lane_waypoints_;
};

class Path;

class PathApproximation {
 public:
  PathApproximation() = default;
  PathApproximation(const Path& path, const double max_error)
      : max_error_(max_error), max_sqr_error_(max_error * max_error) {
    Init(path);
  }
  double max_error() const { return max_error_; }
  const std::vector<int>& original_ids() const { return original_ids_; }
  const std::vector<common::math::LineSegment2d>& segments() const {
    return segments_;
  }

  bool GetProjection(const Path& path, const common::math::Vec2d& point,
                     double* accumulate_s, double* lateral,
                     double* distance) const;

  bool OverlapWith(const Path& path, const common::math::Box2d& box,
                   double width) const;

 protected:
  void Init(const Path& path);
  bool is_within_max_error(const Path& path, const int s, const int t);
  double compute_max_error(const Path& path, const int s, const int t);

  void InitDilute(const Path& path);
  void InitProjections(const Path& path);

 protected:
  double max_error_ = 0;
  double max_sqr_error_ = 0;

  int num_points_ = 0;
  std::vector<int> original_ids_;
  std::vector<common::math::LineSegment2d> segments_;
  std::vector<double> max_error_per_segment_;

  // TODO(All): use direction change checks to early stop.

  // Projection of points onto the diluated segments.
  std::vector<double> projections_;
  double max_projection_;
  int num_projection_samples_ = 0;

  // The original_projection is the projection of original points onto the
  // diluated segments.
  std::vector<double> original_projections_;
  // max_p_to_left[i] = max(p[0], p[1], ... p[i]).
  // min_p_to_right[i] = min(p[i], p[i + 1], ... p[size - 1]).
  std::vector<double> max_original_projections_to_left_;
  std::vector<double> min_original_projections_to_right_;
  std::vector<int> sampled_max_original_projections_to_left_;
};

class InterpolatedIndex {
 public:
  InterpolatedIndex(int id, double offset) : id(id), offset(offset) {}
  int id = 0;
  double offset = 0.0;
};

class Path {
 public:
  Path() = default;
  explicit Path(const std::vector<MapPathPoint>& path_points);
  explicit Path(std::vector<MapPathPoint>&& path_points);
  explicit Path(std::vector<LaneSegment>&& path_points);
  explicit Path(const std::vector<LaneSegment>& path_points);

  Path(const std::vector<MapPathPoint>& path_points,
       const std::vector<LaneSegment>& lane_segments);
  Path(std::vector<MapPathPoint>&& path_points,
       std::vector<LaneSegment>&& lane_segments);

  Path(const std::vector<MapPathPoint>& path_points,
       const std::vector<LaneSegment>& lane_segments,
       const double max_approximation_error);
  Path(std::vector<MapPathPoint>&& path_points,
       std::vector<LaneSegment>&& lane_segments,
       const double max_approximation_error);

  // Return smooth coordinate by interpolated index or accumulate_s.
  MapPathPoint GetSmoothPoint(const InterpolatedIndex& index) const;
  MapPathPoint GetSmoothPoint(double s) const;

  // Compute accumulate s value of the index.
  double GetSFromIndex(const InterpolatedIndex& index) const;
  // Compute interpolated index by accumulate_s.
  InterpolatedIndex GetIndexFromS(double s) const;

  // get the index of the lane from s by accumulate_s
  InterpolatedIndex GetLaneIndexFromS(double s) const;

  std::vector<hdmap::LaneSegment> GetLaneSegments(const double start_s,
                                                  const double end_s) const;

  bool GetNearestPoint(const common::math::Vec2d& point, double* accumulate_s,
                       double* lateral) const;
  bool GetNearestPoint(const common::math::Vec2d& point, double* accumulate_s,
                       double* lateral, double* distance) const;
  bool GetProjectionWithHueristicParams(const common::math::Vec2d& point,
                                        const double hueristic_start_s,
                                        const double hueristic_end_s,
                                        double* accumulate_s, double* lateral,
                                        double* min_distance) const;
  bool GetProjection(const common::math::Vec2d& point, double* accumulate_s,
                     double* lateral) const;
  bool GetProjection(const common::math::Vec2d& point, double* accumulate_s,
                     double* lateral, double* distance) const;

  bool GetHeadingAlongPath(const common::math::Vec2d& point,
                           double* heading) const;

  int num_points() const { return num_points_; }
  int num_segments() const { return num_segments_; }
  const std::vector<MapPathPoint>& path_points() const { return path_points_; }
  const std::vector<LaneSegment>& lane_segments() const {
    return lane_segments_;
  }
  const std::vector<LaneSegment>& lane_segments_to_next_point() const {
    return lane_segments_to_next_point_;
  }
  const std::vector<common::math::Vec2d>& unit_directions() const {
    return unit_directions_;
  }
  const std::vector<double>& accumulated_s() const { return accumulated_s_; }
  const std::vector<common::math::LineSegment2d>& segments() const {
    return segments_;
  }
  const PathApproximation* approximation() const { return &approximation_; }
  double length() const { return length_; }

  const PathOverlap* NextLaneOverlap(double s) const;

  const std::vector<PathOverlap>& lane_overlaps() const {
    return lane_overlaps_;
  }
  const std::vector<PathOverlap>& signal_overlaps() const {
    return signal_overlaps_;
  }
  const std::vector<PathOverlap>& yield_sign_overlaps() const {
    return yield_sign_overlaps_;
  }
  const std::vector<PathOverlap>& stop_sign_overlaps() const {
    return stop_sign_overlaps_;
  }
  const std::vector<PathOverlap>& crosswalk_overlaps() const {
    return crosswalk_overlaps_;
  }
  const std::vector<PathOverlap>& junction_overlaps() const {
    return junction_overlaps_;
  }
  const std::vector<PathOverlap>& pnc_junction_overlaps() const {
    return pnc_junction_overlaps_;
  }
  const std::vector<PathOverlap>& clear_area_overlaps() const {
    return clear_area_overlaps_;
  }
  const std::vector<PathOverlap>& speed_bump_overlaps() const {
    return speed_bump_overlaps_;
  }
  const std::vector<PathOverlap>& parking_space_overlaps() const {
    return parking_space_overlaps_;
  }

  double GetLaneLeftWidth(const double s) const;
  double GetLaneRightWidth(const double s) const;
  bool GetLaneWidth(const double s, double* lane_left_width,
                    double* lane_right_width) const;

  double GetRoadLeftWidth(const double s) const;
  double GetRoadRightWidth(const double s) const;
  bool GetRoadWidth(const double s, double* road_left_width,
                    double* road_ight_width) const;

  bool IsOnPath(const common::math::Vec2d& point) const;
  bool OverlapWith(const common::math::Box2d& box, double width) const;

  std::string DebugString() const;

 protected:
  void Init();
  void InitPoints();
  void InitLaneSegments();
  void InitWidth();
  void InitPointIndex();
  void InitOverlaps();

  double GetSample(const std::vector<double>& samples, const double s) const;

  using GetOverlapFromLaneFunc =
      std::function<const std::vector<OverlapInfoConstPtr>&(const LaneInfo&)>;
  void GetAllOverlaps(GetOverlapFromLaneFunc GetOverlaps_from_lane,
                      std::vector<PathOverlap>* const overlaps) const;

 protected:
  int num_points_ = 0;
  int num_segments_ = 0;
  std::vector<MapPathPoint> path_points_;
  std::vector<LaneSegment> lane_segments_;
  std::vector<double> lane_accumulated_s_;
  std::vector<LaneSegment> lane_segments_to_next_point_;
  std::vector<common::math::Vec2d> unit_directions_;
  double length_ = 0.0;
  std::vector<double> accumulated_s_;
  std::vector<common::math::LineSegment2d> segments_;
  bool use_path_approximation_ = false;
  PathApproximation approximation_;

  // Sampled every fixed length.
  int num_sample_points_ = 0;
  std::vector<double> lane_left_width_;
  std::vector<double> lane_right_width_;
  std::vector<double> road_left_width_;
  std::vector<double> road_right_width_;
  std::vector<int> last_point_index_;

  std::vector<PathOverlap> lane_overlaps_;
  std::vector<PathOverlap> signal_overlaps_;
  std::vector<PathOverlap> yield_sign_overlaps_;
  std::vector<PathOverlap> stop_sign_overlaps_;
  std::vector<PathOverlap> crosswalk_overlaps_;
  std::vector<PathOverlap> parking_space_overlaps_;
  std::vector<PathOverlap> junction_overlaps_;
  std::vector<PathOverlap> pnc_junction_overlaps_;
  std::vector<PathOverlap> clear_area_overlaps_;
  std::vector<PathOverlap> speed_bump_overlaps_;
};

}  // namespace hdmap
}  // namespace apollo