update velodyne to cybertron version

modules/common/proto/header.proto

@@ -30,4 +30,6 @@ message Header {
   optional uint32 version = 7 [default = 1];
 
   optional StatusPb status = 8;
+
+  optional string frame_id = 9;
 }

modules/drivers/gnss/conf/gnss_conf.pb.txt

@@ -48,20 +48,21 @@ tf {
 wheel_parameters: "SETWHEELPARAMETERS 100 1 1\r\n"
 
 gpsbin_folder: "/apollo/data/gpsbin"
-heading_channel_name: "/apollo/data/gpsbin"
-epochobservation_channel_name: "/apollo/data/gpsbin"
-gnssephemeris_channel_name: "/apollo/data/gpsbin"
-insstat_channel_name: "/apollo/data/gpsbin"
-ins_channel_name: "/apollo/data/gpsbin"
-corrimu_channel_name: "/apollo/data/gpsbin"
-insstatus_channel_name: "/apollo/data/gpsbin"
-gnss_channel_name: "/apollo/data/gpsbin"
-gnssstatus_channel_name: "/apollo/data/gpsbin"
-raw_channel_name: "/apollo/data/gpsbin"
-rtcm_channel_name: "/apollo/data/gpsbin"
-stream_channel_name: "/apollo/data/gpsbin"
-rawimu_channel_name: "/apollo/data/gpsbin"
-gps_channel_name: "/apollo/data/gpsbin"
+heading_channel_name: "/apollo/sensor/gnss/heading"
+epochobservation_channel_name: "/apollo/sensor/gnss/rtk_obs"
+gnssephemeris_channel_name: "/apollo/sensor/gnss/rtk_eph"
+insstat_channel_name: "/apollo/sensor/gnss/ins_stat"
+ins_channel_name: "/apollo/sensor/gnss/ins"
+corrimu_channel_name: "/apollo/sensor/gnss/corrected_imu"
+insstatus_channel_name: "/apollo/sensor/gnss/ins_status"
+gnss_channel_name: "/apollo/sensor/gnss/gnss"
+gnssstatus_channel_name: "/apollo/sensor/gnss/gnss_status"
+raw_channel_name: "/apollo/sensor/gnss/raw_data"
+rtcm_channel_name: "/apollo/sensor/gnss/rtcm_data"
+stream_channel_name: "/apollo/sensor/gnss/stream_status"
+rawimu_channel_name: "/apollo/sensor/gnss/imu"
+gps_channel_name: "/apollo/sensor/gnss/odometry"
+bestpos_channel_name: "/apollo/sensor/gnss/best_pose"
 
 #########################################################################
 # notice: only for debug, won't configure device through driver online!!!

modules/drivers/gnss/dag/dag_streaming_gnss.dag

@@ -3,7 +3,7 @@ module_config {
     module_library : "/apollo/bazel-bin/modules/drivers/gnss/libgnss_component.so"
 
     components {
-      class_name : "::apollo::drivers::gnss::GnssDriverComponent"
+      class_name : "GnssDriverComponent"
       config {
         name : "gnss"
         config_file_path : "/apollo/modules/drivers/gnss/conf/gnss_conf.pb.txt"

modules/drivers/gnss/parser/data_parser.cc

@@ -91,7 +91,7 @@ bool DataParser::Init() {
   gnssstatus_writer_ = node_->CreateWriter<GnssStatus>(config_.gnssstatus_channel_name());  
   insstatus_writer_ = node_->CreateWriter<InsStatus>(config_.insstatus_channel_name());
   gnss_writer_ = node_->CreateWriter<Gnss>(config_.gnss_channel_name());
-  gnssbestpose_writer_ = node_->CreateWriter<GnssBestPose>(config_.insstatus_channel_name());
+  gnssbestpose_writer_ = node_->CreateWriter<GnssBestPose>(config_.bestpos_channel_name());
   corrimu_writer_ = node_->CreateWriter<CorrectedImu>(config_.corrimu_channel_name());
   ins_writer_ = node_->CreateWriter<Ins>(config_.ins_channel_name());
   insstat_writer_ = node_->CreateWriter<InsStat>(config_.insstat_channel_name());

modules/drivers/gnss/proto/config.proto

@@ -132,5 +132,6 @@ message Config {
   optional string rtcm_channel_name = 26;
   optional string rawimu_channel_name = 27;
   optional string gps_channel_name = 28;
+  optional string bestpos_channel_name = 29;
 
 }

modules/drivers/proto/pointcloud.proto

@@ -17,4 +17,6 @@ message PointCloud {
   optional bool is_dense = 3;
   repeated PointXYZIT point = 4;
   optional double measurement_time = 5;
+  optional uint32 width = 6;
+  optional uint32 height = 7;
 }

modules/drivers/velodyne/conf/readme.txt

@@ -0,0 +1,5 @@
+first check imutoantoffset saved in device, method below:
+screen /dev/ttyUSB2
+
+log imutoantoffsets
+

modules/drivers/velodyne/conf/velodyne64_conf.pb.txt

@@ -0,0 +1,15 @@
+frame_id: "velodyne64"
+scan_channel: "/apollo/sensor/velodyne64/VelodyneScanUnified"
+rpm: 600.0
+model: HDL64E_S3D
+mode: STRONGEST
+prefix_angle: 18000
+firing_data_port: 2368
+use_sensor_sync: false
+max_range: 100.0
+min_range: 0.9 
+calibration_online: true
+calibration_file: "/home/caros/cybertron/params/velodyne64_S3_calibration.yaml"
+organized: false
+convert_channel_name: "/apollo/sensor/velodyne64/PointCloud2"
+compensator_channel_name: "/apollo/sensor/velodyne64/compensator/PointCloud2" 

modules/drivers/velodyne/dag/dag_streaming_velodyne.dag

@@ -0,0 +1,41 @@
+# Define all coms in DAG streaming.
+module_config {
+    module_library : "/apollo/bazel-bin/modules/drivers/velodyne/driver/libvelodyne_driver_component.so"
+
+    components {
+      class_name : "VelodyneDriverComponent"
+      config {
+        name : "velodyne_driver"
+        config_file_path : "/apollo/modules/drivers/velodyne/conf/velodyne64_conf.pb.txt"
+        readers {channel: ""}
+      }
+    }
+}
+
+module_config {
+    module_library : "/apollo/bazel-bin/modules/drivers/velodyne/parser/libvelodyne_convert_component.so"
+
+    components {
+      class_name : "VelodyneConvertComponent"
+      config {
+        name : "velodyne_convert"
+        config_file_path : "/apollo/modules/drivers/velodyne/conf/velodyne64_conf.pb.txt"
+        readers {channel: "/apollo/sensor/velodyne64/VelodyneScan"}
+      }
+    }
+}
+
+module_config {
+    module_library : "/apollo/bazel-bin/modules/drivers/velodyne/parser/libvelodyne_compensator_component.so"
+
+    components {
+      class_name : "VelodyneCompensatorComponent"
+      config {
+        name : "velodyne_compensator"
+        config_file_path : "/apollo/modules/drivers/velodyne/conf/velodyne64_conf.pb.txt"
+        readers {channel: "/apollo/sensor/velodyne64/PointCloud2"}
+      }
+    }
+}
+
+

modules/drivers/velodyne/driver/BUILD

@@ -0,0 +1,49 @@
+load("//tools:cpplint.bzl", "cpplint")
+
+package(default_visibility = ["//visibility:public"])
+
+cc_binary(
+    name = "libvelodyne_driver_component.so",
+    deps = [":velodyne_driver_component_lib"],
+    linkopts = ["-shared"],
+)
+
+cc_library(
+    name = "velodyne_driver_component_lib",
+    srcs = ["velodyne_driver_component.cc"],
+    hdrs = ["velodyne_driver_component.h"],
+    deps = [
+        "//modules/drivers/velodyne/driver:driver",
+        "//modules/common/util:message_util",
+        "//framework:cybertron",
+    ],
+    copts = ['-DMODULE_NAME=\\"velodyne\\"']
+)
+
+cc_library(
+    name = "driver",
+    srcs = [
+        "driver.cc",
+        "driver16.cc",
+        "driver32.cc",
+        "driver64.cc",
+        "input.cc",
+        "socket_input.cc",
+    ],
+    hdrs = [
+        "driver.h",
+        "input.h",
+        "socket_input.h",
+    ],
+    linkopts = [
+        "-lpcap",
+    ],
+    deps = [
+        "//modules/common/util",
+        "//modules/drivers/velodyne/proto:velodyne_proto",
+        "//framework:cybertron",
+        "@pcl",
+    ],
+)
+
+cpplint()

modules/drivers/velodyne/velodyne_driver/src/driver/driver.cpp → modules/drivers/velodyne/driver/driver.cc

@@ -14,13 +14,15 @@
  * limitations under the License.
  *****************************************************************************/
 
-#include "driver.h"
-
 #include <cmath>
 #include <ctime>
 #include <string>
 
-#include "ros/ros.h"
+#include <cybertron/cybertron.h>
+
+#include "modules/drivers/velodyne/proto/config.pb.h"
+#include "modules/drivers/velodyne/proto/velodyne.pb.h"
+#include "modules/drivers/velodyne/driver/driver.h"
 
 namespace apollo {
 namespace drivers {
@@ -41,7 +43,7 @@ void VelodyneDriver::set_base_time_from_nmea_time(NMEATimePtr nmea_time,
   // set last gps time using gps socket packet
   last_gps_time_ = (nmea_time->min * 60 + nmea_time->sec) * 1e6;
 
-  ROS_INFO("Set base unix time : %d-%d-%d %d:%d:%d", time.tm_year, time.tm_mon,
+  LOG_INFO_FORMAT("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>(timegm(&time));
   basetime = unix_base * 1e6;
@@ -60,18 +62,19 @@ bool VelodyneDriver::set_base_time() {
   }
 
   set_base_time_from_nmea_time(nmea_time, basetime_);
-  input_->init(config_.firing_data_port);
+  input_->init(config_.firing_data_port());
   return true;
 }
 
-int VelodyneDriver::poll_standard(velodyne_msgs::VelodyneScanUnifiedPtr& scan) {
+int VelodyneDriver::poll_standard(std::shared_ptr<VelodyneScan>& scan) {
   // Since the velodyne delivers data at a very high rate, keep reading and
   // publishing scans as fast as possible.
-  scan->packets.resize(config_.npackets);
-  for (int i = 0; i < config_.npackets; ++i) {
+  // scan->packets.resize(config_.npackets);
+  for (int i = 0; i < config_.npackets(); ++i) {
     while (true) {
       // keep reading until full packet received
-      int rc = input_->get_firing_data_packet(&scan->packets[i]);
+      VelodynePacket* packet = scan->add_firing_pkts();
+      int rc = input_->get_firing_data_packet(packet);
 
       if (rc == 0) {
         break;  // got a full packet?
@@ -91,57 +94,42 @@ void VelodyneDriver::update_gps_top_hour(uint32_t current_time) {
   if (last_gps_time_ > current_time) {
     if (std::abs(last_gps_time_ - current_time) > 3599000000) {
       basetime_ += 3600 * 1e6;
-      ROS_INFO_STREAM("Base time plus 3600s. Model: "
-                      << config_.model << std::fixed << ". current:"
-                      << current_time << ", last time:" << last_gps_time_);
+      AINFO << "Base time plus 3600s. Model: " << config_.model() << std::fixed
+            << ". current:" << current_time << ", last time:" << last_gps_time_;
     } else {
-      ROS_WARN_STREAM("Current stamp:" << std::fixed << current_time
-                                       << " less than previous stamp:"
-                                       << last_gps_time_
-                                       << ". GPS time stamp maybe incorrect!");
+      AWARN << "Current stamp:" << std::fixed << current_time
+            << " less than previous stamp:" << last_gps_time_
+            << ". GPS time stamp maybe incorrect!";
     }
   }
   last_gps_time_ = current_time;
 }
 
 VelodyneDriver* VelodyneDriverFactory::create_driver(
-    ros::NodeHandle private_nh) {
-  Config config;
-  // use private node handle to get parameters
-  private_nh.param("frame_id", config.frame_id, std::string("velodyne"));
-  private_nh.param("model", config.model, std::string("64E"));
-  private_nh.param("topic", config.topic, std::string("velodyne_packets"));
-  private_nh.param("firing_data_port", config.firing_data_port,
-                   FIRING_DATA_PORT);
-  private_nh.param("positioning_data_port", config.positioning_data_port,
-                   POSITIONING_DATA_PORT);
-  private_nh.param("rpm", config.rpm, 600.0);
-  private_nh.param("prefix_angle", config.prefix_angle, 18000);
-
-  if (config.prefix_angle > 35900 || config.prefix_angle < 100) {
-    ROS_WARN_STREAM(
-        "invalid prefix angle, prefix_angle must be between 100 and 35900");
-    if (config.prefix_angle > 35900) {
-      config.prefix_angle = 35900;
-    } else if (config.prefix_angle < 100) {
-      config.prefix_angle = 100;
+    const Config& config) {
+
+  Config new_config = config;
+  if (new_config.prefix_angle() > 35900 || new_config.prefix_angle() < 100) {
+    AWARN << "invalid prefix angle, prefix_angle must be between 100 and 35900";
+    if (new_config.prefix_angle() > 35900) {
+      new_config.set_prefix_angle(35900);
+    } else if (new_config.prefix_angle() < 100) {
+      new_config.set_prefix_angle(100);
     }
   }
-
-  private_nh.param("use_sensor_sync", config.use_sensor_sync);
-
-  if (config.model == "64E_S2" || config.model == "64E_S3S" ||
-      config.model == "64E_S3D_STRONGEST" || config.model == "64E_S3D_LAST" ||
-      config.model == "64E_S3D_DUAL") {
+  if (config.model() == HDL64E_S2 
+      || config.model() == HDL64E_S3S 
+      || config.model() == HDL64E_S3D) {
     return new Velodyne64Driver(config);
-  } else if (config.model == "HDL32E") {
+  } else if (config.model() == HDL32E) {
     return new Velodyne32Driver(config);
-  } else if (config.model == "VLP16") {
+  } else if (config.model() == VLP16) {
     return new Velodyne16Driver(config);
+  } else if (config.model() == VLS128) {
+    return nullptr;
   } else {
-    ROS_ERROR_STREAM(
-        "invalid model, must be 64E_S2|64E_S3S"
-        << "|64E_S3D_STRONGEST|64E_S3D_LAST|64E_S3D_DUAL|VLP16|HDL32E");
+    AERROR << "invalid model, must be 64E_S2|64E_S3S"
+           << "|64E_S3D|VLP16|HDL32E|VLS128";
     return nullptr;
   }
 }