From 0dda063bfc2fb1a83eda24054d788efe2c24386d Mon Sep 17 00:00:00 2001
From: rostest <hhoss@gmx.de>
Date: Wed, 4 May 2022 11:36:01 +0200
Subject: [PATCH] Timestamp Laserscan message corrected #47

Timestamp of pointcloud and laserscan messages identical and computed from lidar ticks by software-pll
---
 CHANGELOG.md                       | 3 +++
 driver/src/sick_generic_caller.cpp | 2 +-
 driver/src/sick_scan_common.cpp    | 6 +++++-
 3 files changed, 9 insertions(+), 2 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 94f84360..fd18bd56 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -8,6 +8,9 @@ features that will be removed in future versions **Removed** for deprecated feat
 
 ## Released ##
 
+### v2.6.3 - 
+  - **Fixed** Timestamp Laserscan message corrected
+
 ### v2.6.2 - 
   - **Fixed** LDMRS spinning problem corrected
 
diff --git a/driver/src/sick_generic_caller.cpp b/driver/src/sick_generic_caller.cpp
index 4cbf930a..ea949eeb 100644
--- a/driver/src/sick_generic_caller.cpp
+++ b/driver/src/sick_generic_caller.cpp
@@ -81,7 +81,7 @@
 
 #define SICK_GENERIC_MAJOR_VER "2"
 #define SICK_GENERIC_MINOR_VER "6"
-#define SICK_GENERIC_PATCH_LEVEL "2"
+#define SICK_GENERIC_PATCH_LEVEL "3"
 
 #include <algorithm> // for std::min
 
diff --git a/driver/src/sick_scan_common.cpp b/driver/src/sick_scan_common.cpp
index bfe2bc49..5edae23c 100644
--- a/driver/src/sick_scan_common.cpp
+++ b/driver/src/sick_scan_common.cpp
@@ -3785,7 +3785,7 @@ namespace sick_scan
         bool FireEncoder = false;
         EncoderMsg.header.frame_id = "Encoder";
         ROS_HEADER_SEQ(EncoderMsg.header, numPacketsProcessed);
-        msg.header.stamp = recvTimeStamp + rosDuration(config_.time_offset);
+        msg.header.stamp = recvTimeStamp + rosDuration(config_.time_offset); // default: ros-timestamp at message received, will be updated by software-pll
         double elevationAngleInRad = 0.0;
         short elevAngleX200 = 0;  // signed short (F5 B2  -> Layer 24
         // F5B2h -> -2638/200= -13.19°
@@ -3940,6 +3940,10 @@ namespace sick_scan
                       dataToProcess = false;
                       break;
                     }
+                    else
+                    {
+                      msg.header.stamp = recvTimeStamp + rosDuration(config_.time_offset); // update timestamp by software-pll
+					}
                   }
 
 #ifdef DEBUG_DUMP_ENABLED