From 468b7a8407272246a146471924861e7bda02196a Mon Sep 17 00:00:00 2001
From: ayin21 <wedmann.lea@gmail.com>
Date: Fri, 19 Jul 2024 07:30:25 +0000
Subject: [PATCH 1/3] publish diagnostics message when not connected

---
 .../game_controller_hl/receiver.py            | 45 ++++++++++++++-----
 1 file changed, 33 insertions(+), 12 deletions(-)

diff --git a/game_controller_hl/game_controller_hl/receiver.py b/game_controller_hl/game_controller_hl/receiver.py
index b5ce20e..cf734ba 100755
--- a/game_controller_hl/game_controller_hl/receiver.py
+++ b/game_controller_hl/game_controller_hl/receiver.py
@@ -22,6 +22,7 @@
 from rclpy.duration import Duration
 from std_msgs.msg import Header
 from construct import Container
+from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
 from game_controller_hl.gamestate import GameStateStruct, ResponseStruct
 from game_controller_hl.utils import get_parameters_from_other_node
 
@@ -65,9 +66,12 @@ def __init__(self, *args, **kwargs):
         # The publisher for the game state
         self.state_publisher = self.create_publisher(GameState, 'gamestate', 1)
 
+        #The publisher for the diagnostics
+        self.diagnostic_pub = self.create_publisher(DiagnosticArray, "diagnostics", 1)
+
         # The time in seconds after which we assume the game controller is lost 
         # and we tell the robot to move
-        self.game_controller_lost_time = 20
+        self.game_controller_lost_time = 5
 
         # The address listening on and the port for sending back the robots meta data
         self.addr = (
@@ -98,7 +102,10 @@ def receive_forever(self):
             # Check if we didn't receive a package for a long time and if so
             # call the fallback behavior
             if self.get_time_since_last_package() > Duration(seconds=self.game_controller_lost_time):
-                self.fallback_behavior()
+                self.publish_diagnostics(False)
+            else:
+                self.publish_diagnostics(True)
+
 
     def receive_and_answer_once(self):
         """ Receives a package, interprets it and sends an answer. """
@@ -128,18 +135,32 @@ def receive_and_answer_once(self):
         except IOError as e:
                 self.get_logger().warn(f"Error while sending keep-alive: {str(e)}")
 
-    def fallback_behavior(self):
+    def publish_diagnostics(self, reiceved_message_lately: bool):
         """ 
-        This is called if we didn't receive a package for a long time.
-        It tells the robot to play anyway.
+        This publishes a Diagnostics Array.
         """
-        self.get_logger().info("No GameController message received, allowing robot to move anyway", throttle_duration_sec=5)
-        self.state_publisher.publish(
-            GameState(
-                header=Header(stamp=self.get_clock().now().to_msg()),
-                game_state = GameState.GAMESTATE_PLAYING
-            )
-        )
+        self.get_logger().info("No GameController message received", throttle_duration_sec=5)
+
+        #initialize DiagnsticArray message
+        diag_array = DiagnosticArray()
+
+        #configure DiagnosticStatus message
+        diag = DiagnosticStatus(name = "Game Controller", hardware_id = "Game Controller" )
+        if not reiceved_message_lately:
+            diag.message = "Lost connection to game controller for " + str(int(self.get_time_since_last_package().nanoseconds/1e9)) + " sec"
+            diag.level = DiagnosticStatus.WARN
+        else:
+            diag.message = "Connected"
+            diag.level = DiagnosticStatus.OK 
+
+        self.get_logger().warn(str(diag)) 
+
+        diag_array.status.append(diag)    
+    
+        #add timestamp to header and publish DiagnosticArray
+        diag_array.header.stamp = self.get_clock().now().to_msg()
+        self.diagnostic_pub.publish(diag_array)
+
 
     def answer_to_gamecontroller(self, peer):
         """ Sends a life sign to the game controller """

From 108659d20812d45f67e3b7a52ba5e781e227065e Mon Sep 17 00:00:00 2001
From: ayin21 <wedmann.lea@gmail.com>
Date: Fri, 19 Jul 2024 08:46:30 +0000
Subject: [PATCH 2/3] fix typo

---
 game_controller_hl/game_controller_hl/receiver.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/game_controller_hl/game_controller_hl/receiver.py b/game_controller_hl/game_controller_hl/receiver.py
index cf734ba..c2fbc57 100755
--- a/game_controller_hl/game_controller_hl/receiver.py
+++ b/game_controller_hl/game_controller_hl/receiver.py
@@ -135,7 +135,7 @@ def receive_and_answer_once(self):
         except IOError as e:
                 self.get_logger().warn(f"Error while sending keep-alive: {str(e)}")
 
-    def publish_diagnostics(self, reiceved_message_lately: bool):
+    def publish_diagnostics(self, received_message_lately: bool):
         """ 
         This publishes a Diagnostics Array.
         """
@@ -146,7 +146,7 @@ def publish_diagnostics(self, reiceved_message_lately: bool):
 
         #configure DiagnosticStatus message
         diag = DiagnosticStatus(name = "Game Controller", hardware_id = "Game Controller" )
-        if not reiceved_message_lately:
+        if not received_message_lately:
             diag.message = "Lost connection to game controller for " + str(int(self.get_time_since_last_package().nanoseconds/1e9)) + " sec"
             diag.level = DiagnosticStatus.WARN
         else:

From 0654b682f5eb9a0f80128d8b63ff0912ae811c95 Mon Sep 17 00:00:00 2001
From: ayin21 <wedmann.lea@gmail.com>
Date: Fri, 19 Jul 2024 08:58:26 +0000
Subject: [PATCH 3/3] add dependencies

---
 game_controller_hl/package.xml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/game_controller_hl/package.xml b/game_controller_hl/package.xml
index 4d3583c..b0dd0d4 100644
--- a/game_controller_hl/package.xml
+++ b/game_controller_hl/package.xml
@@ -19,6 +19,7 @@
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
   <exec_depend>rosidl_default_runtime</exec_depend>
 
+  <depend>diagnostic_msgs</depend>
   <depend>game_controller_hl_interfaces</depend>
   <depend>python3-construct</depend>
   <depend>rclpy</depend>