USARBot.OdometrySensor

class OdometrySensor extends sensor config (USARBot);

/*
 * Wheel encoder based odometry used for skid-steer vehicles.
 *
 * It assumes that the tire distribution is the same for the left and right side. 
 * Data are represented in the vehicle coordinate (SAE J670) in the format (x,y,theta)!
 * The odometry erros come from 1) Encoder error from sensor resolution, 2) Wheel 
 * radius error in measure, and 3) Theta calculation error.
 */
const RadianToUU   = 10430.3783505;

var config float ScanInterval;
var config float EncoderResolution;
var config string LeftTire, RightTire;

var USARRemoteBot rBot;
var KTire lTire, rTire;
var string odometryData;
var vector old_left, old_right, old_pos;
var float base_width, wheel_radius, pLenUnit, lenUnit;
var float ltDis, rtDis;
var vector pos;

var bool bGetRadius;
var float last_radius;

simulated function PostNetBeginPlay()
{
    Super.PostNetBeginPlay();
    odometryData="";
}

function ConvertParam(USARConverter converter)
{
    // convert EncoderResolution to radian. If we are using SI units, 
    // the following is totally unnecessary! 
    EncoderResolution = converter.AngleToUU(EncoderResolution)*0.000095873799;
    if (ScanInterval>=0.1)
        SetTimer(ScanInterval,true);
}

// Find the leftest and rightest tire
function FindTires()
{
    local KRobot kr;
    local float maxOff, minOff, myOff;
    local int lIdx,rIdx,i,j,k;
    local vector RotX, RotY, RotZ;
    local Name pName;
    local int round;

    if (!Platform.IsA('KRobot')) {
        log("Not KRobot!");
        SetTimer(0,false);
        return;
    }
    kr = KRobot(Platform);

    //Try to find the tires
    lIdx = -1;
    rIdx = -1;
    if (LeftTire=="" || RightTire=="") {
        for (i=0;i<kr.JointParts.length;i++) {
            if (ClassIsChildOf(kr.JointParts[i].PartClass,class'KTire')) {
                myOff = 0;
                
                j = i;
                while (kr.JointParts[j].Parent!='' && kr.JointParts[j].Parent!='None') {
                    myOff += kr.JointParts[j].ParentPos.Y - kr.JointParts[j].SelfPos.Y;
                    pName = kr.JointParts[j].Parent;
                    for (k=0;k<kr.JointParts.length;k++)
                        if (kr.JointParts[k].PartName == pName) {
                            j=k;
                            break;
                        }
                    if (k==kr.JointParts.length)
                        break;
                }

                myOff += kr.JointParts[j].ParentPos.Y - kr.JointParts[j].SelfPos.Y;
                if (RightTire=="" && maxOff<myOff) {
                    maxOff = myOff;
                    rIdx = i;
                }
                if (LeftTire=="" && minOff>myOff) {
                    minOff = myOff;
                    lIdx = i;
                }
            }
        }
        if (rIdx>=0)
            log("Odometry: Use "$kr.JointParts[rIdx].PartName$" as Right wheel.");
        if (lIdx>=0)
            log("Odometry: Use "$kr.JointParts[lIdx].PartName$" as Left wheel.");
    }
    if (LeftTire!="")
        lIdx = kr.FindJointPartId(LeftTire);
    if (lIdx<0) {
        log("Invalid LeftTire Name"@LeftTire);
        SetTimer(0,false);
        return;
    }
    lTire = KTire(kr.Parts[lIdx]);
    if (RightTire!="")
        rIdx = kr.FindJointPartId(RightTire);
    if (rIdx<0) {
        log("Invalid RightTire Name"@LeftTire);
        SetTimer(0,false);
        return;
    }
    rTire = KTire(kr.Parts[rIdx]);
    
    GetAxes(Platform.Rotation,RotX,RotY,RotZ);
    base_width = Abs((lTire.Location - rTire.Location) Dot RotY);
    round = int(converter.lengthToUU(1)/base_width)+1;
    base_width = int(base_width*round)/round;
    
    old_left = lTire.Location;
    old_right = rTire.Location;
    
    //log(base_width@rIdx@lIdx@rTire.Location@lTire.location@RotY);
}

// calculate the wheel radius
function FindRadius()
{
    local vector RotX, RotY, RotZ;
    local vector HitLocation,HitNormal;
    local int round;
    
    if (rTire==None || lTire==None) {
        log("Can't find tires");
        return;
    }
    if (!rTire.bTireOnGround || !lTire.bTireOnGround) {
        log("Left tire on ground is"@rTire.bTireOnGround@" Right tire on ground is"@lTire.bTireOnGround);
        return;
    }

    GetAxes(Platform.Rotation,RotX,RotY,RotZ);
    if (Trace(HitLocation, HitNormal, rTire.Location-100*RotZ, rTire.Location, true)==NONE) {
        log("Can't find the ground!");
        SetTimer(0,false);
        return;
    }
    wheel_radius = VSize(rTire.Location-HitLocation);
    pLenUnit = wheel_radius*EncoderResolution; // used for compute ticks
    round = int(converter.lengthToUU(2)/wheel_radius)+1;
    lenUnit = int(wheel_radius*round)*EncoderResolution/round; // used for calculate the len

    bGetRadius = last_radius!=0 && abs(last_radius - wheel_radius)<0.005;
    if (last_radius==0) {
        old_left = lTire.Location;
        old_right = rTire.Location;
    }
    last_radius = wheel_radius;
    //log(base_width@wheel_radius@pLenUnit@lenUnit@round);
}

function timer()
{
    local vector lVec, rVec;
    local vector RotX, RotY, RotZ;
    local float distance, lDis, rDis, diff, delta;
    
    // try to find the tires untill we really get them
    if (lTire==None || rTire==None) FindTires(); 
    // try to find the radius untill we get stable value
    if (!bGetRadius) FindRadius();
    if (pLenUnit==0) return;
    
    // The following simulates using encoder to calculate the wheel's movement
    lVec = lTire.Location - old_left;
    rVec = rTire.Location - old_right;
    GetAxes(Platform.Rotation,RotX,RotY,RotZ);
    if ((lVec Dot RotX)>0)
        lDis = VSize(lVec);
    else
        lDis = -VSize(lVec);
    if ((rVec Dot RotX)>0)
        rDis = VSize(rVec);
    else
        rDis = -VSize(rVec);
    // total tick
    ltDis += lDis;
    rtDis += rDis;
    
    // (current_tick-last_tick)*pLenUnit
    lDis = (int(ltDis/pLenUnit)-int((ltDis-lDis)/pLenUnit))*lenUnit; 
    rDis = (int(rtDis/pLenUnit)-int((rtDis-rDis)/pLenUnit))*lenUnit;
    
    // The following does the odometry calculation
    distance = (lDis + rDis)/2;
    pos.x = old_pos.x + distance*Cos(old_pos.z);
    pos.y = old_pos.y + distance*Sin(old_pos.z);
    
    diff = lDis - rDis;
    if (abs(diff)>0.001)
        delta = Atan(diff,base_width);
    else
        delta = 0;
    pos.z = old_pos.z + delta; // theta in radian
    if (pos.z > Pi) pos.z -= 2*Pi;
    if (pos.z < -Pi) pos.z += 2*Pi;

    // store these data for the next calculation
    old_pos = pos;
    old_left = lTire.Location;
    old_right = rTire.Location;
    
    // generate the data string
    if (converter!=None) {
        odometryData="{Pose "$converter.Str_LengthFromUU(pos.x)
                             $","$converter.Str_LengthFromUU(pos.y)
                             $","$converter.Str_AngleFromUU(pos.z*RadianToUU)$"}";
    } else
        odometryData="{Pose "$pos$"}";
}

function string Set(String opcode, String args)
{
    if(Caps(opcode)=="RESET") {
        pos = vect(0,0,0);
        old_pos = vect(0,0,0);
        old_left = lTire.Location;
        old_right = rTire.Location;
        return "OK";
    }
    return "Failed";
}

function String GetData()
{
    local string outstring;

    if (odometryData == "") return "";

    outstring = "{Name "$ItemName$"} "$odometryData;
    odometryData = "";
    
    //log(Platform.Location@pos);
    return outstring;
}

function String GetConfData()
{
    local string outstring;
    outstring = Super.GetConfData();
    outstring = outstring@"{ScanInterval "$converter.FloatString(ScanInterval)$"}"
                         @"{EncoderResolution "$converter.FloatString(EncoderResolution)$"}";
    return outstring;
}

defaultproperties
{
//Scaled with 4.762 at Mon Sep 25 14:21:52 EDT 2006
    ItemType="Odometry"
    HiddenSensor=true
    ScanInterval=0.2
    EncoderResolution=0.01
        StaticMesh=StaticMesh'USARSim_VehicleParts_Meshes.Sensors.Sensor'
    Mass=0.1
    drawscale=0.4762
}