机器人里程计数据可视化

STM32端返回的x轴线速度、y轴线速度是相对于自身的机体坐标系的速度，而机器人的位置信息是相对于世界坐标系的位置，所以在对速度进行积分前， 要先将机体坐标系下的x轴线速度、y轴线速度转换到世界坐标系 ，如图：

这个坐标变换可以通过一个简单的旋转矩阵来实现

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

/* 对速度进行积分得到位移 */

// 获取当前时间

current_time = ros::Time::now();

// 获取积分间隔

double dt = (current_time - last_time).toSec();

last_time = current_time;

// 将机体系速度转换到里程计坐标系

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;

在机器人中，一般使用四元数/旋转矩阵的形式来表示机器人的姿态，而不是欧拉角形式。所以需要将STM32返回的偏航角转换为四元数，程序如下：

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

以上就获取了完整的机器人里程计数据，接下来需要将里程计数据发布到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;

// 发布坐标变换

odom_broadcaster.sendTransform(odom_trans);

odom.header.stamp = current_time;

odom.header.frame_id = "odom";

odom.child_frame_id = "base_link";

// 设置机器人的位置和姿态

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;

// 设置机器人的速度

odom.twist.twist.linear.x = vx;

odom.twist.twist.linear.y = vy;

odom.twist.twist.angular.z = vth;

// 发布里程计消息

odom_pub.publish(odom);

运行后，打开PC上的Ubuntu，配置ip从而实现远程连接嵌入式处理器上的ROS系统，参照：ROS多机通信（https://blog.csdn.net/qq_42688495/article/details/115260247）

配置完成后，重新打开一个终端，输入：rviz，打开ROS的可视化工具，按照下图操作即可

可视化结果如下：

最后将该rviz配置保存至文件，点击File→Save Config As，将配置保存为xxxx.rviz。下次打开时，在命令行运行：rosrun rviz rviz -d xxxx.rviz即可。