/// <summary>
/// takes current state/pose, new sensors data, odometry and everything else - and evaluates new state and pose
/// </summary>
/// <param name="behaviorData"></param>
public void EvaluatePoseAndState(IBehaviorData behaviorData, IDrive driveController)
{
// consider compass or AHRS reading for determining direction:
if(behaviorData.sensorsData.CompassHeadingDegrees != null)
{
behaviorData.robotPose.direction.heading = behaviorData.sensorsData.CompassHeadingDegrees;
}
// use GPS data to update position:
if (behaviorData.sensorsData.GpsFixType != GpsFixTypes.None)
{
behaviorData.robotPose.geoPosition.Lat = behaviorData.sensorsData.GpsLatitude;
behaviorData.robotPose.geoPosition.Lng = behaviorData.sensorsData.GpsLongitude;
}
// utilize odometry data:
if (driveController.hardwareBrickOdometry != null)
{
// already calculated odometry comes from the hardware brick (i.e. Arduino)
IOdometry odom = driveController.hardwareBrickOdometry;
behaviorData.robotPose.XMeters = odom.XMeters;
behaviorData.robotPose.YMeters = odom.YMeters;
behaviorData.robotPose.ThetaRadians = odom.ThetaRadians;
}
else
{
// we have wheels encoders data and must calculate odometry here:
long[] encoderTicks = new long[] { behaviorData.sensorsData.WheelEncoderLeftTicks, behaviorData.sensorsData.WheelEncoderRightTicks };
driveController.OdometryCompute(behaviorData.robotPose, encoderTicks);
}
}
/// <summary>
/// called often, must return promptly. It is a thread safe function.
/// </summary>
public override void Process()
{
StartedLoop();
// every cycle we create new behaviorData, populating it with existing data objects:
IBehaviorData behaviorData = new BehaviorData()
{
sensorsData = this.currentSensorsData,
robotState = this.robotState,
robotPose = this.robotPose
};
// use sensor data to update robotPose:
robotSlam.EvaluatePoseAndState(behaviorData, driveController);
// populate all behaviors with current behaviorData:
foreach (ISubsumptionTask task in subsumptionTaskDispatcher.Tasks)
{
BehaviorBase behavior = task as BehaviorBase;
if (behavior != null)
{
behavior.behaviorData = behaviorData;
}
}
subsumptionTaskDispatcher.Process(); // calls behaviors, which take sensor outputs, and may compute drive inputs
// look at ActiveTasksCount - it is an indicator of behaviors completed or removed. Zero count means we may need new behaviors combo.
MonitorDispatcherActivity();
// when active behavior is waiting (yielding), no action items are computed.
// otherwise driveInputs will be non-null:
if (behaviorData.driveInputs != null)
{
//Debug.WriteLine("ShortyTheRobot: Process() - have drive inputs V=" + behaviorData.driveInputs.velocity + " Omega=" + behaviorData.driveInputs.omega);
driveController.driveInputs = behaviorData.driveInputs;
driveController.Drive(); // apply driveInputs to motors
robotState.velocity = behaviorData.driveInputs.velocity;
robotState.omega = behaviorData.driveInputs.omega;
}
this.BehaviorData = behaviorData; // for tracing
sensorsController.Process(); // let sensorsController do maintenance
EndingLoop();
}
/// <summary>
/// takes current state/pose, new sensors data, odometry and everything else - and evaluates new state and pose
/// </summary>
/// <param name="behaviorData"></param>
public void EvaluatePoseAndState(IBehaviorData behaviorData, IDrive driveController)
{
bool wasHeadingSetByCompass = false;
IDisplacement displacement = null;
// consider compass or AHRS reading for determining direction:
//if (!useOdometryHeading && behaviorData.sensorsData.CompassHeadingDegrees != null)
//{
// behaviorData.robotPose.direction.heading = behaviorData.sensorsData.CompassHeadingDegrees;
// wasHeadingSetByCompass = true;
//}
// use GPS data to update position:
if (behaviorData.sensorsData.GpsFixType != GpsFixTypes.None)
{
// we need to know displacement in meters:
displacement = new GeoPosition(behaviorData.sensorsData.GpsLongitude, behaviorData.sensorsData.GpsLatitude) - behaviorData.robotPose.geoPosition;
behaviorData.robotPose.geoPosition.Lat = behaviorData.sensorsData.GpsLatitude;
behaviorData.robotPose.geoPosition.Lng = behaviorData.sensorsData.GpsLongitude;
}
else
{
// utilize odometry data
// we might have wheels encoders data - it will be ignored if using hardwareBrickOdometry:
long[] encoderTicks = new long[] { behaviorData.sensorsData.WheelEncoderLeftTicks, behaviorData.sensorsData.WheelEncoderRightTicks };
// compute dXMeters dYMeters dThetaRadians:
displacement = driveController.OdometryCompute(behaviorData.robotPose, encoderTicks);
}
if (displacement != null)
{
// update robotPose: XMeters YMeters, Lat, Lng, ThetaRadians, heading:
behaviorData.robotPose.translate(displacement.dXMeters, displacement.dYMeters);
behaviorData.robotPose.rotate(displacement.dThetaRadians);
}
if (!wasHeadingSetByCompass)
{
// rotate according to odometry:
SetCurrentHeadingByTheta(behaviorData);
}
}
private void SetCurrentHeadingByTheta(IBehaviorData behaviorData)
{
// XMeters and Lng axes are "horizontal right pointed". YMeters and Lat are "vertical up". North (0 degrees) is up.
// Theta 0 is along the X axis, so we need a 90 degrees offset here.
// Positive Theta is counterclockwise, positive heading - clockwise. That's why the minus sign.
double currentHeading = DirectionMath.toDegrees(-behaviorData.robotPose.ThetaRadians + Math.PI / 2);
if (behaviorData.robotPose.direction != null)
{
behaviorData.robotPose.direction.heading = currentHeading;
}
else
{
behaviorData.robotPose.direction = new Direction()
{
heading = currentHeading,
bearing = behaviorData.robotPose.direction == null ? null : behaviorData.robotPose.direction.bearing
};
}
}
public HomeController(IStudentData studentData, IBehaviorData behaviorData)
{
_studentData = studentData;
_behaviorData = behaviorData;
}
/// <summary>
/// determines if an obstacle is too close and comes up with an escape recommendation
/// </summary>
/// <param name="behaviorData"></param>
/// <param name="escapeRecommendation">output</param>
/// <returns></returns>
protected override bool TooClose(IBehaviorData behaviorData)
{
escapeRecommendation = string.Empty;
ISensorsData sensorsData = behaviorData.sensorsData;
double velocity = behaviorData.driveInputs.velocity; // behaviorData.driveInputs guaranteed not null
double adjustedTresholdStopMeters = Math.Max(Math.Min(Math.Abs(tresholdStopMeters * velocity / 0.2d), 0.4d), 0.15d); // limit 0.15 ... 0.4 meters depending on velocity
if (velocity > 0.0d && (sensorsData.IrFrontMeters <= tresholdIrStopMeters || sensorsData.RangerFrontLeftMeters <= adjustedTresholdStopMeters || sensorsData.RangerFrontRightMeters <= adjustedTresholdStopMeters))
{
bool leftSideFree = sensorsData.RangerFrontLeftMeters > adjustedTresholdStopMeters && sensorsData.IrLeftMeters > adjustedTresholdStopMeters; // IR left and right - 10..80 cm
bool rightSideFree = sensorsData.RangerFrontRightMeters > adjustedTresholdStopMeters && sensorsData.IrRightMeters > adjustedTresholdStopMeters;
if (!leftSideFree)
{
escapeRecommendation = "EscapeRight";
}
else if (!rightSideFree)
{
escapeRecommendation = "EscapeLeft";
}
else
{
escapeRecommendation = "Escape"; // advise random turn
}
return true; // activate stop, with escape recommendation
}
if (velocity < 0.0d && (sensorsData.IrRearMeters < adjustedTresholdStopMeters))
{
escapeRecommendation = "EscapeForward";
return true; // activate stop, with escape recommendation
}
//if (velocity == 0.0d && behaviorData.driveInputs.omega == 0.0d)
//{
// bool leftSideFree = sensorsData.IrLeftMeters > adjustedTresholdStopMeters;
// bool rightSideFree = sensorsData.IrRightMeters > adjustedTresholdStopMeters;
// bool rearFree = sensorsData.IrRearMeters > adjustedTresholdStopMeters;
// if (leftSideFree)
// {
// escapeRecommendation = "EscapeLeftTurn";
// }
// else if (rightSideFree)
// {
// escapeRecommendation = "EscapeRightTurn";
// }
// else if (rearFree)
// {
// escapeRecommendation = "EscapeFullTurn";
// }
// else
// {
// escapeRecommendation = "EscapeNone";
// }
// return true; // activate stop, with escape recommendation
//}
return false; // deactivate, no stopping and no escape recommendation
}
/// <summary> /// determines if an obstacle is too close and comes up with an escape recommendation /// </summary> /// <param name="behaviorData"></param> /// <returns></returns> protected abstract bool TooClose(IBehaviorData behaviorData);
/// <summary>
/// determines if an obstacle is too close and comes up with an escape recommendation
/// </summary>
/// <param name="behaviorData"></param>
/// <param name="escapeRecommendation">output</param>
/// <returns></returns>
protected override bool TooClose(IBehaviorData behaviorData)
{
escapeRecommendation = string.Empty;
ISensorsData sensorsData = behaviorData.sensorsData;
double velocity = behaviorData.driveInputs.velocity; // behaviorData.driveInputs guaranteed not null
double adjustedTresholdStopMeters = Math.Max(Math.Min(Math.Abs(tresholdStopMeters * velocity / 0.2d), 0.4d), 0.15d); // limit 0.15 ... 0.4 meters depending on velocity
if (velocity > 0.0d && (sensorsData.IrFrontMeters <= tresholdIrStopMeters || sensorsData.RangerFrontLeftMeters <= adjustedTresholdStopMeters || sensorsData.RangerFrontRightMeters <= adjustedTresholdStopMeters))
{
bool leftSideFree = sensorsData.RangerFrontLeftMeters > adjustedTresholdStopMeters && sensorsData.IrLeftMeters > adjustedTresholdStopMeters; // IR left and right - 10..80 cm
bool rightSideFree = sensorsData.RangerFrontRightMeters > adjustedTresholdStopMeters && sensorsData.IrRightMeters > adjustedTresholdStopMeters;
if (!leftSideFree)
{
escapeRecommendation = "EscapeRight";
}
else if (!rightSideFree)
{
escapeRecommendation = "EscapeLeft";
}
else
{
escapeRecommendation = "Escape"; // advise random turn
}
return(true); // activate stop, with escape recommendation
}
if (velocity < 0.0d && (sensorsData.IrRearMeters < adjustedTresholdStopMeters))
{
escapeRecommendation = "EscapeForward";
return(true); // activate stop, with escape recommendation
}
//if (velocity == 0.0d && behaviorData.driveInputs.omega == 0.0d)
//{
// bool leftSideFree = sensorsData.IrLeftMeters > adjustedTresholdStopMeters;
// bool rightSideFree = sensorsData.IrRightMeters > adjustedTresholdStopMeters;
// bool rearFree = sensorsData.IrRearMeters > adjustedTresholdStopMeters;
// if (leftSideFree)
// {
// escapeRecommendation = "EscapeLeftTurn";
// }
// else if (rightSideFree)
// {
// escapeRecommendation = "EscapeRightTurn";
// }
// else if (rearFree)
// {
// escapeRecommendation = "EscapeFullTurn";
// }
// else
// {
// escapeRecommendation = "EscapeNone";
// }
// return true; // activate stop, with escape recommendation
//}
return(false); // deactivate, no stopping and no escape recommendation
}
/// <summary>
/// called often, must return promptly. It is a thread safe function.
/// </summary>
public override void Process()
{
StartedLoop();
// every cycle we create new behaviorData, populating it with existing data objects:
IBehaviorData behaviorData = new BehaviorData()
{
sensorsData = this.currentSensorsData,
robotState = this.robotState,
robotPose = this.robotPose
};
// use sensor data to update robotPose:
robotSlam.EvaluatePoseAndState(behaviorData, driveController);
// populate all behaviors with current behaviorData:
foreach (ISubsumptionTask task in subsumptionTaskDispatcher.Tasks)
{
BehaviorBase behavior = task as BehaviorBase;
if (behavior != null)
{
behavior.behaviorData = behaviorData;
}
}
subsumptionTaskDispatcher.Process(); // calls behaviors, which take sensor outputs, and may compute drive inputs
// look at ActiveTasksCount - it is an indicator of behaviors completed or removed. Zero count means we may need new behaviors combo.
MonitorDispatcherActivity();
// when active behavior is waiting (yielding), no action items are computed.
// otherwise driveInputs will be non-null:
if (behaviorData.driveInputs != null)
{
//Debug.WriteLine("ShortyTheRobot: Process() - have drive inputs V=" + behaviorData.driveInputs.velocity + " Omega=" + behaviorData.driveInputs.omega);
driveController.driveInputs = behaviorData.driveInputs;
driveController.Drive(); // apply driveInputs to motors
robotState.velocity = behaviorData.driveInputs.velocity;
robotState.omega = behaviorData.driveInputs.omega;
}
this.BehaviorData = behaviorData; // for tracing
sensorsController.Process(); // let sensorsController do maintenance
EndingLoop();
}
/// <summary> /// determines if an obstacle is too close and comes up with an escape recommendation /// </summary> /// <param name="behaviorData"></param> /// <returns></returns> protected abstract bool TooClose(IBehaviorData behaviorData);
