提交 01feae0b 编写于 作者: F Fan Zhu 提交者: Jiangtao Hu

Resolve conflict

上级 74715d42
load("//tools:cpplint.bzl", "cpplint")
package(default_visibility = ["//visibility:public"])
cc_library(
name = "graph",
deps = [
":routing_topo_node",
":routing_topo_graph",
],
)
cc_library(
name = "routing_topo_node",
srcs = [
"topo_node.cc",
],
hdrs = [
"topo_node.h",
],
deps = [
"//modules/common/proto:common_proto",
"//modules/map/proto:map_proto",
"//modules/routing/proto:routing_proto",
],
)
cc_library(
name = "routing_topo_graph",
srcs = [
"topo_graph.cc",
],
hdrs = [
"topo_graph.h",
],
deps = [
":routing_topo_node",
"//modules/common/proto:common_proto",
"//modules/map/proto:map_proto",
"//modules/routing/proto:routing_proto",
"//modules/common:common",
"//modules/routing/common"
],
)
cpplint()
......@@ -20,12 +20,9 @@
#include <limits>
#include <queue>
#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"
#include "modules/routing/graph/topo_graph.h"
#include "modules/routing/graph/topo_node.h"
#include "modules/routing/strategy/a_star_strategy.h"
namespace apollo {
namespace routing {
......@@ -96,7 +93,7 @@ bool AStarStrategy::search(
const std::unordered_set<const TopoNode*>& black_list,
std::vector<const TopoNode*>* const result_nodes) {
clear();
ROS_INFO("Start A* search algorithm.");
AINFO << "Start A* search algorithm.";
_closed_set.insert(black_list.begin(), black_list.end());
......@@ -114,7 +111,7 @@ bool AStarStrategy::search(
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.");
AERROR << "Failed to reconstruct route.";
return false;
}
return true;
......@@ -152,8 +149,7 @@ bool AStarStrategy::search(
}
}
}
ROS_ERROR("Failed to find goal lane with id %s",
dest_node->lane_id().c_str());
AERROR << "Failed to find goal lane";
return false;
}
......
......@@ -20,7 +20,7 @@
#include <unordered_map>
#include <unordered_set>
#include "strategy/strategy.h"
#include "modules/routing/strategy/strategy.h"
namespace apollo {
namespace routing {
......
#load("//tools:cpplint.bzl", "cpplint")
#
#package(default_visibility = ["//visibility:public"])
#
#cc_library(
# name = "edge_creator",
# srcs = [
# "edge_creator.cc",
# ],
# hdrs = [
# "edge_creator.h",
# ],
# deps = [
# "//external:gflags",
# ],
#)
#
#cc_library(
# name = "graph_creator",
# srcs = [
# "graph_creator.cc",
# ],
# hdrs = [
# "graph_creator.h",
# ],
# deps = [
# "//external:gflags",
# ":edge_creator",
# ":node_creator",
# ],
#)
#
#cc_library(
# name = "node_creator",
# srcs = [
# "node_creator.cc",
# ],
# hdrs = [
# "node_creator.h",
# ],
# deps = [
# "//external:gflags",
# ],
#)
#
#cc_binary(
# name = "topo_creator",
# srcs = ["topo_creator.cc"],
# deps = [
# "//external:gflags",
# ":graph_creator",
# "//modules/common:log",
# "//modules/routing/common",
# "//modules/control/proto:control_proto",
# ],
#)
#
#cpplint()
load("//tools:cpplint.bzl", "cpplint")
package(default_visibility = ["//visibility:public"])
cc_library(
name = "edge_creator",
srcs = [
"edge_creator.cc",
],
hdrs = [
"edge_creator.h",
],
deps = [
"//external:gflags",
"//modules/routing/proto:routing_proto",
"//modules/map/proto:map_proto",
"//modules/common:common",
"//modules/routing/common"
],
)
cc_library(
name = "graph_creator",
srcs = [
"graph_creator.cc",
],
hdrs = [
"graph_creator.h",
],
deps = [
"//external:gflags",
":edge_creator",
":node_creator",
"//modules/routing/proto:routing_proto",
"//modules/map/proto:map_proto",
"//modules/common:common",
"//modules/routing/common"
],
)
cc_library(
name = "node_creator",
srcs = [
"node_creator.cc",
],
hdrs = [
"node_creator.h",
],
deps = [
"//external:gflags",
"//modules/routing/proto:routing_proto",
"//modules/map/proto:map_proto",
"//modules/common:common",
"//modules/routing/common"
],
)
cc_binary(
name = "topo_creator",
srcs = ["topo_creator.cc"],
deps = [
"//external:gflags",
":graph_creator",
"//modules/common:log",
"//modules/routing/proto:routing_proto",
"//modules/map/proto:map_proto",
"//modules/common:common",
"//modules/routing/common"
],
)
cpplint()
......@@ -23,9 +23,9 @@ namespace routing {
namespace {
using ::apollo::routing::common::Node;
using ::apollo::routing::common::Edge;
using ::apollo::routing::common::CurveRange;
using ::apollo::routing::Node;
using ::apollo::routing::Edge;
using ::apollo::routing::CurveRange;
const double CHANGE_PENALTY = 50; // equal to 50 meter length
const double BASE_CHANGING_LENGTH = 50;
......
......@@ -25,22 +25,22 @@ namespace routing {
class EdgeCreator {
public:
static void get_pb_edge(
const ::apollo::routing::common::Node& node_from,
const ::apollo::routing::common::Node& node_to,
const ::apollo::routing::common::Edge::DirectionType& type,
::apollo::routing::common::Edge* pb_edge);
const ::apollo::routing::Node& node_from,
const ::apollo::routing::Node& node_to,
const ::apollo::routing::Edge::DirectionType& type,
::apollo::routing::Edge* pb_edge);
private:
static void init_edge_info(
const ::apollo::routing::common::Node& node_from,
const ::apollo::routing::common::Node& node_to,
const ::apollo::routing::common::Edge::DirectionType& type,
::apollo::routing::common::Edge* pb_edge);
const ::apollo::routing::Node& node_from,
const ::apollo::routing::Node& node_to,
const ::apollo::routing::Edge::DirectionType& type,
::apollo::routing::Edge* pb_edge);
static void init_edge_cost(
const ::apollo::routing::common::Node& node_from,
const ::apollo::routing::common::Node& node_to,
const ::apollo::routing::common::Edge::DirectionType& type,
::apollo::routing::common::Edge* pb_edge);
const ::apollo::routing::Node& node_from,
const ::apollo::routing::Node& node_to,
const ::apollo::routing::Edge::DirectionType& type,
::apollo::routing::Edge* pb_edge);
};
} // namespace routing
......
......@@ -14,11 +14,11 @@
* limitations under the License.
*****************************************************************************/
#include "modules/routing/topo_creator/topo_creator/graph_creator.h"
#include "modules/routing/topo_creator/graph_creator.h"
#include "glog/logging.h"
#include "common/utils.h"
#include "modules/routing/common/utils.h"
#include "modules/routing/topo_creator/edge_creator.h"
#include "modules/routing/topo_creator/node_creator.h"
......@@ -29,10 +29,10 @@ namespace {
using ::google::protobuf::RepeatedPtrField;
using ::apollo::common::hdmap::Id;
using ::apollo::hdmap::Id;
using ::apollo::routing::common::Node;
using ::apollo::routing::common::Edge;
using ::apollo::routing::Node;
using ::apollo::routing::Edge;
std::string get_edge_id(const std::string& from_id, const std::string& to_id) {
return from_id + "->" + to_id;
......
......@@ -20,8 +20,8 @@
#include <unordered_map>
#include <unordered_set>
#include "modules/routing/topo_creator/map.pb.h"
#include "modules/routing/topo_creator/topo_graph.pb.h"
#include "modules/map/proto/map.pb.h"
#include "modules/routing/proto/topo_graph.pb.h"
namespace apollo {
namespace routing {
......@@ -37,16 +37,16 @@ class GraphCreator {
private:
void add_edge(
const ::apollo::routing::common::Node& from_node,
const ::google::protobuf::RepeatedPtrField<::apollo::common::hdmap::Id>&
const ::apollo::routing::Node& from_node,
const ::google::protobuf::RepeatedPtrField<::apollo::hdmap::Id>&
to_node_vec,
const ::apollo::routing::common::Edge::DirectionType& type);
const ::apollo::routing::Edge::DirectionType& type);
private:
std::string _base_map_file_path;
std::string _dump_topo_file_path;
::apollo::common::hdmap::Map _pbmap;
::apollo::routing::common::Graph _graph;
::apollo::hdmap::Map _pbmap;
::apollo::routing::Graph _graph;
std::unordered_map<std::string, int> _node_index_map;
std::unordered_map<std::string, std::string> _road_id_map;
std::unordered_set<std::string> _showed_edge_id_set;
......
......@@ -25,13 +25,13 @@ namespace {
using ::google::protobuf::RepeatedPtrField;
using ::apollo::common::hdmap::Lane;
using ::apollo::common::hdmap::LaneBoundary;
using ::apollo::common::hdmap::LaneBoundaryType;
using ::apollo::hdmap::Lane;
using ::apollo::hdmap::LaneBoundary;
using ::apollo::hdmap::LaneBoundaryType;
using ::apollo::routing::common::Node;
using ::apollo::routing::common::Edge;
using ::apollo::routing::common::CurveRange;
using ::apollo::routing::Node;
using ::apollo::routing::Edge;
using ::apollo::routing::CurveRange;
const double BASE_SPEED = 4.167; // 15kmh
const double LEFT_TURN_PENALTY = 50; // meter
......@@ -113,11 +113,11 @@ void NodeCreator::init_node_cost(const Lane& lane, Node* const node) {
(speed_limit >= BASE_SPEED) ? (1 / sqrt(speed_limit / BASE_SPEED)) : 1.0;
double cost = lane_length * ratio;
if (lane.has_turn()) {
if (lane.turn() == ::apollo::common::hdmap::Lane::LEFT_TURN) {
if (lane.turn() == ::apollo::hdmap::Lane::LEFT_TURN) {
cost += LEFT_TURN_PENALTY;
} else if (lane.turn() == ::apollo::common::hdmap::Lane::RIGHT_TURN) {
} else if (lane.turn() == ::apollo::hdmap::Lane::RIGHT_TURN) {
cost += RIGHT_TURN_PENALTY;
} else if (lane.turn() == ::apollo::common::hdmap::Lane::U_TURN) {
} else if (lane.turn() == ::apollo::hdmap::Lane::U_TURN) {
cost += UTURN_PENALTY;
}
}
......
......@@ -17,29 +17,29 @@
#ifndef MODULES_ROUTING_TOPO_CREATOR_NODE_CREATOR_H
#define MODULES_ROUTING_TOPO_CREATOR_NODE_CREATOR_H
#include "modules/routing/topo_creator/map_lane.pb.h"
#include "modules/routing/topo_creator/topo_graph.pb.h"
#include "modules/map/proto/map_lane.pb.h"
#include "modules/routing/proto/topo_graph.pb.h"
namespace apollo {
namespace routing {
class NodeCreator {
public:
static void get_pb_node(const ::apollo::common::hdmap::Lane& lane,
static void get_pb_node(const ::apollo::hdmap::Lane& lane,
const std::string& road_id,
::apollo::routing::common::Node* pb_node);
::apollo::routing::Node* pb_node);
private:
static void add_out_boundary(
const ::apollo::common::hdmap::LaneBoundary& bound, double lane_length,
const ::apollo::hdmap::LaneBoundary& bound, double lane_length,
::google::protobuf::RepeatedPtrField<
::apollo::routing::common::CurveRange>* const out_range);
::apollo::routing::CurveRange>* const out_range);
static void init_node_info(const ::apollo::common::hdmap::Lane& lane,
static void init_node_info(const ::apollo::hdmap::Lane& lane,
const std::string& road_id,
::apollo::routing::common::Node* const node);
static void init_node_cost(const ::apollo::common::hdmap::Lane& lane,
::apollo::routing::common::Node* const node);
::apollo::routing::Node* const node);
static void init_node_cost(const ::apollo::hdmap::Lane& lane,
::apollo::routing::Node* const node);
};
} // namespace routing
......
......@@ -30,8 +30,8 @@ DEFINE_string(dump_topo_path, "/home/fuxiaoxin/Desktop/routing_map.bin",
int main(int argc, char **argv) {
google::InitGoogleLogging(argv[0]);
google::ParseCommandLineFlags(&argc, &argv, true);
std::unique_ptr<::adu::routing::GraphCreator> creator_ptr;
creator_ptr.reset(new ::adu::routing::GraphCreator(
std::unique_ptr<::apollo::routing::GraphCreator> creator_ptr;
creator_ptr.reset(new ::apollo::routing::GraphCreator(
FLAGS_base_map_dir + "/" + FLAGS_base_map_name, FLAGS_dump_topo_path));
if (!creator_ptr->create()) {
LOG(ERROR) << "Create routing topo failed!";
......
Markdown is supported
0% .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑!
想要评论请 注册