diff --git a/package.xml b/package.xml index b5a1003..6724bfc 100644 --- a/package.xml +++ b/package.xml @@ -13,24 +13,24 @@ catkin - rospy + rclpy rqt_gui rqt_gui_py std_msgs - rosaria_msgs - - rospy + ros2aria_msgs + + rclpy rqt_gui rqt_gui_py std_msgs - rosaria_msgs + ros2aria_msgs - rospy + rclpy rqt_gui rqt_gui_py std_msgs message_runtime - rosaria_msgs + ros2aria_msgs ament_python diff --git a/safety_user_plugin/ros_wrapper.py b/safety_user_plugin/ros_wrapper.py index fa018dd..e8a35e1 100644 --- a/safety_user_plugin/ros_wrapper.py +++ b/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): - for key in self.rostopics_hz_dict: - self.robots_hz_value[key] = self.rostopics_hz_dict[key].get_hz( - "/PIONIER{0}/RosAria/user_stop".format(key) - ) + def update_hz_values(self): + print("update_hz_values") + # for key in self.rostopics_hz_dict: + # 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 = [] - - # published_topics_list = rospy.get_published_topics(namespace="/") - published_topics = [] - - # for list_ in published_topics_list: - # published_topics.append(list_[0]) - - for topic in published_topics: - if topic.find("RosAria") == -1 or topic.find("robot_info") == -1: + # Można to zrobić lepiej np. czekać na publikowaną wiadomość na robot_info + for pioneer_id in range(6): + published_topics_list = [] + try: + published_topics_list = self.get_publisher_names_and_types_by_node( + "ros2aria", "/pioneer" + str(pioneer_id) + ) + except NodeNameNonExistentError: 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) diff --git a/safety_user_plugin/user_plugin.py b/safety_user_plugin/user_plugin.py index 3e4f027..4a581c0 100755 --- a/safety_user_plugin/user_plugin.py +++ b/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)