robot_localization包调用失败

tzh

运行环境： Ubuntu 22.04
ROS版本： ros2 humble

出错情况描述： 只使用IMU数据作为robot_localization的输入，启动节点后无报错，但/odometry/filtered话题无输出，也不存在odom->base_link的tf转换。

robot_localization的配置文件如下：

### ekf config file ###
ekf_filter_node:
    ros__parameters:
        frequency: 30.0
        two_d_mode: false
        publish_acceleration: true
        publish_tf: true
        map_frame: map              # 如果未指定，默认为"map"
        odom_frame: odom            # 如果未指定，默认为"odom"
        base_link_frame: base_link  # 如果未指定，默认为"base_link"
        world_frame: odom           # 如果未指定，默认为odom_frame的值

        imu0: /imu
        imu0_config: [false, false, false,
                      true,  true,  true,
                      false, false, false,
                      true,  true,  true,
                      true,  true,  true]

robot_localization的launch文件如下：

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    imu_tf = Node(
        name='imu_tf',
        package='tf2_ros',
        executable='static_transform_publisher',
        arguments=['0.0', '0.0', '0.0', '0', '0',
                   '0', '1', 'base_link', 'imu']
    )

    imu = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('imu_driver'), 'launch'),
            '/imu_driver.launch.py']),
    )

    localization = Node(
        package='robot_localization',
        executable='ekf_node',
        name='robot_localization',
        parameters=[
            "/home/tzh/Projects/it_ws/src/navigation/launch/localization.config.yaml"],
        output='screen'
    )

    return LaunchDescription([
        imu,
        imu_tf,
        localization
    ])

运行robot_localization的launch文件后控制台输出如下：

[INFO] [launch]: All log files can be found below /home/tzh/.ros/log/2024-10-31-10-23-00-921171-tzh-pc-64155
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [witmotion_ros_node-1]: process started with pid [64156]
[INFO] [static_transform_publisher-2]: process started with pid [64158]
[INFO] [ekf_node-3]: process started with pid [64160]
[static_transform_publisher-2] [WARN] [1730341381.030685183] []: Old-style arguments are deprecated; see --help for new-style arguments
[static_transform_publisher-2] [INFO] [1730341381.056635045] [imu_tf]: Spinning until stopped - publishing transform
[static_transform_publisher-2] translation: ('0.000000', '0.000000', '0.000000')
[static_transform_publisher-2] rotation: ('0.000000', '0.000000', '0.000000', '1.000000')
[static_transform_publisher-2] from 'base_link' to 'imu'
[witmotion_ros_node-1] [INFO] [1730341381.057299339] [witmotion]: Starting node with lib version (1.2.28~dev_1cfcd85).
[witmotion_ros_node-1] [INFO] [1730341381.057468826] [ROSWitmotionSensorController]: Controller started
[witmotion_ros_node-1] [INFO] [1730341381.057618467] [MinimalPublisher]: QT spin !!!!!

/imu话题的输出节选如下：

---
header:
  stamp:
    sec: 1730341404
    nanosec: 365809160
  frame_id: imu
orientation:
  x: -0.022146457388615474
  y: -0.0021896924432268476
  z: -0.0007197822277023602
  w: 0.9997520800606935
orientation_covariance:
- 0.0479
- 0.0
- 0.0
- 0.0
- 0.0207
- 0.0
- 0.0
- 0.0
- 0.0041
angular_velocity:
  x: 0.0
  y: 0.0
  z: 0.0
angular_velocity_covariance:
- 0.0663
- 0.0
- 0.0
- 0.0
- 0.1453
- 0.0
- 0.0
- 0.0
- 0.0378
linear_acceleration:
  x: 0.043110352009534836
  y: -0.43110352754592896
  z: 9.747729301452637
linear_acceleration_covariance:
- 0.0364
- 0.0
- 0.0
- 0.0
- 0.0048
- 0.0
- 0.0
- 0.0
- 0.0796
---

机器人小伊

从你提供的信息来看，你已经正确配置了robot_localization的EKF（扩展卡尔曼滤波器）节点，并且IMU数据也在正常发布。然而，/odometry/filtered话题没有输出，同时也没有odom -> base_link的tf转换，这通常意味着EKF节点没有成功接收到有效的输入数据或者配置有误。以下是一些可能的解决步骤和检查点：

检查IMU数据格式：
- 确保/imu话题发布的IMU数据格式与robot_localization期望的格式一致。特别是orientation、angular_velocity和linear_acceleration的协方差矩阵需要正确设置。
- 协方差矩阵的对角线元素通常表示测量噪声的不确定性，而非对角线元素通常设置为0。检查你的IMU驱动是否正确地设置了这些值。
检查IMU的tf变换：
- 你已经使用static_transform_publisher为imu和base_link之间创建了一个静态变换。然而，通常IMU应该直接固定在机器人上，并且其tf变换应该是动态的（通过IMU驱动发布）。如果IMU驱动本身不发布tf变换，你可能需要修改IMU驱动或者继续使用static_transform_publisher（尽管这通常不是最佳实践）。
- 确认static_transform_publisher是否确实在运行，并且没有错误。
检查EKF配置：
- 你的EKF配置文件中，imu0_config数组设置了哪些数据是有效的。根据你提供的配置，看起来你已经启用了所有相关的IMU数据（角速度、线加速度和四元数）。
- 确保frequency参数与IMU数据的发布频率相匹配或稍高。
检查日志输出：
- 查看EKF节点的日志输出，看是否有任何错误或警告信息。这可以通过ROS2的日志系统或使用ros2 node info和ros2 topic list等命令来完成。
- 特别是检查是否有关于数据订阅或tf变换的错误。
测试IMU数据：
- 尝试使用ros2 topic echo /imu来实时查看IMU数据，确保数据在持续发布且格式正确。
- 你可以尝试使用其他IMU数据生成器或模拟器来测试EKF节点，以排除IMU硬件或驱动的问题。
检查ROS2环境：
- 确保你的ROS2环境设置正确，所有依赖包都已正确安装。
- 尝试在一个干净的ROS2工作空间中重新构建和部署robot_localization和IMU驱动。
使用调试工具：
- 使用ros2 topic list -v查看所有话题的详细信息，包括发布者和订阅者。
- 使用ros2 node info /ekf_node查看EKF节点的详细信息，包括它订阅了哪些话题。

如果以上步骤都不能解决问题，你可能需要更深入地检查robot_localization的源代码或寻求社区的帮助。在ROS2社区论坛或GitHub仓库中提问时，提供完整的配置文件、日志输出和任何相关的代码片段将有助于他人更快地诊断问题。

以上为生成回答，仅供参考～

tzh

问题已解决！

原因是配置文件的节点名称和launch文件的节点名称没对上，launch文件中节点名称是robot_localization，而配置文件中是ekf_filter_node，导致配置文件没有生效，所以节点没订阅上/imu话题，导致/odometry/filtered话题无输出。