Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
de282e16
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,发现更多精彩内容 >>
提交
de282e16
编写于
2月 14, 2018
作者:
Y
YajiaZhang
提交者:
Liangliang Zhang
2月 14, 2018
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
planning: logic change and code restructure for signal light handling in lattice planner
上级
3e59dfca
变更
2
隐藏空白更改
内联
并排
Showing
2 changed file
with
66 addition
and
80 deletion
+66
-80
modules/planning/lattice/behavior_decider/signal_light_scenario.cc
...lanning/lattice/behavior_decider/signal_light_scenario.cc
+56
-67
modules/planning/lattice/behavior_decider/signal_light_scenario.h
...planning/lattice/behavior_decider/signal_light_scenario.h
+10
-13
未找到文件。
modules/planning/lattice/behavior_decider/signal_light_scenario.cc
浏览文件 @
de282e16
...
...
@@ -20,6 +20,7 @@
#include "modules/planning/lattice/behavior_decider/signal_light_scenario.h"
#include <algorithm>
#include <limits>
#include <string>
#include <vector>
...
...
@@ -40,119 +41,107 @@ using apollo::common::util::WithinBound;
using
apollo
::
perception
::
TrafficLight
;
using
apollo
::
perception
::
TrafficLightDetection
;
void
SignalLightScenario
::
Reset
()
{}
bool
SignalLightScenario
::
Init
()
{
exist_
=
true
;
return
exist_
;
}
int
SignalLightScenario
::
ComputeScenarioDecision
(
Frame
*
frame
,
ReferenceLineInfo
*
const
reference_line_info
,
PlanningTarget
*
planning_target
)
{
CHECK
(
frame
!=
nullptr
);
if
(
!
FindValidSignalLight
(
reference_line_info
))
{
auto
signals_from_map
=
FindValidSignalLightFromMap
(
reference_line_info
);
if
(
signals_from_map
.
empty
())
{
ADEBUG
<<
"No valid signal light along reference line"
;
return
0
;
}
ReadSignals
();
auto
detected_signals
=
GetPerceptionDetectedSignals
();
double
stop_s
=
std
::
numeric_limits
<
double
>::
max
();
for
(
const
auto
signal_from_map
:
signals_from_map
)
{
auto
it_signal
=
detected_signals
.
find
(
signal_from_map
->
object_id
);
if
(
it_signal
==
detected_signals
.
end
())
{
AWARN
<<
"Cannot detect signal which is marked on the map; signal id: "
<<
signal_from_map
->
object_id
;
continue
;
}
for
(
const
hdmap
::
PathOverlap
*
signal_light
:
signal_lights_along_reference_line_
)
{
const
TrafficLight
signal
=
GetSignal
(
signal_light
->
object_id
);
double
stop_deceleration
=
GetStopDeceleration
(
reference_line_info
,
signal_light
);
EstimateStopDeceleration
(
reference_line_info
,
signal_from_map
);
if
((
signal
.
color
()
==
TrafficLight
::
RED
&&
if
((
it_signal
->
second
->
color
()
==
TrafficLight
::
RED
&&
stop_deceleration
<
FLAGS_max_stop_deceleration
)
||
// (signal.color() == TrafficLight::UNKNOWN &&
// stop_deceleration < FLAGS_max_stop_deceleration) ||
(
signal
.
color
()
==
TrafficLight
::
YELLOW
&&
(
it_signal
->
second
->
color
()
==
TrafficLight
::
YELLOW
&&
stop_deceleration
<
FLAGS_max_stop_deacceleration_for_yellow_light
))
{
CreateStopObstacle
(
frame
,
reference_line_info
,
signal_light
);
CreateStopObstacle
(
frame
,
reference_line_info
,
signal_from_map
);
stop_s
=
std
::
min
(
stop_s
,
signal_from_map
->
start_s
);
}
}
if
(
stop_s
<
std
::
numeric_limits
<
double
>::
max
())
{
planning_target
->
set_stop_point
(
stop_s
);
}
return
0
;
}
bool
SignalLightScenario
::
FindValidSignalLight
(
std
::
vector
<
const
hdmap
::
PathOverlap
*>
SignalLightScenario
::
FindValidSignalLightFromMap
(
ReferenceLineInfo
*
const
reference_line_info
)
{
const
std
::
vector
<
hdmap
::
PathOverlap
>&
signal_lights
=
reference_line_info
->
reference_line
().
map_path
().
signal_overlaps
();
if
(
signal_lights
.
size
()
<
=
0
)
{
if
(
signal_lights
.
size
()
=
=
0
)
{
ADEBUG
<<
"No signal lights from reference line."
;
return
false
;
}
else
{
ADEBUG
<<
"Found signal_lights size="
<<
signal_lights
.
size
();
return
std
::
vector
<
const
hdmap
::
PathOverlap
*>
();
}
signal_lights_along_reference_line_
.
clear
();
for
(
const
hdmap
::
PathOverlap
&
signal_light
:
signal_lights
)
{
ADEBUG
<<
"Found signal_lights size="
<<
signal_lights
.
size
();
std
::
vector
<
const
hdmap
::
PathOverlap
*>
signal_lights_along_reference_line
;
for
(
const
auto
&
signal_light
:
signal_lights
)
{
if
(
signal_light
.
start_s
+
FLAGS_max_stop_distance_buffer
>
reference_line_info
->
AdcSlBoundary
().
end_s
())
{
signal_lights_along_reference_line
_
.
push_back
(
&
signal_light
);
signal_lights_along_reference_line
.
push_back
(
&
signal_light
);
}
}
return
signal_lights_along_reference_line
_
.
size
()
>
0
;
return
signal_lights_along_reference_line
;
}
void
SignalLightScenario
::
ReadSignals
()
{
detected_signals_
.
clear
();
detected_signals_
.
clear
();
if
(
AdapterManager
::
GetTrafficLightDetection
()
->
Empty
())
{
return
;
}
if
(
AdapterManager
::
GetTrafficLightDetection
()
->
GetDelaySec
()
>
FLAGS_signal_expire_time_sec
)
{
AWARN
<<
"traffic signals msg is expired: "
<<
AdapterManager
::
GetTrafficLightDetection
()
->
GetDelaySec
();
return
;
std
::
unordered_map
<
std
::
string
,
const
TrafficLight
*>
SignalLightScenario
::
GetPerceptionDetectedSignals
()
{
if
(
AdapterManager
::
GetTrafficLightDetection
()
->
Empty
()
||
(
AdapterManager
::
GetTrafficLightDetection
()
->
GetDelaySec
()
>
FLAGS_signal_expire_time_sec
))
{
ADEBUG
<<
"traffic light signals msg is either empty or outdated."
;
return
std
::
unordered_map
<
std
::
string
,
const
TrafficLight
*>
();
}
const
TrafficLightDetection
&
detection
=
std
::
unordered_map
<
std
::
string
,
const
TrafficLight
*>
detected_signals
;
const
auto
&
signal_msg
=
AdapterManager
::
GetTrafficLightDetection
()
->
GetLatestObserved
();
for
(
int
j
=
0
;
j
<
detection
.
traffic_light_size
();
j
++
)
{
const
TrafficLight
&
signal
=
detection
.
traffic_light
(
j
);
detected_signals_
[
signal
.
id
()]
=
&
signal
;
}
}
TrafficLight
SignalLightScenario
::
GetSignal
(
const
std
::
string
&
signal_id
)
{
const
auto
*
result
=
apollo
::
common
::
util
::
FindPtrOrNull
(
detected_signals_
,
signal_id
);
if
(
result
==
nullptr
)
{
TrafficLight
traffic_light
;
traffic_light
.
set_id
(
signal_id
);
traffic_light
.
set_color
(
TrafficLight
::
UNKNOWN
);
traffic_light
.
set_confidence
(
0.0
);
traffic_light
.
set_tracking_time
(
0.0
);
return
traffic_light
;
for
(
int
j
=
0
;
j
<
signal_msg
.
traffic_light_size
();
++
j
)
{
const
auto
&
signal
=
signal_msg
.
traffic_light
(
j
);
detected_signals
[
signal
.
id
()]
=
&
signal
;
}
return
*
result
;
return
detected_signals
;
}
double
SignalLightScenario
::
Get
StopDeceleration
(
double
SignalLightScenario
::
Estimate
StopDeceleration
(
ReferenceLineInfo
*
const
reference_line_info
,
const
hdmap
::
PathOverlap
*
signal_light
)
{
double
adc
_speed
=
double
ego
_speed
=
common
::
VehicleStateProvider
::
instance
()
->
linear_velocity
();
if
(
adc_speed
<
FLAGS_max_stop_speed
)
{
return
0.0
;
}
double
stop_distance
=
0.0
;
double
adc_front_s
=
reference_line_info
->
AdcSlBoundary
().
end_s
();
double
stop_line_s
=
signal_light
->
start_s
;
if
(
stop_line_s
>
adc_front_s
)
{
stop_distance
=
stop_line_s
-
adc_front_s
;
double
ego_front_s
=
reference_line_info
->
AdcSlBoundary
().
end_s
();
double
stop_s
=
signal_light
->
start_s
;
if
(
stop_s
>=
ego_front_s
)
{
double
stop_distance
=
stop_s
-
ego_front_s
;
return
(
ego_speed
*
ego_speed
)
/
stop_distance
*
0.5
;
}
else
{
stop_distance
=
stop_line_s
+
FLAGS_max_stop_distance_buffer
-
adc_front_s
;
}
if
(
stop_distance
<
1e-5
)
{
// longitudinal_acceleration_lower_bound is a negative value.
return
-
FLAGS_longitudinal_acceleration_lower_bound
;
}
return
(
adc_speed
*
adc_speed
)
/
(
2
*
stop_distance
);
}
void
SignalLightScenario
::
CreateStopObstacle
(
...
...
modules/planning/lattice/behavior_decider/signal_light_scenario.h
浏览文件 @
de282e16
...
...
@@ -33,33 +33,30 @@ namespace planning {
class
SignalLightScenario
:
public
Scenario
{
public:
void
Reset
()
override
;
void
Reset
()
override
{}
bool
Init
()
override
;
bool
Init
()
override
{
return
true
;
}
bool
ScenarioExist
()
const
override
{
return
exist_
;
}
bool
ScenarioExist
()
const
override
{
return
true
;
}
virtual
int
ComputeScenarioDecision
(
Frame
*
frame
,
ReferenceLineInfo
*
const
reference_line_info
,
PlanningTarget
*
planning_target
);
private:
bool
exist_
=
false
;
std
::
unordered_map
<
std
::
string
,
const
apollo
::
perception
::
TrafficLight
*>
GetPerceptionDetectedSignals
();
std
::
vector
<
const
hdmap
::
PathOverlap
*>
FindValidSignalLightFromMap
(
ReferenceLineInfo
*
const
reference_line_info
);
void
ReadSignals
();
bool
FindValidSignalLight
(
ReferenceLineInfo
*
const
reference_line_info
);
double
EstimateStopDeceleration
(
ReferenceLineInfo
*
const
reference_line_info
,
const
hdmap
::
PathOverlap
*
signal_light
);
apollo
::
perception
::
TrafficLight
GetSignal
(
const
std
::
string
&
signal_id
);
double
GetStopDeceleration
(
ReferenceLineInfo
*
const
reference_line_info
,
const
hdmap
::
PathOverlap
*
signal_light
);
void
CreateStopObstacle
(
Frame
*
frame
,
ReferenceLineInfo
*
const
reference_line_info
,
const
hdmap
::
PathOverlap
*
signal_light
);
std
::
vector
<
const
hdmap
::
PathOverlap
*>
signal_lights_along_reference_line_
;
std
::
unordered_map
<
std
::
string
,
const
apollo
::
perception
::
TrafficLight
*>
detected_signals_
;
DECLARE_SCENARIO
(
SignalLightScenario
);
};
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录