/// <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);
}
}
/// <summary>
/// calculates robot pose change based on current wheel encoders ticks
/// </summary>
/// <param name="robotPose">Used to retrieve current position only. Will be adjusted in SLAM based on wheels travel.</param>
/// <param name="encoderTicks">raw data - wheel encoder ticks - left, right... - ignored if using hardwareBrickOdometry</param>
/// <returns>Displacement - to be applied in SLAM module. Can return null if there is no displacement</returns>
public IDisplacement OdometryCompute(IRobotPose robotPose, long[] encoderTicks)
{
IDisplacement ret = null; // null will be returned if there is no displacement
if (hardwareBrickOdometry != null)
{
// already calculated odometry comes from the hardware brick (i.e. Arduino)
double dx = hardwareBrickOdometry.XMeters - robotPose.XMeters;
double dy = hardwareBrickOdometry.YMeters - robotPose.YMeters;
double dz = 0.0d;
double dth = hardwareBrickOdometry.ThetaRadians - robotPose.ThetaRadians;
if (dx != 0.0d || dy != 0.0d || dz != 0.0d || dth != 0.0d)
{
ret = new Displacement(dx, dy, dz, dth);
}
}
else
{
// we have wheels encoders data and must calculate odometry here:
DisplacementOdometry disp = odometry.Process(encoderTicks);
if (disp.halfPhi != 0.0d || disp.dCenter != 0.0d)
{
double thetaMid = robotPose.ThetaRadians + disp.halfPhi; // radians in the middle of the turn
// calculate displacement in the middle of the turn:
double dx = disp.dCenter * Math.Cos(thetaMid); // meters
double dy = disp.dCenter * Math.Sin(thetaMid); // meters
double dz = 0.0d;
double dThetaRadians = disp.halfPhi * 2.0d; // actual turn
ret = new Displacement(dx, dy, dz, dThetaRadians);
}
}
return(ret);
}
