From 98c2257a00c585756cc7bd50cd2978d889e9027d Mon Sep 17 00:00:00 2001 From: Olek Bojda Date: Wed, 26 Sep 2018 15:23:55 +0200 Subject: [PATCH] Scheme of whole program prepared. Also masterstop button works --- safety_master_plugin/scripts/master_info.py | 16 ++ safety_master_plugin/scripts/master_plugin.py | 88 ++++++++- .../scripts/qt_master_wrapper.py | 170 ++++++++++++++++++ safety_master_plugin/scripts/restrictions.py | 8 + safety_master_plugin/scripts/robot_info.py | 30 ++++ .../scripts/ros_master_wrapper.py | 160 +++++++++++++++++ 6 files changed, 467 insertions(+), 5 deletions(-) create mode 100644 safety_master_plugin/scripts/master_info.py create mode 100644 safety_master_plugin/scripts/qt_master_wrapper.py create mode 100644 safety_master_plugin/scripts/restrictions.py create mode 100644 safety_master_plugin/scripts/robot_info.py create mode 100644 safety_master_plugin/scripts/ros_master_wrapper.py diff --git a/safety_master_plugin/scripts/master_info.py b/safety_master_plugin/scripts/master_info.py new file mode 100644 index 0000000..d610292 --- /dev/null +++ b/safety_master_plugin/scripts/master_info.py @@ -0,0 +1,16 @@ +#!/usr/bin/env python + +# from robot_info import RobotInfo + +class MasterInfo: + + def __init__(self): + self.robots_id_list = [] + self.master_stop_state = None + self.restrictions = None + + def get_master_stop_state(self): + return self.master_stop_state + + def get_restrictions(self): + return self.restrictions \ No newline at end of file diff --git a/safety_master_plugin/scripts/master_plugin.py b/safety_master_plugin/scripts/master_plugin.py index 6b23abc..3a0f01a 100755 --- a/safety_master_plugin/scripts/master_plugin.py +++ b/safety_master_plugin/scripts/master_plugin.py @@ -1,8 +1,13 @@ +from ros_master_wrapper import ROSWrapper +from qt_master_wrapper import QtWrapper +from restrictions import Restrictions +from master_info import MasterInfo +from master_widget import MasterWidget from qt_gui.plugin import Plugin -from master_widget import MasterWidget +from enums.stop_state import StopState as SS class MasterPlugin(Plugin): @@ -11,12 +16,85 @@ class MasterPlugin(Plugin): self.setObjectName('Master Plugin - L1.5 safety system') self._widget = MasterWidget(context) - # self._qt_wrapper = QtWrapper(self._widget) - # self._ros_wrapper = ROSWrapper() + self._qt_wrapper = QtWrapper(self._widget) + + self._master_info = MasterInfo() - # self.set_callback_functions() + + # self._qt_wrapper.connect_signals() + # self._qt_wrapper.set_sliders_to_initial_values() + self._ros_wrapper = ROSWrapper() + # Setup functions to get _master_info states from _ros_wrapper + self._ros_wrapper.setup_get_master_stop_state_function(self._master_info.get_master_stop_state) + self._ros_wrapper.setup_get_restrictions_function(self._master_info.get_restrictions) + self.set_callback_functions() + self._qt_wrapper.connect_signals() + + # Stopping master at the beginning + self.master_stopped() def set_callback_functions(self): - raise NotImplementedError + self.set_ROS_callback_functions() + self.set_Qt_callback_functions() + + def set_ROS_callback_functions(self): + self._ros_wrapper.set_robots_list_update_callback(self.handle_robots_list_update) + self._ros_wrapper.set_robot_info_update_callback(self.handle_robot_info_update) + + def set_Qt_callback_functions(self): + self._qt_wrapper.set_master_stop_state_updated_callback(self.handle_master_stop_state_updated) + self._qt_wrapper.set_restrictions_updated_callback(self.handle_restrictions_updated) + + + def handle_robots_list_update(self,robots_id_list): + self.update_robots_list(robots_id_list) + + def handle_robot_info_update(self,robot_info): + self.update_robot_info(robot_info) + + def handle_master_stop_state_updated(self): + if self._master_info.master_stop_state == SS.RUNNING: + self.master_stopped() + elif self._master_info.master_stop_state == SS.STOPPED: + self.master_started() + else: + raise ValueError('master_stop_state value undefined') + + def handle_restrictions_updated(self,restrictions): + self.restrictions_changed(restrictions) + + def update_robots_list(self,robots_id_list): + self._master_info.robots_id_list = robots_id_list + self._qt_wrapper.update_robots_list_gui(robots_id_list) + # raise NotImplementedError + + def new_robot_registered(self,robot_id): + raise NotImplementedError + # self._qt_wrapper.add_robot_subscriber(robot_id) + self._ros_wrapper.add_robot_subscriber(robot_id) + + def robot_unregistered(self,robot_id): + raise NotImplementedError + # self._qt_wrapper.remove_robot_subscriber(robot_id) + self._ros_wrapper.remove_robot_subscriber(robot_id) + + def update_robot_info(self,robot_info): + # TODO emit signal to qt_wrapper where it will update info about one of robots + self._qt_wrapper.update_robot_info(self,robot_info) + raise NotImplementedError + + def master_started(self): + self._master_info.master_stop_state = SS.RUNNING + self._ros_wrapper.master_started() + self._qt_wrapper.master_started() + + def master_stopped(self): + self._master_info.master_stop_state = SS.STOPPED + self._ros_wrapper.master_stopped() + self._qt_wrapper.master_stopped() + + def restrictions_changed(self,restrictions): + self._master_info.restrictions = restrictions + self._ros_wrapper.restrictions_changed(restrictions) \ No newline at end of file diff --git a/safety_master_plugin/scripts/qt_master_wrapper.py b/safety_master_plugin/scripts/qt_master_wrapper.py new file mode 100644 index 0000000..118bcc0 --- /dev/null +++ b/safety_master_plugin/scripts/qt_master_wrapper.py @@ -0,0 +1,170 @@ +#!/usr/bin/env python +import os +import rospy +import rospkg + +import math +import datetime + +from enums.clutch_state import ClutchState as CS +from enums.stop_state import StopState as SS + +from python_qt_binding.QtGui import QPixmap +from python_qt_binding.QtGui import QTextCursor +from python_qt_binding.QtWidgets import QMessageBox + + +class QtWrapper: + + def __init__(self,widget): + self.widget = widget + self.displayed_robots_id_list = [] + + self.disable_sliders_tracking() + # self.set_sliders_to_initial_values() + + self.master_stop_state = None + # self.ok_pixmap = QPixmap('/home/olek/safetysystemL1.5/src/SafetySystem/safety_user_plugin/ui/Ok.jpg') + # self.cancel_pixmap = QPixmap('/home/olek/safetysystemL1.5/src/SafetySystem/safety_user_plugin/ui/Cancel.jpg') + + def set_sliders_to_initial_values(self): + raise NotImplementedError + + def disable_sliders_tracking(self): + self.widget.distanceSlider.setTracking(False) + self.widget.angularSlider.setTracking(False) + self.widget.linearSlider.setTracking(False) + + + def set_master_stop_state_updated_callback(self,callback_function): + self.master_stop_state_updated_callback = callback_function + + def set_restrictions_updated_callback(self,callback_function): + self.restrictions_updated_callback = callback_function + + def handle_restrictions_update(self): + raise NotImplementedError + # restrictions = Restrictions() + # Do something + # self.restrictions_updated_callback(restrictions) + + + def connect_signals(self): + # self.widget.distanceSlider.valueChanged.connect(self.handle_restrictions_update) + # self.widget.angularSlider.valueChanged.connect(self.handle_restrictions_update) + # self.widget.linearSlider.valueChanged.connect(self.handle_restrictions_update) + self.widget.masterstopButton.clicked.connect(self.handle_masterstopButton_clicked) + + # def handle_emitted_signal(self,method_name): + # exec('self.{0}()'.format(method_name)) + + # def handle_emitted_signal_with_list_argument(self,method_name,argument): + # if method_name == 'update_selected_robot_info': + # self.update_selected_robot_info(argument[0]) + + # else: + # method_with_argument = 'self.{0}({1})'.format(method_name,argument[0]) + # exec(method_with_argument) + + def handle_masterstopButton_clicked(self): + if self.master_stop_state == SS.RUNNING: + self.master_stop_state_updated_callback() + else: + reply = QMessageBox.warning(self.widget,"UWAGA","Na pewno chcesz odblokowac wszystkie roboty?",QMessageBox.Yes,QMessageBox.No) + + if reply == QMessageBox.Yes: + 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 update_robots_list_gui(self,robots_id_list): + robots_id_list.sort() + id_strings_list = (('PIONIER'+str(x)) for x in robots_id_list) + + robots_to_add = [] + robots_to_remove = [] + + for robot_id in robots_id_list: + if robot_id not in self.displayed_robots_id_list: + robots_to_add.append(robot_id) + + for robot_id in self.displayed_robots_id_list: + if robot_id not in robots_id_list: + robots_to_remove.append(robot_id) + + for robot_id in robots_to_remove: + self.remove_robot_from_list(robot_id) + + for robot_id in robots_to_add: + self.add_robot_to_list(robot_id) + + + # def remove_robot_from_list(self,robot_id): + # count = self.widget.robotsList.count() + + # for i in range(count): + # if str(robot_id) in self.widget.robotsList.itemText(i): + # self.widget.robotsList.removeItem(i) + # self.displayed_robots_id_list.remove(robot_id) + # return + + + # def add_robot_to_list(self,robot_id): + # self.widget.robotsList.addItem('PIONIER'+str(robot_id)) + # self.displayed_robots_id_list.append(robot_id) + + + def master_stopped(self): + self.master_stop_state = SS.STOPPED + self.display_master_stop_off() + self.log_info('Przycisk masterSTOP zostal nacisniety. Zatrzymuje roboty') + + def master_started(self): + self.master_stop_state = SS.RUNNING + self.display_master_stop_on() + self.log_info('Przycisk masterSTOP odcisniety') + + + def log_info(self,info_text): + time = datetime.datetime.now().strftime('[%H:%M:%S]') + + cursor = self.widget.logsBrowser.textCursor() + cursor.movePosition(QTextCursor.End) + self.widget.logsBrowser.setTextCursor(cursor) + self.widget.logsBrowser.insertHtml('' + str(time) + '. ' + info_text + '
') + self.scroll_to_bottom() + # self.widget.logsBrowser.insertHtml(str(self.logger_counter) + '\t[INFO]\t' + info_text) + + def log_warning(self,warning_text): + time = datetime.datetime.now().strftime('[%H:%M:%S]') + + cursor = self.widget.logsBrowser.textCursor() + cursor.movePosition(QTextCursor.End) + self.widget.logsBrowser.setTextCursor(cursor) + self.widget.logsBrowser.textCursor().movePosition(QTextCursor.End) + self.widget.logsBrowser.insertHtml('' + str(time) + '. ' + warning_text + '
') + self.scroll_to_bottom() + # self.widget.logsBrowser.append(str(self.logger_counter) + '\t[WARN]\t' + warning_text) + + def log_error(self,error_text): + time = datetime.datetime.now().strftime('[%H:%M:%S]') + + cursor = self.widget.logsBrowser.textCursor() + cursor.movePosition(QTextCursor.End) + self.widget.logsBrowser.setTextCursor(cursor) + self.widget.logsBrowser.textCursor().movePosition(QTextCursor.End) + self.widget.logsBrowser.insertHtml('' + str(time) + '. ' + error_text + '
') + self.scroll_to_bottom() + # self.widget.logsBrowser.append(str(self.logger_counter) + '\t[ERROR]\t' + error_text) + + def scroll_to_bottom(self): + scrollbar = self.widget.logsBrowser.verticalScrollBar() + scrollbar.setValue(scrollbar.maximum()) \ No newline at end of file diff --git a/safety_master_plugin/scripts/restrictions.py b/safety_master_plugin/scripts/restrictions.py new file mode 100644 index 0000000..bea4082 --- /dev/null +++ b/safety_master_plugin/scripts/restrictions.py @@ -0,0 +1,8 @@ +#!/usr/bin/env python + +class Restrictions: + + def __init__(self,restrictions_msg): + self.distance = restrictions_msg.distance.data + self.linear_velocity = restrictions_msg.linear_velocity.data + self.angular_velocity = restrictions_msg.angular_velocity.data \ No newline at end of file diff --git a/safety_master_plugin/scripts/robot_info.py b/safety_master_plugin/scripts/robot_info.py new file mode 100644 index 0000000..4f50b36 --- /dev/null +++ b/safety_master_plugin/scripts/robot_info.py @@ -0,0 +1,30 @@ +#!/usr/bin/env python + +from enums.clutch_state import ClutchState as CS +from enums.stop_state import StopState as SS + +class RobotInfo: + + def __init__(self,robot_id): + self.robot_id = robot_id + self.battery_voltage = None + self.linear_velocity = None + self.angular_velocity = None + self.state = None + self.clutch = None + + def update_robot_info(self,robot_info_msg): + self.robot_id = robot_info_msg.robot_id.data + self.battery_voltage = robot_info_msg.battery_voltage.data + self.linear_velocity = [robot_info_msg.twist.linear.x,robot_info_msg.twist.linear.y,robot_info_msg.twist.linear.z] + self.angular_velocity = [robot_info_msg.twist.angular.x,robot_info_msg.twist.angular.y,robot_info_msg.twist.angular.z] + + if robot_info_msg.state.data == True: + self.state = SS.RUNNING + else: + self.state = SS.STOPPED + + if robot_info_msg.clutch.data == True: + self.clutch = CS.ENGAGED + else: + self.clutch = CS.DISENGAGED \ No newline at end of file diff --git a/safety_master_plugin/scripts/ros_master_wrapper.py b/safety_master_plugin/scripts/ros_master_wrapper.py new file mode 100644 index 0000000..77625ad --- /dev/null +++ b/safety_master_plugin/scripts/ros_master_wrapper.py @@ -0,0 +1,160 @@ +#!/usr/bin/env python +import rospy +import rospkg + +from std_msgs.msg import Bool +from safety_user_plugin.msg import RobotInfoMsg +from safety_user_plugin.msg import RestrictionsMsg + +from robot_info import RobotInfo +from restrictions import Restrictions + +from enums.stop_state import StopState as SS + +class ROSWrapper: + + def __init__(self): + self.robots_list_update_callback = None + self.robot_info_update_callback = None + + self.robot_info_subscribers = {} + self.master_stop_publisher = None + self.restrictions_publisher = None + + self.robots_list_timer = None + self.periodic_timer = None + + self.setup_subscribers_and_publishers() + # def setup_node(self): + # rospy.init_node('safety_user_plugin', anonymous=True) + + def setup_subscribers_and_publishers(self): + self.master_stop_publisher = rospy.Publisher('/RosAria/master_stop',Bool,queue_size = 1) + self.restrictions_publisher = rospy.Publisher('/RosAria/restrictions',RestrictionsMsg,queue_size = 1) + + # self.periodic_timer = rospy.Timer(rospy.Duration(0.5),self.publish_periodic_update) + + def add_robot_subscriber(self,robot_id): + if robot_id not in self.robot_info_subscribers: + topic_name = '/RosAria/PIONIER{0}/robot_info'.format(robot_id) + new_sub = rospy.Subscriber(topic_name, RobotInfoMsg, self.handle_robot_info_update) + self.robot_info_subscribers[robot_id] = new_sub + else: + raise RuntimeError('Subscriber for PIONIER{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 unsubscribe(self,subscriber): + if subscriber != None: + subscriber.unregister() + + def unregister_publisher(self,publisher): + if publisher != None: + publisher.unregister() + + def shutdown_timer(self,timer): + if timer != None: + timer.shutdown() + + def publish_periodic_update(self,event): + restrictions = self.get_restrictions() + restrictions_msg = RestrictionsMsg() + restrictions_msg.distance.data = restrictions.distance + restrictions_msg.linear_velocity.data = restrictions.linear_velocity + restrictions_msg.angular_velocity.data = restrictions.angular_velocity + + stop_state = self.get_master_stop_state() + stop_msg = Bool() + + if stop_state == SS.RUNNING: + stop_msg.data = True + elif stop_state == SS.STOPPED: + stop_msg.data = False + else: + stop_msg.data = False + raise ValueError('stop_state UNKNOWN when attempting to publish periodic update') + + self.restrictions_publisher.publish(restrictions_msg) + self.master_stop_publisher.publish(stop_msg) + + def cancel_subscribers_and_publishers(self): + self.shutdown_timer(self.robots_list_timer) + self.shutdown_timer(self.periodic_timer) + + self.unregister_publisher(self.master_stop_publisher) + self.unregister_publisher(self.restrictions_publisher) + + for key, value in self.robot_info_subscribers.iteritems(): + self.unsubscribe(value) + + def unregister_node(self): + self.cancel_subscribers_and_publishers() + rospy.signal_shutdown('Closing safety master plugin') + + def setup_get_master_stop_state_function(self,function): + self.get_master_stop_state = function + + def setup_get_restrictions_function(self,function): + self.get_restrictions = function + + # ROSWrapper Callbacks + def get_robots_list(self,event): + robots_id_list = [] + + published_topics_list = rospy.get_published_topics(namespace='/') + published_topics = [] + + for list_ in published_topics_list: + published_topics.append(list_[0]) + + for topic in published_topics: + if topic.find('RosAria') ==-1 or topic.find('robot_info') == -1: + pass + else: + robot_number = topic.split('/') + robot_number = robot_number[2] + robot_number = robot_number[7:] + if len(robot_number) > 0: + robot_number = int(robot_number) + robots_id_list.append(robot_number) + + self.robots_list_update_callback(robots_id_list) + + + def handle_robot_info_update(self,msg): + _robot_info = RobotInfo(0) + _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 + self.robots_list_timer = rospy.Timer(rospy.Duration(0.5),self.get_robots_list) + + 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): + msg = Bool() + msg.data = False + self.master_stop_publisher.publish(msg) + + def restrictions_changed(self,restrictions): + msg = RestrictionsMsg() + + msg.distance.data = restrictions.distance + msg.linear_velocity.data = restrictions.linear_velocity + msg.angular_velocity.data = restrictions.angular_velocity + + self.restrictions_publisher.publish(msg) \ No newline at end of file