From a13d35c43d12e22214185c6cc9f983d9867303f2 Mon Sep 17 00:00:00 2001
From: Liangliang Zhang <zhangliangliang@baidu.com>
Date: Wed, 10 Jan 2018 11:35:49 -0800
Subject: [PATCH] driviers: fixed code issues in velodyne driver.

---
 .../impl/parser/novatel/novatel_parser.cpp    |  23 +-
 .../src/impl/parser/rtcm/rtcm3_parser.cpp     |  13 +-
 modules/drivers/usb_cam/src/usb_cam.cpp       | 737 +++++++-----------
 .../velodyne_driver/src/driver/driver.h       |  13 +-
 .../velodyne_driver/src/driver/driver16.cpp   |   2 +-
 .../velodyne_driver/src/driver/driver64.cpp   |   4 +-
 .../src/lib/calibration.cpp                   |   4 +-
 .../src/lib/online_calibration.cpp            |   7 +-
 .../src/lib/velodyne16_parser.cpp             |   3 +-
 .../src/lib/velodyne_parser.cpp               |   2 +-
 10 files changed, 311 insertions(+), 497 deletions(-)

diff --git a/modules/drivers/gnss/src/impl/parser/novatel/novatel_parser.cpp b/modules/drivers/gnss/src/impl/parser/novatel/novatel_parser.cpp
index 88f671e314..d7d1951e77 100644
--- a/modules/drivers/gnss/src/impl/parser/novatel/novatel_parser.cpp
+++ b/modules/drivers/gnss/src/impl/parser/novatel/novatel_parser.cpp
@@ -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) {
diff --git a/modules/drivers/gnss/src/impl/parser/rtcm/rtcm3_parser.cpp b/modules/drivers/gnss/src/impl/parser/rtcm/rtcm3_parser.cpp
index 14f5bfbffd..9c83241955 100644
--- a/modules/drivers/gnss/src/impl/parser/rtcm/rtcm3_parser.cpp
+++ b/modules/drivers/gnss/src/impl/parser/rtcm/rtcm3_parser.cpp
@@ -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
diff --git a/modules/drivers/usb_cam/src/usb_cam.cpp b/modules/drivers/usb_cam/src/usb_cam.cpp
index b76751d58c..0ff07e4d64 100644
--- a/modules/drivers/usb_cam/src/usb_cam.cpp
+++ b/modules/drivers/usb_cam/src/usb_cam.cpp
@@ -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;
 }
-
 }
diff --git a/modules/drivers/velodyne/velodyne_driver/src/driver/driver.h b/modules/drivers/velodyne/velodyne_driver/src/driver/driver.h
index 7525e350d3..590d3e5550 100644
--- a/modules/drivers/velodyne/velodyne_driver/src/driver/driver.h
+++ b/modules/drivers/velodyne/velodyne_driver/src/driver/driver.h
@@ -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);
diff --git a/modules/drivers/velodyne/velodyne_driver/src/driver/driver16.cpp b/modules/drivers/velodyne/velodyne_driver/src/driver/driver16.cpp
index e9c9a0ca65..067fc513ef 100644
--- a/modules/drivers/velodyne/velodyne_driver/src/driver/driver16.cpp
+++ b/modules/drivers/velodyne/velodyne_driver/src/driver/driver16.cpp
@@ -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)
diff --git a/modules/drivers/velodyne/velodyne_driver/src/driver/driver64.cpp b/modules/drivers/velodyne/velodyne_driver/src/driver/driver64.cpp
index f40e5a8efd..2ef7fa97f8 100644
--- a/modules/drivers/velodyne/velodyne_driver/src/driver/driver64.cpp
+++ b/modules/drivers/velodyne/velodyne_driver/src/driver/driver64.cpp
@@ -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
diff --git a/modules/drivers/velodyne/velodyne_pointcloud/src/lib/calibration.cpp b/modules/drivers/velodyne/velodyne_pointcloud/src/lib/calibration.cpp
index 521c0044a8..8b4413bce4 100644
--- a/modules/drivers/velodyne/velodyne_pointcloud/src/lib/calibration.cpp
+++ b/modules/drivers/velodyne/velodyne_pointcloud/src/lib/calibration.cpp
@@ -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;
   }
 
diff --git a/modules/drivers/velodyne/velodyne_pointcloud/src/lib/online_calibration.cpp b/modules/drivers/velodyne/velodyne_pointcloud/src/lib/online_calibration.cpp
index b11cb296b4..ea964eddec 100644
--- a/modules/drivers/velodyne/velodyne_pointcloud/src/lib/online_calibration.cpp
+++ b/modules/drivers/velodyne/velodyne_pointcloud/src/lib/online_calibration.cpp
@@ -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
diff --git a/modules/drivers/velodyne/velodyne_pointcloud/src/lib/velodyne16_parser.cpp b/modules/drivers/velodyne/velodyne_pointcloud/src/lib/velodyne16_parser.cpp
index ec83431fae..793d961186 100644
--- a/modules/drivers/velodyne/velodyne_pointcloud/src/lib/velodyne16_parser.cpp
+++ b/modules/drivers/velodyne/velodyne_pointcloud/src/lib/velodyne16_parser.cpp
@@ -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) %
diff --git a/modules/drivers/velodyne/velodyne_pointcloud/src/lib/velodyne_parser.cpp b/modules/drivers/velodyne/velodyne_pointcloud/src/lib/velodyne_parser.cpp
index ca7f12e173..d0c397292e 100644
--- a/modules/drivers/velodyne/velodyne_pointcloud/src/lib/velodyne_parser.cpp
+++ b/modules/drivers/velodyne/velodyne_pointcloud/src/lib/velodyne_parser.cpp
@@ -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;
-- 
GitLab