diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 3b1097fd..4d73fe16 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -2591,29 +2591,14 @@ bool deskew_impl( int timeDatatype = 6; for(size_t i=0; i secLast) + { + // scans are not ordered, we need to search min/max + static bool warned = false; + if(!warned) { + ROS_WARN("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 secLast) + { + secLast = sec; + } + } + } + else + { + for(size_t i=0; i secLast) + { + secLast = sec; + } + } + } + } + + firstStamp = ros::Time(secFirst); + lastStamp = ros::Time(secLast); + } else { ROS_ERROR("Not supported time datatype %d!", timeDatatype); return false; } - + if(!(timeDatatype >=6 && timeDatatype<=8)) + { + ROS_ERROR("Only lidar timestamp channel data type 6, 7 or 8 is supported! (received %d)", timeDatatype); + return false; + } if(lastStamp < firstStamp) { ROS_ERROR("Last stamp (%f) is smaller than first stamp (%f) (header=%f)!", lastStamp.toSec(), firstStamp.toSec(), input.header.stamp.toSec()); @@ -2896,11 +2934,16 @@ bool deskew_impl( unsigned int nsec = *((const unsigned int*)(&output.data[u*output.point_step]+offsetTime)); stamp = input.header.stamp+ros::Duration(0, nsec); } - else + else if(timeDatatype == 7) //float 32 { float sec = *((const float*)(&output.data[u*output.point_step]+offsetTime)); stamp = input.header.stamp+ros::Duration().fromSec(sec); } + else if(timeDatatype == 8) //float64 + { + double sec = *((const double*)(&output.data[u*output.point_step]+offsetTime)); + stamp = ros::Time(sec); + } rtabmap::Transform transform; if(slerp) @@ -2944,10 +2987,14 @@ bool deskew_impl( { *((unsigned int*)(dataPtr+offsetTime)) = 0; } - else + else if(timeDatatype == 7) { *((float*)(dataPtr+offsetTime)) = 0; } + else if(timeDatatype == 8) + { + *((double*)(dataPtr+offsetTime)) = 0; + } } } } @@ -2966,11 +3013,16 @@ bool deskew_impl( unsigned int nsec = *((const unsigned int*)(&output.data[v*output.row_step]+offsetTime)); stamp = input.header.stamp+ros::Duration(0, nsec); } - else + else if(timeDatatype == 7) //float 32 { float sec = *((const float*)(&output.data[v*output.row_step]+offsetTime)); stamp = input.header.stamp+ros::Duration().fromSec(sec); } + else if(timeDatatype == 8) + { + double sec = *((const double*)(&output.data[v*output.row_step]+offsetTime)); + stamp = ros::Time(sec); + } rtabmap::Transform transform; if(slerp) @@ -3014,10 +3066,14 @@ bool deskew_impl( { *((unsigned int*)(dataPtr+offsetTime)) = 0; } - else + else if(timeDatatype == 7) { *((float*)(dataPtr+offsetTime)) = 0; } + else if(timeDatatype == 8) + { + *((double*)(dataPtr+offsetTime)) = 0; + } } } }