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; }