Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
a13d35c4
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,发现更多精彩内容 >>
提交
a13d35c4
编写于
1月 10, 2018
作者:
L
Liangliang Zhang
提交者:
Calvin Miao
1月 10, 2018
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
driviers: fixed code issues in velodyne driver.
上级
41b07434
变更
10
隐藏空白更改
内联
并排
Showing
10 changed file
with
311 addition
and
497 deletion
+311
-497
modules/drivers/gnss/src/impl/parser/novatel/novatel_parser.cpp
...s/drivers/gnss/src/impl/parser/novatel/novatel_parser.cpp
+11
-12
modules/drivers/gnss/src/impl/parser/rtcm/rtcm3_parser.cpp
modules/drivers/gnss/src/impl/parser/rtcm/rtcm3_parser.cpp
+4
-9
modules/drivers/usb_cam/src/usb_cam.cpp
modules/drivers/usb_cam/src/usb_cam.cpp
+276
-461
modules/drivers/velodyne/velodyne_driver/src/driver/driver.h
modules/drivers/velodyne/velodyne_driver/src/driver/driver.h
+8
-5
modules/drivers/velodyne/velodyne_driver/src/driver/driver16.cpp
.../drivers/velodyne/velodyne_driver/src/driver/driver16.cpp
+1
-1
modules/drivers/velodyne/velodyne_driver/src/driver/driver64.cpp
.../drivers/velodyne/velodyne_driver/src/driver/driver64.cpp
+2
-2
modules/drivers/velodyne/velodyne_pointcloud/src/lib/calibration.cpp
...vers/velodyne/velodyne_pointcloud/src/lib/calibration.cpp
+2
-2
modules/drivers/velodyne/velodyne_pointcloud/src/lib/online_calibration.cpp
...lodyne/velodyne_pointcloud/src/lib/online_calibration.cpp
+5
-2
modules/drivers/velodyne/velodyne_pointcloud/src/lib/velodyne16_parser.cpp
...elodyne/velodyne_pointcloud/src/lib/velodyne16_parser.cpp
+1
-2
modules/drivers/velodyne/velodyne_pointcloud/src/lib/velodyne_parser.cpp
.../velodyne/velodyne_pointcloud/src/lib/velodyne_parser.cpp
+1
-1
未找到文件。
modules/drivers/gnss/src/impl/parser/novatel/novatel_parser.cpp
浏览文件 @
a13d35c4
...
...
@@ -757,17 +757,19 @@ bool NovatelParser::handle_raw_imu(const novatel::RawImu* imu) {
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) << "; "
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
// << "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."
);
ROS_ERROR_STREAM_THROTTLE
(
5
,
"Unsupported IMU type ADUS16488."
);
return
false
;
}
gyro_scale
=
param
.
gyro_scale
*
param
.
sampling_rate_hz
;
...
...
@@ -938,9 +940,7 @@ bool NovatelParser::handle_glo_eph(const novatel::GLO_Ephemeris* glo_emph) {
void
NovatelParser
::
set_observation_time
()
{
int
week
=
0
;
double
second
=
0.0
;
second
=
time2gpst
(
_raw
.
time
,
&
week
);
double
second
=
time2gpst
(
_raw
.
time
,
&
week
);
_gnss_observation
.
set_gnss_time_type
(
apollo
::
drivers
::
gnss
::
GPS_TIME
);
_gnss_observation
.
set_gnss_week
(
week
);
_gnss_observation
.
set_gnss_second_s
(
second
);
...
...
@@ -948,9 +948,8 @@ void NovatelParser::set_observation_time() {
bool
NovatelParser
::
decode_gnss_observation
(
const
uint8_t
*
obs_data
,
const
uint8_t
*
obs_data_end
)
{
int
status
=
0
;
while
(
obs_data
<
obs_data_end
)
{
status
=
input_oem4
(
&
_raw
,
*
obs_data
++
);
const
int
status
=
input_oem4
(
&
_raw
,
*
obs_data
++
);
switch
(
status
)
{
case
1
:
// observation data
if
(
_raw
.
obs
.
n
==
0
)
{
...
...
modules/drivers/gnss/src/impl/parser/rtcm/rtcm3_parser.cpp
浏览文件 @
a13d35c4
...
...
@@ -41,7 +41,7 @@ constexpr bool is_zero(T value) {
class
Rtcm3Parser
:
public
Parser
{
public:
Rtcm3Parser
(
bool
is_base_satation
);
explicit
Rtcm3Parser
(
bool
is_base_satation
);
virtual
MessageType
get_message
(
MessagePtr
&
message_ptr
);
private:
...
...
@@ -168,9 +168,8 @@ void Rtcm3Parser::fill_glonass_orbit(
// orbit.set_tk(eph.tof.time + eph.tof.sec);
int
week
=
0
;
double
second
=
0.0
;
second
=
time2gpst
(
eph
.
toe
,
&
week
);
double
second
=
time2gpst
(
eph
.
toe
,
&
week
);
orbit
.
set_week_num
(
week
);
orbit
.
set_week_second_s
(
second
);
orbit
.
set_toe
(
second
);
...
...
@@ -187,10 +186,7 @@ void Rtcm3Parser::fill_glonass_orbit(
void
Rtcm3Parser
::
set_observation_time
()
{
int
week
=
0
;
double
second
=
0.0
;
second
=
time2gpst
(
_rtcm
.
time
,
&
week
);
double
second
=
time2gpst
(
_rtcm
.
time
,
&
week
);
_observation
.
set_gnss_time_type
(
apollo
::
drivers
::
gnss
::
GPS_TIME
);
_observation
.
set_gnss_week
(
week
);
_observation
.
set_gnss_second_s
(
second
);
...
...
@@ -201,9 +197,8 @@ Parser::MessageType Rtcm3Parser::get_message(MessagePtr &message_ptr) {
return
MessageType
::
NONE
;
}
int
status
=
0
;
while
(
_data
<
_data_end
)
{
status
=
input_rtcm3
(
&
_rtcm
,
*
_data
++
);
// parse data use rtklib
const
int
status
=
input_rtcm3
(
&
_rtcm
,
*
_data
++
);
// parse data use rtklib
switch
(
status
)
{
case
1
:
// observation data
...
...
modules/drivers/usb_cam/src/usb_cam.cpp
浏览文件 @
a13d35c4
...
...
@@ -34,40 +34,38 @@
*
*********************************************************************/
#define __STDC_CONSTANT_MACROS
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <fcntl.h>
/* low-level i/o */
#include <unistd.h>
#include <errno.h>
#include <fcntl.h>
/* low-level i/o */
#include <malloc.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/
mman
.h>
#include <
sys/ioctl
.h>
#include <sys/
types
.h>
#include <
unistd
.h>
#include <iostream>
#include <sstream>
#include <ros/ros.h>
#include <boost/lexical_cast.hpp>
#include <sensor_msgs/fill_image.h>
#include <boost/lexical_cast.hpp>
#include <usb_cam/usb_cam.h>
#define CLEAR(x) memset
(&(x), 0, sizeof
(x))
#define CLEAR(x) memset
(&(x), 0, sizeof
(x))
namespace
usb_cam
{
static
void
errno_exit
(
const
char
*
s
)
{
static
void
errno_exit
(
const
char
*
s
)
{
ROS_ERROR
(
"%s error %d, %s"
,
s
,
errno
,
strerror
(
errno
));
exit
(
EXIT_FAILURE
);
}
static
int
xioctl
(
int
fd
,
int
request
,
void
*
arg
)
{
static
int
xioctl
(
int
fd
,
int
request
,
void
*
arg
)
{
int
r
;
do
...
...
@@ -78,168 +76,78 @@ static int xioctl(int fd, int request, void * arg)
}
const
unsigned
char
uchar_clipping_table
[]
=
{
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -128 - -121
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -120 - -113
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -112 - -105
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -104 - -97
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -96 - -89
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -88 - -81
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -80 - -73
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -72 - -65
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -64 - -57
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -56 - -49
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -48 - -41
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -40 - -33
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -32 - -25
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -24 - -17
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -16 - -9
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -8 - -1
0
,
1
,
2
,
3
,
4
,
5
,
6
,
7
,
8
,
9
,
10
,
11
,
12
,
13
,
14
,
15
,
16
,
17
,
18
,
19
,
20
,
21
,
22
,
23
,
24
,
25
,
26
,
27
,
28
,
29
,
30
,
31
,
32
,
33
,
34
,
35
,
36
,
37
,
38
,
39
,
40
,
41
,
42
,
43
,
44
,
45
,
46
,
47
,
48
,
49
,
50
,
51
,
52
,
53
,
54
,
55
,
56
,
57
,
58
,
59
,
60
,
61
,
62
,
63
,
64
,
65
,
66
,
67
,
68
,
69
,
70
,
71
,
72
,
73
,
74
,
75
,
76
,
77
,
78
,
79
,
80
,
81
,
82
,
83
,
84
,
85
,
86
,
87
,
88
,
89
,
90
,
91
,
92
,
93
,
94
,
95
,
96
,
97
,
98
,
99
,
100
,
101
,
102
,
103
,
104
,
105
,
106
,
107
,
108
,
109
,
110
,
111
,
112
,
113
,
114
,
115
,
116
,
117
,
118
,
119
,
120
,
121
,
122
,
123
,
124
,
125
,
126
,
127
,
128
,
129
,
130
,
131
,
132
,
133
,
134
,
135
,
136
,
137
,
138
,
139
,
140
,
141
,
142
,
143
,
144
,
145
,
146
,
147
,
148
,
149
,
150
,
151
,
152
,
153
,
154
,
155
,
156
,
157
,
158
,
159
,
160
,
161
,
162
,
163
,
164
,
165
,
166
,
167
,
168
,
169
,
170
,
171
,
172
,
173
,
174
,
175
,
176
,
177
,
178
,
179
,
180
,
181
,
182
,
183
,
184
,
185
,
186
,
187
,
188
,
189
,
190
,
191
,
192
,
193
,
194
,
195
,
196
,
197
,
198
,
199
,
200
,
201
,
202
,
203
,
204
,
205
,
206
,
207
,
208
,
209
,
210
,
211
,
212
,
213
,
214
,
215
,
216
,
217
,
218
,
219
,
220
,
221
,
222
,
223
,
224
,
225
,
226
,
227
,
228
,
229
,
230
,
231
,
232
,
233
,
234
,
235
,
236
,
237
,
238
,
239
,
240
,
241
,
242
,
243
,
244
,
245
,
246
,
247
,
248
,
249
,
250
,
251
,
252
,
253
,
254
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 256-263
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 264-271
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 272-279
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 280-287
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 288-295
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 296-303
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 304-311
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 312-319
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 320-327
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 328-335
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 336-343
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 344-351
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 352-359
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 360-367
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 368-375
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 376-383
};
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -128 - -121
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -120 - -113
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -112 - -105
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -104 - -97
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -96 - -89
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -88 - -81
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -80 - -73
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -72 - -65
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -64 - -57
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -56 - -49
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -48 - -41
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -40 - -33
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -32 - -25
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -24 - -17
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -16 - -9
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
// -8 - -1
0
,
1
,
2
,
3
,
4
,
5
,
6
,
7
,
8
,
9
,
10
,
11
,
12
,
13
,
14
,
15
,
16
,
17
,
18
,
19
,
20
,
21
,
22
,
23
,
24
,
25
,
26
,
27
,
28
,
29
,
30
,
31
,
32
,
33
,
34
,
35
,
36
,
37
,
38
,
39
,
40
,
41
,
42
,
43
,
44
,
45
,
46
,
47
,
48
,
49
,
50
,
51
,
52
,
53
,
54
,
55
,
56
,
57
,
58
,
59
,
60
,
61
,
62
,
63
,
64
,
65
,
66
,
67
,
68
,
69
,
70
,
71
,
72
,
73
,
74
,
75
,
76
,
77
,
78
,
79
,
80
,
81
,
82
,
83
,
84
,
85
,
86
,
87
,
88
,
89
,
90
,
91
,
92
,
93
,
94
,
95
,
96
,
97
,
98
,
99
,
100
,
101
,
102
,
103
,
104
,
105
,
106
,
107
,
108
,
109
,
110
,
111
,
112
,
113
,
114
,
115
,
116
,
117
,
118
,
119
,
120
,
121
,
122
,
123
,
124
,
125
,
126
,
127
,
128
,
129
,
130
,
131
,
132
,
133
,
134
,
135
,
136
,
137
,
138
,
139
,
140
,
141
,
142
,
143
,
144
,
145
,
146
,
147
,
148
,
149
,
150
,
151
,
152
,
153
,
154
,
155
,
156
,
157
,
158
,
159
,
160
,
161
,
162
,
163
,
164
,
165
,
166
,
167
,
168
,
169
,
170
,
171
,
172
,
173
,
174
,
175
,
176
,
177
,
178
,
179
,
180
,
181
,
182
,
183
,
184
,
185
,
186
,
187
,
188
,
189
,
190
,
191
,
192
,
193
,
194
,
195
,
196
,
197
,
198
,
199
,
200
,
201
,
202
,
203
,
204
,
205
,
206
,
207
,
208
,
209
,
210
,
211
,
212
,
213
,
214
,
215
,
216
,
217
,
218
,
219
,
220
,
221
,
222
,
223
,
224
,
225
,
226
,
227
,
228
,
229
,
230
,
231
,
232
,
233
,
234
,
235
,
236
,
237
,
238
,
239
,
240
,
241
,
242
,
243
,
244
,
245
,
246
,
247
,
248
,
249
,
250
,
251
,
252
,
253
,
254
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 256-263
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 264-271
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 272-279
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 280-287
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 288-295
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 296-303
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 304-311
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 312-319
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 320-327
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 328-335
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 336-343
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 344-351
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 352-359
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 360-367
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 368-375
255
,
255
,
255
,
255
,
255
,
255
,
255
,
255
,
// 376-383
};
const
int
clipping_table_offset
=
128
;
/** Clip a value to the range 0<val<255. For speed this is done using an
* array, so can only cope with numbers in the range -128<val<383.
*/
static
unsigned
char
CLIPVALUE
(
int
val
)
{
static
unsigned
char
CLIPVALUE
(
int
val
)
{
// Old method (if)
/* val = val < 0 ? 0 : val; */
/* return val > 255 ? 255 : val; */
...
...
@@ -269,13 +177,13 @@ static unsigned char CLIPVALUE(int val)
* [ B ] [ 1.0 2.041 0.002 ] [ V ]
*
*/
static
void
YUV2RGB
(
const
unsigned
char
y
,
const
unsigned
char
u
,
const
unsigned
char
v
,
unsigned
char
*
r
,
unsigned
char
*
g
,
unsigned
char
*
b
)
{
static
void
YUV2RGB
(
const
unsigned
char
y
,
const
unsigned
char
u
,
const
unsigned
char
v
,
unsigned
char
*
r
,
unsigned
char
*
g
,
unsigned
char
*
b
)
{
const
int
y2
=
(
int
)
y
;
const
int
u2
=
(
int
)
u
-
128
;
const
int
v2
=
(
int
)
v
-
128
;
//std::cerr << "YUV=("<<y2<<","<<u2<<","<<v2<<")"<<std::endl;
//
std::cerr << "YUV=("<<y2<<","<<u2<<","<<v2<<")"<<std::endl;
// This is the normal YUV conversion, but
// appears to be incorrect for the firewire cameras
...
...
@@ -286,7 +194,7 @@ static void YUV2RGB(const unsigned char y, const unsigned char u, const unsigned
int
r2
=
y2
+
((
v2
*
37221
)
>>
15
);
int
g2
=
y2
-
(((
u2
*
12975
)
+
(
v2
*
18949
))
>>
15
);
int
b2
=
y2
+
((
u2
*
66883
)
>>
15
);
//std::cerr << " RGB=("<<r2<<","<<g2<<","<<b2<<")"<<std::endl;
//
std::cerr << " RGB=("<<r2<<","<<g2<<","<<b2<<")"<<std::endl;
// Cap the values.
*
r
=
CLIPVALUE
(
r2
);
...
...
@@ -294,13 +202,11 @@ static void YUV2RGB(const unsigned char y, const unsigned char u, const unsigned
*
b
=
CLIPVALUE
(
b2
);
}
void
uyvy2rgb
(
char
*
YUV
,
char
*
RGB
,
int
NumPixels
)
{
void
uyvy2rgb
(
char
*
YUV
,
char
*
RGB
,
int
NumPixels
)
{
int
i
,
j
;
unsigned
char
y0
,
y1
,
u
,
v
;
unsigned
char
r
,
g
,
b
;
for
(
i
=
0
,
j
=
0
;
i
<
(
NumPixels
<<
1
);
i
+=
4
,
j
+=
6
)
{
for
(
i
=
0
,
j
=
0
;
i
<
(
NumPixels
<<
1
);
i
+=
4
,
j
+=
6
)
{
u
=
(
unsigned
char
)
YUV
[
i
+
0
];
y0
=
(
unsigned
char
)
YUV
[
i
+
1
];
v
=
(
unsigned
char
)
YUV
[
i
+
2
];
...
...
@@ -316,24 +222,22 @@ void uyvy2rgb(char *YUV, char *RGB, int NumPixels)
}
}
static
void
mono102mono8
(
char
*
RAW
,
char
*
MONO
,
int
NumPixels
)
{
static
void
mono102mono8
(
char
*
RAW
,
char
*
MONO
,
int
NumPixels
)
{
int
i
,
j
;
for
(
i
=
0
,
j
=
0
;
i
<
(
NumPixels
<<
1
);
i
+=
2
,
j
+=
1
)
{
//first byte is low byte, second byte is high byte; smash together and convert to 8-bit
MONO
[
j
]
=
(
unsigned
char
)(((
RAW
[
i
+
0
]
>>
2
)
&
0x3F
)
|
((
RAW
[
i
+
1
]
<<
6
)
&
0xC0
));
for
(
i
=
0
,
j
=
0
;
i
<
(
NumPixels
<<
1
);
i
+=
2
,
j
+=
1
)
{
// first byte is low byte, second byte is high byte; smash together and
// convert to 8-bit
MONO
[
j
]
=
(
unsigned
char
)(((
RAW
[
i
+
0
]
>>
2
)
&
0x3F
)
|
((
RAW
[
i
+
1
]
<<
6
)
&
0xC0
));
}
}
static
void
yuyv2rgb
(
char
*
YUV
,
char
*
RGB
,
int
NumPixels
)
{
static
void
yuyv2rgb
(
char
*
YUV
,
char
*
RGB
,
int
NumPixels
)
{
int
i
,
j
;
unsigned
char
y0
,
y1
,
u
,
v
;
unsigned
char
r
,
g
,
b
;
for
(
i
=
0
,
j
=
0
;
i
<
(
NumPixels
<<
1
);
i
+=
4
,
j
+=
6
)
{
for
(
i
=
0
,
j
=
0
;
i
<
(
NumPixels
<<
1
);
i
+=
4
,
j
+=
6
)
{
y0
=
(
unsigned
char
)
YUV
[
i
+
0
];
u
=
(
unsigned
char
)
YUV
[
i
+
1
];
y1
=
(
unsigned
char
)
YUV
[
i
+
2
];
...
...
@@ -349,28 +253,32 @@ static void yuyv2rgb(char *YUV, char *RGB, int NumPixels)
}
}
void
rgb242rgb
(
char
*
YUV
,
char
*
RGB
,
int
NumPixels
)
{
void
rgb242rgb
(
char
*
YUV
,
char
*
RGB
,
int
NumPixels
)
{
memcpy
(
RGB
,
YUV
,
NumPixels
*
3
);
}
UsbCam
::
UsbCam
()
:
io_
(
IO_METHOD_MMAP
),
fd_
(
-
1
),
n_buffers_
(
0
),
avframe_camera_
(
NULL
),
avframe_rgb_
(
NULL
),
avcodec_
(
NULL
),
avoptions_
(
NULL
),
avcodec_context_
(
NULL
),
avframe_camera_size_
(
0
),
avframe_rgb_size_
(
0
),
video_sws_
(
NULL
),
image_
(
NULL
),
is_capturing_
(
false
)
{
}
UsbCam
::~
UsbCam
()
{
:
io_
(
IO_METHOD_MMAP
),
fd_
(
-
1
),
n_buffers_
(
0
),
avframe_camera_
(
NULL
),
avframe_rgb_
(
NULL
),
avcodec_
(
NULL
),
avoptions_
(
NULL
),
avcodec_context_
(
NULL
),
avframe_camera_size_
(
0
),
avframe_rgb_size_
(
0
),
video_sws_
(
NULL
),
image_
(
NULL
),
is_capturing_
(
false
)
{}
UsbCam
::~
UsbCam
()
{
shutdown
();
}
int
UsbCam
::
init_mjpeg_decoder
(
int
image_width
,
int
image_height
)
{
int
UsbCam
::
init_mjpeg_decoder
(
int
image_width
,
int
image_height
)
{
avcodec_register_all
();
avcodec_
=
avcodec_find_decoder
(
AV_CODEC_ID_MJPEG
);
if
(
!
avcodec_
)
{
if
(
!
avcodec_
)
{
ROS_ERROR
(
"Could not find MJPEG decoder"
);
return
0
;
}
...
...
@@ -379,7 +287,8 @@ int UsbCam::init_mjpeg_decoder(int image_width, int image_height)
avframe_camera_
=
avcodec_alloc_frame
();
avframe_rgb_
=
avcodec_alloc_frame
();
avpicture_alloc
((
AVPicture
*
)
avframe_rgb_
,
PIX_FMT_RGB24
,
image_width
,
image_height
);
avpicture_alloc
((
AVPicture
*
)
avframe_rgb_
,
PIX_FMT_RGB24
,
image_width
,
image_height
);
avcodec_context_
->
codec_id
=
AV_CODEC_ID_MJPEG
;
avcodec_context_
->
width
=
image_width
;
...
...
@@ -390,20 +299,20 @@ int UsbCam::init_mjpeg_decoder(int image_width, int image_height)
avcodec_context_
->
codec_type
=
AVMEDIA_TYPE_VIDEO
;
#endif
avframe_camera_size_
=
avpicture_get_size
(
PIX_FMT_YUV422P
,
image_width
,
image_height
);
avframe_rgb_size_
=
avpicture_get_size
(
PIX_FMT_RGB24
,
image_width
,
image_height
);
avframe_camera_size_
=
avpicture_get_size
(
PIX_FMT_YUV422P
,
image_width
,
image_height
);
avframe_rgb_size_
=
avpicture_get_size
(
PIX_FMT_RGB24
,
image_width
,
image_height
);
/* open it */
if
(
avcodec_open2
(
avcodec_context_
,
avcodec_
,
&
avoptions_
)
<
0
)
{
if
(
avcodec_open2
(
avcodec_context_
,
avcodec_
,
&
avoptions_
)
<
0
)
{
ROS_ERROR
(
"Could not open MJPEG Decoder"
);
return
0
;
}
return
1
;
}
void
UsbCam
::
mjpeg2rgb
(
char
*
MJPEG
,
int
len
,
char
*
RGB
,
int
NumPixels
)
{
void
UsbCam
::
mjpeg2rgb
(
char
*
MJPEG
,
int
len
,
char
*
RGB
,
int
NumPixels
)
{
int
got_picture
;
memset
(
RGB
,
0
,
avframe_rgb_size_
);
...
...
@@ -414,20 +323,20 @@ void UsbCam::mjpeg2rgb(char *MJPEG, int len, char *RGB, int NumPixels)
av_init_packet
(
&
avpkt
);
avpkt
.
size
=
len
;
avpkt
.
data
=
(
unsigned
char
*
)
MJPEG
;
decoded_len
=
avcodec_decode_video2
(
avcodec_context_
,
avframe_camera_
,
&
got_picture
,
&
avpkt
);
avpkt
.
data
=
(
unsigned
char
*
)
MJPEG
;
decoded_len
=
avcodec_decode_video2
(
avcodec_context_
,
avframe_camera_
,
&
got_picture
,
&
avpkt
);
if
(
decoded_len
<
0
)
{
if
(
decoded_len
<
0
)
{
ROS_ERROR
(
"Error while decoding frame."
);
return
;
}
#else
avcodec_decode_video
(
avcodec_context_
,
avframe_camera_
,
&
got_picture
,
(
uint8_t
*
)
MJPEG
,
len
);
avcodec_decode_video
(
avcodec_context_
,
avframe_camera_
,
&
got_picture
,
(
uint8_t
*
)
MJPEG
,
len
);
#endif
if
(
!
got_picture
)
{
if
(
!
got_picture
)
{
ROS_ERROR
(
"Webcam: expected picture but didn't get it..."
);
return
;
}
...
...
@@ -435,30 +344,32 @@ void UsbCam::mjpeg2rgb(char *MJPEG, int len, char *RGB, int NumPixels)
int
xsize
=
avcodec_context_
->
width
;
int
ysize
=
avcodec_context_
->
height
;
int
pic_size
=
avpicture_get_size
(
avcodec_context_
->
pix_fmt
,
xsize
,
ysize
);
if
(
pic_size
!=
avframe_camera_size_
)
{
ROS_ERROR
(
"outbuf size mismatch. pic_size: %d bufsize: %d"
,
pic_size
,
avframe_camera_size_
);
if
(
pic_size
!=
avframe_camera_size_
)
{
ROS_ERROR
(
"outbuf size mismatch. pic_size: %d bufsize: %d"
,
pic_size
,
avframe_camera_size_
);
return
;
}
video_sws_
=
sws_getContext
(
xsize
,
ysize
,
avcodec_context_
->
pix_fmt
,
xsize
,
ysize
,
PIX_FMT_RGB24
,
SWS_BILINEAR
,
NULL
,
NULL
,
NULL
);
sws_scale
(
video_sws_
,
avframe_camera_
->
data
,
avframe_camera_
->
linesize
,
0
,
ysize
,
avframe_rgb_
->
data
,
avframe_rgb_
->
linesize
);
video_sws_
=
sws_getContext
(
xsize
,
ysize
,
avcodec_context_
->
pix_fmt
,
xsize
,
ysize
,
PIX_FMT_RGB24
,
SWS_BILINEAR
,
NULL
,
NULL
,
NULL
);
sws_scale
(
video_sws_
,
avframe_camera_
->
data
,
avframe_camera_
->
linesize
,
0
,
ysize
,
avframe_rgb_
->
data
,
avframe_rgb_
->
linesize
);
sws_freeContext
(
video_sws_
);
int
size
=
avpicture_layout
((
AVPicture
*
)
avframe_rgb_
,
PIX_FMT_RGB24
,
xsize
,
ysize
,
(
uint8_t
*
)
RGB
,
avframe_rgb_size_
);
if
(
size
!=
avframe_rgb_size_
)
{
int
size
=
avpicture_layout
((
AVPicture
*
)
avframe_rgb_
,
PIX_FMT_RGB24
,
xsize
,
ysize
,
(
uint8_t
*
)
RGB
,
avframe_rgb_size_
);
if
(
size
!=
avframe_rgb_size_
)
{
ROS_ERROR
(
"webcam: avpicture_layout error: %d"
,
size
);
return
;
}
}
bool
UsbCam
::
process_image
(
const
void
*
src
,
int
len
,
boost
::
shared_ptr
<
CameraImage
>
dest
)
{
bool
UsbCam
::
process_image
(
const
void
*
src
,
int
len
,
boost
::
shared_ptr
<
CameraImage
>
dest
)
{
if
(
src
==
NULL
||
dest
==
NULL
)
{
ROS_ERROR
(
"process image error. len: %d, width: %d, height: %d"
,
len
,
dest
->
width
,
dest
->
height
);
ROS_ERROR
(
"process image error. len: %d, width: %d, height: %d"
,
len
,
dest
->
width
,
dest
->
height
);
return
false
;
}
if
(
pixelformat_
==
V4L2_PIX_FMT_YUYV
||
pixelformat_
==
V4L2_PIX_FMT_UYVY
)
{
...
...
@@ -470,28 +381,24 @@ bool UsbCam::process_image(const void * src, int len, boost::shared_ptr<CameraIm
return
true
;
}
int
UsbCam
::
read_frame
()
{
int
UsbCam
::
read_frame
()
{
struct
v4l2_buffer
buf
;
unsigned
int
i
;
int
len
;
bool
result
=
false
;
switch
(
io_
)
{
switch
(
io_
)
{
case
IO_METHOD_READ
:
len
=
read
(
fd_
,
buffers_
[
0
].
start
,
buffers_
[
0
].
length
);
if
(
len
==
-
1
)
{
switch
(
errno
)
{
if
(
len
==
-
1
)
{
switch
(
errno
)
{
case
EAGAIN
:
return
0
;
case
EIO
:
/* Could ignore EIO, see spec. */
/* Could ignore EIO, see spec. */
/* fall through */
/* fall through */
default:
errno_exit
(
"read"
);
...
...
@@ -500,7 +407,7 @@ int UsbCam::read_frame()
result
=
process_image
(
buffers_
[
0
].
start
,
len
,
image_
);
if
(
!
result
)
{
return
0
;
return
0
;
}
break
;
...
...
@@ -511,17 +418,15 @@ int UsbCam::read_frame()
buf
.
type
=
V4L2_BUF_TYPE_VIDEO_CAPTURE
;
buf
.
memory
=
V4L2_MEMORY_MMAP
;
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_DQBUF
,
&
buf
))
{
switch
(
errno
)
{
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_DQBUF
,
&
buf
))
{
switch
(
errno
)
{
case
EAGAIN
:
return
0
;
case
EIO
:
/* Could ignore EIO, see spec. */
/* Could ignore EIO, see spec. */
/* fall through */
/* fall through */
default:
errno_exit
(
"VIDIOC_DQBUF"
);
...
...
@@ -539,8 +444,7 @@ int UsbCam::read_frame()
return
0
;
}
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_QBUF
,
&
buf
))
errno_exit
(
"VIDIOC_QBUF"
);
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_QBUF
,
&
buf
))
errno_exit
(
"VIDIOC_QBUF"
);
break
;
...
...
@@ -550,17 +454,15 @@ int UsbCam::read_frame()
buf
.
type
=
V4L2_BUF_TYPE_VIDEO_CAPTURE
;
buf
.
memory
=
V4L2_MEMORY_USERPTR
;
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_DQBUF
,
&
buf
))
{
switch
(
errno
)
{
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_DQBUF
,
&
buf
))
{
switch
(
errno
)
{
case
EAGAIN
:
return
0
;
case
EIO
:
/* Could ignore EIO, see spec. */
/* Could ignore EIO, see spec. */
/* fall through */
/* fall through */
default:
errno_exit
(
"VIDIOC_DQBUF"
);
...
...
@@ -568,18 +470,18 @@ int UsbCam::read_frame()
}
for
(
i
=
0
;
i
<
n_buffers_
;
++
i
)
if
(
buf
.
m
.
userptr
==
(
unsigned
long
)
buffers_
[
i
].
start
&&
buf
.
length
==
buffers_
[
i
].
length
)
if
(
buf
.
m
.
userptr
==
(
unsigned
long
)
buffers_
[
i
].
start
&&
buf
.
length
==
buffers_
[
i
].
length
)
break
;
assert
(
i
<
n_buffers_
);
len
=
buf
.
bytesused
;
result
=
process_image
((
void
*
)
buf
.
m
.
userptr
,
len
,
image_
);
if
(
!
result
)
{
return
0
;
return
0
;
}
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_QBUF
,
&
buf
))
errno_exit
(
"VIDIOC_QBUF"
);
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_QBUF
,
&
buf
))
errno_exit
(
"VIDIOC_QBUF"
);
break
;
}
...
...
@@ -591,18 +493,15 @@ bool UsbCam::is_capturing() {
return
is_capturing_
;
}
void
UsbCam
::
stop_capturing
(
void
)
{
if
(
!
is_capturing_
)
{
return
;
void
UsbCam
::
stop_capturing
(
void
)
{
if
(
!
is_capturing_
)
{
return
;
}
is_capturing_
=
false
;
enum
v4l2_buf_type
type
;
switch
(
io_
)
{
switch
(
io_
)
{
case
IO_METHOD_READ
:
/* Nothing to do. */
break
;
...
...
@@ -618,25 +517,21 @@ void UsbCam::stop_capturing(void)
}
}
void
UsbCam
::
start_capturing
(
void
)
{
if
(
is_capturing_
)
{
return
;
void
UsbCam
::
start_capturing
(
void
)
{
if
(
is_capturing_
)
{
return
;
}
unsigned
int
i
=
0
;
enum
v4l2_buf_type
type
;
switch
(
io_
)
{
switch
(
io_
)
{
case
IO_METHOD_READ
:
/* Nothing to do. */
break
;
case
IO_METHOD_MMAP
:
for
(
i
=
0
;
i
<
n_buffers_
;
++
i
)
{
for
(
i
=
0
;
i
<
n_buffers_
;
++
i
)
{
struct
v4l2_buffer
buf
;
CLEAR
(
buf
);
...
...
@@ -645,8 +540,7 @@ void UsbCam::start_capturing(void)
buf
.
memory
=
V4L2_MEMORY_MMAP
;
buf
.
index
=
i
;
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_QBUF
,
&
buf
))
errno_exit
(
"VIDIOC_QBUF"
);
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_QBUF
,
&
buf
))
errno_exit
(
"VIDIOC_QBUF"
);
}
type
=
V4L2_BUF_TYPE_VIDEO_CAPTURE
;
...
...
@@ -657,8 +551,7 @@ void UsbCam::start_capturing(void)
break
;
case
IO_METHOD_USERPTR
:
for
(
i
=
0
;
i
<
n_buffers_
;
++
i
)
{
for
(
i
=
0
;
i
<
n_buffers_
;
++
i
)
{
struct
v4l2_buffer
buf
;
CLEAR
(
buf
);
...
...
@@ -669,8 +562,7 @@ void UsbCam::start_capturing(void)
buf
.
m
.
userptr
=
(
unsigned
long
)
buffers_
[
i
].
start
;
buf
.
length
=
buffers_
[
i
].
length
;
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_QBUF
,
&
buf
))
errno_exit
(
"VIDIOC_QBUF"
);
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_QBUF
,
&
buf
))
errno_exit
(
"VIDIOC_QBUF"
);
}
type
=
V4L2_BUF_TYPE_VIDEO_CAPTURE
;
...
...
@@ -680,19 +572,17 @@ void UsbCam::start_capturing(void)
break
;
}
is_capturing_
=
true
;
is_capturing_
=
true
;
}
void
UsbCam
::
uninit_device
(
void
)
{
void
UsbCam
::
uninit_device
(
void
)
{
unsigned
int
i
;
switch
(
io_
)
{
switch
(
io_
)
{
case
IO_METHOD_READ
:
if
(
buffers_
[
0
].
start
)
{
delete
[]
(
unsigned
char
*
)
buffers_
[
0
].
start
;
buffers_
[
0
].
start
=
NULL
;
delete
[](
unsigned
char
*
)
buffers_
[
0
].
start
;
buffers_
[
0
].
start
=
NULL
;
}
break
;
...
...
@@ -710,26 +600,23 @@ void UsbCam::uninit_device(void)
}
}
break
;
}
}
}
void
UsbCam
::
init_read
(
unsigned
int
buffer_size
)
{
//buffers_ = (buffer*)calloc(1, sizeof(*buffers_));
void
UsbCam
::
init_read
(
unsigned
int
buffer_size
)
{
// buffers_ = (buffer*)calloc(1, sizeof(*buffers_));
buffers_
.
resize
(
1
);
buffers_
[
0
].
length
=
buffer_size
;
buffers_
[
0
].
start
=
(
void
*
)
new
unsigned
char
[
buffer_size
];
buffers_
[
0
].
start
=
(
void
*
)
new
unsigned
char
[
buffer_size
];
if
(
!
buffers_
[
0
].
start
)
{
if
(
!
buffers_
[
0
].
start
)
{
ROS_ERROR
(
"Out of memory"
);
exit
(
EXIT_FAILURE
);
}
}
void
UsbCam
::
init_mmap
(
void
)
{
void
UsbCam
::
init_mmap
(
void
)
{
struct
v4l2_requestbuffers
req
;
CLEAR
(
req
);
...
...
@@ -738,15 +625,11 @@ void UsbCam::init_mmap(void)
req
.
type
=
V4L2_BUF_TYPE_VIDEO_CAPTURE
;
req
.
memory
=
V4L2_MEMORY_MMAP
;
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_REQBUFS
,
&
req
))
{
if
(
EINVAL
==
errno
)
{
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_REQBUFS
,
&
req
))
{
if
(
EINVAL
==
errno
)
{
ROS_ERROR_STREAM
(
camera_dev_
<<
" does not support memory mapping"
);
exit
(
EXIT_FAILURE
);
}
else
{
}
else
{
errno_exit
(
"VIDIOC_REQBUFS"
);
}
}
...
...
@@ -760,8 +643,7 @@ void UsbCam::init_mmap(void)
// buffers_ = (buffer*)calloc(req.count, sizeof(*buffers_));
buffers_
.
resize
(
req
.
count
);
for
(
n_buffers_
=
0
;
n_buffers_
<
req
.
count
;
++
n_buffers_
)
{
for
(
n_buffers_
=
0
;
n_buffers_
<
req
.
count
;
++
n_buffers_
)
{
struct
v4l2_buffer
buf
;
CLEAR
(
buf
);
...
...
@@ -770,21 +652,19 @@ void UsbCam::init_mmap(void)
buf
.
memory
=
V4L2_MEMORY_MMAP
;
buf
.
index
=
n_buffers_
;
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_QUERYBUF
,
&
buf
))
errno_exit
(
"VIDIOC_QUERYBUF"
);
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_QUERYBUF
,
&
buf
))
errno_exit
(
"VIDIOC_QUERYBUF"
);
buffers_
[
n_buffers_
].
length
=
buf
.
length
;
buffers_
[
n_buffers_
].
start
=
mmap
(
NULL
/* start anywhere */
,
buf
.
length
,
PROT_READ
|
PROT_WRITE
/* required */
,
MAP_SHARED
/* recommended */
,
fd_
,
buf
.
m
.
offset
);
buffers_
[
n_buffers_
].
start
=
mmap
(
NULL
/* start anywhere */
,
buf
.
length
,
PROT_READ
|
PROT_WRITE
/* required */
,
MAP_SHARED
/* recommended */
,
fd_
,
buf
.
m
.
offset
);
if
(
MAP_FAILED
==
buffers_
[
n_buffers_
].
start
)
errno_exit
(
"mmap"
);
if
(
MAP_FAILED
==
buffers_
[
n_buffers_
].
start
)
errno_exit
(
"mmap"
);
}
}
void
UsbCam
::
init_userp
(
unsigned
int
buffer_size
)
{
void
UsbCam
::
init_userp
(
unsigned
int
buffer_size
)
{
struct
v4l2_requestbuffers
req
;
unsigned
int
page_size
;
...
...
@@ -797,16 +677,12 @@ void UsbCam::init_userp(unsigned int buffer_size)
req
.
type
=
V4L2_BUF_TYPE_VIDEO_CAPTURE
;
req
.
memory
=
V4L2_MEMORY_USERPTR
;
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_REQBUFS
,
&
req
))
{
if
(
EINVAL
==
errno
)
{
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_REQBUFS
,
&
req
))
{
if
(
EINVAL
==
errno
)
{
ROS_ERROR_STREAM
(
camera_dev_
<<
" does not support "
"user pointer i/o"
);
"user pointer i/o"
);
exit
(
EXIT_FAILURE
);
}
else
{
}
else
{
errno_exit
(
"VIDIOC_REQBUFS"
);
}
}
...
...
@@ -814,51 +690,42 @@ void UsbCam::init_userp(unsigned int buffer_size)
// buffers_ = (buffer*)calloc(4, sizeof(*buffers_));
buffers_
.
resize
(
4
);
for
(
n_buffers_
=
0
;
n_buffers_
<
4
;
++
n_buffers_
)
{
for
(
n_buffers_
=
0
;
n_buffers_
<
4
;
++
n_buffers_
)
{
buffers_
[
n_buffers_
].
length
=
buffer_size
;
buffers_
[
n_buffers_
].
start
=
memalign
(
/* boundary */
page_size
,
buffer_size
);
buffers_
[
n_buffers_
].
start
=
memalign
(
/* boundary */
page_size
,
buffer_size
);
if
(
!
buffers_
[
n_buffers_
].
start
)
{
if
(
!
buffers_
[
n_buffers_
].
start
)
{
ROS_ERROR
(
"Out of memory"
);
exit
(
EXIT_FAILURE
);
}
}
}
void
UsbCam
::
init_device
(
int
image_width
,
int
image_height
,
int
framerate
)
{
void
UsbCam
::
init_device
(
int
image_width
,
int
image_height
,
int
framerate
)
{
struct
v4l2_capability
cap
;
struct
v4l2_cropcap
cropcap
;
struct
v4l2_crop
crop
;
struct
v4l2_format
fmt
;
unsigned
int
min
;
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_QUERYCAP
,
&
cap
))
{
if
(
EINVAL
==
errno
)
{
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_QUERYCAP
,
&
cap
))
{
if
(
EINVAL
==
errno
)
{
ROS_ERROR_STREAM
(
camera_dev_
<<
" is no V4L2 device"
);
exit
(
EXIT_FAILURE
);
}
else
{
}
else
{
errno_exit
(
"VIDIOC_QUERYCAP"
);
}
}
if
(
!
(
cap
.
capabilities
&
V4L2_CAP_VIDEO_CAPTURE
))
{
if
(
!
(
cap
.
capabilities
&
V4L2_CAP_VIDEO_CAPTURE
))
{
ROS_ERROR_STREAM
(
camera_dev_
<<
" is no video capture device"
);
exit
(
EXIT_FAILURE
);
}
switch
(
io_
)
{
switch
(
io_
)
{
case
IO_METHOD_READ
:
if
(
!
(
cap
.
capabilities
&
V4L2_CAP_READWRITE
))
{
if
(
!
(
cap
.
capabilities
&
V4L2_CAP_READWRITE
))
{
ROS_ERROR_STREAM
(
camera_dev_
<<
" does not support read i/o"
);
exit
(
EXIT_FAILURE
);
}
...
...
@@ -867,8 +734,7 @@ void UsbCam::init_device(int image_width, int image_height, int framerate)
case
IO_METHOD_MMAP
:
case
IO_METHOD_USERPTR
:
if
(
!
(
cap
.
capabilities
&
V4L2_CAP_STREAMING
))
{
if
(
!
(
cap
.
capabilities
&
V4L2_CAP_STREAMING
))
{
ROS_ERROR_STREAM
(
camera_dev_
<<
" does not support streaming i/o"
);
exit
(
EXIT_FAILURE
);
}
...
...
@@ -882,15 +748,12 @@ void UsbCam::init_device(int image_width, int image_height, int framerate)
cropcap
.
type
=
V4L2_BUF_TYPE_VIDEO_CAPTURE
;
if
(
0
==
xioctl
(
fd_
,
VIDIOC_CROPCAP
,
&
cropcap
))
{
if
(
0
==
xioctl
(
fd_
,
VIDIOC_CROPCAP
,
&
cropcap
))
{
crop
.
type
=
V4L2_BUF_TYPE_VIDEO_CAPTURE
;
crop
.
c
=
cropcap
.
defrect
;
/* reset to default */
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_S_CROP
,
&
crop
))
{
switch
(
errno
)
{
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_S_CROP
,
&
crop
))
{
switch
(
errno
)
{
case
EINVAL
:
/* Cropping not supported. */
break
;
...
...
@@ -899,9 +762,7 @@ void UsbCam::init_device(int image_width, int image_height, int framerate)
break
;
}
}
}
else
{
}
else
{
/* Errors ignored. */
}
...
...
@@ -919,18 +780,15 @@ void UsbCam::init_device(int image_width, int image_height, int framerate)
fmt
.
fmt
.
pix
.
pixelformat
=
pixelformat_
;
fmt
.
fmt
.
pix
.
field
=
V4L2_FIELD_INTERLACED
;
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_S_FMT
,
&
fmt
))
errno_exit
(
"VIDIOC_S_FMT"
);
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_S_FMT
,
&
fmt
))
errno_exit
(
"VIDIOC_S_FMT"
);
/* Note VIDIOC_S_FMT may change width and height. */
/* Buggy driver paranoia. */
min
=
fmt
.
fmt
.
pix
.
width
*
2
;
if
(
fmt
.
fmt
.
pix
.
bytesperline
<
min
)
fmt
.
fmt
.
pix
.
bytesperline
=
min
;
if
(
fmt
.
fmt
.
pix
.
bytesperline
<
min
)
fmt
.
fmt
.
pix
.
bytesperline
=
min
;
min
=
fmt
.
fmt
.
pix
.
bytesperline
*
fmt
.
fmt
.
pix
.
height
;
if
(
fmt
.
fmt
.
pix
.
sizeimage
<
min
)
fmt
.
fmt
.
pix
.
sizeimage
=
min
;
if
(
fmt
.
fmt
.
pix
.
sizeimage
<
min
)
fmt
.
fmt
.
pix
.
sizeimage
=
min
;
image_width
=
fmt
.
fmt
.
pix
.
width
;
image_height
=
fmt
.
fmt
.
pix
.
height
;
...
...
@@ -950,8 +808,7 @@ void UsbCam::init_device(int image_width, int image_height, int framerate)
else
ROS_DEBUG
(
"Set framerate to be %i"
,
framerate
);
switch
(
io_
)
{
switch
(
io_
)
{
case
IO_METHOD_READ
:
init_read
(
fmt
.
fmt
.
pix
.
sizeimage
);
break
;
...
...
@@ -966,43 +823,38 @@ void UsbCam::init_device(int image_width, int image_height, int framerate)
}
}
void
UsbCam
::
close_device
(
void
)
{
if
(
-
1
==
close
(
fd_
))
errno_exit
(
"close"
);
void
UsbCam
::
close_device
(
void
)
{
if
(
-
1
==
close
(
fd_
))
errno_exit
(
"close"
);
fd_
=
-
1
;
}
void
UsbCam
::
open_device
(
void
)
{
void
UsbCam
::
open_device
(
void
)
{
struct
stat
st
;
if
(
-
1
==
stat
(
camera_dev_
.
c_str
(),
&
st
))
{
ROS_ERROR_STREAM
(
"Cannot identify '"
<<
camera_dev_
<<
"': "
<<
errno
<<
", "
<<
strerror
(
errno
));
if
(
-
1
==
stat
(
camera_dev_
.
c_str
(),
&
st
))
{
ROS_ERROR_STREAM
(
"Cannot identify '"
<<
camera_dev_
<<
"': "
<<
errno
<<
", "
<<
strerror
(
errno
));
exit
(
EXIT_FAILURE
);
}
if
(
!
S_ISCHR
(
st
.
st_mode
))
{
if
(
!
S_ISCHR
(
st
.
st_mode
))
{
ROS_ERROR_STREAM
(
camera_dev_
<<
" is no device"
);
exit
(
EXIT_FAILURE
);
}
fd_
=
open
(
camera_dev_
.
c_str
(),
O_RDWR
/* required */
|
O_NONBLOCK
,
0
);
fd_
=
open
(
camera_dev_
.
c_str
(),
O_RDWR
/* required */
|
O_NONBLOCK
,
0
);
if
(
-
1
==
fd_
)
{
ROS_ERROR_STREAM
(
"Cannot open '"
<<
camera_dev_
<<
"': "
<<
errno
<<
", "
<<
strerror
(
errno
));
if
(
-
1
==
fd_
)
{
ROS_ERROR_STREAM
(
"Cannot open '"
<<
camera_dev_
<<
"': "
<<
errno
<<
", "
<<
strerror
(
errno
));
exit
(
EXIT_FAILURE
);
}
}
void
UsbCam
::
start
(
const
std
::
string
&
dev
,
io_method
io_method
,
void
UsbCam
::
start
(
const
std
::
string
&
dev
,
io_method
io_method
,
pixel_format
pixel_format
,
int
image_width
,
int
image_height
,
int
framerate
)
{
int
framerate
)
{
camera_dev_
=
dev
;
io_
=
io_method
;
...
...
@@ -1011,23 +863,17 @@ void UsbCam::start(const std::string& dev, io_method io_method,
pixelformat_
=
V4L2_PIX_FMT_YUYV
;
else
if
(
pixel_format
==
PIXEL_FORMAT_UYVY
)
pixelformat_
=
V4L2_PIX_FMT_UYVY
;
else
if
(
pixel_format
==
PIXEL_FORMAT_MJPEG
)
{
else
if
(
pixel_format
==
PIXEL_FORMAT_MJPEG
)
{
pixelformat_
=
V4L2_PIX_FMT_MJPEG
;
init_mjpeg_decoder
(
image_width
,
image_height
);
}
else
if
(
pixel_format
==
PIXEL_FORMAT_YUVMONO10
)
{
//actually format V4L2_PIX_FMT_Y16 (10-bit mono expresed as 16-bit pixels), but we need to use the advertised type (yuyv)
}
else
if
(
pixel_format
==
PIXEL_FORMAT_YUVMONO10
)
{
// actually format V4L2_PIX_FMT_Y16 (10-bit mono expresed as 16-bit pixels),
// but we need to use the advertised type (yuyv)
pixelformat_
=
V4L2_PIX_FMT_YUYV
;
monochrome_
=
true
;
}
else
if
(
pixel_format
==
PIXEL_FORMAT_RGB24
)
{
}
else
if
(
pixel_format
==
PIXEL_FORMAT_RGB24
)
{
pixelformat_
=
V4L2_PIX_FMT_RGB24
;
}
else
{
}
else
{
ROS_ERROR
(
"Unknown pixel format."
);
exit
(
EXIT_FAILURE
);
}
...
...
@@ -1036,61 +882,52 @@ void UsbCam::start(const std::string& dev, io_method io_method,
init_device
(
image_width
,
image_height
,
framerate
);
start_capturing
();
// instead of malloc with smart pointer
// instead of malloc with smart pointer
image_
=
boost
::
make_shared
<
CameraImage
>
();
image_
->
width
=
image_width
;
image_
->
height
=
image_height
;
image_
->
bytes_per_pixel
=
2
;
//
corrected 11/10/15 (BYTES not BITS per pixel)
image_
->
bytes_per_pixel
=
2
;
//
corrected 11/10/15 (BYTES not BITS per pixel)
image_
->
image_size
=
image_
->
width
*
image_
->
height
*
image_
->
bytes_per_pixel
;
image_
->
is_new
=
0
;
image_
->
image
=
new
char
[
image_
->
image_size
]();
if
(
!
image_
->
image
)
{
ROS_ERROR
(
"Outof memory!"
);
exit
(
EXIT_FAILURE
);
if
(
!
image_
->
image
)
{
ROS_ERROR
(
"Outof memory!"
);
exit
(
EXIT_FAILURE
);
}
}
void
UsbCam
::
shutdown
(
void
)
{
void
UsbCam
::
shutdown
(
void
)
{
stop_capturing
();
uninit_device
();
close_device
();
if
(
avcodec_context_
)
{
if
(
avcodec_context_
)
{
avcodec_close
(
avcodec_context_
);
av_free
(
avcodec_context_
);
avcodec_context_
=
NULL
;
}
if
(
avframe_camera_
)
av_free
(
avframe_camera_
);
if
(
avframe_camera_
)
av_free
(
avframe_camera_
);
avframe_camera_
=
NULL
;
if
(
avframe_rgb_
)
av_free
(
avframe_rgb_
);
if
(
avframe_rgb_
)
av_free
(
avframe_rgb_
);
avframe_rgb_
=
NULL
;
}
bool
UsbCam
::
grab_image
(
sensor_msgs
::
Image
*
msg
,
int
timeout
)
{
bool
UsbCam
::
grab_image
(
sensor_msgs
::
Image
*
msg
,
int
timeout
)
{
// grab the image
bool
get_new_image
=
grab_image
(
timeout
);
if
(
!
get_new_image
)
{
return
false
;
return
false
;
}
// stamp the image
msg
->
header
.
stamp
.
sec
=
image_
->
tv_sec
;
msg
->
header
.
stamp
.
nsec
=
1000
*
image_
->
tv_usec
;
// fill the info
if
(
monochrome_
)
{
if
(
monochrome_
)
{
fillImage
(
*
msg
,
"mono8"
,
image_
->
height
,
image_
->
width
,
image_
->
width
,
image_
->
image
);
}
else
{
}
else
{
// fillImage(*msg, "rgb8", image_->height, image_->width, 3 * image_->width,
// image_->image);
msg
->
encoding
=
"yuyv"
;
...
...
@@ -1105,8 +942,7 @@ bool UsbCam::grab_image(sensor_msgs::Image* msg, int timeout)
return
true
;
}
bool
UsbCam
::
grab_image
(
int
timeout
)
{
bool
UsbCam
::
grab_image
(
int
timeout
)
{
fd_set
fds
;
struct
timeval
tv
;
int
r
=
0
;
...
...
@@ -1120,19 +956,16 @@ bool UsbCam::grab_image(int timeout)
r
=
select
(
fd_
+
1
,
&
fds
,
NULL
,
NULL
,
&
tv
);
if
(
-
1
==
r
)
{
if
(
EINTR
==
errno
)
{
ROS_ERROR
(
"select error EINTR."
);
return
false
;
if
(
-
1
==
r
)
{
if
(
EINTR
==
errno
)
{
ROS_ERROR
(
"select error EINTR."
);
return
false
;
}
errno_exit
(
"select"
);
}
if
(
0
==
r
)
{
if
(
0
==
r
)
{
ROS_WARN_STREAM
(
"camera is offline:"
<<
camera_dev_
);
// reset usb when camera timeout
// reset_device();
...
...
@@ -1140,8 +973,7 @@ bool UsbCam::grab_image(int timeout)
}
int
get_new_image
=
read_frame
();
if
(
!
get_new_image
)
{
if
(
!
get_new_image
)
{
ROS_ERROR
(
"read frame error."
);
return
false
;
}
...
...
@@ -1151,40 +983,30 @@ bool UsbCam::grab_image(int timeout)
}
// enables/disables auto focus
void
UsbCam
::
set_auto_focus
(
int
value
)
{
void
UsbCam
::
set_auto_focus
(
int
value
)
{
struct
v4l2_queryctrl
queryctrl
;
struct
v4l2_ext_control
control
;
memset
(
&
queryctrl
,
0
,
sizeof
(
queryctrl
));
queryctrl
.
id
=
V4L2_CID_FOCUS_AUTO
;
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_QUERYCTRL
,
&
queryctrl
))
{
if
(
errno
!=
EINVAL
)
{
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_QUERYCTRL
,
&
queryctrl
))
{
if
(
errno
!=
EINVAL
)
{
perror
(
"VIDIOC_QUERYCTRL"
);
return
;
}
else
{
}
else
{
ROS_INFO
(
"V4L2_CID_FOCUS_AUTO is not supported"
);
return
;
}
}
else
if
(
queryctrl
.
flags
&
V4L2_CTRL_FLAG_DISABLED
)
{
}
else
if
(
queryctrl
.
flags
&
V4L2_CTRL_FLAG_DISABLED
)
{
ROS_INFO
(
"V4L2_CID_FOCUS_AUTO is not supported"
);
return
;
}
else
{
}
else
{
memset
(
&
control
,
0
,
sizeof
(
control
));
control
.
id
=
V4L2_CID_FOCUS_AUTO
;
control
.
value
=
value
;
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_S_CTRL
,
&
control
))
{
if
(
-
1
==
xioctl
(
fd_
,
VIDIOC_S_CTRL
,
&
control
))
{
perror
(
"VIDIOC_S_CTRL"
);
return
;
}
...
...
@@ -1197,8 +1019,7 @@ void UsbCam::set_auto_focus(int value)
* @param param The name of the parameter to set
* @param param The value to assign
*/
void
UsbCam
::
set_v4l_parameter
(
const
std
::
string
&
param
,
int
value
)
{
void
UsbCam
::
set_v4l_parameter
(
const
std
::
string
&
param
,
int
value
)
{
set_v4l_parameter
(
param
,
boost
::
lexical_cast
<
std
::
string
>
(
value
));
}
/**
...
...
@@ -1207,11 +1028,12 @@ void UsbCam::set_v4l_parameter(const std::string& param, int value)
* @param param The name of the parameter to set
* @param param The value to assign
*/
void
UsbCam
::
set_v4l_parameter
(
const
std
::
string
&
param
,
const
std
::
string
&
value
)
{
void
UsbCam
::
set_v4l_parameter
(
const
std
::
string
&
param
,
const
std
::
string
&
value
)
{
// build the command
std
::
stringstream
ss
;
ss
<<
"v4l2-ctl --device="
<<
camera_dev_
<<
" -c "
<<
param
<<
"="
<<
value
<<
" 2>&1"
;
ss
<<
"v4l2-ctl --device="
<<
camera_dev_
<<
" -c "
<<
param
<<
"="
<<
value
<<
" 2>&1"
;
std
::
string
cmd
=
ss
.
str
();
// capture the output
...
...
@@ -1219,22 +1041,17 @@ void UsbCam::set_v4l_parameter(const std::string& param, const std::string& valu
int
buffer_size
=
256
;
char
buffer
[
buffer_size
];
FILE
*
stream
=
popen
(
cmd
.
c_str
(),
"r"
);
if
(
stream
)
{
if
(
stream
)
{
while
(
!
feof
(
stream
))
if
(
fgets
(
buffer
,
buffer_size
,
stream
)
!=
NULL
)
output
.
append
(
buffer
);
if
(
fgets
(
buffer
,
buffer_size
,
stream
)
!=
NULL
)
output
.
append
(
buffer
);
pclose
(
stream
);
// any output should be an error
if
(
output
.
length
()
>
0
)
ROS_WARN
(
"%s"
,
output
.
c_str
());
}
else
if
(
output
.
length
()
>
0
)
ROS_WARN
(
"%s"
,
output
.
c_str
());
}
else
ROS_WARN
(
"usb_cam_node could not run '%s'"
,
cmd
.
c_str
());
}
UsbCam
::
io_method
UsbCam
::
io_method_from_string
(
const
std
::
string
&
str
)
{
UsbCam
::
io_method
UsbCam
::
io_method_from_string
(
const
std
::
string
&
str
)
{
if
(
str
==
"mmap"
)
return
IO_METHOD_MMAP
;
else
if
(
str
==
"read"
)
...
...
@@ -1245,8 +1062,7 @@ UsbCam::io_method UsbCam::io_method_from_string(const std::string& str)
return
IO_METHOD_UNKNOWN
;
}
UsbCam
::
pixel_format
UsbCam
::
pixel_format_from_string
(
const
std
::
string
&
str
)
{
UsbCam
::
pixel_format
UsbCam
::
pixel_format_from_string
(
const
std
::
string
&
str
)
{
if
(
str
==
"yuyv"
)
return
PIXEL_FORMAT_YUYV
;
else
if
(
str
==
"uyvy"
)
...
...
@@ -1260,5 +1076,4 @@ UsbCam::pixel_format UsbCam::pixel_format_from_string(const std::string& str)
else
return
PIXEL_FORMAT_UNKNOWN
;
}
}
modules/drivers/velodyne/velodyne_driver/src/driver/driver.h
浏览文件 @
a13d35c4
...
...
@@ -29,6 +29,8 @@ namespace velodyne {
// configuration parameters
struct
Config
{
Config
()
:
npackets
(
0
),
rpm
(
0.0
),
firing_data_port
(
0
),
positioning_data_port
(
0
)
{}
std
::
string
frame_id
;
///< tf frame ID
std
::
string
model
;
///< device model name
std
::
string
topic
;
...
...
@@ -41,7 +43,7 @@ struct Config {
class
VelodyneDriver
{
public:
VelodyneDriver
();
~
VelodyneDriver
()
{}
virtual
~
VelodyneDriver
()
{}
virtual
bool
poll
(
void
)
=
0
;
virtual
void
init
(
ros
::
NodeHandle
&
node
)
=
0
;
...
...
@@ -63,8 +65,8 @@ class VelodyneDriver {
class
Velodyne64Driver
:
public
VelodyneDriver
{
public:
Velodyne64Driver
(
Config
config
);
~
Velodyne64Driver
()
{}
explicit
Velodyne64Driver
(
const
Config
&
config
);
virtual
~
Velodyne64Driver
()
{}
void
init
(
ros
::
NodeHandle
&
node
);
bool
poll
(
void
);
...
...
@@ -74,8 +76,8 @@ class Velodyne64Driver : public VelodyneDriver {
class
Velodyne16Driver
:
public
VelodyneDriver
{
public:
Velodyne16Driver
(
Config
config
);
~
Velodyne16Driver
()
{}
explicit
Velodyne16Driver
(
const
Config
&
config
);
virtual
~
Velodyne16Driver
()
{}
void
init
(
ros
::
NodeHandle
&
node
);
bool
poll
(
void
);
...
...
@@ -84,6 +86,7 @@ class Velodyne16Driver : public VelodyneDriver {
private:
boost
::
shared_ptr
<
Input
>
positioning_input_
;
};
class
VelodyneDriverFactory
{
public:
static
VelodyneDriver
*
create_driver
(
ros
::
NodeHandle
private_nh
);
...
...
modules/drivers/velodyne/velodyne_driver/src/driver/driver16.cpp
浏览文件 @
a13d35c4
...
...
@@ -26,7 +26,7 @@ namespace apollo {
namespace
drivers
{
namespace
velodyne
{
Velodyne16Driver
::
Velodyne16Driver
(
Config
config
)
:
config_
(
config
)
{}
Velodyne16Driver
::
Velodyne16Driver
(
const
Config
&
config
)
:
config_
(
config
)
{}
void
Velodyne16Driver
::
init
(
ros
::
NodeHandle
&
node
)
{
double
packet_rate
=
754
;
// packet frequency (Hz)
...
...
modules/drivers/velodyne/velodyne_driver/src/driver/driver64.cpp
浏览文件 @
a13d35c4
...
...
@@ -25,11 +25,11 @@ namespace apollo {
namespace
drivers
{
namespace
velodyne
{
Velodyne64Driver
::
Velodyne64Driver
(
Config
config
)
{
Velodyne64Driver
::
Velodyne64Driver
(
const
Config
&
config
)
{
config_
=
config
;
}
void
Velodyne64Driver
::
init
(
ros
::
NodeHandle
&
node
)
{
void
Velodyne64Driver
::
init
(
ros
::
NodeHandle
&
node
)
{
double
packet_rate
=
0
;
// packet frequency (Hz)
if
(
config_
.
model
==
"64E_S2"
||
config_
.
model
==
"64E_S3S"
)
{
packet_rate
=
3472.17
;
// 1333312 / 384
...
...
modules/drivers/velodyne/velodyne_pointcloud/src/lib/calibration.cpp
浏览文件 @
a13d35c4
...
...
@@ -153,7 +153,7 @@ void operator>>(const YAML::Node& node, Calibration& calibration) {
}
YAML
::
Emitter
&
operator
<<
(
YAML
::
Emitter
&
out
,
const
std
::
pair
<
int
,
LaserCorrection
>
correction
)
{
const
std
::
pair
<
int
,
LaserCorrection
>
&
correction
)
{
out
<<
YAML
::
BeginMap
;
out
<<
YAML
::
Key
<<
LASER_ID
<<
YAML
::
Value
<<
correction
.
first
;
out
<<
YAML
::
Key
<<
ROT_CORRECTION
<<
YAML
::
Value
...
...
@@ -190,7 +190,7 @@ YAML::Emitter& operator<<(YAML::Emitter& out, const Calibration& calibration) {
for
(
std
::
map
<
int
,
LaserCorrection
>::
const_iterator
it
=
calibration
.
laser_corrections_
.
begin
();
it
!=
calibration
.
laser_corrections_
.
end
();
it
++
)
{
it
!=
calibration
.
laser_corrections_
.
end
();
++
it
)
{
out
<<
*
it
;
}
...
...
modules/drivers/velodyne/velodyne_pointcloud/src/lib/online_calibration.cpp
浏览文件 @
a13d35c4
...
...
@@ -102,11 +102,14 @@ int OnlineCalibration::decode(
void
OnlineCalibration
::
get_unit_index
()
{
int
size
=
status_values_
.
size
();
// simple check only for value, maybe need more check fro status tyep
// simple check only for value, maybe need more check fro status type
// TODO(velodyne driver developers): fix the following code logic
/*
int start_index = 0;
if (unit_indexs_.size() > 0) {
start_index = unit_indexs_.back() + 5;
}
*/
for
(
int
start_index
=
0
;
start_index
<
size
-
5
;
++
start_index
)
{
if
(
status_values_
[
start_index
]
==
85
// "U"
&&
status_values_
[
start_index
+
1
]
==
78
// "N"
...
...
@@ -149,4 +152,4 @@ void OnlineCalibration::dump(const std::string& file_path) {
}
// namespace velodyne
}
// namespace drivers
}
// namespace apollo
\ No newline at end of file
}
// namespace apollo
modules/drivers/velodyne/velodyne_pointcloud/src/lib/velodyne16_parser.cpp
浏览文件 @
a13d35c4
...
...
@@ -66,7 +66,6 @@ double Velodyne16Parser::get_timestamp(double base_time, float time_offset,
*/
void
Velodyne16Parser
::
unpack
(
const
velodyne_msgs
::
VelodynePacket
&
pkt
,
VPointCloud
&
pc
)
{
float
azimuth
=
0.0
;
float
azimuth_diff
=
0.0
;
float
last_azimuth_diff
=
0.0
;
float
azimuth_corrected_f
=
0.0
;
...
...
@@ -76,7 +75,7 @@ void Velodyne16Parser::unpack(const velodyne_msgs::VelodynePacket& pkt,
double
basetime
=
raw
->
gps_timestamp
;
// usec
for
(
int
block
=
0
;
block
<
BLOCKS_PER_PACKET
;
block
++
)
{
azimuth
=
(
float
)
(
raw
->
blocks
[
block
].
rotation
);
float
azimuth
=
static_cast
<
float
>
(
raw
->
blocks
[
block
].
rotation
);
if
(
block
<
(
BLOCKS_PER_PACKET
-
1
))
{
azimuth_diff
=
(
float
)((
36000
+
raw
->
blocks
[
block
+
1
].
rotation
-
raw
->
blocks
[
block
].
rotation
)
%
...
...
modules/drivers/velodyne/velodyne_pointcloud/src/lib/velodyne_parser.cpp
浏览文件 @
a13d35c4
...
...
@@ -149,7 +149,7 @@ void VelodyneParser::compute_coords(const union RawDistance &raw_distance,
sin_rot_table_
[
rotation
]
*
corrections
.
cos_rot_correction
-
cos_rot_table_
[
rotation
]
*
corrections
.
sin_rot_correction
;
double
vert_offset
=
corrections
.
vert_offset_correction
;
//
double vert_offset = corrections.vert_offset_correction;
// Compute the distance in the xy plane (w/o accounting for rotation)
double
xy_distance
=
distance
*
corrections
.
cos_vert_correction
;
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录