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

Working with one robot

Browse Source 
Signed-off-by: Jakub Delicat <delicat.kuba@gmail.com>
This commit is contained in:
Jakub Delicat 2023-08-23 19:43:24 +02:00
parent c57830c690
commit 1c621bf684
5 changed files with 48 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
safety_user_plugin/english.py
View File

@ -38,6 +38,9 @@ class EnglishTexts(LanguageTexts):
obstacle_removed_text = "Obstacle removed"
# Logger error/problem messages
master_plugin_inactive_text = (
"Master is inactive. Ask the teacher about the master plugin."
)
selection_error_text = "Select the PIONEER from robots list first"
cannot_start_masterstop_text = "Robot can't be started. MasterSTOP pressed!"
cannot_start_obstacle_text = "Robot can't be started. Obstacle detected!"

3
safety_user_plugin/polish.py
View File

@ -42,6 +42,9 @@ class PolishTexts(LanguageTexts):
obstacle_removed_text = "Przeszkoda usunięta"
# Logger error/problem messages
master_plugin_inactive_text = (
"Master nieaktywny. Zapytaj prowadzącego o odpalenie programu."
)
selection_error_text = "Najpierw wybierz PIONEERA z listy robotów"
cannot_start_masterstop_text = "Nie można wystartować robota. MasterSTOP wciśnięty!"
cannot_start_obstacle_text = (

3
safety_user_plugin/qt_wrapper.py
View File

@ -140,6 +140,9 @@ class QtWrapper:
def handle_not_selected_error(self):
self.log_error(self.language.selection_error_text)
def log_master_inactive(self):
self.log_warning(self.language.master_plugin_inactive_text)
def handle_openButton_clicked(self):
selected_robot_id = self.get_selected_robot_id()

98
safety_user_plugin/ros_wrapper.py
View File

@ -29,6 +29,8 @@ class ROSWrapper(Node):
self.selected_robot_info_update_callback = None
self.master_stop_update_callback = None
self.restrictions_update_callback = None
self.master_connection_lost_callback = None
self.robot_connection_lost_callback = None
self.get_user_stop_state = None
self.get_clutch_state = None
@ -92,11 +94,10 @@ class ROSWrapper(Node):
# 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
# )
self.shutdown_timer(self.connection_timer)
self.connection_timer = self.create_timer(
5.0, self.handle_robot_connection_lost
)
# if self.master_connection_timer != None:
# self.shutdown_timer(self.master_connection_timer)
@ -109,18 +110,21 @@ class ROSWrapper(Node):
# self.hz_update_timer = rospy.Timer(rospy.Duration(0.2), self.update_hz_values)
def unsubscribe(self, subscriber):
if subscriber != None:
subscriber.destroy()
if subscriber is not None:
self.destroy_subscription(subscriber)
subscriber = None
def unregister_publisher(self, publisher):
if publisher != None:
publisher.destroy()
while publisher is not None:
self.destroy_publisher(publisher)
publisher = None
def shutdown_timer(self, timer):
if timer != None:
timer.shutdown()
while timer is not None:
self.destroy_timer(timer)
timer = None
def publish_periodic_update(self, event):
def publish_periodic_update(self):
stop_state = self.get_user_stop_state()
clutch_state = self.get_clutch_state()
@ -149,11 +153,11 @@ class ROSWrapper(Node):
# "/PIONEER{0}/RosAria/user_stop".format(key)
# )
def handle_robot_connection_lost(self, event):
def handle_robot_connection_lost(self):
self.shutdown_timer(self.connection_timer)
self.robot_connection_lost_callback()
def handle_master_connection_lost(self, event):
def handle_master_connection_lost(self):
self.shutdown_timer(self.master_connection_timer)
self.master_connection_lost_callback()
@ -165,38 +169,21 @@ class ROSWrapper(Node):
self.unregister_publisher(self.user_stop_publisher)
self.unregister_publisher(self.clutch_publisher)
self.periodic_timer = None
self.robot_info_subscriber = None
self.master_stop_subscriber = None
self.restrictions_subscriber = None
self.user_stop_publisher = None
self.clutch_publisher = None
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
# )
else:
raise RuntimeError(
"Attempting to restart connection_timer when it is not initialized"
self.shutdown_timer(self.connection_timer)
self.connection_timer = self.create_timer(
5.0, self.handle_robot_connection_lost
)
def restart_master_connection_timer(self):
if self.master_connection_timer != None:
print("Destroy connection timer")
# self.master_connection_timer.destroy()
# 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"
self.shutdown_timer(self.master_connection_timer)
self.master_connection_timer = self.create_timer(
1.5, self.handle_master_connection_lost
)
def unregister_node(self):
self.cancel_subscribers_and_publishers()
print("Unregister node.")
# rospy.signal_shutdown("Closing safety user plugin")
def select_robot(self, robot_id):
@ -211,14 +198,12 @@ class ROSWrapper(Node):
# ROSWrapper Callbacks
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 = []
# NOTE: Można to zrobić lepiej np. czekać na publikowaną wiadomość na robot_info
for pioneer_id in range(6):
for pioneer_id in range(1, 7):
published_topics_list = []
try:
published_topics_list = self.get_publisher_names_and_types_by_node(
@ -231,40 +216,20 @@ class ROSWrapper(Node):
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:
# Add hz subscriber
print("ADDED SUB")
# self.rostopics_hz_dict[robot_id] = rostopic.ROSTopicHz(-1)
# self.robots_hz_subscribers_dict[robot_id] = rospy.Subscriber(
# "/PIONEER{0}/RosAria/user_stop".format(robot_id),
# Bool,
# self.rostopics_hz_dict[robot_id].callback_hz,
# callback_args="/PIONEER{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:
# Remove old subscriber
self.unsubscribe(self.robots_hz_subscribers_dict[robot_id])
self.robots_hz_subscribers_dict[robot_id] = None
self.rostopics_hz_dict[robot_id] = None
def release_robot(self):
self.disengage_clutch()
self.user_robot_disabled()
self.unsubscribe(self.robot_info_subscriber)
self.unsubscribe(self.restrictions_subscriber)
self.unsubscribe(self.master_stop_subscriber)
self.shutdown_timer(self.periodic_timer)
self.shutdown_timer(self.connection_timer)
self.shutdown_timer(self.master_connection_timer)
def handle_selected_robot_info_update(self, msg):
# Restarting connection timer to avoid raising robot_connection_lost_callback
# self.restart_connection_timer()
self.restart_connection_timer()
_robot_info = RobotInfo(0)
_robot_info.update_robot_info(msg)
@ -272,8 +237,7 @@ class ROSWrapper(Node):
def handle_master_stop_update(self, msg):
# Restarting master connection timer to avoid raising master_connection_lost_callback
# TODO: Czy trzeba resetować?
# self.restart_master_connection_timer()
self.restart_master_connection_timer()
master_stop_state = SS.UNKNOWN
if msg.data is False:

24
safety_user_plugin/user_plugin.py
View File

@ -34,7 +34,6 @@ class UserPlugin(Plugin):
def __init__(self, context):
super(UserPlugin, self).__init__(context)
self.setObjectName("User Plugin - L1.5 safety system")
# setStyleSheet("background-color:black;")
self.cbhandler = CallbackHandler()
self._widget = UserWidget(context)
@ -117,7 +116,7 @@ class UserPlugin(Plugin):
self._qt_wrapper.update_robots_list_gui(robots_id_list)
def handle_selected_robot_info_update(self, robot_info):
if robot_info != None:
if robot_info is not None:
self.update_selected_robot_info(robot_info)
else:
raise RuntimeError("Updated robot_info is NoneType object")
@ -137,7 +136,7 @@ class UserPlugin(Plugin):
# Qt callback functions
def handle_robot_selection(self, robot_id):
if self._user_info.selected_robot == None:
if self._user_info.selected_robot is None:
# TODO: hz subskrypcje służą do sprawdzaNIA czy robot jest już zajęty
# if self._ros_wrapper.robots_hz_value[robot_id] == None:
self.select_robot(robot_id)
@ -165,9 +164,9 @@ class UserPlugin(Plugin):
elif self._user_info.user_stop_state == SS.STOPPED:
self.user_started()
else:
raise ValueError("user_stop_state value undefined")
self._qt_wrapper.log_master_inactive()
else:
raise ValueError("master_stop_state value undefined")
self._qt_wrapper.log_master_inactive()
def handle_clutch_switched(self):
if self._user_info.clutch_state == CS.ENGAGED:
@ -196,42 +195,35 @@ class UserPlugin(Plugin):
if self._user_info.selected_robot != None:
self.user_stopped()
self.call_qt_wrapper_method("master_stopped")
# self._qt_wrapper.master_stopped()
def master_started(self):
self._user_info.master_stop_state = SS.RUNNING
if self._user_info.selected_robot != None:
self.call_qt_wrapper_method("master_started")
# self._qt_wrapper.master_started()
def user_stopped(self):
self._user_info.user_stop_state = SS.STOPPED
self._ros_wrapper.user_robot_disabled()
self.call_qt_wrapper_method("user_stopped")
# self._qt_wrapper.user_stopped()
def user_started(self):
self._user_info.user_stop_state = SS.RUNNING
self._ros_wrapper.user_robot_enabled()
self.call_qt_wrapper_method("user_started")
# self._qt_wrapper.user_started()
def engage_clutch(self):
self._user_info.clutch_state = CS.ENGAGED
self._ros_wrapper.engage_clutch()
self.call_qt_wrapper_method("engage_clutch")
# self._qt_wrapper.engage_clutch()
def disengage_clutch(self):
self._user_info.clutch_state = CS.DISENGAGED
self._ros_wrapper.disengage_clutch()
self.call_qt_wrapper_method("disengage_clutch")
# self._qt_wrapper.disengage_clutch()
def select_robot(self, robot_id):
self._ros_wrapper.select_robot(robot_id)
# self._qt_wrapper.select_robot(robot_id)
self.call_qt_wrapper_method_with_argument("select_robot", [robot_id])
self._user_info.select_robot(robot_id)
@ -247,21 +239,18 @@ class UserPlugin(Plugin):
def release_robot(self):
self._ros_wrapper.release_robot()
self.call_qt_wrapper_method("release_robot")
# self._qt_wrapper.release_robot()
self._user_info.release_robot()
self._user_info.master_stop_state = SS.UNKNOWN
def connection_to_robot_lost(self, lost_robot_id):
# pass
self._ros_wrapper.release_robot()
self._user_info.release_robot()
self.call_qt_wrapper_method_with_argument(
"connection_to_robot_lost", [lost_robot_id]
)
# self._qt_wrapper.connection_to_robot_lost(lost_robot_id)
def connection_to_master_lost(self):
if self._user_info.selected_robot != None:
if self._user_info.selected_robot is not None:
self.master_stopped()
self.call_qt_wrapper_method("connection_to_master_lost")
@ -273,7 +262,7 @@ class UserPlugin(Plugin):
self.call_qt_wrapper_method("obstacle_removed")
def update_selected_robot_info(self, robot_info):
if self._user_info.selected_robot != None:
if self._user_info.selected_robot is not None:
if (
robot_info.obstacle_detected is True
and self._user_info.selected_robot.obstacle_detected is False
@ -289,4 +278,3 @@ class UserPlugin(Plugin):
self.call_qt_wrapper_method_with_argument(
"update_selected_robot_info", [robot_info]
)
# self._qt_wrapper.update_selected_robot_info(robot_info)