Compare commits
No commits in common. "ros2-development" and "master" have entirely different histories.
ros2-devel
...
master
@ -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)
|
||||
201
CMakeLists.txt
Normal file
201
CMakeLists.txt
Normal file
@ -0,0 +1,201 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(safety_master_plugin)
|
||||
|
||||
## Compile as C++11, supported in ROS Kinetic and newer
|
||||
# add_compile_options(-std=c++11)
|
||||
|
||||
## Find catkin macros and libraries
|
||||
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
||||
## is used, also find other catkin packages
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
rospy
|
||||
rqt_gui
|
||||
rqt_gui_py
|
||||
std_msgs
|
||||
)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
# find_package(Boost REQUIRED COMPONENTS system)
|
||||
|
||||
|
||||
## Uncomment this if the package has a setup.py. This macro ensures
|
||||
## modules and global scripts declared therein get installed
|
||||
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
|
||||
catkin_python_setup()
|
||||
|
||||
################################################
|
||||
## Declare ROS messages, services and actions ##
|
||||
################################################
|
||||
|
||||
## To declare and build messages, services or actions from within this
|
||||
## package, follow these steps:
|
||||
## * Let MSG_DEP_SET be the set of packages whose message types you use in
|
||||
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend tag for "message_generation"
|
||||
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
|
||||
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
|
||||
## but can be declared for certainty nonetheless:
|
||||
## * add a run_depend tag for "message_runtime"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "message_generation" and every package in MSG_DEP_SET to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * add "message_runtime" and every package in MSG_DEP_SET to
|
||||
## catkin_package(CATKIN_DEPENDS ...)
|
||||
## * uncomment the add_*_files sections below as needed
|
||||
## and list every .msg/.srv/.action file to be processed
|
||||
## * uncomment the generate_messages entry below
|
||||
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
|
||||
|
||||
## Generate messages in the 'msg' folder
|
||||
# add_message_files(
|
||||
# FILES
|
||||
# Message1.msg
|
||||
# Message2.msg
|
||||
# )
|
||||
|
||||
## Generate services in the 'srv' folder
|
||||
# add_service_files(
|
||||
# FILES
|
||||
# Service1.srv
|
||||
# Service2.srv
|
||||
# )
|
||||
|
||||
## Generate actions in the 'action' folder
|
||||
# add_action_files(
|
||||
# FILES
|
||||
# Action1.action
|
||||
# Action2.action
|
||||
# )
|
||||
|
||||
## Generate added messages and services with any dependencies listed here
|
||||
# generate_messages(
|
||||
# DEPENDENCIES
|
||||
# std_msgs
|
||||
# )
|
||||
|
||||
################################################
|
||||
## Declare ROS dynamic reconfigure parameters ##
|
||||
################################################
|
||||
|
||||
## To declare and build dynamic reconfigure parameters within this
|
||||
## package, follow these steps:
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "dynamic_reconfigure" to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * uncomment the "generate_dynamic_reconfigure_options" section below
|
||||
## and list every .cfg file to be processed
|
||||
|
||||
## Generate dynamic reconfigure parameters in the 'cfg' folder
|
||||
# generate_dynamic_reconfigure_options(
|
||||
# cfg/DynReconf1.cfg
|
||||
# cfg/DynReconf2.cfg
|
||||
# )
|
||||
|
||||
###################################
|
||||
## catkin specific configuration ##
|
||||
###################################
|
||||
## The catkin_package macro generates cmake config files for your package
|
||||
## Declare things to be passed to dependent projects
|
||||
## INCLUDE_DIRS: uncomment this if your package contains header files
|
||||
## LIBRARIES: libraries you create in this project that dependent projects also need
|
||||
## CATKIN_DEPENDS: catkin_packages dependent projects also need
|
||||
## DEPENDS: system dependencies of this project that dependent projects also need
|
||||
catkin_package(
|
||||
# INCLUDE_DIRS include
|
||||
# LIBRARIES safety_master_plugin
|
||||
# CATKIN_DEPENDS rospy std_msgs
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
include_directories(
|
||||
# include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
## Declare a C++ library
|
||||
# add_library(${PROJECT_NAME}
|
||||
# src/${PROJECT_NAME}/safety_master_plugin.cpp
|
||||
# )
|
||||
|
||||
## Add cmake target dependencies of the library
|
||||
## as an example, code may need to be generated before libraries
|
||||
## either from message generation or dynamic reconfigure
|
||||
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Declare a C++ executable
|
||||
## With catkin_make all packages are built within a single CMake context
|
||||
## The recommended prefix ensures that target names across packages don't collide
|
||||
# add_executable(${PROJECT_NAME}_node src/safety_master_plugin_node.cpp)
|
||||
|
||||
## Rename C++ executable without prefix
|
||||
## The above recommended prefix causes long target names, the following renames the
|
||||
## target back to the shorter version for ease of user use
|
||||
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
|
||||
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
|
||||
|
||||
## Add cmake target dependencies of the executable
|
||||
## same as for the library above
|
||||
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
# add_dependencies(safety_user_plugin ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Specify libraries to link a library or executable target against
|
||||
# target_link_libraries(${PROJECT_NAME}_node
|
||||
# ${catkin_LIBRARIES}
|
||||
# )
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# all install targets should use catkin DESTINATION variables
|
||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
||||
|
||||
## Mark executable scripts (Python etc.) for installation
|
||||
## in contrast to setup.py, you can choose the destination
|
||||
install(PROGRAMS
|
||||
# scripts/my_python_script
|
||||
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
## Mark executables and/or libraries for installation
|
||||
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
|
||||
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark cpp header files for installation
|
||||
# install(DIRECTORY include/${PROJECT_NAME}/
|
||||
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
# FILES_MATCHING PATTERN "*.h"
|
||||
# PATTERN ".svn" EXCLUDE
|
||||
# )
|
||||
|
||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
||||
# install(FILES
|
||||
# # myfile1
|
||||
# # myfile2
|
||||
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
# )
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_safety_master_plugin.cpp)
|
||||
# if(TARGET ${PROJECT_NAME}-test)
|
||||
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
|
||||
# endif()
|
||||
|
||||
## Add folders to be run by python nosetests
|
||||
# catkin_add_nosetests(test)
|
||||
37
package.xml
37
package.xml
@ -1,24 +1,35 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<package format="2">
|
||||
<name>safety_master_plugin</name>
|
||||
<version>0.2.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="l15@kcir.pwr.edu.pl">Laboratory L1.5</maintainer>
|
||||
<version>0.0.0</version>
|
||||
<description>The safety_master_plugin package</description>
|
||||
|
||||
<maintainer email="aleksander.bojda@pwr.edu.pl">Aleksander Bojda</maintainer>
|
||||
|
||||
<author email="aleksander.bojda@pwr.edu.pl">Aleksander Bojda</author>
|
||||
<author email="jakub.delicat@pwr.edu.pl">Jakub Delicat</author>
|
||||
<license>MIT</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>rqt_gui</depend>
|
||||
<depend>rqt_gui_py</depend>
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<depend>std_msgs</depend>
|
||||
<depend>ros2aria_msgs</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>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>rosaria_msgs</build_export_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>rosaria_msgs</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
<rqt_gui plugin="${prefix}/plugin.xml"/>
|
||||
</export>
|
||||
</package>
|
||||
|
||||
@ -1,11 +1,11 @@
|
||||
<library path="scripts">
|
||||
<class name="safety_master_plugin" type="safety_master_plugin.master_plugin.MasterPlugin" base_class_type="rqt_gui_py::Plugin">
|
||||
<class name="safety_master_plugin" type="master_plugin.MasterPlugin" base_class_type="rqt_gui_py::Plugin">
|
||||
<description>
|
||||
Safety system master plugin
|
||||
</description>
|
||||
<qtgui>
|
||||
<group>
|
||||
<label>L1.5 Safety system</label>
|
||||
<label>Safety system</label>
|
||||
<icon type="theme">folder</icon>
|
||||
</group>
|
||||
<label>Master</label>
|
||||
|
||||
@ -1,6 +0,0 @@
|
||||
|
||||
|
||||
class ClutchState:
|
||||
ENGAGED = 4
|
||||
DISENGAGED = 5
|
||||
UNKNOWN = 6
|
||||
@ -1,127 +0,0 @@
|
||||
from qt_gui.plugin import Plugin
|
||||
from python_qt_binding.QtCore import QTimer
|
||||
|
||||
from safety_master_plugin.master_restrictions import Restrictions
|
||||
|
||||
from safety_master_plugin.master_info import MasterInfo
|
||||
from safety_master_plugin.master_widget import MasterWidget
|
||||
from safety_master_plugin.stop_state import StopState as SS
|
||||
from safety_master_plugin.master_callback_handler import CallbackHandler
|
||||
from safety_master_plugin.ros_master_wrapper import ROSWrapper
|
||||
from safety_master_plugin.qt_master_wrapper import QtWrapper
|
||||
|
||||
|
||||
class MasterPlugin(Plugin):
|
||||
def __init__(self, context):
|
||||
super(MasterPlugin, self).__init__(context)
|
||||
self.setObjectName("Master Plugin - L1.5 safety system")
|
||||
|
||||
self._widget = MasterWidget(context)
|
||||
self._ros_wrapper = ROSWrapper()
|
||||
self._qt_wrapper = QtWrapper(self._widget)
|
||||
self.set_Qt_callback_functions()
|
||||
|
||||
self._master_info = MasterInfo()
|
||||
|
||||
self.cbhandler = CallbackHandler()
|
||||
self.cbhandler.connect_robot_info_signal(self._qt_wrapper.update_robot_info)
|
||||
self.cbhandler.connect_add_robot_signal(self._qt_wrapper.add_robot)
|
||||
self.cbhandler.connect_remove_robot_signal(self._qt_wrapper.remove_robot)
|
||||
|
||||
self._qt_wrapper.connect_signals()
|
||||
self._qt_wrapper.set_sliders_to_initial_values()
|
||||
|
||||
# Setup functions to get _master_info states from _ros_wrapper
|
||||
self._ros_wrapper.setup_get_master_stop_state_function(
|
||||
self._master_info.get_master_stop_state
|
||||
)
|
||||
self._ros_wrapper.setup_get_restrictions_function(
|
||||
self._master_info.get_restrictions
|
||||
)
|
||||
|
||||
self.set_callback_functions()
|
||||
|
||||
# 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)
|
||||
|
||||
# Stopping master at the beginning
|
||||
self.disable_robots()
|
||||
|
||||
def set_callback_functions(self):
|
||||
self.set_ROS_callback_functions()
|
||||
self.set_Qt_callback_functions()
|
||||
|
||||
def set_ROS_callback_functions(self):
|
||||
self._ros_wrapper.set_robots_list_update_callback(
|
||||
self.handle_robots_list_update
|
||||
)
|
||||
self._ros_wrapper.set_robot_info_update_callback(self.handle_robot_info_update)
|
||||
|
||||
def set_Qt_callback_functions(self):
|
||||
self._qt_wrapper.set_master_stop_state_updated_callback(
|
||||
self.handle_master_stop_state_updated
|
||||
)
|
||||
self._qt_wrapper.set_restrictions_updated_callback(
|
||||
self.handle_restrictions_updated
|
||||
)
|
||||
|
||||
def handle_robots_list_update(self, robots_id_list):
|
||||
self.update_robots_list(robots_id_list)
|
||||
|
||||
def handle_robot_info_update(self, robot_info):
|
||||
self.update_robot_info(robot_info)
|
||||
|
||||
def handle_master_stop_state_updated(self):
|
||||
"""Handles master stop state when it changes e. g. on button click and confirmed."""
|
||||
if self._master_info.master_stop_state == SS.STOPPED:
|
||||
self.enable_robots()
|
||||
elif self._master_info.master_stop_state == SS.RUNNING:
|
||||
self.disable_robots()
|
||||
else:
|
||||
raise ValueError("master_stop_state value undefined")
|
||||
|
||||
def handle_restrictions_updated(self, restrictions):
|
||||
self.restrictions_changed(restrictions)
|
||||
|
||||
def update_robots_list(self, robots_id_list):
|
||||
for robot_id in robots_id_list:
|
||||
if robot_id not in self._master_info.robots_id_list:
|
||||
self.new_robot_registered(robot_id)
|
||||
|
||||
for robot_id in self._master_info.robots_id_list:
|
||||
if robot_id not in robots_id_list:
|
||||
self.robot_unregistered(robot_id)
|
||||
|
||||
self._master_info.robots_id_list = robots_id_list
|
||||
|
||||
def new_robot_registered(self, robot_id):
|
||||
self._ros_wrapper.add_robot_subscriber(robot_id)
|
||||
self.cbhandler.add_robot_signal.emit(robot_id)
|
||||
|
||||
def robot_unregistered(self, robot_id):
|
||||
self._ros_wrapper.remove_robot_subscriber(robot_id)
|
||||
self.cbhandler.remove_robot_signal.emit(robot_id)
|
||||
|
||||
def update_robot_info(self, robot_info):
|
||||
self.cbhandler.robot_info_signal.emit(robot_info)
|
||||
|
||||
def enable_robots(self):
|
||||
self._master_info.master_stop_state = SS.RUNNING
|
||||
self._ros_wrapper.when_robots_enabled()
|
||||
self._qt_wrapper.when_robots_enabled()
|
||||
|
||||
def disable_robots(self):
|
||||
self._master_info.master_stop_state = SS.STOPPED
|
||||
self._ros_wrapper.when_robots_disabled()
|
||||
self._qt_wrapper.when_robots_disabled()
|
||||
|
||||
def restrictions_changed(self, restrictions):
|
||||
self._master_info.restrictions = restrictions
|
||||
self._ros_wrapper.restrictions_changed(restrictions)
|
||||
|
||||
def shutdown_plugin(self):
|
||||
self._ros_wrapper.unregister_node()
|
||||
@ -1,11 +0,0 @@
|
||||
from safety_master_plugin.master_plugin import MasterPlugin
|
||||
from rqt_gui.main import Main
|
||||
import sys
|
||||
|
||||
def main():
|
||||
"""Run the plugin."""
|
||||
Main().main(sys.argv, standalone="safety_master_plugin")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -1,38 +0,0 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
from safety_master_plugin.clutch_state import ClutchState as CS
|
||||
from safety_master_plugin.stop_state import StopState as SS
|
||||
|
||||
|
||||
class RobotInfo:
|
||||
def __init__(self, robot_id):
|
||||
self.robot_id = robot_id
|
||||
self.battery_voltage = None
|
||||
self.linear_velocity = None
|
||||
self.angular_velocity = None
|
||||
self.state = None
|
||||
self.clutch = None
|
||||
|
||||
def update_robot_info(self, robot_info_msg):
|
||||
self.robot_id = robot_info_msg.robot_id.data
|
||||
self.battery_voltage = robot_info_msg.battery_voltage.data
|
||||
self.linear_velocity = [
|
||||
robot_info_msg.twist.linear.x,
|
||||
robot_info_msg.twist.linear.y,
|
||||
robot_info_msg.twist.linear.z,
|
||||
]
|
||||
self.angular_velocity = [
|
||||
robot_info_msg.twist.angular.x,
|
||||
robot_info_msg.twist.angular.y,
|
||||
robot_info_msg.twist.angular.z,
|
||||
]
|
||||
|
||||
if robot_info_msg.state.data 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
|
||||
@ -1,192 +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_master_plugin.robot_info import RobotInfo
|
||||
from safety_master_plugin.master_restrictions import Restrictions
|
||||
|
||||
from safety_master_plugin.stop_state import StopState as SS
|
||||
|
||||
|
||||
class ROSWrapper(Node):
|
||||
def __init__(self):
|
||||
super().__init__("safety_master_plugin")
|
||||
self.robots_list_update_callback = None
|
||||
self.robot_info_update_callback = None
|
||||
self.get_master_stop_state = None
|
||||
self.get_restrictions = None
|
||||
|
||||
self.robot_info_subscribers = {}
|
||||
self.master_stop_publisher = None
|
||||
self.restrictions_publisher = None
|
||||
|
||||
self.setup_subscribers_and_publishers()
|
||||
self.subs = []
|
||||
|
||||
def setup_subscribers_and_publishers(self):
|
||||
publishers_qos = QoSProfile(
|
||||
reliability=QoSReliabilityPolicy.RELIABLE,
|
||||
durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
|
||||
history=QoSHistoryPolicy.SYSTEM_DEFAULT,
|
||||
liveliness=QoSLivelinessPolicy.AUTOMATIC,
|
||||
depth=10,
|
||||
)
|
||||
|
||||
self.master_stop_publisher = self.create_publisher(
|
||||
Bool, "/pioneers/master_stop", qos_profile=publishers_qos
|
||||
)
|
||||
self.restrictions_publisher = self.create_publisher(
|
||||
RestrictionsMsg, "/pioneers/restrictions", qos_profile=publishers_qos
|
||||
)
|
||||
|
||||
def add_robot_subscriber(self, robot_id):
|
||||
if robot_id not in self.robot_info_subscribers:
|
||||
topic_name = "/pioneer{0}/robot_info".format(robot_id)
|
||||
subscription_qos = QoSProfile(
|
||||
reliability=QoSReliabilityPolicy.RELIABLE,
|
||||
durability=QoSDurabilityPolicy.VOLATILE,
|
||||
history=QoSHistoryPolicy.SYSTEM_DEFAULT,
|
||||
liveliness=QoSLivelinessPolicy.AUTOMATIC,
|
||||
depth=10,
|
||||
)
|
||||
|
||||
self.robot_info_subscribers[robot_id] = self.create_subscription(
|
||||
RobotInfoMsg,
|
||||
topic_name,
|
||||
self.handle_robot_info_update,
|
||||
qos_profile=subscription_qos,
|
||||
)
|
||||
else:
|
||||
raise RuntimeError(
|
||||
"Subscriber for pioneer{0} already in dictionary".format(robot_id)
|
||||
)
|
||||
|
||||
def remove_robot_subscriber(self, robot_id):
|
||||
if robot_id in self.robot_info_subscribers:
|
||||
self.unsubscribe(self.robot_info_subscribers[robot_id])
|
||||
del self.robot_info_subscribers[robot_id]
|
||||
else:
|
||||
raise RuntimeError(
|
||||
"Subscriber for PIONEER{0} not in dictionary. Cannot be removed".format(
|
||||
robot_id
|
||||
)
|
||||
)
|
||||
|
||||
def unsubscribe(self, subscriber):
|
||||
if subscriber is not None:
|
||||
self.destroy_subscription(subscriber)
|
||||
|
||||
def unregister_publisher(self, publisher):
|
||||
if publisher is not None:
|
||||
self.destroy_publisher(publisher)
|
||||
|
||||
def publish_periodic_update(self):
|
||||
restrictions = self.get_restrictions()
|
||||
restrictions_msg = RestrictionsMsg()
|
||||
restrictions_msg.distance.data = restrictions.distance
|
||||
restrictions_msg.linear_velocity.data = restrictions.linear_velocity
|
||||
restrictions_msg.angular_velocity.data = restrictions.angular_velocity
|
||||
|
||||
stop_state = self.get_master_stop_state()
|
||||
stop_msg = Bool()
|
||||
|
||||
if stop_state == SS.RUNNING:
|
||||
stop_msg.data = False
|
||||
elif stop_state == SS.STOPPED:
|
||||
stop_msg.data = True
|
||||
else:
|
||||
stop_msg.data = False
|
||||
raise ValueError(
|
||||
"stop_state UNKNOWN when attempting to publish periodic update"
|
||||
)
|
||||
|
||||
self.restrictions_publisher.publish(restrictions_msg)
|
||||
self.master_stop_publisher.publish(stop_msg)
|
||||
|
||||
def cancel_subscribers_and_publishers(self):
|
||||
self.when_robots_disabled()
|
||||
|
||||
self.unregister_publisher(self.master_stop_publisher)
|
||||
self.unregister_publisher(self.restrictions_publisher)
|
||||
|
||||
for subscriber in self.robot_info_subscribers.values():
|
||||
self.unsubscribe(subscriber)
|
||||
|
||||
def unregister_node(self):
|
||||
self.cancel_subscribers_and_publishers()
|
||||
|
||||
def setup_get_master_stop_state_function(self, function):
|
||||
self.get_master_stop_state = function
|
||||
|
||||
def setup_get_restrictions_function(self, function):
|
||||
self.get_restrictions = function
|
||||
|
||||
# ROSWrapper Callbacks
|
||||
def get_robots_list(self):
|
||||
robots_id_list = []
|
||||
|
||||
# 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)
|
||||
|
||||
self.robots_list_update_callback(robots_id_list)
|
||||
|
||||
def handle_robot_info_update(self, msg):
|
||||
_robot_info = RobotInfo(0)
|
||||
_robot_info.update_robot_info(msg)
|
||||
self.robot_info_update_callback(_robot_info)
|
||||
|
||||
# MasterPlugin Callbacks
|
||||
def set_robots_list_update_callback(self, callback_function):
|
||||
self.robots_list_update_callback = callback_function
|
||||
|
||||
def set_robot_info_update_callback(self, callback_function):
|
||||
self.robot_info_update_callback = callback_function
|
||||
|
||||
def when_robots_enabled(self):
|
||||
msg = Bool()
|
||||
msg.data = False
|
||||
self.master_stop_publisher.publish(msg)
|
||||
|
||||
def when_robots_disabled(self):
|
||||
msg = Bool()
|
||||
msg.data = True
|
||||
self.master_stop_publisher.publish(msg)
|
||||
|
||||
def restrictions_changed(self, restrictions):
|
||||
msg = RestrictionsMsg()
|
||||
|
||||
msg.distance.data = restrictions.distance
|
||||
msg.linear_velocity.data = restrictions.linear_velocity
|
||||
msg.angular_velocity.data = restrictions.angular_velocity
|
||||
|
||||
self.restrictions_publisher.publish(msg)
|
||||
|
||||
def qt_timer_callback(self):
|
||||
if self.publish_periodic_update is not None:
|
||||
self.publish_periodic_update()
|
||||
|
||||
if self.get_robots_list is not None:
|
||||
self.get_robots_list()
|
||||
|
||||
rclpy.spin_once(self, timeout_sec=0.001)
|
||||
@ -1,2 +0,0 @@
|
||||
[flake8]
|
||||
max-line-length = 110
|
||||
@ -1,6 +0,0 @@
|
||||
|
||||
|
||||
class StopState:
|
||||
STOPPED = 1
|
||||
RUNNING = 2
|
||||
UNKNOWN = 3
|
||||
@ -1,6 +1,6 @@
|
||||
from python_qt_binding.QtCore import QObject
|
||||
from python_qt_binding.QtCore import pyqtSignal
|
||||
from .robot_info import RobotInfo
|
||||
from robot_info import RobotInfo
|
||||
|
||||
class CallbackHandler(QObject):
|
||||
robot_info_signal = pyqtSignal(object)
|
||||
110
scripts/master_plugin.py
Executable file
110
scripts/master_plugin.py
Executable file
@ -0,0 +1,110 @@
|
||||
from ros_master_wrapper import ROSWrapper
|
||||
from qt_master_wrapper import QtWrapper
|
||||
from master_restrictions import Restrictions
|
||||
|
||||
from master_info import MasterInfo
|
||||
from master_widget import MasterWidget
|
||||
|
||||
from qt_gui.plugin import Plugin
|
||||
|
||||
from enums.stop_state import StopState as SS
|
||||
|
||||
from master_callback_handler import CallbackHandler
|
||||
|
||||
class MasterPlugin(Plugin):
|
||||
|
||||
def __init__(self,context):
|
||||
super(MasterPlugin, self).__init__(context)
|
||||
self.setObjectName('Master Plugin - L1.5 safety system')
|
||||
|
||||
self._widget = MasterWidget(context)
|
||||
self._ros_wrapper = ROSWrapper()
|
||||
self._qt_wrapper = QtWrapper(self._widget)
|
||||
self._master_info = MasterInfo()
|
||||
|
||||
self.cbhandler = CallbackHandler()
|
||||
self.cbhandler.connect_robot_info_signal(self._qt_wrapper.update_robot_info)
|
||||
self.cbhandler.connect_add_robot_signal(self._qt_wrapper.add_robot)
|
||||
self.cbhandler.connect_remove_robot_signal(self._qt_wrapper.remove_robot)
|
||||
|
||||
|
||||
# self._qt_wrapper.connect_signals()
|
||||
# self._qt_wrapper.set_sliders_to_initial_values()
|
||||
|
||||
|
||||
# Setup functions to get _master_info states from _ros_wrapper
|
||||
self._ros_wrapper.setup_get_master_stop_state_function(self._master_info.get_master_stop_state)
|
||||
self._ros_wrapper.setup_get_restrictions_function(self._master_info.get_restrictions)
|
||||
|
||||
self.set_callback_functions()
|
||||
self._qt_wrapper.connect_signals()
|
||||
|
||||
# Stopping master at the beginning
|
||||
self.master_stopped()
|
||||
|
||||
def set_callback_functions(self):
|
||||
self.set_ROS_callback_functions()
|
||||
self.set_Qt_callback_functions()
|
||||
|
||||
def set_ROS_callback_functions(self):
|
||||
self._ros_wrapper.set_robots_list_update_callback(self.handle_robots_list_update)
|
||||
self._ros_wrapper.set_robot_info_update_callback(self.handle_robot_info_update)
|
||||
|
||||
def set_Qt_callback_functions(self):
|
||||
self._qt_wrapper.set_master_stop_state_updated_callback(self.handle_master_stop_state_updated)
|
||||
self._qt_wrapper.set_restrictions_updated_callback(self.handle_restrictions_updated)
|
||||
|
||||
|
||||
def handle_robots_list_update(self,robots_id_list):
|
||||
self.update_robots_list(robots_id_list)
|
||||
|
||||
def handle_robot_info_update(self,robot_info):
|
||||
self.update_robot_info(robot_info)
|
||||
|
||||
def handle_master_stop_state_updated(self):
|
||||
if self._master_info.master_stop_state == SS.RUNNING:
|
||||
self.master_stopped()
|
||||
elif self._master_info.master_stop_state == SS.STOPPED:
|
||||
self.master_started()
|
||||
else:
|
||||
raise ValueError('master_stop_state value undefined')
|
||||
|
||||
def handle_restrictions_updated(self,restrictions):
|
||||
self.restrictions_changed(restrictions)
|
||||
|
||||
def update_robots_list(self,robots_id_list):
|
||||
|
||||
for robot_id in robots_id_list:
|
||||
if robot_id not in self._master_info.robots_id_list:
|
||||
self.new_robot_registered(robot_id)
|
||||
|
||||
for robot_id in self._master_info.robots_id_list:
|
||||
if robot_id not in robots_id_list:
|
||||
self.robot_unregistered(robot_id)
|
||||
|
||||
self._master_info.robots_id_list = robots_id_list
|
||||
|
||||
def new_robot_registered(self,robot_id):
|
||||
self._ros_wrapper.add_robot_subscriber(robot_id)
|
||||
self.cbhandler.add_robot_signal.emit(robot_id)
|
||||
|
||||
def robot_unregistered(self,robot_id):
|
||||
self._ros_wrapper.remove_robot_subscriber(robot_id)
|
||||
self.cbhandler.remove_robot_signal.emit(robot_id)
|
||||
|
||||
def update_robot_info(self,robot_info):
|
||||
self.cbhandler.robot_info_signal.emit(robot_info)
|
||||
|
||||
def master_started(self):
|
||||
self._master_info.master_stop_state = SS.RUNNING
|
||||
self._ros_wrapper.master_started()
|
||||
self._qt_wrapper.master_started()
|
||||
|
||||
def master_stopped(self):
|
||||
self._master_info.master_stop_state = SS.STOPPED
|
||||
self._ros_wrapper.master_stopped()
|
||||
self._qt_wrapper.master_stopped()
|
||||
|
||||
def restrictions_changed(self,restrictions):
|
||||
self._master_info.restrictions = restrictions
|
||||
self._ros_wrapper.restrictions_changed(restrictions)
|
||||
@ -1,6 +1,7 @@
|
||||
#!/usr/bin/env python
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
import rospkg
|
||||
|
||||
from python_qt_binding.QtWidgets import QWidget
|
||||
from python_qt_binding import loadUi
|
||||
|
||||
@ -8,7 +9,7 @@ class MasterWidget(QWidget):
|
||||
|
||||
def __init__(self,context):
|
||||
super(MasterWidget, self).__init__()
|
||||
ui_file = os.path.join(get_package_share_directory('safety_master_plugin'), 'ui', 'master_view.ui')
|
||||
ui_file = os.path.join(rospkg.RosPack().get_path('safety_master_plugin'), 'ui', 'master_view.ui')
|
||||
loadUi(ui_file, self)
|
||||
self.setObjectName('Master Plugin - L1.5 safety system')
|
||||
|
||||
@ -1,12 +1,16 @@
|
||||
#!/usr/bin/env python
|
||||
# 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_master_plugin.clutch_state import ClutchState as CS
|
||||
from safety_master_plugin.stop_state import StopState as SS
|
||||
from safety_master_plugin.master_restrictions import Restrictions
|
||||
from enums.clutch_state import ClutchState as CS
|
||||
from enums.stop_state import StopState as SS
|
||||
|
||||
from master_restrictions import Restrictions
|
||||
|
||||
from python_qt_binding.QtGui import QPixmap
|
||||
from python_qt_binding.QtGui import QTextCursor
|
||||
@ -20,25 +24,21 @@ from python_qt_binding.QtCore import Qt
|
||||
|
||||
|
||||
class QtWrapper:
|
||||
|
||||
def __init__(self,widget):
|
||||
self.widget = widget
|
||||
self.displayed_robots_id_list = []
|
||||
|
||||
self.master_stop_state_updated_callback = None
|
||||
self.restrictions_updated_callback = None
|
||||
|
||||
self.master_stop_state = None
|
||||
self.init_robots_list()
|
||||
|
||||
# self.disable_sliders_tracking()
|
||||
|
||||
ui_directory_path = "{0}/ui".format(
|
||||
get_package_share_directory("safety_master_plugin")
|
||||
)
|
||||
ui_directory_path = '{0}/ui'.format(rospkg.RosPack().get_path('safety_master_plugin'))
|
||||
|
||||
self.distance_pixmap = QPixmap(f"{ui_directory_path}/Distance.png")
|
||||
self.angular_pixmap = QPixmap(f"{ui_directory_path}/Angular.png")
|
||||
self.linear_pixmap = QPixmap(f"{ui_directory_path}/Linear.png")
|
||||
self.distance_pixmap = QPixmap('{0}/Distance.png'.format(ui_directory_path))
|
||||
self.angular_pixmap = QPixmap('{0}/Angular.png'.format(ui_directory_path))
|
||||
self.linear_pixmap = QPixmap('{0}/Linear.png'.format(ui_directory_path))
|
||||
self.widget.distanceImage.setPixmap(self.distance_pixmap)
|
||||
self.widget.angularImage.setPixmap(self.angular_pixmap)
|
||||
self.widget.linearImage.setPixmap(self.linear_pixmap)
|
||||
@ -54,43 +54,33 @@ class QtWrapper:
|
||||
|
||||
def init_robots_list(self):
|
||||
self.stdItemModel = QStandardItemModel( 0, 6, self.widget.robotsList)
|
||||
self.stdItemModel.setHeaderData(0, Qt.Horizontal, "Robot")
|
||||
self.stdItemModel.setHeaderData(1, Qt.Horizontal, "Stan baterii")
|
||||
self.stdItemModel.setHeaderData(2, Qt.Horizontal, "Stan robota")
|
||||
self.stdItemModel.setHeaderData(3, Qt.Horizontal, "Stan sprzęgła")
|
||||
self.stdItemModel.setHeaderData(4, Qt.Horizontal, "Prędk. lin.")
|
||||
self.stdItemModel.setHeaderData(5, Qt.Horizontal, "Prędk. obr.")
|
||||
self.stdItemModel.setHeaderData( 0, Qt.Horizontal, 'Robot' )
|
||||
self.stdItemModel.setHeaderData( 1, Qt.Horizontal, 'Stan baterii' )
|
||||
self.stdItemModel.setHeaderData( 2, Qt.Horizontal, 'Stan robota' )
|
||||
self.stdItemModel.setHeaderData( 3, Qt.Horizontal, 'Stan sprzęgła' )
|
||||
self.stdItemModel.setHeaderData( 4, Qt.Horizontal, 'Prędk. lin.' )
|
||||
self.stdItemModel.setHeaderData( 5, Qt.Horizontal, 'Prędk. obr.' )
|
||||
|
||||
self.widget.robotsList.setModel(self.stdItemModel)
|
||||
|
||||
self.widget.robotsList.horizontalHeader().setSectionResizeMode(
|
||||
0, QHeaderView.Stretch
|
||||
)
|
||||
self.widget.robotsList.horizontalHeader().setSectionResizeMode(
|
||||
1, QHeaderView.Stretch
|
||||
)
|
||||
self.widget.robotsList.horizontalHeader().setSectionResizeMode(
|
||||
2, QHeaderView.Stretch
|
||||
)
|
||||
self.widget.robotsList.horizontalHeader().setSectionResizeMode(
|
||||
3, QHeaderView.Stretch
|
||||
)
|
||||
self.widget.robotsList.horizontalHeader().setSectionResizeMode(
|
||||
4, QHeaderView.Stretch
|
||||
)
|
||||
self.widget.robotsList.horizontalHeader().setSectionResizeMode(
|
||||
5, QHeaderView.Stretch
|
||||
)
|
||||
self.widget.robotsList.horizontalHeader().setSectionResizeMode(0, QHeaderView.Stretch)
|
||||
self.widget.robotsList.horizontalHeader().setSectionResizeMode(1, QHeaderView.Stretch)
|
||||
self.widget.robotsList.horizontalHeader().setSectionResizeMode(2, QHeaderView.Stretch)
|
||||
self.widget.robotsList.horizontalHeader().setSectionResizeMode(3, QHeaderView.Stretch)
|
||||
self.widget.robotsList.horizontalHeader().setSectionResizeMode(4, QHeaderView.Stretch)
|
||||
self.widget.robotsList.horizontalHeader().setSectionResizeMode(5, QHeaderView.Stretch)
|
||||
|
||||
self.widget.robotsList.horizontalHeader().setSectionsMovable(False)
|
||||
# self.widget.robotsList.setSelectionBehavior(QAbstractItemView.SelectRows)
|
||||
self.widget.robotsList.setSelectionMode(QAbstractItemView.NoSelection)
|
||||
|
||||
|
||||
def disable_sliders_tracking(self):
|
||||
self.widget.distanceSlider.setTracking(False)
|
||||
self.widget.angularSlider.setTracking(False)
|
||||
self.widget.linearSlider.setTracking(False)
|
||||
|
||||
|
||||
def set_master_stop_state_updated_callback(self,callback_function):
|
||||
self.master_stop_state_updated_callback = callback_function
|
||||
|
||||
@ -98,9 +88,9 @@ class QtWrapper:
|
||||
self.restrictions_updated_callback = callback_function
|
||||
|
||||
def handle_restrictions_update(self):
|
||||
distance = float(self.widget.distanceSlider.value()) / 100
|
||||
angular_velocity = float(self.widget.angularSlider.value()) / 100
|
||||
linear_velocity = float(self.widget.linearSlider.value()) / 100
|
||||
distance = (float(self.widget.distanceSlider.value())/100)
|
||||
angular_velocity = (float(self.widget.angularSlider.value())/100)
|
||||
linear_velocity = (float(self.widget.linearSlider.value())/100)
|
||||
|
||||
self.widget.distanceLabel.setText("{:.2f}".format(distance))
|
||||
self.widget.angularLabel.setText("{:.2f}".format(angular_velocity))
|
||||
@ -110,80 +100,67 @@ class QtWrapper:
|
||||
|
||||
self.restrictions_updated_callback(restrictions)
|
||||
|
||||
|
||||
def connect_signals(self):
|
||||
self.widget.distanceSlider.valueChanged.connect(self.handle_restrictions_update)
|
||||
self.widget.angularSlider.valueChanged.connect(self.handle_restrictions_update)
|
||||
self.widget.linearSlider.valueChanged.connect(self.handle_restrictions_update)
|
||||
self.widget.masterstopButton.clicked.connect(
|
||||
self.handle_master_stop_button_clicked
|
||||
)
|
||||
self.widget.masterstopButton.clicked.connect(self.handle_masterstopButton_clicked)
|
||||
|
||||
self.set_sliders_to_initial_values()
|
||||
|
||||
def handle_master_stop_button_clicked(self):
|
||||
def handle_masterstopButton_clicked(self):
|
||||
if self.master_stop_state == SS.RUNNING:
|
||||
self.master_stop_state_updated_callback()
|
||||
else:
|
||||
reply = QMessageBox.warning(
|
||||
self.widget,
|
||||
"UWAGA",
|
||||
"Na pewno chcesz odblokować wszystkie roboty?",
|
||||
QMessageBox.Yes,
|
||||
QMessageBox.No,
|
||||
)
|
||||
reply = QMessageBox.warning(self.widget,"UWAGA","Na pewno chcesz odblokować wszystkie roboty?",QMessageBox.Yes,QMessageBox.No)
|
||||
|
||||
if reply == QMessageBox.Yes:
|
||||
self.master_stop_state_updated_callback()
|
||||
|
||||
def display_master_stop_on(self):
|
||||
self.widget.masterstopButton.setStyleSheet(
|
||||
"QPushButton { color: black; background-color: red; font: bold 20px}"
|
||||
)
|
||||
self.widget.masterstopButton.setText("Odblokuj roboty")
|
||||
self.widget.masterstopButton.setStyleSheet("QPushButton { color: black; background-color: green; font: bold 20px}")
|
||||
self.widget.masterstopButton.setText('Zatrzymaj roboty')
|
||||
|
||||
def display_master_stop_off(self):
|
||||
self.widget.masterstopButton.setStyleSheet(
|
||||
"QPushButton { color: black; background-color: green; font: bold 20px}"
|
||||
)
|
||||
self.widget.masterstopButton.setText("Zatrzymaj roboty")
|
||||
self.widget.masterstopButton.setStyleSheet("QPushButton { color: black; background-color: red; font: bold 20px}")
|
||||
self.widget.masterstopButton.setText('Odblokuj roboty')
|
||||
|
||||
|
||||
def update_robot_info(self,robot_info):
|
||||
items_to_add = []
|
||||
items_to_add.append(QStandardItem(f"pioneer{robot_info.robot_id}"))
|
||||
items_to_add.append(QStandardItem("{:.2f}".format(robot_info.battery_voltage)))
|
||||
items_to_add.append(QStandardItem('PIONIER{}'.format(robot_info.robot_id)))
|
||||
items_to_add.append(QStandardItem('{:.2f}'.format(robot_info.battery_voltage)))
|
||||
|
||||
if robot_info.state == SS.RUNNING:
|
||||
item = QStandardItem("Działa")
|
||||
item = QStandardItem('Działa')
|
||||
item.setBackground(QBrush(Qt.green))
|
||||
items_to_add.append(item)
|
||||
elif robot_info.state == SS.STOPPED:
|
||||
item = QStandardItem("Zatrzymany")
|
||||
item = QStandardItem('Zatrzymany')
|
||||
item.setBackground(QBrush(Qt.red))
|
||||
items_to_add.append(item)
|
||||
else:
|
||||
raise ValueError("Undefined value of robot_info.robot_state")
|
||||
raise ValueError('Undefined value of robot_info.robot_state')
|
||||
|
||||
if robot_info.clutch == CS.ENGAGED:
|
||||
item = QStandardItem("Sprzęgnięte")
|
||||
item = QStandardItem('Sprzęgnięte')
|
||||
item.setBackground(QBrush(Qt.green))
|
||||
items_to_add.append(item)
|
||||
elif robot_info.clutch == CS.DISENGAGED:
|
||||
item = QStandardItem("Rozprzęgnięte")
|
||||
item = QStandardItem('Rozprzęgnięte')
|
||||
item.setBackground(QBrush(Qt.red))
|
||||
items_to_add.append(item)
|
||||
else:
|
||||
raise ValueError("Undefined value of robot_info.clutch_state")
|
||||
raise ValueError('Undefined value of robot_info.clutch_state')
|
||||
|
||||
linear_vel = math.sqrt(
|
||||
robot_info.linear_velocity[0] ** 2 + robot_info.linear_velocity[1] ** 2
|
||||
)
|
||||
items_to_add.append(QStandardItem("{:.2f}".format(linear_vel)))
|
||||
items_to_add.append(
|
||||
QStandardItem("{:.2f}".format(robot_info.angular_velocity[2]))
|
||||
)
|
||||
linear_vel = math.sqrt(robot_info.linear_velocity[0]**2 + robot_info.linear_velocity[1]**2)
|
||||
items_to_add.append(QStandardItem('{:.2f}'.format(linear_vel)))
|
||||
|
||||
items_list = self.stdItemModel.findItems(
|
||||
f"pioneer{robot_info.robot_id}", Qt.MatchExactly
|
||||
)
|
||||
items_to_add.append(QStandardItem('{:.2f}'.format(robot_info.angular_velocity[2])))
|
||||
|
||||
|
||||
items_list = self.stdItemModel.findItems('PIONIER{0}'.format(robot_info.robot_id),Qt.MatchExactly)
|
||||
if len(items_list) > 0:
|
||||
for item in items_list:
|
||||
row_number = item.row()
|
||||
@ -195,73 +172,70 @@ class QtWrapper:
|
||||
item.setEditable(0)
|
||||
self.stdItemModel.setItem(row_number,i,item)
|
||||
|
||||
def when_robots_disabled(self):
|
||||
def master_stopped(self):
|
||||
self.master_stop_state = SS.STOPPED
|
||||
self.display_master_stop_on()
|
||||
self.log_info("Przycisk masterSTOP zostal naciśnięty. Zatrzymuję roboty")
|
||||
|
||||
def when_robots_enabled(self):
|
||||
self.master_stop_state = SS.RUNNING
|
||||
self.display_master_stop_off()
|
||||
self.log_info("Przycisk masterSTOP odciśnięty")
|
||||
self.log_info('Przycisk masterSTOP zostal naciśnięty. Zatrzymuję roboty')
|
||||
|
||||
def master_started(self):
|
||||
self.master_stop_state = SS.RUNNING
|
||||
self.display_master_stop_on()
|
||||
self.log_info('Przycisk masterSTOP odciśnięty')
|
||||
|
||||
|
||||
def add_robot(self,robot_id):
|
||||
|
||||
if robot_id not in self.displayed_robots_id_list:
|
||||
new_robot_info = QStandardItem("pioneer{}".format(robot_id))
|
||||
new_robot_info=QStandardItem('PIONIER{}'.format(robot_id))
|
||||
new_robot_info.setTextAlignment(Qt.AlignCenter)
|
||||
new_robot_info.setEditable(0)
|
||||
# new_robot_info.setBackground(QBrush(Qt.white))
|
||||
self.stdItemModel.setItem(self.stdItemModel.rowCount(),0,new_robot_info)
|
||||
|
||||
self.displayed_robots_id_list.append(robot_id)
|
||||
self.log_info(f"pioneer{robot_id} połączony")
|
||||
self.log_info('PIONIER{0} połączony'.format(robot_id))
|
||||
else:
|
||||
raise RuntimeError("Adding robot, which is already in master GUI")
|
||||
raise RuntimeError('Adding robot, which is already in master GUI')
|
||||
|
||||
def remove_robot(self,robot_id):
|
||||
items_list = self.stdItemModel.findItems(f"pioneer{robot_id}", Qt.MatchExactly)
|
||||
items_list = self.stdItemModel.findItems('PIONIER{0}'.format(robot_id),Qt.MatchExactly)
|
||||
|
||||
if len(items_list) > 0:
|
||||
for item in items_list:
|
||||
row_number = item.row()
|
||||
self.stdItemModel.removeRow(row_number)
|
||||
|
||||
self.log_info(f"pioneer{robot_id} rozłączony")
|
||||
self.log_info('PIONIER{0} rozłączony'.format(robot_id))
|
||||
self.displayed_robots_id_list.remove(robot_id)
|
||||
|
||||
def log_info(self,info_text):
|
||||
time = datetime.datetime.now().strftime("[%H:%M:%S]")
|
||||
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(
|
||||
f'<font color="black"> {time} {info_text}</font><br>'
|
||||
)
|
||||
# self.widget.logsBrowser.insertHtml('<font color="black">' + str(time) + '. ' + info_text + '</font><br>')
|
||||
self.widget.logsBrowser.insertHtml('<font color="black"> {0} {1}</font><br>'.format(time,info_text))
|
||||
self.scroll_to_bottom()
|
||||
|
||||
def log_warning(self,warning_text):
|
||||
time = datetime.datetime.now().strftime("[%H:%M:%S]")
|
||||
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()
|
||||
|
||||
def log_error(self,error_text):
|
||||
time = datetime.datetime.now().strftime("[%H:%M:%S]")
|
||||
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()
|
||||
|
||||
def scroll_to_bottom(self):
|
||||
30
scripts/robot_info.py
Normal file
30
scripts/robot_info.py
Normal file
@ -0,0 +1,30 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
from enums.clutch_state import ClutchState as CS
|
||||
from enums.stop_state import StopState as SS
|
||||
|
||||
class RobotInfo:
|
||||
|
||||
def __init__(self,robot_id):
|
||||
self.robot_id = robot_id
|
||||
self.battery_voltage = None
|
||||
self.linear_velocity = None
|
||||
self.angular_velocity = None
|
||||
self.state = None
|
||||
self.clutch = None
|
||||
|
||||
def update_robot_info(self,robot_info_msg):
|
||||
self.robot_id = robot_info_msg.robot_id.data
|
||||
self.battery_voltage = robot_info_msg.battery_voltage.data
|
||||
self.linear_velocity = [robot_info_msg.twist.linear.x,robot_info_msg.twist.linear.y,robot_info_msg.twist.linear.z]
|
||||
self.angular_velocity = [robot_info_msg.twist.angular.x,robot_info_msg.twist.angular.y,robot_info_msg.twist.angular.z]
|
||||
|
||||
if robot_info_msg.state.data == True:
|
||||
self.state = SS.RUNNING
|
||||
else:
|
||||
self.state = SS.STOPPED
|
||||
|
||||
if robot_info_msg.clutch.data == True:
|
||||
self.clutch = CS.ENGAGED
|
||||
else:
|
||||
self.clutch = CS.DISENGAGED
|
||||
159
scripts/ros_master_wrapper.py
Normal file
159
scripts/ros_master_wrapper.py
Normal file
@ -0,0 +1,159 @@
|
||||
#!/usr/bin/env python
|
||||
import rospy
|
||||
import rospkg
|
||||
|
||||
from std_msgs.msg import Bool
|
||||
from rosaria_msgs.msg import RobotInfoMsg
|
||||
from rosaria_msgs.msg import RestrictionsMsg
|
||||
|
||||
from robot_info import RobotInfo
|
||||
from master_restrictions import Restrictions
|
||||
|
||||
from enums.stop_state import StopState as SS
|
||||
|
||||
class ROSWrapper:
|
||||
|
||||
def __init__(self):
|
||||
self.robots_list_update_callback = None
|
||||
self.robot_info_update_callback = None
|
||||
|
||||
self.robot_info_subscribers = {}
|
||||
self.master_stop_publisher = None
|
||||
self.restrictions_publisher = None
|
||||
|
||||
self.robots_list_timer = None
|
||||
self.periodic_timer = None
|
||||
|
||||
self.setup_subscribers_and_publishers()
|
||||
|
||||
|
||||
def setup_subscribers_and_publishers(self):
|
||||
self.master_stop_publisher = rospy.Publisher('/PIONIER/master_stop',Bool,queue_size = 1)
|
||||
self.restrictions_publisher = rospy.Publisher('/PIONIER/restrictions',RestrictionsMsg,queue_size = 1)
|
||||
|
||||
self.periodic_timer = rospy.Timer(rospy.Duration(0.5),self.publish_periodic_update)
|
||||
|
||||
def add_robot_subscriber(self,robot_id):
|
||||
if robot_id not in self.robot_info_subscribers:
|
||||
topic_name = '/PIONIER{0}/RosAria/robot_info'.format(robot_id)
|
||||
new_sub = rospy.Subscriber(topic_name, RobotInfoMsg, self.handle_robot_info_update)
|
||||
self.robot_info_subscribers[robot_id] = new_sub
|
||||
else:
|
||||
raise RuntimeError('Subscriber for PIONIER{0} already in dictionary'.format(robot_id))
|
||||
|
||||
def remove_robot_subscriber(self,robot_id):
|
||||
if robot_id in self.robot_info_subscribers:
|
||||
self.unsubscribe(self.robot_info_subscribers[robot_id])
|
||||
del self.robot_info_subscribers[robot_id]
|
||||
else:
|
||||
raise RuntimeError('Subscriber for PIONIER{0} not in dictionary. Cannot be removed'.format(robot_id))
|
||||
|
||||
def unsubscribe(self,subscriber):
|
||||
if subscriber != None:
|
||||
subscriber.unregister()
|
||||
|
||||
def unregister_publisher(self,publisher):
|
||||
if publisher != None:
|
||||
publisher.unregister()
|
||||
|
||||
def shutdown_timer(self,timer):
|
||||
if timer != None:
|
||||
timer.shutdown()
|
||||
|
||||
def publish_periodic_update(self,event):
|
||||
restrictions = self.get_restrictions()
|
||||
restrictions_msg = RestrictionsMsg()
|
||||
restrictions_msg.distance.data = restrictions.distance
|
||||
restrictions_msg.linear_velocity.data = restrictions.linear_velocity
|
||||
restrictions_msg.angular_velocity.data = restrictions.angular_velocity
|
||||
|
||||
stop_state = self.get_master_stop_state()
|
||||
stop_msg = Bool()
|
||||
|
||||
if stop_state == SS.RUNNING:
|
||||
stop_msg.data = True
|
||||
elif stop_state == SS.STOPPED:
|
||||
stop_msg.data = False
|
||||
else:
|
||||
stop_msg.data = False
|
||||
raise ValueError('stop_state UNKNOWN when attempting to publish periodic update')
|
||||
|
||||
self.restrictions_publisher.publish(restrictions_msg)
|
||||
self.master_stop_publisher.publish(stop_msg)
|
||||
|
||||
def cancel_subscribers_and_publishers(self):
|
||||
self.shutdown_timer(self.robots_list_timer)
|
||||
self.shutdown_timer(self.periodic_timer)
|
||||
|
||||
self.unregister_publisher(self.master_stop_publisher)
|
||||
self.unregister_publisher(self.restrictions_publisher)
|
||||
|
||||
for key, value in self.robot_info_subscribers.iteritems():
|
||||
self.unsubscribe(value)
|
||||
|
||||
def unregister_node(self):
|
||||
self.cancel_subscribers_and_publishers()
|
||||
rospy.signal_shutdown('Closing safety master plugin')
|
||||
|
||||
def setup_get_master_stop_state_function(self,function):
|
||||
self.get_master_stop_state = function
|
||||
|
||||
def setup_get_restrictions_function(self,function):
|
||||
self.get_restrictions = function
|
||||
|
||||
# ROSWrapper Callbacks
|
||||
def get_robots_list(self,event):
|
||||
robots_id_list = []
|
||||
|
||||
published_topics_list = rospy.get_published_topics(namespace='/')
|
||||
published_topics = []
|
||||
|
||||
for list_ in published_topics_list:
|
||||
published_topics.append(list_[0])
|
||||
|
||||
for topic in published_topics:
|
||||
if topic.find('RosAria') ==-1 or topic.find('robot_info') == -1:
|
||||
pass
|
||||
else:
|
||||
robot_number = topic.split('/')
|
||||
robot_number = robot_number[1]
|
||||
robot_number = robot_number[7:]
|
||||
if len(robot_number) > 0:
|
||||
robot_number = int(robot_number)
|
||||
robots_id_list.append(robot_number)
|
||||
|
||||
self.robots_list_update_callback(robots_id_list)
|
||||
|
||||
|
||||
def handle_robot_info_update(self,msg):
|
||||
_robot_info = RobotInfo(0)
|
||||
_robot_info.update_robot_info(msg)
|
||||
self.robot_info_update_callback(_robot_info)
|
||||
|
||||
|
||||
# MasterPlugin Callbacks
|
||||
def set_robots_list_update_callback(self,callback_function):
|
||||
self.robots_list_update_callback = callback_function
|
||||
self.robots_list_timer = rospy.Timer(rospy.Duration(0.5),self.get_robots_list)
|
||||
|
||||
def set_robot_info_update_callback(self,callback_function):
|
||||
self.robot_info_update_callback = callback_function
|
||||
|
||||
def master_started(self):
|
||||
msg = Bool()
|
||||
msg.data = True
|
||||
self.master_stop_publisher.publish(msg)
|
||||
|
||||
def master_stopped(self):
|
||||
msg = Bool()
|
||||
msg.data = False
|
||||
self.master_stop_publisher.publish(msg)
|
||||
|
||||
def restrictions_changed(self,restrictions):
|
||||
msg = RestrictionsMsg()
|
||||
|
||||
msg.distance.data = restrictions.distance
|
||||
msg.linear_velocity.data = restrictions.linear_velocity
|
||||
msg.angular_velocity.data = restrictions.angular_velocity
|
||||
|
||||
self.restrictions_publisher.publish(msg)
|
||||
@ -1,9 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import sys
|
||||
|
||||
from safety_master_plugin.master_plugin import MasterPlugin
|
||||
from rqt_gui.main import Main
|
||||
|
||||
|
||||
sys.exit(Main().main(sys.argv, standalone='safety_master_plugin.master_plugin.MasterPlugin'))
|
||||
@ -1,5 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/safety_master_plugin
|
||||
[install]
|
||||
install_scripts=$base/lib/safety_master_plugin
|
||||
|
||||
35
setup.py
35
setup.py
@ -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_master_plugin'
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.2.0',
|
||||
packages=[package_name],
|
||||
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': ['master_plugin_node = safety_master_plugin.master_plugin_node:main'],
|
||||
},
|
||||
setup_args = generate_distutils_setup(
|
||||
package_dir={'': 'scripts'}
|
||||
)
|
||||
|
||||
setup(**setup_args)
|
||||
Loading…
Reference in New Issue
Block a user