diff --git a/src/custom_interfaces/CMakeLists.txt b/src/custom_interfaces/CMakeLists.txt index 35db97f..fc551d6 100644 --- a/src/custom_interfaces/CMakeLists.txt +++ b/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 ) diff --git a/src/custom_interfaces/srv/StateMachineSrv.srv b/src/custom_interfaces/srv/StateMachineSrv.srv new file mode 100644 index 0000000..608f8b5 --- /dev/null +++ b/src/custom_interfaces/srv/StateMachineSrv.srv @@ -0,0 +1,4 @@ +string command +float32 value +--- +bool success diff --git a/src/pubsub/pubsub/pubsub_library_v3.py b/src/pubsub/pubsub/pubsub_library_v3.py index 2513b15..519e108 100644 --- a/src/pubsub/pubsub/pubsub_library_v3.py +++ b/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: - * request. - * response. + access values using class access: + * request.var_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. = .... """ 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( - 'Service call failed %r' % (e,)) + self.get_logger().info('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 ".Response()" else: return self.future.done(), None diff --git a/src/pubsub/pubsub/service_host.py b/src/pubsub/pubsub/service_host.py index ca37dad..728ac75 100644 --- a/src/pubsub/pubsub/service_host.py +++ b/src/pubsub/pubsub/service_host.py @@ -10,12 +10,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...") diff --git a/src/pubsub/pubsub/statemachine_client_example.py b/src/pubsub/pubsub/statemachine_client_example.py new file mode 100644 index 0000000..b539839 --- /dev/null +++ b/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() + diff --git a/src/pubsub/pubsub/statemachine_host_example.py b/src/pubsub/pubsub/statemachine_host_example.py new file mode 100644 index 0000000..6808e29 --- /dev/null +++ b/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() + diff --git a/src/pubsub/pubsub/listener.py b/src/pubsub/pubsub/topic_listener.py similarity index 100% rename from src/pubsub/pubsub/listener.py rename to src/pubsub/pubsub/topic_listener.py diff --git a/src/pubsub/pubsub/talker.py b/src/pubsub/pubsub/topic_talker.py similarity index 100% rename from src/pubsub/pubsub/talker.py rename to src/pubsub/pubsub/topic_talker.py diff --git a/src/pubsub/setup.py b/src/pubsub/setup.py index 895d498..f7f03ec 100644 --- a/src/pubsub/setup.py +++ b/src/pubsub/setup.py @@ -20,10 +20,12 @@ setup( tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'talker = pubsub.talker:main', - 'listener = pubsub.listener:main', + 'talker = pubsub.topic_talker: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' ], }, )