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

Compare commits

base: L1.5:ros2-development
Branches Tags
L1.5:master
L1.5:ros2-development
L1.5:groups
..
compare: L1.5:master
Branches Tags
L1.5:ros2-development
L1.5:groups
L1.5:master

No commits in common. "ros2-development" and "master" have entirely different histories.
ros2-devel ... master

30 changed files with 777 additions and 755 deletions
Show Stats Expand all files Collapse all files

14
.pre-commit-config.yaml
View File

@ -1,14 +0,0 @@
# .pre-commit-config.yaml
repos:
- repo: https://github.com/pre-commit/pre-commit-hooks
rev: v4.1.0 # this is optional, use `pre-commit autoupdate` to get the latest rev!
hooks:
- id: check-yaml
- id: end-of-file-fixer
- id: trailing-whitespace
- repo: https://github.com/psf/black
rev: 23.7.0
hooks:
- id: black
name: Format Python Code (black)

204
CMakeLists.txt Normal file
View File

@ -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)

22
package.xml
View File

@ -1,39 +1,35 @@
<?xml version="1.0"?>
<package format="2">
<name>safety_user_plugin</name>
<version>0.2.0</version>
<version>0.0.0</version>
<description>The safety_user_plugin package</description>
<maintainer email="l15@kcir.pwr.edu.pl">Laboratory L1.5</maintainer>
<author email="aleksander.bojda@pwr.edu.pl">Aleksander Bojda</author>
<author email="jakub.delicat@pwr.edu.pl">Jakub Delicat</author>
<maintainer email="aleksander.bojda@pwr.edu.pl">Aleksander Bojda</maintainer>
<license>MIT</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>rclpy</build_depend>
<build_depend>rospy</build_depend>
<build_depend>rqt_gui</build_depend>
<build_depend>rqt_gui_py</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>ros2aria_msgs</build_depend>
<build_export_depend>rclpy</build_export_depend>
<build_depend>rosaria_msgs</build_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>rqt_gui</build_export_depend>
<build_export_depend>rqt_gui_py</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>ros2aria_msgs</build_export_depend>
<build_export_depend>rosaria_msgs</build_export_depend>
<exec_depend>rclpy</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>rqt_gui</exec_depend>
<exec_depend>rqt_gui_py</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>ros2aria_msgs</exec_depend>
<exec_depend>rosaria_msgs</exec_depend>
<export>
<build_type>ament_python</build_type>
<rqt_gui plugin="${prefix}/plugin.xml"/>
</export>
</package>

6
plugin.xml
View File

@ -1,15 +1,15 @@
<library path="scripts">
<class name="safety_user_plugin" type="safety_user_plugin.user_plugin.UserPlugin" base_class_type="rqt_gui_py::Plugin">
<class name="safety_user_plugin" type="user_plugin.UserPlugin" base_class_type="rqt_gui_py::Plugin">
<description>
Safety system user plugin
</description>
<qtgui>
<group>
<label>L1.5 Safety system</label>
<label>Safety system</label>
<icon type="theme">folder</icon>
</group>
<label>User</label>
<icon type="theme">system-help</icon>
</qtgui>
</class>
</library>
</library>

0
resource/safety_user_plugin
View File

0
safety_user_plugin/enums/__init__.py
View File

40
safety_user_plugin/robot_info.py
View File

@ -1,40 +0,0 @@
#!/usr/bin/env python
from safety_user_plugin.enums.clutch_state import ClutchState as CS
from safety_user_plugin.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 is True:
self.state = SS.RUNNING
else:
self.state = SS.STOPPED
if robot_info_msg.clutch.data is True:
self.clutch = CS.ENGAGED
else:
self.clutch = CS.DISENGAGED

311
safety_user_plugin/ros_wrapper.py
View File

@ -1,311 +0,0 @@
#!/usr/bin/env python
import rclpy
from rclpy.node import Node, NodeNameNonExistentError
from rclpy.qos import (
QoSProfile,
QoSReliabilityPolicy,
QoSHistoryPolicy,
QoSLivelinessPolicy,
QoSDurabilityPolicy,
)
from std_msgs.msg import Bool
from ros2aria_msgs.msg import RobotInfoMsg
from ros2aria_msgs.msg import RestrictionsMsg
from safety_user_plugin.robot_info import RobotInfo
from safety_user_plugin.restrictions import Restrictions
from safety_user_plugin.enums.clutch_state import ClutchState as CS
from safety_user_plugin.enums.stop_state import StopState as SS
class ROSWrapper(Node):
def __init__(self):
# TODO: mogą być problemy z tymi samymi nazwami node'ów, gry różne kompy będą miały odpalony
super().__init__("safety_user_plugin")
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.master_connection_lost_callback = None
self.robot_connection_lost_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.user_stop_state_to_publish = True
self.clutch_publisher = None
self.robots_list_timer = None
self.connection_timer = None
self.master_connection_timer = None
self.robot_stop_timer = None
self.time_of_last_msg_of_user_stop = [
None for i in range(6)
]
self.user_stop_subscribers = [
None for i in range(6)
]
self.robots_hz_value = [
None for i in range(6)
]
self.hz_update_timer = self.create_timer(0.1, self.update_taken_robots)
self._qos = QoSProfile(
reliability=QoSReliabilityPolicy.RELIABLE,
durability=QoSDurabilityPolicy.VOLATILE,
history=QoSHistoryPolicy.SYSTEM_DEFAULT,
liveliness=QoSLivelinessPolicy.AUTOMATIC,
depth=10,
)
self._create_user_stops_subscribers_and_callbacks()
def _create_user_stops_subscribers_and_callbacks(self):
for i in range(6):
self.user_stop_subscribers[i] = self.create_subscription(
Bool,
f"/pioneer{i+1}/user_stop",
lambda msg, i_in = i: self._update_user_stop_period(msg, i_in),
qos_profile=self._qos,
)
def _update_user_stop_period(self, msg, robot_id):
self.time_of_last_msg_of_user_stop[robot_id] = self.get_clock().now()
def setup_subscribers_and_publishers(self, robot_id):
self.robot_info_subscriber = self.create_subscription(
RobotInfoMsg,
f"/pioneer{robot_id}/robot_info",
self.handle_selected_robot_info_update,
qos_profile=self._qos,
)
self.master_stop_subscriber = self.create_subscription(
Bool,
"/pioneers/master_stop",
self.handle_master_stop_update,
qos_profile=self._qos,
)
self.restrictions_subscriber = self.create_subscription(
RestrictionsMsg,
"/pioneers/restrictions",
self.handle_restrictions_update,
qos_profile=self._qos,
)
self.user_stop_publisher = self.create_publisher(
Bool, f"/pioneer{robot_id}/user_stop", 10
)
self.clutch_publisher = self.create_publisher(
Bool, f"/pioneer{robot_id}/clutch", 10
)
self.shutdown_timer(self.connection_timer)
self.connection_timer = self.create_timer(
5.0, self.handle_robot_connection_lost
)
self.shutdown_timer(self.master_connection_timer)
self.master_connection_timer = self.create_timer(
1.5, self.handle_master_connection_lost
)
self.shutdown_timer(self.robot_stop_timer)
self.robot_stop_timer = self.create_timer(
0.1, self.publish_robot_stop
)
def unsubscribe(self, subscriber):
if subscriber is not None:
self.destroy_subscription(subscriber)
subscriber = None
def unregister_publisher(self, publisher):
while publisher is not None:
self.destroy_publisher(publisher)
publisher = None
def shutdown_timer(self, timer):
while timer is not None:
self.destroy_timer(timer)
timer = None
def publish_robot_stop(self):
if self.user_stop_publisher is not None:
msg = Bool()
msg.data = self.user_stop_state_to_publish
self.user_stop_publisher.publish(msg)
def update_taken_robots(self):
for i, last_time in zip(range(6), self.time_of_last_msg_of_user_stop):
if last_time is None:
self.robots_hz_value[i] = None
continue
self.robots_hz_value[i] = 1.0/float((self.get_clock().now() - last_time).nanoseconds) * 10e9
if self.robots_hz_value[i] < 10.0:
self.robots_hz_value[i] = None
def handle_robot_connection_lost(self):
self.shutdown_timer(self.connection_timer)
self.robot_connection_lost_callback()
def handle_master_connection_lost(self):
self.shutdown_timer(self.master_connection_timer)
self.master_connection_lost_callback()
def restart_connection_timer(self):
self.shutdown_timer(self.connection_timer)
self.connection_timer = self.create_timer(
5.0, self.handle_robot_connection_lost
)
def restart_master_connection_timer(self):
self.shutdown_timer(self.master_connection_timer)
self.master_connection_timer = self.create_timer(
1.5, self.handle_master_connection_lost
)
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)
def unregister_node(self):
self.release_robot()
self.cancel_subscribers_and_publishers()
print("Unregister node.")
def select_robot(self, robot_id):
self.setup_subscribers_and_publishers(robot_id+1)
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_and_refresh_subscribers(self):
robots_id_list = self._get_robots_list()
self.robots_list_update_callback(robots_id_list)
def _get_robots_list(self):
robots_id_list = []
# NOTE: Można to zrobić lepiej np. czekać na publikowaną wiadomość na robot_info
for pioneer_id in range(1, 7):
published_topics_list = []
try:
published_topics_list = self.get_publisher_names_and_types_by_node(
"ros2aria", "/pioneer" + str(pioneer_id)
)
except NodeNameNonExistentError:
pass
if len(published_topics_list) > 0:
robots_id_list.append(pioneer_id)
return robots_id_list
def release_robot(self):
# self.shutdown_timer(self.periodic_timer)
self.shutdown_timer(self.robot_stop_timer)
self.shutdown_timer(self.connection_timer)
self.shutdown_timer(self.master_connection_timer)
self.disengage_clutch()
self.user_robot_disabled()
self.unsubscribe(self.robot_info_subscriber)
self.unsubscribe(self.restrictions_subscriber)
self.unsubscribe(self.master_stop_subscriber)
self.unregister_publisher(self.user_stop_publisher)
self.unregister_publisher(self.clutch_publisher)
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 is False:
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 = self.create_timer(
0.5, self.get_robots_list_and_refresh_subscribers
)
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):
if self.clutch_publisher is not None:
msg = Bool()
msg.data = True
self.clutch_publisher.publish(msg)
def disengage_clutch(self):
if self.clutch_publisher is not None:
msg = Bool()
msg.data = False
self.clutch_publisher.publish(msg)
def user_robot_enabled(self):
if self.user_stop_publisher is not None:
self.user_stop_state_to_publish = False
msg = Bool()
msg.data = self.user_stop_state_to_publish
self.user_stop_publisher.publish(msg)
def user_robot_disabled(self):
if self.user_stop_publisher is not None:
self.user_stop_state_to_publish = True
msg = Bool()
msg.data = self.user_stop_state_to_publish
self.user_stop_publisher.publish(msg)
def qt_timer_callback(self):
rclpy.spin_once(self, timeout_sec=0.001)

11
safety_user_plugin/user_plugin_node.py
View File

@ -1,11 +0,0 @@
from safety_user_plugin.user_plugin import UserPlugin
from rqt_gui.main import Main
import sys
def main():
"""Run the plugin."""
Main().main(sys.argv, standalone="safety_user_plugin")
if __name__ == "__main__":
main()

22
safety_user_plugin/user_widget.py
View File

@ -1,22 +0,0 @@
#!/usr/bin/env python
import os
from ament_index_python.packages import get_package_share_directory
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(
get_package_share_directory("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)

52
safety_user_plugin/english.py → scripts/english.py
View File

@ -1,16 +1,16 @@
#!/usr/bin/env python
# coding=utf-8
from safety_user_plugin.language import LanguageTexts
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"
select_text = 'Select'
stop_robot_text = "STOP robot"
unlock_robot_text = "UNLOCK robot"
@ -18,39 +18,35 @@ class EnglishTexts(LanguageTexts):
disengage_clutch_text = "Disengage clutch"
engage_clutch_text = "Engage clutch"
release_robot_text = "Release robot"
release_robot_text = 'Release robot'
# Logger info texts
robot_selected_text = "PIONEER{0} selected!"
robot_released_text = "Robot released"
stopped_and_engaged_text = "Robot will be stopped and clutch engaged"
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"
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"
robot_stopped_text = 'Robot stopped'
robot_started_text = 'Robot started'
clutch_disengaged_text = "Disengaging the clutch"
clutch_engaged_text = "Engaging the clutch"
clutch_disengaged_text = 'Disengaging the clutch'
clutch_engaged_text = 'Engaging the clutch'
obstacle_detected_text = "Obstacle detected. Robot stopped"
obstacle_removed_text = "Obstacle removed"
obstacle_detected_text = 'Obstacle detected. Robot stopped'
obstacle_removed_text = 'Obstacle removed'
# Logger error/problem messages
master_plugin_inactive_text = (
"Master is inactive. Ask the teacher about the master plugin."
)
selection_error_text = "Select the PIONEER from robots list first"
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 PIONEER{0}"
connection_to_master_lost_text = (
"Connection lost with masterSTOP. Ask the teacher to run it"
)
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 = '<html><head/><body><p align="center"><span style=" font-weight:600;">Linear velocity</span></p></body></html>'
@ -63,5 +59,7 @@ class EnglishTexts(LanguageTexts):
restrictions_text = '<html><head/><body><p align="center"><span style=" font-weight:600;">Restrictions</span></p></body></html>'
history_text = '<html><head/><body><p align="center"><span style=" font-weight:600;">Message history</span></p></body></html>'
def __init__(self):
pass
pass

0
safety_user_plugin/__init__.py → scripts/enums/__init__.py
View File

0
safety_user_plugin/enums/clutch_state.py → scripts/enums/clutch_state.py
View File

0
safety_user_plugin/enums/stop_state.py → scripts/enums/stop_state.py
View File

0
safety_user_plugin/language.py → scripts/language.py
View File

60
safety_user_plugin/polish.py → scripts/polish.py
View File

@ -1,16 +1,16 @@
#!/usr/bin/env python
# coding=utf-8
from safety_user_plugin.language import LanguageTexts
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"
select_text = 'Wybierz'
stop_robot_text = "Zatrzymaj robota"
unlock_robot_text = "Odblokuj robota"
@ -18,45 +18,35 @@ class PolishTexts(LanguageTexts):
disengage_clutch_text = "Rozłącz sprzęgło"
engage_clutch_text = "Połącz sprzęgło"
release_robot_text = "Zwolnij robota"
release_robot_text = 'Zwolnij robota'
# Logger info texts
robot_selected_text = "PIONEER{0} wybrany!"
robot_released_text = "Odłączam robota"
stopped_and_engaged_text = "Robot zostanie zatrzymany i sprzęgnięty"
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"
)
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ł"
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"
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'
obstacle_detected_text = "Przeszkoda wykryta. Zatrzymuję robota"
obstacle_removed_text = "Przeszkoda usunięta"
# Logger error/problem messages
master_plugin_inactive_text = (
"Master nieaktywny. Zapytaj prowadzącego o odpalenie programu."
)
selection_error_text = "Najpierw wybierz PIONEERA 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 PIONEER{0}"
connection_to_master_lost_text = (
"Utrata połączenia z masterstopem. Poproś prowadzącego o jego uruchomienie"
)
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 = '<html><head/><body><p align="center"><span style=" font-weight:600;">Prędkość liniowa</span></p></body></html>'
@ -70,4 +60,4 @@ class PolishTexts(LanguageTexts):
history_text = '<html><head/><body><p align="center"><span style=" font-weight:600;">Historia komunikatów</span></p></body></html>'
def __init__(self):
pass
pass

214
safety_user_plugin/qt_wrapper.py → scripts/qt_wrapper.py
View File

@ -2,54 +2,53 @@
# coding=utf-8
import os
import rospy
import rospkg
import math
import datetime
from ament_index_python.packages import get_package_share_directory
from safety_user_plugin.enums.clutch_state import ClutchState as CS
from safety_user_plugin.enums.stop_state import StopState as SS
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 safety_user_plugin.polish import PolishTexts as PT
from safety_user_plugin.english import EnglishTexts as ET
from polish import PolishTexts as PT
from english import EnglishTexts as ET
class QtWrapper:
def __init__(self, widget):
def __init__(self,widget):
self.widget = widget
self.displayed_robots_id_list = []
lang = os.getenv("LANG")
if lang[:2] == "pl":
self.language = PT
self.log_info("Polska wersja pluginu uruchomiona")
else:
lang = rospy.get_param('lang')
if lang == 'eng':
self.language = ET
self.log_info("Started english version of plugin NEW")
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'))
ui_directory_path = "{0}/ui".format(
get_package_share_directory("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.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.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.widget.linearImage.setPixmap(self.linear_pixmap.scaled(20,48))
self.ok_pixmap = QPixmap("{0}/Ok.png".format(ui_directory_path))
self.cancel_pixmap = QPixmap("{0}/Cancel.png".format(ui_directory_path))
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()
@ -57,9 +56,7 @@ class QtWrapper:
self.widget.choiceButton.clicked.disconnect()
def connect_robot_signals(self):
self.widget.clutchButton.clicked.connect(
self.handle_clutchButton_clicked_callback
)
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)
@ -68,76 +65,62 @@ class QtWrapper:
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":
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])
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,
)
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 "PIONEER" in current_text:
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.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.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.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.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 log_master_inactive(self):
self.log_warning(self.language.master_plugin_inactive_text)
def handle_openButton_clicked(self):
selected_robot_id = self.get_selected_robot_id()
@ -149,22 +132,22 @@ class QtWrapper:
def handle_closeButton_clicked(self):
self.handle_closeButton_clicked_callback()
def set_robot_selection_callback(self, callback):
def set_robot_selection_callback(self,callback):
self.handle_openButton_clicked_callback = callback
def set_robot_release_callback(self, callback):
def set_robot_release_callback(self,callback):
self.handle_closeButton_clicked_callback = callback
def set_user_stop_state_updated_callback(self, callback):
def set_user_stop_state_updated_callback(self,callback):
self.handle_stopButton_clicked_callback = callback
def set_clutch_switched_callback(self, callback):
def set_clutch_switched_callback(self,callback):
self.handle_clutchButton_clicked_callback = callback
def select_robot(self, robot_id):
def select_robot(self,robot_id):
self.disconnect_signals()
self.connect_robot_signals()
self.log_info(self.language.robot_selected_text.format(robot_id+1))
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)
@ -175,9 +158,9 @@ class QtWrapper:
self.log_info(self.language.robot_released_text)
self.clear_robot_info()
def update_robots_list_gui(self, robots_id_list):
def update_robots_list_gui(self,robots_id_list):
robots_id_list.sort()
id_strings_list = (("PIONEER" + str(x)) for x in robots_id_list)
id_strings_list = (('PIONIER'+str(x)) for x in robots_id_list)
robots_to_add = []
robots_to_remove = []
@ -196,7 +179,8 @@ class QtWrapper:
for robot_id in robots_to_add:
self.add_robot_to_list(robot_id)
def remove_robot_from_list(self, robot_id):
def remove_robot_from_list(self,robot_id):
count = self.widget.robotsList.count()
for i in range(count):
@ -204,25 +188,23 @@ class QtWrapper:
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("PIONEER{0}".format(robot_id))
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("PIONEER{0}".format(robot_info.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.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:
@ -234,16 +216,16 @@ class QtWrapper:
self.display_state_off()
if robot_info.obstacle_detected == True:
self.widget.obstaclestopLabel.setPixmap(self.cancel_pixmap.scaled(40, 40))
self.widget.obstaclestopLabel.setPixmap(self.cancel_pixmap.scaled(40,40))
else:
self.widget.obstaclestopLabel.setPixmap(self.ok_pixmap.scaled(40, 40))
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.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.widget.masterstopLabel.setPixmap(self.ok_pixmap.scaled(40,40))
self.log_info(self.language.master_stop_released_text)
def user_stopped(self):
@ -261,7 +243,7 @@ class QtWrapper:
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)
@ -271,56 +253,44 @@ class QtWrapper:
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 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]")
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(
'<font color="black"> {0} {1}</font><br>'.format(time, info_text)
)
self.widget.logsBrowser.insertHtml('<font color="black"> {0} {1}</font><br>'.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]")
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(
'<font color="orange"> {0} {1}</font><br>'.format(time, warning_text)
)
self.widget.logsBrowser.insertHtml('<font color="orange"> {0} {1}</font><br>'.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]")
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(
'<font color="red"> {0} {1}</font><br>'.format(time, error_text)
)
self.widget.logsBrowser.insertHtml('<font color="red"> {0} {1}</font><br>'.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):
def connection_to_robot_lost(self,lost_robot_id):
self.clear_robot_info()
self.disconnect_signals()
self.connect_signals()
@ -341,20 +311,20 @@ class QtWrapper:
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.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.setText('-')
self.widget.stopButton.setStyleSheet("")
self.widget.clutchButton.setText("-")
self.widget.clutchButton.setText('-')
self.widget.clutchButton.setStyleSheet("")
self.widget.angularText.setText(self.language.angular_text)

0
safety_user_plugin/restrictions.py → scripts/restrictions.py
View File

32
scripts/robot_info.py Normal file
View File

@ -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

269
scripts/ros_wrapper.py Normal file
View File

@ -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)

9
scripts/safety_user_plugin
View File

@ -1,9 +0,0 @@
#!/usr/bin/env python3
import sys
from safety_user_plugin.user_plugin import UserPlugin
from rqt_gui.main import Main
sys.exit(Main().main(sys.argv, standalone='safety_user_plugin.user_plugin.UserPlugin'))

12
safety_user_plugin/user_info.py → scripts/user_info.py
View File

@ -1,9 +1,9 @@
#!/usr/bin/env python
from safety_user_plugin.robot_info import RobotInfo
from robot_info import RobotInfo
class UserInfo:
def __init__(self):
self.selected_robot = None
self.robots_id_list = []
@ -11,16 +11,16 @@ class UserInfo:
self.master_stop_state = None
self.clutch_state = None
def select_robot(self, robot_id):
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):
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):
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
@ -34,4 +34,4 @@ class UserInfo:
return self.user_stop_state
def get_clutch_state(self):
return self.clutch_state
return self.clutch_state

196
safety_user_plugin/user_plugin.py → scripts/user_plugin.py
View File

@ -5,35 +5,36 @@ import os
from qt_gui.plugin import Plugin
from python_qt_binding.QtCore import QObject
from python_qt_binding.QtCore import pyqtSignal
from python_qt_binding.QtCore import QTimer
from safety_user_plugin.qt_wrapper import QtWrapper
from safety_user_plugin.ros_wrapper import ROSWrapper
from safety_user_plugin.user_widget import UserWidget
from safety_user_plugin.user_info import UserInfo
from qt_wrapper import QtWrapper
from ros_wrapper import ROSWrapper
from user_widget import UserWidget
from user_info import UserInfo
from safety_user_plugin.enums.clutch_state import ClutchState as CS
from safety_user_plugin.enums.stop_state import StopState as SS
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)
signal_with_list_argument = pyqtSignal(str,list)
def __init__(self):
super(CallbackHandler, self).__init__()
super(CallbackHandler, self).__init__()
def connect(self, slot):
def connect(self,slot):
self.signal.connect(slot)
def list_connect(self, slot):
def list_connect(self,slot):
self.signal_with_list_argument.connect(slot)
class UserPlugin(Plugin):
def __init__(self, context):
def __init__(self,context):
super(UserPlugin, self).__init__(context)
self.setObjectName("User Plugin - L1.5 safety system")
self.setObjectName('User Plugin - L1.5 safety system')
# setStyleSheet("background-color:black;")
self.cbhandler = CallbackHandler()
self._widget = UserWidget(context)
@ -46,37 +47,25 @@ class UserPlugin(Plugin):
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_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._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
)
self.cbhandler.signal_with_list_argument.connect(self._qt_wrapper.handle_emitted_signal_with_list_argument)
# timer to consecutively send twist messages
self._update_parameter_timer = QTimer(self)
self._update_parameter_timer.timeout.connect(
self._ros_wrapper.qt_timer_callback
)
self._update_parameter_timer.start(100)
def call_qt_wrapper_method(self, method_name):
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 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()
@ -84,72 +73,64 @@ class UserPlugin(Plugin):
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
)
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_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):
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 is not None:
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")
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:
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")
raise ValueError('Undefined master_stop_state received')
def handle_restrictions_update(self, restrictions):
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 is None:
if self._ros_wrapper.robots_hz_value[robot_id-1] is None:
self.select_robot(robot_id-1)
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")
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")
raise RuntimeError('Cannot release. No robot selected')
def handle_user_stop_state_updated(self):
if self._user_info.selected_robot.obstacle_detected == True:
@ -163,9 +144,9 @@ class UserPlugin(Plugin):
elif self._user_info.user_stop_state == SS.STOPPED:
self.user_started()
else:
self._qt_wrapper.log_master_inactive()
raise ValueError('user_stop_state value undefined')
else:
self._qt_wrapper.log_master_inactive()
raise ValueError('master_stop_state value undefined')
def handle_clutch_switched(self):
if self._user_info.clutch_state == CS.ENGAGED:
@ -173,16 +154,14 @@ class UserPlugin(Plugin):
elif self._user_info.clutch_state == CS.DISENGAGED:
self.engage_clutch()
else:
raise ValueError("clutch_state value undefined")
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"
)
raise RuntimeError('Connection lost callback received when no robot was selected')
def handle_master_connection_lost(self):
self.connection_to_master_lost()
@ -190,40 +169,47 @@ class UserPlugin(Plugin):
# 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.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.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_robot_disabled()
self.call_qt_wrapper_method("user_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_robot_enabled()
self.call_qt_wrapper_method("user_started")
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.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.call_qt_wrapper_method('disengage_clutch')
# self._qt_wrapper.disengage_clutch()
def select_robot(self, robot_id):
def select_robot(self,robot_id):
self._ros_wrapper.select_robot(robot_id)
self.call_qt_wrapper_method_with_argument("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
@ -237,43 +223,37 @@ class UserPlugin(Plugin):
def release_robot(self):
self._ros_wrapper.release_robot()
self.call_qt_wrapper_method("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):
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.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 is not None:
if self._user_info.selected_robot != None:
self.master_stopped()
self.call_qt_wrapper_method("connection_to_master_lost")
self.call_qt_wrapper_method('connection_to_master_lost')
def obstacle_detected(self):
self.call_qt_wrapper_method("obstacle_detected")
self.call_qt_wrapper_method('obstacle_detected')
self.user_stopped()
def obstacle_removed(self):
self.call_qt_wrapper_method("obstacle_removed")
self.call_qt_wrapper_method('obstacle_removed')
def update_selected_robot_info(self, robot_info):
if self._user_info.selected_robot is not None:
if (
robot_info.obstacle_detected is True
and self._user_info.selected_robot.obstacle_detected is False
):
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 is False
and self._user_info.selected_robot.obstacle_detected is True
):
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.call_qt_wrapper_method_with_argument('update_selected_robot_info',[robot_info])
# self._qt_wrapper.update_selected_robot_info(robot_info)

18
scripts/user_widget.py Normal file
View File

@ -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)

5
setup.cfg
View File

@ -1,5 +0,0 @@
[develop]
script_dir=$base/lib/safety_user_plugin
[install]
install_scripts=$base/lib/safety_user_plugin

35
setup.py
View File

@ -1,31 +1,8 @@
import os
from glob import glob
from setuptools import setup
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
package_name = 'safety_user_plugin'
submodules = 'safety_user_plugin/enums'
setup(
name=package_name,
version='0.2.0',
packages=[package_name, submodules],
data_files=[
('share/ament_index/resource_index/packages', ['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
('share/' + package_name, ['plugin.xml']),
(os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))),
(os.path.join('share', package_name, 'ui'), glob(os.path.join('ui', '*.png'))),
(os.path.join('share', package_name, 'ui'), glob(os.path.join('ui', '*.ui'))),
(os.path.join('lib/', package_name), glob(os.path.join('ui', '*.ui'))),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='jdelicat',
maintainer_email='jakub.delicat@pwr.edu.pl',
description='TODO: Package description',
license='TODO: License declaration',
tests_require=['pytest'],
entry_points={
'console_scripts': ['user_plugin_node = safety_user_plugin.user_plugin_node:main'],
},
setup_args = generate_distutils_setup(
package_dir={'': 'scripts'}
)
setup(**setup_args)

BIN
ui/Cancel.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 17 KiB

BIN
ui/Cancel.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 28 KiB

BIN
ui/Ok.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 13 KiB

BIN
ui/Ok.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 23 KiB