int main(int argc, char **argv) { ros::init(argc, argv, "odometryNode"); Odometry controller; controller.init(argc, argv); GCRobotics::Pose_msg data; tf::TransformBroadcaster br; tf::Transform transform; tf::Quaternion q; ros::Rate r(10); // 10 hz while (ros::ok()) { data.x = controller.X; data.y = controller.Y; data.z = 0; data.heading = controller.heading; controller.pub.publish(data); transform.setOrigin( // Set global position to send to tf tf::Vector3( controller.X*sin(controller.heading) + controller.Y*cos(controller.heading) , controller.X*cos(controller.heading) - controller.Y*sin(controller.heading) , 0.0 ) ); q.setRPY(0,0,controller.heading); // Convert heading calculation into quaternion to give to tf. transform.setRotation(q); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "odom", "base_link")); // Send the transform ros::spinOnce(); r.sleep(); } }