Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
c7778a23
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,发现更多精彩内容 >>
提交
c7778a23
编写于
9月 25, 2018
作者:
J
Jiangtao Hu
提交者:
Liangliang Zhang
9月 25, 2018
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
perception: add onboard/transform_wrapper BUILD.
上级
1bf66e15
变更
4
隐藏空白更改
内联
并排
Showing
4 changed file
with
41 addition
and
17 deletion
+41
-17
modules/perception/onboard/transform_wrapper/BUILD
modules/perception/onboard/transform_wrapper/BUILD
+21
-0
modules/perception/onboard/transform_wrapper/transform_wrapper.cc
...perception/onboard/transform_wrapper/transform_wrapper.cc
+16
-16
modules/planning/planning_component.cc
modules/planning/planning_component.cc
+2
-1
modules/planning/std_planning.cc
modules/planning/std_planning.cc
+2
-0
未找到文件。
modules/perception/onboard/transform_wrapper/BUILD
0 → 100644
浏览文件 @
c7778a23
load
(
"//tools:cpplint.bzl"
,
"cpplint"
)
package
(
default_visibility
=
[
"//visibility:public"
])
cc_library
(
name
=
"transform_wrapper"
,
srcs
=
[
"transform_wrapper.cc"
,
],
hdrs
=
[
"transform_wrapper.h"
,
],
deps
=
[
"//external:gflags"
,
"//modules/perception/common/sensor_manager"
,
"//modules/transform:tf2_buffer_lib"
,
"@eigen"
,
],
)
cpplint
()
modules/perception/onboard/transform_wrapper/transform_wrapper.cc
浏览文件 @
c7778a23
...
...
@@ -42,7 +42,7 @@ void TransformCache::AddTransform(const StampedTransform& transform) {
}
double
delt
=
transform
.
timestamp
-
transforms_
.
back
().
timestamp
;
if
(
delt
<
0.0
)
{
LOG_
INFO
<<
"ERROR: add earlier transform to transform cache"
;
A
INFO
<<
"ERROR: add earlier transform to transform cache"
;
return
;
}
...
...
@@ -67,11 +67,11 @@ bool TransformCache::QueryTransform(double timestamp,
double
delt
=
timestamp
-
transforms_
.
back
().
timestamp
;
if
(
delt
>
max_duration
)
{
LOG_
INFO
<<
"ERROR: query timestamp is "
<<
delt
A
INFO
<<
"ERROR: query timestamp is "
<<
delt
<<
"s later than cached timestamp"
;
return
false
;
}
else
if
(
delt
<
0.0
)
{
LOG_
INFO
<<
"ERROR: query earlier timestamp than transform cache"
;
A
INFO
<<
"ERROR: query earlier timestamp than transform cache"
;
return
false
;
}
...
...
@@ -79,9 +79,9 @@ bool TransformCache::QueryTransform(double timestamp,
if
(
size
==
1
)
{
(
*
transform
)
=
transforms_
.
back
();
transform
->
timestamp
=
timestamp
;
LOG_
INFO
<<
"use transform at "
<<
GLOG_TIMESTAMP
(
transforms_
.
back
().
timestamp
)
<<
" for "
<<
GLOG_TIMESTAMP
(
timestamp
);
A
INFO
<<
"use transform at "
<<
std
::
to_string
(
transforms_
.
back
().
timestamp
)
<<
" for "
<<
std
::
to_string
(
timestamp
);
}
else
{
double
ratio
=
(
timestamp
-
transforms_
[
size
-
2
].
timestamp
)
/
...
...
@@ -100,10 +100,10 @@ bool TransformCache::QueryTransform(double timestamp,
transforms_
[
size
-
2
].
translation
.
z
()
*
(
1
-
ratio
)
+
transforms_
[
size
-
1
].
translation
.
z
()
*
ratio
;
LOG_INFO
<<
"estimate pose at "
<<
GLOG_TIMESTAMP
(
timestamp
)
AINFO
<<
"estimate pose at "
<<
std
::
to_string
(
timestamp
)
<<
" from poses at "
<<
GLOG_TIMESTAMP
(
transforms_
[
size
-
2
].
timestamp
)
<<
" and "
<<
GLOG_TIMESTAMP
(
transforms_
[
size
-
1
].
timestamp
);
<<
std
::
to_string
(
transforms_
[
size
-
2
].
timestamp
)
<<
" and "
<<
std
::
to_string
(
transforms_
[
size
-
1
].
timestamp
);
}
return
true
;
}
...
...
@@ -136,7 +136,7 @@ bool TransformWrapper::GetSensor2worldTrans(
double
timestamp
,
Eigen
::
Affine3d
*
sensor2world_trans
,
Eigen
::
Affine3d
*
novatel2world_trans
)
{
if
(
!
inited_
)
{
LOG_
ERROR
<<
"TransformWrapper not Initialized,"
A
ERROR
<<
"TransformWrapper not Initialized,"
<<
" unable to call GetSensor2worldTrans."
;
return
false
;
}
...
...
@@ -151,7 +151,7 @@ bool TransformWrapper::GetSensor2worldTrans(
sensor2novatel_extrinsics_
.
reset
(
new
Eigen
::
Affine3d
);
*
sensor2novatel_extrinsics_
=
trans_sensor2novatel
.
translation
*
trans_sensor2novatel
.
rotation
;
LOG_
INFO
<<
"Get sensor2novatel extrinsics successfully."
;
A
INFO
<<
"Get sensor2novatel extrinsics successfully."
;
}
StampedTransform
trans_novatel2world
;
...
...
@@ -179,7 +179,7 @@ bool TransformWrapper::GetSensor2worldTrans(
if
(
novatel2world_trans
!=
nullptr
)
{
*
novatel2world_trans
=
novatel2world
;
}
LOG_INFO
<<
"Get pose timestamp: "
<<
GLOG_TIMESTAMP
(
timestamp
)
AINFO
<<
"Get pose timestamp: "
<<
std
::
to_string
(
timestamp
)
<<
", pose: "
<<
std
::
endl
<<
(
*
sensor2world_trans
).
matrix
();
return
true
;
...
...
@@ -187,7 +187,7 @@ bool TransformWrapper::GetSensor2worldTrans(
bool
TransformWrapper
::
GetExtrinsics
(
Eigen
::
Affine3d
*
trans
)
{
if
(
!
inited_
||
trans
==
nullptr
||
sensor2novatel_extrinsics_
==
nullptr
)
{
LOG_
ERROR
<<
"TransformWrapper get extrinsics failed"
;
A
ERROR
<<
"TransformWrapper get extrinsics failed"
;
return
false
;
}
*
trans
=
*
sensor2novatel_extrinsics_
;
...
...
@@ -223,7 +223,7 @@ bool TransformWrapper::QueryTrans(double timestamp, StampedTransform* trans,
std
::
string
err_string
;
if
(
!
tf2_buffer_
->
canTransform
(
frame_id
,
child_frame_id
,
query_time
,
FLAGS_obs_tf2_buff_size
,
&
err_string
))
{
LOG_ERROR
<<
"Can not find transform. "
<<
GLOG_TIMESTAMP
(
timestamp
)
AERROR
<<
"Can not find transform. "
<<
std
::
to_string
(
timestamp
)
<<
" frame_id: "
<<
frame_id
<<
" child_frame_id: "
<<
child_frame_id
<<
" Error info: "
<<
err_string
;
...
...
@@ -245,7 +245,7 @@ bool TransformWrapper::QueryTrans(double timestamp, StampedTransform* trans,
stamped_transform
.
transform
().
rotation
().
qy
(),
stamped_transform
.
transform
().
rotation
().
qz
());
}
catch
(
tf2
::
TransformException
&
ex
)
{
LOG_
ERROR
<<
ex
.
what
();
A
ERROR
<<
ex
.
what
();
return
false
;
}
return
true
;
...
...
@@ -255,7 +255,7 @@ bool TransformWrapper::GetExtrinsicsBySensorId(
const
std
::
string
&
from_sensor_id
,
const
std
::
string
&
to_sensor_id
,
Eigen
::
Affine3d
*
trans
)
{
if
(
trans
==
nullptr
)
{
LOG_
ERROR
<<
"TransformWrapper get extrinsics failed"
;
A
ERROR
<<
"TransformWrapper get extrinsics failed"
;
return
false
;
}
...
...
modules/planning/planning_component.cc
浏览文件 @
c7778a23
...
...
@@ -38,7 +38,8 @@ bool PlanningComponent::Init() {
routing_reader_
=
node_
->
CreateReader
<
RoutingResponse
>
(
FLAGS_routing_response_topic
,
[
this
](
const
std
::
shared_ptr
<
RoutingResponse
>&
routing
)
{
ADEBUG
<<
"Received routing data: run routing callback."
;
AINFO
<<
"Received routing data: run routing callback 2:"
<<
routing
->
header
().
DebugString
();
std
::
lock_guard
<
std
::
mutex
>
lock
(
mutex_
);
routing_
.
CopyFrom
(
*
routing
);
});
...
...
modules/planning/std_planning.cc
浏览文件 @
c7778a23
...
...
@@ -65,6 +65,8 @@ bool IsVehicleStateValid(const VehicleState& vehicle_state) {
bool
IsDifferentRouting
(
const
RoutingResponse
&
first
,
const
RoutingResponse
&
second
)
{
AERROR
<<
"first routing:"
<<
first
.
header
().
DebugString
()
<<
" second routing:"
<<
second
.
header
().
DebugString
();
if
(
first
.
has_header
()
&&
second
.
has_header
())
{
if
(
first
.
header
().
sequence_num
()
!=
second
.
header
().
sequence_num
())
{
return
true
;
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录