getting x, y, theta from wheel encoder values for P3DX robot
I am using getLeftEncoder() and getRightEnoder() functions from Aria library. i am publishing left and right encoder values using custom message. I calculated x, y and theta. The robot has to follow 2*2 square grid for 5 times. The plot i am getting is not desired and kind of random. can anybody tell me where am i getting wrong?
Below is the code i have used-
#include "ros/ros.h"
#include "std_msgs/Float64.h"
#include "rosaria/custom_encoder.h"
#include <iostream>
#include <fstream>
#include <math.h>
using namespace std;
float b_actual = 0.3226;
float Cm = 0.0000078125; /*(units = meters/pulse)*/
float Cl = 1.0000;
float Cr = 1.0000;
float thetaold = 0;
float xold = 0;
float yold = 0;
float _previousEnL = 0;
float _previousEnR = 0;
float dU, EnL, EnR, dUL, dUR, dtheta, x, y, theta, _currentEnL, _currentEnR;
void chatterCallback(const rosaria::custom_encoder::ConstPtr& msg)
{
_currentEnL = msg->Left_Encoder;
_currentEnR = msg->Right_Encoder;
EnL = _currentEnL - _previousEnL;
EnR = _currentEnR - _previousEnR;
ROS_INFO("left encoder:[%f]:", EnL);
ROS_INFO("right encoder:[%f]:", EnR);
dUL = Cl * Cm * EnL;
dUR = Cr * Cm * EnR;
dU = (dUL + dUR)/2;
dtheta = (dUL - dUR)/b_actual;
theta = thetaold + dtheta;
x = xold + dU * cos (theta);
y = yold + dU * sin (theta);
thetaold = theta;
xold = x;
yold = y;
_previousEnL = _currentEnL;
_previousEnR = _currentEnR;
ofstream People("mr.txt", std::ios_base::app);
People <<x<< ", "<<y<< "," <<theta<<std::endl;
ROS_INFO("Modified wheel Pose: (%f, %f, %f)", x, y, theta);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "subs_pose");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("/RosAria/custom_Encoder", 1000, chatterCallback);
ros::spin();
return 0;
}