service call example created

This commit is contained in:
winterhalderp 2021-04-21 01:46:37 +02:00
parent a62eafff60
commit 2707bc0b42
9 changed files with 139 additions and 16 deletions

View File

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

View File

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

View File

@ -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.<name>
* response.<name>
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.<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(
'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 "<service_type_name>.Response()"
else:
return self.future.done(), None

View File

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

View File

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

View File

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

View File

@ -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'
],
},
)