Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
fff57828
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,发现更多精彩内容 >>
提交
fff57828
编写于
11月 20, 2017
作者:
L
Lei Zhang
提交者:
GitHub
11月 20, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
drivers:gnss add imu parse. (#1351)
* drivers:gnss add imu parse. * drivers:gnss add imu parse.
上级
4ac90905
变更
3
隐藏空白更改
内联
并排
Showing
3 changed file
with
96 addition
and
1 deletion
+96
-1
modules/drivers/gnss/src/impl/parser/novatel/novatel_messages.h
...s/drivers/gnss/src/impl/parser/novatel/novatel_messages.h
+2
-1
modules/drivers/gnss/src/impl/parser/novatel/novatel_parser.cpp
...s/drivers/gnss/src/impl/parser/novatel/novatel_parser.cpp
+83
-0
modules/drivers/gnss/src/parser/data_parser.cpp
modules/drivers/gnss/src/parser/data_parser.cpp
+11
-0
未找到文件。
modules/drivers/gnss/src/impl/parser/novatel/novatel_messages.h
浏览文件 @
fff57828
...
...
@@ -45,6 +45,7 @@ enum MessageId : uint16_t {
PSRPOS
=
47
,
PSRVEL
=
100
,
RAWIMU
=
268
,
RAWIMUS
=
325
,
RAWIMUX
=
1461
,
RAWIMUSX
=
1462
,
MARK1PVA
=
1067
,
...
...
@@ -473,7 +474,7 @@ static_assert(sizeof(RawImuX) == 40, "Incorrect size of RawImuX");
struct
RawImu
{
uint32_t
gps_week
;
double
gps_seconds
;
// Seconds of week.
char
imuStatus
[
4
]
;
// Status of the IMU. The content varies with IMU type.
uint32_t
imuStatus
;
// Status of the IMU. The content varies with IMU type.
int32_t
z_velocity_change
;
// change in velocity along z axis.
int32_t
y_velocity_change_neg
;
// -change in velocity along y axis.
int32_t
x_velocity_change
;
// change in velocity along x axis.
...
...
modules/drivers/gnss/src/impl/parser/novatel/novatel_parser.cpp
浏览文件 @
fff57828
...
...
@@ -134,6 +134,8 @@ class NovatelParser : public Parser {
bool
handle_raw_imu_x
(
const
novatel
::
RawImuX
*
imu
);
bool
handle_raw_imu
(
const
novatel
::
RawImu
*
imu
);
bool
handle_bds_eph
(
const
novatel
::
BDS_Ephemeris
*
bds_emph
);
bool
handle_gps_eph
(
const
novatel
::
GPS_Ephemeris
*
gps_emph
);
...
...
@@ -398,6 +400,19 @@ Parser::MessageType NovatelParser::prepare_message(MessagePtr& message_ptr) {
}
break
;
case
novatel
::
RAWIMU
:
case
novatel
::
RAWIMUS
:
if
(
message_length
!=
sizeof
(
novatel
::
RawImu
))
{
ROS_ERROR
(
"Incorrect message_length"
);
break
;
}
if
(
handle_raw_imu
(
reinterpret_cast
<
novatel
::
RawImu
*>
(
message
)))
{
message_ptr
=
&
_imu
;
return
MessageType
::
IMU
;
}
break
;
case
novatel
::
INSPVAX
:
if
(
message_length
!=
sizeof
(
novatel
::
InsPvaX
))
{
ROS_ERROR
(
"Incorrect message_length"
);
...
...
@@ -736,6 +751,74 @@ bool NovatelParser::handle_raw_imu_x(const novatel::RawImuX* imu) {
return
true
;
}
bool
NovatelParser
::
handle_raw_imu
(
const
novatel
::
RawImu
*
imu
)
{
double
gyro_scale
=
0.0
;
double
accel_scale
=
0.0
;
float
imu_measurement_span
=
1.0
/
200.0
;
if
(
is_zero
(
_gyro_scale
))
{
novatel
::
ImuParameter
param
=
novatel
::
get_imu_parameter
(
novatel
::
ImuType
::
ADIS16488
);
//ROS_INFO_STREAM("IMU type: " << static_cast<unsigned>(imu->imu_type) << "; "
// << "Gyro scale: " << param.gyro_scale << "; "
// << "Accel scale: " << param.accel_scale << "; "
// << "Sampling rate: " << param.sampling_rate_hz
// << ".");
if
(
is_zero
(
param
.
sampling_rate_hz
))
{
ROS_ERROR_STREAM_THROTTLE
(
5
,
"Unsupported IMU type ADUS16488."
);
return
false
;
}
gyro_scale
=
param
.
gyro_scale
*
param
.
sampling_rate_hz
;
accel_scale
=
param
.
accel_scale
*
param
.
sampling_rate_hz
;
imu_measurement_span
=
1.0
/
param
.
sampling_rate_hz
;
_imu
.
set_measurement_span
(
imu_measurement_span
);
}
else
{
gyro_scale
=
_gyro_scale
;
accel_scale
=
_accel_scale
;
imu_measurement_span
=
_imu_measurement_span
;
_imu
.
set_measurement_span
(
imu_measurement_span
);
}
double
time
=
imu
->
gps_week
*
SECONDS_PER_WEEK
+
imu
->
gps_seconds
;
if
(
_imu_measurement_time_previous
>
0.0
&&
fabs
(
time
-
_imu_measurement_time_previous
-
imu_measurement_span
)
>
1e-4
)
{
ROS_WARN_STREAM
(
"Unexpected delay between two IMU measurements at: "
<<
time
-
_imu_measurement_time_previous
);
}
_imu
.
set_measurement_time
(
time
);
switch
(
_imu_frame_mapping
)
{
case
5
:
// Default mapping.
rfu_to_flu
(
imu
->
x_velocity_change
*
accel_scale
,
-
imu
->
y_velocity_change_neg
*
accel_scale
,
imu
->
z_velocity_change
*
accel_scale
,
_imu
.
mutable_linear_acceleration
());
rfu_to_flu
(
imu
->
x_angle_change
*
gyro_scale
,
-
imu
->
y_angle_change_neg
*
gyro_scale
,
imu
->
z_angle_change
*
gyro_scale
,
_imu
.
mutable_angular_velocity
());
break
;
case
6
:
rfu_to_flu
(
-
imu
->
y_velocity_change_neg
*
accel_scale
,
imu
->
x_velocity_change
*
accel_scale
,
-
imu
->
z_velocity_change
*
accel_scale
,
_imu
.
mutable_linear_acceleration
());
rfu_to_flu
(
-
imu
->
y_angle_change_neg
*
gyro_scale
,
imu
->
x_angle_change
*
gyro_scale
,
-
imu
->
z_angle_change
*
gyro_scale
,
_imu
.
mutable_angular_velocity
());
break
;
default:
ROS_ERROR_STREAM_THROTTLE
(
5
,
"Unsupported IMU frame mapping: "
<<
_imu_frame_mapping
);
}
_imu_measurement_time_previous
=
time
;
return
true
;
}
bool
NovatelParser
::
handle_gps_eph
(
const
novatel
::
GPS_Ephemeris
*
gps_emph
)
{
_gnss_ephemeris
.
set_gnss_type
(
apollo
::
drivers
::
gnss
::
GnssType
::
GPS_SYS
);
...
...
modules/drivers/gnss/src/parser/data_parser.cpp
浏览文件 @
fff57828
...
...
@@ -265,6 +265,17 @@ void DataParser::publish_imu_message(const MessagePtr message) {
boost
::
shared_ptr
<::
apollo
::
drivers
::
gnss
::
Imu
>
raw_imu
(
new
::
apollo
::
drivers
::
gnss
::
Imu
(
*
As
<::
apollo
::
drivers
::
gnss
::
Imu
>
(
message
)));
::
apollo
::
drivers
::
gnss
::
Imu
*
imu
=
As
<::
apollo
::
drivers
::
gnss
::
Imu
>
(
message
);
raw_imu
->
mutable_linear_acceleration
()
->
set_x
(
-
imu
->
linear_acceleration
().
y
());
raw_imu
->
mutable_linear_acceleration
()
->
set_y
(
-
imu
->
linear_acceleration
().
x
());
raw_imu
->
mutable_linear_acceleration
()
->
set_z
(
imu
->
linear_acceleration
().
z
());
raw_imu
->
mutable_angular_velocity
()
->
set_x
(
-
imu
->
angular_velocity
().
y
());
raw_imu
->
mutable_angular_velocity
()
->
set_y
(
-
imu
->
angular_velocity
().
x
());
raw_imu
->
mutable_angular_velocity
()
->
set_z
(
imu
->
angular_velocity
().
z
());
raw_imu
->
mutable_header
()
->
set_timestamp_sec
(
ros
::
Time
::
Time
::
now
().
toSec
());
_raw_imu_publisher
.
publish
(
raw_imu
);
}
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录