STM32端返回的x軸線速度、y軸線速度是相對于自身的機體坐標系的速度，而機器人的位置信息是相對于世界坐標系的位置，所以在對速度進行積分前， 要先將機體坐標系下的x軸線速度、y軸線速度轉(zhuǎn)換到世界坐標系 ，如圖：

這個坐標變換可以通過一個簡單的旋轉(zhuǎn)矩陣來實現(xiàn)

其中θ就是機器人的偏航角。相應的程序如下：

/* 對速度進行積分得到位移 */
// 獲取當前時間
current_time = ros::Time::now();
// 獲取積分間隔
double dt = (current_time - last_time).toSec();
last_time = current_time;
// 將機體系速度轉(zhuǎn)換到里程計坐標系
double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
// 速度積分
x += delta_x;
y += delta_y;

在機器人中，一般使用四元數(shù)/旋轉(zhuǎn)矩陣的形式來表示機器人的姿態(tài)，而不是歐拉角形式。所以需要將STM32返回的偏航角轉(zhuǎn)換為四元數(shù)，程序如下：

geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

以上就獲取了完整的機器人里程計數(shù)據(jù)，接下來需要將里程計數(shù)據(jù)發(fā)布到ROS中。

nav_msgs::Odometry odom;
geometry_msgs::TransformStamped odom_trans;


odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";


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;
// 發(fā)布坐標變換
odom_broadcaster.sendTransform(odom_trans);


odom.header.stamp = current_time;
odom.header.frame_id = "odom";
odom.child_frame_id = "base_link";


// 設(shè)置機器人的位置和姿態(tài)
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;


// 設(shè)置機器人的速度
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = vy;
odom.twist.twist.angular.z = vth;


// 發(fā)布里程計消息
odom_pub.publish(odom);

運行后，打開PC上的Ubuntu，配置ip從而實現(xiàn)遠程連接嵌入式處理器上的ROS系統(tǒng)，參照：ROS多機通信（https://blog.csdn.net/qq_42688495/article/details/115260247）

配置完成后，重新打開一個終端，輸入：rviz，打開ROS的可視化工具，按照下圖操作即可

可視化結(jié)果如下：

最后將該rviz配置保存至文件，點擊File→Save Config As，將配置保存為xxxx.rviz。下次打開時，在命令行運行：rosrun rviz rviz -d xxxx.rviz即可。