def test_ops(self):
p0 = almath.Position2D(0.1, 0.2)
p1 = almath.Position2D(0.3, -0.2)
pAdd = p0 + p1
pExp = almath.Position2D(p0.x + p1.x, p0.y + p1.y)
self.assertTrue(pAdd.isNear(pExp))
factor = 0.2
pFact = p0 * factor
pExp = almath.Position2D(p0.x * factor, p0.y * factor)
self.assertTrue(pFact.isNear(pExp))
def __init__(self, size, metersPerPixel, originOffest):
self.size = size
self.metersPerPixel = metersPerPixel
# Metric coordinates of the (0, 0) pixel.
self.originOffset = m.Position2D(0, 0)
self.originOffset.x = originOffest.x
self.originOffset.y = originOffest.y
def publishMap(self):
if self.current_places == None:
self.logger.warning("No places loaded")
return None
# Get the map from navigation.
map = self.nav.getMetricalMap()
mpp = map[0]
size = map[1]
originOffset = m.Position2D(map[3])
data = map[4]
# Fit the size of the image
img = np.array(data, np.uint8).reshape(size, size, 1)
img = (100 - img) * 2.5
img = img.transpose((1, 0, 2)) # Do not transpose the channels.
tabletSize = 736
img = cv2.resize(img, (tabletSize, tabletSize))
mpp = size * mpp / tabletSize
size = tabletSize
#convert to color
cv_img = img.astype(np.uint8)
color_img = cv2.cvtColor(cv_img, cv2.COLOR_GRAY2RGB)
self.occMap = OccupancyMapParams(size, mpp, originOffset)
self.occMap.originOffset = originOffset
# png
flag, buff = cv2.imencode(".png", color_img)
# base 64
buff64 = base64.b64encode(buff)
full = "data:image/png;base64," + buff64
# show app
self.memory.raiseEvent(self.events["metricalMap"],
[mpp, size, map[3], full])
def __init__(self, size, mpp, originOffest):
self.size = size
self.mpp = mpp
# Metric coordinates of the (0, 0) pixel.
self.originOffset = m.Position2D(0, 0)
self.originOffset.x = originOffest.x
self.originOffset.y = originOffest.y
self.minMetersPerPixel = 0.0001
def __init__(self, ip):
self.IP = ip
self.running = True
self.occupancyMap = OccupancyMap(700, 0.01, m.Position2D(0, 0))
self.robotPose = m.Pose2D(0, 0, 0)
self.robotRadius = 0.3 # meters
self.nodeNumber = 0
self.map = []
self.path = []
self.localizationResult = []
self.downSelectedPositionWorld = m.Position2D()
self.currentTargetRobotPose2D = m.Pose2D(200, 200, 0)
self.downScrollPosition = m.Position2D()
self.lastUpdateScrollPosition = m.Position2D(10000000, 10000000)
#init
self.connectionToRobot()
self.initPygame()
def publishLabels(self):
if self.current_places == None:
self.logger.warning("No places loaded")
return None
place_list = []
for place in self.current_places["places"]:
current_place = self.current_places["places"][place]
pos = self.occMap.getPixelFromPosition(
m.Position2D(current_place[0], current_place[1]))
place_list.append([[pos.x, pos.y], place])
self.memory.raiseEvent(self.events["places"], place_list)
def getRightClickPositionInMap(self, e):
RIGHTBUTTON = 3
sendCommand = False
# Find the new position2D of the robot.
if e.type == pygame.MOUSEBUTTONDOWN and e.button == RIGHTBUTTON:
self.downSelectedPositionWorld = self.occupancyMap.getPosition2DFromPixel(
m.Position2D(e.pos[0], e.pos[1]))
if self.downSelectedPositionWorld.distance(
m.position2DFromPose2D(
self.currentTargetRobotPose2D)) > 0.1:
self.currentTargetRobotPose2D = m.pose2DFromPosition2D(
self.downSelectedPositionWorld)
sendCommand = True
return sendCommand
def drawMap(self):
if len(self.metricalMap) != 5:
return
mpp = self.metricalMap[0]
size = self.metricalMap[1]
originOffset = m.Position2D(self.metricalMap[3])
data = self.metricalMap[4]
# Show the metrical self.metricalMap.
img = np.array(data, np.uint8).reshape(size, size, 1)
img = (100 - img) * 2.5
exploMap = OccupancyMap(size, mpp, originOffset)
for i in range(0, size):
for j in range(0, size):
pix = img[i][j][0]
if pix < 200.0:
pixColor = [pix, pix, pix]
pi = m.Position2D(i, j)
pW = exploMap.getPosition2DFromPixel(pi)
point = self.occupancyMap.getPixelFromPosition2D(pW)
r = int(0.05 / self.occupancyMap.mpp)
pygame.draw.circle(
self.screen, pixColor,
[int(point.x), int(point.y)], r)
def drawLocalizationResult(self):
if len(self.localizationResult) != 2:
return
incert = m.Pose2D(self.localizationResult[1])
ellipseRadius = math.sqrt(incert.x * incert.x + incert.y * incert.y)
r = int(ellipseRadius / self.occupancyMap.mpp)
# Do not draw if radius circle is less than 3 pixels.
if r > 3:
pose = m.Pose2D(self.localizationResult[0])
p = self.occupancyMap.getPixelFromPosition2D(
m.Position2D(pose.x, pose.y)).toVector()
pInts = (int(p[0]), int(p[1]))
zoneColor = [100, 100, 200]
pygame.draw.circle(self.screen, zoneColor, pInts, r, 3)
def clipFootStepToAvoidCollision(footMove, isLeftSupport):
""" Clip the foot move to avoid collision with the other foot.
footMove is an almath.Pose2D (x, y, theta position).
isLeftSupport must be set to True if the move is on the right leg
(the robot is supporting itself on the left leg).
"""
# Bounding boxes of NAO's feet.
rFootBox = [almath.Position2D(0.11, 0.038), # rFootBoxFL
almath.Position2D(0.11, -0.050), # rFootBoxFR
almath.Position2D(-0.047, -0.050), # rFootBoxRR
almath.Position2D(-0.047, 0.038)] # rFootBoxRL
lFootBox = [almath.Position2D(0.11, 0.050), # lFootBoxFL
almath.Position2D(0.11, -0.038), # lFootBoxFR
almath.Position2D(-0.047, -0.038), # lFootBoxRR
almath.Position2D(-0.047, 0.050)] # lFootBoxRL
# Use ALMath method.
almath.avoidFootCollision(lFootBox, rFootBox, isLeftSupport, footMove)
def test_distance(self):
p0 = almath.Position2D()
self.assertAlmostEqual(p0.distance(p0), 0.0)
p1 = almath.Position2D(0.2, 0.0)
self.assertAlmostEqual(p0.distance(p1), p1.x)
def test_init(self):
p = almath.Position2D()
self.assertAlmostEqual(p.x, 0.0)
self.assertAlmostEqual(p.y, 0.0)
def getPositionFromPixel(self, pixel):
return m.Position2D(
pixel.x * self.metersPerPixel + self.originOffset.x,
-pixel.y * self.metersPerPixel + self.originOffset.y)
## Copyright (c) 2012 Aldebaran Robotics. All rights reserved. ## Use of this source code is governed by a BSD-style license that can be ## found in the COPYING file. import almath as m print 'Check print of almath struct' print 'Check Pose2D' pose2D = m.Pose2D(0.0, 1.3, 2.9) print pose2D.__repr__() print pose2D print '' print 'Check Position2D' position2D = m.Position2D(0.1, 0.2) print position2D.__repr__() print position2D print '' print 'Check Position3D' position3D = m.Position3D(0.1, 0.2, 0.3) print position3D.__repr__() print position3D print '' print 'Check Position6D' position6D = m.Position6D(0.1, 0.2, 0.3, 0.4, 0.5, 0.6) print position6D.__repr__() print position6D print ''
def getDeltaPositionFromDeltaPixel(self, pixel):
return m.Position2D(-pixel.y * self.mpp, -pixel.x * self.mpp)
def getPixelFromPosition2D(self, position):
return m.Position2D((position.x - self.originOffset.x) / self.mpp,
(self.originOffset.y - position.y) / self.mpp)
def getPosition2DFromPixel(self, pixel):
return m.Position2D(pixel.x * self.mpp + self.originOffset.x,
-pixel.y * self.mpp + self.originOffset.y)
def getPixelFromPosition(self, position):
return m.Position2D(
(position.x - self.originOffset.x) / self.metersPerPixel,
(self.originOffset.y - position.y) / self.metersPerPixel)
def center(self):
center = m.Position2D(self.occupancyMap.size / 2,
self.occupancyMap.size / 2)
self.occupancyMap.scroll(
m.position2DFromPose2D(self.robotPose) -
self.occupancyMap.getPosition2DFromPixel(center))