使用robot_localization包进行odom里程计数据与IMU陀螺仪数据融合

（重要：如何正确食用本教程，本教程开始教设置有噪点的Odom仿真，然后教如何使用robot_localization经行融合滤除有噪点的Odom数据。这样做是为了前后对比学习，证明看到确实有效的滤波融合结果）

环境：

          ubuntu22.04+ROS2 humble

（本教程使用gazebo+rviz2仿真，形象地说明如何使用localization包如配置ekf.yam文件里面的参数，以及比对融合前后的效果，并且提供完整的测试代码包）

如何安装robot_localization

github:https://github.com/cra-ros-pkg/robot_localization 。在github上可以找到robot_localization的humble版本的包

使用码源安装：

mkdir -p my_localization/src

cd my_localization/src/

git clone https://github.com/cra-ros-pkg/robot_localization.git --branch humble-devel

安装仿真环境

fishbot_description是鱼佬制作的一个gazebo小车仿真包，我通过百度云分享在下面

通过网盘分享的文件：fishbot_description.zip
链接: https://pan.baidu.com/s/1pYWqd3I6nUI9SYQ059WjVQ?pwd=2g4x 提取码: 2g4x

下载解压后：

编译：colcon build

可以尝试运行仿真环境：

ros2 launch fishbot_description gazebo.launch.py

（如果出现错误，等了很久都没有出现gazebo world,请重启虚拟机环境）

添加Odom噪声

       因为在仿真环境中是比较理想的环境，为了模拟现实中的打滑，因此需要给里程计发出的消息增加一些噪点。

     在fishbot_description中的src目录下新建odom_noise.py文件

---

写入下面的代码

import rclpy  
from rclpy.node import Node  
from nav_msgs.msg import Odometry    
import random  
  
class NoisyOdom(Node):  
    def __init__(self):  
        super().__init__('noisy_odom_node')  
        self.odom_subscription = self.create_subscription(  
            Odometry,  
            '/odom1',  
            self.odom_callback,  
            10)  # 队列大小  
        self.odom_publisher = self.create_publisher(Odometry, '/noisy_odom', 10)  
        self.odom_msg = Odometry()  
  
    def odom_callback(self, msg):  
        # 复制消息以避免修改原始消息  
        self.add_noise(msg)  
        self.odom_publisher.publish(msg)  
  
    def add_noise(self, msg):  
        rand_float = random.uniform(-0.2, 0.2)  
        msg.pose.pose.position.y += rand_float  
  
def main(args=None):  
    rclpy.init(args=args)  
    noisy_odom = NoisyOdom()  
    rclpy.spin(noisy_odom)  
    rclpy.shutdown()  
  
if __name__ == '__main__':  
    main()

写好噪声代码后还需设置setup.py

"odom_noise= fishbot_description.odom_noise:main"

 编译：colcon build

查看增加噪声后的效果：

运行仿真：

ros2 launch fishbot_description gazebo.launch.py

 

运行添加噪点的代码 

ros2 run fishbot_description odom_noise 

 打开rviz2

rviz2

使用Add 添加插件 ，注意我的插件配置，和订阅的话题。odom1话题对应仿真环境中理想的里程计话题（红色），noise_odom对应的是加了噪声后的里程计话题（粉色）。粉色的箭头一直在红色的箭头左右漂移。说明这就是有噪声，接下来我们见用robot_locaztion进行融合滤波，将粉色的箭头纠正。