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
