def main(args=None): global logger rclpy.init(args=args) node = rclpy.create_node('minimal_action_server') logger = node.get_logger() # Use a ReentrantCallbackGroup to enable processing multiple goals concurrently # Default goal callback accepts all goals # Default cancel callback rejects cancel requests action_server = ActionServer( node, Fibonacci, 'fibonacci', execute_callback=execute_callback, cancel_callback=cancel_callback, callback_group=ReentrantCallbackGroup()) # Use a MultiThreadedExecutor to enable processing goals concurrently executor = MultiThreadedExecutor() rclpy.spin(node, executor=executor) action_server.destroy() node.destroy_node() rclpy.shutdown()
def test_cancel_goal_accept(self): def execute_callback(goal_handle): # Wait, to give the opportunity to cancel time.sleep(3.0) self.assertTrue(goal_handle.is_cancel_requested) goal_handle.set_canceled() return Fibonacci.Result() def cancel_callback(request): return CancelResponse.ACCEPT executor = MultiThreadedExecutor(context=self.context) action_server = ActionServer( self.node, Fibonacci, 'fibonacci', callback_group=ReentrantCallbackGroup(), execute_callback=execute_callback, handle_accepted_callback=lambda gh: None, cancel_callback=cancel_callback, ) goal_uuid = UUID(uuid=list(uuid.uuid4().bytes)) goal_order = 10 goal_msg = Fibonacci.Impl.SendGoalService.Request() goal_msg.goal_id = goal_uuid goal_msg.goal.order = goal_order goal_future = self.mock_action_client.send_goal(goal_msg) rclpy.spin_until_future_complete(self.node, goal_future, executor) goal_handle = goal_future.result() self.assertTrue(goal_handle.accepted) cancel_srv = CancelGoal.Request() cancel_srv.goal_info.goal_id = goal_uuid cancel_srv.goal_info.stamp.sec = 0 cancel_srv.goal_info.stamp.nanosec = 0 cancel_future = self.mock_action_client.cancel_goal(cancel_srv) rclpy.spin_until_future_complete(self.node, cancel_future, executor) cancel_result = cancel_future.result() self.assertEqual(len(cancel_result.goals_canceling), 1) assert all( cancel_result.goals_canceling[0].goal_id.uuid == goal_uuid.uuid) action_server.destroy() executor.shutdown()
def test_constructor_no_defaults(self): action_server = ActionServer( self.node, Fibonacci, 'fibonacci', execute_callback=self.execute_goal_callback, callback_group=ReentrantCallbackGroup(), goal_callback=lambda req: GoalResponse.REJECT, handle_accepted_callback=lambda gh: None, cancel_callback=lambda req: CancelResponse.REJECT, goal_service_qos_profile=rclpy.qos.qos_profile_default, result_service_qos_profile=rclpy.qos.qos_profile_default, cancel_service_qos_profile=rclpy.qos.qos_profile_default, feedback_pub_qos_profile=rclpy.qos.qos_profile_default, status_pub_qos_profile=rclpy.qos.qos_profile_default, result_timeout=300, ) action_server.destroy()
def test_goal_callback_invalid_return(self): def goal_callback(goal): return 'Invalid return type' action_server = ActionServer( self.node, Fibonacci, 'fibonacci', execute_callback=self.execute_goal_callback, goal_callback=goal_callback, handle_accepted_callback=lambda gh: None, ) goal_msg = Fibonacci.Impl.SendGoalService.Request() goal_msg.goal_id = UUID(uuid=list(uuid.uuid4().bytes)) future = self.mock_action_client.send_goal(goal_msg) rclpy.spin_until_future_complete(self.node, future, self.executor) # An invalid return type in the goal callback should translate to a rejected goal self.assertFalse(future.result().accepted) action_server.destroy()
def test_expire_goals_one(self): # 1 second timeout action_server = ActionServer( self.node, Fibonacci, 'fibonacci', execute_callback=self.execute_goal_callback, result_timeout=1, ) goal_msg = Fibonacci.Impl.SendGoalService.Request() goal_msg.goal_id = UUID(uuid=list(uuid.uuid4().bytes)) goal_future = self.mock_action_client.send_goal(goal_msg) rclpy.spin_until_future_complete(self.node, goal_future, self.executor) self.assertEqual(1, len(action_server._goal_handles)) # After two seconds the internal goal handle should be destroyed self.timed_spin(2.1) self.assertEqual(0, len(action_server._goal_handles)) action_server.destroy()
def test_single_goal_accept(self): goal_uuid = UUID(uuid=list(uuid.uuid4().bytes)) goal_order = 10 def goal_callback(goal): nonlocal goal_order self.assertEqual(goal.order, goal_order) return GoalResponse.ACCEPT handle_accepted_callback_triggered = False def handle_accepted_callback(goal_handle): nonlocal goal_order nonlocal goal_uuid nonlocal handle_accepted_callback_triggered handle_accepted_callback_triggered = True self.assertEqual(goal_handle.status, GoalStatus.STATUS_ACCEPTED) self.assertEqual(goal_handle.goal_id, goal_uuid) self.assertEqual(goal_handle.request.order, goal_order) action_server = ActionServer( self.node, Fibonacci, 'fibonacci', execute_callback=self.execute_goal_callback, goal_callback=goal_callback, handle_accepted_callback=handle_accepted_callback, ) goal_msg = Fibonacci.Impl.SendGoalService.Request() goal_msg.goal_id = goal_uuid goal_msg.goal.order = goal_order future = self.mock_action_client.send_goal(goal_msg) rclpy.spin_until_future_complete(self.node, future, self.executor) self.assertTrue(future.result().accepted) self.assertTrue(handle_accepted_callback_triggered) action_server.destroy()
def test_duplicate_goal(self): executor = MultiThreadedExecutor(context=self.context) action_server = ActionServer( self.node, Fibonacci, 'fibonacci', execute_callback=self.execute_goal_callback, callback_group=ReentrantCallbackGroup(), handle_accepted_callback=lambda gh: None, ) # Send a goal with the same ID twice goal_msg = Fibonacci.Impl.SendGoalService.Request() goal_msg.goal_id = UUID(uuid=list(uuid.uuid4().bytes)) future0 = self.mock_action_client.send_goal(goal_msg) future1 = self.mock_action_client.send_goal(goal_msg) rclpy.spin_until_future_complete(self.node, future0, executor) rclpy.spin_until_future_complete(self.node, future1, executor) # Exactly one of the goals should be accepted self.assertNotEqual(future0.result().accepted, future1.result().accepted) action_server.destroy()
class MinimalActionServer(Node): def __init__(self): super().__init__('minimal_action_server') self._action_server = ActionServer( self, Fibonacci, 'fibonacci', execute_callback=self.execute_callback, callback_group=ReentrantCallbackGroup()) def destroy(self): self._action_server.destroy() super().destroy_node() async def execute_callback(self, goal_handle): self.get_logger().info('executing...') msg = Fibonacci.Feedback() msg.sequence = [0, 1] for i in range(1, goal_handle.request.order): if goal_handle.is_cancel_requested: goal_handle.canceled() self.get_logger().info('goal_canceled') return Fibonacci.Result() msg.sequence.append(msg.sequence[i] + msg.sequence[i - 1]) self.get_logger().info('feedback:{}'.format(msg.sequence)) goal_handle.publish_feedback(msg) time.sleep(1) # dummy job goal_handle.set_succeeded() result = Fibonacci.Result() result.sequence = msg.sequence self.get_logger().info('result:{}'.format(result.sequence)) return result
class MinimalActionServer(Node): """Minimal action server that processes one goal at a time.""" def __init__(self): super().__init__('minimal_action_server') self._goal_handle = None self._goal_lock = threading.Lock() self._action_server = ActionServer( self, Fibonacci, 'fibonacci', execute_callback=self.execute_callback, goal_callback=self.goal_callback, handle_accepted_callback=self.handle_accepted_callback, cancel_callback=self.cancel_callback, callback_group=ReentrantCallbackGroup()) def destroy(self): self._action_server.destroy() super().destroy_node() def goal_callback(self, goal_request): """Accept or reject a client request to begin an action.""" self.get_logger().info('Received goal request') return GoalResponse.ACCEPT def handle_accepted_callback(self, goal_handle): with self._goal_lock: # This server only allows one goal at a time if self._goal_handle is not None and self._goal_handle.is_active: self.get_logger().info('Aborting previous goal') # Abort the existing goal self._goal_handle.abort() self._goal_handle = goal_handle goal_handle.execute() def cancel_callback(self, goal): """Accept or reject a client request to cancel an action.""" self.get_logger().info('Received cancel request') return CancelResponse.ACCEPT def execute_callback(self, goal_handle): """Execute the goal.""" self.get_logger().info('Executing goal...') # Append the seeds for the Fibonacci sequence feedback_msg = Fibonacci.Feedback() feedback_msg.sequence = [0, 1] # Start executing the action for i in range(1, goal_handle.request.order): # If goal is flagged as no longer active (ie. another goal was accepted), # then stop executing if not goal_handle.is_active: self.get_logger().info('Goal aborted') return Fibonacci.Result() if goal_handle.is_cancel_requested: goal_handle.canceled() self.get_logger().info('Goal canceled') return Fibonacci.Result() # Update Fibonacci sequence feedback_msg.sequence.append(feedback_msg.sequence[i] + feedback_msg.sequence[i - 1]) self.get_logger().info('Publishing feedback: {0}'.format( feedback_msg.sequence)) # Publish the feedback goal_handle.publish_feedback(feedback_msg) # Sleep for demonstration purposes time.sleep(1) goal_handle.succeed() # Populate result message result = Fibonacci.Result() result.sequence = feedback_msg.sequence self.get_logger().info('Returning result: {0}'.format(result.sequence)) return result
class MinimalActionServer(Node): def __init__(self): super().__init__('minimal_action_server') self._goal_queue = collections.deque() self._goal_queue_lock = threading.Lock() self._current_goal = None self._action_server = ActionServer( self, Fibonacci, 'fibonacci', handle_accepted_callback=self.handle_accepted_callback, execute_callback=self.execute_callback, goal_callback=self.goal_callback, cancel_callback=self.cancel_callback, callback_group=ReentrantCallbackGroup()) def destroy(self): self._action_server.destroy() super().destroy_node() def handle_accepted_callback(self, goal_handle): """Starts or defers execution of an already accepted goal.""" with self._goal_queue_lock: if self._current_goal is not None: # Put incoming goal in the queue self._goal_queue.append(goal_handle) self.get_logger().info('Goal put in the queue') else: # Start goal execution right away self._current_goal = goal_handle self._current_goal.execute() def goal_callback(self, goal_request): """Accepts or rejects a client request to begin an action.""" self.get_logger().info('Received goal request') return GoalResponse.ACCEPT def cancel_callback(self, goal_handle): """Accepts or rejects a client request to cancel an action.""" self.get_logger().info('Received cancel request') return CancelResponse.ACCEPT def execute_callback(self, goal_handle): """Executes a goal.""" try: self.get_logger().info('Executing goal...') # Append the seeds for the Fibonacci sequence feedback_msg = Fibonacci.Feedback() feedback_msg.sequence = [0, 1] # Start executing the action for i in range(1, goal_handle.request.order): if goal_handle.is_cancel_requested: goal_handle.canceled() self.get_logger().info('Goal canceled') return Fibonacci.Result() # Update Fibonacci sequence feedback_msg.sequence.append(feedback_msg.sequence[i] + feedback_msg.sequence[i - 1]) self.get_logger().info('Publishing feedback: {0}'.format( feedback_msg.sequence)) # Publish the feedback goal_handle.publish_feedback(feedback_msg) # Sleep for demonstration purposes time.sleep(1) goal_handle.succeed() # Populate result message result = Fibonacci.Result() result.sequence = feedback_msg.sequence self.get_logger().info('Returning result: {0}'.format( result.sequence)) return result finally: with self._goal_queue_lock: try: # Start execution of the next goal in the queue. self._current_goal = self._goal_queue.popleft() self.get_logger().info('Next goal pulled from the queue') self._current_goal.execute() except IndexError: # No goal in the queue. self._current_goal = None
class GPIOActionServer(Node): def __init__(self): super().__init__('pi_gpio_server') pin_list = RaspberryPIGPIO.read_pins_from_file() self.pin_dic = {} for pin in pin_list: pin_id, type = pin.split(',') self.pin_dic[pin_id] = RaspberryPIGPIO(int(pin_id), type) self._goal_handle = None self._goal_lock = threading.Lock() #Node, action_type, action_name, execute_callback self._action_server = ActionServer( self, GPIO_Action, 'pi_gpio_server', execute_callback=self.execute_callback, goal_callback=self.goal_callback, handle_accepted_callback=self.handle_accepted_callback, cancel_callback=self.cancel_callback, callback_group=ReentrantCallbackGroup()) def destroy(self): self._action_server.destroy() super().destroy_node() def goal_callback(self, goal_request): self.get_logger().info('Received goal request') return GoalResponse.ACCEPT def handle_accepted_callback(self, goal_handle): with self._goal_lock: if self._goal_handle is not None and self._goal_handle.is_active: self.get_logger().info('Aborting previous goal') self._goal_handle.abort() self._goal_handle = goal_handle goal_handle.execute() def cancel_callback(self, goal): self.get_logger().info('Received cancel request') return CancelResponse.ACCEPT def execute_callback(self, goal_handle): """Executes the goal.""" self.get_logger().info('Executing goal...') # Populate goal message goal_msg = goal_handle.request.gpio # Populate feedback message feedback_msg = GPIO_Action.Feedback() feedback_msg.feedback = 1 # Populate result message result = GPIO_Action.Result() if not goal_handle.is_active: self.get_logger().info('Goal aborted') return GPIO_Action.Result() if goal_handle.is_cancel_requested: goal_handle.canceled() self.get_logger().info('Goal canceled') return GPIO_Action.Result() # Publish the feedback goal_handle.publish_feedback(feedback_msg) # get the pin ide and action type pin_id, action_type = goal_msg.split(',') if action_type == "high": self.pin_dic[pin_id].set_pin(1) time.sleep(0.1) result.value = 3 elif action_type == "low": self.pin_dic[pin_id].set_pin(0) time.sleep(0.1) result.value = 3 elif action_type == "read": result.value = GPIO.input(int(pin_id)) goal_handle.succeed() return result
class ActionServerWrapperProcess(multiprocessing.Process): def __init__(self, exit_future): multiprocessing.Process.__init__(self) self._node = rclpy.create_node('minimal_action_server') self._executor = MultiThreadedExecutor() self._exit_future = exit_future self._action_server = ActionServer( self._node, Fibonacci, 'fibonacci', execute_callback=self.execute_callback, callback_group=ReentrantCallbackGroup(), goal_callback=self.goal_callback, cancel_callback=self.cancel_callback) def goal_callback(self, goal_request): """Accepts or rejects a client request to begin an action.""" # This server allows multiple goals in parallel self._node.get_logger().info('Received goal request') return GoalResponse.ACCEPT def cancel_callback(self, goal_handle): """Accepts or rejects a client request to cancel an action.""" self._node.get_logger().info('Received cancel request') return CancelResponse.ACCEPT async def execute_callback(self, goal_handle): """Executes a goal.""" self._node.get_logger().info('Executing goal...') # Append the seeds for the Fibonacci sequence feedback_msg = Fibonacci.Feedback() feedback_msg.sequence = [0, 1] # Start executing the action for i in range(1, goal_handle.request.order): if goal_handle.is_cancel_requested: goal_handle.canceled() self._node.get_logger().info('Goal canceled') return Fibonacci.Result() # Update Fibonacci sequence feedback_msg.sequence.append(feedback_msg.sequence[i] + feedback_msg.sequence[i-1]) self._node.get_logger().info('Publishing feedback: {0}'.format(feedback_msg.sequence)) # Publish the feedback goal_handle.publish_feedback(feedback_msg) # Sleep for demonstration purposes #time.sleep(0.5) goal_handle.succeed() # Populate result message result = Fibonacci.Result() result.sequence = feedback_msg.sequence self._node.get_logger().info('Returning result: {0}'.format(result.sequence)) return result def run(self): print("Server is entering...") while not self._exit_future.done(): rclpy.spin_until_future_complete( self._node, self._exit_future, executor=self._executor, timeout_sec=1) self._action_server.destroy() self._executor.shutdown() self._node.destroy_node() print('Server is exiting...')
class MoveDistanceActionServer(Node): def __init__(self): super().__init__('move_distance_action') self._goal_lock = threading.Lock() self._goal_handle = None self._action_server = ActionServer( self, MoveDistance, 'move_distance', execute_callback=self.execute_callback, handle_accepted_callback=self.handle_accepted_callback, callback_group=ReentrantCallbackGroup(), goal_callback=self.goal_callback, cancel_callback=self.cancel_callback) def destroy(self): self._action_server.destroy() super().destroy_node() def goal_callback(self, goal_request): """Accept or reject a client request to begin an action.""" self.get_logger().info('Received goal request') return GoalResponse.ACCEPT def handle_accepted_callback(self, goal_handle): with self._goal_lock: # This server only allows one goal at a time if self._goal_handle is not None and self._goal_handle.is_active: self.get_logger().info('Aborting previous goal') # Abort the existing goal self._goal_handle.abort() self._goal_handle = goal_handle goal_handle.execute() def cancel_callback(self, goal_handle): """Accept or reject a client request to cancel an action.""" self.get_logger().info('Received cancel request') return CancelResponse.ACCEPT async def execute_callback(self, goal_handle): """Execute a goal.""" self.get_logger().info('Executing goal...') # Append the seeds for the Fibonacci sequence feedback_msg = MoveDistance.Feedback() feedback_msg.progress.x = 0.0 for i in range(5): if goal_handle.is_cancel_requested: goal_handle.canceled() self.get_logger().info('Goal canceled') return MoveDistance.Result() # Publish the feedback feedback_msg.progress.x = feedback_msg.progress.x + 0.1 goal_handle.publish_feedback(feedback_msg) # delay for demonstration purposes time.sleep(1) goal_handle.succeed() # Populate result message result = MoveDistance.Result() result.complete = True self.get_logger().info('Goal finished') return result
class Supervisor(Node): PUBLISH_PD = 1 # seconds def __init__(self, ns="/l2"): super().__init__('l2_supervisor', namespace=ns) self.get_logger().info("Init") self.state_pub = self.create_publisher(L2SequenceArray, 'state', 10) self.timer = self.create_timer(self.PUBLISH_PD, self.gc_state_callback) self.client = docker.from_env() self.config = {} self.state = [] self.path = "/config/" self.load_config() self.watch_handler = FileSystemEventHandler() self.watch_handler.on_modified = self.load_config self.observer = Observer() self.observer.schedule(self.watch_handler, self.path, recursive=True) self.observer.start() self._action_server = ActionServer( self, L2SequenceAction, 'sequence', execute_callback=self.execute_callback, goal_callback=self.goal_callback, cancel_callback=self.cancel_callback, callback_group=ReentrantCallbackGroup()) def destroy_node(self): self._action_server.destroy() super().destroy_node() def load_config(self, event=None): self.get_logger().info("Loading from %s" % self.path) self.config = {} for dirName, subdirList, fileList in os.walk(self.path): for fName in fileList: if os.path.splitext(fName)[1] not in [".yaml", ".yml"]: continue self.get_logger().info("\t...%s" % fName) with open(os.path.join(dirName, fName), 'r') as stream: try: self.config.update(yaml.safe_load(stream)) except yaml.YAMLError as exc: self.get_logger().error(str(exc)) self.get_logger().info("======== Merged Config ========") for (k, v) in self.config.items(): self.get_logger().info(k + ":") for kv2 in v.items(): self.get_logger().info("\t%s: %s" % kv2) self.get_logger().info("======== End Merged Config ========") def goal_callback(self, goal_request): """Accepts or rejects a client request to begin an action.""" for item in goal_request.sequence.items: if self.config.get(item.name) is None: self.get_logger().info( 'Rejecting incoming sequence; no config match for item %s' % item.name) return GoalResponse.REJECT self.get_logger().info('Accepting %d new sequence items' % len(goal_request.sequence.items)) return GoalResponse.ACCEPT def cancel_callback(self, goal): """Accepts or rejects a client request to cancel an action.""" self.get_logger().info('Accepting cancel request') return CancelResponse.ACCEPT def execute_callback(self, goal_handle): """Executes the goal.""" self.get_logger().info('Adding goal to work queue & executing...') work = Work(goal_handle, self) self.state.append(work) return work.execute() # Blocks thread until execution complete def gc_state_callback(self): seqs = L2SequenceArray() for s in self.state: seqs.sequences.append(L2Sequence(items=s.sequence.items)) self.state_pub.publish(seqs) # clean up state self.state = [s for s in self.state if s.active()]
class FSM(Node): def __init__(self): super().__init__('fsm') self.mode = "idle" self._goal_handle = None self._action_server = ActionServer( self, RunVirtualRail, 'run_virtual_rail', execute_callback=self.execute_callback, goal_callback=self.goal_callback, cancel_callback=self.cancel_callback) #self.request_mode_service = self.create_service(self.request_mode) self.subscription = self.create_subscription( Joy, 'joy', self.manual_mode, 10) self.subscription def destroy(self): self._action_server.destroy() super().destroy_node() def request_mode(self,request, response): self.mode = request.mode response.success = True return response def goal_callback(self, goal_request='idle'): """Accepts or rejects a client request to begin an action.""" if self.mode == "virtual_rail": return GoalResponse.ACCEPT else: return GoalResponse.REJECT def cancel_callback(self, goal_handle): """Accepts or rejects a client request to cancel an action.""" self.get_logger().info('Received cancel request') return CancelResponse.ACCEPT async def execute_callback(self, goal_handle): # this is where the actual virtual rail execution code goes # Start executing the action while True: start_time = current_time() # check if the goal has been cancelled if goal_handle.is_cancel_requested: goal_handle.canceled() result = Fibonacci.Result() result.success = False return Fibonacci.Result() if path_complete(): goal_handle.succeed() result = Fibonacci.Result() result.success = True return Fibonacci.Result() # Publish a pose to the position controller msg=Pose() pose.data = calc_position_setpoint(time, percent_complete) self.abs_pose_publisher.publish(msg) # Publish the feedback, could be current setpoint, percent complete, whatever we need to display in the GUI goal_handle.publish_feedback(feedback_msg) # Sleep for loop time while (current_time() - start_time < loop_time): ros.spin_once() # this is te asynchronous bit, allows other call backs to run sunch as cancelling the goal, rejecting incoming goals, basically Python asyncio def manual_mode(self, msg): x = msg.axes[1] y = msg.axes[0] a = math.atan2(x, y)+math.pi #v = math.sqrt(x**2 + y**2) w = msg.axes[3] v = -(((msg.axes[5]+1)/2)-1) drive_base_cmd = [a,v,w] self.get_logger().info('I heard: "%s"' % drive_base_cmd) def idle_mode(): pass # do nothing def check_vbus(): if vbus < threshold: self.state = error
class MinimalActionServer(Node): def __init__(self): super().__init__('minimal_action_server') self._goal_handle = None self._action_server = ActionServer( self, Fibonacci, 'fibonacci', execute_callback=self.execute_callback, callback_group=ReentrantCallbackGroup(), goal_callback=self.goal_callback, handle_accepted_callback=self.handle_accepted_callback, cancel_callback=self.cancel_callback) def destroy(self): self._action_server.destroy() super().destroy_node() def goal_callback(self, goal_request): """Accepts or rejects a client request to begin an action.""" self.get_logger().info('Received goal request') return GoalResponse.ACCEPT def handle_accepted_callback(self, goal_handle): """Provides a handle to an accepted goal.""" self.get_logger().info('Deferring execution...') self._goal_handle = goal_handle self._timer = self.create_timer(3.0, self.timer_callback) def cancel_callback(self, goal_handle): """Accepts or rejects a client request to cancel an action.""" self.get_logger().info('Received cancel request') return CancelResponse.ACCEPT def timer_callback(self): # Execute the defered goal if self._goal_handle is not None: self._goal_handle.execute() self._timer.cancel() async def execute_callback(self, goal_handle): """Executes a goal.""" self.get_logger().info('Executing goal...') # Append the seeds for the Fibonacci sequence feedback_msg = Fibonacci.Feedback() feedback_msg.sequence = [0, 1] # Start executing the action for i in range(1, goal_handle.request.order): if goal_handle.is_cancel_requested: goal_handle.set_canceled() self.get_logger().info('Goal canceled') return Fibonacci.Result() # Update Fibonacci sequence feedback_msg.sequence.append(feedback_msg.sequence[i] + feedback_msg.sequence[i - 1]) self.get_logger().info('Publishing feedback: {0}'.format( feedback_msg.sequence)) # Publish the feedback goal_handle.publish_feedback(feedback_msg) # Sleep for demonstration purposes time.sleep(1) goal_handle.set_succeeded() # Populate result message result = Fibonacci.Result() result.sequence = feedback_msg.sequence self.get_logger().info('Returning result: {0}'.format(result.sequence)) return result
class MinimalActionServer(Node): def __init__(self): super().__init__('minimal_action_server') self._goal_handle = None self._action_server = ActionServer( self, Fibonacci, 'fibonacci', execute_callback=self.execute_callback, callback_group=ReentrantCallbackGroup(), goal_callback=self.goal_callback, handle_accepted_callback=self.handle_accepted_callback, cancel_callback=self.cancel_callback) def destroy(self): self._action_server.destroy() super().destroy_node() def goal_callback(self, goal_request): """Accepts or rejects a client request to begin an action.""" self.get_logger().info('Received goal request') return GoalResponse.ACCEPT def handle_accepted_callback(self, goal_handle): """Provides a handle to an accepted goal.""" self.get_logger().info('Deferring execution...') self._goal_handle = goal_handle self._timer = self.create_timer(3.0, self.timer_callback) def cancel_callback(self, goal_handle): """Accepts or rejects a client request to cancel an action.""" self.get_logger().info('Received cancel request') return CancelResponse.ACCEPT def timer_callback(self): # Execute the defered goal if self._goal_handle is not None: self._goal_handle.execute() self._timer.cancel() async def execute_callback(self, goal_handle): """Executes a goal.""" self.get_logger().info('Executing goal...') # Append the seeds for the Fibonacci sequence feedback_msg = Fibonacci.Feedback() feedback_msg.sequence = [0, 1] # Start executing the action for i in range(1, goal_handle.request.order): if goal_handle.is_cancel_requested: goal_handle.canceled() self.get_logger().info('Goal canceled') return Fibonacci.Result() # Update Fibonacci sequence feedback_msg.sequence.append(feedback_msg.sequence[i] + feedback_msg.sequence[i-1]) self.get_logger().info('Publishing feedback: {0}'.format(feedback_msg.sequence)) # Publish the feedback goal_handle.publish_feedback(feedback_msg) # Sleep for demonstration purposes time.sleep(1) goal_handle.succeed() # Populate result message result = Fibonacci.Result() result.sequence = feedback_msg.sequence self.get_logger().info('Returning result: {0}'.format(result.sequence)) return result
class SubtaskNodeBase(Node, metaclass=ABCMeta): """サブタスクを定義する抽象クラス """ def __init__(self): super().__init__(self.node_name()) self._dict = self.init_argument() self.cb_group = ReentrantCallbackGroup() self._action_server = ActionServer( self, TsDoSubtask, "subtask_node_" + str(self.id()), execute_callback=self.execute_callback, callback_group=self.cb_group, goal_callback=self.goal_callback, cancel_callback=self.cancel_callback) # self.srv = self.create_service(TsDoTask, \ # "subtask_node_" + str(self.id()), \ # self._service_callback, \ # # self._test_service_callback, \ # callback_group = self.cb_group) def destroy(self): self._action_server.destroy() super().destroy_node() def goal_callback(self, goal_request): self.get_logger().info("Received goal request") return GoalResponse.ACCEPT def cancel_callback(self, goal_handle): self.get_logger().info('Received cancel request') self._timer = self.create_timer( 0.0, self._cancel_service_callback, callback_group=ReentrantCallbackGroup()) return CancelResponse.ACCEPT async def execute_callback(self, goal_handle): self.get_logger().info(">> Start") self._dict.update(json.loads(goal_handle.request.arg_json)) result = await self.service_callback(self._dict, TsDoSubtask.Result(), goal_handle) self.get_logger().warning(result.message) if goal_handle.is_cancel_requested: goal_handle.canceled() result.message = "Canceled" elif result.message == "Success": goal_handle.succeed() elif result.message == "Canceled": goal_handle.canceled() else: # Abortとして処理 goal_handle.abort() if result.message != "Success": self.get_logger().warning(f"return {result.message}") return result @abstractmethod def node_name(self) -> str: """ROS2 ノードの名前""" pass @abstractmethod def id(self) -> int: """データベースID""" pass # async def _service_callback(self, request, response): # """引数更新・実行ログ用""" # self.get_logger().info(">> Start") # # 引数を更新 # self._dict.update(json.loads(request.arg_json)) # response = await self.service_callback(self._dict, response) # self.get_logger().info(f">> {response.message}") # return response @abstractmethod async def service_callback(self, request, response, goal_handle) -> "response": """実行時の働き""" pass async def _cancel_service_callback(self): self._timer.cancel() await self.cancel_service_callback() async def cancel_service_callback(self): """キャンセルときに必要な動作がある場合オーバーライドする """ pass def init_argument(self) -> dict: """サブタスクに引数が必要な場合、オーバーライドしてください Returns: dict: タスクの引数をkey、デフォルト値をvalueとして辞書にしたもの """ return {} def _test_service_callback(self, request, response, goal_handle) -> "response": self.get_logger().info("Callback accepted") wait = random.randint(5, 15) self.get_logger().info(f"execute {wait} seconds") time.sleep(wait) self.get_logger().info("success") response.message = "Success" return response
class TaskSchedulerManager(Node): """TaskNodeを管理するマネージャノード """ def __init__(self): """コンストラクタ """ self.cb_group = ReentrantCallbackGroup() super().__init__("task_scheduler_manager") self.get_logger().info('task_scheduler_manager') self.action_server = ActionServer(self, TsReq, "tms_ts_master", self.execute_callback, goal_callback=self.goal_callback, cancel_callback=self.cancel_callback, callback_group=self.cb_group) def destroy_node(self): """デストラクタ """ self.action_server.destroy() super().destroy_node() def goal_callback(self, goal_handle): """タスク実行要求受付 """ self.get_logger().info('Received Task Request') return GoalResponse.ACCEPT def cancel_callback(self, goal_handle): """タスクのキャンセル """ self.get_logger().info('Received Cancel task') self._cancel_timer = self.create_timer(0.0, self._cancel, callback_group=self.cb_group) return CancelResponse.ACCEPT async def _cancel(self): self._cancel_timer.cancel() await self.goal_handle_client.cancel_goal_async() async def execute_callback(self, goal_handle): """タスク処理 """ global executor self.goal_handle = goal_handle task_id = goal_handle.request.task_id arg_json = goal_handle.request.arg_json if arg_json != '': # 引数が存在するとき self.arg_data = json.loads(arg_json) else: self.arg_data = {} self.get_logger().info(f'Execute task{task_id}, args:{self.arg_data}') task_tree = await self.convert_task_to_subtasks(task_id, self.arg_data) pprint(task_tree) # generate task_node task_node = TaskNode(task_tree) executor.add_node(task_node) # added last added task ## execute client = ActionClient(self, TsDoTask, task_node.name, callback_group=ReentrantCallbackGroup()) if not client.wait_for_server(timeout_sec=5.0): self.get_logger().error('No action server available') goal_handle.abort() result = TsReq.Result() result.message = "Abort" return result goal = TsDoTask.Goal() self.goal_handle_client = await client.send_goal_async(goal) if not self.goal_handle_client.accepted: self.get_logger().info("goal reject") goal_handle.abort() result = TsReq.Result() result.message = "Goal Reject" return result self.get_logger().info("goal accept") self.future_result = await self.goal_handle_client.get_result_async() # 結果を返す msg = self.future_result.result.message if msg == "Success": goal_handle.succeed() result = TsReq.Result() result.message = "Success" else: goal_handle.abort() result = TsReq.Result() result.message = msg # result.message = "Success" # task_node.destroy_node() return result async def convert_task_to_subtasks(self, task_id, arg_data): def func(m): arg_str = m.groups()[0] args = arg_str.split('.') answer = arg_data.copy() for arg in args: answer = answer.get(arg, {}) if answer == {}: self._is_valid_subtask_replace = False return '"ARGUMENT ERROR"' else: return str(answer) subtask_list = [] tmsdb_data = await self.call_dbreader(task_id) # this is list subtask_str = tmsdb_data[0].etcdata self.get_logger().info(f"find task '{tmsdb_data[0].name}'") self.get_logger().info(f"read task '{subtask_str}'") subtask_raw_list = re.findall(r'[0-9]+\$\{.*?\}|[0-9]+|\+|\|', subtask_str) self._is_valid_subtask_replace = True for subtask_raw in subtask_raw_list: subtask = subtask_raw.split("$") generated_subtask = [] for elem in subtask: elem = re.sub(r"\((.*?)\)",\ func,\ elem) generated_subtask.append(elem) subtask_list.append(generated_subtask) if self._is_valid_subtask_replace == False: self.get_logger().warning('task argument error!') return [] # 構文解析 _stack = [] for s in subtask_list: if s == ['+']: pre1 = _stack.pop(-1) pre2 = _stack.pop(-1) _stack.append(['serial', pre2, pre1]) elif s == ['|']: pre1 = _stack.pop(-1) pre2 = _stack.pop(-1) _stack.append(['parallel', pre2, pre1]) else: _stack.append(['subtask', s]) syntax_tree = _stack print(syntax_tree) if len(syntax_tree) != 1: self.get_logger().warning('subtask syntax error!') return [] return syntax_tree[0] async def call_dbreader(self, id): """[tms_db_reader] DBからデータを読み取る """ self.cli_dbreader = self.create_client(TmsdbGetData, 'tms_db_reader', callback_group=self.cb_group) while not self.cli_dbreader.wait_for_service(timeout_sec=1.0): self.get_logger().info( 'service "tms_db_reader" not available, waiting again...') req = TmsdbGetData.Request() req.tmsdb.id = id + 100000 # TODO: why add 100000 ? self.future_dbreader = self.cli_dbreader.call_async(req) await self.future_dbreader if self.future_dbreader.result() is not None: res = self.future_dbreader.result().tmsdb return res else: self.get_logger().info('Service "tms_db_reader" call failed %r' % (self.future_dbreader.exception(), )) async def read_name(self, id): """[tms_db_reader] DBから名前を読み取る """ tmsdb = await self.call_dbreader(int(id)) return tmsdb[0].name
class TaskSchedulerManager(Node): """TaskNodeを管理するマネージャノード """ def __init__(self): self.task_node_list = [] self.cb_group = ReentrantCallbackGroup() self.name = 'task_scheduler_manager' self.goal_handles = [] super().__init__('task_scheduler_manager') self.action_server = ActionServer( self, TsReq, "tms_ts_master", self.execute_callback, callback_group=self.cb_group, goal_callback=self.goal_callback, cancel_callback=self.cancel_callback ) def destroy_node(self): self.action_server.destroy() super().destroy_node() def goal_callback(self, goal_request): """タスク実行要求をACCEPT or REJECTする """ self.get_logger().info('Received Task Request') return GoalResponse.ACCEPT def cancel_callback(self,goal_handle): """タスク停止要求をACCEPT or REJECTする """ self.get_logger().info('Received Cancel Task') for goal_h in self.goal_handles: try: goal_h.cancel_goal_async() except rclpy.handle.InvalidHandle as e: print(e) return CancelResponse.ACCEPT async def execute_callback(self, goal_handle): """タスク処理 """ global executor task_id = goal_handle.request.task_id data_str = goal_handle.request.arg_json if data_str != '': self.arg_data = json.loads(data_str) else: self.arg_data = {} # syntax analyze task_tree = await self.convert_task_to_subtasks(task_id, self.arg_data) if task_tree == []: # ERROR goal_handle.abort() result = TsReq.Result() result.message = "Syntax Error" return result # generate task_node task_node = TaskNode(task_tree) self.task_node_list.append(task_node) executor.add_node(self.task_node_list[len(self.task_node_list) - 1]) # added last added task ## execute client = ActionClient(self, TsDoTask, task_node.name, callback_group=ReentrantCallbackGroup()) client.wait_for_server() goal = TsDoTask.Goal() goal_handle_client = await client.send_goal_async(goal) self.goal_handles.append(goal_handle_client) if not goal_handle_client.accepted: self.get_logger().info("goal reject") goal_handle.abort() result = TsReq.Result() result.message = "Goal Reject" return result self.get_logger().info("goal accept") self.future_result = await goal_handle_client.get_result_async() #if not self.future_result.done(): if True: goal_handle.abort() result = TsReq.Result() result.message = "Abort" return result # return result goal_handle.succeed() result = TsReq.Result() result.message = "Success" task_node.destroy_node() return result async def convert_task_to_subtasks(self, task_id, arg_data): def func(m): arg_str = m.groups()[0] args = arg_str.split('.') answer = arg_data.copy() for arg in args: answer = answer.get(arg, {}) if answer == {}: self._is_valid_subtask_replace = False return '"ARGUMENT ERROR"' else: return str(answer) subtask_list = [] tmsdb_data = await self.call_dbreader(task_id) # this is list subtask_str = tmsdb_data[0].etcdata print(f"[{self.name}] >> find task '{tmsdb_data[0].name}'") print(f"[{self.name}] >> read task '{subtask_str}'") subtask_raw_list = re.findall(r'[0-9]+\$\{.*?\}|[0-9]+|\+|\|', subtask_str) self._is_valid_subtask_replace = True for subtask_raw in subtask_raw_list: subtask = subtask_raw.split("$") generated_subtask = [] for elem in subtask: elem = re.sub(r"\((.*?)\)",\ func,\ elem) generated_subtask.append(elem) subtask_list.append(generated_subtask) print(f"subtask_list: {subtask_list}") if self._is_valid_subtask_replace == False: print(f"[{self.name}] >> argument error!") return [] # 構文解析 _stack = [] for s in subtask_list: if s == ['+']: pre1 = _stack.pop(-1) pre2 = _stack.pop(-1) _stack.append(['serial', pre2, pre1]) elif s == ['|']: pre1 = _stack.pop(-1) pre2 = _stack.pop(-1) _stack.append(['parallel', pre2, pre1]) else: _stack.append(['subtask', s]) syntax_tree = _stack print(f"[{self.name}] >> analyze task") print(syntax_tree) if len(syntax_tree) != 1: print(f"[{self.name}] >> syntax error!") return [] return syntax_tree[0] async def call_dbreader(self, id): """[tms_db_reader] DBからデータを読み取る """ self.cli_dbreader = self.create_client(TmsdbGetData, 'tms_db_reader', callback_group=self.cb_group) while not self.cli_dbreader.wait_for_service(timeout_sec=1.0): self.get_logger().info('service "tms_db_reader" not available, waiting again...') req = TmsdbGetData.Request() req.tmsdb.id = id + 100000 # TODO: why add 100000 ? self.future_dbreader = self.cli_dbreader.call_async(req) await self.future_dbreader if self.future_dbreader.result() is not None: res = self.future_dbreader.result().tmsdb return res else: self.get_logger().info('Service "tms_db_reader" call failed %r' % (self.future_dbreader.exception(),)) async def read_name(self, id): tmsdb = await self.call_dbreader(int(id)) return tmsdb[0].name
class MinimalActionServer(Node): """Minimal action server that processes one goal at a time.""" def __init__(self): super().__init__('minimal_action_server') self._goal_handle = None self._goal_lock = threading.Lock() self._action_server = ActionServer( self, Fibonacci, 'fibonacci', execute_callback=self.execute_callback, goal_callback=self.goal_callback, handle_accepted_callback=self.handle_accepted_callback, cancel_callback=self.cancel_callback, callback_group=ReentrantCallbackGroup()) def destroy(self): self._action_server.destroy() super().destroy_node() def goal_callback(self, goal_request): """Accepts or rejects a client request to begin an action.""" self.get_logger().info('Received goal request') return GoalResponse.ACCEPT def handle_accepted_callback(self, goal_handle): with self._goal_lock: # This server only allows one goal at a time if self._goal_handle is not None and self._goal_handle.is_active: self.get_logger().info('Aborting previous goal') # Abort the existing goal self._goal_handle.abort() self._goal_handle = goal_handle goal_handle.execute() def cancel_callback(self, goal): """Accepts or rejects a client request to cancel an action.""" self.get_logger().info('Received cancel request') return CancelResponse.ACCEPT def execute_callback(self, goal_handle): """Executes the goal.""" self.get_logger().info('Executing goal...') # Append the seeds for the Fibonacci sequence feedback_msg = Fibonacci.Feedback() feedback_msg.sequence = [0, 1] # Start executing the action for i in range(1, goal_handle.request.order): # If goal is flagged as no longer active (ie. another goal was accepted), # then stop executing if not goal_handle.is_active: self.get_logger().info('Goal aborted') return Fibonacci.Result() if goal_handle.is_cancel_requested: goal_handle.canceled() self.get_logger().info('Goal canceled') return Fibonacci.Result() # Update Fibonacci sequence feedback_msg.sequence.append(feedback_msg.sequence[i] + feedback_msg.sequence[i-1]) self.get_logger().info('Publishing feedback: {0}'.format(feedback_msg.sequence)) # Publish the feedback goal_handle.publish_feedback(feedback_msg) # Sleep for demonstration purposes time.sleep(1) goal_handle.succeed() # Populate result message result = Fibonacci.Result() result.sequence = feedback_msg.sequence self.get_logger().info('Returning result: {0}'.format(result.sequence)) return result
def test_cancel_defered_goal(self): server_goal_handle = None def handle_accepted_callback(gh): nonlocal server_goal_handle server_goal_handle = gh def cancel_callback(request): return CancelResponse.ACCEPT def execute_callback(gh): # The goal should already be in state CANCELING self.assertTrue(gh.is_cancel_requested) gh.canceled() return Fibonacci.Result() action_server = ActionServer( self.node, Fibonacci, 'fibonacci', callback_group=ReentrantCallbackGroup(), execute_callback=execute_callback, handle_accepted_callback=handle_accepted_callback, cancel_callback=cancel_callback, ) goal_uuid = UUID(uuid=list(uuid.uuid4().bytes)) goal_msg = Fibonacci.Impl.SendGoalService.Request() goal_msg.goal_id = goal_uuid goal_future = self.mock_action_client.send_goal(goal_msg) rclpy.spin_until_future_complete(self.node, goal_future, self.executor) send_goal_response = goal_future.result() self.assertTrue(send_goal_response.accepted) self.assertIsNotNone(server_goal_handle) self.assertEqual(server_goal_handle.status, GoalStatus.STATUS_ACCEPTED) # Cancel the goal, before execution cancel_srv = CancelGoal.Request() cancel_srv.goal_info.goal_id = goal_uuid cancel_srv.goal_info.stamp.sec = 0 cancel_srv.goal_info.stamp.nanosec = 0 cancel_future = self.mock_action_client.cancel_goal(cancel_srv) rclpy.spin_until_future_complete(self.node, cancel_future, self.executor) cancel_result = cancel_future.result() self.assertEqual(len(cancel_result.goals_canceling), 1) self.assertEqual(server_goal_handle.status, GoalStatus.STATUS_CANCELING) # Execute the goal server_goal_handle.execute() # Get the result and exepect it to have canceled status get_result_future = self.mock_action_client.get_result(goal_uuid) rclpy.spin_until_future_complete(self.node, get_result_future, self.executor) result = get_result_future.result() self.assertEqual(result.status, GoalStatus.STATUS_CANCELED) self.assertEqual(server_goal_handle.status, GoalStatus.STATUS_CANCELED) action_server.destroy()
class MinimalActionServer(Node): def __init__(self): super().__init__('minimal_action_server_6') self._action_server = ActionServer( self, Simple, 'simple_6', execute_callback=self.execute_callback, callback_group=ReentrantCallbackGroup(), goal_callback=self.goal_callback, cancel_callback=self.cancel_callback) self.get_logger().info('SERVER 6 : ONLINE') def destroy(self): self._action_server.destroy() super().destroy_node() def goal_callback(self, goal_request): """Accepts or rejects a client request to begin an action.""" # This server allows multiple goals in parallel self.get_logger().info('SERVER 6 : Received goal request') return GoalResponse.ACCEPT def cancel_callback(self, goal_handle): """Accepts or rejects a client request to cancel an action.""" self.get_logger().info('SERVER 6 : Received cancel request') return CancelResponse.ACCEPT async def execute_callback(self, goal_handle): """Executes a goal.""" self.get_logger().info('SERVER 6 : Executing goal...') # Append the seeds for the Fibonacci sequence feedback_msg = Simple.Feedback() feedback_msg.time_passed = [0] # Start executing the action for i in range(1, 5): if goal_handle.is_cancel_requested: goal_handle.canceled() self.get_logger().info('SERVER 6 : Goal canceled') return Simple.Result() # Update Fibonacci sequence feedback_msg.time_passed.append(i) self.get_logger().info('SERVER 6 : Publishing feedback: {0}'.format(feedback_msg.time_passed)) # Publish the feedback goal_handle.publish_feedback(feedback_msg) # Sleep for demonstration purposes time.sleep(1) goal_handle.succeed() # Populate result message result = Simple.Result() result.transition = 't8' self.get_logger().info('SERVER 6 : Returning result: {0}'.format(result.transition)) return result
class RandomCrawlActionServer(Node): def __init__(self): super().__init__('action_server') self.env = TurtlebotEnv() self.action_size = self.env.action_space() print(self.action_size) self.state = self.env.reset() self.observation_space = len(self.state) self.state = np.reshape(self.state, [1, self.observation_space]) pkg_share_directory = get_package_share_directory('nav2_turtlebot3_rl') path = os.path.join(pkg_share_directory, "saved_models/random_crawl_waffle.h5") self.model = load_model(path) q = self.model.predict(self.state) self._goal_handle = None self._goal_lock = threading.Lock() self._action_server = ActionServer( self, RandomCrawl, 'RandomCrawl', execute_callback=self.execute_callback, handle_accepted_callback=self.handle_accepted_callback, goal_callback=self.goal_callback, cancel_callback=self.cancel_callback, callback_group=ReentrantCallbackGroup()) def destroy(self): self.env.cleanup() self._action_server.destroy() super().destroy_node() def goal_callback(self, goal_request): self.get_logger().info('Received goal request') return GoalResponse.ACCEPT def handle_accepted_callback(self, goal_handle): with self._goal_lock: # This server only allows one goal at a time if self._goal_handle is not None and self._goal_handle.is_active: self.get_logger().info('Aborting previous goal') # Abort the existing goal self._goal_handle.abort() self._goal_handle = goal_handle goal_handle.execute() def cancel_callback(self, goal_handle): self.get_logger().info('Received cancel request') return CancelResponse.ACCEPT def execute_callback(self, goal_handle): while not goal_handle.is_cancel_requested and rclpy.ok(): q_values = self.model.predict(self.state) action = np.argmax(q_values) next_state, reward, terminal = self.env.step(action) next_state = np.reshape(next_state, [1, self.observation_space]) self.state = next_state sleep(parameters.LOOP_RATE) goal_handle.succeed() self.env.stop_action() result = RandomCrawl.Result() return result
class MinimalActionServer(Node): def __init__(self): super().__init__('minimal_action_server') self._sequence = [] self._action_server = ActionServer( self, Fibonacci, 'fibonacci', execute_callback=lambda x, self_=self: self_.execute_callback(x), callback_group=ReentrantCallbackGroup(), goal_callback=lambda x, self_=self: self_.goal_callback(x), cancel_callback=lambda x, self_=self: self_.cancel_callback(x)) def destroy(self): self._action_server.destroy() super().destroy_node() def goal_callback(self, goal_request): """Accepts or rejects a client request to begin an action.""" # This server allows multiple goals in parallel self.get_logger().info('Received goal request') return GoalResponse.ACCEPT def cancel_callback(self, goal_handle): """Accepts or rejects a client request to cancel an action.""" self.get_logger().info('Received cancel request') return CancelResponse.ACCEPT async def execute_callback(self, goal_handle): """Executes a goal.""" self.get_logger().info('Executing goal...') # Append the seeds for the Fibonacci sequence feedback_msg = Fibonacci.Feedback() self._sequence = [0, 1] # Start executing the action for i in range(1, goal_handle.request.order): if goal_handle.is_cancel_requested: goal_handle.canceled() self.get_logger().info('Goal canceled') return Fibonacci.Result() # Update Fibonacci sequence #feedback_msg.sequence.append(feedback_msg.sequence[i] + feedback_msg.sequence[i-1]) self._sequence.append(self._sequence[i] + self._sequence[i - 1]) #self.get_logger().info('Publishing feedback: {0}'.format(feedback_msg.sequence)) self.get_logger().info('Publishing feedback: {0}'.format( self._sequence)) feedback_msg.value = self._sequence[-1] # Publish the feedback goal_handle.publish_feedback(feedback_msg) # Sleep for demonstration purposes time.sleep(1) goal_handle.succeed() # Populate result message result = Fibonacci.Result() result.result = self._sequence[-1] if len(self._sequence) > 0 else 1 #self.get_logger().info('Returning result: {0}'.format(result.sequence)) self.get_logger().info('Returning result: {0}'.format(result.result)) return result
class AsyncActionServer(Node): def __init__(self): super().__init__('async_action_server') self.logger = self.get_logger() # fibonacciアクションサーバーの作成 # (execute_callback実行は複数同時処理を許可) cg01 = ReentrantCallbackGroup() # cg01 = MutuallyExclusiveCallbackGroup() self._srv = ActionServer(self, Fibonacci, 'fibonacci', execute_callback=self._execute_callback, callback_group=cg01) # ---------- Timer 01 ---------- self._timer01_cnt = 0 self._timer01 = self.create_timer(timer_period_sec=3.0, callback=self._on_timer01, callback_group=cg01) # ---------- Timer 01 ---------- self._timer02_cnt = 0 self._timer02 = self.create_timer(timer_period_sec=3.0, callback=self._on_timer02, callback_group=cg01) self._dtstr = self._get_time_now() self.logger.info('----- [action_server]Start! -----') async def _execute_callback(self, goal_handle): # アクションの実行 self.get_logger().info('executing...') dtstr = self._get_time_now() - self._dtstr self.logger.info("[{}]Action start!".format(dtstr)) # フィボナッチ数列の初期値0, 1を設定 msg = Fibonacci.Feedback() msg.sequence = [0, 1] # フィボナッチ数列を一つずつ追加 for i in range(1, goal_handle.request.order): if goal_handle.is_cancel_requested: # アクションのキャンセル goal_handle.set_canceled() self.get_logger().info('goal canceled') return Fibonacci.Result() # フィボナッチ数列の更新 msg.sequence.append(msg.sequence[i] + msg.sequence[i - 1]) self.get_logger().info('feedback: {0}'.format(msg.sequence)) # アクションのフィードバックの送信 goal_handle.publish_feedback(msg) # 1秒待機(重たい処理の代わり) time.sleep(1) # アクションの実行結果の送信 goal_handle.succeed() result = Fibonacci.Result() result.sequence = msg.sequence self.get_logger().info('result: {0}'.format(result.sequence)) dtstr = self._get_time_now() - self._dtstr self.logger.info("[{}]Action end!".format(dtstr)) return result def _on_timer01(self) -> None: dtstr = self._get_time_now() - self._dtstr self.logger.info("[{}]Timer01_in[{}] -----".format( dtstr, self._timer01_cnt)) time.sleep(1) dtstr = self._get_time_now() - self._dtstr self.logger.info("[{}]Timer01_out[{}] -----".format( dtstr, self._timer01_cnt)) self._timer01_cnt += 1 def _on_timer02(self) -> None: dtstr = self._get_time_now() - self._dtstr self.logger.info("[{}]Timer02_in[{}] -----".format( dtstr, self._timer02_cnt)) time.sleep(1) dtstr = self._get_time_now() - self._dtstr self.logger.info("[{}]Timer02_out[{}] -----".format( dtstr, self._timer02_cnt)) self._timer02_cnt += 1 def destroy(self): # アクションサーバーの終了 self._srv.destroy() super().destroy_node() def _get_time_now(self): dtnow = dt.datetime.now() return dtnow