def computeOdom(self):
while True:
self.computeSpeed()
xdot = (1 / 2) * math.cos(self.theta) * self.speedRight + (
1 / 2) * math.cos(self.theta) * self.speedLeft
ydot = (1 / 2) * math.sin(self.theta) * self.speedRight + (
1 / 2) * math.sin(self.theta) * self.speedLeft
thetadot = (1.0 / WHEEL_BASE) * self.speedRight - (
1.0 / WHEEL_BASE) * self.speedLeft
self.x = self.x + 0.05 * xdot
self.y = self.y + 0.05 * ydot
self.theta = self.theta + 0.05 * thetadot
q = euler2quat(0.0, 0.0, self.theta)
odom = Odometry()
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = 'odom'
odom.header = h
odom.child_frame_id = 'base_link'
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.orientation.x = q[1]
odom.pose.pose.orientation.y = q[2]
odom.pose.pose.orientation.z = q[3]
odom.pose.pose.orientation.w = q[0]
self.odomPub.publish(odom)
time.sleep(0.05)
def load_Y_values(csv_filename):
'''
Given a csv file, will return a Y matrix containing all the pose information
associated with each training example as well as a list of filenames
'''
with open(csv_filename, newline='') as csvfile:
filenames = []
reader = csv.reader(csvfile)
data = list(reader)[1:]
file_examples = []
# for each image
for i in range(len(data)):
list_of_params = data[i][1].split()
examples = []
k = 0
# for each car in the image
while k < len(list_of_params):
pose = list_of_params[k+1:k+7]
pose = [float(i) for i in pose]
translation = pose[3:]
eulers = pose[:3]
# parameters are ordered roll, pitch, yaw (input dataset is yaw, pitch, roll)
quaternion_rot = list(euler2quat(eulers[2], eulers[1], eulers[0]))
examples.append(quaternion_rot + translation)
k += 7
examples = sorted(examples,key=lambda x: x[5])
file_examples.append(examples)
filenames.append(str(data[i][0]) + '.jpg')
return file_examples, filenames
def create_model_state(self, name, pos, orientation_euler):
model_state = ModelState()
model_state.model_name = name
model_state.pose.position = pos
quat = euler2quat(0, orientation_euler, 0)
model_state.pose.orientation.x = quat[3]
model_state.pose.orientation.y = quat[1]
model_state.pose.orientation.z = quat[2]
model_state.pose.orientation.w = quat[0]
return model_state
def apply_transform(self):
quat_rot = squaternion.euler2quat(*self.rotation, True)
glTranslatef(*self.transform)
rot_angle = np.degrees(2 * np.arccos(quat_rot.w))
square_coef = np.sqrt(1 - np.square(quat_rot.w))
if rot_angle > 0.001 and square_coef > 0.001:
rot_x = quat_rot.x / square_coef
rot_y = quat_rot.y / square_coef
rot_z = quat_rot.z / square_coef
glRotatef(rot_angle, rot_x, rot_y, rot_z)
glScalef(*self.scale)
def refresh_pos(self):
if self.rot_xy:
quat_rot = squaternion.euler2quat(self.rot_xy[1], self.rot_xy[0], 0, True)
rot_angle = np.degrees(2 * np.arccos(quat_rot.w))
square_coef = np.sqrt(1 - np.square(quat_rot.w))
if rot_angle > 0.001 and square_coef > 0.001:
rot_x = quat_rot.x / square_coef
rot_y = quat_rot.y / square_coef
rot_z = quat_rot.z / square_coef
glRotatef(rot_angle, rot_x, rot_y, rot_z)
glTranslatef(*[-coor for coor in self.transform])
glTranslatef(*[-coor for coor in self.pos])
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 read_bno():
"""Function to read the BNO sensor and update the bno_data object with the
latest BNO orientation, etc. state. Must be run in its own thread because
it will never return!"""
ser = serial.Serial('COM4', 9600)
time.sleep(2)
while True:
serRead = str(ser.readline()) #read from serial port
serRead = serRead.split(' | ') #split the string into parts at '|'
tmilli = serRead[0].split('|')
degx, degy, degz = 0, 0, 0
if len(tmilli) == 2:
tmilli = tmilli[1]
if len(serRead) == 8: #Make sure we're reading a complete line
AcX = serRead[1].split(' = ')
AcY = serRead[2].split(' = ')
AcZ = serRead[3].split(' = ')
Tmp = serRead[4].split(' = ')
GyX = serRead[5].split(' = ') #roll
GyY = serRead[6].split(' = ') #pitch
GyZ = serRead[7].split(' = ') #yaw
GyZ = GyZ[1].split('\\') #remove \\r\\n at end of line
AcX = int(AcX[1]) / 16384
AcY = int(AcY[1]) / 16384
AcZ = int(AcZ[1]) / 16384
GyX = int(GyX[1]) / 131
GyY = int(GyY[1]) / 131
GyZ = int(GyZ[0]) / 131
degx = ((math.atan(float(AcY) / math.sqrt(pow(AcX, 2) + pow(AcZ, 2))) * 180 / math.pi)-.58)*0.04 + (degx + (GyX-6.8) * (int(tmilli)/1000))*.96
degy = ((math.atan(-1 * AcX / math.sqrt(pow(AcY, 2) + pow(AcZ, 2))) * 180 / math.pi)+1.58)*0.04 + (degy + (GyY-2.5) * (int(tmilli)/1000))*.96
degz = degz + (GyZ-0.24)*(int(tmilli)/1000)
#Inputs are ANGLES not angular acceleration
#BELOW INPUTS ARE euler2quat(roll, pitch, yaw, degrees=False), default is in radians...send true if in degrees
q = euler2quat(degx, degz, degy, True) #typecast, scale and convert euler angles to quaternion vector
# Capture the lock on the bno_changed condition so the bno_data shared
# state can be updated.
with bno_changed:
bno_data["euler"] = [GyZ, GyY, GyX]
bno_data["temp"] = float(Tmp[1])
bno_data["quaternion"] = [q[1], q[2],q[3], q[0]]
bno_data["calibration"] = [int(3), int(3), int(3), int(3)] #"3 indicates fully calibrated; 0 indicates not calibrated" Page 68 BNO055
bno_data["acceleration"] = [AcX, AcY, AcZ]
# Notify any waiting threads that the BNO state has been updated.
bno_changed.notifyAll()
print("notified all")
# Sleep until the next reading.
time.sleep(1.0 / BNO_UPDATE_FREQUENCY_HZ)
def getAllChildren(startPoint):
for o in startPoint:
if o.get('type') == 'JOINT':
matrix = list(map(float, o[0].text.split()))
#pos = list(map(float, o[0].text.split()))
posX = float(matrix[3])
posY = float(matrix[7])
posZ = float(matrix[11])
scaleX = 1
scaleY = 1
scaleZ = 1
#pos = list(map(float, o[4].text.split()))
#rot = list(map(str, o[1].text.split()))
#rot2 = list(map(float, o[2].text.split()))
#rot3 = list(map(float, o[3].text.split()))
#rotx = math.ceil(float(math.cos(matrix[5]) + math.sin(-matrix[6]) + math.sin(matrix[9]) + math.cos(matrix[10])))
#roty = math.ceil(float(math.cos(matrix[0]) + math.sin(-matrix[2]) + math.sin(matrix[8]) + math.cos(matrix[10])))
#rotz = math.ceil(float(math.cos(matrix[0]) + math.sin(-matrix[1]) + math.sin(matrix[4]) + math.cos(matrix[5])))
rotx = matrix[0] + matrix[4] + matrix[8]
roty = matrix[1] + matrix[5] + matrix[9]
rotz = matrix[2] + matrix[6] + matrix[10]
print(rotx, ' ', roty, ' ', rotz)
q = euler2quat(rotz, roty, rotx, degrees=True)
print(q[1], ' ', q[2], ' ', q[3], ' ', q[0])
#print(scale)
#rint(parent_map[o].get('sid'))
if parent_map[o].get('type') == 'NODE':
classic = parent_map[o].get('name')
else:
classic = parent_map[o].get('sid')
tempArray = [
o.get('sid'), classic, q[1], q[2], q[3], q[0], posX, posY,
posZ, scaleX, scaleY, scaleZ
]
fullNodeArray.append(tempArray)
getAllChildren(o)
while True:
if tracking:
#bajamos la velocidad
rospy.set_param("/robot/move_base/TebLocalPlannerROS/max_vel_x", "0.3")
rospy.set_param("/robot/move_base/TebLocalPlannerROS/max_vel_x_backwards", "0.15")
rospy.set_param("/robot/move_base/TebLocalPlannerROS/max_vel_y", "0.15")
rospy.set_param("/robot/move_base/TebLocalPlannerROS/max_vel_theta", "0.3")
rospy.set_param("/robot/move_base/TebLocalPlannerROS/acc_lim_y", "0.05")
rospy.set_param("/robot/move_base/TebLocalPlannerROS/acc_lim_x", "0.05")
goal = []
goal.append(calculate_goal_position(robot, person, SECURITY_RADIUS)) # index 0
q = euler2quat(0, 0, goal[0].theta) # theta en rad
gp = MoveBaseGoal()
gp.target_pose.header.stamp = rospy.Time.now()
gp.target_pose.header.frame_id = "robot_map"
gp.target_pose.pose.position.x = goal[0].x
gp.target_pose.pose.position.y = goal[0].y
gp.target_pose.pose.position.z = goal[0].z
gp.target_pose.pose.orientation.x = q.x
gp.target_pose.pose.orientation.y = q.y
gp.target_pose.pose.orientation.z = q.z
gp.target_pose.pose.orientation.w = q.w
print ("GOAL: x = " + str(goal[0].x) + " y = " + str(goal[0].y))
print ("ROBOT: x = " + str(robot.x) + " y = " + str(robot.y))
msg = Imu()
while not geckopy.is_shutdown():
a, m, g = imu.get()
msg.linear_acceleration.x = a[0]
msg.linear_acceleration.y = a[1]
msg.linear_acceleration.z = a[2]
msg.angular_velocity.x = g[0]
msg.angular_velocity.y = g[1]
msg.angular_velocity.z = g[2]
msg.magnetic_field.x = m[0]
msg.magnetic_field.y = m[1]
msg.magnetic_field.z = m[2]
r, p, h = imu.getOrientation(a, m)
q = euler2quat(r, p, h, degrees=True)
msg.orientation.w = q.w
msg.orientation.x = q.x
msg.orientation.y = q.y
msg.orientation.z = q.z
print(msg)
pub.publish(protobufPack(msg))
rate.sleep()
if if_comm:
conn.setinit()
obs = env.reset()
env_obj = env.unwrapped
while True:
action = policy.act(obs, env)
obs, r, done, _ = env.step(action)
if freq_count == frequency:
x, y = conn.gettarget()
env_obj.set_flag(x, y)
freq_count = 0
pos = env_obj.body_xyz
quat = np.array(euler2quat(*env_obj.body_rpy))
pad = np.append(pos, quat)
joints = np.array([])
for j in env_obj.jdict:
joints = np.append(joints, env_obj.jdict[j].current_position())
joints = joints[0::2]
remote_data = np.concatenate([pad] + [joints])
if if_comm:
conn.setqpos(remote_data)
score += r
frame += 1
if if_render:
env.render("human")
freq_count += 1
