Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
582a082d
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,发现更多精彩内容 >>
提交
582a082d
编写于
2月 01, 2018
作者:
L
Liangliang Zhang
提交者:
Jiangtao Hu
2月 02, 2018
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
DataGenerator: refactored data generator so that we can easily register and manager sensors.
上级
c8fa7f2a
变更
6
显示空白变更内容
内联
并排
Showing
6 changed file
with
240 addition
and
124 deletion
+240
-124
modules/perception/tool/data_generator/BUILD
modules/perception/tool/data_generator/BUILD
+9
-2
modules/perception/tool/data_generator/data_generator.cc
modules/perception/tool/data_generator/data_generator.cc
+2
-111
modules/perception/tool/data_generator/data_generator.h
modules/perception/tool/data_generator/data_generator.h
+2
-11
modules/perception/tool/data_generator/sensor.h
modules/perception/tool/data_generator/sensor.h
+6
-0
modules/perception/tool/data_generator/velodyne64.cc
modules/perception/tool/data_generator/velodyne64.cc
+154
-0
modules/perception/tool/data_generator/velodyne64.h
modules/perception/tool/data_generator/velodyne64.h
+67
-0
未找到文件。
modules/perception/tool/data_generator/BUILD
浏览文件 @
582a082d
...
@@ -4,8 +4,15 @@ package(default_visibility = ["//visibility:public"])
...
@@ -4,8 +4,15 @@ package(default_visibility = ["//visibility:public"])
cc_library
(
cc_library
(
name
=
"data_generator_lib"
,
name
=
"data_generator_lib"
,
srcs
=
[
"data_generator.cc"
],
srcs
=
[
hdrs
=
[
"data_generator.h"
,
"sensor.h"
],
"data_generator.cc"
,
"velodyne64.cc"
,
],
hdrs
=
[
"data_generator.h"
,
"sensor.h"
,
"velodyne64.h"
,
],
copts
=
[
copts
=
[
"-Wno-deprecated"
,
"-Wno-deprecated"
,
],
],
...
...
modules/perception/tool/data_generator/data_generator.cc
浏览文件 @
582a082d
...
@@ -83,51 +83,16 @@ void DataGenerator::RunOnce() {
...
@@ -83,51 +83,16 @@ void DataGenerator::RunOnce() {
return
;
return
;
}
}
const
auto
&
point_cloud_msg
=
// Update VehicleState
AdapterManager
::
GetPointCloud
()
->
GetLatestObserved
();
ADEBUG
<<
"PointCloud: "
<<
point_cloud_msg
.
header
;
const
auto
&
localization
=
const
auto
&
localization
=
AdapterManager
::
GetLocalization
()
->
GetLatestObserved
();
AdapterManager
::
GetLocalization
()
->
GetLatestObserved
();
ADEBUG
<<
"Localization: "
<<
localization
.
DebugString
();
ADEBUG
<<
"Localization: "
<<
localization
.
DebugString
();
const
auto
&
chassis
=
AdapterManager
::
GetChassis
()
->
GetLatestObserved
();
const
auto
&
chassis
=
AdapterManager
::
GetChassis
()
->
GetLatestObserved
();
ADEBUG
<<
"Chassis: "
<<
chassis
.
DebugString
();
ADEBUG
<<
"Chassis: "
<<
chassis
.
DebugString
();
VehicleStateProvider
::
instance
()
->
Update
(
localization
,
chassis
);
VehicleStateProvider
::
instance
()
->
Update
(
localization
,
chassis
);
AINFO
<<
"VehicleState updated."
;
AINFO
<<
"VehicleState updated."
;
Process
(
point_cloud_msg
);
// TODO(Liangliang): register more sensors and use factory to manager.
}
bool
DataGenerator
::
Process
(
const
sensor_msgs
::
PointCloud2
&
message
)
{
PointCloudPtr
cld
(
new
PointCloud
);
TransPointCloudMsgToPCL
(
message
,
&
cld
);
AINFO
<<
"PointCloud size = "
<<
cld
->
points
.
size
();
std
::
shared_ptr
<
Matrix4d
>
velodyne_to_novatel_trans
=
std
::
make_shared
<
Matrix4d
>
();
if
(
!
GetTrans
(
FLAGS_novatel_frame_name
,
FLAGS_velodyne64_frame_name
,
message
.
header
.
stamp
.
toSec
(),
velodyne_to_novatel_trans
.
get
()))
{
AERROR
<<
"Fail to transform velodyne64 to novatel at time: "
<<
message
.
header
.
stamp
.
toSec
();
return
false
;
}
if
(
!
TransformPointCloudToWorld
(
velodyne_to_novatel_trans
,
&
cld
))
{
AERROR
<<
"Fail to transform point cloud to world."
;
return
false
;
}
// TODO(All): output labeled obstacle config data
for
(
size_t
i
=
0
;
i
<
cld
->
points
.
size
();
++
i
)
{
auto
&
point
=
cld
->
points
[
i
];
(
*
data_file_
)
<<
point
.
x
<<
" "
<<
point
.
y
<<
" "
<<
point
.
x
<<
" "
<<
point
.
intensity
<<
", "
;
}
(
*
data_file_
)
<<
"
\n
"
;
return
true
;
}
}
Status
DataGenerator
::
Start
()
{
Status
DataGenerator
::
Start
()
{
...
@@ -144,80 +109,6 @@ void DataGenerator::Stop() {
...
@@ -144,80 +109,6 @@ void DataGenerator::Stop() {
delete
data_file_
;
delete
data_file_
;
}
}
void
DataGenerator
::
TransPointCloudMsgToPCL
(
const
sensor_msgs
::
PointCloud2
&
cloud_msg
,
PointCloudPtr
*
cloud_pcl
)
{
// transform from ros to pcl
pcl
::
PointCloud
<
pcl_util
::
PointXYZIT
>
in_cloud
;
pcl
::
fromROSMsg
(
cloud_msg
,
in_cloud
);
// transform from xyzit to xyzi
PointCloudPtr
&
cloud
=
*
cloud_pcl
;
cloud
->
header
=
in_cloud
.
header
;
cloud
->
width
=
in_cloud
.
width
;
cloud
->
height
=
in_cloud
.
height
;
cloud
->
is_dense
=
in_cloud
.
is_dense
;
cloud
->
sensor_origin_
=
in_cloud
.
sensor_origin_
;
cloud
->
sensor_orientation_
=
in_cloud
.
sensor_orientation_
;
cloud
->
points
.
resize
(
in_cloud
.
points
.
size
());
size_t
points_num
=
0
;
for
(
size_t
i
=
0
;
i
<
in_cloud
.
size
();
++
i
)
{
pcl_util
::
PointXYZIT
&
pt
=
in_cloud
.
points
[
i
];
if
(
!
isnan
(
pt
.
x
)
&&
!
isnan
(
pt
.
y
)
&&
!
isnan
(
pt
.
z
)
&&
!
isnan
(
pt
.
intensity
))
{
cloud
->
points
[
points_num
].
x
=
pt
.
x
;
cloud
->
points
[
points_num
].
y
=
pt
.
y
;
cloud
->
points
[
points_num
].
z
=
pt
.
z
;
cloud
->
points
[
points_num
].
intensity
=
pt
.
intensity
;
++
points_num
;
}
}
cloud
->
points
.
resize
(
points_num
);
}
bool
DataGenerator
::
GetTrans
(
const
std
::
string
&
to_frame
,
const
std
::
string
&
from_frame
,
const
double
query_time
,
Matrix4d
*
trans
)
{
CHECK_NOTNULL
(
trans
);
ros
::
Time
query_stamp
(
query_time
);
const
auto
&
tf2_buffer
=
AdapterManager
::
Tf2Buffer
();
const
double
kTf2BuffSize
=
10
/
1000.0
;
// buff size
std
::
string
err_msg
;
if
(
!
tf2_buffer
.
canTransform
(
to_frame
,
from_frame
,
query_stamp
,
ros
::
Duration
(
kTf2BuffSize
),
&
err_msg
))
{
AERROR
<<
"Cannot transform frame from "
<<
from_frame
<<
" to frame "
<<
to_frame
<<
" , err: "
<<
err_msg
<<
". Frames: "
<<
tf2_buffer
.
allFramesAsString
();
return
false
;
}
geometry_msgs
::
TransformStamped
transform_stamped
;
try
{
transform_stamped
=
tf2_buffer
.
lookupTransform
(
to_frame
,
from_frame
,
query_stamp
);
}
catch
(
tf2
::
TransformException
&
ex
)
{
AERROR
<<
"Exception: "
<<
ex
.
what
();
return
false
;
}
Affine3d
affine_3d
;
tf
::
transformMsgToEigen
(
transform_stamped
.
transform
,
affine_3d
);
*
trans
=
affine_3d
.
matrix
();
ADEBUG
<<
from_frame
<<
" to "
<<
to_frame
<<
", trans = "
<<
*
trans
;
return
true
;
}
bool
DataGenerator
::
TransformPointCloudToWorld
(
std
::
shared_ptr
<
Matrix4d
>
velodyne_trans
,
PointCloudPtr
*
cld
)
{
Affine3d
affine_3d_trans
(
*
velodyne_trans
);
for
(
size_t
i
=
0
;
i
<
(
*
cld
)
->
points
.
size
();
++
i
)
{
auto
&
pt
=
(
*
cld
)
->
points
[
i
];
PointD
point
=
{
pt
.
x
,
pt
.
y
,
pt
.
z
,
0
};
PointD
point_world
=
pcl
::
transformPoint
(
point
,
affine_3d_trans
);
pt
.
x
=
point_world
.
x
;
pt
.
y
=
point_world
.
y
;
pt
.
z
=
point_world
.
z
;
}
return
true
;
}
}
// namespace data_generator
}
// namespace data_generator
}
// namespace perception
}
// namespace perception
}
// namespace apollo
}
// namespace apollo
modules/perception/tool/data_generator/data_generator.h
浏览文件 @
582a082d
...
@@ -24,10 +24,9 @@
...
@@ -24,10 +24,9 @@
#include <memory>
#include <memory>
#include <string>
#include <string>
#include "sensor_msgs/PointCloud2.h"
#include "Eigen/Core"
#include "Eigen/Core"
#include "ros/include/ros/ros.h"
#include "ros/include/ros/ros.h"
#include "sensor_msgs/PointCloud2.h"
#include "modules/perception/tool/data_generator/proto/config.pb.h"
#include "modules/perception/tool/data_generator/proto/config.pb.h"
...
@@ -54,16 +53,8 @@ class DataGenerator : public apollo::common::ApolloApp {
...
@@ -54,16 +53,8 @@ class DataGenerator : public apollo::common::ApolloApp {
private:
private:
void
RunOnce
();
void
RunOnce
();
bool
Process
(
const
sensor_msgs
::
PointCloud2
&
message
);
void
OnTimer
(
const
ros
::
TimerEvent
&
);
void
TransPointCloudMsgToPCL
(
const
sensor_msgs
::
PointCloud2
&
cloud_msg
,
void
OnTimer
(
const
ros
::
TimerEvent
&
);
pcl_util
::
PointCloudPtr
*
cloud_pcl
);
bool
GetTrans
(
const
std
::
string
&
to_frame
,
const
std
::
string
&
from_frame
,
const
double
query_time
,
Eigen
::
Matrix4d
*
trans
);
bool
TransformPointCloudToWorld
(
std
::
shared_ptr
<
Eigen
::
Matrix4d
>
velodyne_trans
,
pcl_util
::
PointCloudPtr
*
cld
);
void
RegisterSensors
();
void
RegisterSensors
();
...
...
modules/perception/tool/data_generator/sensor.h
浏览文件 @
582a082d
...
@@ -21,6 +21,7 @@
...
@@ -21,6 +21,7 @@
#ifndef MODULES_PERCEPTION_DATA_GENERATOR_SENSOR_H_
#ifndef MODULES_PERCEPTION_DATA_GENERATOR_SENSOR_H_
#define MODULES_PERCEPTION_DATA_GENERATOR_SENSOR_H_
#define MODULES_PERCEPTION_DATA_GENERATOR_SENSOR_H_
#include <sstream>
#include <string>
#include <string>
#include "modules/perception/tool/data_generator/proto/config.pb.h"
#include "modules/perception/tool/data_generator/proto/config.pb.h"
...
@@ -40,9 +41,14 @@ class Sensor {
...
@@ -40,9 +41,14 @@ class Sensor {
return
config_
.
sensor_frame_name
();
return
config_
.
sensor_frame_name
();
}
}
virtual
bool
Process
()
=
0
;
virtual
bool
Process
()
=
0
;
virtual
const
std
::
string
&
data
()
const
{
return
data_
;
}
protected:
protected:
SensorConfig
config_
;
SensorConfig
config_
;
std
::
stringstream
ss_
;
std
::
string
data_
;
};
};
}
// namespace data_generator
}
// namespace data_generator
...
...
modules/perception/tool/data_generator/velodyne64.cc
0 → 100644
浏览文件 @
582a082d
/******************************************************************************
* 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
**/
#include "modules/perception/tool/data_generator/velodyne64.h"
namespace
apollo
{
namespace
perception
{
namespace
data_generator
{
using
apollo
::
common
::
ErrorCode
;
using
apollo
::
common
::
Status
;
using
apollo
::
common
::
VehicleState
;
using
apollo
::
common
::
VehicleStateProvider
;
using
apollo
::
common
::
adapter
::
AdapterManager
;
using
apollo
::
perception
::
pcl_util
::
PointCloud
;
using
apollo
::
perception
::
pcl_util
::
PointCloudPtr
;
using
apollo
::
perception
::
pcl_util
::
PointD
;
using
apollo
::
perception
::
pcl_util
::
PointXYZIT
;
using
Eigen
::
Affine3d
;
using
Eigen
::
Matrix4d
;
bool
Velodyne64
::
Process
()
{
const
auto
&
point_cloud_msg
=
AdapterManager
::
GetPointCloud
()
->
GetLatestObserved
();
ADEBUG
<<
"PointCloud: "
<<
point_cloud_msg
.
header
;
ProcessPointCloudData
(
point_cloud_msg
);
return
true
;
}
bool
Velodyne64
::
ProcessPointCloudData
(
const
sensor_msgs
::
PointCloud2
&
message
)
{
PointCloudPtr
cld
(
new
PointCloud
);
TransPointCloudMsgToPCL
(
message
,
&
cld
);
AINFO
<<
"PointCloud size = "
<<
cld
->
points
.
size
();
std
::
shared_ptr
<
Matrix4d
>
velodyne_to_novatel_trans
=
std
::
make_shared
<
Matrix4d
>
();
if
(
!
GetTrans
(
FLAGS_novatel_frame_name
,
FLAGS_velodyne64_frame_name
,
message
.
header
.
stamp
.
toSec
(),
velodyne_to_novatel_trans
.
get
()))
{
AERROR
<<
"Fail to transform velodyne64 to novatel at time: "
<<
message
.
header
.
stamp
.
toSec
();
return
false
;
}
if
(
!
TransformPointCloudToWorld
(
velodyne_to_novatel_trans
,
&
cld
))
{
AERROR
<<
"Fail to transform point cloud to world."
;
return
false
;
}
for
(
size_t
i
=
0
;
i
<
cld
->
points
.
size
();
++
i
)
{
auto
&
point
=
cld
->
points
[
i
];
ss_
<<
point
.
x
<<
" "
<<
point
.
y
<<
" "
<<
point
.
x
<<
" "
<<
point
.
intensity
<<
", "
;
}
ss_
<<
"
\n
"
;
data_
=
ss_
.
str
();
return
true
;
}
void
Velodyne64
::
TransPointCloudMsgToPCL
(
const
sensor_msgs
::
PointCloud2
&
cloud_msg
,
PointCloudPtr
*
cloud_pcl
)
{
// transform from ros to pcl
pcl
::
PointCloud
<
pcl_util
::
PointXYZIT
>
in_cloud
;
pcl
::
fromROSMsg
(
cloud_msg
,
in_cloud
);
// transform from xyzit to xyzi
PointCloudPtr
&
cloud
=
*
cloud_pcl
;
cloud
->
header
=
in_cloud
.
header
;
cloud
->
width
=
in_cloud
.
width
;
cloud
->
height
=
in_cloud
.
height
;
cloud
->
is_dense
=
in_cloud
.
is_dense
;
cloud
->
sensor_origin_
=
in_cloud
.
sensor_origin_
;
cloud
->
sensor_orientation_
=
in_cloud
.
sensor_orientation_
;
cloud
->
points
.
resize
(
in_cloud
.
points
.
size
());
size_t
points_num
=
0
;
for
(
size_t
i
=
0
;
i
<
in_cloud
.
size
();
++
i
)
{
pcl_util
::
PointXYZIT
&
pt
=
in_cloud
.
points
[
i
];
if
(
!
isnan
(
pt
.
x
)
&&
!
isnan
(
pt
.
y
)
&&
!
isnan
(
pt
.
z
)
&&
!
isnan
(
pt
.
intensity
))
{
cloud
->
points
[
points_num
].
x
=
pt
.
x
;
cloud
->
points
[
points_num
].
y
=
pt
.
y
;
cloud
->
points
[
points_num
].
z
=
pt
.
z
;
cloud
->
points
[
points_num
].
intensity
=
pt
.
intensity
;
++
points_num
;
}
}
cloud
->
points
.
resize
(
points_num
);
}
bool
Velodyne64
::
GetTrans
(
const
std
::
string
&
to_frame
,
const
std
::
string
&
from_frame
,
const
double
query_time
,
Matrix4d
*
trans
)
{
CHECK_NOTNULL
(
trans
);
ros
::
Time
query_stamp
(
query_time
);
const
auto
&
tf2_buffer
=
AdapterManager
::
Tf2Buffer
();
const
double
kTf2BuffSize
=
10
/
1000.0
;
// buff size
std
::
string
err_msg
;
if
(
!
tf2_buffer
.
canTransform
(
to_frame
,
from_frame
,
query_stamp
,
ros
::
Duration
(
kTf2BuffSize
),
&
err_msg
))
{
AERROR
<<
"Cannot transform frame from "
<<
from_frame
<<
" to frame "
<<
to_frame
<<
" , err: "
<<
err_msg
<<
". Frames: "
<<
tf2_buffer
.
allFramesAsString
();
return
false
;
}
geometry_msgs
::
TransformStamped
transform_stamped
;
try
{
transform_stamped
=
tf2_buffer
.
lookupTransform
(
to_frame
,
from_frame
,
query_stamp
);
}
catch
(
tf2
::
TransformException
&
ex
)
{
AERROR
<<
"Exception: "
<<
ex
.
what
();
return
false
;
}
Affine3d
affine_3d
;
tf
::
transformMsgToEigen
(
transform_stamped
.
transform
,
affine_3d
);
*
trans
=
affine_3d
.
matrix
();
ADEBUG
<<
from_frame
<<
" to "
<<
to_frame
<<
", trans = "
<<
*
trans
;
return
true
;
}
bool
Velodyne64
::
TransformPointCloudToWorld
(
std
::
shared_ptr
<
Matrix4d
>
velodyne_trans
,
PointCloudPtr
*
cld
)
{
Affine3d
affine_3d_trans
(
*
velodyne_trans
);
for
(
size_t
i
=
0
;
i
<
(
*
cld
)
->
points
.
size
();
++
i
)
{
auto
&
pt
=
(
*
cld
)
->
points
[
i
];
PointD
point
=
{
pt
.
x
,
pt
.
y
,
pt
.
z
,
0
};
PointD
point_world
=
pcl
::
transformPoint
(
point
,
affine_3d_trans
);
pt
.
x
=
point_world
.
x
;
pt
.
y
=
point_world
.
y
;
pt
.
z
=
point_world
.
z
;
}
return
true
;
}
}
// namespace data_generator
}
// namespace perception
}
// namespace apollo
modules/perception/tool/data_generator/velodyne64.h
0 → 100644
浏览文件 @
582a082d
/******************************************************************************
* 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
**/
#ifndef MODULES_PERCEPTION_DATA_GENERATOR_VELODYNE64_H_
#define MODULES_PERCEPTION_DATA_GENERATOR_VELODYNE64_H_
#include <memory>
#include <string>
#include "Eigen/Core"
#include "eigen_conversions/eigen_msg.h"
#include "pcl_conversions/pcl_conversions.h"
#include "ros/include/ros/ros.h"
#include "sensor_msgs/PointCloud2.h"
#include "modules/perception/tool/data_generator/proto/config.pb.h"
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/log.h"
#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/perception/lib/pcl_util/pcl_types.h"
#include "modules/perception/tool/data_generator/common/data_generator_gflags.h"
#include "modules/perception/tool/data_generator/sensor.h"
namespace
apollo
{
namespace
perception
{
namespace
data_generator
{
class
Velodyne64
:
public
Sensor
{
public:
explicit
Velodyne64
(
const
SensorConfig
&
config
)
:
Sensor
(
config
)
{}
virtual
~
Velodyne64
()
=
default
;
bool
Process
()
override
;
private:
bool
ProcessPointCloudData
(
const
sensor_msgs
::
PointCloud2
&
message
);
void
TransPointCloudMsgToPCL
(
const
sensor_msgs
::
PointCloud2
&
cloud_msg
,
pcl_util
::
PointCloudPtr
*
cloud_pcl
);
bool
GetTrans
(
const
std
::
string
&
to_frame
,
const
std
::
string
&
from_frame
,
const
double
query_time
,
Eigen
::
Matrix4d
*
trans
);
bool
TransformPointCloudToWorld
(
std
::
shared_ptr
<
Eigen
::
Matrix4d
>
velodyne_trans
,
pcl_util
::
PointCloudPtr
*
cld
);
};
}
// namespace data_generator
}
// namespace perception
}
// namespace apollo
#endif // MODULES_PERCEPTION_DATA_GENERATOR_VELODYNE64_H_
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录