2021-04-21 01:46:37 +02:00
|
|
|
import rclpy
|
|
|
|
from rclpy.node import Node
|
|
|
|
|
2021-04-21 11:14:15 +02:00
|
|
|
from pubsub.pubsub_library_v3 import MinimalServiceProvider
|
2021-04-21 01:46:37 +02:00
|
|
|
|
|
|
|
# Service Type Files
|
|
|
|
from custom_interfaces.srv import StateMachineSrv
|
|
|
|
|
|
|
|
|
2021-04-21 11:14:15 +02:00
|
|
|
# Device Class
|
|
|
|
class StateMachineCMDServiceProvider(MinimalServiceProvider):
|
2021-04-21 01:46:37 +02:00
|
|
|
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()
|
|
|
|
|