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