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"
# Logger info texts
robot_selected_text = "PIONIER{0} selected!"
robot_selected_text = "PIONEER{0} selected!"
robot_released_text = "Robot released"
stopped_and_engaged_text = "Robot will be stopped and clutch engaged"
@ -44,7 +44,7 @@ class EnglishTexts(LanguageTexts):
cannot_select_robot_text = (
"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 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"
# Logger info texts
robot_selected_text = "PIONIER{0} wybrany!"
robot_selected_text = "PIONEER{0} wybrany!"
robot_released_text = "Odłączam robota"
stopped_and_engaged_text = "Robot zostanie zatrzymany i sprzęgnięty"
@ -42,7 +42,7 @@ class PolishTexts(LanguageTexts):
obstacle_removed_text = "Przeszkoda usunięta"
# 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_obstacle_text = (
"Nie mozna wystartować. Przeszkoda w polu działania robota"
@ -50,7 +50,7 @@ class PolishTexts(LanguageTexts):
cannot_select_robot_text = (
"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 = (
"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):
current_text = self.widget.robotsList.currentText()
if "PIONIER" in current_text:
if "PIONEER" in current_text:
return int(current_text[7:])
else:
return None
@ -179,7 +179,7 @@ class QtWrapper:
def update_robots_list_gui(self, robots_id_list):
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_remove = []
@ -208,7 +208,7 @@ class QtWrapper:
return
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)
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
)
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.linearLabel.setText("{:.2f}".format(linear_vel))
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
if robot_info_msg.state.data == True:
if robot_info_msg.state.data is True:
self.state = SS.RUNNING
else:
self.state = SS.STOPPED
if robot_info_msg.clutch.data == True:
if robot_info_msg.clutch.data is True:
self.clutch = CS.ENGAGED
else:
self.clutch = CS.DISENGAGED

81
safety_user_plugin/ros_wrapper.py
View File

@ -1,6 +1,13 @@
#!/usr/bin/env python
import rclpy
from rclpy.node import Node, NodeNameNonExistentError
from rclpy.qos import (
QoSProfile,
QoSReliabilityPolicy,
QoSHistoryPolicy,
QoSLivelinessPolicy,
QoSDurabilityPolicy,
)
from std_msgs.msg import Bool
from ros2aria_msgs.msg import RobotInfoMsg
@ -43,30 +50,38 @@ class ROSWrapper(Node):
self.rostopics_hz_dict = {}
self.robots_hz_value = {}
# def setup_node(self):
# rospy.init_node('safety_user_plugin', anonymous=True)
self._qos = QoSProfile(
reliability=QoSReliabilityPolicy.RELIABLE,
durability=QoSDurabilityPolicy.VOLATILE,
history=QoSHistoryPolicy.SYSTEM_DEFAULT,
liveliness=QoSLivelinessPolicy.AUTOMATIC,
depth=10,
)
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(
# "/{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.master_stop_subscriber = self.create_subscription(
Bool,
"/pioneers/master_stop",
self.handle_master_stop_update,
qos_profile=self._qos,
)
# 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(
# "/{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 = self.create_publisher(
Bool, f"/pioneer{robot_id}/user_stop", 10
)
self.clutch_publisher = self.create_publisher(
Bool, f"/pioneer{robot_id}/clutch", 10
)
# if self.periodic_timer != None:
# self.shutdown_timer(self.periodic_timer)
@ -92,11 +107,11 @@ class ROSWrapper(Node):
def unsubscribe(self, subscriber):
if subscriber != None:
subscriber.unregister()
subscriber.destroy()
def unregister_publisher(self, publisher):
if publisher != None:
publisher.unregister()
publisher.destroy()
def shutdown_timer(self, timer):
if timer != None:
@ -107,9 +122,9 @@ class ROSWrapper(Node):
# clutch_state = self.get_clutch_state()
if stop_state == SS.RUNNING:
self.user_started()
self.user_robot_enabled()
elif stop_state == SS.STOPPED:
self.user_stopped()
self.user_robot_disabled()
else:
raise ValueError(
"stop_state UNKNOWN when attempting to publish periodic update"
@ -126,7 +141,7 @@ class ROSWrapper(Node):
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)
# "/PIONEER{0}/RosAria/user_stop".format(key)
# )
def handle_robot_connection_lost(self, event):
@ -165,7 +180,8 @@ class ROSWrapper(Node):
def restart_master_connection_timer(self):
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(
# rospy.Duration(1.5), self.handle_master_connection_lost
# )
@ -190,8 +206,8 @@ 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._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):
@ -218,10 +234,10 @@ class ROSWrapper(Node):
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),
# "/PIONEER{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),
# callback_args="/PIONEER{0}/RosAria/user_stop".format(robot_id),
# )
def _remove_hz_subscribers(self, robots_id_list):
@ -234,6 +250,8 @@ class ROSWrapper(Node):
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.shutdown_timer(self.periodic_timer)
self.shutdown_timer(self.connection_timer)
@ -249,10 +267,11 @@ class ROSWrapper(Node):
def handle_master_stop_update(self, msg):
# 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
if msg.data == True:
if msg.data is False:
master_stop_state = SS.RUNNING
else:
master_stop_state = SS.STOPPED
@ -295,15 +314,15 @@ class ROSWrapper(Node):
msg.data = False
self.clutch_publisher.publish(msg)
def user_started(self):
msg = Bool()
msg.data = True
self.user_stop_publisher.publish(msg)
def user_stopped(self):
def user_robot_enabled(self):
msg = Bool()
msg.data = False
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):
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._ros_wrapper.setup_node()
self.set_callback_functions()
# At the end!
@ -139,10 +138,11 @@ class UserPlugin(Plugin):
# Qt callback functions
def handle_robot_selection(self, robot_id):
if self._user_info.selected_robot == None:
if self._ros_wrapper.robots_hz_value[robot_id] == None:
self.select_robot(robot_id)
else:
self._qt_wrapper.robot_selected_by_another_group_notify()
# 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)
# else:
# self._qt_wrapper.robot_selected_by_another_group_notify()
else:
raise RuntimeError("User already selected robot")
@ -207,13 +207,13 @@ class UserPlugin(Plugin):
def user_stopped(self):
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._qt_wrapper.user_stopped()
def user_started(self):
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._qt_wrapper.user_started()
@ -275,13 +275,13 @@ class UserPlugin(Plugin):
def update_selected_robot_info(self, robot_info):
if self._user_info.selected_robot != None:
if (
robot_info.obstacle_detected == True
and self._user_info.selected_robot.obstacle_detected == False
robot_info.obstacle_detected is True
and self._user_info.selected_robot.obstacle_detected is False
):
self.obstacle_detected()
elif (
robot_info.obstacle_detected == False
and self._user_info.selected_robot.obstacle_detected == True
robot_info.obstacle_detected is False
and self._user_info.selected_robot.obstacle_detected is True
):
self.obstacle_removed()