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