Simulate Odometry with Ackermann Locomotion

edit

Hello,

I am trying to simulate odometry in an ackermann vehicle with gazebo but the error that I get after twenty meters is about 4 meters. I think the result of the GetAngle method is strange when the robot is stopped, because in every cycle the number representing the angle of the wheel decreases. The code that i have is the following:

        double currentLeftposition = frontLeft->GetAngle(positionAxis).Radian(); 
        double currentRightPosition = frontRight->GetAngle(positionAxis).Radian(); 
        geometry_msgs::TransformStamped odom_trans;
        nav_msgs::Odometry odom; 
        math::Pose currentPose;
        math::Vector3 position;
        math::Quaternion rotation; 
        double xPos,yPos,orientation;
        double currentLinearVelocity=rear->GetVelocity(1);
        double twoPI=2*M_PI;
        double rearWheelRotationAngle=rear->GetAngle(positionAxis).Radian();
        double angleDiff=angles::shortest_angular_distance(prevRearWheelRotationAngle,rearWheelRotationAngle);
        prevRearWheelRotationAngle=rearWheelRotationAngle;
        double revolution=angleDiff/twoPI;
        double revolution=(currentLinearVelocity*stepTime.Double())/twoPI;
        double wheelCircuference=wheelDiameter*M_PI;
        double distanceTravelledByWheel=revolution*wheelCircuference;
        double steering=getMiddleWheelAngle(-currentLeftposition,-currentRightPosition); 
        double turningAngle=tan(steering)/distanceBetweenAxis;
        double previousOrientation=previousPose.rot.GetYaw();
        math::Vector3 previousPosition=previousPose.pos;
        if(fabs(turningAngle)<0.001) //assume that the car is going ahead
        {
            xPos= previousPosition.x + distanceTravelledByWheel*cos(previousOrientation);
            yPos= previousPosition.y + distanceTravelledByWheel*sin(previousOrientation);
            orientation=previousOrientation;
         }
         else
         {
            double delta_theta=turningAngle*distanceTravelledByWheel;
            double radius=1.0f/turningAngle;
            double cx=previousPosition.x - sin(previousOrientation)*radius;
            double cy=previousPosition.y + cos(previousOrientation)*radius;
            orientation = remainder(previousOrientation + delta_theta,twoPI);
             xPos=cx + sin(orientation)*radius;
             yPos=cy - cos(orientation)*radius;
          }
          position.x=xPos;
          position.y=yPos;
          position.z=0;
          /*std::ofstream log("/home/bernardo/odom.dat", std::ios_base::app | std::ios_base::out);
          log << xPos << "   " << yPos << "\n";*/
          rotation.SetFromEuler(0,0,orientation);