使用 qrb_ros_follow_path 服务测试路径跟随 - Qualcomm Dragonwing Documentation

主要特性包括：

地图上的路径点和虚拟路径管理。
到目标路径点的路径跟随导航。
路径跟随期间的避障。

软件结构

下图展示了 qrb_ros_follow_path 服务的软件结构：

Layered stack on RBx with 2D lidar ROS, robot base, 2D lidar SLAM, follow path service, and follow path manager running on ROS2 Jazzy. — qrb_ros_follow_path 的软件结构。

`qrb_ros_follow_path` 服务中使用的 ROS 节点和模块

下表描述了 qrb_ros_follow_path 服务中使用的 ROS 节点和模块。

节点名	说明
qrb_ros_follow_path	此 ROS2 包实现了用于路径点、虚拟路径、路径跟随导航、位置和激光雷达数据的动作/服务客户端和服务器、发布者和订阅者。
qrb_follow_path_manager	这是一个 C++ 库，提供用于路径点和虚拟路径管理以及路径跟随导航的 API。 • Manager：库接口。 • Virtual Path Manager：管理路径点和虚拟路径，并计算路径跟随。 • Follow Path Planner：向 Robot Base Controller 发送线速度和角速度，检查跟随路径上的障碍物，并支持动态避障。 • Obstacle Detector：使用激光扫描和栅格图检测动态障碍物。 • PID Controller：计算速度并检查是否到达目标位姿。
Robot Base and Robot Base Controller	这是一个用于自主移动机器人 (AMR) 底座控制的 ROS2 包。
2D lidar SLAM ROS 和 2D lidar SLAM	提供建图和定位。
2D lidar ROS	提供激光雷达数据。

`qrb_ros_follow_path_service` API

下表描述了 qrb_ros_follow_path_service API。

接口	名称	类型	说明
Subscriber	`developer_mode`	`std_msgs::msg::Int16`	接收调试信息。
Subscriber	`robot_base_exception`	`qrb_ros_robot_base_msgs::msg::Exception`	接收异常信息。
Service server	`virtual_path`	`qrb_ros_navigation_msgs::srv::VirtualPath`	管理路径点或虚拟路径。
Service server	`compute_follow_path`	`qrb_ros_navigation_msgs::srv::ComputeFollowPath`	计算路径跟随，包括经过的路径点。
Service server	`follow_path_sub_cmd`	`qrb_ros_amr_msgs::srv::SubCmd`	取消、暂停或恢复路径跟随。
Action server	`wfollowpath`	`qrb_ros_navigation_msgs::action::FollowPath`	请求路径跟随导航。

前提条件

你已根据安装 QIR SDK完成设备设置，并在设备上安装了 ROS2 Jazzy 和 Qualcomm Intelligent Robotics (QIR) SDK。

开箱即用运行 `qrb_ros_follow_path` 服务

除非另有说明，否则在设备上运行以下命令。

安装 qrb_ros_follow_path 服务包

sudo apt install ros-jazzy-nav2-msgs
sudo apt install ros-jazzy-nav-2d-msgs
sudo apt install ros-jazzy-qrb-ros-amr-msgs
sudo apt install ros-jazzy-qrb-ros-navigation-msgs
sudo apt install ros-jazzy-qrb-follow-path-manager
sudo apt install ros-jazzy-qrb-ros-follow-path

设置 2D 激光雷达 SLAM

通过完成开箱即用 2D 激光雷达 SLAM 中的步骤 1-6 来设置 2D 激光雷达 SLAM。确保已完成建图。

在设备上设置示例环境

source /opt/ros/jazzy/setup.bash
source install/setup.bash
ros2 launch qrb_ros_follow_path qrb_ros_follow_path_bringup.launch.py

测试路径跟随服务

你可以根据 qrb_ros_follow_path_service API 表实现一个 ROS 节点，以完成路径跟随导航。
你也可以使用这些 ROS 命令完成路径跟随导航。

a. 在主机（Ubuntu 24.04）上安装 rqt-robot-steering。

sudo apt install ros-jazzy-rqt-robot-steering

b. 在主机上运行 rqt-robot-steering。

ros2 run rqt_robot_steering rqt_robot_steering

c. 使用 rqt_robot_steering 将机器人底座移动到预期位置。d. 在主机（Ubuntu 24.04）上安装并设置 rviz2。

sudo apt install ros-jazzy-rviz2
ros2 run rviz2 rviz2

e. 从 rviz 获取位姿并将其转换为 2D 位置 (x, y, angle)。f. 使用当前位置添加一个路径点。例如，如果当前位置是 (0.1, 0.3, 3.0)，使用以下命令：

ros2 service call /virtual_path qrb_ros_navigation_msgs/srv/VirtualPath "{api_id: 2, waypoint: {x: 0.1, y: 0.3, z: 3.0}}"

从服务响应中获取路径点 ID。例如，路径点 ID 为 1。g. 使用 rqt_robot_steering 将机器人底座移动到另一个位置。h. 从 rviz 获取位姿并将其转换为 2D 位置 (x, y, angle)。i. 使用当前位置添加一个路径点。例如，如果当前位置是 (0.3, 0.1, 3.0)，使用以下命令：

ros2 service call /virtual_path qrb_ros_navigation_msgs/srv/VirtualPath "{api_id: 2, waypoint: {x: 0.3, y: 0.1, z: 3.0}}"

从服务响应中获取路径点 id。例如，路径点 ID 为 2。j. 设置路径点 (id = 2) 和路径点 (id = 1) 之间的虚拟路径。

ros2 service call /virtual_path qrb_ros_navigation_msgs/srv/VirtualPath "{api_id: 6, waypoint_id: 1, adjacent_waypoints: [2]}"

k. 启动从路径点 (id = 2) 到路径点 (id = 1) 的路径跟随操作。

ros2 action send_goal /wfollowpath qrb_ros_navigation_msgs/action/FollowPath "{goal: 1}"

从源码构建并运行 `follow-path` 服务

当你需要修改服务代码时，可以从源码构建 follow-path 服务。设备端步骤包括安装依赖、克隆源码仓库、构建包并运行结果。

设备步骤

安装依赖

sudo apt install ros-jazzy-nav2-msgs
sudo apt install ros-jazzy-nav-2d-msgs

下载并构建

source /opt/ros/jazzy/setup.bash
git clone https://github.com/qualcomm-qrb-ros/qrb_ros_follow_path_service.git
git clone https://github.com/qualcomm-qrb-ros/qrb_ros_interfaces.git
colcon build

运行并测试服务

根据开箱即用运行 qrb_ros_follow_path 服务的步骤 2 到步骤 4 运行并测试。

​软件结构

​qrb_ros_follow_path 服务中使用的 ROS 节点和模块

​qrb_ros_follow_path_service API

​前提条件

​开箱即用运行 qrb_ros_follow_path 服务

​从源码构建并运行 follow-path 服务

​设备步骤

软件结构

`qrb_ros_follow_path` 服务中使用的 ROS 节点和模块

`qrb_ros_follow_path_service` API

前提条件

开箱即用运行 `qrb_ros_follow_path` 服务

从源码构建并运行 `follow-path` 服务

设备步骤