ROS2 接口文档

本文档详细描述了 GR-Navigation 导航系统的所有ROS2接口，包括话题、服务和动作。

环境准备

请在每个新开的终端中，先初始化 ROS 2 与机器人 DDS 发现环境，再执行本文的 ros2 命令、Python 示例或 C++ 示例：

Bashsource /opt/fftai/fourierdds/shells/discovery_env.sh --ros2
export ROS_DOMAIN_ID=123

该脚本会加载 ROS 2 Humble 环境、配置机器人 DDS 发现，并设置 ROS 2 使用的 Fast DDS 中间件；ROS_DOMAIN_ID 需要按机器人当前配置显式设置。执行成功后，本文的标准消息类型和已部署的傅利叶 ROS 2 接口包（例如 fourier_msgs、map_manager）都会从 ROS 2 环境加载。

如果执行 ros2 interface show fourier_msgs/srv/GetCurrentAction 或其他本文引用的傅利叶接口时提示找不到包，说明机器人尚未部署 ROS 2 接口包；请先完成对应机器人软件版本的部署，再运行本文示例。

GRW 机器人默认在设备 namespace 下运行。先将实际 namespace 写入环境变量（例如本机可从 ros2 topic list 看到 /GRW01AA0001/...）：

Bashexport ROBOT_NAMESPACE=/GRW01AA0001  # 替换为实际机器人 namespace

本文 Python/C++ 示例全部使用相对接口名。运行时为进程设置 namespace，即可直接访问该机器人的接口：

Bashpython3 example.py --ros-args -r __ns:=$ROBOT_NAMESPACE
./example --ros-args -r __ns:=$ROBOT_NAMESPACE

命令行示例中的接口名则须显式加此前缀，例如 $ROBOT_NAMESPACE/slam/set_mode。若机器人没有 namespace，可将 ROBOT_NAMESPACE 设为空并去掉命令中的多余 /。

如果仅需要在不连接机器人的环境中编译或运行本地 ROS 2 程序，可执行：

Bashsource /opt/ros/humble/setup.bash
export ROS_DOMAIN_ID=123

特别注意：

如果导航系统在自定义 namespace 下运行，那么所有 node、topic、service、action 都会增加 namespace 前缀，但是消息体结构不会发生变化。

e.g.

无 namespace	有 namespace（`robot1`）
/slam/set_mode	/robot1/slam/set_mode
`ros2 service call $ROBOT_NAMESPACE/slam/set_mode fourier_msgs/srv/SetMode "{mode: 'mapping'}"`	`ros2 service call /robot1/slam/set_mode fourier_msgs/srv/SetMode "{mode: 'mapping'}"`
/slam/mode_status	/robot1/slam/mode_status
ros2 topic echo $ROBOT_NAMESPACE/slam/mode_status	ros2 topic echo /robot1/slam/mode_status

1. 建图定位模式切换

1.1 /slam/set_mode (服务)

在建图模式和定位模式之间切换

服务类型: fourier_msgs/srv/SetMode

调用示例:

Bash# 切换到建图模式
ros2 service call $ROBOT_NAMESPACE/slam/set_mode fourier_msgs/srv/SetMode "{mode: 'mapping'}"

# 切换到定位模式
ros2 service call $ROBOT_NAMESPACE/slam/set_mode fourier_msgs/srv/SetMode "{mode: 'localization'}"

Python调用:

Pythonfrom fourier_msgs.srv import SetMode
import rclpy
from rclpy.node import Node

class ModeClient(Node):
    def __init__(self):
        super().__init__('mode_client')
        self.client = self.create_client(SetMode, 'slam/set_mode')
        self.client.wait_for_service()

    def switch_mode(self, mode: str):
        request = SetMode.Request()
        request.mode = mode
        future = self.client.call_async(request)
        rclpy.spin_until_future_complete(self, future)
        return future.result()

# 使用示例
rclpy.init()
client = ModeClient()
result = client.switch_mode('localization')
print(f"Success: {result.success}, Message: {result.message}")

C++调用:

C++#include <rclcpp/rclcpp.hpp>
#include <fourier_msgs/srv/set_mode.hpp>

class ModeClient : public rclcpp::Node {
public:
    ModeClient() : Node("mode_client") {
        client_ = create_client<fourier_msgs::srv::SetMode>("slam/set_mode");
        client_->wait_for_service();
    }

    bool switch_mode(const std::string& mode) {
        auto request = std::make_shared<fourier_msgs::srv::SetMode::Request>();
        request->mode = mode;
        auto future = client_->async_send_request(request);
        if (rclcpp::spin_until_future_complete(shared_from_this(), future) ==
            rclcpp::FutureReturnCode::SUCCESS) {
            auto result = future.get();
            RCLCPP_INFO(get_logger(), "Result: %s", result->message.c_str());
            return result->success;
        }
        return false;
    }

private:
    rclcpp::Client<fourier_msgs::srv::SetMode>::SharedPtr client_;
};

// 使用示例
int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto client = std::make_shared<ModeClient>();
    client->switch_mode("localization");
    rclcpp::shutdown();
    return 0;
}

2. 建图模式接口

2.1 /clear_map (话题)

清除当前地图并重启建图

消息类型: fourier_msgs_pod/msg/ShortString

功能说明:

消息内容不参与解析；收到任意合法 ShortString 后，导航模块清除当前地图。
此接口只清除当前运行时地图数据，不会删除已保存的复合地图归档。删除已保存地图请使用 /clear_composite_map。

发布示例:

Bashros2 topic pub $ROBOT_NAMESPACE/clear_map fourier_msgs_pod/msg/ShortString "{}" --once

Python发布:

Pythonfrom fourier_msgs_pod.msg import ShortString
import rclpy
from rclpy.node import Node
from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy

class ClearMapPublisher(Node):
    def __init__(self):
        super().__init__('clear_map_publisher')
        qos = QoSProfile(
            history=HistoryPolicy.KEEP_LAST,
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
        )
        self.publisher = self.create_publisher(ShortString, 'clear_map', qos)

    def clear(self):
        msg = ShortString()
        self.publisher.publish(msg)
        self.get_logger().info('Clear map command sent')

rclpy.init()
node = ClearMapPublisher()
try:
    # 等待 DDS 端点发现；立即退出会使一次性命令尚未匹配订阅端。
    rclpy.spin_once(node, timeout_sec=1.0)
    node.clear()
    rclpy.spin_once(node, timeout_sec=1.0)
finally:
    node.destroy_node()
    rclpy.shutdown()

C++发布:

C++#include <chrono>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <thread>
#include <fourier_msgs_pod/msg/short_string.hpp>

class ClearMapPublisher : public rclcpp::Node {
public:
    ClearMapPublisher() : Node("clear_map_publisher") {
        const auto qos = rclcpp::QoS(rclcpp::KeepLast(10))
            .best_effort()
            .transient_local();
        publisher_ = create_publisher<fourier_msgs_pod::msg::ShortString>("clear_map", qos);
    }

    void clear() {
        auto msg = fourier_msgs_pod::msg::ShortString();
        publisher_->publish(msg);
        RCLCPP_INFO(get_logger(), "Clear map command sent");
    }

private:
    rclcpp::Publisher<fourier_msgs_pod::msg::ShortString>::SharedPtr publisher_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<ClearMapPublisher>();
    rclcpp::spin_some(node);
    std::this_thread::sleep_for(std::chrono::seconds(1));
    node->clear();
    std::this_thread::sleep_for(std::chrono::seconds(1));
    rclcpp::shutdown();
    return 0;
}

2.2 /map (话题)

发布2D占用栅格地图数据

消息类型: nav_msgs/msg/OccupancyGrid

数据来源:

建图模式：导航模块发布当前的 2D 占用栅格地图。
定位模式：导航模块发布当前已加载地图的 2D 占用栅格地图。

数据约定:

参数	值	说明
Frame ID	map	地图坐标系
data	-1 / 0 / 100	分别表示未知、空闲、占用
分辨率、尺寸与原点	运行时地图数据	由当前地图决定，不应在客户端写死

订阅示例:

Bashros2 topic echo $ROBOT_NAMESPACE/map

3. 定位模式接口

3.1 /initialpose (话题)

设置机器人初始位姿

消息类型: geometry_msgs/msg/PoseWithCovarianceStamped

QoS 要求: 本话题订阅端使用 BEST_EFFORT + TRANSIENT_LOCAL。发布端必须使用相同的可靠性与持久性策略；ros2 topic pub 默认的 Reliable + Volatile 无法与订阅端匹配，消息不会被处理。此外一次性 --once 发布可能因端点匹配时序而丢失，建议持续发布数秒。

发布示例:

Bash# 设置初始位姿：位置(1.0, 2.0, 0.0)，朝向yaw=90度(四元数表示)
# 持续发布约 6 秒后自动退出，确保 DDS 端点完成匹配且消息被处理
timeout 6 ros2 topic pub --qos-reliability best_effort --qos-durability transient_local -r 1 \
  $ROBOT_NAMESPACE/initialpose geometry_msgs/msg/PoseWithCovarianceStamped \
  "{header: {frame_id: 'map'}, pose: {pose: {position: {x: 1.0, y: 2.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.707, w: 0.707}}}}"

Python发布:

Pythonfrom geometry_msgs.msg import PoseWithCovarianceStamped
import rclpy
from rclpy.node import Node
import math
from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy

class InitialPosePublisher(Node):
    def __init__(self):
        super().__init__('initialpose_publisher')
        qos = QoSProfile(
            history=HistoryPolicy.KEEP_LAST,
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
        )
        self.publisher = self.create_publisher(PoseWithCovarianceStamped, 'initialpose', qos)

    def set_pose(self, x: float, y: float, yaw: float):
        msg = PoseWithCovarianceStamped()
        msg.header.frame_id = 'map'
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.pose.pose.position.x = x
        msg.pose.pose.position.y = y
        msg.pose.pose.position.z = 0.0
        # yaw转四元数
        msg.pose.pose.orientation.x = 0.0
        msg.pose.pose.orientation.y = 0.0
        msg.pose.pose.orientation.z = math.sin(yaw / 2.0)
        msg.pose.pose.orientation.w = math.cos(yaw / 2.0)
        self.publisher.publish(msg)

rclpy.init()
node = InitialPosePublisher()
try:
    rclpy.spin_once(node, timeout_sec=1.0)
    node.set_pose(1.0, 2.0, 1.57)  # x=1, y=2, yaw=90度
    rclpy.spin_once(node, timeout_sec=1.0)
finally:
    node.destroy_node()
    rclpy.shutdown()

C++发布:

C++#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <cmath>
#include <chrono>
#include <memory>
#include <thread>

class InitialPosePublisher : public rclcpp::Node {
public:
    InitialPosePublisher() : Node("initialpose_publisher") {
        const auto qos = rclcpp::QoS(rclcpp::KeepLast(10))
            .best_effort()
            .transient_local();
        publisher_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("initialpose", qos);
    }

    void set_pose(double x, double y, double yaw) {
        auto msg = geometry_msgs::msg::PoseWithCovarianceStamped();
        msg.header.frame_id = "map";
        msg.header.stamp = now();
        msg.pose.pose.position.x = x;
        msg.pose.pose.position.y = y;
        msg.pose.pose.position.z = 0.0;
        // yaw转四元数
        msg.pose.pose.orientation.x = 0.0;
        msg.pose.pose.orientation.y = 0.0;
        msg.pose.pose.orientation.z = std::sin(yaw / 2.0);
        msg.pose.pose.orientation.w = std::cos(yaw / 2.0);
        publisher_->publish(msg);
    }

private:
    rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr publisher_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<InitialPosePublisher>();
    rclcpp::spin_some(node);
    std::this_thread::sleep_for(std::chrono::seconds(1));
    node->set_pose(1.0, 2.0, 1.57);
    std::this_thread::sleep_for(std::chrono::seconds(1));
    rclcpp::shutdown();
    return 0;
}

3.2 /slam/load_map (服务)

加载地图并切换到定位模式

服务类型: fourier_msgs/srv/LoadMap

功能说明:

map_path 必须是由 /save_composite_map 保存的地图归档目录。
服务会加载归档、切换到定位模式，并使用请求中的 x、y、yaw 设置初始位姿；z 字段在 GRW 上不参与处理。

调用示例:

Bashros2 service call $ROBOT_NAMESPACE/slam/load_map fourier_msgs/srv/LoadMap \
  "{map_path: '/home/user/maps/office', x: 0.0, y: 0.0, z: 0.0, yaw: 0.0}"

Python调用:

Pythonfrom fourier_msgs.srv import LoadMap
import rclpy
from rclpy.node import Node

class MapLoader(Node):
    def __init__(self):
        super().__init__('map_loader')
        self.client = self.create_client(LoadMap, 'slam/load_map')
        if not self.client.wait_for_service(timeout_sec=10.0):
            raise RuntimeError('slam/load_map service unavailable')

    def load(self, path: str, x=0.0, y=0.0, z=0.0, yaw=0.0):
        request = LoadMap.Request()
        request.map_path = path
        request.x = x
        request.y = y
        request.z = z
        request.yaw = yaw
        future = self.client.call_async(request)
        rclpy.spin_until_future_complete(self, future, timeout_sec=30.0)
        result = future.result()
        if result is None:
            raise RuntimeError('slam/load_map request timed out')
        print(f'result={result.result}, message={result.message}')
        return result.result == 0  # 0 表示成功

rclpy.init()
loader = MapLoader()
try:
    success = loader.load('/home/user/maps/office', x=1.0, y=2.0, yaw=1.57)
    print(f'load success: {success}')
finally:
    loader.destroy_node()
    rclpy.shutdown()

C++调用:

C++#include <chrono>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <fourier_msgs/srv/load_map.hpp>
#include <stdexcept>
#include <string>

class MapLoader : public rclcpp::Node {
public:
    MapLoader() : Node("map_loader") {
        client_ = create_client<fourier_msgs::srv::LoadMap>("slam/load_map");
        if (!client_->wait_for_service(std::chrono::seconds(10))) {
            throw std::runtime_error("slam/load_map service unavailable");
        }
    }

    bool load(const std::string& path, double x=0.0, double y=0.0,
              double z=0.0, double yaw=0.0) {
        auto request = std::make_shared<fourier_msgs::srv::LoadMap::Request>();
        request->map_path = path;
        request->x = x;
        request->y = y;
        request->z = z;
        request->yaw = yaw;
        auto future = client_->async_send_request(request);
        if (rclcpp::spin_until_future_complete(shared_from_this(), future) ==
            rclcpp::FutureReturnCode::SUCCESS) {
            return future.get()->result == 0;
        }
        return false;
    }

private:
    rclcpp::Client<fourier_msgs::srv::LoadMap>::SharedPtr client_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    try {
        auto loader = std::make_shared<MapLoader>();
        const bool ok = loader->load("/home/user/maps/office", 1.0, 2.0, 0.0, 1.57);
        RCLCPP_INFO(loader->get_logger(), "load success: %s", ok ? "true" : "false");
    } catch (const std::exception& e) {
        RCLCPP_ERROR(rclcpp::get_logger("map_loader"), "%s", e.what());
    }
    rclcpp::shutdown();
    return 0;
}

3.3 /robot_pose (话题)

发布机器人在地图坐标系下的二维位姿

消息类型: geometry_msgs/msg/PoseStamped

说明: 发布当前定位结果。位置使用 x、y，朝向为绕 Z 轴的 yaw 转换得到的四元数；z 为 0。

发布频率: 10Hz

Frame ID: map

QoS 要求: GRW 的发布端使用 BEST_EFFORT + TRANSIENT_LOCAL。Python 和 C++ 订阅器必须使用相同的可靠性和持久性策略；使用 ROS 2 默认的 Reliable QoS 将无法与发布端匹配，导致收不到消息。

订阅示例:

Bashros2 topic echo $ROBOT_NAMESPACE/robot_pose

Python订阅:

Pythonfrom geometry_msgs.msg import PoseStamped
import rclpy
from rclpy.node import Node
from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy

class PoseSubscriber(Node):
    def __init__(self):
        super().__init__('pose_subscriber')
        qos = QoSProfile(
            history=HistoryPolicy.KEEP_LAST,
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
        )
        self.subscription = self.create_subscription(
            PoseStamped, 'robot_pose', self.pose_callback, qos)

    def pose_callback(self, msg):
        pos = msg.pose.position
        ori = msg.pose.orientation
        print(f"Position: x={pos.x:.3f}, y={pos.y:.3f}, z={pos.z:.3f}")
        print(f"Orientation: x={ori.x:.3f}, y={ori.y:.3f}, z={ori.z:.3f}, w={ori.w:.3f}")

rclpy.init()
node = PoseSubscriber()
rclpy.spin(node)

C++订阅:

C++#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>

class PoseSubscriber : public rclcpp::Node {
public:
    PoseSubscriber() : Node("pose_subscriber") {
        const auto qos = rclcpp::QoS(rclcpp::KeepLast(10))
            .best_effort()
            .transient_local();
        subscription_ = create_subscription<geometry_msgs::msg::PoseStamped>(
            "robot_pose", qos,
            [this](geometry_msgs::msg::PoseStamped::SharedPtr msg) {
                RCLCPP_INFO(get_logger(), "Position: [%.3f, %.3f, %.3f]",
                    msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
            });
    }

private:
    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr subscription_;
};

3.5 /odom_status_code (话题)

发布定位系统状态码

消息类型: std_msgs/msg/Int8

说明: 每秒发布一次定位质量查询是否成功的状态，用于判断底盘是否能够提供定位质量。

状态码定义:

状态码	名称	说明
0	无定位质量	未能取得定位质量
1	定位质量可用	已成功取得定位质量

订阅示例:

Bashros2 topic echo $ROBOT_NAMESPACE/odom_status_code

Python订阅:

Pythonfrom std_msgs.msg import Int8
import rclpy
from rclpy.node import Node
from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy

class OdomStatusSubscriber(Node):
    STATUS_NAMES = {
        0: "定位质量不可用",
        1: "定位质量可用",
    }

    def __init__(self):
        super().__init__('odom_status_subscriber')
        qos = QoSProfile(
            history=HistoryPolicy.KEEP_LAST,
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
        )
        self.subscription = self.create_subscription(
            Int8, 'odom_status_code', self.status_callback, qos)

    def status_callback(self, msg):
        status_name = self.STATUS_NAMES.get(msg.data, "UNKNOWN")
        print(f"定位状态: {status_name} (code: {msg.data})")

        if msg.data == 0:
            print("未取得定位质量")

rclpy.init()
node = OdomStatusSubscriber()
rclpy.spin(node)

C++订阅:

C++#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/int8.hpp>

class OdomStatusSubscriber : public rclcpp::Node {
public:
    OdomStatusSubscriber() : Node("odom_status_subscriber") {
        const auto qos = rclcpp::QoS(rclcpp::KeepLast(10))
            .best_effort()
            .transient_local();
        subscription_ = create_subscription<std_msgs::msg::Int8>(
            "odom_status_code", qos,
            [this](std_msgs::msg::Int8::SharedPtr msg) {
                std::string status_name = getStatusName(msg->data);
                RCLCPP_INFO(get_logger(), "定位状态: %s (code: %d)",
                    status_name.c_str(), msg->data);

                if (msg->data == 0) {
                    RCLCPP_WARN(get_logger(), "未取得定位质量");
                }
            });
    }

private:
    std::string getStatusName(int8_t code) {
        switch(code) {
            case 0: return "定位质量不可用";
            case 1: return "定位质量可用";
            default: return "UNKNOWN";
        }
    }

    rclcpp::Subscription<std_msgs::msg::Int8>::SharedPtr subscription_;
};

3.6 /odom_status_score (话题)

发布定位置信度分数

消息类型: std_msgs/msg/Int8

说明: 每秒发布一次定位质量评分。数据范围为 0–100；未能取得定位质量时发布 0。

数据说明:

data = 定位质量评分 (0–100)
当 /odom_status_code 为 0 时，data 为 0

订阅示例:

Bashros2 topic echo $ROBOT_NAMESPACE/odom_status_score

Python订阅（联合状态码使用）:

Pythonfrom std_msgs.msg import Int8
import rclpy
from rclpy.node import Node
from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy

class OdomMonitor(Node):
    def __init__(self):
        super().__init__('odom_monitor')
        self.status_code = 0
        self.status_score = 0
        qos = QoSProfile(
            history=HistoryPolicy.KEEP_LAST,
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
        )

        self.code_sub = self.create_subscription(
            Int8, 'odom_status_code', self.code_callback, qos)
        self.score_sub = self.create_subscription(
            Int8, 'odom_status_score', self.score_callback, qos)

    def code_callback(self, msg):
        self.status_code = msg.data
        self.print_status()

    def score_callback(self, msg):
        self.status_score = msg.data
        self.print_status()

    def print_status(self):
        if self.status_code == 1:
            print(f"定位质量评分: {self.status_score}/100")
        else:
            print("未取得定位质量")

rclpy.init()
node = OdomMonitor()
rclpy.spin(node)

C++订阅（联合状态码使用）:

C++#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/int8.hpp>

class OdomMonitor : public rclcpp::Node {
public:
    OdomMonitor() : Node("odom_monitor"), status_code_(0), status_score_(0) {
        const auto qos = rclcpp::QoS(rclcpp::KeepLast(10))
            .best_effort()
            .transient_local();
        code_sub_ = create_subscription<std_msgs::msg::Int8>(
            "odom_status_code", qos,
            [this](std_msgs::msg::Int8::SharedPtr msg) {
                status_code_ = msg->data;
                print_status();
            });

        score_sub_ = create_subscription<std_msgs::msg::Int8>(
            "odom_status_score", qos,
            [this](std_msgs::msg::Int8::SharedPtr msg) {
                status_score_ = msg->data;
                print_status();
            });
    }

private:
    void print_status() {
        if (status_code_ == 1) {
            RCLCPP_INFO(get_logger(), "定位质量评分: %d/100", status_score_);
        } else {
            RCLCPP_WARN(get_logger(), "未取得定位质量");
        }
    }

    rclcpp::Subscription<std_msgs::msg::Int8>::SharedPtr code_sub_;
    rclcpp::Subscription<std_msgs::msg::Int8>::SharedPtr score_sub_;
    int8_t status_code_;
    int8_t status_score_;
};

3.7 重定位

定位丢失或位姿不确定时，触发底盘重新估计机器人位姿。

3.7.2 /slam/trigger_local_relocalization (服务)

触发底盘重定位

服务类型: fourier_msgs/srv/VPRLocalRelocalization

功能说明: 当前实现不使用 polygon_vertices，调用后直接触发不限定搜索区域的重定位；不支持按多边形限制的局部重定位。响应中的 candidate_count 固定为 0，不能将其作为候选位姿数量使用。

调用示例:

Bashros2 service call $ROBOT_NAMESPACE/slam/trigger_local_relocalization \
  fourier_msgs/srv/VPRLocalRelocalization \
  "{polygon_vertices: []}"

4. 导航相关接口

4.1 navigate_to_pose (动作)

导航到指定目标点

动作类型: nav2_msgs/action/NavigateToPose

实现说明: GRW 提供标准 ROS 2 NavigateToPose action，一次只接受一个目标；已有目标尚未结束时，新的目标请求会被拒绝，客户端应先取消当前目标。目标坐标应使用 map 坐标系。服务端读取目标的 x、y 和由四元数 z、w 计算出的平面朝向；不会校验或转换 frame_id。

终态状态码（以实际实现为准）：

goal 终态在 status 话题与 get_result 响应中以字节发布：4=成功、5=取消、6=中止（与 grstate 契约一致）。
该编号与标准 action_msgs 常量（CANCELED=4、SUCCEEDED=5）相反。机器人交付环境内的客户端（CLI、rclpy、rclcpp）与实现一致，显示正确；未改装的标准 ROS 2 Humble 环境会把成功（4）读作 CANCELED。这是当前已知兼容性问题，编号对齐修复版本发布前，外部标准环境请按字节语义判读终态。
经 /cancel_current_action 取消的 goal 会停止机器人并上报终态 5（取消），要求 fourier-third-navi 同时包含提交 5655cae（fix(nav): report canceled goals as CANCELED, not SUCCEEDED）。更早版本取消后终态上报 4（与成功不可区分），只能以服务响应和机器人实际停止判断（见 4.4）。
底盘硬急停期间：goal 仍会被接受但机器人不动，服务端约 30 秒后报 ABORTED（6）。应在急停解除后重新发送目标。

版本要求: 标准 ROS 2 ActionClient 可正常发现本 Action 并接收 feedback，要求机器人 fourier-third-navi 包含提交 4c7ef59（fix(nav): align action feedback QoS with ROS 2 clients，feedback 发布端为 RELIABLE + TRANSIENT_LOCAL）及以上。更早版本上命令行、Python 和 C++ Action 客户端会持续等待服务端而无法使用。

Goal（目标请求）

字段	类型	描述
pose	geometry_msgs/PoseStamped	目标位姿。GRW 使用 `position.x`、`position.y` 以及由 `orientation.z`、`orientation.w` 表示的平面朝向；应以 `map` 坐标系发送。
behavior_tree	string	此字段属于标准 Action 类型；当前 GRW 实现不解析该字段，应留空。

Result（执行结果）

ROS 2 Humble nav2_msgs 的 NavigateToPose_Result 为空结构，服务端不填充任何结果字段，CLI 实际返回 result: {}。导航是否成功必须以 Action 终态状态码为准（见上方"终态状态码"说明），不应从 result 载荷读取任何字段。

Feedback（实时反馈）

字段	类型	描述
current_pose	geometry_msgs/PoseStamped	当前仅写入 `pose.position.x` 和 `pose.position.y`；header、朝向与其余位置字段未填充，不能将其作为完整位姿消息使用。

当前实现不提供导航时长、预计剩余时间、恢复次数或剩余距离。需要完整机器人位姿时，请订阅 /robot_pose。

调用示例

命令行:

Bash# 导航到位置(1.0, 2.0)，朝向yaw=45度
ros2 action send_goal $ROBOT_NAMESPACE/navigate_to_pose nav2_msgs/action/NavigateToPose \
  "{pose: {header: {frame_id: 'map'}, pose: {position: {x: 1.0, y: 2.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.383, w: 0.924}}}}"

# 带反馈信息
ros2 action send_goal $ROBOT_NAMESPACE/navigate_to_pose nav2_msgs/action/NavigateToPose \
  "{pose: {header: {frame_id: 'map'}, pose: {position: {x: 1.0, y: 2.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.383, w: 0.924}}}}" --feedback

Python调用:

Pythonfrom geometry_msgs.msg import PoseStamped
from nav2_msgs.action import NavigateToPose
import math
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node

class NavigateClient(Node):
    def __init__(self):
        super().__init__('navigate_client')
        self.client = ActionClient(self, NavigateToPose, 'navigate_to_pose')

    def send_goal(self, x: float, y: float, yaw: float):
        goal = NavigateToPose.Goal()
        goal.pose = PoseStamped()
        goal.pose.header.frame_id = 'map'
        goal.pose.header.stamp = self.get_clock().now().to_msg()
        goal.pose.pose.position.x = x
        goal.pose.pose.position.y = y
        goal.pose.pose.orientation.z = math.sin(yaw / 2.0)
        goal.pose.pose.orientation.w = math.cos(yaw / 2.0)
        goal.behavior_tree = ''

        self.client.wait_for_server()
        return self.client.send_goal_async(goal)

rclpy.init()
node = NavigateClient()
node.send_goal(1.0, 2.0, 0.785)
rclpy.spin(node)

C++：

C++#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <cmath>

using NavigateToPose = nav2_msgs::action::NavigateToPose;
using GoalHandleNavigateToPose = rclcpp_action::ClientGoalHandle<NavigateToPose>;

class NavigationClient : public rclcpp::Node {
public:
    NavigationClient() : Node("navigation_client") {
        client_ = rclcpp_action::create_client<NavigateToPose>(
            this, "navigate_to_pose");
        client_->wait_for_action_server();
    }

    void navigate_to(double x, double y, double yaw) {
        // 构造目标
        auto goal = NavigateToPose::Goal();
        goal.pose.header.frame_id = "map";
        goal.pose.header.stamp = now();
        goal.pose.pose.position.x = x;
        goal.pose.pose.position.y = y;
        goal.pose.pose.position.z = 0.0;
        goal.pose.pose.orientation.x = 0.0;
        goal.pose.pose.orientation.y = 0.0;
        goal.pose.pose.orientation.z = std::sin(yaw / 2.0);
        goal.pose.pose.orientation.w = std::cos(yaw / 2.0);

        // 设置回调
        auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();

        // 反馈回调：当前实现仅填充 current_pose 的 x、y
        send_goal_options.feedback_callback =
            [this](GoalHandleNavigateToPose::SharedPtr,
                   const std::shared_ptr<const NavigateToPose::Feedback> feedback) {
                RCLCPP_INFO(get_logger(), "当前位置: [%.2f, %.2f]",
                    feedback->current_pose.pose.position.x,
                    feedback->current_pose.pose.position.y);
            };

        // 结果回调
        send_goal_options.result_callback =
            [this](const GoalHandleNavigateToPose::WrappedResult& result) {
                switch (result.code) {
                    case rclcpp_action::ResultCode::SUCCEEDED:
                        RCLCPP_INFO(get_logger(), "导航成功!");
                        break;
                    case rclcpp_action::ResultCode::ABORTED:
                        RCLCPP_ERROR(get_logger(), "导航中止");
                        break;
                    case rclcpp_action::ResultCode::CANCELED:
                        RCLCPP_WARN(get_logger(), "导航取消");
                        break;
                    default:
                        RCLCPP_ERROR(get_logger(), "未知结果");
                        break;
                }
            };

        // 发送目标
        client_->async_send_goal(goal, send_goal_options);
    }

    void cancel_navigation() {
        client_->async_cancel_all_goals();
    }

private:
    rclcpp_action::Client<NavigateToPose>::SharedPtr client_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto client = std::make_shared<NavigationClient>();
    client->navigate_to(1.0, 2.0, 0.785);  // x=1, y=2, yaw=45度
    rclcpp::spin(client);
    rclcpp::shutdown();
    return 0;
}

4.2 /plan (话题)

发布全局规划路径

消息类型: nav_msgs/msg/Path

说明: 发布当前规划路径，坐标系为 map。

发布频率: 1Hz

订阅示例:

Bashros2 topic echo $ROBOT_NAMESPACE/plan

4.4 cancel_current_action (服务)

取消当前正在执行的导航动作

服务类型: fourier_msgs/srv/CancelCurrentAction

功能说明:

调用后正在执行的导航动作停止、机器人停下；响应 success=true, message='ok' 表示服务已执行。
没有进行中的动作时调用同样返回 success=true, message='ok'，不会因"无可取消目标"而返回失败。
取消后的 navigate_to_pose 终态为 5（取消），要求 fourier-third-navi 包含提交 5655cae 及以上；更早版本取消后终态上报 4、与成功不可区分（见 4.1 终态状态码说明），只能以服务响应和机器人实际停止判断取消是否生效。
取消应尽早发出：新起进程（尤其是 C++ 客户端）会引入数秒 DDS 发现延迟，短距离目标可能在取消生效前已到达。

调用示例:

Bashros2 service call $ROBOT_NAMESPACE/cancel_current_action fourier_msgs/srv/CancelCurrentAction

Python调用:

Pythonfrom fourier_msgs.srv import CancelCurrentAction
import rclpy
from rclpy.node import Node

class ActionCanceller(Node):
    def __init__(self):
        super().__init__('action_canceller')
        self.client = self.create_client(CancelCurrentAction, 'cancel_current_action')
        self.client.wait_for_service()

    def cancel(self):
        request = CancelCurrentAction.Request()
        future = self.client.call_async(request)
        rclpy.spin_until_future_complete(self, future)
        result = future.result()
        return result.success

rclpy.init()
canceller = ActionCanceller()
if canceller.cancel():
    print("Action cancelled successfully")

C++调用:

C++#include <rclcpp/rclcpp.hpp>
#include <fourier_msgs/srv/cancel_current_action.hpp>

class ActionCanceller : public rclcpp::Node {
public:
    ActionCanceller() : Node("action_canceller") {
        client_ = create_client<fourier_msgs::srv::CancelCurrentAction>(
            "cancel_current_action");
        client_->wait_for_service();
    }

    bool cancel() {
        auto request = std::make_shared<fourier_msgs::srv::CancelCurrentAction::Request>();
        auto future = client_->async_send_request(request);
        if (rclcpp::spin_until_future_complete(shared_from_this(), future) ==
            rclcpp::FutureReturnCode::SUCCESS) {
            return future.get()->success;
        }
        return false;
    }

private:
    rclcpp::Client<fourier_msgs::srv::CancelCurrentAction>::SharedPtr client_;
};

4.5 get_current_action (服务)

获取当前正在执行的动作信息

服务类型: fourier_msgs/srv/GetCurrentAction

功能说明: 返回当前动作的名称、描述及状态码。状态码编号与 4.1 的终态状态码一致（4=成功、5=取消、6=中止；执行中为 2，空闲为 0），注意与标准 action_msgs 常量相反，见 4.1 说明。该服务适合显示当前动作信息；导航终态应以 navigate_to_pose action 的终态状态码为准（result 载荷为空，见 4.1）。状态码说明见 get_current_action。

调用示例:

Bashros2 service call $ROBOT_NAMESPACE/get_current_action fourier_msgs/srv/GetCurrentAction

Python调用:

Pythonfrom fourier_msgs.srv import GetCurrentAction
import rclpy
from rclpy.node import Node

class ActionMonitor(Node):
    def __init__(self):
        super().__init__('action_monitor')
        self.client = self.create_client(GetCurrentAction, 'get_current_action')
        self.client.wait_for_service()

    def get_status(self):
        request = GetCurrentAction.Request()
        future = self.client.call_async(request)
        rclpy.spin_until_future_complete(self, future)
        result = future.result()
        if result.success:
            print(f"Action: {result.action_name}")
            print(f"Status: {result.status_description}")
        return result

rclpy.init()
monitor = ActionMonitor()
monitor.get_status()

C++调用:

C++#include <rclcpp/rclcpp.hpp>
#include <fourier_msgs/srv/get_current_action.hpp>

class ActionMonitor : public rclcpp::Node {
public:
    ActionMonitor() : Node("action_monitor") {
        client_ = create_client<fourier_msgs::srv::GetCurrentAction>(
            "get_current_action");
        client_->wait_for_service();
    }

    void get_status() {
        auto request = std::make_shared<fourier_msgs::srv::GetCurrentAction::Request>();
        auto future = client_->async_send_request(request);
        if (rclcpp::spin_until_future_complete(shared_from_this(), future) ==
            rclcpp::FutureReturnCode::SUCCESS) {
            auto result = future.get();
            if (result->success) {
                RCLCPP_INFO(get_logger(), "Action: %s, Status: %s",
                    result->action_name.c_str(),
                    result->status_description.c_str());
            }
        }
    }

private:
    rclcpp::Client<fourier_msgs::srv::GetCurrentAction>::SharedPtr client_;
};

8. 事件通知接口

系统提供事件通知功能，用于实时发布导航过程中的重要事件。

8.1 /Humanoid_nav/events (话题)

发布聚合后的系统事件

消息类型: fourier_msgs/msg/EventsInfo

发布频率: 1Hz

订阅示例:

Bashros2 topic echo $ROBOT_NAMESPACE/Humanoid_nav/events

当前实现仅在有活动导航动作时发布 navigation_active 事件。

Python 订阅示例:

Pythonfrom fourier_msgs.msg import EventsInfo
import rclpy
from rclpy.node import Node
from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy

class EventsSubscriber(Node):
    def __init__(self):
        super().__init__('events_subscriber')
        qos = QoSProfile(
            history=HistoryPolicy.KEEP_LAST,
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
        )
        self.subscription = self.create_subscription(
            EventsInfo, 'Humanoid_nav/events', self.events_callback, qos)

    def events_callback(self, msg):
        for event in msg.events:
            print(f"[{event.event_type}] {event.message} (from: {event.source})")

rclpy.init()
rclpy.spin(EventsSubscriber())

C++ 订阅示例:

C++#include <rclcpp/rclcpp.hpp>
#include <fourier_msgs/msg/events_info.hpp>

class EventsSubscriber : public rclcpp::Node {
public:
    EventsSubscriber() : Node("events_subscriber") {
        const auto qos = rclcpp::QoS(rclcpp::KeepLast(10))
            .best_effort()
            .transient_local();
        subscription_ = create_subscription<fourier_msgs::msg::EventsInfo>(
            "Humanoid_nav/events", qos,
            [this](fourier_msgs::msg::EventsInfo::SharedPtr msg) {
                for (const auto& event : msg->events) {
                    RCLCPP_INFO(get_logger(), "[%s] %s (from: %s)",
                        event.event_type.c_str(), event.message.c_str(), event.source.c_str());
                }
            });
    }
private:
    rclcpp::Subscription<fourier_msgs::msg::EventsInfo>::SharedPtr subscription_;
};

9. 地图管理接口

GRW 使用单层地图。本节列出导航模块实际注册的复合地图、POI、虚拟墙和禁区服务。

9.1 消息类型定义

9.1.1 CompositeMapInfo 消息

消息类型: map_manager/msg/CompositeMapInfo

该消息用于 /load_composite_map 的响应。当前 GRW 仅在加载成功时填写 map_id，其值为归档目录名。

字段	类型	说明
map_id	string	已加载地图归档的标识。

9.1.2 POI 消息

消息类型: map_manager/msg/POI

POI 用于标记地图中的二维点位，由 /poi/add、/poi/list 和 /poi/remove 管理。

字段	类型	说明
id	string	POI 标识；新增、查询和删除使用该值。
name	string	POI 名称。
pose	geometry_msgs/Pose2D	二维位姿，使用 `x`、`y` 和 `theta`。

9.1.3 VirtualWall 消息

消息类型: map_manager/msg/VirtualWall

虚拟墙由两个二维端点定义，由 /wall/add、/wall/list、/wall/update 和 /wall/remove 管理。

字段	类型	说明
id	int32	虚拟墙数值标识；新增成功后由服务返回，更新和删除时使用。
start	geometry_msgs/Point	起点，使用 `x`、`y`。
end	geometry_msgs/Point	终点，使用 `x`、`y`。

9.1.4 ForbiddenArea 消息

消息类型: map_manager/msg/ForbiddenArea

禁区由四个顶点构成的轴对齐矩形定义，由 /forbidden/add、/forbidden/list、/forbidden/update 和 /forbidden/remove 管理。

字段	类型	说明
id	int32	禁区数值标识；新增成功后由服务返回，更新和删除时使用。
boundary	geometry_msgs/Polygon	`points` 必须为四个顶点构成的轴对齐矩形。

floor_id 是上述 POI、虚拟墙和禁区消息的保留字段，GRW 不使用该值。

9.2 复合地图服务

服务	类型	说明
`/save_composite_map`	`map_manager/srv/SaveCompositeMap`	请求将当前地图保存到目标目录；仅当 `success=true` 时才会生成地图归档及 `map.pgm`、`map.yaml`
`/load_composite_map`	`map_manager/srv/LoadCompositeMap`	从归档目录加载地图；加载后应通过 `/initialpose` 设置初始位姿
`/clear_composite_map`	`map_manager/srv/ClearCompositeMap`	清空当前运行时地图，不删除已保存归档
`/delete_map_archive`	`map_manager/srv/SaveCompositeMap`	删除 `map_path` 指定的已保存归档，不清空当前运行时地图

调用示例:

Bashros2 service call $ROBOT_NAMESPACE/save_composite_map map_manager/srv/SaveCompositeMap \
  "{map_path: '/data/maps/office', compress: false}"

ros2 service call $ROBOT_NAMESPACE/load_composite_map map_manager/srv/LoadCompositeMap \
  "{map_path: '/data/maps/office'}"

ros2 service call $ROBOT_NAMESPACE/clear_composite_map map_manager/srv/ClearCompositeMap "{dummy: false}"

compress 目前不是 GRW 导出格式选择开关；请使用目录路径并以响应中的 saved_path 为准。

行为说明: 极端情况下本服务可能返回 success=false, message='save failed'；重启机器人后重试通常可恢复。请保留已有可用归档作为兜底。

注记生命周期: POI、虚拟墙与禁区是当前运行图的注记。/save_composite_map 会把注记随地图归档一起保存，从该归档加载后注记恢复；通过系统侧（控制台/云端）切图时，系统会以系统侧保存的注记为准重建当前图，本接口添加的注记不会在系统切图后保留。需要跨切图持久的注记请通过系统通道管理。

9.3 语义元素服务

对象	新增	查询	更新	删除
POI	/poi/add `map_manager/srv/AddPOI`	/poi/list `map_manager/srv/ListPOIs`	不支持	/poi/remove `map_manager/srv/RemovePOI`
虚拟墙	/wall/add `map_manager/srv/AddVirtualWall`	/wall/list `map_manager/srv/ListVirtualWalls`	/wall/update `map_manager/srv/AddVirtualWall`	/wall/remove `map_manager/srv/RemoveVirtualWall`
禁区	/forbidden/add `map_manager/srv/AddForbiddenArea`	/forbidden/list `map_manager/srv/ListForbiddenAreas`	/forbidden/update `map_manager/srv/AddForbiddenArea`	/forbidden/remove `map_manager/srv/RemoveForbiddenArea`

约束:

GRW 忽略 POI、虚拟墙和禁区中的 floor_id。
虚拟墙和禁区的标识为服务返回的数值 ID；更新时须在对象的 id 字段传入该 ID。
禁区仅支持由四个顶点组成的轴对齐矩形；其他多边形将失败。

环境准备​

特别注意：​

1. 建图定位模式切换​

1.1 /slam/set_mode (服务)​

2. 建图模式接口​

2.1 /clear_map (话题)​

2.2 /map (话题)​

3. 定位模式接口​

3.1 /initialpose (话题)​

3.2 /slam/load_map (服务)​

3.3 /robot_pose (话题)​

3.5 /odom_status_code (话题)​

3.6 /odom_status_score (话题)​

3.7 重定位​

3.7.2 /slam/trigger_local_relocalization (服务)​

4. 导航相关接口​

4.1 navigate_to_pose (动作)​

Goal（目标请求）​

Result（执行结果）​

Feedback（实时反馈）​

调用示例​

4.2 /plan (话题)​

4.4 cancel_current_action (服务)​

4.5 get_current_action (服务)​

8. 事件通知接口​

8.1 /Humanoid_nav/events (话题)​

9. 地图管理接口​

9.1 消息类型定义​

9.1.1 CompositeMapInfo 消息​

9.1.2 POI 消息​

9.1.3 VirtualWall 消息​

9.1.4 ForbiddenArea 消息​

9.2 复合地图服务​

9.3 语义元素服务​

环境准备

特别注意：

1. 建图定位模式切换

1.1 /slam/set_mode (服务)

2. 建图模式接口

2.1 /clear_map (话题)

2.2 /map (话题)

3. 定位模式接口

3.1 /initialpose (话题)

3.2 /slam/load_map (服务)

3.3 /robot_pose (话题)

3.5 /odom_status_code (话题)

3.6 /odom_status_score (话题)

3.7 重定位

3.7.2 /slam/trigger_local_relocalization (服务)

4. 导航相关接口

4.1 navigate_to_pose (动作)

Goal（目标请求）

Result（执行结果）

Feedback（实时反馈）

调用示例

4.2 /plan (话题)

4.4 cancel_current_action (服务)

4.5 get_current_action (服务)

8. 事件通知接口

8.1 /Humanoid_nav/events (话题)

9. 地图管理接口

9.1 消息类型定义

9.1.1 CompositeMapInfo 消息

9.1.2 POI 消息

9.1.3 VirtualWall 消息

9.1.4 ForbiddenArea 消息

9.2 复合地图服务

9.3 语义元素服务