diff --git a/src/nautilus_scripts/setup.py b/src/nautilus_scripts/setup.py index f34243b..4aa013f 100644 --- a/src/nautilus_scripts/setup.py +++ b/src/nautilus_scripts/setup.py @@ -8,4 +8,4 @@ packages=['subway_car', 'nautilus_utils', 'coral_bleaching'], package_dir={'': 'src'}) -setup(**setup_args) \ No newline at end of file +setup(**setup_args) diff --git a/src/robot_module/CMakeLists.txt b/src/robot_module/CMakeLists.txt index a20227e..aba0938 100644 --- a/src/robot_module/CMakeLists.txt +++ b/src/robot_module/CMakeLists.txt @@ -8,8 +8,10 @@ project(robot_module) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS + roscpp rospy std_msgs + message_generation ) ## System dependencies are found with CMake's conventions @@ -53,11 +55,11 @@ catkin_python_setup() # ) ## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) +add_service_files( + FILES + acquire_lock.srv + release_lock.srv +) ## Generate actions in the 'action' folder # add_action_files( @@ -67,10 +69,10 @@ catkin_python_setup() # ) ## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs -# ) +generate_messages( + DEPENDENCIES + std_msgs +) ################################################ ## Declare ROS dynamic reconfigure parameters ## @@ -193,6 +195,8 @@ include_directories( catkin_install_python(PROGRAMS scripts/test.py + scripts/example_lock_client.py + scripts/lock_server.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/src/robot_module/doc/lock_service.puml b/src/robot_module/doc/lock_service.puml new file mode 100644 index 0000000..dff6531 --- /dev/null +++ b/src/robot_module/doc/lock_service.puml @@ -0,0 +1,27 @@ +@startuml Locking Service Sequence Diagram +title Locking Service Sequence Diagram +participant Auto_Script as auto +participant UI as ui +participant Lock_Service as ls + +ls -> ui : Sends enable message over topic + +... + +auto -> ls : Sends a lock request +ls -> ui : Send disable message over topic +ls --> auto : Send lock confirmation + +... + +Second_Auto_Script -> ls : Sends lock request +return Send failure message +Second_Auto_Script -> Second_Auto_Script : Periodically ping\nto try and get lock + +... + +auto -> ls : Send lock release message +return Acknowledge lock release (no need to wait) +ls -> ui : Send enable message over topic + +@enduml diff --git a/src/robot_module/package.xml b/src/robot_module/package.xml index 3727d24..29533f9 100644 --- a/src/robot_module/package.xml +++ b/src/robot_module/package.xml @@ -50,10 +50,15 @@ catkin rospy + roscpp std_msgs + message_generation rospy + roscpp std_msgs + message_runtime rospy + roscpp std_msgs diff --git a/src/robot_module/scripts/example_lock_client.py b/src/robot_module/scripts/example_lock_client.py new file mode 100644 index 0000000..38c50f6 --- /dev/null +++ b/src/robot_module/scripts/example_lock_client.py @@ -0,0 +1,23 @@ +#!/usr/bin/env python3 + +from locks.lock_service_client import LockClient +import time +import rospy + +if __name__ == '__main__': + rospy.init_node('test_lock_client') + rospy.on_shutdown(lambda: rospy.loginfo("shutting down test client")) + + # Test case 1 : manage lock using the with statement + with LockClient('test_client') as lc: + time.sleep(0.25) + rospy.loginfo("lock acquired automatically") + rospy.loginfo("lock released automatically") + + + # Test case 2 : manage lock manually + lc = LockClient('test_client') + lc.lock() + rospy.loginfo("lock acquired manually") + lc.unlock() + rospy.loginfo("lock released manually") diff --git a/src/robot_module/scripts/lock_server.py b/src/robot_module/scripts/lock_server.py new file mode 100644 index 0000000..f469aea --- /dev/null +++ b/src/robot_module/scripts/lock_server.py @@ -0,0 +1,7 @@ +#!/usr/bin/env python3 + +from locks.lock_service_server import LockServer + +if __name__ == '__main__': + ls = LockServer() + ls.init_server() diff --git a/src/robot_module/scripts/test.py b/src/robot_module/scripts/test.py index 0056bdd..1a2f684 100644 --- a/src/robot_module/scripts/test.py +++ b/src/robot_module/scripts/test.py @@ -1,4 +1,6 @@ -from robot_module import RobotModule +#!/usr/bin/env python3 + +from robot_module.main import RobotModule import time robot = RobotModule("test") diff --git a/src/robot_module/setup.py b/src/robot_module/setup.py index b79b5dd..7463071 100644 --- a/src/robot_module/setup.py +++ b/src/robot_module/setup.py @@ -4,7 +4,7 @@ from catkin_pkg.python_setup import generate_distutils_setup setup_args = generate_distutils_setup( - packages=['robot_module'], + packages=['robot_module', 'locks', 'services'], package_dir={'': 'src'} ) diff --git a/src/robot_module/src/locks/lock_service_client.py b/src/robot_module/src/locks/lock_service_client.py new file mode 100644 index 0000000..9139cea --- /dev/null +++ b/src/robot_module/src/locks/lock_service_client.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python3 + +from robot_module.srv import acquire_lock, release_lock +from std_msgs.msg import String +import rospy +import time + +class LockClient: + def __init__(self, name): + self.name = name + + + def __enter__(self): + acquire_attempts = 0 + lock_acquired = self.lock() + + # use exponential backoff to try again without flooding network + while not lock_acquired: + rospy.loginfo("Couldn't acquire the lock, trying again") + time.sleep(1 * (2**acquire_attempts)) + acquire_attempts += 1 + + lock_acquired = self.lock() + + + def __exit__(self, exc_type, exc_value, traceback): + self.unlock() + + + def lock(self) -> bool: + rospy.wait_for_service('acquire_service') + try: + acquire_lock_fn = rospy.ServiceProxy('acquire_service', acquire_lock) + lock_server_response = acquire_lock_fn(self.name) + return lock_server_response.result + except rospy.ServiceException as e: + rospy.logerr("couldn't access acquire_service service") + return False + + + def unlock(self): + rospy.wait_for_service('release_service') + try: + release_lock_fn = rospy.ServiceProxy('release_service', release_lock) + release_lock_fn(self.name) + except rospy.ServiceException as e: + rospy.logerr("couldn't access release_service service... something's broken, destroy everything and try again") diff --git a/src/robot_module/src/locks/lock_service_server.py b/src/robot_module/src/locks/lock_service_server.py new file mode 100644 index 0000000..abda581 --- /dev/null +++ b/src/robot_module/src/locks/lock_service_server.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python3 + +from threading import Lock +from robot_module.srv import acquire_lock, release_lock +from std_msgs.msg import String, Bool +import rospy + +# Consider tossing this into a command line parameter for on-the-fly retargeting +ui_enablement_topic = '/ui/enablement' + +class LockServer: + def __init__(self): + self.lock_holder = None + self.ui_publisher = None + self.mutex = Lock() + + + def handle_acquire_request(self, req): + with self.mutex: + req_name = req.requester + + # If no autonomous script is holding on, preempt our UI + if self.lock_holder is None: + self.lock_holder = req_name + self.ui_publisher.publish(False) + return True + + # Be forgiving and allow a single caller to acquire a lock more than once + return self.lock_holder == req_name + + + def handle_release_request(self, req): + with self.mutex: + req_name = req.requester + + # If the client holding the lock is releasing, give control back to UI + if self.lock_holder is not None and req_name == self.lock_holder: + self.lock_holder = None + self.ui_publisher.publish(True) + return True + + return False + + + def init_server(self): + rospy.init_node('lock_server') + rospy.on_shutdown(lambda : rospy.loginfo("Shutting down Lock Server")) + + rospy.loginfo("Starting Lock Server") + self.ui_publisher = rospy.Publisher(ui_enablement_topic, Bool, queue_size=1) + + # services can't take in extra parameters like a subscriber can, + # so we have to use this funny workaround + # https://answers.ros.org/question/247540/pass-parameters-to-a-service-handler-in-rospy/ + acquire_service = rospy.Service('acquire_service', acquire_lock, lambda msg: self.handle_acquire_request(msg)) + release_service = rospy.Service('release_service', release_lock, lambda msg: self.handle_release_request(msg)) + + rospy.spin() diff --git a/src/robot_module/src/robot_module/__init__.py b/src/robot_module/src/robot_module/__init__.py deleted file mode 100644 index a9d3ba5..0000000 --- a/src/robot_module/src/robot_module/__init__.py +++ /dev/null @@ -1,3 +0,0 @@ -from .main import RobotModule - -__all__ = ["RobotModule"] diff --git a/src/robot_module/src/robot_module/image_sub.py b/src/robot_module/src/robot_module/image_sub.py index 5fa1aba..a747e10 100644 --- a/src/robot_module/src/robot_module/image_sub.py +++ b/src/robot_module/src/robot_module/image_sub.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 + import rospy from .subscriber import ServerSub diff --git a/src/robot_module/src/robot_module/main.py b/src/robot_module/src/robot_module/main.py index c1e735f..3c23abc 100644 --- a/src/robot_module/src/robot_module/main.py +++ b/src/robot_module/src/robot_module/main.py @@ -1,5 +1,7 @@ +#!/usr/bin/env python3 + import rospy -from .services.movement import Movement +from services.movement import Movement # Service topics # subscriber_topics = { diff --git a/src/robot_module/src/robot_module/services/movement.py b/src/robot_module/src/services/movement.py similarity index 100% rename from src/robot_module/src/robot_module/services/movement.py rename to src/robot_module/src/services/movement.py diff --git a/src/robot_module/src/robot_module/services/service.py b/src/robot_module/src/services/service.py similarity index 100% rename from src/robot_module/src/robot_module/services/service.py rename to src/robot_module/src/services/service.py diff --git a/src/robot_module/srv/acquire_lock.srv b/src/robot_module/srv/acquire_lock.srv new file mode 100644 index 0000000..5106a13 --- /dev/null +++ b/src/robot_module/srv/acquire_lock.srv @@ -0,0 +1,3 @@ +string requester +--- +bool result \ No newline at end of file diff --git a/src/robot_module/srv/release_lock.srv b/src/robot_module/srv/release_lock.srv new file mode 100644 index 0000000..5106a13 --- /dev/null +++ b/src/robot_module/srv/release_lock.srv @@ -0,0 +1,3 @@ +string requester +--- +bool result \ No newline at end of file