ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Computing Robot_position of robot using Robot_localization

asked 2015-01-14 05:07:28 -0600

KDROS gravatar image

I am using Robot_localization package for getting robot position.

I am using Firebird VI Robot (basic robot) which is not compatible with ROS. But i want to use for getting robot position.

What I have

  1. I can get encoder count of wheel (odometry data)

  2. I can get accelerometer, mgnetometer,gyroscope data, using Serial Communication, (after some time interval)

I want to fuse it to get actual position from ROS.

Q1. What are the prerequisite for this complete set up. Robot_localization is using 3-4 node, I am interested in ekf only, Is it possible to get position from ekf node of robot_localization.

Q2. How to write publisher for odometry and IMU data. I know the requirements but not sure how to write it in code.Specially for Imu_data message.

/Odom can I use this one in my work?? Correct me If i am wrong. Suggest me in good direction.

edit retag flag offensive close merge delete

3 Answers

Sort by ยป oldest newest most voted

answered 2015-01-14 08:16:31 -0600

Tom Moore gravatar image

robot_localization, like many ROS packages, contains multiple nodes, but you certainly don't need to use them all. As of this writing, it contains an EKF implementation (ekf_localization_node), a UKF implementation (ukf_localization_node), and a node that aids users in working with GPS data (navsat_transform_node). To answer Q1, yes, you can just use ekf_localization_node.

For Q2, @Anas Alhashimi posted a good link. What you really need to know is (a) the data and units required for the Odometry and Imu messages, and (b) a way of converting your raw data into the format required by those messages. For calculating odometry in (a), this looks like a good article. My guess is that your IMU will give you information about the data it produces; it may be a straightforward process of just filling out the data fields of the message and publishing it. Be careful, though. robot_localization makes some assumptions about your IMU data. See this post for more information.

edit flag offensive delete link more


for just testing, I am thinking to write IMU n odom data publisher, and run ekf_node. I am thinking correct? what should I change in launch file. U have contributed in this package. You can give me best answer.

KDROS gravatar image KDROS  ( 2015-01-14 09:31:06 -0600 )edit

Let's cross that bridge when we come to it. The best path would be for you to put together your IMU and wheel encoder odometry nodes, and then make sure they're producing the correct data. After that, you can post a new question about how to configure ekf_localization_node for your setup.

Tom Moore gravatar image Tom Moore  ( 2015-01-14 09:59:08 -0600 )edit

Is it possible to calculate pose without writting /tf.?? If no, then tell me basic /tf file required for this particular task.

KDROS gravatar image KDROS  ( 2015-01-14 10:27:35 -0600 )edit

Ok Tom, I am doing that, I am writting dummy publisher, then Soon I will post new question.

KDROS gravatar image KDROS  ( 2015-01-14 10:32:16 -0600 )edit

Yes, messages and tf transforms are two separate things. You don't need to output a tf transform for a wheel odometry node.

Tom Moore gravatar image Tom Moore  ( 2015-01-14 10:39:04 -0600 )edit

answered 2015-01-14 06:14:39 -0600

updated 2015-01-14 06:23:57 -0600

Hello, This answer is for Q2.

You can use this to publish the odometry. But you have to change these lines:

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

double vx = 0.1;
double vy = -0.1;
double vth = 0.1;

These values must be taken from your robot hardware (maybe through serial connection). This link could be helpful for you if you get wheel encoders.

edit flag offensive delete link more


I will try with that.. for Imu i found this link . I think it will work. But its different then imu_msg format. it is not using array[9].

KDROS gravatar image KDROS  ( 2015-01-14 09:28:45 -0600 )edit

answered 2015-01-15 00:20:45 -0600

KDROS gravatar image

updated 2015-01-15 00:33:46 -0600

Tom and Anas, Thanx, I tried and wrote 2 different files,

Imu.cpp for publishing imu data, (hard coded) for testing

int main(int argc, char** argv)

    ros::init(argc, argv, "Imu_publisher");

  ros::NodeHandle n;

 ros::Publisher imu_pub = n.advertise<sensor_msgs::Imu>("imu", 50);

 //tf::TransformBroadcaster imu_broadcaster;

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

  double vx = 0.1;
 double vy = -0.1;
 double vth = 0.1;

  ros::Time current_time, last_time;

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

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

  ros::Rate r(1.0);



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

     //next, we'll publish the IMU message over ROS

     sensor_msgs::Imu fl;

        // fl.header.seq=_simulationFrameID;


           fl.orientation.x=0.0; //TODO orientation










          //publish the message


          last_time = current_time;




and Odom.cpp for publishing /odom data,

  #include <ros/ros.h>
  #include <tf/transform_broadcaster.h>
  #include <nav_msgs/Odometry.h>
  int main(int argc, char** argv){
  ros::init(argc, argv, "odometry_publisher");
  ros::NodeHandle n;
  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
  tf::TransformBroadcaster odom_broadcaster;
  double x = 0.0;
  double y = 0.0;
  double th = 0.0;

  double vx = 0.1;
  double vy = -0.1;
  double vth = 0.1;

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

    ros::Rate r(1.0);
    current_time = ros::Time::now();

   //compute odometry in a typical way given the velocities of the robot
     double dt = (current_time - last_time).toSec();
       double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
       double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
      double delta_th = vth * dt;

      x += delta_x;
       y += delta_y;
     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_link";

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

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

//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.twist.twist.linear.x = vx;
  odom.twist.twist.linear.y = vy;
  odom.twist.twist.angular.z = vth;

   //publish the message

      last_time = current_time;

is it correct? I am getting covariance matrix, 0.0 for all, why? is there any error?

edit flag offensive delete link more


they work fine with me. you can fill the covariance matrix if you know how good your measurement are (the variance of each element) the relation between the elements. If x and y are independent, then their covariance is zero.

If you don't have these values just leave them zero.

Anas Alhashimi gravatar image Anas Alhashimi  ( 2015-01-15 04:01:11 -0600 )edit

Now help me, how can I use this publishers in robot_localization or robot_pose_ekf. just for testing purpose I hard coded the value, later i will integrate it with main prgm.

KDROS gravatar image KDROS  ( 2015-01-15 05:54:20 -0600 )edit

I used these with robot_localization, How to check same in the rviz

KDROS gravatar image KDROS  ( 2015-01-16 03:52:19 -0600 )edit

In Rviz you can view the odometry and the pose to see the difference.

Anas Alhashimi gravatar image Anas Alhashimi  ( 2015-01-17 06:55:32 -0600 )edit

yup, I read somewhere that I can see it in rviz but how to do that??

KDROS gravatar image KDROS  ( 2015-01-17 09:00:47 -0600 )edit

Question Tools



Asked: 2015-01-14 05:07:28 -0600

Seen: 1,264 times

Last updated: Jan 15 '15