add feature 'calibration online' in velodyne driver

modules/drivers/velodyne/velodyne/launch/export_pcd.launch

 <launch>
+    <arg name="velodyne64_calibration_online" default="true" />
+    <!--If 'velodyne64_calibration_online' is true, 'velodyne64_calibration_file' will not use  -->
     <arg name="velodyne64_calibration_file" default="$(find velodyne_pointcloud)/params/64E_S3_calibration_example.yaml"/>
     <arg name="extrinsics_velodyne64" default="$(find velodyne_pointcloud)/params/velodyne64_novatel_extrinsics_example.yaml"/>
     <arg name="velodyne64_frame_id" default="velodyne64"/>
@@ -31,6 +33,7 @@
   <include file="$(find velodyne_pointcloud)/launch/convert_nodelet.launch">
     <arg name="node_name" value="sensor_velodyne64_convert"/>
     <arg name="model" value="$(arg model)"/>
+    <arg name="calibration_online" value="$(arg velodyne64_calibration_online)" />
     <arg name="calibration" value="$(arg velodyne64_calibration_file)" />
     <arg name="organized" value="$(arg organized)"/>
     <arg name="min_range" value="$(arg min_range)" />

modules/drivers/velodyne/velodyne/launch/replay_velodyne.launch

 <launch>
+    <arg name="velodyne64_calibration_online" default="true" />
+    <!--If 'velodyne64_calibration_online' is true, 'velodyne64_calibration_file' will not use  -->
     <arg name="param_folder" default="$(find velodyne_pointcloud)/params"/>
     <arg name="velodyne64_calibration_file" default="$(arg param_folder)/64E_S3_calibration_example.yaml"/> 
     <arg name="extrinsics_velodyne64" default="$(arg param_folder)/velodyne64_novatel_extrinsics_example.yaml"/>
@@ -14,7 +16,7 @@
   <include file="$(find velodyne_pointcloud)/launch/rosbag_replay.launch">
     <arg name="rate" default="1" />
     <arg name="start" default="0" />
-    <arg name="bag" value=""/>
+    <arg name="bag" value="" />
   </include> 
 
   <include file="$(find velodyne_pointcloud)/launch/nodelet_manager.launch">
@@ -23,6 +25,7 @@
   <!-- start cloud nodelet using test calibration file -->
   <include file="$(find velodyne_pointcloud)/launch/convert_nodelet.launch">
     <arg name="model" default="64E_S3D_STRONGEST" />
+    <arg name="calibration_online" value="$(arg velodyne64_calibration_online)" />
     <arg name="calibration" default="$(arg velodyne64_calibration_file)" />
     <arg name="organized" default="$(arg organized)"/>
     <arg name="min_range" default="$(arg min_range)" />

modules/drivers/velodyne/velodyne/launch/start_velodyne.launch

 <launch>
+    <arg name="velodyne64_calibration_online" default="true" />
+    <!--If 'velodyne64_calibration_online' is true, 'velodyne64_calibration_file' will not use  -->
     <arg name="velodyne64_calibration_file" default="$(find velodyne_pointcloud)/params/64E_S3_calibration_example.yaml"/>
     <arg name="extrinsics_velodyne64" default="$(find velodyne_pointcloud)/params/velodyne64_novatel_extrinsics_example.yaml"/>
     <arg name="velodyne64_frame_id" default="velodyne64"/>
@@ -30,6 +32,7 @@
   <include file="$(find velodyne_pointcloud)/launch/convert_nodelet.launch">
     <arg name="node_name" value="sensor_velodyne64_convert"/>
     <arg name="model" value="$(arg model)"/>
+    <arg name="calibration_online" value="$(arg velodyne64_calibration_online)" />
     <arg name="calibration" default="$(arg velodyne64_calibration_file)" />
     <arg name="organized" default="$(arg organized)"/>
     <arg name="min_range" default="$(arg min_range)" />

modules/drivers/velodyne/velodyne/launch/velodyne_check.launch

 <launch>
+    <arg name="velodyne64_calibration_online" default="true" />
+    <!--If 'velodyne64_calibration_online' is true, 'velodyne64_calibration_file' will not use  -->
     <arg name="velodyne64_calibration_file" default="$(find velodyne_pointcloud)/params/64E_S3_calibration_example.yaml"/>
     <arg name="extrinsics_velodyne64" default="$(find velodyne_pointcloud)/params/velodyne64_novatel_extrinsics_example.yaml"/>
     <arg name="velodyne64_frame_id" default="velodyne64"/>
@@ -30,6 +32,7 @@
   <include file="$(find velodyne_pointcloud)/launch/convert_nodelet.launch">
     <arg name="node_name" value="sensor_velodyne64_convert"/>
     <arg name="model" value="$(arg model)"/>
+    <arg name="calibration_online" value="$(arg velodyne64_calibration_online)" />
     <arg name="calibration" default="$(arg velodyne64_calibration_file)" />
     <arg name="organized" default="$(arg organized)"/>
     <arg name="min_range" default="$(arg min_range)" />

modules/drivers/velodyne/velodyne_pointcloud/include/velodyne_pointcloud/calibration.h

 /**
- * \file  calibration.h 
+ * \file  calibration.h
  *
  * \author  Piyush Khandelwal (piyushk@cs.utexas.edu)
  * Copyright (C) 2012, Austin Robot Technology, University of Texas at Austin
@@ -74,9 +74,7 @@ class Calibration {
 
  public:
   Calibration() : _initialized(false) {}
-  Calibration(const std::string& calibration_file) {
-    read(calibration_file);
-  }
+  Calibration(const std::string& calibration_file) { read(calibration_file); }
 
   void read(const std::string& calibration_file);
   void write(const std::string& calibration_file);

modules/drivers/velodyne/velodyne_pointcloud/include/velodyne_pointcloud/const_variables.h

@@ -132,7 +132,7 @@ const float INNER_TIME_64E_S3[12][32] = {
     {57.6, 56.3, 55.1, 53.9, 50.4, 49.1, 47.9, 46.7, 43.2, 41.9, 40.7,
      39.5, 36,   34.7, 33.5, 32.3, 28.8, 27.5, 26.3, 25.1, 21.6, 20.3,
      19.1, 17.9, 14.4, 13.1, 11.9, 10.7, 7.2,  5.9,  4.7,  3.5}};
-     
+
 }  // namespace velodyne
 }  // namespace drivers
 }  // namespace apollo

modules/drivers/velodyne/velodyne_pointcloud/include/velodyne_pointcloud/online_calibration.h

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#ifndef MODULES_DRIVERS_VELODYNE_VELODYNE_POINTCLOUD_ONLINE_CALIBRATION_H_
+#define MODULES_DRIVERS_VELODYNE_VELODYNE_POINTCLOUD_ONLINE_CALIBRATION_H_
+
+#include <ros/ros.h>
+#include <vector>
+#include "velodyne_msgs/VelodyneScanUnified.h"
+#include "velodyne_pointcloud/calibration.h"
+
+namespace apollo {
+namespace drivers {
+namespace velodyne {
+
+constexpr double DEGRESS_TO_RADIANS = 3.1415926535897 / 180.0;
+
+class OnlineCalibration {
+ public:
+  OnlineCalibration() {}
+  ~OnlineCalibration() {}
+
+  int decode(const velodyne_msgs::VelodyneScanUnified::ConstPtr& scan_msgs);
+  void dump(const std::string& file_path);
+  void get_unit_index();
+  bool inited() const { return _inited; }
+  Calibration calibration() const { return _calibration; }
+
+ private:
+  bool _inited;
+  Calibration _calibration;
+  std::vector<uint8_t> _status_types;
+  std::vector<uint8_t> _status_values;
+  // store first two "unit#" value index
+  std::vector<int> _unit_indexs;
+};
+
+}  // namespace velodyne
+}  // namespace drivers
+}  // namespace apollo
+
+#endif  // MODULES_DRIVERS_VELODYNE_VELODYNE_POINTCLOUD_ONLINE_CALIBRATION_H_
\ No newline at end of file

modules/drivers/velodyne/velodyne_pointcloud/include/velodyne_pointcloud/velodyne_parser.h

@@ -32,6 +32,7 @@
 #include "velodyne_msgs/VelodyneScanUnified.h"
 #include "velodyne_pointcloud/calibration.h"
 #include "velodyne_pointcloud/const_variables.h"
+#include "velodyne_pointcloud/online_calibration.h"
 #include "velodyne_pointcloud/point_types.h"
 
 namespace apollo {
@@ -141,6 +142,7 @@ struct Config {
   double min_angle;
   double view_direction;
   double view_width;
+  bool calibration_online;
   std::string calibration_file;
   std::string model;  // VLP16,32E, 64E_32
   bool organized;     // is point cloud order
@@ -176,12 +178,8 @@ class VelodyneParser {
   // order point cloud fod IDL by velodyne model
   virtual void order(VPointCloud::Ptr &cloud) = 0;
 
-  const Calibration &get_calibration() {
-    return _calibration;
-  }
-  const double get_last_timestamp() {
-    return _last_time_stamp;
-  }
+  const Calibration &get_calibration() { return _calibration; }
+  const double get_last_timestamp() { return _last_time_stamp; }
 
  protected:
   const float (*_inner_time)[12][32];
@@ -250,6 +248,8 @@ class Velodyne64Parser : public VelodyneParser {
   bool _is_s2;
   int _offsets[64];
 
+  OnlineCalibration _online_calibration;
+
 };  // class Velodyne64Parser
 
 class VelodyneParserFactory {

modules/drivers/velodyne/velodyne_pointcloud/launch/convert_nodelet.launch

 <launch>
   <arg name="model" default="64E_S3D_STRONGEST" />
-  <arg name="calibration" default="$(find velodyne_pointcloud)/params/64E_S3_calibration_example.yaml" />
+  <arg name="calibration_online" default="true" />
+  <!--If 'calibration_online' is true, 'calibration' will not use -->
+  <arg name="calibration" default="" />
   <arg name="min_range" default="0.9" />
   <arg name="max_range" default="70.0" />
   <arg name="organized" default="false"/>
@@ -11,6 +13,7 @@
 
   <node pkg="nodelet" type="nodelet" name="$(arg node_name)"
         args="load velodyne_pointcloud/ConvertNodelet velodyne_nodelet_manager" output="screen">
+    <param name="calibration_online" value="$(arg calibration_online)"/>
     <param name="calibration" value="$(arg calibration)"/>
     <param name="min_range" value="$(arg min_range)"/>
     <param name="max_range" value="$(arg max_range)"/>

modules/drivers/velodyne/velodyne_pointcloud/src/compensator_nodelet.cpp

@@ -37,8 +37,7 @@ class CompensatorNodelet : public nodelet::Nodelet {
 /** @brief Nodelet initialization. */
 void CompensatorNodelet::onInit() {
   ROS_INFO("Compensator nodelet init");
-  _compensator.reset(new Compensator(
-      getNodeHandle(), getPrivateNodeHandle()));
+  _compensator.reset(new Compensator(getNodeHandle(), getPrivateNodeHandle()));
 }
 
 }  // namespace velodyne

modules/drivers/velodyne/velodyne_pointcloud/src/convert.cpp

@@ -33,6 +33,7 @@ void Convert::init(ros::NodeHandle& node, ros::NodeHandle& private_nh) {
   private_nh.param("view_direction", _config.view_direction, 0.0);
   private_nh.param("view_width", _config.view_width, 2.0 * M_PI);
   private_nh.param("model", _config.model, std::string("64E_S2"));
+  private_nh.param("calibration_online", _config.calibration_online, true);
   private_nh.param("calibration", _config.calibration_file, std::string(""));
   private_nh.param("organized", _config.organized, false);
   private_nh.param("topic_packets", _topic_packets, TOPIC_PACKTES);

modules/drivers/velodyne/velodyne_pointcloud/src/lib/CMakeLists.txt

@@ -5,6 +5,7 @@ add_library(velodyne_parser
     # velodyne32_parser.cpp  
     # velodyne16_parser.cpp
     calibration.cpp
+    online_calibration.cpp
     util.cpp
 )
 target_link_libraries(velodyne_parser

modules/drivers/velodyne/velodyne_pointcloud/src/lib/calibration.cpp

 /**
  * \file  calibration.cc
- * \brief  
+ * \brief
  *
  * \author  Piyush Khandelwal (piyushk@cs.utexas.edu)
  * Copyright (C) 2012, Austin Robot Technology,

modules/drivers/velodyne/velodyne_pointcloud/src/lib/online_calibration.cpp

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#include "velodyne_pointcloud/online_calibration.h"
+
+namespace apollo {
+namespace drivers {
+namespace velodyne {
+
+int OnlineCalibration::decode(
+    const velodyne_msgs::VelodyneScanUnified::ConstPtr& scan_msgs) {
+  if (_inited) {
+    return 0;
+  }
+  for (auto& packet : scan_msgs->packets) {
+    _status_types.emplace_back(packet.data[1204]);
+    _status_values.emplace_back(packet.data[1205]);
+  }
+  // read calibration when get 2s packet
+  if (_status_types.size() < 5789 * 2) {
+    ROS_INFO("Need more scan msgs");
+    return -1;
+  }
+  get_unit_index();
+  if (_unit_indexs.size() < 2) {
+    // can not find two unit# index, may be lost packet
+    ROS_ERROR_STREAM("unit count less than 2, maybe lost packets");
+    return -1;
+  }
+
+  if (_unit_indexs[1] - _unit_indexs[0] != 65 * 64) {  // 64 lasers
+    // lost packet
+    ROS_ERROR_STREAM("two unit distance is wrong");
+    return -1;
+  }
+
+  int start_index = _unit_indexs[0];
+  for (int i = 0; i < 64; ++i) {
+    LaserCorrection laser_correction;
+
+    int index_16 = start_index + i * 64 + 16;
+    int index_32 = start_index + i * 64 + 32;
+    int index_48 = start_index + i * 64 + 48;
+
+    laser_correction.laser_ring = _status_values[index_16];
+
+    laser_correction.vert_correction =
+        *(int16_t*)(&_status_values[index_16 + 1]) / 100.0 * DEGRESS_TO_RADIANS;
+
+    laser_correction.rot_correction =
+        *(int16_t*)(&_status_values[index_16 + 3]) / 100.0 * DEGRESS_TO_RADIANS;
+
+    laser_correction.dist_correction =
+        *(int16_t*)(&_status_values[index_16 + 5]) / 10.0 / 100.0;  // to meter
+
+    laser_correction.dist_correction_x =
+        *(int16_t*)(&_status_values[index_32]) / 10.0 / 100.0;  // to meter
+
+    laser_correction.dist_correction_y =
+        *(int16_t*)(&_status_values[index_32 + 2]) / 10.0 / 100.0;  // to meter
+
+    laser_correction.vert_offset_correction =
+        *(int16_t*)(&_status_values[index_32 + 4]) / 10.0 / 100.0;  // to meter
+
+    laser_correction.horiz_offset_correction =
+        (int16_t)((int16_t)_status_values[index_48] << 8 |
+                  _status_values[index_32 + 6]) /
+        10.0 / 100.0;  // to meter
+
+    laser_correction.focal_distance =
+        *(int16_t*)(&_status_values[index_48 + 1]) / 10.0 / 100.0;  // to meter
+
+    laser_correction.focal_slope =
+        *(int16_t*)(&_status_values[index_48 + 3]) / 10.0;  // to meter
+
+    laser_correction.max_intensity = _status_values[index_48 + 6];
+    laser_correction.min_intensity = _status_values[index_48 + 5];
+
+    _calibration._laser_corrections[i] = laser_correction;
+    _calibration._num_lasers = 64;
+    _calibration._initialized = true;
+  }
+  _inited = true;
+}
+
+void OnlineCalibration::get_unit_index() {
+  int size = _status_values.size();
+  // simple check only for value, maybe need more check fro status tyep
+  for (int i = 0; i < size - 5; ++i) {
+    if (_status_values[i] == 85            // "U"
+        && _status_values[i + 1] == 78     // "N"
+        && _status_values[i + 2] == 73     // "I"
+        && _status_values[i + 3] == 84     // "T"
+        && _status_values[i + 4] == 35) {  // "#"
+      _unit_indexs.emplace_back(i);
+    }
+  }
+}
+
+void OnlineCalibration::dump(const std::string& file_path) {
+  if (!_inited) {
+    ROS_ERROR("Please decode calibraion info first");
+  }
+  std::ofstream ofs(file_path.c_str(), std::ios::out);
+  ofs << "lasers:" << std::endl;
+  for (auto& correction : _calibration._laser_corrections) {
+    ofs << "- {";
+    ofs << "dist_correction: " << correction.second.dist_correction << ", ";
+    ofs << "dist_correction_x: " << correction.second.dist_correction_x << ", ";
+    ofs << "dist_correction_y: " << correction.second.dist_correction_y << ", ";
+    ofs << "focal_distance: " << correction.second.focal_distance << ", ";
+    ofs << "focal_slope: " << correction.second.focal_slope << ", ";
+    ofs << "horiz_offset_correction: "
+        << correction.second.horiz_offset_correction << ", ";
+    ofs << "laser_id: " << correction.second.laser_ring << ", ";
+    ofs << "max_intensity: " << correction.second.max_intensity << ", ";
+    ofs << "min_intensity: " << correction.second.min_intensity << ", ";
+    ofs << "rot_correction: " << correction.second.rot_correction << ", ";
+    ofs << "vert_correction: " << correction.second.vert_correction << ", ";
+    ofs << "vert_offset_correction: "
+        << correction.second.vert_offset_correction;
+    ofs << "}" << std::endl;
+  }
+  ofs << "num_lasers: " << _calibration._num_lasers << std::endl;
+  ofs.close();
+}
+
+}  // namespace velodyne
+}  // namespace drivers
+}  // namespace apollo
\ No newline at end of file

modules/drivers/velodyne/velodyne_pointcloud/src/lib/velodyne64_parser.cpp

@@ -148,6 +148,13 @@ void Velodyne64Parser::init_offsets() {
 void Velodyne64Parser::generate_pointcloud(
     const velodyne_msgs::VelodyneScanUnified::ConstPtr& scan_msg,
     VPointCloud::Ptr& pointcloud) {
+  if (_config.calibration_online) {
+    if (_online_calibration.decode(scan_msg) == -1 ||
+        !_online_calibration.inited()) {
+      return;
+    }
+    _calibration = _online_calibration.calibration();
+  }
   // allocate a point cloud with same time and frame ID as raw data
   pointcloud->header.frame_id = scan_msg->header.frame_id;
   pointcloud->height = 1;

modules/drivers/velodyne/velodyne_pointcloud/src/lib/velodyne_parser.cpp

@@ -95,12 +95,14 @@ void VelodyneParser::init_angle_params(double view_direction,
 
 /** Set up for on-line operation. */
 void VelodyneParser::setup() {
-  _calibration.read(_config.calibration_file);
+  if (!_config.calibration_online) {
+    _calibration.read(_config.calibration_file);
 
-  if (!_calibration._initialized) {
-    ROS_FATAL_STREAM(
-        "Unable to open calibration file: " << _config.calibration_file);
-    ROS_BREAK();
+    if (!_calibration._initialized) {
+      ROS_FATAL_STREAM(
+          "Unable to open calibration file: " << _config.calibration_file);
+      ROS_BREAK();
+    }
   }
 
   // setup angle parameters.

modules/drivers/velodyne/velodyne_pointcloud/src/pcd_exporter.cpp

@@ -61,7 +61,8 @@ void PCDExporter::init() {
   ROS_INFO_STREAM("pcd_folder :" << _pcd_folder);
   // check output directory, if not exist create it
   if (!boost::filesystem::exists(_pcd_folder)) {
-    ROS_INFO_STREAM("The directory " << _pcd_folder << " is not exists, create now");
+    ROS_INFO_STREAM("The directory " << _pcd_folder
+                                     << " is not exists, create now");
     if (boost::filesystem::create_directory(_pcd_folder)) {
       ROS_INFO("Create directory success.");
     } else {

modules/drivers/velodyne/velodyne_pointcloud/src/pcd_exporter_nodelet.cpp

@@ -27,7 +27,7 @@ namespace velodyne {
 class PCDExporterNodelet : public nodelet::Nodelet {
  public:
   PCDExporterNodelet() {}
-  ~PCDExporterNodelet() { }
+  ~PCDExporterNodelet() {}
 
  private:
   virtual void onInit();
@@ -37,7 +37,7 @@ class PCDExporterNodelet : public nodelet::Nodelet {
 /** @brief Nodelet initialization. */
 void PCDExporterNodelet::onInit() {
   ROS_INFO("Pcd exporter nodelet init");
-  _instance.reset(new PCDExporter(getNodeHandle(),getPrivateNodeHandle()));
+  _instance.reset(new PCDExporter(getNodeHandle(), getPrivateNodeHandle()));
   _instance->init();
 }
 

modules/drivers/velodyne/velodyne_pointcloud/src/pointcloud_dump.cpp

@@ -20,8 +20,8 @@
 #include <fstream>
 #include <iostream>
 
-#include "velodyne_pointcloud/util.h"
 #include "velodyne_pointcloud/pointcloud_dump.h"
+#include "velodyne_pointcloud/util.h"
 
 namespace apollo {
 namespace drivers {