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

opened window | removed ros1 wrappers

Browse Source 
Signed-off-by: Jakub Delicat <delicat.kuba@gmail.com>
This commit is contained in:
Jakub Delicat 2023-08-16 12:38:35 +02:00
parent ce405ab4f1
commit 2a755bf328
4 changed files with 74 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
plugin.xml
View File

@ -1,11 +1,11 @@
<library path="scripts">
<class name="safety_user_plugin" type="user_plugin.UserPlugin" base_class_type="rqt_gui_py::Plugin">
<class name="safety_user_plugin" type="safety_user_plugin.user_plugin.UserPlugin" base_class_type="rqt_gui_py::Plugin">
<description>
Safety system user plugin
</description>
<qtgui>
<group>
<label>Safety system</label>
<label>L1.5 Safety system</label>
<icon type="theme">folder</icon>
</group>
<label>User</label>

5
safety_user_plugin/qt_wrapper.py
View File

@ -6,6 +6,9 @@ import os
import math
import datetime
from ament_index_python.packages import get_package_share_directory
from safety_user_plugin.enums.clutch_state import ClutchState as CS
from safety_user_plugin.enums.stop_state import StopState as SS
@ -39,7 +42,7 @@ class QtWrapper:
self.clear_robot_info()
ui_directory_path = "{0}/ui".format(
rospkg.RosPack().get_path("safety_user_plugin")
get_package_share_directory("safety_user_plugin")
)
self.distance_pixmap = QPixmap("{0}/Distance.png".format(ui_directory_path))

111
safety_user_plugin/ros_wrapper.py
View File

@ -33,7 +33,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 = rospy.Timer(rospy.Duration(0.2), self.update_hz_values)
self.robots_hz_subscribers_dict = {}
self.rostopics_hz_dict = {}
@ -45,46 +45,46 @@ class ROSWrapper:
def setup_subscribers_and_publishers(self, robot_id):
robot_name = "PIONIER" + str(robot_id)
self.robot_info_subscriber = rospy.Subscriber(
"/{0}/RosAria/robot_info".format(robot_name),
RobotInfoMsg,
self.handle_selected_robot_info_update,
)
self.master_stop_subscriber = rospy.Subscriber(
"/PIONIER/master_stop", Bool, self.handle_master_stop_update
)
self.restrictions_subscriber = rospy.Subscriber(
"/PIONIER/restrictions", RestrictionsMsg, self.handle_restrictions_update
)
# self.robot_info_subscriber = rospy.Subscriber(
# "/{0}/RosAria/robot_info".format(robot_name),
# RobotInfoMsg,
# self.handle_selected_robot_info_update,
# )
# self.master_stop_subscriber = rospy.Subscriber(
# "/PIONIER/master_stop", Bool, self.handle_master_stop_update
# )
# self.restrictions_subscriber = rospy.Subscriber(
# "/PIONIER/restrictions", RestrictionsMsg, self.handle_restrictions_update
# )
self.user_stop_publisher = rospy.Publisher(
"/{0}/RosAria/user_stop".format(robot_name), Bool, queue_size=1
)
self.clutch_publisher = rospy.Publisher(
"/{0}/RosAria/clutch".format(robot_name), Bool, queue_size=1
)
# self.user_stop_publisher = rospy.Publisher(
# "/{0}/RosAria/user_stop".format(robot_name), Bool, queue_size=1
# )
# self.clutch_publisher = rospy.Publisher(
# "/{0}/RosAria/clutch".format(robot_name), Bool, queue_size=1
# )
if self.periodic_timer != None:
self.shutdown_timer(self.periodic_timer)
self.periodic_timer = rospy.Timer(
rospy.Duration(0.1), self.publish_periodic_update
)
print("NEW")
if self.connection_timer != None:
self.shutdown_timer(self.connection_timer)
self.connection_timer = rospy.Timer(
rospy.Duration(5), self.handle_robot_connection_lost
)
# if self.periodic_timer != None:
# self.shutdown_timer(self.periodic_timer)
# self.periodic_timer = rospy.Timer(
# rospy.Duration(0.1), self.publish_periodic_update
# )
# print("NEW")
# if self.connection_timer != None:
# self.shutdown_timer(self.connection_timer)
# self.connection_timer = rospy.Timer(
# rospy.Duration(5), self.handle_robot_connection_lost
# )
if self.master_connection_timer != None:
self.shutdown_timer(self.master_connection_timer)
self.master_connection_timer = rospy.Timer(
rospy.Duration(1.5), self.handle_master_connection_lost
)
# if self.master_connection_timer != None:
# self.shutdown_timer(self.master_connection_timer)
# self.master_connection_timer = rospy.Timer(
# rospy.Duration(1.5), self.handle_master_connection_lost
# )
if self.hz_update_timer != None:
self.shutdown_timer(self.hz_update_timer)
self.hz_update_timer = rospy.Timer(rospy.Duration(0.2), self.update_hz_values)
# if self.hz_update_timer != None:
# self.shutdown_timer(self.hz_update_timer)
# self.hz_update_timer = rospy.Timer(rospy.Duration(0.2), self.update_hz_values)
def unsubscribe(self, subscriber):
if subscriber != None:
@ -150,9 +150,9 @@ class ROSWrapper:
def restart_connection_timer(self):
if self.connection_timer != None:
self.connection_timer.shutdown()
self.connection_timer = rospy.Timer(
rospy.Duration(1.5), self.handle_robot_connection_lost
)
# self.connection_timer = rospy.Timer(
# rospy.Duration(1.5), self.handle_robot_connection_lost
# )
else:
raise RuntimeError(
"Attempting to restart connection_timer when it is not initialized"
@ -161,9 +161,9 @@ class ROSWrapper:
def restart_master_connection_timer(self):
if self.master_connection_timer != None:
self.master_connection_timer.shutdown()
self.master_connection_timer = rospy.Timer(
rospy.Duration(1.5), self.handle_master_connection_lost
)
# self.master_connection_timer = rospy.Timer(
# rospy.Duration(1.5), self.handle_master_connection_lost
# )
else:
raise RuntimeError(
"Attempting to restart master_connection_timer when it is not initialized"
@ -171,7 +171,7 @@ class ROSWrapper:
def unregister_node(self):
self.cancel_subscribers_and_publishers()
rospy.signal_shutdown("Closing safety user plugin")
# rospy.signal_shutdown("Closing safety user plugin")
def select_robot(self, robot_id):
self.setup_subscribers_and_publishers(robot_id)
@ -186,11 +186,11 @@ class ROSWrapper:
def get_robots_list(self, event):
robots_id_list = []
published_topics_list = rospy.get_published_topics(namespace="/")
# published_topics_list = rospy.get_published_topics(namespace="/")
published_topics = []
for list_ in published_topics_list:
published_topics.append(list_[0])
# 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:
@ -207,13 +207,14 @@ class ROSWrapper:
for robot_id in robots_id_list:
if robot_id not in self.robots_hz_subscribers_dict:
# Add hz subscriber
self.rostopics_hz_dict[robot_id] = rostopic.ROSTopicHz(-1)
self.robots_hz_subscribers_dict[robot_id] = rospy.Subscriber(
"/PIONIER{0}/RosAria/user_stop".format(robot_id),
Bool,
self.rostopics_hz_dict[robot_id].callback_hz,
callback_args="/PIONIER{0}/RosAria/user_stop".format(robot_id),
)
print("ADDED SUB")
# self.rostopics_hz_dict[robot_id] = rostopic.ROSTopicHz(-1)
# self.robots_hz_subscribers_dict[robot_id] = rospy.Subscriber(
# "/PIONIER{0}/RosAria/user_stop".format(robot_id),
# Bool,
# self.rostopics_hz_dict[robot_id].callback_hz,
# callback_args="/PIONIER{0}/RosAria/user_stop".format(robot_id),
# )
# Removing old hz subscribers
for robot_key in self.robots_hz_subscribers_dict:
@ -258,7 +259,7 @@ 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 = rospy.Timer(rospy.Duration(0.5), self.get_robots_list)
def set_selected_robot_info_update_callback(self, callback_function):
self.selected_robot_info_update_callback = callback_function

14
safety_user_plugin/user_widget.py
View File

@ -1,18 +1,22 @@
#!/usr/bin/env python
import os
import rospkg
from ament_index_python.packages import get_package_share_directory
from python_qt_binding.QtWidgets import QWidget
from python_qt_binding import loadUi
class UserWidget(QWidget):
class UserWidget(QWidget):
def __init__(self, context):
super(UserWidget, self).__init__()
ui_file = os.path.join(rospkg.RosPack().get_path('safety_user_plugin'), 'ui', 'user_view.ui')
ui_file = os.path.join(
get_package_share_directory("safety_user_plugin"), "ui", "user_view.ui"
)
loadUi(ui_file, self)
self.setObjectName('User Plugin - L1.5 safety system')
self.setObjectName("User Plugin - L1.5 safety system")
if context.serial_number() > 1:
self.setWindowTitle(self.windowTitle() + (' (%d)' % context.serial_number()))
self.setWindowTitle(
self.windowTitle() + (" (%d)" % context.serial_number())
)
context.add_widget(self)