ROS2_pubsub/src/pubsub/pubsub/statemachine_host_example.py

53 lines
1.6 KiB
Python
Raw Normal View History

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