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
