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