In this section, we will see the C++ interpretation of the diff_tf.py node,
which subscribes the encoder data and computes the odometry, and publishes
the odometry and tf of the robot. We can see the C++ interpretation of this node, called diff_tf.cpp, which can be found in the src folder of a package named chefbot_navig_cpp.

Discussed next are the important code snippets of this code and their explanations. The following code snippet is the constructor of the class Odometry_calc. This class contains the definition of computing odometry. The following code declares the subscriber for the left and right wheel encoders along with the publisher for odom value:

Odometry_calc::Odometry_calc(){ 
 
  //Initialize variables used in the node 
   init_variables(); 
 
  ROS_INFO("Started odometry computing node"); 
 
  //Subscribing left and right wheel encoder values 
  l_wheel_sub = n.subscribe("/lwheel",10, &Odometry_calc::leftencoderCb, this); 
   
  r_wheel_sub = n.subscribe("/rwheel",10, &Odometry_calc::rightencoderCb, this); 
 
   //Creating a publisher for odom 
    odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);    
     
 
  //Retrieving parameters of this node 
  get_node_params(); 
}   

The following code is the update loop of computing odometry. It computes the delta distance moved and the angle rotated by the robot using the encoder values, base width of the robot, and ticks per meter of the encoder. After calculating the delta distance and the delta theta, we can compute the final x, y, and theta using the standard differential drive robot equations.

  if ( now > t_next) { 
 
    elapsed = now.toSec() - then.toSec();  
 
    if(enc_left == 0){ 
      d_left = 0; 
      d_right = 0; 
    } 
    else{ 
      d_left = (left - enc_left) / ( ticks_meter); 
      d_right = (right - enc_right) / ( ticks_meter); 
    } 
     
    enc_left = left; 
    enc_right = right; 
 
    d = (d_left + d_right ) / 2.0; 
 
    th = ( d_right - d_left ) / base_width; 
     
    dx = d /elapsed; 
 
    dr = th / elapsed; 
 
    if ( d != 0){ 
 
                  x = cos( th ) * d; 
                  y = -sin( th ) * d; 
 
                  // calculate the final position of the robot 
                  x_final = x_final + ( cos( theta_final ) * x - sin( theta_final ) * y ); 
                  y_final = y_final + ( sin( theta_final ) * x + cos( theta_final ) * y ); 
 
      } 
       if( th != 0) 
              theta_final = theta_final + th; 

After computing the robot position and the orientation from the preceding code snippet, we can feed the odom values to the odom message header and in the tf header, which will publish the topics in /odom and /tf.

        geometry_msgs::Quaternion odom_quat ; 
 
        odom_quat.x = 0.0; 
        odom_quat.y = 0.0; 
        odom_quat.z = 0.0; 
 
          odom_quat.z = sin( theta_final / 2 );   
          odom_quat.w = cos( theta_final / 2 ); 
 
        //first, we'll publish the transform over tf 
        geometry_msgs::TransformStamped odom_trans; 
        odom_trans.header.stamp = now; 
        odom_trans.header.frame_id = "odom"; 
        odom_trans.child_frame_id = "base_footprint"; 
 
        odom_trans.transform.translation.x = x_final; 
        odom_trans.transform.translation.y = y_final; 
        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 = now; 
        odom.header.frame_id = "odom"; 
 
        //set the position 
        odom.pose.pose.position.x = x_final; 
        odom.pose.pose.position.y = y_final; 
        odom.pose.pose.position.z = 0.0; 
        odom.pose.pose.orientation = odom_quat; 
 
        //set the velocity 
        odom.child_frame_id = "base_footprint"; 
        odom.twist.twist.linear.x = dx; 
        odom.twist.twist.linear.y = 0; 
        odom.twist.twist.angular.z = dr; 
 
        //publish the message 
        odom_pub.publish(odom);