ros2 foxy robot-localization 里程计 imu多传感器融合踩坑指南

前言

Robot_localization是一个基于卡尔曼滤波 ROS的包，可以对里程计 imu gps多种传感器进行数据融合，进而完成机器人的定位的算法。对于不懂卡尔曼滤波的小伙伴，通过修改配置文件就能顺利的使用大佬们造好的轮子，极大的降低了使用多传感器定位的难度。

使用环境：

ubuntu 20.04  ros2  foxy

在线安装robot-localization

sudo apt install ros-foxy-robot-localization

robot_localization中有两个用于融合传感器数据的两种不同的算法节点ekf_localization_node和ukf_localization_node，它们的主要区别在于使用的滤波器算法不同。

运行测试

运行扩展卡尔曼滤波器（EKF）：ros2 launch robot_localization ekf.launch.py

报错 退出
[INFO] [launch]: All log files can be found below /home/m/.ros/log/2023-07-14-12-26-37-916195-ubun-10497
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [ekf_node-1]: process started with pid [10499]
[ERROR] [ekf_node-1]: process has died [pid 10499, exit code -11, cmd '/opt/ros/foxy/lib/robot_localization/ekf_node --ros-args -r __node:=ekf_filter_node --params-file /opt/ros/foxy/share/robot_localization/params/ekf.yaml'].
运行无迹卡尔曼滤波器（UKF）：ros2 launch robot_localization ukf.launch.py  

报错 退出
[INFO] [launch]: All log files can be found below /home/m/.ros/log/2023-07-14-13-06-00-945589-ubun-11298
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [ukf_node-1]: process started with pid [11300]
[ERROR] [ukf_node-1]: process has died [pid 11300, exit code -11, cmd '/opt/ros/foxy/lib/robot_localization/ukf_node --ros-args -r __node:=ukf_filter_node --params-file /opt/ros/foxy/share/robot_localization/params/ukf.yaml'].

不知道什么原因，我的在线安装的robot-localization工作不正常，一运行就报错退出，有知道原因的小伙伴评论区留言，没办法，只能卸载了用源码安装了。

卸载

sudo apt remove ros-foxy-robot-localization
 

源码安装robot-localization

源文件下载地址：

文件 · foxy-devel · mirrors / cra-ros-pkg / robot_localization · GitCode

根据使用的ros版本选择适合的程序下载（一定要注意下载的版本和本机的ros版本是否配套，不配套编译会过不了的），解压后放在ros工作空间src目录下。

编译

colcon build --packages-select robot_localization 

文件结构

测试运行

m@ubun:~/ros2_ws$ source install/setup.bash
ros2 launch robot_localization ekf.launch.py

能看到这个说明你安装成功了，下面该做的就是修改配置文件，把别人造好的轮子转起来。 

文件目录

先看一下我们关心的2个文件夹 launch params

ekf.launch.py    //扩展卡尔曼滤波器（EKF） 启动程序

ekf.yaml            //扩展卡尔曼滤波器配置文件        

为了不破坏源文件，我们复制一份ekf.launch.py原文件改名

odom_ekf.launch.py  并修改引用的配置文件的名字

# Copyright 2018 Open Source Robotics Foundation, Inc.
# Copyright 2019 Samsung Research America
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory
import launch_ros.actions
import os
import yaml
from launch.substitutions import EnvironmentVariable
import pathlib
import launch.actions
from launch.actions import DeclareLaunchArgument

def generate_launch_description():
    return LaunchDescription([
        launch_ros.actions.Node(
            package='robot_localization',
            executable='ekf_node',
            name='ekf_filter_node',
            output='screen',
            parameters=[os.path.join(get_package_share_directory("robot_localization"), 'params', 'odom_ekf.yaml')], 
#只是这里把ekf.yaml修改成了odom_ekf.yaml 
           ),
])

复制一份ekf.yaml原文件改名

odom_ekf.yaml  翻译备注并修改文件内容

### ekf config file ###
###ekf配置文件###
ekf_filter_node:
    ros__parameters:
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
# computation until it receives at least one message from one of the inputs. It will then run continuously at the
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.

# 滤波器输出位置估计的频率，单位为Hz。请注意，过滤器在从其中一个输入接收到至少一条消息之前不会开始计算。
# 然后，无论是否接收到更多的测量值，它都将以此处指定的频率连续运行。如果未指定，则默认为30。        
        frequency: 30.0

# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
# filter will generate new output. Defaults to 1 / frequency if not specified.

# 我们认为传感器超时的时间段（以秒为单位）。在这种情况下，我们在EKF上执行一个预测周期，而不进行校正。
# 这个参数可以被认为是滤波器产生新输出的最小频率。如果未指定，则默认为1/频率。
        sensor_timeout: 0.1

# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
# by, for example, an IMU. Defaults to false if unspecified.

# ekf_localization_node and ukf_localization_node都使用3D全向运动模型。
# 如果此参数设置为true，则不会在状态估计中使用任何三维信息。
# 如果您在平面环境中操作，并且希望忽略地平面中可能检测到的微小变化的影响，请使用此选项
# 例如通过IMU。如果未指定，则默认为false。 
        two_d_mode: true

# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
# future dating the transform, which is required for interaction with some other packages. Defaults