Odometry Problem [closed]

asked 2014-08-11 07:59:58 -0600

955 ●27 ●35 ●55 http://ca.linkedin.com...

Hello folks,

I am making an odometry node for an omnidirectional robot using mechanum wheels. I've followed the tutorial on the ROS wiki, making changes as needed for my particular robot. The problem is that while I get values for the twist values, I get NaN for the pose. You can find the code here. I've tried debugging my code, and I get values up to the delta_th line. When I look at x, y, and th though that is where I get NaN. The variable types match, so that shouldn't be a problem. When I try adding a constant (1 and 1.0) instead of the delta then I do get a value returned. I am unsure why this is. Can anyone see what I am doing wrong?

Thanks,

edit retag flag offensive reopen merge delete

Closed for the following reason the question is answered, right answer was accepted by Icehawk101
close date 2014-08-11 10:21:10.587256

Comments

did you if dt_front, dt_rear are getting zero ?

bvbdort ( 2014-08-11 08:14:18 -0600 )edit

I checked that, they are getting values.

Icehawk101 ( 2014-08-11 08:20:05 -0600 )edit

Why not just add a lot of ROS_INFO_STREAM statements before and after every calculation until you find the culprit? Also, just from a "good practices" standpoint, you really should initialize the variables in lines 6 - 11 in your main function (before you subscribe to any callbacks).

Tom Moore ( 2014-08-11 08:30:08 -0600 )edit

dt_front: 0.999999 dt_rear: 0.999926 v_w1: 0.122492 v_w2: -0.119761 v_w3: 0.119651 v_w4: -0.117248 avg_dt: 0.999963 vx before: 0.000160434 vx after: 0.000160434 vy before: -0.0149735 vy after: -0.0149735 vth before: -2.32918e-05 vth after: -2.32918e-05

Icehawk101 ( 2014-08-11 09:21:16 -0600 )edit

delta_x: 0.000160428 delta_y: -0.0149729 delta_th: -2.32909e-05 x before: -nan x after: -nan y before: -nan y after: -nan th before: nan th after: nan

Icehawk101 ( 2014-08-11 09:21:19 -0600 )edit

add a comment

2

answered 2014-08-11 10:18:29 -0600

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

13689 ●74 ●240 ●232 http://github.com/ayrton04

Well, your x and y "before" values are -nan. Adding anything to nan will still yield nan. If I had to guess, you are going through one loop iteration before your callbacks actually receive any data.

Also, consider this bit of code:

while(n.ok())
    {
        ros::spinOnce();               // check for incoming messages
        fr_enc = n.subscribe("front_encoders", 1, feCallBack);
        rr_enc = n.subscribe("rear_encoders", 1, reCallBack);

You are repeatedly subscribing to the topics for every loop iteration. Move lines 56 and 57 out of the while loop:

fr_enc = n.subscribe("front_encoders", 1, feCallBack);
rr_enc = n.subscribe("rear_encoders", 1, reCallBack);

while(n.ok())
    {
        ros::spinOnce();               // check for incoming messages