📚 课程目录第2章状态机控制机器人左右转弯

2. 状态机控制机器人左右转弯

任务目标

设计一个基于ROS的状态机节点，接收字符串指令（left或right），切换至对应转弯状态，并向/cmd_vel发布角速度指令，驱动机器人完成左转或右转。状态机包含IDLE、TURN_LEFT、TURN_RIGHT三个状态，支持随时切换与停止。

实现方案

使用ROS smach库构建状态机。每个状态通过订阅/turn_cmd话题接收指令，根据指令内容执行状态转移。转弯动作通过发布到/cmd_vel的geometry_msgs/Twist消息实现，角速度方向决定左右转。

完整代码

#!/usr/bin/env python3
import rospy
import smach
import smach_ros
from geometry_msgs.msg import Twist
from std_msgs.msg import String

class IDLE(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['turn_left', 'turn_right', 'idle'])
        self.cmd = 'idle'
        self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        self.sub = rospy.Subscriber('/turn_cmd', String, self.cb)

    def cb(self, msg):
        self.cmd = msg.data

    def execute(self, ud):
        rospy.loginfo('State: IDLE')
        # 停止运动
        self.pub.publish(Twist())
        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            if self.cmd == 'left':
                return 'turn_left'
            elif self.cmd == 'right':
                return 'turn_right'
            rate.sleep()
        return 'idle'

class TurnState(smach.State):
    def __init__(self, direction):
        smach.State.__init__(self, outcomes=['done', 'switch'])
        self.direction = direction  # 'left' 或 'right'
        self.cmd = 'idle'
        self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        self.sub = rospy.Subscriber('/turn_cmd', String, self.cb)

    def cb(self, msg):
        self.cmd = msg.data

    def execute(self, ud):
        rospy.loginfo('State: TURN_%s', self.direction.upper())
        twist = Twist()
        # 角速度：左转为正，右转为负
        twist.angular.z = 0.5 if self.direction == 'left' else -0.5
        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            self.pub.publish(twist)
            if self.cmd == 'stop':
                return 'done'
            elif self.cmd != self.direction:
                return 'switch'
            rate.sleep()
        return 'done'

def main():
    rospy.init_node('turn_state_machine')
    sm = smach.StateMachine(outcomes=['stopped'])
    with sm:
        smach.StateMachine.add('IDLE', IDLE(),
                               transitions={'turn_left': 'TURN_LEFT',
                                            'turn_right': 'TURN_RIGHT',
                                            'idle': 'IDLE'})
        smach.StateMachine.add('TURN_LEFT', TurnState('left'),
                               transitions={'done': 'IDLE',
                                            'switch': 'TURN_RIGHT'})
        smach.StateMachine.add('TURN_RIGHT', TurnState('right'),
                               transitions={'done': 'IDLE',
                                            'switch': 'TURN_LEFT'})
    sis = smach_ros.IntrospectionServer('smach_server', sm, '/SM_ROOT')
    sis.start()
    outcome = sm.execute()
    rospy.spin()
    sis.stop()

if __name__ == '__main__':
    main()

运行与验证

依赖安装：sudo apt install ros-noetic-smach ros-noetic-smach-ros（ROS Noetic；其他发行版替换对应版本号）
启动节点：chmod +x turn_state_machine.py && rosrun your_pkg turn_state_machine.py

发布转弯指令（另开终端）：

rostopic pub -1 /turn_cmd std_msgs/String "data: 'left'"
rostopic pub -1 /turn_cmd std_msgs/String "data: 'right'"
rostopic pub -1 /turn_cmd std_msgs/String "data: 'stop'"

观察输出：rostopic echo /cmd_vel，应看到angular.z分别为0.5（左转）、-0.5（右转）、0.0（停止）。

通过条件：终端日志依次显示State: IDLE → State: TURN_LEFT → State: TURN_RIGHT，且/cmd_vel的角速度值与指令方向一致；发布stop后回到IDLE且角速度为0。

状态转移说明

状态机在IDLE等待指令；收到left进入TURN_LEFT，收到right进入TURN_RIGHT。转弯状态中若收到stop则回到IDLE，收到相反方向指令则直接切换至另一转弯状态。

常见问题

无法导入smach：确认已安装smach与smach_ros，且使用Python 3运行（ROS Noetic默认）。
话题不匹配：确保发布/turn_cmd时消息类型为std_msgs/String，且data字段为left、right或stop。
机器人不转：检查/cmd_vel是否被其他节点占用，或机器人驱动是否订阅该话题。