Removed spam, initialized y-button

mission/joystick_interface/joystick_interface/joystick_interface.py

@@ -79,6 +79,9 @@ def __init__(self):
         self.last_button_press_time_ = 0
         self.debounce_duration_ = 0.25
         self.state_ = States.NO_GO
+        self.previous_state_ = None
+
+        self.mode_logger_done_ = False
 
         self.joystick_buttons_map_ = []
 
@@ -160,17 +163,32 @@ def transition_to_xbox_mode(self):
         """
         Turns off the controller and signals that the operational mode has switched to Xbox mode.
         """
-        self.operational_mode_signal_publisher_.publish(String(data="XBOX"))
+        msg = String()
+        msg.data = "XBOX"
+        self.operational_mode_signal_publisher_.publish(msg)
         self.state_ = States.XBOX_MODE
+        self.previous_state_ = States.XBOX_MODE
+        self.mode_logger_done_ = False
 
     def transition_to_autonomous_mode(self):
         """
         Publishes a zero force wrench message and signals that the system is turning on autonomous mode.
         """
         wrench_msg = self.create_2d_wrench_message(0.0, 0.0, 0.0)
         self.wrench_publisher_.publish(wrench_msg)
-        self.operational_mode_signal_publisher_.publish(String(data="autonomous mode"))
+        msg = String()
+        msg.data = "autonomous mode"
+        self.operational_mode_signal_publisher_.publish(msg)
         self.state_ = States.AUTONOMOUS_MODE
+        self.previous_state_ = States.AUTONOMOUS_MODE
+        self.mode_logger_done_ = False
+
+    def set_home_position(self):
+        """
+        Calls a service that communicates to the guidance system to set the current
+        position as the home position.
+        """
+        pass
 
     def joystick_cb(self, msg : Joy) -> Wrench:
         """
@@ -217,6 +235,7 @@ def joystick_cb(self, msg : Joy) -> Wrench:
         xbox_control_mode_button = buttons.get("A", 0)
         software_killswitch_button = buttons.get("B", 0)
         software_control_mode_button = buttons.get("X", 0)
+        software_set_home_button = buttons.get("Y", 0)
         left_trigger = self.left_trigger_linear_converter(axes.get('LT', 0.0))
         right_trigger = self.right_trigger_linear_converter(axes.get('RT', 0.0))
 
@@ -240,7 +259,12 @@ def joystick_cb(self, msg : Joy) -> Wrench:
                 # signal that killswitch is not blocking
                 self.software_killswitch_signal_publisher_.publish(
                     Bool(data=False))
-                self.transition_to_xbox_mode()
+
+                if self.previous_state_ == States.XBOX_MODE:
+                    self.transition_to_xbox_mode()
+
+                else:
+                    self.transition_to_autonomous_mode()
                 return
 
             else:
@@ -257,18 +281,29 @@ def joystick_cb(self, msg : Joy) -> Wrench:
         wrench_msg = self.create_2d_wrench_message(surge, sway, yaw)
 
         if self.state_ == States.XBOX_MODE:
-            self.get_logger().info("XBOX mode", throttle_duration_sec=1)
+            if not self.mode_logger_done_:
+                self.get_logger().info("XBOX mode")
+                self.mode_logger_done_ = True
+
             self.wrench_publisher_.publish(wrench_msg)
 
             if software_control_mode_button:
                 self.transition_to_autonomous_mode()
 
-        if self.state_ == States.AUTONOMOUS_MODE:
-            self.get_logger().info("autonomous mode", throttle_duration_sec=1)
+        elif self.state_ == States.AUTONOMOUS_MODE:
+            if not self.mode_logger_done_:
+                self.get_logger().info("autonomous mode")
+                self.mode_logger_done_ = True
 
             if xbox_control_mode_button:
                 self.transition_to_xbox_mode()
 
+        else:
+            if not self.mode_logger_done_:
+                self.get_logger().info("Killswitch is active")
+                self.mode_logger_done_ = True
+            return
+
         return wrench_msg