[基于STM32底盘控制与ROS上层导航小车制作]第六节 ROS计算和发布里程计与tf变换

系列文章目录

第一节 stm32电机驱动与编码器读取反馈

第二节 stm32电机pid控制

第三节 stm32线速度标定

第四节 stm32添加mpu6050得到angle角度 

第五节 实现STM32与ubuntu系统下的ROS串口DMA通信，传输底盘速度等信息

第六节 ROS计算和发布里程计与tf变换

系列教材包含

底盘搭建与stm32代码编写，ros激光雷达建图与导航编写，实现动态避障与导航

底盘包含

两轮差速轮（自制）

四轮全向轮（所属团队大家一起搭建）

底盘不同就是在底盘的运动分解与控制，ros部分里程计计算和本地规划器不同，两轮用的base_local_Planner，四轮用的teb_local_planner,来规划发布速度控制底盘

gjhhust / Repositories · GitHubhttps://github.com/gjhhust?tab=repositories

---

目录

系列文章目录

前言

1.位移信息计算

2.速度信息

3.发布里程计

二、发布tf变换

1.内容计算

2.发布tf

三.rviz查看信息与launch集成节点

总结

---

---

前言

今天我们收到底盘的速度与角度信息后进行odom里程计发送

---

一、发布里程计

发布里程计之前我们看看里程计需要计算哪些数据

     //next, we'll publish the odometry message over ROS
     nav_msgs::Odometry odom;
     odom.header.stamp = current_time;
     odom.header.frame_id = "odom";
 
     //set the position
     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;

     //set the velocity
     odom.child_frame_id = "base_link";
     odom.twist.twist.linear.x = vx;
     odom.twist.twist.linear.y = vy;
     odom.twist.twist.angular.z = vth;
 
    //publish the message
     odom_pub.publish(odom);

这是一个简单的发布例子，可以看出需要机器人的位置信息与速度信息，我们知道的只有速度，由数学可知位置当然可以积分出来了，这个计算方法每种底盘模型都不一样，我这里还是介绍两轮差速模型

 由数学推导可知当前位置与上一次位置的关系是

 d是小车中心的唯一，则就是两次航向角的夹角,D_L是左轮位移，D_R是右轮位移

1.位移信息计算

 则综上可得到如下计算

//中心位移
double center_d = (left_speed*time + right_speed*time)/2;

position_x = position_x_last + center_d *cos(theta) ;
position_y = position_y_last + center_d *sin(theta) ;

对于odom.pose.pose.orientation，也就是方向四元数，我们得到底盘传来的yaw后调用函数

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

则会自动将yaw转化为四元数，其它方向角度可暂时不管

2.速度信息

为角度差，t为两次时间差

3.发布里程计

我们这里将里程计的坐标系定为odom，车体底盘定为base_footprint

 // 发布里程计消息
        ros::Publisher pub;
        //发布odometry消息，消息队列为1
        pub = n.advertise("odom", 1);

        nav_msgs::Odometry msgl;
        msgl.header.stamp = time;
        msgl.header.frame_id = "odom";

        msgl.pose.pose.position.x = position_x;
        msgl.pose.pose.position.y = position_y;
        msgl.pose.pose.position.z = 0.0;
        msgl.pose.pose.orientation = odom_quat;

        msgl.child_frame_id = "base_footprint";
        msgl.twist.twist.linear.x = linear;
        msgl.twist.twist.linear.y = 0;
        msgl.twist.twist.angular.z = angluar;

        pub.publish(msgl);

二、发布tf变换

1.内容计算

与里程计相同

2.发布tf

        ros::Time time = ros::Time::now();
        // 发布TF
        tf::TransformBroadcaster odom_broadcaster;

        geometry_msgs::TransformStamped odom_trans;
        odom_trans.header.stamp = time;
        odom_trans.header.frame_id = "odom";
        odom_trans.child_frame_id  = "base_footprint";

        geometry_msgs::Quaternion odom_quat;
        odom_quat = tf::createQuaternionMsgFromYaw(yaw);
        odom_trans.transform.translation.x = position_x;
        odom_trans.transform.translation.y = position_y;
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = odom_quat;
        
        odom_broadcaster.sendTransform(odom_trans);

三.rviz查看信息与launch集成节点

在CMakelist.txt内生成总的执行文件，两个文件一个串口函数定义，一个负责接受地盘数据并且发布里程计与坐标转换

add_executable(robot_start 
          src/publish_node.cpp 
			    src/mbot_linux_serial.cpp )
target_link_libraries(robot_start ${catkin_LIBRARIES})

机器人启动文件，并打开rviz

终端内

catkin_make
source ./devel/setup.bash
roslaunch my_robot robot_start #填写你的launch文件

此时rviz内左下角添加tf组件，手动转动车体，查看yaw轴方向是否准备，拨动车轮，看odom坐标系与base_footprint的平移是否正常

---

总结

这时你可以连接好底盘与上位机，并发送数据，上位机打开rviz查看