八界机器人 ROS2 Humble 高阶开发 SOP

wangxiaoxuan217

215人浏览 · 2026-10-09 14:20:16

wangxiaoxuan217 · 2026-10-09 14:20:16 发布

面向对象：需要在八界机器人上自定义 ROS2 节点的开发者（全文以 Python rclpy 为例）

目标平台：机器人搭载 ARM aarch64 处理器

开发方式：在 x86_64 PC 上编写 Python 代码 → rsync 推送源码 → 在机器人上首次 colcon build → ros2 run 运行（Ubuntu 22.04 + ROS2 Humble）

前置要求：ROS2 Humble 基础、Python 3.10

文档版本：2.4.0d

修改日期：2026/09/07

版本：common_msgs 0.0.0 / ROS2 Humble

1. 架构概览

1.1 运行平台

项目	说明
机器人处理器	ARM aarch64 64 位
操作系统	Ubuntu 22.04 LTS
ROS2 发行版	Humble Hawksbill
Python 版本	3.10 系统预装

1.2 开发与部署流程

┌──────────────────────────┐     SSH / rsync     ┌─────────────────────────┐
│  x86_64 开发 PC          │ ◄──────────────────► │  八界机器人 (ARM aarch64) │
│  (Ubuntu 20.04/22.04)   │                      │  Ubuntu 22.04 + ROS2     │
│                          │                      │                          │
│  1. 编写 Python 代码       │   推送源码           │  4. colcon build（首次）   │
│  2. Git 版本管理          │ ──────────────────► │  5. source & ros2 run    │
│  3. rsync 推送到机器人     │                      │  6. 节点直接运行          │
└──────────────────────────┘                      └─────────────────────────┘

核心原则：PC 上写 Python 代码，rsync 推送进机器人，首次 colcon build 后 ros2 run 运行。日常改 .py 无需 rebuild。

1.3 系统分层

┌──────────────────────────────────────────────────┐
│          你的自定义 Python ROS2 节点 (rclpy)        │
│              (推送即运行，无需编译)                  │
├──────────────────────────────────────────────────┤
│   Action Client    │  Service Client  │ Subscriber│
├────────────────────┼──────────────────┼───────────┤
│       八界机器人 ROS2 节点 (Actions/Services/Topics)       │
├────────────────────┴──────────────────┴───────────┤
│            common_msgs (接口定义包)                 │
├───────────────────────────────────────────────────┤
│   ROS2 Humble + Nav2 + MoveIt2 + SLAM (ARM 原生)    │
├───────────────────────────────────────────────────┤
│      机器人硬件 ARM aarch64 (传感器/执行器/MCU)       │
└───────────────────────────────────────────────────┘

1.4 关键节点一览

节点名	功能	主要接口类型
`/nav2_area_navi_points`	区域导航、观测点计算	Action
`/semantic_explore`	语义建图	Action
`/semantic_locate`	语义定位	Action
`/arm_adaptive`	机械臂自适应抓/放/观测	Action
`/search_object_node`	搜索物体	Action
`/robot_control_node`	头部/升降控制	Action
`/recharge_node`	回充	Action
`/explore_node`	探索建图	Action
`/face_locate`	人脸定位	Action
`/grasp_server`	抓取/放置姿态检测	Service
`/visual_model_interface`	VLM 视觉识别	Service
`/chassis_driver_node`	底盘驱动/传感器	Topic/Service
`/hrs_slam_node`	SLAM 建图/重定位	Action/Service/Topic

1.5 设计原则

Action 用于长耗时任务：导航、建图、搜索、抓取等（可取消、有反馈）
Service 用于短耗时请求：位姿检测、VLM 识别、地图管理
Topic 用于数据流：相机图像、点云、里程计、传感器状态
标注"可释放"的接口为对外公开接口，标注"内部接口"的请勿直接调用

2. 环境准备

2.1 机器人端环境 ARM aarch64 / Ubuntu 22\.04

机器人系统已预装 Ubuntu 22.04 + ROS2 Humble。你的自定义节点直接在机器人上编译运行。

确认机器人端环境：

# SSH 登录机器人
ssh Agi@<机器人IP>
确认系统架构
uname -m                    # 预期输出: aarch64
lsb_release -a              # 预期输出: Ubuntu 22.04
确认 ROS2 Humble
source /opt/ros/humble/setup.bash
ros2 --version
printenv ROS_DISTRO          # 预期输出: humble
确认 Python
python3 --version            # 预期输出: Python 3.10.x
确认 ROS2 Python 包可用
python3 -c "import rclpy; print('rclpy OK')"

2.2 开发机端环境 x86\_64 PC

开发 PC 用于编写代码、版本管理、以及将代码推送到机器人。

# 确认开发机 ROS2 (用于 ros2 interface show 等查询命令)
source /opt/ros/humble/setup.bash
ros2 --version
安装 SSH 和 rsync (推送到机器人)
sudo apt install -y openssh-client rsync
配置机器人 SSH 免密登录 (可选但推荐)
ssh-copy-id Agi@<机器人IP>

2.3 编译 common_msgs 接口包

common_msgs 定义了所有八界机器人 ROS2 接口（.action / .srv / .msg）。

关键事实：common_msgs 是一个纯接口定义包，编译产出是 C++ 头文件和 Python 类型桩——这些都是文本/生成代码，不产生 **.so 二进制文件，不挑 CPU 架构**。因此无论在 x86 开发机还是 ARM 机器人上编译，产物完全一样。

怎么做：选下面任意一种方式都可以。

方式一：在机器人上编译（最简单）

只需两条命令——把源码拷贝过去，然后 colcon build：

# === 在开发机上执行：拷贝源码到机器人 ===
scp -r /path/to/doc/common_msgs Agi@<机器人IP>:~/bajie_ws/src/
=== 在机器人上执行：编译 ===
ssh Agi@<机器人IP>
cd ~/bajie_ws
source /opt/ros/humble/setup.bash
colcon build --packages-select common_msgs --symlink-install
source install/setup.bash
验证
ros2 interface list | grep common_msgs

方式二：在开发机上编译，产物推送机器人

因为 common_msgs 不产生二进制，开发机上直接 colcon build 就行，不需要交叉编译器：

# === 在开发机上执行 ===
cd ~/bajie_ws
colcon build --packages-select common_msgs --symlink-install
推送 install 目录到机器人
rsync -avz install/ Agi@<机器人IP>:~/bajie_ws/install/

验证编译成功：

# 能列出接口说明一切正常
ros2 interface show common_msgs/action/AreaNav
ros2 interface show common_msgs/srv/VlmLocation
ros2 interface list | grep common_msgs | wc -l   # 应输出 100+

2.4 创建自定义节点工作空间

# SSH 到机器人
ssh Agi@192.168.3.12
cd ~/bajie_ws/src
创建 Python ROS2 包
ros2 pkg create --build-type ament_python my_robot_app 

--dependencies rclpy common_msgs geometry_msgs sensor_msgs std_msgs
首次编译（注册 entry_points）
cd ~/bajie_ws
source /opt/ros/humble/setup.bash
colcon build --symlink-install
source install/setup.bash

注意：部分机器人上 setuptools ≥80.x 不支持 --symlink-install，如报错去掉该参数即可。

2.5 代码部署与运行

PC 写 Python 代码 → rsync 推送 → 机器人上 ros2 run
（日常改 .py 无需 rebuild，仅首次或改 entry_points 才 colcon build）

推送与运行：

# === 在开发机上：推送源码 ===
rsync -avz --exclude='__pycache__' --exclude='*.pyc' \
  ~/bajie_ws/src/my_robot_app/ \
  Agi@192.168.3.12:~/bajie_ws/src/my_robot_app/
=== 在机器人上：运行 ===
ssh Agi@192.168.3.12
source ~/bajie_ws/install/setup.bash
ros2 run my_robot_app <节点名>

一键部署脚本 **sync.sh**（在 ros_node_demo/ 目录下已提供）：

cd ros_node_demo
仅同步源码
./sync.sh
同步 + 编译
./sync.sh --build
同步 + 编译 + 运行
./sync.sh --run device_info
./sync.sh --run status_monitor --args "report_interval:=5.0"
./sync.sh --run obj_search --args "name:=玩具 area_type:=-1"
./sync.sh --run gripper_ctrl --args "action:=open"
指定机器人 IP
./sync.sh --ip 192.168.3.100 --run device_info

环境变量（可写入 ~/.bashrc 避免每次输入）：

export ROBOT_IP=192.168.3.12
export ROBOT_USER=Agi
export ROBOT_PASS=robot

2.6 机器人端 ROS2 环境变量

开发节点前，确保机器人的 .bashrc 已正确配置 ROS2 环境：

# 在机器人上 ~/.bashrc 中追加
source /opt/ros/humble/setup.bash
source ~/bajie_ws/install/setup.bash   # 自定义工作空间
八界机器人使用 CycloneDDS（必须配置，否则无法发现节点）
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export CYCLONEDDS_URI=file:///factory/default.xml
常用 ROS2 环境变量
export ROS_DOMAIN_ID=0                  # 机器人上所有节点使用同一 Domain ID
export RCUTILS_COLORIZED_OUTPUT=1       # 彩色日志输出
export RCUTILS_CONSOLE_OUTPUT_FORMAT="[{severity}] [{name}]: {message}"

3. 通信接口体系

3.1 Action 接口（长耗时任务）

Action 是 ROS2 中用于长耗时、可取消、有反馈的异步任务机制。八界机器人中大多数强业务接口都是 Action。

标准 Action 调用模式：

import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from common_msgs.action import AreaNav
from geometry_msgs.msg import Point

class MyNavNode(Node): def init(self): super().init('my_nav_node') self._client = ActionClient(self, AreaNav, '/nav2_area_navi_points')

def send_nav_goal(self):
    if not self._client.wait_for_server(timeout_sec=5.0):
        self.get_logger().error('Action server not available')
        return

    goal = AreaNav.Goal()
    goal.module_name = 'my_nav_node'
    goal.obj_level = 2

    # 设置目标区域 (地图坐标系, 米)
    for x, y in [(1.0, 0.5), (1.0, -0.5), (0.4, -0.5), (0.4, 0.5)]:
        p = Point()
        p.x, p.y = x, y
        goal.object_area.append(p)

    self._client.send_goal_async(goal).add_done_callback(self._goal_response_cb)

def _goal_response_cb(self, future):
    goal_handle = future.result()
    if not goal_handle.accepted:
        self.get_logger().error('Goal rejected')
        return
    self.get_logger().info('Goal accepted')
    goal_handle.get_result_async().add_done_callback(self._result_cb)

def _result_cb(self, future):
    result = future.result().result
    self.get_logger().info(f'Nav done, state={result.result_state}')</pre>

3.2 Service 接口（短耗时请求）

Service 用于同步或短耗时的请求-响应模式。

标准 Service Client 示例：

from common_msgs.srv import VlmLocation

class MyVisionNode(Node): def init(self): super().init('my_vision_node') self.client = self.create_client(VlmLocation, '/visual_model_interface/location')

async def locate_object(self, object_name: str):
    if not self.client.wait_for_service(timeout_sec=3.0):
        self.get_logger().error('VLM service not available')
        return None

    req = VlmLocation.Request()
    req.task_id = 'my_task_001'
    req.names = [object_name]
    req.area_name = '客厅'

    future = self.client.call_async(req)
    resp = await future
    self.get_logger().info(f'Found {len(resp.box)} objects')
    return resp</pre>

3.3 Topic 接口（数据流订阅）

Topic 用于持续的数据流，如相机图像、传感器状态。

订阅传感器数据示例：

from sensor_msgs.msg import Image, CameraInfo, PointCloud2
from nav_msgs.msg import Odometry

class CameraSubscriber(Node): def init(self): super().init('camera_subscriber') # 头部彩色图 self.color_sub = self.create_subscription( Image, '/berxel_upper/berxel_upper/color/image_raw', self._color_cb, 10) # 头部点云 self.cloud_sub = self.create_subscription( PointCloud2, '/berxel_upper/raw/berxel_cloudpoint', self._cloud_cb, 10) # 里程计 self.odom_sub = self.create_subscription( Odometry, '/odom', self._odom_cb, 10)

def _color_cb(self, msg: Image):
    self.get_logger().info(f'Head color: {msg.width}x{msg.height}')

def _cloud_cb(self, msg: PointCloud2):
    self.get_logger().info(f'Head cloud: {msg.width}x{msg.height}')

def _odom_cb(self, msg: Odometry):
    self.get_logger().info(
        f'Odom: x={msg.pose.pose.position.x:.2f}, y={msg.pose.pose.position.y:.2f}')</pre>

4. 核心开发场景

场景	节点名	接口类型	已验证
A: 自定义导航	—	Action	模板
B: 视觉感知与抓取	—	Action + Service	模板
C: 语义地图管理	—	Action	模板
D: 设备信息采集	`device_info`	Topic std\_msgs	✅
E: 任务编排	—	Action + Service	模板
F: 电量与状态监控	`status_monitor`	Topic common\_msgs	✅
G: 夹爪开闭控制	`gripper_ctrl`	Action control\_msgs	✅
H: 底盘传感器仪表盘	`chassis_dash`	Topic × 13	✅
I: 里程计精度验证	`odom_check`	Topic nav\_msgs	✅
J: 地图管理	`map_mgr`	Service	✅
K: 物体搜索	`obj_search`	Action	✅
L: 头部/升降控制	`body_ctrl`	Action	✅
M: MCU 系统信息	`mcu_info`	Service	⚠️ 数据空

✅ = 已在机器人 192.168.3.12 上运行验证通过。模板 = 示例代码，待真机验证。

4.1 场景 A：自定义导航节点（模板）

目标：让机器人导航到指定的语义区域或地图坐标点。

涉及接口：

接口	类型	路径	说明
AreaNav	Action	`/nav2_area_navi_points`	区域导航（带区域轮廓）
SemanticNavigation	Action Nav2	`/navigate_to_pose`	Nav2 标准导航
MapRelocation	Service	`/slam/relocation/upper`	重定位

Python 完整示例 — 多区域顺序导航：

#!/usr/bin/env python3
"""
自定义多区域顺序导航节点
依次导航到客厅 → 厨房 → 卧室，每个区域停留 5 秒
"""
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from common_msgs.action import AreaNav
from geometry_msgs.msg import Point
import time

class SequentialNavNode(Node): def init(self): super().init('sequential_nav') self._nav_client = ActionClient(self, AreaNav, '/nav2_area_navi_points') self.areas = [ {'name': '客厅', 'points': [(1.0, 0.5), (1.0, -0.5), (0.4, -0.5), (0.4, 0.5)]}, {'name': '厨房', 'points': [(2.0, 0.5), (2.0, -0.5), (1.4, -0.5), (1.4, 0.5)]}, ] self.current_idx = 0 self.get_logger().info('SequentialNavNode started, waiting for server...')

def start(self):
    if not self._nav_client.wait_for_server(timeout_sec=10.0):
        self.get_logger().error('/nav2_area_navi_points not available')
        return
    self.get_logger().info('Server available, starting navigation sequence')
    self._navigate_next()

def _navigate_next(self):
    if self.current_idx &gt;= len(self.areas):
        self.get_logger().info('All areas completed!')
        return

    area = self.areas[self.current_idx]
    self.get_logger().info(f'Navigating to: {area["name"]} ({self.current_idx+1}/{len(self.areas)})')

    goal = AreaNav.Goal()
    goal.module_name = 'sequential_nav'
    goal.obj_level = 1  # 房间级别

    for x, y in area['points']:
        p = Point()
        p.x, p.y = x, y
        goal.object_area.append(p)

    self._nav_client.send_goal_async(goal).add_done_callback(self._goal_response_cb)

def _goal_response_cb(self, future):
    goal_handle = future.result()
    if not goal_handle.accepted:
        self.get_logger().error(f'Navigation to {self.areas[self.current_idx]["name"]} rejected')
    else:
        self.get_logger().info(f'Navigation accepted: {self.areas[self.current_idx]["name"]}')
        goal_handle.get_result_async().add_done_callback(self._result_cb)

def _result_cb(self, future):
    result = future.result().result
    area_name = self.areas[self.current_idx]['name']
    if result.result_state == 0:
        self.get_logger().info(f'Arrived at {area_name}')
    else:
        self.get_logger().error(f'Navigation failed: state={result.result_state}')
    self.current_idx += 1
    self.get_logger().info('Waiting 5s before next area...')
    time.sleep(5)
    self._navigate_next()

def main(): rclpy.init() node = SequentialNavNode() node.start() try: rclpy.spin(node) except KeyboardInterrupt: pass finally: node.destroy_node() rclpy.shutdown()

if name == 'main': main()

4.2 场景 B：视觉感知与抓取节点（模板）

目标：使用 VLM 定位物体 → 获取 6D 位姿 → 执行自适应抓取。

涉及接口：

接口	类型	路径	说明
VlmLocation	Service	`/visual_model_interface/location`	VLM 物体识别定位
Omni6dPose	Service	`/omni_6d_pose`	物体 6D 位姿检测
ArmAdaptivePose	Action	`/arm/adaptive`	机械臂自适应抓取/观测
GraspDetect	Service	`/general_grasp_detect`	抓取姿态检测
PlaceDetectOnPutWhere	Service	`/place_detect_on_put_where`	放置位姿推荐

Python 完整示例 — VLM 找物 + 自适应抓取：

#!/usr/bin/env python3
"""
自定义抓取节点：VLM 识别目标 → 6D 位姿 → 机械臂自适应抓取
"""
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from common_msgs.action import ArmAdaptivePose
from common_msgs.srv import VlmLocation, Omni6dPose, GraspDetect
from common_msgs.msg import PoseSixD
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2

class SmartGraspNode(Node): """VLM 感知 + 自适应抓取一体化节点"""

def __init__(self):
    super().__init__('smart_grasp_node')

    # Service Clients
    self.vlm_client = self.create_client(VlmLocation, '/visual_model_interface/location')
    self.pose_client = self.create_client(Omni6dPose, '/omni_6d_pose')
    self.grasp_client = self.create_client(GraspDetect, '/general_grasp_detect')

    # Action Client
    self.arm_client = ActionClient(self, ArmAdaptivePose, '/arm/adaptive')

    # 订阅头部相机
    self.bridge = CvBridge()
    self.latest_image = None
    self.image_sub = self.create_subscription(
        Image, '/berxel_upper/berxel_upper/color/image_raw',
        self._image_cb, 10)

    self.get_logger().info('SmartGraspNode ready')

def _image_cb(self, msg: Image):
    self.latest_image = msg

async def locate_and_grasp(self, object_name: str):
    """完整抓取链路：VLM 定位 → 6D 位姿 → 自适应抓取"""
    # ── Step 1: 等待服务就绪 ──
    await self._wait_for_services()

    # ── Step 2: VLM 定位 ──
    self.get_logger().info(f'[1/5] VLM locating: {object_name}')
    if self.latest_image is None:
        self.get_logger().error('No image available')
        return

    vlm_req = VlmLocation.Request()
    vlm_req.task_id = self.get_name()
    vlm_req.names = [object_name]
    vlm_req.area_name = '客厅'
    vlm_req.img = self.latest_image

    vlm_resp = await self.vlm_client.call_async(vlm_req)
    if len(vlm_resp.box) == 0:
        self.get_logger().error(f'VLM: {object_name} not found')
        return
    target_box = vlm_resp.box[0]
    self.get_logger().info(f'VLM found: {target_box.name} bbox={target_box.bbox}')

    # ── Step 3: 6D 位姿检测 ──
    self.get_logger().info('[2/5] Computing 6D pose')
    pose_req = Omni6dPose.Request()
    pose_req.img = self.latest_image
    pose_req.bbox = target_box.bbox

    pose_resp = await self.pose_client.call_async(pose_req)
    self.get_logger().info(f'6D pose: pos=({pose_resp.position.x:.3f}, '
                           f'{pose_resp.position.y:.3f}, {pose_resp.position.z:.3f})')

    # ── Step 4: 抓取姿态检测 ──
    self.get_logger().info('[3/5] Detecting grasp pose')
    grasp_req = GraspDetect.Request()
    grasp_req.points = pose_resp.position
    grasp_req.bbox = target_box.bbox

    grasp_resp = await self.grasp_client.call_async(grasp_req)
    self.get_logger().info(f'Grasp pose count: {len(grasp_resp.grasp_poses)}')

    # ── Step 5: 机械臂自适应抓取 ──
    self.get_logger().info('[4/5] Executing arm adaptive grasp')
    arm_goal = ArmAdaptivePose.Goal()
    arm_goal.task_type = ArmAdaptivePose.Goal.TASK_ARM_ADAPTIVE  # 抓取模式
    arm_goal.task_id = 0
    arm_goal.gripper_type = ArmAdaptivePose.Goal.GRIPPER_CLOSE  # 夹爪关闭
    arm_goal.poses_six_d = PoseSixD()
    arm_goal.poses_six_d.position = pose_resp.position
    arm_goal.poses_six_d.orientation = pose_resp.orientation

    future = self.arm_client.send_goal_async(arm_goal)
    goal_handle = (await future)
    if not goal_handle.accepted:
        self.get_logger().error('Arm task rejected')
        return

    result_future = goal_handle.get_result_async()
    result = (await result_future).result
    self.get_logger().info(f'[5/5] Grasp done! result={result.result}')

async def _wait_for_services(self):
    """等待所有服务就绪"""
    services = [
        (self.vlm_client, '/visual_model_interface/location'),
        (self.pose_client, '/omni_6d_pose'),
        (self.grasp_client, '/general_grasp_detect'),
    ]
    for client, name in services:
        if not client.wait_for_service(timeout_sec=10.0):
            raise RuntimeError(f'Service {name} not available')
    if not self.arm_client.wait_for_server(timeout_sec=10.0):
        raise RuntimeError('Action /arm/adaptive not available')

def main(): rclpy.init() node = SmartGraspNode()

# 异步执行抓取任务
import asyncio
loop = asyncio.get_event_loop()
loop.create_task(node.locate_and_grasp('玩具汽车'))

rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()

if name == 'main': main()

4.3 场景 C：语义地图管理节点（模板）

目标：查询/添加/删除/修改语义地图中的家具和分区。

涉及接口：

接口	类型	路径	说明
MapManager	Action	`/map_manager`	语义地图增删改查
SlamMap	Service	`/slam_map_manager`	存储 SLAM 地图
SemanticExplore	Action	`/semantic_explore_action`	语义建图
SemanticPartitionUpdate	Action	`/semantic_partition_update_action`	手动分区更新

Python 示例 — 查询并列出所有家具：

#!/usr/bin/env python3
"""语义地图查询工具节点"""
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from common_msgs.action import MapManager
from common_msgs.msg import ObjectInfo

class MapQueryNode(Node): def init(self): super().init('map_query_node') self._client = ActionClient(self, MapManager, '/map_manager') self.get_logger().info('MapQueryNode ready')

def query_all_furniture(self):
    if not self._client.wait_for_server(timeout_sec=5.0):
        self.get_logger().error('/map_manager not available')
        return

    goal = MapManager.Goal()
    goal.cmd = 1  # QUERY

    self.get_logger().info('Querying semantic map...')
    self._client.send_goal_async(goal).add_done_callback(self._result_cb)

def add_furniture(self, name: str, model_name: str, content: list, direction: list):
    """添加一个新家具到语义地图"""
    goal = MapManager.Goal()
    goal.cmd = 0  # ADD
    # 设置 object_info 字段（由 MapManager action 的 feedback 承载）
    self._client.send_goal_async(goal).add_done_callback(self._result_cb)

def _result_cb(self, future):
    result = future.result().result
    if result.error_code == 0:
        self.get_logger().info(f'Map operation success, objects: {len(result.objects_info)}')
        for obj in result.objects_info:
            self.get_logger().info(
                f'  [{obj.obj_type}] {obj.only_name} ({obj.obj_name})'
                f'  parent={obj.parent}  uuid={obj.uuid}')
    else:
        self.get_logger().error(f'Map operation failed: code={result.error_code}')

def main(): rclpy.init() node = MapQueryNode() node.query_all_furniture() rclpy.spin(node) node.destroy_node() rclpy.shutdown()

if name == 'main': main()

4.4 场景 D：设备信息采集

目标：采集机器人 CPU/内存/磁盘/网络等系统级信息，定时输出并发布到 Topic。纯 Python 实现，不依赖 common_msgs 也能运行。

发布 Topic：

Topic	类型	说明
`/device/info`	`std_msgs/String`	设备信息汇总字符串

Python 完整示例：

#!/usr/bin/env python3
"""
八界机器人 — 设备信息采集节点
采集 CPU/内存/磁盘/网络/ROS2 环境信息，定时输出并发布到 /device/info
运行: ros2 run ros_node_demo device_info
"""
import os, platform, socket, time
from datetime import datetime
import rclpy
from rclpy.node import Node
from std_msgs.msg import String

def _read_file(path: str, default: str = "N/A") -> str: try: with open(path) as f: return f.read().strip() except Exception: return default

class DeviceInfoNode(Node): def init(self): super().init('device_info') self.declare_parameter('report_interval', 10.0) self.interval = self.get_parameter('report_interval').value self.pub = self.create_publisher(String, '/device/info', 10) self.timer = self.create_timer(self.interval, self._report) self.get_logger().info(f'DeviceInfoNode started (interval={self.interval}s)') self._report()

def _report(self):
    info = self._collect()
    self.get_logger().info('\n' + info)
    msg = String(); msg.data = info; self.pub.publish(msg)

def _collect(self) -&gt; str:
    now = datetime.now().strftime('%Y-%m-%d %H:%M:%S')
    hostname = socket.gethostname()

    # CPU
    nproc = os.cpu_count() or 0
    load = _read_file('/proc/loadavg').split()[0:3]
    load_str = ' / '.join(load) if len(load) == 3 else 'N/A'

    # 内存
    mem = {}
    try:
        with open('/proc/meminfo') as f:
            for line in f:
                p = line.split(':')
                if len(p) == 2: mem[p[0].strip()] = p[1].strip()
    except: pass
    total_mb = int(mem.get('MemTotal','0').split()[0]) / 1024
    avail_mb = int(mem.get('MemAvailable','0').split()[0]) / 1024

    # 磁盘
    try:
        stat = os.statvfs('/')
        total_gb = stat.f_frsize * stat.f_blocks / (1024**3)
        avail_gb = stat.f_frsize * stat.f_bavail / (1024**3)
    except: total_gb = avail_gb = 0

    # 网络
    ifaces = []
    for name in sorted(os.listdir('/sys/class/net/')):
        if name == 'lo': continue
        st = _read_file(f'/sys/class/net/{name}/operstate','?')
        ifaces.append(f'      {name}: {st}')

    return (
        f'==== 八界机器人设备信息  {now} ====\n'
        f'  主机名: {hostname}  架构: {platform.machine()}\n'
        f'  CPU: {nproc} 核  负载: {load_str}\n'
        f'  内存: {total_mb:.0f}MB 总量  {avail_mb:.0f}MB 可用\n'
        f'  磁盘: {total_gb:.1f}GB 总量  {avail_gb:.1f}GB 可用\n'
        f'  ROS2: {os.environ.get("ROS_DISTRO","N/A")}  '
        f'Domain={os.environ.get("ROS_DOMAIN_ID","N/A")}\n'
        f'  网络接口:\n' + '\n'.join(ifaces or ['      (无)']) + '\n'
        f'=========================================='
    )

def main(): rclpy.init() node = DeviceInfoNode() try: rclpy.spin(node) except KeyboardInterrupt: pass finally: node.destroy_node(); rclpy.shutdown()

if name == 'main': main()

运行方式（已在机器人 192.168.3.12 上验证通过）：

ssh Agi@192.168.3.12
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export CYCLONEDDS_URI=file:///factory/default.xml
source ~/bajie_ws/install/setup.bash
ros2 run ros_node_demo device_info
自定义间隔
ros2 run ros_node_demo device_info --ros-args -p report_interval:=5.0

4.5 场景 E：任务编排节点

目标：将多个 Action 串联成完整的业务工作流。

工作流模式：

建图完成 → 语义建图 → 重定位 → 导航到区域 → 头部搜索物体 → VLM 定位 → 6D 位姿 → 
观测自适应 → 手臂拍照 → 抓取检测 → 自适应抓取 → 导航到放置区域 → 放置检测 → 执行放置

Python 示例 — 玩具收纳完整工作流：

#!/usr/bin/env python3
"""
玩具收纳自动化工作流节点
流程：语义建图 → 重定位 → 导航到玩具区 → 搜索玩具 → 抓取 → 放入收纳筐
"""
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from common_msgs.action import (
    SemanticExplore, SemanticLocate, AreaNav,
    Search, ArmAdaptivePose, Recharge
)
from common_msgs.srv import (
    VlmLocation, Omni6dPose, PlaceDetectOnPutWhere
)
from common_msgs.msg import PoseSixD
import time

class ToyStorageWorkflow(Node): """玩具收纳端到端工作流"""

def __init__(self):
    super().__init__('toy_storage_workflow')

    # Action Clients
    self.semantic_explore_client = ActionClient(self, SemanticExplore, '/semantic_explore_action')
    self.semantic_locate_client = ActionClient(self, SemanticLocate, '/semantic_locate_action')
    self.area_nav_client = ActionClient(self, AreaNav, '/nav2_area_navi_points')
    self.search_client = ActionClient(self, Search, '/searching_manager_action')
    self.arm_client = ActionClient(self, ArmAdaptivePose, '/arm/adaptive')
    self.recharge_client = ActionClient(self, Recharge, '/recharge/action')

    # Service Clients
    self.vlm_client = self.create_client(VlmLocation, '/visual_model_interface/location')
    self.pose_client = self.create_client(Omni6dPose, '/omni_6d_pose')
    self.place_client = self.create_client(PlaceDetectOnPutWhere, '/place_detect_on_put_where')

    self.get_logger().info('=== ToyStorageWorkflow ready ===')

async def run_workflow(self):
    """执行完整玩具收纳工作流"""
    try:
        # Step 1: 语义建图
        self.get_logger().info('[1/8] Semantic explore...')
        await self._execute_semantic_explore()

        # Step 2: 语义定位 (找收纳筐)
        self.get_logger().info('[2/8] Locating storage container...')
        container = await self._locate_container()

        # Step 3: 导航到玩具区域
        self.get_logger().info('[3/8] Navigating to toy area...')
        await self._navigate_to_area('玩具收纳区')

        # Step 4: 搜索玩具
        self.get_logger().info('[4/8] Searching for toys...')
        toys = await self._search_objects('玩具', '玩具收纳区')

        if not toys:
            self.get_logger().warn('No toys found, workflow ending')
            return

        # Step 5: 逐个抓取玩具
        for i, toy in enumerate(toys):
            self.get_logger().info(f'[5/8] Grabbing toy {i+1}/{len(toys)}...')
            await self._grab_object(toy)

            # Step 6: 导航到容器
            self.get_logger().info(f'[6/8] Navigating to container...')
            # (使用 container 位置导航)

            # Step 7: 放入容器
            self.get_logger().info(f'[7/8] Placing in container...')
            await self._place_in_container(container)

        # Step 8: 回充
        self.get_logger().info('[8/8] Returning to dock...')
        await self._return_to_dock()

        self.get_logger().info('=== Workflow completed! ===')

    except Exception as e:
        self.get_logger().error(f'Workflow failed: {e}')

async def _execute_semantic_explore(self):
    """执行语义建图"""
    if not self.semantic_explore_client.wait_for_server(timeout_sec=10.0):
        raise RuntimeError('SemanticExplore not available')

    goal = SemanticExplore.Goal()
    goal.task_name = 'semantic_explore'
    goal.task_id = 'workflow_001'

    future = self.semantic_explore_client.send_goal_async(goal)
    goal_handle = (await future)
    if not goal_handle.accepted:
        raise RuntimeError('Semantic explore rejected')

    result = (await goal_handle.get_result_async()).result
    if result.error_code != 0:
        raise RuntimeError(f'Semantic explore failed: {result.error_code}')

async def _locate_container(self):
    """定位收纳容器"""
    if not self.semantic_locate_client.wait_for_server(timeout_sec=10.0):
        raise RuntimeError('SemanticLocate not available')
    # ... 实现定位逻辑
    pass

async def _navigate_to_area(self, area_name: str):
    """导航到指定语义区域"""
    if not self.area_nav_client.wait_for_server(timeout_sec=10.0):
        raise RuntimeError('AreaNav not available')
    # ... 实现导航逻辑
    pass

async def _search_objects(self, object_type: str, area: str):
    """搜索物体"""
    if not self.search_client.wait_for_server(timeout_sec=10.0):
        raise RuntimeError('Search not available')

    goal = Search.Goal()
    goal.name = object_type
    goal.area_type = Search.Goal.AREA_TYPE_TOY_AREA

    future = self.search_client.send_goal_async(goal)
    goal_handle = (await future)
    if not goal_handle.accepted:
        return []
    result = (await goal_handle.get_result_async()).result
    return result.object_boxes_6dpose

async def _grab_object(self, pose_6d: PoseSixD):
    """抓取物体"""
    goal = ArmAdaptivePose.Goal()
    goal.task_type = ArmAdaptivePose.Goal.TASK_ARM_ADAPTIVE
    goal.gripper_type = ArmAdaptivePose.Goal.GRIPPER_CLOSE
    goal.poses_six_d = pose_6d

    future = self.arm_client.send_goal_async(goal)
    goal_handle = (await future)
    if not goal_handle.accepted:
        raise RuntimeError('Grasp rejected')
    result = (await goal_handle.get_result_async()).result
    if result.result != 0:
        raise RuntimeError(f'Grasp failed: {result.result}')

async def _place_in_container(self, container):
    """放入容器"""
    # 使用 PlaceDetectOnPutWhere 获取放置位姿
    pass

async def _return_to_dock(self):
    """回充"""
    if not self.recharge_client.wait_for_server(timeout_sec=10.0):
        self.get_logger().warn('Recharge server not available, skipping')
        return
    goal = Recharge.Goal()
    future = self.recharge_client.send_goal_async(goal)
    goal_handle = (await future)
    result = (await goal_handle.get_result_async()).result
    self.get_logger().info(f'Recharge result: {result.error_code}')

def main(): rclpy.init() import asyncio node = ToyStorageWorkflow() loop = asyncio.get_event_loop() loop.create_task(node.run_workflow()) rclpy.spin(node) node.destroy_node() rclpy.shutdown()

if name == 'main': main()

4.6 场景 F：电量与状态监控

目标：订阅机器人电量、告警、关节、里程计、驱动轮/机械臂使能状态，定时汇总输出。

涉及 Topic：

Topic	类型	说明
`/power/battery/state`	`common_msgs/BatteryState`	电池电量百分比与充放电状态
`/robot/alarm`	`common_msgs/Alarm`	MCU 异常告警
`/joint_states`	`sensor_msgs/JointState`	关节轴状态
`/odom`	`nav_msgs/Odometry`	里程计位置
`/actuator/motor/driver/state`	`std_msgs/Bool`	驱动轮使能/失能
`/handle/state`	`std_msgs/Bool`	机械臂使能/失能

Python 完整示例：

#!/usr/bin/env python3
"""
八界机器人状态监控节点
获取电量、告警、关节、里程计、驱动轮/机械臂使能状态
"""
from datetime import datetime
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy

from common_msgs.msg import BatteryState, Alarm from sensor_msgs.msg import JointState from nav_msgs.msg import Odometry from std_msgs.msg import Bool

class StatusMonitor(Node): def init(self): super().init('status_monitor') self.declare_parameter('report_interval', 3.0) interval = self.get_parameter('report_interval').value qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT)

    # ── 电池 ──
    self.battery = None
    self.create_subscription(BatteryState, '/power/battery/state',
                             self._battery_cb, qos)

    # ── 告警 ──
    self.alarms = []
    self.create_subscription(Alarm, '/robot/alarm',
                             self._alarm_cb, qos)

    # ── 关节状态 ──
    self.joint_state = None
    self.create_subscription(JointState, '/joint_states',
                             self._joint_cb, qos)

    # ── 里程计 ──
    self.odom = None
    self.create_subscription(Odometry, '/odom',
                             self._odom_cb, qos)

    # ── 驱动轮使能 ──
    self.driver_enabled = None
    self.create_subscription(Bool, '/actuator/motor/driver/state',
                             lambda m: setattr(self, 'driver_enabled', m.data), qos)

    # ── 机械臂使能 ──
    self.arm_enabled = None
    self.create_subscription(Bool, '/handle/state',
                             lambda m: setattr(self, 'arm_enabled', m.data), qos)

    # 定时输出
    self.timer = self.create_timer(interval, self._report)
    self.get_logger().info(f'StatusMonitor started (interval={interval}s)')

def _battery_cb(self, msg: BatteryState):
    self.battery = msg

def _alarm_cb(self, msg: Alarm):
    self.alarms.append(msg)
    self.get_logger().warn(f'ALARM: code={msg.code}')

def _joint_cb(self, msg: JointState):
    self.joint_state = msg

def _odom_cb(self, msg: Odometry):
    self.odom = msg

def _report(self):
    now = datetime.now().strftime('%H:%M:%S')
    lines = ['─' * 48, f'  八界机器人状态  [{now}]', '─' * 48]

    # 电池
    if self.battery:
        smap = {0: '未知', 1: '充电中', 2: '放电中', 3: '未充电', 4: '已充满'}
        s = smap.get(self.battery.status, '?')
        lines.append(f'  [电池] {self.battery.percent:.0f}%  |  {s}')
    else:
        lines.append(f'  [电池] (等待数据...)')

    # 里程计
    if self.odom:
        p = self.odom.pose.pose.position
        lines.append(f'  [里程] x={p.x:.2f}  y={p.y:.2f}  z={p.z:.2f}')
    else:
        lines.append(f'  [里程] (等待数据...)')

    # 关节
    if self.joint_state:
        lines.append(f'  [关节] {len(self.joint_state.name)} 个轴')
    else:
        lines.append(f'  [关节] (等待数据...)')

    # 使能状态
    parts = []
    if self.driver_enabled is not None:
        parts.append(f'驱动轮={"OK" if self.driver_enabled else "OFF"}')
    if self.arm_enabled is not None:
        parts.append(f'机械臂={"OK" if self.arm_enabled else "OFF"}')
    lines.append(f'  [使能] {", ".join(parts) if parts else "(等待数据...)"}')

    # 告警
    lines.append(f'  [告警] 累计 {len(self.alarms)} 条')
    lines.append('─' * 48)
    self.get_logger().info('\n'.join(lines))

def main(): rclpy.init() node = StatusMonitor() try: rclpy.spin(node) except KeyboardInterrupt: pass finally: node.destroy_node() rclpy.shutdown()

if name == 'main': main()

运行方式（已在机器人 192.168.3.12 上验证通过）：

# 登录机器人
ssh Agi@192.168.3.12
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export CYCLONEDDS_URI=file:///factory/default.xml
加载环境并运行（默认每 3 秒）
source ~/bajie_ws/install/setup.bash
ros2 run ros_node_demo status_monitor
自定义间隔（每 5 秒）
ros2 run ros_node_demo status_monitor --ros-args -p report_interval:=5.0

预期输出（ROS2 应用运行时）：

────────────────────────────────────────────────
  八界机器人状态  [14:32:15]
────────────────────────────────────────────────
  [电池] 85%  |  放电中
  [里程] x=1.23  y=0.56  z=0.00
  [关节] 7 个轴
  [使能] 驱动轮=OK, 机械臂=OK
  [告警] 累计 0 条
────────────────────────────────────────────────

说明：BatteryState 的 status 字段对应关系：0=未知, 1=充电中, 2=放电中, 3=未充电, 4=已充满。告警码列表见 §7.4 错误码速查。

同时启动两个节点：

ssh Agi@192.168.3.12
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export CYCLONEDDS_URI=file:///factory/default.xml
source ~/bajie_ws/install/setup.bash
后台同时运行两个节点
ros2 run ros_node_demo device_info --ros-args -p report_interval:=10.0 &
ros2 run ros_node_demo status_monitor --ros-args -p report_interval:=5.0 &

4.7 场景 G：夹爪开闭控制

目标：通过 ROS2 标准接口控制夹爪打开和关闭。

涉及接口：

接口	类型	路径	说明
GripperCommand	Action	`/hand_controller/gripper_cmd`	标准夹爪控制 `control_msgs`

关键参数：position 为夹爪关节弧度，0.0=张开\~0\.54rad，0.3=闭合\~0\.01rad。

Python 完整示例：

#!/usr/bin/env python3
"""
八界机器人 — 夹爪开闭控制节点

用法: ros2 run ros_node_demo gripper_ctrl --ros-args -p action:=open ros2 run ros_node_demo gripper_ctrl --ros-args -p action:=close """ import time import rclpy from rclpy.node import Node from rclpy.action import ActionClient from control_msgs.action import GripperCommand

class GripperControl(Node): def init(self): super().init('gripper_ctrl') self.declare_parameter('action', 'open') self.action = self.get_parameter('action').value self._client = ActionClient(self, GripperCommand, '/hand_controller/gripper_cmd') self._goal_done = False

def execute(self):
    # position: 夹爪角度(弧度), 0.0=张开(~0.54rad), 0.3=闭合(~0.01rad)
    task_map = {
        'open':  (0.0, '打开'),
        'close': (0.3, '关闭'),
    }
    pos, label = task_map.get(self.action, (None, None))
    if pos is None:
        self.get_logger().error(f'无效动作: {self.action}，可选: open / close')
        return False

    if not self._client.wait_for_server(timeout_sec=5.0):
        self.get_logger().error('/hand_controller/gripper_cmd not available')
        return False

    self.get_logger().info(f'执行夹爪{label} (position={pos} rad)...')
    goal = GripperCommand.Goal()
    goal.command.position = pos
    goal.command.max_effort = 30.0
    self._client.send_goal_async(goal).add_done_callback(self._goal_cb)
    return True

def _goal_cb(self, future):
    goal_handle = future.result()
    if not goal_handle.accepted:
        self.get_logger().error('指令被拒绝')
        self._goal_done = True
        return
    self.get_logger().info('指令已接受，等待执行...')
    goal_handle.get_result_async().add_done_callback(self._result_cb)

def _result_cb(self, future):
    r = future.result().result
    self.get_logger().info(f'完成: position={r.position:.3f}rad  reached_goal={r.reached_goal}')
    self._goal_done = True

def is_done(self):
    return self._goal_done

def main(): rclpy.init() node = GripperControl() executor = rclpy.executors.SingleThreadedExecutor() executor.add_node(node)

if node.execute():
    timeout = 15.0
    start = time.time()
    while not node.is_done() and time.time() - start &lt; timeout:
        executor.spin_once(timeout_sec=0.1)
    if not node.is_done():
        node.get_logger().warn('等待结果超时')

executor.remove_node(node)
node.destroy_node()
rclpy.shutdown()

if name == 'main': main()

运行方式（已在机器人 192.168.3.12 上验证通过）：

ssh Agi@192.168.3.12
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export CYCLONEDDS_URI=file:///factory/default.xml
source ~/bajie_ws/install/setup.bash
打开夹爪
ros2 run ros_node_demo gripper_ctrl --ros-args -p action:=open
关闭夹爪
ros2 run ros_node_demo gripper_ctrl --ros-args -p action:=close

命令行快速测试：

# 打开
ros2 action send_goal /hand_controller/gripper_cmd control_msgs/action/GripperCommand \
  '{command: {position: 0.0, max_effort: 30.0}}'
关闭
ros2 action send_goal /hand_controller/gripper_cmd control_msgs/action/GripperCommand 

'{command: {position: 0.3, max_effort: 30.0}}'

4.8 场景 H：底盘传感器仪表盘

目标：一次性订阅 13 个底盘传感器 Topic，定时输出结构化仪表盘。

涉及 Topic：

Topic	类型	说明
`/power/battery/state`	`common_msgs/BatteryState`	电池电量
`/power/mode/state`	`common_msgs/PowerMode`	功耗模式
`/robot/alarm`	`common_msgs/Alarm`	告警
`/sensor/imu/chassis`	`sensor_msgs/Imu`	底盘 IMU
`/sensor/imu/jaw`	`sensor_msgs/Imu`	夹爪 IMU
`/sensor/bumper`	`common_msgs/BoolArray`	撞板
`/sensor/touch`	`common_msgs/SensorTouch`	触觉
`/sensor/downview`	`common_msgs/Uint16Array`	下视
`/sensor/keystroke`	`common_msgs/BoolArray`	开关量
`/joint_states`	`sensor_msgs/JointState`	关节
`/odom`	`nav_msgs/Odometry`	里程计
`/actuator/motor/driver/status`	`common_msgs/DriveMotorStatus`	驱动轮状态

Python 完整示例：

#!/usr/bin/env python3
"""底盘传感器仪表盘 — 订阅 13 个 Topic，定时输出结构化视图"""
from datetime import datetime
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy
from common_msgs.msg import (
    BatteryState, Alarm, PowerMode, BoolArray,
    SensorTouch, Uint16Array, DriveMotorStatus,
)
from sensor_msgs.msg import JointState, Imu
from nav_msgs.msg import Odometry

class ChassisDashboard(Node): def init(self): super().init('chassis_dash') self.declare_parameter('report_interval', 5.0) interval = self.get_parameter('report_interval').value qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT)

    self.battery = None
    self.create_subscription(BatteryState, '/power/battery/state', self._cb('battery'), qos)
    self.power_mode = None
    self.create_subscription(PowerMode, '/power/mode/state', self._cb('power_mode'), qos)
    self.alarms = []
    self.create_subscription(Alarm, '/robot/alarm', self._alarm_cb, qos)
    self.imu_chassis = None
    self.create_subscription(Imu, '/sensor/imu/chassis', self._cb('imu_chassis'), qos)
    self.imu_jaw = None
    self.create_subscription(Imu, '/sensor/imu/jaw', self._cb('imu_jaw'), qos)
    self.bumper = None
    self.create_subscription(BoolArray, '/sensor/bumper', self._cb('bumper'), qos)
    self.touch = None
    self.create_subscription(SensorTouch, '/sensor/touch', self._cb('touch'), qos)
    self.downview = None
    self.create_subscription(Uint16Array, '/sensor/downview', self._cb('downview'), qos)
    self.keystroke = None
    self.create_subscription(BoolArray, '/sensor/keystroke', self._cb('keystroke'), qos)
    self.joint_state = None
    self.create_subscription(JointState, '/joint_states', self._cb('joint_state'), qos)
    self.odom = None
    self.create_subscription(Odometry, '/odom', self._cb('odom'), qos)
    self.driver_status = None
    self.create_subscription(DriveMotorStatus, '/actuator/motor/driver/status',
                             self._cb('driver_status'), qos)

    self.timer = self.create_timer(interval, self._report)
    self.get_logger().info(f'ChassisDashboard started (13 topics, interval={interval}s)')

def _cb(self, name):
    return lambda msg: setattr(self, name, msg)

def _alarm_cb(self, msg: Alarm):
    self.alarms.append(msg)

def _report(self):
    now = datetime.now().strftime('%H:%M:%S')
    b = self.battery
    bat_str = f'{b.percent:.0f}%  { {0:"未知",1:"充电中",2:"放电中",3:"未充电",4:"已充满"}.get(b.status,"?")}' if b else '(等待)'
    o = self.odom
    odom_str = f'x={o.pose.pose.position.x:.2f} y={o.pose.pose.position.y:.2f}' if o else '(等待)'
    imu_c = self.imu_chassis
    imu_str = f'a=({imu_c.linear_acceleration.x:.1f},{imu_c.linear_acceleration.y:.1f},{imu_c.linear_acceleration.z:.2f})' if imu_c else '(等待)'
    j = self.joint_state
    joint_str = f'{len(j.name)}轴  {", ".join(f"{n}={p:.2f}" for n,p in zip(j.name[:3],j.position[:3]))}' if j and j.name else '(等待)'

    lines = [
        '┌──────────────────────────────────────────┐',
        f'│  底盘仪表盘  [{now}]              │',
        '├──────────────────────────────────────────┤',
        f'│ 电池: {bat_str:35s} │',
        f'│ 里程: {odom_str:35s} │',
        f'│ IMU:  {imu_str:35s} │',
        f'│ 关节: {joint_str:35s} │',
        f'│ 触觉: {"已连接" if self.touch else "(等待)":35s} │',
        f'│ 下视: {f"{len(self.downview.data)}个值" if self.downview else "(等待)":35s} │',
        f'│ 驱动: {"已连接" if self.driver_status else "(等待)":35s} │',
        f'│ 告警: {f"{len(self.alarms)}条" if len(self.alarms)==0 else f"{len(self.alarms)}条!!!":35s} │',
        '└──────────────────────────────────────────┘',
    ]
    self.get_logger().info('\n'.join(lines))

def main(): rclpy.init() node = ChassisDashboard() try: rclpy.spin(node) except KeyboardInterrupt: pass finally: node.destroy_node(); rclpy.shutdown()

if name == 'main': main()

运行方式（已在机器人 192.168.3.12 上验证通过）：

ssh Agi@192.168.3.12
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export CYCLONEDDS_URI=file:///factory/default.xml
source ~/bajie_ws/install/setup.bash
ros2 run ros_node_demo chassis_dash
自定义间隔
ros2 run ros_node_demo chassis_dash --ros-args -p report_interval:=3.0

预期输出（ROS2 应用运行时）：

┌──────────────────────────────────────────┐
│  底盘仪表盘  [15:32:15]                  │
├──────────────────────────────────────────┤
│ 电池: 77%  放电中                        │
│ 里程: x=0.59 y=-0.49                     │
│ IMU:  a=(-0.0,-0.0,9.91)                │
│ 关节: 7轴  joint1=-0.04, joint2=2.31     │
│ 触觉: 已连接                              │
│ 下视: 8个值                               │
│ 驱动: 已连接                              │
│ 告警: 0条                                 │
└──────────────────────────────────────────┘

4.9 场景 I：里程计精度验证

目标：订阅 /odom，追踪累计里程、直线位移、路径效率、实时速度，验证里程计精度。

涉及接口：

接口	类型	路径	说明
Odometry	Topic	`/odom`	里程计数据

Python 完整示例：

#!/usr/bin/env python3
"""里程计精度验证 — 追踪移动轨迹，计算里程/位移/速度"""
import math, time
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy
from nav_msgs.msg import Odometry

class OdomChecker(Node): def init(self): super().init('odom_check') self.declare_parameter('report_interval', 2.0) interval = self.get_parameter('report_interval').value qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT)

    self.start_pos = None
    self.start_time = None
    self.current_pos = (0.0, 0.0)
    self.current_yaw = 0.0
    self.current_speed = 0.0
    self.last_pos = None
    self.cumulative_distance = 0.0
    self.samples = 0

    self.create_subscription(Odometry, '/odom', self._odom_cb, qos)
    self.timer = self.create_timer(interval, self._report)
    self.get_logger().info(f'OdomChecker started (interval={interval}s)')

def _odom_cb(self, msg: Odometry):
    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
    now = time.time()
    qz = msg.pose.pose.orientation.z
    qw = msg.pose.pose.orientation.w
    yaw = math.atan2(2.0 * qw * qz, 1.0 - 2.0 * qz * qz)

    self.current_pos = (x, y)
    self.current_yaw = yaw
    self.current_speed = math.sqrt(
        msg.twist.twist.linear.x ** 2 + msg.twist.twist.linear.y ** 2)

    if self.start_pos is None:
        self.start_pos = (x, y)
        self.start_time = now
        self.last_pos = (x, y)
        return

    dx = x - self.last_pos[0]
    dy = y - self.last_pos[1]
    self.cumulative_distance += math.sqrt(dx*dx + dy*dy)
    self.samples += 1
    self.last_pos = (x, y)

def _report(self):
    if self.start_pos is None:
        self.get_logger().info('等待 /odom 数据...')
        return

    elapsed = time.time() - self.start_time
    sx, sy = self.start_pos
    cx, cy = self.current_pos
    displacement = math.sqrt((cx-sx)**2 + (cy-sy)**2)
    efficiency = (displacement / self.cumulative_distance * 100
                  if self.cumulative_distance &gt; 0 else 0)

    lines = [
        '┌──────────────────────────────────────────┐',
        f'│  里程计精度验证                           │',
        '├──────────────────────────────────────────┤',
        f'│ 起点:    ({sx:7.3f}, {sy:7.3f})           │',
        f'│ 当前位置:({cx:7.3f}, {cy:7.3f})  yaw={self.current_yaw:6.3f} │',
        f'│                                          │',
        f'│ 累计里程:   {self.cumulative_distance:8.3f} m              │',
        f'│ 直线位移:   {displacement:8.3f} m              │',
        f'│ 路径效率:   {efficiency:7.1f}%              │',
        f'│                                          │',
        f'│ 当前速度:   {self.current_speed:8.3f} m/s            │',
        f'│ 运行时间:   {elapsed:8.1f} s              │',
        f'│ 采样点数:   {self.samples:8d}              │',
        '└──────────────────────────────────────────┘',
    ]
    self.get_logger().info('\n'.join(lines))

def main(): rclpy.init() node = OdomChecker() try: rclpy.spin(node) except KeyboardInterrupt: pass finally: node.destroy_node(); rclpy.shutdown()

if name == 'main': main()

运行方式：

ssh Agi@192.168.3.12
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export CYCLONEDDS_URI=file:///factory/default.xml
source ~/bajie_ws/install/setup.bash
ros2 run ros_node_demo odom_check

指标说明：

累计里程：odom 上报的所有位移累加（路径总长）
直线位移：起点到当前位置的欧氏距离
路径效率：直线位移 ÷ 累计里程 × 100%（越高越直）
采样频率：约 100Hz（3 秒约 300 个样本）

4.10 场景 J：地图管理

目标：通过 ROS2 Service 管理 SLAM 地图——查询、保存、备份、重定位、完成建图。

涉及接口：

接口	类型	路径	说明
SlamMap	Service	`/slam_map_manager`	SLAM 地图数据库 CRUD
MapSave	Service	`/slam/save_map/upper`	保存地图
MapBackup	Service	`/slam/map_backup`	备份/恢复地图
Relocation	Service	`/slam/relocation/upper`	全局重定位
Trigger	Service	`/slam/finish_create_map`	完成建图
Trigger	Service	`/slam/reset_slam`	重置 SLAM

Python 完整示例：

#!/usr/bin/env python3
"""地图管理 — 查询/保存/备份/重定位/建图控制"""
import rclpy
from rclpy.node import Node
from common_msgs.srv import MapSave, MapBackup, SlamMap, Relocation
from std_srvs.srv import Trigger

class MapManager(Node): def init(self): super().init('map_mgr') self.declare_parameter('cmd', 'list') self.cmd = self.get_parameter('cmd').value

    self._svc = {
        'list':     self.create_client(SlamMap,   '/slam_map_manager'),
        'save':     self.create_client(MapSave,   '/slam/save_map/upper'),
        'backup':   self.create_client(MapBackup, '/slam/map_backup'),
        'relocate': self.create_client(Relocation,'/slam/relocation/upper'),
        'finish':   self.create_client(Trigger,   '/slam/finish_create_map'),
        'reset':    self.create_client(Trigger,   '/slam/reset_slam'),
    }
    self.execute()

def execute(self):
    handlers = {
        'list':     self._do_list,
        'save':     self._do_save,
        'backup':   self._do_backup,
        'relocate': self._do_relocate,
        'finish':   lambda: self._do_trigger('finish', '完成建图'),
        'reset':    lambda: self._do_trigger('reset', '重置SLAM'),
    }
    h = handlers.get(self.cmd)
    if h: h()
    else: self.get_logger().error(f'未知命令: {self.cmd}')

def _do_list(self):
    cli = self._svc['list']
    if not cli.wait_for_service(timeout_sec=3.0): return
    req = SlamMap.Request(cmd=SlamMap.Request.QUERY_DEFAULT)
    future = cli.call_async(req)
    rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)
    if future.done():
        maps = future.result().data
        self.get_logger().info(f'共 {len(maps)} 张地图:')
        for m in maps:
            self.get_logger().info(
                f'  id={m.map_id}  name={m.map_name}  '
                f'size={m.width}x{m.height}  res={m.resolution}')

def _do_save(self):
    cli = self._svc['save']
    if not cli.wait_for_service(timeout_sec=3.0): return
    req = MapSave.Request(map_name='my_house')
    future = cli.call_async(req)
    rclpy.spin_until_future_complete(self, future, timeout_sec=10.0)
    if future.done():
        if future.result().error_code == 0:
            self.get_logger().info('地图保存成功')
        else:
            self.get_logger().error(f'保存失败: code={future.result().error_code}')

def _do_backup(self):
    cli = self._svc['backup']
    if not cli.wait_for_service(timeout_sec=3.0): return
    req = MapBackup.Request(op_type=MapBackup.Request.TYPE_MAP_COPY,
                            map_name='my_house')
    future = cli.call_async(req)
    rclpy.spin_until_future_complete(self, future, timeout_sec=10.0)
    if future.done():
        if future.result().error_code == 0:
            self.get_logger().info('地图备份成功')
        else:
            self.get_logger().error(f'备份失败: code={future.result().error_code}')

def _do_relocate(self):
    cli = self._svc['relocate']
    if not cli.wait_for_service(timeout_sec=3.0): return
    req = Relocation.Request(type=Relocation.Request.TYPE_GLOBAL_RELOCATION)
    self.get_logger().info('执行全局重定位...')
    future = cli.call_async(req)
    rclpy.spin_until_future_complete(self, future, timeout_sec=30.0)
    if future.done():
        if future.result().error_code == 0:
            self.get_logger().info('重定位成功')
        else:
            self.get_logger().error(f'重定位失败: code={future.result().error_code}')

def _do_trigger(self, name, label):
    cli = self._svc[name]
    if not cli.wait_for_service(timeout_sec=3.0): return
    req = Trigger.Request()
    future = cli.call_async(req)
    rclpy.spin_until_future_complete(self, future, timeout_sec=10.0)
    if future.done():
        if future.result().success:
            self.get_logger().info(f'{label}成功')
        else:
            self.get_logger().error(f'{label}失败')

def main(): rclpy.init() node = MapManager() try: rclpy.spin(node) except KeyboardInterrupt: pass finally: node.destroy_node(); rclpy.shutdown()

if name == 'main': main()

运行方式（已在机器人 192.168.3.12 上验证通过）：

ssh Agi@192.168.3.12
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export CYCLONEDDS_URI=file:///factory/default.xml
source ~/bajie_ws/install/setup.bash
列出所有地图
ros2 run ros_node_demo map_mgr --ros-args -p cmd:=list
保存当前地图
ros2 run ros_node_demo map_mgr --ros-args -p cmd:=save
备份地图
ros2 run ros_node_demo map_mgr --ros-args -p cmd:=backup
全局重定位
ros2 run ros_node_demo map_mgr --ros-args -p cmd:=relocate
完成建图
ros2 run ros_node_demo map_mgr --ros-args -p cmd:=finish
重置SLAM
ros2 run ros_node_demo map_mgr --ros-args -p cmd:=reset

验证结果：

命令	结果
`cmd:=list`	✅ 1 张地图：`000`，1200×1200，res=50
`cmd:=save`	✅ 保存成功
`cmd:=relocate`	✅ 服务调用成功

4.11 场景 K：物体搜索

目标：在指定区域或原地搜索物体，返回 6D 抓取位姿列表。

涉及接口：

接口	类型	路径	说明
Search	Action	`/searching_manager_action`	搜索物体

area_type 枚举：-1=原地/点, 1=房间, 2=桌面, 3=小物体, 4=玩具区

Python 完整示例：

#!/usr/bin/env python3
"""物体搜索 — 在指定区域搜索物体，返回 6D 位姿"""
import time
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from common_msgs.action import Search
from common_msgs.msg import ObjectInfo

class ObjectSearch(Node): def init(self): super().init('obj_search') self.declare_parameter('name', '玩具') self.declare_parameter('area', '客厅') self.declare_parameter('area_type', Search.Goal.AREA_TYPE_AREA) self.name = self.get_parameter('name').value self.area = self.get_parameter('area').value self.area_type = self.get_parameter('area_type').value self._client = ActionClient(self, Search, '/searching_manager_action') self._goal_done = False

def execute(self):
    if not self._client.wait_for_server(timeout_sec=5.0):
        self.get_logger().error('/searching_manager_action not available')
        return False
    goal = Search.Goal(
        name=self.name, area_type=self.area_type,
        task_id=f'search_{int(time.time())}',
        destinations=[ObjectInfo(obj_name=self.area)])
    self.get_logger().info(f'搜索: name={self.name}, area_type={self.area_type}...')
    self._client.send_goal_async(goal).add_done_callback(self._goal_cb)
    return True

def _goal_cb(self, future):
    gh = future.result()
    if not gh.accepted:
        self.get_logger().error('搜索被拒绝'); self._goal_done = True; return
    self.get_logger().info('搜索已接受，等待结果...')
    gh.get_result_async().add_done_callback(self._result_cb)

def _result_cb(self, future):
    result = future.result().result
    objs = result.object_boxes_6dpose
    if result.error_code == 0 and objs:
        self.get_logger().info(f'找到 {len(objs)} 个物体:')
        for i, o in enumerate(objs):
            p = o.pose_grasp.pose.position
            self.get_logger().info(
                f'  [{i}] id={o.id}  '
                f'pos=({p.x:.3f},{p.y:.3f},{p.z:.3f})  '
                f'box=({o.object_box_length.x:.3f},{o.object_box_length.y:.3f},{o.object_box_length.z:.3f})')
    else:
        self.get_logger().info(f'未找到物体 (code={result.error_code})')
    self._goal_done = True

def is_done(self): return self._goal_done

def main(): rclpy.init() node = ObjectSearch() executor = rclpy.executors.SingleThreadedExecutor() executor.add_node(node) if node.execute(): t0 = time.time() while not node.is_done() and time.time() - t0 < 120: executor.spin_once(timeout_sec=0.2) if not node.is_done(): node.get_logger().warn('搜索超时') executor.remove_node(node) node.destroy_node(); rclpy.shutdown()

if name == 'main': main()

运行方式（已在机器人 192.168.3.12 上验证通过）：

ssh Agi@192.168.3.12
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export CYCLONEDDS_URI=file:///factory/default.xml
source ~/bajie_ws/install/setup.bash
原地搜索玩具（推荐）
ros2 run ros_node_demo obj_search --ros-args -p name:=玩具 -p area_type:=-1
在客厅搜索
ros2 run ros_node_demo obj_search --ros-args -p name:=鞋子 -p area:=客厅
在玩具区搜索
ros2 run ros_node_demo obj_search --ros-args -p name:=玩具 -p area_type:=4

验证结果：

$ ros2 run ros_node_demo obj_search --ros-args -p name:=玩具 -p area_type:=-1
[INFO] 搜索: name=玩具, area_type=-1...
[INFO] 搜索已接受，等待结果...
[INFO] 找到 1 个物体:
[INFO]   [0] id=0  pos=(0.499,-0.042,0.038)  box=(0.072,0.103,0.056)

4.12 场景 L：头部/升降控制

目标：控制机器人头部 pitch 角度和升降高度。

涉及接口：

接口	类型	路径	说明
RobotControl	Action	`/robot/control`	头部/升降控制

参数范围：pitch_angle：0, 1\.3rad 0\.83=水平，lift_dis：0, 0\.44m

Python 完整示例：

#!/usr/bin/env python3
"""头部/升降控制"""
import time, rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from common_msgs.action import RobotControl

class BodyControl(Node): def init(self): super().init('body_ctrl') self.declare_parameter('cmd', 'head') self.declare_parameter('value', 0.83) self.cmd = self.get_parameter('cmd').value self.value = self.get_parameter('value').value self._client = ActionClient(self, RobotControl, '/robot/control') self._goal_done = False

def execute(self):
    if not self._client.wait_for_server(timeout_sec=5.0):
        self.get_logger().error('/robot/control not available'); return False
    cmd_map = {
        'head': (RobotControl.Goal.TASK_HEAD_PITCH_CONTROL, '头部', f'{self.value} rad'),
        'lift': (RobotControl.Goal.TASK_LIFT_CONTROL, '升降', f'{self.value} m'),
        'lift_ready':   (RobotControl.Goal.TASK_LIFT_READY, '升降归位', ''),
        'lift_lowest':  (RobotControl.Goal.TASK_LIFT_LOWEST_POSITION, '升降最低', ''),
        'lift_highest': (RobotControl.Goal.TASK_LIFT_HIGHEST_POSITION, '升降最高', ''),
    }
    tt, label, vs = cmd_map.get(self.cmd, (None, '', ''))
    if tt is None:
        self.get_logger().error(f'未知: {self.cmd}'); return False
    goal = RobotControl.Goal(task_type=tt)
    if self.cmd == 'head': goal.pitch_angle = self.value
    elif self.cmd == 'lift': goal.lift_dis = self.value
    self.get_logger().info(f'执行{label}: {vs}')
    self._client.send_goal_async(goal).add_done_callback(self._goal_cb)
    return True

def _goal_cb(self, f):
    gh = f.result()
    if not gh.accepted: self.get_logger().error('拒绝'); self._goal_done = True; return
    self.get_logger().info('已接受...')
    gh.get_result_async().add_done_callback(self._result_cb)

def _result_cb(self, f):
    r = f.result().result
    self.get_logger().info('成功' if r.error_code == 0 else f'失败: {r.error_code}')
    self._goal_done = True

def is_done(self): return self._goal_done

def main(): rclpy.init() node = BodyControl() executor = rclpy.executors.SingleThreadedExecutor() executor.add_node(node) if node.execute(): t0 = time.time() while not node.is_done() and time.time() - t0 < 15: executor.spin_once(timeout_sec=0.1) if not node.is_done(): node.get_logger().warn('超时') executor.remove_node(node); node.destroy_node(); rclpy.shutdown()

if name == 'main': main()

运行方式（已在机器人 192.168.3.12 上验证通过）：

ssh Agi@192.168.3.12
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export CYCLONEDDS_URI=file:///factory/default.xml
source ~/bajie_ws/install/setup.bash
头部水平
ros2 run ros_node_demo body_ctrl --ros-args -p cmd:=head -p value:=0.83
升降到 0.22m
ros2 run ros_node_demo body_ctrl --ros-args -p cmd:=lift -p value:=0.22
升降归位
ros2 run ros_node_demo body_ctrl --ros-args -p cmd:=lift_ready

验证结果：

命令	结果
`cmd:=head value:=0.3`	✅ 抬头
`cmd:=head value:=0.83`	✅ 水平
`cmd:=head value:=1.2`	✅ 低头
`cmd:=lift value:=0.22`	✅ 升降 0.22m
`cmd:=lift_ready`	✅ 归位

头部 pitch 范围 0, 1\.3 rad，升降范围 0, 0\.44 m。

4.13 场景 M：MCU 系统信息

目标：查询 MCU 系统信息（软硬件版本、电池、温度、通信延迟）及任务状态。

涉及接口：

接口	类型	路径	说明
BoardInfo	Service	`/system/info`	MCU 系统信息
SystemTaskState	Service	`/system/task/state`	任务执行状态

Python 完整示例：

#!/usr/bin/env python3
"""MCU 系统信息查询"""
import rclpy
from rclpy.node import Node
from common_msgs.srv import BoardInfo, SystemTaskState

class McuInfo(Node): def init(self): super().init('mcu_info') self.declare_parameter('cmd', 'info') self.cmd = self.get_parameter('cmd').value self._svc = { 'info': self.create_client(BoardInfo, '/system/info'), 'task': self.create_client(SystemTaskState, '/system/task/state'), } self.execute()

def execute(self):
    h = {'info': self._do_info, 'task': self._do_task}.get(self.cmd)
    if h: h()
    else: self.get_logger().error(f'未知: {self.cmd}')

def _do_info(self):
    cli = self._svc['info']
    if not cli.wait_for_service(timeout_sec=3.0): return
    future = cli.call_async(BoardInfo.Request())
    rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)
    if not future.done(): self.get_logger().error('超时'); return
    r = future.result()
    self.get_logger().info('══ MCU 系统信息 ══')
    if r.sv: self.get_logger().info(f'[软件] {", ".join(r.sv)}')
    if r.hv: self.get_logger().info(f'[硬件] {", ".join(r.hv)}')
    b = r.bat_state
    if b.percent &gt; 0:
        smap = {0: '未知', 1: '充电中', 2: '放电中', 3: '未充电', 4: '已充满'}
        self.get_logger().info(f'[电池] {b.percent:.0f}% {smap.get(b.status,"?")}')
    if r.ic_temp:
        self.get_logger().info(f'[温度] {", ".join(f"{t:.1f}°C" for t in r.ic_temp)}')
    if r.delay_app:
        self.get_logger().info(f'[延迟] app={",".join(str(d) for d in r.delay_app)}us')
    self.get_logger().info('═══════════════════')

def _do_task(self):
    cli = self._svc['task']
    if not cli.wait_for_service(timeout_sec=3.0): return
    for name in ['chassis', 'charge']:
        req = SystemTaskState.Request(name=name)
        future = cli.call_async(req)
        rclpy.spin_until_future_complete(self, future, timeout_sec=3.0)
        if future.done():
            self.get_logger().info(f'[{name}] state -&gt; code={future.result().error_code}')

def main(): rclpy.init() node = McuInfo() try: rclpy.spin(node) except KeyboardInterrupt: pass finally: node.destroy_node(); rclpy.shutdown()

if name == 'main': main()

运行方式：

# 单次查询
ros2 run ros_node_demo mcu_info --ros-args -p cmd:=info
每 3 秒轮询
ros2 run ros_node_demo mcu_info --ros-args -p cmd:=watch

注意：/system/info 服务调用成功但返回数据为空。底盘传感器的实时数据走 Topic（电池/IMU/关节），可改用 chassis_dash 节点监控。

5. 完整示例：玩具收纳自动化节点

下面提供一个可独立运行的完整玩具收纳自动化 ROS2 节点，完全基于 ROS2 原生 Action/Service/Topic 接口，整合了感知、导航、抓取、放置的业务编排逻辑。

5.1 项目结构

my_robot_app/
├── package.xml
├── setup.py
├── setup.cfg
├── resource/
│   └── my_robot_app
├── my_robot_app/
│   ├── __init__.py
│   └── toy_storage_node.py
├── launch/
│   └── toy_storage.launch.py
└── config/
    └── toy_storage_params.yaml

5.2 Python 完整实现

#!/usr/bin/env python3
"""
玩具收纳自动化 ROS2 节点 — 完整可运行版本

功能：

连接机器人
查询语义地图，找到收纳区域
头部拍照 + VLM/OVD 检测玩具
计算 6D 位姿 + 抓取
放入收纳容器
回充

运行方式: ros2 run my_robot_app toy_storage_node --ros-args -p robot_ip:=10.10.10.11 """ import rclpy from rclpy.node import Node from rclpy.action import ActionClient from rclpy.callback_groups import ReentrantCallbackGroup

ROS2 接口

from common_msgs.action import ( AreaNav, ArmAdaptivePose, Search, Recharge, SemanticLocate ) from common_msgs.srv import VlmLocation, Omni6dPose, PlaceDetectOnPutWhere from common_msgs.msg import PoseSixD

图像处理

from sensor_msgs.msg import Image, CameraInfo from cv_bridge import CvBridge import cv2 import numpy as np

class ToyStorageNode(Node): """玩具收纳自动化节点"""

def __init__(self):
    super().__init__('toy_storage_node',
                     callback_group=ReentrantCallbackGroup())

    # ── 参数 ──
    self.declare_parameter('robot_ip', '10.10.10.11')
    self.declare_parameter('toy_area', '玩具收纳区')
    self.declare_parameter('toy_labels', ['玩具汽车', '玩偶', '玩具积木', '玩具球'])
    self.declare_parameter('container_name', '收纳筐')
    self.declare_parameter('max_toys', 5)

    self.robot_ip = self.get_parameter('robot_ip').value
    self.toy_area = self.get_parameter('toy_area').value
    self.toy_labels = self.get_parameter('toy_labels').value
    self.container_name = self.get_parameter('container_name').value
    self.max_toys = self.get_parameter('max_toys').value

    # ── Action Clients ──
    self.area_nav_client = ActionClient(self, AreaNav, '/nav2_area_navi_points')
    self.arm_client = ActionClient(self, ArmAdaptivePose, '/arm/adaptive')
    self.search_client = ActionClient(self, Search, '/searching_manager_action')
    self.recharge_client = ActionClient(self, Recharge, '/recharge/action')
    self.locate_client = ActionClient(self, SemanticLocate, '/semantic_locate_action')

    # ── Service Clients ──
    self.vlm_client = self.create_client(VlmLocation, '/visual_model_interface/location')
    self.pose_client = self.create_client(Omni6dPose, '/omni_6d_pose')
    self.place_client = self.create_client(PlaceDetectOnPutWhere, '/place_detect_on_put_where')

    # ── 图像 ──
    self.bridge = CvBridge()
    self.head_image = None
    self.head_depth = None
    self.image_sub = self.create_subscription(
        Image, '/berxel_upper/berxel_upper/color/image_raw',
        self._head_color_cb, 10)
    self.depth_sub = self.create_subscription(
        Image, '/berxel_upper/berxel_upper/aligned_depth_to_color/image_raw',
        self._head_depth_cb, 10)

    # ── 状态 ──
    self.storage_count = 0
    self.get_logger().info(f'ToyStorageNode ready (robot={self.robot_ip})')

# ── 图像回调 ──
def _head_color_cb(self, msg: Image):
    self.head_image = msg

def _head_depth_cb(self, msg: Image):
    self.head_depth = msg

# ── 等待辅助 ──
async def _wait_for_services(self):
    services = [
        (self.vlm_client, 'VlmLocation'),
        (self.pose_client, 'Omni6dPose'),
        (self.place_client, 'PlaceDetectOnPutWhere'),
    ]
    for client, name in services:
        if not client.wait_for_service(timeout_sec=10.0):
            raise RuntimeError(f'{name} not available')

    actions = [
        (self.area_nav_client, 'AreaNav'),
        (self.arm_client, 'ArmAdaptivePose'),
        (self.search_client, 'Search'),
    ]
    for client, name in actions:
        if not client.wait_for_server(timeout_sec=10.0):
            raise RuntimeError(f'{name} not available')

# ── 核心工作流 ──
async def run(self):
    """执行完整玩具收纳流程"""
    try:
        self.get_logger().info('=' * 50)
        self.get_logger().info('Starting Toy Storage Workflow')
        self.get_logger().info(f'Target area: {self.toy_area}')
        self.get_logger().info(f'Toy labels: {self.toy_labels}')
        self.get_logger().info('=' * 50)

        await self._wait_for_services()

        # Phase 1: 搜索玩具
        toys = await self._phase_search_toys()
        if not toys:
            self.get_logger().info('No toys to store. Done!')
            await self._phase_return_dock()
            return

        # Phase 2: 逐个收纳
        for i, toy in enumerate(toys):
            self.get_logger().info(f'\n--- Processing toy {i+1}/{len(toys)} ---')
            await self._phase_navigate_to_toy(toy)
            await self._phase_grab_toy(toy)
            await self._phase_navigate_to_container()
            await self._phase_place_toy()
            self.storage_count += 1

        # Phase 3: 回充
        await self._phase_return_dock()

        self.get_logger().info(f'\n=== Done! Stored {self.storage_count} toys ===')

    except Exception as e:
        self.get_logger().error(f'Workflow error: {e}')
        raise

async def _phase_search_toys(self):
    """Phase 1: 搜索散落的玩具"""
    self.get_logger().info('[Phase 1] Searching for toys...')

    # 先用 VLM 识别
    if self.head_image is None:
        self.get_logger().warn('No head image yet, waiting 3s...')
        await self._sleep(3)

    if self.head_image:
        vlm_req = VlmLocation.Request()
        vlm_req.task_id = 'toy_storage'
        vlm_req.names = self.toy_labels
        vlm_req.area_name = self.toy_area
        vlm_req.img = self.head_image

        vlm_resp = await self.vlm_client.call_async(vlm_req)
        self.get_logger().info(f'VLM found {len(vlm_resp.box)} objects')

        if vlm_resp.box:
            return list(vlm_resp.box)

    # 备选：使用 Search Action
    search_goal = Search.Goal()
    search_goal.name = '玩具'
    search_goal.area_type = Search.Goal.AREA_TYPE_TOY_AREA

    future = self.search_client.send_goal_async(search_goal)
    goal_handle = (await future)
    if not goal_handle.accepted:
        return []
    result = (await goal_handle.get_result_async()).result
    return result.object_boxes_6dpose

async def _phase_navigate_to_toy(self, toy):
    """导航到玩具附近"""
    self.get_logger().info(f'[Phase 2a] Navigating to toy: {toy.name}')

    goal = AreaNav.Goal()
    goal.module_name = 'toy_storage'
    goal.obj_level = 3  # 小物体级别

    # 使用物体位置构造导航目标区域
    if hasattr(toy, 'bbox'):
        from geometry_msgs.msg import Point
        x1, y1, x2, y2 = toy.bbox
        for x, y in [(x1, y1), (x2, y1), (x2, y2), (x1, y2)]:
            p = Point(); p.x = x; p.y = y
            goal.object_area.append(p)

    future = self.area_nav_client.send_goal_async(goal)
    goal_handle = (await future)
    if not goal_handle.accepted:
        raise RuntimeError(f'Nav to {toy.name} rejected')

    result = (await goal_handle.get_result_async()).result
    self.get_logger().info(f'Nav result: state={result.result_state}')

async def _phase_grab_toy(self, toy):
    """抓取玩具"""
    self.get_logger().info('[Phase 2b] Grabbing toy...')

    # 获取 6D 位姿
    pose_req = Omni6dPose.Request()
    if self.head_image:
        pose_req.img = self.head_image
    if hasattr(toy, 'bbox'):
        pose_req.bbox = toy.bbox

    pose_resp = await self.pose_client.call_async(pose_req)
    self.get_logger().info(
        f'6D pose: ({pose_resp.position.x:.3f}, '
        f'{pose_resp.position.y:.3f}, {pose_resp.position.z:.3f})')

    # 执行自适应抓取
    arm_goal = ArmAdaptivePose.Goal()
    arm_goal.task_type = ArmAdaptivePose.Goal.TASK_ARM_ADAPTIVE
    arm_goal.task_id = 0
    arm_goal.gripper_type = ArmAdaptivePose.Goal.GRIPPER_CLOSE
    arm_goal.poses_six_d = PoseSixD()
    arm_goal.poses_six_d.position = pose_resp.position
    arm_goal.poses_six_d.orientation = pose_resp.orientation

    future = self.arm_client.send_goal_async(arm_goal)
    goal_handle = (await future)
    if not goal_handle.accepted:
        raise RuntimeError('Grasp rejected')

    result = (await goal_handle.get_result_async()).result
    if result.result != 0:
        raise RuntimeError(f'Grasp failed: code={result.result}')
    self.get_logger().info('Grasp success!')

async def _phase_navigate_to_container(self):
    """导航到收纳容器"""
    self.get_logger().info('[Phase 2c] Navigating to container...')
    # 使用 SemanticLocate 定位容器位置
    locate_goal = SemanticLocate.Goal()
    # ... 设置容器定位参数
    # 简化实现：假设已在容器附近

async def _phase_place_toy(self):
    """放入收纳容器"""
    self.get_logger().info('[Phase 2d] Placing in container...')

    if not self.head_image:
        self.get_logger().warn('No image for placement, skipping')
        return

    # 使用 PlaceDetectOnPutWhere 获取放置推荐位姿
    place_req = PlaceDetectOnPutWhere.Request()
    place_req.image = self.head_image
    # ... 设置其他参数

    place_resp = await self.place_client.call_async(place_req)
    self.get_logger().info(f'Place position: {place_resp.position}')

    # 执行放置
    arm_goal = ArmAdaptivePose.Goal()
    arm_goal.task_type = ArmAdaptivePose.Goal.TASK_PALLET_ADAPTIVE
    arm_goal.gripper_type = ArmAdaptivePose.Goal.GRIPPER_OPEN
    arm_goal.poses_six_d = PoseSixD()
    arm_goal.poses_six_d.position = place_resp.position

    future = self.arm_client.send_goal_async(arm_goal)
    goal_handle = (await future)
    if not goal_handle.accepted:
        raise RuntimeError('Place rejected')

    result = (await goal_handle.get_result_async()).result
    self.get_logger().info(f'Place result: {result.result}')

async def _phase_return_dock(self):
    """回充"""
    self.get_logger().info('[Phase 3] Returning to dock...')
    if not self.recharge_client.wait_for_server(timeout_sec=5.0):
        self.get_logger().warn('Recharge server unavailable')
        return

    goal = Recharge.Goal()
    future = self.recharge_client.send_goal_async(goal)
    goal_handle = (await future)
    if goal_handle.accepted:
        result = (await goal_handle.get_result_async()).result
        self.get_logger().info(f'Recharge started: code={result.error_code}')

async def _sleep(self, seconds: float):
    """异步等待"""
    import asyncio
    await asyncio.sleep(seconds)

def main(): rclpy.init() import asyncio

node = ToyStorageNode()

async def run():
    await node.run()

asyncio.ensure_future(run())

try:
    rclpy.spin(node)
except KeyboardInterrupt:
    node.get_logger().info('Interrupted by user')
finally:
    node.destroy_node()
    rclpy.shutdown()

if name == 'main': main()

5.3 Launch 文件

# launch/toy_storage.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration

def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument( 'robot_ip', default_value='10.10.10.11', description='Robot IP address'),

    DeclareLaunchArgument(
        'toy_area', default_value='玩具收纳区',
        description='Target toy area name'),

    DeclareLaunchArgument(
        'max_toys', default_value='5',
        description='Maximum toys to collect'),

    Node(
        package='my_robot_app',
        executable='toy_storage_node',
        name='toy_storage_node',
        output='screen',
        parameters=[{
            'robot_ip': LaunchConfiguration('robot_ip'),
            'toy_area': LaunchConfiguration('toy_area'),
            'max_toys': LaunchConfiguration('max_toys'),
        }],
    ),
])</pre>

5.4 参数配置

# config/toy_storage_params.yaml
toy_storage_node:
  ros__parameters:
    robot_ip: "10.10.10.11"
    toy_area: "玩具收纳区"
    toy_labels: ["玩具汽车", "玩偶", "玩具积木", "玩具球", "玩具恐龙"]
    container_name: "收纳筐"
    max_toys: 5
    head_camera_topic: "/berxel_upper/berxel_upper/color/image_raw"
    depth_camera_topic: "/berxel_upper/berxel_upper/aligned_depth_to_color/image_raw"
    nav_timeout_sec: 120.0
    arm_timeout_sec: 180.0
    search_timeout_sec: 60.0

6. 常用接口速查

6.1 Action 接口速查表（可释放）

Action 路径	类型	核心功能	关键参数
`/nav2_area_navi_points`	AreaNav	区域导航	`object_area`, `obj_level`
`/arm/adaptive`	ArmAdaptivePose	机械臂自适应	`task_type`, `poses_six_d`, `gripper_type`
`/arm/multi_angle_ob/task`	MultiAngleObservation	多角度观测	`observation_type`
`/searching_manager_action`	Search	搜索物体	`name`, `type[]`, `subtype[]`, `area_type`
`/semantic_explore_action`	SemanticExplore	语义建图	`task_name`, `partitions[]`
`/semantic_locate_action`	SemanticLocate	语义定位	区域/承载物信息
`/explore_action`	Explore	探索建图	—
`/explore_locate_action`	SemanticLocation	单承载物定位	承载物 ID
`/face_locate_action`	FaceLocate	人脸定位	人脸图像
`/global_path_calculate`	GlobalPathCalculate	全局路径计算	起点/终点
`/map_manager`	MapManager	语义地图增删改查	`cmd`0=ADD,1=QUERY,2=MODIFY,4=DELETE
`/navi_adaptive_pose`	NaviAdaptivePose	导航自适应	`poses_six_d`, 搜索范围
`/observation_adaptive`	ObservationAdaptive	导航观测点计算	区域信息
`/pitch/control`	PitchControl	头部自适应	头部角度
`/recharge/action`	Recharge	回充	—
`/robot/control`	RobotControl	头部/升降控制	`height`, `pitch`
`/teleop`	Teleop	遥控	速度指令
`/slam/get_map`	MapGet	获取 SLAM 地图	—
`/hand_controller/gripper_cmd`	GripperCommand	夹爪开闭控制	`position`rad: 0.0=张开, 0.3=闭合
`/mcu/update`	UpdateMcu	MCU OTA 升级	固件数据

6.2 Service 接口速查表（可释放，关键）

Service 路径	类型	核心功能
`/visual_model_interface/location`	VlmLocation	VLM 物体识别定位
`/visual_model_interface/semantic`	VlmSemantic	VLM 家具/场景识别
`/visual_model_interface/room_property`	VlmRoomProperty	VLM 房间属性识别
`/omni_6d_pose`	Omni6dPose	物体 6D 位姿检测
`/omni_6d_pose_with_tf`	Omni6dPoseWithTF	物体 6D 位姿检测带 TF
`/general_grasp_detect`	GraspDetect	抓取姿态检测
`/place_detect_on_6d_pose`	PlaceDetectOn6dPose	3D 点位放置检测
`/place_detect_on_put_where`	PlaceDetectOnPutWhere	2D 图片推荐放置点
`/sam_bounding_box`	SamBoundingBox	SAM 图像分割输入 bbox
`/sam_omni_6d_pose`	SamOmni6dPose	SAM 图像分割输入 6D pose
`/slam/relocation/upper`	Relocation	上板重定位
`/slam/reset_slam`	Trigger	重置地图开始建图
`/slam/finish_create_map`	Trigger	设置完成建图标志
`/slam/save_map/upper`	MapSave	保存地图
`/slam_map_manager`	SlamMap	SLAM 地图存入数据库
`/map_split`	MapSplit	地图分割
`/compute_near_carrier_position`	ComputeNearCarrierPosition	计算最近承载物位置
`/arm/escape`	ArmEscape	机械臂脱困
`/arm/pause_resume`	PauseResume	机械臂暂停/继续
`/explore_command`	ExploreCommand	探索过程暂停/继续控制
`/semantic_explore_command`	SemanticExploreCommand	语义建图暂停/继续
`/grasp/detection/query`	GraspDetection	指尖掉落检测查询
`/sensor/calib/set`	SensorCalib	传感器标定
`/sensor/odom/reset`	Trigger	Odom 清除
`/actuator/motor/driver/enable_srv`	SetBool	驱动轮使能/失能
`/actuator/motor/driver/set_suspension_lock`	SuspensionCtrl	悬挂控制

6.3 Topic 速查表（常用可释放）

Topic	类型	说明
`/berxel_upper/.../color/image_raw`	Image	头部彩色图
`/berxel_upper/.../aligned_depth_to_color/image_raw`	Image	头部深度图
`/berxel_upper/raw/berxel_cloudpoint`	PointCloud2	头部点云
`/berxel_arm/.../color/image_raw`	Image	手部彩色图
`/berxel_arm/.../aligned_depth_to_color/image_raw`	Image	手部深度图
`/berxel_arm/raw/berxel_cloudpoint`	PointCloud2	手部点云
`/odom`	Odometry	里程计
`/scan`	LaserScan	LDS 滤波后数据
`/scan_raw`	LaserScan	LDS 原始数据
`/line_laser_mid`	PointCloud2	线激光原始数据
`/mid_filtered`	PointCloud2	过滤后线激光
`/joint_states`	JointState	关节反馈
`/power/battery/state`	BatteryState	电池信息
`/robot/alarm`	Alarm	MCU 异常上报
`/slam/map_info`	SlamMapInfo	实时 SLAM 建图过程
`/berxel_lower/obstacle`	PointCloud2	下视障碍物点云
`/berxel_lower/cliff_obstacle`	PointCloud2	悬崖点云
`/berxel_lower/unknown`	PointCloud2	未知点云
`/global_costmap/costmap_raw`	Costmap	全局代价地图
`/grasp_locate/cloth_cloud`	PointCloud2	衣物点云
`/grasp_locate/grasp_poses`	PoseArray	抓取位姿
`/semantic_locate/object/obb_cloud`	PointCloud2	语义建图 OBB
`/semantic_locate/object/input_cloud`	PointCloud2	语义建图输入点云
`/explore_locate/object/obb_cloud`	PointCloud2	单承载物 OBB
`/sensor/imu/chassis`	Imu	底盘 IMU
`/sensor/imu/jaw`	Imu	夹爪 IMU

7. 调试与排障

7.1 常用调试命令

# SSH 登录机器人
ssh Agi@<机器人IP>
确认 ARM 架构
uname -m                    # 必须输出 aarch64
确认 ROS2 环境
echo $ROS_DISTRO            # 必须输出 humble
ros2 --version
列出所有 ROS2 节点
ros2 node list
列出所有 Action
ros2 action list
查看 Action 接口定义
ros2 interface show common_msgs/action/AreaNav
手动调用 Action
ros2 action send_goal /nav2_area_navi_points common_msgs/action/AreaNav "{...}"
列出所有 Service
ros2 service list
手动调用 Service
ros2 service call /visual_model_interface/location common_msgs/srv/VlmLocation "{...}"
列出所有 Topic
ros2 topic list
查看 Topic 数据
ros2 topic echo /odom
ros2 topic echo /power/battery/state
查看节点图
ros2 run rqt_graph rqt_graph
查看 TF 树
ros2 run tf2_tools view_frames
查看机器人 CPU/内存
htop
free -h
查看 ROS2 日志
tail -f ~/.ros/log/latest/*.log

7.2 常见问题

问题	原因	解决方案
Action server not available	对应节点未启动	`ros2 node list` 检查节点是否运行
Goal rejected	参数错误或机器人状态不满足	检查 goal 参数，用 `ros2 interface show` 查看字段
VLM 返回空	API KEY 未配置或图像质量差	检查机器人 Web 页面 VLM 配置，调整光照/角度
导航失败 code=28677	未先执行重定位	先调用 `/slam/relocation/upper`
机械臂执行失败 code=36864	目标超出工作范围	调整机器人位置或目标位姿
TF 断开 code=36969	TF 树异常	`ros2 run tf2_tools view_frames` 检查
`colcon build` 报架构错误	在 x86 开发机上错误地对 ARM 包执行了编译	直接在机器人上 `colcon build`
Python 节点 `ros2 run` 找不到	`setup.py`/`setup.cfg` 入口点未更新	检查 `setup.cfg` 中 `console_scripts` 并 `colcon build`
`import common_msgs` 失败	common_msgs 未编译或未 source	确保在机器人上 `colcon build` 并 `source install/setup.bash`

7.3 ARM 特有注意事项

注意点	说明
Python 节点无需编译	`.py` 文件跨平台通用，推送即可运行
首次需 colcon build	注册 entry_points，日常改 `.py` 无需 rebuild
机器人资源有限	ARM 处理器性能低于 x86 PC，Python 节点足够轻量
setuptools 版本	部分机器人上 setuptools ≥80.x 不支持 `--symlink-install`，去掉该参数即可
sudo 权限	机器人上通常无 `sudo` 权限或受限，所有安装应在用户目录下完成
网络延迟	从开发机 ssh 操作机器人时确保网络稳定，大文件用 `rsync --partial`
ROS_DOMAIN_ID	确保开发节点与机器人现有节点使用相同的 `ROS_DOMAIN_ID`（默认 0）

7.4 错误码速查

错误码	含义	推荐处理
0	成功	—
10016	导航参数无效	检查 `object_area` 坐标合法性
10129	视觉模型超时	增加超时或检查服务负载
10132	未识别出有效物体	调整相机角度/光照/标签
10144	图片请求参数无效	检查 `camera_type` 参数
28674	路线搜索失败	检查地图完整性和目标可达性
28675	目标点不可达	调整目标坐标
28676	路径被阻挡	清除路径障碍物
28677	导航失败未重定位	先执行 `eco_relocate` / `/slam/relocation/upper`
36864	机械臂执行失败	检查目标在工作范围内
36866	轨迹规划失败	清除周围障碍物
36868	目标超出工作范围	重新调整观测位姿
36870	逆解失败	重新观测/调整目标位姿
36881	传入参数错误	检查 `poses_six_d` 等参数
36883	自适应失败	检查目标类型和参数匹配

7.5 开发检查清单

机器人架构确认：uname -m 输出 aarch64
common_msgs 包已在机器人上编译并 source
Python rclpy 可用（python3 -c "import rclpy"）
机器人已完成建图（用于导航相关功能）
VLM API KEY 已配置（用于视觉识别功能）
首次 colcon build 成功，日常改 .py 无需 rebuild
Action Server 可用（ros2 action list 确认）
参数类型与接口定义一致（ros2 interface show 确认）
异常处理完备（超时、拒绝、失败等）
日志级别合理（--ros-args --log-level DEBUG）
Launch 文件可完整启动所有依赖节点
长时间任务已处理 SIGINT/KeyboardInterrupt
部署脚本 deploy\.sh 已配置正确的机器人 IP
ROS_DOMAIN_ID 设置正确（与机器人现有节点保持一致）

DAMO开发者矩阵

DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐

PhysicalWords：一种用于构建机器人操作中潜推理过程的词汇体系

DAMO开发者矩阵

超级具身智能集群协作研究（51）：跨域语义互操作与知识融合演进机制详解

DAMO开发者矩阵

强化学习第十三课：为什么 Q-Table 到机器人这里会失效？

因为产生数据时可以使用：但学习目标却使用：代表更贪心的目标策略。所以：\[ \boxed{\text{行为策略和目标策略可以不同}} \]因为：它按照当前行为策略实际选择出的 \(a_{t+1}\) 来更新。所以采数据和被评价的是同一套策略。Q-Table 的最大问题：\[ \boxed{\text{状态空间太大/连续时无法枚举}} \]解决：用神经网络：代替 Q-Table。

DAMO开发者矩阵

所有评论(0)

查看更多评论

wangxiaoxuan217

@weixin_37540091

已为社区贡献11条内容

八界机器人 ROS2 Humble 高阶开发 SOP

wangxiaoxuan217

1. 架构概览

1.1 运行平台

1.2 开发与部署流程

1.3 系统分层

1.4 关键节点一览

1.5 设计原则

2. 环境准备

2.1 机器人端环境 ARM aarch64 / Ubuntu 22\.04

2.2 开发机端环境 x86\_64 PC

2.3 编译 common_msgs 接口包

2.4 创建自定义节点工作空间

2.5 代码部署与运行

2.6 机器人端 ROS2 环境变量

3. 通信接口体系

3.1 Action 接口（长耗时任务）

3.2 Service 接口（短耗时请求）

3.3 Topic 接口（数据流订阅）

4. 核心开发场景

4.1 场景 A：自定义导航节点（模板）

4.2 场景 B：视觉感知与抓取节点（模板）

4.3 场景 C：语义地图管理节点（模板）

4.4 场景 D：设备信息采集

4.5 场景 E：任务编排节点

4.6 场景 F：电量与状态监控

4.7 场景 G：夹爪开闭控制

4.8 场景 H：底盘传感器仪表盘

4.9 场景 I：里程计精度验证

4.10 场景 J：地图管理

4.11 场景 K：物体搜索

4.12 场景 L：头部/升降控制

4.13 场景 M：MCU 系统信息

5. 完整示例：玩具收纳自动化节点

5.1 项目结构

5.2 Python 完整实现

ROS2 接口

图像处理

5.3 Launch 文件

5.4 参数配置

6. 常用接口速查

6.1 Action 接口速查表（可释放）

6.2 Service 接口速查表（可释放，关键）

6.3 Topic 速查表（常用可释放）

7. 调试与排障

7.1 常用调试命令

7.2 常见问题

7.3 ARM 特有注意事项

7.4 错误码速查

7.5 开发检查清单

所有评论(0)

温馨提示：您尚未绑定手机号

wangxiaoxuan217