/// <summary>
///
/// </summary>
/// <param name="ddg"></param>
/// <param name="speaker"></param>
/// <param name="trackFileName">can be null, for a saved track</param>
public BehaviorRouteFollowing(IDriveGeometry ddg, ISpeaker speaker, string trackFileName)
: base(ddg)
{
this.speaker = speaker;
BehaviorActivateCondition = bd =>
{
return nextWp != null;
};
BehaviorDeactivateCondition = bd =>
{
return nextWp == null;
};
if (String.IsNullOrWhiteSpace(trackFileName))
{
speaker.Speak("Loading saved track");
try
{
missionTrack = null;
// Load stored waypoints:
// on the PC, from C:\Users\sergei\AppData\Local\Packages\RobotPluckyPackage_sjh4qacv6p1wm\LocalState\MyTrack.xml
// RPi: \\172.16.1.175\c$\Data\Users\DefaultAccount\AppData\Local\Packages\RobotPluckyPackage_sjh4qacv6p1wm\LocalState
Track track = SerializableStorage<Track>.Load(savedTrackFileName).Result;
if (track != null)
{
missionTrack = track;
speaker.Speak("Loaded file " + missionTrack.Count + " trackpoints");
}
}
catch (Exception ex)
{
speaker.Speak("could not load saved track file");
}
if(missionTrack == null)
{
speaker.Speak("failed to load saved track file");
missionTrack = new Track();
}
nextWp = missionTrack.nextTargetWp;
}
else
{
speaker.Speak("Loading file " + trackFileName);
missionTrack = new Track();
try
{
missionTrack.Init(trackFileName);
speaker.Speak("Loaded file " + missionTrack.Count + " trackpoints");
nextWp = missionTrack.nextTargetWp;
}
catch (Exception ex)
{
speaker.Speak("could not load planned track file");
}
}
}
/// <summary>
/// Computes goalBearingDegrees based on current location and the next trackpoint
/// </summary>
/// <returns></returns>
public override IEnumerator<ISubsumptionTask> Execute()
{
FiredOn = true;
while (!MustExit && !MustTerminate)
{
nextWp = missionTrack.nextTargetWp;
while (!MustActivate && !MustExit && !MustTerminate) // wait for activation
{
yield return RobotTask.Continue; // dormant state - no need to calculate
}
// we have next trackpoint to go to:
if (MustExit || MustTerminate)
{
break;
}
//int i = 0;
while (!MustDeactivate && !MustExit && !MustTerminate)
{
nextWp = missionTrack.nextTargetWp;
if(nextWp == null)
{
break;
}
Direction dirToWp = nextWp.directionToWp(behaviorData.robotPose.geoPosition, behaviorData.robotPose.direction);
Distance distToWp = nextWp.distanceToWp(behaviorData.robotPose.geoPosition);
if (distToWp.Meters < 2.0d)
{
nextWp.trackpointState = TrackpointState.Passed; // will be ignored on the next cycle
speaker.Speak("Waypoint " + nextWp.number + " passed");
}
else if (nextWp.estimatedTimeOfArrival.HasValue
&& (DateTime.Now - nextWp.estimatedTimeOfArrival.Value).TotalSeconds > WAYPOINT_CANTREACH_SECONDS)
{
nextWp.trackpointState = TrackpointState.CouldNotReach; // will be ignored on the next cycle
speaker.Speak("Waypoint " + nextWp.number + " could not reach");
}
else
{
goalBearingRelativeDegrees = dirToWp.bearingRelative.Value; // update robotState
goalDistanceMeters = distToWp.Meters;
Debug.WriteLine(string.Format("IP: Trackpoint {0} distToWp.Meters= {1}", nextWp.number, distToWp.Meters));
if (!nextWp.estimatedTimeOfArrival.HasValue)
{
nextWp.trackpointState = TrackpointState.SelectedAsTarget;
double maxVelocityMSec = ToVelocity(behaviorData.robotState.powerLevelPercent); // m/sec
double timeToReachSec = distToWp.Meters / maxVelocityMSec;
nextWp.estimatedTimeOfArrival = DateTime.Now.AddSeconds(timeToReachSec);
speaker.Speak("Next trackpoint " + Math.Round(distToWp.Meters) + " meters away. I expect reaching it in " + Math.Round(timeToReachSec) + " seconds");
}
}
yield return RobotTask.Continue;
}
speaker.Speak("No more trackpoints");
goalBearingRelativeDegrees = null; // BehaviorGoToAngle will stop the robot
goalDistanceMeters = null;
}
FiredOn = false;
Debug.WriteLine("BehaviorRouteFollowing: " + (MustExit ? "MustExit" : "completed"));
yield break;
}
