def __init__(self, name, id, frame_id, use_vision=True):
# self._imu_pred = np.zeros(6) #class confs, t
# self._imu_pred_hist = np.empty(6) #class confs, t
# self._imu_state_hist = np.array([4, 1, datetime.datetime.min, datetime.datetime.min]) #class, conf, tStart, tEnd
self.left_h_pos = [None, None, None]
self.left_h_rot = [None, None, None, None]
self.right_h_pos = [None, None, None]
self.right_h_rot = [None, None, None, None]
self.status = None
self.name = name
self.id = id
self.frame_id = f"{frame_id}_{self.name}"
self.task_data = None
self.task = None
self.ACTION_CATEGORIES = None
self.col_names = None
self.db = database()
self.shimmer_ready = 1
self.use_vision = use_vision
if self.use_vision:
self.screw_counter = screw_counter(self.frame_id,
self.id,
self.name,
type='raw_count')
# self._final_state_hist = np.array([4, 1, datetime.datetime.min, datetime.datetime.min], ndmin=2) #class, conf, tStart, tEnd
self.curr_action_no = 0
self.curr_action_type = None
self.capability_obj = capability_class(frame_id=self.frame_id,
user_id=self.id)
def __init__(self, no, id, name, root):
self.no = no
self.id = id
self.name = name
self.root = root
self.imu_pred = np.zeros(5)
self.task_name = args.task_type
self.task_data = None
self.status = "unknown"
self.current_action_no = None
self.screw_counts = [None, None]
self.shimmer = [None, None, None]
self.shimmer_info = []
self.current_action_type = None
self.next_action_pub = rospy.Publisher('NextActionOverride',
String,
queue_size=10)
self.fig = Figure()
self.ax = self.fig.subplots(1, 1)
self.db = database()
self.create_user_frame()
self.update_action_plot()
def __init__(self):
self.db = database()
self.task_name = "stack_tower"
self.task_overview = None
self.next_action_id = 0
self.next_action = None
self.next_task_time = None
self.finished = False
self.update_task_data()
def __init__(self, parent):
self.fcancel = False
self.toplevel = Tk.Toplevel(parent)
self.toplevel.geometry('350x150')
self.toplevel.resizable(False, False)
self.var = Tk.StringVar()
db = database()
col_names, users = db.query_table('users', 'all')
users = pd.DataFrame(users, columns=col_names)
self.default = "Select User"
options = [self.default]
for _, row in users.iterrows():
options.append((row['user_id'], row['user_name']))
options = tuple(options)
label = Tk.Label(self.toplevel,
text="Choose New User:"******"OK",
command=self.toplevel.destroy,
width=10,
height=1,
padx=30,
pady=10,
anchor='center')
cancel_button = Tk.Button(self.toplevel,
text="Cancel",
command=self.cancel,
width=10,
height=1,
padx=30,
pady=10,
anchor='center')
label.grid(row=0, column=0, columnspan=2) #, sticky="nsew")
om.grid(row=1, column=0, columnspan=2) #, sticky="nsew")
ok_button.grid(row=2, column=0) #, sticky="nsew")
cancel_button.grid(row=2, column=1) #, sticky="nsew")
# Adjust spacing of objects
self.toplevel.grid_columnconfigure(0, weight=1)
self.toplevel.grid_columnconfigure(1, weight=1)
self.toplevel.grid_rowconfigure(0, weight=1)
self.toplevel.grid_rowconfigure(1, weight=1)
self.toplevel.grid_rowconfigure(2, weight=1)
def test_robot_control_node():
# ROS node setup
rospy.init_node(f'test_robot_control_node', anonymous=True)
frame_id = 'test_robot_control_node'
diag_obj = diag_class(frame_id=frame_id,
user_id=0,
user_name="robot",
queue=1)
capability_obj = capability_class(frame_id=frame_id, user_id=0)
task_name = 'assemble_box'
try:
db = database()
cols, data = db.query_table(task_name, 'all')
task_data = pd.DataFrame(data, columns=cols)
except Exception as e:
print(e)
raise e
rate = rospy.Rate(1 / 10) # 0.1hz
i = 0
col_names, act_data = db.query_table('current_actions', rows=0)
while (not rospy.is_shutdown()): # and (i < len(data)):
try:
print('here')
capability_obj.publish(i, [task_data.loc[i]['action_name']])
time = datetime.datetime.utcnow()
data_ins = "%s" + (", %s" * (len(col_names) - 1))
separator = ', '
sql_cmd = f"""INSERT INTO current_actions ({separator.join(col_names)})
VALUES (1, 'unknown', '{time}', '{task_name}', {int(task_data.loc[i]['action_no'])}, '{time}')
ON CONFLICT (user_id) DO UPDATE SET updated_t='{time}', task_name='{task_name}', current_action_no={int(task_data.loc[i]['action_no'])}, start_time='{time}';"""
db.gen_cmd(sql_cmd)
diag_obj.publish(0, "Running")
print(task_data.loc[i])
i = i + 1
if i == len(data):
i = 0
except Exception as e:
print(f"test_robot_control_node connection error: {e}")
diag_obj.publish(2, f"Error: {e}")
raise
rate.sleep()
print('here2')
def __init__(self):
self.db = database()
self.current_data = None
self.fut_cols = [
'user_id', 'user_name', 'task_name', 'current_action_no',
'est_t_remain', 'robo_task_t', 'robot_start_t', 'done'
]
self.future_estimates = pd.DataFrame(columns=self.fut_cols)
self.task_overview = None
self.robot_status = None
self.robot_start_t = datetime.datetime.now().time()
self.update_predictions()
self.task_now = None
self.action_no_now = None
self.done = False
def __init__(self):
self.db = database()
# Create GUI
self.root = Tk.Tk()
self.root.wm_title("HRC Interaction System")
self.root.resizable(True, True)
self.create_system_frame()
self.users = []
self.root.grid_columnconfigure(0, weight=1)
for i in range(len(self.users)):
self.root.grid_columnconfigure(i + 1, weight=1)
self.root.grid_rowconfigure(0, weight=1)
def setup_user(users, frame_id, task, name=None, use_vision=True):
id = len(users) + 1
if name is None:
name = "unknown"
users.append(User(name, id, frame_id, use_vision))
db = database()
time = datetime.datetime.utcnow()
sql_cmd = f"""DELETE FROM current_actions WHERE user_id = {id};"""
db.gen_cmd(sql_cmd)
sql_cmd = f"""DELETE FROM users WHERE user_id = {id};"""
db.gen_cmd(sql_cmd)
db.insert_data_list("users", ['user_id', 'user_name', 'last_active'],
[(id, name, time)])
users[id - 1].update_task(task)
return users
def collate_imu_seq(imu_state_hist, imu_pred_hist):
# 'Dilation' filter to remove single erroneous predictions
if np.shape(imu_state_hist)[0] >= 4:
if (
imu_state_hist[-1, 0] == imu_state_hist[-3, 0]
): # & (imu_state_hist[-2, 1] <= 0.00000000001): # WHAT IS THIS DOING???
imu_state_hist[-2, 0] = imu_state_hist[
-1, 0] #NEED TO CHECK CONFIDENCES HERE
# Group predictions of same type together
if imu_state_hist[-2, 0] == imu_state_hist[-3, 0]:
i = np.where(imu_pred_hist[:, -1] == imu_state_hist[-3, 2])[0][
0] #Get index where action starts
imu_state_hist[-2, 1] = np.mean(
imu_pred_hist[i:-1, int(imu_state_hist[-2, 0])].astype(
float)) #Not convinced about this mean
imu_state_hist[-2, 2] = imu_state_hist[-3, 2] # set start time
imu_state_hist = np.delete(imu_state_hist, -3, 0)
else:
# Can publish new episode to sql
db = database()
date = datetime.date.today()
start_t = imu_state_hist[-3, 2]
end_t = imu_state_hist[-3, 3]
dur = end_t - start_t #datetime.datetime.combine(datetime.date.min, end_t) - datetime.datetime.combine(datetime.date.min, start_t)
#table:str(table name), columns:[str(column name),], data:[('data1', data2, ),]
db.insert_data_list("Episodes", [
"date", "start_t", "end_t", "duration", "user_id", "hand",
"capability", "task_id"
], [(date, start_t, end_t, dur, 0, "R", CATEGORIES[int(
imu_state_hist[-3, 0])], 0)])
# imu_pred_hist = np.array(imu_pred_hist[-1, :])
# imu_pred_hist = np.reshape(imu_pred_hist, (1, 5))
return imu_state_hist, imu_pred_hist
db.disconnect()
diag_obj.publish(0, "Running")
except Exception as e:
print(f"Database connection error: {e}")
diag_obj.publish(2, f"Error: {e}")
rate.sleep()
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='Run database main ROS node')
parser.add_argument('--disp',
'-V',
help='Enable displaying of camera image',
default=False,
action="store_true")
args = parser.parse_known_args()[0]
db = None
try:
db = database()
database_run(db)
except rospy.ROSInterruptException:
print("database_run ROS exception")
except Exception as e:
print("**Database Error**")
traceback.print_exc(file=sys.stdout)
finally:
if db is not None:
shutdown(db)
pass
def robot_control_node():
# ROS node setup
frame_id = 'robot_control_node'
rospy.init_node(frame_id, anonymous=True)
diag_obj = diag_class(frame_id=frame_id,
user_id=0,
user_name="robot",
queue=1)
diag_obj.publish(1, "Starting")
rospy.Subscriber("SystemStatus", diagnostics, sys_stat_callback)
global database_stat
# Wait for postgresql node to be ready
while database_stat != 0 and not rospy.is_shutdown():
print(f"Waiting for postgresql node status, currently {database_stat}")
diag_obj.publish(1, "Waiting for postgresql node")
time.sleep(0.5)
global user_node_stat
# Wait for users node to be ready
while user_node_stat != 0 and not rospy.is_shutdown():
print(f"Waiting for users node status, currently {user_node_stat}")
diag_obj.publish(1, "Waiting for users node")
time.sleep(0.5)
predictor = future_predictor()
robot_task = robot_solo_task()
move_obj = move_class(frame_id=frame_id, queue=10)
rospy.Subscriber("CurrentState", capability,
predictor.user_prediction_callback)
rospy.Subscriber("RobotStatus", String, predictor.robot_stat_callback)
rate = rospy.Rate(1) # 1hz
db = database()
home = False
while not rospy.is_shutdown():
try:
predictor.update_predictions()
# Update future prediction in sql table
for _, row in predictor.future_estimates.iterrows():
sql_cmd = f"""DELETE FROM robot_future_estimates WHERE user_id = {row['user_id']};"""
db.gen_cmd(sql_cmd)
db.insert_data_list("robot_future_estimates",
predictor.fut_cols, [tuple(row.tolist())])
# Select row with minimum time until robot required
row = predictor.future_estimates[
predictor.future_estimates.robot_start_t ==
predictor.future_estimates.robot_start_t.min()]
if (row['robot_start_t'][0] < pd.Timedelta(0)) and (row['done'][0]
== False):
home = False
# if time to next colab < action time start colab action
action = predictor.task_overview.loc[
row['current_action_no']]['action_name'].values[0]
predictor.task_now = row['task_name'][0]
predictor.action_no_now = row['current_action_no'][0]
print(f"action: {action}")
# Wait for confirmation task has been received
while predictor.robot_status != action:
move_obj.publish(action)
predictor.robot_start_t = datetime.datetime.now().time()
predictor.done = False
# Wait for confirmation task has been completed
while not predictor.done:
time.sleep(0.01)
predictor.future_estimates.loc[
predictor.future_estimates['user_id'] ==
row['user_id'].values[0], 'done'] = True
elif ((row['robot_start_t'][0] > robot_task.next_task_time) or
(row['done'][0] == True)) and (not robot_task.finished):
home = False
# if time to colb action > time to do solo action
predictor.task_now = robot_task.task_name
predictor.action_no_now = robot_task.next_action_id
print(f"Robot solo task {robot_task.next_action}")
# Wait for confirmation task has been received
while predictor.robot_status != robot_task.next_action:
move_obj.publish(robot_task.next_action)
predictor.robot_start_t = datetime.datetime.now().time()
predictor.done = False
# Wait for confirmation task has been completed
while not predictor.done:
time.sleep(0.01)
robot_task.update_progress()
elif not home:
# else wait for next colab action
predictor.task_now = None
predictor.action_no_now = None
print(f"Sending robot home")
# Wait for confirmation task has been received
while predictor.robot_status != 'home':
move_obj.publish('home')
predictor.robot_start_t = datetime.datetime.now().time()
predictor.done = False
# Wait for confirmation task has been completed
while not predictor.done:
time.sleep(0.01)
# move_obj.publish('')
home = True
diag_obj.publish(0, "Running")
rospy.loginfo(f"{frame_id} active")
except Exception as e:
print(f"robot_control_node error: {e}")
diag_obj.publish(2, f"Error: {e}")
traceback.print_exc(file=sys.stdout)
rate.sleep()