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

added disabling robot when released

Browse Source 
Signed-off-by: Jakub Delicat <delicat.kuba@gmail.com>
This commit is contained in:
Jakub Delicat 2023-08-16 16:41:01 +02:00
parent fc4c829787
commit 9164745938
6 changed files with 72 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
safety_user_plugin/english.py
View File

@ -21,7 +21,7 @@ class EnglishTexts(LanguageTexts):
release_robot_text = "Release robot" release_robot_text = "Release robot"
# Logger info texts # Logger info texts
robot_selected_text = "PIONIER{0} selected!" robot_selected_text = "PIONEER{0} selected!"
robot_released_text = "Robot released" robot_released_text = "Robot released"
stopped_and_engaged_text = "Robot will be stopped and clutch engaged" stopped_and_engaged_text = "Robot will be stopped and clutch engaged"
@ -44,7 +44,7 @@ class EnglishTexts(LanguageTexts):
cannot_select_robot_text = ( cannot_select_robot_text = (
"Robot can't be selected. It is already selected by another group" "Robot can't be selected. It is already selected by another group"
) )
connection_to_robot_lost_text = "Connection lost with robot PIONIER{0}" connection_to_robot_lost_text = "Connection lost with robot PIONEER{0}"
connection_to_master_lost_text = ( connection_to_master_lost_text = (
"Connection lost with masterSTOP. Ask the teacher to run it" "Connection lost with masterSTOP. Ask the teacher to run it"
) )

6
safety_user_plugin/polish.py
View File

@ -21,7 +21,7 @@ class PolishTexts(LanguageTexts):
release_robot_text = "Zwolnij robota" release_robot_text = "Zwolnij robota"
# Logger info texts # Logger info texts
robot_selected_text = "PIONIER{0} wybrany!" robot_selected_text = "PIONEER{0} wybrany!"
robot_released_text = "Odłączam robota" robot_released_text = "Odłączam robota"
stopped_and_engaged_text = "Robot zostanie zatrzymany i sprzęgnięty" stopped_and_engaged_text = "Robot zostanie zatrzymany i sprzęgnięty"
@ -42,7 +42,7 @@ class PolishTexts(LanguageTexts):
obstacle_removed_text = "Przeszkoda usunięta" obstacle_removed_text = "Przeszkoda usunięta"
# Logger error/problem messages # Logger error/problem messages
selection_error_text = "Najpierw wybierz PIONIERA z listy robotów" 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_masterstop_text = "Nie można wystartować robota. MasterSTOP wciśnięty!"
cannot_start_obstacle_text = ( cannot_start_obstacle_text = (
"Nie mozna wystartować. Przeszkoda w polu działania robota" "Nie mozna wystartować. Przeszkoda w polu działania robota"
@ -50,7 +50,7 @@ class PolishTexts(LanguageTexts):
cannot_select_robot_text = ( cannot_select_robot_text = (
"Nie mozna wybrać robota. Robot został już wybrany przez inną grupę" "Nie mozna wybrać robota. Robot został już wybrany przez inną grupę"
) )
connection_to_robot_lost_text = "Utrata połączenia z robotem PIONIER{0}" connection_to_robot_lost_text = "Utrata połączenia z robotem PIONEER{0}"
connection_to_master_lost_text = ( connection_to_master_lost_text = (
"Utrata połączenia z masterstopem. Poproś prowadzącego o jego uruchomienie" "Utrata połączenia z masterstopem. Poproś prowadzącego o jego uruchomienie"
) )

8
safety_user_plugin/qt_wrapper.py
View File

@ -102,7 +102,7 @@ class QtWrapper:
def get_selected_robot_id(self): def get_selected_robot_id(self):
current_text = self.widget.robotsList.currentText() current_text = self.widget.robotsList.currentText()
if "PIONIER" in current_text: if "PIONEER" in current_text:
return int(current_text[7:]) return int(current_text[7:])
else: else:
return None return None
@ -179,7 +179,7 @@ class QtWrapper:
def update_robots_list_gui(self, robots_id_list): def update_robots_list_gui(self, robots_id_list):
robots_id_list.sort() robots_id_list.sort()
id_strings_list = (("PIONIER" + str(x)) for x in robots_id_list) id_strings_list = (("PIONEER" + str(x)) for x in robots_id_list)
robots_to_add = [] robots_to_add = []
robots_to_remove = [] robots_to_remove = []
@ -208,7 +208,7 @@ class QtWrapper:
return return
def add_robot_to_list(self, robot_id): def add_robot_to_list(self, robot_id):
self.widget.robotsList.addItem("PIONIER{0}".format(robot_id)) self.widget.robotsList.addItem("PIONEER{0}".format(robot_id))
self.displayed_robots_id_list.append(robot_id) self.displayed_robots_id_list.append(robot_id)
def update_selected_robot_info(self, robot_info): def update_selected_robot_info(self, robot_info):
@ -216,7 +216,7 @@ class QtWrapper:
robot_info.linear_velocity[0] ** 2 + robot_info.linear_velocity[1] ** 2 robot_info.linear_velocity[0] ** 2 + robot_info.linear_velocity[1] ** 2
) )
self.widget.idLabel.setText("PIONIER{0}".format(robot_info.robot_id)) self.widget.idLabel.setText("PIONEER{0}".format(robot_info.robot_id))
self.widget.batteryLabel.setText("{:.2f}".format(robot_info.battery_voltage)) self.widget.batteryLabel.setText("{:.2f}".format(robot_info.battery_voltage))
self.widget.linearLabel.setText("{:.2f}".format(linear_vel)) self.widget.linearLabel.setText("{:.2f}".format(linear_vel))
self.widget.angularLabel.setText( self.widget.angularLabel.setText(

4
safety_user_plugin/robot_info.py
View File

@ -29,12 +29,12 @@ class RobotInfo:
] ]
self.obstacle_detected = robot_info_msg.obstacle_detected.data self.obstacle_detected = robot_info_msg.obstacle_detected.data
if robot_info_msg.state.data == True: if robot_info_msg.state.data is True:
self.state = SS.RUNNING self.state = SS.RUNNING
else: else:
self.state = SS.STOPPED self.state = SS.STOPPED
if robot_info_msg.clutch.data == True: if robot_info_msg.clutch.data is True:
self.clutch = CS.ENGAGED self.clutch = CS.ENGAGED
else: else:
self.clutch = CS.DISENGAGED self.clutch = CS.DISENGAGED

81
safety_user_plugin/ros_wrapper.py
View File

@ -1,6 +1,13 @@
#!/usr/bin/env python #!/usr/bin/env python
import rclpy import rclpy
from rclpy.node import Node, NodeNameNonExistentError from rclpy.node import Node, NodeNameNonExistentError
from rclpy.qos import (
QoSProfile,
QoSReliabilityPolicy,
QoSHistoryPolicy,
QoSLivelinessPolicy,
QoSDurabilityPolicy,
)
from std_msgs.msg import Bool from std_msgs.msg import Bool
from ros2aria_msgs.msg import RobotInfoMsg from ros2aria_msgs.msg import RobotInfoMsg
@ -43,30 +50,38 @@ class ROSWrapper(Node):
self.rostopics_hz_dict = {} self.rostopics_hz_dict = {}
self.robots_hz_value = {} self.robots_hz_value = {}
# def setup_node(self): self._qos = QoSProfile(
# rospy.init_node('safety_user_plugin', anonymous=True) reliability=QoSReliabilityPolicy.RELIABLE,
durability=QoSDurabilityPolicy.VOLATILE,
history=QoSHistoryPolicy.SYSTEM_DEFAULT,
liveliness=QoSLivelinessPolicy.AUTOMATIC,
depth=10,
)
def setup_subscribers_and_publishers(self, robot_id): def setup_subscribers_and_publishers(self, robot_id):
robot_name = "PIONIER" + str(robot_id) robot_name = "PIONEER" + str(robot_id)
# self.robot_info_subscriber = rospy.Subscriber( # self.robot_info_subscriber = rospy.Subscriber(
# "/{0}/RosAria/robot_info".format(robot_name), # "/{0}/RosAria/robot_info".format(robot_name),
# RobotInfoMsg, # RobotInfoMsg,
# self.handle_selected_robot_info_update, # self.handle_selected_robot_info_update,
# ) # )
# self.master_stop_subscriber = rospy.Subscriber( self.master_stop_subscriber = self.create_subscription(
# "/PIONIER/master_stop", Bool, self.handle_master_stop_update Bool,
# ) "/pioneers/master_stop",
self.handle_master_stop_update,
qos_profile=self._qos,
)
# self.restrictions_subscriber = rospy.Subscriber( # self.restrictions_subscriber = rospy.Subscriber(
# "/PIONIER/restrictions", RestrictionsMsg, self.handle_restrictions_update # "/PIONEER/restrictions", RestrictionsMsg, self.handle_restrictions_update
# ) # )
# self.user_stop_publisher = rospy.Publisher( self.user_stop_publisher = self.create_publisher(
# "/{0}/RosAria/user_stop".format(robot_name), Bool, queue_size=1 Bool, f"/pioneer{robot_id}/user_stop", 10
# ) )
# self.clutch_publisher = rospy.Publisher( self.clutch_publisher = self.create_publisher(
# "/{0}/RosAria/clutch".format(robot_name), Bool, queue_size=1 Bool, f"/pioneer{robot_id}/clutch", 10
# ) )
# if self.periodic_timer != None: # if self.periodic_timer != None:
# self.shutdown_timer(self.periodic_timer) # self.shutdown_timer(self.periodic_timer)
@ -92,11 +107,11 @@ class ROSWrapper(Node):
def unsubscribe(self, subscriber): def unsubscribe(self, subscriber):
if subscriber != None: if subscriber != None:
subscriber.unregister() subscriber.destroy()
def unregister_publisher(self, publisher): def unregister_publisher(self, publisher):
if publisher != None: if publisher != None:
publisher.unregister() publisher.destroy()
def shutdown_timer(self, timer): def shutdown_timer(self, timer):
if timer != None: if timer != None:
@ -107,9 +122,9 @@ class ROSWrapper(Node):
# clutch_state = self.get_clutch_state() # clutch_state = self.get_clutch_state()
if stop_state == SS.RUNNING: if stop_state == SS.RUNNING:
self.user_started() self.user_robot_enabled()
elif stop_state == SS.STOPPED: elif stop_state == SS.STOPPED:
self.user_stopped() self.user_robot_disabled()
else: else:
raise ValueError( raise ValueError(
"stop_state UNKNOWN when attempting to publish periodic update" "stop_state UNKNOWN when attempting to publish periodic update"
@ -126,7 +141,7 @@ class ROSWrapper(Node):
print("update_hz_values") print("update_hz_values")
# for key in self.rostopics_hz_dict: # for key in self.rostopics_hz_dict:
# self.robots_hz_value[key] = self.rostopics_hz_dict[key].get_hz( # self.robots_hz_value[key] = self.rostopics_hz_dict[key].get_hz(
# "/PIONIER{0}/RosAria/user_stop".format(key) # "/PIONEER{0}/RosAria/user_stop".format(key)
# ) # )
def handle_robot_connection_lost(self, event): def handle_robot_connection_lost(self, event):
@ -165,7 +180,8 @@ class ROSWrapper(Node):
def restart_master_connection_timer(self): def restart_master_connection_timer(self):
if self.master_connection_timer != None: if self.master_connection_timer != None:
self.master_connection_timer.shutdown() print("Destroy connection timer")
# self.master_connection_timer.destroy()
# self.master_connection_timer = rospy.Timer( # self.master_connection_timer = rospy.Timer(
# rospy.Duration(1.5), self.handle_master_connection_lost # rospy.Duration(1.5), self.handle_master_connection_lost
# ) # )
@ -190,8 +206,8 @@ class ROSWrapper(Node):
# ROSWrapper Callbacks # ROSWrapper Callbacks
def get_robots_list_and_refresh_subscribers(self): def get_robots_list_and_refresh_subscribers(self):
robots_id_list = self._get_robots_list() robots_id_list = self._get_robots_list()
self._create_hz_subscribers(robots_id_list) # self._create_hz_subscribers(robots_id_list)
self._remove_hz_subscribers(robots_id_list) # self._remove_hz_subscribers(robots_id_list)
self.robots_list_update_callback(robots_id_list) self.robots_list_update_callback(robots_id_list)
def _get_robots_list(self): def _get_robots_list(self):
@ -218,10 +234,10 @@ class ROSWrapper(Node):
print("ADDED SUB") print("ADDED SUB")
# self.rostopics_hz_dict[robot_id] = rostopic.ROSTopicHz(-1) # self.rostopics_hz_dict[robot_id] = rostopic.ROSTopicHz(-1)
# self.robots_hz_subscribers_dict[robot_id] = rospy.Subscriber( # self.robots_hz_subscribers_dict[robot_id] = rospy.Subscriber(
# "/PIONIER{0}/RosAria/user_stop".format(robot_id), # "/PIONEER{0}/RosAria/user_stop".format(robot_id),
# Bool, # Bool,
# self.rostopics_hz_dict[robot_id].callback_hz, # self.rostopics_hz_dict[robot_id].callback_hz,
# callback_args="/PIONIER{0}/RosAria/user_stop".format(robot_id), # callback_args="/PIONEER{0}/RosAria/user_stop".format(robot_id),
# ) # )
def _remove_hz_subscribers(self, robots_id_list): def _remove_hz_subscribers(self, robots_id_list):
@ -234,6 +250,8 @@ class ROSWrapper(Node):
self.rostopics_hz_dict[robot_id] = None self.rostopics_hz_dict[robot_id] = None
def release_robot(self): def release_robot(self):
self.disengage_clutch()
self.user_robot_disabled()
self.unsubscribe(self.robot_info_subscriber) self.unsubscribe(self.robot_info_subscriber)
self.shutdown_timer(self.periodic_timer) self.shutdown_timer(self.periodic_timer)
self.shutdown_timer(self.connection_timer) self.shutdown_timer(self.connection_timer)
@ -249,10 +267,11 @@ class ROSWrapper(Node):
def handle_master_stop_update(self, msg): def handle_master_stop_update(self, msg):
# Restarting master connection timer to avoid raising master_connection_lost_callback # Restarting master connection timer to avoid raising master_connection_lost_callback
self.restart_master_connection_timer() # TODO: Czy trzeba resetować?
# self.restart_master_connection_timer()
master_stop_state = SS.UNKNOWN master_stop_state = SS.UNKNOWN
if msg.data == True: if msg.data is False:
master_stop_state = SS.RUNNING master_stop_state = SS.RUNNING
else: else:
master_stop_state = SS.STOPPED master_stop_state = SS.STOPPED
@ -295,15 +314,15 @@ class ROSWrapper(Node):
msg.data = False msg.data = False
self.clutch_publisher.publish(msg) self.clutch_publisher.publish(msg)
def user_started(self): def user_robot_enabled(self):
msg = Bool()
msg.data = True
self.user_stop_publisher.publish(msg)
def user_stopped(self):
msg = Bool() msg = Bool()
msg.data = False msg.data = False
self.user_stop_publisher.publish(msg) self.user_stop_publisher.publish(msg)
def user_robot_disabled(self):
msg = Bool()
msg.data = True
self.user_stop_publisher.publish(msg)
def qt_timer_callback(self): def qt_timer_callback(self):
rclpy.spin_once(self, timeout_sec=0.001) rclpy.spin_once(self, timeout_sec=0.001)

22
safety_user_plugin/user_plugin.py
View File

@ -54,7 +54,6 @@ class UserPlugin(Plugin):
self._user_info.get_clutch_state self._user_info.get_clutch_state
) )
# self._ros_wrapper.setup_node()
self.set_callback_functions() self.set_callback_functions()
# At the end! # At the end!
@ -139,10 +138,11 @@ class UserPlugin(Plugin):
# Qt callback functions # Qt callback functions
def handle_robot_selection(self, robot_id): def handle_robot_selection(self, robot_id):
if self._user_info.selected_robot == None: if self._user_info.selected_robot == None:
if self._ros_wrapper.robots_hz_value[robot_id] == None: # TODO: hz subskrypcje służą do sprawdzaNIA czy robot jest już zajęty
self.select_robot(robot_id) # if self._ros_wrapper.robots_hz_value[robot_id] == None:
else: self.select_robot(robot_id)
self._qt_wrapper.robot_selected_by_another_group_notify() # else:
# self._qt_wrapper.robot_selected_by_another_group_notify()
else: else:
raise RuntimeError("User already selected robot") raise RuntimeError("User already selected robot")
@ -207,13 +207,13 @@ class UserPlugin(Plugin):
def user_stopped(self): def user_stopped(self):
self._user_info.user_stop_state = SS.STOPPED self._user_info.user_stop_state = SS.STOPPED
self._ros_wrapper.user_stopped() self._ros_wrapper.user_robot_disabled()
self.call_qt_wrapper_method("user_stopped") self.call_qt_wrapper_method("user_stopped")
# self._qt_wrapper.user_stopped() # self._qt_wrapper.user_stopped()
def user_started(self): def user_started(self):
self._user_info.user_stop_state = SS.RUNNING self._user_info.user_stop_state = SS.RUNNING
self._ros_wrapper.user_started() self._ros_wrapper.user_robot_enabled()
self.call_qt_wrapper_method("user_started") self.call_qt_wrapper_method("user_started")
# self._qt_wrapper.user_started() # self._qt_wrapper.user_started()
@ -275,13 +275,13 @@ class UserPlugin(Plugin):
def update_selected_robot_info(self, robot_info): def update_selected_robot_info(self, robot_info):
if self._user_info.selected_robot != None: if self._user_info.selected_robot != None:
if ( if (
robot_info.obstacle_detected == True robot_info.obstacle_detected is True
and self._user_info.selected_robot.obstacle_detected == False and self._user_info.selected_robot.obstacle_detected is False
): ):
self.obstacle_detected() self.obstacle_detected()
elif ( elif (
robot_info.obstacle_detected == False robot_info.obstacle_detected is False
and self._user_info.selected_robot.obstacle_detected == True and self._user_info.selected_robot.obstacle_detected is True
): ):
self.obstacle_removed() self.obstacle_removed()