From 42dee5c2c5e54e97c9832425a53c2d2fa370b55f Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 19 Sep 2023 20:54:35 +0200 Subject: [PATCH] Added blocking taken robots Signed-off-by: Jakub Delicat --- safety_user_plugin/ros_wrapper.py | 6 ++---- safety_user_plugin/user_plugin.py | 9 ++++----- 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/safety_user_plugin/ros_wrapper.py b/safety_user_plugin/ros_wrapper.py index dda18d6..3b1c343 100644 --- a/safety_user_plugin/ros_wrapper.py +++ b/safety_user_plugin/ros_wrapper.py @@ -78,7 +78,6 @@ class ROSWrapper(Node): ) def _update_user_stop_period(self, msg, robot_id): - print(f"Got user stop message for {robot_id}: {msg.data}") self.time_of_last_msg_of_user_stop[robot_id] = self.get_clock().now() def setup_subscribers_and_publishers(self, robot_id): @@ -152,9 +151,8 @@ class ROSWrapper(Node): continue self.robots_hz_value[i] = 1.0/float((self.get_clock().now() - last_time).nanoseconds) * 10e9 - if self.robots_hz_value[i] < 20.0: + if self.robots_hz_value[i] < 10.0: self.robots_hz_value[i] = None - print(f"hz values: {self.robots_hz_value}") def handle_robot_connection_lost(self): self.shutdown_timer(self.connection_timer) @@ -190,7 +188,7 @@ class ROSWrapper(Node): print("Unregister node.") def select_robot(self, robot_id): - self.setup_subscribers_and_publishers(robot_id) + self.setup_subscribers_and_publishers(robot_id+1) def setup_get_user_stop_state_function(self, function): self.get_user_stop_state = function diff --git a/safety_user_plugin/user_plugin.py b/safety_user_plugin/user_plugin.py index 9fede0b..d6835b1 100755 --- a/safety_user_plugin/user_plugin.py +++ b/safety_user_plugin/user_plugin.py @@ -137,11 +137,10 @@ class UserPlugin(Plugin): # Qt callback functions def handle_robot_selection(self, robot_id): if self._user_info.selected_robot is None: - # TODO: hz subskrypcje służą do sprawdzaNIA czy robot jest już zajęty - # if self._ros_wrapper.robots_hz_value[robot_id] == None: - self.select_robot(robot_id) - # else: - # self._qt_wrapper.robot_selected_by_another_group_notify() + if self._ros_wrapper.robots_hz_value[robot_id-1] is None: + self.select_robot(robot_id-1) + else: + self._qt_wrapper.robot_selected_by_another_group_notify() else: raise RuntimeError("User already selected robot")