提交 c738ccb1 编写于 作者: H henryhu6 提交者: Jiangtao Hu

Routing: remove gflags from topo creator

上级 5e95878b
......@@ -36,14 +36,3 @@ 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_double(base_speed, 4.167, "base speed for node creator, in m/s");
DEFINE_double(left_turn_penalty, 50,
"left turn penalty for node creater, in meter");
DEFINE_double(right_turn_penalty, 50,
"right turn penalty for node creater, in meter");
DEFINE_double(uturn_penalty, 50,
"left turn penalty for node creater, in meter");
DEFINE_double(change_penalty, 50, "change penalty for edge creater, in meter");
DEFINE_double(base_changing_length, 50,
"base change length penalty for edge creater, in meter");
......@@ -33,11 +33,4 @@ DECLARE_bool(enable_debug_mode);
DECLARE_string(debug_route_path);
DECLARE_string(debug_passage_region_path);
DECLARE_double(base_speed);
DECLARE_double(left_turn_penalty);
DECLARE_double(right_turn_penalty);
DECLARE_double(uturn_penalty);
DECLARE_double(change_penalty);
DECLARE_double(base_changing_length);
#endif // MODULES_ROUTING_COMMON_ROUTING_GFLAGS_H_
......@@ -17,20 +17,21 @@
#include "modules/routing/topo_creator/edge_creator.h"
#include <math.h>
#include "modules/routing/common/routing_gflags.h"
namespace apollo {
namespace routing {
void EdgeCreator::GetPbEdge(const Node& node_from, const Node& node_to,
const Edge::DirectionType& type, Edge* pb_edge) {
InitEdgeInfo(node_from, node_to, type, pb_edge);
InitEdgeCost(node_from, node_to, type, pb_edge);
const Edge::DirectionType& type, Edge* pb_edge,
const RoutingConfig *routing_config) {
InitEdgeInfo(node_from, node_to, type, pb_edge, routing_config);
InitEdgeCost(node_from, node_to, type, pb_edge, routing_config);
}
void EdgeCreator::InitEdgeInfo(const Node& node_from, const Node& node_to,
const Edge::DirectionType& type,
Edge* const edge) {
Edge* const edge,
const RoutingConfig *routing_config) {
edge->set_from_lane_id(node_from.lane_id());
edge->set_to_lane_id(node_to.lane_id());
edge->set_direction_type(type);
......@@ -38,7 +39,8 @@ void EdgeCreator::InitEdgeInfo(const Node& node_from, const Node& node_to,
void EdgeCreator::InitEdgeCost(const Node& node_from, const Node& node_to,
const Edge::DirectionType& type,
Edge* const edge) {
Edge* const edge,
const RoutingConfig *routing_config) {
edge->set_cost(0.0);
if (type == Edge::LEFT || type == Edge::RIGHT) {
const auto& target_range =
......@@ -48,10 +50,10 @@ void EdgeCreator::InitEdgeCost(const Node& node_from, const Node& node_to,
changing_area_length += range.end().s() - range.start().s();
}
double ratio =
(changing_area_length >= FLAGS_base_changing_length)
? pow(changing_area_length / FLAGS_base_changing_length, -1.5)
: 1.0;
edge->set_cost(FLAGS_change_penalty * ratio);
(changing_area_length >= routing_config->base_changing_length()) ?
pow(changing_area_length / routing_config->base_changing_length(), -1.5)
: 1.0;
edge->set_cost(routing_config->change_penalty() * ratio);
}
}
......
......@@ -18,6 +18,7 @@
#define MODULES_ROUTING_TOPO_CREATOR_EDGE_CREATOR_H
#include "modules/routing/proto/topo_graph.pb.h"
#include "modules/routing/proto/routing_config.pb.h"
namespace apollo {
namespace routing {
......@@ -26,15 +27,15 @@ class EdgeCreator {
public:
static void GetPbEdge(const Node& node_from, const Node& node_to,
const Edge::DirectionType& type,
Edge* pb_edge);
Edge* pb_edge, const RoutingConfig * routingconfig);
private:
static void InitEdgeInfo(const Node& node_from, const Node& node_to,
const Edge::DirectionType& type,
Edge* pb_edge);
Edge* pb_edge, const RoutingConfig * routingconfig);
static void InitEdgeCost(const Node& node_from, const Node& node_to,
const Edge::DirectionType& type,
Edge* pb_edge);
Edge* pb_edge, const RoutingConfig * routingconfig);
};
} // namespace routing
......
......@@ -98,10 +98,11 @@ bool GraphCreator::Create() {
node_index_map_[lane_id] = graph_.node_size();
const auto iter = road_id_map_.find(lane_id);
if (iter != road_id_map_.end()) {
NodeCreator::GetPbNode(lane, iter->second, graph_.add_node());
NodeCreator::GetPbNode(lane, iter->second, graph_.add_node(),
routing_conf_);
} else {
LOG(WARNING) << "Failed to find road id of lane " << lane_id;
NodeCreator::GetPbNode(lane, "", graph_.add_node());
NodeCreator::GetPbNode(lane, "", graph_.add_node(), routing_conf_);
}
}
......@@ -175,7 +176,8 @@ void GraphCreator::AddEdge(const Node& from_node,
continue;
}
const auto& to_node = graph_.node(iter->second);
EdgeCreator::GetPbEdge(from_node, to_node, type, graph_.add_edge());
EdgeCreator::GetPbEdge(from_node, to_node, type, graph_.add_edge(),
routing_conf_);
}
}
......
......@@ -19,8 +19,6 @@
#include <cmath>
#include <algorithm>
#include "modules/routing/common/routing_gflags.h"
namespace apollo {
namespace routing {
......@@ -57,9 +55,9 @@ double GetLaneLength(const Lane& lane) {
} // namespace
void NodeCreator::GetPbNode(const Lane& lane, const std::string& road_id,
Node* pb_node) {
InitNodeInfo(lane, road_id, pb_node);
InitNodeCost(lane, pb_node);
Node* pb_node, const RoutingConfig * routingconfig){
InitNodeInfo(lane, road_id, pb_node, routingconfig);
InitNodeCost(lane, pb_node, routingconfig);
}
void NodeCreator::AddOutBoundary(
......@@ -82,7 +80,8 @@ void NodeCreator::AddOutBoundary(
}
void NodeCreator::InitNodeInfo(const Lane& lane, const std::string& road_id,
Node* const node) {
Node* const node,
const RoutingConfig * routingconfig) {
double lane_length = GetLaneLength(lane);
node->set_lane_id(lane.id().id());
node->set_road_id(road_id);
......@@ -98,21 +97,22 @@ void NodeCreator::InitNodeInfo(const Lane& lane, const std::string& road_id,
}
}
void NodeCreator::InitNodeCost(const Lane& lane, Node* const node) {
void NodeCreator::InitNodeCost(const Lane& lane, Node* const node,
const RoutingConfig * routingconfig) {
double lane_length = GetLaneLength(lane);
double speed_limit =
(lane.has_speed_limit()) ? lane.speed_limit() : FLAGS_base_speed;
double ratio = (speed_limit >= FLAGS_base_speed)
? (1 / sqrt(speed_limit / FLAGS_base_speed))
(lane.has_speed_limit()) ? lane.speed_limit() : routingconfig->base_speed();
double ratio = (speed_limit >= routingconfig->base_speed())
? (1 / sqrt(speed_limit / routingconfig->base_speed()))
: 1.0;
double cost = lane_length * ratio;
if (lane.has_turn()) {
if (lane.turn() == Lane::LEFT_TURN) {
cost += FLAGS_left_turn_penalty;
cost += routingconfig->left_turn_penalty();
} else if (lane.turn() == Lane::RIGHT_TURN) {
cost += FLAGS_right_turn_penalty;
cost += routingconfig->right_turn_penalty();
} else if (lane.turn() == Lane::U_TURN) {
cost += FLAGS_uturn_penalty;
cost += routingconfig->uturn_penalty();
}
}
node->set_cost(cost);
......
......@@ -21,6 +21,7 @@
#include "modules/map/proto/map_lane.pb.h"
#include "modules/routing/proto/topo_graph.pb.h"
#include "modules/routing/proto/routing_config.pb.h"
namespace apollo {
namespace routing {
......@@ -29,7 +30,7 @@ class NodeCreator {
public:
static void GetPbNode(const ::apollo::hdmap::Lane& lane,
const std::string& road_id,
Node* pb_node);
Node* pb_node, const RoutingConfig * routingconfig);
private:
static void AddOutBoundary(
......@@ -38,8 +39,10 @@ class NodeCreator {
static void InitNodeInfo(const ::apollo::hdmap::Lane& lane,
const std::string& road_id,
Node* const node);
static void InitNodeCost(const ::apollo::hdmap::Lane& lane, Node* const node);
Node* const node,
const RoutingConfig * routingconfig);
static void InitNodeCost(const ::apollo::hdmap::Lane& lane, Node* const node,
const RoutingConfig * routingconfig);
};
} // namespace routing
......
Markdown is supported
0% .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑!
想要评论请 注册