Implement locking service

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

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)
+auto -> ui : Send enable message over topic
+
+@enduml

src/robot_module/package.xml

@@ -50,10 +50,15 @@
   <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>rospy</build_depend>
+  <build_depend>roscpp</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
   <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>message_runtime</exec_depend>
   <exec_depend>rospy</exec_depend>
+  <exec_depend>roscpp</exec_depend>
   <exec_depend>std_msgs</exec_depend>
 
 

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

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

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

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'}
 )
 

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

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

src/robot_module/src/robot_module/image_sub.py

@@ -1,3 +1,5 @@
+#!/usr/bin/env python3
+
 import rospy
 from .subscriber import ServerSub
 

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 = {

src/robot_module/srv/acquire_lock.srv

@@ -0,0 +1,3 @@
+string requester
+---
+bool result

src/robot_module/srv/release_lock.srv

@@ -0,0 +1,3 @@
+string requester
+---
+bool result