def rotate(self, angle):
#rotate point anticlockwise about origin
x = self.x
y = self.y
self.x = x * deg.cos(angle) - y * deg.sin(angle)
self.y = x * deg.sin(angle) + y * deg.cos(angle)
return self
def findInfoMarker(marker):
global camera_distance
#angle between normal to marker and line between marker and robot
ang = marker.orientation.rot_y - marker.centre.polar.rot_y
#coordinates of marker
mp = markers.markerCoordinate(marker)
#angle of marker (measured CCW from positive x-axis)
ma = markers.markerAngle(marker)
d = marker.dist * 1000 #convert to mm
#position of robot relative to marker
p = Position(d * deg.cos(ang), d * deg.sin(ang))
#robot angle, robot position
orient = wtf2(marker.centre.polar.rot_y, marker.orientation.rot_y)
ra = (orient + ma + 180) % 360
rp = mp + p.rotate(ma)
"""
cd = camera_distance
rp -= Position(cd * deg.cos(ra),cd * deg.sin(ra))
"""
rp = wtf(rp, markers.markerSide(marker))
if marker.info.code == 6:
print("Or {0} ma {1} ra {2}".format(marker.orientation.rot_y, ma, ra))
#print("Pos: {0} Ang: {2} Code: {1}".format(toSimCoords(rp),marker.info.code,ra))
return [rp, ra]
def perpDist(start, end, p):
a1 = deg.atan(safeDiv(end.y - start.y, end.x - start.x))
a2 = deg.atan(safeDiv(p.y - start.y, p.x - start.x))
a = a1 - a2
d = p.dist(start)
opp = d * deg.sin(a)
return math.fabs(opp)
def getPre(curr, pos, ang):
global position_distance, preposition_distance, collision_distance
poss = []
d = position_distance + preposition_distance
for a in (0, 90, 180, 270):
x = d * deg.sin(ang + a)
y = d * deg.cos(ang + a)
p = Position(x, y) + pos
#if arena.A.ptClear(p,collision_distance):
poss.append(p)
if curr is None:
cp = position.getPosition()
if cp is None:
curr = position.translateToZone(Position(0, 0))
else:
curr = cp[0]
poss.sort(key=lambda pt: pt.dist(curr))
p = poss[0]
if p.x < 100:
p.x = 100
if p.y < 100:
p.y = 100
if p.x > 5650:
p.x = 5650
if p.y > 5650:
p.y = 5650
return p
def __init__(self,*args,**kwargs):
if 'code' in kwargs:
self.code,self.color = kwargs['code'],kwargs['color']
self.p,self.a = kwargs['p'],kwargs['a']
self.x,self.y,self.ts = self.p.x,self.p.y,R.time()
return
elif len(args) >= 4:
marker = args[0]
rx = args[1]
ry = args[2]
ra = args[3]
else:
raise TypeError
#ra is the robot's angle, measured anticlockwise from x-axis
#ra - marker.rot_y is the angle between the x-axis and the
#line between the cube and the robot
#ra - marker.orientation.rot_y is the anticlockwise angle
#between the negative x-axis and the normal to the marker
#self.a is the anticlockwise angle between the marker's normal
#and the positive x-axis
#print("\n\n--------------------")
d = marker.dist * 1000
orient = position.wtf2(marker.rot_y,marker.orientation.rot_y)
#print(f"Marker distance {d} orientation {marker.orientation.rot_y}")
self.a = (ra - orient + 180) % 360
#print(f"Marker angle {self.a}")
adiff = ra - marker.rot_y
#print(f"Adiff {adiff}")
self.x = rx + deg.sin(adiff) * d
self.y = ry + deg.cos(adiff) * d
p = conversions.toSimCoords(Position(self.x,self.y))
#print("Before correction: {0} {1}".format(p,self.a))
#apply corrections to x and y due to size of cube
self.x -= deg.sin(self.a) * 180
self.y -= deg.cos(self.a) * 180
self.__updateP()
#print("After correction:",conversions.toSimCoords(self.p))
self.color = marker.info.marker_type
self.code = marker.info.code
self.ts = R.time()