Select Git revision
vtkVelodyneHDLPositionPacketInterpreter.cxx
Timothée Couble authored
vtkVelodyneHDLPositionPacketInterpreter.cxx 17.94 KiB
#include "vtkVelodyneHDLPositionPacketInterpreter.h"
#include "vtkVelodyneHDLPositionPacket.h"
#include "InterpreterHelper.h"
#include <vtkDoubleArray.h>
#include <vtkMath.h>
#include <vtkNew.h>
#include <vtkPointData.h>
#include <vtkTransform.h>
#include <vtkTupleInterpolator.h>
#include "vtkCustomTransformInterpolator.h"
#include "NMEAParser.h"
#include "statistics.h"
#include <iomanip>
#include <sstream>
#include <boost/algorithm/string.hpp>
#include <proj.h>
//-----------------------------------------------------------------------------
// This function is not used anymore
// It was used to apply the transform to lidar points
// This mecanism is broken by thi commit :
// message commit : "Prepare multi sensor support to the Lidar Provider and clean up a bit"
// sha : f7b88847830c213e4df72fbbb660c3fcf80e80b4
void vtkVelodyneHDLPositionPacketInterpreter::FillInterpolatorFromGPSAndHeading(
vtkPoints* points, vtkDataArray* gpsTime, vtkDataArray* times, vtkDataArray* headings)
{
vtkNew<vtkTupleInterpolator> headingInterpolator;
// assert(gpsTime is sorted)
assert(points->GetNumberOfPoints() == times->GetNumberOfTuples());
this->Interp->SetInterpolationTypeToLinear();
this->Interp->Initialize();
headingInterpolator->SetInterpolationTypeToLinear();
headingInterpolator->SetNumberOfComponents(2);
assert(times->GetNumberOfTuples() == gpsTime->GetNumberOfTuples());
double lastGPS = 0.0;
for (vtkIdType i = 0, k = times->GetNumberOfTuples(); i < k; ++i)
{
const double currGPS = gpsTime->GetTuple1(i);
if (currGPS != lastGPS)
{
// Get position and heading
double pos[3];
points->GetPoint(i, pos);
const double heading = headings->GetTuple1(i);
// Check the input data
bool isTranslationFinite =
vtkMath::IsFinite(pos[0]) && vtkMath::IsFinite(pos[1]) && vtkMath::IsFinite(pos[2]);
bool isRotationFinite = vtkMath::IsFinite(heading);
// Compute transform
// Here we want to compute the transform to go
// from the solid referential frame to the world
// georeferenced frame. Hence, given the position
// and orientation of the GPS in the solid frame we
// need to apply the transform from the solid frame
// to the GPS (backward gps pose) and then the transform
// from the GPS to the world georeferenced frame
vtkNew<vtkTransform> transformGpsWorld, transformVehiculeWorld;
transformGpsWorld->PostMultiply();
if (isRotationFinite) transformGpsWorld->RotateZ(heading);
else
vtkGenericWarningMacro("Error in GPS rotation");
if (isTranslationFinite)
transformGpsWorld->Translate(pos);
else
vtkGenericWarningMacro("Error in GPS position");
// Compute transform from vehicule to GPS
// and then compose with the transform GPS to world
vtkNew<vtkMatrix4x4> gpsToWorld, vehiculeToGps, vehiculeToWorld;
if(this->SensorTransform) this->SensorTransform->GetMatrix(vehiculeToGps.Get());
transformGpsWorld->GetMatrix(gpsToWorld.Get());
vehiculeToGps->Invert();
vtkMatrix4x4::Multiply4x4(gpsToWorld.Get(), vehiculeToGps.Get(), vehiculeToWorld.Get());
transformVehiculeWorld->SetMatrix(vehiculeToWorld.Get());
transformVehiculeWorld->Modified();
// Add the transform to the interpolator
this->Interp->AddTransform(currGPS, transformVehiculeWorld.GetPointer());
// Compute heading vector for interpolation
double ha = proj_torad(heading);
double hv[2] = { cos(ha), sin(ha) };
headingInterpolator->AddTuple(currGPS, hv);
}
}
}
//-----------------------------------------------------------------------------
vtkStandardNewMacro(vtkVelodyneHDLPositionPacketInterpreter)
//-----------------------------------------------------------------------------
vtkVelodyneHDLPositionPacketInterpreter::vtkVelodyneHDLPositionPacketInterpreter()
{
this->Interp = vtkSmartPointer<vtkCustomTransformInterpolator>::New();
this->ResetCurrentData();
this->pointcount = 0;
this->Offset[0] = 0.0;
this->Offset[1] = 0.0;
this->Offset[2] = 0.0;
this->UseGPGGASentences = false;
this->PPSSynced = false;
this->LastPPSState = PPSState::PPS_ABSENT;
this->HasTimeshiftEstimation = false;
this->TimeshiftMeasurements.clear();
this->AssumedHardwareLag = 0.094; // always positive, in seconds
this->hasLastGPSUpdateTime = false;
this->lastGPSUpdateTime = 0.0;
this->GPSTimeOffset = 0.0;
this->convertedGPSUpdateTime = 0.0;
this->hasLastLidarUpdateTime = false;
this->lastLidarUpdateTime = 0.0;
this->lidarTimeOffset = 0.0;
this->previousConvertedGPSUpdateTime = -1.0; // negative means "no previous"
}
//----------------------------------------------------------------------------
void vtkVelodyneHDLPositionPacketInterpreter::InitArrays()
{
this->points = vtkSmartPointer<vtkPoints>::New();
AddArrayToTableIfNotNull(this->lats, this->RawInformation, "lat");
AddArrayToTableIfNotNull(this->lons, this->RawInformation, "lon"); AddArrayToTableIfNotNull(this->times, this->RawInformation, "time");
AddArrayToTableIfNotNull(this->gpsTime, this->RawInformation, "gpsTime");
AddArrayToTableIfNotNull(this->gyro1, this->RawInformation, "gyro1");
AddArrayToTableIfNotNull(this->gyro2, this->RawInformation, "gyro2");
AddArrayToTableIfNotNull(this->gyro3, this->RawInformation, "gyro3");
AddArrayToTableIfNotNull(this->temp1, this->RawInformation, "temp1");
AddArrayToTableIfNotNull(this->temp2, this->RawInformation, "temp2");
AddArrayToTableIfNotNull(this->temp3, this->RawInformation, "temp3");
AddArrayToTableIfNotNull(this->accel1x, this->RawInformation, "accel1x");
AddArrayToTableIfNotNull(this->accel1y, this->RawInformation, "accel1y");
AddArrayToTableIfNotNull(this->accel2x, this->RawInformation, "accel2x");
AddArrayToTableIfNotNull(this->accel2y, this->RawInformation, "accel2y");
AddArrayToTableIfNotNull(this->accel3x, this->RawInformation, "accel3x");
AddArrayToTableIfNotNull(this->accel3y, this->RawInformation, "accel3y");
AddArrayToTableIfNotNull(this->heading, this->RawInformation, "heading");
this->PositionOrientation->SetPoints(this->points);
// Add also Time array to the position orientation vtkPolyData:
// So we can color by time
this->PositionOrientation->GetPointData()->AddArray(this->times);
}
//----------------------------------------------------------------------------
bool vtkVelodyneHDLPositionPacketInterpreter::IsValidPacket(unsigned char const * packetData,
unsigned int dataLength)
{
if (dataLength != 512)
{
// Data-Packet Specifications says that position-packets are 512 byte long.
return false;
}
for (int i = 0; i < 14; ++i)
{
if (packetData[i] != 0)
{
std::cerr << "unexpected data in first zeros block\n";
return false;
}
}
return true;
}
//----------------------------------------------------------------------------
void vtkVelodyneHDLPositionPacketInterpreter::ProcessPacket(unsigned char const * packetData,
unsigned int vtkNotUsed(dataLength))
{
PositionPacket position;
// Reinterpret the memory
for (int i = 0; i < 3; ++i)
{
memcpy(position.gyro + i, packetData + 14 + i * 8, 2);
memcpy(position.temp + i, packetData + 14 + i * 8 + 2, 2);
memcpy(position.accelx + i, packetData + 14 + i * 8 + 4, 2);
memcpy(position.accely + i, packetData + 14 + i * 8 + 6, 2);
}
for (int i = 0; i < 3; ++i)
{
// Selector only least significant 12 bits
position.gyro[i] &= BIT_12_MASK;
position.temp[i] &= BIT_12_MASK;
position.accelx[i] &= BIT_12_MASK;
position.accely[i] &= BIT_12_MASK;
// Perform 12 bit twos complement
position.gyro[i] =
-2048 * ((position.gyro[i] & SIGN_12_MASK) >> 11) + (position.gyro[i] & REMAINDER_12_MASK);
position.temp[i] =
-2048 * ((position.temp[i] & SIGN_12_MASK) >> 11) + (position.temp[i] & REMAINDER_12_MASK);
position.accelx[i] = -2048 * ((position.accelx[i] & SIGN_12_MASK) >> 11) +
(position.accelx[i] & REMAINDER_12_MASK);
position.accely[i] = -2048 * ((position.accely[i] & SIGN_12_MASK) >> 11) +
(position.accely[i] & REMAINDER_12_MASK);
}
memcpy(&position.tohTimestamp, packetData + 14 + 3 * 8 + 160, 4);
// ethernet payload starts at byte 2A, PPS byte is at F4 inside full ethernet
// frame, so PPS in payload is at F4 - 2A = 244 - 42 = 202
memcpy(&position.PPSSync, packetData + 202, 1);
const int sentence_start = 14 + 8 + 8 + 8 + 160 + 4 + 4;
std::copy(packetData + sentence_start,
packetData + sentence_start + 306,
position.sentance);
// protection to terminate the string in case the sentence does not fit in the
// 306 bytes.
position.sentance[305] = '\0';
if (!hasLastLidarUpdateTime)
{
hasLastLidarUpdateTime = true;
lastLidarUpdateTime = position.tohTimestamp;
}
else
{
if (position.tohTimestamp - lastLidarUpdateTime < - 1e6 * 0.5 * 3600.0)
{
// tod wrap detected
lidarTimeOffset += 1e6 * 3600.0;
}
}
double convertedLidarUpdateTime = position.tohTimestamp + lidarTimeOffset;
// Fill all arrays with information
double x, y, z, lat, lon, heading, gpsUpdateTime;
if (std::string(position.sentance).size() == 0)
{
// If there is no sentence to parse (no gps connected),
// we use the following:
x = 0.0;
y = 0.0;
z = 0.0;
lat = 0.0;
lon = 0.0;
heading = 0.0;
// the following value is wrong (no gps update so no update time),
// so it should also be 0.0 (invalid)
// but this value is kept to not risk breaking anything:
gpsUpdateTime = position.tohTimestamp;
}
else
{
NMEAParser parser;
NMEALocation parsedNMEA;
parsedNMEA.Init();
std::string NMEASentence = std::string(position.sentance);
boost::trim_right(NMEASentence);
std::vector<std::string> NMEAwords = parser.SplitWords(NMEASentence);
if (!parser.ChecksumValid(NMEASentence))
{
vtkGenericWarningMacro("NMEA sentence: "
<< "<" << NMEASentence << ">"
<< "has invalid checksum"); // TODO: should we skip or should we expect lazy NMEA implementers ?
}
if ((this->UseGPGGASentences && !parser.IsGPGGA(NMEAwords))
|| (!this->UseGPGGASentences && !parser.IsGPRMC(NMEAwords)))
{
return; // not the NMEA sentence we are interested in, skipping
}
if ( !( (parser.IsGPGGA(NMEAwords) && parser.ParseGPGGA(NMEAwords, parsedNMEA))
|| (parser.IsGPRMC(NMEAwords) && parser.ParseGPRMC(NMEAwords, parsedNMEA)) ))
{
vtkGenericWarningMacro("Failed to parse NMEA sentence: "
<< "<" << NMEASentence << ">");
return; // skipping this PositionPacket
}
// Gathering information on time synchronization between Lidar & GPS,
// see Velodyne doc mentioned at definition of PositionPacket above.
// We assume the Lidar will remain synchronized on gps (UTC) time even
// if some NMEA packets without fix are received later (tunnel, hill ...).
if (!this->PPSSynced
&& position.PPSSync == PPSState::PPS_LOCKED
&& parsedNMEA.Valid)
{
this->PPSSynced = true;
}
this->LastPPSState = static_cast<PPSState>(position.PPSSync);
lat = parsedNMEA.Lat;
lon = parsedNMEA.Long;
x = 0.0;
y = 0.0;
this->utmProjector.Project(lat, lon, x, y);
z = 0.0;
// If sentence is GPGGA, we have a chance to get an altitude
if (parser.IsGPGGA(NMEAwords))
{
if (parsedNMEA.HasAltitude)
{
if (parsedNMEA.HasGeoidalSeparation)
{
// setting z to Height above ellipsoid
// (coherent with setting 'datum=WGS84' in proj4)
z = parsedNMEA.Altitude + parsedNMEA.GeoidalSeparation;
}
else
{
// Two possibilities: either Altitude is actually height above
// ellipsoid, or it is effectively height above local MSL.
// In this case we could need something better than this
// (such as a look up table for geoid separation like GeographicLib)
z = parsedNMEA.Altitude;
}
}
else
{
z = 0.0;
}
}
if (parsedNMEA.HasTrackAngle)
{
heading = parsedNMEA.TrackAngle;
}
else
{
heading = 0.0;
}
gpsUpdateTime = parsedNMEA.UTCSecondsOfDay;
if (!hasLastGPSUpdateTime)
{
hasLastGPSUpdateTime = true;
lastGPSUpdateTime = gpsUpdateTime;
}
else
{
if (gpsUpdateTime - lastGPSUpdateTime < - 12.0 * 3600.0)
{
// tod wrap detected
GPSTimeOffset += 24.0 * 3600.0;
}
}
convertedGPSUpdateTime = gpsUpdateTime + GPSTimeOffset;
if (previousConvertedGPSUpdateTime < 0.0)
{
previousConvertedGPSUpdateTime = convertedGPSUpdateTime;
}
if (convertedGPSUpdateTime > previousConvertedGPSUpdateTime
&& parsedNMEA.Valid)
{
// We have detected that this position packet is the first one since
// last gps fix (there are more position packets than there are fixes)
// and that the new NMEA sentence refers to a valid fix,
// so we can do an estimation of the timeshift.
this->HasTimeshiftEstimation = true;
// To understand this formula, think that we want to add this timeshift
// to a lidar time to get a gps time, and that the "lidar instant" that
// corresponds to the new fix is earlier than convertedLidarUpdateTime,
// because of the time it took the information to go from GPS to Lidar.
// Possible improvement: store the different estimations
// (one per new fix) and return the median.
this->TimeshiftMeasurements.push_back(
convertedGPSUpdateTime -
(1e-6 * convertedLidarUpdateTime - this->AssumedHardwareLag));
}
previousConvertedGPSUpdateTime = convertedGPSUpdateTime;
}
if (this->pointcount == 0)
{
this->Offset[0] = x;
this->Offset[1] = y;
}
x -= this->Offset[0];
y -= this->Offset[1];
// Update the transforms here and then call internal transform
if (SensorTransform) this->SensorTransform->Update();
// Apply sensor transform
double pos[3] = {x, y, z};
if (SensorTransform) this->SensorTransform->InternalTransformPoint(pos, pos);
this->points->InsertNextPoint(pos[0], pos[1], pos[2]);
this->lats->InsertNextValue(lat);
this->lons->InsertNextValue(lon);
this->times->InsertNextValue(convertedLidarUpdateTime);
this->gpsTime->InsertNextValue(convertedGPSUpdateTime);
this->gyro1->InsertNextValue(position.gyro[0] * GYRO_SCALE);
this->gyro2->InsertNextValue(position.gyro[1] * GYRO_SCALE);
this->gyro3->InsertNextValue(position.gyro[2] * GYRO_SCALE);
this->temp1->InsertNextValue(position.temp[0] * TEMP_SCALE + TEMP_OFFSET);
this->temp2->InsertNextValue(position.temp[1] * TEMP_SCALE + TEMP_OFFSET); this->temp3->InsertNextValue(position.temp[2] * TEMP_SCALE + TEMP_OFFSET);
this->accel1x->InsertNextValue(position.accelx[0] * ACCEL_SCALE);
this->accel2x->InsertNextValue(position.accelx[1] * ACCEL_SCALE);
this->accel3x->InsertNextValue(position.accelx[2] * ACCEL_SCALE);
this->accel1y->InsertNextValue(position.accely[0] * ACCEL_SCALE);
this->accel2y->InsertNextValue(position.accely[1] * ACCEL_SCALE);
this->accel3y->InsertNextValue(position.accely[2] * ACCEL_SCALE);
this->heading->InsertNextValue(heading);
this->pointcount++;
}
void vtkVelodyneHDLPositionPacketInterpreter::FillInterpolatorFromPositionOrientation()
{
// Optionally interpolate the GPS values... note that we assume that the
// first GPS point is not 0,0 if we have valid GPS data; otherwise we assume
// that the GPS data is garbage and ignore it
if (lats->GetNumberOfTuples() && lons->GetNumberOfTuples() &&
(lats->GetValue(0) != 0.0 || lons->GetValue(0) != 0.0))
{
this->FillInterpolatorFromGPSAndHeading(points, gpsTime, times, this->heading);
}
}
//-----------------------------------------------------------------------------
vtkCustomTransformInterpolator* vtkVelodyneHDLPositionPacketInterpreter::GetInterpolator()
{
return this->Interp.GetPointer();
}
//-----------------------------------------------------------------------------
double vtkVelodyneHDLPositionPacketInterpreter::GetTimeshiftEstimation() {
if (this->TimeshiftMeasurements.size() == 0)
{
vtkGenericWarningMacro("Error : timeshift estimation asked"
" but no measurement available.");
return 0.0;
}
return ComputeMedian(this->TimeshiftMeasurements);
}
//-----------------------------------------------------------------------------
std::string vtkVelodyneHDLPositionPacketInterpreter::GetTimeSyncInfo()
{
std::string PPSDesc;
if (this->LastPPSState == PPSState::PPS_ABSENT)
{
PPSDesc = "absent";
}
else if (this->LastPPSState == PPSState::PPS_ATTEMPTING_TO_SYNC)
{
PPSDesc = "attempting to sync";
}
else if (this->LastPPSState == PPSState::PPS_LOCKED)
{
PPSDesc = "locked";
}
else if (this->LastPPSState == PPSState::PPS_ERROR)
{
PPSDesc = "error";
}
else
{
PPSDesc = "unknown";
}
if (!this->PPSSynced && this->HasTimeshiftEstimation)
{
std::ostringstream timeshiftEstimation;
timeshiftEstimation << std::fixed << std::setprecision(6) << this->GetTimeshiftEstimation();
std::ostringstream assumedHWLag;
assumedHWLag << std::fixed << std::setprecision(6)
<< this->AssumedHardwareLag;
vtkGenericWarningMacro("Recovered timeshift despite missing PPS sync: "
<< timeshiftEstimation.str()
<< " seconds (assuming hardware lag of: "
<< assumedHWLag.str()
<< " seconds)."
<< " Add this value to the lidar timestamps (ToH)"
<< " to get GPS UTC time (mod. 1 hour).");
}
return " Lidar PPS signal: "
+ PPSDesc
+ " - "
+ (this->PPSSynced ? "Lidar clock synced on GPS UTC ToH"
: "NO Lidar clock sync on GPS UTC ToH")
+ " ";
}