Clean up routing

modules/routing/common/routing_gflags.cpp → modules/routing/common/routing_gflags.cc

@@ -18,19 +18,23 @@
 
 DEFINE_string(node_name, "router", "the name for this node");
 DEFINE_string(node_namespace, "router", "the namespace for this node");
-DEFINE_string(signal_probe_service, "/router/routing_signal", "the service name for signal probe");
+DEFINE_string(signal_probe_service, "/router/routing_signal",
+              "the service name for signal probe");
 
 DEFINE_bool(enable_old_routing, true, "enable old routing");
-DEFINE_string(route_topic_for_broadcast, "/router/routing", "the default routing topic");
+DEFINE_string(route_topic_for_broadcast, "/router/routing",
+              "the default routing topic");
 DEFINE_bool(use_road_id, true, "enable use road id to cut routing result");
 
 DEFINE_string(graph_dir, "", "the default directory of topology graph data");
-DEFINE_string(graph_file_name, "routing_map.bin", "the default file name of topology graph data");
+DEFINE_string(graph_file_name, "routing_map.bin",
+              "the default file name of topology graph data");
 
 DEFINE_string(rosparam_name_routing_init_status, "/pnc/routing_initialized",
-        "true if routing init ok, used by ADS test");
+              "true if routing init ok, used by ADS test");
 
 DEFINE_bool(enable_debug_mode, true, "enable debug mode");
-DEFINE_string(debug_route_path, "", "the default path of routing result debug file");
-DEFINE_string(debug_passage_region_path, "", "the default path of passage region debug file");
-
+DEFINE_string(debug_route_path, "",
+              "the default path of routing result debug file");
+DEFINE_string(debug_passage_region_path, "",
+              "the default path of passage region debug file");

modules/routing/common/routing_gflags.h

@@ -36,5 +36,4 @@ DECLARE_bool(enable_debug_mode);
 DECLARE_string(debug_route_path);
 DECLARE_string(debug_passage_region_path);
 
-#endif // BAIDU_ADU_ROUTING_COMMON_ROUTING_GFLAGS_H
-
+#endif  // BAIDU_ADU_ROUTING_COMMON_ROUTING_GFLAGS_H

modules/routing/common/routing_macros.h

@@ -19,26 +19,25 @@
 
 #include <cstddef>
 
-#define DISALLOW_COPY_AND_ASSIGN(classname)\
-private:\
-classname(const classname&);\
-classname& operator = (const classname&);
+#define DISALLOW_COPY_AND_ASSIGN(classname) \
+ private:                                   \
+  classname(const classname&);              \
+  classname& operator=(const classname&);
 
-#define DISALLOW_IMPLICIT_CONSTRUCTORS(classname)\
-private:\
-classname();\
-DISALLOW_COPY_AND_ASSIGN(classname);
+#define DISALLOW_IMPLICIT_CONSTRUCTORS(classname) \
+ private:                                         \
+  classname();                                    \
+  DISALLOW_COPY_AND_ASSIGN(classname);
 
-#define DECLARE_ARBITER_SINGLETON(classname)\
-public:\
-    static classname * instance() {\
-        static classname instance;\
-        return &instance;\
-    };\
-\
-DISALLOW_IMPLICIT_CONSTRUCTORS(classname)\
-\
-private:
-
-#endif // BAIDU_ADU_ROUTING_COMMON_ROUTING_MACROS_H
+#define DECLARE_ARBITER_SINGLETON(classname) \
+ public:                                     \
+  static classname* instance() {             \
+    static classname instance;               \
+    return &instance;                        \
+  };                                         \
+                                             \
+  DISALLOW_IMPLICIT_CONSTRUCTORS(classname)  \
+                                             \
+ private:
 
+#endif  // BAIDU_ADU_ROUTING_COMMON_ROUTING_MACROS_H

modules/routing/common/utils.cpp → modules/routing/common/utils.cc

@@ -28,41 +28,45 @@ using ::google::protobuf::io::FileInputStream;
 using ::google::protobuf::io::ZeroCopyInputStream;
 using ::google::protobuf::io::CodedInputStream;
 
-bool FileUtils::load_protobuf_data_from_file(const std::string& file_path,
-                                             ::google::protobuf::Message* const proto_data) {
-    int fd = open(file_path.c_str(), O_RDONLY);
-    if (fd == -1) {
-        ROS_ERROR("File %s not found", file_path.c_str());
-        return false;
-    }
-    std::unique_ptr<ZeroCopyInputStream> raw_input(new FileInputStream(fd));
-    std::unique_ptr<CodedInputStream> coded_input(new CodedInputStream(raw_input.get()));
-    coded_input->SetTotalBytesLimit(INT_MAX, 536870912);  //  0..512M..2G
-    bool ret = proto_data->ParseFromCodedStream(coded_input.get());
-    close(fd);
-    return ret;
+bool FileUtils::load_protobuf_data_from_file(
+    const std::string& file_path,
+    ::google::protobuf::Message* const proto_data) {
+  int fd = open(file_path.c_str(), O_RDONLY);
+  if (fd == -1) {
+    ROS_ERROR("File %s not found", file_path.c_str());
+    return false;
+  }
+  std::unique_ptr<ZeroCopyInputStream> raw_input(new FileInputStream(fd));
+  std::unique_ptr<CodedInputStream> coded_input(
+      new CodedInputStream(raw_input.get()));
+  coded_input->SetTotalBytesLimit(INT_MAX, 536870912);  //  0..512M..2G
+  bool ret = proto_data->ParseFromCodedStream(coded_input.get());
+  close(fd);
+  return ret;
 }
 
-bool FileUtils::dump_protobuf_data_to_file(const std::string& file_path,
-                                           const ::google::protobuf::Message* const proto_data) {
-    return dump_protobuf_data_to_file(file_path, *proto_data);
+bool FileUtils::dump_protobuf_data_to_file(
+    const std::string& file_path,
+    const ::google::protobuf::Message* const proto_data) {
+  return dump_protobuf_data_to_file(file_path, *proto_data);
 }
 
-bool FileUtils::dump_protobuf_data_to_file(const std::string& file_path,
-                                           const ::google::protobuf::Message& proto_data) {
-    int fd = open(file_path.c_str(), O_CREAT| O_TRUNC| O_RDWR, 0644);
-    if (fd < 0 || !proto_data.SerializeToFileDescriptor(fd)) {
-        ROS_ERROR("Error occured while trying to write to file: %s", file_path.c_str());
-        return false;
-    }
-    if (close(fd) != 0) {
-        ROS_ERROR("Error occured while close fd. Please check the file!");
-        return false;
-    }
-    ROS_INFO("Proto data dump done!");
-    return true;
+bool FileUtils::dump_protobuf_data_to_file(
+    const std::string& file_path,
+    const ::google::protobuf::Message& proto_data) {
+  int fd = open(file_path.c_str(), O_CREAT | O_TRUNC | O_RDWR, 0644);
+  if (fd < 0 || !proto_data.SerializeToFileDescriptor(fd)) {
+    ROS_ERROR("Error occured while trying to write to file: %s",
+              file_path.c_str());
+    return false;
+  }
+  if (close(fd) != 0) {
+    ROS_ERROR("Error occured while close fd. Please check the file!");
+    return false;
+  }
+  ROS_INFO("Proto data dump done!");
+  return true;
 }
 
-} // namespace routing
-} // namespace adu
-
+}  // namespace routing
+}  // namespace adu

modules/routing/common/utils.h

@@ -17,25 +17,27 @@
 #ifndef BAIDU_ADU_ROUTING_COMMON_UTILS_H
 #define BAIDU_ADU_ROUTING_COMMON_UTILS_H
 
-#include <string>
 #include <google/protobuf/message.h>
+#include <string>
 
 namespace adu {
 namespace routing {
 
 class FileUtils {
-public:
-    static bool load_protobuf_data_from_file(const std::string& file_path,
-                                             ::google::protobuf::Message* const proto_data);
-    static bool dump_protobuf_data_to_file(const std::string& file_path,
-                                           const ::google::protobuf::Message& proto_data);
-
-    static bool dump_protobuf_data_to_file(const std::string& file_path,
-                                           const ::google::protobuf::Message* const proto_data);
+ public:
+  static bool load_protobuf_data_from_file(
+      const std::string& file_path,
+      ::google::protobuf::Message* const proto_data);
+  static bool dump_protobuf_data_to_file(
+      const std::string& file_path,
+      const ::google::protobuf::Message& proto_data);
+
+  static bool dump_protobuf_data_to_file(
+      const std::string& file_path,
+      const ::google::protobuf::Message* const proto_data);
 };
 
-} // namespace routing
-} // namespace adu
-
-#endif // BAIDU_ADU_ROUTING_COMMON_UTILS_H
+}  // namespace routing
+}  // namespace adu
 
+#endif  // BAIDU_ADU_ROUTING_COMMON_UTILS_H

modules/routing/core/arbiter.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 "core/arbiter.h"
+
+#include "core/navigator.h"
+#include "std_msgs/String.h"
+
+namespace adu {
+namespace routing {
+
+Arbiter::Arbiter() {
+  _node_handle_ptr.reset(new ros::NodeHandle(FLAGS_node_namespace));
+  if (FLAGS_enable_old_routing) {
+    ROS_INFO("Use old routing request and result!");
+    _service = _node_handle_ptr->advertiseService(
+        FLAGS_signal_probe_service, &Arbiter::on_request_old_routing, this);
+  } else {
+    ROS_INFO("Use new routing request and result!");
+    _service = _node_handle_ptr->advertiseService(FLAGS_signal_probe_service,
+                                                  &Arbiter::on_request, this);
+  }
+  _publisher = _node_handle_ptr->advertise<std_msgs::String>(
+      FLAGS_route_topic_for_broadcast, 1);
+  std::string graph_path = FLAGS_graph_dir + "/" + FLAGS_graph_file_name;
+  ROS_INFO("Use routing topology graph path: %s", graph_path.c_str());
+  _navigator_ptr.reset(new Navigator(graph_path));
+
+  // set init ok for ADS test, when it knows routing is ok, it can replay bags
+  _node_handle_ptr->setParam(FLAGS_rosparam_name_routing_init_status, true);
+}
+
+Arbiter::~Arbiter() {}
+
+bool Arbiter::on_request(arbiter::routing_signal::Request& req,
+                         arbiter::routing_signal::Response& res) {
+  ROS_INFO("Get new routing request!!!");
+  ::adu::common::router::RoutingRequest request_proto;
+  ::adu::common::router::RoutingResult response_proto;
+  if (!request_proto.ParseFromString(req.routing_request.data)) {
+    ROS_ERROR("The request proto is invalid.");
+    return false;
+  }
+  if (!_navigator_ptr->search_route(request_proto, &response_proto)) {
+    ROS_ERROR("Failed to search route with navigator.");
+    return false;
+  }
+  if (!response_proto.SerializeToString(&(res.routing_response.data))) {
+    ROS_ERROR("Failed to serialize routing response.");
+    return false;
+  }
+  if (request_proto.broadcast()) {
+    std_msgs::String publish_msg;
+    if (!response_proto.SerializeToString(&(publish_msg.data))) {
+      ROS_ERROR("Failed to serialize routing response.");
+      return false;
+    }
+    _publisher.publish(publish_msg);
+  }
+  return true;
+}
+
+bool Arbiter::on_request_old_routing(arbiter::routing_signal::Request& req,
+                                     arbiter::routing_signal::Response& res) {
+  ROS_INFO("Get old routing request!!!");
+  ::adu::common::routing::RoutingRequest request_proto;
+  ::adu::common::routing::RoutingResult response_proto;
+  if (!request_proto.ParseFromString(req.routing_request.data)) {
+    ROS_ERROR("The request proto is invalid.");
+    return false;
+  }
+  if (!_navigator_ptr->search_route(request_proto, &response_proto)) {
+    ROS_ERROR("Failed to search route with navigator.");
+    return false;
+  }
+  if (!response_proto.SerializeToString(&(res.routing_response.data))) {
+    ROS_ERROR("Failed to serialize routing response.");
+    return false;
+  }
+  if (request_proto.broadcast()) {
+    std_msgs::String publish_msg;
+    if (!response_proto.SerializeToString(&(publish_msg.data))) {
+      ROS_ERROR("Failed to serialize routing response.");
+      return false;
+    }
+    _publisher.publish(publish_msg);
+  }
+  return true;
+}
+
+bool Arbiter::run() {
+  if (!_navigator_ptr->is_ready()) {
+    ROS_ERROR("Navigator is not ready!");
+    return false;
+  }
+  ROS_INFO("Routing service is ready.");
+  ros::spin();
+  return false;
+}
+
+}  // namespace routing
+}  // namespace adu

modules/routing/core/arbiter.cpp

-/******************************************************************************
-  * 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 "core/arbiter.h"
-
-#include "core/navigator.h"
-#include "std_msgs/String.h"
-
-namespace adu {
-namespace routing {
-
-Arbiter::Arbiter() {
-    _node_handle_ptr.reset(new ros::NodeHandle(FLAGS_node_namespace));
-    if (FLAGS_enable_old_routing) {
-        ROS_INFO("Use old routing request and result!");
-        _service = _node_handle_ptr->advertiseService(FLAGS_signal_probe_service,
-                                                      &Arbiter::on_request_old_routing,
-                                                      this);
-    } else {
-        ROS_INFO("Use new routing request and result!");
-        _service = _node_handle_ptr->advertiseService(FLAGS_signal_probe_service,
-                                                      &Arbiter::on_request,
-                                                      this);
-    }
-    _publisher = _node_handle_ptr->advertise<std_msgs::String>(FLAGS_route_topic_for_broadcast, 1);
-    std::string graph_path = FLAGS_graph_dir + "/" + FLAGS_graph_file_name;
-    ROS_INFO("Use routing topology graph path: %s", graph_path.c_str());
-    _navigator_ptr.reset(new Navigator(graph_path));
-
-    // set init ok for ADS test, when it knows routing is ok, it can replay bags
-    _node_handle_ptr->setParam(FLAGS_rosparam_name_routing_init_status, true);
-}
-
-Arbiter::~Arbiter() { }
-
-bool Arbiter::on_request(arbiter::routing_signal::Request& req,
-                         arbiter::routing_signal::Response& res) {
-    ROS_INFO("Get new routing request!!!");
-    ::adu::common::router::RoutingRequest request_proto;
-    ::adu::common::router::RoutingResult response_proto;
-    if (!request_proto.ParseFromString(req.routing_request.data)) {
-        ROS_ERROR("The request proto is invalid.");
-        return false;
-    }
-    if (!_navigator_ptr->search_route(request_proto, &response_proto)) {
-        ROS_ERROR("Failed to search route with navigator.");
-        return false;
-    }
-    if (!response_proto.SerializeToString(&(res.routing_response.data))) {
-        ROS_ERROR("Failed to serialize routing response.");
-        return false;
-    }
-    if (request_proto.broadcast()) {
-        std_msgs::String publish_msg;
-        if (!response_proto.SerializeToString(&(publish_msg.data))) {
-            ROS_ERROR("Failed to serialize routing response.");
-            return false;
-        }
-        _publisher.publish(publish_msg);
-    }
-    return true;
-}
-
-bool Arbiter::on_request_old_routing(arbiter::routing_signal::Request& req,
-                                     arbiter::routing_signal::Response& res) {
-    ROS_INFO("Get old routing request!!!");
-    ::adu::common::routing::RoutingRequest request_proto;
-    ::adu::common::routing::RoutingResult response_proto;
-    if (!request_proto.ParseFromString(req.routing_request.data)) {
-        ROS_ERROR("The request proto is invalid.");
-        return false;
-    }
-    if (!_navigator_ptr->search_route(request_proto, &response_proto)) {
-        ROS_ERROR("Failed to search route with navigator.");
-        return false;
-    }
-    if (!response_proto.SerializeToString(&(res.routing_response.data))) {
-        ROS_ERROR("Failed to serialize routing response.");
-        return false;
-    }
-    if (request_proto.broadcast()) {
-        std_msgs::String publish_msg;
-        if (!response_proto.SerializeToString(&(publish_msg.data))) {
-            ROS_ERROR("Failed to serialize routing response.");
-            return false;
-        }
-        _publisher.publish(publish_msg);
-    }
-    return true;
-}
-
-bool Arbiter::run() {
-    if (!_navigator_ptr->is_ready()) {
-        ROS_ERROR("Navigator is not ready!");
-        return false;
-    }
-    ROS_INFO("Routing service is ready.");
-    ros::spin();
-    return false;
-}
-
-} // namespace routing
-} // namespace adu
-

modules/routing/core/arbiter.h

@@ -20,9 +20,9 @@
 #include <memory>
 #include "ros/ros.h"
 
+#include "arbiter/routing_signal.h"
 #include "common/routing_gflags.h"
 #include "common/routing_macros.h"
-#include "arbiter/routing_signal.h"
 
 namespace adu {
 namespace routing {
@@ -30,27 +30,27 @@ namespace routing {
 class Navigator;
 
 class Arbiter {
-public:
-    ~Arbiter();
-    bool run();
-
-private:
-    bool on_request(arbiter::routing_signal::Request& req,
-                    arbiter::routing_signal::Response& res);
-
-    // for old routing request
-    bool on_request_old_routing(arbiter::routing_signal::Request& req,
-                                arbiter::routing_signal::Response& res);
-private:
-    std::unique_ptr<ros::NodeHandle> _node_handle_ptr;
-    ros::ServiceServer _service;
-    ros::Publisher _publisher;
-    std::unique_ptr<Navigator> _navigator_ptr;
-    DECLARE_ARBITER_SINGLETON(Arbiter);
+ public:
+  ~Arbiter();
+  bool run();
+
+ private:
+  bool on_request(arbiter::routing_signal::Request& req,
+                  arbiter::routing_signal::Response& res);
+
+  // for old routing request
+  bool on_request_old_routing(arbiter::routing_signal::Request& req,
+                              arbiter::routing_signal::Response& res);
+
+ private:
+  std::unique_ptr<ros::NodeHandle> _node_handle_ptr;
+  ros::ServiceServer _service;
+  ros::Publisher _publisher;
+  std::unique_ptr<Navigator> _navigator_ptr;
+  DECLARE_ARBITER_SINGLETON(Arbiter);
 };
 
-} // namespace routing
-} // namespace adu
-
-#endif // BAIDU_ADU_ROUTING_CORE_ARBITER_H
+}  // namespace routing
+}  // namespace adu
 
+#endif  // BAIDU_ADU_ROUTING_CORE_ARBITER_H

modules/routing/core/black_list_range_generator.cpp

-/******************************************************************************
-  * 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 "ros/ros.h"
-
-#include "core/black_list_range_generator.h"
-
-#include "graph/sub_topo_graph.h"
-#include "graph/topo_graph.h"
-#include "graph/topo_node.h"
-
-namespace adu {
-namespace routing {
-
-namespace {
-
-NodeWithRange get_node_with_range(const TopoNode* node, double start_s, double end_s) {
-    if (node == nullptr) {
-        ROS_ERROR("Get null pointer to topo node when getting node with range.");
-        exit(-1);
-    }
-    if (start_s < 0.0 || end_s > node->length()) {
-        ROS_ERROR("Invalid s, start_s: %f, end_s: %f, length: %f.", start_s, end_s, node->length());
-        exit(-1);
-    }
-    NodeSRange range(start_s, end_s);
-    NodeWithRange black_node_with_range(range, node);
-    return black_node_with_range;
-}
-
-void generate_black_set_from_road(const ::adu::common::router::RoutingRequest& request,
-                                  const TopoGraph* graph,
-                                  std::unordered_set<const TopoNode*>* const black_list) {
-    for (const auto& road_id : request.blacklisted_road()) {
-        graph->get_nodes_by_road_id(road_id, black_list);
-    }
-}
-
-void generate_black_set_from_road(const ::adu::common::router::RoutingRequest& request,
-                                  const TopoGraph* graph,
-                                  std::vector<NodeWithRange>* const black_list) {
-    for (const auto& road_id : request.blacklisted_road()) {
-        std::unordered_set<const TopoNode*> road_nodes_set;
-        graph->get_nodes_by_road_id(road_id, &road_nodes_set);
-        for (const auto& node : road_nodes_set) {
-            black_list->push_back(get_node_with_range(node, 0.0, node->length()));
-        }
-    }
-}
-
-template <typename T>
-void generate_black_set_from_lane(const T& request,
-                                  const TopoGraph* graph,
-                                  std::unordered_set<const TopoNode*>* const black_list) {
-    for (const auto& lane : request.blacklisted_lane()) {
-        const auto* node = graph->get_node(lane.id());
-        if (node == nullptr) {
-            continue;
-        }
-        black_list->insert(node);
-    }
-}
-
-template <typename T>
-void generate_black_set_from_lane(const T& request,
-                                  const TopoGraph* graph,
-                                  std::vector<NodeWithRange>* const black_list) {
-    for (const auto& lane : request.blacklisted_lane()) {
-        const auto* node = graph->get_node(lane.id());
-        if (node == nullptr) {
-            continue;
-        }
-        black_list->push_back(get_node_with_range(node, lane.start_s(), lane.end_s()));
-    }
-}
-
-} // namespace
-
-// old request
-void BlackListRangeGenerator::generate_black_set_from_request(
-                                    const ::adu::common::routing::RoutingRequest& request,
-                                    const TopoGraph* graph,
-                                    std::unordered_set<const TopoNode*>* const black_list) const {
-    generate_black_set_from_lane(request, graph, black_list);
-}
-
-// new request
-void BlackListRangeGenerator::generate_black_set_from_request(
-                                    const ::adu::common::router::RoutingRequest& request,
-                                    const TopoGraph* graph,
-                                    std::unordered_set<const TopoNode*>* const black_list) const {
-    generate_black_set_from_lane(request, graph, black_list);
-    generate_black_set_from_road(request, graph, black_list);
-}
-
-// old request
-void BlackListRangeGenerator::generate_black_vec_from_request(
-                                    const ::adu::common::routing::RoutingRequest& request,
-                                    const TopoGraph* graph,
-                                    const TopoNode* src_node,
-                                    const TopoNode* dest_node,
-                                    std::vector<NodeWithRange>* const black_list) const {
-    generate_black_set_from_lane(request, graph, black_list);
-    double start_s = request.start().s();
-    double end_s = request.end().s();
-    if (src_node == dest_node && start_s > end_s) {
-        black_list->push_back(get_node_with_range(src_node, end_s, start_s));
-    } else {
-        NodeSRange range(end_s, start_s);
-        NodeWithRange black_node_with_range(range, src_node);
-        black_list->push_back(black_node_with_range);
-
-    }
- 
-        
-
-}
-
-    // new request
-void BlackListRangeGenerator::generate_black_vec_from_request(
-                                    const ::adu::common::router::RoutingRequest& request,
-                                    const TopoGraph* graph,
-                                    const TopoNode* src_node,
-                                    const TopoNode* dest_node,
-                                    std::vector<NodeWithRange>* const black_list) const {
-    generate_black_set_from_lane(request, graph, black_list);
-    generate_black_set_from_road(request, graph, black_list);
-}
-
-} // namespace routing
-} // namespace adu

modules/routing/core/black_list_range_generator.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 BAIDU_ADU_ROUTING_CORE_BLACK_LIST_RANGE_GENERATOR_H
-#define BAIDU_ADU_ROUTING_CORE_BLACK_LIST_RANGE_GENERATOR_H
-
-#include <memory>
-#include <unordered_set>
-
-#include "routing.pb.h"
-#include "router.pb.h"
-
-#include "graph/topo_range.h"
-
-namespace adu {
-namespace routing {
-
-class TopoGraph;
-class TopoNode;
-
-class BlackListRangeGenerator {
-public:
-    BlackListRangeGenerator() = default;
-    ~BlackListRangeGenerator() = default;
-
-    // old request
-    void generate_black_set_from_request(const ::adu::common::routing::RoutingRequest& request,
-                                    const TopoGraph* graph,
-                                    std::unordered_set<const TopoNode*>* const black_list) const;
-
-    // new request
-    void generate_black_set_from_request(const ::adu::common::router::RoutingRequest& request,
-                                    const TopoGraph* graph,
-                                    std::unordered_set<const TopoNode*>* const black_list) const;
-
-    // old request
-    void generate_black_vec_from_request(const ::adu::common::routing::RoutingRequest& request,
-                                    const TopoGraph* graph,
-                                    const TopoNode* src_node,
-                                    const TopoNode* dest_node,
-                                    std::vector<NodeWithRange>* const black_list) const;
-
-    // new request
-    void generate_black_vec_from_request(const ::adu::common::router::RoutingRequest& request,
-                                    const TopoGraph* graph,
-                                    const TopoNode* src_node,
-                                    const TopoNode* dest_node,
-                                    std::vector<NodeWithRange>* const black_list) const;
-};
-
-} // namespace routing
-} // namespace adu
-
-#endif // BAIDU_ADU_ROUTING_CORE_BLACK_LIST_RANGE_GENERATOR_H
-

modules/routing/core/navigator.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 "core/navigator.h"
+
+#include <assert.h>
+#include <algorithm>
+#include <fstream>
+
+#include "boost/lexical_cast.hpp"
+#include "ros/console.h"
+#include "ros/time.h"
+
+#include <float.h>
+#include "common/routing_gflags.h"
+#include "common/utils.h"
+#include "graph/topo_edge.h"
+#include "graph/topo_graph.h"
+#include "graph/topo_node.h"
+#include "strategy/a_star_strategy.h"
+
+namespace adu {
+namespace routing {
+
+namespace {
+
+using ::adu::common::router::RoutingResult_Road;
+using ::adu::common::router::RoutingResult_Junction;
+using ::adu::common::router::RoutingResult_PassageRegion;
+using ::adu::common::router::RoutingResult_LaneSegment;
+using ::adu::common::router::RoutingResult_LaneChangeInfo;
+
+void get_nodes_of_ways_based_on_virtual(
+    const std::vector<const TopoNode*>& nodes,
+    std::vector<std::vector<const TopoNode*> >* const nodes_of_ways) {
+  ROS_INFO("Cut routing ways based on is_virtual.");
+  assert(!nodes.empty());
+  nodes_of_ways->clear();
+  std::vector<const TopoNode*> nodes_of_way;
+  nodes_of_way.push_back(nodes.at(0));
+  bool last_is_virtual = nodes.at(0)->is_virtual();
+  for (auto iter = nodes.begin() + 1; iter != nodes.end(); ++iter) {
+    if ((*iter)->is_virtual() != last_is_virtual) {
+      nodes_of_ways->push_back(nodes_of_way);
+      nodes_of_way.clear();
+    }
+    nodes_of_way.push_back(*iter);
+    last_is_virtual = (*iter)->is_virtual();
+  }
+  nodes_of_ways->push_back(nodes_of_way);
+}
+
+void get_nodes_of_ways(
+    const std::vector<const TopoNode*>& nodes,
+    std::vector<std::vector<const TopoNode*> >* const nodes_of_ways) {
+  ROS_INFO("Cut routing ways based on road id.");
+  assert(!nodes.empty());
+  nodes_of_ways->clear();
+  std::vector<const TopoNode*> nodes_of_way;
+  nodes_of_way.push_back(nodes.at(0));
+  std::string last_road_id = nodes.at(0)->road_id();
+  for (auto iter = nodes.begin() + 1; iter != nodes.end(); ++iter) {
+    if ((*iter)->road_id() != last_road_id) {
+      nodes_of_ways->push_back(nodes_of_way);
+      nodes_of_way.clear();
+    }
+    nodes_of_way.push_back(*iter);
+    last_road_id = (*iter)->road_id();
+  }
+  nodes_of_ways->push_back(nodes_of_way);
+}
+
+void extract_basic_passages(
+    const std::vector<const TopoNode*>& nodes,
+    std::vector<std::vector<const TopoNode*> >* const nodes_of_passages,
+    std::vector<RoutingResult_LaneChangeInfo::Type>* const lane_change_types) {
+  assert(!nodes.empty());
+  nodes_of_passages->clear();
+  lane_change_types->clear();
+  std::vector<const TopoNode*> nodes_of_passage;
+  nodes_of_passage.push_back(nodes.at(0));
+  for (auto iter = nodes.begin() + 1; iter != nodes.end(); ++iter) {
+    auto edge = (*(iter - 1))->get_out_edge_to(*iter);
+    if (edge == nullptr) {
+      ROS_ERROR("Get null pointer to edge from %s to %s.",
+                (*(iter - 1))->lane_id().c_str(), (*iter)->lane_id().c_str());
+      exit(-1);
+    }
+    if (edge->type() == TET_LEFT || edge->type() == TET_RIGHT) {
+      nodes_of_passages->push_back(nodes_of_passage);
+      nodes_of_passage.clear();
+      if (edge->type() == TET_LEFT) {
+        lane_change_types->push_back(
+            RoutingResult_LaneChangeInfo::LEFT_FORWARD);
+      } else {
+        lane_change_types->push_back(
+            RoutingResult_LaneChangeInfo::RIGHT_FORWARD);
+      }
+    }
+    nodes_of_passage.push_back(*iter);
+  }
+  nodes_of_passages->push_back(nodes_of_passage);
+}
+
+void passage_lane_ids_to_passage_region(
+    const std::vector<const TopoNode*>& nodes,
+    const NodeRangeManager& range_manager,
+    RoutingResult_PassageRegion* const region) {
+  for (const auto& node : nodes) {
+    RoutingResult_LaneSegment* seg = region->add_segment();
+    seg->set_id(node->lane_id());
+    NodeRange range = range_manager.get_node_range(node);
+    seg->set_start_s(range.start_s);
+    seg->set_end_s(range.end_s);
+  }
+}
+
+double calculate_distance(const std::vector<const TopoNode*>& nodes,
+                          const NodeRangeManager& range_manager) {
+  NodeRange range = range_manager.get_node_range(nodes.at(0));
+  double distance = range.end_s - range.start_s;
+  for (auto iter = nodes.begin() + 1; iter != nodes.end(); ++iter) {
+    auto edge = (*(iter - 1))->get_out_edge_to(*iter);
+    if (edge->type() != TET_FORWARD) {
+      continue;
+    }
+    range = range_manager.get_node_range(*iter);
+    distance += range.end_s - range.start_s;
+  }
+  return distance;
+}
+
+template <typename T>
+void show_request_info(const T& request) {
+  const auto& start = request.start();
+  const auto& end = request.end();
+  ROS_INFO("Start point:\tlane id: %s s: %f x: %f y: %f", start.id().c_str(),
+           start.s(), start.pose().x(), start.pose().y());
+
+  for (const auto& wp : request.waypoint()) {
+    ROS_INFO("Way Point:\tlane id: %s s: %f x: %f y: %f", wp.id().c_str(),
+             wp.s(), wp.pose().x(), wp.pose().y());
+  }
+
+  for (const auto& bl : request.blacklisted_lane()) {
+    ROS_INFO("Way Point:\tlane id: %s start_s: %f end_s %f", bl.id().c_str(),
+             bl.start_s(), bl.end_s());
+  }
+
+  ROS_INFO("End point:\tlane id: %s s: %f x: %f y: %f", end.id().c_str(),
+           end.s(), end.pose().x(), end.pose().y());
+}
+
+void generate_black_set_from_road(
+    const ::adu::common::router::RoutingRequest& request,
+    const TopoGraph* graph,
+    std::unordered_set<const TopoNode*>* const black_list) {
+  for (const auto& road_id : request.blacklisted_road()) {
+    graph->get_nodes_by_road_id(road_id, black_list);
+  }
+}
+
+template <typename T>
+void generate_black_set_from_lane(
+    const T& request, const TopoGraph* graph,
+    std::unordered_set<const TopoNode*>* const black_list) {
+  for (const auto& point : request.blacklisted_lane()) {
+    const auto* node = graph->get_node(point.id());
+    if (node == nullptr) {
+      continue;
+    }
+    black_list->insert(node);
+  }
+}
+
+template <typename T>
+bool get_way_nodes(const T& request, const TopoGraph* graph,
+                   std::vector<const TopoNode*>* const way_nodes,
+                   NodeRangeManager* const range_manager) {
+  const auto* start_node = graph->get_node(request.start().id());
+  if (start_node == nullptr) {
+    ROS_ERROR("Can't find start point in graph! Id: %s",
+              request.start().id().c_str());
+    return false;
+  }
+  way_nodes->push_back(start_node);
+
+  for (const auto& point : request.waypoint()) {
+    const auto* cur_node = graph->get_node(point.id());
+    if (cur_node == nullptr) {
+      ROS_ERROR("Can't find way point in graph! Id: %s", point.id().c_str());
+      return false;
+    }
+    way_nodes->push_back(cur_node);
+  }
+
+  const auto* end_node = graph->get_node(request.end().id());
+  if (end_node == nullptr) {
+    ROS_ERROR("Can't find end point in graph! Id: %s",
+              request.start().id().c_str());
+    return false;
+  }
+  way_nodes->push_back(end_node);
+
+  range_manager->init_node_range(request.start().s(), request.end().s(),
+                                 start_node, end_node);
+
+  return true;
+}
+
+bool search_route_by_strategy(
+    const TopoGraph* graph, const std::vector<const TopoNode*>& way_nodes,
+    const std::unordered_set<const TopoNode*>& black_list,
+    std::vector<const TopoNode*>* const result_nodes) {
+  std::unique_ptr<Strategy> strategy_ptr;
+  strategy_ptr.reset(new AStarStrategy());
+
+  result_nodes->clear();
+  for (size_t i = 1; i < way_nodes.size(); ++i) {
+    std::vector<const TopoNode*> cur_result_nodes;
+    const auto* cur_start = way_nodes[i - 1];
+    const auto* cur_end = way_nodes[i];
+    if (!strategy_ptr->search(graph, cur_start, cur_end, black_list,
+                              &cur_result_nodes)) {
+      ROS_ERROR("Failed to search route with waypoint from %s to %s",
+                cur_start->lane_id().c_str(), cur_end->lane_id().c_str());
+      return false;
+    }
+    auto end_iter = cur_result_nodes.end();
+    if (i != way_nodes.size() - 1) {
+      --end_iter;
+    }
+    result_nodes->insert(result_nodes->end(), cur_result_nodes.begin(),
+                         end_iter);
+  }
+  return true;
+}
+
+}  // namespace
+
+Navigator::Navigator(const std::string& topo_file_path) : _is_ready(false) {
+  _graph.reset(new TopoGraph());
+  if (!_graph->load_graph(topo_file_path)) {
+    ROS_INFO("Navigator init graph failed! File path: %s.",
+             topo_file_path.c_str());
+    return;
+  }
+  _is_ready = true;
+  ROS_INFO("The navigator is ready.");
+};
+
+Navigator::~Navigator() {}
+
+bool Navigator::is_ready() const { return _is_ready; }
+
+// search new request to new response
+bool Navigator::search_route(
+    const ::adu::common::router::RoutingRequest& request,
+    ::adu::common::router::RoutingResult* response) const {
+  if (!is_ready()) {
+    ROS_ERROR("Topo graph is not ready!");
+    return false;
+  }
+  show_request_info(request);
+
+  std::vector<const TopoNode*> way_nodes;
+  NodeRangeManager range_manager;
+
+  if (!get_way_nodes(request, _graph.get(), &way_nodes, &range_manager)) {
+    ROS_ERROR("Can't find way point in graph!");
+    return false;
+  }
+
+  std::vector<const TopoNode*> result_nodes;
+  std::unordered_set<const TopoNode*> black_list;
+  generate_black_set_from_lane(request, _graph.get(), &black_list);
+  generate_black_set_from_road(request, _graph.get(), &black_list);
+
+  if (!search_route_by_strategy(_graph.get(), way_nodes, black_list,
+                                &result_nodes)) {
+    ROS_ERROR("Can't find route from request!");
+    return false;
+  }
+
+  if (!generate_passage_region(request, result_nodes, black_list, range_manager,
+                               response)) {
+    ROS_ERROR(
+        "Failed to generate new passage regions based on route result lane "
+        "ids");
+    return false;
+  }
+
+  if (FLAGS_enable_debug_mode) {
+    dump_debug_data(result_nodes, range_manager, *response);
+  }
+  return true;
+}
+
+// search old request to old response
+bool Navigator::search_route(
+    const ::adu::common::routing::RoutingRequest& request,
+    ::adu::common::routing::RoutingResult* response) const {
+  if (!is_ready()) {
+    ROS_ERROR("Topo graph is not ready!");
+    return false;
+  }
+  show_request_info(request);
+
+  std::vector<const TopoNode*> way_nodes;
+  NodeRangeManager range_manager;
+
+  if (!get_way_nodes(request, _graph.get(), &way_nodes, &range_manager)) {
+    ROS_ERROR("Can't find way point in graph!");
+    return false;
+  }
+
+  std::vector<const TopoNode*> result_nodes;
+  std::unordered_set<const TopoNode*> black_list;
+  generate_black_set_from_lane(request, _graph.get(), &black_list);
+
+  if (!search_route_by_strategy(_graph.get(), way_nodes, black_list,
+                                &result_nodes)) {
+    ROS_ERROR("Can't find route from request!");
+    return false;
+  }
+
+  if (!generate_passage_region(request, result_nodes, black_list, range_manager,
+                               response)) {
+    ROS_ERROR(
+        "Failed to generate new passage regions based on route result lane "
+        "ids");
+    return false;
+  }
+
+  if (FLAGS_enable_debug_mode) {
+    ::adu::common::router::RoutingResult new_response;
+    generate_passage_region(result_nodes, black_list, range_manager,
+                            &new_response);
+    dump_debug_data(result_nodes, range_manager, new_response);
+  }
+
+  return true;
+}
+
+// new request to new response
+bool Navigator::generate_passage_region(
+    const ::adu::common::router::RoutingRequest& request,
+    const std::vector<const TopoNode*>& nodes,
+    const std::unordered_set<const TopoNode*>& black_list,
+    const NodeRangeManager& range_manager,
+    ::adu::common::router::RoutingResult* result) const {
+  result->mutable_header()->set_timestamp_sec(ros::Time::now().toSec());
+  result->mutable_header()->set_module_name(FLAGS_node_name);
+  result->mutable_header()->set_sequence_num(1);
+
+  generate_passage_region(nodes, black_list, range_manager, result);
+
+  result->set_map_version(_graph->map_version());
+  result->mutable_measurement()->set_distance(
+      calculate_distance(nodes, range_manager));
+  result->mutable_routing_request()->CopyFrom(request);
+  return true;
+}
+
+// use internal generate result
+void Navigator::generate_passage_region(
+    const std::vector<const TopoNode*>& nodes,
+    const std::unordered_set<const TopoNode*>& black_list,
+    const NodeRangeManager& range_manager,
+    ::adu::common::router::RoutingResult* result) const {
+  std::vector<std::vector<const TopoNode*> > nodes_of_ways;
+  if (FLAGS_use_road_id) {
+    get_nodes_of_ways(nodes, &nodes_of_ways);
+  } else {
+    get_nodes_of_ways_based_on_virtual(nodes, &nodes_of_ways);
+  }
+  size_t num_of_roads = 0;
+  size_t num_of_junctions = 0;
+  for (size_t i = 0; i < nodes_of_ways.size(); ++i) {
+    ROS_INFO("-----------------Way %d-----------------", int(i));
+    const std::vector<const TopoNode*>& nodes_of_way = nodes_of_ways.at(i);
+    if (!nodes_of_way.at(0)->is_virtual()) {
+      std::vector<std::vector<const TopoNode*> > nodes_of_basic_passages;
+      std::vector<RoutingResult_LaneChangeInfo::Type> lane_change_types;
+      extract_basic_passages(nodes_of_way, &nodes_of_basic_passages,
+                             &lane_change_types);
+
+      RoutingResult_Road road;
+      road.set_id("r" + boost::lexical_cast<std::string>(num_of_roads));
+      auto node = nodes_of_basic_passages.front().front();
+      road.mutable_in_lane()->set_id(node->lane_id());
+      road.mutable_in_lane()->set_s(range_manager.get_node_start_s(node));
+      node = nodes_of_basic_passages.back().back();
+      road.mutable_out_lane()->set_id(node->lane_id());
+      road.mutable_out_lane()->set_s(range_manager.get_node_end_s(node));
+      for (const auto& nodes_of_passage : nodes_of_basic_passages) {
+        passage_lane_ids_to_passage_region(nodes_of_passage, range_manager,
+                                           road.add_passage_region());
+        // TODO: set trajectory of region
+      }
+      for (size_t i = 0; i < lane_change_types.size(); ++i) {
+        RoutingResult_LaneChangeInfo* lc_info = road.add_lane_change_info();
+        lc_info->set_type(lane_change_types.at(i));
+        lc_info->set_start_passage_region_index(i);
+        lc_info->set_end_passage_region_index(i + 1);
+      }
+      result->add_route()->mutable_road_info()->CopyFrom(road);
+      num_of_roads++;
+    } else {
+      RoutingResult_Junction junction;
+      junction.set_id("j" + boost::lexical_cast<std::string>(num_of_junctions));
+      junction.set_in_road_id(
+          "r" + boost::lexical_cast<std::string>(num_of_roads - 1));
+      junction.set_out_road_id("r" +
+                               boost::lexical_cast<std::string>(num_of_roads));
+      RoutingResult_PassageRegion region;
+      for (const auto& node : nodes_of_way) {
+        RoutingResult_LaneSegment* seg = region.add_segment();
+        seg->set_id(node->lane_id());
+        NodeRange range = range_manager.get_node_range(node);
+        seg->set_start_s(range.start_s);
+        seg->set_end_s(range.end_s);
+      }
+      // TODO: set trajectory of region
+      junction.mutable_passage_region()->CopyFrom(region);
+      result->add_route()->mutable_junction_info()->CopyFrom(junction);
+      ROS_INFO("Junction passage!!!");
+      for (const auto& node : nodes_of_way) {
+        ROS_INFO("lane id: %s", node->lane_id().c_str());
+      }
+      num_of_junctions++;
+    }
+  }
+}
+
+// old request to old response
+bool Navigator::generate_passage_region(
+    const ::adu::common::routing::RoutingRequest& request,
+    const std::vector<const TopoNode*>& nodes,
+    const std::unordered_set<const TopoNode*>& black_list,
+    const NodeRangeManager& range_manager,
+    ::adu::common::routing::RoutingResult* result) const {
+  result->mutable_header()->set_timestamp_sec(ros::Time::now().toSec());
+  result->mutable_header()->set_module_name(FLAGS_node_name);
+  result->mutable_header()->set_sequence_num(1);
+  for (const auto& node : nodes) {
+    auto* seg = result->add_route();
+    ;
+    seg->set_id(node->lane_id());
+    NodeRange range = range_manager.get_node_range(node);
+    seg->set_start_s(range.start_s);
+    seg->set_end_s(range.end_s);
+  }
+  result->mutable_routing_request()->CopyFrom(request);
+  return true;
+}
+
+void Navigator::dump_debug_data(
+    const std::vector<const TopoNode*>& nodes,
+    const NodeRangeManager& range_manager,
+    const ::adu::common::router::RoutingResult& response) const {
+  std::string debug_string;
+  ROS_INFO("Route lane id\tis virtual\tstart s\tend s");
+  for (const auto& node : nodes) {
+    NodeRange range = range_manager.get_node_range(node);
+    debug_string += node->lane_id() + "," +
+                    boost::lexical_cast<std::string>(node->is_virtual()) + "," +
+                    boost::lexical_cast<std::string>(range.start_s) + "," +
+                    boost::lexical_cast<std::string>(range.end_s) + "\n";
+    ROS_INFO("%s\t%d\t%.2f\t%.2f", node->lane_id().c_str(), node->is_virtual(),
+             range.start_s, range.end_s);
+  }
+
+  std::ofstream fout(FLAGS_debug_route_path);
+  fout << debug_string;
+  fout.close();
+
+  std::string dump_path = FLAGS_debug_passage_region_path;
+  if (!FileUtils::dump_protobuf_data_to_file(dump_path, response)) {
+    ROS_ERROR("Failed to dump passage region debug file.");
+  }
+  ROS_INFO("Passage region debug file is dumped successfully. Dump path: %s.",
+           dump_path.c_str());
+}
+
+}  // namespace routing
+}  // namespace adu

modules/routing/core/navigator.cpp

-/******************************************************************************
-  * 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 "core/navigator.h"
-
-#include <assert.h>
-#include <fstream>
-#include <algorithm>
-
-#include "ros/console.h"
-#include "ros/time.h"
-#include "boost/lexical_cast.hpp"
-
-#include "common/routing_gflags.h"
-#include "common/utils.h"
-#include "strategy/a_star_strategy.h"
-#include "core/black_list_range_generator.h"
-#include "core/routing_result_generator.h"
-#include "core/router_result_generator.h"
-#include "graph/sub_topo_graph.h"
-#include "graph/topo_graph.h"
-#include "graph/topo_edge.h"
-#include "graph/topo_node.h"
-
-namespace adu {
-namespace routing {
-
-namespace {
-
-template <typename T>
-void show_request_info(const T& request) {
-    const auto& start = request.start();
-    const auto& end = request.end();
-    ROS_INFO("Start point:\tlane id: %s s: %f x: %f y: %f",
-             start.id().c_str(),
-             start.s(),
-             start.pose().x(),
-             start.pose().y());
-
-    for (const auto& wp : request.waypoint()) {
-        ROS_INFO("Way Point:\tlane id: %s s: %f x: %f y: %f",
-                wp.id().c_str(),
-                wp.s(),
-                wp.pose().x(),
-                wp.pose().y());
-    }
-
-    for (const auto& bl : request.blacklisted_lane()) {
-        ROS_INFO("Way Point:\tlane id: %s start_s: %f end_s %f",
-                bl.id().c_str(),
-                bl.start_s(),
-                bl.end_s());
-    }
-
-    ROS_INFO("End point:\tlane id: %s s: %f x: %f y: %f",
-             end.id().c_str(),
-             end.s(),
-             end.pose().x(),
-             end.pose().y());
-}
-
-template <typename T>
-bool get_way_nodes(const T& request,
-                   const TopoGraph* graph,
-                   std::vector<const TopoNode*>* const way_nodes,
-                   NodeSRangeManager* const range_manager) {
-    const auto* start_node = graph->get_node(request.start().id());
-    if (start_node == nullptr) {
-        ROS_ERROR("Can't find start point in graph! Id: %s", request.start().id().c_str());
-        return false;
-    }
-    way_nodes->push_back(start_node);
-
-    for (const auto& point : request.waypoint()) {
-        const auto* cur_node = graph->get_node(point.id());
-        if (cur_node == nullptr) {
-            ROS_ERROR("Can't find way point in graph! Id: %s", point.id().c_str());
-            return false;
-        }
-        way_nodes->push_back(cur_node);
-    }
-
-    const auto* end_node = graph->get_node(request.end().id());
-    if (end_node == nullptr) {
-        ROS_ERROR("Can't find end point in graph! Id: %s", request.start().id().c_str());
-        return false;
-    }
-    way_nodes->push_back(end_node);
-
-    range_manager->init_node_range(request.start().s(), request.end().s(), start_node, end_node);
-
-    return true;
-}
-
-bool search_route_by_strategy(const TopoGraph* graph,
-                              const std::vector<const TopoNode*>& way_nodes,
-                              const std::unordered_set<const TopoNode*>& black_list,
-                              std::vector<const TopoNode*>* const result_nodes) {
-    std::unique_ptr<Strategy> strategy_ptr;
-    strategy_ptr.reset(new AStarStrategy());
-
-    result_nodes->clear();
-    for (size_t i = 1; i < way_nodes.size(); ++i) {
-        std::vector<const TopoNode*> cur_result_nodes;
-        const auto* cur_start = way_nodes[i - 1];
-        const auto* cur_end = way_nodes[i];
-        if (!strategy_ptr->search(graph, cur_start, cur_end, black_list, &cur_result_nodes)) {
-            ROS_ERROR("Failed to search route with waypoint from %s to %s",
-                    cur_start->lane_id().c_str(), cur_end->lane_id().c_str());
-            return false;
-        }
-        auto end_iter = cur_result_nodes.end();
-        if (i != way_nodes.size() - 1) {
-            --end_iter;
-        }
-        result_nodes->insert(result_nodes->end(), cur_result_nodes.begin(), end_iter);
-    }
-    return true;
-}
-
-} // namespace
-
-Navigator::Navigator(const std::string& topo_file_path) : _is_ready(false) {
-    ::adu::routing::common::Graph graph;
-    if (!FileUtils::load_protobuf_data_from_file(topo_file_path, &graph)) {
-        ROS_ERROR("Failed to read topology graph from data.");
-        return;
-    }
-
-    _graph.reset(new TopoGraph());
-    if (!_graph->load_graph(graph)) {
-        ROS_INFO("Navigator init graph failed! File path: %s.", topo_file_path.c_str());
-        return;
-    }
-    _black_list_generator.reset(new BlackListRangeGenerator);
-    _routing_result_generator.reset(new RoutingResultGenerator);
-    _router_result_generator.reset(new RouterResultGenerator);
-    _is_ready = true;
-    ROS_INFO("The navigator is ready.");
-};
-
-Navigator::~Navigator() { }
-
-bool Navigator::is_ready() const {
-    return _is_ready;
-}
-
-// search new request to new response
-bool Navigator::search_route(const ::adu::common::router::RoutingRequest& request,
-                             ::adu::common::router::RoutingResult* response) const {
-    if (!is_ready()) {
-        ROS_ERROR("Topo graph is not ready!");
-        return false;
-    }
-    show_request_info(request);
-
-    std::vector<const TopoNode*> way_nodes;
-    NodeSRangeManager range_manager;
-
-    if (!get_way_nodes(request, _graph.get(), &way_nodes, &range_manager)) {
-        ROS_ERROR("Can't find way point in graph!");
-        return false;
-    }
-
-    std::vector<const TopoNode*> result_nodes;
-    std::unordered_set<const TopoNode*> black_list;
-    _black_list_generator->generate_black_set_from_request(request, _graph.get(), &black_list);
-
-    if (!search_route_by_strategy(_graph.get(), way_nodes, black_list, &result_nodes)) {
-        ROS_ERROR("Can't find route from request!");
-        return false;
-    }
-
-    if (!_router_result_generator->generate_passage_region(_graph->map_version(),
-                                                           request,
-                                                           result_nodes,
-                                                           black_list,
-                                                           range_manager,
-                                                           response)) {
-        ROS_ERROR("Failed to generate new passage regions based on route result lane ids");
-        return false;
-    }
-
-    if (FLAGS_enable_debug_mode) {
-        dump_debug_data(result_nodes, range_manager, *response);
-    }
-    return true;
-}
-
-// search old request to old response
-bool Navigator::search_route(const ::adu::common::routing::RoutingRequest& request,
-                             ::adu::common::routing::RoutingResult* response) const {
-    if (!is_ready()) {
-        ROS_ERROR("Topo graph is not ready!");
-        return false;
-    }
-    show_request_info(request);
-
-    std::vector<const TopoNode*> way_nodes;
-    NodeSRangeManager range_manager;
-
-    if (!get_way_nodes(request, _graph.get(), &way_nodes, &range_manager)) {
-        ROS_ERROR("Can't find way point in graph!");
-        return false;
-    }
-
-    std::vector<const TopoNode*> result_nodes;
-    std::unordered_set<const TopoNode*> black_list;
-    _black_list_generator->generate_black_set_from_request(request, _graph.get(), &black_list);
-
-    if (!search_route_by_strategy(_graph.get(), way_nodes, black_list, &result_nodes)) {
-        ROS_ERROR("Can't find route from request!");
-        return false;
-    }
-
-    if (!_routing_result_generator->generate_passage_region(request,
-                                                            result_nodes,
-                                                            black_list,
-                                                            range_manager,
-                                                            response)) {
-        ROS_ERROR("Failed to generate new passage regions based on route result lane ids");
-        return false;
-    }
-
-    if (FLAGS_enable_debug_mode) {
-        ::adu::common::router::RoutingResult new_response;
-        _router_result_generator->generate_passage_region(result_nodes,
-                                                          black_list,
-                                                          range_manager,
-                                                          &new_response);
-        dump_debug_data(result_nodes, range_manager, new_response);
-    }
-
-    return true;
-}
-
-void Navigator::dump_debug_data(const std::vector<const TopoNode*>& nodes,
-                                const NodeSRangeManager& range_manager,
-                                const ::adu::common::router::RoutingResult& response) const {
-    std::string debug_string;
-    ROS_INFO("Route lane id\tis virtual\tstart s\tend s");
-    for (const auto& node : nodes) {
-        NodeSRange range = range_manager.get_node_range(node);
-        debug_string += node->lane_id() + ","
-            + boost::lexical_cast<std::string>(node->is_virtual()) + ","
-            + boost::lexical_cast<std::string>(range.start_s()) + ","
-            + boost::lexical_cast<std::string>(range.end_s()) + "\n";
-        ROS_INFO("%s\t%d\t%.2f\t%.2f", node->lane_id().c_str(),
-                                       node->is_virtual(),
-                                       range.start_s(),
-                                       range.end_s());
-    }
-
-    std::ofstream fout(FLAGS_debug_route_path);
-    fout << debug_string;
-    fout.close();
-
-    std::string dump_path = FLAGS_debug_passage_region_path;
-    if (!FileUtils::dump_protobuf_data_to_file(dump_path, response)) {
-        ROS_ERROR("Failed to dump passage region debug file.");
-    }
-    ROS_INFO("Passage region debug file is dumped successfully. Dump path: %s.",
-             dump_path.c_str());
-}
-
-} // namespace routing
-} // namespace adu
-

modules/routing/core/navigator.h

@@ -19,51 +19,68 @@
 
 #include <memory>
 
-#include "routing.pb.h"
 #include "router.pb.h"
+#include "routing.pb.h"
 
-#include "graph/topo_range_manager.h"
+#include "core/node_range_manager.h"
 
 #include <unordered_set>
 
 namespace adu {
 namespace routing {
 
-class RoutingResultGenerator;
-class RouterResultGenerator;
-class BlackListRangeGenerator;
 class TopoGraph;
 class TopoNode;
 
 class Navigator {
-public:
-    explicit Navigator(const std::string& topo_file_path);
-    ~Navigator();
-    bool is_ready() const;
-
-    // search old request to old response
-    bool search_route(const ::adu::common::routing::RoutingRequest& request,
-                      ::adu::common::routing::RoutingResult* response) const;
-
-    // search new request to new response
-    bool search_route(const ::adu::common::router::RoutingRequest& request,
-                      ::adu::common::router::RoutingResult* response) const;
-
-private:
-    void dump_debug_data(const std::vector<const TopoNode*>& nodes,
-                         const NodeSRangeManager& range_manager,
-                         const ::adu::common::router::RoutingResult& response) const;
-
-private:
-    bool _is_ready;
-    std::unique_ptr<TopoGraph> _graph;
-    std::unique_ptr<BlackListRangeGenerator> _black_list_generator;
-    std::unique_ptr<RoutingResultGenerator> _routing_result_generator;
-    std::unique_ptr<RouterResultGenerator> _router_result_generator;
-};
+ public:
+  explicit Navigator(const std::string& topo_file_path);
+  ~Navigator();
+  bool is_ready() const;
+
+  // search old request to old response
+  bool search_route(const ::adu::common::routing::RoutingRequest& request,
+                    ::adu::common::routing::RoutingResult* response) const;
+
+  // search new request to new response
+  bool search_route(const ::adu::common::router::RoutingRequest& request,
+                    ::adu::common::router::RoutingResult* response) const;
 
-} // namespace routing
-} // namespace adu
+ private:
+  // new request to new response
+  bool generate_passage_region(
+      const ::adu::common::router::RoutingRequest& request,
+      const std::vector<const TopoNode*>& nodes,
+      const std::unordered_set<const TopoNode*>& black_list,
+      const NodeRangeManager& range_manager,
+      ::adu::common::router::RoutingResult* result) const;
+
+  // use internal generate result
+  void generate_passage_region(
+      const std::vector<const TopoNode*>& nodes,
+      const std::unordered_set<const TopoNode*>& black_list,
+      const NodeRangeManager& range_manager,
+      ::adu::common::router::RoutingResult* result) const;
+
+  // old request to old response
+  bool generate_passage_region(
+      const ::adu::common::routing::RoutingRequest& request,
+      const std::vector<const TopoNode*>& nodes,
+      const std::unordered_set<const TopoNode*>& black_list,
+      const NodeRangeManager& range_manager,
+      ::adu::common::routing::RoutingResult* result) const;
+
+  void dump_debug_data(
+      const std::vector<const TopoNode*>& nodes,
+      const NodeRangeManager& range_manager,
+      const ::adu::common::router::RoutingResult& response) const;
+
+ private:
+  bool _is_ready;
+  std::unique_ptr<TopoGraph> _graph;
+};
 
-#endif // BAIDU_ADU_ROUTING_CORE_NAVIGATOR_H
+}  // namespace routing
+}  // namespace adu
 
+#endif  // BAIDU_ADU_ROUTING_CORE_NAVIGATOR_H

modules/routing/core/node_range_manager.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 "core/node_range_manager.h"
+
+#include "graph/topo_edge.h"
+#include "graph/topo_node.h"
+
+namespace adu {
+namespace routing {
+
+void NodeRangeManager::init_node_range(double start_node_s, double end_node_s,
+                                       const TopoNode* start_node,
+                                       const TopoNode* end_node) {
+  _range_map.clear();
+  init_in_neighbor(start_node, start_node_s, start_node->length());
+  init_out_neighbor(start_node, start_node_s, start_node->length());
+
+  init_in_neighbor(end_node, 0.0, end_node_s);
+  init_out_neighbor(end_node, 0.0, end_node_s);
+}
+
+NodeRange NodeRangeManager::get_node_range(const TopoNode* topo_node) const {
+  const auto& iter = _range_map.find(topo_node);
+  if (iter != _range_map.end()) {
+    return iter->second;
+  }
+  return NodeRange(0.0, topo_node->length());
+}
+
+double NodeRangeManager::get_node_start_s(const TopoNode* topo_node) const {
+  return get_node_range(topo_node).start_s;
+}
+
+double NodeRangeManager::get_node_end_s(const TopoNode* topo_node) const {
+  return get_node_range(topo_node).end_s;
+}
+
+void NodeRangeManager::init_in_neighbor(const TopoNode* cur_node,
+                                        double start_s, double end_s) {
+  for (const auto* edge : cur_node->in_from_left_or_right_edge()) {
+    const auto* from_node = edge->from_node();
+    if (_range_map.count(from_node) != 0) {
+      // in case of start and end in the same lane
+      auto& range = _range_map[from_node];
+      range.start_s = std::max(range.start_s, start_s);
+      range.end_s = std::min(range.end_s, end_s);
+    } else {
+      auto& range = _range_map[from_node];
+      range.start_s = start_s;
+      range.end_s = end_s;
+      init_in_neighbor(from_node, start_s, end_s);
+    }
+  }
+}
+
+void NodeRangeManager::init_out_neighbor(const TopoNode* cur_node,
+                                         double start_s, double end_s) {
+  for (const auto* edge : cur_node->out_to_left_or_right_edge()) {
+    const auto* to_node = edge->to_node();
+    if (_range_map.count(to_node) != 0) {
+      // in case of start and end in the same lane
+      auto& range = _range_map[to_node];
+      range.start_s = std::max(range.start_s, start_s);
+      range.end_s = std::min(range.end_s, end_s);
+    } else {
+      auto& range = _range_map[to_node];
+      range.start_s = start_s;
+      range.end_s = end_s;
+      init_out_neighbor(to_node, start_s, end_s);
+    }
+  }
+}
+
+}  // namespace routing
+}  // namespace adu

modules/routing/test/graph/topo_edge_test.cpp → modules/routing/core/node_range_manager.h

@@ -14,48 +14,45 @@
   * limitations under the License.
   *****************************************************************************/
 
-#include <string>
+#ifndef BAIDU_ADU_ROUTING_CORE_NODE_RANGE_MANAGER_H
+#define BAIDU_ADU_ROUTING_CORE_NODE_RANGE_MANAGER_H
 
-#include "test/test_tool/test_utils.h"
+#include <unordered_map>
 
 namespace adu {
 namespace routing {
 
-using ::adu::routing::common::Node;
-using ::adu::routing::common::Edge;
+struct NodeRange {
+  double start_s;
+  double end_s;
+  NodeRange() : start_s(0.0), end_s(0.0) {}
+  NodeRange(double s1, double s2) : start_s(s1), end_s(s2) {}
+};
 
-TEST(TopoEdgeTestSuit, basic_test) {
-    Node node_1;
-    Node node_2;
-    get_node_for_test(&node_1, TEST_L1, TEST_R1);
-    get_node_for_test(&node_2, TEST_L2, TEST_R2);
+class TopoNode;
 
-    TopoNode topo_node_1(node_1);
-    TopoNode topo_node_2(node_2);
-    Edge edge;
-    get_edge_for_test(&edge, TEST_L1, TEST_L2, Edge::FORWARD);
+class NodeRangeManager {
+ public:
+  NodeRangeManager() = default;
+  ~NodeRangeManager() = default;
 
-    const TopoNode* tn_1 = &topo_node_1;
-    const TopoNode* tn_2 = &topo_node_2;
-    TopoEdge topo_edge(edge, tn_1, tn_2);
+  void init_node_range(double start_node_s, double end_node_s,
+                       const TopoNode* start_node, const TopoNode* end_node);
 
-    ASSERT_EQ(edge.DebugString(), topo_edge.edge().DebugString());
-    ASSERT_DOUBLE_EQ(TEST_EDGE_COST, topo_edge.cost());
-    ASSERT_EQ(TEST_L1, topo_edge.from_lane_id());
-    ASSERT_EQ(TEST_L2, topo_edge.to_lane_id());
-    ASSERT_EQ(TET_FORWARD, topo_edge.type());
+  NodeRange get_node_range(const TopoNode* topo_node) const;
+  double get_node_start_s(const TopoNode* topo_node) const;
+  double get_node_end_s(const TopoNode* topo_node) const;
 
-    ASSERT_EQ(tn_1, topo_edge.from_node());
-    ASSERT_EQ(tn_2, topo_edge.to_node());
-}
+ private:
+  void init_in_neighbor(const TopoNode* cur_node, double start_s, double end_s);
+  void init_out_neighbor(const TopoNode* cur_node, double start_s,
+                         double end_s);
+
+ private:
+  std::unordered_map<const TopoNode*, NodeRange> _range_map;
+};
 
 }  // namespace routing
 }  // namespace adu
 
-int main(int argc, char **argv) {
-    testing::InitGoogleTest(&argc, argv);
-    return RUN_ALL_TESTS();
-}
-
-
-
+#endif  // BAIDU_ADU_ROUTING_CORE_NODE_RANGE_MANAGER_H

modules/routing/core/router_result_generator.cpp

-/******************************************************************************
-  * 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 "core/router_result_generator.h"
-
-#include "ros/console.h"
-#include "ros/time.h"
-#include "boost/lexical_cast.hpp"
-
-#include "common/routing_gflags.h"
-#include "graph/topo_node.h"
-#include "graph/topo_edge.h"
-
-namespace adu {
-namespace routing {
-
-namespace {
-
-using ::adu::common::router::RoutingRequest;
-using ::adu::common::router::RoutingResult;
-using ::adu::common::router::RoutingResult_Road;
-using ::adu::common::router::RoutingResult_Junction;
-using ::adu::common::router::RoutingResult_PassageRegion;
-using ::adu::common::router::RoutingResult_LaneSegment;
-using ::adu::common::router::RoutingResult_LaneChangeInfo;
-
-/*const double MAX_ALLOWED_DISTANCE = 100;
-double node2nodes_min_distance(const TopoNode* src_node,
-    const std::vector<const TopoNode*>& dest_nodes) {
-    double min_dis = std::numeric_limits<double>::max();
-    const ::adu::common::hdmap::Point& src_point = src_node->anchor_point();
-    for (const auto& dest_node : dest_nodes) {
-        const ::adu::common::hdmap::Point& dest_point = dest_node->anchor_point();
-        double distance = sqrt(pow(src_point.x() - dest_point.x(), 2)
-                + pow(src_point.y() - dest_point.y(), 2)
-                + pow(src_point.z() - dest_point.z(), 2));
-        min_dis = (distance < min_dis) ? distance : min_dis;
-    }
-    return min_dis;
-}*/
-
-void get_nodes_of_ways_based_on_virtual(const std::vector<const TopoNode*>& nodes,
-                        std::vector<std::vector<const TopoNode*> >* const nodes_of_ways) {
-    ROS_INFO("Cut routing ways based on is_virtual.");
-    assert(!nodes.empty());
-    nodes_of_ways->clear();
-    std::vector<const TopoNode*> nodes_of_way;
-    nodes_of_way.push_back(nodes.at(0));
-    bool last_is_virtual = nodes.at(0)->is_virtual();
-    for (auto iter = nodes.begin() + 1; iter != nodes.end(); ++iter) {
-        if ((*iter)->is_virtual() != last_is_virtual) {
-            nodes_of_ways->push_back(nodes_of_way);
-            nodes_of_way.clear();
-        }
-        nodes_of_way.push_back(*iter);
-        last_is_virtual = (*iter)->is_virtual();
-    }
-    nodes_of_ways->push_back(nodes_of_way);
-}
-
-void get_nodes_of_ways(const std::vector<const TopoNode*>& nodes,
-                       std::vector<std::vector<const TopoNode*> >* const nodes_of_ways) {
-    ROS_INFO("Cut routing ways based on road id.");
-    assert(!nodes.empty());
-    nodes_of_ways->clear();
-    std::vector<const TopoNode*> nodes_of_way;
-    nodes_of_way.push_back(nodes.at(0));
-    std::string last_road_id = nodes.at(0)->road_id();
-    for (auto iter = nodes.begin() + 1; iter != nodes.end(); ++iter) {
-        if ((*iter)->road_id() != last_road_id) {
-            nodes_of_ways->push_back(nodes_of_way);
-            nodes_of_way.clear();
-        }
-        nodes_of_way.push_back(*iter);
-        last_road_id = (*iter)->road_id();
-    }
-    nodes_of_ways->push_back(nodes_of_way);
-}
-
-void extract_basic_passages(const std::vector<const TopoNode*>& nodes,
-                            std::vector<std::vector<const TopoNode*> >* const nodes_of_passages,
-                            std::vector<RoutingResult_LaneChangeInfo::Type>* const
-                                lane_change_types) {
-    assert(!nodes.empty());
-    nodes_of_passages->clear();
-    lane_change_types->clear();
-    std::vector<const TopoNode*> nodes_of_passage;
-    nodes_of_passage.push_back(nodes.at(0));
-    for (auto iter = nodes.begin() + 1; iter != nodes.end(); ++iter) {
-        auto edge = (*(iter - 1))->get_out_edge_to(*iter);
-        if (edge == nullptr) {
-            ROS_ERROR("Get null pointer to edge from %s to %s.", (*(iter - 1))->lane_id().c_str(),
-                                                                 (*iter)->lane_id().c_str());
-            exit(-1);
-        }
-        if (edge->type() == TET_LEFT || edge->type() == TET_RIGHT) {
-            nodes_of_passages->push_back(nodes_of_passage);
-            nodes_of_passage.clear();
-            if (edge->type() == TET_LEFT) {
-                lane_change_types->push_back(RoutingResult_LaneChangeInfo::LEFT_FORWARD);
-            } else {
-                lane_change_types->push_back(RoutingResult_LaneChangeInfo::RIGHT_FORWARD);
-            }
-        }
-        nodes_of_passage.push_back(*iter);
-    }
-    nodes_of_passages->push_back(nodes_of_passage);
-}
-
-bool is_reachable_from(const TopoNode* node,
-                       const std::vector<const TopoNode*>& to_nodes) {
-    for (const auto& to_node : to_nodes) {
-        auto edge = to_node->get_in_edge_from(node);
-        if (edge != nullptr) {
-            if (edge->type() == TET_LEFT || edge->type() == TET_RIGHT) {
-                return true;
-            }
-        }
-    }
-    return false;
-}
-
-bool is_reachable_to(const TopoNode* node,
-                     const std::vector<const TopoNode*>& from_nodes) {
-    for (const auto& from_node : from_nodes) {
-        auto edge = from_node->get_out_edge_to(node);
-        if (edge != nullptr) {
-            if (edge->type() == TET_LEFT || edge->type() == TET_RIGHT) {
-                return true;
-            }
-        }
-    }
-    return false;
-}
-
-void extend_backward(const std::vector<const TopoNode*>& nodes_of_prev_passage,
-                     const std::unordered_set<const TopoNode*>& black_list,
-                     std::vector<const TopoNode*>* const nodes_of_curr_passage) {
-    bool is_reachable = true;
-    while (is_reachable) {
-        is_reachable = false;
-        for (const auto& edge : nodes_of_curr_passage->front()->in_from_pre_edge()) {
-            const auto& pred_node = edge->from_node();
-            // if pred node is not in the same road
-            if (pred_node->road_id() != nodes_of_curr_passage->front()->road_id()) {
-                continue;
-            }
-            // if pred node has been inserted
-            if (find(nodes_of_curr_passage->begin(),
-                     nodes_of_curr_passage->end(),
-                     pred_node) != nodes_of_curr_passage->end()) {
-                continue;
-            }
-            is_reachable = is_reachable_to(pred_node, nodes_of_prev_passage);
-            // if pred node is reachable from prev passage and not in black list
-            if (is_reachable && black_list.find(pred_node) == black_list.end()) {
-                nodes_of_curr_passage->insert(nodes_of_curr_passage->begin(), pred_node);
-                break;
-            // still need to explore other pred nodes
-            } else {
-                continue;
-            }
-        }
-    }
-}
-
-void extend_forward(const std::vector<const TopoNode*>& nodes_of_next_passage,
-                    const std::unordered_set<const TopoNode*>& black_list,
-                    std::vector<const TopoNode*>* const nodes_of_curr_passage) {
-    bool is_reachable = true;
-    while (is_reachable) {
-        is_reachable = false;
-        for (const auto& edge : nodes_of_curr_passage->back()->out_to_suc_edge()) {
-            const auto& succ_node = edge->to_node();
-            // if succ node is not in the same road
-            if (succ_node->road_id() != nodes_of_curr_passage->back()->road_id()) {
-                continue;
-            }
-            // if succ node has been inserted
-            if (find(nodes_of_curr_passage->begin(),
-                        nodes_of_curr_passage->end(),
-                        succ_node) != nodes_of_curr_passage->end()) {
-                continue;
-            }
-            is_reachable = is_reachable_from(succ_node, nodes_of_next_passage);
-            // if next passage is reachable from succ node and not in black list
-            if (is_reachable && black_list.find(succ_node) == black_list.end()) {
-                nodes_of_curr_passage->push_back(succ_node);
-                break;
-            // still need to explore other succ nodes
-            } else {
-                continue;
-            }
-        }
-    }
-}
-
-void extend_passages(const std::unordered_set<const TopoNode*>& black_list,
-                     std::vector<std::vector<const TopoNode*> >* const nodes_of_passages) {
-    for (size_t i = 0; i < nodes_of_passages->size(); ++i) {
-        if (i < nodes_of_passages->size() - 1) {
-            extend_forward(nodes_of_passages->at(i + 1),
-                           black_list,
-                           &(nodes_of_passages->at(i)));
-        }
-        if (i > 0) {
-            extend_backward(nodes_of_passages->at(i - 1),
-                            black_list,
-                            &(nodes_of_passages->at(i)));
-        }
-    }
-}
-
-void passage_lane_ids_to_passage_region(const std::vector<const TopoNode*>& nodes,
-                                        const NodeSRangeManager& range_manager,
-                                        RoutingResult_PassageRegion* const region) {
-    for (const auto& node : nodes) {
-        RoutingResult_LaneSegment* seg = region->add_segment();
-        seg->set_id(node->lane_id());
-        NodeSRange range = range_manager.get_node_range(node);
-        seg->set_start_s(range.start_s());
-        seg->set_end_s(range.end_s());
-    }
-}
-
-double calculate_distance(const std::vector<const TopoNode*>& nodes,
-                          const NodeSRangeManager& range_manager) {
-    NodeSRange range = range_manager.get_node_range(nodes.at(0));
-    double distance = range.end_s() - range.start_s();
-    for (auto iter = nodes.begin() + 1; iter != nodes.end(); ++iter) {
-        auto edge = (*(iter - 1))->get_out_edge_to(*iter);
-        if (edge->type() != TET_FORWARD) {
-            continue;
-        }
-        range = range_manager.get_node_range(*iter);
-        distance += range.end_s() - range.start_s();
-    }
-    return distance;
-}
-
-} // namespace
-
-// new request to new response
-bool RouterResultGenerator::generate_passage_region(
-                                        const std::string& map_version,
-                                        const ::adu::common::router::RoutingRequest& request,
-                                        const std::vector<const TopoNode*>& nodes,
-                                        const std::unordered_set<const TopoNode*>& black_list,
-                                        const NodeSRangeManager& range_manager,
-                                        ::adu::common::router::RoutingResult* result) const {
-    result->mutable_header()->set_timestamp_sec(ros::Time::now().toSec());
-    result->mutable_header()->set_module_name(FLAGS_node_name);
-    result->mutable_header()->set_sequence_num(1);
-
-    generate_passage_region(nodes, black_list, range_manager, result);
-
-    result->set_map_version(map_version);
-    result->mutable_measurement()->set_distance(calculate_distance(nodes, range_manager));
-    result->mutable_routing_request()->CopyFrom(request);
-    return true;
-}
-
-// use internal generate result
-void RouterResultGenerator::generate_passage_region(const std::vector<const TopoNode*>& nodes,
-                                        const std::unordered_set<const TopoNode*>& black_list,
-                                        const NodeSRangeManager& range_manager,
-                                        ::adu::common::router::RoutingResult* result) const {
-    std::vector<std::vector<const TopoNode*> > nodes_of_ways;
-    if (FLAGS_use_road_id) {
-        get_nodes_of_ways(nodes, &nodes_of_ways);
-    } else {
-        get_nodes_of_ways_based_on_virtual(nodes, &nodes_of_ways);
-    }
-    size_t num_of_roads = 0;
-    size_t num_of_junctions = 0;
-    for (size_t i = 0; i < nodes_of_ways.size(); ++i) {
-        ROS_INFO("-----------------Way %d-----------------", int(i));
-        const std::vector<const TopoNode*>& nodes_of_way = nodes_of_ways.at(i);
-        if (!nodes_of_way.at(0)->is_virtual()) {
-            std::vector<std::vector<const TopoNode*> > nodes_of_basic_passages;
-            std::vector<RoutingResult_LaneChangeInfo::Type> lane_change_types;
-            extract_basic_passages(nodes_of_way,
-                                   &nodes_of_basic_passages,
-                                   &lane_change_types);
-            for (const auto& nodes_of_passage : nodes_of_basic_passages) {
-                ROS_INFO("New passage!!!");
-                for (const auto& node : nodes_of_passage) {
-                    ROS_INFO("lane id: %s", node->lane_id().c_str());
-                }
-            }
-            extend_passages(black_list, &nodes_of_basic_passages);
-            ROS_INFO("------After extend--------");
-            for (const auto& nodes_of_passage : nodes_of_basic_passages) {
-                ROS_INFO("New passage!!!");
-                for (const auto& node : nodes_of_passage) {
-                    ROS_INFO("lane id: %s", node->lane_id().c_str());
-                }
-            }
-
-            RoutingResult_Road road;
-            road.set_id("r" + boost::lexical_cast<std::string>(num_of_roads));
-            auto node = nodes_of_basic_passages.front().front();
-            road.mutable_in_lane()->set_id(node->lane_id());
-            road.mutable_in_lane()->set_s(range_manager.get_node_start_s(node));
-            node = nodes_of_basic_passages.back().back();
-            road.mutable_out_lane()->set_id(node->lane_id());
-            road.mutable_out_lane()->set_s(range_manager.get_node_end_s(node));
-            for (const auto& nodes_of_passage : nodes_of_basic_passages) {
-                passage_lane_ids_to_passage_region(nodes_of_passage,
-                                                   range_manager,
-                                                   road.add_passage_region());
-                // TODO: set trajectory of region
-            }
-            for (size_t i = 0; i < lane_change_types.size(); ++i) {
-                RoutingResult_LaneChangeInfo* lc_info = road.add_lane_change_info();
-                lc_info->set_type(lane_change_types.at(i));
-                lc_info->set_start_passage_region_index(i);
-                lc_info->set_end_passage_region_index(i + 1);
-            }
-            result->add_route()->mutable_road_info()->CopyFrom(road);
-            num_of_roads++;
-        } else {
-            RoutingResult_Junction junction;
-            junction.set_id("j" + boost::lexical_cast<std::string>(num_of_junctions));
-            junction.set_in_road_id("r" + boost::lexical_cast<std::string>(num_of_roads - 1));
-            junction.set_out_road_id("r" + boost::lexical_cast<std::string>(num_of_roads));
-            RoutingResult_PassageRegion region;
-            for (const auto& node : nodes_of_way) {
-                RoutingResult_LaneSegment* seg = region.add_segment();
-                seg->set_id(node->lane_id());
-                NodeSRange range = range_manager.get_node_range(node);
-                seg->set_start_s(range.start_s());
-                seg->set_end_s(range.end_s());
-            }
-            // TODO: set trajectory of region
-            junction.mutable_passage_region()->CopyFrom(region);
-            result->add_route()->mutable_junction_info()->CopyFrom(junction);
-            ROS_INFO("Junction passage!!!");
-            for (const auto& node : nodes_of_way) {
-                ROS_INFO("lane id: %s", node->lane_id().c_str());
-            }
-            num_of_junctions++;
-        }
-    }
-}
-
-} // namespace routing
-} // namespace adu
-

modules/routing/core/router_result_generator.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 BAIDU_ADU_ROUTER_CORE_ROUTER_RESULT_GENERATOR_H
-#define BAIDU_ADU_ROUTER_CORE_ROUTER_RESULT_GENERATOR_H
-
-#include <memory>
-#include <unordered_set>
-
-#include "router.pb.h"
-
-#include "graph/topo_range_manager.h"
-
-namespace adu {
-namespace routing {
-
-class TopoGraph;
-class TopoNode;
-
-class RouterResultGenerator {
-public:
-    RouterResultGenerator() = default;
-    ~RouterResultGenerator() = default;
-
-    // new request to new response
-    bool generate_passage_region(const std::string& map_version,
-                                 const ::adu::common::router::RoutingRequest& request,
-                                 const std::vector<const TopoNode*>& nodes,
-                                 const std::unordered_set<const TopoNode*>& black_list,
-                                 const NodeSRangeManager& range_manager,
-                                 ::adu::common::router::RoutingResult* result) const;
-
-    // use internal generate result
-    void generate_passage_region(const std::vector<const TopoNode*>& nodes,
-                                 const std::unordered_set<const TopoNode*>& black_list,
-                                 const NodeSRangeManager& range_manager,
-                                 ::adu::common::router::RoutingResult* result) const;
-};
-
-} // namespace routing
-} // namespace adu
-
-#endif // BAIDU_ADU_ROUTER_CORE_ROUTER_RESULT_GENERATOR_H
-

modules/routing/core/routing_result_generator.cpp

-/******************************************************************************
-  * 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 "core/routing_result_generator.h"
-
-#include "ros/time.h"
-
-#include "common/routing_gflags.h"
-#include "graph/topo_node.h"
-
-namespace adu {
-namespace routing {
-
-using ::adu::common::routing::RoutingRequest;
-using ::adu::common::routing::RoutingResult;
-
-// old request to old response
-bool RoutingResultGenerator::generate_passage_region(
-                                const RoutingRequest& request,
-                                const std::vector<const TopoNode*>& nodes,
-                                const std::unordered_set<const TopoNode*>& black_list,
-                                const NodeSRangeManager& range_manager,
-                                RoutingResult* result) const {
-    result->mutable_header()->set_timestamp_sec(ros::Time::now().toSec());
-    result->mutable_header()->set_module_name(FLAGS_node_name);
-    result->mutable_header()->set_sequence_num(1);
-    for (const auto& node : nodes) {
-        auto* seg = result->add_route();;
-        seg->set_id(node->lane_id());
-        NodeSRange range = range_manager.get_node_range(node);
-        seg->set_start_s(range.start_s());
-        seg->set_end_s(range.end_s());
-    }
-    result->mutable_routing_request()->CopyFrom(request);
-    return true;
-
-}
-
-} // namespace routing
-} // namespace adu
-

modules/routing/core/routing_result_generator.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 BAIDU_ADU_ROUTING_CORE_ROUTING_RESULT_GENERATOR_H
-#define BAIDU_ADU_ROUTING_CORE_ROUTING_RESULT_GENERATOR_H
-
-#include <memory>
-#include <unordered_set>
-
-#include "routing.pb.h"
-
-#include "graph/topo_range_manager.h"
-
-namespace adu {
-namespace routing {
-
-class TopoGraph;
-class TopoNode;
-
-class RoutingResultGenerator {
-public:
-    RoutingResultGenerator() = default;
-    ~RoutingResultGenerator() = default;
-
-    // old request to old response
-    bool generate_passage_region(const ::adu::common::routing::RoutingRequest& request,
-                                 const std::vector<const TopoNode*>& nodes,
-                                 const std::unordered_set<const TopoNode*>& black_list,
-                                 const NodeSRangeManager& range_manager,
-                                 ::adu::common::routing::RoutingResult* result) const;
-};
-
-} // namespace routing
-} // namespace adu
-
-#endif // BAIDU_ADU_ROUTING_CORE_ROUTING_RESULT_GENERATOR_H
-

modules/routing/graph/sub_topo_graph.cpp

-/******************************************************************************
-  * 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 "graph/sub_topo_graph.h"
-
-#include <cmath>
-
-#include "graph/topo_edge.h"
-#include "graph/topo_node.h"
-
-#include "glog/logging.h"
-
-namespace adu {
-namespace routing {
-
-namespace {
-
-void merge_block_range(const TopoNode* topo_node,
-                       const std::vector<NodeSRange>& origin_range,
-                       std::vector<NodeSRange>* block_range) {
-    std::vector<NodeSRange> sorted_origin_range;
-    sorted_origin_range.insert(sorted_origin_range.end(),
-                               origin_range.begin(),
-                               origin_range.end());
-    sort(sorted_origin_range.begin(), sorted_origin_range.end());
-    int cur_index = 0;
-    int total_size = sorted_origin_range.size();
-    while (cur_index < total_size) {
-        NodeSRange range(sorted_origin_range[cur_index]);
-        ++cur_index;
-        while (cur_index < total_size &&
-               range.merge_range_overlap(sorted_origin_range[cur_index])) {
-            ++cur_index;
-        }
-        if (range.end_s() < topo_node->start_s() || range.start_s() > topo_node->end_s()) {
-            continue;
-        }
-        range.set_start_s(std::max(topo_node->start_s(), range.start_s()));
-        range.set_end_s(std::min(topo_node->end_s(), range.end_s()));
-        block_range->push_back(std::move(range));
-    }
-}
-
-void get_sorted_valid_range(const TopoNode* topo_node,
-                            const std::vector<NodeSRange>& origin_range,
-                            std::vector<NodeSRange>* valid_range) {
-    std::vector<NodeSRange> block_range;
-    merge_block_range(topo_node, origin_range, &block_range);
-    double start_s = topo_node->start_s();
-    double end_s = topo_node->end_s();
-    std::vector<double> all_value;
-    all_value.push_back(start_s);
-    for (const auto& range : block_range) {
-        all_value.push_back(range.start_s());
-        all_value.push_back(range.end_s());
-    }
-    all_value.push_back(end_s);
-    for (size_t i = 0; i < all_value.size(); i += 2) {
-        if (!NodeSRange::is_enough_for_change_lane(all_value[i], all_value[i + 1])) {
-            continue;
-        }
-        NodeSRange new_range(all_value[i], all_value[i + 1]);
-        valid_range->push_back(std::move(new_range));
-    }
-}
-
-} // namespace
-
-SubTopoGraph::SubTopoGraph(const std::vector<NodeWithRange>& black_list) {
-    std::unordered_map<const TopoNode*, std::vector<NodeSRange> > black_map;
-    for (const auto& node_in_bl : black_list) {
-        black_map[node_in_bl.topo_node()].push_back(node_in_bl);
-    }
-    std::vector<NodeSRange> valid_range;
-    for (auto& map_iter : black_map) {
-        valid_range.clear();
-        get_sorted_valid_range(map_iter.first, map_iter.second, &valid_range);
-        init_sub_node_by_valid_range(map_iter.first, valid_range);
-    }
-
-    for (const auto& map_iter : black_map) {
-        init_sub_edge(map_iter.first);
-    }
-}
-
-SubTopoGraph::~SubTopoGraph() { }
-
-void SubTopoGraph::get_sub_edges_into_sub_graph(const TopoEdge* edge,
-                                 std::unordered_set<const TopoEdge*>* const sub_edges) const {
-    const auto* from_node = edge->from_node();
-    const auto* to_node = edge->to_node();
-    std::unordered_set<TopoNode*> sub_nodes;
-    if (from_node->is_sub_node() || to_node->is_sub_node() || !get_sub_nodes(to_node, &sub_nodes)) {
-        sub_edges->insert(edge);
-        return;
-    }
-    for (const auto* sub_node : sub_nodes) {
-        for (const auto* in_edge : sub_node->in_from_all_edge()) {
-            if (in_edge->from_node() == from_node) {
-                sub_edges->insert(in_edge);
-            }
-        }
-    }
-}
-
-const TopoNode* SubTopoGraph::get_sub_node_with_s(const TopoNode* topo_node, double s) const {
-    const auto& map_iter = _sub_node_range_sorted_map.find(topo_node);
-    if (map_iter == _sub_node_range_sorted_map.end()) {
-        return topo_node;
-    }
-    const auto& sorted_vec = map_iter->second;
-    // sorted vec can't be empty!
-    int index = binary_search_for_start_s(sorted_vec, s);
-    if (index < 0) {
-        return nullptr;
-    }
-    return sorted_vec[index].topo_node();
-}
-
-void SubTopoGraph::init_sub_node_by_valid_range(const TopoNode* topo_node,
-                                                const std::vector<NodeSRange>& valid_range) {
-    // Attention: no matter topo node has valid_range or not,
-    // create map value first;
-    auto& sub_node_vec = _sub_node_range_sorted_map[topo_node];
-    auto& sub_node_set = _sub_node_map[topo_node];
-    for (const auto& range : valid_range) {
-        std::shared_ptr<TopoNode> sub_topo_node_ptr;
-        sub_topo_node_ptr.reset(new TopoNode(topo_node, range));
-        NodeWithRange node_with_range(range, sub_topo_node_ptr.get());
-        sub_node_vec.push_back(std::move(node_with_range));
-        sub_node_set.insert(sub_topo_node_ptr.get());
-        _topo_nodes.push_back(std::move(sub_topo_node_ptr));
-    }
-}
-
-void SubTopoGraph::init_sub_edge(const TopoNode* topo_node) {
-    std::unordered_set<TopoNode*> sub_nodes;
-    if (!get_sub_nodes(topo_node, &sub_nodes)) {
-        return;
-    }
-    double start_s = topo_node->start_s();
-    double end_s = topo_node->end_s();
-    const double MIN_DIFF_LENGTH = 0.1e-6;
-
-    for (auto* sub_node : sub_nodes) {
-        init_in_sub_node_sub_edge(sub_node, topo_node->in_from_left_or_right_edge());
-        init_out_sub_node_sub_edge(sub_node, topo_node->out_to_left_or_right_edge());
-        if (std::fabs(sub_node->start_s() - start_s) < MIN_DIFF_LENGTH) {
-            init_in_sub_node_sub_edge(sub_node, topo_node->in_from_pre_edge());
-        }
-        if (std::fabs(end_s - sub_node->end_s()) < MIN_DIFF_LENGTH) {
-            init_out_sub_node_sub_edge(sub_node, topo_node->out_to_suc_edge());
-        }
-    }
-}
-
-void SubTopoGraph::init_in_sub_edge(const TopoNode* sub_node,
-                                    const std::unordered_set<const TopoEdge*> origin_edge) {
-    std::unordered_set<TopoNode*> other_sub_nodes;
-    for (const auto* in_edge : origin_edge) {
-        if (get_sub_nodes(in_edge->from_node(), &other_sub_nodes)) {
-            for (auto* sub_from_node : other_sub_nodes) {
-                if (!sub_from_node->is_overlap_enough(sub_node, in_edge)) {
-                    continue;
-                }
-                std::shared_ptr<TopoEdge> topo_edge_ptr;
-                topo_edge_ptr.reset(new TopoEdge(in_edge->edge(), sub_from_node, sub_node));
-                sub_from_node->add_out_edge(topo_edge_ptr.get());
-                _topo_edges.push_back(std::move(topo_edge_ptr));
-            }
-        }
-    }
-}
-
-void SubTopoGraph::init_in_sub_node_sub_edge(TopoNode* const sub_node,
-                                const std::unordered_set<const TopoEdge*> origin_edge) {
-    std::unordered_set<TopoNode*> other_sub_nodes;
-    for (const auto* in_edge : origin_edge) {
-        if (get_sub_nodes(in_edge->from_node(), &other_sub_nodes)) {
-            for (auto* sub_from_node : other_sub_nodes) {
-                if (!sub_from_node->is_overlap_enough(sub_node, in_edge)) {
-                    continue;
-                }
-                std::shared_ptr<TopoEdge> topo_edge_ptr;
-                topo_edge_ptr.reset(new TopoEdge(in_edge->edge(), sub_from_node, sub_node));
-                sub_node->add_in_edge(topo_edge_ptr.get());
-                sub_from_node->add_out_edge(topo_edge_ptr.get());
-                _topo_edges.push_back(std::move(topo_edge_ptr));
-            }
-        } else {
-            std::shared_ptr<TopoEdge> topo_edge_ptr;
-            topo_edge_ptr.reset(new TopoEdge(in_edge->edge(), in_edge->from_node(), sub_node));
-            sub_node->add_in_edge(topo_edge_ptr.get());
-            _topo_edges.push_back(std::move(topo_edge_ptr));
-        }
-    }
-}
-
-void SubTopoGraph::init_out_sub_edge(const TopoNode* sub_node,
-                                     const std::unordered_set<const TopoEdge*> origin_edge) {
-    std::unordered_set<TopoNode*> other_sub_nodes;
-    for (const auto* out_edge : origin_edge) {
-        if (get_sub_nodes(out_edge->to_node(), &other_sub_nodes)) {
-            for (auto* sub_to_node : other_sub_nodes) {
-                if (!sub_node->is_overlap_enough(sub_to_node, out_edge)) {
-                    continue;
-                }
-                std::shared_ptr<TopoEdge> topo_edge_ptr;
-                topo_edge_ptr.reset(new TopoEdge(out_edge->edge(), sub_node, sub_to_node));
-                sub_to_node->add_out_edge(topo_edge_ptr.get());
-                _topo_edges.push_back(std::move(topo_edge_ptr));
-            }
-        }
-    }
-}
-
-void SubTopoGraph::init_out_sub_node_sub_edge(TopoNode* const sub_node,
-                                const std::unordered_set<const TopoEdge*> origin_edge) {
-    std::unordered_set<TopoNode*> other_sub_nodes;
-    for (const auto* out_edge : origin_edge) {
-        if (get_sub_nodes(out_edge->to_node(), &other_sub_nodes)) {
-            for (auto* sub_to_node : other_sub_nodes) {
-                if (!sub_node->is_overlap_enough(sub_to_node, out_edge)) {
-                    continue;
-                }
-                std::shared_ptr<TopoEdge> topo_edge_ptr;
-                topo_edge_ptr.reset(new TopoEdge(out_edge->edge(), sub_node, sub_to_node));
-                sub_to_node->add_out_edge(topo_edge_ptr.get());
-                sub_node->add_out_edge(topo_edge_ptr.get());
-                _topo_edges.push_back(std::move(topo_edge_ptr));
-            }
-        } else {
-            std::shared_ptr<TopoEdge> topo_edge_ptr;
-            topo_edge_ptr.reset(new TopoEdge(out_edge->edge(), sub_node, out_edge->to_node()));
-            sub_node->add_out_edge(topo_edge_ptr.get());
-            _topo_edges.push_back(std::move(topo_edge_ptr));
-        }
-    }
-}
-
-bool SubTopoGraph::get_sub_nodes(const TopoNode* node,
-                                 std::unordered_set<TopoNode*>* const sub_nodes) const {
-    const auto& iter = _sub_node_map.find(node);
-    if (iter == _sub_node_map.end()) {
-        return false;
-    }
-    sub_nodes->clear();
-    sub_nodes->insert(iter->second.begin(), iter->second.end());
-    return true;
-}
-
-} // namespace routing
-} // namespace adu
-

modules/routing/graph/sub_topo_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 BAIDU_ADU_ROUTING_GRAPH_SUB_TOPO_GRAPH_H
-#define BAIDU_ADU_ROUTING_GRAPH_SUB_TOPO_GRAPH_H
-
-#include <unordered_map>
-#include <unordered_set>
-#include <vector>
-#include <memory>
-#include <queue>
-
-#include "graph/topo_graph.h"
-#include "graph/topo_range.h"
-
-namespace adu {
-namespace routing {
-
-class TopoNode;
-class TopoEdge;
-class TopoGraph;
-
-class SubTopoGraph {
-public:
-    explicit SubTopoGraph(const std::vector<NodeWithRange>& black_list);
-    ~SubTopoGraph();
-
-    // edge: A -> B         not sub edge
-    // 1. A has no sub node, B has no sub node
-    //      return origin edge A -> B
-    // 2. A has no sub node, B has sub node
-    //      return all edge A -> B'
-    //    if B is black lane and no valid edge,
-    //      return empty set
-    // 3. A has sub node, B has sub node
-    //      return empty set
-    // 4. A has sub node, B has no sub node
-    //      return empty set
-    // edge: A -> B is sub edge
-    // 1. return empty set
-    void get_sub_edges_into_sub_graph(const TopoEdge* edge,
-                       std::unordered_set<const TopoEdge*>* const sub_edges) const;
-
-    const TopoNode* get_sub_node_with_s(const TopoNode* topo_node, double s) const;
-
-private:
-    void init_sub_node_by_valid_range(const TopoNode* topo_node,
-                                      const std::vector<NodeSRange>& valid_range);
-    void init_sub_edge(const TopoNode* topo_node);
-
-    void init_in_sub_edge(const TopoNode* sub_node,
-                          const std::unordered_set<const TopoEdge*> origin_edge);
-    void init_in_sub_node_sub_edge(TopoNode* const sub_node,
-                                   const std::unordered_set<const TopoEdge*> origin_edge);
-    void init_out_sub_edge(const TopoNode* sub_node,
-                           const std::unordered_set<const TopoEdge*> origin_edge);
-    void init_out_sub_node_sub_edge(TopoNode* const sub_node,
-                                    const std::unordered_set<const TopoEdge*> origin_edge);
-
-    bool get_sub_nodes(const TopoNode* node, std::unordered_set<TopoNode*>* const sub_nodes) const;
-
-private:
-    std::vector<std::shared_ptr<TopoNode> > _topo_nodes;
-    std::vector<std::shared_ptr<TopoEdge> > _topo_edges;
-    std::unordered_map<const TopoNode*, std::vector<NodeWithRange> > _sub_node_range_sorted_map;
-    std::unordered_map<const TopoNode*, std::unordered_set<TopoNode*> > _sub_node_map;
-};
-
-} // namespace routing
-} // namespace adu
-
-#endif // BAIDU_ADU_ROUTING_GRAPH_SUB_TOPO_GRAPH_H
-

modules/routing/graph/topo_edge.cpp → modules/routing/graph/topo_edge.cc

@@ -26,47 +26,36 @@ namespace routing {
 using ::adu::routing::common::Edge;
 
 TopoEdge::TopoEdge(const ::adu::routing::common::Edge& edge,
-                   const TopoNode* from_node,
-                   const TopoNode* to_node) :
-        _pb_edge(edge), _from_node(from_node), _to_node(to_node) { }
+                   const TopoNode* from_node, const TopoNode* to_node)
+    : _pb_edge(edge), _from_node(from_node), _to_node(to_node) {}
 
-TopoEdge::~TopoEdge() {
-}
+TopoEdge::~TopoEdge() {}
 
-const Edge& TopoEdge::edge() const {
-    return _pb_edge;
-}
+const Edge& TopoEdge::edge() const { return _pb_edge; }
 
-double TopoEdge::cost() const {
-    return _pb_edge.cost();
-}
+double TopoEdge::cost() const { return _pb_edge.cost(); }
 
-const TopoNode* TopoEdge::from_node() const {
-    return _from_node;
-}
+const TopoNode* TopoEdge::from_node() const { return _from_node; }
 
-const TopoNode* TopoEdge::to_node() const {
-    return _to_node;
-}
+const TopoNode* TopoEdge::to_node() const { return _to_node; }
 
 const std::string& TopoEdge::from_lane_id() const {
-    return _pb_edge.from_lane_id();
+  return _pb_edge.from_lane_id();
 }
 
 const std::string& TopoEdge::to_lane_id() const {
-    return _pb_edge.to_lane_id();
+  return _pb_edge.to_lane_id();
 }
 
 TopoEdgeType TopoEdge::type() const {
-    if (_pb_edge.direction_type() == ::adu::routing::common::Edge::LEFT) {
-        return TET_LEFT;
-    }
-    if (_pb_edge.direction_type() == ::adu::routing::common::Edge::RIGHT) {
-        return TET_RIGHT;
-    }
-    return TET_FORWARD;
+  if (_pb_edge.direction_type() == ::adu::routing::common::Edge::LEFT) {
+    return TET_LEFT;
+  }
+  if (_pb_edge.direction_type() == ::adu::routing::common::Edge::RIGHT) {
+    return TET_RIGHT;
+  }
+  return TET_FORWARD;
 }
 
-} // namespace routing
-} // namespace adu
-
+}  // namespace routing
+}  // namespace adu

modules/routing/graph/topo_edge.h

@@ -24,38 +24,36 @@ namespace adu {
 namespace routing {
 
 enum TopoEdgeType {
-    TET_FORWARD,
-    TET_LEFT,
-    TET_RIGHT,
+  TET_FORWARD,
+  TET_LEFT,
+  TET_RIGHT,
 };
 
 class TopoNode;
 
 class TopoEdge {
-public:
-    TopoEdge(const ::adu::routing::common::Edge& edge,
-             const TopoNode* from_node,
-             const TopoNode* to_node);
-
-    ~TopoEdge();
-
-    const ::adu::routing::common::Edge& edge() const;
-    double cost() const;
-    const std::string& from_lane_id() const;
-    const std::string& to_lane_id() const;
-    TopoEdgeType type() const;
-
-    const TopoNode* from_node() const;
-    const TopoNode* to_node() const;
-
-private:
-    ::adu::routing::common::Edge _pb_edge;
-    const TopoNode* _from_node;
-    const TopoNode* _to_node;
-};
+ public:
+  TopoEdge(const ::adu::routing::common::Edge& edge, const TopoNode* from_node,
+           const TopoNode* to_node);
+
+  ~TopoEdge();
+
+  const ::adu::routing::common::Edge& edge() const;
+  double cost() const;
+  const std::string& from_lane_id() const;
+  const std::string& to_lane_id() const;
+  TopoEdgeType type() const;
 
-} // namespace routing
-} // namespace adu
+  const TopoNode* from_node() const;
+  const TopoNode* to_node() const;
+
+ private:
+  ::adu::routing::common::Edge _pb_edge;
+  const TopoNode* _from_node;
+  const TopoNode* _to_node;
+};
 
-#endif // BAIDU_ADU_ROUTING_GRAPH_TOPO_EDGE_H
+}  // namespace routing
+}  // namespace adu
 
+#endif  // BAIDU_ADU_ROUTING_GRAPH_TOPO_EDGE_H

modules/routing/graph/topo_graph.cpp → modules/routing/graph/topo_graph.cc

@@ -14,103 +14,108 @@
   * limitations under the License.
   *****************************************************************************/
 
-#include "ros/ros.h"
-#include "common/utils.h"
 #include "graph/topo_graph.h"
-#include "graph/topo_node.h"
+#include "common/utils.h"
 #include "graph/topo_edge.h"
+#include "graph/topo_node.h"
+#include "ros/ros.h"
 
 namespace adu {
 namespace routing {
 
 void TopoGraph::clear() {
-    _topo_nodes.clear();
-    _topo_edges.clear();
-    _node_index_map.clear();
+  _topo_nodes.clear();
+  _topo_edges.clear();
+  _node_index_map.clear();
 }
 
 bool TopoGraph::load_nodes(const ::adu::routing::common::Graph& graph) {
-    if (graph.node_size() == 0) {
-        ROS_ERROR("No nodes found in topology graph.");
-        return false;
-    }
-    for (const auto& node : graph.node()) {
-        _node_index_map[node.lane_id()] = _topo_nodes.size();
-        std::shared_ptr<TopoNode> topo_node;
-        topo_node.reset(new TopoNode(node));
-        _road_node_map[node.road_id()].insert(topo_node.get());
-        _topo_nodes.push_back(std::move(topo_node));
-    }
-    return true;
+  if (graph.node_size() == 0) {
+    ROS_ERROR("No nodes found in topology graph.");
+    return false;
+  }
+  for (const auto& node : graph.node()) {
+    _node_index_map[node.lane_id()] = _topo_nodes.size();
+    std::shared_ptr<TopoNode> topo_node;
+    topo_node.reset(new TopoNode(node));
+    _road_node_map[node.road_id()].insert(topo_node.get());
+    _topo_nodes.push_back(std::move(topo_node));
+  }
+  return true;
 }
 
 // Need to execute load_nodes() firstly
 bool TopoGraph::load_edges(const ::adu::routing::common::Graph& graph) {
-    if (graph.edge_size() == 0) {
-        ROS_ERROR("No edges found in topology graph.");
-        return false;
-    }
-    for (const auto& edge : graph.edge()) {
-        const std::string& from_lane_id = edge.from_lane_id();
-        const std::string& to_lane_id = edge.to_lane_id();
-        if (_node_index_map.count(from_lane_id) != 1 || _node_index_map.count(to_lane_id) != 1) {
-            ROS_ERROR("Can't find edge: %s -> %s.", from_lane_id.c_str(), to_lane_id.c_str());
-            return false;
-        }
-        std::shared_ptr<TopoEdge> topo_edge;
-        TopoNode* from_node = _topo_nodes[_node_index_map[from_lane_id]].get();
-        TopoNode* to_node = _topo_nodes[_node_index_map[to_lane_id]].get();
-        topo_edge.reset(new TopoEdge(edge, from_node, to_node));
-        from_node->add_out_edge(topo_edge.get());
-        to_node->add_in_edge(topo_edge.get());
-        _topo_edges.push_back(std::move(topo_edge));
+  if (graph.edge_size() == 0) {
+    ROS_ERROR("No edges found in topology graph.");
+    return false;
+  }
+  for (const auto& edge : graph.edge()) {
+    const std::string& from_lane_id = edge.from_lane_id();
+    const std::string& to_lane_id = edge.to_lane_id();
+    if (_node_index_map.count(from_lane_id) != 1 ||
+        _node_index_map.count(to_lane_id) != 1) {
+      ROS_ERROR("Can't find edge: %s -> %s.", from_lane_id.c_str(),
+                to_lane_id.c_str());
+      return false;
     }
-    return true;
+    std::shared_ptr<TopoEdge> topo_edge;
+    TopoNode* from_node = _topo_nodes[_node_index_map[from_lane_id]].get();
+    TopoNode* to_node = _topo_nodes[_node_index_map[to_lane_id]].get();
+    topo_edge.reset(new TopoEdge(edge, from_node, to_node));
+    from_node->add_out_edge(topo_edge.get());
+    to_node->add_in_edge(topo_edge.get());
+    _topo_edges.push_back(std::move(topo_edge));
+  }
+  return true;
 }
 
-bool TopoGraph::load_graph(const ::adu::routing::common::Graph& graph) {
-    clear();
+bool TopoGraph::load_graph(const std::string& file_path) {
+  clear();
 
-    _map_version = graph.hdmap_version();
-    _map_district = graph.hdmap_district();
-    ROS_INFO("Use map district: %s, version: %s", _map_district.c_str(), _map_version.c_str());
+  ::adu::routing::common::Graph graph;
+  if (!::adu::routing::FileUtils::load_protobuf_data_from_file(file_path,
+                                                               &graph)) {
+    ROS_ERROR("Failed to read topology graph from data.");
+    return false;
+  }
 
-    if (!load_nodes(graph)) {
-        ROS_ERROR("Failed to load nodes from topology graph.");
-        return false;
-    }
-    if (!load_edges(graph)) {
-        ROS_ERROR("Failed to load edges from topology graph.");
-        return false;
-    }
-    return true;
-}
+  _map_version = graph.hdmap_version();
+  _map_district = graph.hdmap_district();
+  ROS_INFO("Use map district: %s, version: %s", _map_district.c_str(),
+           _map_version.c_str());
 
-const std::string& TopoGraph::map_version() const {
-    return _map_version;
+  if (!load_nodes(graph)) {
+    ROS_ERROR("Failed to load nodes from topology graph.");
+    return false;
+  }
+  if (!load_edges(graph)) {
+    ROS_ERROR("Failed to load edges from topology graph.");
+    return false;
+  }
+  return true;
 }
 
-const std::string& TopoGraph::map_district() const {
-    return _map_district;
-}
+const std::string& TopoGraph::map_version() const { return _map_version; }
+
+const std::string& TopoGraph::map_district() const { return _map_district; }
 
 const TopoNode* TopoGraph::get_node(const std::string& id) const {
-    const auto& iter = _node_index_map.find(id);
-    if (iter == _node_index_map.end()) {
-        return nullptr;
-    }
-    return _topo_nodes[iter->second].get();
+  const auto& iter = _node_index_map.find(id);
+  if (iter == _node_index_map.end()) {
+    return nullptr;
+  }
+  return _topo_nodes[iter->second].get();
 }
 
-void TopoGraph::get_nodes_by_road_id(const std::string& road_id,
-                        std::unordered_set<const TopoNode*>* const node_in_road) const {
-    const auto& iter = _road_node_map.find(road_id);
-    if (iter != _road_node_map.end()) {
-        node_in_road->insert(iter->second.begin(), iter->second.end());
-    }
+void TopoGraph::get_nodes_by_road_id(
+    const std::string& road_id,
+    std::unordered_set<const TopoNode*>* const node_in_road) const {
+  const auto& iter = _road_node_map.find(road_id);
+  if (iter != _road_node_map.end()) {
+    node_in_road->insert(iter->second.begin(), iter->second.end());
+  }
 }
 
-
-} // namespace routing
-} // namespace adu
-
+}  // namespace routing
+}  // namespace adu

modules/routing/graph/topo_graph.h

@@ -17,10 +17,10 @@
 #ifndef BAIDU_ADU_ROUTING_GRAPH_TOPO_GRAPH_H
 #define BAIDU_ADU_ROUTING_GRAPH_TOPO_GRAPH_H
 
+#include <memory>
 #include <unordered_map>
 #include <unordered_set>
 #include <vector>
-#include <memory>
 
 #include "topo_graph.pb.h"
 
@@ -31,34 +31,35 @@ class TopoNode;
 class TopoEdge;
 
 class TopoGraph {
-public:
-    TopoGraph() = default;
-    ~TopoGraph() = default;
+ public:
+  TopoGraph() = default;
+  ~TopoGraph() = default;
 
-    bool load_graph(const ::adu::routing::common::Graph& graph);
+  bool load_graph(const std::string& filename);
 
-    const std::string& map_version() const;
-    const std::string& map_district() const;
-    const ::adu::routing::TopoNode* get_node(const std::string& id) const;
-    void get_nodes_by_road_id(const std::string& road_id,
-                              std::unordered_set<const TopoNode*>* const node_in_road) const;
+  const std::string& map_version() const;
+  const std::string& map_district() const;
+  const ::adu::routing::TopoNode* get_node(const std::string& id) const;
+  void get_nodes_by_road_id(
+      const std::string& road_id,
+      std::unordered_set<const TopoNode*>* const node_in_road) const;
 
-private:
-    void clear();
-    bool load_nodes(const ::adu::routing::common::Graph& graph);
-    bool load_edges(const ::adu::routing::common::Graph& graph);
+ private:
+  void clear();
+  bool load_nodes(const ::adu::routing::common::Graph& graph);
+  bool load_edges(const ::adu::routing::common::Graph& graph);
 
-private:
-    std::string _map_version;
-    std::string _map_district;
-    std::vector<std::shared_ptr<TopoNode> > _topo_nodes;
-    std::vector<std::shared_ptr<TopoEdge> > _topo_edges;
-    std::unordered_map<std::string, int> _node_index_map;
-    std::unordered_map<std::string, std::unordered_set<const TopoNode*> > _road_node_map;
+ private:
+  std::string _map_version;
+  std::string _map_district;
+  std::vector<std::shared_ptr<TopoNode> > _topo_nodes;
+  std::vector<std::shared_ptr<TopoEdge> > _topo_edges;
+  std::unordered_map<std::string, int> _node_index_map;
+  std::unordered_map<std::string, std::unordered_set<const TopoNode*> >
+      _road_node_map;
 };
 
-} // namespace routing
-} // namespace adu
-
-#endif // BAIDU_ADU_ROUTING_GRAPH_TOPO_GRAPH_H
+}  // namespace routing
+}  // namespace adu
 
+#endif  // BAIDU_ADU_ROUTING_GRAPH_TOPO_GRAPH_H

modules/routing/graph/topo_node.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 "graph/topo_node.h"
+
+#include <math.h>
+
+#include "graph/topo_edge.h"
+
+namespace adu {
+namespace routing {
+
+namespace {
+
+using ::adu::routing::common::Node;
+using ::adu::routing::common::Edge;
+
+}  // namespace
+
+TopoNode::TopoNode(const Node& node)
+    : _pb_node(node), _start_s(0.0), _end_s(_pb_node.length()) {
+  int total_size = 0;
+  for (const auto& seg : central_curve().segment()) {
+    total_size += seg.line_segment().point_size();
+  }
+  int half_size = total_size / 2;
+  for (const auto& seg : central_curve().segment()) {
+    if (half_size < seg.line_segment().point_size()) {
+      _anchor_point = seg.line_segment().point(half_size);
+      break;
+    }
+    half_size -= seg.line_segment().point_size();
+  }
+  _origin_node = this;
+}
+
+TopoNode::TopoNode(const TopoNode* topo_node) : TopoNode(topo_node->node()) {}
+
+TopoNode::~TopoNode() {}
+
+const Node& TopoNode::node() const { return _pb_node; }
+
+double TopoNode::length() const { return _pb_node.length(); }
+
+double TopoNode::cost() const { return _pb_node.cost(); }
+
+bool TopoNode::is_virtual() const { return _pb_node.is_virtual(); }
+
+const std::string& TopoNode::lane_id() const { return _pb_node.lane_id(); }
+
+const std::string& TopoNode::road_id() const { return _pb_node.road_id(); }
+
+const ::adu::common::hdmap::Curve& TopoNode::central_curve() const {
+  return _pb_node.central_curve();
+}
+
+const ::adu::common::hdmap::Point& TopoNode::anchor_point() const {
+  return _anchor_point;
+}
+
+const std::unordered_set<const TopoEdge*>& TopoNode::in_from_all_edge() const {
+  return _in_from_all_edge_set;
+}
+
+const std::unordered_set<const TopoEdge*>& TopoNode::in_from_left_edge() const {
+  return _in_from_left_edge_set;
+}
+
+const std::unordered_set<const TopoEdge*>& TopoNode::in_from_right_edge()
+    const {
+  return _in_from_right_edge_set;
+}
+
+const std::unordered_set<const TopoEdge*>&
+TopoNode::in_from_left_or_right_edge() const {
+  return _in_from_left_or_right_edge_set;
+}
+
+const std::unordered_set<const TopoEdge*>& TopoNode::in_from_pre_edge() const {
+  return _in_from_pre_edge_set;
+}
+
+const std::unordered_set<const TopoEdge*>& TopoNode::out_to_all_edge() const {
+  return _out_to_all_edge_set;
+}
+
+const std::unordered_set<const TopoEdge*>& TopoNode::out_to_left_edge() const {
+  return _out_to_left_edge_set;
+}
+
+const std::unordered_set<const TopoEdge*>& TopoNode::out_to_right_edge() const {
+  return _out_to_right_edge_set;
+}
+
+const std::unordered_set<const TopoEdge*>& TopoNode::out_to_left_or_right_edge()
+    const {
+  return _out_to_left_or_right_edge_set;
+}
+
+const std::unordered_set<const TopoEdge*>& TopoNode::out_to_suc_edge() const {
+  return _out_to_suc_edge_set;
+}
+
+const TopoEdge* TopoNode::get_in_edge_from(const TopoNode* from_node) const {
+  const auto& iter = _in_edge_map.find(from_node);
+  if (iter == _in_edge_map.end()) {
+    return nullptr;
+  }
+  return iter->second;
+}
+
+const TopoEdge* TopoNode::get_out_edge_to(const TopoNode* to_node) const {
+  const auto& iter = _out_edge_map.find(to_node);
+  if (iter == _out_edge_map.end()) {
+    return nullptr;
+  }
+  return iter->second;
+}
+
+const TopoNode* TopoNode::origin_node() const { return _origin_node; }
+
+double TopoNode::start_s() const { return _start_s; }
+
+double TopoNode::end_s() const { return _end_s; }
+
+bool TopoNode::is_sub_node() const { return origin_node() != this; }
+
+void TopoNode::add_in_edge(const TopoEdge* edge) {
+  if (edge->to_node() != this) {
+    return;
+  }
+  if (_in_edge_map.count(edge->from_node()) != 0) {
+    return;
+  }
+  switch (edge->type()) {
+    case TET_LEFT:
+      _in_from_right_edge_set.insert(edge);
+      _in_from_left_or_right_edge_set.insert(edge);
+      break;
+    case TET_RIGHT:
+      _in_from_left_edge_set.insert(edge);
+      _in_from_left_or_right_edge_set.insert(edge);
+      break;
+    default:
+      _in_from_pre_edge_set.insert(edge);
+      break;
+  }
+  _in_from_all_edge_set.insert(edge);
+  _in_edge_map[edge->from_node()] = edge;
+}
+
+void TopoNode::add_out_edge(const TopoEdge* edge) {
+  if (edge->from_node() != this) {
+    return;
+  }
+  if (_out_edge_map.count(edge->to_node()) != 0) {
+    return;
+  }
+  switch (edge->type()) {
+    case TET_LEFT:
+      _out_to_left_edge_set.insert(edge);
+      _out_to_left_or_right_edge_set.insert(edge);
+      break;
+    case TET_RIGHT:
+      _out_to_right_edge_set.insert(edge);
+      _out_to_left_or_right_edge_set.insert(edge);
+      break;
+    default:
+      _out_to_suc_edge_set.insert(edge);
+      break;
+  }
+  _out_to_all_edge_set.insert(edge);
+  _out_edge_map[edge->to_node()] = edge;
+}
+
+void TopoNode::set_origin_node(const TopoNode* origin_node) {
+  _origin_node = origin_node;
+}
+
+void TopoNode::set_start_s(double start_s) { _start_s = start_s; }
+
+void TopoNode::set_end_s(double end_s) { _end_s = end_s; }
+
+}  // namespace routing
+}  // namespace adu

modules/routing/graph/topo_node.cpp

-/******************************************************************************
-  * 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 "graph/topo_node.h"
-
-#include <math.h>
-
-#include "graph/topo_edge.h"
-
-#include "glog/logging.h"
-
-namespace adu {
-namespace routing {
-
-namespace {
-
-using ::google::protobuf::RepeatedPtrField;
-
-using ::adu::routing::common::Node;
-using ::adu::routing::common::Edge;
-using ::adu::routing::common::CurveRange;
-using ::adu::common::hdmap::Curve;
-
-bool find_anchor_point(const Curve& curve, ::adu::common::hdmap::Point* anchor_point) {
-    int total_size = 0;
-    for (const auto& seg : curve.segment()) {
-        total_size += seg.line_segment().point_size();
-    }
-    int half_size = total_size / 2;
-    for (const auto& seg : curve.segment()) {
-        if (half_size < seg.line_segment().point_size()) {
-            *anchor_point = seg.line_segment().point(half_size);
-            return true;
-        }
-        half_size -= seg.line_segment().point_size();
-    }
-    return false;
-}
-
-void convert_out_range(const RepeatedPtrField<CurveRange>& range_vec,
-                       double start_s,
-                       double end_s,
-                       std::vector<NodeSRange>* out_range,
-                       int* prefer_index) {
-    double s_s = 0.0;
-    double e_s = 0.0;
-    for (const auto& c_range : range_vec) {
-        s_s = c_range.start().s();
-        e_s = c_range.end().s();
-        if (e_s < start_s || s_s > end_s || e_s < s_s) {
-            continue;
-        }
-        s_s = std::max(start_s, s_s);
-        e_s = std::min(end_s, e_s);
-        NodeSRange s_range(s_s, e_s);
-        out_range->push_back(std::move(s_range));
-    }
-    sort(out_range->begin(), out_range->end());
-    int max_index = -1;
-    double max_diff = 0.0;
-    for (size_t i = 0; i < out_range->size(); ++i) {
-        if (out_range->at(i).length() > max_diff) {
-            max_index = i;
-            max_diff = out_range->at(i).length();
-        }
-    }
-    *prefer_index = max_index;
-}
-
-} // namespace
-
-bool TopoNode::is_out_range_enough(const std::vector<NodeSRange>& range_vec,
-                                   double start_s,
-                                   double end_s) {
-    int start_index = binary_search_for_s_larger(range_vec, start_s);
-    int end_index = binary_search_for_s_smaller(range_vec, end_s);
-
-    int index_diff = end_index - start_index;
-    if (start_index < 0 || end_index < 0) {
-        return false;
-    }
-    if (index_diff > 1) {
-        return true;
-    }
-
-    double pre_s_s = std::max(start_s, range_vec[start_index].start_s());
-    double suc_e_s = std::min(end_s, range_vec[end_index].end_s());
-
-    if (index_diff == 1) {
-        double dlt = range_vec[start_index].end_s() - pre_s_s;
-        dlt += suc_e_s - range_vec[end_index].start_s();
-        return NodeSRange::is_enough_for_change_lane(dlt);
-    }
-    if (index_diff == 0) {
-        return NodeSRange::is_enough_for_change_lane(pre_s_s, suc_e_s);
-    }
-    return false;
-}
-
-TopoNode::TopoNode(const Node& node) : _pb_node(node), _start_s(0.0), _end_s(_pb_node.length()) {
-    init();
-    _origin_node = this;
-}
-
-TopoNode::TopoNode(const TopoNode* topo_node, const NodeSRange& range) :
-        _pb_node(topo_node->node()) {
-    _origin_node = topo_node;
-    _start_s = range.start_s();
-    _end_s = range.end_s();
-    init();
-}
-
-TopoNode::~TopoNode() { }
-
-void TopoNode::init() {
-    if (!find_anchor_point(central_curve(), &_anchor_point)) {
-        LOG(WARNING) << "Be attention!!! Find anchor point failed for lane: " << lane_id();
-    }
-    convert_out_range(_pb_node.left_out(), _start_s, _end_s,
-                      &_left_out_sorted_range, &_left_prefer_range_index);
-    _is_left_range_enough = (_left_prefer_range_index >= 0) &&
-            _left_out_sorted_range[_left_prefer_range_index].is_enough_for_change_lane();
-
-    convert_out_range(_pb_node.right_out(), _start_s, _end_s,
-                      &_right_out_sorted_range, &_right_prefer_range_index);
-    _is_right_range_enough = (_right_prefer_range_index >= 0) &&
-            _right_out_sorted_range[_right_prefer_range_index].is_enough_for_change_lane();
-}
-
-const Node& TopoNode::node() const {
-    return _pb_node;
-}
-
-double TopoNode::length() const {
-    return _pb_node.length();
-}
-
-double TopoNode::cost() const {
-    return _pb_node.cost();
-}
-
-bool TopoNode::is_virtual() const {
-    return _pb_node.is_virtual();
-}
-
-const std::string& TopoNode::lane_id() const {
-    return _pb_node.lane_id();
-}
-
-const std::string& TopoNode::road_id() const {
-    return _pb_node.road_id();
-}
-
-const ::adu::common::hdmap::Curve& TopoNode::central_curve() const {
-    return _pb_node.central_curve();
-}
-
-const ::adu::common::hdmap::Point& TopoNode::anchor_point() const {
-    return _anchor_point;
-}
-
-const std::vector<NodeSRange>& TopoNode::left_out_range() const {
-    return _left_out_sorted_range;
-}
-
-const std::vector<NodeSRange>& TopoNode::right_out_range() const {
-    return _right_out_sorted_range;
-}
-
-const std::unordered_set<const TopoEdge*>& TopoNode::in_from_all_edge() const {
-    return _in_from_all_edge_set;
-}
-
-const std::unordered_set<const TopoEdge*>& TopoNode::in_from_left_edge() const {
-    return _in_from_left_edge_set;
-}
-
-const std::unordered_set<const TopoEdge*>& TopoNode::in_from_right_edge() const {
-    return _in_from_right_edge_set;
-}
-
-const std::unordered_set<const TopoEdge*>& TopoNode::in_from_left_or_right_edge() const {
-    return _in_from_left_or_right_edge_set;
-}
-
-const std::unordered_set<const TopoEdge*>& TopoNode::in_from_pre_edge() const {
-    return _in_from_pre_edge_set;
-}
-
-const std::unordered_set<const TopoEdge*>& TopoNode::out_to_all_edge() const {
-    return _out_to_all_edge_set;
-}
-
-const std::unordered_set<const TopoEdge*>& TopoNode::out_to_left_edge() const {
-    return _out_to_left_edge_set;
-}
-
-const std::unordered_set<const TopoEdge*>& TopoNode::out_to_right_edge() const {
-    return _out_to_right_edge_set;
-}
-
-const std::unordered_set<const TopoEdge*>& TopoNode::out_to_left_or_right_edge() const {
-    return _out_to_left_or_right_edge_set;
-}
-
-const std::unordered_set<const TopoEdge*>& TopoNode::out_to_suc_edge() const {
-    return _out_to_suc_edge_set;
-}
-
-const TopoEdge* TopoNode::get_in_edge_from(const TopoNode* from_node) const {
-    const auto& iter = _in_edge_map.find(from_node);
-    if (iter == _in_edge_map.end()) {
-        return nullptr;
-    }
-    return iter->second;
-}
-
-const TopoEdge* TopoNode::get_out_edge_to(const TopoNode* to_node) const {
-    const auto& iter = _out_edge_map.find(to_node);
-    if (iter == _out_edge_map.end()) {
-        return nullptr;
-    }
-    return iter->second;
-}
-
-const TopoNode* TopoNode::origin_node() const {
-    return _origin_node;
-}
-
-double TopoNode::start_s() const {
-    return _start_s;
-}
-
-double TopoNode::end_s() const {
-    return _end_s;
-}
-
-bool TopoNode::is_sub_node() const {
-    return origin_node() != this;
-}
-
-bool TopoNode::is_overlap_enough(const TopoNode* sub_node, const TopoEdge* edge_for_type) const {
-    if (edge_for_type->type() == TET_LEFT) {
-        return (_is_left_range_enough &&
-            is_out_range_enough(_left_out_sorted_range, sub_node->start_s(), sub_node->end_s()));
-    }
-    if (edge_for_type->type() == TET_RIGHT) {
-        return (_is_right_range_enough &&
-            is_out_range_enough(_right_out_sorted_range, sub_node->start_s(), sub_node->end_s()));
-    }
-    return true;
-}
-
-void TopoNode::add_in_edge(const TopoEdge* edge) {
-    if (edge->to_node() != this) {
-        return;
-    }
-    if (_in_edge_map.count(edge->from_node()) != 0) {
-        return;
-    }
-    switch (edge->type()) {
-        case TET_LEFT:
-            _in_from_right_edge_set.insert(edge);
-            _in_from_left_or_right_edge_set.insert(edge);
-            break;
-        case TET_RIGHT:
-            _in_from_left_edge_set.insert(edge);
-            _in_from_left_or_right_edge_set.insert(edge);
-            break;
-        default:
-            _in_from_pre_edge_set.insert(edge);
-            break;
-    }
-    _in_from_all_edge_set.insert(edge);
-    _in_edge_map[edge->from_node()] = edge;
-}
-
-void TopoNode::add_out_edge(const TopoEdge* edge) {
-    if (edge->from_node() != this) {
-        return;
-    }
-    if (_out_edge_map.count(edge->to_node()) != 0) {
-        return;
-    }
-    switch (edge->type()) {
-        case TET_LEFT:
-            _out_to_left_edge_set.insert(edge);
-            _out_to_left_or_right_edge_set.insert(edge);
-            break;
-        case TET_RIGHT:
-            _out_to_right_edge_set.insert(edge);
-            _out_to_left_or_right_edge_set.insert(edge);
-            break;
-        default:
-            _out_to_suc_edge_set.insert(edge);
-            break;
-    }
-    _out_to_all_edge_set.insert(edge);
-    _out_edge_map[edge->to_node()] = edge;
-}
-
-} // namespace routing
-} // namespace adu
-

modules/routing/graph/topo_node.h

@@ -20,8 +20,6 @@
 #include <unordered_map>
 #include <unordered_set>
 
-#include "graph/topo_range.h"
-
 #include "map_lane.pb.h"
 #include "topo_graph.pb.h"
 
@@ -31,90 +29,72 @@ namespace routing {
 class TopoEdge;
 
 class TopoNode {
-public:
-    static bool is_out_range_enough(const std::vector<NodeSRange>& range_vec,
-                                    double start_s,
-                                    double end_s);
-
-public:
-    explicit TopoNode(const ::adu::routing::common::Node& node);
-    TopoNode(const TopoNode* topo_node, const NodeSRange& range);
-
-    ~TopoNode();
-
-    const ::adu::routing::common::Node& node() const;
-    double length() const;
-    double cost() const;
-    bool is_virtual() const;
-
-    const std::string& lane_id() const;
-    const std::string& road_id() const;
-    const ::adu::common::hdmap::Curve& central_curve() const;
-    const ::adu::common::hdmap::Point& anchor_point() const;
-    const std::vector<NodeSRange>& left_out_range() const;
-    const std::vector<NodeSRange>& right_out_range() const;
-
-    const std::unordered_set<const TopoEdge*>& in_from_all_edge() const;
-    const std::unordered_set<const TopoEdge*>& in_from_left_edge() const;
-    const std::unordered_set<const TopoEdge*>& in_from_right_edge() const;
-    const std::unordered_set<const TopoEdge*>& in_from_left_or_right_edge() const;
-    const std::unordered_set<const TopoEdge*>& in_from_pre_edge() const;
-    const std::unordered_set<const TopoEdge*>& out_to_all_edge() const;
-    const std::unordered_set<const TopoEdge*>& out_to_left_edge() const;
-    const std::unordered_set<const TopoEdge*>& out_to_right_edge() const;
-    const std::unordered_set<const TopoEdge*>& out_to_left_or_right_edge() const;
-    const std::unordered_set<const TopoEdge*>& out_to_suc_edge() const;
-
-    const TopoEdge* get_in_edge_from(const TopoNode* from_node) const;
-    const TopoEdge* get_out_edge_to(const TopoNode* to_node) const;
-
-    const TopoNode* origin_node() const;
-    double start_s() const;
-    double end_s() const;
-    bool is_sub_node() const;
-
-    bool is_overlap_enough(const TopoNode* sub_node, const TopoEdge* edge_for_type) const;
-
-    void add_in_edge(const TopoEdge* edge);
-    void add_out_edge(const TopoEdge* edge);
-
-private:
-    void init();
-
-private:
-    ::adu::routing::common::Node _pb_node;
-    ::adu::common::hdmap::Point _anchor_point;
-
-    double _start_s;
-    double _end_s;
-
-    bool _is_left_range_enough;
-    int _left_prefer_range_index;
-    bool _is_right_range_enough;
-    int _right_prefer_range_index;
-
-    std::vector<NodeSRange> _left_out_sorted_range;
-    std::vector<NodeSRange> _right_out_sorted_range;
-
-    std::unordered_set<const TopoEdge*> _in_from_all_edge_set;
-    std::unordered_set<const TopoEdge*> _in_from_left_edge_set;
-    std::unordered_set<const TopoEdge*> _in_from_right_edge_set;
-    std::unordered_set<const TopoEdge*> _in_from_left_or_right_edge_set;
-    std::unordered_set<const TopoEdge*> _in_from_pre_edge_set;
-    std::unordered_set<const TopoEdge*> _out_to_all_edge_set;
-    std::unordered_set<const TopoEdge*> _out_to_left_edge_set;
-    std::unordered_set<const TopoEdge*> _out_to_right_edge_set;
-    std::unordered_set<const TopoEdge*> _out_to_left_or_right_edge_set;
-    std::unordered_set<const TopoEdge*> _out_to_suc_edge_set;
-
-    std::unordered_map<const TopoNode*, const TopoEdge*> _out_edge_map;
-    std::unordered_map<const TopoNode*, const TopoEdge*> _in_edge_map;
-
-    const TopoNode* _origin_node;
+ public:
+  explicit TopoNode(const ::adu::routing::common::Node& node);
+  explicit TopoNode(const TopoNode* topo_node);
+
+  ~TopoNode();
+
+  const ::adu::routing::common::Node& node() const;
+  double length() const;
+  double cost() const;
+  bool is_virtual() const;
+
+  const std::string& lane_id() const;
+  const std::string& road_id() const;
+  const ::adu::common::hdmap::Curve& central_curve() const;
+  const ::adu::common::hdmap::Point& anchor_point() const;
+
+  const std::unordered_set<const TopoEdge*>& in_from_all_edge() const;
+  const std::unordered_set<const TopoEdge*>& in_from_left_edge() const;
+  const std::unordered_set<const TopoEdge*>& in_from_right_edge() const;
+  const std::unordered_set<const TopoEdge*>& in_from_left_or_right_edge() const;
+  const std::unordered_set<const TopoEdge*>& in_from_pre_edge() const;
+  const std::unordered_set<const TopoEdge*>& out_to_all_edge() const;
+  const std::unordered_set<const TopoEdge*>& out_to_left_edge() const;
+  const std::unordered_set<const TopoEdge*>& out_to_right_edge() const;
+  const std::unordered_set<const TopoEdge*>& out_to_left_or_right_edge() const;
+  const std::unordered_set<const TopoEdge*>& out_to_suc_edge() const;
+
+  const TopoEdge* get_in_edge_from(const TopoNode* from_node) const;
+  const TopoEdge* get_out_edge_to(const TopoNode* to_node) const;
+
+  const TopoNode* origin_node() const;
+  double start_s() const;
+  double end_s() const;
+  bool is_sub_node() const;
+
+  void add_in_edge(const TopoEdge* edge);
+  void add_out_edge(const TopoEdge* edge);
+  void set_origin_node(const TopoNode* origin_node);
+  void set_start_s(double start_s);
+  void set_end_s(double end_s);
+
+ private:
+  ::adu::routing::common::Node _pb_node;
+  ::adu::common::hdmap::Point _anchor_point;
+
+  double _start_s;
+  double _end_s;
+
+  std::unordered_set<const TopoEdge*> _in_from_all_edge_set;
+  std::unordered_set<const TopoEdge*> _in_from_left_edge_set;
+  std::unordered_set<const TopoEdge*> _in_from_right_edge_set;
+  std::unordered_set<const TopoEdge*> _in_from_left_or_right_edge_set;
+  std::unordered_set<const TopoEdge*> _in_from_pre_edge_set;
+  std::unordered_set<const TopoEdge*> _out_to_all_edge_set;
+  std::unordered_set<const TopoEdge*> _out_to_left_edge_set;
+  std::unordered_set<const TopoEdge*> _out_to_right_edge_set;
+  std::unordered_set<const TopoEdge*> _out_to_left_or_right_edge_set;
+  std::unordered_set<const TopoEdge*> _out_to_suc_edge_set;
+
+  std::unordered_map<const TopoNode*, const TopoEdge*> _out_edge_map;
+  std::unordered_map<const TopoNode*, const TopoEdge*> _in_edge_map;
+
+  const TopoNode* _origin_node;
 };
 
-} // namespace routing
-} // namespace adu
-
-#endif // BAIDU_ADU_ROUTING_GRAPH_TOPO_NODE_H
+}  // namespace routing
+}  // namespace adu
 
+#endif  // BAIDU_ADU_ROUTING_GRAPH_TOPO_NODE_H

modules/routing/graph/topo_range.cpp

-/******************************************************************************
-  * 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 "graph/topo_range.h"
-#include "graph/topo_node.h"
-
-namespace adu {
-namespace routing {
-
-class TopoNode;
-
-const double MIN_LENGTH_FOR_CHANGE_LANE = 10.0;
-
-bool NodeSRange::is_enough_for_change_lane(double start_s, double end_s) {
-    return is_enough_for_change_lane(end_s - start_s);
-}
-
-bool NodeSRange::is_enough_for_change_lane(double length) {
-    return (length > MIN_LENGTH_FOR_CHANGE_LANE);
-}
-
-NodeSRange::NodeSRange() : _start_s(0.0), _end_s(0.0) { }
-
-NodeSRange::NodeSRange(double s1, double s2) : _start_s(s1), _end_s(s2) { }
-
-NodeSRange::NodeSRange(const NodeSRange& other) : _start_s(other.start_s()), _end_s(other.end_s()) {
-}
-
-bool NodeSRange::operator < (const NodeSRange& other) const {
-    return start_s() < other.start_s();
-}
-
-bool NodeSRange::is_valid() const {
-    return _start_s <= _end_s;
-}
-
-double NodeSRange::start_s() const {
-    return _start_s;
-}
-
-double NodeSRange::end_s() const {
-    return _end_s;
-}
-
-double NodeSRange::length() const {
-    return _end_s - _start_s;
-}
-
-bool NodeSRange::is_enough_for_change_lane() const {
-    return NodeSRange::is_enough_for_change_lane(start_s(), end_s());
-}
-
-void NodeSRange::set_start_s(double start_s) {
-    _start_s = start_s;
-}
-
-void NodeSRange::set_end_s(double end_s) {
-    _end_s = end_s;
-}
-
-void NodeSRange::set_range_s(double start_s, double end_s) {
-    _start_s = start_s;
-    _end_s = end_s;
-}
-
-bool NodeSRange::merge_range_overlap(const NodeSRange& other) {
-    if (!is_valid() || !other.is_valid()) {
-        return false;
-    }
-    if (other.start_s() > end_s() || other.end_s() < start_s()) {
-        return false;
-    }
-    set_end_s(std::max(end_s(), other.end_s()));
-    set_start_s(std::min(start_s(), other.start_s()));
-    return true;
-}
-
-NodeWithRange::NodeWithRange(const NodeSRange& range, const TopoNode* node) :
-                            NodeSRange(range), _topo_node(node) { }
-
-NodeWithRange::~NodeWithRange() { }
-
-bool NodeWithRange::operator < (const NodeWithRange& other) const {
-    return start_s() > other.start_s();
-}
-
-const TopoNode* NodeWithRange::topo_node() const {
-    return _topo_node;
-}
-
-} // namespace routing
-} // namespace adu
-

modules/routing/graph/topo_range.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 BAIDU_ADU_ROUTING_GRAPH_TOPO_RANGE_H
-#define BAIDU_ADU_ROUTING_GRAPH_TOPO_RANGE_H
-
-#include <vector>
-#include <algorithm>
-#include <queue>
-#include "glog/logging.h"
-
-namespace adu {
-namespace routing {
-
-class TopoNode;
-
-class NodeSRange {
-public:
-    static bool is_enough_for_change_lane(double start_s, double end_s);
-    static bool is_enough_for_change_lane(double length);
-
-public:
-    NodeSRange();
-    NodeSRange(double s1, double s2);
-    NodeSRange(const NodeSRange& other);
-    ~NodeSRange() = default;
-
-    bool operator < (const NodeSRange& other) const;
-    bool is_valid() const;
-    double start_s() const;
-    double end_s() const;
-    bool is_enough_for_change_lane() const;
-    double length() const;
-
-    void set_start_s(double start_s);
-    void set_end_s(double end_s);
-    void set_range_s(double start_s, double end_s);
-    bool merge_range_overlap(const NodeSRange& other);
-
-private:
-    double _start_s;
-    double _end_s;
-};
-
-class NodeWithRange : public NodeSRange {
-public:
-    NodeWithRange(const NodeSRange& range, const TopoNode* node);
-    ~NodeWithRange();
-    bool operator < (const NodeWithRange& other) const;
-
-    const TopoNode* topo_node() const;
-
-private:
-    const TopoNode* _topo_node;
-};
-
-template <typename T>
-int binary_search_for_s_larger(const std::vector<T>& sorted_vec,
-                               double value_s) {
-    if (sorted_vec.empty()) {
-        return -1;
-    }
-    int start_index = 0;
-    int end_index = sorted_vec.size() - 1;
-    int internal_s = 0.0;
-    int middle_index = 0;
-    while (end_index - start_index > 1) {
-        middle_index = (start_index + end_index) / 2;
-        internal_s = sorted_vec[middle_index].start_s();
-        if (internal_s > value_s) {
-            end_index = middle_index;
-        } else {
-            start_index = middle_index;
-        }
-    }
-    double end_s = sorted_vec[start_index].end_s();
-    if (value_s <= end_s) {
-        return start_index;
-    }
-    return end_index;
-}
-
-template <typename T>
-int binary_search_for_s_smaller(const std::vector<T>& sorted_vec,
-                                double value_s) {
-    if (sorted_vec.empty()) {
-        return -1;
-    }
-    int start_index = 0;
-    int end_index = sorted_vec.size() - 1;
-    int internal_s = 0.0;
-    int middle_index = 0;
-    while (end_index - start_index > 1) {
-        middle_index = (start_index + end_index) / 2;
-        internal_s = sorted_vec[middle_index].end_s();
-        if (internal_s > value_s) {
-            end_index = middle_index;
-        } else {
-            start_index = middle_index;
-        }
-    }
-    double start_s = sorted_vec[end_index].start_s();
-    if (value_s > start_s) {
-        return end_index;
-    }
-    return start_index;
-}
-
-template <typename T>
-int binary_search_check_valid_s_index(const std::vector<T>& sorted_vec,
-                                      int index,
-                                      double value_s) {
-    if (index == -1) {
-        return -1;
-    }
-    double start_s = sorted_vec[index].start_s();
-    double end_s = sorted_vec[index].end_s();
-    if (start_s <= value_s && end_s >= value_s) {
-        return index;
-    }
-    return -1;
-}
-
-template <typename T>
-int binary_search_for_start_s(const std::vector<T>& sorted_vec,
-                              double value_s) {
-    int index = binary_search_for_s_larger(sorted_vec, value_s);
-    return binary_search_check_valid_s_index(sorted_vec, index, value_s);
-}
-
-template <typename T>
-int binary_search_for_end_s(const std::vector<T>& sorted_vec,
-                            double value_s) {
-    int index = binary_search_for_s_smaller(sorted_vec, value_s);
-    return binary_search_check_valid_s_index(sorted_vec, index, value_s);
-}
-
-
-} // namespace routing
-} // namespace adu
-
-#endif // BAIDU_ADU_ROUTING_GRAPH_TOPO_RANGE_H
-

modules/routing/graph/topo_range_manager.cpp

-/******************************************************************************
-  * 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 "ros/ros.h"
-#include "graph/topo_range_manager.h"
-
-#include "graph/topo_node.h"
-#include "graph/topo_edge.h"
-
-namespace adu {
-namespace routing {
-
-void NodeSRangeManager::clear() {
-    _range_map.clear();
-    _is_start_inited.clear();
-    _is_end_inited.clear();
-}
-
-void NodeSRangeManager::init_node_range(double start_node_s,
-                                        double end_node_s,
-                                        const TopoNode* start_node,
-                                        const TopoNode* end_node) {
-    auto& start_range = _range_map[start_node];
-    start_range.set_range_s(start_node_s, start_node->length());
-    _is_start_inited.emplace(start_node);
-    // if start and end are same node
-    if (_range_map.count(end_node) != 0) {
-        auto& end_range = _range_map[end_node];
-        end_range.set_end_s(std::min(end_range.end_s(), end_node_s));
-    } else {
-        auto& end_range = _range_map[end_node];
-        end_range.set_range_s(0.0, end_node_s);
-    }
-    _is_end_inited.emplace(end_node);
-
-    init_out_start(start_node, start_node_s);
-    init_in_end(end_node, end_node_s);
-}
-
-NodeSRange NodeSRangeManager::get_node_range(const TopoNode* topo_node) const {
-    const auto& iter = _range_map.find(topo_node);
-    if (iter != _range_map.end()) {
-        return iter->second;
-    }
-    return NodeSRange(0.0, topo_node->length());
-}
-
-double NodeSRangeManager::get_node_start_s(const TopoNode* topo_node) const {
-    return get_node_range(topo_node).start_s();
-}
-
-double NodeSRangeManager::get_node_end_s(const TopoNode* topo_node) const {
-    return get_node_range(topo_node).end_s();
-}
-
-void NodeSRangeManager::init_in_end(const TopoNode* cur_node, double end_s) {
-    for (const auto* edge : cur_node->in_from_left_or_right_edge()) {
-        const auto* from_node = edge->from_node();
-        if (_is_end_inited.find(from_node) != _is_end_inited.end()) {
-            continue;
-        }
-        if (_range_map.count(from_node) != 0) {
-            auto& range = _range_map[from_node];
-            range.set_end_s(std::min(range.end_s(), end_s));
-        } else {
-            auto& range = _range_map[from_node];
-            range.set_range_s(0.0, end_s);
-        }
-        _is_end_inited.emplace(from_node);
-        init_in_end(from_node, end_s);
-    }
-}
-
-void NodeSRangeManager::init_out_start(const TopoNode* cur_node, double start_s) {
-    for (const auto* edge : cur_node->out_to_left_or_right_edge()) {
-        const auto* to_node = edge->to_node();
-        if (_is_start_inited.find(to_node) != _is_start_inited.end()) {
-            continue;
-        }
-        ROS_INFO("init out start, id: %s, to id: %s",
-                 cur_node->lane_id().c_str(),
-                 to_node->lane_id().c_str());
-        if (_range_map.count(to_node) != 0) {
-            auto& range = _range_map[to_node];
-            range.set_start_s(std::max(range.start_s(), start_s));
-        } else {
-            auto& range = _range_map[to_node];
-            range.set_range_s(start_s, to_node->length());
-        }
-        _is_start_inited.emplace(to_node);
-        init_out_start(to_node, start_s);
-    }
-}
-
-void NodeSRangeManager::init_in_neighbor(const TopoNode* cur_node, double start_s, double end_s) {
-    for (const auto* edge : cur_node->in_from_left_or_right_edge()) {
-        const auto* from_node = edge->from_node();
-        if (_range_map.count(from_node) != 0) {
-            // in case of start and end in the same lane
-            auto& range = _range_map[from_node];
-            range.set_start_s(std::max(range.start_s(), start_s));
-            range.set_end_s(std::min(range.end_s(), end_s));
-        } else {
-            auto& range = _range_map[from_node];
-            range.set_start_s(start_s);
-            range.set_end_s(end_s);
-            init_in_neighbor(from_node, start_s, end_s);
-        }
-    }
-}
-
-void NodeSRangeManager::init_out_neighbor(const TopoNode* cur_node, double start_s, double end_s) {
-    for (const auto* edge : cur_node->out_to_left_or_right_edge()) {
-        const auto* to_node = edge->to_node();
-        if (_range_map.count(to_node) != 0) {
-            // in case of start and end in the same lane
-            auto& range = _range_map[to_node];
-            range.set_start_s(std::max(range.start_s(), start_s));
-            range.set_end_s(std::min(range.end_s(), end_s));
-        } else {
-            auto& range = _range_map[to_node];
-            range.set_start_s(start_s);
-            range.set_end_s(end_s);
-            init_out_neighbor(to_node, start_s, end_s);
-        }
-    }
-}
-
-} // namespace routing
-} // namespace adu
-

modules/routing/graph/topo_range_manager.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 BAIDU_ADU_ROUTING_GRAPH_TOPO_RANGE_MANAGER_H
-#define BAIDU_ADU_ROUTING_GRAPH_TOPO_RANGE_MANAGER_H
-
-#include <unordered_map>
-#include <unordered_set>
-
-#include "graph/topo_range.h"
-
-namespace adu {
-namespace routing {
-
-class TopoNode;
-
-class NodeSRangeManager {
-public:
-    NodeSRangeManager() = default;
-    ~NodeSRangeManager() = default;
-
-    void clear();
-    void init_node_range(double start_node_s,
-                         double end_node_s,
-                         const TopoNode* start_node,
-                         const TopoNode* end_node);
-
-    NodeSRange get_node_range(const TopoNode* topo_node) const;
-    double get_node_start_s(const TopoNode* topo_node) const;
-    double get_node_end_s(const TopoNode* topo_node) const;
-
-private:
-    void init_in_end(const TopoNode* cur_node, double end_s);
-    void init_out_start(const TopoNode* cur_node, double start_s);
-    void init_in_neighbor(const TopoNode* cur_node, double start_s, double end_s);
-    void init_out_neighbor(const TopoNode* cur_node, double start_s, double end_s);
-
-private:
-    std::unordered_map<const TopoNode*, NodeSRange> _range_map;
-    std::unordered_set<const TopoNode*> _is_start_inited;
-    std::unordered_set<const TopoNode*> _is_end_inited;
-};
-
-} // namespace routing
-} // namespace adu
-
-#endif // BAIDU_ADU_ROUTING_GRAPH_TOPO_RANGE_MANAGER_H
-

modules/routing/router_main.cpp → modules/routing/router_main.cc

@@ -20,13 +20,13 @@
 #include "core/arbiter.h"
 
 int main(int argc, char **argv) {
-    google::InitGoogleLogging(argv[0]);
-    google::ParseCommandLineFlags(&argc, &argv, true);
+  google::InitGoogleLogging(argv[0]);
+  google::ParseCommandLineFlags(&argc, &argv, true);
 
-    ros::init(argc, argv, FLAGS_node_name);
-    ROS_INFO("Start router!!!");
-    if (!::adu::routing::Arbiter::instance()->run()) {
-        return -1;
-    }
-    return 0;
+  ros::init(argc, argv, FLAGS_node_name);
+  ROS_INFO("Start router!!!");
+  if (!::adu::routing::Arbiter::instance()->run()) {
+    return -1;
+  }
+  return 0;
 }

modules/routing/strategy/a_star_strategy.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 <algorithm>
+#include <fstream>
+#include <limits>
+#include <limits>
+#include <queue>
+
+#include "boost/lexical_cast.hpp"
+#include "ros/ros.h"
+
+#include "graph/topo_edge.h"
+#include "graph/topo_graph.h"
+#include "graph/topo_node.h"
+#include "strategy/a_star_strategy.h"
+
+namespace adu {
+namespace routing {
+
+namespace {
+
+struct SearchNode {
+  const TopoNode* topo_node;
+  double f;
+
+  SearchNode() : topo_node(nullptr), f(std::numeric_limits<double>::max()) {}
+  SearchNode(const TopoNode* node)
+      : topo_node(node), f(std::numeric_limits<double>::max()) {}
+  SearchNode(const SearchNode& search_node)
+      : topo_node(search_node.topo_node), f(search_node.f) {}
+
+  bool operator<(const SearchNode& node) const {
+    // in order to let the top of preority queue is the smallest one!
+    return f > node.f;
+  }
+
+  bool operator==(const SearchNode& node) const {
+    return topo_node == node.topo_node;
+  }
+};
+
+double get_cost_to_neighbor(const TopoEdge* edge) {
+  return (edge->cost() + edge->to_node()->cost());
+}
+
+bool reconstruct(
+    const std::unordered_map<const TopoNode*, const TopoNode*>& came_from,
+    const TopoNode* dest_node, std::vector<const TopoNode*>* result_nodes) {
+  result_nodes->clear();
+  const TopoNode* cur_node = dest_node;
+  result_nodes->push_back(cur_node);
+  auto iter = came_from.find(cur_node);
+  while (iter != came_from.end()) {
+    cur_node = iter->second;
+    result_nodes->push_back(cur_node);
+    iter = came_from.find(cur_node);
+  }
+  std::reverse(result_nodes->begin(), result_nodes->end());
+  return true;
+}
+
+}  // namespace
+
+void AStarStrategy::clear() {
+  _closed_set.clear();
+  _open_set.clear();
+  _came_from.clear();
+  _g_score.clear();
+}
+
+double AStarStrategy::heuristic_cost(const TopoNode* src_node,
+                                     const TopoNode* dest_node) {
+  const ::adu::common::hdmap::Point& src_point = src_node->anchor_point();
+  const ::adu::common::hdmap::Point& dest_point = dest_node->anchor_point();
+  double distance = sqrt(pow(src_point.x() - dest_point.x(), 2) +
+                         pow(src_point.y() - dest_point.y(), 2) +
+                         pow(src_point.z() - dest_point.z(), 2));
+  return distance;
+}
+
+bool AStarStrategy::search(
+    const TopoGraph* graph, const TopoNode* src_node, const TopoNode* dest_node,
+    const std::unordered_set<const TopoNode*>& black_list,
+    std::vector<const TopoNode*>* const result_nodes) {
+  clear();
+  ROS_INFO("Start A* search algorithm.");
+
+  _closed_set.insert(black_list.begin(), black_list.end());
+
+  std::priority_queue<SearchNode> open_set_detail;
+
+  SearchNode src_search_node(src_node);
+  src_search_node.f = heuristic_cost(src_node, dest_node);
+  open_set_detail.push(src_search_node);
+
+  _open_set.insert(src_node);
+  _g_score[src_node] = 0.0;
+
+  SearchNode current_node;
+  while (!open_set_detail.empty()) {
+    current_node = open_set_detail.top();
+    if (current_node == dest_node) {
+      if (!reconstruct(_came_from, current_node.topo_node, result_nodes)) {
+        ROS_ERROR("Failed to reconstruct route.");
+        return false;
+      }
+      return true;
+    }
+    _open_set.erase(current_node.topo_node);
+    open_set_detail.pop();
+
+    if (_closed_set.count(current_node.topo_node) != 0) {
+      // if showed before, just skip...
+      continue;
+    }
+    _closed_set.emplace(current_node.topo_node);
+
+    double tentative_g_score = 0.0;
+    for (const auto* edge : current_node.topo_node->out_to_all_edge()) {
+      const auto* to_node = edge->to_node();
+      if (_closed_set.count(to_node) == 1) {
+        continue;
+      }
+      tentative_g_score =
+          _g_score[current_node.topo_node] + get_cost_to_neighbor(edge);
+      if (_open_set.count(to_node) != 0 &&
+          tentative_g_score >= _g_score[to_node]) {
+        continue;
+      }
+      _g_score[to_node] = tentative_g_score;
+      SearchNode next_node(to_node);
+      next_node.f =
+          tentative_g_score + heuristic_cost(current_node.topo_node, to_node);
+      open_set_detail.push(next_node);
+      _came_from[to_node] = current_node.topo_node;
+
+      if (_open_set.count(to_node) == 0) {
+        _open_set.insert(to_node);
+      }
+    }
+  }
+  ROS_ERROR("Failed to find goal lane with id %s",
+            dest_node->lane_id().c_str());
+  return false;
+}
+
+}  // namespace routing
+}  // namespace adu

modules/routing/strategy/a_star_strategy.cpp

-/******************************************************************************
-  * 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 <fstream>
-#include <limits>
-#include <algorithm>
-#include <limits>
-#include <queue>
-#include <math.h>
-
-#include "ros/ros.h"
-#include "boost/lexical_cast.hpp"
-
-#include "graph/sub_topo_graph.h"
-#include "graph/topo_graph.h"
-#include "graph/topo_node.h"
-#include "graph/topo_edge.h"
-#include "strategy/a_star_strategy.h"
-
-namespace adu {
-namespace routing {
-
-namespace {
-
-struct SearchNode {
-    const TopoNode* topo_node;
-    double f;
-
-    SearchNode() : topo_node(nullptr), f(std::numeric_limits<double>::max()) { }
-    SearchNode(const TopoNode* node) : topo_node(node), f(std::numeric_limits<double>::max()) { }
-    SearchNode(const SearchNode& search_node) : topo_node(search_node.topo_node), f(search_node.f) {
-    }
-
-    bool operator < (const SearchNode& node) const {
-        // in order to let the top of preority queue is the smallest one!
-        return f > node.f;
-    }
-
-    bool operator == (const SearchNode& node) const {
-        return topo_node == node.topo_node;
-    }
-};
-
-double get_cost_to_neighbor(const TopoEdge* edge) {
-    return (edge->cost() + edge->to_node()->cost());
-}
-
-bool reconstruct(const std::unordered_map<const TopoNode*, const TopoNode*>& came_from,
-                 const TopoNode* dest_node,
-                 std::vector<const TopoNode*>* result_nodes) {
-    result_nodes->clear();
-    const TopoNode* cur_node = dest_node;
-    result_nodes->push_back(cur_node);
-    auto iter = came_from.find(cur_node);
-    while (iter != came_from.end()) {
-        cur_node = iter->second;
-        result_nodes->push_back(cur_node);
-        iter = came_from.find(cur_node);
-    }
-    std::reverse(result_nodes->begin(), result_nodes->end());
-    return true;
-}
-
-} // namespace
-
-void AStarStrategy::clear() {
-    _closed_set.clear();
-    _open_set.clear();
-    _came_from.clear();
-    _g_score.clear();
-}
-
-double AStarStrategy::heuristic_cost(const TopoNode* src_node, const TopoNode* dest_node) {
-    const ::adu::common::hdmap::Point& src_point = src_node->anchor_point();
-    const ::adu::common::hdmap::Point& dest_point = dest_node->anchor_point();
-    double distance = fabs(src_point.x() - dest_point.x()) +
-                      fabs(src_point.y() - dest_point.y());
-    return distance;
-}
-
-bool AStarStrategy::search(const TopoGraph* graph,
-                           const TopoNode* src_node,
-                           const TopoNode* dest_node,
-                           const std::unordered_set<const TopoNode*>& black_list,
-                           std::vector<const TopoNode*>* const result_nodes) {
-    clear();
-    ROS_INFO("Start A* search algorithm.");
-    _closed_set.insert(black_list.begin(), black_list.end());
-
-    std::priority_queue<SearchNode> open_set_detail;
-
-    SearchNode src_search_node(src_node);
-    src_search_node.f = heuristic_cost(src_node, dest_node);
-    open_set_detail.push(src_search_node);
-
-    _open_set.insert(src_node);
-    _g_score[src_node] = 0.0;
-
-    SearchNode current_node;
-    while (!open_set_detail.empty()) {
-        current_node = open_set_detail.top();
-        if (current_node == dest_node) {
-            if (!reconstruct(_came_from, current_node.topo_node, result_nodes)) {
-                ROS_ERROR("Failed to reconstruct route.");
-                return false;
-            }
-            return true;
-        }
-        _open_set.erase(current_node.topo_node);
-        open_set_detail.pop();
-
-        if (_closed_set.count(current_node.topo_node) != 0) {
-            // if showed before, just skip...
-            continue;
-        }
-        _closed_set.emplace(current_node.topo_node);
-
-        double tentative_g_score = 0.0;
-        for (const auto* edge : current_node.topo_node->out_to_all_edge()) {
-            const auto* to_node = edge->to_node();
-            if (_closed_set.count(to_node) == 1) {
-                continue;
-            }
-            tentative_g_score = _g_score[current_node.topo_node] + get_cost_to_neighbor(edge);
-            if (_open_set.count(to_node) != 0 && tentative_g_score >= _g_score[to_node]) {
-                continue;
-            }
-            _g_score[to_node] = tentative_g_score;
-            SearchNode next_node(to_node);
-            next_node.f = tentative_g_score + heuristic_cost(to_node, dest_node);
-            open_set_detail.push(next_node);
-            _came_from[to_node] = current_node.topo_node;
-
-            if (_open_set.count(to_node) == 0) {
-                _open_set.insert(to_node);
-            }
-        }
-    }
-    ROS_ERROR("Failed to find goal lane with id %s", dest_node->lane_id().c_str());
-    return false;
-}
-
-bool AStarStrategy::search(const TopoGraph* graph,
-                           const SubTopoGraph* sub_graph,
-                           const TopoNode* src_node,
-                           const TopoNode* dest_node,
-                           std::vector<const TopoNode*>* const result_nodes) {
-    clear();
-    ROS_INFO("Start A* search algorithm.");
-
-    std::priority_queue<SearchNode> open_set_detail;
-
-    SearchNode src_search_node(src_node);
-    src_search_node.f = heuristic_cost(src_node, dest_node);
-    open_set_detail.push(src_search_node);
-
-    _open_set.insert(src_node);
-    _g_score[src_node] = 0.0;
-
-    SearchNode current_node;
-    std::unordered_set<const TopoEdge*> next_edge_set;
-    std::unordered_set<const TopoEdge*> sub_edge_set;
-    while (!open_set_detail.empty()) {
-        current_node = open_set_detail.top();
-        if (current_node == dest_node) {
-            if (!reconstruct(_came_from, current_node.topo_node, result_nodes)) {
-                ROS_ERROR("Failed to reconstruct route.");
-                return false;
-            }
-            return true;
-        }
-        _open_set.erase(current_node.topo_node);
-        open_set_detail.pop();
-
-        if (_closed_set.count(current_node.topo_node) != 0) {
-            // if showed before, just skip...
-            continue;
-        }
-        _closed_set.emplace(current_node.topo_node);
-
-        double tentative_g_score = 0.0;
-        next_edge_set.clear();
-        for (const auto* edge : current_node.topo_node->out_to_all_edge()) {
-            sub_edge_set.clear();
-            sub_graph->get_sub_edges_into_sub_graph(edge, &sub_edge_set);
-            next_edge_set.insert(sub_edge_set.begin(), sub_edge_set.end());
-        }
-
-        for (const auto* edge : next_edge_set) {
-            const auto* to_node = edge->to_node();
-            if (_closed_set.count(to_node) == 1) {
-                continue;
-            }
-            tentative_g_score = _g_score[current_node.topo_node] + get_cost_to_neighbor(edge);
-            if (_open_set.count(to_node) != 0 && tentative_g_score >= _g_score[to_node]) {
-                continue;
-            }
-            _g_score[to_node] = tentative_g_score;
-            SearchNode next_node(to_node);
-            next_node.f = tentative_g_score + heuristic_cost(to_node, dest_node);
-            open_set_detail.push(next_node);
-            _came_from[to_node] = current_node.topo_node;
-
-            if (_open_set.count(to_node) == 0) {
-                _open_set.insert(to_node);
-            }
-        }
-    }
-    ROS_ERROR("Failed to find goal lane with id %s", dest_node->lane_id().c_str());
-    return false;
-}
-
-} // namespace routing
-} // namespace adu
-

modules/routing/strategy/a_star_strategy.h

@@ -17,8 +17,8 @@
 #ifndef BAIDU_ADU_ROUTING_STRATEGY_A_STAR_STRATEGY_H
 #define BAIDU_ADU_ROUTING_STRATEGY_A_STAR_STRATEGY_H
 
-#include <unordered_set>
 #include <unordered_map>
+#include <unordered_set>
 
 #include "strategy/strategy.h"
 
@@ -26,34 +26,27 @@ namespace adu {
 namespace routing {
 
 class AStarStrategy : public Strategy {
-public:
-    AStarStrategy() = default;
-    ~AStarStrategy() = default;
-
-    virtual bool search(const TopoGraph* graph,
-                        const TopoNode* src_node,
-                        const TopoNode* dest_node,
-                        const std::unordered_set<const TopoNode*>& black_list,
-                        std::vector<const TopoNode*>* const result_nodes);
-
-    virtual bool search(const TopoGraph* graph,
-                        const SubTopoGraph* sub_graph,
-                        const TopoNode* src_node,
-                        const TopoNode* dest_node,
-                        std::vector<const TopoNode*>* const result_nodes);
-private:
-    void clear();
-    double heuristic_cost(const TopoNode* src_node, const TopoNode* dest_node);
-
-private:
-    std::unordered_set<const TopoNode*> _open_set;
-    std::unordered_set<const TopoNode*> _closed_set;
-    std::unordered_map<const TopoNode*, const TopoNode*> _came_from;
-    std::unordered_map<const TopoNode*, double> _g_score;
+ public:
+  AStarStrategy() = default;
+  ~AStarStrategy() = default;
+
+  virtual bool search(const TopoGraph* graph, const TopoNode* src_node,
+                      const TopoNode* dest_node,
+                      const std::unordered_set<const TopoNode*>& black_list,
+                      std::vector<const TopoNode*>* const result_nodes);
+
+ private:
+  void clear();
+  double heuristic_cost(const TopoNode* src_node, const TopoNode* dest_node);
+
+ private:
+  std::unordered_set<const TopoNode*> _open_set;
+  std::unordered_set<const TopoNode*> _closed_set;
+  std::unordered_map<const TopoNode*, const TopoNode*> _came_from;
+  std::unordered_map<const TopoNode*, double> _g_score;
 };
 
-} // namespace routing
-} // namespace adu
-
-#endif // BAIDU_ADU_ROUTING_STRATEGY_A_STAR_STRATEGY_H
+}  // namespace routing
+}  // namespace adu
 
+#endif  // BAIDU_ADU_ROUTING_STRATEGY_A_STAR_STRATEGY_H

modules/routing/strategy/strategy.h

@@ -21,29 +21,20 @@
 namespace adu {
 namespace routing {
 
-class SubTopoGraph;
 class TopoGraph;
 class TopoNode;
 
 class Strategy {
-public:
-    virtual ~Strategy() { };
+ public:
+  virtual ~Strategy(){};
 
-    virtual bool search(const TopoGraph* graph,
-                        const TopoNode* src_node,
-                        const TopoNode* dest_node,
-                        const std::unordered_set<const TopoNode*>& black_list,
-                        std::vector<const TopoNode*>* const result_nodes) = 0;
-
-    virtual bool search(const TopoGraph* graph,
-                        const SubTopoGraph* sub_graph,
-                        const TopoNode* src_node,
-                        const TopoNode* dest_node,
-                        std::vector<const TopoNode*>* const result_nodes) = 0;
+  virtual bool search(const TopoGraph* graph, const TopoNode* src_node,
+                      const TopoNode* dest_node,
+                      const std::unordered_set<const TopoNode*>& black_list,
+                      std::vector<const TopoNode*>* const result_nodes) = 0;
 };
 
-} // namespace routing
-} // namespace adu
-
-#endif // BAIDU_ADU_ROUTING_STRATEGY_STRATEGY_H
+}  // namespace routing
+}  // namespace adu
 
+#endif  // BAIDU_ADU_ROUTING_STRATEGY_STRATEGY_H

modules/routing/test/common/utils_test.cpp → modules/routing/test/common/utils_test.cc

@@ -25,24 +25,22 @@ namespace adu {
 namespace routing {
 
 TEST(FileUtilsTestSuit, test1) {
-    std::string file_path = "./test.bin";
-    std::string map_version = "versionA";
+  std::string file_path = "./test.bin";
+  std::string map_version = "versionA";
 
-    ::adu::routing::common::Graph graph_1;
-    graph_1.set_hdmap_version(map_version);
+  ::adu::routing::common::Graph graph_1;
+  graph_1.set_hdmap_version(map_version);
 
-    ::adu::routing::common::Graph graph_2;
-    ASSERT_TRUE(FileUtils::dump_protobuf_data_to_file(file_path, graph_1));
-    ASSERT_TRUE(FileUtils::load_protobuf_data_from_file(file_path, &graph_2));
-    ASSERT_EQ(map_version, graph_2.hdmap_version());
+  ::adu::routing::common::Graph graph_2;
+  ASSERT_TRUE(FileUtils::dump_protobuf_data_to_file(file_path, graph_1));
+  ASSERT_TRUE(FileUtils::load_protobuf_data_from_file(file_path, &graph_2));
+  ASSERT_EQ(map_version, graph_2.hdmap_version());
 }
 
 }  // namespace routing
 }  // namespace adu
 
 int main(int argc, char **argv) {
-    testing::InitGoogleTest(&argc, argv);
-    return RUN_ALL_TESTS();
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
 }
-
-

modules/routing/test/graph/topo_graph_test.cpp

-/******************************************************************************
-  * 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 "test/test_tool/test_utils.h"
-
-namespace adu {
-namespace routing {
-
-using ::adu::routing::common::Graph;
-
-TEST(TopoGraphTestSuit, test_graph_1) {
-    Graph graph;
-    get_graph_for_test(&graph);
-
-    TopoGraph topo_graph;
-    ASSERT_TRUE(topo_graph.load_graph(graph));
-
-    ASSERT_EQ(TEST_MAP_VERSION, topo_graph.map_version());
-    ASSERT_EQ(TEST_MAP_DISTRICT, topo_graph.map_district());
-
-    const TopoNode* node_1 = topo_graph.get_node(TEST_L1);
-    ASSERT_TRUE(node_1 != nullptr);
-    ASSERT_EQ(1, node_1->in_from_all_edge().size());
-    ASSERT_EQ(1, node_1->in_from_right_edge().size());
-    ASSERT_EQ(2, node_1->out_to_all_edge().size());
-    ASSERT_EQ(1, node_1->out_to_suc_edge().size());
-    ASSERT_EQ(1, node_1->out_to_right_edge().size());
-    ASSERT_DOUBLE_EQ(TEST_LANE_COST, node_1->cost());
-    ASSERT_EQ(TEST_L1, node_1->lane_id());
-    ASSERT_EQ(TEST_R1, node_1->road_id());
-    ASSERT_EQ(node_1, node_1->origin_node());
-    ASSERT_DOUBLE_EQ(0.0, node_1->start_s());
-    ASSERT_DOUBLE_EQ(TEST_LANE_LENGTH, node_1->end_s());
-    ASSERT_DOUBLE_EQ(TEST_LANE_LENGTH, node_1->length());
-    ASSERT_FALSE(node_1->is_sub_node());
-
-    const TopoNode* node_4 = topo_graph.get_node(TEST_L4);
-    ASSERT_TRUE(node_4 != nullptr);
-    ASSERT_EQ(2, node_4->in_from_all_edge().size());
-    ASSERT_EQ(1, node_4->in_from_pre_edge().size());
-    ASSERT_EQ(1, node_4->in_from_left_edge().size());
-    ASSERT_EQ(1, node_4->out_to_all_edge().size());
-    ASSERT_EQ(1, node_4->out_to_left_edge().size());
-    ASSERT_DOUBLE_EQ(TEST_LANE_COST, node_4->cost());
-    ASSERT_EQ(TEST_L4, node_4->lane_id());
-    ASSERT_EQ(TEST_R2, node_4->road_id());
-    ASSERT_EQ(node_4, node_4->origin_node());
-    ASSERT_DOUBLE_EQ(0.0, node_4->start_s());
-    ASSERT_DOUBLE_EQ(TEST_LANE_LENGTH, node_4->end_s());
-    ASSERT_DOUBLE_EQ(TEST_LANE_LENGTH, node_4->length());
-    ASSERT_FALSE(node_4->is_sub_node());
-}
-
-}  // namespace routing
-}  // namespace adu
-
-int main(int argc, char **argv) {
-    testing::InitGoogleTest(&argc, argv);
-    return RUN_ALL_TESTS();
-}
-
-

modules/routing/test/graph/topo_node_test.cpp

-/******************************************************************************
-  * 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 <string>
-
-#include "test/test_tool/test_utils.h"
-
-namespace adu {
-namespace routing {
-
-namespace {
-
-using ::adu::routing::common::Node;
-
-void get_range_vec(std::vector<NodeSRange>* range_vec) {
-    double s_s_1 = 1.0;
-    double e_s_1 = 12.0;
-    double s_s_2 = 15.0;
-    double e_s_2 = 27.0;
-    double s_s_3 = 30.0;
-    double e_s_3 = 45.1;
-    double s_s_4 = 55.5;
-    double e_s_4 = 70.0;
-
-    range_vec->push_back(NodeSRange(s_s_2, e_s_2));
-    range_vec->push_back(NodeSRange(s_s_4, e_s_4));
-    range_vec->push_back(NodeSRange(s_s_1, e_s_1));
-    range_vec->push_back(NodeSRange(s_s_3, e_s_3));
-
-    sort(range_vec->begin(), range_vec->end());
-
-    ASSERT_DOUBLE_EQ(s_s_1, range_vec->at(0).start_s());
-    ASSERT_DOUBLE_EQ(e_s_1, range_vec->at(0).end_s());
-    ASSERT_DOUBLE_EQ(s_s_2, range_vec->at(1).start_s());
-    ASSERT_DOUBLE_EQ(e_s_2, range_vec->at(1).end_s());
-    ASSERT_DOUBLE_EQ(s_s_3, range_vec->at(2).start_s());
-    ASSERT_DOUBLE_EQ(e_s_3, range_vec->at(2).end_s());
-    ASSERT_DOUBLE_EQ(s_s_4, range_vec->at(3).start_s());
-    ASSERT_DOUBLE_EQ(e_s_4, range_vec->at(3).end_s());
-}
-
-} // namespace
-
-TEST(TopoNodeTestSuit, static_func_test) {
-    std::vector<NodeSRange> range_vec;
-    get_range_vec(&range_vec);
-    {
-        double start_s = 13.0;
-        double end_s = 26.0;
-        ASSERT_TRUE(TopoNode::is_out_range_enough(range_vec, start_s, end_s));
-    }
-    {
-        double start_s = 13.0;
-        double end_s = 23.0;
-        ASSERT_FALSE(TopoNode::is_out_range_enough(range_vec, start_s, end_s));
-    }
-    {
-        double start_s = 22.0;
-        double end_s = 32.0;
-        ASSERT_FALSE(TopoNode::is_out_range_enough(range_vec, start_s, end_s));
-    }
-    {
-        double start_s = 31.0;
-        double end_s = 44.0;
-        ASSERT_TRUE(TopoNode::is_out_range_enough(range_vec, start_s, end_s));
-    }
-    {
-        double start_s = -10;
-        double end_s = 100;
-        ASSERT_TRUE(TopoNode::is_out_range_enough(range_vec, start_s, end_s));
-    }
-}
-
-TEST(TopoNodeTestSuit, basic_test) {
-    Node node;
-    get_node_detail_for_test(&node, TEST_L1, TEST_R1);
-    TopoNode topo_node(node);
-    ASSERT_EQ(node.DebugString(), topo_node.node().DebugString());
-    ASSERT_EQ(TEST_L1, topo_node.lane_id());
-    ASSERT_EQ(TEST_R1, topo_node.road_id());
-    ASSERT_DOUBLE_EQ(TEST_MIDDLE_S, topo_node.anchor_point().x());
-    ASSERT_DOUBLE_EQ(0.0, topo_node.anchor_point().y());
-    ASSERT_DOUBLE_EQ(TEST_LANE_LENGTH, topo_node.length());
-    ASSERT_DOUBLE_EQ(TEST_LANE_COST, topo_node.cost());
-    ASSERT_TRUE(topo_node.is_virtual());
-}
-
-}  // namespace routing
-}  // namespace adu
-
-int main(int argc, char **argv) {
-    testing::InitGoogleTest(&argc, argv);
-    return RUN_ALL_TESTS();
-}
-

modules/routing/test/test_tool/test_utils.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 BAIDU_ADU_ROUTING_TEST_TEST_TOOL_TEST_UTILS_H
-#define BAIDU_ADU_ROUTING_TEST_TEST_TOOL_TEST_UTILS_H
-
-#include <string>
-
-#include "graph/topo_graph.h"
-#include "graph/topo_node.h"
-#include "graph/topo_edge.h"
-#include "common/utils.h"
-
-#include "topo_graph.pb.h"
-#include "gtest/gtest.h"
-
-namespace adu {
-namespace routing {
-
-const std::string TEST_MAP_VERSION = "1.0.1";
-const std::string TEST_MAP_DISTRICT = "yongfeng";
-
-const std::string TEST_L1 = "L1";
-const std::string TEST_L2 = "L2";
-const std::string TEST_L3 = "L3";
-const std::string TEST_L4 = "L4";
-const std::string TEST_L5 = "L5";
-const std::string TEST_L6 = "L6";
-
-const std::string TEST_R1 = "R1";
-const std::string TEST_R2 = "R2";
-const std::string TEST_R3 = "R3";
-
-const double TEST_LANE_LENGTH = 100.0;
-const double TEST_LANE_COST = 1.1;
-const double TEST_EDGE_COST = 2.2;
-
-const double TEST_START_S = 0.0;
-const double TEST_MIDDLE_S = 0.0;
-const double TEST_END_S = TEST_LANE_LENGTH;
-
-void get_node_detail_for_test(::adu::routing::common::Node* const node,
-                              const std::string& lane_id,
-                              const std::string& road_id);
-
-void get_node_for_test(::adu::routing::common::Node* const node,
-                       const std::string& lane_id,
-                       const std::string& road_id);
-
-void get_edge_for_test(::adu::routing::common::Edge* const edge,
-                       const std::string& lane_id_1,
-                       const std::string& lane_id_2,
-                       const ::adu::routing::common::Edge::DirectionType& type);
-
-void get_graph_for_test(::adu::routing::common::Graph* graph);
-
-void get_graph_2_for_test(::adu::routing::common::Graph* graph);
-
-} // namespace routing
-} // namespace adu
-
-#endif // BAIDU_ADU_ROUTING_TEST_TEST_TOOL_TEST_UTILS_H
-

modules/routing/tools/.gitattributes

+*.bin filter=lfs diff=lfs merge=lfs -text
\ No newline at end of file

modules/routing/tool/debug_passage_region.py → modules/routing/tools/debug_passage_region.py

-#!/usr/bin/env python
-
-###############################################################################
-# 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
+# Copyright (c) 2017 Baidu.com, Inc. All Rights Reserved
 #
-# 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.
-###############################################################################
+################################################################################
+"""
+This module provides routing result and passage region parse and plot functions.
+
+Authors: fuxiaoxin(fuxiaoxin@baidu.com)
+"""
+
 import sys
 import itertools
 import matplotlib.pyplot as plt

modules/routing/tool/start_topo_creator.sh → modules/routing/tools/start_topo_creator.sh

@@ -42,7 +42,7 @@ base_map_dir="/home/caros/adu_data/map"
 base_map_name="base_map.bin"
 
 # path of routing topology file to be dumped
-dump_topo_path="/home/fuxiaoxin/Desktop/routing_map.bin"
+dump_topo_path="/home/fan/develop/baidu/adu/routing/script/routing_map.bin"
 
 set +x