Revision history [back]

Example how to generate Odometry message from encoders.

#include "pr2_mechanism_model/joint.h"
#include "pr2_controller_interface/controller.h"
#include "nav_msgs/Odometry.h"
#include "sensor_msgs/JointState.h"
#include "tf/transform_broadcaster.h"
#include "ros/ros.h"
#include "math.h"


double delta_x = 0;
double delta_y = 0;
double delta_a1 = 0;
double delta_a2 = 0;
double fi = 0;
double current_fi = 0;
double last_x = 0;
double last_y = 0;
double current_a1, current_a2;
double last_a1 = 0;
double last_a2 = 0;


void OdomCallback(const sensor_msgs::JointState::ConstPtr& msg)
{
  // Getting the positions of right and left back wheels from /joint_states topic 

  current_a1 = msg->position[4];
  current_a2 = msg->position[5];

}


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

    ros::NodeHandle n;
    ros::Publisher odom_pub;
    ros::Subscriber odom_sub;

    odom_sub = n.subscribe("/joint_states",1000,OdomCallback);



    odom_pub = n.advertise<nav_msgs::Odometry>("odom",100);
    tf::TransformBroadcaster odom_broadcaster;

      ros::Rate r(1.0);

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

      double  R = 0.07; //wheel radius
      double d = 0.3;  //distance between two wheels
      double x = 0;     //initial values
      double y = 0;
      double th = 0;
      double vx, vy, vth;

      int count = 0;
      while(n.ok())
      {
            ros::spinOnce();               // check for incoming messages
            current_time = ros::Time::now();

            delta_a1 = current_a1 - last_a1;
            delta_a2 = current_a2 - last_a2;

            last_a1 = current_a1;
            last_a2 = current_a2;
            //ROS_INFO("delta_a %f", delta_a);
            double dt = (current_time - last_time).toSec();

            current_fi = -R*(delta_a2 - delta_a1)/d;
            fi +=current_fi;
            current_fi = 0;
            ROS_INFO("yaw %f", fi);

            delta_x = R*delta_a1*cos(fi);
            delta_y = R*delta_a1*sin(fi);


            x += delta_x;
            y += delta_y;
            th = fi;
            vx = delta_x/dt;
            vy = delta_y/dt;
            vth = current_fi/dt;


            //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
               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 velocity
               odom.child_frame_id = "base_link";
               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);

               last_time = current_time;
               r.sleep();
               count++;
      }

 return 0;
}