fix online_calibration bug (#609)

WORKSPACE.in

@@ -82,7 +82,7 @@ new_http_archive(
     strip_prefix = "ros",
     # url = "https://github.com/ApolloAuto/apollo-platform/releases/download/1.0.0/ros-indigo-apollo-1.0.0.MACHINE_ARCH.tar.gz",
     # temporary ros archive for apollo-1.5
-    url = "https://github.com/fengqikai1414/apollo-platform/releases/download/1.5.0/ros-indigo-apollo-1.5.3-x86_64-dev.tar.gz",
+    url = "https://github.com/fengqikai1414/apollo-platform/releases/download/1.5.0/ros-indigo-apollo-1.5.4-x86_64-dev.tar.gz",
 )
 
 # OpenCV 2.4.13.2

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

@@ -231,7 +231,7 @@ class Velodyne64Parser : public VelodyneParser {
       const velodyne_msgs::VelodyneScanUnified::ConstPtr &scan_msg,
       VPointCloud::Ptr &out_msg);
   void order(VPointCloud::Ptr &cloud);
-  void setup();
+  void setup() override;
 
  private:
   void set_base_time_from_packets(const velodyne_msgs::VelodynePacket &pkt);

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

@@ -31,7 +31,7 @@ int OnlineCalibration::decode(
   }
   // read calibration when get 2s packet
   if (_status_types.size() < 5789 * 2) {
-    ROS_INFO("Need more scan msgs");
+    ROS_INFO("Wait for more scan msgs");
     return -1;
   }
   get_unit_index();
@@ -89,11 +89,22 @@ int OnlineCalibration::decode(
     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;
+    laser_correction.cos_rot_correction = cosf(laser_correction.rot_correction);
+    laser_correction.sin_rot_correction = sinf(laser_correction.rot_correction);
+    laser_correction.cos_vert_correction =
+        cosf(laser_correction.vert_correction);
+    laser_correction.sin_vert_correction =
+        sinf(laser_correction.vert_correction);
+    laser_correction.focal_offset =
+        256 * pow(1 - laser_correction.focal_distance / 13100, 2);
+
+    _calibration._laser_corrections[laser_correction.laser_ring] =
+        laser_correction;
   }
+  _calibration._num_lasers = 64;
+  _calibration._initialized = true;
   _inited = true;
+  return 0;
 }
 
 void OnlineCalibration::get_unit_index() {
@@ -113,6 +124,7 @@ void OnlineCalibration::get_unit_index() {
 void OnlineCalibration::dump(const std::string& file_path) {
   if (!_inited) {
     ROS_ERROR("Please decode calibraion info first");
+    return;
   }
   std::ofstream ofs(file_path.c_str(), std::ios::out);
   ofs << "lasers:" << std::endl;

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

@@ -46,7 +46,7 @@ Velodyne64Parser::Velodyne64Parser(Config config) : VelodyneParser(config) {
 
 void Velodyne64Parser::setup() {
   VelodyneParser::setup();
-  if (_config.organized) {
+  if (!_config.calibration_online && _config.organized) {
     init_offsets();
   }
 }
@@ -103,13 +103,13 @@ void Velodyne64Parser::set_base_time_from_packets(
     time.tm_year = year - 1900;
     time.tm_mon = month - 1;
     time.tm_mday = day;
-    time.tm_hour = hour + _config.time_zone;
+    time.tm_hour = hour;
     time.tm_min = 0;
     time.tm_sec = 0;
 
     ROS_INFO("Set base unix time: (%d.%d.%d %d:%d:%d)", time.tm_year,
              time.tm_mon, time.tm_mday, time.tm_hour, time.tm_min, time.tm_sec);
-    uint64_t unix_base = static_cast<uint64_t>(mktime(&time));
+    uint64_t unix_base = static_cast<uint64_t>(timegm(&time));
     for (int i = 0; i < 4; ++i) {
       _gps_base_usec[i] = unix_base * 1000000;
     }
@@ -148,13 +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()) {
+  if (_config.calibration_online && !_calibration._initialized) {
+    if (_online_calibration.decode(scan_msg) == -1) {
       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;

scripts/apollo_record.sh

@@ -34,6 +34,7 @@ function start() {
         nohup rosbag record -b 2048  \
             /apollo/sensor/gnss/gnss_status \
             /apollo/sensor/gnss/odometry \
+            /apollo/sensor/gnss/ins_stat \
             /apollo/sensor/gnss/corrected_imu \
             /apollo/canbus/chassis \
             /apollo/canbus/chassis_detail \

scripts/velodyne.sh

+#!/usr/bin/env bash
+
+###############################################################################
+# 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.
+###############################################################################
+
+
+DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+
+cd "${DIR}/.."
+
+source "${DIR}/apollo_base.sh"
+
+function start() {
+    LOG="${APOLLO_ROOT_DIR}/data/log/velodyne.out"
+    CMD="roslaunch velodyne start_velodyne.launch"
+    NUM_PROCESSES="$(pgrep -c -f "velodyne_nodelet_manager")"
+    if [ "${NUM_PROCESSES}" -eq 0 ]; then
+       eval "nohup ${CMD} </dev/null >${LOG} 2>&1 &"
+    fi
+}
+
+function stop() {
+    pkill -f start_velodyne
+}
+
+# run command_name module_name
+function run() {
+    case $1 in
+        start)
+            start
+            ;;
+        stop)
+            stop
+            ;;
+        *)
+            start
+            ;;
+    esac
+}
+
+run "$1"