/** \file * * Contains the LineCapture class implementation. * * Copyright (c) 2007,2008,2009 MBARI * MBARI Proprietary Information. All Rights Reserved */ #include "LineCapture.h" #include "LineCaptureIF.h" #include "DockIF.h" #include "controlModule/HorizontalControlIF.h" #include "controlModule/SpeedControlIF.h" #include "estimationModule/TrackAcousticContactIF.h" #include "sensorModule/PowerOnlyIF.h" #include "sensorModule/NanoDVRIF.h" #include "servoModule/DockingServoIF.h" #include "servoModule/DockingStepperIF.h" #include "sensorModule/DDMIF.h" #include "data/Location.h" #include "data/Slate.h" #include "data/ConfigReader.h" #include "data/UniversalURI.h" #include "data/UniversalDataReader.h" #include "data/Point2D.h" #include "units/Units.h" #include /*** IMPORTANT *** The LineCapture behavior should always run in conjunction with an acoustic timeout! ***/ // Number of target positions to store in the repo const size_t LineCapture::TARGET_POS_BIN_SIZE( 4 ); // Min approach rate, in m/s, to determine if the vehicle is closing on the target const float LineCapture::MIN_APPROACH_RATE( 0.2 ); // m/s; value was determined empirically following field tests // Max approach rate, in m/s, to determine if the incoming range reading is an outlier const float LineCapture::MAX_APPROACH_RATE( SpeedControlIF::SPEED_FAST * 5.0f ); // m/s; value was determined empirically following field tests LineCapture::LineCapture( const Str& prefix, const Module* module ) : Behavior( prefix + LineCaptureIF::NAME, module, true, true ), // See initializeVariables() for member variable default values guidanceMode_( UNINITIALIZED ), dockingModuleLoaded_( false ), cameraLoaded_( false ), verbose_( true ), debug_( true ) { logger_.syslog( "Construct." ); // Slate input config settings armRangeCfgReader_ = newConfigReader( LineCaptureIF::ARM_RANGE_CFG ); armSpeedCfgReader_ = newConfigReader( LineCaptureIF::ARM_SPEED_CFG ); lockoutRangeCfgReader_ = newConfigReader( LineCaptureIF::LOCKOUT_RANGE_CFG ); shortFinalRangeCfgReader_ = newConfigReader( LineCaptureIF::SHORTFINAL_RANGE_CFG ); rolloutDistanceCfgReader_ = newConfigReader( LineCaptureIF::ROLLOUT_DISTANCE_CFG ); rolloutSpeedCfgReader_ = newConfigReader( LineCaptureIF::ROLLOUT_SPEED_CFG ); rolloutTimeoutCfgReader_ = newConfigReader( LineCaptureIF::ROLLOUT_TIMEOUT_CFG ); interceptTimeoutCfgReader_ = newConfigReader( LineCaptureIF::INTERCEPT_TIMEOUT_CFG ); latchDelayTimeoutCfgReader_ = newConfigReader( LineCaptureIF::LATCH_DELAY_TIMEOUT_CFG ); kpHeadingTerminalCfgReader_ = newConfigReader( LineCaptureIF::KP_HEADING_TERMINAL_GUIDANCE ); kiHeadingTerminalCfgReader_ = newConfigReader( LineCaptureIF::KI_HEADING_TERMINAL_GUIDANCE ); kpHeadingFinalCfgReader_ = newConfigReader( LineCaptureIF::KP_HEADING_FINAL_APPROACH ); kiHeadingFinalCfgReader_ = newConfigReader( LineCaptureIF::KI_HEADING_FINAL_APPROACH ); navigationGainCfgReader_ = newConfigReader( LineCaptureIF::PRONAV_GAIN ); maxHdgRateCfgReader_ = newConfigReader( HorizontalControlIF::MAX_HDG_RATE_CFG ); verboseCfgReader_ = newConfigReader( LineCaptureIF::VERBOSE ); // Mission setting inputs iirFilterDecaySettingReader_ = newSettingReader( LineCaptureIF::IIR_FILTER_DECAY_SETTING ); // Slate input measurements vehicleLatReader_ = newUniversalReader( UniversalURI::LATITUDE ); vehicleLonReader_ = newUniversalReader( UniversalURI::LONGITUDE ); orientationReader_ = newUniversalReader( UniversalURI::PLATFORM_ORIENTATION ); universalRangeReader_ = newUniversalReader( UniversalURI::ACOUSTIC_CONTACT_RANGE_READING ); xVelocityWrtSeafloorReader_ = newUniversalReader( UniversalURI::PLATFORM_X_VELOCITY_WRT_GROUND ); rangeToContactReader_ = newDataReader( TrackAcousticContactIF::RANGE_TO_CONTACT ); headingToContactReader_ = newDataReader( TrackAcousticContactIF::CONTACT_HEADING_LOWPASS ); contactLatReader_ = newDataReader( TrackAcousticContactIF::CONTACT_LATITUDE_LOWPASS ); contactLonReader_ = newDataReader( TrackAcousticContactIF::CONTACT_LONGITUDE_LOWPASS ); samplePowerOnlyReader_ = newDataReader( PowerOnlyIF::SAMPLE_POWERONLY ); sampleNanoDvrReader_ = newDataReader( NanoDVRIF::SAMPLE_NANO_DVR ); dockingStateReader_ = newDataReader( DockIF::DOCKING_STATE ); cablePresentReader_ = newDataReader( DockIF::DOCK_CABLE_PRESENT ); // Slate output variables speedCmdWriter_ = newDataWriter( SpeedControlIF::SPEED_CMD ); horizontalModeWriter_ = newDataWriter( HorizontalControlIF::HORIZONTAL_MODE ); headingCmdWriter_ = newDataWriter( HorizontalControlIF::HEADING_CMD ); kpHeadingGainWriter_ = newDataWriter( HorizontalControlIF::KP_HEADING_OVERRIDE ); kiHeadingGainWriter_ = newDataWriter( HorizontalControlIF::KI_HEADING_OVERRIDE ); dockingStateCmdWriter_ = newDataWriter( DockIF::DOCKING_STATE_CMD ); guidanceModeWriter_ = newDataWriter( LineCaptureIF::LINE_CAPTURE_GUIDANCE_MODE ); proNavCmdWriter_ = newDataWriter( LineCaptureIF::INTERNAL_PN_CMD ); rawBearingRateWriter_ = newDataWriter( LineCaptureIF::INTERNAL_RAW_BEARING_RATE ); bearingRateWriter_ = newDataWriter( LineCaptureIF::INTERNAL_BEARING_RATE ); rangeClosingWriter_ = newDataWriter( LineCaptureIF::RANGE_CLOSING ); // Check DDM/DockingServo status bool ddmLoaded( false ), dockingServoLoaded( false ), dockingStepperLoaded( false ); Slate::ReadOnce( DDMIF::LOAD_AT_STARTUP, Units::BOOL, ddmLoaded, logger_ ); Slate::ReadOnce( DockingServoIF::LOAD_AT_STARTUP, Units::BOOL, dockingServoLoaded, logger_ ); Slate::ReadOnce( DockingStepperIF::LOAD_AT_STARTUP, Units::BOOL, dockingStepperLoaded, logger_ ); dockingModuleLoaded_ = ( ddmLoaded || dockingServoLoaded || dockingStepperLoaded ); // Check PowerOnly/camera status Slate::ReadOnce( PowerOnlyIF::LOAD_AT_STARTUP, Units::BOOL, powerOnlyLoaded_, logger_ ); Slate::ReadOnce( NanoDVRIF::LOAD_AT_STARTUP, Units::BOOL, cameraLoaded_, logger_ ); posRepo_.reserve( TARGET_POS_BIN_SIZE ); rangeRepo_.reserve( TARGET_POS_BIN_SIZE ); initializeVariables(); } LineCapture::~LineCapture() {} /// Init internal member variables to default values void LineCapture::initializeVariables() { logger_.syslog( "Initializing internal variables to default values." ); // This may seem unnecessary; however, if the behavior is run as part of the // default mission, it will remain on the stack indefinitely, meaning the // constructor will get called only once at load time. So, we package this init // routine and invoke it upon construction and at runtime from initialize(). setGuidanceMode( UNINITIALIZED ); armRangeCfg_ = 40.0; // Objective range in meters for arming docking module (e.g., deploy docking whiskers) lockoutRangeCfg_ = 2.0; // Range to target in meters after which heading command updates cease shortFinalRangeCfg_ = 18; // Range at which further ranges are ignored in final approach. Used because azimuth becomes very noisy as elevation angle increases to 90. Vehicle will continue navigating to the projected waypoint. interceptTimeoutCfg_ = 60; // Time duration to wait for cable/dock sensor after range-rates settle before satisfying latchDelayTimeoutCfg_ = 0; // Time duration to wait after cable present first turns true until closing the latch kpHeadingTerminalCfg_ = 0.8; // HorizontalControl heading proportional gain during TERMINAL_GUIDANCE kiHeadingTerminalCfg_ = 0.002; // HorizontalControl heading integral gain during TERMINAL_GUIDANCE kpHeadingFinalCfg_ = 1.2; // HorizontalControl heading proportional gain during FINAL_APPROACH kiHeadingFinalCfg_ = 0.003; // HorizontalControl heading integral gain during FINAL_APPROACH navigationGainCfg_ = 3.0; // Proportional Navigation gain (should typically be 3 - 5) maxHdgRateCfg_ = 0.2; // LRAUV max turn rate (~12.0 deg/s) rolloutDistanceCfg_ = 100.0; // Distance in meters to rollout in response to a miss rolloutTimeoutCfg_ = 300; // Max time duration for rollout in seconds rolloutTime_ = Timestamp::NOT_SET_TIME; interceptTime_ = Timestamp::NOT_SET_TIME; latchDelayTime_ = Timestamp::NOT_SET_TIME; lockoutTime_ = Timestamp::NOT_SET_TIME; terminalGuidanceSpeed_ = SpeedControlIF::SPEED_FAST; finalApproachSpeed_ = SpeedControlIF::SPEED_FAST; rolloutSpeed_ = SpeedControlIF::SPEED_FAST; speedCmd_ = SpeedControlIF::SPEED_FAST; headingRateCmd_ = 0.0; headingCmd_ = nanf( "" ); cablePresent_ = false; dockingStateCmd_ = DockIF::STANDBY; dockingState_ = DockIF::STANDBY; rejectCount_ = 0; rejectRangeCount_ = 0; verbose_ = false; iirFilter_.setDecay( 0.0 ); iirFilter_.init( 0.0 ); posRepo_.clear(); rangeRepo_.clear(); } /// Initialize function void LineCapture::initialize() { logger_.syslog( "Initialize." ); initializeVariables(); dockingStateReader_->requestData( true ); cablePresentReader_->requestData( true ); orientationReader_->requestData( true ); // Read Slate input setting variables, default to constructor values if unspecified float lockoutRange( nanf( "" ) ); if( lockoutRangeCfgReader_->read( Units::METER, lockoutRange ) && !isnan( lockoutRange ) ) { lockoutRangeCfg_ = fabs( lockoutRange ); } float shortFinalRange( nanf( "" ) ); if( shortFinalRangeCfgReader_->read( Units::METER, shortFinalRange ) && !isnan( shortFinalRange ) ) { shortFinalRangeCfg_ = fabs( shortFinalRange ); } float armRange( nanf( "" ) ); if( armRangeCfgReader_->read( Units::METER, armRange ) && !isnan( armRange ) ) { armRangeCfg_ = fabs( armRange ); } float armSpeed( nanf( "" ) ); if( armSpeedCfgReader_->read( Units::METER, armSpeed ) && !isnan( armSpeed ) ) { finalApproachSpeed_ = fabs( armSpeed ); } float rolloutTimeout( nanf( "" ) ); if( rolloutTimeoutCfgReader_->read( Units::SECOND, rolloutTimeout ) && !isnan( rolloutTimeout ) ) { rolloutTimeoutCfg_ = Timespan( rolloutTimeout ); logger_.syslog( "Rollout timeout set to " + Str( rolloutTimeoutCfg_.asFloat(), 2 ) + " sec.", Syslog::INFO ); } float rolloutSpeed( nanf( "" ) ); if( rolloutSpeedCfgReader_->read( Units::METER, rolloutSpeed ) && !isnan( rolloutSpeed ) ) { rolloutSpeed_ = fabs( rolloutSpeed ); } float rolloutDistance( nanf( "" ) ); if( rolloutDistanceCfgReader_->read( Units::METER, rolloutDistance ) && !isnan( rolloutDistance ) ) { rolloutDistanceCfg_ = rolloutDistance; // Auto-adjsut rolloutTimeout if not specified to accommodate the desired rolloutDistance if( isnan( rolloutTimeout ) && rolloutTimeoutCfg_.asFloat() < ( rolloutDistance * rolloutSpeed_ ) ) { rolloutTimeoutCfg_ = Timespan( fabs( rolloutDistance * rolloutSpeed_ * 1.25 ) ); logger_.syslog( "Rollout timeout adjusted to " + Str( rolloutTimeoutCfg_.asFloat(), 2 ) + " sec to accomodate distance.", Syslog::INFO ); } } float interceptTimeout( nanf( "" ) ); if( interceptTimeoutCfgReader_->read( Units::SECOND, interceptTimeout ) && !isnan( interceptTimeout ) ) { interceptTimeoutCfg_ = Timespan( interceptTimeout ); } float latchDelayTimeout( nanf( "" ) ); if( latchDelayTimeoutCfgReader_->read( Units::SECOND, latchDelayTimeout ) && !isnan( latchDelayTimeout ) ) { latchDelayTimeoutCfg_ = Timespan( latchDelayTimeout ); } float kpHeadingTerminal( nanf( "" ) ); if( kpHeadingTerminalCfgReader_->read( Units::RATIO, kpHeadingTerminal ) && !isnan( kpHeadingTerminal ) ) { kpHeadingTerminalCfg_ = kpHeadingTerminal; } float kiHeadingTerminal( nanf( "" ) ); if( kiHeadingTerminalCfgReader_->read( Units::RECIPROCAL_SECOND, kiHeadingTerminal ) && !isnan( kiHeadingTerminal ) ) { kiHeadingTerminalCfg_ = kiHeadingTerminal; } float kpHeadingFinal( nanf( "" ) ); if( kpHeadingFinalCfgReader_->read( Units::RATIO, kpHeadingFinal ) && !isnan( kpHeadingFinal ) ) { kpHeadingFinalCfg_ = kpHeadingFinal; } float kiHeadingFinal( nanf( "" ) ); if( kiHeadingFinalCfgReader_->read( Units::RECIPROCAL_SECOND, kiHeadingFinal ) && !isnan( kiHeadingFinal ) ) { kiHeadingFinalCfg_ = kiHeadingFinal; } float proNavGain( nanf( "" ) ); if( navigationGainCfgReader_->read( Units::NONE, proNavGain ) ) { if( isnan( proNavGain ) ) { logger_.syslog( "Got invalid Pro-Nav gain. Will use Pure Pursuit guidance.", Syslog::FAULT ); } navigationGainCfg_ = proNavGain; } float maxHdgRate( nanf( "" ) ); if( maxHdgRateCfgReader_->read( Units::RADIAN_PER_SECOND, maxHdgRate ) && !isnan( maxHdgRate ) ) { maxHdgRateCfg_ = maxHdgRate; } verboseCfgReader_->read( verbose_ ); float iirFilterDecaySetting( nanf( "" ) ); if( iirFilterDecaySettingReader_->read( Units::NONE, iirFilterDecaySetting ) && !isnan( iirFilterDecaySetting ) ) { // The IIR filter is initialized with decay of 0.0, so filter is disabled unless a // decay setting is provided. logger_.syslog( "IIR filter is initialized with decay: " + Str( iirFilterDecaySetting, 2 ) + ".", Syslog::IMPORTANT ); iirFilter_.setDecay( iirFilterDecaySetting ); } // Read the docking mode if( dockingModuleLoaded_ ) { int dockingState( DockIF::STANDBY ); if( dockingStateReader_->read( Units::ENUM, dockingState ) ) { dockingState_ = ( DockIF::DockingState )dockingState; } } // Initialize heading command with the vehicle's current heading orientationReader_->read( Units::RADIAN, headingCmd_ ); setGuidanceMode( TERMINAL_GUIDANCE ); } /// Read in the parameters for satisfied or runIfUnsatisfied: return true if OK. bool LineCapture::readParams() { bool ok( true ); switch( guidanceMode_ ) { case UNINITIALIZED: initialize(); // no break case TERMINAL_GUIDANCE: case ROLLOUT: // Read in target pos updates ok &= readTargetPos(); break; case FINAL_APPROACH: case SHORT_FINAL: case INTERCEPT_LOCKOUT: // Read in target pos, but ignore the return value so the behavior can keep running // in final approach stages even if no pos updates were received. This will improve // trigger response time if the vehicle hits the dock. readTargetPos(); break; default: setGuidanceMode( UNINITIALIZED ); ok = false; break; } // Read state of docking module IR emitter if( dockingModuleLoaded_ ) { cablePresentReader_->read( cablePresent_ ); } return ok; } /// Read new target range and position data from the slate bool LineCapture::readTargetPos() { bool ok( true ); // // The rangeToContactReader_ was replaced with the universal reader further // below. rangeToContactReader_ comes from TrackAcousticContact, which // does not write range unless bearing is valid too. The objective right here is // to still use the range even if the bearing is invalid. // ok &= universalRangeReader_->wasTouchedSinceLastRun( this ); if( ok ) { TargetPos contact; ok &= universalRangeReader_->read( Units::METER, contact.range_ ) && !isnan( contact.range_ ); contact.time_ = universalRangeReader_->getTimestamp(); if( ok ) { // Add new target range data to a separate repo. This is only for tracking range. //Ignore bearing and location. ok &= addTargetRange( contact ); } } ok &= contactLatReader_->wasTouchedSinceLastRun( this ); ok &= rangeToContactReader_->wasTouchedSinceLastRun( this ); if( ok ) { TargetPos contact; ok &= headingToContactReader_->read( Units::RADIAN, contact.acBearing_ ) && !isnan( contact.acBearing_ ); ok &= rangeToContactReader_->read( Units::METER, contact.range_ ) && !isnan( contact.range_ ); ok &= contactLatReader_->read( Units::RADIAN, contact.lat_ ) && !isnan( contact.lat_ ); ok &= contactLonReader_->read( Units::RADIAN, contact.lon_ ) && !isnan( contact.lon_ ); contact.time_ = rangeToContactReader_->getTimestamp(); if( ok ) { // Add new target pos data to the repo ok &= addTargetPos( contact ); } } return ok; } /// Add new target position data to the repo bool LineCapture::addTargetPos( TargetPos &newPos ) { if( !posRepo_.empty() ) { // Get the last known target pos... const TargetPos& lastPos = posRepo_.back(); // ... and check for duplicates if( newPos == lastPos ) return false; // Compute the time delta newPos.deltaT_ = ( newPos.time_ - lastPos.time_ ).asDouble(); if( newPos.deltaT_ == 0.0f ) return false; // Compute the approach rate (in m/s), and LOS rotation rate (rad/s) newPos.deltaX_ = newPos.range_ - lastPos.range_; newPos.approachRate_ = newPos.deltaX_ / newPos.deltaT_; // QC the approach rate and reject outlier readings if( fabs( newPos.approachRate_ ) > MAX_APPROACH_RATE ) { Str errMsg = "addTargetPos::Rejecting new target pos. Approach rate out of bounds.\n"; if( debug_ ) { errMsg += " range: " + Str( newPos.range_ ) + " m,\n"; errMsg += " bearing: " + Str( R2D( newPos.bearing_ ) ) + " deg,\n"; errMsg += " deltaT: " + Str( newPos.deltaT_ ) + " s,\n"; errMsg += " deltaX: " + Str( newPos.deltaX_ ) + " m,\n"; errMsg += " approachRate: " + Str( newPos.approachRate_ ) + " m/s,\n"; errMsg += " posRepo size: " + Str( ( int )posRepo_.size() ); } logger_.syslog( errMsg, verbose_ ? Syslog::FAULT : Syslog::ERROR ); // Clear the observation repo if the rejection count limit is reached // (this could happen if the first obs was an outlier) if( ++rejectCount_ >= 5 ) { logger_.syslog( "addTargetPos::Reached target pos rejection count limit. Clearing pos repo.", Syslog::FAULT ); posRepo_.clear(); rejectCount_ = 0; } return false; } // Initialize with previous LOS rate and reset the reletive position vector newPos.losRotationRate_ = lastPos.losRotationRate_; relPosLast_ = Point2D( nanf( "" ) ); } else { // No previous target range data in the repo (first data point). // We'll assume that we're moving toward the target: newPos.approachRate_ = -1.0; calcTargetLOS( newPos ); } // Add the new target pos to the repo posRepo_.push_back( newPos ); rejectCount_ = 0; // Trim the repo to match the allowed size while( posRepo_.size() > TARGET_POS_BIN_SIZE ) { posRepo_.erase( posRepo_.begin() ); } if( debug_ ) logger_.syslog( "\n Added new target pos.\n" " range: " + Str( newPos.range_ ) + " m,\n" " bearing: " + Str( R2D( newPos.acBearing_ ) ) + " deg,\n" " lat: " + Str( R2D( newPos.lat_ ) ) + " deg,\n" " lon: " + Str( R2D( newPos.lon_ ) ) + " deg,\n" " deltaT: " + Str( newPos.deltaT_ ) + " s,\n" " deltaX: " + Str( newPos.deltaX_ ) + " m,\n" " approachRate: " + Str( newPos.approachRate_ ) + " m/s,\n" " posRepo size: " + Str( ( int )posRepo_.size() ) + "\n", Syslog::INFO ); return !posRepo_.empty(); } /// Add new target range data to the range repository. This tracks only range and ignores bearing and location. bool LineCapture::addTargetRange( TargetPos &newPos ) { if( !rangeRepo_.empty() ) { // Get the last known target pos... const TargetPos& lastPos = rangeRepo_.back(); // ... and check for duplicates if( newPos == lastPos ) return false; // Compute the time delta newPos.deltaT_ = ( newPos.time_ - lastPos.time_ ).asDouble(); if( newPos.deltaT_ == 0.0f ) return false; // Compute the approach rate (in m/s), and LOS rotation rate (rad/s) newPos.deltaX_ = newPos.range_ - lastPos.range_; newPos.approachRate_ = newPos.deltaX_ / newPos.deltaT_; // QC the approach rate and reject outlier readings if( fabs( newPos.approachRate_ ) > MAX_APPROACH_RATE ) { Str errMsg = "addTargetRange::Rejecting new target range. Approach rate out of bounds.\n"; if( debug_ ) { errMsg += " range: " + Str( newPos.range_ ) + " m,\n"; errMsg += " deltaT: " + Str( newPos.deltaT_ ) + " s,\n"; errMsg += " deltaX: " + Str( newPos.deltaX_ ) + " m,\n"; errMsg += " approachRate: " + Str( newPos.approachRate_ ) + " m/s,\n"; errMsg += " rangeRepo size: " + Str( ( int )rangeRepo_.size() ); } logger_.syslog( errMsg, verbose_ ? Syslog::FAULT : Syslog::ERROR ); // Clear the observation repo if the rejection count limit is reached // (this could happen if the first obs was an outlier) if( ++rejectRangeCount_ >= 5 ) { logger_.syslog( "addTargetRange::Reached target pos rejection count limit. Clearing pos repo.", Syslog::FAULT ); rangeRepo_.clear(); rejectRangeCount_ = 0; } return false; } } else { // No previous target range data in the repo (first data point). // We'll assume that we're moving toward the target: newPos.approachRate_ = -1.0; } // Add the new target pos to the repo rangeRepo_.push_back( newPos ); rejectRangeCount_ = 0; // Trim the repo to match the allowed size while( rangeRepo_.size() > TARGET_POS_BIN_SIZE ) { rangeRepo_.erase( rangeRepo_.begin() ); } if( debug_ ) logger_.syslog( "\n addTargetRange:: Added new target pos.\n" " range: " + Str( newPos.range_ ) + " m,\n" " deltaT: " + Str( newPos.deltaT_ ) + " s,\n" " deltaX: " + Str( newPos.deltaX_ ) + " m,\n" " approachRate: " + Str( newPos.approachRate_ ) + " m/s,\n" " rangeRepo size: " + Str( ( int )rangeRepo_.size() ) + "\n", Syslog::INFO ); return !rangeRepo_.empty(); } /// Initializes/monitors the intercept timer bool LineCapture::interceptTimeout() { if( interceptTime_ == Timestamp::NOT_SET_TIME ) { logger_.syslog( "Starting intercept timer at range: " + Str( getRange(), 2 ) + " m.", Syslog::INFO ); interceptTime_ = Timestamp::Now(); } return ( interceptTime_.elapsed() > interceptTimeoutCfg_ ); } // Initializes/monitors the latch delay timer bool LineCapture::latchDelayTimeout() { if( latchDelayTime_ == Timestamp::NOT_SET_TIME ) { logger_.syslog( "Starting latch delay timer at range: " + Str( getRange(), 2 ) + " m.", Syslog::INFO ); if( cablePresent_ ) { logger_.syslog( "Cable is present.", Syslog::INFO ); } // Slow down the speed to allow proper latching speedCmd_ = SpeedControlIF::SPEED_SLOW; latchDelayTime_ = Timestamp::Now(); } return ( latchDelayTime_.elapsed() > latchDelayTimeoutCfg_ ); } /// Perform the satisfied: return true if envelope "satisfied" bool LineCapture::calcSatisfied() { bool satisfied( false ); if( guidanceMode_ != ROLLOUT && ( rangeStable() || cablePresent_ ) ) { // Absolute approach rates have stabilized below the threshold indicating the vehicle // may be caught on the line. Wait for cable present sensor or timeout. bool interceptTimedOut = interceptTimeout(); // Counting on short circuit here in the && so as not to start the latch timeout clock early. Beware of changing order. if( ( cablePresent_ && latchDelayTimeout() ) || interceptTimedOut ) { if( reqDockingState( DockIF::STANDBY ) ) { Str satisfyMsg; satisfyMsg = "Docking sequence complete"; satisfyMsg += isnan( getRange() ) ? "." : ( " at range " + Str( getRange(), 2 ) + " m." ); satisfyMsg += cablePresent_ ? " Cable present." : ""; satisfyMsg += interceptTimedOut ? " Reached intercept timeout." : ""; logger_.syslog( satisfyMsg, Syslog::IMPORTANT ); latchDelayTime_ = Timestamp::NOT_SET_TIME; // Reset the latch delay. We either docked or timed out but we're back in standby. satisfied = true; } } } else if( interceptTime_ != Timestamp::NOT_SET_TIME ) { // Approach rates aren't stable. Reset the clock. logger_.syslog( "Stopped intercept timer at range: " + Str( getRange(), 2 ) + " m.", Syslog::INFO ); latchDelayTime_ = Timestamp::NOT_SET_TIME; interceptTime_ = Timestamp::NOT_SET_TIME; setSpeed( guidanceMode_ ); } return satisfied; } /// Just do the run void LineCapture::run() { runIfUnsatisfied(); } /// Just do the satisfied: return true if envelope "satisfied" bool LineCapture::isSatisfied() { bool satisfied( false ); if( readParams() ) { satisfied = calcSatisfied(); } return satisfied; } /// Do the run, and return true if envelope "satisfied" bool LineCapture::runIfUnsatisfied() { bool satisfied( false ); static bool first = true; readParams(); if( !posRepo_.empty() ) { satisfied = calcSatisfied(); if( !satisfied ) { // Get latest target pos TargetPos& target = posRepo_.back(); //TargetPos& target = rangeRepo_.back(); // Update the previous target with the new one unless we're on final/short. We'll let lockoutRange get dealt with down below. if( guidanceMode_ != SHORT_FINAL && guidanceMode_ != INTERCEPT_LOCKOUT ) { if( debug_ ) logger_.syslog( "NOT Ignoring new targets: " + Str( target.range_, 2 ) + " m.", Syslog::INFO ); previousTarget_ = target; // store the latest value first = true; } switch( guidanceMode_ ) { case TERMINAL_GUIDANCE: // Guide the vehicle until FINAL_APPROACH, or pass to ROLLOUT if( target.range_ > armRangeCfg_ || rangeClosing() ) { // Arm the vehicle for intercept if( target.range_ <= armRangeCfg_ ) { // Slow down setSpeed( FINAL_APPROACH ); // Power the camera, and arm the docking module if( debug_ ) logger_.syslog( "Powering the camera and arming the capture device at range: " + Str( target.range_, 2 ) + " m.", Syslog::INFO ); bool cameraOn = powerCameraAndLights(); bool dockingModuleArmed = reqDockingState( DockIF::ARM ); if( dockingModuleArmed && cameraOn ) { logger_.syslog( "Final approach. Armed for intercept at range: " + Str( target.range_, 2 ) + " m.", verbose_ ? Syslog::IMPORTANT : Syslog::INFO ); setGuidanceMode( FINAL_APPROACH ); } } // Update the heading command headingCmd_ = proNav( target ); } else { // Ranges are no longer closing, meaning we missed the target... // Execute the recovery maneuver and re-approach! logger_.syslog( "Target missed at range: " + Str( target.range_, 2 ) + " m. Rolling out.", verbose_ ? Syslog::FAULT : Syslog::ERROR ); setGuidanceMode( ROLLOUT ); } break; case FINAL_APPROACH: // Guide the vehicle until SHORT_FINAL, INTERCEPT_LOCKOUT, or pass to ROLLOUT if( rangeClosing() ) { // First check to see if we're within lockout range since short final is technically optional. if( target.range_ <= lockoutRangeCfg_ ) { logger_.syslog( "Intercept lockout. Range: " + Str( target.range_, 2 ) + " m.", verbose_ ? Syslog::IMPORTANT : Syslog::INFO ); setGuidanceMode( INTERCEPT_LOCKOUT ); } // Check to see if we're within short final range. if( target.range_ <= shortFinalRangeCfg_ ) { logger_.syslog( "Short final. Range: " + Str( target.range_, 2 ) + " m.", verbose_ ? Syslog::IMPORTANT : Syslog::INFO ); setGuidanceMode( SHORT_FINAL ); } // Update the heading command headingCmd_ = proNav( target ); if( debug_ ) logger_.syslog( "HeadingCmd: " + Str( headingCmd_ ) + " and range: " + Str( target.range_, 2 ) + " m.", verbose_ ? Syslog::IMPORTANT : Syslog::INFO ); } else { // Ranges are no longer closing, meaning we missed the target... // Execute the recovery maneuver and re-approach! logger_.syslog( "Target missed at range: " + Str( target.range_, 2 ) + " m. Rolling out.", verbose_ ? Syslog::FAULT : Syslog::ERROR ); setGuidanceMode( ROLLOUT ); } break; case SHORT_FINAL: // Guide the vehicle until INTERCEPT_LOCKOUT, or pass to ROLLOUT. Note that if short final is set very low, this case may be skipped and intercept lockout may occur on final approach if( rangeClosing() ) { if( target.range_ <= lockoutRangeCfg_ ) { logger_.syslog( "Intercept lockout. Range: " + Str( target.range_, 2 ) + " m.", verbose_ ? Syslog::IMPORTANT : Syslog::INFO ); setGuidanceMode( INTERCEPT_LOCKOUT ); break; } if( target.range_ > shortFinalRangeCfg_ ) { logger_.syslog( "No longer in short final. Rolling out. Range: " + Str( target.range_, 2 ) + " m.", verbose_ ? Syslog::IMPORTANT : Syslog::INFO ); previousTarget_ = target; setGuidanceMode( ROLLOUT ); break; } // Don't update the target in Final appraoch if we're within the distance where the azimuth gets noisy. if( debug_ ) logger_.syslog( "Ignoring new targets. Set target to previous: " + Str( target.range_, 2 ) + " m. " + Str( previousTarget_.range_, 2 ) + " m.", Syslog::INFO ); target = previousTarget_; // Update the heading command headingCmd_ = proNav( target ); if( debug_ ) logger_.syslog( "HeadingCmd: " + Str( headingCmd_ ) + " and range: " + Str( target.range_, 2 ) + " m.", verbose_ ? Syslog::IMPORTANT : Syslog::INFO ); } else { // Ranges are no longer closing, meaning we missed the target... // Execute the recovery maneuver and re-approach! logger_.syslog( "Target missed at range: " + Str( target.range_, 2 ) + " m. Rolling out.", verbose_ ? Syslog::FAULT : Syslog::ERROR ); setGuidanceMode( ROLLOUT ); } break; case INTERCEPT_LOCKOUT: // Run until INTERCEPT_LOCKOUT is satisfied (docking module is latched/ranges stable), or pass to ROLLOUT if( rangeClosing() && lockoutTime_.elapsed() < rolloutTimeoutCfg_ ) { // No heading updates. // calcSatisfied reads the state of the docking module and return satisfied once the vehicle is latched. } else { // Ranges are no longer closing (or constant) or lockout has exceeded the rollout time limit, meaning we missed the target... // Execute the recovery maneuver and re-approach! logger_.syslog( "Target missed at range: " + Str( target.range_, 2 ) + " m. Rolling out.", verbose_ ? Syslog::FAULT : Syslog::ERROR ); setGuidanceMode( ROLLOUT ); } break; case ROLLOUT: // Execute the recovery maneuver, resume TERMINAL_GUIDANCE after rollout // Continue on last commanded heading util rollout distance is reached/timer expires if( target.range_ >= rolloutDistanceCfg_ || rolloutTime_.elapsed() >= rolloutTimeoutCfg_ ) { logger_.syslog( "Completed rollout at range: " + Str( target.range_, 2 ) + " m. Resuming terminal guidance.", verbose_ ? Syslog::IMPORTANT : Syslog::INFO ); // Point the vehicle at reciprocal heading of rollout course (i.e, back at the dock) headingCmd_ = AuvMath::NormalizeAngle( headingCmd_ - M_PI ); // Clear pos history posRepo_.clear(); rangeRepo_.clear(); setGuidanceMode( TERMINAL_GUIDANCE ); } break; case UNINITIALIZED: default: break; } } } // Update the vehicle's heading if( !satisfied ) { // Publish commanded variables to the slate headingCmdWriter_->write( Units::RADIAN, headingCmd_ ); horizontalModeWriter_->write( Units::ENUM, HorizontalControlIF::HEADING ); kiHeadingGainWriter_->write( Units::RECIPROCAL_SECOND, kiHeadingOverride_ ); kpHeadingGainWriter_->write( Units::RATIO, kpHeadingOverride_ ); speedCmdWriter_->write( Units::METER_PER_SECOND, speedCmd_ ); // Keep commanding the docking module if needed if( dockingState_ != dockingStateCmd_ ) { cmdDockingState(); } } return satisfied; } float LineCapture::proNav( TargetPos& target ) { double headingCmd( 0.0 ); // Compute contact LOS and LOS-rate calcTargetLOS( target ); if( isnan( navigationGainCfg_ ) || isnan( target.deltaT_ ) ) // TODO: || heading and bearing are > X { // Default to Pure Pursuit guidance when the navigation gain is set to NaN headingCmd = target.acBearing_; logger_.syslog( "\n ProNav pure pursuit: \n" " ac range: " + Str( target.range_ ) + " m,\n" " nav range: " + Str( target.hrange_ ) + " m,\n" " bearing: " + Str( R2D( target.bearing_ ) ) + " deg,\n" " approach rate: " + Str( target.hApproachRate_ ) + " m/s,\n" " LOS rate: " + Str( R2D( target.losRotationRate_ ) ) + " deg/s,\n" " cmd heading: " + Str( R2D( headingCmd_ ) ) + " deg,\n" " new cmd heading: " + Str( R2D( headingCmd ) ) + " deg.\n" , Syslog::INFO ); return headingCmd; } else if( isnan( target.losRotationRate_ ) || isnan( headingCmd_ ) || fabs( target.losRotationRate_ ) > 0.2 ) { logger_.syslog( "ProNav REINIT: LOS rate is " + Str( R2D( target.losRotationRate_ ) ) + " deg/s." " Setting heading cmd to target acoustic bearing of " + Str( R2D( target.acBearing_ ) ) + " deg." , Syslog::ERROR ); headingCmd = target.acBearing_; iirFilter_.init( 0.0 ); return headingCmd; } // Compute the pure Pro-Nav accelation command double accelCmd = navigationGainCfg_ * speedCmd_ * target.losRotationRate_; // Propogate the pro-nav accelation command to vehicle heading Point2D velCmd; double sh = sin( headingCmd_ ); double ch = cos( headingCmd_ ); velCmd.setU( speedCmd_ * ch - accelCmd * sh * dt_.asDouble() ); velCmd.setV( speedCmd_ * sh + accelCmd * ch * dt_.asDouble() ); // Make sure we have a unit vector velCmd *= 1.0 / velCmd.getMagnitude(); headingCmd = atan2( velCmd.getV(), velCmd.getU() ); if( debug_ ) logger_.syslog( "\n ProNav: \n" " ac range: " + Str( target.range_ ) + " m,\n" " nav range: " + Str( target.hrange_ ) + " m,\n" " bearing: " + Str( R2D( target.bearing_ ) ) + " deg,\n" " approach rate: " + Str( target.hApproachRate_ ) + " m/s,\n" " LOS rate: " + Str( R2D( target.losRotationRate_ ) ) + " deg/s,\n" " cmd heading: " + Str( R2D( headingCmd_ ) ) + " deg,\n" " new cmd heading: " + Str( R2D( AuvMath::NormalizeAngle( headingCmd ) ) ) + " deg.\n" , Syslog::INFO ); proNavCmdWriter_->write( Units::NONE, accelCmd ); return AuvMath::NormalizeAngle( headingCmd ); } void LineCapture::calcTargetLOS( TargetPos &target ) { // Get vehicle Lat/Lon double vlat( nanf( "" ) ), vlon( nanf( "" ) ), vheading( nanf( "" ) ), losRotationRate( nanf( "" ) ); if( vehicleLatReader_->read( Units::RADIAN, vlat ) && !isnan( vlat ) && vehicleLonReader_->read( Units::RADIAN, vlon ) && !isnan( vlon ) && orientationReader_->read( Units::RADIAN, vheading ) && !isnan( vheading ) ) { // Compute bearing and horizontal range to contact target.bearing_ = AuvMath::NormalizeAngle( Location::GetBearing( vlat, vlon, target.lat_, target.lon_ ) ); target.hrange_ = Location::GetDistance( vlat, vlon, target.lat_, target.lon_ ); // Assemble the 2D relative position vector Point2D relPos( cos( target.bearing_ ), sin( target.bearing_ ) ); relPos *= target.hrange_; if( !relPosLast_.isNan() ) { // Assemble the 2D relative velocity vector Point2D relVel = ( relPos - relPosLast_ ) * ( 1 / dt_.asDouble() ); // Compute the line-of-sight (LOS) angle time derivative losRotationRate = ( relPos.getX() * relVel.getV() - relPos.getY() * relVel.getU() ) / pow( target.hrange_, 2 ); target.losRotationRate_ = iirFilter_.filter( losRotationRate ); target.hApproachRate_ = ( relPos.getMagnitude() - relPosLast_.getMagnitude() ) / dt_.asDouble(); } rawBearingRateWriter_->write( Units::RADIAN_PER_SECOND, losRotationRate ); bearingRateWriter_->write( Units::RADIAN_PER_SECOND, target.losRotationRate_ ); relPosLast_ = relPos; } } /// Updates the internal LineCapture guidance mode void LineCapture::setGuidanceMode( GuidanceMode newMode ) { Str modeStr( "Transitioning guidance mode to: " ); switch( newMode ) { case UNINITIALIZED: modeStr += "UNINITIALIZED"; break; case TERMINAL_GUIDANCE: iirFilter_.init( 0.0 ); kpHeadingOverride_ = kpHeadingTerminalCfg_; kiHeadingOverride_ = kiHeadingTerminalCfg_; rolloutTime_ = Timestamp::NOT_SET_TIME; modeStr += "TERMINAL_GUIDANCE"; break; case FINAL_APPROACH: kpHeadingOverride_ = kpHeadingFinalCfg_; kiHeadingOverride_ = kiHeadingFinalCfg_; lockoutTime_ = Timestamp::NOT_SET_TIME; modeStr += "FINAL_APPROACH"; break; case SHORT_FINAL: kpHeadingOverride_ = kpHeadingFinalCfg_; kiHeadingOverride_ = kiHeadingFinalCfg_; lockoutTime_ = Timestamp::NOT_SET_TIME; modeStr += "SHORT_FINAL"; break; case INTERCEPT_LOCKOUT: kpHeadingOverride_ = kpHeadingFinalCfg_; kiHeadingOverride_ = kiHeadingFinalCfg_; lockoutTime_ = Timestamp::Now(); modeStr += "INTERCEPT_LOCKOUT"; break; case ROLLOUT: // Disarm the docking module reqDockingState( DockIF::STANDBY ); kpHeadingOverride_ = kpHeadingTerminalCfg_; kiHeadingOverride_ = kiHeadingTerminalCfg_; lockoutTime_ = Timestamp::NOT_SET_TIME; rolloutTime_ = Timestamp::Now(); modeStr += "ROLLOUT"; break; default: modeStr += "UNKNOWN"; break; } // Set the thruster speed to match the new guidance mode setSpeed( newMode ); if( verbose_ || debug_ ) logger_.syslog( modeStr, Syslog::INFO ); guidanceModeWriter_->write( Units::ENUM, newMode ); guidanceMode_ = newMode; } /// Set vehicle speed to match guidance mode void LineCapture::setSpeed( GuidanceMode mode ) { switch( mode ) { case TERMINAL_GUIDANCE: speedCmd_ = terminalGuidanceSpeed_; break; case FINAL_APPROACH: case SHORT_FINAL: case INTERCEPT_LOCKOUT: speedCmd_ = finalApproachSpeed_; break; case ROLLOUT: speedCmd_ = rolloutSpeed_; break; case UNINITIALIZED: default: break; } } /// Determine if the vehicle is closing on the target bool LineCapture::rangeClosing() { int trend = 0; for( size_t i = 0; i < rangeRepo_.size(); ++i ) { // A negative approach rate indicates we're moving toward the target. // However, assigning a positive value to MIN_APPROACH_RATE will help avoid // false aborts when the vehicle is caught on the line. rangeRepo_[i].approachRate_ <= MIN_APPROACH_RATE ? trend-- : trend++; } bool retVal = trend < ( int )TARGET_POS_BIN_SIZE; rangeClosingWriter_->write( Units::BOOL, retVal ); return retVal; } /// Determine if the ranges are stable (vehicle is on the dock line) bool LineCapture::rangeStable() { bool rangeStable( true ); double avgRange( 0 ); double avgApproachRate( 0 ); for( size_t i = 0; i < rangeRepo_.size(); ++i ) { avgApproachRate += rangeRepo_[i].approachRate_ / rangeRepo_.size(); avgRange += rangeRepo_[i].range_ / rangeRepo_.size(); } // Expect absolute approach rates to stabilize below the threshold when the vehicle // is caught on the line. rangeStable &= fabs( avgApproachRate ) < MIN_APPROACH_RATE; rangeStable &= avgRange < armRangeCfg_; // Check DVL x velocity wrt seafloor (if available) to confirm vehicle is no longer underway if( xVelocityWrtSeafloorReader_->isActive() && xVelocityWrtSeafloorReader_->wasTouchedSinceLastRun( this ) ) { float xVel( nanf( "" ) ); if( xVelocityWrtSeafloorReader_->read( Units::METER_PER_SECOND, xVel ) && !isnan( xVel ) ) { rangeStable &= xVel < MIN_APPROACH_RATE; } } return rangeStable; } /// Power on the camera and lights bool LineCapture::powerCameraAndLights() { // Power up the camera, then lights return powerCamera() && powerLights(); } /// Power on camera bool LineCapture::powerCamera() { bool cameraOn( false ); if( cameraLoaded_ ) { // Power the camera, samplePowerOnly will read true once on sampleNanoDvrReader_->requestData( true ); sampleNanoDvrReader_->read( cameraOn ); if( cameraOn ) { sampleNanoDvrReader_->requestData( false ); } } else { // Camera isn't loaded, so simply satisfy return true; } return cameraOn; } /// Power on the camera lights bool LineCapture::powerLights() { bool lightsOn( false ); if( powerOnlyLoaded_ ) { // Power the camera, samplePowerOnly will read true once on samplePowerOnlyReader_->requestData( DataAccessor::MIN_ERROR ); samplePowerOnlyReader_->read( lightsOn ); if( lightsOn ) { samplePowerOnlyReader_->requestData( false ); } } else { // PowerOnly component isn't loaded, so simply satisfy return true; } return lightsOn; } /// Request docking module state change bool LineCapture::reqDockingState( DockIF::DockingState stateReq ) { dockingStateCmd_ = stateReq; return cmdDockingState(); } /// Command the state of the docking module bool LineCapture::cmdDockingState() { if( dockingModuleLoaded_ ) { int dockingState( 0 ); if( dockingStateReader_->read( Units::ENUM, dockingState ) ) { dockingState_ = ( DockIF::DockingState )dockingState; if( dockingState_ == dockingStateCmd_ ) { return true; } else if( dockingState_ == DockIF::ERROR ) { return false; } } // Request the mode change dockingStateCmdWriter_->write( Units::ENUM, ( int )dockingStateCmd_ ); } else { // Docking module isn't loaded, so simply satisfy dockingState_ = dockingStateCmd_; return true; } return false; } /// Retrieve latest target range float LineCapture::getRange() { float range( nanf( "" ) ); if( !rangeRepo_.empty() ) range = rangeRepo_.back().range_; return range; } /// Retrieve latest target bearing float LineCapture::getBearing() { float bearing( nanf( "" ) ); if( !posRepo_.empty() ) bearing = posRepo_.back().bearing_; return bearing; } /// Uninit function void LineCapture::uninitialize() { cablePresentReader_->requestData( false ); dockingStateReader_->requestData( false ); orientationReader_->requestData( false ); samplePowerOnlyReader_->requestData( false ); posRepo_.clear(); rangeRepo_.clear(); setGuidanceMode( UNINITIALIZED ); } /// Mission Component factory interface Behavior* LineCapture::CreateBehavior( const Str& prefix, const Module* module ) { return new LineCapture( prefix, module ); } /// Construct TargetPos struct LineCapture::TargetPos::TargetPos() : time_( Timestamp::NOT_SET_TIME ), lat_( nanf( "" ) ), lon_( nanf( "" ) ), range_( nanf( "" ) ), acBearing_( nanf( "" ) ), bearing_( nanf( "" ) ), hrange_( nanf( "" ) ), deltaT_( nanf( "" ) ), deltaX_( nanf( "" ) ), approachRate_( nanf( "" ) ), losRotationRate_( nanf( "" ) ) {}