Create a new file in chapter5_tutorials/src with the name odometry.cpp, and write a code to publish odometry, you can use the following code snippets or check the code repository to have the entire code:
#include <string> #include <ros/ros.h> #include <sensor_msgs/JointState.h> #include <tf/transform_broadcaster.h> #include <nav_msgs/Odometry.h> int main(int argc, char** argv) { ros::init(argc, argv, "state_publisher"); ros::NodeHandle n; ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 10); ... while (ros::ok()) { current_time = ros::Time::now(); double dt = (current_time - last_time).toSec(); double delta_x = (vx * cos(th) - vy * sin(th)) * dt; double delta_y = (vx * sin(th) + vy * cos(th)) * dt; ...
