From a9d73767d4fb9f841d00076a7d1d5a515887966e Mon Sep 17 00:00:00 2001 From: moscowsky Date: Thu, 25 Jun 2026 14:57:55 +0300 Subject: [PATCH] dump params --- .../fire_extinguisher_emulator.py | 2 +- unity_robot_controller/joy_control_node.py | 15 ++++++-- unity_robot_controller/robot_controller | 2 +- .../unity_robot_controller.py | 35 +++++++++++++++---- 4 files changed, 42 insertions(+), 12 deletions(-) diff --git a/unity_robot_controller/fire_extinguisher_emulator.py b/unity_robot_controller/fire_extinguisher_emulator.py index 012625b..bbf35da 100644 --- a/unity_robot_controller/fire_extinguisher_emulator.py +++ b/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 diff --git a/unity_robot_controller/joy_control_node.py b/unity_robot_controller/joy_control_node.py index 4f12603..3934697 100644 --- a/unity_robot_controller/joy_control_node.py +++ b/unity_robot_controller/joy_control_node.py @@ -144,10 +144,19 @@ def main(args=None): rclpy.init(args=args) node = JoyControlNode(UnityRobotController) - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() + 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() diff --git a/unity_robot_controller/robot_controller b/unity_robot_controller/robot_controller index 730c241..a8efe38 160000 --- a/unity_robot_controller/robot_controller +++ b/unity_robot_controller/robot_controller @@ -1 +1 @@ -Subproject commit 730c2417dd700d72506ba9dfdb153649e279e18b +Subproject commit a8efe38aaf3916aebd956d73831b95292a9afa23 diff --git a/unity_robot_controller/unity_robot_controller.py b/unity_robot_controller/unity_robot_controller.py index aa4cf15..bbc658a 100644 --- a/unity_robot_controller/unity_robot_controller.py +++ b/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())