/// <summary>
/// compose behaviors and load them to dispatcher for execution
/// </summary>
/// <param name="compType"></param>
public void produce(BehaviorCompositionType compType)
{
Debug.WriteLine("BehaviorFactory: produce() : " + compType);
// close all running tasks before composing new behavior set:
subsumptionDispatcher.Close();
while (subsumptionDispatcher.ActiveTasksCount > 0)
{
subsumptionDispatcher.Process();
}
Debug.WriteLine("BehaviorFactory: produce() : all dispatcher tasks closed, creating new combo '" + compType + "'");
isFinishedFW = false;
switch (compType)
{
case BehaviorCompositionType.CruiseAndStop:
//dispatcher.Dispatch(new BehaviorCruise(driveGeometry) {
// name = "BehaviorCruise",
// speaker = this.speaker
//});
//dispatcher.Dispatch(new BehaviorGoToGoal(driveGeometry)
//{
// name = "BehaviorGoToGoal",
// speaker = this.speaker
//});
//dispatcher.Dispatch(new BehaviorGoToPixy(driveGeometry)
//{
// name = "BehaviorGoToPixy",
// speaker = this.speaker
//});
subsumptionDispatcher.Dispatch(new BehaviorGoToAngle(driveGeometry, 10.0d, 5.0d)
{
name = "BehaviorGoToAngle",
speaker = this.speaker
});
subsumptionDispatcher.Dispatch(new BehaviorAvoidObstacles(driveGeometry)
{
name = "BehaviorAvoidObstacles",
speaker = this.speaker
});
subsumptionDispatcher.Dispatch(new BehaviorFollowWall(driveGeometry)
{
name = "BehaviorFollowWall",
speaker = this.speaker,
cruiseSpeed = 15.0d,
avoidanceTurnFactor = 50.0d,
//distanceToWallMeters = 0.17d,
//distanceToWallMeters = 0.25d,
//BehaviorDeactivateCondition = bd => { return BehaviorBase.getCoordinatorData().EnablingRequest.StartsWith("Escape"); },
//BehaviorTerminateCondition = bd => { return bd.sensorsData.IrRearMeters < 0.2d; }
});
subsumptionDispatcher.Dispatch(new BehaviorStopPlucky()
{
name = "BehaviorStop",
speaker = this.speaker,
tresholdStopMeters = 0.4d
});
subsumptionDispatcher.Dispatch(new BehaviorBackAndTurn(driveGeometry)
{
name = "Escape",
speaker = this.speaker,
BehaviorActivateCondition = bd => { return(BehaviorBase.getCoordinatorData().EnablingRequest.StartsWith("Escape")); },
BehaviorDeactivateCondition = bd => { return(true); }, // deactivate after one cycle
BehaviorTerminateCondition = bd => { return(false); } // do not terminate
});
subsumptionDispatcher.Dispatch(new BehaviorAmIStuck()
{
name = "BehaviorAmIStuck",
speaker = this.speaker
});
BehaviorBase.getCoordinatorData().ClearEnablingRequest();
BehaviorBase.getCoordinatorData().ClearGrabbingBehavior();
break;
case BehaviorCompositionType.AroundTheBlock:
BehaviorFollowWall bfw = new BehaviorFollowWall(driveGeometry)
{
name = "BehaviorFollowWall",
speaker = this.speaker,
cruiseSpeed = 15.0d,
avoidanceTurnFactor = 50.0d,
distanceToWallMeters = 0.17d,
//BehaviorDeactivateCondition = bd => { return BehaviorBase.getCoordinatorData().EnablingRequest.StartsWith("Escape"); },
//BehaviorTerminateCondition = bd => { return bd.sensorsData.IrRearMeters < 0.2d; }
};
bfw.BehaviorDeactivateCondition = bd =>
{
isFinishedFW =
//(DateTime.Now - bfw.FiredOnTimestamp).TotalSeconds > 5.0d
(DateTime.Now - activatedFW).TotalSeconds > 5.0d
//&& Math.Abs(DirectionMath.to180(bd.sensorsData.CompassHeadingDegrees - initialCompassHeadingDegrees)) < 5.0d;
&& Math.Abs(bd.robotPose.XMeters) < 0.08d && // forward
Math.Abs(bd.robotPose.YMeters) < 0.25d; // sides
return(isFinishedFW);
};
bfw.BehaviorActivateCondition = bd => {
if (isFinishedFW)
{
return(false);
}
double irLeftMeters = bd.sensorsData.IrLeftMeters;
double sonarLeftMeters = bd.sensorsData.RangerFrontLeftMeters;
double irRightMeters = bd.sensorsData.IrRightMeters;
double sonarRightMeters = bd.sensorsData.RangerFrontRightMeters;
double activateDistanceToWallMeters = bfw.distanceToWallMeters * 1.5d;
if (irLeftMeters < activateDistanceToWallMeters || (irLeftMeters < bfw.distanceToWallMeters && sonarLeftMeters < bfw.distanceToWallMeters))
{
bfw.fireOnLeft = true;
}
if (irRightMeters < activateDistanceToWallMeters || (irRightMeters < bfw.distanceToWallMeters && sonarRightMeters < bfw.distanceToWallMeters))
{
bfw.fireOnRight = true;
}
if ((bfw.fireOnLeft || bfw.fireOnRight) && bd.sensorsData.CompassHeadingDegrees.HasValue)
{
// remember the initial CompassHeadingDegrees:
initialCompassHeadingDegrees = bd.sensorsData.CompassHeadingDegrees.Value;
activatedFW = DateTime.Now;
}
return(bfw.fireOnLeft || bfw.fireOnRight);
};
subsumptionDispatcher.Dispatch(bfw);
subsumptionDispatcher.Dispatch(new BehaviorStopPlucky()
{
name = "BehaviorStop",
speaker = this.speaker,
BehaviorActivateCondition = bd => { return(true); }
});
break;
case BehaviorCompositionType.ChaseColorBlob:
subsumptionDispatcher.Dispatch(new BehaviorGoToPixy(driveGeometry)
{
name = "BehaviorGoToPixy",
speaker = this.speaker
});
subsumptionDispatcher.Dispatch(new BehaviorGoToAngle(driveGeometry, 100.0d, 100.0d)
{
name = "BehaviorGoToAngle",
speaker = this.speaker
});
subsumptionDispatcher.Dispatch(new BehaviorStopPlucky()
{
name = "BehaviorStop",
speaker = this.speaker,
tresholdStopMeters = 0.8d,
//BehaviorActivateCondition = bd => { return bd.driveInputs != null && bd.sensorsData != null && TooClose(bd); }
//BehaviorActivateCondition = bd => { return false; }
});
//subsumptionDispatcher.Dispatch(new BehaviorBackAndTurn(driveGeometry)
//{
// name = "Escape",
// speaker = this.speaker,
// BehaviorActivateCondition = bd => { return BehaviorBase.getCoordinatorData().EnablingRequest.StartsWith("Escape"); },
// BehaviorDeactivateCondition = bd => { return true; }, // deactivate after one cycle
// BehaviorTerminateCondition = bd => { return false; } // do not terminate
//});
break;
case BehaviorCompositionType.RouteFollowing:
subsumptionDispatcher.Dispatch(new BehaviorRouteFollowing(driveGeometry, this.speaker, TrackFileName)
{
name = "BehaviorRouteFollowing"
});
subsumptionDispatcher.Dispatch(new BehaviorGoToAngle(driveGeometry, 100.0d, 100.0d)
{
name = "BehaviorGoToAngle",
speaker = this.speaker
});
subsumptionDispatcher.Dispatch(new BehaviorStopPlucky()
{
name = "BehaviorStop",
speaker = this.speaker,
tresholdStopMeters = 0.6d,
//BehaviorActivateCondition = bd => { return bd.driveInputs != null && bd.sensorsData != null && TooClose(bd); }
//BehaviorActivateCondition = bd => { return false; }
});
subsumptionDispatcher.Dispatch(new BehaviorBackAndTurn(driveGeometry)
{
name = "Escape",
speaker = this.speaker,
BehaviorActivateCondition = bd => { return(BehaviorBase.getCoordinatorData().EnablingRequest.StartsWith("Escape")); },
BehaviorDeactivateCondition = bd => { return(true); }, // deactivate after one cycle
BehaviorTerminateCondition = bd => { return(false); } // do not terminate
});
break;
case BehaviorCompositionType.JoystickAndStop:
subsumptionDispatcher.Dispatch(new BehaviorControlledByJoystick(driveGeometry)
{
name = "BehaviorControlledByJoystick",
speaker = this.speaker
});
//subsumptionDispatcher.Dispatch(new BehaviorAvoidObstacles(driveGeometry)
//{
// name = "BehaviorAvoidObstacles",
// speaker = this.speaker
//});
//subsumptionDispatcher.Dispatch(new BehaviorStop()
//{
// name = "BehaviorStop",
// speaker = this.speaker
//});
break;
case BehaviorCompositionType.Escape:
subsumptionDispatcher.Dispatch(new BehaviorBackAndTurn(driveGeometry)
{
name = "Escape",
speaker = this.speaker,
BehaviorActivateCondition = bd => { return(true); },
BehaviorDeactivateCondition = bd => { return(true); }, // deactivate after one cycle
// BehaviorTerminateCondition = new BehaviorTerminateConditionDelegate(delegate(IBehaviorData bd) { return true; })
BehaviorTerminateCondition = bd => { return(true); } // terminate after first drive-turn cycle
});
BehaviorBase.getCoordinatorData().EnablingRequest = "Escape"; // set it immediately to see the escape action
subsumptionDispatcher.Dispatch(new BehaviorStopPlucky()
{
name = "BehaviorStop",
speaker = this.speaker
});
break;
}
}
