def _process_request_cb(self, msg):
# Process request if addressed to this dispenser
if msg.target_guid != self.name:
return
print("[MockQuietDispenser] REQUEST RECEIVED")
if self.tasks.get(msg.request_guid):
print("QUIET DISPENSER SUCCESS")
status = DISPENSER_RESULT_SUCCESS
else:
self.tasks[msg.request_guid] = False
status = DISPENSER_RESULT_ACKNOWLEDGED
self.timer = self.create_timer(
0.01,
partial(self._timer_cb, msg=msg)
)
result = dispenser_msgs.DispenserResult()
result.time = self.get_clock().now().to_msg()
result.status = status
result.source_guid = self.name
result.request_guid = msg.request_guid
self.result_pub.publish(result)
def _timer_cb(self, msg):
if not self.timer:
return
result = dispenser_msgs.DispenserResult()
result.time = self.get_clock().now().to_msg()
result.status = DISPENSER_RESULT_SUCCESS
result.source_guid = self.name
result.request_guid = msg.request_guid
self.result_pub.publish(result)
self.success_flag = True
self.timer.cancel()
def _timer_cb(self):
# Report Dispenser State if publish_states is set
if self.publish_states:
state = dispenser_msgs.DispenserState()
state.time = self.get_clock().now().to_msg()
state.guid = self.name
if self.tasks:
# the queue contains only unfinished tasks
state.request_guid_queue = [
x.request_guid for x in self.tasks
][self.current_request_idx:]
if state.request_guid_queue:
state.mode = dispenser_msgs.DispenserState.BUSY
else:
state.mode = dispenser_msgs.DispenserState.IDLE
state.seconds_remaining = self.dispense_duration_sec
self.state_pub.publish(state)
if self.tasks:
if self.current_request_idx >= len(self.tasks):
# No more new tasks, do nothing
return
else:
# Tasks is empty, return
return
# If enough time has elapsed, consider dispensing done
time_now = self.get_clock().now().to_msg().sec
time_elapsed = time_now - self.tasks[self.current_request_idx].time.sec
if time_elapsed > self.dispense_duration_sec:
if self.publish_results:
# Send a success message
result = dispenser_msgs.DispenserResult()
result.time = self.get_clock().now().to_msg()
result.status = dispenser_msgs.DispenserResult.SUCCESS
result.source_guid = self.name
result.request_guid = self.tasks[
self.current_request_idx].request_guid
self.result_pub.publish(result)
self.current_request_idx += 1