opened window | removed ros1 wrappers
Signed-off-by: Jakub Delicat <delicat.kuba@gmail.com>
This commit is contained in:
Jakub Delicat
parent
ce405ab4f1
commit
2a755bf328
@ -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>
|
||||
|
||||
@ -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))
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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):
|
||||
|
||||
def __init__(self,context):
|
||||
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)
|
||||
Loading…
Reference in New Issue
Block a user