ROS成长-搭建属于自己的hrobot机器人 三、里程计及tf 反馈分析

	上一部分分析了如何创建一个最小的运动单元，其中包括了键盘下发cmd_vel和base节点如何订阅cmd_vel 。这节将添加里程计和tf反馈的内容。这部分内容是导航节点需要用到的信息。ROS使用tf来决定机器人的位置和静态地图中的传感器数据，但是tf中没有机器人的速度信息，所以导航功能包要求机器人能够通过里程计信息源发布包含速度信息的里程计nav_msgs/Odometry 消息。具体分析如下：
//定义发布消息的名称
  pub_ = nh.advertise("odom", 50);
 tf::TransformBroadcaster odom_broadcaster_;

//计算速度及里程计信息
vx_ = (Speed_Right + Speed_Left) / 2 ; //机器人运动速度
vth_ = (Speed_Right - Speed_Left) / WheelDia*(180/pi); // 机器人旋转角速度
curr_time = ros::Time::now();
//积分运算
double dt = (curr_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;
last_time_ = curr_time;  


current_time_ = ros::Time::now();
// 发布TF
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time_;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id  = "base_footprint";

geometry_msgs::Quaternion odom_quat;
odom_quat = tf::createQuaternionMsgFromYaw(th_);
odom_trans.transform.translation.x = x_;
odom_trans.transform.translation.y = y_;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;

odom_broadcaster_.sendTransform(odom_trans);

// 发布里程计消息
nav_msgs::Odometry msgl;
msgl.header.stamp = current_time_;
msgl.header.frame_id = "odom";

msgl.pose.pose.position.x = x_;
msgl.pose.pose.position.y = y_;
msgl.pose.pose.position.z = 0.0;
msgl.pose.pose.orientation = odom_quat;
msgl.pose.covariance = odom_pose_covariance;//  协方差

msgl.child_frame_id = "base_footprint";
msgl.twist.twist.linear.x = vx_;
msgl.twist.twist.linear.y = vy_;
msgl.twist.twist.angular.z = vth_;
msgl.twist.covariance = odom_twist_covariance;

pub_.publish(msgl);   /**/