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();
}
void
Stats::addData( int seconds, int useconds,
const hydronavutil::Pose& pose,
const hydronavutil::Velocity& velocity,
bool isEnabled )
{
// not keeping track of motion when disabled.
// force to re-aquire starting point next when next enabled.
if ( !isEnabled ) {
haveData_ = false;
return;
}
if ( !haveData_ ) {
lastSeconds_ = seconds;
lastUseconds_ = useconds;
lastPose_ = pose;
lastIsEnabled_ = isEnabled;
haveData_ = true;
return;
}
double deltaX = pose.x() - lastPose_.x();
double deltaY = pose.y() - lastPose_.y();
double localiserDistance = sqrtf( deltaX*deltaX + deltaY*deltaY );
const double timeInterval = (seconds-lastSeconds_) + (useconds-lastUseconds_)/1e6;
const double speed = fabs(velocity.lin());
const double odometryDistance = speed*timeInterval;
const double MAX_LOCALISER_JUMP = 10.0;
if ( localiserDistance < MAX_LOCALISER_JUMP*odometryDistance )
distance_ += localiserDistance;
//else
// std::cout<<"LocalNavStats: suspect localization jump, not integrating this motion element."<<std::endl;
// ignore this data
const double MIN_TIMED_SPEED = 0.10;
if ( fabs(velocity.lin()) > MIN_TIMED_SPEED )
timeInMotion_ += timeInterval;
maxSpeed_ = MAX( maxSpeed_, speed );
lastSeconds_ = seconds;
lastUseconds_ = useconds;
lastPose_ = pose;
lastIsEnabled_ = isEnabled;
}
