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

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

其中θ就是機(jī)器人的偏航角。相應(yīng)的程序如下：

/* 對速度進(jìn)行積分得到位移 */
// 獲取當(dāng)前時(shí)間
current_time = ros::Time::now();
// 獲取積分間隔
double dt = (current_time - last_time).toSec();
last_time = current_time;
// 將機(jī)體系速度轉(zhuǎn)換到里程計(jì)坐標(biāo)系
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;

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

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

以上就獲取了完整的機(jī)器人里程計(jì)數(shù)據(jù)，接下來需要將里程計(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ā)布坐標(biāo)變換
odom_broadcaster.sendTransform(odom_trans);


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


// 設(shè)置機(jī)器人的位置和姿態(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è)置機(jī)器人的速度
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = vy;
odom.twist.twist.angular.z = vth;


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

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

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

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

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