deskewing: added console logging, handling case where timestamps are …

…not ordered in lidar rings
introlab · Sep 10, 2023 · 2c46b2b · 2c46b2b
1 parent 40a9fd2
commit 2c46b2b
Show file tree

Hide file tree

Showing 2 changed files with 103 additions and 10 deletions.
diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp
@@ -2669,27 +2669,118 @@ bool deskew_impl(
 	rclcpp::Time lastStamp;
 	if(timeDatatype == 6) // UINT32
 	{
-		unsigned int nsec = *((const unsigned int*)(&input.data[0]+offsetTime));
-		firstStamp = rclcpp::Time(input.header.stamp)+rclcpp::Duration(0, nsec);
-		nsec = *((const unsigned int*)(&input.data[timeOnColumns?(input.width-1)*input.point_step:(input.height-1)*input.row_step]+offsetTime));
-		lastStamp = rclcpp::Time(input.header.stamp)+rclcpp::Duration(0, nsec);
+		unsigned int nsecFirst = *((const unsigned int*)(&input.data[0]+offsetTime));
+		unsigned int nsecLast = *((const unsigned int*)(&input.data[(input.width-1)*input.point_step + input.row_step*(input.height-1)]+offsetTime));
+
+		if(nsecFirst > nsecLast)
+		{
+			// scans are not ordered, we need to search min/max
+			static bool warned = false;
+			if(!warned) {
+				UWARN("Timestamp channel is not ordered, we will have to parse every scans to "
+					  "determinate first and last time offsets. This will add slightly computation "
+					  "time. This warning is only shown once.");
+				warned = true;
+			}
+			if(timeOnColumns)
+			{
+				for(size_t i=0; i<input.width; ++i)
+				{
+					unsigned int nsec = *((const unsigned int*)(&input.data[(i)*input.point_step]+offsetTime));
+					if(nsec < nsecFirst)
+					{
+						nsecFirst = nsec;
+					}
+					else if(nsec > nsecLast)
+					{
+						nsecLast = nsec;
+					}
+				}
+			}
+			else
+			{
+				for(size_t i=0; i<input.height; ++i)
+				{
+					unsigned int nsec = *((const unsigned int*)(&input.data[input.row_step*(i)]+offsetTime));
+					if(nsec < nsecFirst)
+					{
+						nsecFirst = nsec;
+					}
+					else if(nsec > nsecLast)
+					{
+						nsecLast = nsec;
+					}
+				}
+			}
+		}
+
+		firstStamp = rclcpp::Time(input.header.stamp)+rclcpp::Duration(0, nsecFirst);
+		lastStamp = rclcpp::Time(input.header.stamp)+rclcpp::Duration(0, nsecLast);
 	}
 	else if(timeDatatype == 7) // FLOAT32
 	{
-		float sec = *((const float*)(&input.data[0]+offsetTime));
-		firstStamp = rclcpp::Time(input.header.stamp)+rclcpp::Duration::from_seconds(sec);
-		sec = *((const float*)(&input.data[timeOnColumns?(input.width-1)*input.point_step:(input.height-1)*input.row_step]+offsetTime));
-		lastStamp = rclcpp::Time(input.header.stamp)+rclcpp::Duration::from_seconds(sec);
+		float secFirst = *((const float*)(&input.data[0]+offsetTime));
+		float secLast = *((const float*)(&input.data[(input.width-1)*input.point_step + input.row_step*(input.height-1)]+offsetTime));
+
+		if(secFirst > secLast)
+		{
+			// scans are not ordered, we need to search min/max
+			static bool warned = false;
+			if(!warned) {
+				UWARN("Timestamp channel is not ordered, we will have to parse every scans to "
+					  "determinate first and last time offsets. This will add slightly computation "
+					  "time. This warning is only shown once.");
+				warned = true;
+			}
+			if(timeOnColumns)
+			{
+				for(size_t i=0; i<input.width; ++i)
+				{
+					float sec = *((const float*)(&input.data[(i)*input.point_step]+offsetTime));
+					if(sec < secFirst)
+					{
+						secFirst = sec;
+					}
+					else if(sec > secLast)
+					{
+						secLast = sec;
+					}
+				}
+			}
+			else
+			{
+				for(size_t i=0; i<input.height; ++i)
+				{
+					float sec = *((const float*)(&input.data[input.row_step*(i)]+offsetTime));
+					if(sec < secFirst)
+					{
+						secFirst = sec;
+					}
+					else if(sec > secLast)
+					{
+						secLast = sec;
+					}
+				}
+			}
+		}
+
+		firstStamp = rclcpp::Time(input.header.stamp)+rclcpp::Duration::from_seconds(secFirst);
+		lastStamp = rclcpp::Time(input.header.stamp)+rclcpp::Duration::from_seconds(secLast);
 	}
 	else
 	{
 		UERROR("Not supported time datatype %d!", timeDatatype);
 		return false;
 	}
 
-	if(lastStamp <= firstStamp)
+	if(lastStamp < firstStamp)
+	{
+		UERROR("Last stamp (%f) is smaller than first stamp (%f) (header=%f)!", timestampFromROS(lastStamp), timestampFromROS(firstStamp), timestampFromROS(input.header.stamp));
+		return false;
+	}
+	else if(lastStamp == firstStamp)
 	{
-		UERROR("First and last stamps in the scan are the same!");
+		UERROR("First and last stamps in the scan are the same (%f) (header=%f)!", timestampFromROS(lastStamp), timestampFromROS(input.header.stamp));
 		return false;
 	}
 	std::string errorMsg;

diff --git a/rtabmap_util/src/LidarDeskewingNode.cpp b/rtabmap_util/src/LidarDeskewingNode.cpp
@@ -26,10 +26,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include "rtabmap_util/lidar_deskewing.hpp"
+#include "rtabmap/utilite/ULogger.h"
 #include "rclcpp/rclcpp.hpp"
 
 int main(int argc, char **argv)
 {
+	ULogger::setType(ULogger::kTypeConsole);
 	rclcpp::init(argc, argv);
 	rclcpp::spin(std::make_shared<rtabmap_util::LidarDeskewing>(rclcpp::NodeOptions()));
 	rclcpp::shutdown();