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