dump params

unity_robot_controller/fire_extinguisher_emulator.py

@@ -112,7 +112,7 @@ class FireExtinguisherEmulatorNode(Node):
             print_f = self.get_logger().info
             )
 
-        self.get_logger().info(f"{intersected_spheres}")
+        #self.get_logger().info(f"{intersected_spheres}")
 
         if len(intersected_spheres):
             res.success = True

unity_robot_controller/joy_control_node.py

@@ -144,11 +144,20 @@ def main(args=None):
     rclpy.init(args=args)
 
     node = JoyControlNode(UnityRobotController)
+
+    try:
+        # Keep the node running until an interrupt occurs
         rclpy.spin(node)
+    except KeyboardInterrupt:
+        # Catch Ctrl+C and log a clean exit message
+        node.get_logger().info('KeyboardInterrupt received, shutting down gracefully...')
+        path = node.URC.dump_stats()
+        node.get_logger().info(f"Saved data to {path}")
+    finally:
+        # Clean up resources safely
         node.destroy_node()
         rclpy.shutdown()
 
-
 if __name__ == "__main__":
     main()
 

unity_robot_controller/unity_robot_controller.py

@@ -26,9 +26,16 @@ class UnityRobotController(RobotController):
 
         self._node.declare_parameter('max_tilt_deg', 90.)
 
+        self._node.declare_parameter('discharge_speed_base', 0.0)
+        self._node.declare_parameter('discharge_speed_lights', 0.5)
+
         self._lin_max = self._node.get_parameter('max_linear_speed').value
         self._ang_max = self._node.get_parameter('max_angular_speed').value
 
+        self._discharge_speed_base = self._node.get_parameter('discharge_speed_base').value
+        self._discharge_speed_lights = self._node.get_parameter('discharge_speed_lights').value
+
+
         self._max_tilt = np.deg2rad(self._node.get_parameter('max_tilt_deg').value)
         self._FALL = False
 
@@ -51,13 +58,13 @@ class UnityRobotController(RobotController):
         self.lights_srv = self._node.create_client(Trigger, '/unity/light_trigger')
         self.fall_reset_srv = self._node.create_client(Trigger, '/unity/robot_reset_trigger')
 
-        self.start_time = self.get_time_seconds() # TODO save it when first cmd is given
+        #self.start_time = self.get_time_seconds() # TODO save it when first cmd is given
+        self.t_prev = self.get_time_seconds()
 
-        # TODO create collision subscriber from Unity
 
         self._prev_collision = False
         self._node.create_subscription(Bool, 'collision_detection', self.collision_cb, 10)
-        self.fall_timer = self._node.create_timer(0.1, self.fall_timer_cb)
+        self.proc_timer = self._node.create_timer(0.1, self.proc_timer_cb)
 
     def pub_status(self):
         msg = String()
@@ -70,7 +77,7 @@ class UnityRobotController(RobotController):
 
     def collision_cb(self, msg):
         if msg.data and not self._prev_collision:
-            self._register_collision(self.get_relative_time())
+            self._register_collision()#self.get_relative_time())
             self.pub_status()
         self._prev_collision = msg.data
 
@@ -105,7 +112,7 @@ class UnityRobotController(RobotController):
     def _fe_burst_done_cb(self, future):
         result = future.result()
         if result.success:
-            self._register_exted_sof(self.get_relative_time(), {'id': result.message})
+            self._register_exted_sof({'id': result.message})
             self.pub_status()
 
     def send_fire_ext_pose_cmd(self, horisontal_pose, vertical_pose):
@@ -120,7 +127,19 @@ class UnityRobotController(RobotController):
     def send_unfall_cmd(self):
         self._FALL = False
 
-    def fall_timer_cb(self):
+    def proc_timer_cb(self):
+
+        # LIGHT + CHARGE PROCESS
+        t = self.get_time_seconds()
+        discharge_speed = self._discharge_speed_base
+        if self._LIGHTS:
+            discharge_speed += self._discharge_speed_lights
+        self._decrease_charge(discharge_speed, t - self.t_prev)
+        self.t_prev = t
+        if self._charge_points == 0:
+            self._node.get_logger().info('Robot is dischadged!', throttle_duration_sec=2.5)
+
+        # FALL PROCESS
         try:
             robot_transform = self._tf_buffer.lookup_transform(
                 "map",           # target frame
@@ -154,11 +173,12 @@ class UnityRobotController(RobotController):
         fall = cos_angle >= cos_thresh
         if not self._FALL and fall:
             self._FALL = fall
-            self._register_fall(self.get_relative_time())
+            self._register_fall()#self.get_relative_time())
             self.pub_status()
         self._FALL = fall
 
     def send_lights_cmd(self):
+        super(UnityRobotController, self).send_lights_cmd()
         future = self.lights_srv.call_async(Trigger.Request())
         future.add_done_callback(self._lights_done_cb)
 
@@ -169,6 +189,7 @@ class UnityRobotController(RobotController):
         self._node.get_logger().info(f"Lights is {'ON' if self._LIGHTS else 'OFF'}")
 
     def send_fall_reset_cmd(self):
+        super(UnityRobotController, self).send_fall_reset_cmd()
         if self._FALL:
             self._node.get_logger().info("Trying to reset robot...")
             future = self.fall_reset_srv.call_async(Trigger.Request())