def get_pos_vel_data(db_name):
docs = DBUtils.get_docs(db_name, 'ros_amcl_pose')
num_of_docs = len(docs)
data = np.zeros((num_of_docs, 5))
# data (nx6) is arranged as follows
for i, doc in enumerate(docs):
# get position information from amcl_pose localisation
data[i][0] = DataUtils.get_var_value(doc, 'pose/pose/position/x')
data[i][1] = DataUtils.get_var_value(doc, 'pose/pose/position/y')
quat_x = DataUtils.get_var_value(doc, 'pose/pose/orientation/x')
quat_y = DataUtils.get_var_value(doc, 'pose/pose/orientation/y')
quat_z = DataUtils.get_var_value(doc, 'pose/pose/orientation/z')
quat_w = DataUtils.get_var_value(doc, 'pose/pose/orientation/w')
theta = tf.euler_from_quaternion((quat_w, quat_x, quat_y, quat_z))[2]
data[i][2] = theta
timestamp = DataUtils.get_var_value(doc, 'timestamp')
# get velocity information from odom at the same timestamp as position
odom_doc = DBUtils.get_last_doc_before(db_name, 'ros_ropod_odom',
timestamp)
vel_x = DataUtils.get_var_value(odom_doc, 'twist/twist/linear/x')
vel_y = DataUtils.get_var_value(odom_doc, 'twist/twist/linear/y')
data[i][3] = vel_x
data[i][4] = vel_y
return data
def get_handle_position(log_db_name, event_timestamp):
object_detection_result = DBUtils.get_last_doc_before(log_db_name,
'ros_detect_handle_server_result',
event_timestamp)
detected_handle = get_closest_handle(object_detection_result['result']['objects']['objects'])
detected_handle_bb = detected_handle['bounding_box']
center_position = [detected_handle_bb['center']['x'],
detected_handle_bb['center']['y'],
detected_handle_bb['center']['z']]
return np.array(center_position)
def get_goal_pose(log_db_name, event_timestamp):
goal_pose_doc = DBUtils.get_last_doc_before(log_db_name,
'ros_grasped_handle_pose',
event_timestamp)
goal_position = [goal_pose_doc['pose']['position']['x'],
goal_pose_doc['pose']['position']['y'],
goal_pose_doc['pose']['position']['z']]
goal_orientation_q = [goal_pose_doc['pose']['orientation']['x'],
goal_pose_doc['pose']['orientation']['y'],
goal_pose_doc['pose']['orientation']['z'],
goal_pose_doc['pose']['orientation']['w']]
orientation_euler = tf.euler_from_quaternion(goal_orientation_q)
return np.array(goal_position), np.array(orientation_euler)
def get_handle_bbox(log_db_name, event_timestamp):
object_detection_result = DBUtils.get_last_doc_before(log_db_name,
'ros_detect_handle_server_result',
event_timestamp)
detected_handle = get_closest_handle(object_detection_result['result']['objects']['objects'])
detected_handle_bb = detected_handle['bounding_box']
bb_min = [detected_handle_bb['center']['x'] - (detected_handle_bb['dimensions']['x'] / 2.),
detected_handle_bb['center']['y'] - (detected_handle_bb['dimensions']['y'] / 2.),
detected_handle_bb['center']['z'] - (detected_handle_bb['dimensions']['z'] / 2.)]
bb_max = [detected_handle_bb['center']['x'] + (detected_handle_bb['dimensions']['x'] / 2.),
detected_handle_bb['center']['y'] + (detected_handle_bb['dimensions']['y'] / 2.),
detected_handle_bb['center']['z'] + (detected_handle_bb['dimensions']['z'] / 2.)]
return bb_min, bb_max
def get_object_pose(log_db_name, event_timestamp):
goal_pose_doc = DBUtils.get_last_doc_before(
log_db_name, 'ros_push_pull_object_server_goal', event_timestamp)
goal_position = [
goal_pose_doc['goal']['object_pose']['pose']['position']['x'],
goal_pose_doc['goal']['object_pose']['pose']['position']['y'],
goal_pose_doc['goal']['object_pose']['pose']['position']['z']
]
goal_orientation_q = [
goal_pose_doc['goal']['object_pose']['pose']['orientation']['x'],
goal_pose_doc['goal']['object_pose']['pose']['orientation']['y'],
goal_pose_doc['goal']['object_pose']['pose']['orientation']['z'],
goal_pose_doc['goal']['object_pose']['pose']['orientation']['w']
]
orientation_euler = tf.euler_from_quaternion(goal_orientation_q)
return np.array(goal_position), np.array(orientation_euler)
def get_motion_duration(log_db_name, event_timestamp):
duration_doc = DBUtils.get_last_doc_before(log_db_name,
'ros_motion_duration',
event_timestamp)
return duration_doc['data']
def get_distance_from_edge(log_db_name, event_timestamp):
distance_doc = DBUtils.get_last_doc_before(log_db_name,
'ros_distance_from_edge',
event_timestamp)
return distance_doc['data']
