Skip to content

A
apollo

Pinoxchio / apollo
与 Fork 源项目一致

从无法访问的项目Fork

通知 2
Star 0
Fork 0
A
apollo

体验新版 GitCode，发现更多精彩内容 >>

提交 a13d35c4 编写于 作者: L Liangliang Zhang 提交者： Calvin Miao
浏览文件
操作

driviers: fixed code issues in velodyne driver.

上级 41b07434
隐藏空白更改
内联 并排
Showing with 311 addition and 497 deletion
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.
先完成此消息的编辑！
想要评论请 注册 或