fixed the logic of the buttons
Signed-off-by: Jakub Delicat <delicat.kuba@gmail.com>
This commit is contained in:
Jakub Delicat
parent
978b9026bd
commit
7360abc3f4
@ -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
|
||||
|
||||
@ -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):
|
||||
|
||||
@ -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):
|
||||
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,12 +110,12 @@ 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):
|
||||
def setup_get_master_stop_state_function(self, function):
|
||||
self.get_master_stop_state = function
|
||||
|
||||
def setup_get_restrictions_function(self,function):
|
||||
def setup_get_restrictions_function(self, function):
|
||||
self.get_restrictions = function
|
||||
|
||||
# ROSWrapper Callbacks
|
||||
@ -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,25 +142,24 @@ 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):
|
||||
def set_robots_list_update_callback(self, callback_function):
|
||||
self.robots_list_update_callback = callback_function
|
||||
|
||||
def set_robot_info_update_callback(self,callback_function):
|
||||
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 restrictions_changed(self,restrictions):
|
||||
def when_robots_disabled(self):
|
||||
msg = Bool()
|
||||
msg.data = True
|
||||
self.master_stop_publisher.publish(msg)
|
||||
|
||||
def restrictions_changed(self, restrictions):
|
||||
msg = RestrictionsMsg()
|
||||
|
||||
msg.distance.data = restrictions.distance
|
||||
@ -149,10 +168,10 @@ 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()
|
||||
|
||||
|
||||
if self.get_robots_list is not None:
|
||||
self.get_robots_list()
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user