diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..d193766 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,204 @@ +cmake_minimum_required(VERSION 2.8.3) +project(safety_user_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 + geometry_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 +# RestrictionsMsg.msg +# RobotInfoMsg.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 +# geometry_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( + CATKIN_DEPENDS message_runtime +# 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}) + +## 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/user_plugin.launch b/launch/user_plugin.launch new file mode 100644 index 0000000..4a14bf8 --- /dev/null +++ b/launch/user_plugin.launch @@ -0,0 +1,15 @@ + + + + + + + \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..6255728 --- /dev/null +++ b/package.xml @@ -0,0 +1,35 @@ + + + safety_user_plugin + 0.0.0 + The safety_user_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..0b2b84d --- /dev/null +++ b/plugin.xml @@ -0,0 +1,15 @@ + + + + Safety system user plugin + + + + + folder + + + system-help + + + \ No newline at end of file diff --git a/scripts/english.py b/scripts/english.py new file mode 100644 index 0000000..e8af3d3 --- /dev/null +++ b/scripts/english.py @@ -0,0 +1,65 @@ +#!/usr/bin/env python +# coding=utf-8 + +from language import LanguageTexts + +class EnglishTexts(LanguageTexts): + + # Warning popup for robot unlocking + warning_text_1 = "ATTENTION" + warning_text_2 = "Are You sure You want to unlock the robot?" + + # Button texts + select_text = 'Select' + + stop_robot_text = "STOP robot" + unlock_robot_text = "UNLOCK robot" + + disengage_clutch_text = "Disengage clutch" + engage_clutch_text = "Engage clutch" + + release_robot_text = 'Release robot' + + + # Logger info texts + robot_selected_text = 'PIONIER{0} selected!' + robot_released_text = 'Robot released' + stopped_and_engaged_text = "Robot will be stopped and clutch engaged" + + master_stop_clicked_text = 'MasterSTOP button pressed. Stopping the robots' + master_stop_released_text = 'MasterSTOP button released. Robot can be unlocked' + + robot_stopped_text = 'Robot stopped' + robot_started_text = 'Robot started' + + clutch_disengaged_text = 'Disengaging the clutch' + clutch_engaged_text = 'Engaging the clutch' + + obstacle_detected_text = 'Obstacle detected. Robot stopped' + obstacle_removed_text = 'Obstacle removed' + + + # Logger error/problem messages + selection_error_text = 'Select the PIONEER from robots list first' + cannot_start_masterstop_text = "Robot can't be started. MasterSTOP pressed!" + cannot_start_obstacle_text = "Robot can't be started. Obstacle detected!" + 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_master_lost_text = 'Connection lost with masterSTOP. Ask the teacher to run it' + + + # Description labels + linear_text = '

Linear velocity

' + angular_text = '

Angular velocity

' + id_text = '

Robot ID

' + battery_text = '

Battery state

' + masterstop_text = '

Master Stop

' + obstacle_text = '

Obstacle Stop

' + robot_text = '

Robot state

' + restrictions_text = '

Restrictions

' + history_text = '

Message history

' + + + + def __init__(self): + pass \ 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/language.py b/scripts/language.py new file mode 100644 index 0000000..63aceb6 --- /dev/null +++ b/scripts/language.py @@ -0,0 +1,61 @@ + +class LanguageTexts: + + # Warning popup for robot unlocking + warning_text_1 = "Text not reimplemented!" + warning_text_2 = "Text not reimplemented!" + + # Button texts + select_text = "Text not reimplemented!" + + stop_robot_text = "Text not reimplemented!" + unlock_robot_text = "Text not reimplemented!" + + disengage_clutch_text = "Text not reimplemented!" + engage_clutch_text = "Text not reimplemented!" + + release_robot_text = "Text not reimplemented!" + + + # Logger info texts + robot_selected_text = "Text not reimplemented!" + robot_released_text = "Text not reimplemented!" + stopped_and_engaged_text = "Text not reimplemented!" + + master_stop_clicked_text = "Text not reimplemented!" + master_stop_released_text = "Text not reimplemented!" + + robot_stopped_text = "Text not reimplemented!" + robot_started_text = "Text not reimplemented!" + + clutch_disengaged_text = "Text not reimplemented!" + clutch_engaged_text = "Text not reimplemented!" + + obstacle_detected_text = "Text not reimplemented!" + obstacle_removed_text = "Text not reimplemented!" + + + # Logger error/problem messages + selection_error_text = "Text not reimplemented!" + cannot_start_masterstop_text = "Text not reimplemented!" + cannot_start_obstacle_text = "Text not reimplemented!" + cannot_select_robot_text = "Text not reimplemented!" + connection_to_robot_lost_text = "Text not reimplemented!" + connection_to_master_lost_text = "Text not reimplemented!" + + + # Description labels + angular_text = "Text not reimplemented!" + linear_text = "Text not reimplemented!" + id_text = "Text not reimplemented!" + battery_text = "Text not reimplemented!" + masterstop_text = "Text not reimplemented!" + obstacle_text = "Text not reimplemented!" + robot_text = "Text not reimplemented!" + restrictions_text = "Text not reimplemented!" + history_text = "Text not reimplemented!" + + + + def __init__(self): + pass \ No newline at end of file diff --git a/scripts/polish.py b/scripts/polish.py new file mode 100644 index 0000000..2348d57 --- /dev/null +++ b/scripts/polish.py @@ -0,0 +1,63 @@ +#!/usr/bin/env python +# coding=utf-8 + +from language import LanguageTexts + +class PolishTexts(LanguageTexts): + + # Warning popup for robot unlocking + warning_text_1 = "UWAGA" + warning_text_2 = "Na pewno chcesz odblokować robota?" + + # Button texts + select_text = 'Wybierz' + + stop_robot_text = "Zatrzymaj robota" + unlock_robot_text = "Odblokuj robota" + + disengage_clutch_text = "Rozłącz sprzęgło" + engage_clutch_text = "Połącz sprzęgło" + + release_robot_text = 'Zwolnij robota' + + + # Logger info texts + robot_selected_text = 'PIONIER{0} wybrany!' + robot_released_text = 'Odłączam robota' + stopped_and_engaged_text = "Robot zostanie zatrzymany i sprzęgnięty" + + master_stop_clicked_text = 'Przycisk masterSTOP został naciśnięty. Zatrzymuje roboty' + master_stop_released_text = 'Przycisk masterSTOP odciśnięty. Mozesz uruchomić robota' + + robot_stopped_text = 'Robot zatrzymany' + robot_started_text = 'Robot wystartował' + + clutch_disengaged_text = 'Rozprzęgam sprzęgło' + clutch_engaged_text = 'Sprzęgam sprzegło' + + obstacle_detected_text = 'Przeszkoda wykryta. Zatrzymuję robota' + obstacle_removed_text = 'Przeszkoda usunięta' + + + # Logger error/problem messages + selection_error_text = 'Najpierw wybierz PIONIERA 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' + 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_master_lost_text = 'Utrata połączenia z masterstopem. Poproś prowadzącego o jego uruchomienie' + + + # Description labels + linear_text = '

Prędkość liniowa

' + angular_text = '

Prędkość obrotowa

' + id_text = '

ID robota

' + battery_text = '

Stan baterii

' + masterstop_text = '

Stop mastera

' + obstacle_text = '

Stop przeszkody

' + robot_text = '

Stan robota

' + restrictions_text = '

Ograniczenia

' + history_text = '

Historia komunikatów

' + + def __init__(self): + pass \ No newline at end of file diff --git a/scripts/qt_wrapper.py b/scripts/qt_wrapper.py new file mode 100644 index 0000000..8d3faa0 --- /dev/null +++ b/scripts/qt_wrapper.py @@ -0,0 +1,338 @@ +#!/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 python_qt_binding.QtGui import QPixmap +from python_qt_binding.QtGui import QTextCursor +from python_qt_binding.QtWidgets import QMessageBox + +from polish import PolishTexts as PT +from english import EnglishTexts as ET + +class QtWrapper: + + def __init__(self,widget): + self.widget = widget + self.displayed_robots_id_list = [] + + lang = rospy.get_param('lang') + + if lang == 'eng': + self.language = ET + self.log_info('Started english version of plugin NEW') + elif lang == 'pol': + self.language = PT + self.log_info('Polska wersja pluginu uruchomiona') + else: + raise ValueError('language parameter has value different than "eng" or "pol"') + + self.clear_robot_info() + + ui_directory_path = '{0}/ui'.format(rospkg.RosPack().get_path('safety_user_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.scaled(66,46)) + self.widget.angularImage.setPixmap(self.angular_pixmap) + self.widget.linearImage.setPixmap(self.linear_pixmap.scaled(20,48)) + + self.ok_pixmap = QPixmap('{0}/Ok.jpg'.format(ui_directory_path)) + self.cancel_pixmap = QPixmap('{0}/Cancel.jpg'.format(ui_directory_path)) + + def disconnect_signals(self): + self.widget.clutchButton.clicked.disconnect() + self.widget.stopButton.clicked.disconnect() + self.widget.choiceButton.clicked.disconnect() + + def connect_robot_signals(self): + self.widget.clutchButton.clicked.connect(self.handle_clutchButton_clicked_callback) + self.widget.stopButton.clicked.connect(self.handle_stopButton_clicked) + self.widget.choiceButton.clicked.connect(self.handle_closeButton_clicked) + + def connect_signals(self): + self.widget.clutchButton.clicked.connect(self.handle_not_selected_error) + self.widget.stopButton.clicked.connect(self.handle_not_selected_error) + self.widget.choiceButton.clicked.connect(self.handle_openButton_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_stopButton_clicked(self): + if self.robot_state == SS.RUNNING: + self.handle_stopButton_clicked_callback() + else: + reply = QMessageBox.warning(self.widget,self.language.warning_text_1,self.language.warning_text_2,QMessageBox.Yes,QMessageBox.No) + + if reply == QMessageBox.Yes: + self.handle_stopButton_clicked_callback() + + + def get_selected_robot_id(self): + current_text = self.widget.robotsList.currentText() + + if 'PIONIER' in current_text: + return int(current_text[7:]) + else: + return None + + def display_clutch_on(self): + # self.widget.clutchLabel.setPixmap(self.ok_pixmap.scaled(40,40)) + self.widget.clutchButton.setStyleSheet("QPushButton { color: black; background-color: green; font: bold 20px}") + self.widget.clutchButton.setText(self.language.disengage_clutch_text) + + def display_clutch_off(self): + # self.widget.clutchLabel.setPixmap(self.cancel_pixmap.scaled(40,40)) + self.widget.clutchButton.setStyleSheet("QPushButton { color: black; background-color: red; font: bold 20px}") + self.widget.clutchButton.setText(self.language.engage_clutch_text) + + def display_state_on(self): + self.widget.stateLabel.setPixmap(self.ok_pixmap.scaled(40,40)) + self.widget.stopButton.setStyleSheet("QPushButton { color: black; background-color: green; font: bold 20px}") + self.widget.stopButton.setText(self.language.stop_robot_text) + self.robot_state = SS.RUNNING + + def display_state_off(self): + self.widget.stateLabel.setPixmap(self.cancel_pixmap.scaled(40,40)) + self.widget.stopButton.setStyleSheet("QPushButton { color: black; background-color: red; font: bold 20px}") + self.widget.stopButton.setText(self.language.unlock_robot_text) + self.robot_state = SS.STOPPED + + def handle_not_selected_error(self): + self.log_error(self.language.selection_error_text) + + def handle_openButton_clicked(self): + selected_robot_id = self.get_selected_robot_id() + + if selected_robot_id != None: + self.handle_openButton_clicked_callback(selected_robot_id) + else: + self.log_error(self.language.selection_error_text) + + def handle_closeButton_clicked(self): + self.handle_closeButton_clicked_callback() + + def set_robot_selection_callback(self,callback): + self.handle_openButton_clicked_callback = callback + + def set_robot_release_callback(self,callback): + self.handle_closeButton_clicked_callback = callback + + def set_user_stop_state_updated_callback(self,callback): + self.handle_stopButton_clicked_callback = callback + + def set_clutch_switched_callback(self,callback): + self.handle_clutchButton_clicked_callback = callback + + def select_robot(self,robot_id): + self.disconnect_signals() + self.connect_robot_signals() + self.log_info(self.language.robot_selected_text.format(robot_id)) + self.log_info(self.language.stopped_and_engaged_text) + + self.widget.choiceButton.setText(self.language.release_robot_text) + + def release_robot(self): + self.disconnect_signals() + self.connect_signals() + self.log_info(self.language.robot_released_text) + self.clear_robot_info() + + 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{0}'.format(robot_id)) + self.displayed_robots_id_list.append(robot_id) + + + def update_selected_robot_info(self,robot_info): + linear_vel = math.sqrt(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.batteryLabel.setText("{:.2f}".format(robot_info.battery_voltage)) + self.widget.linearLabel.setText("{:.2f}".format(linear_vel)) + self.widget.angularLabel.setText("{:.2f}".format(robot_info.angular_velocity[2])) + + # self.log_info(str(robot_info.clutch == CS.ENGAGED)) + + if robot_info.clutch == CS.ENGAGED: + self.display_clutch_on() + else: + self.display_clutch_off() + + if robot_info.state == SS.RUNNING: + self.display_state_on() + else: + self.display_state_off() + + if robot_info.obstacle_detected == True: + self.widget.obstaclestopLabel.setPixmap(self.cancel_pixmap.scaled(40,40)) + else: + self.widget.obstaclestopLabel.setPixmap(self.ok_pixmap.scaled(40,40)) + + def master_stopped(self): + self.widget.masterstopLabel.setPixmap(self.cancel_pixmap.scaled(40,40)) + self.log_info(self.language.master_stop_clicked_text) + + def master_started(self): + self.widget.masterstopLabel.setPixmap(self.ok_pixmap.scaled(40,40)) + self.log_info(self.language.master_stop_released_text) + + def user_stopped(self): + self.log_info(self.language.robot_stopped_text) + self.display_state_off() + + def user_started(self): + self.log_info(self.language.robot_started_text) + self.display_state_on() + + def disengage_clutch(self): + self.log_info(self.language.clutch_disengaged_text) + self.display_clutch_off() + + def engage_clutch(self): + self.log_info(self.language.clutch_engaged_text) + self.display_clutch_on() + + def master_is_stopped_notify(self): + self.log_error(self.language.cannot_start_masterstop_text) + + def obstacle_is_detected_notify(self): + self.log_error(self.language.cannot_start_obstacle_text) + + def robot_selected_by_another_group_notify(self): + self.log_error(self.language.cannot_select_robot_text) + + def update_restrictions_gui(self,restrictions): + self.widget.distanceRestrictionLabel.setText("{:.2f}".format(restrictions.distance)) + self.widget.linearRestrictionLabel.setText("{:.2f}".format(restrictions.linear_velocity)) + self.widget.angularRestrictionLabel.setText("{:.2f}".format(restrictions.angular_velocity)) + + 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(' {0} {1}
'.format(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(' {0} {1}
'.format(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(' {0} {1}
'.format(time,error_text)) + self.scroll_to_bottom() + # self.widget.logsBrowser.append(str(self.logger_counter) + '\t[ERROR]\t' + error_text) + + def connection_to_robot_lost(self,lost_robot_id): + self.clear_robot_info() + self.disconnect_signals() + self.connect_signals() + self.log_info(self.language.connection_to_robot_lost_text.format(lost_robot_id)) + + def connection_to_master_lost(self): + self.log_error(self.language.connection_to_master_lost_text) + + def obstacle_detected(self): + self.log_error(self.language.obstacle_detected_text) + # TODO Zmiana + + def obstacle_removed(self): + self.log_info(self.language.obstacle_removed_text) + + def scroll_to_bottom(self): + scrollbar = self.widget.logsBrowser.verticalScrollBar() + scrollbar.setValue(scrollbar.maximum()) + + def clear_robot_info(self): + self.widget.idLabel.setText('-') + self.widget.batteryLabel.setText('-') + self.widget.linearLabel.setText('-') + self.widget.angularLabel.setText('-') + self.widget.stateLabel.setText('-') + self.widget.masterstopLabel.setText('-') + self.widget.obstaclestopLabel.setText('-') + + self.widget.choiceButton.setText(self.language.select_text) + + self.widget.stopButton.setText('-') + self.widget.stopButton.setStyleSheet("") + + self.widget.clutchButton.setText('-') + self.widget.clutchButton.setStyleSheet("") + + self.widget.angularText.setText(self.language.angular_text) + self.widget.batteryText.setText(self.language.battery_text) + self.widget.idText.setText(self.language.id_text) + self.widget.linearText.setText(self.language.linear_text) + self.widget.masterStopText.setText(self.language.masterstop_text) + self.widget.obstacleText.setText(self.language.obstacle_text) + self.widget.robotText.setText(self.language.robot_text) + self.widget.historyLabel.setText(self.language.history_text) + self.widget.restrictionsLabel.setText(self.language.restrictions_text) diff --git a/scripts/restrictions.py b/scripts/restrictions.py new file mode 100644 index 0000000..bea4082 --- /dev/null +++ b/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/scripts/robot_info.py b/scripts/robot_info.py new file mode 100644 index 0000000..6669655 --- /dev/null +++ b/scripts/robot_info.py @@ -0,0 +1,32 @@ +#!/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 + self.obstacle_detected = 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] + self.obstacle_detected = robot_info_msg.obstacle_detected.data + + 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_wrapper.py b/scripts/ros_wrapper.py new file mode 100644 index 0000000..c7d5034 --- /dev/null +++ b/scripts/ros_wrapper.py @@ -0,0 +1,269 @@ +#!/usr/bin/env python +import rospy +import rospkg +import rostopic + +from std_msgs.msg import Bool +from rosaria_msgs.msg import RobotInfoMsg +from rosaria_msgs.msg import RestrictionsMsg + +from robot_info import RobotInfo +from restrictions import Restrictions + +from enums.clutch_state import ClutchState as CS +from enums.stop_state import StopState as SS + +class ROSWrapper: + + def __init__(self): + self.robots_list_update_callback = None + self.selected_robot_info_update_callback = None + self.master_stop_update_callback = None + self.restrictions_update_callback = None + + self.get_user_stop_state = None + self.get_clutch_state = None + + self.robot_info_subscriber = None + self.master_stop_subscriber = None + self.restrictions_subscriber = None + + self.user_stop_publisher = None + self.clutch_publisher = None + + self.periodic_timer = None + self.robots_list_timer = None + self.connection_timer = None + self.master_connection_timer = None + self.hz_update_timer = rospy.Timer(rospy.Duration(0.2),self.update_hz_values) + + self.robots_hz_subscribers_dict = {} + self.rostopics_hz_dict = {} + self.robots_hz_value = {} + + # def setup_node(self): + # rospy.init_node('safety_user_plugin', anonymous=True) + + def setup_subscribers_and_publishers(self,robot_id): + robot_name = 'PIONIER' + 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.restrictions_subscriber = rospy.Subscriber('/PIONIER/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) + + if self.periodic_timer != None: + self.shutdown_timer(self.periodic_timer) + self.periodic_timer = rospy.Timer(rospy.Duration(0.1),self.publish_periodic_update) + print("NEW") + if self.connection_timer != None: + self.shutdown_timer(self.connection_timer) + self.connection_timer = rospy.Timer(rospy.Duration(5),self.handle_robot_connection_lost) + + if self.master_connection_timer != None: + self.shutdown_timer(self.master_connection_timer) + self.master_connection_timer = rospy.Timer(rospy.Duration(1.5),self.handle_master_connection_lost) + + if self.hz_update_timer != None: + self.shutdown_timer(self.hz_update_timer) + self.hz_update_timer = rospy.Timer(rospy.Duration(0.2),self.update_hz_values) + + 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): + stop_state = self.get_user_stop_state() + # clutch_state = self.get_clutch_state() + + if stop_state == SS.RUNNING: + self.user_started() + elif stop_state == SS.STOPPED: + self.user_stopped() + else: + raise ValueError('stop_state UNKNOWN when attempting to publish periodic update') + + # if clutch_state == CS.ENGAGED: + # self.engage_clutch() + # elif clutch_state == CS.DISENGAGED: + # self.disengage_clutch() + # else: + # raise ValueError('clutch_state UNKNOWN when attempting to publish periodic update') + + def update_hz_values(self,event): + 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)) + + + def handle_robot_connection_lost(self,event): + self.shutdown_timer(self.connection_timer) + self.robot_connection_lost_callback() + + def handle_master_connection_lost(self,event): + self.shutdown_timer(self.master_connection_timer) + self.master_connection_lost_callback() + + def cancel_subscribers_and_publishers(self): + self.shutdown_timer(self.periodic_timer) + self.unsubscribe(self.robot_info_subscriber) + self.unsubscribe(self.master_stop_subscriber) + self.unsubscribe(self.restrictions_subscriber) + self.unregister_publisher(self.user_stop_publisher) + self.unregister_publisher(self.clutch_publisher) + + self.periodic_timer = None + self.robot_info_subscriber = None + self.master_stop_subscriber = None + self.restrictions_subscriber = None + self.user_stop_publisher = None + self.clutch_publisher = None + + def restart_connection_timer(self): + if self.connection_timer != None: + self.connection_timer.shutdown() + self.connection_timer = rospy.Timer(rospy.Duration(1.5),self.handle_robot_connection_lost) + else: + raise RuntimeError('Attempting to restart connection_timer when it is not initialized') + + def restart_master_connection_timer(self): + if self.master_connection_timer != None: + self.master_connection_timer.shutdown() + self.master_connection_timer = rospy.Timer(rospy.Duration(1.5),self.handle_master_connection_lost) + else: + raise RuntimeError('Attempting to restart master_connection_timer when it is not initialized') + + def unregister_node(self): + self.cancel_subscribers_and_publishers() + rospy.signal_shutdown('Closing safety user plugin') + + def select_robot(self,robot_id): + self.setup_subscribers_and_publishers(robot_id) + + def setup_get_user_stop_state_function(self,function): + self.get_user_stop_state = function + + def setup_get_clutch_state_function(self,function): + self.get_clutch_state = 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) + + + # Adding new hz subscribers + for robot_id in robots_id_list: + if robot_id not in self.robots_hz_subscribers_dict: + # Add hz subscriber + 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),Bool, + self.rostopics_hz_dict[robot_id].callback_hz,callback_args='/PIONIER{0}/RosAria/user_stop'.format(robot_id)) + + # Removing old hz subscribers + for robot_key in self.robots_hz_subscribers_dict: + if robot_key not in robots_id_list: + # Remove old subscriber + self.unsubscribe(self.robots_hz_subscribers_dict[robot_id]) + self.robots_hz_subscribers_dict[robot_id] = None + self.rostopics_hz_dict[robot_id] = None + + + self.robots_list_update_callback(robots_id_list) + + + def release_robot(self): + self.unsubscribe(self.robot_info_subscriber) + self.shutdown_timer(self.periodic_timer) + self.shutdown_timer(self.connection_timer) + self.shutdown_timer(self.master_connection_timer) + + def handle_selected_robot_info_update(self,msg): + # Restarting connection timer to avoid raising robot_connection_lost_callback + self.restart_connection_timer() + + _robot_info = RobotInfo(0) + _robot_info.update_robot_info(msg) + self.selected_robot_info_update_callback(_robot_info) + + def handle_master_stop_update(self,msg): + # Restarting master connection timer to avoid raising master_connection_lost_callback + self.restart_master_connection_timer() + + master_stop_state = SS.UNKNOWN + if msg.data == True: + master_stop_state = SS.RUNNING + else: + master_stop_state = SS.STOPPED + + self.master_stop_update_callback(master_stop_state) + + def handle_restrictions_update(self,msg): + restrictions = Restrictions(msg) + self.restrictions_update_callback(restrictions) + + # UserPlugin 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_selected_robot_info_update_callback(self,callback_function): + self.selected_robot_info_update_callback = callback_function + + def set_master_stop_update_callback(self,callback_function): + self.master_stop_update_callback = callback_function + + def set_restrictions_update_callback(self,callback_function): + self.restrictions_update_callback = callback_function + + def set_robot_connection_lost_callback(self,callback_function): + self.robot_connection_lost_callback = callback_function + + def set_master_connection_lost_callback(self,callback_function): + self.master_connection_lost_callback = callback_function + + def engage_clutch(self): + msg = Bool() + msg.data = True + self.clutch_publisher.publish(msg) + + def disengage_clutch(self): + msg = Bool() + 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): + msg = Bool() + msg.data = False + self.user_stop_publisher.publish(msg) + diff --git a/scripts/user_info.py b/scripts/user_info.py new file mode 100644 index 0000000..68cd98d --- /dev/null +++ b/scripts/user_info.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python + +from robot_info import RobotInfo + +class UserInfo: + + def __init__(self): + self.selected_robot = None + self.robots_id_list = [] + self.user_stop_state = None + self.master_stop_state = None + self.clutch_state = None + + def select_robot(self,robot_id): + self.selected_robot = RobotInfo(robot_id) + + def release_robot(self): + self.selected_robot = None + + def update_robots_id_list(self,robots_id_list): + self.robots_id_list = robots_id_list + + def update_selected_robot_info(self,new_robot_info): + self.selected_robot.robot_id = new_robot_info.robot_id + self.selected_robot.battery_voltage = new_robot_info.battery_voltage + self.selected_robot.linear_velocity = new_robot_info.linear_velocity + self.selected_robot.angular_velocity = new_robot_info.angular_velocity + self.selected_robot.state = new_robot_info.state + self.selected_robot.clutch = new_robot_info.clutch + self.selected_robot.obstacle_detected = new_robot_info.obstacle_detected + self.clutch_state = new_robot_info.clutch + + def get_user_stop_state(self): + return self.user_stop_state + + def get_clutch_state(self): + return self.clutch_state \ No newline at end of file diff --git a/scripts/user_plugin.py b/scripts/user_plugin.py new file mode 100755 index 0000000..cc7169a --- /dev/null +++ b/scripts/user_plugin.py @@ -0,0 +1,259 @@ +#!/usr/bin/env python + +import os + +from qt_gui.plugin import Plugin +from python_qt_binding.QtCore import QObject +from python_qt_binding.QtCore import pyqtSignal + +from qt_wrapper import QtWrapper +from ros_wrapper import ROSWrapper +from user_widget import UserWidget +from user_info import UserInfo + +from enums.clutch_state import ClutchState as CS +from enums.stop_state import StopState as SS + + +class CallbackHandler(QObject): + signal = pyqtSignal(str) + signal_with_list_argument = pyqtSignal(str,list) + + def __init__(self): + super(CallbackHandler, self).__init__() + + def connect(self,slot): + self.signal.connect(slot) + + def list_connect(self,slot): + self.signal_with_list_argument.connect(slot) + + +class UserPlugin(Plugin): + + def __init__(self,context): + super(UserPlugin, self).__init__(context) + self.setObjectName('User Plugin - L1.5 safety system') + # setStyleSheet("background-color:black;") + self.cbhandler = CallbackHandler() + + self._widget = UserWidget(context) + self._qt_wrapper = QtWrapper(self._widget) + self._ros_wrapper = ROSWrapper() + self._user_info = UserInfo() + + self._user_info.user_stop_state = SS.STOPPED + self._user_info.master_stop_state = SS.UNKNOWN + self._user_info.clutch_state = CS.UNKNOWN + + # Setup functions to get _user_info states from _ros_wrapper + self._ros_wrapper.setup_get_user_stop_state_function(self._user_info.get_user_stop_state) + self._ros_wrapper.setup_get_clutch_state_function(self._user_info.get_clutch_state) + + # self._ros_wrapper.setup_node() + self.set_callback_functions() + + # At the end! + self._qt_wrapper.connect_signals() + self._ros_wrapper.set_robots_list_update_callback(self.handle_robots_list_update) + self.cbhandler.signal.connect(self._qt_wrapper.handle_emitted_signal) + self.cbhandler.signal_with_list_argument.connect(self._qt_wrapper.handle_emitted_signal_with_list_argument) + + + + def call_qt_wrapper_method(self,method_name): + self.cbhandler.signal.emit(method_name) + + def call_qt_wrapper_method_with_argument(self,method_name,argument): + self.cbhandler.signal_with_list_argument.emit(method_name,argument) + + def shutdown_plugin(self): + self._ros_wrapper.unregister_node() + + def set_callback_functions(self): + self.set_Qt_callback_functions() + self.set_ROS_callback_functions() + + def set_ROS_callback_functions(self): + self._ros_wrapper.set_selected_robot_info_update_callback(self.handle_selected_robot_info_update) + self._ros_wrapper.set_master_stop_update_callback(self.handle_master_stop_update) + self._ros_wrapper.set_restrictions_update_callback(self.handle_restrictions_update) + self._ros_wrapper.set_robot_connection_lost_callback(self.handle_robot_connection_lost) + self._ros_wrapper.set_master_connection_lost_callback(self.handle_master_connection_lost) + + def set_Qt_callback_functions(self): + self._qt_wrapper.set_robot_selection_callback(self.handle_robot_selection) + self._qt_wrapper.set_robot_release_callback(self.handle_robot_release) + self._qt_wrapper.set_user_stop_state_updated_callback(self.handle_user_stop_state_updated) + self._qt_wrapper.set_clutch_switched_callback(self.handle_clutch_switched) + + + + + # ROS callback functions + def handle_robots_list_update(self,robots_id_list): + self._user_info.update_robots_id_list(robots_id_list) + self._qt_wrapper.update_robots_list_gui(robots_id_list) + + def handle_selected_robot_info_update(self,robot_info): + if robot_info != None: + self.update_selected_robot_info(robot_info) + else: + raise RuntimeError('Updated robot_info is NoneType object') + + def handle_master_stop_update(self,master_stop_state): + if (master_stop_state != self._user_info.master_stop_state): + self._user_info.master_stop_state = master_stop_state + if master_stop_state == SS.STOPPED: + self.master_stopped() + elif master_stop_state == SS.RUNNING: + self.master_started() + else: + raise ValueError('Undefined master_stop_state received') + + def handle_restrictions_update(self,restrictions): + self._qt_wrapper.update_restrictions_gui(restrictions) + + + # 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() + + else: + raise RuntimeError('User already selected robot') + + def handle_robot_release(self): + if self._user_info.selected_robot != None: + self.release_robot() + else: + raise RuntimeError('Cannot release. No robot selected') + + def handle_user_stop_state_updated(self): + if self._user_info.selected_robot.obstacle_detected == True: + self._qt_wrapper.obstacle_is_detected_notify() + else: + if self._user_info.master_stop_state == SS.STOPPED: + self._qt_wrapper.master_is_stopped_notify() + elif self._user_info.master_stop_state == SS.RUNNING: + if self._user_info.user_stop_state == SS.RUNNING: + self.user_stopped() + elif self._user_info.user_stop_state == SS.STOPPED: + self.user_started() + else: + raise ValueError('user_stop_state value undefined') + else: + raise ValueError('master_stop_state value undefined') + + def handle_clutch_switched(self): + if self._user_info.clutch_state == CS.ENGAGED: + self.disengage_clutch() + elif self._user_info.clutch_state == CS.DISENGAGED: + self.engage_clutch() + else: + raise ValueError('clutch_state value undefined') + + def handle_robot_connection_lost(self): + if self._user_info.selected_robot != None: + lost_robot_id = self._user_info.selected_robot.robot_id + self.connection_to_robot_lost(lost_robot_id) + else: + raise RuntimeError('Connection lost callback received when no robot was selected') + + def handle_master_connection_lost(self): + self.connection_to_master_lost() + + # Operations + def master_stopped(self): + self._user_info.master_stop_state = SS.STOPPED + + if self._user_info.selected_robot != None: + self.user_stopped() + self.call_qt_wrapper_method('master_stopped') + # self._qt_wrapper.master_stopped() + + def master_started(self): + self._user_info.master_stop_state = SS.RUNNING + + if self._user_info.selected_robot != None: + self.call_qt_wrapper_method('master_started') + # self._qt_wrapper.master_started() + + def user_stopped(self): + self._user_info.user_stop_state = SS.STOPPED + self._ros_wrapper.user_stopped() + 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.call_qt_wrapper_method('user_started') + # self._qt_wrapper.user_started() + + def engage_clutch(self): + self._user_info.clutch_state = CS.ENGAGED + self._ros_wrapper.engage_clutch() + self.call_qt_wrapper_method('engage_clutch') + # self._qt_wrapper.engage_clutch() + + def disengage_clutch(self): + self._user_info.clutch_state = CS.DISENGAGED + self._ros_wrapper.disengage_clutch() + self.call_qt_wrapper_method('disengage_clutch') + # self._qt_wrapper.disengage_clutch() + + def select_robot(self,robot_id): + self._ros_wrapper.select_robot(robot_id) + # self._qt_wrapper.select_robot(robot_id) + self.call_qt_wrapper_method_with_argument('select_robot',[robot_id]) + self._user_info.select_robot(robot_id) + + # Stop robot and engage clutch after taking control over it to put it in starting state + self.user_stopped() + self.engage_clutch() + + if self._user_info.master_stop_state == SS.RUNNING: + self.master_started() + elif self._user_info.master_stop_state == SS.STOPPED: + self.master_stopped() + + def release_robot(self): + self._ros_wrapper.release_robot() + self.call_qt_wrapper_method('release_robot') + # self._qt_wrapper.release_robot() + self._user_info.release_robot() + self._user_info.master_stop_state = SS.UNKNOWN + + def connection_to_robot_lost(self,lost_robot_id): + # pass + self._ros_wrapper.release_robot() + self._user_info.release_robot() + self.call_qt_wrapper_method_with_argument('connection_to_robot_lost',[lost_robot_id]) + # self._qt_wrapper.connection_to_robot_lost(lost_robot_id) + + def connection_to_master_lost(self): + if self._user_info.selected_robot != None: + self.master_stopped() + self.call_qt_wrapper_method('connection_to_master_lost') + + def obstacle_detected(self): + self.call_qt_wrapper_method('obstacle_detected') + self.user_stopped() + + def obstacle_removed(self): + self.call_qt_wrapper_method('obstacle_removed') + + 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: + self.obstacle_detected() + elif robot_info.obstacle_detected == False and self._user_info.selected_robot.obstacle_detected == True: + self.obstacle_removed() + + self._user_info.update_selected_robot_info(robot_info) + self.call_qt_wrapper_method_with_argument('update_selected_robot_info',[robot_info]) + # self._qt_wrapper.update_selected_robot_info(robot_info) \ No newline at end of file diff --git a/scripts/user_widget.py b/scripts/user_widget.py new file mode 100644 index 0000000..915a2eb --- /dev/null +++ b/scripts/user_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 UserWidget(QWidget): + + def __init__(self,context): + super(UserWidget, self).__init__() + ui_file = os.path.join(rospkg.RosPack().get_path('safety_user_plugin'), 'ui', 'user_view.ui') + loadUi(ui_file, self) + self.setObjectName('User 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/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/Cancel.jpg b/ui/Cancel.jpg new file mode 100644 index 0000000..ff6a442 Binary files /dev/null and b/ui/Cancel.jpg 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/Ok.jpg b/ui/Ok.jpg new file mode 100644 index 0000000..f052a71 Binary files /dev/null and b/ui/Ok.jpg differ diff --git a/ui/user_view.ui b/ui/user_view.ui new file mode 100644 index 0000000..fd526e6 --- /dev/null +++ b/ui/user_view.ui @@ -0,0 +1,395 @@ + + + Form + + + + 0 + 0 + 952 + 817 + + + + Form + + + true + + + + + + + + + + + + 0 + 0 + + + + + 0 + 40 + + + + + + + + + 0 + 0 + + + + + 16777215 + 60 + + + + Wybierz + + + + + + + + + Qt::Vertical + + + + 20 + 20 + + + + + + + + + + QLayout::SetDefaultConstraint + + + + + <html><head/><body><p align="center"><span style=" font-weight:600;">ID robota</span></p></body></html> + + + + + + + <html><head/><body><p align="center"><span style=" font-weight:600;">Stan baterii</span></p></body></html> + + + + + + + <html><head/><body><p align="center"><span style=" font-weight:600;">Predkosc liniowa</span></p></body></html> + + + + + + + <html><head/><body><p align="center"><span style=" font-weight:600;">Predkosc obrotowa</span></p></body></html> + + + + + + + <html><head/><body><p align="center"><span style=" font-weight:600;">Stan robota</span></p></body></html> + + + + + + + <html><head/><body><p align="center"><span style=" font-weight:600;">Stop mastera</span></p></body></html> + + + + + + + <html><head/><body><p align="center"><span style=" font-weight:600;">Stop przeszkody</span></p></body></html> + + + + + + + + + + + <html><head/><body><p align="center">-</p></body></html> + + + Qt::AlignCenter + + + + + + + <html><head/><body><p align="center">-</p></body></html> + + + Qt::AlignCenter + + + + + + + <html><head/><body><p align="center">-</p></body></html> + + + Qt::AlignCenter + + + + + + + <html><head/><body><p align="center">-</p></body></html> + + + Qt::AlignCenter + + + + + + + <html><head/><body><p align="center">-</p></body></html> + + + Qt::AlignCenter + + + + + + + <html><head/><body><p align="center">-</p></body></html> + + + Qt::AlignCenter + + + + + + + <html><head/><body><p align="center">-</p></body></html> + + + Qt::AlignCenter + + + + + + + + + 15 + + + + + + 100 + 0 + + + + <html><head/><body><p align="center">-</p></body></html> + + + Qt::AlignCenter + + + + + + + + 100 + 0 + + + + <html><head/><body><p align="center">-</p></body></html> + + + Qt::AlignCenter + + + + + + + + 100 + 0 + + + + <html><head/><body><p align="center">-</p></body></html> + + + Qt::AlignCenter + + + + + + + <html><head/><body><p align="center"><span style=" font-weight:600;">Ograniczenia</span></p></body></html> + + + + + + + TextLabel + + + Qt::AlignCenter + + + + + + + TextLabel + + + Qt::AlignCenter + + + + + + + TextLabel + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + QSizePolicy::Fixed + + + + 50 + 20 + + + + + + + + + + + + Qt::Vertical + + + + 20 + 20 + + + + + + + + + + + 0 + 0 + + + + - + + + + + + + + 0 + 0 + + + + - + + + + + + + + + Qt::Vertical + + + + 20 + 20 + + + + + + + + <html><head/><body><p align="center"><span style=" font-weight:600;">Histora komunikatów</span></p></body></html> + + + + + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Ubuntu'; font-size:11pt; font-weight:400; font-style:normal;"> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p></body></html> + + + + + + + +