diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..7108bca --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,201 @@ +cmake_minimum_required(VERSION 2.8.3) +project(safety_master_plugin) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + rospy + rqt_gui + rqt_gui_py + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES safety_master_plugin +# CATKIN_DEPENDS rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/safety_master_plugin.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/safety_master_plugin_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +# add_dependencies(safety_user_plugin ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +install(PROGRAMS +# scripts/my_python_script + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_safety_master_plugin.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/launch/master_plugin.launch b/launch/master_plugin.launch new file mode 100644 index 0000000..eac49d8 --- /dev/null +++ b/launch/master_plugin.launch @@ -0,0 +1,7 @@ + + + \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..a4f058c --- /dev/null +++ b/package.xml @@ -0,0 +1,35 @@ + + + safety_master_plugin + 0.0.0 + The safety_master_plugin package + + Aleksander Bojda + + MIT + + catkin + + rospy + rqt_gui + rqt_gui_py + std_msgs + rosaria_msgs + + rospy + rqt_gui + rqt_gui_py + std_msgs + rosaria_msgs + + rospy + rqt_gui + rqt_gui_py + std_msgs + message_runtime + rosaria_msgs + + + + + diff --git a/plugin.xml b/plugin.xml new file mode 100644 index 0000000..b4423a5 --- /dev/null +++ b/plugin.xml @@ -0,0 +1,15 @@ + + + + Safety system master plugin + + + + + folder + + + system-help + + + \ No newline at end of file diff --git a/scripts/enums/__init__.py b/scripts/enums/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/scripts/enums/clutch_state.py b/scripts/enums/clutch_state.py new file mode 100644 index 0000000..9479b62 --- /dev/null +++ b/scripts/enums/clutch_state.py @@ -0,0 +1,6 @@ + + +class ClutchState: + ENGAGED = 4 + DISENGAGED = 5 + UNKNOWN = 6 \ No newline at end of file diff --git a/scripts/enums/stop_state.py b/scripts/enums/stop_state.py new file mode 100644 index 0000000..17d5939 --- /dev/null +++ b/scripts/enums/stop_state.py @@ -0,0 +1,6 @@ + + +class StopState: + STOPPED = 1 + RUNNING = 2 + UNKNOWN = 3 \ No newline at end of file diff --git a/scripts/master_callback_handler.py b/scripts/master_callback_handler.py new file mode 100644 index 0000000..c616d5d --- /dev/null +++ b/scripts/master_callback_handler.py @@ -0,0 +1,21 @@ +from python_qt_binding.QtCore import QObject +from python_qt_binding.QtCore import pyqtSignal +from robot_info import RobotInfo + +class CallbackHandler(QObject): + robot_info_signal = pyqtSignal(object) + add_robot_signal = pyqtSignal(int) + remove_robot_signal = pyqtSignal(int) + + + def __init__(self): + super(CallbackHandler, self).__init__() + + def connect_robot_info_signal(self,slot): + self.robot_info_signal.connect(slot) + + def connect_add_robot_signal(self,slot): + self.add_robot_signal.connect(slot) + + def connect_remove_robot_signal(self,slot): + self.remove_robot_signal.connect(slot) \ No newline at end of file diff --git a/scripts/master_info.py b/scripts/master_info.py new file mode 100644 index 0000000..d610292 --- /dev/null +++ b/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/scripts/master_plugin.py b/scripts/master_plugin.py new file mode 100755 index 0000000..92f9160 --- /dev/null +++ b/scripts/master_plugin.py @@ -0,0 +1,110 @@ +from ros_master_wrapper import ROSWrapper +from qt_master_wrapper import QtWrapper +from master_restrictions import Restrictions + +from master_info import MasterInfo +from master_widget import MasterWidget + +from qt_gui.plugin import Plugin + +from enums.stop_state import StopState as SS + +from master_callback_handler import CallbackHandler + +class MasterPlugin(Plugin): + + def __init__(self,context): + super(MasterPlugin, self).__init__(context) + self.setObjectName('Master Plugin - L1.5 safety system') + + self._widget = MasterWidget(context) + self._ros_wrapper = ROSWrapper() + self._qt_wrapper = QtWrapper(self._widget) + self._master_info = MasterInfo() + + self.cbhandler = CallbackHandler() + self.cbhandler.connect_robot_info_signal(self._qt_wrapper.update_robot_info) + self.cbhandler.connect_add_robot_signal(self._qt_wrapper.add_robot) + self.cbhandler.connect_remove_robot_signal(self._qt_wrapper.remove_robot) + + + # self._qt_wrapper.connect_signals() + # self._qt_wrapper.set_sliders_to_initial_values() + + + # 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): + 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): + + for robot_id in robots_id_list: + if robot_id not in self._master_info.robots_id_list: + self.new_robot_registered(robot_id) + + for robot_id in self._master_info.robots_id_list: + if robot_id not in robots_id_list: + self.robot_unregistered(robot_id) + + self._master_info.robots_id_list = robots_id_list + + def new_robot_registered(self,robot_id): + self._ros_wrapper.add_robot_subscriber(robot_id) + self.cbhandler.add_robot_signal.emit(robot_id) + + def robot_unregistered(self,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): + 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/scripts/master_restrictions.py b/scripts/master_restrictions.py new file mode 100644 index 0000000..0715601 --- /dev/null +++ b/scripts/master_restrictions.py @@ -0,0 +1,13 @@ +#!/usr/bin/env python + +class Restrictions: + + def __init__(self,distance,angular_velocity,linear_velocity): + self.distance = distance + self.angular_velocity = angular_velocity + self.linear_velocity = linear_velocity + + def update_restrictions(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/scripts/master_widget.py b/scripts/master_widget.py new file mode 100644 index 0000000..ffa07f5 --- /dev/null +++ b/scripts/master_widget.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python +import os +import rospkg + +from python_qt_binding.QtWidgets import QWidget +from python_qt_binding import loadUi + +class MasterWidget(QWidget): + + def __init__(self,context): + super(MasterWidget, self).__init__() + ui_file = os.path.join(rospkg.RosPack().get_path('safety_master_plugin'), 'ui', 'master_view.ui') + loadUi(ui_file, self) + self.setObjectName('Master Plugin - L1.5 safety system') + + if context.serial_number() > 1: + self.setWindowTitle(self.windowTitle() + (' (%d)' % context.serial_number())) + context.add_widget(self) \ No newline at end of file diff --git a/scripts/qt_master_wrapper.py b/scripts/qt_master_wrapper.py new file mode 100644 index 0000000..a97dde9 --- /dev/null +++ b/scripts/qt_master_wrapper.py @@ -0,0 +1,243 @@ +#!/usr/bin/env python +# coding=utf-8 +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 master_restrictions import Restrictions + +from python_qt_binding.QtGui import QPixmap +from python_qt_binding.QtGui import QTextCursor +from python_qt_binding.QtGui import QStandardItem +from python_qt_binding.QtGui import QStandardItemModel +from python_qt_binding.QtGui import QBrush +from python_qt_binding.QtWidgets import QMessageBox +from python_qt_binding.QtWidgets import QHeaderView +from python_qt_binding.QtWidgets import QAbstractItemView +from python_qt_binding.QtCore import Qt + + +class QtWrapper: + + def __init__(self,widget): + self.widget = widget + self.displayed_robots_id_list = [] + + self.master_stop_state = None + self.init_robots_list() + + # self.disable_sliders_tracking() + + ui_directory_path = '{0}/ui'.format(rospkg.RosPack().get_path('safety_master_plugin')) + + self.distance_pixmap = QPixmap('{0}/Distance.png'.format(ui_directory_path)) + self.angular_pixmap = QPixmap('{0}/Angular.png'.format(ui_directory_path)) + self.linear_pixmap = QPixmap('{0}/Linear.png'.format(ui_directory_path)) + self.widget.distanceImage.setPixmap(self.distance_pixmap) + self.widget.angularImage.setPixmap(self.angular_pixmap) + self.widget.linearImage.setPixmap(self.linear_pixmap) + + def set_sliders_to_initial_values(self): + self.widget.distanceSlider.setMaximum(200) + self.widget.angularSlider.setMaximum(200) + self.widget.linearSlider.setMaximum(200) + + self.widget.distanceSlider.setValue(75) + self.widget.angularSlider.setValue(100) + self.widget.linearSlider.setValue(100) + + def init_robots_list(self): + self.stdItemModel = QStandardItemModel( 0, 6, self.widget.robotsList) + self.stdItemModel.setHeaderData( 0, Qt.Horizontal, 'Robot' ) + self.stdItemModel.setHeaderData( 1, Qt.Horizontal, 'Stan baterii' ) + self.stdItemModel.setHeaderData( 2, Qt.Horizontal, 'Stan robota' ) + self.stdItemModel.setHeaderData( 3, Qt.Horizontal, 'Stan sprzęgła' ) + self.stdItemModel.setHeaderData( 4, Qt.Horizontal, 'Prędk. lin.' ) + self.stdItemModel.setHeaderData( 5, Qt.Horizontal, 'Prędk. obr.' ) + + self.widget.robotsList.setModel(self.stdItemModel) + + self.widget.robotsList.horizontalHeader().setSectionResizeMode(0, QHeaderView.Stretch) + self.widget.robotsList.horizontalHeader().setSectionResizeMode(1, QHeaderView.Stretch) + self.widget.robotsList.horizontalHeader().setSectionResizeMode(2, QHeaderView.Stretch) + self.widget.robotsList.horizontalHeader().setSectionResizeMode(3, QHeaderView.Stretch) + self.widget.robotsList.horizontalHeader().setSectionResizeMode(4, QHeaderView.Stretch) + self.widget.robotsList.horizontalHeader().setSectionResizeMode(5, QHeaderView.Stretch) + + self.widget.robotsList.horizontalHeader().setSectionsMovable(False) + # self.widget.robotsList.setSelectionBehavior(QAbstractItemView.SelectRows) + self.widget.robotsList.setSelectionMode(QAbstractItemView.NoSelection) + + + 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): + distance = (float(self.widget.distanceSlider.value())/100) + angular_velocity = (float(self.widget.angularSlider.value())/100) + linear_velocity = (float(self.widget.linearSlider.value())/100) + + self.widget.distanceLabel.setText("{:.2f}".format(distance)) + self.widget.angularLabel.setText("{:.2f}".format(angular_velocity)) + self.widget.linearLabel.setText("{:.2f}".format(linear_velocity)) + + restrictions = Restrictions(distance,angular_velocity,linear_velocity) + + 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) + + self.set_sliders_to_initial_values() + + 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 odblokować 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_robot_info(self,robot_info): + items_to_add = [] + items_to_add.append(QStandardItem('PIONIER{}'.format(robot_info.robot_id))) + items_to_add.append(QStandardItem('{:.2f}'.format(robot_info.battery_voltage))) + + if robot_info.state == SS.RUNNING: + item = QStandardItem('Działa') + item.setBackground(QBrush(Qt.green)) + items_to_add.append(item) + elif robot_info.state == SS.STOPPED: + item = QStandardItem('Zatrzymany') + item.setBackground(QBrush(Qt.red)) + items_to_add.append(item) + else: + raise ValueError('Undefined value of robot_info.robot_state') + + if robot_info.clutch == CS.ENGAGED: + item = QStandardItem('Sprzęgnięte') + item.setBackground(QBrush(Qt.green)) + items_to_add.append(item) + elif robot_info.clutch == CS.DISENGAGED: + item = QStandardItem('Rozprzęgnięte') + item.setBackground(QBrush(Qt.red)) + items_to_add.append(item) + else: + raise ValueError('Undefined value of robot_info.clutch_state') + + linear_vel = math.sqrt(robot_info.linear_velocity[0]**2 + robot_info.linear_velocity[1]**2) + items_to_add.append(QStandardItem('{:.2f}'.format(linear_vel))) + + items_to_add.append(QStandardItem('{:.2f}'.format(robot_info.angular_velocity[2]))) + + + items_list = self.stdItemModel.findItems('PIONIER{0}'.format(robot_info.robot_id),Qt.MatchExactly) + if len(items_list) > 0: + for item in items_list: + row_number = item.row() + else: + row_number = self.stdItemModel.rowCount() + + for i,item in enumerate(items_to_add): + item.setTextAlignment(Qt.AlignCenter) + item.setEditable(0) + self.stdItemModel.setItem(row_number,i,item) + + def master_stopped(self): + self.master_stop_state = SS.STOPPED + self.display_master_stop_off() + self.log_info('Przycisk masterSTOP zostal naciśnięty. Zatrzymuję roboty') + + def master_started(self): + self.master_stop_state = SS.RUNNING + self.display_master_stop_on() + self.log_info('Przycisk masterSTOP odciśnięty') + + + def add_robot(self,robot_id): + + if robot_id not in self.displayed_robots_id_list: + new_robot_info=QStandardItem('PIONIER{}'.format(robot_id)) + new_robot_info.setTextAlignment(Qt.AlignCenter) + new_robot_info.setEditable(0) + # new_robot_info.setBackground(QBrush(Qt.white)) + self.stdItemModel.setItem(self.stdItemModel.rowCount(),0,new_robot_info) + + self.displayed_robots_id_list.append(robot_id) + self.log_info('PIONIER{0} połączony'.format(robot_id)) + else: + raise RuntimeError('Adding robot, which is already in master GUI') + + def remove_robot(self,robot_id): + items_list = self.stdItemModel.findItems('PIONIER{0}'.format(robot_id),Qt.MatchExactly) + + if len(items_list) > 0: + for item in items_list: + row_number = item.row() + self.stdItemModel.removeRow(row_number) + + self.log_info('PIONIER{0} rozłączony'.format(robot_id)) + self.displayed_robots_id_list.remove(robot_id) + + 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.widget.logsBrowser.insertHtml(' {0} {1}
'.format(time,info_text)) + self.scroll_to_bottom() + + 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(' {0} {1}
'.format(time,warning_text)) + self.scroll_to_bottom() + + 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(' {0} {1}
'.format(time,error_text)) + self.scroll_to_bottom() + + def scroll_to_bottom(self): + scrollbar = self.widget.logsBrowser.verticalScrollBar() + scrollbar.setValue(scrollbar.maximum()) diff --git a/scripts/robot_info.py b/scripts/robot_info.py new file mode 100644 index 0000000..4f50b36 --- /dev/null +++ b/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/scripts/ros_master_wrapper.py b/scripts/ros_master_wrapper.py new file mode 100644 index 0000000..0a51d8a --- /dev/null +++ b/scripts/ros_master_wrapper.py @@ -0,0 +1,159 @@ +#!/usr/bin/env python +import rospy +import rospkg + +from std_msgs.msg import Bool +from rosaria_msgs.msg import RobotInfoMsg +from rosaria_msgs.msg import RestrictionsMsg + +from robot_info import RobotInfo +from master_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_subscribers_and_publishers(self): + self.master_stop_publisher = rospy.Publisher('/PIONIER/master_stop',Bool,queue_size = 1) + self.restrictions_publisher = rospy.Publisher('/PIONIER/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 = '/PIONIER{0}/RosAria/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[1] + 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 diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..25facb1 --- /dev/null +++ b/setup.py @@ -0,0 +1,8 @@ +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +setup_args = generate_distutils_setup( + package_dir={'': 'scripts'} +) + +setup(**setup_args) \ No newline at end of file diff --git a/ui/Angular.png b/ui/Angular.png new file mode 100644 index 0000000..6b85fc6 Binary files /dev/null and b/ui/Angular.png differ diff --git a/ui/Distance.png b/ui/Distance.png new file mode 100644 index 0000000..c7c7a08 Binary files /dev/null and b/ui/Distance.png differ diff --git a/ui/Linear.png b/ui/Linear.png new file mode 100644 index 0000000..e2b6a3f Binary files /dev/null and b/ui/Linear.png differ diff --git a/ui/master_view.ui b/ui/master_view.ui new file mode 100644 index 0000000..40440a5 --- /dev/null +++ b/ui/master_view.ui @@ -0,0 +1,210 @@ + + + Form + + + + 0 + 0 + 715 + 569 + + + + Form + + + + + + Qt::ElideMiddle + + + false + + + + + + + Qt::Vertical + + + + 20 + 25 + + + + + + + + + + + + Ograniczenia robotów + + + Qt::AlignCenter + + + + + + + Qt::Vertical + + + QSizePolicy::Fixed + + + + 20 + 10 + + + + + + + + + + + + Obrazek1 + + + Qt::AlignCenter + + + + + + + Qt::Vertical + + + + + + + 0.45 + + + Qt::AlignCenter + + + + + + + + + + + Obrazek2 + + + Qt::AlignCenter + + + + + + + Qt::Vertical + + + + + + + 0.67 + + + Qt::AlignCenter + + + + + + + + + + + Obrazek3 + + + Qt::AlignCenter + + + + + + + Qt::Vertical + + + + + + + 0.45 + + + Qt::AlignCenter + + + + + + + + + + + + + + 0 + 0 + + + + + 0 + 80 + + + + STOP + + + + + + + + + Qt::Vertical + + + + 20 + 25 + + + + + + + + + + + +