1+ # Copyright 2019 Open Source Robotics Foundation, Inc.
2+ #
3+ # Licensed under the Apache License, Version 2.0 (the "License");
4+ # you may not use this file except in compliance with the License.
5+ # You may obtain a copy of the License at
6+ #
7+ # http://www.apache.org/licenses/LICENSE-2.0
8+ #
9+ # Unless required by applicable law or agreed to in writing, software
10+ # distributed under the License is distributed on an "AS IS" BASIS,
11+ # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+ # See the License for the specific language governing permissions and
13+ # limitations under the License.
14+
15+ import threading
16+ import time
17+
18+ from example_interfaces .action import Fibonacci
19+
20+ import rclpy
21+ from rclpy .action import ActionServer , CancelResponse , GoalResponse
22+ from rclpy .callback_groups import ReentrantCallbackGroup
23+ from rclpy .executors import ExternalShutdownException
24+ from rclpy .executors import MultiThreadedExecutor
25+ from rclpy .node import Node
26+
27+
28+ class MinimalActionServer (Node ):
29+ """Minimal action server that processes one goal at a time."""
30+
31+ def __init__ (self ):
32+ super ().__init__ ('minimal_action_server' )
33+ self ._goal_handle = None
34+ self ._goal_lock = threading .Lock ()
35+ self ._action_server = ActionServer (
36+ self ,
37+ Fibonacci ,
38+ 'fibonacci' ,
39+ execute_callback = self .execute_callback ,
40+ goal_callback = self .goal_callback ,
41+ handle_accepted_callback = self .handle_accepted_callback ,
42+ cancel_callback = self .cancel_callback ,
43+ callback_group = ReentrantCallbackGroup ())
44+ self .get_logger ().info ('Starting fibonacci action server..' )
45+
46+ def destroy (self ):
47+ self ._action_server .destroy ()
48+ super ().destroy_node ()
49+
50+ def goal_callback (self , goal_request ):
51+ """Accept or reject a client request to begin an action."""
52+ self .get_logger ().info ('Received goal request' )
53+ return GoalResponse .ACCEPT
54+
55+ def handle_accepted_callback (self , goal_handle ):
56+ with self ._goal_lock :
57+ # This server only allows one goal at a time
58+ if self ._goal_handle is not None and self ._goal_handle .is_active :
59+ self .get_logger ().info ('Aborting previous goal' )
60+ # Abort the existing goal
61+ self ._goal_handle .abort ()
62+ self ._goal_handle = goal_handle
63+
64+ goal_handle .execute ()
65+
66+ def cancel_callback (self , goal ):
67+ """Accept or reject a client request to cancel an action."""
68+ self .get_logger ().info ('Received cancel request' )
69+ return CancelResponse .ACCEPT
70+
71+ def execute_callback (self , goal_handle ):
72+ """Execute the goal."""
73+ self .get_logger ().info ('Executing goal...' )
74+
75+ # Append the seeds for the Fibonacci sequence
76+ feedback_msg = Fibonacci .Feedback ()
77+ feedback_msg .sequence = [0 , 1 ]
78+
79+ # Start executing the action
80+ for i in range (1 , goal_handle .request .order ):
81+ # If goal is flagged as no longer active (ie. another goal was accepted),
82+ # then stop executing
83+ if not goal_handle .is_active :
84+ self .get_logger ().info ('Goal aborted' )
85+ return Fibonacci .Result ()
86+
87+ if goal_handle .is_cancel_requested :
88+ goal_handle .canceled ()
89+ self .get_logger ().info ('Goal canceled' )
90+ return Fibonacci .Result ()
91+
92+ # Update Fibonacci sequence
93+ feedback_msg .sequence .append (feedback_msg .sequence [i ] + feedback_msg .sequence [i - 1 ])
94+
95+ self .get_logger ().info ('Publishing feedback: {0}' .format (feedback_msg .sequence ))
96+
97+ # Publish the feedback
98+ goal_handle .publish_feedback (feedback_msg )
99+
100+ # Sleep for demonstration purposes
101+ time .sleep (1 )
102+
103+ with self ._goal_lock :
104+ if not goal_handle .is_active :
105+ self .get_logger ().info ('Goal aborted' )
106+ return Fibonacci .Result ()
107+
108+ goal_handle .succeed ()
109+
110+ # Populate result message
111+ result = Fibonacci .Result ()
112+ result .sequence = feedback_msg .sequence
113+
114+ self .get_logger ().info ('Returning result: {0}' .format (result .sequence ))
115+
116+ return result
117+
118+
119+ def main (args = None ):
120+ try :
121+ rclpy .init (args = args )
122+ action_server = MinimalActionServer ()
123+
124+ # We use a MultiThreadedExecutor to handle incoming goal requests concurrently
125+ executor = MultiThreadedExecutor ()
126+ rclpy .spin (action_server , executor = executor )
127+ except (KeyboardInterrupt , ExternalShutdownException ):
128+ pass
129+
130+
131+ if __name__ == '__main__' :
132+ main ()
133+
134+
135+ # import time
136+
137+ # from example_interfaces.action import Fibonacci
138+ # from rcl_interfaces.msg import SetParametersResult
139+
140+ # import rclpy
141+ # from rclpy.action import ActionServer, CancelResponse
142+ # from rclpy.executors import ExternalShutdownException
143+ # from rclpy.executors import MultiThreadedExecutor
144+ # from rclpy.node import Node
145+ # from rclpy.parameter import Parameter
146+ # from rclpy.qos import qos_profile_system_default
147+ # from rclpy.service_introspection import ServiceIntrospectionState
148+
149+
150+ # class FibonacciActionServer(Node):
151+
152+ # def __init__(self):
153+ # super().__init__('fibonacci_action_server')
154+ # self._action_server = ActionServer(
155+ # self,
156+ # Fibonacci,
157+ # 'fibonacci',
158+ # self.execute_callback,
159+ # cancel_callback=self.cancel_callback)
160+
161+ # def execute_callback(self, goal_handle):
162+ # self.get_logger().info('Executing goal...')
163+
164+ # feedback_msg = Fibonacci.Feedback()
165+ # feedback_msg.sequence = [0, 1]
166+
167+ # for i in range(1, goal_handle.request.order):
168+ # if goal_handle.is_cancel_requested:
169+ # goal_handle.canceled()
170+ # self.get_logger().info('Goal canceled')
171+ # return Fibonacci.Result()
172+ # feedback_msg.sequence.append(
173+ # feedback_msg.sequence[i] + feedback_msg.sequence[i-1])
174+ # self.get_logger().info('Feedback: {0}'.format(feedback_msg.sequence))
175+ # goal_handle.publish_feedback(feedback_msg)
176+ # time.sleep(1)
177+
178+ # goal_handle.succeed()
179+
180+ # result = Fibonacci.Result()
181+ # result.sequence = feedback_msg.sequence
182+ # return result
183+
184+ # def cancel_callback(self, goal_handle):
185+ # self.get_logger().info('Canceling goal...')
186+ # return CancelResponse.ACCEPT
187+
188+
189+ # def main(args=None):
190+ # try:
191+ # rclpy.init(args=args)
192+ # fibonacci_action_server = FibonacciActionServer()
193+ # fibonacci_action_server.get_logger().info('Init...')
194+ # executor = MultiThreadedExecutor()
195+ # fibonacci_action_server.get_logger().info('Starting..')
196+ # rclpy.spin(fibonacci_action_server, executor=executor)
197+ # except (KeyboardInterrupt, ExternalShutdownException):
198+ # pass
199+
200+
201+ # if __name__ == '__main__':
202+ # main()
0 commit comments