ifly/traffic_line_task/scripts/traffic_line_state_machine.py

#!/usr/bin/env python3
"""Coordinate LED-sign recognition, open-loop junctions, and line following."""

import math
import threading

import rospy
from geometry_msgs.msg import PointStamped, Twist
from nav_msgs.msg import Odometry
from std_msgs.msg import Bool, Float32, String
from std_srvs.srv import SetBool, Trigger, TriggerResponse


class TrafficLineStateMachine:
    STATES = (
        "INIT",
        "WAIT_SIGN",
        "RESTORE_CAMERA",
        "FIRST_MANEUVER",
        "REACQUIRE_LINE",
        "FOLLOW_DIRECT_FINISH",
        "FOLLOW_TO_SECOND",
        "SECOND_JUNCTION_FORWARD",
        "SECOND_RIGHT_MANEUVER",
        "FOLLOW_AFTER_SECOND",
        "WAIT_LANE_RECOVERY",
        "CONFIRM_FINISH",
        "FINISHED",
        "FAULT",
        "ABORTED",
    )
    DIRECTIONS = ("LEFT", "RIGHT", "STRAIGHT")
    TERMINAL_STATES = ("FINISHED", "FAULT", "ABORTED")

    def __init__(self):
        rospy.init_node("traffic_line_task")
        self.lock = threading.RLock()

        self.autostart = self._as_bool(rospy.get_param("~autostart", True))
        self.control_rate = max(1.0, float(rospy.get_param("~control_rate", 20.0)))
        self.cmd_vel_topic = rospy.get_param("~cmd_vel_topic", "/cmd_vel")
        self.odom_topic = rospy.get_param("~odom_topic", "/odom")
        self.lane_valid_topic = rospy.get_param(
            "~lane_valid_topic", "/line_follow_debug/lane_valid"
        )
        self.lookahead_target_topic = rospy.get_param(
            "~lookahead_target_topic", "/line_follow_debug/lookahead_target"
        )
        self.sign_direction_topic = rospy.get_param(
            "~sign_direction_topic", "/traffic_sign/direction"
        )
        self.sign_confidence_topic = rospy.get_param(
            "~sign_confidence_topic", "/traffic_sign/confidence"
        )
        self.sign_enable_service_name = rospy.get_param(
            "~sign_enable_service", "/traffic_sign_recognition/set_enabled"
        )
        self.line_enable_service_name = rospy.get_param(
            "~line_enable_service", "/line_follow_control/set_enabled"
        )

        self.sign_confirmations = max(1, int(rospy.get_param("~sign_confirmations", 2)))
        self.sign_topic_timeout = max(
            0.5, float(rospy.get_param("~sign_topic_timeout", 3.0))
        )
        self.minimum_sign_confidence = max(
            0.0, float(rospy.get_param("~minimum_sign_confidence", 0.25))
        )
        self.camera_settle_time = max(
            0.0, float(rospy.get_param("~camera_settle_time", 2.0))
        )
        self.odom_timeout = max(0.1, float(rospy.get_param("~odom_timeout", 0.5)))
        self.odom_jump_limit_m = max(
            0.01, float(rospy.get_param("~odom_jump_limit_m", 0.25))
        )
        self.maximum_turn_yaw_rad = max(
            0.1, float(rospy.get_param("~maximum_turn_yaw_rad", 2.20))
        )
        self.wrong_way_yaw_tolerance_rad = max(
            0.01, float(rospy.get_param("~wrong_way_yaw_tolerance_rad", 0.10))
        )
        self.straight_yaw_limit_rad = max(
            0.05, float(rospy.get_param("~straight_yaw_limit_rad", 0.35))
        )
        self.maneuver_timeout_scale = max(
            1.0, float(rospy.get_param("~maneuver_timeout_scale", 2.0))
        )
        self.maneuver_timeout_min = max(
            1.0, float(rospy.get_param("~maneuver_timeout_min", 3.0))
        )
        self.transition_stop_time = max(
            0.0, float(rospy.get_param("~transition_stop_time", 0.30))
        )
        self.maneuver_linear_speed = min(
            0.10,
            max(0.01, float(rospy.get_param("~maneuver_linear_speed", 0.10))),
        )
        self.first_turn_angular_speed = min(
            0.20, max(0.01, float(rospy.get_param("~first_turn_angular_speed", 0.20)))
        )
        self.second_turn_angular_speed = min(
            0.20, max(0.01, float(rospy.get_param("~second_turn_angular_speed", 0.20)))
        )
        self.second_junction_forward_speed = min(
            0.10,
            max(
                0.01,
                float(rospy.get_param("~second_junction_forward_speed", 0.10)),
            ),
        )
        self.second_junction_forward_duration = max(
            0.0,
            float(rospy.get_param("~second_junction_forward_duration", 3.0)),
        )
        self.stop_after_second_maneuver = bool(
            rospy.get_param("~stop_after_second_maneuver", False)
        )
        self.use_finish_distance_limits = self._as_bool(
            rospy.get_param("~use_finish_distance_limits", True)
        )

        self.distances = {
            "first_left_turn": float(rospy.get_param("~first_left_turn_distance_m", 0.0)),
            "first_right_turn": float(rospy.get_param("~first_right_turn_distance_m", 0.0)),
            "first_straight_entry": float(
                rospy.get_param("~first_straight_entry_distance_m", 0.0)
            ),
            "direct_finish": float(rospy.get_param("~direct_finish_distance_m", 0.0)),
            "straight_to_second": float(
                rospy.get_param("~straight_to_second_distance_m", 0.0)
            ),
            "second_right_turn": float(
                rospy.get_param("~second_right_turn_distance_m", 0.0)
            ),
            "after_second_finish": float(
                rospy.get_param("~after_second_finish_distance_m", 0.0)
            ),
        }
        self.lane_reacquire_timeout = max(
            0.1, float(rospy.get_param("~lane_reacquire_timeout", 2.0))
        )
        self.second_junction_arm_distance = max(
            0.0,
            float(
                rospy.get_param(
                    "~second_junction_arm_distance_m",
                    0.75 * self.distances["straight_to_second"],
                )
            ),
        )
        self.second_junction_max_distance = max(
            0.0,
            float(
                rospy.get_param(
                    "~second_junction_max_distance_m",
                    1.40 * self.distances["straight_to_second"],
                )
            ),
        )
        self.second_junction_lost_frames = max(
            1, int(rospy.get_param("~second_junction_lost_frames", 3))
        )
        self.lane_stable_frames = max(
            1, int(rospy.get_param("~lane_stable_frames", 5))
        )
        self.target_fresh_timeout = max(
            0.05, float(rospy.get_param("~target_fresh_timeout", 0.5))
        )
        self.finish_lane_loss_confirm_time = max(
            0.1, float(rospy.get_param("~finish_lane_loss_confirm_time", 1.0))
        )
        self.completion_announcement_topic = rospy.get_param(
            "~completion_announcement_topic", "/speech_command/announce"
        )
        self.completion_announcement_text = str(
            rospy.get_param("~completion_announcement_text", "任务完成")
        ).strip()

        self.state = "INIT" if self.autostart else "ABORTED"
        self.state_enter_time = rospy.get_time()
        self.first_direction = "NONE"
        self.fault_reason = ""
        self.sign_candidate = "NONE"
        self.sign_candidate_count = 0
        self.last_sign_time = None
        self.sign_confidence = 0.0
        self.lane_valid = False
        self.last_target_time = None
        self.lane_stable_count = 0
        self.resume_state = None
        self.resume_segment_start_distance = None
        self.next_follow_state = None
        self.second_junction_loss_count = 0
        self.completion_announcement_sent = False
        self.odom_position = None
        self.odom_yaw = None
        self.last_odom_time = None
        self.total_odom_distance = 0.0
        self.odom_fault_reason = ""
        self.segment_start_distance = 0.0
        self.segment_start_yaw = None

        self.cmd_pub = rospy.Publisher(self.cmd_vel_topic, Twist, queue_size=1)
        self.state_pub = rospy.Publisher("~state", String, queue_size=1, latch=True)
        self.direction_pub = rospy.Publisher(
            "~first_direction", String, queue_size=1, latch=True
        )
        self.distance_pub = rospy.Publisher("~segment_distance", Float32, queue_size=1)
        self.finished_pub = rospy.Publisher("~finished", Bool, queue_size=1, latch=True)
        self.fault_pub = rospy.Publisher("~fault", String, queue_size=1, latch=True)
        self.completion_announcement_pub = rospy.Publisher(
            self.completion_announcement_topic, String, queue_size=1
        )

        self.sign_sub = rospy.Subscriber(
            self.sign_direction_topic, String, self.sign_callback, queue_size=1
        )
        self.confidence_sub = rospy.Subscriber(
            self.sign_confidence_topic, Float32, self.confidence_callback, queue_size=1
        )
        self.lane_sub = rospy.Subscriber(
            self.lane_valid_topic, Bool, self.lane_callback, queue_size=1
        )
        self.target_sub = rospy.Subscriber(
            self.lookahead_target_topic,
            PointStamped,
            self.target_callback,
            queue_size=1,
        )
        self.odom_sub = rospy.Subscriber(
            self.odom_topic, Odometry, self.odom_callback, queue_size=10
        )

        self.reset_service = rospy.Service("~reset", Trigger, self.reset_callback)
        self.abort_service = rospy.Service("~abort", Trigger, self.abort_callback)
        self.sign_enable = rospy.ServiceProxy(self.sign_enable_service_name, SetBool)
        self.line_enable = rospy.ServiceProxy(self.line_enable_service_name, SetBool)
        self.timer = rospy.Timer(
            rospy.Duration(1.0 / self.control_rate), self.control_callback
        )
        rospy.on_shutdown(self.shutdown)

        self.state_pub.publish(String(data=self.state))
        self.direction_pub.publish(String(data="NONE"))
        self.finished_pub.publish(Bool(data=False))
        self.fault_pub.publish(String(data=""))
        rospy.loginfo("Traffic-line task ready: state=%s autostart=%s", self.state, self.autostart)

    @staticmethod
    def _as_bool(value):
        if isinstance(value, str):
            return value.strip().lower() in ("1", "true", "yes", "on")
        return bool(value)

    @staticmethod
    def wrap_angle(angle):
        return math.atan2(math.sin(angle), math.cos(angle))

    @staticmethod
    def follow_state_after_first(direction):
        if direction == "STRAIGHT":
            return "FOLLOW_TO_SECOND"
        if direction in ("LEFT", "RIGHT"):
            return "FOLLOW_DIRECT_FINISH"
        raise ValueError("unsupported first direction: %s" % direction)

    @staticmethod
    def yaw_from_odometry(message):
        q = message.pose.pose.orientation
        siny_cosp = 2.0 * (q.w * q.z + q.x * q.y)
        cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
        return math.atan2(siny_cosp, cosy_cosp)

    def sign_callback(self, message):
        with self.lock:
            direction = str(message.data).strip().upper()
            self.last_sign_time = rospy.get_time()
            if self.state != "WAIT_SIGN":
                return
            if direction not in self.DIRECTIONS:
                self.sign_candidate = "NONE"
                self.sign_candidate_count = 0
                return
            if self.sign_confidence < self.minimum_sign_confidence:
                return
            if direction == self.sign_candidate:
                self.sign_candidate_count += 1
            else:
                self.sign_candidate = direction
                self.sign_candidate_count = 1

    def confidence_callback(self, message):
        with self.lock:
            self.sign_confidence = max(0.0, float(message.data))

    def lane_callback(self, message):
        with self.lock:
            self.lane_valid = bool(message.data)

    def target_callback(self, _message):
        with self.lock:
            self.last_target_time = rospy.get_time()

    def odom_callback(self, message):
        with self.lock:
            position = message.pose.pose.position
            current = (float(position.x), float(position.y))
            if self.odom_position is not None:
                step = math.hypot(
                    current[0] - self.odom_position[0],
                    current[1] - self.odom_position[1],
                )
                if step > self.odom_jump_limit_m:
                    self.odom_fault_reason = "ODOM_JUMP: %.3f m" % step
                else:
                    self.total_odom_distance += step
            self.odom_position = current
            self.odom_yaw = self.yaw_from_odometry(message)
            self.last_odom_time = rospy.get_time()

    def reset_callback(self, _request):
        with self.lock:
            self._disable_line()
            self._disable_sign()
            self._publish_stop()
            self._clear_task()
            self._transition("INIT" if self.autostart else "ABORTED")
        return TriggerResponse(success=True, message="traffic-line task reset")

    def abort_callback(self, _request):
        with self.lock:
            self._enter_terminal("ABORTED", "operator abort")
        return TriggerResponse(success=True, message="traffic-line task aborted")

    def _clear_task(self):
        self.first_direction = "NONE"
        self.fault_reason = ""
        self.sign_candidate = "NONE"
        self.sign_candidate_count = 0
        self.last_sign_time = None
        self.sign_confidence = 0.0
        self.lane_stable_count = 0
        self.resume_state = None
        self.next_follow_state = None
        self.second_junction_loss_count = 0
        self.completion_announcement_sent = False
        self.odom_fault_reason = ""
        self.direction_pub.publish(String(data="NONE"))
        self.finished_pub.publish(Bool(data=False))
        self.fault_pub.publish(String(data=""))

    def _transition(self, state, reset_segment=False):
        if state not in self.STATES:
            raise ValueError("unknown traffic-line state: %s" % state)
        previous = self.state
        self.state = state
        self.state_enter_time = rospy.get_time()
        self.lane_stable_count = 0
        if state == "FOLLOW_TO_SECOND":
            self.second_junction_loss_count = 0
        if reset_segment:
            self.segment_start_distance = self.total_odom_distance
            self.segment_start_yaw = self.odom_yaw
        self.state_pub.publish(String(data=state))
        rospy.loginfo("Traffic-line state: %s -> %s", previous, state)

    def _publish_stop(self):
        try:
            self.cmd_pub.publish(Twist())
        except rospy.ROSException:
            # A timer callback can overlap ROS shutdown after the publisher has
            # already been unregistered. The base timeout still provides the
            # final stop in that teardown-only condition.
            if not rospy.is_shutdown():
                raise

    def _publish_maneuver(self, angular):
        command = Twist()
        command.linear.x = self.maneuver_linear_speed
        command.angular.z = angular
        self.cmd_pub.publish(command)

    def _publish_forward(self, speed):
        command = Twist()
        command.linear.x = speed
        self.cmd_pub.publish(command)

    def _service_ready(self, name):
        try:
            rospy.wait_for_service(name, timeout=0.01)
            return True
        except rospy.ROSException:
            return False

    def _set_enabled(self, proxy, name, enabled):
        try:
            response = proxy(enabled)
        except (rospy.ServiceException, rospy.ROSException) as error:
            rospy.logerr("Service %s failed: %s", name, error)
            return False
        if not response.success:
            rospy.logerr("Service %s rejected request: %s", name, response.message)
            return False
        return True

    def _disable_line(self):
        if self._service_ready(self.line_enable_service_name):
            return self._set_enabled(
                self.line_enable, self.line_enable_service_name, False
            )
        return False

    def _enable_line(self):
        return self._set_enabled(self.line_enable, self.line_enable_service_name, True)

    def _disable_sign(self):
        if self._service_ready(self.sign_enable_service_name):
            return self._set_enabled(
                self.sign_enable, self.sign_enable_service_name, False
            )
        return False

    def _enable_sign(self):
        return self._set_enabled(self.sign_enable, self.sign_enable_service_name, True)

    def _configuration_error(self):
        invalid = [name for name, value in self.distances.items() if value <= 0.0]
        if invalid:
            return "CONFIG_INVALID: set positive calibration values for %s" % ", ".join(invalid)
        if self.second_junction_arm_distance <= 0.0:
            return "CONFIG_INVALID: second_junction_arm_distance_m must be positive"
        if self.second_junction_max_distance <= self.second_junction_arm_distance:
            return (
                "CONFIG_INVALID: second_junction_max_distance_m must be greater "
                "than second_junction_arm_distance_m"
            )
        return ""

    def _odom_fresh(self, now):
        return self.last_odom_time is not None and now - self.last_odom_time <= self.odom_timeout

    def _target_fresh(self, now):
        return self.last_target_time is not None and now - self.last_target_time <= self.target_fresh_timeout

    def _lane_ready(self, now):
        return self.lane_valid and self._target_fresh(now)

    def _segment_distance(self):
        return max(0.0, self.total_odom_distance - self.segment_start_distance)

    def _segment_yaw(self):
        if self.segment_start_yaw is None or self.odom_yaw is None:
            return 0.0
        return self.wrap_angle(self.odom_yaw - self.segment_start_yaw)

    def _maneuver_timeout(self, target_distance):
        nominal = target_distance / self.maneuver_linear_speed
        return self.transition_stop_time + max(
            self.maneuver_timeout_min, nominal * self.maneuver_timeout_scale
        )

    def _enter_fault(self, reason):
        self._enter_terminal("FAULT", reason)

    def _enter_terminal(self, state, reason=""):
        self._disable_line()
        self._disable_sign()
        self._publish_stop()
        self.fault_reason = reason if state == "FAULT" else ""
        self.finished_pub.publish(Bool(data=(state == "FINISHED")))
        self.fault_pub.publish(String(data=self.fault_reason))
        self._transition(state)
        if state == "FINISHED" and not self.completion_announcement_sent:
            self.completion_announcement_pub.publish(
                String(data=self.completion_announcement_text)
            )
            self.completion_announcement_sent = True
            rospy.loginfo(
                "Task completion announcement requested: %s",
                self.completion_announcement_text,
            )
        if reason:
            rospy.logerr("Traffic-line %s: %s", state, reason)

    def _begin_first_maneuver(self):
        if not self._disable_line():
            self._enter_fault("LINE_DISABLE_SERVICE_FAILED")
            return
        self._publish_stop()
        self._transition("FIRST_MANEUVER", reset_segment=True)

    def _begin_reacquire(self, next_state):
        if not self._disable_line():
            self._enter_fault("LINE_DISABLE_SERVICE_FAILED")
            return
        self._publish_stop()
        self.next_follow_state = next_state
        self._transition("REACQUIRE_LINE")

    def _enable_follow_state(self, state, reset_segment):
        self._publish_stop()
        if not self._enable_line():
            self._enter_fault("LINE_ENABLE_SERVICE_FAILED")
            return
        self._transition(state, reset_segment=reset_segment)

    def _begin_lane_recovery(self):
        self.resume_state = self.state
        if not self._disable_line():
            self._enter_fault("LINE_DISABLE_SERVICE_FAILED")
            return
        self._publish_stop()
        self._transition("WAIT_LANE_RECOVERY")

    def _run_reacquire(self, now, recovery):
        self._publish_stop()
        if self._lane_ready(now):
            self.lane_stable_count += 1
        else:
            self.lane_stable_count = 0
        if self.lane_stable_count >= self.lane_stable_frames:
            state = self.resume_state if recovery else self.next_follow_state
            self._enable_follow_state(state, reset_segment=not recovery)
            return
        if now - self.state_enter_time > self.lane_reacquire_timeout:
            self._enter_fault("LANE_REACQUIRE_TIMEOUT")

    def _first_maneuver_parameters(self):
        if self.first_direction == "LEFT":
            return self.distances["first_left_turn"], self.first_turn_angular_speed
        if self.first_direction == "RIGHT":
            return self.distances["first_right_turn"], -self.first_turn_angular_speed
        if self.first_direction == "STRAIGHT":
            return self.distances["first_straight_entry"], 0.0
        return 0.0, 0.0

    def _run_maneuver(self, now, target_distance, angular, completed_callback):
        distance = self._segment_distance()
        yaw = self._segment_yaw()
        elapsed = now - self.state_enter_time
        if elapsed < self.transition_stop_time:
            self._publish_stop()
            return
        if angular != 0.0:
            expected_sign = 1.0 if angular > 0.0 else -1.0
            if yaw * expected_sign < -self.wrong_way_yaw_tolerance_rad:
                self._enter_fault("MANEUVER_WRONG_YAW_DIRECTION: %.3f rad" % yaw)
                return
            if abs(yaw) > self.maximum_turn_yaw_rad:
                self._enter_fault("MANEUVER_YAW_LIMIT: %.3f rad" % yaw)
                return
        elif abs(yaw) > self.straight_yaw_limit_rad:
            self._enter_fault("STRAIGHT_YAW_LIMIT: %.3f rad" % yaw)
            return
        if distance >= target_distance:
            self._publish_stop()
            completed_callback()
            return
        if elapsed > self._maneuver_timeout(target_distance):
            self._enter_fault("MANEUVER_TIMEOUT: %.3f/%.3f m" % (distance, target_distance))
            return
        self._publish_maneuver(angular)

    def _run_follow(self, target_distance, completion):
        now = rospy.get_time()
        if not self._lane_ready(now):
            self._begin_lane_recovery()
            return
        if self._segment_distance() >= target_distance:
            if not self._disable_line():
                self._enter_fault("LINE_DISABLE_SERVICE_FAILED")
                return
            self._publish_stop()
            completion()

    def _run_finish_follow(self, now, target_distance):
        if not self.use_finish_distance_limits:
            # Endpoint detection must come from the perception result itself.
            # A stale target alone may mean CPU/image delay and must not be
            # mistaken for the physical end of the painted lane.
            if not self.lane_valid:
                self.resume_state = self.state
                if not self._disable_line():
                    self._enter_fault("LINE_DISABLE_SERVICE_FAILED")
                    return
                self._publish_stop()
                self._transition("CONFIRM_FINISH")
            return
        self._run_follow(
            target_distance,
            lambda: self._enter_terminal("FINISHED"),
        )

    def _run_finish_confirmation(self, now):
        """Stop immediately, then distinguish endpoint loss from a short dropout."""
        self._publish_stop()
        if self.lane_valid:
            if self._lane_ready(now):
                self.lane_stable_count += 1
                if self.lane_stable_count >= self.lane_stable_frames:
                    resume_state = self.resume_state
                    self._enable_follow_state(resume_state, reset_segment=False)
            else:
                self.lane_stable_count = 0
            return

        self.lane_stable_count = 0
        if now - self.state_enter_time >= self.finish_lane_loss_confirm_time:
            rospy.loginfo(
                "Endpoint confirmed after %.2f s without a valid lane.",
                self.finish_lane_loss_confirm_time,
            )
            self._enter_terminal("FINISHED")

    def _run_follow_to_second(self, now):
        """Trigger the second right turn from confirmed lane loss near the junction."""
        distance = self._segment_distance()
        if self._lane_ready(now):
            self.second_junction_loss_count = 0
            if distance > self.second_junction_max_distance:
                if not self._disable_line():
                    self._enter_fault("LINE_DISABLE_SERVICE_FAILED")
                    return
                self._publish_stop()
                self._enter_fault(
                    "SECOND_JUNCTION_NOT_DETECTED: %.3f m" % distance
                )
            return

        if distance < self.second_junction_arm_distance:
            # A loss well before the expected junction is ordinary perception
            # failure, not permission to turn into an arbitrary opening.
            self.second_junction_loss_count = 0
            self._begin_lane_recovery()
            return

        self.second_junction_loss_count += 1
        self._publish_stop()
        if self.second_junction_loss_count < self.second_junction_lost_frames:
            return
        if not self._disable_line():
            self._enter_fault("LINE_DISABLE_SERVICE_FAILED")
            return
        rospy.loginfo(
            "Second junction confirmed by %d lost-lane frames at %.3f m.",
            self.second_junction_loss_count,
            distance,
        )
        self._transition("SECOND_JUNCTION_FORWARD")

    def _run_second_junction_forward(self, now):
        elapsed = max(0.0, now - self.state_enter_time)
        if elapsed >= self.second_junction_forward_duration:
            self._publish_stop()
            rospy.loginfo(
                "Second-junction forward approach complete: %.2f s at %.2f m/s.",
                self.second_junction_forward_duration,
                self.second_junction_forward_speed,
            )
            self._transition("SECOND_RIGHT_MANEUVER", reset_segment=True)
            return
        self._publish_forward(self.second_junction_forward_speed)

    def _complete_second_maneuver(self):
        if self.stop_after_second_maneuver:
            rospy.loginfo(
                "Second-junction test stop reached; line following remains disabled."
            )
            self._enter_terminal("FINISHED")
            return
        self._begin_reacquire("FOLLOW_AFTER_SECOND")

    def control_callback(self, _event):
        with self.lock:
            now = rospy.get_time()
            self.distance_pub.publish(Float32(data=self._segment_distance()))

            if self.state in self.TERMINAL_STATES:
                self._publish_stop()
                return

            # Validate the complete route before requiring odometry or enabling
            # either perception/control node. This guarantees an uncalibrated
            # task fails as CONFIG_INVALID without ever permitting motion.
            if self.state == "INIT":
                config_error = self._configuration_error()
                if config_error:
                    self._enter_fault(config_error)
                    return

            if self.odom_fault_reason:
                self._enter_fault(self.odom_fault_reason)
                return
            if not self._odom_fresh(now):
                if now - self.state_enter_time > self.odom_timeout:
                    self._enter_fault("ODOM_TIMEOUT")
                else:
                    self._publish_stop()
                return

            if self.state == "INIT":
                self._publish_stop()
                if not self._service_ready(self.sign_enable_service_name):
                    if now - self.state_enter_time > self.sign_topic_timeout:
                        self._enter_fault("SIGN_ENABLE_SERVICE_UNAVAILABLE")
                    return
                if not self._service_ready(self.line_enable_service_name):
                    if now - self.state_enter_time > self.sign_topic_timeout:
                        self._enter_fault("LINE_ENABLE_SERVICE_UNAVAILABLE")
                    return
                if not self._disable_line():
                    self._enter_fault("LINE_DISABLE_SERVICE_FAILED")
                    return
                if not self._enable_sign():
                    self._enter_fault("SIGN_ENABLE_SERVICE_FAILED")
                    return
                self.last_sign_time = now
                self._transition("WAIT_SIGN")
                return

            if self.state == "WAIT_SIGN":
                self._publish_stop()
                if self.last_sign_time is None or now - self.last_sign_time > self.sign_topic_timeout:
                    self._enter_fault("SIGN_TOPIC_TIMEOUT")
                    return
                if self.sign_candidate_count >= self.sign_confirmations:
                    self.first_direction = self.sign_candidate
                    self.direction_pub.publish(String(data=self.first_direction))
                    if not self._disable_sign():
                        self._enter_fault("SIGN_DISABLE_SERVICE_FAILED")
                        return
                    self._transition("RESTORE_CAMERA")
                return

            if self.state == "RESTORE_CAMERA":
                self._publish_stop()
                if now - self.state_enter_time >= self.camera_settle_time:
                    self._begin_first_maneuver()
                return

            if self.state == "FIRST_MANEUVER":
                target, angular = self._first_maneuver_parameters()
                next_state = self.follow_state_after_first(self.first_direction)
                self._run_maneuver(
                    now,
                    target,
                    angular,
                    lambda: self._begin_reacquire(next_state),
                )
                return

            if self.state == "REACQUIRE_LINE":
                self._run_reacquire(now, recovery=False)
                return

            if self.state == "WAIT_LANE_RECOVERY":
                self._run_reacquire(now, recovery=True)
                return

            if self.state == "CONFIRM_FINISH":
                self._run_finish_confirmation(now)
                return

            if self.state == "FOLLOW_DIRECT_FINISH":
                self._run_finish_follow(now, self.distances["direct_finish"])
                return

            if self.state == "FOLLOW_TO_SECOND":
                self._run_follow_to_second(now)
                return

            if self.state == "SECOND_JUNCTION_FORWARD":
                self._run_second_junction_forward(now)
                return

            if self.state == "SECOND_RIGHT_MANEUVER":
                self._run_maneuver(
                    now,
                    self.distances["second_right_turn"],
                    -self.second_turn_angular_speed,
                    self._complete_second_maneuver,
                )
                return

            if self.state == "FOLLOW_AFTER_SECOND":
                self._run_finish_follow(now, self.distances["after_second_finish"])
                return

            self._enter_fault("UNHANDLED_STATE: %s" % self.state)

    def shutdown(self):
        with self.lock:
            self._disable_line()
            self._disable_sign()
            self._publish_stop()


if __name__ == "__main__":
    try:
        TrafficLineStateMachine()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass