service call example created

src/custom_interfaces/CMakeLists.txt

@@ -36,6 +36,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/CustomMsg1.msg"
   "msg/CustomMsg2.msg"
   "srv/CustomSrv1.srv"
+  "srv/StateMachineSrv.srv"
   DEPENDENCIES builtin_interfaces
  )
 

src/custom_interfaces/srv/StateMachineSrv.srv

@@ -0,0 +1,4 @@
+string command
+float32 value
+---
+bool success

src/pubsub/pubsub/pubsub_library_v3.py

@@ -117,6 +117,7 @@ class MinimalServiceProvider(Node):
         self.SRV_TYPE = SRV_TYPE
         # Init above laying class "Node" 
         super().__init__(self.NODE_NAME)
+        print("\tStarting Service Host:\t%s"%(self.SRV_NAME))
         self.srv_host = self.create_service(self.SRV_TYPE, self.SRV_NAME, self.service_callback)
 
     def service_callback(self, request, response):
@@ -124,23 +125,31 @@ class MinimalServiceProvider(Node):
         * request: first half of srv-file
         * response: second half of srv-file
         
-        access values using:
+        access values using class access:
-        * request.<name>
+        * request.var_name
-        * response.<name>
+        * response.var_name
         """
-        self.get_logger().info("Service Call received")
+        self.get_logger().info("[srv: %s] Service Call Received"%(self.SRV_NAME))
-
         # Abstraction layer:
         response = self.user_defined(request, response) # Function defined below
-
         return response
 
 
     # Insert your callback code here
     def user_defined(self, request, response):
+        """ Write your user defined code here which will be run at every service call
+
+        Input:
+        * request: srv request class
+        Return:
+        * response: srv response class
+
+        Access these variables using class access, eg. request.var_name
+        """
         # Write user defined code here
         # Adapt to fit your service type
-        response.success = True
+        # You could overwrite this fdunctions with a new function when initializing this class 
+        response.success = True # Change this to fit your service type
         return response
 
 
@@ -153,10 +162,12 @@ class MinimalServiceClientAsync(Node):
         self.SRV_TYPE = SRV_TYPE
         # Init above laying class "Node" 
         super().__init__(self.NODE_NAME)
+        print("Starting Service Client:\t%s"%(self.SRV_NAME))
         self.srv_client = self.create_client(self.SRV_TYPE, self.SRV_NAME)
         while not self.srv_client.wait_for_service(timeout_sec=1.0):
             self.get_logger().info('service not available, waiting again...')
         self.request = self.SRV_TYPE.Request()
+        self.done_was_true = False
 
     def send_request(self, request):
         """
@@ -166,6 +177,7 @@ class MinimalServiceClientAsync(Node):
         * request.<var_name> = ....
         """
         self.request = request
+        self.done_was_true = False
         self.future = self.srv_client.call_async(self.request)
 
     def check_if_service_complete(self):
@@ -179,12 +191,11 @@ class MinimalServiceClientAsync(Node):
             try:
                 response = self.future.result()
             except Exception as e:
-                self.get_logger().info(
+                self.get_logger().info('Service call failed %r' % (e,))
-                    'Service call failed %r' % (e,))
             else:
                 pass
-                # Adapt to fit service type
+                # Adapt to fit your custom service type
                 # self.get_logger().info('Result of add_three_ints: for %d + %d + %d = %d' %(minimal_client.req.a, minimal_client.req.b, minimal_client.req.c, response.sum))
-            return self.future.done(), response
+            return self.future.done(), response # response is of type "<service_type_name>.Response()"
         else:
             return self.future.done(), None

src/pubsub/pubsub/service_host.py

@@ -11,12 +11,16 @@ import pubsub.pubsub_library_v3 as lib
 def main(args=None):
     rclpy.init(args=args)
     
-    minimal_service = lib.MinimalServiceProvider(NODE_NAME="Service_Host_Test", SRV_NAME="test_srv", SRV_TYPE=CustomSrv1)
+    minimal_service = lib.MinimalServiceProvider(NODE_NAME="Srv_host_test_node", SRV_NAME="test_srv", SRV_TYPE=CustomSrv1)
 
     while rclpy.ok():
         try:
             rclpy.spin_once(minimal_service, timeout_sec=0.1)
 
+            # Either the scripts checks to see if new srv calls are available to work off,
+            # or you implement this work inside the class
+            # pro/cons: abstraction is at a maximum, every further level is already too specific for the general task 
+            
         except (KeyboardInterrupt, SystemExit):
             print("\n\nShutting down...")
             minimal_service.destroy_node()

src/pubsub/pubsub/statemachine_client_example.py

@@ -0,0 +1,49 @@
+import rclpy
+from rclpy.node import Node
+
+import pubsub.pubsub_library_v3 as lib
+
+# Service Type Files
+from custom_interfaces.srv import StateMachineSrv
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    
+    client_cmd_srv = lib.MinimalServiceClientAsync(NODE_NAME="client_cmd_srv_node", SRV_NAME="cmd", SRV_TYPE=StateMachineSrv)
+
+    # Create request
+    srv_request = StateMachineSrv.Request()
+    srv_request.command = "c"
+    srv_request.value = 42.0
+    print("Sending Service Request...")
+    client_cmd_srv.send_request(srv_request)
+
+    while rclpy.ok():
+        try:
+            rclpy.spin_once(client_cmd_srv, timeout_sec=0.1)
+
+            # Check request
+            if client_cmd_srv.done_was_true == False:
+                done, response = client_cmd_srv.check_if_service_complete()
+                if done:
+                    client_cmd_srv.done_was_true = True
+                    print("'done': %s"%(done))
+                    print("'resp.success': %s"%(response.success))
+                else:
+                    print("Service not complete")
+            
+            rclpy.spin_once(client_cmd_srv, timeout_sec=0.1)
+            
+            srv_request.command = "a"
+            client_cmd_srv.send_request(srv_request)
+
+        except (KeyboardInterrupt, SystemExit):
+            print("\n\nShutting down...")
+            client_cmd_srv  .destroy_node()
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main() 
+

src/pubsub/pubsub/statemachine_host_example.py

@@ -0,0 +1,52 @@
+import rclpy
+from rclpy.node import Node
+
+import pubsub.pubsub_library_v3 as lib
+
+# Service Type Files
+from custom_interfaces.srv import StateMachineSrv
+
+
+
+class StateMachineCMDServiceProvider(lib.MinimalServiceProvider):
+    def __init__(self, NODE_NAME, SRV_NAME, SRV_TYPE):
+        self.NODE_NAME = NODE_NAME
+        self.SRV_NAME = SRV_NAME
+        self.SRV_TYPE = SRV_TYPE
+        # Init above laying class "Node" 
+        super().__init__(self.NODE_NAME, self.SRV_NAME, self.SRV_TYPE)
+    
+    def user_defined(self, request, response):
+        # StateMachine Code
+        print("Command: %s"%(request.command))
+        print("Value: %f"%(request.value))
+        if request.command == "c":
+            response.success = False
+        else:
+            response.success = True
+        return response
+
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    
+    recv_cmd_srv = StateMachineCMDServiceProvider(NODE_NAME="statemachine_cmd_srv_node", SRV_NAME="cmd", SRV_TYPE=StateMachineSrv)
+
+    while rclpy.ok():
+        try:
+            rclpy.spin_once(recv_cmd_srv, timeout_sec=0.1)
+
+            # Either the scripts checks to see if new srv calls are available to work off,
+            # or you implement this work inside the class
+            # pro/cons: abstraction is at a maximum, every further level is already too specific for the general task 
+            
+        except (KeyboardInterrupt, SystemExit):
+            print("\n\nShutting down...")
+            recv_cmd_srv.destroy_node()
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main() 
+

src/pubsub/setup.py

@@ -20,10 +20,12 @@ setup(
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
-            'talker = pubsub.talker:main',
+            'talker = pubsub.topic_talker:main',
-            'listener = pubsub.listener:main',
+            'listener = pubsub.topic_listener:main',
             'srvhost = pubsub.service_host:main',
-            'srvclient = pubsub.service_client:main'
+            'srvclient = pubsub.service_client:main',
+            'smhost = pubsub.statemachine_host_example:main',
+            'smclient = pubsub.statemachine_client_example:main'
         ],
     },
 )