diff --git a/modules/prediction/common/BUILD b/modules/prediction/common/BUILD index 197cfff1d47afda7defa75177017ef98e006b8f7..8cef2613102cf2f91b3a4d971cda32a8d7739096 100644 --- a/modules/prediction/common/BUILD +++ b/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() diff --git a/modules/prediction/common/prediction_map.h b/modules/prediction/common/prediction_map.h index 6aa2d233e21e71b5985308105264fd2a96706a3d..d1712000f7af48c3bfbf3be35cd6e03f184a71a0 100644 --- a/modules/prediction/common/prediction_map.h +++ b/modules/prediction/common/prediction_map.h @@ -18,11 +18,13 @@ #define MODULES_PREDICTION_COMMON_PREDICTION_MAP_H_ #include +#include #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 hdmap_; DECLARE_SINGLETON(PredictionMap); diff --git a/modules/prediction/common/road_graph.cc b/modules/prediction/common/road_graph.cc new file mode 100644 index 0000000000000000000000000000000000000000..b5c1b40fc7f821b72eb95232c0ab6403440573c6 --- /dev/null +++ b/modules/prediction/common/road_graph.cc @@ -0,0 +1,116 @@ +/****************************************************************************** + * 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 + +#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 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* 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 diff --git a/modules/prediction/common/road_graph.h b/modules/prediction/common/road_graph.h new file mode 100644 index 0000000000000000000000000000000000000000..7763ea18bacaa8ba993238baeb6cbcd998c0466a --- /dev/null +++ b/modules/prediction/common/road_graph.h @@ -0,0 +1,59 @@ +/****************************************************************************** + * 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 + +#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* 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_