Navigation 2 对外提供了动作服务用于导航调用。动作通信是 ROS 2 四大通讯机制之一，前面我们并没有介绍，我们以导航调用为例来简单了解下它。 动作通信和其名字一样，主要用于控制场景，它的优点在于其反馈机制，当客户端发送目标给服务端后，除了等待服务端处理完成，还可以收到服务端的处理进度。启动导航后在终端中使用动作相关命令可以查看当前系统所有动作列表，命令及结果如下： ``` ros2 action list -- /assisted_teleop /backup /compute_path_through_poses /compute_path_to_pose /drive_on_heading /follow_path /follow_waypoints /navigate_through_poses /navigate_to_pose /smooth_path /spin /wait ``` 其中 /navigate_to_pose 就是用于处理导航到点请求的动作。继续使用命令查看该动作的具体信息。 ``` ros2 action info /navigate_to_pose -t --- Action: /navigate_to_pose Action clients: 4 /bt_navigator [nav2_msgs/action/NavigateToPose] /waypoint_follower [nav2_msgs/action/NavigateToPose] /rviz2 [nav2_msgs/action/NavigateToPose] /rviz2 [nav2_msgs/action/NavigateToPose] Action servers: 1 /bt_navigator [nav2_msgs/action/NavigateToPose] ``` 可以看到该动作的客户端、服务端以及消息接口情况，使用指令查看 nav2_msgs/action/NavigateToPose 接口定义，命令及结果如下： ``` ros2 interface show nav2_msgs/action/NavigateToPose --- #goal definition geometry_msgs/PoseStamped pose std_msgs/Header header builtin_interfaces/Time stamp int32 sec uint32 nanosec string frame_id Pose pose Point position float64 x float64 y float64 z Quaternion orientation float64 x 0 float64 y 0 float64 z 0 float64 w 1 string behavior_tree --- #result definition std_msgs/Empty result --- #feedback definition geometry_msgs/PoseStamped current_pose std_msgs/Header header builtin_interfaces/Time stamp int32 sec uint32 nanosec string frame_id Pose pose Point position float64 x float64 y float64 z Quaternion orientation float64 x 0 float64 y 0 float64 z 0 float64 w 1 builtin_interfaces/Duration navigation_time int32 sec uint32 nanosec builtin_interfaces/Duration estimated_time_remaining int32 sec uint32 nanosec int16 number_of_recoveries float32 distance_remaining ``` 从该接口定义可以看出，动作消息的接口分为目标、结果和反馈三个部分，相比服务通信接口的目标和结果多出了反馈这一部分。如果在机器人导航时仔细观察 RViz 左下角 Navigation 2 部分，你会发现它会实时的显示当前导航所花费的时间，距离目标点之间的距离，花费的时间以及脱困次数，这些数据就是来自动作服务的反馈部分。 使用命令行工具可以发送动作请求并接收反馈和结果，以请求机器人移动到地图的指定目标点为例，命令及反馈如下。 ``` ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "{pose: {header: {frame_id: map}, pose: {position: {x: 2, y: 2}}}}" --feedback --- Waiting for an action server to become available... Sending goal: pose: header: stamp: sec: 0 nanosec: 0 frame_id: map pose: position: x: 2.0 y: 2.0 z: 0.0 orientation: x: 0.0 y: 0.0 z: 0.0 w: 1.0 behavior_tree: '' Goal accepted with ID: c5c52646de774f55b4ee71ca5ed3267a ... Feedback: current_pose: header: stamp: sec: 4679 nanosec: 504000000 frame_id: map pose: position: x: 2.079677095742107 y: 1.947119186243544 z: 0.09189999999999998 orientation: x: 0.0 y: 0.0 z: 0.054114175706008266 w: 0.998534754521674 navigation_time: sec: 18 nanosec: 164000000 estimated_time_remaining: sec: 0 nanosec: 0 number_of_recoveries: 2 distance_remaining: 0.10830961167812347 Result: result: {} Goal finished with status: SUCCEEDED ``` 上面的命令用于请求机器人移动到地图坐标系中的 xy 都为 2.0 的点，反馈结果中，Sending goal 部分表示目标，Feedback 部分是反馈，Result 部分为最终结果。 要灵活使用自然还需要学习如何用代码调用，在 Python 中 nav2_simple_commander 库已经将动作客户端代码封装到 BasicNavigator 节点中，使用该节点的对应函数就可以实现导航操作。在 src/fishbot_application/fishbot_application 下新建文件 `nav_to_pose.py`，然后编写如下代码： ``` from geometry_msgs.msg import PoseStamped from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy from rclpy.duration import Duration def main(): rclpy.init() navigator = BasicNavigator() # 等待导航启动完成 navigator.waitUntilNav2Active() # 设置目标点坐标 goal_pose = PoseStamped() goal_pose.header.frame_id = 'map' goal_pose.header.stamp = navigator.get_clock().now().to_msg() goal_pose.pose.position.x = 1.0 goal_pose.pose.position.y = 1.0 goal_pose.pose.orientation.w = 1.0 # 发送目标接收反馈结果 navigator.goToPose(goal_pose) while not navigator.isTaskComplete(): feedback = navigator.getFeedback() navigator.get_logger().info( f'预计: {Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9} s 后到达') # 超时自动取消 if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0): navigator.cancelTask() # 最终结果判断 result = navigator.getResult() if result == TaskResult.SUCCEEDED: navigator.get_logger().info('导航结果：成功') elif result == TaskResult.CANCELED: navigator.get_logger().warn('导航结果：被取消') elif result == TaskResult.FAILED: navigator.get_logger().error('导航结果：失败') else: navigator.get_logger().error('导航结果：返回状态无效') ``` 上面的代码中有关键函数有四个，第一个是 navigator.goToPose 用于发布目标，第二个是 navigator.getFeedback() 用于获取状态反馈，第三个是 navigator.cancelTask() 用于中途取消，第四个是 navigator.getResult() 用于获取最终结果。跳转到 goToPose 的源码可以发现最终发送请求是通过 self.nav_to_pose_client.send_goal_async 函数完成的，而 nav_to_pose_client 就是在 BasicNavigator 函数中定义的动作客户端，定义代码如下： ``` self.nav_to_pose_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') ``` 保存好代码，注册该节点并重新构建功能包，再次运行仿真和导航，初始化位姿后启动该节点，观察 RViz 中机器人运动情况，启动命令及终端打印如下。 ``` ros2 run fishbot_application nav_to_pose --- [INFO] [1685599886.677153365] [basic_navigator]: Nav2 is ready for use! [INFO] [1685599886.707280550] [basic_navigator]: Navigating to goal: 1.0 1.0... [INFO] [1685599886.852565845] [basic_navigator]: 预计剩余: 0.0 s [INFO] [1685599891.432155882] [basic_navigator]: 预计剩余: 170.591972225 s [INFO] [1685599891.532646643] [basic_navigator]: 预计剩余: 98.418445514 s [INFO] [1685599891.635079916] [basic_navigator]: 预计剩余: 77.541805557 s [INFO] [1685599914.104374413] [basic_navigator]: 预计剩余: 1.833780316 s ... [INFO] [1685599918.156745102] [basic_navigator]: 导航结果：成功 ``` 使用 Python 可以通过调用 nav2_simple_commander 库方便的实现导航，但如果项目需要换成 C++ 也并不复杂，使用动作客户端也可以方便的调用，如何实现我并不打算再操作一遍，但我将提供给你一份详细的 C++ 调用导航服务的实现代码。 ``` #include #include "nav2_msgs/action/navigate_to_pose.hpp" // 导入导航动作消息的头文件 #include "rclcpp/rclcpp.hpp" // 导入ROS 2的C++客户端库 #include "rclcpp_action/rclcpp_action.hpp" // 导入ROS 2的C++ Action客户端库 using NavigationAction = nav2_msgs::action::NavigateToPose; // 定义导航动作类型为NavigateToPose class NavToPoseClient : public rclcpp::Node { public: using NavigationActionClient = rclcpp_action::Client; // 定义导航动作客户端类型 using NavigationActionGoalHandle = rclcpp_action::ClientGoalHandle; // 定义导航动作目标句柄类型 NavToPoseClient() : Node("nav_to_pose_client") { // 创建导航动作客户端 action_client_ = rclcpp_action::create_client( this, "navigate_to_pose"); } void sendGoal() { // 等待导航动作服务器上线，等待时间为5秒 while (!action_client_->wait_for_action_server(std::chrono::seconds(5))) { RCLCPP_INFO(get_logger(), "等待Action服务上线。"); } // 设置导航目标点 auto goal_msg = NavigationAction::Goal(); goal_msg.pose.header.frame_id = "map"; // 设置目标点的坐标系为地图坐标系 goal_msg.pose.pose.position.x = 2.0f; // 设置目标点的x坐标为2.0 goal_msg.pose.pose.position.y = 2.0f; // 设置目标点的y坐标为2.0 auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); // 设置请求目标结果回调函数 send_goal_options.goal_response_callback = [this](NavigationActionGoalHandle::SharedPtr goal_handle) { if (goal_handle) { RCLCPP_INFO(get_logger(), "目标点已被服务器接收"); } }; // 设置移动过程反馈回调函数 send_goal_options.feedback_callback = [this]( NavigationActionGoalHandle::SharedPtr goal_handle, const std::shared_ptr feedback) { (void)goal_handle; // 假装调用，避免 warning: unused RCLCPP_INFO(this->get_logger(), "反馈剩余距离:%f", feedback->distance_remaining); }; // 设置执行结果回调函数 send_goal_options.result_callback = [this](const NavigationActionGoalHandle::WrappedResult& result) { if (result.code == rclcpp_action::ResultCode::SUCCEEDED) { RCLCPP_INFO(this->get_logger(), "处理成功！"); } }; // 发送导航目标点 action_client_->async_send_goal(goal_msg, send_goal_options); } NavigationActionClient::SharedPtr action_client_; }; int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = std::make_shared(); node->sendGoal(); rclcpp::spin(node); rclcpp::shutdown(); return 0; } ```