Skip to content

Commit

Permalink
deskewing: added console logging, handling case where timestamps are …
Browse files Browse the repository at this point in the history
…not ordered in lidar rings
  • Loading branch information
matlabbe committed Sep 10, 2023
1 parent 40a9fd2 commit 2c46b2b
Show file tree
Hide file tree
Showing 2 changed files with 103 additions and 10 deletions.
111 changes: 101 additions & 10 deletions rtabmap_conversions/src/MsgConversion.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
2 changes: 2 additions & 0 deletions rtabmap_util/src/LidarDeskewingNode.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down

0 comments on commit 2c46b2b

Please sign in to comment.