add road_graph in prediction module

modules/prediction/common/BUILD

@@ -19,4 +19,29 @@ cc_library(
     deps = [],
 )
 
+cc_library(
+    name = "prediction_map",
+    hdrs = ["prediction_map.h"],
+    srcs = ["prediction_map.cc"],
+    deps = [
+        "@eigen//:eigen",
+        "//modules/map/hdmap:hdmap",
+        "//modules/common:macro",
+        "//modules/prediction/common:prediction_gflags",
+    ],
+
+)
+
+cc_library(
+    name = "road_graph",
+    srcs = ["road_graph.cc"],
+    hdrs = ["road_graph.h"],
+    deps = [
+        "//modules/prediction/proto:lane_graph_proto",
+        "//modules/map/hdmap:hdmap",
+        "//modules/common/proto:error_code_proto",
+        "//modules/prediction/common:prediction_map",
+    ],
+)
+
 cpplint()

modules/prediction/common/prediction_map.h

@@ -18,11 +18,13 @@
 #define MODULES_PREDICTION_COMMON_PREDICTION_MAP_H_
 
 #include <memory>
+#include <string>
 
 #include "Eigen/Dense"
 
 #include "modules/common/macro.h"
 #include "modules/map/hdmap/hdmap_impl.h"
+#include "modules/map/hdmap/hdmap_common.h"
 
 namespace apollo {
 namespace prediction {
@@ -38,6 +40,8 @@ class PredictionMap {
   Eigen::Vector2d PositionOnLane(const apollo::hdmap::LaneInfo& lane_info,
                                  const double s);
 
+  apollo::hdmap::LaneInfo* lane_by_id(const apollo::hdmap::Id& id);
+
  private:
   std::unique_ptr<apollo::hdmap::HDMapImpl> hdmap_;
   DECLARE_SINGLETON(PredictionMap);

modules/prediction/common/road_graph.cc

+/******************************************************************************
+ * 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.
+ *****************************************************************************/
+
+#include <utility>
+
+#include "modules/prediction/common/road_graph.h"
+#include "modules/prediction/common/prediction_map.h"
+
+namespace apollo {
+namespace prediction {
+
+using apollo::hdmap::LaneInfo;
+using apollo::hdmap::Id;
+using apollo::common::ErrorCode;
+
+RoadGraph::RoadGraph()
+    : start_s_(0.0), length_(-1.0), lane_info_ptr_(nullptr) {
+}
+
+RoadGraph::RoadGraph(double start_s, double length,
+                     apollo::hdmap::LaneInfo* lane_info_ptr)
+    : start_s_(start_s), length_(length), lane_info_ptr_(lane_info_ptr) {
+}
+
+RoadGraph::~RoadGraph() {
+  start_s_ = 0.0;
+  length_ = -1.0;
+  lane_info_ptr_ = nullptr;
+}
+
+void RoadGraph::Set(double start_s, double length,
+                    apollo::hdmap::LaneInfo* lane_info_ptr) {
+  start_s_ = start_s;
+  length_ = length;
+  lane_info_ptr_ = lane_info_ptr;
+}
+
+ErrorCode RoadGraph::BuildLaneGraph(LaneGraph* lane_graph_ptr) {
+  if (length_ < 0.0 || lane_info_ptr_ == nullptr) {
+    AERROR << "Invalid road graph settings. Road graph length = " << length_;
+    return ErrorCode::PREDICTION_ERROR;
+  }
+
+  if (lane_graph_ptr == nullptr) {
+    AERROR << "Invalid input lane graph.";
+    return ErrorCode::PREDICTION_ERROR;
+  }
+
+  std::vector<LaneSegment> lane_segments;
+  double accumulated_s = 0.0;
+  ComputeLaneSequence(accumulated_s, start_s_, lane_info_ptr_,
+                      &lane_segments, lane_graph_ptr);
+
+  return ErrorCode::OK;
+}
+
+void RoadGraph::ComputeLaneSequence(
+    double accumulated_s, double start_s,
+    apollo::hdmap::LaneInfo* lane_info_ptr,
+    std::vector<LaneSegment>* lane_segments,
+    LaneGraph* lane_graph_ptr) const {
+  if (lane_info_ptr == nullptr) {
+    AERROR << "Invalid lane.";
+    return;
+  }
+  PredictionMap* map = PredictionMap::instance();
+  if (map == nullptr) {
+    AERROR << "Missing map.";
+    return;
+  }
+  LaneSegment lane_segment;
+  lane_segment.set_lane_id(lane_info_ptr->id().id());
+  lane_segment.set_start_s(start_s);
+  // lane_segment.set_lane_turn_type(map->lane_turn_type(lane_info_ptr->id()));
+  if (accumulated_s + lane_info_ptr->total_length() - start_s >= length_) {
+    lane_segment.set_end_s(length_ - accumulated_s + start_s);
+  } else {
+    lane_segment.set_end_s(lane_info_ptr->total_length());
+  }
+
+  lane_segments->push_back(std::move(lane_segment));
+
+  if (accumulated_s + lane_info_ptr->total_length() - start_s >= length_ ||
+    lane_info_ptr->lane().successor_id_size() == 0) {
+    LaneSequence* sequence = lane_graph_ptr->add_lane_sequence();
+    for (auto& lane_segment : *lane_segments) {
+      sequence->add_lane_segment()->CopyFrom(lane_segment);
+    }
+    sequence->set_label(0);
+  } else {
+    for (const auto& successor_lane_id : lane_info_ptr->lane().successor_id()) {
+      double successor_accumulated_s =
+          accumulated_s + lane_info_ptr->total_length() - start_s;
+      LaneInfo* successor_lane = map->lane_by_id(successor_lane_id);
+      ComputeLaneSequence(successor_accumulated_s, 0.0, successor_lane,
+          lane_segments, lane_graph_ptr);
+    }
+  }
+  lane_segments->pop_back();
+}
+
+}  // namespace prediction
+}  // namespace apollo

modules/prediction/common/road_graph.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.
+ *****************************************************************************/
+
+#ifndef MODULES_PREDICTION_COMMON_ROAD_GRAPH_H_
+#define MODULES_PREDICTION_COMMON_ROAD_GRAPH_H_
+
+#include <vector>
+
+#include "modules/map/hdmap/hdmap_common.h"
+#include "modules/prediction/proto/lane_graph.pb.h"
+#include "modules/common/proto/error_code.pb.h"
+
+namespace apollo {
+namespace prediction {
+
+class RoadGraph {
+ public:
+  RoadGraph();
+
+  RoadGraph(double start_s, double length,
+            apollo::hdmap::LaneInfo* lane_info_ptr);
+
+  virtual ~RoadGraph();
+
+  void Set(double start_s, double length,
+           apollo::hdmap::LaneInfo* lane_info_ptr);
+
+  apollo::common::ErrorCode BuildLaneGraph(LaneGraph* lane_graph);
+
+ private:
+  void ComputeLaneSequence(double accumulated_s,
+                           double start_s,
+                           apollo::hdmap::LaneInfo* lane_info_ptr,
+                           std::vector<LaneSegment>* lane_segments,
+                           LaneGraph* lane_graph_ptr) const;
+
+ private:
+  double start_s_;
+  double length_;
+  apollo::hdmap::LaneInfo* lane_info_ptr_;
+};
+
+}  // namespace prediction
+}  // namespace apollo
+
+#endif  // MODULES_PREDICTION_COMMON_ROAD_GRAPH_H_