navigation_lane.h

Go to the documentation of this file.

/******************************************************************************
 * Copyright 2018 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 <tuple>
#include <unordered_map>
#include <utility>

#include "modules/common/vehicle_state/proto/vehicle_state.pb.h"
#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/localization/proto/localization.pb.h"
#include "modules/map/relative_map/proto/navigation.pb.h"
#include "modules/map/relative_map/proto/relative_map_config.pb.h"
#include "modules/perception/proto/perception_obstacle.pb.h"

namespace apollo {
namespace relative_map {

// A navigation path tuple.
//
// first element: original navigation line index of the current navigation path.
// A negative value indicates illegal.
//
// second element: half of the lateral distance to the left adjacent navigation
// path, that is, the left width of the lane generated by this navigation path.
// If the navigation path is generated based on lane markers, the value is the
// perceived left lane width. If there is no left adjacent navigation path, the
// value is "default_left_width_". A negative value indicates illegal.
//
// third element: half of the lateral distance to the right adjacent navigation
// path, that is, the right width of the lane generated by this navigation path.
// If the navigation path is generated based on lane markers, the value is the
// perceived right lane width. If there is no right adjacent navigation path,
// the value is "default_right_width_". A negative value indicates illegal.
//
// fourth element : a shared pointer of the current navigation path.
typedef std::tuple<int, double, double, std::shared_ptr<NavigationPath>>
    NaviPathTuple;

// A stitching index pair.
// pair.first: the start stitching index of the current navigation line.
// pair.second: the end stitching index of the current navigation line.
typedef std::pair<int, int> StitchIndexPair;

// A projection index pair.
// pair.first: projection index of the vehicle in the current navigation line.
// pair.second: the distance between the vehicle's initial position and the
// projection position in the current navigation line.
typedef std::pair<int, double> ProjIndexPair;

class NavigationLane {
 public:
  NavigationLane() = default;
  explicit NavigationLane(const NavigationLaneConfig& config);
  ~NavigationLane() = default;

  void SetConfig(const NavigationLaneConfig& config);

  void SetVehicleStateProvider(
      common::VehicleStateProvider* vehicle_state_provider);

  void UpdateNavigationInfo(const NavigationInfo& navigation_info);

  void SetDefaultWidth(const double left_width, const double right_width) {
    default_left_width_ = left_width;
    default_right_width_ = right_width;
  }

  bool GeneratePath();

  void UpdatePerception(
      const perception::PerceptionObstacles& perception_obstacles) {
    perception_obstacles_ = perception_obstacles;
  }

  NavigationPath Path() const {
    const auto& current_navi_path = std::get<3>(current_navi_path_tuple_);
    if (current_navi_path) {
      return *current_navi_path;
    }
    return NavigationPath();
  }

  bool CreateMap(const MapGenerationParam& map_config,
                 MapMsg* const map_msg) const;

 private:
  double EvaluateCubicPolynomial(const double c0, const double c1,
                                 const double c2, const double c3,
                                 const double x) const;

  double GetKappa(const double c1, const double c2, const double c3,
                  const double x);

  common::PathPoint GetPathPointByS(const common::Path& path,
                                    const int start_index, const double s,
                                    int* const matched_index);

  void ConvertLaneMarkerToPath(const perception::LaneMarkers& lane_marker,
                               common::Path* const path);

  bool ConvertNavigationLineToPath(const int line_index,
                                   common::Path* const path);

  void MergeNavigationLineAndLaneMarker(const int line_index,
                                        common::Path* const path);

  ProjIndexPair UpdateProjectionIndex(const common::Path& path,
                                      const int line_index);

  void UpdateStitchIndexInfo();

 private:
  // the configuration information required by the `NavigationLane`
  NavigationLaneConfig config_;

  // received from topic: /apollo/perception_obstacles
  perception::PerceptionObstacles perception_obstacles_;

  // received from topic: /apollo/navigation
  NavigationInfo navigation_info_;

  // navigation_path_list_ is a list of navigation paths. The internal paths
  // are arranged from left to right based on the vehicle's driving direction.
  // A navigation path is the combined results from perception and navigation.
  std::list<NaviPathTuple> navigation_path_list_;

  // the navigation path which the vehicle is currently on.
  NaviPathTuple current_navi_path_tuple_;

  // when invalid, left_width_ < 0
  double perceived_left_width_ = -1.0;

  // when invalid, right_width_ < 0
  double perceived_right_width_ = -1.0;

  // The standard lane width of China's expressway is 3.75 meters.
  double default_left_width_ = 1.875;
  double default_right_width_ = 1.875;

  // key: line index,
  // value: last projection index pair in the "key" line.
  std::unordered_map<int, ProjIndexPair> last_project_index_map_;

  // key: line index,
  // value: stitching index pair in the "key" line.
  std::unordered_map<int, StitchIndexPair> stitch_index_map_;

  // in world coordination: ENU
  localization::Pose original_pose_;
  common::VehicleStateProvider* vehicle_state_provider_ = nullptr;
};

}  // namespace relative_map
}  // namespace apollo