orca::RangeScanner2dDescription
convert( const hydroscanutil::ScannerConfig &scannerConfig,
const hydronavutil::Pose &scannerOffset,
const hydrotime::Time &timeStamp )
{
orca::RangeScanner2dDescription orcaDescr;
orcaDescr.timeStamp.seconds = timeStamp.seconds();
orcaDescr.timeStamp.useconds = timeStamp.useconds();
orcaDescr.minRange = 0.0;
orcaDescr.maxRange = scannerConfig.maxRange;
orcaDescr.fieldOfView = scannerConfig.angleIncrement*(scannerConfig.numReturns-1);
orcaDescr.startAngle = scannerConfig.startAngle;
orcaDescr.numberOfSamples = scannerConfig.numReturns;
orcaDescr.offset.p.x = scannerOffset.x();
orcaDescr.offset.p.y = scannerOffset.y();
orcaDescr.offset.p.z = 0.0;
orcaDescr.offset.o.r = 0.0;
orcaDescr.offset.o.p = 0.0;
orcaDescr.offset.o.y = scannerOffset.theta();
orcaDescr.size.l = 0.1;
orcaDescr.size.w = 0.1;
orcaDescr.size.h = 0.1;
return orcaDescr;
}
OdometryDifferentiator::OdometryDifferentiator( const hydronavutil::Pose &initPose )
: prevOdomInitialised_(true)
{
prevX_ = initPose.x();
prevY_ = initPose.y();
prevTheta_ = initPose.theta();
}