机器人里程计数据可视化

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即可。