(重要:如何正确食用本教程,本教程开始教设置有噪点的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进行融合滤波,将粉色的箭头纠正。