diff --git a/game_controller_hl/game_controller_hl/receiver.py b/game_controller_hl/game_controller_hl/receiver.py
index b5ce20e..c2fbc57 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, received_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 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:
+ 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 """
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 @@
rosidl_default_generators
rosidl_default_runtime
+ diagnostic_msgs
game_controller_hl_interfaces
python3-construct
rclpy