def read_angle_data(X = [], Y = [], plot=True, DNN_format=True):
# Load train data
X_train = np.load('./robotsurface/X_train_kaggle.npy')
# Convert quaternion angles to euler angles
X_train_eulers = np.zeros((1703, 3, 128))
for i in range(1703):
for j in range(128):
X_train_eulers[i, :, j] = quat2euler(X_train[i, 4, j], X_train[i, 0, j], X_train[i, 1, j], X_train[i, 2, j], degrees=True)
# Load train labels
y_train = np.loadtxt('./robotsurface/y_train_final_kaggle.csv', str, skiprows=1, delimiter=',')
# Add data to lists
for i in range(1703):
x = X_train_eulers[i,:,:]
X.append(x)
if (DNN_format):
# Create DNN-training suitable version of y: e.g. [0 0 0 1 0 0 0 0 0]
y = np.zeros((9, 1))
y = y.T
n = dtypes[y_train[i, 1]]
y[:, n] = 1
Y.append(y)
else:
# Create sklearn-training suitable version of y: e.g. 'hard_tiles'
Y.append(y_train[i, 1])
def model_states_cb(self, msg):
for i in range(len(msg.name)):
if msg.name[i] != "person" and msg.name[i] != "robot":
continue
pos = msg.pose[i].position
euler = quat2euler(msg.pose[i].orientation.x,
msg.pose[i].orientation.y,
msg.pose[i].orientation.z,
msg.pose[i].orientation.w)
orientation = euler[0]
linear_vel = msg.twist[i].linear.x
angular_vel = msg.twist[i].angular.z
if msg.name[i] == "person":
self.person.update(pos, orientation, linear_vel, angular_vel)
# self.person.log_pose()
now = rospy.Time.now().to_sec()
if (abs(self.last_update_goal - now) > 0.2):
pose_stamped = PoseStamped()
pose_stamped.header.stamp = rospy.Time.now()
pose_stamped.header.frame_id = "odom"
pose_stamped.pose.position = self.person.calculate_ahead(
1.5)
pose_stamped.pose.orientation = msg.pose[i].orientation
self.simple_goal_pub.publish(pose_stamped)
self.last_update_goal = rospy.Time.now().to_sec()
rospy.loginfo("publishing ")
elif msg.name[i] == "robot":
self.robot.update(pos, orientation, linear_vel, angular_vel)
def rot2euler(R):
quat = pyquaternion.Quaternion(matrix=R)
quat2 = Quaternion(quat.elements[0], quat.elements[1], quat.elements[2],
quat.elements[3])
euler = quat2euler(*quat2, degrees=True)
return euler
def callImgTf(msg):
global imgCnt
imgCnt = imgCnt + 1
if (imgCnt % 5 != 0):
return
bridge = CvBridge()
try:
cvImage = bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8")
except CvBridgeError as e:
print(e)
global imgId
dirc = "/home/cair/backup/floor_loop/slam_images4/"
imgName = dirc + "frame{:06d}.jpg".format(imgId)
imgId = imgId + 1
cv2.imwrite(imgName, cvImage)
px, py, pz = trans.transform.translation.x, trans.transform.translation.y, trans.transform.translation.z
qx, qy, qz, qw = trans.transform.rotation.x, trans.transform.rotation.y, trans.transform.rotation.z, \
trans.transform.rotation.w
q = Quaternion(qw, qx, qy, qz)
roll, pitch, yaw = quat2euler(*q, degrees=True)
line = str(px) + " " + str(py) + " " + str(yaw) + "\n"
fileP.write(line)
def sonumuteuler(w, x, y, z):
(
roll,
pitch,
yaw,
) = quat2euler(w, x, y, z, degrees=True)
return roll, pitch, yaw
def turnRight(self, msg, carId):
# Cars x and y position
carPosX = msg.pose[carId].position.x
carPosY = msg.pose[carId].position.y
if self.cars[carId]["maneuverComplete"]:
self.cars[carId]["criticalSectionAquired"] = False
self.cars[carId]["speed"] = 4
return
else:
self.getCriticalSection(msg, carId)
# get angle in radians
pose = msg.pose[carId]
bot_euler = quat2euler(*(pose.orientation.w, pose.orientation.x,
pose.orientation.y, pose.orientation.z),
degrees=False)
rotation = abs(bot_euler[2])
if abs(carPosX) < 7.5 and abs(
carPosY) < 7.5 and self.cars[carId]["speed"] > 2:
self.cars[carId]["speed"] = 2
if abs(carPosX) < 7 and abs(carPosY) < 7:
if abs(carPosY) < 1.5 and abs(
carPosX) > 3.5 and self.cars[carId]["speed"] > abs(
carPosX / 7):
self.cars[carId]["speed"] = abs(carPosX / 7)
elif abs(carPosX) < 1.5 and abs(
carPosY) > 3.5 and self.cars[carId]["speed"] > abs(
carPosY / 7):
self.cars[carId]["speed"] = abs(carPosY / 7)
# start turning, if turn is initiated it should be completed
if abs(carPosX) < 3.5 and abs(carPosY) < 3.5:
if self.cars[carId]["priorityLane"] or self.cars[carId][
"criticalSectionAquired"]:
if abs(carPosX) < 2.6 and abs(carPosY) < 2.6:
self.cars[carId]["speed"] = 0.4 #0.48
self.cars[carId]["angle"] = 0.65 #-0.18
else:
self.cars[carId]["speed"] = 0.4
self.cars[carId]["angle"] = 0
else:
self.cars[carId]["speed"] = 0
# stop turning
if abs(rotation - self.cars[carId]["twist"]) > 1.4:
self.cars[carId]["angle"] = 0
if carPosX > 4 or carPosX < -4 or carPosY > 4 or carPosY < -4:
self.cars[carId]["maneuverComplete"] = True
# stop turning
if abs(rotation - self.cars[carId]["twist"]) > 1.4:
self.cars[carId]["angle"] = 0
if carPosX > 3.5 or carPosX < -3.5 or carPosY > 3.5 or carPosY < -3.5:
self.cars[carId]["maneuverComplete"] = True
def callPose(msg):
px, py, pz = msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z
qx, qy, qz, qw = msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,\
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
q = Quaternion(qw, qx, qy, qz)
roll, pitch, yaw = quat2euler(*q, degrees=True)
line = str(px) + " " + str(py) + " " + str(yaw) + "\n"
fileP.write(line)
def vicon_cb(self, pose_msg):
if self.last_time_observation is not None and abs(rospy.Time.now(
).to_sec() - self.last_time_observation) < 0.025:
return
pos = pose_msg.transform.translation
self.last_time_observation = rospy.Time.now().to_sec()
self.state_["position"] = (pos.x, pos.y)
euler = quat2euler(pose_msg.transform.rotation.x,
pose_msg.transform.rotation.y,
pose_msg.transform.rotation.z,
pose_msg.transform.rotation.w)
self.state_["orientation"] = euler[0]
self.all_states_.append(self.state_.copy())
def get_state(self) -> (float, float, float):
data = self.get()
x = data.translation.x
y = data.translation.y
# extract the quaternions
qx = data.rotation.x
qy = data.rotation.y
qz = data.rotation.z
qw = data.rotation.w
# convert to euler angles
q = Quaternion(qw,qx,qy,qz)
_, _, yaw = quat2euler(*q, degrees = True)
return x, y, yaw
def model_states_cb(self, msg):
for i in range(len(msg.name)):
if msg.name[i] != "person" and msg.name[i] != "robot":
continue
pos = msg.pose[i].position
euler = quat2euler(pos.orientation.x, pos.orientation.y,
pos.orientation.z, pos.orientation.w)
orientation = euler[0]
linear_vel = msg.pose[i].twist.linear.x
angular_vel = msg.pose[i].twist.angular.z
if msg.name[i] == "person":
self.person.update(pos, orientation, linear_vel, angular_vel)
elif msg.name[i] == "robot":
self.robot.update(pos, orientation, linear_vel, angular_vel)
def amcl_pose_callback(msg):
global i, x, y, theta, c_xx, c_yy, c_tt
#global l, matrix
l = msg.pose.covariance
c_xx.append(l[0])
c_yy.append(l[7])
c_tt.append(l[35])
x.append(msg.pose.pose.position.x)
y.append(msg.pose.pose.position.y)
pt = quat2euler(msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, degrees=True)
theta.append(pt[2])
print i
if i == 30:
print_covariance_data(x, y, theta, c_xx, c_yy, c_tt)
i += 1
def odom_process_(self):
l_enc, r_enc, imu = self.enc_l_val, self.enc_r_val, self.imu_msg_list
_, _, current_imu_yaw = quat2euler(imu[0], imu[1], imu[2], imu[3])
if self.first_time_odom_flag:
self.first_time_odom_flag = False
self.imu_yaw_prev = current_imu_yaw
self.l_enc_prev = l_enc
self.r_enc_prev = r_enc
return np.zeros(3)
if np.isnan(current_imu_yaw):
self.logger.error(
'current_imu_yaw value error: {}'.format(current_imu_yaw))
delta_q = current_imu_yaw - self.imu_yaw_prev
delta_l_enc = l_enc - self.l_enc_prev
delta_r_enc = self.r_enc_prev - r_enc
if (abs(delta_l_enc) > self.filter_threshold):
self.logger.error("delta_l_enc jump: {}".format(delta_l_enc))
delta_l_enc = 0
delta_r_enc = 0
if (abs(delta_r_enc) > self.filter_threshold):
self.logger.error("delta_r_enc jump: {}".format(delta_r_enc))
delta_l_enc = 0
delta_r_enc = 0
l_wd = delta_l_enc * self.tick_distance_
r_wd = delta_r_enc * self.tick_distance_
delta_v = (l_wd + r_wd) / 2.
self.imu_yaw_prev = current_imu_yaw
self.l_enc_prev = l_enc
self.r_enc_prev = r_enc
self.enc_l_val, self.enc_r_val, self.imu_msg_list = 0.0, 0.0, [0.0]*4
return np.array([delta_v*np.cos(delta_q), delta_v*np.sin(delta_q), delta_q])
def newModel(self, msg):
if self.firstIter:
for car in self.cars:
# set start angle in radians
pose = msg.pose[car]
bot_euler = quat2euler(
*(pose.orientation.w, pose.orientation.x,
pose.orientation.y, pose.orientation.z),
degrees=False)
self.cars[car]["twist"] = abs(bot_euler[2])
self.firstIter = False
for car in self.cars:
if self.cars[car]["turn"] == "left":
self.turnLeft(msg, car)
elif self.cars[car]["turn"] == "right":
self.turnRight(msg, car)
else:
self.goStraight(msg, car)
def visualize_poses(img, poses):
car_poses = []
for pose in poses:
quat = pose[:4]
quat = quat / np.linalg.norm(quat)
roll, pitch, yaw = quat2euler(*quat)
car_poses.append((yaw, pitch, roll, pose[4], pose[5], pose[6]))
x_l = 1.02
y_l = 0.80
z_l = 2.31
for yaw, pitch, roll, x, y, z in car_poses:
# I think the pitch and yaw should be exchanged
yaw, pitch, roll = -pitch, -yaw, -roll
Rt = np.eye(4)
t = np.array([x, y, z])
Rt[:3, 3] = t
Rt[:3, :3] = euler_to_Rot(yaw, pitch, roll).T
Rt = Rt[:3, :]
P = np.array([[0, 0, 0, 1],
[x_l, y_l, -z_l, 1],
[x_l, y_l, z_l, 1],
[-x_l, y_l, z_l, 1],
[-x_l, y_l, -z_l, 1],
[x_l, -y_l, -z_l, 1],
[x_l, -y_l, z_l, 1],
[-x_l, -y_l, z_l, 1],
[-x_l, -y_l, -z_l, 1]]).T
img_cor_points = np.dot(cam_matrix, np.dot(Rt, P))
img_cor_points = img_cor_points.T
img_cor_points[:, 0] /= img_cor_points[:, 2]
img_cor_points[:, 1] /= img_cor_points[:, 2]
img_cor_points = img_cor_points.astype(int)
img = draw_points(img, img_cor_points)
img = draw_line(img, img_cor_points)
img = Image.fromarray(img)
plt.imshow(img)
plt.show()
}
# Load model
model = load_model(
'Malli1--Euler+PositionData--4208-0.03-acc-0.8532-val_acc-0.7009.hdf5')
# Load test data
X_test = np.load('./robotsurface/X_test_kaggle.npy')
# Convert quaternion angles to euler angles
X_test_eulers = np.zeros((1705, 3, 128))
for i in range(1705):
for j in range(128):
X_test_eulers[i, :, j] = quat2euler(X_test[i, 4, j],
X_test[i, 0, j],
X_test[i, 1, j],
X_test[i, 2, j],
degrees=True)
# Convert linear accelerations to position
X_test_pos = np.zeros((1705, 3, 128))
t = np.linspace(0, 12.8, 128) # Guess of the time axis
for i in range(1705):
vel_x = cumtrapz(X_test[i, 7, :], t, initial=0)
vel_y = cumtrapz(X_test[i, 8, :], t, initial=0)
vel_z = cumtrapz(X_test[i, 9, :], t, initial=0)
pos_x = cumtrapz(vel_x, t, initial=0)
pos_y = cumtrapz(vel_y, t, initial=0)
pos_z = cumtrapz(vel_z, t, initial=0)
X_test_pos[i, 0, :] = pos_x
X_test_pos[i, 1, :] = pos_y
data = pickle.load(open("data.pickle", "rb"))
accel = [x[0][0] for x in data['imu']]
mags = [x[0][1] for x in data['imu']]
gyros = [x[0][2] for x in data['imu']]
imutime = [x[1] for x in data['imu']]
ahrs = AHRS()
save = []
ot = 0
q = Quaternion(1, 0, 0, 0)
for a, m, g, t in zip(accel, mags, gyros, imutime):
# q = ahrs.updateAGM(a,m,g,1,t-ot)
q = ahrs.updateAG(a, g, .1, t - ot)
o = quat2euler(*q, degrees=True)
save.append(o)
ot = t
plt.plot(imutime, [x[0] for x in save], label='x')
plt.plot(imutime, [x[1] for x in save], label='y')
plt.plot(imutime, [x[2] for x in save], label='z')
plt.legend()
plt.grid(True)
plt.show()
# imgs = [np.frombuffer(x[0], dtype=np.uint8).reshape((480,640)) for x in data['camera']]
# itime = [x[1] for x in data['camera']]
# for im in imgs:
# # im = cv2.equalizeHist(im)
def surface_loader(test_size=None,
n_splits=5,
folder='robotsurface' + os.sep,
euler=False):
X_train = load(folder + 'X_train_kaggle.npy')
X_test = load(folder + 'X_test_kaggle.npy')
y_train = loadtxt(folder + 'y_train_final_kaggle.csv',
delimiter=',',
usecols=(1),
dtype='str')
if euler:
X_train_eulers = zeros((1703, 3, 128))
for i in range(1703):
for j in range(128):
X_train_eulers[i, :, j] = quat2euler(X_train[i, 4, j],
X_train[i, 0, j],
X_train[i, 1, j],
X_train[i, 2, j],
degrees=True)
X_train = X_train_eulers
le = LabelEncoder()
le.fit(y_train)
y_train = le.transform(y_train)
data = {'X_train': X_train, 'X_test': X_test, 'y_train': y_train}
if test_size != None:
X_train_s = []
X_test_s = []
y_train_s = []
y_test_s = []
#groups = loadtxt(folder+'groups.csv',
# delimiter=',',usecols=(1),dtype='int')
groups = genfromtxt(folder + 'groups.csv', delimiter=',', dtype=None)
rs = ShuffleSplit(n_splits=n_splits,
test_size=test_size,
random_state=0)
X = arange(36)
N = X_train.shape[0]
split = 0
for indices in rs.split(X):
X_train_split = []
y_train_split = []
X_test_split = []
y_test_split = []
train_index = indices[0]
split += 1
print("Progressing split {}/{}".format(split, n_splits))
printProgressBar(1,
N,
prefix="Progressing sample {}/{}".format(1, N),
suffix='Complete',
length=50)
split_txt = []
for i in range(N):
printProgressBar(i,
N - 1,
prefix="Progressing sample {}/{}".format(
i + 1, N),
suffix='Complete',
length=50)
if groups[i][1] in train_index:
testi_str = f"{groups[i][0]},{groups[i][1]},{groups[i][2]},{0}"
split_txt.append(testi_str.split(','))
X_train_split.append(X_train[i, :, :])
y_train_split.append(y_train[i])
else:
testi_str = f"{groups[i][0]},{groups[i][1]},{groups[i][2]},{1}"
split_txt.append(testi_str.split(','))
X_test_split.append(X_train[i, :, :])
y_test_split.append(y_train[i])
split_txt = asarray(split_txt)
pd.DataFrame(split_txt).to_csv(f"split{split}.csv")
X_train_s.append(asarray(X_train_split))
y_train_s.append(asarray(y_train_split))
X_test_s.append(asarray(X_test_split))
y_test_s.append(asarray(y_test_split))
print()
X_train_s = array(X_train_s)
y_train_s = array(y_train_s)
X_test_s = array(X_test_s)
y_test_s = array(y_test_s)
data.update({
'X_train_s': X_train_s,
'X_test_s': X_test_s,
'y_train_s': y_train_s,
'y_test_s': y_test_s
})
return data, le
def read_all_data(X_train, y_train, X_test, y_test, plot=True, DNN_format=True):
# Load train data
X_load = np.load('./robotsurface/X_train_kaggle.npy')
# Convert quaternion angles to euler angles
X_load_eulers = np.zeros((1703, 3, 128))
for i in range(1703):
for j in range(128):
X_load_eulers[i, :, j] = quat2euler(X_load[i, 4, j], X_load[i, 0, j], X_load[i, 1, j], X_load[i, 2, j], degrees=True)
# Convert linear accelerations to position
X_load_pos = np.zeros((1703, 3, 128))
X_load_vel = np.zeros((1703, 3, 128))
t = np.linspace( 0, 1.28, 128) # Guess of the time axis
for i in range(1703):
X_load_vel[i, 0, :] = cumtrapz(X_load[i, 7, :], t, initial=0)
X_load_vel[i, 1, :] = cumtrapz(X_load[i, 8, :], t, initial=0)
X_load_vel[i, 2, :] = cumtrapz(X_load[i, 9, :], t, initial=0)
pos_x = cumtrapz(X_load_vel[i, 0, :], t, initial=0)
pos_y = cumtrapz(X_load_vel[i, 1, :], t, initial=0)
pos_z = cumtrapz(X_load_vel[i, 2, :], t, initial=0)
X_load_pos[i, 0, :] = pos_x
X_load_pos[i, 1, :] = pos_y
X_load_pos[i, 2, :] = pos_z
# Load train labels
y_load = np.loadtxt('./robotsurface/y_train_final_kaggle.csv', str, skiprows=1, delimiter=',')
X = []
Y = []
# Add data to lists
for i in range(1703):
eulers = X_load_eulers[i,:,:] # Euler angle data
angvels = X_load[i, 4:7, :] # Angular velocity data
xy_pos = X_load_pos[i, :2, :] # XY-position data
xy_vel = X_load_vel[i, :2, :] # XY-velocity data
x = np.concatenate((eulers,angvels, xy_pos, xy_vel))
X.append(x)
if DNN_format:
## Create DNN-training suitable version of y: e.g. [0 0 0 1 0 0 0 0 0]
y = np.zeros((9, 1))
y = y.T
n = dtypes[y_load[i, 1]]
y[:, n] = 1
Y.append(y)
else:
# Create sklearn-training suitable version of y: e.g. 'hard_tiles'
Y.append(y_load[i, 1])
# Load group data
groups = np.loadtxt('./robotsurface/groups.csv', str, skiprows=1, delimiter=',')
# Use only the group info
groups = groups[:, 1]
print(np.shape(X))
print(np.shape(Y))
gss = GroupShuffleSplit(n_splits=1, test_size=0.2)
train_dataset, test_dataset = next(gss.split(X=X, y=Y, groups=groups))
print("Train shape")
print(np.shape(train_dataset))
print("Test shape")
print(np.shape(test_dataset))
for i in range(np.shape(train_dataset)[0]):
X_train.append(X[train_dataset[i]])
y_train.append(Y[train_dataset[i]])
for i in range(np.shape(test_dataset)[0]):
X_test.append(X[test_dataset[i]])
y_test.append(Y[test_dataset[i]])
print("Train shape")
print(np.shape(X_train))
print("Test shape")
print(np.shape(X_test))
#!/usr/bin/env python import numpy as np import math import matplotlib.pyplot as plt from squaternion import quat2euler a = quat2euler(0, 0, 0, 1, degrees=True) print a print a[0] print a[2]
def create_car(self, pose, old_pos) :
bot_x = self.g_scale * pose.position.x
bot_y = self.g_scale * pose.position.y
bot_euler = quat2euler(*(pose.orientation.w,pose.orientation.x,pose.orientation.y,pose.orientation.z), degrees=False)
bot_speed = math.sqrt(math.pow(bot_x-old_pos[0],2)+math.pow(bot_y-old_pos[1],2))/self.timeDelta
return (bot_x, bot_y,bot_euler[2], bot_speed)
import numpy as np
import math
from squaternion import quat2euler, Quaternion
import pyquaternion
if __name__ == '__main__':
thetaY = math.radians(30)
Ry = np.array([[math.cos(thetaY), 0, math.sin(thetaY)], [0, 1, 0],
[-math.sin(thetaY), 0,
math.cos(thetaY)]])
quat = pyquaternion.Quaternion(matrix=Ry)
print(quat.elements)
quat2 = Quaternion(quat.elements[0], quat.elements[1], quat.elements[2],
quat.elements[3])
print(quat2euler(*quat2, degrees=True))
