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();
}
}
