8. 导航与规划消息

本文详述 planning_msgs、nav_msgs（proto Action/Service）、vision_msgs、visualization_msgs 等导航栈上层消息。

8.1 planning_msgs

头文件：autonomy/commsgs/planning_msgs.hpp

导航栈路径与里程计的核心消息包。

8.1.1 Path

字段	类型	说明
`header`	`std_msgs::Header`	路径坐标系（通常 `"map"`）
`poses`	`vector<geometry_msgs::PoseStamped>`	有序路径点

commsgs::planning_msgs::Path path;
path.header.stamp = commsgs::builtin_interfaces::Time::Now();
path.header.frame_id = "map";

commsgs::geometry_msgs::PoseStamped wp;
wp.header = path.header;
wp.pose.position.x = 5.0;
wp.pose.orientation.w = 1.0;
path.poses.push_back(wp);

使用方：PlannerServer 输出、ControllerServer 输入、可视化模块。

8.1.2 Odometry

字段	类型	说明
`header`	`std_msgs::Header`	`frame_id` = 里程计坐标系（`"odom"`）
`child_frame_id`	`string`	机器人坐标系（`"base_link"`）
`pose`	`geometry_msgs::PoseWithCovariance`	位姿估计
`twist`	`geometry_msgs::TwistWithCovariance`	速度估计

8.1.3 Goals

字段	类型	说明
`header`	`std_msgs::Header`	目标坐标系
`goals`	`vector<geometry_msgs::PoseStamped>`	多目标点序列

8.1.4 转换完整度

类型	ToProto	FromProto
`Path`	✅	✅
`Odometry`	⏳ 部分	⏳ 部分
`Goals`	⏳ 部分	⏳ 部分

8.2 nav_msgs

8.2.1 C++ 层

头文件：autonomy/commsgs/nav_msgs.hpp

类型	说明
`SpeedLimit`	速度限制（百分比或绝对值）

commsgs::nav_msgs::SpeedLimit limit;
limit.header.frame_id = "base_link";
limit.percentage = true;
limit.speed_limit = 50.0f;  // 50%

8.2.2 Proto 层 — Service

头文件：autonomy/commsgs/proto/nav_msgs.pb.h

Service	Request	Response
`IsPathValid`	`path`, `max_cost`, `consider_unknown_as_obstacle`	`is_valid`, `invalid_pose_indices`
`ClearEntireCostmap`	空	空

IsPathValid 已在 PlannerServer 中使用：

namespace nav_proto = commsgs::proto::nav_msgs;
using PathValidRequest = nav_proto::IsPathValid_Request;
using PathValidResponse = nav_proto::IsPathValid_Response;

8.2.3 Proto 层 — Action

Nav2 行为树 Action 均在 nav_msgs.proto 定义，统一结构：

message XxxAction {
  message Goal { ... }
  message Feedback { ... }
  message Result {
    error_code.ErrorCode error_code = 1;
    string error_msg = 2;
  }
}

Action	Goal 关键字段	Feedback 关键字段
`NavigateToPoseAction`	`PoseStamped pose`, `behavior_tree`	`current_pose`, `distance_remaining`, `number_of_recoveries`
`NavigateThroughPosesAction`	`repeated PoseStamped poses`	同上
`FollowPathAction`	`Path path`, `controller_id`	`current_pose`, `distance_to_goal`
`ComputePathToPoseAction`	`start`, `goal`, `planner_id`	—
`ComputePathThroughPosesAction`	`start`, `goals`, `planner_id`	—
`SmoothPathAction`	`Path path`, `smoother_id`	—
`FollowWaypointsAction`	`repeated PoseStamped poses`	`current_waypoint_index`
`SpinAction`	`target_yaw`, `time_allowance`	`angular_distance_traveled`
`BackUpAction`	`target_x`, `speed`, `time_allowance`	`distance_traveled`
`DriveOnHeadingAction`	`target_x`, `target_y`, `speed`	`distance_traveled`
`WaitAction`	`Duration time`	`time_left`
`AssistedTeleopAction`	`Duration time_allowance`	—
`DummyBehaviorAction`	—	—

Result 统一携带 error_code.ErrorCode + error_msg，与 §3.9 错误码分段对应。

8.2.4 WaypointStatus

字段	说明
`waypoint_index`	当前航点索引
`waypoint_status`	状态枚举

8.3 error_code

头文件：autonomy/commsgs/proto/error_code.pb.h

仅 proto 层，无 C++ struct 包装。

8.3.1 StatusPb

message StatusPb {
  ErrorCode error_code = 1;
  string msg = 2;
}

8.3.2 常用错误码

错误码	值	场景
`OK`	0	成功
`PLANNING_NO_PATH_FOUND`	6007	无可用路径
`CONTROL_NO_VALID_CMD`	1013	控制器无有效命令
`CONTROL_COLLISION`	1014	碰撞检测
`CONTROL_TF_ERROR`	1011	TF 变换失败
`TASK_CANCELED`	9003	任务取消
`TASK_PLANNER_FAILED`	9009	规划失败
`TASK_CONTROLLER_FAILED`	9010	控制失败
`MAP_NOT_READY`	7003	地图未就绪

完整列表见 autonomy/commsgs/proto/error_code.proto。

8.4 vision_msgs

头文件：autonomy/commsgs/vision_msgs.hpp

目标检测与分类消息，对齐 ROS vision_msgs。

8.4.1 2D 检测

类型	组成	说明
`Point2D`	`x, y` (double)	2D 点
`Pose2D`	`position` + `theta`	2D 位姿
`BoundingBox2D`	`center` + `size_x/y`	2D 包围盒
`ObjectHypothesis`	`class_id`, `score`	分类假设
`Detection2D`	`header` + `bbox` + `results` + `id`	单个 2D 检测
`Detection2DArray`	`header` + `detections`	检测数组

8.4.2 3D 检测

类型	组成	说明
`BoundingBox3D`	`center` (Pose) + `size` (Vector3)	3D 包围盒
`Detection3D`	`header` + `bbox` + `results` + `id`	单个 3D 检测
`Detection3DArray`	`header` + `detections`	检测数组

8.4.3 分类

类型	说明
`Classification`	`class_id` + `score`
`VisionClass`	`class_id` + `class_name`
`LabelInfo`	标签元数据
`VisionInfo`	视觉模块信息

8.4.4 使用方

autonomy/visualization 模块支持发布 Detection2DArray、BoundingBox3DArray 等到可视化服务。

8.5 visualization_msgs

头文件：autonomy/commsgs/visualization_msgs.hpp

RViz 风格可视化消息（转换函数为 stub）。

8.5.1 Marker

字段	说明
`header`	坐标系
`ns`	命名空间
`id`	唯一 ID
`type`	形状枚举（ARROW/CUBE/SPHERE/LINE_STRIP 等）
`action`	ADD / MODIFY / DELETE
`pose`	位姿
`scale`	尺寸 (Vector3)
`color`	颜色 (ColorRGBA)
`points`	点集（LINE_STRIP 等）
`colors`	逐点颜色
`text`	TEXT 类型文本
`lifetime`	显示时长

type 枚举常用值：

值	常量	形状
0	`ARROW`	箭头
1	`CUBE`	立方体
2	`SPHERE`	球体
3	`CYLINDER`	圆柱
4	`LINE_STRIP`	折线
5	`LINE_LIST`	线段列表
6	`CUBE_LIST`	立方体列表
7	`SPHERE_LIST`	球体列表
8	`POINTS`	点云
9	`TEXT_VIEW_FACING`	文字

8.5.2 其他类型

类型	说明
`MarkerArray`	Marker 批量
`InteractiveMarker`	可交互标记
`InteractiveMarkerControl`	交互控制
`InteractiveMarkerFeedback`	交互反馈
`MenuEntry`	右键菜单项
`ImageMarker`	图像上标记
`MeshFile`	3D 网格文件

8.6 其他消息包

8.6.1 trajectory_msgs

类型	说明
`JointTrajectoryPoint`	关节位置/速度/加速度
`JointTrajectory`	关节轨迹序列
`MultiDOFJointTrajectoryPoint`	多自由度轨迹点
`MultiDOFJointTrajectory`	多自由度轨迹

cpp 转换函数为空 stub，适用于机械臂场景。

8.6.2 diagnostic_msgs

类型	说明
`KeyValue`	键值对
`DiagnosticStatus`	诊断状态（OK/WARN/ERROR/STALE）
`DiagnosticArray`	诊断数组

8.6.3 shape_msgs

类型	说明
`SolidPrimitive`	基本几何体（BOX/SPHERE/CYLINDER/CONE/PLANE）
`Mesh`	三角网格
`Plane`	平面
`MeshTriangle`	三角面片

注意：当前 hpp 中 struct 命名空间有误，应为 shape_msgs 而非 builtin_interfaces。

8.6.4 vehicle_msgs

类型	说明
`RobotEvent`	机器人事件（空 struct，待扩展）
`RobotState`	机器人状态（空 struct，待扩展）

8.6.5 pcl_msgs

PCL 相关消息，当前为空 namespace，预留扩展。

8.7 导航栈消息依赖图

                    nav_msgs.proto (Action/Service)
                           │
              ┌────────────┼────────────┐
              ▼            ▼            ▼
      planning_msgs   geometry_msgs   error_code
              │            │
              ▼            ▼
           Path      PoseStamped / Twist
              │            │
              ▼            ▼
         Controller    Transform
              │            │
              ▼            ▼
          cmd_vel       TF tree

8.8 Action 使用模式（Autolink）

#include "autonomy/commsgs/proto/nav_msgs.pb.h"

using ActionT = commsgs::proto::nav_msgs::NavigateToPoseAction;
using Goal = ActionT::Goal;
using Feedback = ActionT::Feedback;
using Result = ActionT::Result;

auto client = node->CreateActionClient<ActionT>("navigate_to_pose");

Goal goal;
*goal.mutable_pose() = commsgs::geometry_msgs::ToProto(target_pose);
goal.set_behavior_tree("navigate_to_pose_w_replanning.xml");

client->SendGoal(goal,
    [](const Result& result) {
        if (result.error_code() != commsgs::proto::error_code::OK) {
            // 处理错误
        }
    },
    [](const Feedback& fb) {
        // distance_remaining = fb.distance_remaining()
    });

8.9 消息选择指南

需求	推荐消息	包
全局路径	`Path`	planning_msgs
单点目标	`PoseStamped`	geometry_msgs
多航点导航	`NavigateThroughPosesAction`	nav_msgs proto
路径跟踪	`FollowPathAction`	nav_msgs proto
路径有效性检查	`IsPathValid` service	nav_msgs proto
速度限制	`SpeedLimit`	nav_msgs
目标检测可视化	`Detection2DArray`	vision_msgs
路径可视化	`Marker` (LINE_STRIP)	visualization_msgs
操作结果	`error_code.ErrorCode`	error_code proto