def goal_reach_inform(msg):
global password
room_id = msg.data
if room_id.startswith('EV') or room_id.startswith(
'Station') or room_id.startswith('Lobby'):
rospy.loginfo('[NS] Receive goal_reach: ' + str(room_id))
rospy.loginfo('[NS] Pass')
else:
tid = round(time.time(), 2)
if password == '':
msg = '[NS]: Need to inform the guest. But the PW is empty. ROOM_ID: ' + str(
room_id)
sent_rss_notification(rss_on, rss_notification,
service_dict['AMR_ID'], msg)
rospy.logwarn(msg)
payload = {'roomId': room_id, 'pw': password, 'tid': tid}
call_success = http_service(phoneServerIP, payload, 'robocall', 10)
if not call_success:
msg = '[NS]: Fail to inform the guest. Request RSS. ROOM_ID: ' + str(
room_id) + "; PW: " + str(password)
sent_rss_notification(rss_on, rss_notification,
service_dict['AMR_ID'], msg)
rospy.logwarn(msg)
# Reset the PW
password = ''
return
def interrupt_agent(msg):
global robotStatus, task_path, lock
lock.acquire()
if robotStatus.mission is not None:
announce = unicode('中断任务执行，进入待命状态', 'utf-8')
rospy.loginfo('tts: ' + announce)
ttsPub.publish(announce)
msg = "[NC] Received Manual Interrupt."
msg += ' Last position: ' + str(robotStatus.position)
msg += ' / Mission: ' + str(robotStatus.mission)
msg += ' / Mission_TS: ' + str(robotStatus.ts_start)
sent_rss_notification(rss_on, rss_notification, service_dict['AMR_ID'],
announce)
rospy.logwarn(msg)
task_path = None
msg = "[NC] Clear task_path."
rospy.logwarn(msg)
robotStatus.mission = None
robotStatus.set_status('available')
msg = "[NC] Cancel Task. Reset Status."
rospy.logwarn(msg)
msg = "[NC] Cancel Move_base Goal."
rospy.logwarn(msg)
goalCancel.publish()
pub_robot_status()
lock.release()
return
def resource_monitor():
global robotStatus, lock
reboot_time = service_dict['rebootTime']
while True:
# Daily Reboot
if time.localtime().tm_hour == reboot_time and time.localtime(
).tm_min == 0:
while True:
if robotStatus.status in ['charging', 'need_charging', 'available'] and \
robotStatus.position is 'station':
lock.acquire()
rospy.loginfo('[NC] Daily Reboot.')
# Change the statue to block new task
robotStatus.set_status('init')
rospy.loginfo('tts: 执行机器人定期重启')
announce = unicode('执行机器人定期重启', 'utf-8')
ttsPub.publish(announce)
rospy.sleep(5)
rebootPub.publish(30.0)
rospy.sleep(120)
lock.release()
else:
rospy.loginfo(
"[NC]: Wait for mission complete Then, reboot.")
rospy.sleep(10)
# Critical Resource Monitor: Nodes, connection
# TODO: Add WiFi Connection Monitor
else:
if resource_check_on:
# Check the node_list_1
fine, result = checkNodeExistence(service_dict['node_list_1'])
if not fine:
for node, alive in result.items():
if not alive:
rospy.logwarn('[NC Resource Monitor]' + node +
" is dead.")
# TODO: Add node relaunch process.
# Check the node_list_2
fine, result = checkNodeExistence(service_dict['node_list_2'])
if not fine:
ts = time.time()
for node, alive in result.items():
if not alive:
msg = '[NC Resource Monitor]' + node + " is dead. Request RSS."
rospy.logfatal(msg)
sent_rss_notification(rss_on, rss_notification,
service_dict['AMR_ID'], msg)
rospy.sleep(30)
else:
rospy.sleep(60)
pass
def release_elevator(msg):
global elevatorIP
rospy.loginfo('[NS] Release EV Control.')
tid = round(time.time(), 2)
payload = {'robot_id': service_dict['AMR_ID'], 'tid': tid}
release_success = http_service(elevatorIP, payload, 'release', 10)
if not release_success:
msg = '[NS]: Fail to inform the EV release command. Just For Record.'
sent_rss_notification(rss_on, rss_notification, service_dict['AMR_ID'],
msg)
rospy.logwarn(msg)
return
def move_base_agent(mission_path, time_limit):
global lock, robotStatus
lock.acquire()
# Subscribe the callback Topic
_sub = rospy.Subscriber('/move_base/result', MoveBaseActionResult,
move_base_callback)
current_goal = mission_path[0]
rospy.loginfo('[NC] Current Goal: ' + str(current_goal))
# Loading the destination coordinate
pose, yaw_tol, xy_tol = goal_agent(current_goal, goal_dict)
# Set Tolerance
# TODO: set_tolerance TEST on real robot
# set_tolerance(yaw_tol, xy_tol)
rospy.loginfo('[NC] Sent the goal to move_base.')
# Publish the move_base goal.
goalPub.publish(pose)
# Release the global status.
robotStatus.set_status('moving')
pub_robot_status()
lock.release()
# Wait for Move_base's confirmation
counter = 0
while True:
if robotStatus.status == 'node_reached':
robotStatus.position = mission_path.pop(0)
if robotStatus.position[0:3] == 'EVW' and task_path[0] == 'EVin':
robotStatus.set_status('waitEV')
pub_robot_status()
_sub.unregister()
# Return the following path.
return mission_path, True
elif robotStatus.status == 'retry':
robotStatus.set_status('moving')
rospy.loginfo('[NC] Assign move_base goal again.')
pub_robot_status()
goalPub.publish(pose)
robotStatus.abortCount['move_base'] += 1
counter += 1
elif counter > time_limit:
msg = '[NC] move_base timeout:' + str(time_limit / 10) + 's.'
_sub.unregister()
rospy.logwarn(msg)
sent_rss_notification(rss_on, rss_notification,
service_dict['AMR_ID'], msg)
return mission_path, False
else:
rospy.sleep(0.1)
counter += 1
def entering_callback(success):
global lock, robotStatus
lock.acquire()
if success:
robotStatus.position = 'Station'
lock.release()
return
else:
robotStatus.position = 'Station'
msg = '[NC] Entering Station Return False.'
rospy.logwarn(msg)
sent_rss_notification(rss_on, rss_notification, service_dict['AMR_ID'],
msg)
lock.release()
return
def pmu_monitor(update):
global pmu_online
if pmu_online != update.data:
pmu_online = update.data
if pmu_online is False:
msg = "[NC]: PMU offline."
rospy.logwarn(msg)
sent_rss_notification(rss_on, rss_notification,
service_dict['AMR_ID'], msg)
else:
msg = "[NC]: PMU online."
rospy.loginfo(msg)
else:
pass
return
def battery_monitor(battery_capacity):
global robotStatus, battery_rss_flag
robotStatus.capacity = battery_capacity.data
if battery_capacity.data < battery_capacity_threshold and (battery_rss_flag
is None):
msg = "[rss_server]: Low Battery ! PLEASE CHECK. Request RSS."
sent_rss_notification(rss_on, rss_notification, service_dict['AMR_ID'],
msg)
rospy.logwarn(msg)
battery_rss_flag = True
elif battery_capacity.data >= 20.0:
battery_rss_flag = None
else:
pass
return
def wifi_connected():
check_list = ['robocall_server_ip', 'robocall_server_ip']
for target in check_list:
if not pinger(service_dict[target]):
msg = '[NC] ' + target + ' connection failed.'
sent_rss_notification(rss_on, rss_notification,
service_dict['AMR_ID'], msg)
rospy.logwarn(msg)
return False
if not pinger(service_dict['hotel_public_ip']):
rospy.logwarn('[NC] Hotel public ip connection failed.')
msg = 'Hotel public ip connection failed'
rospy.logwarn('[NC] ' + msg)
sent_rss_notification(rss_on, rss_notification, service_dict['AMR_ID'],
msg)
return True
def depart_callback(success):
global lock, robotStatus
lock.acquire()
if success:
robotStatus.position = 'Station_out'
robotStatus.set_status = 'node_reached'
pub_robot_status()
else:
robotStatus.position = 'Station_out'
robotStatus.set_status = 'node_reached'
pub_robot_status()
msg = '[NC] Departure Station Return False.'
rospy.logwarn(msg)
sent_rss_notification(rss_on, rss_notification, service_dict['AMR_ID'],
msg)
lock.release()
return
def node_monitor():
# TODO: Node monitor, try 3 mins with 3s interval. Success >> > proceed.Fail >> > Reboot
# Check the node_list_1
fine, result = checkNodeExistence(service_dict['node_list_1'] +
service_dict['node_list_2'])
if not fine:
for node, alive in result.items():
if not alive:
msg = '[NC] Initlization Failed: ' + node + ' is dead. Reboot the Robot.'
rospy.logfatal(msg)
sent_rss_notification(rss_on, rss_notification,
service_dict['AMR_ID'], msg)
msg = '[NC] Initlization Failed. Reboot the Robot.'
sent_rss_notification(rss_on, rss_notification, service_dict['AMR_ID'],
msg)
rospy.logfatal(msg)
return False
else:
return True
def depart_station(mission_path):
global lock, robotStatus
lock.acquire()
rospy.loginfo('[NC] AMR is going to depart the station.')
# Subscribe the callback Function
_sub = rospy.Subscriber('departStationCB', Bool, depart_callback)
rospy.loginfo('tts: 请小心，机器人将离开充电区 ')
msg = unicode('请小心, 机器人将离开充电区.', 'utf-8')
rospy.loginfo("[NC] Departing Station >>> >>> >>> ")
ttsPub.publish(msg)
for i in range(2):
rospy.loginfo('[NC] Departure Start Countdown: ' + str(1 - i))
rospy.sleep(0.9)
departStationPub.publish(True)
robotStatus.set_status('departing_station')
pub_robot_status()
# Release the global status.
lock.release()
counter = 0
# Wait for Departure Complete
while robotStatus.position != 'Station_out':
if counter >= 200:
# rss
robotStatus.position = 'Station_out'
pub_robot_status()
msg = '[NC] Departure Station 20s Timeout.'
rospy.logwarn(msg)
sent_rss_notification(rss_on, rss_notification,
service_dict['AMR_ID'], msg)
break
else:
rospy.sleep(0.1)
_sub.unregister()
rospy.loginfo('[NC] >>> Departure Finished.')
return mission_path[1:]
def navi_center():
global robotStatus, service_dict, rss_notification, path_planner, task_path, goal_dict
rospy.loginfo("[NC] System Launched.")
# ====================================================== #
# 0. Initiation Phase.
# ====================================================== #
# ---------- Service Parameters Loading ------------- #
service_dict = loading_service_parameter()
goal_dict = loading_hotel_goal()
rss_notification.setting(service_dict)
path_planner.setting(service_dict['hotelGoals'])
# ---------- RobotStatus Init ------------------------ #
robotStatus = RobotStatus()
# ---------- Check System Boost up Successfully ------ #
rospy.loginfo('[NC] Init Process: Check subsystem.')
# Check all the necessary node are online and alive.
# TODO: Enable system_health
if resource_check_on:
system_health()
# Start the "All time resource_monitor": Check node, connection & daily reboot
resource_monitor_thread = threading.Thread(target=resource_monitor)
resource_monitor_thread.daemon = True
resource_monitor_thread.start()
# ====================================================== #
# 1. Init >>> Standby Phase
# ====================================================== #
# 1-0. Subscribe Topic Relative to AMR Status or Safety:
# ===== Battery ===== #
rospy.Subscriber('/capacity', Float32, battery_monitor)
rospy.Subscriber('/charging/connected', Bool, station_connection_monitor)
# ===== Safety Monitor ===== #
# TODO: Add safety feature
# PMU, Power Management Unit Monitor
rospy.Subscriber('/PMU/online', Bool, pmu_monitor)
rospy.Subscriber('/charging/online', Bool, station_monitor)
# RSS Channel
rospy.Subscriber('/setPosition', String, rss_reset_position)
# TODO: ADD /EMB /Station
rospy.Subscriber('/emergentStop', Bool, interrupt_agent)
rospy.Subscriber('/base/lock', Bool, bumper_agent)
# Initialize position(default or by TopView_Tag), floor and status
for i in range(6):
rospy.loginfo('[NC] System Start Countdown: ' + str(5 - i))
rospy.sleep(1.0)
status_agent()
rospy.loginfo('[NC] Start Position: ' + str(robotStatus.position))
# Loading the map and position accordingly.
map_client(robotStatus.position, service_dict)
# Register DWA_Planner Service
# TODO: Check is here other way to replace wait_for
"""
try:
rospy.wait_for_service("/move_base/DWAPlannerROS/set_parameters", 5.0)
client = dynamic_reconfigure.client.Client("/move_base/DWAPlannerROS", timeout=30,
config_callback=dynamic_reconfigure_cb)
rospy.loginfo("[NC] AMR Launched Success.")
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("[NC] DWAPlannerROS Service call failed: %s" % (e,))
rospy.loginfo("[NC] AMR Launched Complete.")
"""
rospy.loginfo("[NC] AMR Launched Success.")
# 2. Planning Phase.
# 2-1. Start the 'Goal_Monitor: 'Subscribe a /hotelGoal.
# Monitor and check the delivery task input. Reject or Update the mission accordingly.
# If reject, simply feedback 'busy'. Otherwise, broadcast if through robot_status (by setting_robot_mission()).
rospy.Subscriber('/hotelGoal', String, goal_monitor)
# 3. Task Execution Phase.
while not rospy.is_shutdown():
# Scene I, Got Mission with task_path.
if robotStatus.mission is not None and task_path:
# ================== Main Task Loop ========================================== #
rospy.loginfo('[NC] Task Path: ' + str(task_path))
rospy.loginfo('[NC] Current Position: ' +
str(robotStatus.position))
# Receive task path, And Execute it orderly.
# Case 1, Departure Station
if robotStatus.position == 'Station' and task_path[
0] == 'Station_out':
task_path = depart_station(task_path)
# Case 2. Entering Station
elif robotStatus.position == 'Station_out' and task_path[
0] == 'Station':
task_path = enter_station(task_path)
# Case 3. Entering EV
# elif robotStatus.position[0:3] == 'EVW' and task_path[1] == 'EVin':
elif robotStatus.status is 'waitEV':
# AMR_ID, status, tid, current_floor, target_floor
tid = str(round(time.time(), 2))
current_floor = robotStatus.position[3:] + 'F'
target_floor = task_path[1][3:] + 'F'
rospy.loginfo('[NC] Call the EV to ' + current_floor)
# 3-1. Request EV
ev_request_confirm = ev_agent_call(service_dict['AMR_ID'], tid,
current_floor, target_floor)
if ev_request_confirm:
robotStatus.set_status('waitEV_C')
pub_robot_status()
# 3-2. Wait for EV reach
reach_result = ev_agent_reach(current_floor,
service_dict['ev_timeout'])
if reach_result is 'correct':
rospy.loginfo('[NC] EV reached: ' + str(current_floor))
robotStatus.set_status('checkingEV')
pub_robot_status()
# 3-3. Check the EV Space
safety_check_result = check_elevator(checkEVPub)
if safety_check_result == 'pass':
robotStatus.set_status('enteringEV')
pub_robot_status()
pass
elif safety_check_result == 'not pass':
# Release the EV
release_ev(doorReleasePub, ttsPub)
# Wait for 15s and recall
robotStatus.set_status('waitEV')
pub_robot_status()
rospy.sleep(15.0)
robotStatus.abortCount['ev_check'] += 1
pass
elif safety_check_result == 'yield':
# Release the EV
release_ev(doorReleasePub, ttsPub)
# Try the Yield Position
task_path.insert(0, robotStatus.position)
task_path.insert(0, robotStatus.position + 'S')
robotStatus.set_status('moving')
pub_robot_status()
robotStatus.abortCount['ev_check'] += 1
pass
# 3-2. Wait for EV reach >>> Timeout
elif reach_result == 'timeout':
msg = '[NC] Call EV >>> No response >>> Timeout.'
rospy.logwarn(msg)
sent_rss_notification(rss_on, rss_notification,
service_dict['AMR_ID'], msg)
pass
elif reach_result == 'wrong_floor':
msg = '[NC] Call EV >>> Floor Mismatch.'
rospy.logwarn(msg)
sent_rss_notification(rss_on, rss_notification,
service_dict['AMR_ID'], msg)
pass
else:
# TODO: Add action to EV request >>> No response. More than three times.
msg = '[NC] Unable to sent request to EV.'
rospy.logwarn(msg)
sent_rss_notification(rss_on, rss_notification,
service_dict['AMR_ID'], msg)
robotStatus.error['ev_no_response'] += 1
elif robotStatus.status == 'enteringEV':
# done / aborted / timeout(1200)
counter = 0
entering_result = entering_elevator(current_floor, enterEVPub)
if entering_result == 'done':
robotStatus.position = task_path.pop(0)
robotStatus.set_status('inEV')
pub_robot_status()
rospy.loginfo('[NC] >>> Entering EV Done >>> ')
rospy.loginfo('[NC] Current Position: ' +
str(robotStatus.position))
# Try to Inform the EV.
inform_ev = None
while not inform_ev:
if counter < 3:
inform_ev = inform_ev_done(service_dict['AMR_ID'],
tid, current_floor,
target_floor)
counter += 1
rospy.loginfo('[NC] Call the EV to ' +
str(target_floor))
robotStatus.set_status('inEV_C')
pub_robot_status()
else:
msg = '[NC] Unable to inform EV that AMR has done entering.'
rospy.logwarn(msg)
sent_rss_notification(rss_on, rss_notification,
service_dict['AMR_ID'], msg)
robotStatus.error['ev_no_response'] += 1
break
elif entering_result == 'abort':
task_path.insert(0, robotStatus.position)
robotStatus.set_status('moving')
pub_robot_status()
robotStatus.abortCount['entering'] += 1
pass
elif entering_result == 'timeout':
msg = '[NC] Timeout, Unable to Confirm that AMR has done entering.'
rospy.logwarn(msg)
sent_rss_notification(rss_on, rss_notification,
service_dict['AMR_ID'], msg)
# TODO: Could be node fial. Set recovery mode here.
pass
elif robotStatus.status in ['inEV', 'inEV_C']:
rospy.loginfo('[NC] Task Path: ' + str(task_path))
# TODO: Start EV Door Monitor Here.
reach_result = ev_agent_reach(target_floor,
service_dict['ev_timeout'])
if reach_result == 'correct':
robotStatus.set_status('inEV_R')
pub_robot_status()
# 4-2. Wait for EV reach >>> Timeout
elif reach_result == 'timeout':
msg = '[NC] EV can not reach target floor in 10m.'
rospy.logwarn(msg)
robotStatus.error['ev_no_response'] += 1
pub_robot_status()
sent_rss_notification(rss_on, rss_notification,
service_dict['AMR_ID'], msg)
# >>> Retry
pass
elif reach_result == 'wrong_floor':
msg = '[NC] Receive EV reached. >>> Floor Mismatch.'
rospy.logwarn(msg)
sent_rss_notification(rss_on, rss_notification,
service_dict['AMR_ID'], msg)
# >>> Retry
pass
elif robotStatus.status == 'inEV_R':
# Release the EV
release_ev(doorReleasePub, ttsPub)
# TTS Warning and alight
announce = unicode(
'机器人正要出电梯, 外面的朋友请小心... 外面的朋友请小心.. 机器人正要出电梯. 十分感谢您的配合',
'utf-8')
rospy.loginfo("tts:" + announce)
ttsPub.publish(announce)
robotStatus.set_status('alightingEV')
pub_robot_status()
# Method 1, Move_base
if alight_mb_on:
# time_limit = 10s (100 * 0.1s)
task_path, node_reach = move_base_agent(task_path, 100)
if node_reach:
robotStatus.set_status('moving')
pub_robot_status()
pass
else:
skip_node = task_path.pop(0)
rospy.loginfo('[NC] Skip the node: ' + str(skip_node))
robotStatus.position = skip_node
pub_robot_status()
rospy.loginfo('[NC] Task Path: ' + str(task_path))
pass
else:
target_floor = task_path[0][3:] + 'F'
map_client(target_floor, service_dict)
# Wait for tts
rospy.sleep(1.0)
alight_result = alighting_elevator(target_floor,
alightEVPub)
if alight_result == 'done':
current_node = task_path.pop(0)
robotStatus.position = current_node
robotStatus.set_status('node_reached')
pub_robot_status()
# 4-2. Wait for EV reach >>> Timeout
elif alight_result == 'timeout':
msg = '[NC] EV Alighting timeout. Resent alight command.'
rospy.logwarn(msg)
robotStatus.error['ev_no_response'] += 1
sent_rss_notification(rss_on, rss_notification,
service_dict['AMR_ID'], msg)
# >>> Retry
pass
elif alight_result == 'error':
msg = '[NC] EV Alighting Error. Resent alight command.'
rospy.logwarn(msg)
sent_rss_notification(rss_on, rss_notification,
service_dict['AMR_ID'], msg)
# >>> Retry
pass
else:
# move_base timeout = 5m (300s)
task_path, node_reach = move_base_agent(task_path, 3000)
if node_reach:
if robotStatus.mission == robotStatus.position:
robotStatus.set_status('reached')
pub_robot_status()
pass
else:
msg = '[NC] Send Goal to move_base >>> No response >>> Timeout.'
rospy.logwarn(msg)
sent_rss_notification(rss_on, rss_notification,
service_dict['AMR_ID'], msg)
pass
# Scene II, Got Mission and task_path is empty.
elif robotStatus.mission is not None and robotStatus.status == "reached":
# TODO: Add Ori Correction Here.
if robotStatus.mission.isdigit():
rospy.loginfo('[NC] Start Orientation Adjustment.')
ori_agent(robotStatus.mission, goal_dict)
rospy.loginfo('[NC] Start Orientation Adjustment.')
rospy.loginfo('tts: 行李已送达')
announce = unicode('行李已送达', 'utf-8')
ttsPub.publish(announce)
elif robotStatus.mission == 'Station':
rospy.loginfo('tts: 已回到待命区')
announce = unicode('已回到待命区', 'utf-8')
ttsPub.publish(announce)
else:
rospy.loginfo('tts: 任务已完成')
announce = unicode('任务已完成', 'utf-8')
ttsPub.publish(announce)
# Inform the User Interface, Mission Complete.
hotelGoalReachPub.publish(robotStatus.mission)
# Reset Mission
mission_done()
else:
pass
def move_base_callback(msg):
global robotStatus, lock
lock.acquire()
if msg.status.status == 3:
# node goal reached
robotStatus.set_status('node_reached')
elif msg.status.status == 2:
# move_base goal preempted
rospy.logwarn("[NC] Move_base Goal preempted.")
elif msg.status.status == 4:
# node goal aborted
rospy.logwarn("[NC] Move_base aborted")
robotStatus.abortCount['move_base'] += 1
# Reset Goal
rospy.loginfo("[NC] Retry Goal!")
robotStatus.set_status('retry')
# RSS Notification when aborting 5 times
if robotStatus.abortCount['move_base'] % 10 == 0:
msg = '[NC] AMR is blocked. Move_base aborted 10 times. Request RSS attention.'
rospy.logwarn(msg)
sent_rss_notification(rss_on, rss_notification,
service_dict['AMR_ID'], msg)
# TODO: PHONE Pushing & Logging Thread
"""
# push notification to operator
msg = '机器人被大型障碍物挡住了, 请派服务员前往处理'
threadY = pushingThread(msg)
threadY.start()
# logging
msg = 'Got stopper obstacles'
threadX = loggingThread(msg)
threadX.start()
"""
# Voice Warning
tts_msg = unicode('请借过, 我需要多一点空间', 'utf-8')
rospy.loginfo('tts: ' + tts_msg)
ttsPub.publish(tts_msg)
# Clear costmap
rospy.loginfo("[NC] Call /move_base/clear_costmaps")
clear_costmap()
for i in range(5):
rospy.loginfo('[NC] Move_base Retry Countdown: ' + str(4 - i))
rospy.sleep(0.9)
else:
# msg.status.status == 1,5,6,7 ...etc
str_msg = "[NC] Move_base Issue, status = " + str(msg.status.status)
rospy.logwarn(str_msg)
sent_rss_notification(rss_on, rss_notification, service_dict['AMR_ID'],
str_msg)
robotStatus.set_status('retry')
lock.release()
return
def enter_station(mission_path):
global lock, robotStatus, task_path
lock.acquire()
if pmu_online and station_online:
rospy.loginfo('[NC] AMR is going to enter the Station.')
# Subscribe the callback Function
_sub = rospy.Subscriber('/enterStationCB', Bool, entering_callback)
rospy.loginfo('tts: 机器人到家了..请小心, 机器人将回到充电区. ')
msg = unicode('机器人到家了..请小心, 机器人将回到充电区..', 'utf-8')
rospy.loginfo("[NC] Entering Station >>> >>> >>> ")
ttsPub.publish(msg)
rospy.sleep(3.0)
enterStationPub.publish(True)
robotStatus.set_status('entering_station')
pub_robot_status()
# Release the global status.
lock.release()
# Wait for Entering Complete
counter = 0
while robotStatus.position != 'Station':
if counter >= 200:
msg = '[NC] Entering Station 20s Timeout. Request RSS.'
rospy.logwarn(msg)
sent_rss_notification(rss_on, rss_notification,
service_dict['AMR_ID'], msg)
break
else:
counter += 1
rospy.sleep(0.1)
current_node = mission_path.pop(0)
robotStatus.position = current_node
rospy.loginfo('[NC] >>> Entering Finished.')
# mission_path is empty
if not mission_path:
robotStatus.set_status('reached')
rospy.loginfo('[NC] Path Finished, Goal Reached.')
pub_robot_status()
_sub.unregister()
lock.release()
return mission_path
else:
if not pmu_online:
msg = '[NC] PMU_Offline, Abort Entering. Request Physical Reboot.'
elif not station_online:
msg = '[NC] station_offline, Abort Entering. Request on-site check.'
else:
msg = '[NC] Both offline, Abort Entering. Request on-site check'
rospy.logwarn(msg)
sent_rss_notification(rss_on, rss_notification, service_dict['AMR_ID'],
msg)
if robotStatus.mission == 'Station':
robotStatus.mission = 'Station_out'
robotStatus.set_status('reached')
pub_robot_status()
return mission_path[1:]
else:
msg = '[NC] Entering Abort, But path is not empty: ' + str(
mission_path)
rospy.logwarn(msg)
sent_rss_notification(rss_on, rss_notification,
service_dict['AMR_ID'], msg)
return mission_path[1:]