def turn2(d, r, init_time, init_yaw, current_time, current_yaw):
status=True
msg="Turned successfully"
if init_yaw == None:
rospy.logerr("Initialization time is None, the startup was not correctly done!")
msg="Initialization time is None, the startup was not correctly done!"
return False, msg
else:
orient = Orientation()
correct_yaw = orient.getCorrectedYaw(init_time, init_yaw, current_time, current_yaw)
print " Current_yaw ->" + str(correct_yaw)
if d == 'NORTH':
pass
elif d == 'SOUTH':
init_yaw = init_yaw + radians(180)
elif d == 'EAST':
init_yaw = init_yaw + radians(90)
elif d == 'WEST':
init_yaw = init_yaw - radians(90)
else:
rospy.logerr("Direction is not correct")
print "Direction_yaw ->" + str(init_yaw)
if correct_yaw > init_yaw:
angle = correct_yaw - init_yaw
return turn(degrees(angle), -1)
else:
angle = init_yaw - correct_yaw
return turn(degrees(angle), 1)
def turn2(d, r, init_time, init_yaw, current_time, current_yaw):
status = True
msg = "Turned successfully"
if init_yaw == None:
rospy.logerr(
"Initialization time is None, the startup was not correctly done!")
msg = "Initialization time is None, the startup was not correctly done!"
return False, msg
else:
orient = Orientation()
correct_yaw = orient.getCorrectedYaw(init_time, init_yaw, current_time,
current_yaw)
print " Current_yaw ->" + str(correct_yaw)
if d == 'NORTH':
pass
elif d == 'SOUTH':
init_yaw = init_yaw + radians(180)
elif d == 'EAST':
init_yaw = init_yaw + radians(90)
elif d == 'WEST':
init_yaw = init_yaw - radians(90)
else:
rospy.logerr("Direction is not correct")
print "Direction_yaw ->" + str(init_yaw)
if correct_yaw > init_yaw:
angle = correct_yaw - init_yaw
return turn(degrees(angle), -1)
else:
angle = init_yaw - correct_yaw
return turn(degrees(angle), 1)
def test_orientation_from_good_string_aperture(self):
orientationN_wide = Orientation.from_orientation('<N')
self.assertEqual(orientationN_wide.angleDirection, pi / 2)
self.assertEqual(orientationN_wide.angleAperture, pi)
orientationN_narrow = Orientation.from_orientation('>N')
self.assertEqual(orientationN_narrow.angleDirection, pi / 2)
self.assertEqual(orientationN_narrow.angleAperture, pi / 4)
def __init__(self):
#inicializando variáveis
self.distance_x = 0
self.distance_y = 0
self.center_H = 0
self.alpha = 0
self.beta = 0
self.worldTranslation = [0, 0, 0]
self.distance_x_array = []
self.distance_y_array = []
self.alpha_array = []
self.beta_array = []
self.markerFound = False
#coordenadas dos marcadores
self.coordenadas = []
self.coordenadas.append(
np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0]]))
self.coordenadas.append(
np.array([[0, 0, 18.5], [0, 0, 0], [0, 18.5, 0], [0, 18.5, 18.5]]))
self.coordenadas.append(
np.array([[0, 50, 18.5], [0, 50, 0], [0, 68.5, 0], [0, 68.5,
18.5]]))
self.coordenadas.append(
np.array([[0, 100, 18.5], [0, 100, 0], [0, 118.5, 0],
[0, 118.5, 18.5]]))
self.coordenadas.append(
np.array([[50, 150, 18.5], [50, 150, 0], [68.5, 150, 0],
[68.5, 150, 18.5]]))
self.coordenadas.append(
np.array([[100, 150, 18.5], [100, 150, 0], [118.5, 150, 0],
[118.5, 150, 18.5]]))
self.coordenadas.append(
np.array([[150, 150, 18.5], [150, 150, 0], [168.5, 150, 0],
[168.5, 150, 18.5]]))
self.coordenadas.append(
np.array([[200, 118.5, 18.5], [200, 118.5, 0], [200, 100, 0],
[200, 100, 18.5]]))
self.coordenadas.append(
np.array([[200, 68.5, 18.5], [200, 68.5, 0], [200, 50, 0],
[200, 50, 18.5]]))
self.coordenadas.append(
np.array([[200, 18.5, 18.5], [200, 18.5, 0], [200, 0, 0],
[200, 0, 18.5]]))
self.coordenadas.append(
np.array([[0, 0, 18.5], [0, 0, 0], [0, 18.5, 0], [0, 18.5, 18.5]]))
#self.template = cv2.imread('data/7x7-10.jpg', 0)
#self.finder = Finder(self.template)
self.arucoFinder = ArucoFinder()
self.orientation = Orientation()
#parâmetros da câmera, conseguido com camera calibration
with np.load('camera_array_fisheye.npz') as X:
self.mtx, self.dist, _, _ = [
X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')
]
def test_orientation_from_good_string_length_1(self):
orientationN = Orientation.from_orientation('N')
self.assertEqual(orientationN.angleDirection, pi / 2)
self.assertEqual(orientationN.angleAperture, pi / 2)
orientationS = Orientation.from_orientation('S')
self.assertEqual(orientationS.angleDirection, 3 * pi / 2)
self.assertEqual(orientationS.angleAperture, pi / 2)
orientationW = Orientation.from_orientation('W')
self.assertEqual(orientationW.angleDirection, pi)
self.assertEqual(orientationW.angleAperture, pi / 2)
orientationE = Orientation.from_orientation('E')
self.assertEqual(orientationE.angleDirection, 0)
self.assertEqual(orientationE.angleAperture, pi / 2)
def test_orientation_from_good_string_length_2(self):
orientationNE = Orientation.from_orientation('NE')
self.assertEqual(orientationNE.angleDirection, pi / 4)
self.assertEqual(orientationNE.angleAperture, pi / 4)
orientationSE = Orientation.from_orientation('SE')
self.assertEqual(orientationSE.angleDirection, 7 * pi / 4)
self.assertEqual(orientationSE.angleAperture, pi / 4)
orientationSW = Orientation.from_orientation('SW')
self.assertEqual(orientationSW.angleDirection, 5 * pi / 4)
self.assertEqual(orientationSW.angleAperture, pi / 4)
orientationNW = Orientation.from_orientation('NW')
self.assertEqual(orientationNW.angleDirection, 3 * pi / 4)
self.assertEqual(orientationNW.angleAperture, pi / 4)
def test_orientation_from_good_string_length_multiple(self):
orientationNNE = Orientation.from_orientation('NNE')
self.assertEqual(orientationNNE.angleDirection, 3 * pi / 8)
self.assertEqual(orientationNNE.angleAperture, pi / 8)
orientationENE = Orientation.from_orientation('ENE')
self.assertEqual(orientationENE.angleDirection, pi / 8)
self.assertEqual(orientationENE.angleAperture, pi / 8)
orientationNENE = Orientation.from_orientation('NENE')
self.assertEqual(orientationNENE.angleDirection, 3 * pi / 16)
self.assertEqual(orientationNENE.angleAperture, pi / 16)
orientationSSW = Orientation.from_orientation('SSW')
self.assertEqual(orientationSSW.angleDirection, 11 * pi / 8)
self.assertEqual(orientationSSW.angleAperture, pi / 8)
def test_getRandomDirection(self):
orientation = Orientation.from_angle(pi / 2.0, pi / 4.0)
random.seed(1)
for _ in range(0, 10):
direction = orientation.getRandomDirection()
self.assertGreaterEqual(
direction,
orientation.angleDirection - 0.5 * orientation.angleAperture)
self.assertLess(
direction,
orientation.angleDirection + 0.5 * orientation.angleAperture)
def test_matrix_init(self):
random_m = ttf.random_rotation_matrix()
ornt = Orientation(random_m[:3, :3])
euler = ornt.euler
quat = ornt.quaternion
matrix = ornt.matrix3
self.assertTrue(np.allclose(euler,
ttf.euler_from_matrix(random_m)))
self.assertTrue(np.allclose(quat,
ttf.quaternion_from_matrix(random_m)))
self.assertTrue(np.allclose(matrix, random_m[:3, :3]))
def test_euler_init(self):
random_e = ttf.euler_from_quaternion(ttf.random_quaternion())
ornt = Orientation(random_e)
euler = ornt.euler
quat = ornt.quaternion
matrix = ornt.matrix3
self.assertTrue(np.allclose(euler, random_e))
self.assertTrue(np.allclose(
matrix, ttf.euler_matrix(*random_e)[:3, :3]))
self.assertTrue(np.allclose(
quat, ttf.quaternion_from_euler(*random_e)))
def generate_fractures(self, min_distance, min_radius, max_radius):
fractures = []
for i in range(self.frac_type.n_fractures):
x = uniform(2 * min_distance, 1 - 2 * min_distance)
y = uniform(2 * min_distance, 1 - 2 * min_distance)
z = uniform(2 * min_distance, 1 - 2 * min_distance)
tpl = TPL(self.frac_type.kappa, self.frac_type.r_min,
self.frac_type.r_max, self.frac_type.r_0)
r = tpl.rnd_number()
orient = Orientation(self.frac_type.trend, self.frac_type.plunge,
self.frac_type.k)
axis, angle = orient.compute_axis_angle()
fd = FractureData(x, y, z, r, axis[0], axis[1], axis[2], angle,
i * 100)
fractures.append(fd)
return fractures
def main():
global m
global ot
global vp
global ss
global count_down
v = None
count_start = None
broad_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
broad_sock.bind(('', PORT_TO_BROADCAST))
data = None
addr = None
print 'Wait for broadcast message.'
while True:
data, addr = broad_sock.recvfrom(4096)
if Check_Identity(data) is True:
break
broad_sock.close()
host = addr[0]
print 'Get broadcast message from host:', host
ss = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
ss.connect((host, PORT_FROM_VIDEO))
sr = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sr.bind(('', PORT_TO_VIDEO))
sr.listen(1)
md, addr = sr.accept()
print 'Start to instantiate classes.'
while True:
try:
m = Motor() if not m else m
ot = Orientation() if not ot else ot
if not count_start:
count_start = time.time()
v = VFFmpeg(host) if not v else v
if m and ot and v:
# if m and ot:
break
except Exception, e:
print '[FATAL ERROR]', e
exit(-1)
def __init__(self):
self.distance_x = 0
self.distance_y = 0
self.alpha = 0
self.beta = 0
self.center = 0
self.markerFound = False
self.coordenadas = np.array([[0, 30, 0], [0, 0, 0], [21, 0, 0],
[21, 30, 0]])
self.worldTranslation = [0, 0, 0]
self.template = cv2.imread('data/board.jpg', 0)
self.finder = ArucoFinder()
self.orientation = Orientation()
with np.load('camera_array_fisheye.npz') as X:
self.mtx, self.dist, _, _ = [
X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')
]
def update(self, car, time):
ori = Orientation(car.physics.rotation)
pos = Vec3(car.physics.location) + Vec3(z=12) - 30 * ori.forward
if len(self.points) == 0:
# Initial point
point = TrailPoint(pos, time)
self.points.append(point)
else:
# Add points
prev = self.points[-1]
diff = pos - prev.pos
if diff.longer_than(self.segment_size):
point = TrailPoint(pos, time)
self.points.append(point)
# Remove points
earliest = self.points[0]
if earliest.time + self.duration < time:
self.points = self.points[1:]
self.points = self.points[-MAX_TRAIL_LENGTH:]
def __init__(self, position = None, velocity = None, orientation = None):
""" Initializes the boid.
:param position: Position, random if not given
:type position: Vec2d
:param velocity: Velocity, random if not given
:type velocity: Vec2d
:param orientation: Orientation, random if not given
:type orientation: Orientation
"""
if position == None:
position = engine.Engine.randPosition()
if velocity == None:
velocity = engine.Engine.randForce()
if orientation == None:
orientation = Orientation.new()
self.position = position
self.velocity = velocity
self.orientation = orientation
print("Exiting")
exit()
# Attach the Ctrl+C signal interrupt
signal.signal(signal.SIGINT, ctrlC)
# Setup encoder
encoder = Encoder()
encoder.initEncoders()
# Setup motor control
motorControl = MotorControl(encoder)
orientation = Orientation(encoder, motorControl)
tof = TOF()
pid = PID(0.5, -6, 6)
try:
with open('calibratedSpeeds.json') as json_file:
motorControl.speedMap = json.load(json_file)
except IOError:
input("You must calibrate speeds first. Press enter to continue")
motorControl.calibrateSpeeds()
while True:
print("\n(1) Calibrate speeds")
robot.draw()
robot.connect_color()
markov = Markov(world, start_orientation, "rddesmit")
found = False
while not found:
location = markov.current_estimate()
destination = (1, 1)
# drive
robot.drive(0.10)
markov.move()
# rotate
direction = Orientation.calculate_direction((location[1], location[2]), destination)
while direction != markov._orientation:
rotate_direction = Orientation.rotate(markov._orientation, direction)
if rotate_direction == Orientation.LEFT:
markov.rotate_left()
robot.turn_left()
time.sleep(3)
elif rotate_direction == Orientation.RIGHT:
markov.rotate_right()
robot.turn_right()
time.sleep(3)
else:
pass
# calculate
class DistanceCalculator():
def __init__(self):
#inicializando variáveis
self.distance_x = 0
self.distance_y = 0
self.center_H = 0
self.alpha = 0
self.beta = 0
self.worldTranslation = [0, 0, 0]
self.distance_x_array = []
self.distance_y_array = []
self.alpha_array = []
self.beta_array = []
self.markerFound = False
#coordenadas dos marcadores
self.coordenadas = []
self.coordenadas.append(
np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0]]))
self.coordenadas.append(
np.array([[0, 0, 18.5], [0, 0, 0], [0, 18.5, 0], [0, 18.5, 18.5]]))
self.coordenadas.append(
np.array([[0, 50, 18.5], [0, 50, 0], [0, 68.5, 0], [0, 68.5,
18.5]]))
self.coordenadas.append(
np.array([[0, 100, 18.5], [0, 100, 0], [0, 118.5, 0],
[0, 118.5, 18.5]]))
self.coordenadas.append(
np.array([[50, 150, 18.5], [50, 150, 0], [68.5, 150, 0],
[68.5, 150, 18.5]]))
self.coordenadas.append(
np.array([[100, 150, 18.5], [100, 150, 0], [118.5, 150, 0],
[118.5, 150, 18.5]]))
self.coordenadas.append(
np.array([[150, 150, 18.5], [150, 150, 0], [168.5, 150, 0],
[168.5, 150, 18.5]]))
self.coordenadas.append(
np.array([[200, 118.5, 18.5], [200, 118.5, 0], [200, 100, 0],
[200, 100, 18.5]]))
self.coordenadas.append(
np.array([[200, 68.5, 18.5], [200, 68.5, 0], [200, 50, 0],
[200, 50, 18.5]]))
self.coordenadas.append(
np.array([[200, 18.5, 18.5], [200, 18.5, 0], [200, 0, 0],
[200, 0, 18.5]]))
self.coordenadas.append(
np.array([[0, 0, 18.5], [0, 0, 0], [0, 18.5, 0], [0, 18.5, 18.5]]))
#self.template = cv2.imread('data/7x7-10.jpg', 0)
#self.finder = Finder(self.template)
self.arucoFinder = ArucoFinder()
self.orientation = Orientation()
#parâmetros da câmera, conseguido com camera calibration
with np.load('camera_array_fisheye.npz') as X:
self.mtx, self.dist, _, _ = [
X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')
]
def processImage(self, img):
'''
Processamento de um frame da imagem. Deve encontrar os features
relevantes, calcular a distancia deles, e calcular a posição
absoluta da camera.
Os valores calculados ficam armazenados
em distance_x_array, distance_y_array, alpha_array e beta_array
A cada 5 frames ou mais deve ser utilizado o método mediaDistance
para atualizar self.distance_x, self.distance_y,
self.alpha e self.beta
'''
# se a imagem estiver vazia
if (img.any is None):
return self.distance_x, self.distance_y, self.alpha, self.beta
# calibra a imagem
img = calibrateImagem(img)
# encontra todos os marcadores relevantes
#dst, img2 = self.finder.find_marker(img)
dst, ids = self.arucoFinder.detect(img)
if (len(dst) == 0):
self.markerFound = False
return
self.center_H = (dst[0][0][0] + dst[0][3][0]) / 2
self.foundMarker = True
if (len(ids) == 0):
return
# Para cada marcador encontrado (ids), deve ser encontrado um worldTranslation,
# um alpha e um beta diferente. Calcula-se todos e armazena o resultado
# numa array
worldTranslation_ids_array = []
alpha_ids_array = []
beta_ids_array = []
for i in range(len(ids)):
worldTranslation, alpha, beta = self.calculateDistance(
dst[i], ids[i])
worldTranslation_ids_array.append(worldTranslation)
alpha_ids_array.append(alpha)
beta_ids_array.append(beta)
#calcular média dos resultados na array, para encontrar a posição mais
# precisa possível
T = np.transpose(worldTranslation_ids_array)
distance_x = np.median(T[0][0])
distance_y = np.median(T[0][1])
alpha = np.median(alpha_ids_array)
beta = np.median(beta_ids_array)
# armazena os valores do frame em uma array
self.distance_x_array.append(distance_x)
self.distance_y_array.append(distance_y)
self.alpha_array.append(alpha)
self.beta_array.append(beta)
return
def calculateDistance(self, pts, id):
'''
Dado 4 pontos e o id do marcador, calcula o vetor worldTranslation,
alpha e beta
'''
#tratamento de matrizes para usar no SolvePnp
corners = []
for p in pts:
corners.append(p)
corners = np.asarray(corners, np.float64)
mtx = np.asarray(self.mtx, np.float64)
coord = np.asarray(self.coordenadas[id], np.float64)
#encontra o vetor translação e rotação da câmera
found, rvec, tvec = cv2.solvePnP(coord, corners, mtx, None)
np_rodrigues = np.asarray(rvec[:, :], np.float64)
rot_matrix = cv2.Rodrigues(np_rodrigues)[0]
#converte o vetor translação da camera para o vetor translação do mundo
R_inv = inv(rot_matrix)
worldTranslation = -np.dot(R_inv, tvec)
# cálculo do ângulo
# alpha = np.arctan(rot_matrix[2][2]/rot_matrix[0][2])
# beta = np.arccos(rot_matrix[1][2])
# alpha = alpha*180/3.14
# beta = beta*180/3.14
alpha, beta = self.orientation.calculateOrientation(pts)
if (alpha < 360):
# Conversão do angulo relativo (em relação ao marcador) para angulo
# absoluto (em relação ao mundo)
if (id <= 3):
alpha = alpha + 180
elif (id <= 6):
alpha = alpha + 90
elif (id <= 9):
alpha = alpha
else:
alpha = self.alpha
beta = self.beta
return worldTranslation, alpha, beta
def mediaDistance(self):
'''
Método utilizado a cada 5 frames ou mais. Calcula a média das
distancias nesses 5 frames, e atualiza self.distance's
'''
if (len(self.distance_x_array) == 0):
return
# retira os outliers
self.distance_x_array = self.reject_outliers(self.distance_x_array)
self.distance_y_array = self.reject_outliers(self.distance_y_array)
self.alpha_array = self.reject_outliers(self.alpha_array)
self.beta_array = self.reject_outliers(self.beta_array)
self.distance_x = np.median(self.distance_x_array)
self.distance_y = np.median(self.distance_y_array)
self.alpha = np.median(self.alpha_array)
self.beta = np.median(self.beta_array)
self.distance_x_array = []
self.distance_y_array = []
self.alpha_array = []
self.beta_array = []
def reject_outliers(self, data):
'''
Dado uma array, retira os outliers dele
'''
m = 2
d = np.abs(data - np.median(data))
mdev = np.median(d)
if (mdev == 0):
return data
s = d / mdev if mdev else 0.
newData = []
for i in range(len(s)):
if (s[i] < m):
newData.append(data[i])
return newData
def writeDistance(self, img):
'''
Escreve as distâncias armazenadas na classe na imagem.
'''
cv2.rectangle(img, (img.shape[1] - 240, img.shape[0] - 60),
(img.shape[1] - 20, img.shape[0] - 10), (255, 255, 255),
-1)
font = cv2.FONT_HERSHEY_SIMPLEX
text = ("x = %.2fcm" % self.distance_x)
text2 = ("y = %.2fcm" % self.distance_y)
text3 = ("angulo = %.0fgraus, %.0f " % (self.alpha, self.beta))
cv2.putText(img, text, (img.shape[1] - 220, img.shape[0] - 45), font,
0.5, (0, 0, 0), 2, cv2.LINE_AA)
cv2.putText(img, text2, (img.shape[1] - 220, img.shape[0] - 30), font,
0.5, (0, 0, 0), 2, cv2.LINE_AA)
cv2.putText(img, text3, (img.shape[1] - 220, img.shape[0] - 15), font,
0.5, (0, 0, 0), 2, cv2.LINE_AA)
def test_orientation_from_standard_angle(self):
orientation = Orientation.from_angle(pi / 2.0, pi / 4.0)
self.assertEqual(orientation.angleDirection, pi / 2.0)
self.assertEqual(orientation.angleAperture, pi / 4.0)
except IOError:
input("You must calibrate speeds first. Press enter to continue")
motorControl.calibrateSpeeds()
while True:
print("\n(1) Calibrate speeds")
print("(2) Distance")
print("(3) Orientation")
print("(4) Rectangle")
print("(5) Circle")
taskOption = int(input("Which task you do you want run? (1 - 5): "))
if taskOption == 1:
motorControl.calibrateSpeeds()
elif taskOption == 2:
distance = Distance(encoder, motorControl)
distance.moveDistanceInSeconds()
elif taskOption == 3:
orientation = Orientation(encoder, motorControl)
orientation.rotateDegrees()
elif taskOption == 4:
orientation = Orientation(encoder, motorControl)
rectangle = Rectangle(encoder, motorControl, orientation)
rectangle.travelRectangle()
elif taskOption == 5:
orientation = Orientation(encoder, motorControl)
circle = Circle(encoder, motorControl, orientation)
circle.traverseCircle()
pass
#!/usr/bin/python
from orientation import Orientation
import rospy
if __name__ == '__main__':
rospy.init_node("euler_orientation_test")
orient = Orientation()
while True:
print "Raw yaw -> ", str(orient.getRawYaw())
print "Corrected yaw -> ", str(orient.getCorrectedYaw())
rospy.sleep(3)
def do_collisions(self, script, packet):
ball_pos = Vec3(packet.game_ball.physics.location)
for i in range(len(self.points) - 2):
seg_start = self.points[i].pos
seg_end = self.points[i + 1].pos
seg = seg_end - seg_start
# Ball
if not IGNORE_BALL_COLLISION:
ball_pos_from_seg_pov = ball_pos - seg_start
t = (ball_pos_from_seg_pov.dot(seg) / seg.dot(seg))
ball_proj_seg = seg * t
seg_ball = (ball_pos_from_seg_pov - ball_proj_seg)
if 0 <= t <= 1 and not seg_ball.longer_than(100):
# Collision
seg_ball_u = seg_ball.unit()
vel = Vec3(packet.game_ball.physics.velocity)
refl_vel = vel - 1.9 * vel.dot(seg_ball_u) * seg_ball_u
ball_pos_moved = seg_start + ball_proj_seg + seg_ball_u * 101
script.set_game_state(
GameState(ball=BallState(physics=Physics(
location=ball_pos_moved.to_desired_vec(),
velocity=refl_vel.to_desired_vec()))))
script.particle_burst(
packet.game_info.seconds_elapsed,
seg_start + ball_proj_seg + seg_ball_u * 10,
seg_ball_u, int(1 + abs(vel.dot(seg_ball_u) / 300)**3),
self.team)
hit_strength = abs(seg_ball_u.dot(vel))
script.sounds.ball_hit(hit_strength)
# Cars
for car_index in range(packet.num_cars):
car = packet.game_cars[car_index]
if car.is_demolished \
or (car.is_bot and IGNORE_BOT_COLLISION) \
or (not car.is_bot and IGNORE_HUMAN_COLLISION):
continue
car_ori = Orientation(car.physics.rotation)
car_pos = Vec3(car.physics.location)
car_pos_from_seg_pov = car_pos - seg_start
t = (car_pos_from_seg_pov.dot(seg) / seg.dot(seg))
car_proj_seg = seg * t
seg_car = (car_pos_from_seg_pov - car_proj_seg)
# seg_car_local = relative_location(Vec3(), car_ori, seg_car)
if 0 <= t <= 1 and not seg_car.longer_than(85):
# Collision
seg_car_u = seg_car.unit()
vel = Vec3(car.physics.velocity)
refl_vel = vel - 1.5 * vel.dot(seg_car_u) * seg_car_u
car_pos_moved = seg_start + car_proj_seg + seg_car_u * 86
script.set_game_state(
GameState(
cars={
car_index:
CarState(physics=Physics(
location=car_pos_moved.to_desired_vec(),
velocity=refl_vel.to_desired_vec()))
}))
script.particle_burst(
packet.game_info.seconds_elapsed,
seg_start + car_proj_seg + seg_car_u * 13, seg_car_u,
int(1 + abs(vel.dot(seg_car_u) / 300)**3), self.team)
hit_strength = abs(seg_car_u.dot(vel))
script.sounds.car_hit(hit_strength)
def test_orientation_from_negative_angle(self):
orientation = Orientation.from_angle(-pi / 2.0, pi / 4.0)
self.assertEqual(orientation.angleDirection, 3 * pi / 2)
self.assertEqual(orientation.angleAperture, pi / 4.0)
#!/usr/bin/python # coding:UTF-8 # Author: David # Email: [email protected] # Created: 2016-04-08 01:27 # Last modified: 2016-04-08 06:05 # Filename: find_relative_ypr.py # Description: from orientation import Orientation import time bias_set = False ot = Orientation() start = time.time() try: while True: now = time.time() print '[%5.2f]' % (now-start), if now-start > 20 and not bias_set: ot.bias[0] = ot.get_base_ypr()[0] ot.bias[1] = ot.get_ypr()[0] bias_set = True print ot.get_base_ypr(), ot.get_ypr(), ot.get_orientation(False) except KeyboardInterrupt: ot.exit() finally: ot.exit()
import threading
from ultrasonic import Ultrasonic
from orientation import Orientation
from driver import Driver
from visual import Visual
from stream import Stream
import logging
ROBOT_NAME = 'Robot'
logging.basicConfig(level=logging.INFO, format='%(levelname)s | %(threadName)s | %(message)s')
if __name__ == "__main__":
ultrasonic = Ultrasonic()
orientation = Orientation()
driver = Driver()
visual = Visual()
streaming = Stream()
ultrasonic_thread = threading.Thread(name='Ultrasonic', target=ultrasonic.start, daemon=False)
orientation_thread = threading.Thread(name='Orientation', target=orientation.start, daemon=False)
driver_thread = threading.Thread(name='Driver', target=driver.start, daemon=False)
streaming_thread = threading.Thread(name='Streaming', target=streaming.start, daemon=False)
visual_thread = threading.Thread(name='Visual', target=visual.start, daemon=False)
ultrasonic_thread.start()
orientation_thread.start()
streaming_thread.start()
visual_thread.start()
driver_thread.start()
#!/usr/bin/python # coding:UTF-8 # Author: David # Email: [email protected] # Created: 2016-04-08 01:27 # Last modified: 2016-04-08 06:05 # Filename: find_relative_ypr.py # Description: from orientation import Orientation import time bias_set = False ot = Orientation() start = time.time() try: while True: now = time.time() print '[%5.2f]' % (now - start), if now - start > 20 and not bias_set: ot.bias[0] = ot.get_base_ypr()[0] ot.bias[1] = ot.get_ypr()[0] bias_set = True print ot.get_base_ypr(), ot.get_ypr(), ot.get_orientation(False) except KeyboardInterrupt: ot.exit() finally: ot.exit()
def test_orientation_from_oversized_angle(self):
orientation = Orientation.from_angle(4 * pi, 2 * pi + pi / 4.0)
self.assertEqual(orientation.angleDirection, 0.)
self.assertEqual(orientation.angleAperture, pi / 4.0)
from orientation import Orientation orientation = Orientation() orientationData = orientation.getOrientation()
#!/usr/bin/python
from orientation import Orientation
import rospy
if __name__ == '__main__':
rospy.init_node("euler_orientation_test")
orient = Orientation()
while True:
print "Raw yaw -> ", str(orient.getRawYaw())
print "Corrected yaw -> ", str(orient.getCorrectedYaw())
rospy.sleep(3)
