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);
↧