Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
01feae0b
A
apollo
项目概览
Pinoxchio
/
apollo
与 Fork 源项目一致
从无法访问的项目Fork
通知
2
Star
0
Fork
0
代码
文件
提交
分支
Tags
贡献者
分支图
Diff
Issue
0
列表
看板
标记
里程碑
合并请求
0
Wiki
0
Wiki
分析
仓库
DevOps
项目成员
Pages
A
apollo
项目概览
项目概览
详情
发布
仓库
仓库
文件
提交
分支
标签
贡献者
分支图
比较
Issue
0
Issue
0
列表
看板
标记
里程碑
合并请求
0
合并请求
0
Pages
分析
分析
仓库分析
DevOps
Wiki
0
Wiki
成员
成员
收起侧边栏
关闭侧边栏
动态
分支图
创建新Issue
提交
Issue看板
体验新版 GitCode,发现更多精彩内容 >>
提交
01feae0b
编写于
8月 02, 2017
作者:
F
Fan Zhu
提交者:
Jiangtao Hu
8月 01, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Resolve conflict
上级
74715d42
变更
11
显示空白变更内容
内联
并排
Showing
11 changed file
with
173 addition
and
117 deletion
+173
-117
modules/routing/graph/BUILD
modules/routing/graph/BUILD
+46
-0
modules/routing/strategy/a_star_strategy.cc
modules/routing/strategy/a_star_strategy.cc
+6
-10
modules/routing/strategy/a_star_strategy.h
modules/routing/strategy/a_star_strategy.h
+1
-1
modules/routing/topo_creator/BUILD
modules/routing/topo_creator/BUILD
+72
-58
modules/routing/topo_creator/edge_creator.cc
modules/routing/topo_creator/edge_creator.cc
+3
-3
modules/routing/topo_creator/edge_creator.h
modules/routing/topo_creator/edge_creator.h
+12
-12
modules/routing/topo_creator/graph_creator.cc
modules/routing/topo_creator/graph_creator.cc
+5
-5
modules/routing/topo_creator/graph_creator.h
modules/routing/topo_creator/graph_creator.h
+7
-7
modules/routing/topo_creator/node_creator.cc
modules/routing/topo_creator/node_creator.cc
+9
-9
modules/routing/topo_creator/node_creator.h
modules/routing/topo_creator/node_creator.h
+10
-10
modules/routing/topo_creator/topo_creator.cc
modules/routing/topo_creator/topo_creator.cc
+2
-2
未找到文件。
modules/routing/graph/BUILD
0 → 100644
浏览文件 @
01feae0b
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
()
modules/routing/strategy/a_star_strategy.cc
浏览文件 @
01feae0b
...
...
@@ -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
;
}
...
...
modules/routing/strategy/a_star_strategy.h
浏览文件 @
01feae0b
...
...
@@ -20,7 +20,7 @@
#include <unordered_map>
#include <unordered_set>
#include "strategy/strategy.h"
#include "
modules/routing/
strategy/strategy.h"
namespace
apollo
{
namespace
routing
{
...
...
modules/routing/topo_creator/BUILD
浏览文件 @
01feae0b
#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
()
modules/routing/topo_creator/edge_creator.cc
浏览文件 @
01feae0b
...
...
@@ -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
;
...
...
modules/routing/topo_creator/edge_creator.h
浏览文件 @
01feae0b
...
...
@@ -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
...
...
modules/routing/topo_creator/graph_creator.cc
浏览文件 @
01feae0b
...
...
@@ -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
;
...
...
modules/routing/topo_creator/graph_creator.h
浏览文件 @
01feae0b
...
...
@@ -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
;
...
...
modules/routing/topo_creator/node_creator.cc
浏览文件 @
01feae0b
...
...
@@ -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
;
}
}
...
...
modules/routing/topo_creator/node_creator.h
浏览文件 @
01feae0b
...
...
@@ -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
...
...
modules/routing/topo_creator/topo_creator.cc
浏览文件 @
01feae0b
...
...
@@ -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
<::
a
du
::
routing
::
GraphCreator
>
creator_ptr
;
creator_ptr
.
reset
(
new
::
a
du
::
routing
::
GraphCreator
(
std
::
unique_ptr
<::
a
pollo
::
routing
::
GraphCreator
>
creator_ptr
;
creator_ptr
.
reset
(
new
::
a
pollo
::
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.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录