0
$\begingroup$

Rosanswers logo

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] = 0.2;
    odom.pose.covariance[7] = 0.2;
    odom.pose.covariance[14] = 1e100;
    odom.pose.covariance[21] = 1e100;
    odom.pose.covariance[28] = 1e100;
    odom.pose.covariance[35] = 0.2;

    //set the velocity
    odom.child_frame_id = "base_footprint";
    odom.twist.twist.linear.x = vx;
    odom.twist.twist.linear.y = vy;
    odom.twist.twist.angular.z = vth;

    //publish the message
    odom_pub.publish(odom);
    
    ros::spinOnce();
    r.sleep();
  }
}

This is the output using rostopic echo:

header: 
  seq: 15
  stamp: 
    secs: 1415374147
    nsecs: 340470639
  frame_id: odom
child_frame_id: base_footprint
pose: 
  pose: 
    position: 
      x: nan
      y: nan
      z: 0.0
    orientation: 
      x: nan
      y: nan
      z: nan
      w: nan
  covariance: [0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e+100, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e+100, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e+100, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2]
twist: 
  twist: 
    linear: 
      x: -0.000250038701179
      y: -0.000305255550259
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.00645976072261
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

Originally posted by Icehawk101 on ROS Answers with karma: 955 on 2014-11-07

Post score: 1


Original comments

Comment by dornhege on 2014-11-07:
Please insert the code that you use into this post. Also: Which values are NaN? Always? Computed from what input values?

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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.


Originally posted by paulbovbel with karma: 4518 on 2014-11-07

This answer was ACCEPTED on the original site

Post score: 3


Original comments

Comment by Icehawk101 on 2014-11-07:
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?

Comment by paulbovbel on 2014-11-07:
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

Comment by paulbovbel on 2014-11-07:
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).

Comment by paulbovbel on 2014-11-07:
Still not great, because you're basically converting ticks -> velocity -> displacement, so you're going to get drift.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.