#include "WaypointWall.h" #include "FloatAttribute.h" #include "AngleAttribute.h" #include "BooleanAttribute.h" //#include "StringAttribute.h" #include "IntegerAttribute.h" #include "Syslog.h" #include "WorkSiteIF.h" #include "DvlSideIF.h" #include "MultibeamerIF.h" #include "NavUtils.h" #include "TimeP.h" #include "VehicleConfigurationIF.h" #include "matrixMath.h" #ifndef PI #define PI 3.14159265358979323846 #endif #define R2D(r) (r*180.0/PI) #ifndef PIUPON2 #define PIUPON2 (PI/2.) #endif #define NotSpecified -100000.0 #define AngleNotSpecified ((NotSpecified) / Math::RadsPerDeg) //#define DEFAULT_STRING_ATTR "\"\"" #define DEFAULT_STRING_ATTR "DefaultString" #define INVALID_RANGE 0 //allocate statics for logging int WaypointWall::_logFileRefCnt; WaypointWallLog *WaypointWall::_log; int WaypointWall::_terrainAidIFCnt; static Boolean WaypointWall::TurnLeft = False; WaypointWall::WaypointWall() : Behavior(WaypointWallBehaviorName, Sequential), cos30(cos(PI/6.)) { attributes.add(new FloatAttribute("northing", "Northing (or specify latitude/longitude)", &_northing, NotSpecified)); attributes.add(new FloatAttribute("easting", "Easting (or specify latitude/longitude)", &_easting, NotSpecified)); attributes.add(new AngleAttribute("latitude", "Latitude (or specify northing/easting)", &_latitude, AngleNotSpecified)); attributes.add(new AngleAttribute("longitude", "Longitude (or specify northing/easting)", &_longitude, AngleNotSpecified)); attributes.add(new AngleAttribute("lastLatitude", "Last Latitude (or specify northing/easting)", &_lastLatitude, AngleNotSpecified)); attributes.add(new AngleAttribute("lastLongitude", "Last Longitude (or specify northing/easting)", &_lastLongitude, AngleNotSpecified)); attributes.add(new FloatAttribute("speed", "Speed", &_speed)); attributes.add(new FloatAttribute("initialDepth", "Initial Depth", &_initialDepth, NotSpecified)); attributes.add(new FloatAttribute("finalDepth", "Final Depth", &_finalDepth, NotSpecified)); attributes.add(new FloatAttribute("standOff", "StandOff", &_standOff, NotSpecified)); attributes.add(new FloatAttribute("sonarTimeOut", "SonarTimeOut", &_sonarTimeOut, NotSpecified)); attributes.add(new FloatAttribute("maxCrossTrackError", "Max Cross Track Error", &_maxXte, NotSpecified)); attributes.add(new FloatAttribute("starboardMaxCrossTrackError", "Starbrd Max Cross Track Error", &_maxXteStar, NotSpecified)); attributes.add(new BooleanAttribute("abortOnTimeout", "Should mission abort if waypoint " "times out?", &_abortOnTimeout, False)); // // Type of projection for the range error: // Waypoint=1; Body=2; Minimum=3. // // 29 Jan 2016 "projection" is now obsolete, and has been replaced by // "filterType". We'll leave "projection" here for backward compatibility of // mission files. attributes.add(new IntegerAttribute("projection", "Type of range error", &_projection, 3)); attributes.add(new IntegerAttribute("filterType", "1=min Beam,2=least squares,3=LS+median", &_filterType, 3)); // // Dvl=1; Reson=2; DeltaT=3. attributes.add(new IntegerAttribute("rangeInst", "Range Instrument", &_rangeInst, 3)); // attributes.add(new IntegerAttribute("windowLen", "Median Filter Length", &_windowLen, 31)); // Retained useWallEstimator for backward compatibility of the mission // scripts. It's no longer used, and replaced by useWaypointBearing, the // logical complement. useWallEstimator = True is equivalent to // useWaypointBearing = False; attributes.add(new BooleanAttribute("useWallEstimator", "Now obsolete.", &_useWallEstimator, True)); // // This fixes the bearing based on a waypoint instead of the estimator. attributes.add(new BooleanAttribute("useWaypointBearing", "Otherwise use estimated bearing.", &_useWaypointBearing, False)); attributes.add(new IntegerAttribute("waypointPriority", "0=Ignore wps,1=Abort to wps,2=Prefer wps,3=Ignore wall", &_waypointPriority, 2)); attributes.add(new FloatAttribute("wallAbsentTime", "Time since wall valid.", &_wallAbsentTime, 10.)); attributes.add(new BooleanAttribute("wallFeedForward", "Otherwise zero it.", &_wallFeedForward, False)); attributes.add(new AngleAttribute("initialBearing", "initial Bearing", &_initBearing, AngleNotSpecified)); attributes.add(new FloatAttribute("initialDistance", "Initial distance", &_initDist, NotSpecified)); attributes.add(new BooleanAttribute("reinitFilter", "", &_reinitFilter, False)); attributes.add(new BooleanAttribute("escape", "", &_escape, False)); attributes.add(new FloatAttribute("escapeDistance", "Escape distance", &_escapeDist, NotSpecified)); attributes.add(new BooleanAttribute("termIfTrnConv", "", &_termIfTrnConv, False)); attributes.add(new BooleanAttribute("runIfTrnConv", "", &_runIfTrnConv, False)); // //Beware that these attributes are not parsed until after this constructor //finishes - so you can't use it in a calculation here. // // attributes.add(new StringAttribute("rangeInst", "Range Instrument",)); for( int i=0; i 30 ) { Syslog::write("WaypointWall - Error: fwdCutover is %d, " "and must be >=0 and <=30. Setting it to 15."); _fwdCutover = 15; } } WaypointWall::~WaypointWall() { // //Don't delete _dvlSide because subsequent behaviors may use it, assuming //this destructor runs when this behavior ends. Or does it not run until //the whole stack is done ?? // //if( _dvlSide ) delete _dvlSide; _logFileRefCnt--; if (_logFileRefCnt == 0 && _log) { delete _log; _log = NULL; } if( _terrainAid ) { Syslog::write("WaypointWall - Destructing Behavior::_terrainAid. " "%d interfaces open.",_terrainAidIFCnt); delete _terrainAid; _terrainAid = NULL; _terrainAidIFCnt--; } } void WaypointWall::execute( void ) { double dn, de; double dtw, goal; double ye; double escapeBearing; Boolean outOfTheLane = False; Boolean debug = True; Boolean gotReturn = False; double now = _missionClock->seconds(); struct timespec timeSpec; Time::gettime(&timeSpec); double nowUnix = Time::seconds(&timeSpec); // //check to see if Trn exists and is converged, and determine if we should terminate: if ( _runIfTrnConv ) { if (_terrainAid->trnConnected() ) { if( !_terrainAid->converged() ) { Syslog::write("WaypointWall::execute() -- Trn is NOT converged. Terminate at " "at t=%.2f sec.\n", now); setState(Finished); return; } } else { Syslog::write("WaypointWall::execute() -- Error. Attempted to check if Trn " "is converged, but Trn is not connected, " "at t=%.2f sec.\n", now); } } // Get current position _navigation->state(&_position, &_attitude); _heading = _attitude.yaw; if(_first && _initBearing != NotSpecified ) { _northing = _position.x + _initDist*cos(_initBearing); _easting = _position.y + _initDist*sin(_initBearing); Syslog::write("WaypointWall::Setting the northing and easting for " "acquistion at t=%.2f sec.", now); Syslog::write("WaypointWall::_initBearing=%.2f, " "NotSpecified = %.2f", _initBearing, NotSpecified); } /* distance to waypoint in N, E coords */ dn = _position.x - _northing; de = _position.y - _easting; if(_first) { try { if( _reinitFilter ) { if( _terrainAid->trnConnected() ) { _terrainAid->reinitFilter(); Syslog::write("WaypointWall - Reinitialized Filter at " "t = %.2f.", _missionClock->seconds()); } else { Syslog::write("WaypointWall - Reinitialisation attempted " "but terrainAidIF not present at t = %.2f.", _missionClock->seconds() ); } } } catch (Exception e) { Syslog::write("WaypointWall - Attempted to reinitialize " "at t = %.2f: %s", _missionClock->seconds(), e.msg); } catch (...) { _terrainAid = NULL; Syslog::write("WaypointWall - Exception caught while Attempting " "to reinitialize at t = %.2f.",_missionClock->seconds()); } // // If "escape" is set, then compute the waypoint. Set priority to 2. if( _escape ) { Syslog::write("WaypointWall::Escape - escape is true. Check to see if a " "wall was present."); _dynamicControlIF->getWallData( &_lastWallTime, &_wallBearing ); Syslog::write( "WaypointWall::Escape - wallAbsentTime = %.1f, ", _wallAbsentTime); Syslog::write( "WaypointWall::Escape - _lastWallTime = %.1f, " "mission time = %.2f, _wallBearing = %.1f\n", _lastWallTime, _missionClock->seconds(), R2D(_wallBearing)); if( _lastWallTime + _wallAbsentTime < nowUnix ) { setState(Finished); Syslog::write(" WaypointWall::Escape - Wall absent for " "%.1f seconds at %-15.2f. Terminate.\n", nowUnix -_lastWallTime, nowUnix); Syslog::write( "WaypointWall::Escape - current position (%.1f, %.1f), \n" " reached at t = %-15.2f\n", _position.x, _position.y, _missionClock->seconds()); return; } // // Escape with waypointPriority 2: _waypointPriority = 2; // // Turn until perpendicular to the estimated wall bearing. if( WaypointWall::TurnLeft ) { Syslog::write("WaypointWall::Escape - The wall is to our right, so turn left."); // // Convert the domain to [0, 2*pi], presumably required by the command generator. escapeBearing = _wallBearing - PI/2.; } else { Syslog::write("WaypointWall:: Escape - The wall is to our left, so turn right."); escapeBearing = _wallBearing + PI/2.; } Syslog::write("WaypointWall::Escape - The last good wall bearing was %.2f. " "Escape bearing is %.2f", _wallBearing*180/PI, escapeBearing*180/PI); // // Compute waypoint: _northing = _position.x + _escapeDist * cos(escapeBearing); _easting = _position.y + _escapeDist * sin(escapeBearing); // // Recompute (dn, de) since (_northing, _easting) have been recomputed: dn = _position.x - _northing; de = _position.y - _easting; // // Latch the current depth as the commanded depth: _initialDepth = _position.z; _finalDepth = _position.z; _slope = 0.; Syslog::write( "WaypointWall::Escape - Latching depth = %.1f, " "at mission time = %.2f.", _position.z, _missionClock->seconds()); } //end if( _escape ) // // // Determine the minimum depth. This CANNOT be done in the constructor // because the "attributes" aren't initialized yet. _minDepth = _finalDepth; if( _initialDepth < _finalDepth ) _minDepth = _initialDepth; // // This is Steve R's psi_ref, that is, _bearing = psi_ref. // _bearing = PI + Math::modPi( atan2(_easting - _position.y, _northing - _position.x) - PI); // // If the vehicle has cross-track error because it is either wall // following, or pinned up against a corridor boundary, compute the next // bearing from the previous waypoint, not the vehicle's current // location. This smoothly connects the waypoints and corridor boundaries. // if( _lastLatitude != NotSpecified && _lastLongitude != NotSpecified ) { _bearing = PI + Math::modPi( atan2(_easting - _lastEasting, _northing - _lastNorthing) - PI); } dtw = sqrt( dn*dn + de*de ); if( dtw < 1.0 ) { // // This is here to prevent a divide by zero in the slope calcuation // that follows. Syslog::write("WaypointWall -- ERROR. The distance from the start\n" "to the next waypoint is less than one meter."); //abortMission(); return; } _slope = (_initialDepth - _finalDepth)/dtw; if( _speed < 0.2 ) { Syslog::write("WaypointWall -- ERROR. Commanded speed = %7.4f," " which is too small.", _speed); //abortMission(); return; } // // Compute the climb rate, taking into account the speed commanded for // this waypoint. Abort if it's larger than the limit specified by // vehicle.cfg. // double deltah = _slope*dtw; double sRange = sqrt( dtw*dtw + deltah*deltah ); //dtw >= 10.0 above // // Let theta be the steady-state flight-path angle. Then // Zdot = U sin( theta ) // = U slope dtw / sRange. // where dtw is the horizontal distance and sRange is the (slant) dist. double climbRate = _speed * _slope * dtw / sRange; if( fabs(climbRate) > _vehicleConfig->maxDiveRate()) { Syslog::write("WaypointWall -- ERROR. Waypoint slope = %7.4f," " causes a climb rate of %7.4f,\n which is too large. ", _slope, climbRate); //abortMission(); return; } // // Please DO NOT CHANGE the write statement below. It is automatically // read out of syslog by a shell script, and read into the plotting // routines. Any changes will disrupt the plotting routines. Contact // Rob McEwen if you need to change this. // Syslog::write( "WaypointWall initialization: \n" " Begin waypoint control at t= %-15.2f" " (wplog)\n" " The current location (N,E) = %-15.1f, %-15.1f (wplog)\n" " The next waypoint = %-15.1f, %-15.1f (wplog)\n" " The bearing to the next w.p. is %.1f Degrees.\n" " The distance to the next w.p. is %.1f Meters.\n" " The slope to the next w.p. is %7.4f.\n" " The initial commanded depth is %.1f meters.\n" " The final commanded depth is %.1f meters.\n", _missionClock->seconds(), _position.x, _position.y, _northing, _easting, R2D(_bearing), dtw, _slope, _initialDepth, _finalDepth); _rangeToWall = _standOff; //_first = False; Syslog::write( "WaypointWall:: standOff = %5.2f at %.2f", _standOff, now); Syslog::write( "WaypointWall:: projection = %d at %.2f", _projection, now); Syslog::write( "WaypointWall:: windowLen = %d at %.2f", _windowLen, now); Syslog::write( "WaypointWall:: filterType = %d at %.2f", _filterType, now); Syslog::write( "WaypointWall:: escape = %d at %.2f", _escape, now); Syslog::write( "WaypointWall:: waypointPriority = %d at %.2f", _waypointPriority, now); _halfWindowLen = (_windowLen-1)/2; _phiMF = 0.; _thetaMF = 0.; _psiMF = 0.; _deltaT->get(DeltaTIF::Side, &_deltaTSideData); _tbeams = _deltaTSideData.nbeams; for( int i=0;i<_tbeams;i++) { _BeamRanges[i] = &(_deltaTSideData.beam_ranges[i]); } if( _deltaT->enabled(DeltaTIF::Fwd) ) { Syslog::write("WaypointWall::Forward-looking DeltaT IS enabled at " "t=%.2f sec.", now); _deltaT->get_mounting_angles(DeltaTIF::Fwd, &_phiMF, &_thetaMF, &_psiMF); _deltaT->get(DeltaTIF::Fwd, &_deltaTFwdData); // // Assume that the forward sonar is mounted along the vehicle // centerline, +x, and the aft sonar is perpendicular along vehicle // -y. // // _nbeams is the number of contiguous beams, which should always be // 210 given the above mounting angles. // // aft = 120, fwd = 60 => _beamOverlap = 0, _fwdCutover = 0 // aft = 120, fwd = 90 => _beamOverlap = 15, 0<=_fwdCutover<=15 // aft = 120, fwd = 120 => _beamOverlap = 30, 0<=_fwdCutover<=30 // //_tbeams += _deltaTFwdData.nbeams; _beamOverlap = (_deltaTFwdData.nbeams-60)/2; if( _fwdCutover > _beamOverlap ) { _fwdCutover = _beamOverlap/2; Syslog::write("WaypointWall - Error: fwdCutover > beamOverlap. " "Setting fwdCutover to beamOverlap/2 = %d.", _fwdCutover); } _tbeams = _tbeams + _deltaTFwdData.nbeams - _beamOverlap; // Syslog::write("WaypointWall: Number of beams in the array = %d, " "beamOverlap=%d fwdCutover=%d", _tbeams, _beamOverlap, _fwdCutover); // int j=0; // for( int i=_deltaTSideData.nbeams; i<_tbeams; i++) // { // _BeamRanges[i] = &(_deltaTFwdData.beam_ranges[j]); // j++; // } int j; int startCnt = _deltaTSideData.nbeams - _fwdCutover; int fwdOffset = _beamOverlap - _fwdCutover; for( i=startCnt, j=fwdOffset; i<_tbeams; i++, j++) _BeamRanges[i] = &(_deltaTFwdData.beam_ranges[j]); } else { Syslog::write("WaypointWall::Forward-looking DeltaT IS NOT enabled."); } Syslog::write("WaypointWall::Total number of beams is %d, " "t=%.2f sec.", _tbeams, now); // //Encode switches with bit masking: _switches = 0; if(_useWaypointBearing) _switches |= (1<<0); if(_wallFeedForward) _switches |= (1<<1); _wallPresent = False; } //if(_first) //check to see if we timed out if ( _abortOnTimeout && ( now - startTime() > _timeout)) { Syslog::write("WaypointWall::execute() -- timed out without reaching " "goal, t=%.2f sec.\n", now); setState(Finished); abortMission(); return; } // //check to see if Trn exists and is converged, and determine if we should terminate: if ( _termIfTrnConv ) { if (_terrainAid->trnConnected() ) { if( _terrainAid->converged() ) { Syslog::write("WaypointWall::execute() -- Trn has converged. Terminate at " "at t=%.2f sec.\n", now); setState(Finished); return; } } else { Syslog::write("WaypointWall::execute() -- Error. Attempted to check if Trn " "is converged, but Trn is not connected, " "at t=%.2f sec.\n", now); } } /* transform into dtw, xte coordinates */ dtw = -de*sin(_bearing) - dn*cos(_bearing); // If the wall is present, but goes absent, save the last good y-intercept: if(_wallPresent) _last_yInterWall_B = _yInterWall_B; // The final argument is the beam number, where one count equals one degree. processDeltaT( &_sonarIsAlive, &_sonarHasNewData, &_rangeToWall, &_psiWallwrtoVehicle, &_beamIndx); const short fovMargin = 15; const int avoidAngle = 150; // Psi = 0 in B const double avoidDist = 100.; //meters const int avoidCount = 5; const int detectCount= 30; _counter = 0; double xteWay; // // Compute cross-track error from the line. This includes a sign flip to // make negative feedback. xteWay = dn*sin(_bearing) - de*cos(_bearing); if( _sonarHasNewData ) { _initializeWallBearing = False; _RHT = False; _LHT = False; for( int i=0; i<_tbeams; i++ ) { if( *_BeamRanges[i] > _minAllowableRange ) _counter++; } if( _counter <=5 ) { // If the sonar doesn't see the wall skip the rest and fly the // left-hand edge. _wallPresent = False; _portQuarterFlipFlop = True; _rhtAheadFlipFlop = True; _rhtAngleFlipFlop = True; _lhtFlipFlop = True; } else { _wallPresent = True; _wallPresentFlipFlop = True; _firstAbsence = False; if( _waypointPriority == 1 ) { // // Priority = 1 means follow the wall, unless/until the vehicle // exceeds the corridor width, in which case the corridor logic // further below substitutes the waypoint bearing. // // Priority = 2 defaults to the waypoint bearing unless the wall // gets closer than the standoff, which triggers additional logic, // further below. _useWaypointBearing = False; } if( _deltaT->enabled(DeltaTIF::Fwd) ) { _counter = 0; for( int i=avoidAngle; i<_tbeams; i++ ) //avoidAngle = 150 = 0 deg in B. { if( *_BeamRanges[i] > _minAllowableRange && *_BeamRanges[i] < avoidDist ) { _counter++; } } if( _counter >= avoidCount ) { // // If the sonar sees a wall ahead or to the right, turn right // and skip the rest: _RHT = True; if( _rhtAheadFlipFlop ) { Syslog::write("WaypointWall::>5 Returns in sector 0 to 30 deg. " "in B; Setting RHT at t=%.2f sec.", now); _rhtAheadFlipFlop = False; _rhtAngleFlipFlop = True; } } else if( _beamIndx > 105 && //105 = -45 deg in B. _beamIndx < 195) //195 = +45 deg in B. { _RHT = True; // -45 < PsiMin < 45 deg in B if( _rhtAngleFlipFlop ) { Syslog::write("WaypointWall::-45 < angle-to-wall < 45 deg. " "in B; Setting RHT at t=%.2f sec.", now); _rhtAngleFlipFlop = False; _rhtAheadFlipFlop = True; } } else { _rhtAngleFlipFlop = True; _rhtAheadFlipFlop = True; } } else { if( _beamIndx > 105 ) _RHT = True; Syslog::write("WaypointWall::angle-to-wall > -45 deg. in B; " "and fwd disabled, Setting RHT at t=%.2f sec.", now); } // // Back beams are the weakest. Turn left before they become //unreliable. Bump up the LHT sector from 15 to 40 degrees. if( _beamIndx < 40 && !_RHT ) { _LHT = True; if( _lhtFlipFlop ) { Syslog::write("WaypointWall::angle-to-wall < -120 deg. " "in B; Setting LHT at t=%.2f sec.", now); _lhtFlipFlop = False; } } else { _lhtFlipFlop = True; } } // // Make the above true for ngood>=5 // _xteWall = _standOff - _rangeToWall; } // // Now check to see if the vehicle will be *commanded* out of the // lane. "Captain, you are out of the lane!" // // -y_cmd = - Delta_y + rangeToWall - standoff // = - Delta_y - xteWall // = xteWay - xteWall // // is the commanded location. xteWay has already been multipled by -1 for // negative feedback, hence the -y_cmd. // // Check to see if commanded out of the port side of the lane: // //if( xteWay - _xteWall >= _maxXte ) //if( xteWay >= _maxXte && xteWay - _xteWall >= _maxXte ) //else if( xteWay - _xteWall <= -_maxXteStar || _rangeToWall == 0. ) //else if( (xteWay <= -_maxXteStar // && xteWay - _xteWall <= -_maxXteStar) // || _rangeToWall == 0. ) _hMode = DynamicControlIF::WaypointWall; if( _waypointPriority == 3 ) { _xTrkOffset = 0.; //Because mode=WaypointDepth with zero offset. _psiWallwrtoVehicle = 0.; _LHT = False; _RHT = False; _hMode = DynamicControlIF::WaypointDepth; } // // The sonar has stopped responding. Either abort or return to the line. else if( !_sonarIsAlive ) { _wallPresent = False; if( _waypointPriority > 0 ) { _xTrkOffset = 0.; //Because mode=WaypointDepth with zero offset. _psiWallwrtoVehicle = 0.; _LHT = False; _RHT = False; _hMode = DynamicControlIF::WaypointDepth; if( _sonarAliveFlipFlop ) { Syslog::write("WaypointWall - Error. One of the multibeam sonars " "has stopped responding at t=%.1f. " "Fly the waypoints.", _missionClock->seconds()); _sonarAliveFlipFlop = False; } } else { Syslog::write("WaypointWall - Error. One of the multibeam sonars " "has not responded " "for too long. Abort. waypointPriority = %d;\n " "Side = %.1f, Fwd = %.1f sec; Abort.", _waypointPriority, _nowUnixDT - _lastUnixPingTime, _nowUnixDT - _lastFwdUnixPingTime); abortMission(); } } else if( !_wallPresent ) { if( _waypointPriority > 0 ) { // // Beware that in Wall Mode, xTrkOffset is the position error, not the offset. // _xTrkOffset = xteWay; //Fly the center. _maxXte is the left edge. _useWaypointBearing = True; _psiWallwrtoVehicle = 0.; _LHT = False; _RHT = False; _firstWall = False; if( !_firstAbsence && _wallPresentFlipFlop) { Syslog::write("WaypointWall - The wall has become " "absent at %.1f seconds. WaypointPriority = %d, " "so fly the line.", _missionClock->seconds(), _waypointPriority); _wallPresentFlipFlop = False; } } else { if( _firstAbsence ) { // // Beware that in Wall Mode, xTrkOffset is the position error, not the offset. // _xTrkOffset = xteWay; //Fly the center. _maxXte is the left edge. _psiWallwrtoVehicle = 0.; _LHT = False; _RHT = False; } else { _dynamicControlIF->getWallData( &_lastWallTime, &_wallBearing ); // // Fly the last good wall bearing: // if( _last_yInterWall_B > 0 ) // { // // // // Vehicle apparently lost contact with the wall before having // // turned around. Fly in the other direction until the wall is // // detected or we time out. // _bearing = _wallBearing - PI; // } // else // { // _bearing = _wallBearing; // } // _bearing = _wallBearing; // // Convert to 0-2pi: _bearing = PI + Math::modPi( _bearing - PI ); // //Without the wall we can't compute a lateral error, so we must //open the loop and just fly the heading. This is essentially now //a setpoint. _xTrkOffset = 0.; if( _lastWallTime + _wallAbsentTime < _nowUnixDT ) { Syslog::write("WaypointWall - Error. The wall has been absent " "for %.1f seconds, and waypointPriority = %d. Abort.", _nowUnixDT - _lastWallTime, _waypointPriority); // // Be VERY careful. Use the last good y intercept. Syslog::write("WaypointWall - last_yInterWall_B = %.2f m. ", _last_yInterWall_B); if( _last_yInterWall_B > 0 ) { WaypointWall::TurnLeft = True; } abortMission(); } } } //end waypointPriority == 0 } // end wall absent else if( _waypointPriority == 2 ) { // // Follow the line: _xTrkOffset = xteWay; _useWaypointBearing = True; _initializeWallBearing = False; // if( _escape && _wallPresent && _firstWall ) // { // // // // Follow the line until the wall goes away. Then, start looking for // // any other obstructions or walls. // } // else if ( _xteWall > 0. || _pr2FlipFlop || !_rhtAheadFlipFlop ) if ( _xteWall > 0. || _pr2FlipFlop || !_rhtAheadFlipFlop ) { // // Wall-follow instead: _firstWall = False; if( !_pr2FlipFlop ) { Syslog::write("WaypointWall:: Priority = %d; Transitioning to " "wall-following at t = %.2f", _waypointPriority, _missionClock->seconds()); _pr2FlipFlop = True; _initializeWallBearing = True; } _xTrkOffset = _xteWall; _useWaypointBearing = False; // // Check to see if we have a clear shot to the waypoint. // double waypointBearing = Math::modPi( atan2(_easting - _position.y, _northing - _position.x) ); double wallBearing = Math::modPi(_psiWallwrtoVehicle + _attitude.yaw ); // // First thought was to use if( waypointBearing - wallBearing > 0) to // determine when to resume waypoint control. But this suffers from // the two-pi rollover problem. So, use the cross product instead. // // These unit vectors are coordinatized in NED. double wayHat[3],wallHat[3], cross[3]; wayHat[0] = _northing - _position.x; wayHat[1] = _easting - _position.y; wayHat[2] = 0.; Vnormalize( wayHat ); wallHat[0] = cos(wallBearing); wallHat[1] = sin(wallBearing); wallHat[2] = 0.; Vcross( cross, wayHat, wallHat ); if( cross[2] < 0.) { // // Reset the global waypoint bearing. // The +/- PI funny business translates the range from [-pi,pi] to [0, 2pi]. _bearing = PI + Math::modPi( waypointBearing - PI ); _useWaypointBearing = True; Syslog::write("WaypointWall:: Priority = %d; Resume " "waypoint guidance at t = %.2f, with new bearing %.2f", _waypointPriority, _missionClock->seconds(), waypointBearing*180./PI); _pr2FlipFlop = False; xteWay = dn*sin(_bearing) - de*cos(_bearing); _xTrkOffset = xteWay; Syslog::write("WaypointWall:: wallBearing %.2f deg. at t = %.2f", wallBearing*180./PI, _missionClock->seconds()); // // Compute a new waypoint bearing, and follow it. Syslog::write("WaypointWall:: Compute " "new bearing %.2f deg. at t = %.2f", waypointBearing*180./PI, _missionClock->seconds()); } } // End wall-following // //Don't let it turn left if we're line-following. if( !_pr2FlipFlop && _LHT ) { _LHT = False; Syslog::write("WaypointWall:: Priority = %d; Unsetting " "left-hand turn at t = %.2f", _waypointPriority, _missionClock->seconds()); } } else if( (xteWay - _xteWall >= _maxXte) && _waypointPriority == 1 ) { _xTrkOffset = xteWay - _maxXte; outOfTheLane = True; } else if( (xteWay - _xteWall <= -_maxXteStar) && _waypointPriority == 1 ) { _xTrkOffset = xteWay + _maxXteStar; outOfTheLane = True; } else { _xTrkOffset = _xteWall; _firstWall = False; } // //Since here dtw is the projection along the line of bearing, only //require that the vehicle pass the waypoint, which is equivalent to //dtw<0. // goal = 0.; /* load outputs */ if ( dtw <= goal && ( _waypointPriority != 0 || _firstAbsence ) ) { setState(Finished); // // Please to not modify the following write. // dprintf(" WaypointWall has set the state to finished.\n"); Syslog::write( "WaypointWall (%.1f, %.1f), \n" " reached at t = %-15.2f" " (wplog)\n", _northing, _easting, _missionClock->seconds()); Syslog::write( "Vehicle Position is error is (%.1f, %.1f)\n", _northing-_position.x, _easting-_position.y); } else { /* load command vector */ if (_depth != NotSpecified) { _depth = _finalDepth + _slope*dtw; setVertical(DynamicControlIF::Depth, _depth, 0., 0., (long) _beamIndx, _rangeToWall); } setSpeed(DynamicControlIF::Speed, _speed); // // Set the other switches: // 0 useWaypointBearing // 1 wallFeedForward // 2 outOfTheLane // 3 LHT // 4 RHT // 5 initializeWallBearing // 6 wallPresent // if(_useWaypointBearing) _switches |= (1<<0); else _switches &= ~(1<<0); if(outOfTheLane) _switches |= (1<<2); else _switches &= ~(1<<2); if(_LHT) _switches |= (1<<3); else _switches &= ~(1<<3); if(_RHT) _switches |= (1<<4); else _switches &= ~(1<<4); if(_initializeWallBearing) _switches |= (1<<5); else _switches &= ~(1<<5); if(_wallPresent) _switches |= (1<<6); else _switches &= ~(1<<6); // // Use newEasting to pass the switches. double newEasting = _switches; double newNorthing = 0.; // // use newBearing to pass psiWallwrtoVehicle. double newBearing = _psiWallwrtoVehicle; setHorizontal(_hMode, _bearing, _northing, _easting, newBearing, newNorthing, newEasting, _first, _xTrkOffset); } _log->setFields(this); _log->write(); if( !_first ) _second = False; _first = False; } Boolean WaypointWall::shouldBehaviorStart() { return bothSequence(); } void WaypointWall::processDvl( Boolean *sonarIsAlive, Boolean *sonarHasNewData, double *rangeToWall ) { // // Compute range to the wall from the side-looking Dvl: DeviceIF::Status status = _dvlSide->get(&_dvlData, &_dvlHasNewData); double ctheta, stheta, cphi, sphi; int numValidBeams=0; double beamRange[NUM_BEAMS], h[NUM_BEAMS]; double aveAlt = 0., minRange = 500.; Boolean useMinBeam = False; int minIndx = -1; // TimeIF::TimeSpec now; double nowUnix; Time::gettime(&now); nowUnix = Time::seconds(&now); // // Ignore the _dvlHasNewData passed by get() and flag new data here. Use a != // rather than an < in the "if" below so that a Dvl clock rollover doesn't // set _dvlHasNewData to False. // *sonarHasNewData = False; if( _lastPingTime != _dvlData.pingTime ) { *sonarHasNewData = True; *sonarIsAlive = True; _lastPingTime = _dvlData.pingTime; _lastUnixPingTime = nowUnix; } // // Ensure that the Dvl is still working. // if( nowUnix > _lastUnixPingTime + _sonarTimeOut ) *sonarIsAlive = False; if( *sonarIsAlive && *sonarHasNewData ) { #if 0 ctheta = cos(_attitude.pitch); stheta = sin(_attitude.pitch); cphi = cos(_attitude.roll); sphi = sin(_attitude.roll); #endif // // Unfortunately the beams are not declared as an array in the DvlSideIF.idl. // keep running count of number of consecutive good hits for filtering (_dvlData.beam1 != BAD_BEAM_RANGE) ? _consecGoodHits[0]++:_consecGoodHits[0]=0; (_dvlData.beam2 != BAD_BEAM_RANGE) ? _consecGoodHits[1]++:_consecGoodHits[1]=0; (_dvlData.beam3 != BAD_BEAM_RANGE) ? _consecGoodHits[2]++:_consecGoodHits[2]=0; (_dvlData.beam4 != BAD_BEAM_RANGE) ? _consecGoodHits[3]++:_consecGoodHits[3]=0; // Prevent rollover if (_consecGoodHits[0] >= MIN_GOOD_HITS_WALL) _consecGoodHits[0] = MIN_GOOD_HITS_WALL; if (_consecGoodHits[1] >= MIN_GOOD_HITS_WALL) _consecGoodHits[1] = MIN_GOOD_HITS_WALL; if (_consecGoodHits[2] >= MIN_GOOD_HITS_WALL) _consecGoodHits[2] = MIN_GOOD_HITS_WALL; if (_consecGoodHits[3] >= MIN_GOOD_HITS_WALL) _consecGoodHits[3] = MIN_GOOD_HITS_WALL; beamRange[0] = _dvlData.beam1; beamRange[1] = _dvlData.beam2; beamRange[2] = _dvlData.beam3; beamRange[3] = _dvlData.beam4; //mark beam range as bad if it hasn't tracked the bottom for more than //MIN_GOOD_HITS number of bottom detects if (_consecGoodHits[0] < MIN_GOOD_HITS_WALL) beamRange[0] = BAD_BEAM_RANGE; if (_consecGoodHits[1] < MIN_GOOD_HITS_WALL) beamRange[1] = BAD_BEAM_RANGE; if (_consecGoodHits[2] < MIN_GOOD_HITS_WALL) beamRange[2] = BAD_BEAM_RANGE; if (_consecGoodHits[3] < MIN_GOOD_HITS_WALL) beamRange[3] = BAD_BEAM_RANGE; // // // Beam numbering convention, looking through Dvl from behind. x and z are // the vehicle axes. // // x // ^ // | // 1 | 3 // |------> z // 4 2 // // h[i] is the the z component of each beam's unit vector (in N). // Multiply by the measured range to get the altitude of the i^th beam // assuming a planar level bottom. // //h[0] = ( -stheta*c3s2 - ctheta*sphi*s3s2 + ctheta*cphi*c2 )*_dvlData.beam1; //h[1] = ( stheta*c3s2 + ctheta*sphi*s3s2 + ctheta*cphi*c2 )*_dvlData.beam2; //h[2] = ( -stheta*c3s2 + ctheta*sphi*s3s2 + ctheta*cphi*c2 )*_dvlData.beam3; //h[3] = ( stheta*c3s2 - ctheta*sphi*s3s2 + ctheta*cphi*c2 )*_dvlData.beam4; // h[0] = _dvlData.beam1*cos30; h[1] = _dvlData.beam2*cos30; h[2] = _dvlData.beam3*cos30; h[3] = _dvlData.beam4*cos30; for( int i=0; i20 // degrees from the Dvl centerline. *rangeToWall = MAX_BEAM_RANGE * cos30; } }// if( *sonarIsAlive && *sonarHasNewData ) } void WaypointWall::processMultibeamer( Boolean *sonarIsAlive, Boolean *sonarHasNewData, double *rangeToWall ) { TimeIF::TimeSpec updateTime; Boolean ready; double pingTime; //_multibeamer->get_status( rangeToWall, &updateTime, sonarIsAlive, &ready); pingTime = Time::seconds(&updateTime); if( _lastPingTime != pingTime ) { *sonarHasNewData = True; _lastPingTime = pingTime; } else { *sonarHasNewData = False; } } const short maxWindowLen = 31; void WaypointWall::processDeltaT( Boolean *sonarIsAlive, Boolean *sonarHasNewData, double *rangeToWall, double *psiWallwrtoVehicle, short *beamIndx ) { short startIndx; short nbeams = _deltaTSideData.nbeams; short centerIndx = nbeams/2, offsetIndx = 0; long medianArray[maxWindowLen]; //double *medianPtrArray[maxWindowLen]; struct beams windowBeams[maxWindowLen]; int indexArray[maxWindowLen]; short scale = 1000; const double initMinBeamRange = 500.; int minIndx; int i; Boolean medianFilter = False; const int aheadBeamArrayLen = 5; int aheadCntr = 0; int aheadBeamArray[aheadBeamArrayLen]; for( i = 0; iget(DeltaTIF::Side, &_deltaTSideData); if( _deltaT->enabled(DeltaTIF::Fwd) ) { _deltaT->get(DeltaTIF::Fwd, &_deltaTFwdData); } pingTime = Time::seconds(&(_deltaTSideData.update_time)); *sonarHasNewData = False; if( _lastPingTime != pingTime ) { *sonarHasNewData = True; *sonarIsAlive = True; _lastPingTime = pingTime; _lastUnixPingTime = _nowUnixDT; } Boolean fwdSonarHasNewData = False; if( _deltaT->enabled(DeltaTIF::Fwd) ) { pingFwdTime = Time::seconds(&(_deltaTFwdData.update_time)); if( _lastFwdPingTime != pingFwdTime ) { fwdSonarHasNewData = True; *sonarIsAlive = True; _lastFwdPingTime = pingFwdTime; _lastFwdUnixPingTime = _nowUnixDT; } } // // Ensure that the side-looking DeltaT is still working. // if( _nowUnixDT > _lastUnixPingTime + _sonarTimeOut ) { *sonarIsAlive = False; Syslog::write("WaypointWall - Error. The side-looking sonar " "has not responded for %.1f seconds.", _nowUnixDT - _lastUnixPingTime); } // // Check to see if the forward-looking DeltaT is working. // if( _deltaT->enabled(DeltaTIF::Fwd) ) { if( _nowUnixDT > _lastFwdUnixPingTime + _sonarTimeOut ) { *sonarIsAlive = False; Syslog::write("WaypointWall - Error. The forward-looking sonar " "has not responded for %.1f seconds.", _nowUnixDT - _lastFwdUnixPingTime); } } if( *sonarIsAlive ) _sonarAliveFlipFlop = True; if( *sonarHasNewData || fwdSonarHasNewData ) { // // Locate the minimum range. _minBeamRange = initMinBeamRange; _beamIndxMin = 0; for( i=0; i<_tbeams; i++ ) { if( *_BeamRanges[i] < _minBeamRange && *_BeamRanges[i] > _minAllowableRange ) { _minBeamRange = *_BeamRanges[i]; _beamIndxMin=i; } } if( _minBeamRange == initMinBeamRange ) _minBeamRange = INVALID_RANGE; if( _beamIndxMin >= _tbeams ) _beamIndxMin = _tbeams-1; else if( _beamIndxMin < 0 ) _beamIndxMin = 0; // // Least Squares Computation: int nGoodBeams = 0; _allInPortQuarter = True; float x[MAX_BEAMS_DT]; float y[MAX_BEAMS_DT]; // // Convert Beam ranges into SNAME-standard body-fixed Cartesian coordinates: for( i=0; i<_tbeams; i++ ) { // Boolean beamGood = False; Boolean beamGoodNow = False; Boolean beamGoodLast = False; Boolean beamGoodLastLast = False; double beamAngle_B = (i - 150) * PI/180.; _BeamRangesLastLast[i] = _BeamRangesLast[i]; _BeamRangesLast[i] = *_BeamRanges[i]; // if( *_BeamRanges[i] > INVALID_RANGE && // _BeamRangesLast[i] > INVALID_RANGE && // _BeamRangesLastLast[i] > INVALID_RANGE ) beamGoodNow = *_BeamRanges[i] > INVALID_RANGE; beamGoodLast = beamGoodNow && _BeamRangesLast[i] > INVALID_RANGE; beamGoodLastLast = beamGoodLast && _BeamRangesLastLast[i] > INVALID_RANGE; if( _first && beamGoodNow || _second && beamGoodLast || beamGoodLastLast ) { x[nGoodBeams] = *_BeamRanges[i]*cos( beamAngle_B ); y[nGoodBeams] = *_BeamRanges[i]*sin( beamAngle_B ); if( x[nGoodBeams] > 0. ) { if( aheadCntr < aheadBeamArrayLen ) { aheadBeamArray[aheadCntr] = i; aheadCntr++; // // There have to be two or more beams ahead to turn off the // _allInPortQuarter flag. This is to prevent a single // ahead-hit from toggling it. if( aheadCntr >= 2 ) _allInPortQuarter = False; } } nGoodBeams++; } } // // OK, it's a 2x2. Rather than involve newmat, use brute force. double A[240][2], AtA[2][2] = { {0.,0.}, {0.,0.} }; double detAtA, AtAInv[2][2] = { {0.,0.}, {0.,0.} }; double slope; for( i=0; i 0. ) { AtAInv[0][0] = AtA[1][1]/detAtA; AtAInv[0][1] = -AtA[0][1]/detAtA; AtAInv[1][0] = -AtA[1][0]/detAtA; AtAInv[1][1] = AtA[0][0]/detAtA; double temp[2] = {0.,0.}; for( i=0; i= _tbeams ) _beamIndxLS = _tbeams-1; else if( _beamIndxLS < 0 ) _beamIndxLS = 0; if( _yInterWall_B != INVALID_RANGE && _windowLen > 1 ) { // // Median filter. Select halfWindowLen neighboring beams to either side // of the minimum range beam. Handle the case where *beamIndx is within // halfWindowLen of either end of the array. if( _beamIndxLS < _halfWindowLen ) startIndx = 0; else if( _beamIndxLS > _tbeams - _halfWindowLen - 1 ) startIndx = _tbeams - _windowLen; else startIndx = _beamIndxLS - _halfWindowLen; int nGood = 0; // // Find the median in the window for( i=startIndx; i < startIndx + _windowLen; i++ ) { if( *_BeamRanges[i] > INVALID_RANGE ) { windowBeams[nGood].range = _BeamRanges[i]; windowBeams[nGood].num = i; nGood++; } } if( nGood < 2 ) { _rangeLSMedian = _rangeLS; _beamIndxLSMedian = _beamIndxLS; } else { Math::shellSort( windowBeams, nGood ); int nGoodMedian = nGood/2; _rangeLSMedian = (double) *windowBeams[ nGoodMedian ].range; _beamIndxLSMedian = (short) windowBeams[ nGoodMedian ].num; } } else { _rangeLSMedian = _rangeLS; _beamIndxLSMedian = _beamIndxLS; } switch(_filterType) { case MinBeam: default: *rangeToWall = (double) *_BeamRanges[_beamIndxMin]; *beamIndx = _beamIndxMin; if(_minBeamRange == initMinBeamRange) *psiWallwrtoVehicle = 0.; else *psiWallwrtoVehicle = (double) (_beamIndxMin - 60)*PI/180.; break; case LeastSquares: *rangeToWall = (double) _rangeLS; *beamIndx = _beamIndxLS; if( _yInterWall_B == INVALID_RANGE ) *psiWallwrtoVehicle = 0.; else *psiWallwrtoVehicle = (double) (_beamIndxLS - 60)*PI/180.; break; case LeastSquaresMedian: *rangeToWall = (double) _rangeLSMedian; *beamIndx = _beamIndxLSMedian; if( _yInterWall_B == INVALID_RANGE ) *psiWallwrtoVehicle = 0.; else *psiWallwrtoVehicle = (double) (_beamIndxLSMedian - 60)*PI/180.; break; } //switch // // Conditions to use the least-squares solution: // // 1. There must be at least 5 good beams from the current ping. // 2. At least four of them must also have been good on the last ping. // 3. Three or or more that meet (2) above must be ahead of the vehicle beam. // // Otherwise, we use the minimum beam range to determine the wall normal and the range. // if( (_allInPortQuarter || nGoodBeams < 4) && _wallPresent ) { if( _portQuarterFlipFlop ) { Syslog::write("WaypointWall::processDeltaT - WARNING. All valid beams in port quarter " "or too few beams " "at t = %.1f seconds.", _missionClock->seconds()); Syslog::write("WaypointWall::processDeltaT - There are %d good beams. " "Discarding least-squares solution and substituting " "minimum beam.", nGoodBeams+1); _portQuarterFlipFlop = False; } *rangeToWall = (double) *_BeamRanges[_beamIndxMin]; *beamIndx = _beamIndxMin; if(_minBeamRange == initMinBeamRange) { *psiWallwrtoVehicle = 0.; } else { *psiWallwrtoVehicle = (double) (_beamIndxMin - 60)*PI/180.; } } else { if( !_portQuarterFlipFlop ) { Syslog::write("WaypointWall::processDeltaT - At least one valid beam ahead " "at t = %.1f seconds.", _missionClock->seconds()); Syslog::write("WaypointWall::processDeltaT - There are %d good beams. " "Use least-squares solution.", nGoodBeams+1); // // If all valid beams have been in the port quarter, and now we // detect one or more valid beams ahead, log up to the first // aheadBeamArrayLen of them, for only this ping. int j=0; while( j < aheadCntr) { Syslog::write("WaypointWall::processDeltaT - Beam %d is ahead.", aheadBeamArray[j]); j++; } _portQuarterFlipFlop = True; } } }// if( *sonarIsAlive && *sonarHasNewData ) } Boolean WaypointWall::validInput() { Boolean debug = False; Boolean valid = True; WorkSiteIF workSite("workSite"); dprintf("WaypointWall::validInput() - _northing: %.2f, _easting: %.2f", _northing, _easting); dprintf("WaypointWall::validInput() - _latitude: %.2f, _longitude: %.2f", _latitude, _longitude); Boolean _utmSpecified = False; Boolean _geographicSpecified = False; if (_northing != NotSpecified || _easting != NotSpecified) _utmSpecified = True; if (_latitude != NotSpecified || _longitude != NotSpecified) _geographicSpecified = True; if (_utmSpecified && _geographicSpecified) { printError("Can't specify both UTM and geographic coords"); valid = False; } Boolean coordsSpecified = False; if( _initBearing != NotSpecified || _initDist != NotSpecified ) { // OK, this is either an escape or an acquisition run. Waypoints aren't // specified. if( _initDist == NotSpecified || _initBearing == NotSpecified ) { valid = False; if( _escape ) { printError("Cannot specify \"escape\" and either initBearing " "or initDist"); } else { printError("You must specify both initBearing " "and initDist"); } } else coordsSpecified = True; } else { // // Waypoint location must be specified either in UTM or geographic coords if (_northing != NotSpecified && _easting != NotSpecified) { coordsSpecified = True; } if (_latitude != NotSpecified && _longitude != NotSpecified) { if (coordsSpecified) { printError("Use either UTM or geographic to specify coords"); valid = False; } else { // Convert lat/lon to UTM NavUtils::geoToUtm(_latitude, _longitude, workSite.utmZone(), &_northing, &_easting); coordsSpecified = True; } } } if( _useWaypointBearing ) { if( _waypointPriority == 0 || _waypointPriority == 2 ) { // 0 => no waypoints. // 2 => wall of unknown heading is blocking the vehicle. printError("waypointPriority must be 1 or 3."); valid = False; } } if( _escape && coordsSpecified ) { printError("Cannot specify coordinates when escaping."); valid = False; } if (!_escape && !coordsSpecified) { printError("Either escape or specify either UTM or geographic coordinates."); valid = False; } if( _escape && _escapeDist == NotSpecified ) { printError("You must specify an escape distance if escaping."); valid = False; } if( !_escape && _escapeDist != NotSpecified ) { printError("Escape distance specified, but \"escape\" not set to True."); valid = False; } if (_speed < 0.) { printError("Invalid speed"); valid = False; } if( !_escape && ( _initialDepth == NotSpecified || _finalDepth == NotSpecified ) ) { valid = False; printError("You must specify both initial and final depths."); } if (_abortOnTimeout) { _timeout = duration(); _duration += 10; } if( _waypointPriority < 0 || _waypointPriority > 3 ) { printError("waypointPriority must be 0, 1, 2 or 3."); valid = False; } if( _maxXte != NotSpecified ) { if( _maxXte < 0.) { printError("maxCrossTrackError must be positive."); valid = False; } if( _maxXteStar != NotSpecified ) { if( _maxXteStar < 0.) { printError("starboardMaxCrossTrackError must be positive."); valid = False; } else { Syslog::write("WaypointWall::Setting starboard corridor limit to %5.2f meters.", _maxXteStar); } } else //if not specified: { _maxXteStar = _maxXte; } if( _waypointPriority == 0 || _waypointPriority == 2 ) { printError("Cannot specify corridors when WaypointPriority = 0 or 2."); valid = False; } } else { if( _waypointPriority == 1 ) { printError("Please specify corridor walls when WaypointPriority == 1."); valid = False; } } if (_sonarTimeOut == NotSpecified || _sonarTimeOut < 0.) { printError("sonarTimeOut not specified."); valid = False; } if (_standOff == NotSpecified || _standOff < 0.) { printError("standOff not specified."); valid = False; } if( ( _lastLatitude != NotSpecified || _lastLongitude != NotSpecified ) && _escape) { printError("Cannot specify either lastLatitude or lastLongitude with \"escape\"."); valid = False; } else if(_lastLatitude == NotSpecified && _lastLongitude != NotSpecified || _lastLatitude != NotSpecified && _lastLongitude == NotSpecified) { printError("You must specify both lastLatitude and lastLongitude."); valid = False; } if(_lastLatitude != NotSpecified && _lastLongitude != NotSpecified) { NavUtils::geoToUtm(_lastLatitude, _lastLongitude, workSite.utmZone(), &_lastNorthing, &_lastEasting); Syslog::write("WaypointWall::Converted lastLatitude = %12.8f degrees to %12.2f m", _lastLatitude*180/PI, _lastNorthing); Syslog::write("WaypointWall::Converted lastLongitude = %12.8f degrees to %12.2f m", _lastLongitude*180/PI, _lastEasting); } if( _windowLen < 0 || _windowLen > maxWindowLen || _windowLen % 2 == 0 ) { char errorStr[128]; sprintf(errorStr,"windowLen must be an odd positive integer <= %d", maxWindowLen); printError(errorStr); valid = False; } if( !_useWallEstimator ) { // // In case an old script sets it false. printError("WaypointWall argument useWallEstimator is obsolete. Use useWaypointBearing instead."); valid = False; } Syslog::write("WaypointWall::validInput() - Opening IF to rangeInst = %d", _rangeInst); Boolean IFopen = False; switch( _rangeInst ) { case Dvl: try { if( !_dvlSide ) _dvlSide = new DvlSideIF("dvlSide"); IFopen = True; } catch (Exception e) { Syslog::write("WaypointWall--Caught exception on creation " "of DvlIF:%s\n", e.msg); _dvlSide = NULL; } catch(...) { Syslog::write("WaypointWall -- Failed to initialize connection " "to DvlSideIF"); _dvlSide = NULL; } break; case Multibeamer: try { if( !_multibeamer ) _multibeamer = new MultibeamerIF("Multibeamer"); IFopen = True; } catch (Exception e) { Syslog::write("WaypointWall--Caught exception on creation " "of MultibeamerIF:%s\n", e.msg); _multibeamer = NULL; } catch(...) { Syslog::write("WaypointWall -- Failed to initialize connection " "to MultibeamerIF"); _multibeamer = NULL; } break; case DeltaT: try { if( !_deltaT ) _deltaT = new DeltaTIF("WaypointWall->DeltaTIF"); IFopen = True; } catch (Exception e) { Syslog::write("WaypointWall--Caught exception on creation " "of DeltaTIF:%s\n", e.msg); _deltaT = NULL; } catch(...) { Syslog::write("WaypointWall -- Failed to initialize connection " "to DeltaTIF"); _deltaT = NULL; } break; default: { Syslog::write("WaypointWall:: Error. You must specify the range sensor."); //abortMission(); valid = False; } break; } if( !IFopen ) { Syslog::write("WaypointWall:: Error. The interface to %d failed to open. " "Aborting.", _rangeInst); //abortMission(); valid = False; } else { Syslog::write("Using range finder %d.", _rangeInst); } if( _reinitFilter && _runIfTrnConv ) { printError("Cannot specify both reinitFilter and runIfTrnConv; only one can be true."); valid = False; } // // Only try to open the interface if it is needed, as determined by the // argument list. Do it here rather than in the constructor because the // "attributes" aren't yet parsed in the constructor. if( (_reinitFilter || _runIfTrnConv || _termIfTrnConv) && !_terrainAid ) { try { _terrainAid = new TerrainAidIF("TerrainAidIFServer"); Syslog::write("WaypointBottom - Opened TerrainAidIFServer %d.", _terrainAidIFCnt); _terrainAidIFCnt++; } catch (Exception e) { Syslog::write("WaypointBottom - %s", e.msg); _terrainAid = NULL; } catch (...) { _terrainAid = NULL; Syslog::write("WaypointBottom - Could not open TerrainAidIFServer."); } } return valid; }