subscriber fix

Author: sWintermoor

src/bitbots_motion/bitbots_rl_motion/handlers/ball_handler.py

@@ -9,9 +9,8 @@ def __init__(self, config):
     def ball_pos_callback(self, msg):
         self.ball_pos = msg
 
+    def has_data(self):
+        return (self.ball_pos != None)
 
     def get_ball_pos(self):
-        try:
-            return self.ball_pos
-        except:
-            return None
+        return self.ball_pos

src/bitbots_motion/bitbots_rl_motion/handlers/command_handler.py

@@ -9,11 +9,11 @@ def __init__(self, config):
         self._cmd_vel = None
 
     def get_command(self):
-        try:
-            command = np.array([self._cmd_vel.linear.x, self._cmd_vel.linear.y, self._cmd_vel.angular.z], dtype=np.float32)
-            return command
-        except: 
-            return None
+        command = np.array([self._cmd_vel.linear.x, self._cmd_vel.linear.y, self._cmd_vel.angular.z], dtype=np.float32)
+        return command
+
+    def has_data(self):
+        return (self._cmd_vel != None)
 
     def cmd_vel_callback(self, msg):
         self._cmd_vel = msg

src/bitbots_motion/bitbots_rl_motion/handlers/gravity_handler.py

@@ -9,27 +9,26 @@ def __init__(self, config):
         super().__init__(config)
 
         self._imu_data = None
-        self._gravity = None
 
     # Callables
     def imu_callback(self, msg):
         self._imu_data = msg
 
+    def has_data(self):
+        return (self._imu_data != None)
 
     def get_gravity(self):
-        try:
-            gravity = (
-                quat2mat(
-                    [
-                        self._imu_data.orientation.w,
-                        self._imu_data.orientation.x,
-                        self._imu_data.orientation.y,
-                        self._imu_data.orientation.z,
-                    ]
-                )
-                @ euler2mat(0, -0.0, 0)
-            ).T @ np.array([0, 0, -1], dtype=np.float32)
-            return gravity
-        except:
-            return None
+        gravity = (
+            quat2mat(
+                [
+                    self._imu_data.orientation.w,
+                    self._imu_data.orientation.x,
+                    self._imu_data.orientation.y,
+                    self._imu_data.orientation.z,
+                ]
+            )
+            @ euler2mat(0, -0.0, 0)
+        ).T @ np.array([0, 0, -1], dtype=np.float32)
+        return gravity
+
 

src/bitbots_motion/bitbots_rl_motion/handlers/gyro_handler.py

@@ -10,7 +10,9 @@ def __init__(self, config):
     # Callables
     def imu_callback(self, msg):
         self._imu_data = msg
-        
+
+    def has_data(self):
+        return (self._imu_data != None)
 
     def get_gyro(self):
         try:

src/bitbots_motion/bitbots_rl_motion/handlers/handler.py

@@ -1,3 +1,6 @@
 class Handler:
     def __init__(self, config):
         self._config = config
+
+    def has_data(self):
+        pass

src/bitbots_motion/bitbots_rl_motion/handlers/joint_handler.py

@@ -17,6 +17,9 @@ def __init__(self, config):
     def joint_state_callback(self, msg):
         self._joint_state = msg
 
+    def has_data(self):
+        return (self._joint_state != None)
+
     def get_angle_data(self):
         try:
             joint_angles = (

src/bitbots_motion/bitbots_rl_motion/nodes/rl_node.py

@@ -28,6 +28,8 @@
 from rclpy.qos import QoSProfile
 from rclpy.subscription import Subscription
 
+from handlers.handler import Handler
+
 
 class RLNode(Node):
     """Node to control the wolfgang humanoid."""
@@ -56,6 +58,14 @@ def _load_config(self, path: str):
 
     # TODO: fix
     def _timer_callback(self):
+        if not self._config:
+            raise ConfigError("Configuration is missing!")
+
+        # Prüfen ob alle Subscriber schon mindestens eine Nachricht hatten
+        if not self._all_sensors_ready():
+            self.get_logger().warning("Waiting for all sensors to be available", throttle_duration_sec=1.0)
+            return
+
         if self._config:
             for subscription in self._subs:
                 if subscription is None:
@@ -89,6 +99,13 @@ def _timer_callback(self):
             self._phase.set_phase(np.fmod(phase_tp1 + np.pi, 2 * np.pi) - np.pi)
         else:
             raise ConfigError("Configuration is missing! Try to run self.config() in init.")
+        
+    def _all_sensors_ready(self):
+        for handler in self._handlers:
+            if not handler.has_data():
+                return False
+            
+        return True
 
     def load_model(self, model):
         path_to_model = os.path.join(get_package_share_directory("bitbots_rl_motion"), "models", model)
@@ -107,20 +124,19 @@ def load_model(self, model):
         for out in self._onnx_model.graph.output:
             self._onnx_output_name.append(out)
 
-        self._timer = self.create_timer(self._config["phase"]["control_dt"], self._timer_callback)
-
         self._subs = []
+        self._handlers = []
 
-        for (key, value) in self.__dict__.values():
-            if type(value) is Subscription:
-                self._subs.append(key)
+        for key, value in self.__dict__.items():
+            if isinstance(value, Subscription):
+                self._subs.append(value)
+            if isinstance(value, Handler):
+                self._handlers.append(value)
+        
+        self._timer = self.create_timer(self._config["phase"]["control_dt"], self._timer_callback)
 
         self.load_phase()
 
-    def obs(self):
-        # Should be defined in subclass
-        pass
-
     def publisher(self):
         # Should be defined in subclass
         pass