private bool amIStuck(out string escapeRecommendation)
{
double commandedVelocity = behaviorData.driveInputs.velocity; // behaviorData.driveInputs guaranteed not null
double commandedOmega = behaviorData.driveInputs.omega;
escapeRecommendation = commandedVelocity > 0.0d ? "Escape" : "EscapeForward";
ISensorsData sensorsData = behaviorData.sensorsData;
if (Math.Abs(commandedVelocity) < 0.001 && Math.Abs(commandedOmega) < 0.001)
{
// no movement expected, keep resetting the measurement start:
last = DateTime.Now;
LeftTicksLast = sensorsData.WheelEncoderLeftTicks;
RightTicksLast = sensorsData.WheelEncoderRightTicks;
return(false);
}
bool ret = false;
if ((DateTime.Now - last).TotalSeconds >= stuckIntervalSeconds)
{
if (sensorsData.WheelEncoderLeftTicks == LeftTicksLast && sensorsData.WheelEncoderRightTicks == RightTicksLast)
{
ret = true;
}
last = DateTime.Now;
LeftTicksLast = sensorsData.WheelEncoderLeftTicks;
RightTicksLast = sensorsData.WheelEncoderRightTicks;
}
return(ret);
}
/// <summary>
/// makes a copy of the source, and fills the instance timestamp with current time.
/// preserves individual timestamps.
/// </summary>
/// <param name="src"></param>
public SensorsData(ISensorsData src)
: this()
{
this.RangerSensors = src.RangerSensors;
this.IrLeftMeters = src.IrLeftMeters;
this.IrLeftMetersTimestamp = src.IrLeftMetersTimestamp;
this.IrRightMeters = src.IrRightMeters;
this.IrRightMetersTimestamp = src.IrRightMetersTimestamp;
this.IrFrontMeters = src.IrFrontMeters;
this.IrFrontMetersTimestamp = src.IrFrontMetersTimestamp;
this.IrRearMeters = src.IrRearMeters;
this.IrRearMetersTimestamp = src.IrRearMetersTimestamp;
this.RangerFrontLeftMeters = src.RangerFrontLeftMeters;
this.RangerFrontLeftMetersTimestamp = src.RangerFrontLeftMetersTimestamp;
this.RangerFrontRightMeters = src.RangerFrontRightMeters;
this.RangerFrontRightMetersTimestamp = src.RangerFrontRightMetersTimestamp;
this.RangerRearLeftMeters = src.RangerRearLeftMeters;
this.RangerRearLeftMetersTimestamp = src.RangerRearLeftMetersTimestamp;
this.RangerRearRightMeters = src.RangerRearRightMeters;
this.RangerRearRightMetersTimestamp = src.RangerRearRightMetersTimestamp;
this.WheelEncoderLeftTicks = src.WheelEncoderLeftTicks;
this.WheelEncoderRightTicks = src.WheelEncoderRightTicks;
this.CompassHeadingDegrees = src.CompassHeadingDegrees;
this.GpsFixType = src.GpsFixType;
this.GpsLatitude = src.GpsLatitude;
this.GpsLongitude = src.GpsLongitude;
this.GpsAltitude = src.GpsAltitude;
this.GpsNsat = src.GpsNsat;
this.GpsHdop = src.GpsHdop;
this.FixAgeMs = src.FixAgeMs;
this.GpsTimeUTC = src.GpsTimeUTC;
this.GpsTimestamp = src.GpsTimestamp;
this.TargetingCameraBearingDegrees = src.TargetingCameraBearingDegrees;
this.TargetingCameraInclinationDegrees = src.TargetingCameraInclinationDegrees;
this.TargetingCameraTimestamp = src.TargetingCameraTimestamp;
this._BatteryVoltage = src.BatteryVoltage;
this.BatteryVoltageTimestamp = src.BatteryVoltageTimestamp;
//this.timestamp = src.timestamp;
}
public SensorsDataPlucky(ISensorsData src)
: base(src)
{
}
/// <summary>
/// makes a copy of the source, and fills the instance timestamp with current time.
/// preserves individual timestamps.
/// </summary>
/// <param name="src"></param>
public SensorsData(ISensorsData src)
: this()
{
this.RangerSensors = src.RangerSensors;
this.IrLeftMeters = src.IrLeftMeters;
this.IrLeftMetersTimestamp = src.IrLeftMetersTimestamp;
this.IrRightMeters = src.IrRightMeters;
this.IrRightMetersTimestamp = src.IrRightMetersTimestamp;
this.IrFrontMeters = src.IrFrontMeters;
this.IrFrontMetersTimestamp = src.IrFrontMetersTimestamp;
this.IrRearMeters = src.IrRearMeters;
this.IrRearMetersTimestamp = src.IrRearMetersTimestamp;
this.RangerFrontLeftMeters = src.RangerFrontLeftMeters;
this.RangerFrontLeftMetersTimestamp = src.RangerFrontLeftMetersTimestamp;
this.RangerFrontRightMeters = src.RangerFrontRightMeters;
this.RangerFrontRightMetersTimestamp = src.RangerFrontRightMetersTimestamp;
this.RangerRearLeftMeters = src.RangerRearLeftMeters;
this.RangerRearLeftMetersTimestamp = src.RangerRearLeftMetersTimestamp;
this.RangerRearRightMeters = src.RangerRearRightMeters;
this.RangerRearRightMetersTimestamp = src.RangerRearRightMetersTimestamp;
this.WheelEncoderLeftTicks = src.WheelEncoderLeftTicks;
this.WheelEncoderRightTicks = src.WheelEncoderRightTicks;
this.CompassHeadingDegrees = src.CompassHeadingDegrees;
this.GpsFixType = src.GpsFixType;
this.GpsLatitude = src.GpsLatitude;
this.GpsLongitude = src.GpsLongitude;
this.GpsAltitude = src.GpsAltitude;
this.GpsNsat = src.GpsNsat;
this.GpsHdop = src.GpsHdop;
this.FixAgeMs = src.FixAgeMs;
this.GpsTimeUTC = src.GpsTimeUTC;
this.GpsTimestamp = src.GpsTimestamp;
this.TargetingCameraBearingDegrees = src.TargetingCameraBearingDegrees;
this.TargetingCameraInclinationDegrees = src.TargetingCameraInclinationDegrees;
this.TargetingCameraTimestamp = src.TargetingCameraTimestamp;
this._BatteryVoltage = src.BatteryVoltage;
this.BatteryVoltageTimestamp = src.BatteryVoltageTimestamp;
//this.timestamp = src.timestamp;
}
/// <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
}
public void DisplayRobotSensors(ISensorsData sensorsData)
{
//Debug.WriteLine("RobotDashboard: DisplayRobotSensors() sensors: " + sensorsData.ToString());
sensorsDashboard1.SensorsDataBlock.Post(sensorsData);
/*
RobotPoseBase robotPose = new RobotPoseBase() { X = xLast, Y = yLast, Theta = thetaLast };
PoseBase obstaclePoseRel = new PoseBase() { X = 0.0d, Y = 0.0d, Theta = 0.0d }; // relative to sensor
double distanceMeters = sensorsData.IrLeftMeters; // range 10...80 cm
IRangerSensor sensor = sensorsData.RangerSensors["IrLeft"];
if (sensor.InRange(distanceMeters))
{
obstaclePoseRel.X = distanceMeters;
//PoseBase sensorPose = robotPose * irLeftSensor.Pose; // sensor Pose absolute
//PoseBase obstaclePose = sensorPose * obstaclePoseRel; // obstacle Pose absolute
PoseBase obstaclePose = robotPose * sensor.Pose * obstaclePoseRel; // obstacle Pose absolute
mainWindow1.AddMapable(new ObstacleData(obstaclePose.X, obstaclePose.Y, Colors.Magenta));
}
distanceMeters = sensorsData.IrRightMeters; // range 10...80 cm
sensor = sensorsData.RangerSensors["IrRight"];
if (sensor.InRange(distanceMeters))
{
obstaclePoseRel.X = distanceMeters;
PoseBase obstaclePose = robotPose * sensor.Pose * obstaclePoseRel; // obstacle Pose absolute
mainWindow1.AddMapable(new ObstacleData(obstaclePose.X, obstaclePose.Y, Colors.Green));
}
distanceMeters = sensorsData.IrFrontMeters; // range 10...80 cm
sensor = sensorsData.RangerSensors["IrFront"];
if (sensor.InRange(distanceMeters))
{
obstaclePoseRel.X = distanceMeters;
PoseBase obstaclePose = robotPose * sensor.Pose * obstaclePoseRel; // obstacle Pose absolute
mainWindow1.AddMapable(new ObstacleData(obstaclePose.X, obstaclePose.Y, Colors.Blue));
}
distanceMeters = sensorsData.IrRearMeters; // range 10...80 cm
sensor = sensorsData.RangerSensors["IrRear"];
if (sensor.InRange(distanceMeters))
{
obstaclePoseRel.X = distanceMeters;
PoseBase obstaclePose = robotPose * sensor.Pose * obstaclePoseRel; // obstacle Pose absolute
mainWindow1.AddMapable(new ObstacleData(obstaclePose.X, obstaclePose.Y, Colors.Blue));
}
distanceMeters = sensorsData.SonarLeftMeters; // range 3...250 cm
sensor = sensorsData.RangerSensors["SonarLeft"];
if (sensor.InRange(distanceMeters))
{
obstaclePoseRel.X = distanceMeters;
PoseBase obstaclePose = robotPose * sensor.Pose * obstaclePoseRel; // obstacle Pose absolute
mainWindow1.AddMapable(new ObstacleData(obstaclePose.X, obstaclePose.Y, Colors.Purple));
}
distanceMeters = sensorsData.SonarRightMeters; // range 3...250 cm
sensor = sensorsData.RangerSensors["SonarRight"];
if (sensor.InRange(distanceMeters))
{
obstaclePoseRel.X = distanceMeters;
PoseBase obstaclePose = robotPose * sensor.Pose * obstaclePoseRel; // obstacle Pose absolute
mainWindow1.AddMapable(new ObstacleData(obstaclePose.X, obstaclePose.Y, Colors.Purple));
}
*/
}
public SensorsDataShorty(ISensorsData src)
: base(src)
{
}
public void DisplayRobotSensors(ISensorsData sensorsData)
{
Debug.WriteLine("HttpServer: DisplayRobotSensors() sensors: " + sensorsData.ToString());
}
public void DisplayRobotSensors(ISensorsData sensorsData)
{
Debug.WriteLine("HttpServer: DisplayRobotSensors() sensors: " + sensorsData.ToString());
}
private bool computeAvoidanceOmega(ref double velocity, ref double avoidanceOmega)
{
bool avoid = false;
ISensorsData sensorsData = behaviorData.sensorsData;
double adjustedAvoidanceTresholdMeters = Math.Max(Math.Min(Math.Abs(avoidanceTresholdMeters * velocity / 0.2d), 0.6d), 0.20d); // limit 0.2 ... 0.6 meters depending on velocity
bool fronIrClose = sensorsData.IrFrontMeters <= tresholdIrAvoidanceMeters;
bool leftSonarClose = sensorsData.RangerFrontLeftMeters <= adjustedAvoidanceTresholdMeters;
bool rightSonarClose = sensorsData.RangerFrontRightMeters <= adjustedAvoidanceTresholdMeters;
bool leftIrClose = sensorsData.IrLeftMeters <= adjustedAvoidanceTresholdMeters;
bool rightIrClose = sensorsData.IrRightMeters <= adjustedAvoidanceTresholdMeters;
if (velocity > 0.0d &&
(fronIrClose || leftSonarClose || rightSonarClose || leftIrClose || rightIrClose))
{
bool leftSideFree = !leftSonarClose && !leftIrClose;
bool rightSideFree = !rightSonarClose && !rightIrClose;
if (!leftSideFree && !rightSideFree)
{
avoidanceOmega = avoidanceFactorOmega * Math.Sign(sensorsData.RangerFrontLeftMeters - sensorsData.RangerFrontRightMeters);
velocity *= avoidanceFactorVelocity;
avoid = true;
}
else if (!leftSideFree)
{
avoidanceOmega = -avoidanceFactorOmega; // weer to the right
velocity *= avoidanceFactorVelocity;
avoid = true;
}
else if (!rightSideFree)
{
avoidanceOmega = avoidanceFactorOmega; // weer to the left
velocity *= avoidanceFactorVelocity;
avoid = true;
}
else if (fronIrClose)
{
avoidanceOmega = avoidanceFactorOmega * Math.Sign(sensorsData.RangerFrontLeftMeters - sensorsData.RangerFrontRightMeters);
velocity *= -avoidanceFactorVelocity;
avoid = true;
}
}
if (!avoid)
{
if (lostObstacleLast == DateTime.MinValue)
{
lostObstacleLast = DateTime.Now;
}
double since = (DateTime.Now - lostObstacleLast).TotalSeconds;
double adjustmentInterval = 0.5d;
if (since < adjustmentInterval)
{
// keep avoiding for short time after losing the obstacle:
double factor = (adjustmentInterval - since) / adjustmentInterval; // 1..0 during the adjustment interval
avoidanceOmega = lastAvoidanceOmega * factor;
velocity *= (avoidanceFactorVelocity + (1.0d - avoidanceFactorVelocity) * (1.0d - factor)); // avoidanceFactorVelocity..1 during the adjustment interval
avoid = true;
}
}
else
{
lastAvoidanceOmega = avoidanceOmega;
lostObstacleLast = DateTime.MinValue;
}
return(avoid);
}
public void DisplayRobotSensors(ISensorsData sensorsData)
{
//Debug.WriteLine("RobotDashboard: DisplayRobotSensors() sensors: " + sensorsData.ToString());
sensorsDashboard1.SensorsDataBlock.Post(sensorsData);
/*
* RobotPoseBase robotPose = new RobotPoseBase() { X = xLast, Y = yLast, Theta = thetaLast };
* PoseBase obstaclePoseRel = new PoseBase() { X = 0.0d, Y = 0.0d, Theta = 0.0d }; // relative to sensor
*
* double distanceMeters = sensorsData.IrLeftMeters; // range 10...80 cm
* IRangerSensor sensor = sensorsData.RangerSensors["IrLeft"];
*
* if (sensor.InRange(distanceMeters))
* {
* obstaclePoseRel.X = distanceMeters;
* //PoseBase sensorPose = robotPose * irLeftSensor.Pose; // sensor Pose absolute
* //PoseBase obstaclePose = sensorPose * obstaclePoseRel; // obstacle Pose absolute
*
* PoseBase obstaclePose = robotPose * sensor.Pose * obstaclePoseRel; // obstacle Pose absolute
*
* mainWindow1.AddMapable(new ObstacleData(obstaclePose.X, obstaclePose.Y, Colors.Magenta));
* }
*
* distanceMeters = sensorsData.IrRightMeters; // range 10...80 cm
* sensor = sensorsData.RangerSensors["IrRight"];
* if (sensor.InRange(distanceMeters))
* {
* obstaclePoseRel.X = distanceMeters;
* PoseBase obstaclePose = robotPose * sensor.Pose * obstaclePoseRel; // obstacle Pose absolute
*
* mainWindow1.AddMapable(new ObstacleData(obstaclePose.X, obstaclePose.Y, Colors.Green));
* }
*
* distanceMeters = sensorsData.IrFrontMeters; // range 10...80 cm
* sensor = sensorsData.RangerSensors["IrFront"];
* if (sensor.InRange(distanceMeters))
* {
* obstaclePoseRel.X = distanceMeters;
* PoseBase obstaclePose = robotPose * sensor.Pose * obstaclePoseRel; // obstacle Pose absolute
*
* mainWindow1.AddMapable(new ObstacleData(obstaclePose.X, obstaclePose.Y, Colors.Blue));
* }
*
* distanceMeters = sensorsData.IrRearMeters; // range 10...80 cm
* sensor = sensorsData.RangerSensors["IrRear"];
* if (sensor.InRange(distanceMeters))
* {
* obstaclePoseRel.X = distanceMeters;
* PoseBase obstaclePose = robotPose * sensor.Pose * obstaclePoseRel; // obstacle Pose absolute
*
* mainWindow1.AddMapable(new ObstacleData(obstaclePose.X, obstaclePose.Y, Colors.Blue));
* }
*
* distanceMeters = sensorsData.SonarLeftMeters; // range 3...250 cm
* sensor = sensorsData.RangerSensors["SonarLeft"];
* if (sensor.InRange(distanceMeters))
* {
* obstaclePoseRel.X = distanceMeters;
* PoseBase obstaclePose = robotPose * sensor.Pose * obstaclePoseRel; // obstacle Pose absolute
*
* mainWindow1.AddMapable(new ObstacleData(obstaclePose.X, obstaclePose.Y, Colors.Purple));
* }
*
* distanceMeters = sensorsData.SonarRightMeters; // range 3...250 cm
* sensor = sensorsData.RangerSensors["SonarRight"];
* if (sensor.InRange(distanceMeters))
* {
* obstaclePoseRel.X = distanceMeters;
* PoseBase obstaclePose = robotPose * sensor.Pose * obstaclePoseRel; // obstacle Pose absolute
*
* mainWindow1.AddMapable(new ObstacleData(obstaclePose.X, obstaclePose.Y, Colors.Purple));
* }
*/
}
