added update qt timer

Signed-off-by: Jakub Delicat <delicat.kuba@gmail.com>

package.xml

@@ -13,24 +13,24 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
-  <build_depend>rospy</build_depend>
+  <build_depend>rclpy</build_depend>
   <build_depend>rqt_gui</build_depend>
   <build_depend>rqt_gui_py</build_depend>
   <build_depend>std_msgs</build_depend>
-  <build_depend>rosaria_msgs</build_depend>
+  <build_depend>ros2aria_msgs</build_depend>
 
-  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>rclpy</build_export_depend>
   <build_export_depend>rqt_gui</build_export_depend>
   <build_export_depend>rqt_gui_py</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
-  <build_export_depend>rosaria_msgs</build_export_depend>
+  <build_export_depend>ros2aria_msgs</build_export_depend>
 
-  <exec_depend>rospy</exec_depend>
+  <exec_depend>rclpy</exec_depend>
   <exec_depend>rqt_gui</exec_depend>
   <exec_depend>rqt_gui_py</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>message_runtime</exec_depend>
-  <exec_depend>rosaria_msgs</exec_depend>
+  <exec_depend>ros2aria_msgs</exec_depend>
 
   <export>
     <build_type>ament_python</build_type>

safety_user_plugin/ros_wrapper.py

@@ -1,5 +1,6 @@
 #!/usr/bin/env python
 import rclpy
+from rclpy.node import Node, NodeNameNonExistentError
 
 from std_msgs.msg import Bool
 from ros2aria_msgs.msg import RobotInfoMsg
@@ -12,8 +13,11 @@ from safety_user_plugin.enums.clutch_state import ClutchState as CS
 from safety_user_plugin.enums.stop_state import StopState as SS
 
 
-class ROSWrapper:
+class ROSWrapper(Node):
     def __init__(self):
+        # TODO: mogą być problemy z tymi samymi nazwami node'ów, gry różne kompy będą miały odpalony
+        # safety user system
+        super().__init__("safety_user_plugin")
         self.robots_list_update_callback = None
         self.selected_robot_info_update_callback = None
         self.master_stop_update_callback = None
@@ -33,7 +37,7 @@ class ROSWrapper:
         self.robots_list_timer = None
         self.connection_timer = None
         self.master_connection_timer = None
-        # self.hz_update_timer = rospy.Timer(rospy.Duration(0.2), self.update_hz_values)
+        self.hz_update_timer = self.create_timer(0.2, self.update_hz_values)
 
         self.robots_hz_subscribers_dict = {}
         self.rostopics_hz_dict = {}
@@ -118,11 +122,12 @@ class ROSWrapper:
         # else:
         #     raise ValueError('clutch_state UNKNOWN when attempting to publish periodic update')
 
-    def update_hz_values(self, event):
+    def update_hz_values(self):
-        for key in self.rostopics_hz_dict:
+        print("update_hz_values")
-            self.robots_hz_value[key] = self.rostopics_hz_dict[key].get_hz(
+        # for key in self.rostopics_hz_dict:
-                "/PIONIER{0}/RosAria/user_stop".format(key)
+        #     self.robots_hz_value[key] = self.rostopics_hz_dict[key].get_hz(
-            )
+        #         "/PIONIER{0}/RosAria/user_stop".format(key)
+        #     )
 
     def handle_robot_connection_lost(self, event):
         self.shutdown_timer(self.connection_timer)
@@ -183,26 +188,29 @@ class ROSWrapper:
         self.get_clutch_state = function
 
     # ROSWrapper Callbacks
-    def get_robots_list(self, event):
+    def get_robots_list_and_refresh_subscribers(self):
+        robots_id_list = self._get_robots_list()
+        self._create_hz_subscribers(robots_id_list)
+        self._remove_hz_subscribers(robots_id_list)
+        self.robots_list_update_callback(robots_id_list)
+
+    def _get_robots_list(self):
         robots_id_list = []
-
+        # Można to zrobić lepiej np. czekać na publikowaną wiadomość na robot_info
-        # published_topics_list = rospy.get_published_topics(namespace="/")
+        for pioneer_id in range(6):
-        published_topics = []
+            published_topics_list = []
-
+            try:
-        # for list_ in published_topics_list:
+                published_topics_list = self.get_publisher_names_and_types_by_node(
-        #     published_topics.append(list_[0])
+                    "ros2aria", "/pioneer" + str(pioneer_id)
-
+                )
-        for topic in published_topics:
+            except NodeNameNonExistentError:
-            if topic.find("RosAria") == -1 or topic.find("robot_info") == -1:
                 pass
-            else:
-                robot_number = topic.split("/")
-                robot_number = robot_number[1]
-                robot_number = robot_number[7:]
-                if len(robot_number) > 0:
-                    robot_number = int(robot_number)
-                    robots_id_list.append(robot_number)
 
+            if len(published_topics_list):
+                robots_id_list.append(pioneer_id)
+        return robots_id_list
+
+    def _create_hz_subscribers(self, robots_id_list):
         # Adding new hz subscribers
         for robot_id in robots_id_list:
             if robot_id not in self.robots_hz_subscribers_dict:
@@ -216,6 +224,7 @@ class ROSWrapper:
                 #     callback_args="/PIONIER{0}/RosAria/user_stop".format(robot_id),
                 # )
 
+    def _remove_hz_subscribers(self, robots_id_list):
         # Removing old hz subscribers
         for robot_key in self.robots_hz_subscribers_dict:
             if robot_key not in robots_id_list:
@@ -224,8 +233,6 @@ class ROSWrapper:
                 self.robots_hz_subscribers_dict[robot_id] = None
                 self.rostopics_hz_dict[robot_id] = None
 
-        self.robots_list_update_callback(robots_id_list)
-
     def release_robot(self):
         self.unsubscribe(self.robot_info_subscriber)
         self.shutdown_timer(self.periodic_timer)
@@ -259,7 +266,9 @@ class ROSWrapper:
     # UserPlugin Callbacks
     def set_robots_list_update_callback(self, callback_function):
         self.robots_list_update_callback = callback_function
-        # self.robots_list_timer = rospy.Timer(rospy.Duration(0.5), self.get_robots_list)
+        self.robots_list_timer = self.create_timer(
+            0.5, self.get_robots_list_and_refresh_subscribers
+        )
 
     def set_selected_robot_info_update_callback(self, callback_function):
         self.selected_robot_info_update_callback = callback_function
@@ -295,3 +304,6 @@ class ROSWrapper:
         msg = Bool()
         msg.data = False
         self.user_stop_publisher.publish(msg)
+
+    def qt_timer_callback(self):
+        rclpy.spin_once(self, timeout_sec=0.001)

safety_user_plugin/user_plugin.py

@@ -5,6 +5,7 @@ import os
 from qt_gui.plugin import Plugin
 from python_qt_binding.QtCore import QObject
 from python_qt_binding.QtCore import pyqtSignal
+from python_qt_binding.QtCore import QTimer
 
 from safety_user_plugin.qt_wrapper import QtWrapper
 from safety_user_plugin.ros_wrapper import ROSWrapper
@@ -66,6 +67,13 @@ class UserPlugin(Plugin):
             self._qt_wrapper.handle_emitted_signal_with_list_argument
         )
 
+        # timer to consecutively send twist messages
+        self._update_parameter_timer = QTimer(self)
+        self._update_parameter_timer.timeout.connect(
+            self._ros_wrapper.qt_timer_callback
+        )
+        self._update_parameter_timer.start(100)
+
     def call_qt_wrapper_method(self, method_name):
         self.cbhandler.signal.emit(method_name)