Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
662e6496
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,发现更多精彩内容 >>
提交
662e6496
编写于
9月 14, 2018
作者:
D
Dong Li
提交者:
Liangliang Zhang
9月 14, 2018
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
tools: migrated manual_traffic_light
上级
87300a2e
变更
6
隐藏空白更改
内联
并排
Showing
6 changed file
with
185 addition
and
154 deletion
+185
-154
apollo.sh
apollo.sh
+2
-1
modules/planning/BUILD
modules/planning/BUILD
+3
-3
modules/tools/manual_traffic_light/BUILD
modules/tools/manual_traffic_light/BUILD
+9
-1
modules/tools/manual_traffic_light/manual_traffic_light.cc
modules/tools/manual_traffic_light/manual_traffic_light.cc
+153
-149
modules/tools/manual_traffic_light/manual_traffic_light.dag
modules/tools/manual_traffic_light/manual_traffic_light.dag
+11
-0
modules/tools/manual_traffic_light/manual_traffic_light.launch
...es/tools/manual_traffic_light/manual_traffic_light.launch
+7
-0
未找到文件。
apollo.sh
浏览文件 @
662e6496
...
...
@@ -111,7 +111,8 @@ function generate_build_targets() {
//modules/map/hdmap/...
//modules/planning/...
//modules/canbus/...
//modules/third_party_perception/..."
//modules/third_party_perception/...
//modules/tools/manual_traffic_light/..."
# //modules/drivers/radar/conti_radar/...
# //modules/drivers/gnss/...
# //modules/monitor/...
...
...
modules/planning/BUILD
浏览文件 @
662e6496
...
...
@@ -25,19 +25,19 @@ cc_library(
cc_binary
(
name
=
"libplanning_component.so"
,
deps
=
[
":planning_component_lib"
],
linkopts
=
[
"-shared"
],
deps
=
[
":planning_component_lib"
],
)
cc_library
(
name
=
"planning_lib"
,
srcs
=
[
"std_planning.cc"
,
"planning_base.cc"
,
"std_planning.cc"
,
],
hdrs
=
[
"std_planning.h"
,
"planning_base.h"
,
"std_planning.h"
,
],
deps
=
[
"//modules/common"
,
...
...
modules/tools/manual_traffic_light/BUILD
浏览文件 @
662e6496
...
...
@@ -2,12 +2,14 @@ load("//tools:cpplint.bzl", "cpplint")
package
(
default_visibility
=
[
"//visibility:public"
])
cc_
bin
ary
(
cc_
libr
ary
(
name
=
"manual_traffic_light"
,
srcs
=
[
"manual_traffic_light.cc"
],
copts
=
[
"-DMODULE_NAME=
\\\"
manual_traffic_light
\\\"
"
],
data
=
[
"//modules/map:map_data"
],
deps
=
[
"//external:gflags"
,
"//framework:cybertron"
,
"//modules/common"
,
"//modules/common/configs:config_gflags"
,
"//modules/common/util"
,
...
...
@@ -17,4 +19,10 @@ cc_binary(
],
)
cc_binary
(
name
=
"libmanual_traffic_light_component.so"
,
linkopts
=
[
"-shared"
],
deps
=
[
":manual_traffic_light"
],
)
cpplint
()
modules/tools/manual_traffic_light/manual_traffic_light.cc
浏览文件 @
662e6496
...
...
@@ -20,18 +20,18 @@
#include <fstream>
#include <iostream>
#include "cybertron/component/timer_component.h"
#include "cybertron/cybertron.h"
#include "gflags/gflags.h"
#include "ros/include/ros/ros.h"
#include "modules/localization/proto/localization.pb.h"
#include "modules/perception/proto/traffic_light_detection.pb.h"
#include "modules/common/adapters/adapter_gflags.h"
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/configs/config_gflags.h"
#include "modules/common/log.h"
#include "modules/common/util/color.h"
#include "modules/common/util/file.h"
#include "modules/common/util/message_util.h"
#include "modules/common/util/string_util.h"
#include "modules/map/hdmap/adapter/opendrive_adapter.h"
#include "modules/map/hdmap/hdmap_common.h"
...
...
@@ -39,9 +39,6 @@
#include "modules/map/hdmap/hdmap_util.h"
using
apollo
::
common
::
PointENU
;
using
apollo
::
common
::
adapter
::
AdapterConfig
;
using
apollo
::
common
::
adapter
::
AdapterManager
;
using
apollo
::
common
::
adapter
::
AdapterManagerConfig
;
using
apollo
::
common
::
color
::
ANSI_GREEN
;
using
apollo
::
common
::
color
::
ANSI_RED
;
using
apollo
::
common
::
color
::
ANSI_RESET
;
...
...
@@ -58,140 +55,23 @@ DEFINE_bool(all_lights, false, "set all lights on the map");
DEFINE_double
(
traffic_light_distance
,
1000.0
,
"only retrieves traffic lights within this distance"
);
bool
g_is_green
=
false
;
bool
g_updated
=
true
;
bool
GetAllTrafficLights
(
std
::
vector
<
SignalInfoConstPtr
>
*
traffic_lights
)
{
static
auto
map_filename
=
apollo
::
hdmap
::
BaseMapFile
();
static
apollo
::
hdmap
::
Map
map_proto
;
static
std
::
vector
<
SignalInfoConstPtr
>
map_traffic_lights
;
if
(
map_proto
.
lane_size
()
==
0
&&
map_traffic_lights
.
empty
())
{
AERROR
<<
"signal size: "
<<
map_proto
.
signal_size
();
if
(
apollo
::
common
::
util
::
EndWith
(
map_filename
,
".xml"
))
{
if
(
!
apollo
::
hdmap
::
adapter
::
OpendriveAdapter
::
LoadData
(
map_filename
,
&
map_proto
))
{
return
false
;
}
}
else
if
(
!
apollo
::
common
::
util
::
GetProtoFromFile
(
map_filename
,
&
map_proto
))
{
return
false
;
}
for
(
const
auto
&
signal
:
map_proto
.
signal
())
{
const
auto
*
hdmap
=
HDMapUtil
::
BaseMapPtr
();
if
(
!
hdmap
)
{
AERROR
<<
"Invalid HD Map."
;
return
false
;
}
map_traffic_lights
.
push_back
(
hdmap
->
GetSignalById
(
signal
.
id
()));
}
}
*
traffic_lights
=
map_traffic_lights
;
return
true
;
}
bool
GetTrafficLightsWithinDistance
(
std
::
vector
<
SignalInfoConstPtr
>
*
traffic_lights
)
{
CHECK_NOTNULL
(
traffic_lights
);
AdapterManager
::
Observe
();
if
(
AdapterManager
::
GetLocalization
()
->
Empty
())
{
AERROR
<<
"No localization received"
;
return
false
;
}
const
auto
*
hdmap
=
HDMapUtil
::
BaseMapPtr
();
if
(
!
hdmap
)
{
AERROR
<<
"Invalid HD Map."
;
return
false
;
}
auto
position
=
AdapterManager
::
GetLocalization
()
->
GetLatestObserved
().
pose
().
position
();
int
ret
=
hdmap
->
GetForwardNearestSignalsOnLane
(
position
,
FLAGS_traffic_light_distance
,
traffic_lights
);
if
(
ret
!=
0
)
{
AERROR
<<
"failed to get signals from position: "
<<
position
.
ShortDebugString
()
<<
" with distance: "
<<
FLAGS_traffic_light_distance
<<
" on map"
;
return
false
;
}
return
true
;
}
bool
CreateTrafficLightDetection
(
const
std
::
vector
<
SignalInfoConstPtr
>
&
signals
,
TrafficLight
::
Color
color
,
TrafficLightDetection
*
detection
)
{
CHECK_NOTNULL
(
detection
);
for
(
const
auto
&
iter
:
signals
)
{
auto
*
light
=
detection
->
add_traffic_light
();
light
->
set_color
(
color
);
light
->
set_confidence
(
1.0
);
light
->
set_tracking_time
(
1.0
);
light
->
set_id
(
iter
->
id
().
id
());
}
AdapterManager
::
FillTrafficLightDetectionHeader
(
"manual_traffic_light"
,
detection
);
return
true
;
}
TrafficLight
::
Color
GetKeyBoardColorInput
()
{
int8_t
revent
=
0
;
// short
struct
pollfd
fd
=
{
STDIN_FILENO
,
POLLIN
,
revent
};
switch
(
poll
(
&
fd
,
1
,
100
))
{
case
-
1
:
AERROR
<<
"Failed to read keybapoard"
;
break
;
case
0
:
break
;
default:
char
ch
=
'x'
;
std
::
cin
>>
ch
;
if
(
ch
==
'c'
)
{
g_is_green
=
!
g_is_green
;
g_updated
=
true
;
}
break
;
}
if
(
g_is_green
)
{
return
TrafficLight
::
GREEN
;
}
else
{
return
TrafficLight
::
RED
;
}
}
bool
IsDifferent
(
const
std
::
unordered_set
<
std
::
string
>
&
str_set
,
const
std
::
vector
<
std
::
string
>
&
str_vec
)
{
if
(
str_vec
.
size
()
!=
str_set
.
size
())
{
class
ManualTrafficLight
final
:
public
::
apollo
::
cybertron
::
TimerComponent
{
public:
bool
Init
()
{
localization_reader_
=
node_
->
CreateReader
<
LocalizationEstimate
>
(
"/apollo/localization/pose"
,
[
this
](
const
std
::
shared_ptr
<
LocalizationEstimate
>
&
localization
)
{
ADEBUG
<<
"Received chassis data: run chassis callback."
;
OnLocalization
(
localization
);
});
traffic_light_detection_writer_
=
node_
->
CreateWriter
<
TrafficLightDetection
>
(
"/apollo/perception/traffic_light"
);
return
true
;
}
for
(
const
auto
&
ss
:
str_vec
)
{
if
(
str_set
.
count
(
ss
)
==
0
)
{
return
true
;
}
}
return
false
;
}
int
main
(
int
argc
,
char
*
argv
[])
{
google
::
InitGoogleLogging
(
argv
[
0
]);
google
::
ParseCommandLineFlags
(
&
argc
,
&
argv
,
true
);
ros
::
init
(
argc
,
argv
,
"manual_traffic_light"
);
AdapterManagerConfig
manager_config
;
auto
*
config
=
manager_config
.
add_config
();
config
->
set_type
(
AdapterConfig
::
TRAFFIC_LIGHT_DETECTION
);
config
->
set_mode
(
AdapterConfig
::
PUBLISH_ONLY
);
config
->
set_message_history_limit
(
1
);
config
=
manager_config
.
add_config
();
config
->
set_type
(
AdapterConfig
::
LOCALIZATION
);
config
->
set_mode
(
AdapterConfig
::
RECEIVE_ONLY
);
config
->
set_message_history_limit
(
1
);
manager_config
.
set_is_ros
(
true
);
AdapterManager
::
Init
(
manager_config
);
std
::
unordered_set
<
std
::
string
>
prev_traffic_lights
;
ros
::
Rate
loop_rate
(
10
);
while
(
ros
::
ok
())
{
ros
::
spinOnce
();
loop_rate
.
sleep
();
bool
Proc
()
{
std
::
vector
<
SignalInfoConstPtr
>
signals
;
bool
result
=
false
;
if
(
FLAGS_all_lights
)
{
...
...
@@ -201,7 +81,6 @@ int main(int argc, char *argv[]) {
}
if
(
!
result
)
{
ADEBUG
<<
"Failed to get traffic signals from current location on map"
;
continue
;
}
else
{
ADEBUG
<<
"Received "
<<
signals
.
size
()
<<
" traffic lights"
;
}
...
...
@@ -209,19 +88,19 @@ int main(int argc, char *argv[]) {
for
(
const
auto
&
ptr
:
signals
)
{
signal_ids
.
emplace_back
(
ptr
->
id
().
id
());
}
if
(
IsDifferent
(
prev_traffic_lights
,
signal_ids
))
{
prev_traffic_lights
=
if
(
IsDifferent
(
prev_traffic_lights
_
,
signal_ids
))
{
prev_traffic_lights
_
=
std
::
unordered_set
<
std
::
string
>
(
signal_ids
.
begin
(),
signal_ids
.
end
());
g_updated
=
true
;
updated_
=
true
;
}
TrafficLightDetection
traffic_light_detection
;
TrafficLight
::
Color
color
=
GetKeyBoardColorInput
();
ADEBUG
<<
"Color: "
<<
TrafficLight
::
Color_Name
(
color
);
if
(
g_updated
)
{
g_updated
=
false
;
const
char
*
print_color
=
g_is_green
?
ANSI_GREEN
:
ANSI_RED
;
if
(
updated_
)
{
updated_
=
false
;
const
char
*
print_color
=
is_green_
?
ANSI_GREEN
:
ANSI_RED
;
std
::
cout
<<
print_color
<<
"Current Light: "
<<
(
g_is_green
?
"GREEN"
:
"RED"
);
<<
"Current Light: "
<<
(
is_green_
?
"GREEN"
:
"RED"
);
if
(
signal_ids
.
empty
())
{
if
(
FLAGS_all_lights
)
{
std
::
cout
<<
" No lights in the map"
;
...
...
@@ -244,7 +123,132 @@ int main(int argc, char *argv[]) {
<<
std
::
endl
;
}
CreateTrafficLightDetection
(
signals
,
color
,
&
traffic_light_detection
);
AdapterManager
::
PublishTrafficLightDetection
(
traffic_light_detection
);
traffic_light_detection_writer_
->
Write
(
traffic_light_detection
);
return
true
;
}
private:
bool
GetAllTrafficLights
(
std
::
vector
<
SignalInfoConstPtr
>
*
traffic_lights
)
{
static
auto
map_filename
=
apollo
::
hdmap
::
BaseMapFile
();
static
apollo
::
hdmap
::
Map
map_proto
;
static
std
::
vector
<
SignalInfoConstPtr
>
map_traffic_lights
;
if
(
map_proto
.
lane_size
()
==
0
&&
map_traffic_lights
.
empty
())
{
AERROR
<<
"signal size: "
<<
map_proto
.
signal_size
();
if
(
apollo
::
common
::
util
::
EndWith
(
map_filename
,
".xml"
))
{
if
(
!
apollo
::
hdmap
::
adapter
::
OpendriveAdapter
::
LoadData
(
map_filename
,
&
map_proto
))
{
return
false
;
}
}
else
if
(
!
apollo
::
common
::
util
::
GetProtoFromFile
(
map_filename
,
&
map_proto
))
{
return
false
;
}
for
(
const
auto
&
signal
:
map_proto
.
signal
())
{
const
auto
*
hdmap
=
HDMapUtil
::
BaseMapPtr
();
if
(
!
hdmap
)
{
AERROR
<<
"Invalid HD Map."
;
return
false
;
}
map_traffic_lights
.
push_back
(
hdmap
->
GetSignalById
(
signal
.
id
()));
}
}
*
traffic_lights
=
map_traffic_lights
;
return
true
;
}
bool
GetTrafficLightsWithinDistance
(
std
::
vector
<
SignalInfoConstPtr
>
*
traffic_lights
)
{
CHECK_NOTNULL
(
traffic_lights
);
if
(
!
has_localization_
)
{
AERROR
<<
"No localization received"
;
return
false
;
}
const
auto
*
hdmap
=
HDMapUtil
::
BaseMapPtr
();
if
(
!
hdmap
)
{
AERROR
<<
"Invalid HD Map."
;
return
false
;
}
auto
position
=
localization_
.
pose
().
position
();
int
ret
=
hdmap
->
GetForwardNearestSignalsOnLane
(
position
,
FLAGS_traffic_light_distance
,
traffic_lights
);
if
(
ret
!=
0
)
{
AERROR
<<
"failed to get signals from position: "
<<
position
.
ShortDebugString
()
<<
" with distance: "
<<
FLAGS_traffic_light_distance
<<
" on map"
;
return
false
;
}
return
true
;
}
bool
CreateTrafficLightDetection
(
const
std
::
vector
<
SignalInfoConstPtr
>
&
signals
,
TrafficLight
::
Color
color
,
TrafficLightDetection
*
detection
)
{
CHECK_NOTNULL
(
detection
);
for
(
const
auto
&
iter
:
signals
)
{
auto
*
light
=
detection
->
add_traffic_light
();
light
->
set_color
(
color
);
light
->
set_confidence
(
1.0
);
light
->
set_tracking_time
(
1.0
);
light
->
set_id
(
iter
->
id
().
id
());
}
::
apollo
::
common
::
util
::
FillHeader
(
"manual_traffic_light"
,
detection
);
return
true
;
}
return
0
;
}
TrafficLight
::
Color
GetKeyBoardColorInput
()
{
int8_t
revent
=
0
;
// short
struct
pollfd
fd
=
{
STDIN_FILENO
,
POLLIN
,
revent
};
switch
(
poll
(
&
fd
,
1
,
100
))
{
case
-
1
:
AERROR
<<
"Failed to read keybapoard"
;
break
;
case
0
:
break
;
default:
char
ch
=
'x'
;
std
::
cin
>>
ch
;
if
(
ch
==
'c'
)
{
is_green_
=
!
is_green_
;
updated_
=
true
;
}
break
;
}
if
(
is_green_
)
{
return
TrafficLight
::
GREEN
;
}
else
{
return
TrafficLight
::
RED
;
}
}
bool
IsDifferent
(
const
std
::
unordered_set
<
std
::
string
>
&
str_set
,
const
std
::
vector
<
std
::
string
>
&
str_vec
)
{
if
(
str_vec
.
size
()
!=
str_set
.
size
())
{
return
true
;
}
for
(
const
auto
&
ss
:
str_vec
)
{
if
(
str_set
.
count
(
ss
)
==
0
)
{
return
true
;
}
}
return
false
;
}
void
OnLocalization
(
const
std
::
shared_ptr
<
LocalizationEstimate
>
&
localization
)
{
localization_
=
*
localization
;
has_localization_
=
true
;
}
private:
bool
is_green_
=
false
;
bool
updated_
=
true
;
bool
has_localization_
=
false
;
LocalizationEstimate
localization_
;
std
::
unordered_set
<
std
::
string
>
prev_traffic_lights_
;
std
::
shared_ptr
<::
apollo
::
cybertron
::
Writer
<
TrafficLightDetection
>>
traffic_light_detection_writer_
=
nullptr
;
std
::
shared_ptr
<::
apollo
::
cybertron
::
Reader
<
LocalizationEstimate
>>
localization_reader_
=
nullptr
;
};
CYBERTRON_REGISTER_COMPONENT
(
ManualTrafficLight
);
modules/tools/manual_traffic_light/manual_traffic_light.dag
0 → 100644
浏览文件 @
662e6496
# Define all coms in DAG streaming.
module_config {
module_library : "/apollo/bazel-bin/modules/tools/manual_traffic_light/libmanual_traffic_light_component.so"
timer_components {
class_name : "ManualTrafficLight"
config {
name: "manual_traffic_light_componenet"
interval: 200 # milliseconds
}
}
}
modules/tools/manual_traffic_light/manual_traffic_light.launch
0 → 100644
浏览文件 @
662e6496
<cybertron>
<module>
<name>manual_traffic_light</name>
<dag_conf>/apollo/modules/tools/manual_traffic_light/manual_traffic_light.dag</dag_conf>
<process_name>manual_traffic_light</process_name>
</module>
</cybertron>
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录