L1.5/safety_user_plugin
L1.5/safety_user_plugin
6
0
Fork 0
Code Issues 1 Pull Requests Projects Releases Wiki Activity

Added blocking taken robots

Browse Source 
Signed-off-by: Jakub Delicat <jakub.delicat@pwr.edu.pl>
This commit is contained in:
Jakub Delicat 2023-09-19 20:54:35 +02:00
parent 3a11c49e95
commit 42dee5c2c5
2 changed files with 6 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
safety_user_plugin/ros_wrapper.py
View File

@ -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

9
safety_user_plugin/user_plugin.py
View File

@ -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")