有限状态机(Finite State Machine)是机器人任务编排的核心工具。它将复杂的机器人行为分解为离散的状态，通过定义清晰的状态转换规则来管理行为流程。
#!/usr/bin/env python3
# patrol_state_machine.py - 巡逻任务状态机
# ✅ Docker验证通过
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist, PoseStamped
from std_msgs.msg import String, Bool
from sensor_msgs.msg import BatteryState
import math, enum
class RobotState(enum.Enum):
IDLE = 0
NAVIGATING = 1
PATROLLING = 2
CHARGING = 3
EMERGENCY_STOP = 4
ERROR = 5
class PatrolStateMachine(Node):
def __init__(self):
super().__init__('patrol_state_machine')
# 参数
self.declare_parameter('waypoints', '[]') # JSON列表
self.declare_parameter('battery_low_threshold', 20.0)
self.declare_parameter('battery_critical_threshold', 10.0)
self.battery_low = self.get_parameter('battery_low_threshold').value
self.battery_critical = self.get_parameter('battery_critical_threshold').value
# 状态
self.state = RobotState.IDLE
self.prev_state = RobotState.IDLE
self.battery_level = 100.0
self.current_waypoint_idx = 0
self.waypoints = [(1.0,0.0),(2.0,1.0),(1.0,2.0),(0.0,1.0)]
self.reached_goal = False
# 订阅
self.battery_sub = self.create_subscription(
BatteryState, '/battery_status', self._battery_cb, 10)
self.nav_result_sub = self.create_subscription(
Bool, '/navigation_result', self._nav_result_cb, 10)
self.cmd_sub = self.create_subscription(
String, '/smach_command', self._cmd_cb, 10)
# 发布
self.state_pub = self.create_publisher(String, '/robot_state', 10)
self.goal_pub = self.create_publisher(PoseStamped, '/goal_pose', 10)
self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
# 状态机定时器
self.timer = self.create_timer(0.5, self._state_machine_update)
self.get_logger().info('🔄 巡逻状态机已启动')
def _battery_cb(self, msg):
self.battery_level = msg.percentage
if self.battery_level < self.battery_critical and self.state != RobotState.EMERGENCY_STOP:
self._transition(RobotState.EMERGENCY_STOP)
def _nav_result_cb(self, msg):
self.reached_goal = msg.data
def _cmd_cb(self, msg):
if msg.data == 'start_patrol':
self.current_waypoint_idx = 0
self._transition(RobotState.PATROLLING)
elif msg.data == 'stop':
self._transition(RobotState.IDLE)
elif msg.data == 'emergency_stop':
self._transition(RobotState.EMERGENCY_STOP)
elif msg.data == 'resume':
self._transition(RobotState.PATROLLING)
def _transition(self, new_state):
if new_state == self.state: return
self.get_logger().info(
f'状态转换: {self.state.name} → {new_state.name}')
self.prev_state = self.state
self.state = new_state
# 发布状态
state_msg = String(); state_msg.data = self.state.name
self.state_pub.publish(state_msg)
def _state_machine_update(self):
if self.state == RobotState.IDLE:
pass # 等待命令
elif self.state == RobotState.PATROLLING:
# 检查电量
if self.battery_level < self.battery_low:
self._transition(RobotState.CHARGING)
return
# 发送下一个航点
if self.reached_goal or self.current_waypoint_idx == 0:
if self.current_waypoint_idx < len(self.waypoints):
wp = self.waypoints[self.current_waypoint_idx]
goal = PoseStamped()
goal.header.frame_id = 'map'
goal.header.stamp = self.get_clock().now().to_msg()
goal.pose.position.x = wp[0]
goal.pose.position.y = wp[1]
goal.pose.orientation.w = 1.0
self.goal_pub.publish(goal)
self.current_waypoint_idx += 1
self.reached_goal = False
else:
self.current_waypoint_idx = 0 # 循环
self.get_logger().info('巡逻一圈完成，继续下一圈')
elif self.state == RobotState.CHARGING:
if self.battery_level > 90.0:
self._transition(RobotState.PATROLLING)
elif self.state == RobotState.EMERGENCY_STOP:
# 紧急停车
self.cmd_vel_pub.publish(Twist())
self.get_logger().error('🛑 紧急停止状态！等待恢复命令')
def main(args=None):
rclpy.init(args=args); rclpy.spin(PatrolStateMachine()); rclpy.shutdown()#!/usr/bin/env python3
# concurrent_state_machine.py - 并发状态执行
import rclpy, threading
from rclpy.node import Node
from std_msgs.msg import String
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan, BatteryState
import math
class ConcurrentStateMachine(Node):
def __init__(self):
super().__init__('concurrent_sm')
self.running = True
# 多个状态检查线程
self.battery_level = 100.0
self.min_obstacle_dist = float('inf')
self.state = 'NORMAL'
self.battery_sub = self.create_subscription(
BatteryState, '/battery_status', lambda m: setattr(self,'battery_level',m.percentage), 10)
self.scan_sub = self.create_subscription(
LaserScan, '/scan', self._scan_cb, 10)
self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10)
self.state_pub = self.create_publisher(String, '/robot_state', 10)
self.timer = self.create_timer(0.2, self._update)
self.get_logger().info('🔄 并发状态机已启动')
def _scan_cb(self, msg):
valid = [r for r in msg.ranges if msg.range_min < r < msg.range_max]
self.min_obstacle_dist = min(valid) if valid else float('inf')
def _update(self):
# 并发评估多个条件
prev = self.state
if self.battery_level < 10:
self.state = 'EMERGENCY'
elif self.min_obstacle_dist < 0.3:
self.state = 'OBSTACLE_AVOID'
elif self.battery_level < 20:
self.state = 'LOW_BATTERY'
else:
self.state = 'NORMAL'
if self.state != prev:
self.get_logger().info(f'状态: {prev}→{self.state}')
msg = String(); msg.data = self.state
self.state_pub.publish(msg)
# 状态对应行为
cmd = Twist()
if self.state == 'EMERGENCY':
cmd.linear.x = 0 # 停车
elif self.state == 'OBSTACLE_AVOID':
cmd.angular.z = 0.5 # 旋转避障
elif self.state == 'LOW_BATTERY':
cmd.linear.x = 0.1 # 慢速
else:
cmd.linear.x = 0.3 # 正常速度
self.cmd_pub.publish(cmd)
def main(args=None):
rclpy.init(args=args); rclpy.spin(ConcurrentStateMachine()); rclpy.shutdown()在PatrolStateMachine中添加ERROR状态，当导航连续失败3次时进入。
在PATROLLING状态内部嵌套NAVIGATING→ARRIVED→NEXT_WAYPOINT子状态。
使用smach_viewer生成状态机图。
经验值：+250 XP