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))