Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
4db2eba3
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,发现更多精彩内容 >>
提交
4db2eba3
编写于
11月 26, 2018
作者:
L
Liangliang Zhang
提交者:
Jiangtao Hu
12月 13, 2018
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Planning: enabled collision detection when adc is stopped.
上级
6231c363
变更
8
显示空白变更内容
内联
并排
Showing
8 changed file
with
55 addition
and
70 deletion
+55
-70
modules/planning/common/ego_info.cc
modules/planning/common/ego_info.cc
+23
-13
modules/planning/common/ego_info.h
modules/planning/common/ego_info.h
+10
-4
modules/planning/common/frame.cc
modules/planning/common/frame.cc
+10
-43
modules/planning/common/frame.h
modules/planning/common/frame.h
+1
-2
modules/planning/common/speed_profile_generator_test.cc
modules/planning/common/speed_profile_generator_test.cc
+1
-2
modules/planning/conf/planning.conf
modules/planning/conf/planning.conf
+1
-0
modules/planning/navi_planning.cc
modules/planning/navi_planning.cc
+2
-4
modules/planning/std_planning.cc
modules/planning/std_planning.cc
+7
-2
未找到文件。
modules/planning/common/ego_info.cc
浏览文件 @
4db2eba3
...
@@ -30,19 +30,33 @@ using common::math::Vec2d;
...
@@ -30,19 +30,33 @@ using common::math::Vec2d;
using
common
::
math
::
Box2d
;
using
common
::
math
::
Box2d
;
EgoInfo
::
EgoInfo
()
{
EgoInfo
::
EgoInfo
()
{
common
::
VehicleConfig
ego_vehicle_config_
=
ego_vehicle_config_
=
common
::
VehicleConfigHelper
::
GetConfig
();
common
::
VehicleConfigHelper
::
GetConfig
();
}
}
bool
EgoInfo
::
Update
(
const
common
::
TrajectoryPoint
&
start_point
,
bool
EgoInfo
::
Update
(
const
common
::
TrajectoryPoint
&
start_point
,
const
common
::
VehicleState
&
vehicle_state
,
const
common
::
VehicleState
&
vehicle_state
)
{
const
std
::
vector
<
const
Obstacle
*>&
obstacles
)
{
set_start_point
(
start_point
);
set_start_point
(
start_point
);
set_vehicle_state
(
vehicle_state
);
set_vehicle_state
(
vehicle_state
);
Calculate
FrontObstacleClearDistance
(
obstacles
);
Calculate
EgoBox
(
vehicle_state
);
return
true
;
return
true
;
}
}
void
EgoInfo
::
CalculateEgoBox
(
const
common
::
VehicleState
&
vehicle_state
)
{
const
auto
&
param
=
ego_vehicle_config_
.
vehicle_param
();
AINFO
<<
"param: "
<<
param
.
DebugString
();
Vec2d
vec_to_center
(
(
param
.
front_edge_to_center
()
-
param
.
back_edge_to_center
())
/
2.0
,
(
param
.
left_edge_to_center
()
-
param
.
right_edge_to_center
())
/
2.0
);
Vec2d
position
(
vehicle_state
.
x
(),
vehicle_state
.
y
());
Vec2d
center
(
position
+
vec_to_center
.
rotate
(
vehicle_state
.
heading
()));
const
double
buffer
=
0.1
;
// in meters
ego_box_
=
Box2d
(
center
,
vehicle_state
.
heading
(),
param
.
length
()
+
buffer
,
param
.
width
()
+
buffer
);
}
void
EgoInfo
::
Clear
()
{
void
EgoInfo
::
Clear
()
{
start_point_
.
Clear
();
start_point_
.
Clear
();
vehicle_state_
.
Clear
();
vehicle_state_
.
Clear
();
...
@@ -60,15 +74,11 @@ void EgoInfo::CalculateFrontObstacleClearDistance(
...
@@ -60,15 +74,11 @@ void EgoInfo::CalculateFrontObstacleClearDistance(
Vec2d
center
(
position
+
vec_to_center
.
rotate
(
vehicle_state_
.
heading
()));
Vec2d
center
(
position
+
vec_to_center
.
rotate
(
vehicle_state_
.
heading
()));
const
double
buffer
=
0.1
;
// in meters
Box2d
ego_box
(
center
,
vehicle_state_
.
heading
(),
param
.
length
()
+
buffer
,
param
.
width
()
+
buffer
);
const
double
adc_half_diagnal
=
ego_box
.
diagonal
()
/
2.0
;
Vec2d
unit_vec_heading
=
Vec2d
::
CreateUnitVec2d
(
vehicle_state_
.
heading
());
Vec2d
unit_vec_heading
=
Vec2d
::
CreateUnitVec2d
(
vehicle_state_
.
heading
());
// Due to the error of ego heading, only short range distance is meaningful
// Due to the error of ego heading, only short range distance is meaningful
const
double
kDistanceThreshold
=
50.0
;
constexpr
double
kDistanceThreshold
=
50.0
;
constexpr
double
buffer
=
0.1
;
// in meters
const
double
impact_region_length
=
const
double
impact_region_length
=
param
.
length
()
+
buffer
+
kDistanceThreshold
;
param
.
length
()
+
buffer
+
kDistanceThreshold
;
Box2d
ego_front_region
(
center
+
unit_vec_heading
*
kDistanceThreshold
/
2.0
,
Box2d
ego_front_region
(
center
+
unit_vec_heading
*
kDistanceThreshold
/
2.0
,
...
@@ -81,9 +91,9 @@ void EgoInfo::CalculateFrontObstacleClearDistance(
...
@@ -81,9 +91,9 @@ void EgoInfo::CalculateFrontObstacleClearDistance(
continue
;
continue
;
}
}
double
dist
=
ego_box
.
center
().
DistanceTo
(
double
dist
=
ego_box
_
.
center
().
DistanceTo
(
obstacle
->
PerceptionBoundingBox
().
center
())
-
obstacle
->
PerceptionBoundingBox
().
center
())
-
adc_half_diagnal
;
ego_box_
.
diagonal
()
/
2.0
;
if
(
front_clear_distance_
<
0.0
||
dist
<
front_clear_distance_
)
{
if
(
front_clear_distance_
<
0.0
||
dist
<
front_clear_distance_
)
{
front_clear_distance_
=
dist
;
front_clear_distance_
=
dist
;
...
...
modules/planning/common/ego_info.h
浏览文件 @
4db2eba3
...
@@ -44,8 +44,8 @@ class EgoInfo {
...
@@ -44,8 +44,8 @@ class EgoInfo {
~
EgoInfo
()
=
default
;
~
EgoInfo
()
=
default
;
bool
Update
(
const
common
::
TrajectoryPoint
&
start_point
,
bool
Update
(
const
common
::
TrajectoryPoint
&
start_point
,
const
common
::
VehicleState
&
vehicle_state
,
const
common
::
VehicleState
&
vehicle_state
);
const
std
::
vector
<
const
Obstacle
*>&
obstacles
);
void
Clear
();
void
Clear
();
common
::
TrajectoryPoint
start_point
()
const
{
return
start_point_
;
}
common
::
TrajectoryPoint
start_point
()
const
{
return
start_point_
;
}
...
@@ -54,6 +54,11 @@ class EgoInfo {
...
@@ -54,6 +54,11 @@ class EgoInfo {
double
front_clear_distance
()
const
{
return
front_clear_distance_
;
}
double
front_clear_distance
()
const
{
return
front_clear_distance_
;
}
common
::
math
::
Box2d
ego_box
()
const
{
return
ego_box_
;
}
void
CalculateFrontObstacleClearDistance
(
const
std
::
vector
<
const
Obstacle
*>&
obstacles
);
private:
private:
FRIEND_TEST
(
EgoInfoTest
,
EgoInfoSimpleTest
);
FRIEND_TEST
(
EgoInfoTest
,
EgoInfoSimpleTest
);
...
@@ -65,8 +70,7 @@ class EgoInfo {
...
@@ -65,8 +70,7 @@ class EgoInfo {
start_point_
=
start_point
;
start_point_
=
start_point
;
}
}
void
CalculateFrontObstacleClearDistance
(
void
CalculateEgoBox
(
const
common
::
VehicleState
&
vehicle_state
);
const
std
::
vector
<
const
Obstacle
*>&
obstacles
);
// stitched point (at stitching mode)
// stitched point (at stitching mode)
// or real vehicle point (at non-stitching mode)
// or real vehicle point (at non-stitching mode)
...
@@ -79,6 +83,8 @@ class EgoInfo {
...
@@ -79,6 +83,8 @@ class EgoInfo {
common
::
VehicleConfig
ego_vehicle_config_
;
common
::
VehicleConfig
ego_vehicle_config_
;
common
::
math
::
Box2d
ego_box_
;
DECLARE_SINGLETON
(
EgoInfo
);
DECLARE_SINGLETON
(
EgoInfo
);
};
};
...
...
modules/planning/common/frame.cc
浏览文件 @
4db2eba3
...
@@ -36,6 +36,7 @@
...
@@ -36,6 +36,7 @@
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/map/pnc_map/path.h"
#include "modules/map/pnc_map/path.h"
#include "modules/map/pnc_map/pnc_map.h"
#include "modules/map/pnc_map/pnc_map.h"
#include "modules/planning/common/ego_info.h"
#include "modules/planning/common/planning_context.h"
#include "modules/planning/common/planning_context.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/reference_line/reference_line_provider.h"
#include "modules/planning/reference_line/reference_line_provider.h"
...
@@ -48,6 +49,7 @@ using apollo::common::Status;
...
@@ -48,6 +49,7 @@ using apollo::common::Status;
using
apollo
::
common
::
VehicleStateProvider
;
using
apollo
::
common
::
VehicleStateProvider
;
using
apollo
::
common
::
math
::
Box2d
;
using
apollo
::
common
::
math
::
Box2d
;
using
apollo
::
common
::
math
::
Vec2d
;
using
apollo
::
common
::
math
::
Vec2d
;
using
apollo
::
common
::
math
::
Polygon2d
;
using
apollo
::
hdmap
::
HDMapUtil
;
using
apollo
::
hdmap
::
HDMapUtil
;
using
apollo
::
hdmap
::
LaneSegment
;
using
apollo
::
hdmap
::
LaneSegment
;
using
apollo
::
hdmap
::
ParkingSpaceInfoConstPtr
;
using
apollo
::
hdmap
::
ParkingSpaceInfoConstPtr
;
...
@@ -393,10 +395,9 @@ Status Frame::InitFrameData() {
...
@@ -393,10 +395,9 @@ Status Frame::InitFrameData() {
Obstacle
::
CreateObstacles
(
*
local_view_
.
prediction_obstacles
))
{
Obstacle
::
CreateObstacles
(
*
local_view_
.
prediction_obstacles
))
{
AddObstacle
(
*
ptr
);
AddObstacle
(
*
ptr
);
}
}
if
(
FLAGS_enable_collision_detection
)
{
if
(
FLAGS_enable_collision_detection
&&
planning_start_point_
.
v
()
<
1e-3
)
{
const
auto
*
collision_obstacle
=
const
auto
*
collision_obstacle
=
FindCollisionObstacle
();
FindCollisionObstacleWithInExtraRadius
(
FLAGS_max_collision_distance
);
if
(
collision_obstacle
!=
nullptr
)
{
if
(
collision_obstacle
)
{
std
::
string
err_str
=
std
::
string
err_str
=
"Found collision with obstacle: "
+
collision_obstacle
->
Id
();
"Found collision with obstacle: "
+
collision_obstacle
->
Id
();
monitor_logger_buffer_
.
ERROR
(
err_str
);
monitor_logger_buffer_
.
ERROR
(
err_str
);
...
@@ -406,53 +407,19 @@ Status Frame::InitFrameData() {
...
@@ -406,53 +407,19 @@ Status Frame::InitFrameData() {
return
Status
::
OK
();
return
Status
::
OK
();
}
}
const
Obstacle
*
Frame
::
FindCollisionObstacleWithInExtraRadius
(
const
Obstacle
*
Frame
::
FindCollisionObstacle
()
const
{
const
double
radius
)
const
{
CHECK_GT
(
radius
,
kMathEpsilon
)
<<
"Extra safety radius must be positive number!"
;
if
(
obstacles_
.
Items
().
empty
())
{
if
(
obstacles_
.
Items
().
empty
())
{
return
nullptr
;
return
nullptr
;
}
}
const
auto
&
param
=
common
::
VehicleConfigHelper
::
Instance
()
->
GetConfig
().
vehicle_param
();
const
auto
&
adc_polygon
=
Polygon2d
(
EgoInfo
::
Instance
()
->
ego_box
());
Vec2d
position
(
vehicle_state_
.
x
(),
vehicle_state_
.
y
());
Vec2d
vec_to_center
(
(
param
.
front_edge_to_center
()
-
param
.
back_edge_to_center
())
/
2.0
,
(
param
.
left_edge_to_center
()
-
param
.
right_edge_to_center
())
/
2.0
);
Vec2d
center
(
position
+
vec_to_center
.
rotate
(
vehicle_state_
.
heading
()));
Box2d
adc_box
(
center
,
vehicle_state_
.
heading
(),
param
.
length
(),
param
.
width
());
const
double
adc_half_diagnal
=
adc_box
.
diagonal
()
/
2.0
;
for
(
const
auto
&
obstacle
:
obstacles_
.
Items
())
{
for
(
const
auto
&
obstacle
:
obstacles_
.
Items
())
{
if
(
obstacle
->
IsVirtual
())
{
if
(
obstacle
->
IsVirtual
())
{
continue
;
continue
;
}
}
double
center_dist
=
const
auto
&
obstacle_polygon
=
obstacle
->
PerceptionPolygon
();
adc_box
.
center
().
DistanceTo
(
obstacle
->
PerceptionBoundingBox
().
center
());
if
(
obstacle_polygon
.
HasOverlap
(
adc_polygon
))
{
if
(
center_dist
>
obstacle
->
PerceptionBoundingBox
().
diagonal
()
/
2.0
+
adc_half_diagnal
+
radius
)
{
ADEBUG
<<
"Obstacle : "
<<
obstacle
->
Id
()
<<
" is too far to collide"
;
continue
;
}
double
distance
=
obstacle
->
PerceptionPolygon
().
DistanceTo
(
adc_box
);
if
(
FLAGS_ignore_overlapped_obstacle
&&
distance
<
kMathEpsilon
)
{
bool
all_points_in
=
true
;
for
(
const
auto
&
point
:
obstacle
->
PerceptionPolygon
().
points
())
{
if
(
!
adc_box
.
IsPointIn
(
point
))
{
all_points_in
=
false
;
break
;
}
}
if
(
all_points_in
)
{
ADEBUG
<<
"Skip overlapped obstacle, which is often caused by lidar "
"calibration error"
;
continue
;
}
}
if
(
distance
<
radius
)
{
AERROR
<<
"Found collision with obstacle "
<<
obstacle
->
Id
()
<<
"Within extra safety radius of : "
<<
radius
;
return
obstacle
;
return
obstacle
;
}
}
}
}
...
...
modules/planning/common/frame.h
浏览文件 @
4db2eba3
...
@@ -166,8 +166,7 @@ class Frame {
...
@@ -166,8 +166,7 @@ class Frame {
* @return pointer to the obstacle if such obstacle exists, otherwise
* @return pointer to the obstacle if such obstacle exists, otherwise
* @return false if no colliding obstacle.
* @return false if no colliding obstacle.
*/
*/
const
Obstacle
*
FindCollisionObstacleWithInExtraRadius
(
const
Obstacle
*
FindCollisionObstacle
()
const
;
const
double
radius
)
const
;
/**
/**
* @brief create a static virtual obstacle
* @brief create a static virtual obstacle
...
...
modules/planning/common/speed_profile_generator_test.cc
浏览文件 @
4db2eba3
...
@@ -36,9 +36,8 @@ TEST(SpeedProfileGeneratorTest, GenerateFallbackSpeedProfile) {
...
@@ -36,9 +36,8 @@ TEST(SpeedProfileGeneratorTest, GenerateFallbackSpeedProfile) {
adc_planning_point
.
set_v
(
FLAGS_polynomial_speed_fallback_velocity
+
0.1
);
adc_planning_point
.
set_v
(
FLAGS_polynomial_speed_fallback_velocity
+
0.1
);
common
::
VehicleState
vs
;
common
::
VehicleState
vs
;
const
std
::
vector
<
const
Obstacle
*>
obstacles
;
EgoInfo
::
Instance
()
->
Update
(
adc_planning_point
,
vs
,
obstacles
);
EgoInfo
::
Instance
()
->
Update
(
adc_planning_point
,
vs
);
auto
speed_data2
=
SpeedProfileGenerator
::
GenerateFallbackSpeedProfile
();
auto
speed_data2
=
SpeedProfileGenerator
::
GenerateFallbackSpeedProfile
();
EXPECT_FALSE
(
speed_data2
.
Empty
());
EXPECT_FALSE
(
speed_data2
.
Empty
());
}
}
...
...
modules/planning/conf/planning.conf
浏览文件 @
4db2eba3
...
@@ -15,6 +15,7 @@
...
@@ -15,6 +15,7 @@
--
enable_scenario_side_pass
--
enable_scenario_side_pass
--
enable_scenario_stop_sign_unprotected
--
enable_scenario_stop_sign_unprotected
--
noenable_scenario_traffic_light_right_turn_unprotected
--
noenable_scenario_traffic_light_right_turn_unprotected
--
enable_collision_detection
# --smoother_config_filename=/apollo/modules/planning/conf/spiral_smoother_config.pb.txt
# --smoother_config_filename=/apollo/modules/planning/conf/spiral_smoother_config.pb.txt
# --smoother_config_filename=/apollo/modules/planning/conf/cosTheta_smoother_config.pb.txt
# --smoother_config_filename=/apollo/modules/planning/conf/cosTheta_smoother_config.pb.txt
...
...
modules/planning/navi_planning.cc
浏览文件 @
4db2eba3
...
@@ -109,8 +109,7 @@ Status NaviPlanning::InitFrame(const uint32_t sequence_num,
...
@@ -109,8 +109,7 @@ Status NaviPlanning::InitFrame(const uint32_t sequence_num,
ADCTrajectory
*
output_trajectory
)
{
ADCTrajectory
*
output_trajectory
)
{
frame_
.
reset
(
new
Frame
(
sequence_num
,
local_view_
,
planning_start_point
,
frame_
.
reset
(
new
Frame
(
sequence_num
,
local_view_
,
planning_start_point
,
start_time
,
vehicle_state
,
start_time
,
vehicle_state
,
reference_line_provider_
.
get
(),
reference_line_provider_
.
get
(),
output_trajectory
));
output_trajectory
));
std
::
list
<
ReferenceLine
>
reference_lines
;
std
::
list
<
ReferenceLine
>
reference_lines
;
std
::
list
<
hdmap
::
RouteSegments
>
segments
;
std
::
list
<
hdmap
::
RouteSegments
>
segments
;
...
@@ -220,8 +219,7 @@ void NaviPlanning::RunOnce(const LocalView& local_view,
...
@@ -220,8 +219,7 @@ void NaviPlanning::RunOnce(const LocalView& local_view,
return
;
return
;
}
}
EgoInfo
::
Instance
()
->
Update
(
stitching_trajectory
.
back
(),
vehicle_state
,
EgoInfo
::
Instance
()
->
Update
(
stitching_trajectory
.
back
(),
vehicle_state
);
frame_
->
obstacles
());
if
(
FLAGS_enable_record_debug
)
{
if
(
FLAGS_enable_record_debug
)
{
frame_
->
RecordInputDebug
(
trajectory_pb
->
mutable_debug
());
frame_
->
RecordInputDebug
(
trajectory_pb
->
mutable_debug
());
...
...
modules/planning/std_planning.cc
浏览文件 @
4db2eba3
...
@@ -247,10 +247,15 @@ void StdPlanning::RunOnce(const LocalView& local_view,
...
@@ -247,10 +247,15 @@ void StdPlanning::RunOnce(const LocalView& local_view,
last_publishable_trajectory_
.
get
());
last_publishable_trajectory_
.
get
());
const
uint32_t
frame_num
=
seq_num_
++
;
const
uint32_t
frame_num
=
seq_num_
++
;
bool
update_ego_info
=
EgoInfo
::
Instance
()
->
Update
(
stitching_trajectory
.
back
(),
vehicle_state
);
status
=
InitFrame
(
frame_num
,
stitching_trajectory
.
back
(),
start_timestamp
,
status
=
InitFrame
(
frame_num
,
stitching_trajectory
.
back
(),
start_timestamp
,
vehicle_state
,
trajectory_pb
);
vehicle_state
,
trajectory_pb
);
bool
update_ego_info
=
EgoInfo
::
Instance
()
->
Update
(
stitching_trajectory
.
back
(),
vehicle_state
,
frame_
->
obstacles
());
if
(
update_ego_info
&&
status
.
ok
())
{
EgoInfo
::
Instance
()
->
CalculateFrontObstacleClearDistance
(
frame_
->
obstacles
());
}
if
(
FLAGS_enable_record_debug
)
{
if
(
FLAGS_enable_record_debug
)
{
frame_
->
RecordInputDebug
(
trajectory_pb
->
mutable_debug
());
frame_
->
RecordInputDebug
(
trajectory_pb
->
mutable_debug
());
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录