亚博microros小车-原生ubuntu支持系列：16 机器人状态估计

本来想测试下gmapping建图，但是底层依赖了yahboomcar_bringup做底层的数据处理，所以先把依赖的工程导入。

程序启动后，会订阅imu和odom数据，过滤掉一部分的imu数据后，然后与odom数据进行融合，最后输出一个融合后的odom数据用于估计机器人的状态，该数据多用于建图和导航中。

src/yahboomcar_bringup/launch/新建启动脚本yahboomcar_bringup_launch.py

from ament_index_python.packages import get_package_share_path

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import Command, LaunchConfiguration

from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue

import os
from ament_index_python.packages import get_package_share_directory

from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

print("---------------------robot_type = x3---------------------")
def generate_launch_description():
    imu_filter_config = os.path.join(              
        get_package_share_directory('yahboomcar_bringup'),
        'param',
        'imu_filter_param.yaml'
    ) 

    imu_filter_node = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('imu_complementary_filter'), 'launch'),
            '/complementary_filter.launch.py'])
    )
    

    ekf_node = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('robot_localization'), 'launch'),
            '/ekf.launch.py'])
    )
    
    
     
    description_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
        get_package_share_directory('yahboomcar_description'), 'launch'),
         '/description_launch.py'])
    )   
    
    base_link_to_imu_tf_node = Node(
     package='tf2_ros',
     executable='static_transform_publisher',
     name='base_link_to_base_imu',
     arguments=['-0.002999', '-0.0030001','0.031701','0','0','0','base_link','imu_frame']
    ) 
    
    return LaunchDescription([
        imu_filter_node,
        ekf_node,
        base_link_to_imu_tf_node,
        description_launch
    ])

该launch文件启动了以下几个节点：

imu_filter_node：过滤imu数据节点，主要是过滤部分的imu数据；
ekf_node：融合节点，主要是融合odom数据和过滤的imu数据。
base_link_to_imu_tf_node：发布一个静态变化，主要是发布imu模块与小车的位姿变换。
description_launch：加载URDF模型。

从代码来看，结合之前的迁移，还差了2工程。robot_localization、imu_complementary_filter

亚博官网资料对此缺乏介绍。只说启动：

ros2 launch yahboomcar_bringup yahboomcar_bringup_launch.py

网上找资料，大概看看。

robot_localization

功能包简介

robot_localization是状态估计节点的集合，每个节点都是非线性状态估计器的一种实现，用于在3D空间中移动的机器人。它包括两个状态估计节点ekf_localization_node和ukf_localization_node。另外，robot_localization提供navsat_transform_node，它有助于集成GPS数据。

robot_localization wiki — robot_localization 2.6.12 documentation

官方文档，https://github.com/cra-ros-pkg/robot_localization

说的比较抽象，简单理解下，机器人状态估计需要用到很多传感器，因为对单一的传感器来讲，都存在各自的优缺点，需要融合估计。

安装，一般是使用：

sudo apt-get install

但是，yahboomcar_bringup_launch.py里面使用里脚本加载，所以需要使用源码安装。

这个编译安装缺一些包，比如

Could not find a package configuration file provided by “GeographicLib"

sudo apt-get install ros-melodic-geographic-*
sudo apt-get install geographiclib-*
sudo apt-get install libgeographic-*

其他的编译报错缺啥包就对应的安装吧，比较麻烦些。

imu_tools

imu_complementary_filter 网上搜了下，就是imu_tools,这个工程在yahboomcar_ws工程找不到。在外面。官网也没有单独提这个，还是udo apt-get install ros-humble-imu-tools不可行，需要源码编译。

https://github.com/CCNYRoboticsLab/imu_tools/tree/humble

与 IMU 相关的过滤器和可视化器。堆栈包含：

    imu_filter_madgwick：一种过滤器，可将来自常规 IMU 设备的角速度，加速度和（可选）磁读数融合到方向中。
    imu_complementary_filter：一种滤波器，该滤波器使用基于互补融合的新颖方法将来自通用 IMU 设备的角速度，加速度和（可选）磁读数融合到方向四元数中。
    rviz_imu_plugin：rviz 的插件，可显示 sensor_msgs::Imu 消息。

测试运行

先启动小车代理

ros2 launch yahboomcar_bringup yahboomcar_bringup_launch.py

启动日志：

bohu@bohu-TM1701:~/yahboomcar/yahboomcar_ws$ ros2 launch yahboomcar_bringup yahboomcar_bringup_launch.py
[INFO] [launch]: All log files can be found below /home/bohu/.ros/log/2025-01-29-17-43-00-359232-bohu-TM1701-218123
[INFO] [launch]: Default logging verbosity is set to INFO
---------------------robot_type = x3---------------------
[INFO] [complementary_filter_node-1]: process started with pid [218125]
[INFO] [ekf_node-2]: process started with pid [218127]
[INFO] [static_transform_publisher-3]: process started with pid [218129]
[INFO] [joint_state_publisher-4]: process started with pid [218131]
[INFO] [robot_state_publisher-5]: process started with pid [218133]
[INFO] [static_transform_publisher-6]: process started with pid [218135]
[static_transform_publisher-3] [WARN] [1738143780.898682373] []: Old-style arguments are deprecated; see --help for new-style arguments
[static_transform_publisher-6] [WARN] [1738143780.900906973] []: Old-style arguments are deprecated; see --help for new-style arguments
[complementary_filter_node-1] [INFO] [1738143781.138198744] [complementary_filter_gain_node]: Starting ComplementaryFilterROS
[static_transform_publisher-6] [INFO] [1738143781.140989389] [static_transform_publisher_WQtu1UbMwwhEa1kH]: Spinning until stopped - publishing transform
[static_transform_publisher-6] translation: ('0.000000', '0.000000', '0.050000')
[static_transform_publisher-6] rotation: ('0.000000', '0.000000', '0.000000', '1.000000')
[static_transform_publisher-6] from 'base_footprint' to 'base_link'
[static_transform_publisher-3] [INFO] [1738143781.142566675] [base_link_to_base_imu]: Spinning until stopped - publishing transform
[static_transform_publisher-3] translation: ('-0.002999', '-0.003000', '0.031701')
[static_transform_publisher-3] rotation: ('0.000000', '0.000000', '0.000000', '1.000000')
[static_transform_publisher-3] from 'base_link' to 'imu_frame'
[robot_state_publisher-5] [WARN] [1738143781.154460630] [kdl_parser]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.
[robot_state_publisher-5] [INFO] [1738143781.154576150] [robot_state_publisher]: got segment base_link
[robot_state_publisher-5] [INFO] [1738143781.154647599] [robot_state_publisher]: got segment imu_Link
[robot_state_publisher-5] [INFO] [1738143781.154656936] [robot_state_publisher]: got segment jq1_Link
[robot_state_publisher-5] [INFO] [1738143781.154663678] [robot_state_publisher]: got segment jq2_Link
[robot_state_publisher-5] [INFO] [1738143781.154670400] [robot_state_publisher]: got segment radar_Link
[robot_state_publisher-5] [INFO] [1738143781.154676689] [robot_state_publisher]: got segment yh_Link
[robot_state_publisher-5] [INFO] [1738143781.154682893] [robot_state_publisher]: got segment yq_Link
[robot_state_publisher-5] [INFO] [1738143781.154689248] [robot_state_publisher]: got segment zh_Link
[robot_state_publisher-5] [INFO] [1738143781.154695418] [robot_state_publisher]: got segment zq_Link

查看节点间的通讯图，

ros2 run rqt_graph rqt_graph

融合的节点是/ekf_filter_node，可以查询下这个节点的相关信息，终端输入，

ros2 node info /ekf_filter_node

bohu@bohu-TM1701:~/imu_ws$ ros2 node info /ekf_filter_node
/ekf_filter_node
  Subscribers:
    /imu/data: sensor_msgs/msg/Imu
    /odom_raw: nav_msgs/msg/Odometry
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /set_pose: geometry_msgs/msg/PoseWithCovarianceStamped
  Publishers:
    /diagnostics: diagnostic_msgs/msg/DiagnosticArray
    /odom: nav_msgs/msg/Odometry
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /rosout: rcl_interfaces/msg/Log
    /tf: tf2_msgs/msg/TFMessage
  Service Servers:
    /ekf_filter_node/describe_parameters: rcl_interfaces/srv/DescribeParameters
    /ekf_filter_node/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
    /ekf_filter_node/get_parameters: rcl_interfaces/srv/GetParameters
    /ekf_filter_node/list_parameters: rcl_interfaces/srv/ListParameters
    /ekf_filter_node/set_parameters: rcl_interfaces/srv/SetParameters
    /ekf_filter_node/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
    /enable: std_srvs/srv/Empty
    /set_pose: robot_localization/srv/SetPose
    /toggle: robot_localization/srv/ToggleFilterProcessing
  Service Clients:

  Action Servers:

  Action Clients:

该节点订阅了/imu/data和/odom_raw数据，然后发布一个/odom数据。