odometery problem

edit

#include <iostream> #include <stdio.h> #include <string.h> #include <math.h> #include "roboteq_motor/RoboteqDevice.h" #include "roboteq_motor/ErrorCodes.h" #include "roboteq_motor/Constants.h" #include <unistd.h> #include <ros ros.h=""> #include <std_msgs string.h=""> #include <ros ros.h=""> #include <tf transform_broadcaster.h=""> #include <nav_msgs odometry.h="">

using namespace std; int status; RoboteqDevice device;

int main(int argc, char *argv[]) {

ros::init(argc, argv, "hello"); ros::NodeHandle nh; string response = ""; status = device.Connect("/dev/ttyACM0"); if(status != RQ_SUCCESS) { cout<<"Error connecting to device: "<<status&lt;&lt;"."&lt;<endl; return="" 1;="" }<="" p="">

//Wait 10 ms before sending another command to device

int encoder1; int encoder2; double distance; ros::Publisher odom_pub = nh.advertise<nav_msgs::odometry>("odom", 10); tf::TransformBroadcaster odom_broadcaster; double x = 0.0; double y = 0.0; double th = 0.0; ros::Time current_time, last_time; current_time = ros::Time::now(); last_time = ros::Time::now(); ros::Rate r(100);

//first, we'll publish the transform over tf geometry_msgs::TransformStamped odom_trans; odom_trans.header.frame_id = "odom"; odom_trans.child_frame_id = "base_link";

while(nh.ok()){

current_time = ros::Time::now();

device.SetConfig(_EMOD, 1); device.SetConfig(_EMOD, 2); device.SetConfig(_EPPR, 1, 200); device.SetConfig(_EPPR, 2, 200); device.SetConfig(_MXPR, 1, 83); device.SetConfig(_MXPF, 1, 83); device.SetCommand(_GO, 1, 100); device.SetCommand(_GO, 2, -100); device.GetValue(_ABCNTR, 1, encoder1); device.GetValue(_ABCNTR, 2, encoder2); encoder1 /= 982 / -2.66; encoder2 /= 966 / 2.66;

cout<< encoder1 << " * "; cout<< encoder2;

distance= (encoder1 + encoder2) / 2; th= ((encoder1 - encoder2 ) / 36 ); x= distance * sin(th); y= distance * cos(th); cout<<" ("<<x&lt;&lt;"-"&lt;<y&lt;&lt;"-"&lt;<th&lt;&lt;") ";<="" p="">

geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

odom_trans.header.stamp = current_time;
odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = 0.0;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;

//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";

//set the position
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;

//set the velocity

odom.child_frame_id = "base_link";

last_time = current_time;

//send the transform
odom_broadcaster.sendTransform(odom_trans);
//publish the message
odom_pub.publish(odom);

r.sleep();

} device.Disconnect();

return 0;

}

hello, this is my odometer code.. i have very weird results in the rviz . can someone explain why I got those weird results ?

Thanks in advance