Ask Your Question
1

Odometry Pose NaN [closed]

asked 2014-11-07 08:42:56 -0600

updated 2014-11-07 09:36:25 -0600

Hello,

I'm trying to set up the odometry for a robot. I've followed the robot setup tutorial on the wiki, edited for my own robot. The package can be found on github in the src/omni_odom_take_2.cpp file. When I subscibe to the omni_odom topic I am getting NaN for the pose. The velocities, covariance, and header info is there, but nothing for the pose. Any ideas what I am doing wrong?

Thanks

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <ax2550/StampedEncoders.h>
#include <tf/transform_datatypes.h>

int wheel1_new = 0;
int wheel2_new = 0;
int wheel3_new = 0;
int wheel4_new = 0;
double dt_front = 0;
double dt_rear = 0;

double x = 0.0;
double y = 0.0;
double th = 0.0;

double vx = 0.0;
double vy = 0.0;
double vth = 0.0;

const double wheel_radius = 0.125;
const double wheel_circumference = 0.785;
const double encoder_resolution = 1250*4*20;
const double k = 0.47 + 0.55; //the sum of the distance between the wheel's x-coord and the origin, and the y-coord and the origin
const double dist_per_tick = wheel_circumference / encoder_resolution;

void feCallBack(const ax2550::StampedEncoders::ConstPtr& msg)
{
  wheel1_new = msg->encoders.left_wheel;
  wheel4_new = msg->encoders.right_wheel;
  dt_front = msg->encoders.time_delta;
}

void reCallBack(const ax2550::StampedEncoders::ConstPtr& msg)
{
  wheel2_new = msg->encoders.left_wheel;
  wheel3_new = msg->encoders.right_wheel;
  dt_rear = msg->encoders.time_delta;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "omni_odom");

  ros::NodeHandle n;
  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("omni_odom", 60);
  ros::Subscriber fr_enc = n.subscribe("/omnimaxbot/front/encoders", 100, feCallBack);
  ros::Subscriber rr_enc = n.subscribe("/omnimaxbot/rear/encoders", 100, reCallBack);
  tf::TransformBroadcaster odom_broadcaster;

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

  ros::Rate r(1.0);
  while(n.ok())
  {
    current_time = ros::Time::now();

    double avg_dt = (dt_front + dt_rear)/2.0;

    if(avg_dt == 0)
    {
        avg_dt = dt_front;
    }

    //compute the velocities
    double v_w1 = (wheel1_new * dist_per_tick)/dt_front;
    double v_w2 = (wheel2_new * dist_per_tick)/dt_rear;
    double v_w3 = (wheel3_new * dist_per_tick)/dt_rear;
    double v_w4 = (wheel4_new * dist_per_tick)/dt_front;

    vx = (wheel_radius/4)*(v_w1+v_w2+v_w3+v_w4);
    vy = (wheel_radius/4)*(-v_w1+v_w2-v_w3+v_w4);
    vth = (wheel_radius/(4*k))*(-v_w1-v_w2+v_w3+v_w4);

    //compute odometry in a typical way given the velocities of the robot
    double delta_x = vx * avg_dt;
    double delta_y = vy * avg_dt;
    double delta_th = vth * avg_dt;

    x = x + delta_x;
    y = y + delta_y;
    th = th + delta_th;

    //since all odometry is 6DOF we'll need a quaternion created from yaw
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

    //first, we'll publish the transform over tf
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = current_time;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_footprint";
    odom_trans.transform.translation.x = x;
    odom_trans.transform.translation.y = y;
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = odom_quat;

    //send the transform
    odom_broadcaster.sendTransform(odom_trans);

    //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 covariance
    odom.pose.covariance[0 ...
(more)
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-11-07 10:30:23.920075

Comments

Please insert the code that you use into this post. Also: Which values are NaN? Always? Computed from what input values?

dornhege gravatar imagedornhege ( 2014-11-07 08:51:38 -0600 )edit

2 Answers

Sort by ยป oldest newest most voted
3

answered 2014-11-07 10:16:45 -0600

paulbovbel gravatar image

Best guess - because you initialize dt_front and dt_rear as 0, until those get updated by the callbacks, you're dividing by 0, causing NaN to be stored in x.

After the callbacks, your velocities will be processed properly, however x = NaN + delta_x will always be NaN.

The key with NaNs is to think - where could you have divided by 0?


This code could use some rearchitecting on the whole, it's not a good idea to be running the loop separately from the callbacks, as you have the significant chance of either dropping odometry information (getting callbacks faster than your code is looping), or looping over stale data (no data update between two loops - how does this affect your estimate?).

Your best bet may be to look at odometry models for 4 wheel robots, it's definitely not trivial.

edit flag offensive delete link more

Comments

You were correct. Thanks for the second set of eyes. I initialized them to be very small, but not quite 0, and it started working. I will look at rearchitecting the code. Isn't the SpinOnce() at the bottom of the loop supposed to check the callback to avoid missing data?

Icehawk101 gravatar imageIcehawk101 ( 2014-11-07 10:32:44 -0600 )edit

spinOnce will process all the callbacks in the queue. So if you had: - two feCallBack's queued: you will lose all ticks from the first message - zero feCallback's queued: you will loop again over stale data

paulbovbel gravatar imagepaulbovbel ( 2014-11-07 10:41:20 -0600 )edit

A more effective way would be to run a low-pass filter for your wheel velocities, and update within your message callbacks. Then run a ros::timer to call your odometry publish function at regular intervals (e.g. 10 Hz).

paulbovbel gravatar imagepaulbovbel ( 2014-11-07 10:43:52 -0600 )edit

Still not great, because you're basically converting ticks -> velocity -> displacement, so you're going to get drift.

paulbovbel gravatar imagepaulbovbel ( 2014-11-07 10:44:57 -0600 )edit
0

answered 2014-11-07 10:11:03 -0600

dornhege gravatar image

I suspect that this is not ROS related, but a problem in your code. Can you output whatever you send out in your odometry code. I think it's gonna be the same.

It seems that at some point you introduce NaNs from somewhere. Unless someone else spots something, you should just output your intermediate values to debug that.

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2014-11-07 08:42:56 -0600

Seen: 1,292 times

Last updated: Nov 07 '14