#!/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