def callback_odom(self, msg): if self.rosb: self.last_odom = msg self.T = np.array((msg.pose.pose.position.x, msg.pose.pose.position.y)).reshape(1, 2) q = Quaternion(msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w) (r, p, y) = q.to_euler() self.theta = r self.R_plus = np.array([[np.cos(self.theta), -np.sin(self.theta)], [np.sin(self.theta), np.cos(self.theta)]]).reshape(2, 2) self.R_minus = np.array( [[np.cos(-self.theta), -np.sin(-self.theta)], [np.sin(-self.theta), np.cos(-self.theta)]]).reshape(2, 2) self.pose = np.array( [msg.pose.pose.position.x, msg.pose.pose.position.y, y], dtype=np.float32) self.pose_history_x.append(self.pose[0]) self.pose_history_y.append(self.pose[1]) self.thetta_history.append(self.theta) self.rosb = False
def cb(data): global f, count, bridge try: curr_img = bridge.imgmsg_to_cv2(data.image, "bgr8") except CvBridgeError as e: print(e) w = data.pose.pose.orientation.w x = data.pose.pose.orientation.x y = data.pose.pose.orientation.y z = data.pose.pose.orientation.z lon = data.pose.pose.position.x lat = data.pose.pose.position.y alt = data.pose.pose.position.z q = Quaternion(w, x, y, z) e = q.to_euler(degrees=True) roll = float(e[0]) pitch = float(e[1]) yaw = float(e[2]) st = str (count) + ".jpg" + "," + '%.3f'%lon + "," + \ '%.3f'%lat + "," + '%.3f'%alt + "," + \ '%.3f'%yaw + "," + '%.3f'%pitch + "," + \ '%.3f'%roll + "\n" print(st) f.write(st) cv2.imwrite(dirname + '/' + str(count) + '.jpg', curr_img) count += 1
def move_camera(self, t): """ This function moves the camera in a circle around the table. After 1000 steps the camera moves lower (to get a different angle and maybe more nice data) TODO: Maybe other camera movement? :param t: step :param camera: camera frame :param angle: angle in which camera need to move :param radius: around which camera is moving :return: updated angle """ cam_px, cam_py, cam_pz = self.camera.getPosition() cam_q1, cam_q2, cam_q3, cam_q4 = self.camera.getQuaternion() q = Quaternion(cam_q1, cam_q2, cam_q3, cam_q4) e1, e2, e3 = q.to_euler(degrees=True) cam_px_new = cam_px + np.cos(self.angle) * self.radius cam_py_new = cam_py + np.sin(self.angle) * self.radius if t % 1000 == 0: cam_pz = cam_pz - 0.05 e1 = e1 + 2.5 quat_new = Quaternion.from_euler(e1, e2, e3 + 5, degrees=True) self.camera.setPosition([cam_px_new, cam_py_new, cam_pz]) self.camera.setQuaternion([quat_new[0], quat_new[1], quat_new[2], quat_new[3]]) self.angle += 0.0872665 if t == 12000: self.camera.setPosition([0.6, -.75, 1.85]) self.camera.setQuaternion(self.cam_quat) self.angle = 0 self.radius = 0.075
def update(a, w, dt, q=None): na = norm(a) # if not initialized: if q is None: ax, ay, az = na if az >= 0.0: n = sqrt((az+1)*0.5) inn = 1.0/(2.0*n) q = Quaternion(n, -ay*inn, ax*inn, 0) else: n = (1-az)*0.5) inn = 1.0/(2.0*n) q = Quaternion(-ay*inn, n, 0, ax*inn) # initalized = True return q # predict quaternion from gyro readings qp = q + 0.5*q*Quaternion(0, *w)*dt qp = qp.normalize # calculate quaternion from acceleration a = .9 # q = qp*((1.0-a)*Quaternion() + scale(a, quatAcc(na, qp))) q = qp*scale(a, quatAcc(na, qp)) q = q.normalize return q
def cb(self, data): try: curr_img = self.bridge.imgmsg_to_cv2(data.image, "bgr8") except CvBridgeError as e: print(e) w = data.pose.pose.orientation.w x = data.pose.pose.orientation.x y = data.pose.pose.orientation.y z = data.pose.pose.orientation.z pos_x = data.pose.pose.position.x pos_y = data.pose.pose.position.y pos_z = data.pose.pose.position.z q = Quaternion(w, x, y, z) e = q.to_euler(degrees=True) roll = float(e[0]) pitch = float(e[1]) yaw = float(e[2]) self.pose = [pos_x, pos_y, pos_z, yaw, roll, pitch] print("pose:", self.pose) self.combine(curr_img, self.pose)
def save_imu_pose(client, filename, id): filename = filename + '.jpg' vehicle_name = "Drone" + str(id) imu_data = client.getImuData(vehicle_name=vehicle_name) state = client.getMultirotorState(vehicle_name=vehicle_name) lon = state.kinematics_estimated.position.y_val + real_init_pos[id][ 0] # x val lat = state.kinematics_estimated.position.x_val + real_init_pos[id][ 1] # y val alt = state.kinematics_estimated.position.z_val w = imu_data.orientation.w_val x = imu_data.orientation.x_val y = imu_data.orientation.y_val z = imu_data.orientation.z_val q = Quaternion(w, x, y, z) e = q.to_euler(degrees=True) roll = float(e[0]) pitch = float(e[1]) yaw = float(e[2]) st = (os.path.basename(filename)) + "," + '%.3f'%lon + "," + \ '%.3f'%lat + "," + '%.3f'%alt + "," + \ '%.3f'%yaw + "," + '%.3f'%pitch + "," + \ '%.3f'%roll + "\n" print(st) global file_handle file_handle.write(st)
def test_quaternion(): q = Quaternion(1,2,3,4) assert q.w == 1 assert q.x == 2 assert q.y == 3 assert q.z == 4 assert len(q) == 4 for i, ii in zip(q, (1,2,3,4)): assert i == ii assert q.to_dict() == {'w':1, 'x':2, 'y':3, 'z':4} assert q[0] == 1 assert q[1] == 2 assert q[2] == 3 assert q[3] == 4 assert q[3] == q[-1] assert q[0] == q.scalar assert q[1:] == q.vector assert q[-3:] == q.vector qq = q.conjugate assert q.w == qq.w == 1 assert q.x == -qq.x == 2 assert q.y == -qq.y == 3 assert q.z == -qq.z == 4 with pytest.raises(IndexError): q[4]
def show_image_pose(frame, pose): cv2.imshow('frame', frame) if (cv2.waitKey(25) & 0xFF == ord('q')): return False posx = pose.pose.position.x posy = pose.pose.position.y posz = pose.pose.position.z w = pose.pose.orientation.w x = pose.pose.orientation.x y = pose.pose.orientation.y z = pose.pose.orientation.z q = Quaternion(w, x, y, z) e = q.to_euler(degrees=True) roll = float(e[0]) pitch = float(e[1]) yaw = float(e[2]) print("orientation:", roll, pitch, yaw) print("position:", posx, posy, posz) print("------------------------------") return True
def robot_pose_cb(self, data): self.robot_x = data.pose.pose.position.x self.robot_y = data.pose.pose.position.y q = Quaternion(data.pose.pose.orientation.w, data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z) self.robot_th = q.to_euler(degrees=False)[2]
def test_norm_mag(): q = Quaternion(1, 1, 1, 1) assert q.magnitude == 2.0 q = q.normalize assert q.magnitude == 1.0 q = Quaternion(0, 0, 0, 0) assert q.magnitude == 0.0
def updateAG(self, a, g, beta, dt, degrees=True): q0, q1, q2, q3 = self.q if degrees: g = (x * DEG2RAD for x in g) gx, gy, gz = g ax, ay, az = a # Rate of change of quaternion from gyroscope qDot1 = 0.5 * (-q1 * gx - q2 * gy - q3 * gz) qDot2 = 0.5 * (q0 * gx + q2 * gz - q3 * gy) qDot3 = 0.5 * (q0 * gy - q1 * gz + q3 * gx) qDot4 = 0.5 * (q0 * gz + q1 * gy - q2 * gx) # Compute feedback only if accelerometer measurement valid (avoids NaN # in accelerometer normalisation) ax, ay, az = normalize3(ax, ay, az) # Auxiliary variables to avoid repeated arithmetic _2q0 = 2.0 * q0 _2q1 = 2.0 * q1 _2q2 = 2.0 * q2 _2q3 = 2.0 * q3 _4q0 = 4.0 * q0 _4q1 = 4.0 * q1 _4q2 = 4.0 * q2 _8q1 = 8.0 * q1 _8q2 = 8.0 * q2 q0q0 = q0 * q0 q1q1 = q1 * q1 q2q2 = q2 * q2 q3q3 = q3 * q3 # Gradient decent algorithm corrective step s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay s1 = _4q1 * q3q3 - _2q3 * ax + 4.0 * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az s2 = 4.0 * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az s3 = 4.0 * q1q1 * q3 - _2q1 * ax + 4.0 * q2q2 * q3 - _2q2 * ay s0, s1, s2, s3 = Quaternion(s0, s1, s2, s3).normalize # Apply feedback step qDot1 -= beta * s0 qDot2 -= beta * s1 qDot3 -= beta * s2 qDot4 -= beta * s3 # Integrate rate of change of quaternion to yield quaternion q0 += qDot1 * dt q1 += qDot2 * dt q2 += qDot3 * dt q3 += qDot4 * dt self.q = Quaternion(q0, q1, q2, q3).normalize return self.q
def test_rotation_matrix(): q = Quaternion(1,0,0,0) r = q.to_rot() for i, row in enumerate(((1, 0, 0), (0, 1, 0), (0, 0, 1))): for j, col in enumerate(row): assert r[i][j] == col with pytest.raises(NotImplementedError): Quaternion.from_rot(r)
def update_pose(self, data): try: self.pose.x = data.pose[self.robot_id].position.x self.pose.y = data.pose[self.robot_id].position.y quaternion = Quaternion(data.pose[self.robot_id].orientation.x, data.pose[self.robot_id].orientation.y, data.pose[self.robot_id].orientation.z, data.pose[self.robot_id].orientation.w) self.pose.theta = (quaternion.to_euler()[0] / pi) + 0.25 except IndexError: None
def compensate(self, accel, mag): """ """ try: mx, my, mz = normalize3(*mag) ax, ay, az = normalize3(*accel) pitch = asin(-ax) if abs(pitch) >= pi / 2: roll = 0.0 else: roll = asin(ay / cos(pitch)) # mx, my, mz = mag x = mx * cos(pitch) + mz * sin(pitch) y = mx * sin(roll) * sin(pitch) + my * cos(roll) - mz * sin( roll) * cos(pitch) heading = atan2(y, x) # wrap heading between 0 and 360 degrees if heading > 2 * pi: heading -= 2 * pi elif heading < 0: heading += 2 * pi if self.angle_units == Angle.degrees: roll *= rad2deg pitch *= rad2deg heading *= rad2deg elif self.angle_units == Angle.quaternion: return Quaternion.from_euler(roll, pitch, heading) return ( roll, pitch, heading, ) except ZeroDivisionError as e: print('Error', e) if self.angle_units == Angle.quaternion: return Quaternion(1, 0, 0, 0) else: return ( 0.0, 0.0, 0.0, )
def test_euler(): q = Quaternion.from_euler(0, 0, 0) assert q == Quaternion(1, 0, 0, 0), f"{q} != Quaternion(1,0,0,0)" assert q.magnitude == 1.0, f"{q.magnitude} magnitude != 1.0" delta = 10 for i in range(-179, 180, delta): for j in range(-89, 90, delta): for k in range(-179, 180, delta): q = Quaternion.from_euler(i, j, k, degrees=True) e = q.to_euler(degrees=True) for a, b in zip((i, j, k,), e): diff = abs(a - b) assert (diff < 0.001), "{}: {} {} {}".format(diff, i, j, k)
def callback_odom(self, msg): self.T = np.array((msg.pose.pose.position.x, msg.pose.pose.position.y)).reshape(1, 2) q = Quaternion(msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w) (r, p, y) = q.to_euler() theta = r self.R_plus = np.array([[np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]]).reshape(2, 2) self.R_minus = np.array([[np.cos(-theta), -np.sin(-theta)], [np.sin(-theta), np.cos(-theta)]]).reshape(2, 2)
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 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 = Quaternion(msg.pose[i].orientation.w, msg.pose[i].orientation.x, msg.pose[i].orientation.y, msg.pose[i].orientation.z).to_euler() 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 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 callback(self): # self.hz_update() a = self.imu.acceleration g = self.imu.gyro if self.calibrate: msg = { "timestamp": self.get_clock().now().nanoseconds * 1e-9, "a": a, "g": g } self.deque.append(msg) if not self.continue_pub: return self.imu_msg.header.stamp = self.get_clock().now().to_msg() self.imu_msg.linear_acceleration.x = a[0] self.imu_msg.linear_acceleration.y = a[1] self.imu_msg.linear_acceleration.z = a[2] self.imu_msg.angular_velocity.x = g[0] self.imu_msg.angular_velocity.y = g[1] self.imu_msg.angular_velocity.z = g[2] # generally I would also have the magnetometer to determine heading # not sure I like this solution/hack :P # q = self.compass.get_quaternion(a, None) q = Quaternion() self.imu_msg.orientation.x = q.x self.imu_msg.orientation.y = q.y self.imu_msg.orientation.z = q.z self.imu_msg.orientation.w = q.w self.pub_imu.publish(self.imu_msg)
def test_compare(): a = Quaternion(1,1,1,1) b = Quaternion(1,0,0,1) c = Quaternion(1,1,1,1) assert a == a assert a == c assert c == a assert a.normalize == c.normalize assert a != b assert b != a assert a.normalize != b.normalize assert a.magnitude == c.magnitude assert a.magnitude != b.magnitude
def pose_cb(self, data): w = data.pose.orientation.w x = data.pose.orientation.x y = data.pose.orientation.y z = data.pose.orientation.z pos_x = data.pose.position.x pos_y = data.pose.position.y pos_z = data.pose.position.z q = Quaternion(w, x, y, z) e = q.to_euler(degrees=True) roll = float(e[0]) pitch = float(e[1]) yaw = float(e[2]) self.pose = [pos_x, pos_y, pos_z, yaw, roll, pitch]
def pose_callback(data): global yaw, pitch, roll global lon, lat, alt w = data.pose.orientation.w x = data.pose.orientation.x y = data.pose.orientation.y z = data.pose.orientation.z lon = float(data.pose.position.x) + float(offset[0]) lat = float(data.pose.position.y) + float(offset[1]) alt = float(data.pose.position.z) + float(offset[2]) q = Quaternion(w, x, y, z) e = q.to_euler(degrees=True) roll = float(e[0]) pitch = float(e[1]) yaw = float(e[2])
def dwa_wrapper(final_list): odom_dec = {} q = Quaternion(final_list[0].pose.pose.orientation.w, final_list[0].pose.pose.orientation.x, final_list[0].pose.pose.orientation.y, final_list[0].pose.pose.orientation.z) e = q.to_euler(degrees=False) odom_dec["x"] = final_list[0].pose.pose.position.x odom_dec["y"] = final_list[0].pose.pose.position.y odom_dec["theta"] = e[2] odom_dec["u"] = final_list[0].twist.twist.linear.x odom_dec["omega"] = final_list[0].twist.twist.angular.z cnfg = Config(odom_dec, final_list[1]) obs = Obstacles(final_list[2].ranges, cnfg) v_list, w_list, cost_list = DWA(cnfg, obs) return v_list, w_list, cost_list, final_list[3]
def callback_modelstates(self, msg): for i in range(len(msg.name)): if (msg.name[i][0:5] == "actor"): actor_id = int(msg.name[i][5]) posx = msg.pose[i].position.x posy = msg.pose[i].position.y posz = msg.pose[i].position.z x = msg.pose[i].orientation.x y = msg.pose[i].orientation.y z = msg.pose[i].orientation.z w = msg.pose[i].orientation.w q = Quaternion(w, x, y, z) e = q.to_euler(degrees=True) self.pedestrian_pose[actor_id] = [posx, posy, e[2]]
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 getStates(self, data): pose = data.pose self.link0_position = np.array([ pose[4].position.x, pose[4].position.y, Quaternion(w=pose[4].orientation.w, x=pose[4].orientation.x, y=pose[4].orientation.y, z=pose[4].orientation.z).to_euler()[2] ]) self.object_position = np.array([ pose[5].position.x, pose[5].position.y, Quaternion(w=pose[5].orientation.w, x=pose[5].orientation.x, y=pose[5].orientation.y, z=pose[5].orientation.z).to_euler()[2] ]) self.target_position = np.array( [pose[7].position.x, pose[7].position.y]) self.object_hight = data.pose[5].position.z
def compensate(self, accel, mag): """ """ try: mx, my, mz = self.normalize(*mag) # ax, ay, az = self.grav(*accel) ax, ay, az = self.normalize(*accel) pitch = asin(-ax) if abs(pitch) >= pi / 2: roll = 0.0 else: roll = asin(ay / cos(pitch)) # mx, my, mz = mag x = mx * cos(pitch) + mz * sin(pitch) y = mx * sin(roll) * sin(pitch) + my * cos(roll) - mz * sin( roll) * cos(pitch) heading = atan2(y, x) # wrap heading between 0 and 360 degrees if heading > 2 * pi: heading -= 2 * pi elif heading < 0: heading += 2 * pi if self.angle_units == self.DEGREES: roll *= 180 / pi pitch *= 180 / pi heading *= 180 / pi elif self.angle_units == self.QUATERNIONS: return euler2quat(roll, pitch, heading) return ( roll, pitch, heading, ) except ZeroDivisionError as e: print('Error', e) if self.angle_units == self.QUATERNIONS: return Quaternion(1, 0, 0, 0) else: return ( 0.0, 0.0, 0.0, )
def Angles(): streamMotion = resolve_stream('type', 'Motion') # Resolve stream inlet2 = StreamInlet(streamMotion[0]) # Create a new inlet to read Motion data from the stream sample, timestamp = inlet2.pull_sample() # Get a new sample with timestamp # Motion data is received as Quaternion # This converts quaternion to angles with degrees as units # The Q1,Q2,Q3,Q4 are at index 3,4,5,6 ofthe array input q = Quaternion(sample[3], sample[4], sample[5], sample[6]) e = q.to_euler(degrees=True) # Processes/stores the angles correctly angles = [] angles.append(e) angles = np.array(angles) angles.flatten() np.rint([angles]) print(angles) # Angles are stored in an array, we return them separated return angles[0][0],angles[0][1],angles[0][2]
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