added update qt timer
Signed-off-by: Jakub Delicat <delicat.kuba@gmail.com>
This commit is contained in:
Jakub Delicat
parent
2a755bf328
commit
fc4c829787
12
package.xml
12
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>
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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)
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user