def getRobotPos(self, curr_pos, init=False): self.robot_abs = curr_pos if init: self.robot_init = self.robot_abs self.robot_rel = vecDiff(self.robot_abs, self.robot_init) # displacement from start position (center of map) xpix = float2int(curr_pos[0]*self.mm2pix) # robot wrt map center (mO) + mO wrt matrix origin (xO) = robot wrt xO [pix] ypix = self.mapSize_pix-float2int(curr_pos[1]*self.mm2pix) # y is negative because pixels increase downwards (+y_mm = -y_pix = -np rows) self.robot_pix = (xpix, ypix, self.robot_rel[2]) self.trajectory.append(self.robot_pix)
def drawPointMap(self, points):
if self.USE_POINT_MAP:
for (dist, ang) in (point for point in points if point[0] > 0): # only draw valid points
# pixel location of scan point # point wrt robot + robot wrt x0 = point wrt x0
x_pix = float2int( self.mapCenter_pix + ( self.robot_rel[0] + dist*np.sin(radians(ang+self.robot_rel[2])) )*self.mm2pix )
y_pix = float2int( self.mapCenter_pix - ( self.robot_rel[1] + dist*np.cos(radians(ang+self.robot_rel[2])) )*self.mm2pix )
try:
self.pointMap[y_pix, x_pix] -= self.mapIncr*(self.pointMap[y_pix, x_pix] > self.mapMin) # decrement value at location if above minimum value
except IndexError:
pass # scan out of bounds
def getRobotPos(self, curr_pos, init=False):
self.robot_abs = curr_pos
if init: self.robot_init = self.robot_abs
self.robot_rel = vecDiff(
self.robot_abs, self.robot_init
) # displacement from start position (center of map)
xpix = float2int(
curr_pos[0] * self.mm2pix
) # robot wrt map center (mO) + mO wrt matrix origin (xO) = robot wrt xO [pix]
ypix = self.mapSize_pix - float2int(
curr_pos[1] * self.mm2pix
) # y is negative because pixels increase downwards (+y_mm = -y_pix = -np rows)
self.robot_pix = (xpix, ypix, self.robot_rel[2])
self.trajectory.append(self.robot_pix)
def drawPointMap(self, points):
if self.USE_POINT_MAP:
for (dist, ang) in (point for point in points
if point[0] > 0): # only draw valid points
# pixel location of scan point # point wrt robot + robot wrt x0 = point wrt x0
x_pix = float2int(self.mapCenter_pix +
(self.robot_rel[0] + dist *
np.sin(radians(ang + self.robot_rel[2]))) *
self.mm2pix)
y_pix = float2int(self.mapCenter_pix -
(self.robot_rel[1] + dist *
np.cos(radians(ang + self.robot_rel[2]))) *
self.mm2pix)
try:
self.pointMap[y_pix, x_pix] -= self.mapIncr * (
self.pointMap[y_pix, x_pix] > self.mapMin
) # decrement value at location if above minimum value
except IndexError:
pass # scan out of bounds
