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

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

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

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

python 复制代码

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 ``` ![](https://i-blog.csdnimg.cn/direct/cb66c56c927e45e8b0b9cf543bcb5cbd.png) 融合的节点是/ekf_filter_node，可以查询下这个节点的相关信息，终端输入， ``` ros2 node info /ekf_filter_node ``` ```bash 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数据。