ros:Time current time; ros::Time last time; current_time ros::Time:now(); last_time ros::Time::now(); tf::TransformBroadcaster broadcaster; ros::Rate loop rate(20); const double degree =M PI/180: //message declarations geometry_msgs::TransformStamped odom trans: odom trans.header.frame id ="odom"; odom trans.child frame id="base footprint"; 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; double delta th=vth dt; x+=delta_x; y+=delta_y; th+=delta th; geometry msgs::Quaternion odom quat; odom_quat=tf::createQuaternionMsgFromRollPitch Yaw(0,0,th); ∥update transform odom_trans.header.stamp=current_time; odom trans.transform.translation.x=x; odom trans.transform.translation.y=y; odom_trans.transform.translation.z=0.0; odom trans.transform.rotation tf::createQuaternionMsgFrom Ya w(th), //filling the odometry nav_msgs::Odometry odom; odom.header.stamp current time: odom.header.frame id ="odom"; odom.child frame id ="base footprint"; /∥position odom.pose.pose.position.x=x; odom.pose.pose.position.y=y; odom.pose.pose.position.z=0.0; odom.pose.pose.orientation=odom_quat; ∥velocity odom.twist.twist.linear.x vx: odom.twist.twist.linear.y =vy; odom.twist.twist.linear.z=0.0; odom.twist.twist.angular.x=0.0; odom.twist.twist.angular.y=0.0; odom.twist.twist.angular.z=vth; last time=current_time; -13-- 13 - ros::Time current_time; ros::Time last_time; current_time = ros::Time::now(); last_time = ros::Time::now(); tf::TransformBroadcaster broadcaster; ros::Rate loop_rate(20); const double degree = M_PI/180; // message declarations geometry_msgs::TransformStamped odom_trans; odom_trans.header.frame_id = "odom"; odom_trans.child_frame_id = "base_footprint"; 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; double delta_th = vth * dt; x += delta_x; y += delta_y; th += delta_th; geometry_msgs::Quaternion odom_quat; odom_quat = tf::createQuaternionMsgFromRollPitchYaw(0,0,th); // update transform odom_trans.header.stamp = current_time; odom_trans.transform.translation.x = x; odom_trans.transform.translation.y = y; odom_trans.transform.translation.z = 0.0; odom_trans.transform.rotation = tf::createQuaternionMsgFromYa w(th); //filling the odometry nav_msgs::Odometry odom; odom.header.stamp = current_time; odom.header.frame_id = "odom"; odom.child_frame_id = "base_footprint"; // position odom.pose.pose.position.x = x; odom.pose.pose.position.y = y; odom.pose.pose.position.z = 0.0; odom.pose.pose.orientation = odom_quat; // velocity odom.twist.twist.linear.x = vx; odom.twist.twist.linear.y = vy; odom.twist.twist.linear.z = 0.0; odom.twist.twist.angular.x = 0.0; odom.twist.twist.angular.y = 0.0; odom.twist.twist.angular.z = vth; last_time = current_time;