예제 #1
0
    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();
}
예제 #3
0
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;
}