def send_experiment_feedback(robot_id, feedback_msg,
prev_feedback_received):
experiment_ongoing = True
if feedback_msg and feedback_msg['robot_id'] == robot_id:
if feedback_msg['feedback_type'] == 'ROBOT-EXPERIMENT-FEEDBACK':
experiment_ongoing = False
socketio.sleep(0.1)
elif not feedback_msg:
feedback_msg = dict()
feedback_msg['timestamp'] = time.time()
if prev_feedback_received:
feedback_msg['feedback_type'] = 'ROBOT-EXPERIMENT-FEEDBACK'
feedback_msg[
'result'] = '{0} is not responding anymore'.format(
robot_id)
else:
feedback_msg['feedback_type'] = 'EXPERIMENT-ERROR'
feedback_msg[
'result'] = '{0} is not responding; could not start experiment'.format(
robot_id)
experiment_ongoing = False
socketio.emit('experiment_feedback',
json.dumps(feedback_msg),
namespace='/experiments')
return experiment_ongoing
def get_robot_status(robot_ids):
while True:
for robot in robot_ids:
status_msg = zyre_communicator.get_status(robot)
socketio.emit('status_msg',
status_msg,
namespace='/robot_status')
socketio.sleep(0.1)
def on_connect():
current_map = config.get_current_map()
map_msg = config.get_map(current_map)
map_msg.pop('_id', None)
socketio.emit('map', map_msg, namespace='/task_scheduling')
robots = config.get_robots()
global status_thread
with status_thread_lock:
if status_thread is None:
status_thread = socketio.start_background_task(
target=emit_robot_pose, robot_ids=robots)
def emit_robot_pose(robot_ids):
while True:
robot_pose_msg_list = list()
for robot in robot_ids:
msg = zyre_communicator.get_pose(robot)
if msg:
robot_pose_msg_list.append(
map_utils.get_robot_pose_msg(msg))
if robot_pose_msg_list:
socketio.emit('robot_pose',
robot_pose_msg_list,
namespace='/task_scheduling')
socketio.sleep(1.0)
def get_robot_data(robot_id):
robot_smart_wheel_count = config.get_robot_smart_wheel_count(robot_id)
while True:
data_msg = zyre_communicator.get_query_data(
robot_data_query_msgs[robot_id])
variables, data = DataUtils.parse_bb_latest_data_msg(data_msg)
short_vel_vars = [
cmd_vel_var_name_mapping[x] for x in cmd_vel_vars
]
wheel_data = []
for i in range(robot_smart_wheel_count):
wheel_data.append({})
for var in wheel_vars:
expanded_var_name = var.replace('*', str(i))
wheel_data[i][expanded_var_name] = None
vel_data = list()
if variables:
try:
for i in range(robot_smart_wheel_count):
for var in wheel_vars:
expanded_var_name = var.replace('*', str(i))
wheel_data[i][expanded_var_name] = data[
variables.index(expanded_var_name)]
socketio.emit('wheel_data',
json.dumps({
'variables':
robot_wheel_var_name_mapping[robot_id],
'data':
wheel_data
}),
namespace='/real_time_monitoring')
except ValueError as exc:
print('[real_time_monitoring] Smart wheel data error')
print(str(exc))
try:
vel_data = [data[variables.index(x)] for x in cmd_vel_vars]
socketio.emit('cmd_vel_data',
json.dumps({
'variables': short_vel_vars,
'data': vel_data
}),
namespace='/real_time_monitoring')
except ValueError as exc:
print('[real_time_monitoring] Velocity data error')
print(str(exc))
socketio.sleep(1)
def get_robot_status(robot_ids):
robot_status_msgs = {
robot: {
'robot_id': robot,
'timestamp': None,
'component_statuses': dict()
}
for robot in robot_ids
}
while True:
for robot in robot_ids:
status_msg = zyre_communicator.get_status(robot)
if status_msg['payload']['monitors'] is not None:
component_statuses = get_component_statuses(status_msg)
robot_status_msgs[robot]['timestamp'] = status_msg[
'header']['timestamp']
robot_status_msgs[robot][
'component_statuses'] = component_statuses
else:
robot_status_msgs[robot]['component_statuses'] = None
socketio.emit('status_msg',
robot_status_msgs[robot],
namespace='/component_management')
socketio.sleep(0.1)
def send_diagnostic_data(session_id, black_box_id, diagnostic_vars):
end_query_time = int(time.time())
start_query_time = end_query_time - 10
query_msg = DataUtils.get_bb_query_msg(session_id, black_box_id,
diagnostic_vars,
start_query_time,
end_query_time)
query_result = zyre_communicator.get_query_data(query_msg)
try:
variables, data = DataUtils.parse_bb_data_msg(query_result)
vel_vars, vel_data = get_variable_data('velocity', variables, data)
socketio.emit('vel_data',
json.dumps({
'variables': vel_vars,
'data': vel_data
}),
namespace='/experiments')
accel_vars, accel_data = get_variable_data('accel', variables,
data)
socketio.emit('accel_data',
json.dumps({
'variables': accel_vars,
'data': accel_data
}),
namespace='/experiments')
gyro_vars, gyro_data = get_variable_data('gyro', variables, data)
socketio.emit('gyro_data',
json.dumps({
'variables': gyro_vars,
'data': gyro_data
}),
namespace='/experiments')
except Exception as exc:
print('[send_diagnostic_data] {0} does not seem to be responding'.
format(black_box_id))
print(str(exc))
global data_thread
data_thread = None