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

fixed the logic of the buttons

Browse Source 
Signed-off-by: Jakub Delicat <delicat.kuba@gmail.com>
This commit is contained in:
Jakub Delicat 2023-08-16 16:17:54 +02:00
parent 978b9026bd
commit 7360abc3f4
3 changed files with 87 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
safety_master_plugin/master_plugin.py
View File

@ -45,11 +45,13 @@ class MasterPlugin(Plugin):
# timer to consecutively send twist messages
self._update_parameter_timer = QTimer(self)
self._update_parameter_timer.timeout.connect(self._ros_wrapper.timer_callback)
self._update_parameter_timer.timeout.connect(
self._ros_wrapper.qt_timer_callback
)
self._update_parameter_timer.start(100)
# Stopping master at the beginning
self.master_stopped()
self.disable_robots()
def set_callback_functions(self):
self.set_ROS_callback_functions()
@ -77,10 +79,10 @@ class MasterPlugin(Plugin):
def handle_master_stop_state_updated(self):
"""Handles master stop state when it changes e. g. on button click and confirmed."""
if self._master_info.master_stop_state == SS.RUNNING:
self.master_stopped()
elif self._master_info.master_stop_state == SS.STOPPED:
self.master_started()
if self._master_info.master_stop_state == SS.STOPPED:
self.enable_robots()
elif self._master_info.master_stop_state == SS.RUNNING:
self.disable_robots()
else:
raise ValueError("master_stop_state value undefined")
@ -103,21 +105,21 @@ class MasterPlugin(Plugin):
self.cbhandler.add_robot_signal.emit(robot_id)
def robot_unregistered(self, robot_id):
# self._ros_wrapper.remove_robot_subscriber(robot_id)
self._ros_wrapper.remove_robot_subscriber(robot_id)
self.cbhandler.remove_robot_signal.emit(robot_id)
def update_robot_info(self, robot_info):
self.cbhandler.robot_info_signal.emit(robot_info)
def master_started(self):
def enable_robots(self):
self._master_info.master_stop_state = SS.RUNNING
self._ros_wrapper.master_started()
self._qt_wrapper.master_started()
self._ros_wrapper.when_robots_enabled()
self._qt_wrapper.when_robots_enabled()
def master_stopped(self):
def disable_robots(self):
self._master_info.master_stop_state = SS.STOPPED
self._ros_wrapper.master_stopped()
self._qt_wrapper.master_stopped()
self._ros_wrapper.when_robots_disabled()
self._qt_wrapper.when_robots_disabled()
def restrictions_changed(self, restrictions):
self._master_info.restrictions = restrictions

20
safety_master_plugin/qt_master_wrapper.py
View File

@ -137,17 +137,17 @@ class QtWrapper:
self.master_stop_state_updated_callback()
def display_master_stop_on(self):
self.widget.masterstopButton.setStyleSheet(
"QPushButton { color: black; background-color: green; font: bold 20px}"
)
self.widget.masterstopButton.setText("Zatrzymaj roboty")
def display_master_stop_off(self):
self.widget.masterstopButton.setStyleSheet(
"QPushButton { color: black; background-color: red; font: bold 20px}"
)
self.widget.masterstopButton.setText("Odblokuj roboty")
def display_master_stop_off(self):
self.widget.masterstopButton.setStyleSheet(
"QPushButton { color: black; background-color: green; font: bold 20px}"
)
self.widget.masterstopButton.setText("Zatrzymaj roboty")
def update_robot_info(self, robot_info):
items_to_add = []
items_to_add.append(QStandardItem("pioneer{}".format(robot_info.robot_id)))
@ -197,14 +197,14 @@ class QtWrapper:
item.setEditable(0)
self.stdItemModel.setItem(row_number, i, item)
def master_stopped(self):
def when_robots_disabled(self):
self.master_stop_state = SS.STOPPED
self.display_master_stop_off()
self.display_master_stop_on()
self.log_info("Przycisk masterSTOP zostal naciśnięty. Zatrzymuję roboty")
def master_started(self):
def when_robots_enabled(self):
self.master_stop_state = SS.RUNNING
self.display_master_stop_on()
self.display_master_stop_off()
self.log_info("Przycisk masterSTOP odciśnięty")
def add_robot(self, robot_id):

91
safety_master_plugin/ros_master_wrapper.py
View File

@ -1,7 +1,13 @@
#!/usr/bin/env python
import rclpy
from rclpy.node import Node, NodeNameNonExistentError
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSLivelinessPolicy, QoSDurabilityPolicy
from rclpy.qos import (
QoSProfile,
QoSReliabilityPolicy,
QoSHistoryPolicy,
QoSLivelinessPolicy,
QoSDurabilityPolicy,
)
from std_msgs.msg import Bool
from ros2aria_msgs.msg import RobotInfoMsg
@ -12,10 +18,10 @@ from safety_master_plugin.master_restrictions import Restrictions
from safety_master_plugin.stop_state import StopState as SS
class ROSWrapper(Node):
class ROSWrapper(Node):
def __init__(self):
super().__init__('safety_master_plugin')
super().__init__("safety_master_plugin")
self.robots_list_update_callback = None
self.robot_info_update_callback = None
@ -30,37 +36,47 @@ class ROSWrapper(Node):
durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
history=QoSHistoryPolicy.SYSTEM_DEFAULT,
liveliness=QoSLivelinessPolicy.AUTOMATIC,
depth=10
depth=10,
)
self.subs = []
def setup_subscribers_and_publishers(self):
self.master_stop_publisher = self.create_publisher(Bool, '/pioneers/master_stop', 10)
self.restrictions_publisher = self.create_publisher(RestrictionsMsg, '/pioneers/restrictions', 10)
self.master_stop_publisher = self.create_publisher(
Bool, "/pioneers/master_stop", 10
)
self.restrictions_publisher = self.create_publisher(
RestrictionsMsg, "/pioneers/restrictions", 10
)
def add_robot_subscriber(self, robot_id):
if robot_id not in self.robot_info_subscribers:
topic_name = '/pioneer{0}/robot_info'.format(robot_id)
self.robot_info_subscribers[robot_id] = self.create_subscription(RobotInfoMsg, topic_name, self.handle_robot_info_update, 10)
topic_name = "/pioneer{0}/robot_info".format(robot_id)
self.robot_info_subscribers[robot_id] = self.create_subscription(
RobotInfoMsg, topic_name, self.handle_robot_info_update, 10
)
else:
raise RuntimeError('Subscriber for pioneer{0} already in dictionary'.format(robot_id))
raise RuntimeError(
"Subscriber for pioneer{0} already in dictionary".format(robot_id)
)
# def remove_robot_subscriber(self,robot_id):
# if robot_id in self.robot_info_subscribers:
# self.unsubscribe(self.robot_info_subscribers[robot_id])
# del self.robot_info_subscribers[robot_id]
# else:
# raise RuntimeError('Subscriber for PIONIER{0} not in dictionary. Cannot be removed'.format(robot_id))
def remove_robot_subscriber(self, robot_id):
if robot_id in self.robot_info_subscribers:
self.unsubscribe(self.robot_info_subscribers[robot_id])
del self.robot_info_subscribers[robot_id]
else:
raise RuntimeError(
"Subscriber for PIONEER{0} not in dictionary. Cannot be removed".format(
robot_id
)
)
# def unsubscribe(self,subscriber):
# if subscriber != None:
# subscriber.unregister()
# def unregister_publisher(self,publisher):
# if publisher != None:
# publisher.unregister()
def unsubscribe(self, subscriber):
if subscriber is not None:
subscriber.destroy()
def unregister_publisher(self, publisher):
if publisher is not None:
publisher.destroy()
def publish_periodic_update(self):
restrictions = self.get_restrictions()
@ -73,12 +89,14 @@ class ROSWrapper(Node):
stop_msg = Bool()
if stop_state == SS.RUNNING:
stop_msg.data = True
elif stop_state == SS.STOPPED:
stop_msg.data = False
elif stop_state == SS.STOPPED:
stop_msg.data = True
else:
stop_msg.data = False
raise ValueError('stop_state UNKNOWN when attempting to publish periodic update')
raise ValueError(
"stop_state UNKNOWN when attempting to publish periodic update"
)
self.restrictions_publisher.publish(restrictions_msg)
self.master_stop_publisher.publish(stop_msg)
@ -92,7 +110,7 @@ class ROSWrapper(Node):
def unregister_node(self):
self.cancel_subscribers_and_publishers()
rospy.signal_shutdown('Closing safety master plugin')
rospy.signal_shutdown("Closing safety master plugin")
def setup_get_master_stop_state_function(self, function):
self.get_master_stop_state = function
@ -108,7 +126,9 @@ class ROSWrapper(Node):
for pioneer_id in range(6):
published_topics_list = []
try:
published_topics_list = self.get_publisher_names_and_types_by_node('ros2aria', '/pioneer'+str(pioneer_id))
published_topics_list = self.get_publisher_names_and_types_by_node(
"ros2aria", "/pioneer" + str(pioneer_id)
)
except NodeNameNonExistentError:
pass
@ -122,7 +142,6 @@ class ROSWrapper(Node):
_robot_info.update_robot_info(msg)
self.robot_info_update_callback(_robot_info)
# MasterPlugin Callbacks
def set_robots_list_update_callback(self, callback_function):
self.robots_list_update_callback = callback_function
@ -130,16 +149,16 @@ class ROSWrapper(Node):
def set_robot_info_update_callback(self, callback_function):
self.robot_info_update_callback = callback_function
def master_started(self):
msg = Bool()
msg.data = True
self.master_stop_publisher.publish(msg)
def master_stopped(self):
def when_robots_enabled(self):
msg = Bool()
msg.data = False
self.master_stop_publisher.publish(msg)
def when_robots_disabled(self):
msg = Bool()
msg.data = True
self.master_stop_publisher.publish(msg)
def restrictions_changed(self, restrictions):
msg = RestrictionsMsg()
@ -149,7 +168,7 @@ class ROSWrapper(Node):
self.restrictions_publisher.publish(msg)
def timer_callback(self):
def qt_timer_callback(self):
if self.publish_periodic_update is not None:
self.publish_periodic_update()