/// <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); }