Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
7461c419
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,体验更适合开发者的 AI 搜索 >>
未验证
提交
7461c419
编写于
11月 29, 2018
作者:
Y
Yifei Jiang
提交者:
GitHub
11月 29, 2018
浏览文件
操作
浏览文件
下载
差异文件
Merge branch 'v3.1_dev' into r2.5.0
上级
c748a604
621de127
变更
24
隐藏空白更改
内联
并排
Showing
24 changed file
with
2212 addition
and
851 deletion
+2212
-851
modules/localization/lmd/BUILD
modules/localization/lmd/BUILD
+7
-1
modules/localization/lmd/common/BUILD
modules/localization/lmd/common/BUILD
+34
-0
modules/localization/lmd/common/pose_list.cc
modules/localization/lmd/common/pose_list.cc
+163
-0
modules/localization/lmd/common/pose_list.h
modules/localization/lmd/common/pose_list.h
+80
-0
modules/localization/lmd/common/pose_list_test.cc
modules/localization/lmd/common/pose_list_test.cc
+141
-0
modules/localization/lmd/common/tm_list.h
modules/localization/lmd/common/tm_list.h
+171
-0
modules/localization/lmd/common/tm_list_test.cc
modules/localization/lmd/common/tm_list_test.cc
+138
-0
modules/localization/lmd/lmd_localization.cc
modules/localization/lmd/lmd_localization.cc
+133
-824
modules/localization/lmd/lmd_localization.h
modules/localization/lmd/lmd_localization.h
+39
-24
modules/localization/lmd/lmd_localization_test.cc
modules/localization/lmd/lmd_localization_test.cc
+1
-1
modules/localization/lmd/predictor/BUILD
modules/localization/lmd/predictor/BUILD
+16
-0
modules/localization/lmd/predictor/output/BUILD
modules/localization/lmd/predictor/output/BUILD
+40
-0
modules/localization/lmd/predictor/output/predictor_output.cc
...les/localization/lmd/predictor/output/predictor_output.cc
+329
-0
modules/localization/lmd/predictor/output/predictor_output.h
modules/localization/lmd/predictor/output/predictor_output.h
+79
-0
modules/localization/lmd/predictor/output/predictor_output_test.cc
...ocalization/lmd/predictor/output/predictor_output_test.cc
+113
-0
modules/localization/lmd/predictor/output/predictor_print_error.cc
...ocalization/lmd/predictor/output/predictor_print_error.cc
+155
-0
modules/localization/lmd/predictor/output/predictor_print_error.h
...localization/lmd/predictor/output/predictor_print_error.h
+70
-0
modules/localization/lmd/predictor/predictor.h
modules/localization/lmd/predictor/predictor.h
+119
-0
modules/localization/lmd/predictor/raw/BUILD
modules/localization/lmd/predictor/raw/BUILD
+42
-0
modules/localization/lmd/predictor/raw/predictor_gps.cc
modules/localization/lmd/predictor/raw/predictor_gps.cc
+67
-0
modules/localization/lmd/predictor/raw/predictor_gps.h
modules/localization/lmd/predictor/raw/predictor_gps.h
+74
-0
modules/localization/lmd/predictor/raw/predictor_imu.cc
modules/localization/lmd/predictor/raw/predictor_imu.cc
+119
-0
modules/localization/lmd/predictor/raw/predictor_imu.h
modules/localization/lmd/predictor/raw/predictor_imu.h
+78
-0
modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/cc_lane_post_processor.cc
..._process/cc_lane_post_processor/cc_lane_post_processor.cc
+4
-1
未找到文件。
modules/localization/lmd/BUILD
浏览文件 @
7461c419
...
...
@@ -18,7 +18,13 @@ cc_library(
"//modules/common/proto:common_proto"
,
"//modules/common/status"
,
"//modules/common/time"
,
"//modules/common/util:thread_pool"
,
"//modules/localization:localization_base"
,
"//modules/localization/common:localization_common"
,
"//modules/localization/lmd/predictor:localization_lmd_predictor"
,
"//modules/localization/lmd/predictor/output:localization_lmd_predictor_output"
,
"//modules/localization/lmd/predictor/raw:localization_lmd_predictor_raw_gps"
,
"//modules/localization/lmd/predictor/raw:localization_lmd_predictor_raw_imu"
,
"//modules/localization/proto:localization_config_proto"
,
"//modules/localization/proto:localization_proto"
,
"//modules/localization/proto:odometry_lane_marker_proto"
,
...
...
@@ -35,10 +41,10 @@ cc_test(
],
data
=
[
"//modules/localization:localization_testdata"
],
deps
=
[
":lmd_localization"
,
"//modules/common:log"
,
"//modules/common/time"
,
"//modules/common/util"
,
"//modules/localization/lmd:lmd_localization"
,
"@gtest//:main"
,
],
)
...
...
modules/localization/lmd/common/BUILD
0 → 100644
浏览文件 @
7461c419
load
(
"//tools:cpplint.bzl"
,
"cpplint"
)
package
(
default_visibility
=
[
"//visibility:public"
])
cc_library
(
name
=
"localization_lmd_common"
,
srcs
=
[
"pose_list.cc"
,
],
hdrs
=
[
"pose_list.h"
,
"tm_list.h"
,
],
deps
=
[
"//modules/common:log"
,
"//modules/common/math"
,
"//modules/localization/proto:localization_proto"
,
],
)
cc_test
(
name
=
"localization_lmd_common_test"
,
size
=
"small"
,
srcs
=
[
"pose_list_test.cc"
,
"tm_list_test.cc"
,
],
deps
=
[
":localization_lmd_common"
,
"@gtest//:main"
,
],
)
cpplint
()
modules/localization/lmd/common/pose_list.cc
0 → 100644
浏览文件 @
7461c419
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/localization/lmd/common/pose_list.h"
#include <cmath>
#include "modules/common/log.h"
#include "modules/common/math/quaternion.h"
namespace
apollo
{
namespace
localization
{
using
apollo
::
common
::
math
::
QuaternionToHeading
;
namespace
{
template
<
class
T
>
T
InterpolateXYZ
(
const
T
&
p1
,
const
T
&
p2
,
double
frac1
)
{
T
p
;
double
frac2
=
1.0
-
frac1
;
if
(
p1
.
has_x
()
&&
!
std
::
isnan
(
p1
.
x
())
&&
p2
.
has_x
()
&&
!
std
::
isnan
(
p2
.
x
()))
{
p
.
set_x
(
p1
.
x
()
*
frac2
+
p2
.
x
()
*
frac1
);
}
if
(
p1
.
has_y
()
&&
!
std
::
isnan
(
p1
.
y
())
&&
p2
.
has_y
()
&&
!
std
::
isnan
(
p2
.
y
()))
{
p
.
set_y
(
p1
.
y
()
*
frac2
+
p2
.
y
()
*
frac1
);
}
if
(
p1
.
has_z
()
&&
!
std
::
isnan
(
p1
.
z
())
&&
p2
.
has_z
()
&&
!
std
::
isnan
(
p2
.
z
()))
{
p
.
set_z
(
p1
.
z
()
*
frac2
+
p2
.
z
()
*
frac1
);
}
return
p
;
}
template
<
class
T
>
T
InterpolateQuaternion
(
const
T
&
p1
,
const
T
&
p2
,
double
frac1
)
{
T
p
;
T
p21
=
p2
;
auto
cosa
=
p1
.
qw
()
*
p2
.
qw
()
+
p1
.
qx
()
*
p2
.
qx
()
+
p1
.
qy
()
*
p2
.
qy
()
+
p1
.
qz
()
*
p2
.
qz
();
if
(
cosa
<
0.0
)
{
p21
.
set_qw
(
-
p2
.
qw
());
p21
.
set_qx
(
-
p2
.
qx
());
p21
.
set_qy
(
-
p2
.
qy
());
p21
.
set_qz
(
-
p2
.
qz
());
cosa
=
-
cosa
;
}
double
frac2
;
constexpr
double
kCriticalCosa
=
0.9995
;
if
(
cosa
>
kCriticalCosa
)
{
// if cosa near 1.0, user linear interpolation
frac2
=
1.0
-
frac1
;
}
else
{
auto
sina
=
std
::
sqrt
(
1.0
-
cosa
*
cosa
);
auto
a
=
std
::
atan2
(
sina
,
cosa
);
frac2
=
std
::
sin
((
1.0
-
frac1
)
*
a
)
/
sina
;
frac1
=
std
::
sin
(
frac1
*
a
)
/
sina
;
}
p
.
set_qw
(
p1
.
qw
()
*
frac2
+
p21
.
qw
()
*
frac1
);
p
.
set_qx
(
p1
.
qx
()
*
frac2
+
p21
.
qx
()
*
frac1
);
p
.
set_qy
(
p1
.
qy
()
*
frac2
+
p21
.
qy
()
*
frac1
);
p
.
set_qz
(
p1
.
qz
()
*
frac2
+
p21
.
qz
()
*
frac1
);
return
p
;
}
}
// namespace
bool
PoseList
::
FindMatchingPose
(
double
timestamp_sec
,
Pose
*
pose
)
const
{
CHECK_NOTNULL
(
pose
);
auto
p
=
RangeOf
(
timestamp_sec
);
if
(
p
.
first
==
end
()
||
p
.
second
==
end
())
{
return
false
;
}
InterpolatePose
(
p
.
first
->
first
,
p
.
first
->
second
,
p
.
second
->
first
,
p
.
second
->
second
,
timestamp_sec
,
pose
);
return
true
;
}
bool
PoseList
::
FindNearestPose
(
double
timestamp_sec
,
Pose
*
pose
)
const
{
CHECK_NOTNULL
(
pose
);
if
(
!
FindMatchingPose
(
timestamp_sec
,
pose
))
{
auto
it
=
Nearest
(
timestamp_sec
);
if
(
it
==
end
())
{
return
false
;
}
pose
->
CopyFrom
(
it
->
second
);
}
return
true
;
}
void
PoseList
::
InterpolatePose
(
double
timestamp_sec1
,
const
Pose
&
pose1
,
double
timestamp_sec2
,
const
Pose
&
pose2
,
double
timestamp_sec
,
Pose
*
pose
)
{
CHECK_GE
(
timestamp_sec
,
timestamp_sec1
);
CHECK_GE
(
timestamp_sec2
,
timestamp_sec
);
CHECK_NOTNULL
(
pose
);
auto
time_diff
=
timestamp_sec2
-
timestamp_sec1
;
if
(
fabs
(
time_diff
)
>
0.0
)
{
auto
frac1
=
(
timestamp_sec
-
timestamp_sec1
)
/
time_diff
;
if
(
pose1
.
has_position
()
&&
pose2
.
has_position
())
{
auto
val
=
InterpolateXYZ
(
pose1
.
position
(),
pose2
.
position
(),
frac1
);
pose
->
mutable_position
()
->
CopyFrom
(
val
);
}
if
(
pose1
.
has_orientation
()
&&
pose2
.
has_orientation
()
&&
pose1
.
orientation
().
has_qw
()
&&
pose1
.
orientation
().
has_qx
()
&&
pose1
.
orientation
().
has_qy
()
&&
pose1
.
orientation
().
has_qz
()
&&
pose2
.
orientation
().
has_qw
()
&&
pose2
.
orientation
().
has_qx
()
&&
pose2
.
orientation
().
has_qy
()
&&
pose2
.
orientation
().
has_qz
())
{
auto
val
=
InterpolateQuaternion
(
pose1
.
orientation
(),
pose2
.
orientation
(),
frac1
);
pose
->
mutable_orientation
()
->
CopyFrom
(
val
);
pose
->
set_heading
(
QuaternionToHeading
(
pose
->
orientation
().
qw
(),
pose
->
orientation
().
qx
(),
pose
->
orientation
().
qy
(),
pose
->
orientation
().
qz
()));
}
if
(
pose1
.
has_linear_velocity
()
&&
pose2
.
has_linear_velocity
())
{
auto
val
=
InterpolateXYZ
(
pose1
.
linear_velocity
(),
pose2
.
linear_velocity
(),
frac1
);
pose
->
mutable_linear_velocity
()
->
CopyFrom
(
val
);
}
if
(
pose1
.
has_angular_velocity
()
&&
pose2
.
has_angular_velocity
())
{
auto
val
=
InterpolateXYZ
(
pose1
.
angular_velocity
(),
pose2
.
angular_velocity
(),
frac1
);
pose
->
mutable_angular_velocity
()
->
CopyFrom
(
val
);
}
if
(
pose1
.
has_linear_acceleration
()
&&
pose2
.
has_linear_acceleration
())
{
auto
val
=
InterpolateXYZ
(
pose1
.
linear_acceleration
(),
pose2
.
linear_acceleration
(),
frac1
);
pose
->
mutable_linear_acceleration
()
->
CopyFrom
(
val
);
}
if
(
pose1
.
has_euler_angles
()
&&
pose2
.
has_euler_angles
())
{
auto
val
=
InterpolateXYZ
(
pose1
.
euler_angles
(),
pose2
.
euler_angles
(),
frac1
);
pose
->
mutable_euler_angles
()
->
CopyFrom
(
val
);
}
}
}
}
// namespace localization
}
// namespace apollo
modules/localization/lmd/common/pose_list.h
0 → 100644
浏览文件 @
7461c419
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
/**
* @file pose_list.h
* @brief The class of PoseList.
*/
#ifndef MODULES_LOCALIZATION_LMD_COMMON_POSE_LIST_H_
#define MODULES_LOCALIZATION_LMD_COMMON_POSE_LIST_H_
#include "modules/localization/proto/localization.pb.h"
#include "modules/localization/lmd/common/tm_list.h"
/**
* @namespace apollo::localization
* @brief apollo::localization
*/
namespace
apollo
{
namespace
localization
{
/**
* @class PoseList
*
* @brief A time marked list of poses.
*/
class
PoseList
:
public
TimeMarkedList
<
Pose
>
{
public:
explicit
PoseList
(
double
memory_cycle_sec
=
0.0
)
:
TimeMarkedList
<
Pose
>
(
memory_cycle_sec
)
{}
/**
* @brief Get a pose by timestamp, may need interpolation.
* @param timestamp_sec The required timestamp.
* @param pose The output.
* @return True if found.
*/
bool
FindMatchingPose
(
double
timestamp_sec
,
Pose
*
pose
)
const
;
/**
* @brief Get a nearest pose by timestamp, may need interpolation.
* @param timestamp_sec The required timestamp.
* @param pose The output.
* @return True if found.
*/
bool
FindNearestPose
(
double
timestamp_sec
,
Pose
*
pose
)
const
;
/**
* @brief Get a pose by interpolating two poses.
* @param timestamp_sec1 The timestamp of pose 1.
* @param pose1 The pose 1.
* @param timestamp_sec2 The timestamp of pose 2.
* @param pose2 The pose 2.
* @param timestamp_sec The required timestamp, must >=timestamp_sec1 and
* <=timestamp_sec2.
* @param pose The output.
*/
static
void
InterpolatePose
(
double
timestamp_sec1
,
const
Pose
&
pose1
,
double
timestamp_sec2
,
const
Pose
&
pose2
,
double
timestamp_sec
,
Pose
*
pose
);
};
}
// namespace localization
}
// namespace apollo
#endif // MODULES_LOCALIZATION_LMD_COMMON_POSE_LIST_H_
modules/localization/lmd/common/pose_list_test.cc
0 → 100644
浏览文件 @
7461c419
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/localization/lmd/common/pose_list.h"
#include "gtest/gtest.h"
namespace
apollo
{
namespace
localization
{
TEST
(
PoseListTest
,
FindMatchingPose
)
{
Pose
pose1
;
pose1
.
mutable_position
()
->
set_x
(
0.0
);
pose1
.
mutable_position
()
->
set_y
(
0.0
);
pose1
.
mutable_position
()
->
set_z
(
0.0
);
pose1
.
mutable_orientation
()
->
set_qw
(
1.0
);
pose1
.
mutable_orientation
()
->
set_qx
(
0.0
);
pose1
.
mutable_orientation
()
->
set_qy
(
0.0
);
pose1
.
mutable_orientation
()
->
set_qz
(
0.0
);
pose1
.
mutable_linear_velocity
()
->
set_x
(
0.0
);
pose1
.
mutable_linear_velocity
()
->
set_y
(
0.0
);
pose1
.
mutable_linear_velocity
()
->
set_z
(
0.0
);
pose1
.
mutable_angular_velocity
()
->
set_x
(
0.0
);
pose1
.
mutable_angular_velocity
()
->
set_y
(
0.0
);
pose1
.
mutable_angular_velocity
()
->
set_z
(
0.0
);
pose1
.
mutable_linear_acceleration
()
->
set_x
(
0.0
);
pose1
.
mutable_linear_acceleration
()
->
set_y
(
0.0
);
pose1
.
mutable_linear_acceleration
()
->
set_z
(
0.0
);
pose1
.
mutable_euler_angles
()
->
set_x
(
0.0
);
pose1
.
mutable_euler_angles
()
->
set_y
(
0.0
);
pose1
.
mutable_euler_angles
()
->
set_z
(
0.0
);
Pose
pose2
;
pose2
.
mutable_position
()
->
set_x
(
1.0
);
pose2
.
mutable_position
()
->
set_y
(
1.0
);
pose2
.
mutable_position
()
->
set_z
(
1.0
);
pose2
.
mutable_orientation
()
->
set_qw
(
0.707
);
pose2
.
mutable_orientation
()
->
set_qx
(
0.0
);
pose2
.
mutable_orientation
()
->
set_qy
(
0.0
);
pose2
.
mutable_orientation
()
->
set_qz
(
0.707
);
pose2
.
mutable_linear_velocity
()
->
set_x
(
1.0
);
pose2
.
mutable_linear_velocity
()
->
set_y
(
1.0
);
pose2
.
mutable_linear_velocity
()
->
set_z
(
1.0
);
pose2
.
mutable_angular_velocity
()
->
set_x
(
1.0
);
pose2
.
mutable_angular_velocity
()
->
set_y
(
1.0
);
pose2
.
mutable_angular_velocity
()
->
set_z
(
1.0
);
pose2
.
mutable_linear_acceleration
()
->
set_x
(
1.0
);
pose2
.
mutable_linear_acceleration
()
->
set_y
(
1.0
);
pose2
.
mutable_linear_acceleration
()
->
set_z
(
1.0
);
pose2
.
mutable_euler_angles
()
->
set_x
(
1.0
);
pose2
.
mutable_euler_angles
()
->
set_y
(
1.0
);
pose2
.
mutable_euler_angles
()
->
set_z
(
1.0
);
PoseList
pose_list
(
100.0
);
pose_list
.
Push
(
0.0
,
pose1
);
pose_list
.
Push
(
1.0
,
pose2
);
Pose
pose
;
EXPECT_TRUE
(
pose_list
.
FindMatchingPose
(
0.333
,
&
pose
));
EXPECT_NEAR
(
0.333
,
pose
.
position
().
x
(),
1e-10
);
EXPECT_NEAR
(
0.333
,
pose
.
position
().
y
(),
1e-10
);
EXPECT_NEAR
(
0.333
,
pose
.
position
().
z
(),
1e-10
);
EXPECT_NEAR
(
0.965981
,
pose
.
orientation
().
qw
(),
1e-6
);
EXPECT_NEAR
(
0.0
,
pose
.
orientation
().
qx
(),
1e-6
);
EXPECT_NEAR
(
0.0
,
pose
.
orientation
().
qy
(),
1e-6
);
EXPECT_NEAR
(
0.258537
,
pose
.
orientation
().
qz
(),
1e-6
);
EXPECT_NEAR
(
0.333
,
pose
.
linear_velocity
().
x
(),
1e-10
);
EXPECT_NEAR
(
0.333
,
pose
.
linear_velocity
().
y
(),
1e-10
);
EXPECT_NEAR
(
0.333
,
pose
.
linear_velocity
().
z
(),
1e-10
);
EXPECT_NEAR
(
0.333
,
pose
.
angular_velocity
().
x
(),
1e-10
);
EXPECT_NEAR
(
0.333
,
pose
.
angular_velocity
().
y
(),
1e-10
);
EXPECT_NEAR
(
0.333
,
pose
.
angular_velocity
().
z
(),
1e-10
);
EXPECT_NEAR
(
0.333
,
pose
.
linear_acceleration
().
x
(),
1e-10
);
EXPECT_NEAR
(
0.333
,
pose
.
linear_acceleration
().
y
(),
1e-10
);
EXPECT_NEAR
(
0.333
,
pose
.
linear_acceleration
().
z
(),
1e-10
);
EXPECT_NEAR
(
0.333
,
pose
.
euler_angles
().
x
(),
1e-10
);
EXPECT_NEAR
(
0.333
,
pose
.
euler_angles
().
y
(),
1e-10
);
EXPECT_NEAR
(
0.333
,
pose
.
euler_angles
().
z
(),
1e-10
);
EXPECT_TRUE
(
pose_list
.
FindMatchingPose
(
1e-11
,
&
pose
));
EXPECT_NEAR
(
0.0
,
pose
.
position
().
x
(),
1e-10
);
EXPECT_NEAR
(
0.0
,
pose
.
position
().
y
(),
1e-10
);
EXPECT_NEAR
(
0.0
,
pose
.
position
().
z
(),
1e-10
);
EXPECT_TRUE
(
pose_list
.
FindMatchingPose
(
1.0
,
&
pose
));
EXPECT_NEAR
(
1.0
,
pose
.
position
().
x
(),
1e-10
);
EXPECT_NEAR
(
1.0
,
pose
.
position
().
y
(),
1e-10
);
EXPECT_NEAR
(
1.0
,
pose
.
position
().
z
(),
1e-10
);
EXPECT_FALSE
(
pose_list
.
FindMatchingPose
(
-
1.0
,
&
pose
));
EXPECT_FALSE
(
pose_list
.
FindMatchingPose
(
2.0
,
&
pose
));
}
TEST
(
PoseListTest
,
FindNearestPose
)
{
PoseList
pose_list
(
100.0
);
Pose
pose
;
EXPECT_FALSE
(
pose_list
.
FindNearestPose
(
0.0
,
&
pose
));
Pose
pose1
;
pose1
.
mutable_position
()
->
set_x
(
0.0
);
pose1
.
mutable_position
()
->
set_y
(
0.0
);
pose1
.
mutable_position
()
->
set_z
(
0.0
);
pose_list
.
Push
(
10.0
,
pose1
);
Pose
pose2
;
pose2
.
mutable_position
()
->
set_x
(
1.0
);
pose2
.
mutable_position
()
->
set_y
(
1.0
);
pose2
.
mutable_position
()
->
set_z
(
1.0
);
pose_list
.
Push
(
11.0
,
pose2
);
EXPECT_TRUE
(
pose_list
.
FindNearestPose
(
9.0
,
&
pose
));
EXPECT_NEAR
(
0.0
,
pose
.
position
().
x
(),
1e-10
);
EXPECT_NEAR
(
0.0
,
pose
.
position
().
y
(),
1e-10
);
EXPECT_NEAR
(
0.0
,
pose
.
position
().
z
(),
1e-10
);
EXPECT_TRUE
(
pose_list
.
FindNearestPose
(
14.0
,
&
pose
));
EXPECT_NEAR
(
1.0
,
pose
.
position
().
x
(),
1e-10
);
EXPECT_NEAR
(
1.0
,
pose
.
position
().
y
(),
1e-10
);
EXPECT_NEAR
(
1.0
,
pose
.
position
().
z
(),
1e-10
);
EXPECT_TRUE
(
pose_list
.
FindNearestPose
(
10.5
,
&
pose
));
EXPECT_NEAR
(
0.5
,
pose
.
position
().
x
(),
1e-10
);
EXPECT_NEAR
(
0.5
,
pose
.
position
().
y
(),
1e-10
);
EXPECT_NEAR
(
0.5
,
pose
.
position
().
z
(),
1e-10
);
}
}
// namespace localization
}
// namespace apollo
modules/localization/lmd/common/tm_list.h
0 → 100644
浏览文件 @
7461c419
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
/**
* @file tm_list.h
* @brief The class of TimeMarkedList.
*/
#ifndef MODULES_LOCALIZATION_LMD_COMMON_TM_LIST_H_
#define MODULES_LOCALIZATION_LMD_COMMON_TM_LIST_H_
#include <algorithm>
#include <map>
#include <utility>
/**
* @namespace apollo::localization
* @brief apollo::localization
*/
namespace
apollo
{
namespace
localization
{
/**
* @class TimeMarkedList
*
* @brief A time marked list.
*/
template
<
class
Data
>
class
TimeMarkedList
:
public
std
::
map
<
double
,
Data
>
{
public:
typedef
typename
std
::
map
<
double
,
Data
>::
const_iterator
Iterator
;
explicit
TimeMarkedList
(
double
memory_cycle_sec
=
0.0
)
{
memory_cycle_sec_
=
std
::
max
(
0.0
,
memory_cycle_sec
);
}
double
MemoryCycleSec
()
const
{
return
memory_cycle_sec_
;
}
bool
Push
(
double
timestamp_sec
,
const
Data
&
data
)
{
return
Push
(
timestamp_sec
,
Data
(
data
));
}
bool
Push
(
double
timestamp_sec
,
Data
&&
data
)
{
auto
latest
=
Latest
();
if
(
latest
!=
std
::
map
<
double
,
Data
>::
end
()
&&
latest
->
first
>=
timestamp_sec
)
{
return
false
;
}
std
::
map
<
double
,
Data
>::
emplace
(
timestamp_sec
,
std
::
move
(
data
));
auto
oldest
=
Oldest
();
if
(
oldest
!=
std
::
map
<
double
,
Data
>::
end
())
{
auto
oldest1
=
oldest
;
oldest1
++
;
if
(
oldest1
!=
std
::
map
<
double
,
Data
>::
end
()
&&
timestamp_sec
-
oldest
->
first
>
memory_cycle_sec_
&&
timestamp_sec
-
oldest1
->
first
>=
memory_cycle_sec_
)
{
std
::
map
<
double
,
Data
>::
erase
(
oldest
);
}
}
return
true
;
}
Iterator
Latest
()
const
{
if
(
!
std
::
map
<
double
,
Data
>::
empty
())
{
auto
iter
=
std
::
map
<
double
,
Data
>::
end
();
iter
--
;
return
iter
;
}
else
{
return
std
::
map
<
double
,
Data
>::
end
();
}
}
Iterator
Oldest
()
const
{
return
std
::
map
<
double
,
Data
>::
begin
();
}
template
<
class
T
>
bool
Older
(
const
TimeMarkedList
<
T
>&
other
)
const
{
auto
it
=
other
.
Latest
();
if
(
it
!=
other
.
end
())
{
return
Older
(
it
->
first
);
}
return
false
;
}
bool
Older
(
double
timestamp_sec
)
const
{
auto
it
=
Latest
();
if
(
it
!=
std
::
map
<
double
,
Data
>::
end
())
{
return
it
->
first
<
timestamp_sec
;
}
return
false
;
}
template
<
class
T
>
bool
Newer
(
const
TimeMarkedList
<
T
>&
other
)
const
{
auto
it
=
other
.
Latest
();
if
(
it
!=
other
.
end
())
{
return
Newer
(
it
->
first
);
}
return
false
;
}
bool
Newer
(
double
timestamp_sec
)
const
{
auto
it
=
Latest
();
if
(
it
!=
std
::
map
<
double
,
Data
>::
end
())
{
return
it
->
first
>
timestamp_sec
;
}
return
false
;
}
std
::
pair
<
Iterator
,
Iterator
>
RangeOf
(
double
timestamp_sec
)
const
{
auto
upper
=
std
::
map
<
double
,
Data
>::
lower_bound
(
timestamp_sec
);
Iterator
lower
;
if
(
upper
!=
std
::
map
<
double
,
Data
>::
end
())
{
if
(
upper
==
std
::
map
<
double
,
Data
>::
begin
())
{
lower
=
std
::
map
<
double
,
Data
>::
end
();
}
else
{
lower
=
upper
;
lower
--
;
}
}
else
{
if
(
!
std
::
map
<
double
,
Data
>::
empty
())
{
lower
=
std
::
map
<
double
,
Data
>::
end
();
lower
--
;
}
else
{
lower
=
std
::
map
<
double
,
Data
>::
end
();
}
}
return
std
::
make_pair
(
lower
,
upper
);
}
Iterator
Nearest
(
double
timestamp_sec
)
const
{
auto
p
=
RangeOf
(
timestamp_sec
);
if
(
p
.
first
==
std
::
map
<
double
,
Data
>::
end
()
&&
p
.
second
==
std
::
map
<
double
,
Data
>::
end
())
{
return
std
::
map
<
double
,
Data
>::
end
();
}
if
(
p
.
first
==
std
::
map
<
double
,
Data
>::
end
())
{
return
p
.
second
;
}
else
if
(
p
.
second
==
std
::
map
<
double
,
Data
>::
end
())
{
return
p
.
first
;
}
if
(
timestamp_sec
-
p
.
first
->
first
<
p
.
second
->
first
-
timestamp_sec
)
{
return
p
.
first
;
}
else
{
return
p
.
second
;
}
}
private:
double
memory_cycle_sec_
;
};
}
// namespace localization
}
// namespace apollo
#endif // MODULES_LOCALIZATION_LMD_COMMON_TM_LIST_H_
modules/localization/lmd/common/tm_list_test.cc
0 → 100644
浏览文件 @
7461c419
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/localization/lmd/common/tm_list.h"
#include "gtest/gtest.h"
namespace
apollo
{
namespace
localization
{
TEST
(
TimeMarkedListTest
,
InsertAndFind
)
{
TimeMarkedList
<
int
>
tm
(
100.0
);
for
(
auto
i
=
0
;
i
<
200
;
++
i
)
{
tm
.
Push
(
static_cast
<
double
>
(
i
),
i
);
}
EXPECT_EQ
(
101
,
tm
.
size
());
auto
latest
=
tm
.
Latest
();
EXPECT_NE
(
tm
.
end
(),
latest
);
if
(
latest
!=
tm
.
end
())
{
EXPECT_EQ
(
199
,
latest
->
second
);
}
auto
oldest
=
tm
.
Oldest
();
EXPECT_NE
(
tm
.
end
(),
oldest
);
if
(
oldest
!=
tm
.
end
())
{
EXPECT_EQ
(
99
,
oldest
->
second
);
}
}
TEST
(
TimeMarkedListTest
,
Older
)
{
TimeMarkedList
<
int
>
tm
(
100.0
);
for
(
auto
i
=
0
;
i
<
20
;
++
i
)
{
tm
.
Push
(
static_cast
<
double
>
(
i
),
i
);
}
TimeMarkedList
<
int
>
tm1
(
100.0
);
for
(
auto
i
=
0
;
i
<
30
;
++
i
)
{
tm1
.
Push
(
static_cast
<
double
>
(
i
),
i
);
}
TimeMarkedList
<
int
>
tm2
(
100.0
);
EXPECT_TRUE
(
tm
.
Older
(
tm1
));
EXPECT_FALSE
(
tm1
.
Older
(
tm
));
EXPECT_FALSE
(
tm
.
Older
(
tm2
));
EXPECT_FALSE
(
tm2
.
Older
(
tm1
));
}
TEST
(
TimeMarkedListTest
,
Newer
)
{
TimeMarkedList
<
int
>
tm
(
100.0
);
for
(
auto
i
=
0
;
i
<
20
;
++
i
)
{
tm
.
Push
(
static_cast
<
double
>
(
i
),
i
);
}
TimeMarkedList
<
int
>
tm1
(
100.0
);
for
(
auto
i
=
0
;
i
<
30
;
++
i
)
{
tm1
.
Push
(
static_cast
<
double
>
(
i
),
i
);
}
TimeMarkedList
<
int
>
tm2
(
100.0
);
EXPECT_TRUE
(
tm1
.
Newer
(
tm
));
EXPECT_FALSE
(
tm
.
Newer
(
tm1
));
EXPECT_FALSE
(
tm2
.
Newer
(
tm
));
EXPECT_FALSE
(
tm1
.
Newer
(
tm2
));
}
TEST
(
TimeMarkedListTest
,
RangeOf
)
{
TimeMarkedList
<
double
>
tm
(
100.0
);
auto
p
=
tm
.
RangeOf
(
10.0
);
EXPECT_EQ
(
tm
.
end
(),
p
.
first
);
EXPECT_EQ
(
tm
.
end
(),
p
.
second
);
for
(
double
i
=
5.0
;
i
<=
20.0
;
i
+=
1.0
)
{
tm
.
Push
(
i
,
i
);
}
p
=
tm
.
RangeOf
(
10.5
);
EXPECT_NE
(
tm
.
end
(),
p
.
first
);
EXPECT_NE
(
tm
.
end
(),
p
.
second
);
EXPECT_NEAR
(
10.0
,
p
.
first
->
first
,
1e-10
);
EXPECT_NEAR
(
11.0
,
p
.
second
->
first
,
1e-10
);
p
=
tm
.
RangeOf
(
3.0
);
EXPECT_EQ
(
tm
.
end
(),
p
.
first
);
EXPECT_NE
(
tm
.
end
(),
p
.
second
);
EXPECT_NEAR
(
5.0
,
p
.
second
->
first
,
1e-10
);
p
=
tm
.
RangeOf
(
30.0
);
EXPECT_NE
(
tm
.
end
(),
p
.
first
);
EXPECT_EQ
(
tm
.
end
(),
p
.
second
);
EXPECT_NEAR
(
20.0
,
p
.
first
->
first
,
1e-10
);
p
=
tm
.
RangeOf
(
5.0
);
EXPECT_EQ
(
tm
.
end
(),
p
.
first
);
EXPECT_NE
(
tm
.
end
(),
p
.
second
);
EXPECT_NEAR
(
5.0
,
p
.
second
->
first
,
1e-10
);
}
TEST
(
TimeMarkedListTest
,
Nearest
)
{
TimeMarkedList
<
double
>
tm
(
100.0
);
auto
it
=
tm
.
Nearest
(
10.0
);
EXPECT_EQ
(
tm
.
end
(),
it
);
for
(
auto
i
=
5.0
;
i
<=
20.0
;
i
+=
1.0
)
{
tm
.
Push
(
i
,
i
);
}
it
=
tm
.
Nearest
(
3.0
);
EXPECT_NE
(
tm
.
end
(),
it
);
EXPECT_NEAR
(
5.0
,
it
->
first
,
1e-10
);
it
=
tm
.
Nearest
(
21.0
);
EXPECT_NE
(
tm
.
end
(),
it
);
EXPECT_NEAR
(
20.0
,
it
->
first
,
1e-10
);
it
=
tm
.
Nearest
(
15.7
);
EXPECT_NE
(
tm
.
end
(),
it
);
EXPECT_NEAR
(
16.0
,
it
->
first
,
1e-10
);
}
}
// namespace localization
}
// namespace apollo
modules/localization/lmd/lmd_localization.cc
浏览文件 @
7461c419
/******************************************************************************
* Copyright 201
7
The Apollo Authors. All Rights Reserved.
* Copyright 201
8
The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
...
...
@@ -16,295 +16,33 @@
#include "modules/localization/lmd/lmd_localization.h"
#include <algorithm>
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/log.h"
#include "modules/common/math/euler_angles_zxy.h"
#include "modules/common/math/math_utils.h"
#include "modules/common/math/quaternion.h"
#include "modules/common/time/time.h"
#include "modules/common/util/thread_pool.h"
#include "modules/localization/common/localization_gflags.h"
#include "modules/localization/lmd/predictor/output/predictor_output.h"
#include "modules/localization/lmd/predictor/output/predictor_print_error.h"
#include "modules/localization/lmd/predictor/raw/predictor_gps.h"
#include "modules/localization/lmd/predictor/raw/predictor_imu.h"
namespace
apollo
{
namespace
localization
{
using
apollo
::
canbus
::
Chassis
;
using
apollo
::
common
::
Point3D
;
using
apollo
::
common
::
PointENU
;
using
apollo
::
common
::
Quaternion
;
using
apollo
::
common
::
ErrorCode
;
using
apollo
::
common
::
Status
;
using
apollo
::
common
::
adapter
::
AdapterManager
;
using
apollo
::
common
::
adapter
::
ChassisAdapter
;
using
apollo
::
common
::
adapter
::
GpsAdapter
;
using
apollo
::
common
::
adapter
::
ImuAdapter
;
using
apollo
::
common
::
math
::
HeadingToQuaternion
;
using
apollo
::
common
::
math
::
QuaternionRotate
;
using
apollo
::
common
::
math
::
QuaternionToHeading
;
using
apollo
::
common
::
math
::
RotateAxis
;
using
apollo
::
common
::
monitor
::
MonitorMessageItem
;
using
apollo
::
common
::
time
::
Clock
;
using
apollo
::
common
::
time
::
ToSecond
;
using
apollo
::
common
::
util
::
ThreadPool
;
using
apollo
::
perception
::
PerceptionObstacles
;
template
<
class
T
>
static
T
InterpolateXYZ
(
const
T
&
p1
,
const
T
&
p2
,
const
double
frac1
)
{
T
p
;
double
frac2
=
1.0
-
frac1
;
if
(
p1
.
has_x
()
&&
!
std
::
isnan
(
p1
.
x
())
&&
p2
.
has_x
()
&&
!
std
::
isnan
(
p2
.
x
()))
{
p
.
set_x
(
p1
.
x
()
*
frac2
+
p2
.
x
()
*
frac1
);
}
if
(
p1
.
has_y
()
&&
!
std
::
isnan
(
p1
.
y
())
&&
p2
.
has_y
()
&&
!
std
::
isnan
(
p2
.
y
()))
{
p
.
set_y
(
p1
.
y
()
*
frac2
+
p2
.
y
()
*
frac1
);
}
if
(
p1
.
has_z
()
&&
!
std
::
isnan
(
p1
.
z
())
&&
p2
.
has_z
()
&&
!
std
::
isnan
(
p2
.
z
()))
{
p
.
set_z
(
p1
.
z
()
*
frac2
+
p2
.
z
()
*
frac1
);
}
return
p
;
}
template
<
class
T
>
static
T
InterpolateXYZW
(
const
T
&
p1
,
const
T
&
p2
,
const
double
frac1
)
{
T
p
;
double
frac2
=
1.0
-
frac1
;
if
(
p1
.
has_qx
()
&&
!
std
::
isnan
(
p1
.
qx
())
&&
p2
.
has_qx
()
&&
!
std
::
isnan
(
p2
.
qx
()))
{
p
.
set_qx
(
p1
.
qx
()
*
frac2
+
p2
.
qx
()
*
frac1
);
}
if
(
p1
.
has_qy
()
&&
!
std
::
isnan
(
p1
.
qy
())
&&
p2
.
has_qy
()
&&
!
std
::
isnan
(
p2
.
qy
()))
{
p
.
set_qy
(
p1
.
qy
()
*
frac2
+
p2
.
qy
()
*
frac1
);
}
if
(
p1
.
has_qz
()
&&
!
std
::
isnan
(
p1
.
qz
())
&&
p2
.
has_qz
()
&&
!
std
::
isnan
(
p2
.
qz
()))
{
p
.
set_qz
(
p1
.
qz
()
*
frac2
+
p2
.
qz
()
*
frac1
);
}
if
(
p1
.
has_qw
()
&&
!
std
::
isnan
(
p1
.
qw
())
&&
p2
.
has_qw
()
&&
!
std
::
isnan
(
p2
.
qw
()))
{
p
.
set_qw
(
p1
.
qw
()
*
frac2
+
p2
.
qw
()
*
frac1
);
}
return
p
;
}
static
bool
InterpolateGPS
(
const
Gps
&
gps1
,
const
Gps
&
gps2
,
const
double
timestamp_sec
,
Gps
*
gps_msg
)
{
if
(
!
(
gps1
.
has_header
()
&&
gps1
.
header
().
has_timestamp_sec
()
&&
gps2
.
has_header
()
&&
gps2
.
header
().
has_timestamp_sec
()
&&
gps1
.
has_localization
()
&&
gps2
.
has_localization
()))
{
AERROR
<<
"gps1 and gps2 has no header or no some fields"
;
return
false
;
}
if
(
timestamp_sec
-
gps1
.
header
().
timestamp_sec
()
<
FLAGS_timestamp_sec_tolerance
)
{
AERROR
<<
"[InterpolateGPS]: the given time stamp["
<<
timestamp_sec
<<
"] is older than the 1st message["
<<
gps1
.
header
().
timestamp_sec
()
<<
"]"
;
*
gps_msg
=
gps1
;
}
else
if
(
timestamp_sec
-
gps2
.
header
().
timestamp_sec
()
>
FLAGS_timestamp_sec_tolerance
)
{
AERROR
<<
"[InterpolateGPS]: the given time stamp["
<<
timestamp_sec
<<
"] is newer than the 2nd message["
<<
gps2
.
header
().
timestamp_sec
()
<<
"]"
;
*
gps_msg
=
gps1
;
}
else
{
*
gps_msg
=
gps1
;
gps_msg
->
mutable_header
()
->
set_timestamp_sec
(
timestamp_sec
);
double
time_diff
=
gps2
.
header
().
timestamp_sec
()
-
gps1
.
header
().
timestamp_sec
();
if
(
fabs
(
time_diff
)
>=
0.001
)
{
double
frac1
=
(
timestamp_sec
-
gps1
.
header
().
timestamp_sec
())
/
time_diff
;
const
auto
&
gps1_pose
=
gps1
.
localization
();
const
auto
&
gps2_pose
=
gps2
.
localization
();
if
(
gps1_pose
.
has_position
()
&&
gps2_pose
.
has_position
())
{
auto
val
=
InterpolateXYZ
(
gps1_pose
.
position
(),
gps2_pose
.
position
(),
frac1
);
gps_msg
->
mutable_localization
()
->
mutable_position
()
->
CopyFrom
(
val
);
}
if
(
gps1_pose
.
has_orientation
()
&&
gps2_pose
.
has_orientation
())
{
auto
val
=
InterpolateXYZW
(
gps1_pose
.
orientation
(),
gps2_pose
.
orientation
(),
frac1
);
gps_msg
->
mutable_localization
()
->
mutable_orientation
()
->
CopyFrom
(
val
);
}
if
(
gps1_pose
.
has_linear_velocity
()
&&
gps2_pose
.
has_linear_velocity
())
{
auto
val
=
InterpolateXYZ
(
gps1_pose
.
linear_velocity
(),
gps2_pose
.
linear_velocity
(),
frac1
);
gps_msg
->
mutable_localization
()
->
mutable_linear_velocity
()
->
CopyFrom
(
val
);
}
}
}
return
true
;
}
static
bool
InterpolateIMU
(
const
CorrectedImu
&
imu1
,
const
CorrectedImu
&
imu2
,
const
double
timestamp_sec
,
CorrectedImu
*
imu_msg
)
{
if
(
!
(
imu1
.
has_header
()
&&
imu1
.
header
().
has_timestamp_sec
()
&&
imu2
.
has_header
()
&&
imu2
.
header
().
has_timestamp_sec
()))
{
AERROR
<<
"imu1 and imu2 has no header or no timestamp_sec in header"
;
return
false
;
}
if
(
timestamp_sec
-
imu1
.
header
().
timestamp_sec
()
<
FLAGS_timestamp_sec_tolerance
)
{
AERROR
<<
"[InterpolateIMU]: the given time stamp["
<<
timestamp_sec
<<
"] is older than the 1st message["
<<
imu1
.
header
().
timestamp_sec
()
<<
"]"
;
*
imu_msg
=
imu1
;
}
else
if
(
timestamp_sec
-
imu2
.
header
().
timestamp_sec
()
>
FLAGS_timestamp_sec_tolerance
)
{
AERROR
<<
"[InterpolateIMU]: the given time stamp["
<<
timestamp_sec
<<
"] is newer than the 2nd message["
<<
imu2
.
header
().
timestamp_sec
()
<<
"]"
;
*
imu_msg
=
imu1
;
}
else
{
*
imu_msg
=
imu1
;
imu_msg
->
mutable_header
()
->
set_timestamp_sec
(
timestamp_sec
);
double
time_diff
=
imu2
.
header
().
timestamp_sec
()
-
imu1
.
header
().
timestamp_sec
();
if
(
fabs
(
time_diff
)
>=
0.001
)
{
double
frac1
=
(
timestamp_sec
-
imu1
.
header
().
timestamp_sec
())
/
time_diff
;
if
(
imu1
.
has_imu
()
&&
imu1
.
imu
().
has_angular_velocity
()
&&
imu2
.
has_imu
()
&&
imu2
.
imu
().
has_angular_velocity
())
{
auto
val
=
InterpolateXYZ
(
imu1
.
imu
().
angular_velocity
(),
imu2
.
imu
().
angular_velocity
(),
frac1
);
imu_msg
->
mutable_imu
()
->
mutable_angular_velocity
()
->
CopyFrom
(
val
);
}
if
(
imu1
.
has_imu
()
&&
imu1
.
imu
().
has_linear_acceleration
()
&&
imu2
.
has_imu
()
&&
imu2
.
imu
().
has_linear_acceleration
())
{
auto
val
=
InterpolateXYZ
(
imu1
.
imu
().
linear_acceleration
(),
imu2
.
imu
().
linear_acceleration
(),
frac1
);
imu_msg
->
mutable_imu
()
->
mutable_linear_acceleration
()
->
CopyFrom
(
val
);
}
if
(
imu1
.
has_imu
()
&&
imu1
.
imu
().
has_euler_angles
()
&&
imu2
.
has_imu
()
&&
imu2
.
imu
().
has_euler_angles
())
{
auto
val
=
InterpolateXYZ
(
imu1
.
imu
().
euler_angles
(),
imu2
.
imu
().
euler_angles
(),
frac1
);
imu_msg
->
mutable_imu
()
->
mutable_euler_angles
()
->
CopyFrom
(
val
);
}
}
}
return
true
;
}
static
bool
InterpolateChassis
(
const
Chassis
&
chassis1
,
const
Chassis
&
chassis2
,
const
double
timestamp_sec
,
Chassis
*
chassis_msg
)
{
if
(
!
(
chassis1
.
has_header
()
&&
chassis1
.
header
().
has_timestamp_sec
()
&&
chassis2
.
has_header
()
&&
chassis2
.
header
().
has_timestamp_sec
()))
{
AERROR
<<
"chassis1 and chassis2 has no header or no timestamp_sec in header"
;
return
false
;
}
if
(
timestamp_sec
-
chassis1
.
header
().
timestamp_sec
()
<
FLAGS_timestamp_sec_tolerance
)
{
AERROR
<<
"[InterpolateChassis]: the given time stamp["
<<
timestamp_sec
<<
"] is older than the 1st message["
<<
chassis1
.
header
().
timestamp_sec
()
<<
"]"
;
*
chassis_msg
=
chassis1
;
}
else
if
(
timestamp_sec
-
chassis2
.
header
().
timestamp_sec
()
>
FLAGS_timestamp_sec_tolerance
)
{
AERROR
<<
"[InterpolateChassis]: the given time stamp["
<<
timestamp_sec
<<
"] is newer than the 2nd message["
<<
chassis2
.
header
().
timestamp_sec
()
<<
"]"
;
*
chassis_msg
=
chassis1
;
}
else
{
*
chassis_msg
=
chassis1
;
chassis_msg
->
mutable_header
()
->
set_timestamp_sec
(
timestamp_sec
);
double
time_diff
=
chassis2
.
header
().
timestamp_sec
()
-
chassis1
.
header
().
timestamp_sec
();
if
(
fabs
(
time_diff
)
>=
0.001
)
{
double
frac1
=
(
timestamp_sec
-
chassis1
.
header
().
timestamp_sec
())
/
time_diff
;
double
frac2
=
1.0
-
frac1
;
if
(
chassis1
.
has_speed_mps
())
{
float
val
=
0.0
;
if
(
!
std
::
isnan
(
chassis1
.
speed_mps
())
&&
!
std
::
isnan
(
chassis2
.
speed_mps
()))
{
val
=
chassis1
.
speed_mps
()
*
frac2
+
chassis2
.
speed_mps
()
*
frac1
;
}
chassis_msg
->
set_speed_mps
(
val
);
}
if
(
chassis1
.
has_odometer_m
())
{
float
val
=
0.0
;
if
(
!
std
::
isnan
(
chassis1
.
odometer_m
())
&&
!
std
::
isnan
(
chassis2
.
odometer_m
()))
{
val
=
chassis1
.
odometer_m
()
*
frac2
+
chassis2
.
odometer_m
()
*
frac1
;
}
chassis_msg
->
set_odometer_m
(
val
);
}
}
}
return
true
;
}
static
void
FillPoseFromImu
(
const
Pose
&
imu
,
Pose
*
pose
)
{
// linear acceleration
if
(
imu
.
has_linear_acceleration
())
{
if
(
FLAGS_enable_map_reference_unify
)
{
if
(
pose
->
has_orientation
())
{
// linear_acceleration:
// convert from vehicle reference to map reference
Eigen
::
Vector3d
orig
(
imu
.
linear_acceleration
().
x
(),
imu
.
linear_acceleration
().
y
(),
imu
.
linear_acceleration
().
z
());
auto
vec
=
QuaternionRotate
(
pose
->
orientation
(),
orig
);
pose
->
mutable_linear_acceleration
()
->
set_x
(
vec
[
0
]);
pose
->
mutable_linear_acceleration
()
->
set_y
(
vec
[
1
]);
pose
->
mutable_linear_acceleration
()
->
set_z
(
vec
[
2
]);
// linear_acceleration_vrf
pose
->
mutable_linear_acceleration_vrf
()
->
CopyFrom
(
imu
.
linear_acceleration
());
}
else
{
AERROR
<<
"[PrepareLocalizationMsg]: "
<<
"fail to convert linear_acceleration"
;
}
}
else
{
pose
->
mutable_linear_acceleration
()
->
CopyFrom
(
imu
.
linear_acceleration
());
}
}
// angular velocity
if
(
imu
.
has_angular_velocity
())
{
if
(
FLAGS_enable_map_reference_unify
)
{
if
(
pose
->
has_orientation
())
{
// angular_velocity:
// convert from vehicle reference to map reference
Eigen
::
Vector3d
orig
(
imu
.
angular_velocity
().
x
(),
imu
.
angular_velocity
().
y
(),
imu
.
angular_velocity
().
z
());
auto
vec
=
QuaternionRotate
(
pose
->
orientation
(),
orig
);
pose
->
mutable_angular_velocity
()
->
set_x
(
vec
[
0
]);
pose
->
mutable_angular_velocity
()
->
set_y
(
vec
[
1
]);
pose
->
mutable_angular_velocity
()
->
set_z
(
vec
[
2
]);
// angular_velocity_vrf
pose
->
mutable_angular_velocity_vrf
()
->
CopyFrom
(
imu
.
angular_velocity
());
}
else
{
AERROR
<<
"[PrepareLocalizationMsg]: "
<<
"fail to convert angular_velocity"
;
}
}
else
{
pose
->
mutable_angular_velocity
()
->
CopyFrom
(
imu
.
angular_velocity
());
}
}
// euler angle
if
(
imu
.
has_euler_angles
())
pose
->
mutable_euler_angles
()
->
CopyFrom
(
imu
.
euler_angles
());
}
namespace
{
constexpr
double
kDefaultMemoryCycle
=
2.0
;
constexpr
int
kDefaultThreadPoolSize
=
2
;
}
// namespace
LMDLocalization
::
LMDLocalization
()
:
monitor_logger_
(
MonitorMessageItem
::
LOCALIZATION
),
...
...
@@ -313,12 +51,61 @@ LMDLocalization::LMDLocalization()
LMDLocalization
::~
LMDLocalization
()
{}
Status
LMDLocalization
::
Start
()
{
auto
publish_func
=
[
&
](
double
timestamp_sec
,
const
Pose
&
pose
)
{
// prepare localization message
LocalizationEstimate
localization
;
AdapterManager
::
FillLocalizationHeader
(
FLAGS_localization_module_name
,
&
localization
);
localization
.
set_measurement_time
(
timestamp_sec
);
localization
.
mutable_pose
()
->
CopyFrom
(
pose
);
localization
.
mutable_pose
()
->
mutable_position
()
->
set_x
(
pose
.
position
().
x
()
-
map_offset_
[
0
]);
localization
.
mutable_pose
()
->
mutable_position
()
->
set_y
(
pose
.
position
().
y
()
-
map_offset_
[
1
]);
localization
.
mutable_pose
()
->
mutable_position
()
->
set_z
(
pose
.
position
().
z
()
-
map_offset_
[
2
]);
// publish localization messages
AdapterManager
::
PublishLocalization
(
localization
);
PublishPoseBroadcastTF
(
localization
);
return
Status
::
OK
();
};
// initialize predictors
Predictor
*
predictor
=
new
PredictorGps
(
kDefaultMemoryCycle
);
predictors_
.
emplace
(
predictor
->
Name
(),
PredictorHandler
(
predictor
));
gps_
=
&
predictors_
[
predictor
->
Name
()];
predictor
=
new
PredictorImu
(
kDefaultMemoryCycle
);
predictors_
.
emplace
(
predictor
->
Name
(),
PredictorHandler
(
predictor
));
imu_
=
&
predictors_
[
predictor
->
Name
()];
predictor
=
new
PredictorOutput
(
kDefaultMemoryCycle
,
publish_func
);
predictors_
.
emplace
(
predictor
->
Name
(),
PredictorHandler
(
predictor
));
output_
=
&
predictors_
[
predictor
->
Name
()];
predictor
=
new
PredictorPrintError
(
kDefaultMemoryCycle
);
predictors_
.
emplace
(
predictor
->
Name
(),
PredictorHandler
(
predictor
));
// check predictors' dep
for
(
const
auto
&
p
:
predictors_
)
{
auto
&
ph
=
p
.
second
;
for
(
const
auto
&
dep_p
:
ph
.
predictor
->
DepPredicteds
())
{
if
(
predictors_
.
find
(
dep_p
.
first
)
==
predictors_
.
end
())
{
AERROR
<<
"Can not find predictor["
<<
dep_p
.
first
<<
"]"
;
predictors_
.
clear
();
return
Status
(
ErrorCode
::
LOCALIZATION_ERROR
,
"Can not find predictor with name [
\"
"
+
dep_p
.
first
+
"
\"
]"
);
}
}
}
// initialize thread pool
ThreadPool
::
Init
(
kDefaultThreadPoolSize
);
// initialize adapter manager
AdapterManager
::
Init
(
FLAGS_lmd_adapter_config_file
);
// GPS
AdapterManager
::
AddImuCallback
(
&
LMDLocalization
::
OnImu
,
this
);
AdapterManager
::
AddGpsCallback
(
&
LMDLocalization
::
OnGps
,
this
);
// Perception Obstacles
AdapterManager
::
AddChassisCallback
(
&
LMDLocalization
::
OnChassis
,
this
);
AdapterManager
::
AddPerceptionObstaclesCallback
(
&
LMDLocalization
::
OnPerceptionObstacles
,
this
);
...
...
@@ -334,581 +121,103 @@ Status LMDLocalization::Start() {
Status
LMDLocalization
::
Stop
()
{
timer_
.
stop
();
ThreadPool
::
Stop
();
return
Status
::
OK
();
}
void
LMDLocalization
::
OnGps
(
const
Gps
&
gps
)
{
// take a snapshot of the current received messages
AdapterManager
::
Observe
();
// TODO(all): only for init
if
(
has_last_pose_
)
return
;
Pose
new_pose
;
double
new_timestamp_sec
;
if
(
!
GetGpsPose
(
gps
,
&
new_pose
,
&
new_timestamp_sec
))
return
;
if
(
!
has_last_pose_
||
last_pose_timestamp_sec_
<
new_timestamp_sec
)
{
if
(
!
has_last_pose_
)
AINFO
<<
"initialize pose"
;
void
LMDLocalization
::
OnImu
(
const
CorrectedImu
&
imu
)
{
if
(
!
imu_
->
Busy
())
{
// update messages
auto
*
predictor
=
static_cast
<
PredictorImu
*>
(
imu_
->
predictor
.
get
());
for
(
const
auto
&
imu
:
imu_list_
)
{
predictor
->
UpdateImu
(
imu
);
}
imu_list_
.
clear
();
predictor
->
UpdateImu
(
imu
);
has_last_pose_
=
true
;
last_pose_
.
CopyFrom
(
new_pose
);
last_pose_timestamp_sec_
=
new_timestamp_sec
;
// predicting
Predicting
();
}
else
{
imu_list_
.
emplace_back
(
imu
);
}
}
void
LMDLocalization
::
On
PerceptionObstacles
(
const
PerceptionObstacles
&
obstacles
)
{
if
(
has_last_pose_
&&
obstacles
.
has_lane_marker
())
{
if
(
!
obstacles
.
has_header
()
||
!
obstacles
.
header
().
has_timestamp_sec
())
{
AERROR
<<
"obstacles has no header or no some fields"
;
return
;
void
LMDLocalization
::
On
Gps
(
const
Gps
&
gps
)
{
if
(
!
gps_
->
Busy
()
)
{
// update messages
auto
*
predictor
=
static_cast
<
PredictorGps
*>
(
gps_
->
predictor
.
get
());
for
(
const
auto
&
gps
:
gps_list_
)
{
predictor
->
UpdateGps
(
gps
)
;
}
gps_list_
.
clear
();
predictor
->
UpdateGps
(
gps
);
// take a snapshot of the current received messages
AdapterManager
::
Observe
();
auto
timestamp_sec
=
obstacles
.
header
().
timestamp_sec
();
if
(
last_pose_timestamp_sec_
>
timestamp_sec
)
return
;
// predict pose
Pose
new_pose
;
if
(
!
PredictPose
(
last_pose_
,
last_pose_timestamp_sec_
,
timestamp_sec
,
&
new_pose
))
{
AERROR
<<
"predict pose failed"
;
return
;
}
const
auto
&
position_estimated
=
new_pose
.
position
();
auto
heading_estimated
=
new_pose
.
heading
();
// TODO(all): add registration with perception
auto
position
=
position_estimated
;
auto
heading
=
heading_estimated
;
// position
// world frame -> map frame
new_pose
.
mutable_position
()
->
set_x
(
position
.
x
()
-
map_offset_
[
0
]);
new_pose
.
mutable_position
()
->
set_y
(
position
.
y
()
-
map_offset_
[
1
]);
new_pose
.
mutable_position
()
->
set_z
(
position
.
z
()
-
map_offset_
[
2
]);
// TODO(all): heading to orientation
// orientation
new_pose
.
set_heading
(
heading
);
auto
orientation
=
HeadingToQuaternion
(
heading
);
new_pose
.
mutable_orientation
()
->
set_qx
(
orientation
.
x
());
new_pose
.
mutable_orientation
()
->
set_qy
(
orientation
.
y
());
new_pose
.
mutable_orientation
()
->
set_qz
(
orientation
.
z
());
new_pose
.
mutable_orientation
()
->
set_qw
(
orientation
.
w
());
ADEBUG
<<
"before pc registration"
;
PrintPoseError
(
last_pose_
,
last_pose_timestamp_sec_
);
last_pose_
.
CopyFrom
(
new_pose
);
last_pose_timestamp_sec_
=
timestamp_sec
;
ADEBUG
<<
"after pc registration"
;
PrintPoseError
(
last_pose_
,
last_pose_timestamp_sec_
);
// predicting
Predicting
();
}
else
{
gps_list_
.
emplace_back
(
gps
);
}
}
void
LMDLocalization
::
OnTimer
(
const
ros
::
TimerEvent
&
event
)
{
if
(
!
has_last_pose_
)
return
;
void
LMDLocalization
::
OnChassis
(
const
Chassis
&
chassis
)
{}
void
LMDLocalization
::
OnPerceptionObstacles
(
const
PerceptionObstacles
&
obstacles
)
{}
void
LMDLocalization
::
OnTimer
(
const
ros
::
TimerEvent
&
event
)
{
// take a snapshot of the current received messages
AdapterManager
::
Observe
();
// Prepare localization message.
LocalizationEstimate
localization
;
PrepareLocalizationMsg
(
&
localization
);
// publish localization messages
AdapterManager
::
PublishLocalization
(
localization
);
PublishPoseBroadcastTF
(
localization
);
ADEBUG
<<
"[OnTimer]: Localization message publish success!"
;
// predicting
Predicting
();
// watch dog
RunWatchDog
();
}
void
LMDLocalization
::
PrepareLocalizationMsg
(
LocalizationEstimate
*
localization
)
{
localization
->
Clear
();
// header
AdapterManager
::
FillLocalizationHeader
(
FLAGS_localization_module_name
,
localization
);
// set timestamp
auto
*
imu_adapter
=
AdapterManager
::
GetImu
();
if
(
imu_adapter
->
Empty
())
{
AERROR
<<
"IMU message Queue is empty!"
;
return
;
}
auto
imu_msg
=
imu_adapter
->
GetLatestObserved
();
if
(
!
imu_msg
.
has_header
()
||
!
imu_msg
.
header
().
has_timestamp_sec
())
{
AERROR
<<
"imu has no header or no timestamp_sec in header"
;
return
;
}
auto
timestamp_sec
=
std
::
max
(
imu_msg
.
header
().
timestamp_sec
(),
last_pose_timestamp_sec_
);
// predict pose
Pose
new_pose
;
if
(
!
PredictPose
(
last_pose_
,
last_pose_timestamp_sec_
,
timestamp_sec
,
&
new_pose
))
{
AERROR
<<
"predict pose failed"
;
return
;
}
last_pose_
=
new_pose
;
last_pose_timestamp_sec_
=
timestamp_sec
;
// measurement_time
localization
->
set_measurement_time
(
last_pose_timestamp_sec_
);
// pose
localization
->
mutable_pose
()
->
CopyFrom
(
last_pose_
);
}
bool
LMDLocalization
::
GetGpsPose
(
const
Gps
&
gps
,
Pose
*
pose
,
double
*
timestamp_sec
)
{
if
(
!
gps
.
has_header
()
||
!
gps
.
header
().
has_timestamp_sec
()
||
!
gps
.
has_localization
()
||
!
gps
.
localization
().
has_position
())
{
AERROR
<<
"gps has no header or no some fields"
;
return
false
;
}
*
timestamp_sec
=
gps
.
header
().
timestamp_sec
();
const
auto
&
gps_pose
=
gps
.
localization
();
// position
// world frame -> map frame
pose
->
mutable_position
()
->
set_x
(
gps_pose
.
position
().
x
()
-
map_offset_
[
0
]);
pose
->
mutable_position
()
->
set_y
(
gps_pose
.
position
().
y
()
-
map_offset_
[
1
]);
pose
->
mutable_position
()
->
set_z
(
gps_pose
.
position
().
z
()
-
map_offset_
[
2
]);
// orientation
if
(
gps_pose
.
has_orientation
())
{
pose
->
mutable_orientation
()
->
CopyFrom
(
gps_pose
.
orientation
());
auto
heading
=
QuaternionToHeading
(
gps_pose
.
orientation
().
qw
(),
gps_pose
.
orientation
().
qx
(),
gps_pose
.
orientation
().
qy
(),
gps_pose
.
orientation
().
qz
());
pose
->
set_heading
(
heading
);
}
// linear velocity
if
(
gps_pose
.
has_linear_velocity
())
pose
->
mutable_linear_velocity
()
->
CopyFrom
(
gps_pose
.
linear_velocity
());
// IMU
CorrectedImu
imu_msg
;
if
(
!
FindMatchingIMU
(
*
timestamp_sec
,
&
imu_msg
))
return
false
;
CHECK
(
imu_msg
.
has_imu
());
const
auto
&
imu
=
imu_msg
.
imu
();
FillPoseFromImu
(
imu
,
pose
);
return
true
;
}
bool
LMDLocalization
::
PredictPose
(
const
Pose
&
old_pose
,
double
old_timestamp_sec
,
double
new_timestamp_sec
,
Pose
*
new_pose
)
{
return
PredictByLinearIntergrate
(
old_pose
,
old_timestamp_sec
,
new_timestamp_sec
,
new_pose
);
}
bool
LMDLocalization
::
FindMatchingGPS
(
double
timestamp_sec
,
Gps
*
gps_msg
)
{
auto
*
gps_adapter
=
AdapterManager
::
GetGps
();
if
(
gps_adapter
->
Empty
())
{
AERROR
<<
"Cannot find Matching GPS. "
<<
"GPS message Queue is empty! timestamp["
<<
timestamp_sec
<<
"]"
;
return
false
;
}
// scan gps buffer, find first gps message that is newer than the given
// timestamp
GpsAdapter
::
Iterator
gps_it
=
gps_adapter
->
begin
();
for
(;
gps_it
!=
gps_adapter
->
end
();
++
gps_it
)
{
if
((
*
gps_it
)
->
header
().
timestamp_sec
()
-
timestamp_sec
>
FLAGS_timestamp_sec_tolerance
)
{
break
;
}
}
if
(
gps_it
!=
gps_adapter
->
end
())
{
// found one
if
(
gps_it
==
gps_adapter
->
begin
())
{
AERROR
<<
"Gps queue too short or request too old. "
<<
"Oldest timestamp["
<<
gps_adapter
->
GetOldestObserved
().
header
().
timestamp_sec
()
<<
"], Newest timestamp["
<<
gps_adapter
->
GetLatestObserved
().
header
().
timestamp_sec
()
<<
"], timestamp["
<<
timestamp_sec
<<
"]"
;
*
gps_msg
=
gps_adapter
->
GetOldestObserved
();
// the oldest gps
}
else
{
// here is the normal case
auto
gps_it_1
=
gps_it
;
gps_it_1
--
;
if
(
!
(
*
gps_it
)
->
has_header
()
||
!
(
*
gps_it_1
)
->
has_header
())
{
AERROR
<<
"gps1 and gps_it_1 must both have header."
;
return
false
;
}
if
(
!
InterpolateGPS
(
**
gps_it_1
,
**
gps_it
,
timestamp_sec
,
gps_msg
))
{
AERROR
<<
"failed to interpolate GPS"
;
return
false
;
}
}
}
else
{
// give the newest gps, without extrapolation
*
gps_msg
=
gps_adapter
->
GetLatestObserved
();
if
(
gps_msg
==
nullptr
)
{
AERROR
<<
"Fail to get latest observed gps_msg."
;
return
false
;
}
if
(
!
gps_msg
->
has_header
())
{
AERROR
<<
"gps_msg must have header."
;
return
false
;
}
}
return
true
;
}
// namespace localization
bool
LMDLocalization
::
FindMatchingIMU
(
double
timestamp_sec
,
CorrectedImu
*
imu_msg
)
{
auto
*
imu_adapter
=
AdapterManager
::
GetImu
();
if
(
imu_adapter
->
Empty
())
{
AERROR
<<
"Cannot find Matching IMU. "
<<
"IMU message Queue is empty! timestamp["
<<
timestamp_sec
<<
"]"
;
return
false
;
}
// scan imu buffer, find first imu message that is newer than the given
// timestamp
ImuAdapter
::
Iterator
imu_it
=
imu_adapter
->
begin
();
for
(;
imu_it
!=
imu_adapter
->
end
();
++
imu_it
)
{
if
((
*
imu_it
)
->
header
().
timestamp_sec
()
-
timestamp_sec
>
FLAGS_timestamp_sec_tolerance
)
{
break
;
}
}
if
(
imu_it
!=
imu_adapter
->
end
())
{
// found one
if
(
imu_it
==
imu_adapter
->
begin
())
{
AERROR
<<
"IMU queue too short or request too old. "
<<
"Oldest timestamp["
<<
imu_adapter
->
GetOldestObserved
().
header
().
timestamp_sec
()
<<
"], Newest timestamp["
<<
imu_adapter
->
GetLatestObserved
().
header
().
timestamp_sec
()
<<
"], timestamp["
<<
timestamp_sec
<<
"]"
;
*
imu_msg
=
imu_adapter
->
GetOldestObserved
();
// the oldest imu
}
else
{
// here is the normal case
auto
imu_it_1
=
imu_it
;
imu_it_1
--
;
if
(
!
(
*
imu_it
)
->
has_header
()
||
!
(
*
imu_it_1
)
->
has_header
())
{
AERROR
<<
"imu1 and imu_it_1 must both have header."
;
return
false
;
}
if
(
!
InterpolateIMU
(
**
imu_it_1
,
**
imu_it
,
timestamp_sec
,
imu_msg
))
{
AERROR
<<
"failed to interpolate IMU"
;
return
false
;
}
}
}
else
{
// give the newest imu, without extrapolation
*
imu_msg
=
imu_adapter
->
GetLatestObserved
();
if
(
imu_msg
==
nullptr
)
{
AERROR
<<
"Fail to get latest observed imu_msg."
;
return
false
;
}
if
(
!
imu_msg
->
has_header
())
{
AERROR
<<
"imu_msg must have header."
;
return
false
;
}
}
return
true
;
}
bool
LMDLocalization
::
FindMatchingChassis
(
double
timestamp_sec
,
Chassis
*
chassis_msg
)
{
auto
*
chassis_adapter
=
AdapterManager
::
GetChassis
();
if
(
chassis_adapter
->
Empty
())
{
AERROR
<<
"Cannot find Matching Chassis. "
<<
"Chassis message Queue is empty! timestamp["
<<
timestamp_sec
<<
"]"
;
return
false
;
}
// scan chassis buffer, find first chassis message that is newer than the
// given timestamp
ChassisAdapter
::
Iterator
chassis_it
=
chassis_adapter
->
begin
();
for
(;
chassis_it
!=
chassis_adapter
->
end
();
++
chassis_it
)
{
if
((
*
chassis_it
)
->
header
().
timestamp_sec
()
-
timestamp_sec
>
FLAGS_timestamp_sec_tolerance
)
{
break
;
}
}
if
(
chassis_it
!=
chassis_adapter
->
end
())
{
// found one
if
(
chassis_it
==
chassis_adapter
->
begin
())
{
AERROR
<<
"Chassis queue too short or request too old. "
<<
"Oldest timestamp["
<<
chassis_adapter
->
GetOldestObserved
().
header
().
timestamp_sec
()
<<
"], Newest timestamp["
<<
chassis_adapter
->
GetLatestObserved
().
header
().
timestamp_sec
()
<<
"], timestamp["
<<
timestamp_sec
<<
"]"
;
*
chassis_msg
=
chassis_adapter
->
GetOldestObserved
();
// the oldest chassis
}
else
{
// here is the normal case
auto
chassis_it_1
=
chassis_it
;
chassis_it_1
--
;
if
(
!
(
*
chassis_it
)
->
has_header
()
||
!
(
*
chassis_it_1
)
->
has_header
())
{
AERROR
<<
"chassis_it and chassis_it_1 must both have header."
;
return
false
;
}
if
(
!
InterpolateChassis
(
**
chassis_it_1
,
**
chassis_it
,
timestamp_sec
,
chassis_msg
))
{
AERROR
<<
"failed to interpolate IMU"
;
return
false
;
void
LMDLocalization
::
Predicting
()
{
bool
finish
=
false
;
while
(
!
finish
)
{
finish
=
true
;
// update predicteds from deps
for
(
auto
&
p
:
predictors_
)
{
auto
&
ph
=
p
.
second
;
if
(
!
ph
.
Busy
())
{
for
(
const
auto
&
dep_p
:
ph
.
predictor
->
DepPredicteds
())
{
const
auto
&
dep_name
=
dep_p
.
first
;
const
auto
&
dep_ph
=
predictors_
[
dep_name
];
if
(
dep_ph
.
Busy
())
{
continue
;
}
const
auto
&
dep_predicted
=
dep_p
.
second
;
if
(
dep_predicted
.
empty
()
||
dep_predicted
.
Older
(
dep_ph
.
predictor
->
Predicted
()))
{
ph
.
predictor
->
UpdateDepPredicted
(
dep_name
,
dep_ph
.
predictor
->
Predicted
());
}
}
}
}
}
else
{
// give the newest imu, without extrapolation
*
chassis_msg
=
chassis_adapter
->
GetLatestObserved
();
if
(
chassis_msg
==
nullptr
)
{
AERROR
<<
"Fail to get latest observed chassis_msg."
;
return
false
;
}
if
(
!
chassis_msg
->
has_header
())
{
AERROR
<<
"chassis_msg must have header."
;
return
false
;
}
}
return
true
;
}
bool
LMDLocalization
::
PredictByLinearIntergrate
(
const
Pose
&
old_pose
,
double
old_timestamp_sec
,
double
new_timestamp_sec
,
Pose
*
new_pose
)
{
if
(
!
old_pose
.
has_position
()
||
!
old_pose
.
has_orientation
()
||
!
old_pose
.
has_linear_velocity
())
{
AERROR
<<
"old_pose has no some fields"
;
return
false
;
}
auto
*
imu_adapter
=
AdapterManager
::
GetImu
();
if
(
imu_adapter
->
Empty
())
{
AERROR
<<
"Cannot find Matching IMU. "
<<
"IMU message Queue is empty!"
;
return
false
;
}
auto
imu_it
=
imu_adapter
->
end
();
do
{
imu_it
--
;
if
((
*
imu_it
)
->
header
().
timestamp_sec
()
-
old_timestamp_sec
>
FLAGS_timestamp_sec_tolerance
)
break
;
}
while
(
imu_it
!=
imu_adapter
->
begin
());
imu_it
++
;
if
(
imu_it
==
imu_adapter
->
end
())
{
AERROR
<<
std
::
setprecision
(
15
)
<<
"IMU queue too short or request too old. "
<<
"Oldest timestamp["
<<
imu_adapter
->
GetOldestObserved
().
header
().
timestamp_sec
()
<<
"], Newest timestamp["
<<
imu_adapter
->
GetLatestObserved
().
header
().
timestamp_sec
()
<<
"], timestamp["
<<
old_timestamp_sec
<<
"]"
;
return
false
;
}
CorrectedImu
imu
;
if
(
imu_it
!=
imu_adapter
->
begin
())
{
auto
imu_it_1
=
imu_it
;
imu_it_1
--
;
if
(
!
(
*
imu_it
)
->
has_header
()
||
!
(
*
imu_it_1
)
->
has_header
()
||
!
(
*
imu_it
)
->
has_imu
()
||
!
(
*
imu_it_1
)
->
has_imu
())
{
AERROR
<<
"imu_it and imu_it_1 must both have header or some fields."
;
return
false
;
}
if
(
!
InterpolateIMU
(
**
imu_it
,
**
imu_it_1
,
old_timestamp_sec
,
&
imu
))
{
AERROR
<<
"failed to interpolate IMU"
;
return
false
;
}
}
else
{
imu
.
CopyFrom
(
**
imu_it
);
}
new_pose
->
CopyFrom
(
old_pose
);
auto
timestamp_sec
=
old_timestamp_sec
;
bool
finished
=
false
;
while
(
!
finished
)
{
CorrectedImu
imu_1
;
double
timestamp_sec_1
;
auto
imu_it_1
=
imu_it
;
if
(
imu_it_1
!=
imu_adapter
->
begin
())
{
imu_it_1
--
;
if
(
!
(
*
imu_it_1
)
->
has_header
()
||
!
(
*
imu_it_1
)
->
header
().
has_timestamp_sec
())
{
AERROR
<<
"imu_it_1 must have header and timestamp_sec."
;
return
false
;
}
// predict
for
(
auto
&
p
:
predictors_
)
{
auto
&
ph
=
p
.
second
;
if
(
!
ph
.
Busy
()
&&
ph
.
predictor
->
Updateable
())
{
finish
=
false
;
timestamp_sec_1
=
(
*
imu_it_1
)
->
header
().
timestamp_sec
();
if
(
timestamp_sec_1
<
new_timestamp_sec
)
{
imu_1
.
CopyFrom
(
**
imu_it_1
);
}
else
{
timestamp_sec_1
=
new_timestamp_sec
;
if
(
!
InterpolateIMU
(
**
imu_it
,
**
imu_it_1
,
timestamp_sec_1
,
&
imu_1
))
{
AERROR
<<
"failed to interpolate IMU"
;
return
false
;
if
(
ph
.
predictor
->
UpdatingOnAdapterThread
())
{
ph
.
predictor
->
Update
();
}
else
{
auto
predictor
=
ph
.
predictor
;
ph
.
fut
=
ThreadPool
::
pool
()
->
push
([
=
](
int
thread_id
)
mutable
->
Status
{
return
predictor
->
Update
()
;
})
;
}
finished
=
true
;
}
}
else
{
timestamp_sec_1
=
new_timestamp_sec
;
imu_1
.
CopyFrom
(
imu
);
finished
=
true
;
}
if
(
!
imu
.
has_imu
()
||
!
imu_1
.
has_imu
()
||
!
imu
.
imu
().
has_linear_acceleration
()
||
!
imu_1
.
imu
().
has_linear_acceleration
()
||
!
imu
.
imu
().
has_angular_velocity
()
||
!
imu_1
.
imu
().
has_angular_velocity
())
{
AERROR
<<
"imu or imu1 has no some fields"
;
return
false
;
}
// auto angular_velocity = imu.imu().angular_velocity();
// auto angular_velocity1 = imu_1.imu().angular_velocity();
auto
orientation
=
new_pose
->
orientation
();
auto
orientation_1
=
orientation
;
Point3D
linear_acceleration
;
if
(
FLAGS_enable_map_reference_unify
)
{
Eigen
::
Vector3d
orig
(
imu
.
imu
().
linear_acceleration
().
x
(),
imu
.
imu
().
linear_acceleration
().
y
(),
imu
.
imu
().
linear_acceleration
().
z
());
auto
vec
=
QuaternionRotate
(
orientation
,
orig
);
linear_acceleration
.
set_x
(
vec
[
0
]);
linear_acceleration
.
set_y
(
vec
[
1
]);
linear_acceleration
.
set_z
(
vec
[
2
]);
}
else
{
linear_acceleration
.
CopyFrom
(
imu
.
imu
().
linear_acceleration
());
}
Point3D
linear_acceleration_1
;
if
(
FLAGS_enable_map_reference_unify
)
{
Eigen
::
Vector3d
orig
(
imu_1
.
imu
().
linear_acceleration
().
x
(),
imu_1
.
imu
().
linear_acceleration
().
y
(),
imu_1
.
imu
().
linear_acceleration
().
z
());
auto
vec
=
QuaternionRotate
(
orientation_1
,
orig
);
linear_acceleration_1
.
set_x
(
vec
[
0
]);
linear_acceleration_1
.
set_y
(
vec
[
1
]);
linear_acceleration_1
.
set_z
(
vec
[
2
]);
}
else
{
linear_acceleration_1
.
CopyFrom
(
imu_1
.
imu
().
linear_acceleration
());
}
auto
linear_velocity
=
new_pose
->
linear_velocity
();
Point3D
linear_velocity_1
;
auto
dt
=
timestamp_sec_1
-
timestamp_sec
;
linear_velocity_1
.
set_x
(
(
linear_acceleration
.
x
()
+
linear_acceleration_1
.
x
())
/
2.0
*
dt
+
linear_velocity
.
x
());
linear_velocity_1
.
set_y
(
(
linear_acceleration
.
y
()
+
linear_acceleration_1
.
y
())
/
2.0
*
dt
+
linear_velocity
.
y
());
linear_velocity_1
.
set_z
(
(
linear_acceleration
.
z
()
+
linear_acceleration_1
.
z
())
/
2.0
*
dt
+
linear_velocity
.
z
());
auto
position
=
new_pose
->
position
();
PointENU
position_1
;
position_1
.
set_x
(
(
linear_acceleration
.
x
()
/
3.0
+
linear_acceleration_1
.
x
()
/
6.0
)
*
dt
*
dt
+
linear_velocity
.
x
()
*
dt
+
position
.
x
());
position_1
.
set_y
(
(
linear_acceleration
.
y
()
/
3.0
+
linear_acceleration_1
.
y
()
/
6.0
)
*
dt
*
dt
+
linear_velocity
.
y
()
*
dt
+
position
.
y
());
position_1
.
set_z
(
(
linear_acceleration
.
z
()
/
3.0
+
linear_acceleration_1
.
z
()
/
6.0
)
*
dt
*
dt
+
linear_velocity
.
z
()
*
dt
+
position
.
z
());
new_pose
->
mutable_position
()
->
CopyFrom
(
position_1
);
new_pose
->
mutable_orientation
()
->
CopyFrom
(
orientation_1
);
new_pose
->
set_heading
(
QuaternionToHeading
(
new_pose
->
orientation
().
qw
(),
new_pose
->
orientation
().
qx
(),
new_pose
->
orientation
().
qy
(),
new_pose
->
orientation
().
qz
()));
new_pose
->
mutable_linear_velocity
()
->
CopyFrom
(
linear_velocity_1
);
FillPoseFromImu
(
imu_1
.
imu
(),
new_pose
);
if
(
!
finished
)
{
timestamp_sec
=
timestamp_sec_1
;
imu_it
=
imu_it_1
;
imu
.
CopyFrom
(
imu_1
);
}
}
return
true
;
}
void
LMDLocalization
::
PrintPoseError
(
const
Pose
&
pose
,
double
timestamp_sec
)
{
Gps
gps
;
if
(
!
FindMatchingGPS
(
timestamp_sec
,
&
gps
))
return
;
Pose
gps_pose
;
double
t
;
if
(
!
GetGpsPose
(
gps
,
&
gps_pose
,
&
t
))
return
;
if
(
!
pose
.
has_position
()
||
!
gps_pose
.
has_position
()
||
!
pose
.
has_orientation
()
||
!
gps_pose
.
has_orientation
()
||
!
pose
.
has_linear_velocity
()
||
!
gps_pose
.
has_linear_velocity
())
return
;
auto
heading
=
QuaternionToHeading
(
pose
.
orientation
().
qw
(),
pose
.
orientation
().
qx
(),
pose
.
orientation
().
qy
(),
pose
.
orientation
().
qz
());
auto
gps_heading
=
QuaternionToHeading
(
gps_pose
.
orientation
().
qw
(),
gps_pose
.
orientation
().
qx
(),
gps_pose
.
orientation
().
qy
(),
gps_pose
.
orientation
().
qz
());
double
flu_vx
,
flu_vy
;
RotateAxis
(
gps_heading
,
gps_pose
.
linear_velocity
().
x
(),
gps_pose
.
linear_velocity
().
y
(),
&
flu_vx
,
&
flu_vy
);
double
flu_dx
,
flu_dy
;
RotateAxis
(
gps_heading
,
pose
.
position
().
x
()
-
gps_pose
.
position
().
x
(),
pose
.
position
().
y
()
-
gps_pose
.
position
().
y
(),
&
flu_dx
,
&
flu_dy
);
double
flu_dvx
,
flu_dvy
;
RotateAxis
(
gps_heading
,
pose
.
linear_velocity
().
x
(),
pose
.
linear_velocity
().
y
(),
&
flu_dvx
,
&
flu_dvy
);
flu_dvx
-=
flu_vx
;
flu_dvy
-=
flu_vy
;
ADEBUG
<<
"timestamp["
<<
timestamp_sec
<<
"]"
;
ADEBUG
<<
"true heading["
<<
gps_heading
<<
"]"
;
ADEBUG
<<
"heading error["
<<
heading
-
gps_heading
<<
"]"
;
ADEBUG
<<
"true position, x["
<<
gps_pose
.
position
().
x
()
<<
"], y["
<<
gps_pose
.
position
().
y
()
<<
"], z["
<<
gps_pose
.
position
().
z
()
<<
"]"
;
ADEBUG
<<
"position error, station["
<<
flu_dx
<<
"], lateral["
<<
flu_dy
<<
"]"
;
ADEBUG
<<
"true velocity, station["
<<
flu_vx
<<
"], lateral["
<<
flu_vy
<<
"]"
;
ADEBUG
<<
"velocity error, station["
<<
flu_dvx
<<
"], lateral["
<<
flu_dvy
<<
"]"
;
}
void
LMDLocalization
::
RunWatchDog
()
{
...
...
modules/localization/lmd/lmd_localization.h
浏览文件 @
7461c419
/******************************************************************************
* Copyright 201
7
The Apollo Authors. All Rights Reserved.
* Copyright 201
8
The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
...
...
@@ -22,24 +22,26 @@
#ifndef MODULES_LOCALIZATION_LMD_LMD_LOCALIZATION_H_
#define MODULES_LOCALIZATION_LMD_LMD_LOCALIZATION_H_
#include <algorithm>
#include <sstream>
#include <chrono>
#include <future>
#include <list>
#include <map>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "ros/include/ros/ros.h"
#include "modules/canbus/proto/chassis.pb.h"
#include "modules/common/monitor_log/monitor_log_buffer.h"
#include "modules/common/status/status.h"
#include "modules/localization/lmd/predictor/predictor.h"
#include "modules/localization/localization_base.h"
#include "modules/localization/proto/gps.pb.h"
#include "modules/localization/proto/imu.pb.h"
#include "modules/localization/proto/localization.pb.h"
#include "modules/perception/proto/perception_obstacle.pb.h"
#include "modules/common/monitor_log/monitor_log_buffer.h"
#include "modules/common/status/status.h"
#include "modules/localization/localization_base.h"
/**
* @namespace apollo::localization
* @brief apollo::localization
...
...
@@ -53,6 +55,23 @@ namespace localization {
* @brief Generate localization info based on LMD.
*/
class
LMDLocalization
:
public
LocalizationBase
{
struct
PredictorHandler
{
std
::
shared_ptr
<
Predictor
>
predictor
;
std
::
future
<
apollo
::
common
::
Status
>
fut
;
PredictorHandler
()
=
default
;
explicit
PredictorHandler
(
Predictor
*
predictor
)
{
this
->
predictor
.
reset
(
predictor
);
}
bool
Busy
()
const
{
if
(
!
fut
.
valid
())
return
false
;
auto
s
=
fut
.
wait_for
(
std
::
chrono
::
seconds
::
zero
());
return
s
!=
std
::
future_status
::
ready
;
}
};
public:
LMDLocalization
();
virtual
~
LMDLocalization
();
...
...
@@ -70,32 +89,28 @@ class LMDLocalization : public LocalizationBase {
apollo
::
common
::
Status
Stop
()
override
;
private:
void
OnImu
(
const
CorrectedImu
&
imu
);
void
OnGps
(
const
Gps
&
gps
);
void
OnChassis
(
const
apollo
::
canbus
::
Chassis
&
chassis
);
void
OnPerceptionObstacles
(
const
apollo
::
perception
::
PerceptionObstacles
&
obstacles
);
void
OnTimer
(
const
ros
::
TimerEvent
&
event
);
void
PrepareLocalizationMsg
(
LocalizationEstimate
*
localization
);
bool
GetGpsPose
(
const
Gps
&
gps
,
Pose
*
pose
,
double
*
timestamp_sec
);
bool
PredictPose
(
const
Pose
&
old_pose
,
double
old_timestamp_sec
,
double
new_timestamp_sec
,
Pose
*
new_pose
);
bool
FindMatchingGPS
(
double
timestamp_sec
,
Gps
*
gps_msg
);
bool
FindMatchingIMU
(
double
timestamp_sec
,
CorrectedImu
*
imu_msg
);
bool
FindMatchingChassis
(
double
timestamp_sec
,
apollo
::
canbus
::
Chassis
*
chassis_msg
);
bool
PredictByLinearIntergrate
(
const
Pose
&
old_pose
,
double
old_timestamp_sec
,
double
new_timestamp_sec
,
Pose
*
new_pose
);
void
PrintPoseError
(
const
Pose
&
pose
,
double
timestamp_sec
);
void
Predicting
();
void
RunWatchDog
();
private:
ros
::
Timer
timer_
;
apollo
::
common
::
monitor
::
MonitorLogger
monitor_logger_
;
const
std
::
vector
<
double
>
map_offset_
;
bool
has_last_pose_
=
false
;
Pose
last_pose_
;
double
last_pose_timestamp_sec_
;
std
::
map
<
std
::
string
,
PredictorHandler
>
predictors_
;
PredictorHandler
*
gps_
;
PredictorHandler
*
imu_
;
PredictorHandler
*
output_
;
std
::
list
<
CorrectedImu
>
imu_list_
;
std
::
list
<
Gps
>
gps_list_
;
std
::
list
<
apollo
::
perception
::
PerceptionObstacles
>
obstacles_list_
;
};
}
// namespace localization
...
...
modules/localization/lmd/lmd_localization_test.cc
浏览文件 @
7461c419
/******************************************************************************
* Copyright 201
7
The Apollo Authors. All Rights Reserved.
* Copyright 201
8
The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
...
...
modules/localization/lmd/predictor/BUILD
0 → 100644
浏览文件 @
7461c419
load
(
"//tools:cpplint.bzl"
,
"cpplint"
)
package
(
default_visibility
=
[
"//visibility:public"
])
cc_library
(
name
=
"localization_lmd_predictor"
,
hdrs
=
[
"predictor.h"
,
],
deps
=
[
"//modules/common/status"
,
"//modules/localization/lmd/common:localization_lmd_common"
,
],
)
cpplint
()
modules/localization/lmd/predictor/output/BUILD
0 → 100644
浏览文件 @
7461c419
load
(
"//tools:cpplint.bzl"
,
"cpplint"
)
package
(
default_visibility
=
[
"//visibility:public"
])
cc_library
(
name
=
"localization_lmd_predictor_output"
,
srcs
=
[
"predictor_output.cc"
,
"predictor_print_error.cc"
,
],
hdrs
=
[
"predictor_output.h"
,
"predictor_print_error.h"
,
],
deps
=
[
"//modules/common:log"
,
"//modules/common/math"
,
"//modules/common/proto:common_proto"
,
"//modules/common/status"
,
"//modules/localization/common:localization_common"
,
"//modules/localization/lmd/common:localization_lmd_common"
,
"//modules/localization/lmd/predictor:localization_lmd_predictor"
,
"//modules/localization/proto:localization_proto"
,
"@gtest"
,
],
)
cc_test
(
name
=
"localization_lmd_predictor_output_test"
,
size
=
"small"
,
srcs
=
[
"predictor_output_test.cc"
],
deps
=
[
":localization_lmd_predictor_output"
,
"@gtest//:main"
,
],
)
cpplint
()
modules/localization/lmd/predictor/output/predictor_output.cc
0 → 100644
浏览文件 @
7461c419
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/localization/lmd/predictor/output/predictor_output.h"
#include <cmath>
#include "modules/common/proto/geometry.pb.h"
#include "modules/common/log.h"
#include "modules/common/math/euler_angles_zxy.h"
#include "modules/common/math/math_utils.h"
#include "modules/common/math/quaternion.h"
#include "modules/localization/common/localization_gflags.h"
namespace
apollo
{
namespace
localization
{
using
apollo
::
common
::
Point3D
;
using
apollo
::
common
::
PointENU
;
using
apollo
::
common
::
Quaternion
;
using
apollo
::
common
::
Status
;
using
apollo
::
common
::
math
::
EulerAnglesZXYd
;
using
apollo
::
common
::
math
::
QuaternionRotate
;
using
apollo
::
common
::
math
::
QuaternionToHeading
;
namespace
{
template
<
class
T
>
T
QuaternionRotateXYZ
(
const
T
v
,
const
Quaternion
&
orientation
)
{
auto
vec
=
QuaternionRotate
(
orientation
,
Eigen
::
Vector3d
(
v
.
x
(),
v
.
y
(),
v
.
z
()));
T
ret
;
ret
.
set_x
(
vec
[
0
]);
ret
.
set_y
(
vec
[
1
]);
ret
.
set_z
(
vec
[
2
]);
return
ret
;
}
void
FillPoseFromImu
(
const
Pose
&
imu_pose
,
Pose
*
pose
)
{
// linear acceleration
if
(
imu_pose
.
has_linear_acceleration
())
{
if
(
FLAGS_enable_map_reference_unify
)
{
if
(
pose
->
has_orientation
())
{
pose
->
mutable_linear_acceleration
()
->
CopyFrom
(
QuaternionRotateXYZ
(
imu_pose
.
linear_acceleration
(),
pose
->
orientation
()));
pose
->
mutable_linear_acceleration_vrf
()
->
CopyFrom
(
imu_pose
.
linear_acceleration
());
}
else
{
AERROR
<<
"Fail to convert linear_acceleration"
;
}
}
else
{
pose
->
mutable_linear_acceleration
()
->
CopyFrom
(
imu_pose
.
linear_acceleration
());
}
}
// angular velocity
if
(
imu_pose
.
has_angular_velocity
())
{
if
(
FLAGS_enable_map_reference_unify
)
{
if
(
FLAGS_enable_map_reference_unify
)
{
if
(
pose
->
has_orientation
())
{
pose
->
mutable_angular_velocity
()
->
CopyFrom
(
QuaternionRotateXYZ
(
imu_pose
.
angular_velocity
(),
pose
->
orientation
()));
pose
->
mutable_angular_velocity_vrf
()
->
CopyFrom
(
imu_pose
.
angular_velocity
());
}
else
{
AERROR
<<
"Fail to convert angular_velocity"
;
}
}
else
{
pose
->
mutable_angular_velocity
()
->
CopyFrom
(
imu_pose
.
angular_velocity
());
}
}
else
{
pose
->
mutable_angular_velocity
()
->
CopyFrom
(
imu_pose
.
angular_velocity
());
}
}
// euler angle
if
(
imu_pose
.
has_euler_angles
())
pose
->
mutable_euler_angles
()
->
CopyFrom
(
imu_pose
.
euler_angles
());
}
}
// namespace
PredictorOutput
::
PredictorOutput
(
double
memory_cycle_sec
,
const
std
::
function
<
Status
(
double
,
const
Pose
&
)
>&
publish_loc_func
)
:
Predictor
(
memory_cycle_sec
),
publish_loc_func_
(
publish_loc_func
)
{
name_
=
kPredictorOutputName
;
dep_predicteds_
.
emplace
(
kPredictorGpsName
,
PoseList
(
memory_cycle_sec
));
dep_predicteds_
.
emplace
(
kPredictorImuName
,
PoseList
(
memory_cycle_sec
));
on_adapter_thread_
=
true
;
}
PredictorOutput
::~
PredictorOutput
()
{}
bool
PredictorOutput
::
Updateable
()
const
{
const
auto
&
imu
=
dep_predicteds_
.
find
(
kPredictorImuName
)
->
second
;
if
(
predicted_
.
empty
())
{
const
auto
&
gps
=
dep_predicteds_
.
find
(
kPredictorGpsName
)
->
second
;
return
!
gps
.
empty
()
&&
!
imu
.
empty
();
}
else
{
return
!
imu
.
empty
()
&&
predicted_
.
Older
(
imu
);
}
}
Status
PredictorOutput
::
Update
()
{
if
(
predicted_
.
empty
())
{
const
auto
&
gps
=
dep_predicteds_
[
kPredictorGpsName
];
auto
gps_latest
=
gps
.
Latest
();
auto
timestamp_sec
=
gps_latest
->
first
;
auto
pose
=
gps_latest
->
second
;
if
(
!
pose
.
has_heading
()
&&
pose
.
has_orientation
())
{
pose
.
set_heading
(
QuaternionToHeading
(
pose
.
orientation
().
qw
(),
pose
.
orientation
().
qx
(),
pose
.
orientation
().
qy
(),
pose
.
orientation
().
qz
()));
}
const
auto
&
imu
=
dep_predicteds_
[
kPredictorImuName
];
Pose
imu_pose
;
imu
.
FindNearestPose
(
timestamp_sec
,
&
imu_pose
);
// fill pose from imu
FillPoseFromImu
(
imu_pose
,
&
pose
);
// push pose to list
predicted_
.
Push
(
timestamp_sec
,
pose
);
// publish
return
publish_loc_func_
(
timestamp_sec
,
pose
);
}
else
{
// get timestamp from imu
const
auto
&
imu
=
dep_predicteds_
[
kPredictorImuName
];
auto
timestamp_sec
=
imu
.
Latest
()
->
first
;
// base pose for prediction
auto
base_timestamp_sec
=
predicted_
.
Latest
()
->
first
;
auto
base_pose
=
predicted_
.
Latest
()
->
second
;
// predict
Pose
pose
;
PredictByImu
(
base_timestamp_sec
,
base_pose
,
timestamp_sec
,
&
pose
);
// push pose to list
predicted_
.
Push
(
timestamp_sec
,
pose
);
// publish
return
publish_loc_func_
(
timestamp_sec
,
pose
);
}
}
bool
PredictorOutput
::
PredictByImu
(
double
old_timestamp_sec
,
const
Pose
&
old_pose
,
double
new_timestamp_sec
,
Pose
*
new_pose
)
{
if
(
!
old_pose
.
has_position
()
||
!
old_pose
.
has_orientation
()
||
!
old_pose
.
has_linear_velocity
())
{
AERROR
<<
"Pose has no some fields"
;
return
false
;
}
const
auto
&
imu
=
dep_predicteds_
[
kPredictorImuName
];
auto
p
=
imu
.
RangeOf
(
old_timestamp_sec
);
auto
it
=
p
.
first
;
if
(
it
==
imu
.
end
())
{
if
(
p
.
second
!=
imu
.
end
())
{
it
=
p
.
second
;
}
else
{
AERROR
<<
std
::
setprecision
(
15
)
<<
"Cannot get the lower of range from imu with timestamp["
<<
old_timestamp_sec
<<
"]"
;
return
false
;
}
}
auto
timestamp_sec
=
old_timestamp_sec
;
new_pose
->
CopyFrom
(
old_pose
);
bool
finished
=
false
;
while
(
!
finished
)
{
Pose
imu_pose
;
double
timestamp_sec_1
;
Pose
imu_pose_1
;
auto
it_1
=
it
;
it_1
++
;
if
(
it_1
!=
imu
.
end
()
&&
new_timestamp_sec
>=
it_1
->
first
)
{
PoseList
::
InterpolatePose
(
it
->
first
,
it
->
second
,
it_1
->
first
,
it_1
->
second
,
timestamp_sec
,
&
imu_pose
);
timestamp_sec_1
=
it_1
->
first
;
imu_pose_1
=
it_1
->
second
;
}
else
{
timestamp_sec_1
=
new_timestamp_sec
;
imu_pose
=
it
->
second
;
imu_pose_1
=
imu_pose
;
}
if
(
new_timestamp_sec
<=
timestamp_sec_1
)
{
finished
=
true
;
}
if
(
!
finished
&&
timestamp_sec_1
<=
timestamp_sec
)
{
it
=
it_1
;
imu_pose
=
imu_pose_1
;
continue
;
}
if
(
!
imu_pose
.
has_linear_acceleration
()
||
!
imu_pose_1
.
has_linear_acceleration
()
||
!
imu_pose
.
has_angular_velocity
()
||
!
imu_pose_1
.
has_angular_velocity
())
{
AERROR
<<
"Imu_pose or imu_pose_1 has no some fields"
;
return
false
;
}
auto
dt
=
timestamp_sec_1
-
timestamp_sec
;
auto
orientation
=
new_pose
->
orientation
();
Point3D
angular_velocity
;
if
(
FLAGS_enable_map_reference_unify
)
{
angular_velocity
.
CopyFrom
(
QuaternionRotateXYZ
(
imu_pose
.
angular_velocity
(),
orientation
));
}
else
{
angular_velocity
.
CopyFrom
(
imu_pose
.
angular_velocity
());
}
Point3D
angular_velocity_1
;
if
(
FLAGS_enable_map_reference_unify
)
{
angular_velocity_1
.
CopyFrom
(
QuaternionRotateXYZ
(
imu_pose_1
.
angular_velocity
(),
orientation
));
}
else
{
angular_velocity_1
.
CopyFrom
(
imu_pose_1
.
angular_velocity
());
}
Point3D
angular_vel
;
angular_vel
.
set_x
((
angular_velocity
.
x
()
+
angular_velocity_1
.
x
())
/
2.0
);
angular_vel
.
set_y
((
angular_velocity
.
y
()
+
angular_velocity_1
.
y
())
/
2.0
);
angular_vel
.
set_z
((
angular_velocity
.
z
()
+
angular_velocity_1
.
z
())
/
2.0
);
EulerAnglesZXYd
euler_a
(
orientation
.
qw
(),
orientation
.
qx
(),
orientation
.
qy
(),
orientation
.
qz
());
auto
derivation_roll
=
angular_vel
.
x
()
+
std
::
sin
(
euler_a
.
roll
())
*
std
::
tan
(
euler_a
.
pitch
())
*
angular_vel
.
y
()
+
std
::
cos
(
euler_a
.
roll
())
*
std
::
tan
(
euler_a
.
pitch
())
*
angular_vel
.
z
();
auto
derivation_pitch
=
std
::
cos
(
euler_a
.
roll
())
*
angular_vel
.
y
()
-
std
::
sin
(
euler_a
.
roll
())
*
angular_vel
.
z
();
auto
derivation_yaw
=
std
::
sin
(
euler_a
.
roll
())
/
std
::
cos
(
euler_a
.
pitch
())
*
angular_vel
.
y
()
+
std
::
cos
(
euler_a
.
roll
())
/
std
::
cos
(
euler_a
.
pitch
())
*
angular_vel
.
z
();
EulerAnglesZXYd
euler_b
(
euler_a
.
roll
()
+
derivation_roll
*
dt
,
euler_a
.
pitch
()
+
derivation_pitch
*
dt
,
euler_a
.
yaw
()
+
derivation_yaw
*
dt
);
auto
q
=
euler_b
.
ToQuaternion
();
Quaternion
orientation_1
;
orientation_1
.
set_qw
(
q
.
w
());
orientation_1
.
set_qx
(
q
.
x
());
orientation_1
.
set_qy
(
q
.
y
());
orientation_1
.
set_qz
(
q
.
z
());
Point3D
linear_acceleration
;
if
(
FLAGS_enable_map_reference_unify
)
{
linear_acceleration
.
CopyFrom
(
QuaternionRotateXYZ
(
imu_pose
.
linear_acceleration
(),
orientation
));
}
else
{
linear_acceleration
.
CopyFrom
(
imu_pose
.
linear_acceleration
());
}
Point3D
linear_acceleration_1
;
if
(
FLAGS_enable_map_reference_unify
)
{
linear_acceleration_1
.
CopyFrom
(
QuaternionRotateXYZ
(
imu_pose_1
.
linear_acceleration
(),
orientation_1
));
}
else
{
linear_acceleration_1
.
CopyFrom
(
imu_pose_1
.
linear_acceleration
());
}
auto
linear_velocity
=
new_pose
->
linear_velocity
();
Point3D
linear_velocity_1
;
linear_velocity_1
.
set_x
(
(
linear_acceleration
.
x
()
+
linear_acceleration_1
.
x
())
/
2.0
*
dt
+
linear_velocity
.
x
());
linear_velocity_1
.
set_y
(
(
linear_acceleration
.
y
()
+
linear_acceleration_1
.
y
())
/
2.0
*
dt
+
linear_velocity
.
y
());
linear_velocity_1
.
set_z
(
(
linear_acceleration
.
z
()
+
linear_acceleration_1
.
z
())
/
2.0
*
dt
+
linear_velocity
.
z
());
auto
position
=
new_pose
->
position
();
PointENU
position_1
;
position_1
.
set_x
(
(
linear_acceleration
.
x
()
/
3.0
+
linear_acceleration_1
.
x
()
/
6.0
)
*
dt
*
dt
+
linear_velocity
.
x
()
*
dt
+
position
.
x
());
position_1
.
set_y
(
(
linear_acceleration
.
y
()
/
3.0
+
linear_acceleration_1
.
y
()
/
6.0
)
*
dt
*
dt
+
linear_velocity
.
y
()
*
dt
+
position
.
y
());
position_1
.
set_z
(
(
linear_acceleration
.
z
()
/
3.0
+
linear_acceleration_1
.
z
()
/
6.0
)
*
dt
*
dt
+
linear_velocity
.
z
()
*
dt
+
position
.
z
());
new_pose
->
mutable_position
()
->
CopyFrom
(
position_1
);
new_pose
->
mutable_orientation
()
->
CopyFrom
(
orientation_1
);
new_pose
->
set_heading
(
QuaternionToHeading
(
new_pose
->
orientation
().
qw
(),
new_pose
->
orientation
().
qx
(),
new_pose
->
orientation
().
qy
(),
new_pose
->
orientation
().
qz
()));
new_pose
->
mutable_linear_velocity
()
->
CopyFrom
(
linear_velocity_1
);
FillPoseFromImu
(
imu_pose_1
,
new_pose
);
if
(
!
finished
)
{
timestamp_sec
=
timestamp_sec_1
;
it
=
it_1
;
imu_pose
=
imu_pose_1
;
}
}
return
true
;
}
}
// namespace localization
}
// namespace apollo
modules/localization/lmd/predictor/output/predictor_output.h
0 → 100644
浏览文件 @
7461c419
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
/**
* @file predictor_output.h
* @brief The class of PredictorOutput.
*/
#ifndef MODULES_LOCALIZATION_LMD_PREDICTOR_OUTPUT_PREDICTOR_OUTPUT_H_
#define MODULES_LOCALIZATION_LMD_PREDICTOR_OUTPUT_PREDICTOR_OUTPUT_H_
#include <functional>
#include "gtest/gtest.h"
#include "modules/localization/lmd/predictor/predictor.h"
/**
* @namespace apollo::localization
* @brief apollo::localization
*/
namespace
apollo
{
namespace
localization
{
/**
* @class PredictorOutput
*
* @brief Implementation of predictor.
*/
class
PredictorOutput
:
public
Predictor
{
public:
explicit
PredictorOutput
(
double
memory_cycle_sec
,
const
std
::
function
<
apollo
::
common
::
Status
(
double
,
const
Pose
&
)
>
&
publish_loc_func
);
virtual
~
PredictorOutput
();
/**
* @brief Overrided implementation of the virtual function "Updateable" in the
* base class "Predictor".
* @return True if yes; no otherwise.
*/
bool
Updateable
()
const
override
;
/**
* @brief Overrided implementation of the virtual function "Update" in the
* base class "Predictor".
* @return Status::OK() if success; error otherwise.
*/
apollo
::
common
::
Status
Update
()
override
;
private:
bool
PredictByImu
(
double
old_timestamp_sec
,
const
Pose
&
old_pose
,
double
new_timestamp_sec
,
Pose
*
new_pose
);
private:
std
::
function
<
apollo
::
common
::
Status
(
double
,
const
Pose
&
)
>
publish_loc_func_
;
FRIEND_TEST
(
PredictorOutputTest
,
PredictByImu1
);
FRIEND_TEST
(
PredictorOutputTest
,
PredictByImu2
);
};
}
// namespace localization
}
// namespace apollo
#endif // MODULES_LOCALIZATION_LMD_PREDICTOR_OUTPUT_PREDICTOR_OUTPUT_H_
modules/localization/lmd/predictor/output/predictor_output_test.cc
0 → 100644
浏览文件 @
7461c419
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/localization/lmd/predictor/output/predictor_output.h"
#include "gtest/gtest.h"
namespace
apollo
{
namespace
localization
{
using
apollo
::
common
::
Status
;
class
PredictorOutputTest
:
public
::
testing
::
Test
{
public:
PredictorOutputTest
()
:
predictor_
(
10.0
,
[
&
](
double
,
const
Pose
&
)
{
return
Status
::
OK
();
})
{}
protected:
PredictorOutput
predictor_
;
};
TEST_F
(
PredictorOutputTest
,
PredictByImu1
)
{
auto
&
imu
=
predictor_
.
dep_predicteds_
[
kPredictorImuName
];
imu
.
clear
();
Pose
imu_pose
;
imu_pose
.
mutable_linear_acceleration
()
->
set_x
(
1.0
);
imu_pose
.
mutable_linear_acceleration
()
->
set_y
(
1.0
);
imu_pose
.
mutable_linear_acceleration
()
->
set_z
(
0.0
);
imu_pose
.
mutable_angular_velocity
()
->
set_x
(
0.0
);
imu_pose
.
mutable_angular_velocity
()
->
set_y
(
0.0
);
imu_pose
.
mutable_angular_velocity
()
->
set_z
(
0.0
);
imu
.
Push
(
0.0
,
imu_pose
);
imu_pose
.
mutable_linear_acceleration
()
->
set_x
(
0.5
);
imu_pose
.
mutable_linear_acceleration
()
->
set_y
(
0.5
);
imu_pose
.
mutable_linear_acceleration
()
->
set_z
(
0.0
);
imu
.
Push
(
0.5
,
imu_pose
);
imu
.
Push
(
3.0
,
imu_pose
);
Pose
old_pose
;
old_pose
.
mutable_position
()
->
set_x
(
0.0
);
old_pose
.
mutable_position
()
->
set_y
(
0.0
);
old_pose
.
mutable_position
()
->
set_z
(
0.0
);
old_pose
.
mutable_orientation
()
->
set_qw
(
1.0
);
old_pose
.
mutable_orientation
()
->
set_qx
(
0.0
);
old_pose
.
mutable_orientation
()
->
set_qy
(
0.0
);
old_pose
.
mutable_orientation
()
->
set_qz
(
0.0
);
old_pose
.
mutable_linear_velocity
()
->
set_x
(
0.0
);
old_pose
.
mutable_linear_velocity
()
->
set_y
(
0.0
);
old_pose
.
mutable_linear_velocity
()
->
set_z
(
0.0
);
Pose
new_pose
;
EXPECT_TRUE
(
predictor_
.
PredictByImu
(
0.0
,
old_pose
,
1.0
,
&
new_pose
));
EXPECT_NEAR
(
0.354
,
new_pose
.
position
().
x
(),
1e-3
);
EXPECT_NEAR
(
0.354
,
new_pose
.
position
().
y
(),
1e-3
);
EXPECT_NEAR
(
0.0
,
new_pose
.
position
().
z
(),
1e-3
);
EXPECT_NEAR
(
0.625
,
new_pose
.
linear_velocity
().
x
(),
1e-3
);
EXPECT_NEAR
(
0.625
,
new_pose
.
linear_velocity
().
y
(),
1e-3
);
EXPECT_NEAR
(
0.0
,
new_pose
.
linear_velocity
().
z
(),
1e-3
);
}
TEST_F
(
PredictorOutputTest
,
PredictByImu2
)
{
auto
&
imu
=
predictor_
.
dep_predicteds_
[
kPredictorImuName
];
imu
.
clear
();
Pose
imu_pose
;
imu_pose
.
mutable_linear_acceleration
()
->
set_x
(
1.0
);
imu_pose
.
mutable_linear_acceleration
()
->
set_y
(
1.0
);
imu_pose
.
mutable_linear_acceleration
()
->
set_z
(
0.0
);
imu_pose
.
mutable_angular_velocity
()
->
set_x
(
0.0
);
imu_pose
.
mutable_angular_velocity
()
->
set_y
(
0.0
);
imu_pose
.
mutable_angular_velocity
()
->
set_z
(
0.0
);
imu
.
Push
(
0.0
,
imu_pose
);
imu_pose
.
mutable_linear_acceleration
()
->
set_x
(
1.0
);
imu_pose
.
mutable_linear_acceleration
()
->
set_y
(
1.0
);
imu_pose
.
mutable_linear_acceleration
()
->
set_z
(
0.0
);
imu
.
Push
(
1.0
,
imu_pose
);
Pose
old_pose
;
old_pose
.
mutable_position
()
->
set_x
(
0.0
);
old_pose
.
mutable_position
()
->
set_y
(
0.0
);
old_pose
.
mutable_position
()
->
set_z
(
0.0
);
old_pose
.
mutable_orientation
()
->
set_qw
(
1.0
);
old_pose
.
mutable_orientation
()
->
set_qx
(
0.0
);
old_pose
.
mutable_orientation
()
->
set_qy
(
0.0
);
old_pose
.
mutable_orientation
()
->
set_qz
(
0.0
);
old_pose
.
mutable_linear_velocity
()
->
set_x
(
0.0
);
old_pose
.
mutable_linear_velocity
()
->
set_y
(
0.0
);
old_pose
.
mutable_linear_velocity
()
->
set_z
(
0.0
);
Pose
new_pose
;
EXPECT_TRUE
(
predictor_
.
PredictByImu
(
0.0
,
old_pose
,
1.0
,
&
new_pose
));
EXPECT_NEAR
(
0.5
,
new_pose
.
position
().
x
(),
1e-3
);
EXPECT_NEAR
(
0.5
,
new_pose
.
position
().
y
(),
1e-3
);
EXPECT_NEAR
(
0.0
,
new_pose
.
position
().
z
(),
1e-3
);
EXPECT_NEAR
(
1.0
,
new_pose
.
linear_velocity
().
x
(),
1e-3
);
EXPECT_NEAR
(
1.0
,
new_pose
.
linear_velocity
().
y
(),
1e-3
);
EXPECT_NEAR
(
0.0
,
new_pose
.
linear_velocity
().
z
(),
1e-3
);
}
}
// namespace localization
}
// namespace apollo
modules/localization/lmd/predictor/output/predictor_print_error.cc
0 → 100644
浏览文件 @
7461c419
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/localization/lmd/predictor/output/predictor_print_error.h"
#include <algorithm>
#include <iomanip>
#include "modules/common/math/math_utils.h"
#include "modules/common/math/quaternion.h"
namespace
apollo
{
namespace
localization
{
using
apollo
::
common
::
Status
;
using
apollo
::
common
::
math
::
QuaternionToHeading
;
using
apollo
::
common
::
math
::
RotateAxis
;
namespace
{
constexpr
double
kPrintErrorInterval
=
0.01
;
}
// namespace
PredictorPrintError
::
PredictorPrintError
(
double
memory_cycle_sec
)
:
Predictor
(
memory_cycle_sec
)
{
name_
=
kPredictorPrintErrorName
;
dep_predicteds_
.
emplace
(
kPredictorGpsName
,
PoseList
(
memory_cycle_sec
));
dep_predicteds_
.
emplace
(
kPredictorOutputName
,
PoseList
(
memory_cycle_sec
));
on_adapter_thread_
=
true
;
}
PredictorPrintError
::~
PredictorPrintError
()
{}
bool
PredictorPrintError
::
Updateable
()
const
{
const
auto
&
gps
=
dep_predicteds_
.
find
(
kPredictorGpsName
)
->
second
;
const
auto
&
output
=
dep_predicteds_
.
find
(
kPredictorOutputName
)
->
second
;
return
!
gps
.
empty
()
&&
!
output
.
empty
()
&&
(
!
has_latest_timestamp_sec_
||
latest_timestamp_sec_
+
kPrintErrorInterval
<
DepsTimestamp
());
}
Status
PredictorPrintError
::
Update
()
{
auto
deps_timestamp_sec
=
DepsTimestamp
();
if
(
!
has_latest_timestamp_sec_
)
{
has_latest_timestamp_sec_
=
true
;
latest_timestamp_sec_
=
deps_timestamp_sec
;
return
Status
::
OK
();
}
for
(
auto
timestamp_sec
=
latest_timestamp_sec_
;
timestamp_sec
+
kPrintErrorInterval
<
deps_timestamp_sec
;
timestamp_sec
+=
kPrintErrorInterval
)
{
const
auto
&
output
=
dep_predicteds_
[
kPredictorOutputName
];
Pose
pose
;
output
.
FindMatchingPose
(
timestamp_sec
,
&
pose
);
PrintPoseError
(
timestamp_sec
,
pose
);
}
latest_timestamp_sec_
=
deps_timestamp_sec
;
return
Status
::
OK
();
}
double
PredictorPrintError
::
DepsTimestamp
()
const
{
auto
g_it
=
dep_predicteds_
.
find
(
kPredictorGpsName
)
->
second
.
Latest
();
auto
o_it
=
dep_predicteds_
.
find
(
kPredictorOutputName
)
->
second
.
Latest
();
return
std
::
min
(
o_it
->
first
,
g_it
->
first
);
}
void
PredictorPrintError
::
PrintPoseError
(
double
timestamp_sec
,
const
Pose
&
pose
)
{
const
auto
&
gps
=
dep_predicteds_
[
kPredictorGpsName
];
Pose
gps_pose
;
if
(
!
gps
.
FindMatchingPose
(
timestamp_sec
,
&
gps_pose
))
{
return
;
}
if
(
!
pose
.
has_position
()
||
!
gps_pose
.
has_position
()
||
!
pose
.
has_orientation
()
||
!
gps_pose
.
has_orientation
()
||
!
pose
.
has_linear_velocity
()
||
!
gps_pose
.
has_linear_velocity
())
{
AERROR
<<
"Poses has not some feilds"
;
return
;
}
auto
heading
=
QuaternionToHeading
(
pose
.
orientation
().
qw
(),
pose
.
orientation
().
qx
(),
pose
.
orientation
().
qy
(),
pose
.
orientation
().
qz
());
auto
gps_heading
=
QuaternionToHeading
(
gps_pose
.
orientation
().
qw
(),
gps_pose
.
orientation
().
qx
(),
gps_pose
.
orientation
().
qy
(),
gps_pose
.
orientation
().
qz
());
double
flu_vx
,
flu_vy
;
RotateAxis
(
gps_heading
,
gps_pose
.
linear_velocity
().
x
(),
gps_pose
.
linear_velocity
().
y
(),
&
flu_vx
,
&
flu_vy
);
// TODO(all): estimate acceleration
double
flu_ax
=
0.0
,
flu_ay
=
0.0
;
Pose
gps_pose0
;
constexpr
double
kTimeDiff
=
0.01
;
if
(
gps
.
FindMatchingPose
(
timestamp_sec
-
kTimeDiff
,
&
gps_pose0
)
&&
gps_pose0
.
has_orientation
()
&&
gps_pose0
.
has_linear_velocity
())
{
auto
gps_heading0
=
QuaternionToHeading
(
gps_pose0
.
orientation
().
qw
(),
gps_pose0
.
orientation
().
qx
(),
gps_pose0
.
orientation
().
qy
(),
gps_pose0
.
orientation
().
qz
());
double
flu_vx0
,
flu_vy0
;
RotateAxis
(
gps_heading0
,
gps_pose0
.
linear_velocity
().
x
(),
gps_pose0
.
linear_velocity
().
y
(),
&
flu_vx0
,
&
flu_vy0
);
flu_ax
=
(
flu_vx
-
flu_vx0
)
/
kTimeDiff
;
flu_ay
=
(
flu_vy
-
flu_vy0
)
/
kTimeDiff
;
}
double
flu_dx
,
flu_dy
;
RotateAxis
(
gps_heading
,
pose
.
position
().
x
()
-
gps_pose
.
position
().
x
(),
pose
.
position
().
y
()
-
gps_pose
.
position
().
y
(),
&
flu_dx
,
&
flu_dy
);
double
flu_dvx
,
flu_dvy
;
RotateAxis
(
gps_heading
,
pose
.
linear_velocity
().
x
(),
pose
.
linear_velocity
().
y
(),
&
flu_dvx
,
&
flu_dvy
);
flu_dvx
-=
flu_vx
;
flu_dvy
-=
flu_vy
;
double
flu_dax
,
flu_day
;
RotateAxis
(
gps_heading
,
pose
.
linear_acceleration
().
x
(),
pose
.
linear_acceleration
().
y
(),
&
flu_dax
,
&
flu_day
);
flu_dax
-=
flu_ax
;
flu_day
-=
flu_ay
;
ADEBUG
<<
std
::
setprecision
(
15
)
<<
"Timestamp["
<<
timestamp_sec
<<
"]"
;
ADEBUG
<<
std
::
setprecision
(
15
)
<<
"True heading["
<<
gps_heading
<<
"], heading error["
<<
heading
-
gps_heading
<<
"]"
;
ADEBUG
<<
std
::
setprecision
(
15
)
<<
"True position, x["
<<
gps_pose
.
position
().
x
()
<<
"], y["
<<
gps_pose
.
position
().
y
()
<<
"], z["
<<
gps_pose
.
position
().
z
()
<<
"], position error, station["
<<
flu_dx
<<
"], lateral["
<<
flu_dy
<<
"]"
;
ADEBUG
<<
std
::
setprecision
(
15
)
<<
"True velocity, station["
<<
flu_vx
<<
"], lateral["
<<
flu_vy
<<
"], velocity error, station["
<<
flu_dvx
<<
"], lateral["
<<
flu_dvy
<<
"]"
;
ADEBUG
<<
std
::
setprecision
(
15
)
<<
"True acceleration, station["
<<
flu_ax
<<
"], lateral["
<<
flu_ay
<<
"], acceleration error, station["
<<
flu_dax
<<
"], lateral["
<<
flu_day
<<
"]"
;
}
}
// namespace localization
}
// namespace apollo
modules/localization/lmd/predictor/output/predictor_print_error.h
0 → 100644
浏览文件 @
7461c419
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
/**
* @file predictor_print_error.h
* @brief The class of PredictorPrintError.
*/
#ifndef MODULES_LOCALIZATION_LMD_PREDICTOR_OUTPUT_PREDICTOR_PRINT_ERROR_H_
#define MODULES_LOCALIZATION_LMD_PREDICTOR_OUTPUT_PREDICTOR_PRINT_ERROR_H_
#include "modules/localization/lmd/predictor/predictor.h"
/**
* @namespace apollo::localization
* @brief apollo::localization
*/
namespace
apollo
{
namespace
localization
{
/**
* @class PredictorPrintError
*
* @brief Implementation of predictor.
*/
class
PredictorPrintError
:
public
Predictor
{
public:
explicit
PredictorPrintError
(
double
memory_cycle_sec
);
virtual
~
PredictorPrintError
();
/**
* @brief Overrided implementation of the virtual function "Updateable" in the
* base class "Predictor".
* @return True if yes; no otherwise.
*/
bool
Updateable
()
const
override
;
/**
* @brief Overrided implementation of the virtual function "Update" in the
* base class "Predictor".
* @return Status::OK() if success; error otherwise.
*/
apollo
::
common
::
Status
Update
()
override
;
private:
double
DepsTimestamp
()
const
;
void
PrintPoseError
(
double
timestamp_sec
,
const
Pose
&
pose
);
private:
bool
has_latest_timestamp_sec_
=
false
;
double
latest_timestamp_sec_
;
};
}
// namespace localization
}
// namespace apollo
#endif // MODULES_LOCALIZATION_LMD_PREDICTOR_OUTPUT_PREDICTOR_PRINT_ERROR_H_
modules/localization/lmd/predictor/predictor.h
0 → 100644
浏览文件 @
7461c419
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
/**
* @file predictor.h
* @brief The class of Predictor.
*/
#ifndef MODULES_LOCALIZATION_LMD_COMMON_PREDICTOR_PREDICTOR_H_
#define MODULES_LOCALIZATION_LMD_COMMON_PREDICTOR_PREDICTOR_H_
#include <map>
#include <string>
#include <utility>
#include "modules/common/log.h"
#include "modules/common/status/status.h"
#include "modules/localization/lmd/common/pose_list.h"
/**
* @namespace apollo::localization
* @brief apollo::localization
*/
namespace
apollo
{
namespace
localization
{
constexpr
char
kPredictorPrintErrorName
[]
=
"print_error"
;
constexpr
char
kPredictorOutputName
[]
=
"output"
;
constexpr
char
kPredictorImuName
[]
=
"imu"
;
constexpr
char
kPredictorGpsName
[]
=
"gps"
;
/**
* @class Predictor
*
* @brief Interface for implementing predictor.
*/
class
Predictor
{
public:
explicit
Predictor
(
double
memory_cycle_sec_
)
:
predicted_
(
memory_cycle_sec_
)
{}
virtual
~
Predictor
()
{}
/**
* @brief Get name of the predictor.
* @return A name.
*/
const
std
::
string
&
Name
()
const
{
return
name_
;
}
/**
* @brief Get names of the deps.
* @return Predicteds of deps.
*/
const
std
::
map
<
std
::
string
,
PoseList
>&
DepPredicteds
()
const
{
return
dep_predicteds_
;
}
/**
* @brief Update predicted list of deps.
*/
void
UpdateDepPredicted
(
const
std
::
string
&
name
,
const
PoseList
&
predicted
)
{
UpdateDepPredicted
(
name
,
PoseList
(
predicted
));
}
void
UpdateDepPredicted
(
const
std
::
string
&
name
,
PoseList
&&
predicted
)
{
auto
it
=
dep_predicteds_
.
find
(
name
);
CHECK
(
it
!=
dep_predicteds_
.
end
());
it
->
second
=
std
::
move
(
predicted
);
}
/**
* @brief Get predicted list.
* @return Predicted list.
*/
const
PoseList
&
Predicted
()
const
{
return
predicted_
;
}
/**
* @brief Is predicted running on adapter's thread.
* @return True if yes; no otherwise.
*/
bool
UpdatingOnAdapterThread
()
const
{
return
on_adapter_thread_
;
}
/**
* @brief Is predicted list updateable.
* @return True if yes; no otherwise.
*/
virtual
bool
Updateable
()
const
=
0
;
/**
* @brief Predict a new pose and add it to predicted list.
* @return Status::OK() if success; error otherwise.
*/
virtual
apollo
::
common
::
Status
Update
()
=
0
;
protected:
std
::
string
name_
;
std
::
map
<
std
::
string
,
PoseList
>
dep_predicteds_
;
PoseList
predicted_
;
bool
on_adapter_thread_
=
false
;
};
}
// namespace localization
}
// namespace apollo
#endif // MODULES_LOCALIZATION_LMD_COMMON_PREDICTOR_PREDICTOR_H_
modules/localization/lmd/predictor/raw/BUILD
0 → 100644
浏览文件 @
7461c419
load
(
"//tools:cpplint.bzl"
,
"cpplint"
)
package
(
default_visibility
=
[
"//visibility:public"
])
cc_library
(
name
=
"localization_lmd_predictor_raw_base"
,
deps
=
[
"//modules/localization/lmd/predictor:localization_lmd_predictor"
,
],
)
cc_library
(
name
=
"localization_lmd_predictor_raw_imu"
,
srcs
=
[
"predictor_imu.cc"
,
],
hdrs
=
[
"predictor_imu.h"
,
],
deps
=
[
":localization_lmd_predictor_raw_base"
,
"//modules/common:log"
,
"//modules/localization/proto:imu_proto"
,
],
)
cc_library
(
name
=
"localization_lmd_predictor_raw_gps"
,
srcs
=
[
"predictor_gps.cc"
,
],
hdrs
=
[
"predictor_gps.h"
,
],
deps
=
[
":localization_lmd_predictor_raw_base"
,
"//modules/common:log"
,
"//modules/localization/proto:gps_proto"
,
],
)
cpplint
()
modules/localization/lmd/predictor/raw/predictor_gps.cc
0 → 100644
浏览文件 @
7461c419
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include <iomanip>
#include "modules/localization/lmd/predictor/raw/predictor_gps.h"
#include "modules/common/log.h"
namespace
apollo
{
namespace
localization
{
using
apollo
::
common
::
Status
;
PredictorGps
::
PredictorGps
(
double
memory_cycle_sec
)
:
Predictor
(
memory_cycle_sec
)
{
name_
=
kPredictorGpsName
;
on_adapter_thread_
=
true
;
}
PredictorGps
::~
PredictorGps
()
{}
bool
PredictorGps
::
UpdateGps
(
const
Gps
&
gps
)
{
if
(
!
gps
.
has_header
()
||
!
gps
.
header
().
has_timestamp_sec
()
||
!
gps
.
has_localization
())
{
AERROR
<<
"Message has not some feilds"
;
return
false
;
}
auto
timestamp_sec
=
gps
.
header
().
timestamp_sec
();
if
(
!
predicted_
.
Push
(
timestamp_sec
,
gps
.
localization
()))
{
AWARN
<<
std
::
setprecision
(
15
)
<<
"Failed push pose to list, with timestamp["
<<
timestamp_sec
<<
"]"
;
return
false
;
}
return
true
;
}
bool
PredictorGps
::
Updateable
()
const
{
return
!
predicted_
.
empty
()
&&
predicted_
.
Newer
(
latest_timestamp_sec_
);
}
Status
PredictorGps
::
Update
()
{
if
(
!
predicted_
.
empty
())
{
auto
latest
=
predicted_
.
Latest
();
latest_timestamp_sec_
=
latest
->
first
;
}
return
Status
::
OK
();
}
}
// namespace localization
}
// namespace apollo
modules/localization/lmd/predictor/raw/predictor_gps.h
0 → 100644
浏览文件 @
7461c419
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
/**
* @file predictor.h
* @brief The class of PredictorGps.
*/
#ifndef MODULES_LOCALIZATION_LMD_PREDICTOR_RAW_PREDICTOR_GPS_H_
#define MODULES_LOCALIZATION_LMD_PREDICTOR_RAW_PREDICTOR_GPS_H_
#include "modules/localization/proto/gps.pb.h"
#include "modules/localization/lmd/predictor/predictor.h"
/**
* @namespace apollo::localization
* @brief apollo::localization
*/
namespace
apollo
{
namespace
localization
{
/**
* @class PredictorImu
*
* @brief Implementation of predictor.
*/
class
PredictorGps
:
public
Predictor
{
public:
explicit
PredictorGps
(
double
memory_cycle_sec
);
virtual
~
PredictorGps
();
/**
* @brief Update poses from gps.
* @param gps The message from gps.
* @return True if success; false if not needed.
*/
bool
UpdateGps
(
const
Gps
&
gps
);
/**
* @brief Overrided implementation of the virtual function "Updateable" in the
* base class "Predictor".
* @return True if yes; no otherwise.
*/
bool
Updateable
()
const
override
;
/**
* @brief Overrided implementation of the virtual function "Update" in the
* base class "Predictor".
* @return Status::OK() if success; error otherwise.
*/
apollo
::
common
::
Status
Update
()
override
;
private:
double
latest_timestamp_sec_
;
};
}
// namespace localization
}
// namespace apollo
#endif // MODULES_LOCALIZATION_LMD_PREDICTOR_RAW_PREDICTOR_GPS_H_
modules/localization/lmd/predictor/raw/predictor_imu.cc
0 → 100644
浏览文件 @
7461c419
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include <iomanip>
#include "modules/localization/lmd/predictor/raw/predictor_imu.h"
#include "modules/common/log.h"
namespace
apollo
{
namespace
localization
{
using
apollo
::
common
::
Status
;
PredictorImu
::
PredictorImu
(
double
memory_cycle_sec
)
:
Predictor
(
memory_cycle_sec
),
raw_imu_
(
memory_cycle_sec
)
{
name_
=
kPredictorImuName
;
on_adapter_thread_
=
true
;
}
PredictorImu
::~
PredictorImu
()
{}
bool
PredictorImu
::
UpdateImu
(
const
CorrectedImu
&
imu
)
{
if
(
!
imu
.
has_header
()
||
!
imu
.
header
().
has_timestamp_sec
()
||
!
imu
.
has_imu
())
{
AERROR
<<
"Message has not some feilds"
;
return
false
;
}
auto
timestamp_sec
=
imu
.
header
().
timestamp_sec
();
if
(
!
raw_imu_
.
Push
(
timestamp_sec
,
imu
.
imu
()))
{
AWARN
<<
std
::
setprecision
(
15
)
<<
"Failed push pose to list, with timestamp["
<<
timestamp_sec
<<
"]"
;
return
false
;
}
return
WindowFilter
(
timestamp_sec
);
}
bool
PredictorImu
::
Updateable
()
const
{
return
!
predicted_
.
empty
()
&&
predicted_
.
Newer
(
latest_timestamp_sec_
);
}
Status
PredictorImu
::
Update
()
{
if
(
!
predicted_
.
empty
())
{
auto
latest
=
predicted_
.
Latest
();
latest_timestamp_sec_
=
latest
->
first
;
}
return
Status
::
OK
();
}
bool
PredictorImu
::
WindowFilter
(
double
timestamp_sec
)
{
Pose
pose
;
pose
.
mutable_linear_acceleration
()
->
set_x
(
0.0
);
pose
.
mutable_linear_acceleration
()
->
set_y
(
0.0
);
pose
.
mutable_linear_acceleration
()
->
set_z
(
0.0
);
pose
.
mutable_angular_velocity
()
->
set_x
(
0.0
);
pose
.
mutable_angular_velocity
()
->
set_y
(
0.0
);
pose
.
mutable_angular_velocity
()
->
set_z
(
0.0
);
constexpr
double
kWindowSize
=
0.20
;
constexpr
double
kSamplingInterval
=
0.01
;
std
::
size_t
count
=
0
;
for
(
auto
t
=
timestamp_sec
;
t
>=
timestamp_sec
-
kWindowSize
;
t
-=
kSamplingInterval
)
{
Pose
sample
;
raw_imu_
.
FindNearestPose
(
t
,
&
sample
);
pose
.
mutable_linear_acceleration
()
->
set_x
(
pose
.
linear_acceleration
().
x
()
+
sample
.
linear_acceleration
().
x
());
pose
.
mutable_linear_acceleration
()
->
set_y
(
pose
.
linear_acceleration
().
y
()
+
sample
.
linear_acceleration
().
y
());
pose
.
mutable_linear_acceleration
()
->
set_z
(
pose
.
linear_acceleration
().
z
()
+
sample
.
linear_acceleration
().
z
());
pose
.
mutable_angular_velocity
()
->
set_x
(
pose
.
angular_velocity
().
x
()
+
sample
.
angular_velocity
().
x
());
pose
.
mutable_angular_velocity
()
->
set_y
(
pose
.
angular_velocity
().
y
()
+
sample
.
angular_velocity
().
y
());
pose
.
mutable_angular_velocity
()
->
set_z
(
pose
.
angular_velocity
().
z
()
+
sample
.
angular_velocity
().
z
());
count
++
;
}
pose
.
mutable_linear_acceleration
()
->
set_x
(
pose
.
linear_acceleration
().
x
()
/
count
);
pose
.
mutable_linear_acceleration
()
->
set_y
(
pose
.
linear_acceleration
().
y
()
/
count
);
pose
.
mutable_linear_acceleration
()
->
set_z
(
pose
.
linear_acceleration
().
z
()
/
count
);
pose
.
mutable_angular_velocity
()
->
set_x
(
pose
.
angular_velocity
().
x
()
/
count
);
pose
.
mutable_angular_velocity
()
->
set_y
(
pose
.
angular_velocity
().
y
()
/
count
);
pose
.
mutable_angular_velocity
()
->
set_z
(
pose
.
angular_velocity
().
z
()
/
count
);
if
(
!
predicted_
.
Push
(
timestamp_sec
,
pose
))
{
AWARN
<<
std
::
setprecision
(
15
)
<<
"Failed push pose to list, with timestamp["
<<
timestamp_sec
<<
"]"
;
return
false
;
}
return
true
;
}
}
// namespace localization
}
// namespace apollo
modules/localization/lmd/predictor/raw/predictor_imu.h
0 → 100644
浏览文件 @
7461c419
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
/**
* @file predictor.h
* @brief The class of PredictorImu.
*/
#ifndef MODULES_LOCALIZATION_LMD_PREDICTOR_RAW_PREDICTOR_IMU_H_
#define MODULES_LOCALIZATION_LMD_PREDICTOR_RAW_PREDICTOR_IMU_H_
#include "modules/localization/proto/imu.pb.h"
#include "modules/localization/lmd/predictor/predictor.h"
/**
* @namespace apollo::localization
* @brief apollo::localization
*/
namespace
apollo
{
namespace
localization
{
/**
* @class PredictorImu
*
* @brief Implementation of predictor.
*/
class
PredictorImu
:
public
Predictor
{
public:
explicit
PredictorImu
(
double
memory_cycle_sec
);
virtual
~
PredictorImu
();
/**
* @brief Update poses from imu.
* @param imu The message from imu.
* @return True if success; false if not needed.
*/
bool
UpdateImu
(
const
CorrectedImu
&
imu
);
/**
* @brief Overrided implementation of the virtual function "Updateable" in the
* base class "Predictor".
* @return True if yes; no otherwise.
*/
bool
Updateable
()
const
override
;
/**
* @brief Overrided implementation of the virtual function "Update" in the
* base class "Predictor".
* @return Status::OK() if success; error otherwise.
*/
apollo
::
common
::
Status
Update
()
override
;
private:
bool
WindowFilter
(
double
timestamp_sec
);
private:
double
latest_timestamp_sec_
;
PoseList
raw_imu_
;
};
}
// namespace localization
}
// namespace apollo
#endif // MODULES_LOCALIZATION_LMD_PREDICTOR_RAW_PREDICTOR_IMU_H_
modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/cc_lane_post_processor.cc
浏览文件 @
7461c419
...
...
@@ -579,7 +579,10 @@ bool CCLanePostProcessor::ProcessWithoutCC(
Eigen
::
Matrix
<
double
,
3
,
1
>
xy_p
=
trans_mat_
*
img_point
;
Eigen
::
Matrix
<
double
,
2
,
1
>
xy_point
;
Eigen
::
Matrix
<
double
,
2
,
1
>
uv_point
;
if
(
std
::
abs
(
xy_p
(
2
))
<
1e-6
)
continue
;
if
(
std
::
abs
(
xy_p
(
2
))
<
1e-6
)
{
++
x
;
continue
;
}
xy_point
<<
xy_p
(
0
)
/
xy_p
(
2
),
xy_p
(
1
)
/
xy_p
(
2
);
if
(
xy_point
(
0
)
<
0
||
xy_point
(
0
)
>
300
||
abs
(
xy_point
(
1
))
>
30
)
{
++
x
;
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录