1. 使用行为树实现机器人前进与后退

任务目标

构建一个ROS 2行为树节点，使机器人能够根据外部指令（前进/后退）执行对应的速度命令，并在动作完成后正确终止。本案例使用py_trees库实现，不依赖具体机器人硬件，通过发布cmd_vel话题模拟运动指令。

环境依赖

• ROS 2（Humble或更新版本）
• Python 3.8+
• py_trees：pip install py_trees
• rclpy（ROS 2 Python客户端库）

实现方案

行为树结构如下：根节点为Sequence，依次执行三个子节点：指令解析节点（从ROS 2参数或话题读取指令）、动作执行节点（发布速度命令）、终止检查节点（确认动作完成）。每个节点均为自定义py_trees.behaviour.Behaviour子类，通过tick()驱动状态流转。

完整代码

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
import py_trees
from py_trees.common import Status

class CommandParser(py_trees.behaviour.Behaviour):
    """从ROS参数读取指令: forward/backward"""
    def __init__(self, node, name="ParseCommand"):
        super().__init__(name)
        self.node = node
        self.command = None

    def setup(self):
        self.node.declare_parameter('command', 'forward')
        return True

    def update(self):
        self.command = self.node.get_parameter('command').value
        if self.command in ['forward', 'backward']:
            return Status.SUCCESS
        return Status.FAILURE

class MoveRobot(py_trees.behaviour.Behaviour):
    """发布cmd_vel速度命令"""
    def __init__(self, node, name="MoveRobot"):
        super().__init__(name)
        self.node = node
        self.publisher = None
        self.speed = 0.0

    def setup(self):
        self.publisher = self.node.create_publisher(Twist, 'cmd_vel', 10)
        return True

    def update(self):
        cmd = self.node.get_parameter('command').value
        twist = Twist()
        if cmd == 'forward':
            twist.linear.x = 0.5
        else:
            twist.linear.x = -0.5
        self.publisher.publish(twist)
        self.node.get_logger().info(f"Publishing: {twist.linear.x}")
        return Status.SUCCESS

class CheckMotion(py_trees.behaviour.Behaviour):
    """检查动作是否完成（此处模拟1秒后完成）"""
    def __init__(self, node, name="CheckMotion"):
        super().__init__(name)
        self.node = node
        self.start_time = None

    def initialise(self):
        self.start_time = self.node.get_clock().now()

    def update(self):
        elapsed = (self.node.get_clock().now() - self.start_time).nanoseconds / 1e9
        if elapsed >= 1.0:
            return Status.SUCCESS
        return Status.RUNNING

class RobotBehaviorTree(Node):
    def __init__(self):
        super().__init__('behavior_tree_node')
        self.root = self.create_root()

    def create_root(self):
        parser = CommandParser(self)
        mover = MoveRobot(self)
        checker = CheckMotion(self)
        sequence = py_trees.composites.Sequence("MainSequence", children=[parser, mover, checker])
        return py_trees.trees.BehaviourTree(sequence)

    def run(self):
        self.root.tick_tock(period_ms=100)
        try:
            rclpy.spin(self)
        except KeyboardInterrupt:
            pass
        finally:
            self.root.shutdown()

def main(args=None):
    rclpy.init(args=args)
    tree_node = RobotBehaviorTree()
    tree_node.run()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

运行与验证

1. 启动行为树节点（默认执行前进）：
   ros2 run your_package behavior_tree_node.py
2. 观察终端日志，应看到Publishing: 0.5，持续约1秒后节点进入空闲状态。
3. 另开终端验证速度话题：
   ros2 topic echo /cmd_vel，应看到linear.x: 0.5。
4. 测试后退指令：
   ros2 run your_package behavior_tree_node.py --ros-args -p command:=backward
   此时日志显示Publishing: -0.5，话题数据为linear.x: -0.5。

通过条件：行为树能根据参数正确发布对应方向的速度命令，且CheckMotion节点在1秒后返回SUCCESS，整棵树正常终止，无报错。

关键说明

• 本案例用CheckMotion模拟动作完成检测，实际项目中可替换为里程计回调或到达目标点判断。
• 行为树通过tick_tock周期性驱动，RUNNING状态使节点持续执行直到条件满足。
• 若需接入真实机器人，只需保留MoveRobot节点，将CheckMotion替换为实际传感器反馈即可。