i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

deskewing: added console logging, handling case where timestamps are not ordered in lidar rings

Browse Source
This commit is contained in:
matlabbe
2023-09-10 15:56:23 -07:00
parent 40a9fd2f2d
commit 2c46b2b3ad
2 changed files with 103 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_conversions/src/MsgConversion.cpp
+101 -10
View File
@@ -2669,17 +2669,103 @@ 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
{
@@ -2687,9 +2773,14 @@ bool deskew_impl(
return false;
}
if(lastStamp <= firstStamp)
if(lastStamp < firstStamp)
{
UERROR("First and last stamps in the scan are the same!");
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 (%f) (header=%f)!", timestampFromROS(lastStamp), timestampFromROS(input.header.stamp));
return false;
}
std::string errorMsg;