Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

need odometry insite for two wheel robot

I am trying to write a odometry node for a two wheel car running in gazebo. The robot keeps running backward as you can see from the png file i've attached. It moves away from the odom frame and at the same time the map frame moves in the oppiste direction (correct direction and the correct forward scanner direction).

class PublishOdometry
{
public:
  PublishOdometry()
  {
     pubOdometry = n.advertise<nav_msgs::Odometry>("odom", 10);

     // Listen and wait for initial pose from nav
     subInitialPose = n.subscribe("/initialpose", 1, &PublishOdometry::handelerInitialPose,this);
  }


void handelerInitialPose(geometry_msgs::PoseWithCovarianceStamped msg)
{
     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = "odom";
     odom_trans.child_frame_id = "base_link";
     odom_trans.header.stamp =  msg.header.stamp;
     odom_trans.transform.translation.x = msg.pose.pose.position.x;
     odom_trans.transform.translation.y = msg.pose.pose.position.y;
     odom_trans.transform.translation.z = msg.pose.pose.position.z;
     odom_trans.transform.rotation = msg.pose.pose.orientation;

     nav_msgs::Odometry odometry;
     odometry.header.frame_id = "odom";
     odometry.child_frame_id  = "base_link";
     odometry.header.stamp    = msg.header.stamp;
     odometry.pose = msg.pose;

     // set starting point
     theta = msg.pose.pose.orientation.z;
     x =  msg.pose.pose.position.x;

     // publish initial postion
     odom_broadcaster.sendTransform(odom_trans);
     pubOdometry.publish(odometry);

     // start sync'd joint state callbacks 
     j1_sub.reset(new Subscriber<control_msgs::JointControllerState>(n, "/rrbot/joint1_position_controller/state", 10));
     j2_sub.reset(new Subscriber<control_msgs::JointControllerState>(n, "/rrbot/joint2_position_controller/state", 10));
     sync.reset(new TimeSynchronizer<control_msgs::JointControllerState, control_msgs::JointControllerState>(*j1_sub, *j2_sub, 10));
     sync->registerCallback(boost::bind(&PublishOdometry::handelerOdometry, this, _1, _2));
}


void handelerOdometry(const control_msgs::JointControllerStateConstPtr& msg1, const control_msgs::JointControllerStateConstPtr& msg2)
{
     double wheel_track = .26;   //distance between two wheels in meters  : in urdf dist is .3 in sdf it is .16 wheels are .05

     float   j1 = msg1->process_value;     // values are velocities: using velocity controller:  meters/sec
     float   j2 = msg2->process_value;   

     double  dt = (msg1->header.stamp - timeold).toSec();      // elapsed time in seconds
     timeold = msg1->header.stamp;  

     float   dx     =   ( ((j1 + j2)) / 2.0) * dt;             // distance in meters
     float   dtheta =   ( ((j1 - j2)) / wheel_track) * dt;     // rotation???????????????????????????????????


     theta = theta + dtheta;                                   // theta and x set on initialpose
     theta = fmodf(theta + M_PI, 2*M_PI) - M_PI;               // adj to range 
     x = x + dx;                                               // adj distance to angle


//**************************************************************

     geometry_msgs::Quaternion  quaternion;
     quaternion.x = 0.0;
     quaternion.y = 0.0; 
     quaternion.z = -sin(theta);           
     quaternion.w = -cos(theta);  

     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = "odom";
     odom_trans.child_frame_id = "base_link";
     odom_trans.header.stamp =  msg1->header.stamp;
     odom_trans.transform.translation.x = x;
     odom_trans.transform.translation.y = 0;
     odom_trans.transform.translation.z = 0;
     odom_trans.transform.rotation = quaternion;

     odom_broadcaster.sendTransform(odom_trans);


//**************************************************************

     nav_msgs::Odometry odometry;
     odometry.header.frame_id = "odom";
     odometry.child_frame_id  = "base_link";
     odometry.header.stamp    = msg1->header.stamp;

     odometry.pose.pose.position.x  = x;
     odometry.pose.pose.position.y  = 0;
     odometry.pose.pose.position.z  = 0;
     odometry.pose.pose.orientation = quaternion;
     odometry.twist.twist.linear.x  = dx/dt;       // convet back to velocity
     odometry.twist.twist.linear.y  = 0;
     odometry.twist.twist.angular.z = dtheta/dt;   // convet back to velocity

     pubOdometry.publish(odometry);             

}

private:

   double x;
   float theta;
   ros::Time timeold;

   ros::NodeHandle n;
   ros::Publisher pubOdometry;
   tf2_ros::TransformBroadcaster odom_broadcaster;
   ros::Subscriber subInitialPose;


   boost::shared_ptr<Subscriber<control_msgs::JointControllerState> > j1_sub;
   boost::shared_ptr<Subscriber<control_msgs::JointControllerState> > j2_sub;
   boost::shared_ptr<TimeSynchronizer<control_msgs::JointControllerState, control_msgs::JointControllerState> > sync;


};  // Enodof Class



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

  // publish static latch transformations 

  tf2_ros::StaticTransformBroadcaster static_broadcaster;
  geometry_msgs::TransformStamped msg;

  msg.header.stamp = ros::Time::now();
  msg.transform.rotation.x =  0.0;
  msg.transform.rotation.y =  0.0;
  msg.transform.rotation.z =  0.0;
  msg.transform.rotation.w =  1.0;

  msg.header.frame_id = "base_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.1;
  msg.child_frame_id = "camera_link";
  static_broadcaster.sendTransform(msg);

  msg.header.frame_id = "camera_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.2;
  msg.child_frame_id = "hokuyo_frame";
  static_broadcaster.sendTransform(msg);

  msg.header.frame_id = "camera_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.3;
  msg.child_frame_id = "camera_frame";
  static_broadcaster.sendTransform(msg);

  PublishOdometry sp;

  ros::spin();

  return 0;
}

need odometry insite for two wheel robot

I am trying to write a odometry node for a two wheel car running in gazebo. The robot keeps running backward as you can see from the png file i've attached. It moves away from the odom frame and at the same time the map frame moves in the oppiste direction (correct direction and the correct forward scanner direction).

class PublishOdometry
{
public:
  PublishOdometry()
  {
     pubOdometry = n.advertise<nav_msgs::Odometry>("odom", 10);

     // Listen and wait for initial pose from nav
     subInitialPose = n.subscribe("/initialpose", 1, &PublishOdometry::handelerInitialPose,this);
  }


void handelerInitialPose(geometry_msgs::PoseWithCovarianceStamped msg)
{
     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = "odom";
     odom_trans.child_frame_id = "base_link";
     odom_trans.header.stamp =  msg.header.stamp;
     odom_trans.transform.translation.x = msg.pose.pose.position.x;
     odom_trans.transform.translation.y = msg.pose.pose.position.y;
     odom_trans.transform.translation.z = msg.pose.pose.position.z;
     odom_trans.transform.rotation = msg.pose.pose.orientation;

     nav_msgs::Odometry odometry;
     odometry.header.frame_id = "odom";
     odometry.child_frame_id  = "base_link";
     odometry.header.stamp    = msg.header.stamp;
     odometry.pose = msg.pose;

     // set starting point
     theta = msg.pose.pose.orientation.z;
     x =  msg.pose.pose.position.x;

     // publish initial postion
     odom_broadcaster.sendTransform(odom_trans);
     pubOdometry.publish(odometry);

     // start sync'd joint state callbacks 
     j1_sub.reset(new Subscriber<control_msgs::JointControllerState>(n, "/rrbot/joint1_position_controller/state", 10));
     j2_sub.reset(new Subscriber<control_msgs::JointControllerState>(n, "/rrbot/joint2_position_controller/state", 10));
     sync.reset(new TimeSynchronizer<control_msgs::JointControllerState, control_msgs::JointControllerState>(*j1_sub, *j2_sub, 10));
     sync->registerCallback(boost::bind(&PublishOdometry::handelerOdometry, this, _1, _2));
}


void handelerOdometry(const control_msgs::JointControllerStateConstPtr& msg1, const control_msgs::JointControllerStateConstPtr& msg2)
{
     double wheel_track = .26;   //distance between two wheels in meters  : in urdf dist is .3 in sdf it is .16 wheels are .05

     float   j1 = msg1->process_value;     // values are velocities: using velocity controller:  meters/sec
     float   j2 = msg2->process_value;   

     double  dt = (msg1->header.stamp - timeold).toSec();      // elapsed time in seconds
     timeold = msg1->header.stamp;  

     float   dx     =   ( ((j1 + j2)) / 2.0) * dt;             // distance in meters
     float   dtheta =   ( ((j1 - j2)) / wheel_track) * dt;     // rotation???????????????????????????????????


     theta = theta + dtheta;                                   // theta and x set on initialpose
     theta = fmodf(theta + M_PI, 2*M_PI) - M_PI;               // adj to range 
     x = x + dx;                                               // adj distance to angle


//**************************************************************

     geometry_msgs::Quaternion  quaternion;
     quaternion.x = 0.0;
     quaternion.y = 0.0; 
     quaternion.z = -sin(theta);           
     quaternion.w = -cos(theta);  

     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = "odom";
     odom_trans.child_frame_id = "base_link";
     odom_trans.header.stamp =  msg1->header.stamp;
     odom_trans.transform.translation.x = x;
     odom_trans.transform.translation.y = 0;
     odom_trans.transform.translation.z = 0;
     odom_trans.transform.rotation = quaternion;

     odom_broadcaster.sendTransform(odom_trans);


//**************************************************************

     nav_msgs::Odometry odometry;
     odometry.header.frame_id = "odom";
     odometry.child_frame_id  = "base_link";
     odometry.header.stamp    = msg1->header.stamp;

     odometry.pose.pose.position.x  = x;
     odometry.pose.pose.position.y  = 0;
     odometry.pose.pose.position.z  = 0;
     odometry.pose.pose.orientation = quaternion;
     odometry.twist.twist.linear.x  = dx/dt;       // convet back to velocity
     odometry.twist.twist.linear.y  = 0;
     odometry.twist.twist.angular.z = dtheta/dt;   // convet back to velocity

     pubOdometry.publish(odometry);             

}

private:

   double x;
   float theta;
   ros::Time timeold;

   ros::NodeHandle n;
   ros::Publisher pubOdometry;
   tf2_ros::TransformBroadcaster odom_broadcaster;
   ros::Subscriber subInitialPose;


   boost::shared_ptr<Subscriber<control_msgs::JointControllerState> > j1_sub;
   boost::shared_ptr<Subscriber<control_msgs::JointControllerState> > j2_sub;
   boost::shared_ptr<TimeSynchronizer<control_msgs::JointControllerState, control_msgs::JointControllerState> > sync;


};  // Enodof Class



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

  // publish static latch transformations 

  tf2_ros::StaticTransformBroadcaster static_broadcaster;
  geometry_msgs::TransformStamped msg;

  msg.header.stamp = ros::Time::now();
  msg.transform.rotation.x =  0.0;
  msg.transform.rotation.y =  0.0;
  msg.transform.rotation.z =  0.0;
  msg.transform.rotation.w =  1.0;

  msg.header.frame_id = "base_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.1;
  msg.child_frame_id = "camera_link";
  static_broadcaster.sendTransform(msg);

  msg.header.frame_id = "camera_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.2;
  msg.child_frame_id = "hokuyo_frame";
  static_broadcaster.sendTransform(msg);

  msg.header.frame_id = "camera_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.3;
  msg.child_frame_id = "camera_frame";
  static_broadcaster.sendTransform(msg);

  PublishOdometry sp;

  ros::spin();

  return 0;
}

image description

need odometry insite insight for two wheel robot

I am trying to write a odometry node for a two wheel car running in gazebo. The robot keeps running backward as you can see from the png file i've attached. It moves away from the odom frame and at the same time the map frame moves in the oppiste direction (correct direction and the correct forward scanner direction).

class PublishOdometry
{
public:
  PublishOdometry()
  {
     pubOdometry = n.advertise<nav_msgs::Odometry>("odom", 10);

     // Listen and wait for initial pose from nav
     subInitialPose = n.subscribe("/initialpose", 1, &PublishOdometry::handelerInitialPose,this);
  }


void handelerInitialPose(geometry_msgs::PoseWithCovarianceStamped msg)
{
     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = "odom";
     odom_trans.child_frame_id = "base_link";
     odom_trans.header.stamp =  msg.header.stamp;
     odom_trans.transform.translation.x = msg.pose.pose.position.x;
     odom_trans.transform.translation.y = msg.pose.pose.position.y;
     odom_trans.transform.translation.z = msg.pose.pose.position.z;
     odom_trans.transform.rotation = msg.pose.pose.orientation;

     nav_msgs::Odometry odometry;
     odometry.header.frame_id = "odom";
     odometry.child_frame_id  = "base_link";
     odometry.header.stamp    = msg.header.stamp;
     odometry.pose = msg.pose;

     // set starting point
     theta = msg.pose.pose.orientation.z;
     x =  msg.pose.pose.position.x;

     // publish initial postion
     odom_broadcaster.sendTransform(odom_trans);
     pubOdometry.publish(odometry);

     // start sync'd joint state callbacks 
     j1_sub.reset(new Subscriber<control_msgs::JointControllerState>(n, "/rrbot/joint1_position_controller/state", 10));
     j2_sub.reset(new Subscriber<control_msgs::JointControllerState>(n, "/rrbot/joint2_position_controller/state", 10));
     sync.reset(new TimeSynchronizer<control_msgs::JointControllerState, control_msgs::JointControllerState>(*j1_sub, *j2_sub, 10));
     sync->registerCallback(boost::bind(&PublishOdometry::handelerOdometry, this, _1, _2));
}


void handelerOdometry(const control_msgs::JointControllerStateConstPtr& msg1, const control_msgs::JointControllerStateConstPtr& msg2)
{
     double wheel_track = .26;   //distance between two wheels in meters  : in urdf dist is .3 in sdf it is .16 wheels are .05

     float   j1 = msg1->process_value;     // values are velocities: using velocity controller:  meters/sec
     float   j2 = msg2->process_value;   

     double  dt = (msg1->header.stamp - timeold).toSec();      // elapsed time in seconds
     timeold = msg1->header.stamp;  

     float   dx     =   ( ((j1 + j2)) / 2.0) * dt;             // distance in meters
     float   dtheta =   ( ((j1 - j2)) / wheel_track) * dt;     // rotation???????????????????????????????????


     theta = theta + dtheta;                                   // theta and x set on initialpose
     theta = fmodf(theta + M_PI, 2*M_PI) - M_PI;               // adj to range 
     x = x + dx;                                               // adj distance to angle


//**************************************************************

     geometry_msgs::Quaternion  quaternion;
     quaternion.x = 0.0;
     quaternion.y = 0.0; 
     quaternion.z = -sin(theta);           
     quaternion.w = -cos(theta);  

     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = "odom";
     odom_trans.child_frame_id = "base_link";
     odom_trans.header.stamp =  msg1->header.stamp;
     odom_trans.transform.translation.x = x;
     odom_trans.transform.translation.y = 0;
     odom_trans.transform.translation.z = 0;
     odom_trans.transform.rotation = quaternion;

     odom_broadcaster.sendTransform(odom_trans);


//**************************************************************

     nav_msgs::Odometry odometry;
     odometry.header.frame_id = "odom";
     odometry.child_frame_id  = "base_link";
     odometry.header.stamp    = msg1->header.stamp;

     odometry.pose.pose.position.x  = x;
     odometry.pose.pose.position.y  = 0;
     odometry.pose.pose.position.z  = 0;
     odometry.pose.pose.orientation = quaternion;
     odometry.twist.twist.linear.x  = dx/dt;       // convet back to velocity
     odometry.twist.twist.linear.y  = 0;
     odometry.twist.twist.angular.z = dtheta/dt;   // convet back to velocity

     pubOdometry.publish(odometry);             

}

private:

   double x;
   float theta;
   ros::Time timeold;

   ros::NodeHandle n;
   ros::Publisher pubOdometry;
   tf2_ros::TransformBroadcaster odom_broadcaster;
   ros::Subscriber subInitialPose;


   boost::shared_ptr<Subscriber<control_msgs::JointControllerState> > j1_sub;
   boost::shared_ptr<Subscriber<control_msgs::JointControllerState> > j2_sub;
   boost::shared_ptr<TimeSynchronizer<control_msgs::JointControllerState, control_msgs::JointControllerState> > sync;


};  // Enodof Class



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

  // publish static latch transformations 

  tf2_ros::StaticTransformBroadcaster static_broadcaster;
  geometry_msgs::TransformStamped msg;

  msg.header.stamp = ros::Time::now();
  msg.transform.rotation.x =  0.0;
  msg.transform.rotation.y =  0.0;
  msg.transform.rotation.z =  0.0;
  msg.transform.rotation.w =  1.0;

  msg.header.frame_id = "base_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.1;
  msg.child_frame_id = "camera_link";
  static_broadcaster.sendTransform(msg);

  msg.header.frame_id = "camera_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.2;
  msg.child_frame_id = "hokuyo_frame";
  static_broadcaster.sendTransform(msg);

  msg.header.frame_id = "camera_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.3;
  msg.child_frame_id = "camera_frame";
  static_broadcaster.sendTransform(msg);

  PublishOdometry sp;

  ros::spin();

  return 0;
}

image description

need odometry insight for two wheel robot

Update: the velocity commands are calculated from /cmd_vel from move base. For some reason they are negative as it thinks the robot is facing the wrong way. If it wanted to turn around it would rotate , yes.

I am trying to write a odometry node for a two wheel car running in gazebo. The robot keeps running backward as you can see from the png file i've attached. It moves away from the odom frame and at the same time the map frame moves in the oppiste direction (correct direction and the correct forward scanner direction).

class PublishOdometry
{
public:
  PublishOdometry()
  {
     pubOdometry = n.advertise<nav_msgs::Odometry>("odom", 10);

     // Listen and wait for initial pose from nav
     subInitialPose = n.subscribe("/initialpose", 1, &PublishOdometry::handelerInitialPose,this);
  }


void handelerInitialPose(geometry_msgs::PoseWithCovarianceStamped msg)
{
     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = "odom";
     odom_trans.child_frame_id = "base_link";
     odom_trans.header.stamp =  msg.header.stamp;
     odom_trans.transform.translation.x = msg.pose.pose.position.x;
     odom_trans.transform.translation.y = msg.pose.pose.position.y;
     odom_trans.transform.translation.z = msg.pose.pose.position.z;
     odom_trans.transform.rotation = msg.pose.pose.orientation;

     nav_msgs::Odometry odometry;
     odometry.header.frame_id = "odom";
     odometry.child_frame_id  = "base_link";
     odometry.header.stamp    = msg.header.stamp;
     odometry.pose = msg.pose;

     // set starting point
     theta = msg.pose.pose.orientation.z;
     x =  msg.pose.pose.position.x;

     // publish initial postion
     odom_broadcaster.sendTransform(odom_trans);
     pubOdometry.publish(odometry);

     // start sync'd joint state callbacks 
     j1_sub.reset(new Subscriber<control_msgs::JointControllerState>(n, "/rrbot/joint1_position_controller/state", 10));
     j2_sub.reset(new Subscriber<control_msgs::JointControllerState>(n, "/rrbot/joint2_position_controller/state", 10));
     sync.reset(new TimeSynchronizer<control_msgs::JointControllerState, control_msgs::JointControllerState>(*j1_sub, *j2_sub, 10));
     sync->registerCallback(boost::bind(&PublishOdometry::handelerOdometry, this, _1, _2));
}


void handelerOdometry(const control_msgs::JointControllerStateConstPtr& msg1, const control_msgs::JointControllerStateConstPtr& msg2)
{
     double wheel_track = .26;   //distance between two wheels in meters  : in urdf dist is .3 in sdf it is .16 wheels are .05

     float   j1 = msg1->process_value;     // values are velocities: using velocity controller:  meters/sec
     float   j2 = msg2->process_value;   

     double  dt = (msg1->header.stamp - timeold).toSec();      // elapsed time in seconds
     timeold = msg1->header.stamp;  

     float   dx     =   ( ((j1 + j2)) / 2.0) * dt;             // distance in meters
     float   dtheta =   ( ((j1 - j2)) / wheel_track) * dt;     // rotation???????????????????????????????????


     theta = theta + dtheta;                                   // theta and x set on initialpose
     theta = fmodf(theta + M_PI, 2*M_PI) - M_PI;               // adj to range 
     x = x + dx;                                               // adj distance to angle


//**************************************************************

     geometry_msgs::Quaternion  quaternion;
     quaternion.x = 0.0;
     quaternion.y = 0.0; 
     quaternion.z = -sin(theta);           
     quaternion.w = -cos(theta);  

     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = "odom";
     odom_trans.child_frame_id = "base_link";
     odom_trans.header.stamp =  msg1->header.stamp;
     odom_trans.transform.translation.x = x;
     odom_trans.transform.translation.y = 0;
     odom_trans.transform.translation.z = 0;
     odom_trans.transform.rotation = quaternion;

     odom_broadcaster.sendTransform(odom_trans);


//**************************************************************

     nav_msgs::Odometry odometry;
     odometry.header.frame_id = "odom";
     odometry.child_frame_id  = "base_link";
     odometry.header.stamp    = msg1->header.stamp;

     odometry.pose.pose.position.x  = x;
     odometry.pose.pose.position.y  = 0;
     odometry.pose.pose.position.z  = 0;
     odometry.pose.pose.orientation = quaternion;
     odometry.twist.twist.linear.x  = dx/dt;       // convet back to velocity
     odometry.twist.twist.linear.y  = 0;
     odometry.twist.twist.angular.z = dtheta/dt;   // convet back to velocity

     pubOdometry.publish(odometry);             

}

private:

   double x;
   float theta;
   ros::Time timeold;

   ros::NodeHandle n;
   ros::Publisher pubOdometry;
   tf2_ros::TransformBroadcaster odom_broadcaster;
   ros::Subscriber subInitialPose;


   boost::shared_ptr<Subscriber<control_msgs::JointControllerState> > j1_sub;
   boost::shared_ptr<Subscriber<control_msgs::JointControllerState> > j2_sub;
   boost::shared_ptr<TimeSynchronizer<control_msgs::JointControllerState, control_msgs::JointControllerState> > sync;


};  // Enodof Class



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

  // publish static latch transformations 

  tf2_ros::StaticTransformBroadcaster static_broadcaster;
  geometry_msgs::TransformStamped msg;

  msg.header.stamp = ros::Time::now();
  msg.transform.rotation.x =  0.0;
  msg.transform.rotation.y =  0.0;
  msg.transform.rotation.z =  0.0;
  msg.transform.rotation.w =  1.0;

  msg.header.frame_id = "base_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.1;
  msg.child_frame_id = "camera_link";
  static_broadcaster.sendTransform(msg);

  msg.header.frame_id = "camera_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.2;
  msg.child_frame_id = "hokuyo_frame";
  static_broadcaster.sendTransform(msg);

  msg.header.frame_id = "camera_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.3;
  msg.child_frame_id = "camera_frame";
  static_broadcaster.sendTransform(msg);

  PublishOdometry sp;

  ros::spin();

  return 0;
}

image description

need odometry insight for two wheel robot

Updated my odometry: Still robot not following trajectory.

#include <string>
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <control_msgs/JointControllerState.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <sensor_msgs/JointState.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>


using namespace sensor_msgs;
using namespace message_filters;

class PublishOdometry
{
public:
  PublishOdometry()
  {
     pubOdometry = n.advertise<nav_msgs::Odometry>("odom", 10);

     // Listen and wait for initial pose from nav
     subInitialPose = n.subscribe("/initialpose", 1, &PublishOdometry::handelerInitialPose,this);
  }


void handelerInitialPose(geometry_msgs::PoseWithCovarianceStamped msg)
{
     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = "odom";
     odom_trans.child_frame_id = "base_link";
     odom_trans.header.stamp =  msg.header.stamp;
     odom_trans.transform.translation.x = msg.pose.pose.position.x;
     odom_trans.transform.translation.y = msg.pose.pose.position.y;
     odom_trans.transform.translation.z = msg.pose.pose.position.z;
     odom_trans.transform.rotation = msg.pose.pose.orientation;

     nav_msgs::Odometry odometry;
     odometry.header.frame_id = "odom";
     odometry.child_frame_id  = "base_link";
     odometry.header.stamp    = msg.header.stamp;
     odometry.pose = msg.pose;

     // set starting point
     theta = msg.pose.pose.orientation.z;
     x = msg.pose.pose.position.x;
     y = msg.pose.pose.position.y;

     // publish initial postion
     odom_broadcaster.sendTransform(odom_trans);
     pubOdometry.publish(odometry);

     // start sync'd joint state callbacks 
     j1_sub.reset(new Subscriber<control_msgs::JointControllerState>(n, "/rrbot/joint1_position_controller/state", 10));
     j2_sub.reset(new Subscriber<control_msgs::JointControllerState>(n, "/rrbot/joint2_position_controller/state", 10));
     sync.reset(new TimeSynchronizer<control_msgs::JointControllerState, control_msgs::JointControllerState>(*j1_sub, *j2_sub, 10));
     sync->registerCallback(boost::bind(&PublishOdometry::handelerOdometry, this, _1, _2));
}


void handelerOdometry(const control_msgs::JointControllerStateConstPtr& msg1, const control_msgs::JointControllerStateConstPtr& msg2)
{

// based on: http://rossum.sourceforge.net/papers/DiffSteer/

     double wheel_track = .21;   //distance between two wheels in meters  : in urdf dist is .3 in sdf it is .16 wheels are .05

     float   j1 = msg1->process_value;   // values are velocities: using velocity controller:  meters/sec
     float   j2 = msg2->process_value;   

     if (msg1->set_point == 0) j1 =0;
     if (msg2->set_point == 0) j2 =0;

     double  dt = msg1->time_step;                        // elapsed time in seconds
     float   dx     =   ( ((j1 + j2)*dt ) / 2.0);         // distance in meters
     float   dtheta =   ( ((j1 - j2)*dt ) / wheel_track);  // rotation

     if (abs(dtheta)>.001) 
     {
        x = x + (dx/dtheta)*(dx*sin(theta + dtheta)-sin(theta));                                    
        y = y - (dx/dtheta)*(dx*cos(theta + dtheta)-cos(theta));  
        theta = theta + dtheta;                                   
        theta = fmodf(theta + M_PI, 2*M_PI) - M_PI ; 
     } 
     else // straight line dtheta == 0
     {
        x = x - dx*cos(theta);
        y = y + dx*sin(theta);
     }



//**************************************************************

     geometry_msgs::Quaternion  quaternion;
     quaternion.x = 0.0;
     quaternion.y = 0.0; 
     quaternion.z = -sin(theta);           
     quaternion.w = cos(theta);  

     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = "odom";
     odom_trans.child_frame_id = "base_link";
     odom_trans.header.stamp =  msg1->header.stamp;
     odom_trans.transform.translation.x = x;
     odom_trans.transform.translation.y = y;
     odom_trans.transform.translation.z = 0;
     odom_trans.transform.rotation = quaternion;

     odom_broadcaster.sendTransform(odom_trans);


     ROS_INFO("pv1=[%f] pv2=[%f]",msg1->process_value, msg2->process_value);
     ROS_INFO("er1=[%f] er2=[%f] ",msg1->error, msg2->error);
     ROS_INFO("sp1=[%f] sp2=[%f] ",msg1->set_point, msg2->set_point);
     ROS_INFO("dx =[%f] z=[%f]  w=[%f] theta=[%f]", dx, quaternion.z, quaternion.w, theta);
     ROS_INFO("x =[%e] ,y=[%e]", x, y);
//**************************************************************


     nav_msgs::Odometry odometry;
     odometry.header.frame_id = "odom";
     odometry.child_frame_id  = "base_link";
     odometry.header.stamp    = msg1->header.stamp;

     odometry.pose.pose.position.x  = x;
     odometry.pose.pose.position.y  = y;
     odometry.pose.pose.position.z  = 0;
     odometry.pose.pose.orientation = quaternion;
     odometry.twist.twist.linear.x  = dx/dt;       // convet back to velocity
     odometry.twist.twist.linear.y  = 0;
     odometry.twist.twist.angular.z = dtheta/dt;   // convet back to velocity

     pubOdometry.publish(odometry);              // only seems to confuse orientation 

}

private:

   double x,y;
   float theta;

   ros::NodeHandle n;
   ros::Publisher pubOdometry;
   tf2_ros::TransformBroadcaster odom_broadcaster;
   ros::Subscriber subInitialPose;


   boost::shared_ptr<Subscriber<control_msgs::JointControllerState> > j1_sub;
   boost::shared_ptr<Subscriber<control_msgs::JointControllerState> > j2_sub;
   boost::shared_ptr<TimeSynchronizer<control_msgs::JointControllerState, control_msgs::JointControllerState> > sync;


};  // Enodof Class



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

  // publish static latch transformations 

  tf2_ros::StaticTransformBroadcaster static_broadcaster;
  geometry_msgs::TransformStamped msg;

  msg.header.stamp = ros::Time::now();
  msg.transform.rotation.x =  0.0;
  msg.transform.rotation.y =  0.0;
  msg.transform.rotation.z =  0.0;
  msg.transform.rotation.w =  1.0;

  msg.header.frame_id = "base_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.1;
  msg.child_frame_id = "camera_link";
  static_broadcaster.sendTransform(msg);

  msg.header.frame_id = "camera_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.2;
  msg.child_frame_id = "hokuyo_frame";
  static_broadcaster.sendTransform(msg);

  msg.header.frame_id = "camera_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.3;
  msg.child_frame_id = "camera_frame";
  static_broadcaster.sendTransform(msg);

  PublishOdometry sp;

  ros::spin();

  return 0;
}

Update: the velocity commands are calculated from /cmd_vel from move base. For some reason they are negative as it thinks the robot is facing the wrong way. If it wanted to turn around it would rotate , yes.

I am trying to write a odometry node for a two wheel car running in gazebo. The robot keeps running backward as you can see from the png file i've attached. It moves away from the odom frame and at the same time the map frame moves in the oppiste direction (correct direction and the correct forward scanner direction).

class PublishOdometry
{
public:
  PublishOdometry()
  {
     pubOdometry = n.advertise<nav_msgs::Odometry>("odom", 10);

     // Listen and wait for initial pose from nav
     subInitialPose = n.subscribe("/initialpose", 1, &PublishOdometry::handelerInitialPose,this);
  }


void handelerInitialPose(geometry_msgs::PoseWithCovarianceStamped msg)
{
     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = "odom";
     odom_trans.child_frame_id = "base_link";
     odom_trans.header.stamp =  msg.header.stamp;
     odom_trans.transform.translation.x = msg.pose.pose.position.x;
     odom_trans.transform.translation.y = msg.pose.pose.position.y;
     odom_trans.transform.translation.z = msg.pose.pose.position.z;
     odom_trans.transform.rotation = msg.pose.pose.orientation;

     nav_msgs::Odometry odometry;
     odometry.header.frame_id = "odom";
     odometry.child_frame_id  = "base_link";
     odometry.header.stamp    = msg.header.stamp;
     odometry.pose = msg.pose;

     // set starting point
     theta = msg.pose.pose.orientation.z;
     x =  msg.pose.pose.position.x;

     // publish initial postion
     odom_broadcaster.sendTransform(odom_trans);
     pubOdometry.publish(odometry);

     // start sync'd joint state callbacks 
     j1_sub.reset(new Subscriber<control_msgs::JointControllerState>(n, "/rrbot/joint1_position_controller/state", 10));
     j2_sub.reset(new Subscriber<control_msgs::JointControllerState>(n, "/rrbot/joint2_position_controller/state", 10));
     sync.reset(new TimeSynchronizer<control_msgs::JointControllerState, control_msgs::JointControllerState>(*j1_sub, *j2_sub, 10));
     sync->registerCallback(boost::bind(&PublishOdometry::handelerOdometry, this, _1, _2));
}


void handelerOdometry(const control_msgs::JointControllerStateConstPtr& msg1, const control_msgs::JointControllerStateConstPtr& msg2)
{
     double wheel_track = .26;   //distance between two wheels in meters  : in urdf dist is .3 in sdf it is .16 wheels are .05

     float   j1 = msg1->process_value;     // values are velocities: using velocity controller:  meters/sec
     float   j2 = msg2->process_value;   

     double  dt = (msg1->header.stamp - timeold).toSec();      // elapsed time in seconds
     timeold = msg1->header.stamp;  

     float   dx     =   ( ((j1 + j2)) / 2.0) * dt;             // distance in meters
     float   dtheta =   ( ((j1 - j2)) / wheel_track) * dt;     // rotation???????????????????????????????????


     theta = theta + dtheta;                                   // theta and x set on initialpose
     theta = fmodf(theta + M_PI, 2*M_PI) - M_PI;               // adj to range 
     x = x + dx;                                               // adj distance to angle


//**************************************************************

     geometry_msgs::Quaternion  quaternion;
     quaternion.x = 0.0;
     quaternion.y = 0.0; 
     quaternion.z = -sin(theta);           
     quaternion.w = -cos(theta);  

     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = "odom";
     odom_trans.child_frame_id = "base_link";
     odom_trans.header.stamp =  msg1->header.stamp;
     odom_trans.transform.translation.x = x;
     odom_trans.transform.translation.y = 0;
     odom_trans.transform.translation.z = 0;
     odom_trans.transform.rotation = quaternion;

     odom_broadcaster.sendTransform(odom_trans);


//**************************************************************

     nav_msgs::Odometry odometry;
     odometry.header.frame_id = "odom";
     odometry.child_frame_id  = "base_link";
     odometry.header.stamp    = msg1->header.stamp;

     odometry.pose.pose.position.x  = x;
     odometry.pose.pose.position.y  = 0;
     odometry.pose.pose.position.z  = 0;
     odometry.pose.pose.orientation = quaternion;
     odometry.twist.twist.linear.x  = dx/dt;       // convet back to velocity
     odometry.twist.twist.linear.y  = 0;
     odometry.twist.twist.angular.z = dtheta/dt;   // convet back to velocity

     pubOdometry.publish(odometry);             

}

private:

   double x;
   float theta;
   ros::Time timeold;

   ros::NodeHandle n;
   ros::Publisher pubOdometry;
   tf2_ros::TransformBroadcaster odom_broadcaster;
   ros::Subscriber subInitialPose;


   boost::shared_ptr<Subscriber<control_msgs::JointControllerState> > j1_sub;
   boost::shared_ptr<Subscriber<control_msgs::JointControllerState> > j2_sub;
   boost::shared_ptr<TimeSynchronizer<control_msgs::JointControllerState, control_msgs::JointControllerState> > sync;


};  // Enodof Class



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

  // publish static latch transformations 

  tf2_ros::StaticTransformBroadcaster static_broadcaster;
  geometry_msgs::TransformStamped msg;

  msg.header.stamp = ros::Time::now();
  msg.transform.rotation.x =  0.0;
  msg.transform.rotation.y =  0.0;
  msg.transform.rotation.z =  0.0;
  msg.transform.rotation.w =  1.0;

  msg.header.frame_id = "base_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.1;
  msg.child_frame_id = "camera_link";
  static_broadcaster.sendTransform(msg);

  msg.header.frame_id = "camera_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.2;
  msg.child_frame_id = "hokuyo_frame";
  static_broadcaster.sendTransform(msg);

  msg.header.frame_id = "camera_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.3;
  msg.child_frame_id = "camera_frame";
  static_broadcaster.sendTransform(msg);

  PublishOdometry sp;

  ros::spin();

  return 0;
}

image description

need odometry insight for two wheel robot

Updated my odometry: new code follows: Still robot not following trajectory. Not sure why map is following base_link? odom stays fixed.

image description

#include <string>
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <control_msgs/JointControllerState.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <sensor_msgs/JointState.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>


using namespace sensor_msgs;
using namespace message_filters;

class PublishOdometry
{
public:
  PublishOdometry()
  {
     pubOdometry = n.advertise<nav_msgs::Odometry>("odom", 10);

     // Listen and wait for initial pose from nav
     subInitialPose = n.subscribe("/initialpose", 1, &PublishOdometry::handelerInitialPose,this);
  }


void handelerInitialPose(geometry_msgs::PoseWithCovarianceStamped msg)
{
     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = "odom";
     odom_trans.child_frame_id = "base_link";
     odom_trans.header.stamp =  msg.header.stamp;
     odom_trans.transform.translation.x = msg.pose.pose.position.x;
     odom_trans.transform.translation.y = msg.pose.pose.position.y;
     odom_trans.transform.translation.z = msg.pose.pose.position.z;
     odom_trans.transform.rotation = msg.pose.pose.orientation;

     nav_msgs::Odometry odometry;
     odometry.header.frame_id = "odom";
     odometry.child_frame_id  = "base_link";
     odometry.header.stamp    = msg.header.stamp;
     odometry.pose = msg.pose;

     // set starting point
     theta = msg.pose.pose.orientation.z;
     x = msg.pose.pose.position.x;
     y = msg.pose.pose.position.y;

     // publish initial postion
     odom_broadcaster.sendTransform(odom_trans);
     pubOdometry.publish(odometry);

     // start sync'd joint state callbacks 
     j1_sub.reset(new Subscriber<control_msgs::JointControllerState>(n, "/rrbot/joint1_position_controller/state", 10));
     j2_sub.reset(new Subscriber<control_msgs::JointControllerState>(n, "/rrbot/joint2_position_controller/state", 10));
     sync.reset(new TimeSynchronizer<control_msgs::JointControllerState, control_msgs::JointControllerState>(*j1_sub, *j2_sub, 10));
     sync->registerCallback(boost::bind(&PublishOdometry::handelerOdometry, this, _1, _2));
}


void handelerOdometry(const control_msgs::JointControllerStateConstPtr& msg1, const control_msgs::JointControllerStateConstPtr& msg2)
{

// based on: http://rossum.sourceforge.net/papers/DiffSteer/

     double wheel_track = .21;   //distance between two wheels in meters  : in urdf dist is .3 in sdf it is .16 wheels are .05

     float   j1 = msg1->process_value;   // values are velocities: using velocity controller:  meters/sec
     float   j2 = msg2->process_value;   

     if (msg1->set_point == 0) j1 =0;
     if (msg2->set_point == 0) j2 =0;

     double  dt = msg1->time_step;                        // elapsed time in seconds
     float   dx     =   ( ((j1 + j2)*dt ) / 2.0);         // distance in meters
     float   dtheta =   ( ((j1 - j2)*dt ) / wheel_track);  // rotation

     if (abs(dtheta)>.001) 
     {
        x = x + (dx/dtheta)*(dx*sin(theta + dtheta)-sin(theta));                                    
        y = y - (dx/dtheta)*(dx*cos(theta + dtheta)-cos(theta));  
        theta = theta + dtheta;                                   
        theta = fmodf(theta + M_PI, 2*M_PI) - M_PI ; 
     } 
     else // straight line dtheta == 0
     {
        x = x - dx*cos(theta);
        y = y + dx*sin(theta);
     }



//**************************************************************

     geometry_msgs::Quaternion  quaternion;
     quaternion.x = 0.0;
     quaternion.y = 0.0; 
     quaternion.z = -sin(theta);           
     quaternion.w = cos(theta);  

     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = "odom";
     odom_trans.child_frame_id = "base_link";
     odom_trans.header.stamp =  msg1->header.stamp;
     odom_trans.transform.translation.x = x;
     odom_trans.transform.translation.y = y;
     odom_trans.transform.translation.z = 0;
     odom_trans.transform.rotation = quaternion;

     odom_broadcaster.sendTransform(odom_trans);


     ROS_INFO("pv1=[%f] pv2=[%f]",msg1->process_value, msg2->process_value);
     ROS_INFO("er1=[%f] er2=[%f] ",msg1->error, msg2->error);
     ROS_INFO("sp1=[%f] sp2=[%f] ",msg1->set_point, msg2->set_point);
     ROS_INFO("dx =[%f] z=[%f]  w=[%f] theta=[%f]", dx, quaternion.z, quaternion.w, theta);
     ROS_INFO("x =[%e] ,y=[%e]", x, y);
//**************************************************************


     nav_msgs::Odometry odometry;
     odometry.header.frame_id = "odom";
     odometry.child_frame_id  = "base_link";
     odometry.header.stamp    = msg1->header.stamp;

     odometry.pose.pose.position.x  = x;
     odometry.pose.pose.position.y  = y;
     odometry.pose.pose.position.z  = 0;
     odometry.pose.pose.orientation = quaternion;
     odometry.twist.twist.linear.x  = dx/dt;       // convet back to velocity
     odometry.twist.twist.linear.y  = 0;
     odometry.twist.twist.angular.z = dtheta/dt;   // convet back to velocity

     pubOdometry.publish(odometry);              // only seems to confuse orientation 

}

private:

   double x,y;
   float theta;

   ros::NodeHandle n;
   ros::Publisher pubOdometry;
   tf2_ros::TransformBroadcaster odom_broadcaster;
   ros::Subscriber subInitialPose;


   boost::shared_ptr<Subscriber<control_msgs::JointControllerState> > j1_sub;
   boost::shared_ptr<Subscriber<control_msgs::JointControllerState> > j2_sub;
   boost::shared_ptr<TimeSynchronizer<control_msgs::JointControllerState, control_msgs::JointControllerState> > sync;


};  // Enodof Class



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

  // publish static latch transformations 

  tf2_ros::StaticTransformBroadcaster static_broadcaster;
  geometry_msgs::TransformStamped msg;

  msg.header.stamp = ros::Time::now();
  msg.transform.rotation.x =  0.0;
  msg.transform.rotation.y =  0.0;
  msg.transform.rotation.z =  0.0;
  msg.transform.rotation.w =  1.0;

  msg.header.frame_id = "base_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.1;
  msg.child_frame_id = "camera_link";
  static_broadcaster.sendTransform(msg);

  msg.header.frame_id = "camera_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.2;
  msg.child_frame_id = "hokuyo_frame";
  static_broadcaster.sendTransform(msg);

  msg.header.frame_id = "camera_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.3;
  msg.child_frame_id = "camera_frame";
  static_broadcaster.sendTransform(msg);

  PublishOdometry sp;

  ros::spin();

  return 0;
}

Update: the velocity commands are calculated from /cmd_vel from move base. For some reason they are negative as it thinks the robot is facing the wrong way. If it wanted to turn around it would rotate , yes.

I am trying to write a odometry node for a two wheel car running in gazebo. The robot keeps running backward as you can see from the png file i've attached. It moves away from the odom frame and at the same time the map frame moves in the oppiste direction (correct direction and the correct forward scanner direction).

class PublishOdometry
{
public:
  PublishOdometry()
  {
     pubOdometry = n.advertise<nav_msgs::Odometry>("odom", 10);

     // Listen and wait for initial pose from nav
     subInitialPose = n.subscribe("/initialpose", 1, &PublishOdometry::handelerInitialPose,this);
  }


void handelerInitialPose(geometry_msgs::PoseWithCovarianceStamped msg)
{
     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = "odom";
     odom_trans.child_frame_id = "base_link";
     odom_trans.header.stamp =  msg.header.stamp;
     odom_trans.transform.translation.x = msg.pose.pose.position.x;
     odom_trans.transform.translation.y = msg.pose.pose.position.y;
     odom_trans.transform.translation.z = msg.pose.pose.position.z;
     odom_trans.transform.rotation = msg.pose.pose.orientation;

     nav_msgs::Odometry odometry;
     odometry.header.frame_id = "odom";
     odometry.child_frame_id  = "base_link";
     odometry.header.stamp    = msg.header.stamp;
     odometry.pose = msg.pose;

     // set starting point
     theta = msg.pose.pose.orientation.z;
     x =  msg.pose.pose.position.x;

     // publish initial postion
     odom_broadcaster.sendTransform(odom_trans);
     pubOdometry.publish(odometry);

     // start sync'd joint state callbacks 
     j1_sub.reset(new Subscriber<control_msgs::JointControllerState>(n, "/rrbot/joint1_position_controller/state", 10));
     j2_sub.reset(new Subscriber<control_msgs::JointControllerState>(n, "/rrbot/joint2_position_controller/state", 10));
     sync.reset(new TimeSynchronizer<control_msgs::JointControllerState, control_msgs::JointControllerState>(*j1_sub, *j2_sub, 10));
     sync->registerCallback(boost::bind(&PublishOdometry::handelerOdometry, this, _1, _2));
}


void handelerOdometry(const control_msgs::JointControllerStateConstPtr& msg1, const control_msgs::JointControllerStateConstPtr& msg2)
{
     double wheel_track = .26;   //distance between two wheels in meters  : in urdf dist is .3 in sdf it is .16 wheels are .05

     float   j1 = msg1->process_value;     // values are velocities: using velocity controller:  meters/sec
     float   j2 = msg2->process_value;   

     double  dt = (msg1->header.stamp - timeold).toSec();      // elapsed time in seconds
     timeold = msg1->header.stamp;  

     float   dx     =   ( ((j1 + j2)) / 2.0) * dt;             // distance in meters
     float   dtheta =   ( ((j1 - j2)) / wheel_track) * dt;     // rotation???????????????????????????????????


     theta = theta + dtheta;                                   // theta and x set on initialpose
     theta = fmodf(theta + M_PI, 2*M_PI) - M_PI;               // adj to range 
     x = x + dx;                                               // adj distance to angle


//**************************************************************

     geometry_msgs::Quaternion  quaternion;
     quaternion.x = 0.0;
     quaternion.y = 0.0; 
     quaternion.z = -sin(theta);           
     quaternion.w = -cos(theta);  

     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = "odom";
     odom_trans.child_frame_id = "base_link";
     odom_trans.header.stamp =  msg1->header.stamp;
     odom_trans.transform.translation.x = x;
     odom_trans.transform.translation.y = 0;
     odom_trans.transform.translation.z = 0;
     odom_trans.transform.rotation = quaternion;

     odom_broadcaster.sendTransform(odom_trans);


//**************************************************************

     nav_msgs::Odometry odometry;
     odometry.header.frame_id = "odom";
     odometry.child_frame_id  = "base_link";
     odometry.header.stamp    = msg1->header.stamp;

     odometry.pose.pose.position.x  = x;
     odometry.pose.pose.position.y  = 0;
     odometry.pose.pose.position.z  = 0;
     odometry.pose.pose.orientation = quaternion;
     odometry.twist.twist.linear.x  = dx/dt;       // convet back to velocity
     odometry.twist.twist.linear.y  = 0;
     odometry.twist.twist.angular.z = dtheta/dt;   // convet back to velocity

     pubOdometry.publish(odometry);             

}

private:

   double x;
   float theta;
   ros::Time timeold;

   ros::NodeHandle n;
   ros::Publisher pubOdometry;
   tf2_ros::TransformBroadcaster odom_broadcaster;
   ros::Subscriber subInitialPose;


   boost::shared_ptr<Subscriber<control_msgs::JointControllerState> > j1_sub;
   boost::shared_ptr<Subscriber<control_msgs::JointControllerState> > j2_sub;
   boost::shared_ptr<TimeSynchronizer<control_msgs::JointControllerState, control_msgs::JointControllerState> > sync;


};  // Enodof Class



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

  // publish static latch transformations 

  tf2_ros::StaticTransformBroadcaster static_broadcaster;
  geometry_msgs::TransformStamped msg;

  msg.header.stamp = ros::Time::now();
  msg.transform.rotation.x =  0.0;
  msg.transform.rotation.y =  0.0;
  msg.transform.rotation.z =  0.0;
  msg.transform.rotation.w =  1.0;

  msg.header.frame_id = "base_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.1;
  msg.child_frame_id = "camera_link";
  static_broadcaster.sendTransform(msg);

  msg.header.frame_id = "camera_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.2;
  msg.child_frame_id = "hokuyo_frame";
  static_broadcaster.sendTransform(msg);

  msg.header.frame_id = "camera_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.3;
  msg.child_frame_id = "camera_frame";
  static_broadcaster.sendTransform(msg);

  PublishOdometry sp;

  ros::spin();

  return 0;
}

image description