Implement Behaviors to Interact with ROS Services

docs/sources/conf.py

@@ -146,6 +146,7 @@
     'ros2topic', 'ros2topic.api',
     'sensor_msgs', 'sensor_msgs.msg',
     'std_msgs', 'std_msgs.msg',
+    'std_srvs', 'std_srvs.srv',
     'tf2_ros',
     'unique_identifier_msgs', 'unique_identifier_msgs.msg'
 ]

package.xml

@@ -34,6 +34,7 @@
   <depend>rclpy</depend>
   <depend>ros2topic</depend>
   <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
   <depend>unique_identifier_msgs</depend>
 
   <!-- behaviour dependencies (subscribers, transforms, ...) -->

py_trees_ros/__init__.py

@@ -23,6 +23,7 @@
 from . import mock
 from . import programs
 from . import publishers
+from . import service_clients
 from . import subscribers
 from . import transforms
 from . import trees

py_trees_ros/action_clients.py

@@ -58,7 +58,7 @@ class FromBlackboard(py_trees.behaviour.Behaviour):
             name="WaitForGoal",
             variable_name="/my_goal"
         )
-        action_client = py_trees_ros.aciton_clients.FromBlackboard(
+        action_client = py_trees_ros.action_clients.FromBlackboard(
             action_type=py_trees_actions.Dock,
             action_name="dock",
             name="ActionClient"
@@ -153,6 +153,8 @@ def setup(self, **kwargs):
         result = None
         if self.wait_for_server_timeout_sec > 0.0:
             result = self.action_client.wait_for_server(timeout_sec=self.wait_for_server_timeout_sec)
+        elif self.wait_for_server_timeout_sec == 0.0:
+            result = True # don't wait and don't check if the server is ready
         else:
             iterations = 0
             period_sec = -1.0*self.wait_for_server_timeout_sec

py_trees_ros/mock/__init__.py

@@ -15,3 +15,5 @@
 
 from . import actions
 from . import dock
+from . import services
+from . import set_bool

py_trees_ros/mock/services.py

@@ -0,0 +1,78 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+#
+# License: BSD
+#   https://raw.githubusercontent.com/splintered-reality/py_trees_ros_tutorials/devel/LICENSE
+#
+##############################################################################
+# Documentation
+##############################################################################
+
+"""
+Service server templates.
+"""
+
+##############################################################################
+# Imports
+##############################################################################
+
+import time
+
+import rclpy
+
+##############################################################################
+# Service Server
+##############################################################################
+#
+# References:
+#
+#  service client        : https://github.com/ros2/rclpy/blob/rolling/rclpy/rclpy/client.py
+#  service server        : https://github.com/ros2/rclpy/blob/rolling/rclpy/rclpy/service.py
+#  service examples      : https://github.com/ros2/examples/tree/rolling/rclpy/services
+
+class GenericServer(object):
+    """
+    Generic service server that can be used for testing.
+
+    Args:
+        node_name (:obj:`str`): name of the node
+        service_name (:obj:`str`): name of the service
+        service_type (:obj:`type`): type of the service
+        sleep_time (:obj:`float`): time to sleep before returning a response
+        callback (:obj:`Optional[callable]`): callback to execute when the service is called
+    """
+    def __init__(self,
+                 node_name,
+                 service_name,
+                 service_type,
+                 sleep_time=1.0,
+                 callback=None):
+        self.node = rclpy.create_node(node_name)
+
+        self.sleep_time = sleep_time
+        self.callback = callback
+
+        # Create the service
+        self.server = self.node.create_service(
+            service_type,
+            service_name,
+            self.execute_callback
+        )
+
+    def execute_callback(self, request, response):
+        """
+        Execute the callback and populate the response.
+        """
+        if self.callback is not None:
+            return self.callback(request, response)
+
+        # Do nothing
+        time.sleep(self.sleep_time)
+        return response
+
+    def shutdown(self):
+        """
+        Cleanup
+        """
+        self.server.destroy()
+        self.node.destroy_node()

py_trees_ros/mock/set_bool.py

@@ -0,0 +1,55 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+#
+# License: BSD
+#   https://raw.githubusercontent.com/splintered-reality/py_trees_ros/devel/LICENSE
+#
+##############################################################################
+# Documentation
+##############################################################################
+
+"""
+Mocks a SetBool service
+"""
+
+
+##############################################################################
+# Imports
+##############################################################################
+
+import time
+
+from std_srvs.srv import SetBool
+
+from . import services
+
+##############################################################################
+# Class
+##############################################################################
+
+
+class SetBoolServer(services.GenericServer):
+    """
+    Simple server that docks if the goal is true, undocks otherwise.
+    """
+    SUCCESS_MESSAGE = "Succeeded in setting bool to True"
+    FAILURE_MESSAGE = "Failed to set bool to False"
+
+    def __init__(self, sleep_time=1.0):
+        super().__init__(
+            node_name="set_bool_server",
+            service_name="set_bool",
+            service_type=SetBool,
+            sleep_time=sleep_time,
+            callback=self.callback,
+        )
+
+    def callback(self, request, response):
+        time.sleep(self.sleep_time)
+        if request.data:
+            response.success = True
+            response.message = SetBoolServer.SUCCESS_MESSAGE
+        else:
+            response.success = False
+            response.message = SetBoolServer.FAILURE_MESSAGE
+        return response