Added config option to timestamp a full scan based on first velo pack…

…et instead of last packet

velodyne_driver/config/VLP16-velodyne_driver_node-params.yaml

@@ -11,3 +11,4 @@ velodyne_driver_node:
         model: VLP16
         rpm: 600.0
         port: 2368
+        timestamp_first_packet: false

velodyne_driver/config/VLP32C-velodyne_driver_node-params.yaml

@@ -11,3 +11,4 @@ velodyne_driver_node:
         model: 32C
         rpm: 600.0
         port: 2368
+        timestamp_first_packet: false

velodyne_driver/include/velodyne_driver/driver.hpp

@@ -71,6 +71,7 @@ class VelodyneDriver final : public rclcpp::Node
     int cut_angle;                   // cutting angle in 1/100°
     double time_offset;              // time in seconds added to each velodyne time stamp
     bool enabled;                    // polling is enabled
+    bool timestamp_first_packet;
   }
   config_;
 

velodyne_driver/src/driver/driver.cpp

@@ -80,6 +80,7 @@ VelodyneDriver::VelodyneDriver(const rclcpp::NodeOptions & options)
   std::string dump_file = this->declare_parameter("pcap", std::string(""));
   double cut_angle = this->declare_parameter("cut_angle", 2.0 * M_PI);
   int udp_port = this->declare_parameter("port", static_cast<int>(DATA_PORT_NUMBER));
+  config_.timestamp_first_packet = this->declare_parameter("timestamp_first_packet", false);
 
   future_ = exit_signal_.get_future();
 
@@ -133,6 +134,13 @@ VelodyneDriver::VelodyneDriver(const rclcpp::NodeOptions & options)
     cut_angle = -0.01;
   }
 
+  // if we are timestamping based on the first or last packet in the scan
+  if (config_.timestamp_first_packet) {
+    RCLCPP_INFO(
+      this->get_logger(),
+      "Setting velodyne scan start time to timestamp of first packet");
+  }
+
   // Convert cut_angle from radian to one-hundredth degree,
   // which is used in velodyne packets
   config_.cut_angle = static_cast<int>((cut_angle * 360 / (2 * M_PI)) * 100);
@@ -253,7 +261,8 @@ bool VelodyneDriver::poll()
 
   // publish message using time of last packet read
   RCLCPP_DEBUG(this->get_logger(), "Publishing a full Velodyne scan.");
-  builtin_interfaces::msg::Time stamp = scan->packets.back().stamp;
+  builtin_interfaces::msg::Time stamp =
+    config_.timestamp_first_packet ? scan->packets.front().stamp : scan->packets.back().stamp;
   scan->header.stamp = stamp;
   scan->header.frame_id = config_.frame_id;
   output_->publish(std::move(scan));