- 本教程来自：Nav2中文网
- Nav2交流社区：https://fishros.org.cn/forum
- ROS2/Nav2千人交流群：(QQ)139707339
- 更多精彩教程请关注微信公众号：鱼香ROS
- 欢迎添加机器人小伊微信，解锁机器人学习特殊服务
在本指南中，我们将了解如何将机器人的里程计系统与 Nav2 集成。 首先，我们将简要介绍里程计，以及为使 Nav2 正常运行而需要发布的必要消息和转换。 接下来，我们将展示如何使用两种不同的情况设置里程计。 在第一种情况下，我们将展示如何为具有可用车轮编码器的机器人设置里程计系统。 在第二种情况下，我们将使用 Gazebo 在 sam_bot（我们在上一节中构建的机器人）上构建一个模拟运行中的里程计系统的演示。之后，我们将讨论如何使用robot_localization 包融合各种里程计来源以提供平滑的里程计。 最后，我们还将展示如何使用 robot_localization 发布odom => base_link 转换。 sam_bot (机器人我们建立在上一节) 使用Gazebo。之后，我们将讨论如何使用“robot_localization”包融合各种里程计来源以提供平滑的里程计。 最后，我们还将展示如何使用 robot_localization 发布odom => base_link 转换 。 [songhuangong@1695]
参见：本教程中的完整源代码可以在 sam_bot_description 包下的 navigation2_tutorials 存储库中找到。 请注意，在完成本指南中的所有教程后，存储库包含完整的代码。 [songhuangong@1696]
里程计系统根据机器人的运动提供对机器人姿态和速度的本地地准确估计。里程计信息可以从各种来源获得，例如 IMU、LIDAR、RADAR、VIO 和车轮编码器。需要注意的一点是，IMU会随着时间而漂移，而车轮编码器会随着行驶距离而漂移，因此它们经常一起使用以抵消彼此的负面特性。 [songhuangong@1698]
“odom”框架和与之相关的转换，使用的是机器人的里程计系统来发布连续的定位信息，但随着时间或距离（取决于传感器模式和漂移）变得不那么准确。尽管如此，机器人仍然可以使用该信息来导航其紧邻的位置（例如避免碰撞）。为了获得一段时间内始终如一的准确里程表信息，“map”框架提供了用于校正 odom 框架的全局准确信息。 [songhuangong@1699]
正如前面的指南和REP 105 _ 中所讨论的，odom 框架通过 odom => base_link 转换连接到系统的其余部分和 Nav2。此转换由 tf2 发布或诸如“robot_localization”之类的框架发布，“robot_localization”还提供了额外的功能。我们将在下一节中更多地讨论“robot_localization”。 [songhuangong@1700]
除了所需的“odom”=>“base_link”转换，Nav2 还需要发布“nav_msgs/Odometry”消息，因为该消息提供了机器人的速度信息。详细来说，nav_msgs/Odometry 消息包含以下信息： [songhuangong@1701]
# This represents estimates of position and velocity in free space.
# The pose in this message should be specified in the coordinate frame given by header.frame_id
# The twist in this message should be specified in the coordinate frame given by the child_frame_id
# Includes the frame id of the pose parent.
std_msgs/Header header
# Frame id the pose is pointing at. The twist is in this coordinate frame.
string child_frame_id
# Estimated pose that is typically relative to a fixed world frame.
geometry_msgs/PoseWithCovariance pose
# Estimated linear and angular velocity relative to child_frame_id.
geometry_msgs/TwistWithCovariance twist
这条信息告诉我们机器人的姿势和速度的估计。 header 消息在给定的坐标系中提供带时间戳的数据。 pose 信息提供了机器人相对于 header.frame_id 中指定的frame的位置和方向。twist 消息给出了相对于child_frame_id 中定义的frame的线速度和角速度。 [songhuangong@1702]
为您的物理机器人设置 Nav2 的里程计系统在很大程度上取决于您的机器人可以使用哪些里程计传感器。由于您的机器人可能有大量配置，具体设置说明将不在本教程的范围内。相反，我们将提供一些基本示例和有用的资源来帮助您为 Nav2 配置您的机器人。 [songhuangong@1704]
首先，我们将使用一个带有车轮编码器的机器人作为其里程计源的示例。请注意，Nav2 不需要车轮编码器，但在大多数设置中都很常见。设置里程计的目标是计算里程计信息并发布 nav_msgs/Odometry 消息和 odom => base_link 在 ROS 2 上的转换。要计算此信息，您需要设置一些将车轮编码器信息转换为里程信息的代码，类似于下面的代码段： [songhuangong@1705]
linear = (right_wheel_est_vel + left_wheel_est_vel) / 2
angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation;
变量 right_wheel_est_vel 和 left_wheel_est_vel 分别是左右车轮的估计速度，wheel separation 是车轮之间的距离。 right_wheel_est_vel 和 left_wheel_est_vel 的值可以通过简单地获取车轮关节位置随时间的变化来获得。然后，此信息可用于发布到 Nav2 。有关如何执行此操作的基本示例可以在位于here _的里程计导航文档中找到 [小鱼@1706]
我们推荐的手动发布此信息的替代方法是通过“ros2_control”框架。 ros2_control 框架包含用于实时控制ROS 2 机器人的各种包。对于车轮编码器，ros2_control 在ros2_controller 包下有diff_drive_controller（差动驱动控制器）。diff_drive_controller 接收在 cmd_vel 主题上发布的 geometry_msgs/Twist 消息，计算里程信息，并在 odom 主题上发布 nav_msgs/Odometry 消息。其他处理不同类型传感器的包也可以在 ros2_control 中找到。 [songhuangong@1707]
参见： 有关更多信息，请参见 ros2_control文档 和 diff_drive_controller的Github仓库。 [小鱼@1708]
对于其他类型的传感器，如 IMU、VIO 等，它们各自的 ROS 驱动程序应该有关于如何发布里程信息的文档。请记住，Nav2 需要发布 nav_msgs/Odometry 消息和 odom => base_link 转换，这应该是您设置里程计系统时的目标。 [songhuangong@1709]
在本节中，我们将使用 Gazebo 来模拟 sam_bot 的里程计系统，这是我们在本教程系列的上一节中构建的机器人。您可以先阅读该指南或在 此处 获取完整的源代码。 [songhuangong@1711]
注解
如果您正在开发自己的物理机器人并且已经设置了里程计传感器，您可以选择跳过本节并进入下一节，这节中我们将融合 IMU 和里程计消息以提供流畅的 odom => ` base_link 转换。 [songhuangong@1712]
作为本节的概述，我们将首先设置 Gazebo 以及使其与 ROS 2 一起使用所需的必要软件包。接下来，我们将添加 Gazebo 插件，用于模拟 IMU 传感器和差分驱动里程计系统，以便发布sensor_msgs/Imu 和 nav_msgs/Odometry 消息。最后，我们将在 Gazebo 环境中生成“sam_bot”，并在 ROS 2中 验证发布的“sensor_msgs/Imu”和“nav_msgs/Odometry”消息。 [songhuangong@1713]
Gazebo 是一个 3D 模拟器，可以让我们观察我们的虚拟机器人在模拟环境中的功能。要开始在 ROS 2 中使用 Gazebo，请按照 Gazebo 安装文档 中的安装说明进行操作。 [songhuangong@1715]
我们还需要安装 gazebo_ros_pkgs 包来模拟里程计并在 Gazebo 中使用 ROS 2 控制机器人： [songhuangong@1716]
sudo apt install ros--gazebo-ros-pkgs
您可以按照 此处 的说明测试您是否已成功设置 ROS2 和 Gazebo 环境。 [songhuangong@1717]
请注意，我们使用 URDF 描述了 sam_bot。然而，Gazebo 使用 Simulation Description Format (SDF) 来描述模拟环境中的机器人。幸运的是Gazebo 自动将兼容的 URDF 文件转换为 SDF。 URDF 与 Gazebo 兼容的主要要求是在每个 元素中都有一个 元素。这个要求已经在 sam_bot 的 URDF 文件中得到满足，所以它已经可以在 Gazebo 中使用了。 [songhuangong@1718]
参见： 有关如何在Gazebo中使用 URDF 的更多信息，请参阅 教程：在 Gazebo 中使用 URDF 。 [songhuangong@1719]
将 Gazebo 插件添加到 URDF [songhuangong@1720]我们现在将 IMU 传感器和 Gazebo 的差分驱动插件添加到我们的 URDF。有关 Gazebo 中可用的不同插件的概述，请查看 Tutorial: Using Gazebo plugins with ROS。 [songhuangong@1721]
对于我们的机器人，我们将使用 GazeboRosImuSensor 这是一个 传感器插件。 传感器插件必须附加到link，因此我们将创建 IMU 传感器将附加到的imu_link。此链接将在 元素下引用。接下来，我们将 /demo/imu 设置为 IMU 将发布其信息的主题，我们将遵守 REP145 通过将initalOrientationAsReference 设置为false。我们还将使用 Gazebo 的 传感器噪声模型 为传感器配置添加一些噪声。 [songhuangong@1722]
现在，我们将根据上面的描述设置我们的 IMU 传感器插件，方法是在 URDF 中的 行之前添加以下行： [songhuangong@1723]
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158 /demo
159 ~/out:=imu
160
161 false
162
163 true
164 100
165 true
166
167
168
169
170 0.0
171 2e-4
172 0.0000075
173 0.0000008
174
175
176
177
178 0.0
179 2e-4
180 0.0000075
181 0.0000008
182
183
184
185
186 0.0
187 2e-4
188 0.0000075
189 0.0000008
190
191
192
193
194
195
196 0.0
197 1.7e-2
198 0.1
199 0.001
200
201
202
203
204 0.0
205 1.7e-2
206 0.1
207 0.001
208
209
210
211
212 0.0
213 1.7e-2
214 0.1
215 0.001
216
217
218
219
220
221
现在，让我们添加差分驱动模型插件。我们将配置插件，以便在 /demo/odom 主题上发布nav_msgs/Odometry 消息。左右轮的关节将设置为sam_bot的轮关节。车轮间距和车轮直径分别根据wheel_ygap和wheel_radius的定义值设置。 [songhuangong@1724]
要将这个插件包含在我们的 URDF 中，请在IMU插件的 标签之后添加以下行： [songhuangong@1725]
223
224
225
226 /demo
227
228
229
230 drivewhl_l_joint
231 drivewhl_r_joint
232
233
234 0.4
235 0.2
236
237
238 20
239 1.0
240
241
242 true
243 false
244 true
245
246 odom
247 base_link
248
249
我们现在将编辑我们的启动文件，launch/display.launch.py，以生成Gazebo 中的“sam_bot”。由于我们将模拟我们的机器人，我们可以通过删除 generate_launch_description() 中的以下行来删除联合状态发布者的GUI： [songhuangong@1727]
joint_state_publisher_gui_node = launch_ros.actions.Node(
package='joint_state_publisher_gui',
executable='joint_state_publisher_gui',
name='joint_state_publisher_gui',
condition=launch.conditions.IfCondition(LaunchConfiguration('gui'))
)
删除 return launch.LaunchDescription 中的以下行。 [songhuangong@1728]
joint_state_publisher_gui_node,
接下来，打开 package.xml 并删除该行： [songhuangong@1729]
joint_state_publisher_gui
要启动 Gazebo，请在“joint_state_publisher_node”这行之前添加以下内容 [songhuangong@1730]
launch.actions.ExecuteProcess(cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'], output='screen'),
我们现在将在 Gazebo 中添加一个生成“sam_bot”的节点。再次打开 launch/display.launch.py 并将以下行粘贴到 return launch.LaunchDescription([ 之前。 [songhuangong@1731]
spawn_entity = launch_ros.actions.Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-entity', 'sam_bot', '-topic', 'robot_description'],
output='screen'
)
然后在 rviz_node``行之前添加行``spawn_entity, ，如下所示。 [songhuangong@1732]
robot_state_publisher_node,
spawn_entity,
rviz_node
])
让我们运行我们的包来检查 /demo/imu 和 /demo/odom 主题是否在系统中处于活动状态。 [songhuangong@1733]
导航到项目的根目录并执行以下行： [songhuangong@1734]
colcon build
. install/setup.bash
ros2 launch sam_bot_description display.launch.py
Gazebo 应该会启动，并且您应该会看到 sam_bot 的 3D 模型： [songhuangong@1735]
要查看系统中的活动 topic，请打开一个新终端并执行： [songhuangong@1736]
ros2 topic list
您应该在主题列表中看到 /demo/imu 和 /demo/odom 。 [songhuangong@1737]
要查看有关主题的更多信息，请执行： [songhuangong@1738]
ros2 topic info /demo/imu
ros2 topic info /demo/odom
您应该会看到类似于以下的输出： [songhuangong@1739]
Type: sensor_msgs/msg/Imu
Publisher count: 1
Subscription count: 0
Type: nav_msgs/msg/Odometry
Publisher count: 1
Subscription count: 0
观察/demo/imu 主题发布sensor_msgs/Imu 类型的消息，/demo/odom 主题发布nav_msgs/Odometry 类型的消息。关于这些主题发布的信息分别来自 gazebo的 IMU 传感器和差分驱动器的 模拟。另请注意，这两个主题目前都没有订阅者。在下一节中，我们将创建一个 robot_localization 节点来订阅这两个主题。然后，它将使用在这两个主题上发布的消息为 Nav2 提供融合后的局部准确且平滑的里程计信息。 [songhuangong@1740]
robot_localization 包 用于 并融合从 N 个 里程计传感器输入的数据，并提供局部准确的平滑里程计信息。这些信息可以通过 nav_msgs/Odometry, sensor_msgs/Imu , geometry_msgs/PoseWithCovarianceStamped 和 geometry_msgs/TwistWithCovarianceStamped 消息提供给包。 [songhuangong@1742]
通常的机器人设置至少包括车轮编码器和 IMU 作为其里程计传感器源。 当为robot_localization 提供多个源时，它能够通过使用状态估计节点来融合传感器给出的里程信息。 这些节点使用扩展卡尔曼滤波器（ ekf_node ）或无迹卡尔曼滤波器（ ukf_node ）来实现这种融合。 此外，该软件包还实现了一个navsat_transform_node ，它在使用 GPS 时将地理坐标转换为机器人的世界坐标系。 [songhuangong@1743]
如果在其配置中启用的话，融合的传感器数据由 robot_localization 包通过 odometry/filtered 和 accel/filtered 主题发布. 此外，它还可以向发布 /tf 主题发布 odom => [](https://dev.nav2.fishros.com/doc/setup_guides/odom/setup_odom.html#id1)base_link的 转换。 [songhuangong@1744]
参见：关于 robot_localization 的更多细节可以在官方的 Robot Localization Documentation中找到。 [songhuangong@1745]
如果您的机器人只能提供一个里程计源，则使用robot_localization 除了平滑之外，影响很小。 在这种情况下，另一种方法是通过 tf2 广播器在您的单一里程计节点源中发布转换。 尽管如此，您仍然可以选择使用 robot_localization 来发布变换，并且在输出中仍然可以观察到一些平滑属性。 [songhuangong@1746]
参见：有关如何编写 tf2 广播器的更多信息，您可以查看编写 tf2 广播器 (C++) (Python) [songhuangong@1747]
在本节的其余部分，我们将展示如何使用“robot_localization”来融合“sam_bot”的传感器。 它将在 /demo/Imu 上发布的 sensor_msgs/Imu 消息和在 /demo/odom 上发布 nav_msgs/Odometry 消息，然后它将在 /demo/odom 上发布数据 `odometry/filtered、accel/filtered 和 /tf 主题。 [songhuangong@1748]
配置Robot Localization [songhuangong@1749]现在让我们配置 robot_localization 包以使用扩展卡尔曼滤波器（ekf_node）来融合里程信息并发布 odom => base_link 变换。 [songhuangong@1750]
首先，安装 robot_localization 包，使用您的机器包管理器或执行以下命令 ： [songhuangong@1751]
sudo apt install ros--robot-localization
接下来，我们使用YAML文件指定 ekf_node 的参数。在项目的根目录下创建一个名为 config 的目录，并创建一个名为 ekf.yaml 的文件。将以下代码行复制到您的 ekf.yaml 文件中： [songhuangong@1752]
### ekf config file ###
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 theinputs. It will then run continuously at the
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
frequency: 30.0
# 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.
two_d_mode: false
# Whether to publish the acceleration state. Defaults to false if unspecified.
publish_acceleration: true
# Whether to broadcast the transformation over the /tf topic. Defaultsto true if unspecified.
publish_tf: true
# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of odom_frame.
# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set "world_frame"
# to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark
# observations) then:
# 3a. Set your "world_frame" to your map_frame value
# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state estimation node
# from robot_localization! However, that instance should *not* fuse the global data.
map_frame: map # Defaults to "map" if unspecified
odom_frame: odom # Defaults to "odom" if unspecified
base_link_frame: base_link # Defaults to "base_link" ifunspecified
world_frame: odom # Defaults to the value ofodom_frame if unspecified
odom0: demo/odom
odom0_config: [true, true, true,
false, false, false,
false, false, false,
false, false, true,
false, false, false]
imu0: demo/imu
imu0_config: [false, false, false,
true, true, true,
false, false, false,
false, false, false,
false, false, false]
在此配置中，我们定义参数值 frequency ， two_d_mode ， publish_acceleration ， publish_tf ， map_frame ， odom_frame ， base_link_frame ， world_frame 。有关其他参数可以修改，看到 Parameters of state estimation nodes ，样品 efk.yaml 可以发现 here 。 [songhuangong@1753]
要将传感器输入添加到ekf_filter_node，请将序列中的下一个数字添加到其基本名称（odom、imu、pose、twist）。在我们的例子中，我们有一个nav_msgs/Odometry和一个sensor_msgs/Imu作为过滤器的输入，因此我们使用odom0和imu0。我们将odom0的值设置为demo/odom，这是发布nav_msgs/Odometry的主题。类似地，我们将imu0的值设置为发布sensor_msgs/Imu的主题，即demo/imu。 [songhuangong@1754]
您可以使用_config 参数指定过滤器将使用来自传感器的哪些值。 该参数的取值顺序为x、y、z、roll、pitch、yaw、vx、vy、vz、vroll、vpitch、vyaw、ax、ay、az。 在我们的示例中，我们将odom0_config中的所有内容设置为false，除了第 1、2、3 和 12 个条目，这意味着过滤器将仅使用 x、y、z 和 vyaw 值 odom0。 [songhuangong@1755]
在imu0_config矩阵中，您会注意到只使用了翻滚、俯仰和偏航。 典型的移动机器人级 IMU 还将提供角速度和线性加速度。 为了让 robot_localization 正常工作，你不应该融合多个相互衍生的字段。 由于角速度在 IMU 内部融合以提供翻滚、俯仰和偏航估计，因此我们不应融合用于推导该信息的角速度。 我们也不会融合角速度，因为它在不使用高质量（且昂贵）的 IMU 时具有噪声特性。 [songhuangong@1756]
参见： 有关将输入数据配置到 robot_localization 的更多建议，请参阅“准备您的数据以用robots_localization base_link 转换。 在单独的命令行终端中运行以下命令： [songhuangong@1772]
ros2 run tf2_ros tf2_echo odom base_link
您应该会看到类似于下面所示的连续输出。 [小鱼@1773]
At time 8.842000000
- Translation: [0.003, -0.000, 0.127]
- Rotation: in Quaternion [-0.000, 0.092, 0.003, 0.996]
At time 9.842000000
- Translation: [0.002, -0.000, 0.127]
- Rotation: in Quaternion [-0.000, 0.092, 0.003, 0.996]
在本指南中，我们讨论了Nav2期望的来自里程计系统的信息和变换。我们已经看到了如何建立里程计系统以及如何验证发布的消息。我们还讨论了如何使用多个里程计传感器来提供使用 robot_localization 的过滤和平滑里程计。我们还检查了 robot_localization 是否正确发布了 odom => base_link 变换。 [待校准@1774]
- 本文主要校准贡献：songhuangong
- 本文遵循知识共享协议，禁止未授权商用转载
