def setUp(self): og = OccupancyGrid() og.info.resolution = 0.5 og.info.height = 20 og.info.width = 20 og.info.origin.position.x = 0.0 og.info.origin.position.y = 0.0 og.info.origin.position.y = 0.0 og.info.origin.orientation.z = 0.0 og.info.origin.orientation.w = 0.0 og.info.origin.orientation.x = 0.0 og.info.origin.orientation.y = 0.0 mapArray = np.array([0] * 400) mapArray.shape = (20, 20) for xi in range(20): for yi in range(20): if xi < 3 or xi > 15: mapArray[xi, yi] = 100 if yi < 3 or yi > 15: mapArray[xi, yi] = 100 if yi < 10 and xi > 10: mapArray[xi, yi] = 75 mapArray.shape = 400 og.data = list(mapArray) self.mapData = MapData(og)
def atlaas_grid(filepath, var_threshold=0.1, stamp=None, frame_id="/map"): """ Usage: atlaas.merge("atlaas.*x*.tif", "out.tif") if os.path.isfile("out.tif"): atlaas_grid("out.tif") """ g = gdal2(filepath) bnp = g.bands[g.names["N_POINTS"]] bma = g.bands[g.names["Z_MAX"]] bme = g.bands[g.names["Z_MEAN"]] bva = g.bands[g.names["VARIANCE"]] data = numpy.where(bva > var_threshold, bma, bme) delta = data.max() - data.min() ddis = (100.0 * (data - data.min()) / delta).astype("uint8") ddis[bnp < 1] = 0 # no points, unknown og = OccupancyGrid() og.data = ddis.flatten() if stamp: og.header.stamp = stamp og.header.frame_id = frame_id og.info.resolution = g.scale_x og.info.height, og.info.width = data.shape og.info.origin.position.x, og.info.origin.position.y = g.u2c(g.utm_x, g.utm_y) og.info.origin.orientation.x = 1 # flip to transform UTM-ROS (scale_y < 0) return og
def viz_data(grid, closed): # Visualize DATA global pub global grid_res, grid_transform occ_msg = OccupancyGrid() occ_msg.info.resolution = grid_res occ_msg.info.width = len(grid.nodelist) occ_msg.info.height = len(grid.nodelist[0]) occ_msg.header.frame_id = "map" occ_msg.info.origin.position.x = grid_transform[0, 3] occ_msg.info.origin.position.y = grid_transform[1, 3] quaternion = tf.transformations.quaternion_from_matrix(grid_transform) occ_msg.info.origin.orientation.x = quaternion[0] occ_msg.info.origin.orientation.y = quaternion[1] occ_msg.info.origin.orientation.z = quaternion[2] occ_msg.info.origin.orientation.w = quaternion[3] occ_data = [0] * (occ_msg.info.width * occ_msg.info.height) width = occ_msg.info.width height = occ_msg.info.height # occ_data[width * start.x + start.y] = 100 # occ_data[width * goal.x + goal.y] = 100 top_cost = 1 for node in closed: if node.g_cost > top_cost: top_cost = node.g_cost for node in closed: cost = node.g_cost / top_cost cost *= 225 if cost > 99: cost -= 228 occ_data[width * node.x + node.y] = cost occ_msg.data = occ_data pub.publish(occ_msg)
def updateCoverage(self): # Reinitialize coverage map ogm_shape = self.ogm.shape self.coverage = numpy.zeros(ogm_shape) # YOUR CODE HERE ------------------------------------------------------ # Update the coverage field using the self.robot_path veriable. # We suppose that in every pose the robot covers an area of 2m x 2m # around it # 0 is for the uncovered, 100 is for the covered # PS. Try to make it fast :) # PS2. Do not have coverage values on obstacles or unknown space! # If done correctly, the coverage will appear purple in rviz # --------------------------------------------------------------------- # Publishing coverage ogm to see it in rviz coverage_ogm = OccupancyGrid() coverage_ogm.header.frame_id = "map" coverage_ogm.info = self.ogm_info coverage_ogm.data = numpy.zeros(self.ogm_info.width * self.ogm_info.height) for i in range(0, self.ogm_info.width): for j in range(0, self.ogm_info.height): coverage_ogm.data[i + self.ogm_info.width * j] = self.coverage[i][j] self.coverage_publisher.publish(coverage_ogm)
def setUp(self): self.bg = BumpGuesser("bump_filter", 1000, (10, 10), 0.8) og = OccupancyGrid() og.info.resolution = 0.5 og.info.height = 20 og.info.width = 20 og.info.origin.position.x = 0.0 og.info.origin.position.y = 0.0 og.info.origin.orientation.z = 0.0 # 5 * 2.0**0.5 og.info.origin.orientation.w = 0.0 # 5 * 2.0**0.5 og.info.origin.orientation.x = 0.0 og.info.origin.orientation.y = 0.0 mapArray = np.array([0] * 400) mapArray.shape = (20, 20) for xi in range(20): for yi in range(20): if xi < 3 or xi > 15: mapArray[xi, yi] = 100 if yi < 3 or yi > 15: mapArray[xi, yi] = 100 if yi < 10 and xi > 10: mapArray[xi, yi] = 75 mapArray.shape = 400 og.data = list(mapArray) mapData = MapData(og) self.bg.addMap(mapData)
def map_message(self): """ Return a nav_msgs/OccupancyGrid representation of this map. """ grid_msg = OccupancyGrid() grid_msg.header.stamp = rospy.Time.now() grid_msg.header.frame_id = "map" grid_msg.info.resolution = self.occ_map.map_res grid_msg.info.width = self.occ_map.width grid_msg.info.height = self.occ_map.height grid_msg.info.origin = Pose(Point(self.occ_map.map_limit[0], self.occ_map.map_limit[2], 0), Quaternion(0, 0, 0, 1)) flat_grid = copy.deepcopy(self.map.reshape((self.occ_map.map_size,))) for i in range(self.occ_map.map_size): if flat_grid[i] > self.occ_map.prob_occ: flat_grid[i] = 100 elif flat_grid[i] < self.occ_map.prob_free: flat_grid[i] = 0 else: flat_grid[i] = -1 flat_grid = np.round(flat_grid) flat_grid = flat_grid.astype(int) grid_msg.data = list(flat_grid) return grid_msg
def read_map(data): global workingOnMap workingOnMap = True print "manually reducing map resolution" # change the map's resolution newOccGrid, newMapHeight, newMapWidth = increaseMapResolution(data) # expand the obstacles newOccGrid = expandObstacles(newOccGrid, desiredMapRes, newMapWidth, newMapHeight) # publish the new grid newPublishedGrid = OccupancyGrid() newPublishedGrid.info.resolution = desiredMapRes newPublishedGrid.info.origin.position.x= data.info.origin.position.x newPublishedGrid.info.origin.position.y= data.info.origin.position.y newPublishedGrid.info.width = newMapWidth newPublishedGrid.info.height = newMapHeight newPublishedGrid.data = tuple(newOccGrid) print "publishing map" scaledMapPub.publish(newPublishedGrid) # publish the amount that the obstacles were expanded by expansionVal = Float64() expansionVal.data = expansionWidth obstacleExpansionDimensionPub.publish(expansionVal) workingOnMap = False
def load_map(fp): ogrid = OccupancyGrid() step = None offset = None yaml_file = fp + ".yaml" csv_file = fp + ".csv" with open(yaml_file, "r") as infile: for k, v in yaml.load(infile.read()).iteritems(): if k == "Header": ogrid.header = v elif k == "MapMetaData": ogrid.info = v step = v.resolution offset = (v.origin.position.x, v.origin.position.y) elif k == "GridFile": print "GridFile: ", v else: print "Unexpected k : ", k raise TypeError("Unexpected key type in yaml file: " + yaml_file) ogrid.data = list(np.loadtxt(csv_file, dtype=np.int8, delimiter=",")) print "Map loaded" return ogrid, step, offset
def handleSearchedCombinedMessage(data): global findFrontierPub #get map dimensions data width=data.info.width height=data.info.height searchedMap = data.data frontier = [0] * (width * height) for h in range(height): for w in range(width): if searchedMap[h*width+w] == -1: frontier[h*width+w]=-1 if searchedMap[h*width+w] == 100: if isFrontier(searchedMap,h,w,height,width): frontier[h*width+w]=100 mapMsg=OccupancyGrid() mapMsg.header.stamp=rospy.Time.now() mapMsg.header.frame_id=data.header.frame_id mapMsg.info.resolution=data.info.resolution mapMsg.info.width=width mapMsg.info.height=height mapMsg.info.origin=data.info.origin mapMsg.data=frontier findFrontierPub.publish(mapMsg)
def to_message(self): """ Return a nav_msgs/OccupancyGrid representation of this map. """ grid_msg = OccupancyGrid() # Set up the header. grid_msg.header.stamp = rospy.Time.now() grid_msg.header.frame_id = "map" # .info is a nav_msgs/MapMetaData message. grid_msg.info.resolution = self.resolution grid_msg.info.width = self.width grid_msg.info.height = self.height # Rotated maps are not supported... quaternion represents no # rotation. grid_msg.info.origin = Pose(Point(self.origin_x, self.origin_y, 0), Quaternion(0, 0, 0, 1)) # Flatten the numpy array into a list of integers from 0-100. # This assumes that the grid entries are probalities in the # range 0-1. This code will need to be modified if the grid # entries are given a different interpretation (like # log-odds). flat_grid = self.grid.reshape((self.grid.size,)) * 100 grid_msg.data = list(np.round(flat_grid)) return grid_msg
def publish(self, publisher, real = False): costGrid = OccupancyGrid() costGrid.header.frame_id = self.frame_id costGrid.header.stamp = rospy.Time.now() costGrid.info = self.map_info temparr = copy.deepcopy(self.data) #protect ourselves from volitility if not real: # map cost_map to between 0 and 127 for fancy colors in rviz maxVal = max(temparr) minVal = float('inf') for cost in temparr: if not (cost == 0) and (cost < minVal): minVal = cost minVal *= 1.01 factor = 90.0/(maxVal - minVal) if(maxVal == minVal): return # costGrid.data = [(int((i - minVal) * factor) if (i != 0) else 0) for i in cost_map.data] costGrid.data = [] for i in temparr: if(i!=0): costGrid.data.append(int((i - minVal) * factor)) else: costGrid.data.append(0) else: costGrid.data = temparr publisher.publish(costGrid)
def pubMap(self): # Publish updated map grid_msg = OccupancyGrid() grid_msg.header.stamp = rospy.Time.now() grid_msg.header.frame_id = "map" # .info is a nav_msgs/MapMetaData message. grid_msg.info.resolution = self.resolution grid_msg.info.width = self.map_info.width grid_msg.info.height = self.map_info.height # Rotated maps are not supported... quaternion represents no # rotation. grid_msg.info.origin = Pose(Point(self.map_origin[0], self.map_origin[1], 0),Quaternion(0, 0, 0, 1)) # Flatten the numpy array into a list of integers from 0-100. # This assumes that the grid entries are probalities in the # range 0-1. This code will need to be modified if the grid # entries are given a different interpretation (like # log-odds). flat_grid = self.grid.reshape((self.grid_map.size,)) * 100 grid_msg.data = list(np.round(flat_grid)) self._map_data_pub.publish(grid_msg.info) self._map_pub.publish(grid_msg) return grid_msg
def occ_grid_cb(data): global p,ready,first rospy.loginfo("map_reduction.py:occ_grid_cb() /ready_for_wps: %s",str(ready)) if not ready.data: return nav_map = OccupancyGrid() w,h = data.info.width,data.info.height pt = data.data p = array(pt).reshape(w,h)#.T #<-- Transposes if not 'grid_loc' in globals(): return x,y = grid_loc.position.x, grid_loc.position.y nav_map.info = data.info if not 'size' in globals(): size = 64 if 'rtl' in globals() and rtl: ##Intelligently set mapsize, scale map, and send waypoints to home pass nav_map.info.width = size nav_map.info.height = size tmp_map = p[int(x)-size/2:int(x)+size/2,int(y)-size/2:int(y)+size/2] if first: tmp_map[:,:size/2] = 100 nav_map.data = tmp_map.flatten() nav_map_pub.publish(nav_map)
def processOccupancyGrid(gridMessage, scaleFactor, cacheEmptyCells): grid = OccupancyGrid(gridMessage.data, gridMessage.info.height, gridMessage.info.width, gridMessage.info.resolution, (gridMessage.info.origin.position.x, gridMessage.info.origin.position.y)) grid.scale(scaleFactor, cacheEmptyCells=cacheEmptyCells) grid.expandObstacles() return grid
def updateCombined(): global data0 global data1 global data2 global mapData global searchedCombinePub rospy.wait_for_message('robot_0/robotSearched', OccupancyGrid, timeout=None) rospy.wait_for_message('robot_1/robotSearched', OccupancyGrid, timeout=None) rospy.wait_for_message('robot_2/robotSearched', OccupancyGrid, timeout=None) mapMsg=OccupancyGrid() while not rospy.is_shutdown(): map0 = numpy.array(data0.data) map1 = numpy.array(data1.data) map2 = numpy.array(data2.data) combined = numpy.minimum(map0,map1) combined = numpy.minimum(combined,map2) mapMsg.header.stamp=rospy.Time.now() mapMsg.header.frame_id=mapData.header.frame_id mapMsg.info.resolution=mapData.info.resolution mapMsg.info.width=mapData.info.width mapMsg.info.height=mapData.info.height mapMsg.info.origin=mapData.info.origin mapMsg.data=combined.tolist() searchedCombinePub.publish(mapMsg) rospy.sleep(1.0)
def run(self): while self.is_looping(): navigation_tf_msg = TFMessage() try: motion_to_robot = almath.Pose2D(self.motion.getRobotPosition(True)) localization = self.navigation.getRobotPositionInMap() exploration_path = self.navigation.getExplorationPath() except Exception as e: navigation_tf_msg.transforms.append(self.get_navigation_tf(almath.Pose2D())) self.publishers["map_tf"].publish(navigation_tf_msg) self.rate.sleep() continue if len(localization) > 0 and len(localization[0]) == 3: navigation_to_robot = almath.Pose2D(localization[0][0], localization[0][1], localization[0][2]) navigation_to_motion = navigation_to_robot * almath.pinv(motion_to_robot) navigation_tf_msg.transforms.append(self.get_navigation_tf(navigation_to_motion)) self.publishers["map_tf"].publish(navigation_tf_msg) if len(localization) > 0: if self.publishers["uncertainty"].get_num_connections() > 0: uncertainty = Marker() uncertainty.header.frame_id = "/base_footprint" uncertainty.header.stamp = rospy.Time.now() uncertainty.type = Marker.CYLINDER uncertainty.scale = Vector3(localization[1][0], localization[1][1], 0.2) uncertainty.pose.position = Point(0, 0, 0) uncertainty.pose.orientation = self.get_ros_quaternion( almath.Quaternion_fromAngleAndAxisRotation(0, 0, 0, 1)) uncertainty.color = ColorRGBA(1, 0.5, 0.5, 0.5) self.publishers["uncertainty"].publish(uncertainty) if self.publishers["map"].get_num_connections() > 0: aggregated_map = None try: aggregated_map = self.navigation.getMetricalMap() except Exception as e: print("error " + str(e)) if aggregated_map is not None: map_marker = OccupancyGrid() map_marker.header.stamp = rospy.Time.now() map_marker.header.frame_id = "/map" map_marker.info.resolution = aggregated_map[0] map_marker.info.width = aggregated_map[1] map_marker.info.height = aggregated_map[2] map_marker.info.origin.orientation = self.get_ros_quaternion( almath.Quaternion_fromAngleAndAxisRotation(-1.57, 0, 0, 1)) map_marker.info.origin.position.x = aggregated_map[3][0] map_marker.info.origin.position.y = aggregated_map[3][1] map_marker.data = aggregated_map[4] self.publishers["map"].publish(map_marker) if self.publishers["exploration_path"].get_num_connections() > 0: path = Path() path.header.stamp = rospy.Time.now() path.header.frame_id = "/map" for node in exploration_path: current_node = PoseStamped() current_node.pose.position.x = node[0] current_node.pose.position.y = node[1] path.poses.append(current_node) self.publishers["exploration_path"].publish(path) self.rate.sleep()
def get_message(self): ogrid = OccupancyGrid() ogrid.header = navigator_tools.make_header(frame="enu") ogrid.info.resolution = self.resolution ogrid.info.height, ogrid.info.width = self.grid.shape ogrid.info.origin = self.origin grid = np.copy(self.grid) ogrid.data = np.clip(grid.flatten() - 1, -100, 100).astype(np.int8) return ogrid
def pub_grid(self, *args): grid = self.grid_drawer.img ogrid = OccupancyGrid() ogrid.header.frame_id = '/enu' ogrid.header.stamp = rospy.Time.now() ogrid.info = self.map_meta_data ogrid.data = np.subtract(np.flipud(grid).flatten(), 1).astype(np.int8).tolist() self.ogrid_pub.publish(ogrid)
def publishCombined(self): #Enter Main Loop while not rospy.is_shutdown(): #Convert to Numpy Arrays map = [] for i in range(0, self.numRobots): map.append(numpy.array(self.searchedData[i].data)) combined2 = map[0] if self.numRobots > 1: #Find Minimum of all maps for i in range(1, self.numRobots): combined2 = numpy.minimum(combined2,map[i]) #Pack Occupancy Grid Message mapMsg=OccupancyGrid() mapMsg.header.stamp=rospy.Time.now() mapMsg.header.frame_id=self.mapData.header.frame_id mapMsg.info.resolution=self.mapData.info.resolution mapMsg.info.width=self.mapData.info.width mapMsg.info.height=self.mapData.info.height mapMsg.info.origin=self.mapData.info.origin mapMsg.data=combined2.tolist() #Convert combined Occupancy grid values to grayscal image values combined2[combined2 == -1] = 150 #Unknown -1->150 (gray) combined2[combined2 == 100] = 255 #Not_Searched 100->255 (white) #Searched=0 (black) #Calculate percentage of open area searched numNotSearched = combined2[combined2==255].size numSearched = combined2[combined2==0].size percentSearched = 100*float(numSearched)/(numNotSearched+numSearched) percentSearchedMsg = Float32() percentSearchedMsg.data = percentSearched self.percentPub.publish(percentSearchedMsg) #Pack Image Message imageMsg=Image() imageMsg.header.stamp = rospy.Time.now() imageMsg.header.frame_id = self.mapData.header.frame_id imageMsg.height = self.mapData.info.height imageMsg.width = self.mapData.info.width imageMsg.encoding = 'mono8' imageMsg.is_bigendian = 0 imageMsg.step = self.mapData.info.width imageMsg.data = combined2.tolist() #Publish Combined Occupancy Grid and Image self.searchedCombinePub.publish(mapMsg) self.imagePub.publish(imageMsg) #Update Every 0.5 seconds rospy.sleep(1.0)
def convert_to_occ_grid(self): res = OccupancyGrid() res.header.stamp = rospy.Time.now() res.header.frame_id = "odom_combined" res.info.resolution = 0.1 res.info.width = 1000 res.info.height = 1000 res.info.origin.position.x = -50 res.info.origin.position.y = -50 res.data = self.grid return res
def to_message(self): """ Return a nav_msgs/OccupancyGrid representation of this map. """ grid_msg = OccupancyGrid() grid_msg.header.stamp = rospy.Time.now() grid_msg.header.frame_id = "map" grid_msg.info.resolution = self.resolution grid_msg.info.width = self.width grid_msg.info.height = self.height grid_msg.info.origin = Pose(Point(self.origin_x, self.origin_y, 0), Quaternion(0, 0, 0, 1)) grid_msg.data = self._numpy_to_data() return grid_msg
def publish_grid_map(): grid_map = OccupancyGrid() grid_map.info.map_load_time = rospy.Time.now() grid_map.info.width = slam_map.size grid_map.info.height = slam_map.size grid_map.info.resolution = slam_map.scale grid_map.info.origin.position.x = -slam_map.size / 2 * slam_map.scale grid_map.info.origin.position.y = -slam_map.size / 2 * slam_map.scale grid_map.info.origin.position.z = 0 grid_map.data = slam_map.to_ros_grid() pub.publish(grid_map)
def grilla2occupancy_grid(self): mapa_msg = OccupancyGrid() mapa_msg.header.frame_id = 'mapa' mapa_msg.info.resolution = self.resolucion mapa_msg.info.width = self.dimension_x mapa_msg.info.height = self.dimension_y mapa_msg.data = range(self.dimension_x * self.dimension_y) for i in range(len(self.grilla.flat)): mapa_msg.data[i] = self.grilla.flat[i] return mapa_msg
def updateMap(e_o_indexes, w_map): newGrid = OccupancyGrid() newGrid.info = w_map.info tmp_data = list(w_map.data) for index in e_o_indexes: if index >= 0 and index < len(w_map.data): tmp_data[index] = 100 newGrid.data = tuple(tmp_data) return newGrid
def publishListAsOccupancyGrid(list, publisher): # TODO: does this need to be a deep copy? occupancyGrid = OccupancyGrid() occupancyGrid.info.height = G_GridRows occupancyGrid.info.width = G_GridCols occupancyGrid.info.resolution = G_GridResolution occupancyGrid.info.origin.position.x = G_GridOriginX occupancyGrid.info.origin.position.y = G_GridOriginY occupancyGrid.info.origin.orientation.w = 1 occupancyGrid.header.frame_id = "map" occupancyGrid.data = tuple(list) publisher.publish(occupancyGrid)
def mapResize(newRes, mapInfo, mapData): oldRes = mapInfo.resolution oldw = mapInfo.width oldh = mapInfo.height oldMapInfo = copy.deepcopy(mapInfo) nMapData = [] nMapDataD = {} #change the size of the new map. The offsets of the new map are already copied from the old map. mapInfo.resolution = newRes mapInfo.width = int(round( (oldw*(oldRes/newRes)) )) mapInfo.height = int(round( (oldh*(oldRes/newRes)) )) #print "old map dimensions" #print oldMapInfo.width #print oldMapInfo.height #print "new map dimensions w x h" #print mapInfo.width #print mapInfo.height #populate the new map at the defined resolution with all cells at -1 #print "generating the new map" for x in range(mapInfo.width * mapInfo.height): nMapDataD[x] = 0 #print "summing new blocks" for i in range(len(mapData)): gp = gridToGlobal(indexToGrid(i, oldMapInfo), oldMapInfo) nIndex = gridToIndex(globalToGrid(gp, mapInfo), mapInfo) if nIndex >= (mapInfo.width * mapInfo.height -1): nIndex = mapInfo.width * mapInfo.height-1 nMapDataD[nIndex] = (nMapDataD[nIndex] + mapData[i]) for i in nMapDataD: value = nMapDataD[i] / ((newRes*newRes) - (oldRes * oldRes)) if value > 5: value = 100 elif value < 0: value = -1 else: value = 0 nMapData.append(numpy.int8(value)) newMapOC = OccupancyGrid() newMapOC.info = mapInfo newMapOC.data = tuple(nMapData) return newMapOC
def main(): # Inicializar el nodo de ROS y asignarle un nombre rospy.init_node('nodo_publicador') # Publicador de mensajes pub = rospy.Publisher('/mygrid', OccupancyGrid, queue_size=10) # Crear el mensaje a enviar (sin contenido) msg = OccupancyGrid() # Frecuencia con la que se envia el mensaje (en Hz) rate = rospy.Rate(10) # Contador cnt = 0 # Tamano w = 640 h = 480 # Datos del mensaje msg.header.stamp = rospy.Time.now() msg.info.width = w msg.info.height = h msg.info.resolution = 0.005 msg.info.origin.position.x = 0.0 msg.info.origin.position.y = 0.0 msg.info.origin.position.z = 0.0 msg.info.origin.orientation.x = 0.0 msg.info.origin.orientation.y = 0.0 msg.info.origin.orientation.z = 0.0 msg.info.origin.orientation.w = 1.0 msg.header.frame_id = 'mygrid' # Valores n = w * h mymap = n * [ 10., ] msg.data = mymap # Bucle principal while not rospy.is_shutdown(): # Ingresar los datos del mensaje # if (cnt%20 == 0): # mensaje.linear.x = -mensaje.linear.x # mensaje.linear.y = 0.0 # mensaje.linear.z = 0.0 # mensaje.angular.x = 0.0 # mensaje.angular.y = 0.0 # mensaje.angular.z = 0.0 # Publicar el mensaje pub.publish(msg) # Mostrar el mensaje # print(mensaje) # Incrementar el contador # cnt += 1 # Delay del bucle para que cumpla la frecuencia indicada rate.sleep()
def publish_occupancy_grid(self): grid = OccupancyGrid() grid.data = [] grid.data = self.map.ravel().astype(int).tolist() grid.header.stamp = rospy.Time.now() grid.header.frame_id = '/world' grid.info.height = np.float(self.map.shape[0]) grid.info.width = np.float(self.map.shape[1]) grid.info.resolution = self.res grid.info.origin.position.x = -self.y_shape*self.res+self.initial_pose[1].astype(np.float) grid.info.origin.position.y = -self.initial_pose[0].astype(np.float) # self.map_publisher.publish(grid)
def listToMap(listMap, header, metadata): grid = OccupancyGrid() grid.info = metadata grid.header = header grid.header.frame_id='/map' # For each row for rowNumber in range(len(listMap)): # For each column for columnNumber in range(len(listMap[rowNumber])): grid.data.append(listMap[rowNumber][columnNumber]) return grid
def publish_map(self): now = self.get_clock().now() msg = OccupancyGrid() msg.header.stamp = now.to_msg() msg.header.frame_id = 'map' msg.info.resolution = RESOLUTION msg.info.width = MAP_WIDTH msg.info.height = MAP_HEIGHT msg.info.origin.position.x = WORLD_ORIGIN_X msg.info.origin.position.y = WORLD_ORIGIN_Y msg.data = self.map self.map_publisher.publish(msg)
def atlaas8u_grid(filepath, stamp=None, frame_id='/map'): g = gdal2(filepath) b = g.bands[0] if len(g.bands.shape) > 2 else g.bands og = OccupancyGrid() og.data = (b.astype('float') / 2.55).astype('uint8').flatten() if stamp: og.header.stamp = stamp og.header.frame_id = frame_id og.info.resolution = g.scale_x og.info.height, og.info.width = b.shape og.info.origin.position.x, og.info.origin.position.y = g.u2c( g.utm_x, g.utm_y) og.info.origin.orientation.x = 1 # flip to transform UTM-ROS (scale_y < 0) return og
def publish_score(self, obs_map): SCALE = 255. / 100 grid = OccupancyGrid() grid.header.stamp = rospy.Time.now() grid.header.frame_id = 'map' grid.header.seq = self.seq grid.info = self.map_info score_array = obs_map[::-1, :, M.SCORE] / SCALE score_list = list(map(int, score_array.reshape(-1))) grid.data = score_list self.score_pub.publish(grid)
def mergemaps(self, object_map, hiddenpref_map): merged = OccupancyGrid() merged.header.frame_id = "map" merged.info = self._static_map.info for y in range(object_map.info.height): for x in range(object_map.info.width): v = (object_map.data[y * object_map.info.width + x] * hiddenpref_map.data[y * hiddenpref_map.info.width + x]) #v = v/100 merged.data.append(v) if not self.overlaps: merged.data = map(lambda c: 100 if c > 0 else 0, merged.data) self.mergemap_pub.publish(merged)
def atlaas8u_grid(filepath, stamp=None, frame_id="/map"): g = gdal2(filepath) b = g.bands[0] if len(g.bands.shape) > 2 else g.bands og = OccupancyGrid() og.data = (b.astype("float") / 2.55).astype("uint8").flatten() if stamp: og.header.stamp = stamp og.header.frame_id = frame_id og.info.resolution = g.scale_x og.info.height, og.info.width = b.shape og.info.origin.position.x, og.info.origin.position.y = g.u2c(g.utm_x, g.utm_y) og.info.origin.orientation.x = 1 # flip to transform UTM-ROS (scale_y < 0) return og
def publish_visibility_grid(self): visible_grid = OccupancyGrid() visible_grid.header.stamp = rospy.Time.now() visible_grid.header.frame_id = "map" visible_grid.info.resolution = self.height / self.division visible_grid.info.width = self.division visible_grid.info.height = self.division visible_grid.info.origin.orientation.w = 1 visible_grid.data = [ self.convert(self.is_grid_active[x][y]) for y in range(self.division) for x in range(self.division) ] self.pub_visibility_grid.publish(visible_grid)
def mapResize(newRes, mapInfo, mapData): oldRes = mapInfo.resolution oldw = mapInfo.width oldh = mapInfo.height oldMapInfo = copy.deepcopy(mapInfo) nMapData = [] nMapDataD = {} #change the size of the new map. The offsets of the new map are already copied from the old map. mapInfo.resolution = newRes mapInfo.width = int(round((oldw * (oldRes / newRes)))) mapInfo.height = int(round((oldh * (oldRes / newRes)))) #print "old map dimensions" #print oldMapInfo.width #print oldMapInfo.height #print "new map dimensions w x h" #print mapInfo.width #print mapInfo.height #populate the new map at the defined resolution with all cells at -1 #print "generating the new map" for x in range(mapInfo.width * mapInfo.height): nMapDataD[x] = 0 #print "summing new blocks" for i in range(len(mapData)): gp = gridToGlobal(indexToGrid(i, oldMapInfo), oldMapInfo) nIndex = gridToIndex(globalToGrid(gp, mapInfo), mapInfo) if nIndex >= (mapInfo.width * mapInfo.height - 1): nIndex = mapInfo.width * mapInfo.height - 1 nMapDataD[nIndex] = (nMapDataD[nIndex] + mapData[i]) for i in nMapDataD: value = nMapDataD[i] / ((newRes * newRes) - (oldRes * oldRes)) if value > 5: value = 100 else: value = 0 nMapData.append(numpy.int8(value)) newMapOC = OccupancyGrid() newMapOC.info = mapInfo newMapOC.data = tuple(nMapData) return newMapOC
def get_message(self): if self.grid is None: fprint("Ogrid was requested but no ogrid was found. Using blank.", msg_color='yellow') self.grid = np.zeros((self.height / self.resolution, self.width / self.resolution)) ogrid = OccupancyGrid() ogrid.header = navigator_tools.make_header(frame="enu") ogrid.info.resolution = self.resolution ogrid.info.height, ogrid.info.width = self.grid.shape ogrid.info.origin = self.origin grid = np.copy(self.grid) ogrid.data = np.clip(grid.flatten() - 1, -100, 100).astype(np.int8) return ogrid
def __init__(self): print 'status init' self.map = OccupancyGrid() self.sharedmap = OccupancyGrid() self.mission = StringStamped() self.missionext = StringStamped() self.robotpose = PoseWithCovarianceStamped() self.robotposeext = PoseWithCovarianceStamped() self.robotstatus = StringStamped() self.robotstatusext = StringStamped() self.cloud = PointCloud() self.cloud_ext = PointCloud() self.image = Image() self.image_ext = Image()
def publish_test_map(self, points, metadata, map_header): """ For testing purposes, publishes a map highlighting certain points. points is a list of tuples (x, y) in the map's coordinate system. """ test_map = np.zeros((metadata.height, metadata.width)) for x, y in points: test_map[y, x] = 100 test_map_msg = OccupancyGrid() test_map_msg.header = map_header test_map_msg.header.stamp = rospy.Time.now() test_map_msg.info = metadata test_map_msg.data = list(np.ravel(test_map)) self.test_map_pub.publish(test_map_msg)
def __init__(self, posx, posy, block): if posx: self.pos = Odometry() self.map_data = OccupancyGrid() self.goal = PoseStamped() self.pos.pose.pose.position.x = posx self.pos.pose.pose.position.y = posy self.map_data.info.origin.position.x = -1.0 self.map_data.info.origin.position.y = -1.0 self.map_data.info.resolution = 0.0500000007451 self.map_data.info.width = 1024 self.map_data.info.height = 640 self.goal.header.frame_id = 'map' self.goal.pose.position.x = self.pos.pose.pose.position.x self.goal.pose.position.y = self.pos.pose.pose.position.y + 2.0 self.goal.pose.orientation.z = 0.7 self.goal.pose.orientation.w = 0.7 self.yaw = pi / 2 self.block = block #self.coord = {} self.counter = -1 self.counter0 = 1 self.counter1 = 0 self.start = time.time() - 5 #self.lastx = self.pos.pose.pose.position.x #self.lasty = self.pos.pose.pose.position.y #self.lastyaw = self.yaw else: self.pos = Odometry() self.map_data = OccupancyGrid() self.goal = PoseStamped() self.pos.pose.pose.position.x = -5.0 self.pos.pose.pose.position.y = -22.0 self.map_data.info.origin.position.x = -31.4 self.map_data.info.origin.position.y = -31.4 self.map_data.info.resolution = 0.0500000007451 self.map_data.info.width = 1056 self.map_data.info.height = 800 self.goal.header.frame_id = 'map' self.goal.pose.position.x = self.pos.pose.pose.position.x self.goal.pose.position.y = self.pos.pose.pose.position.y + 2.0 self.goal.pose.orientation.z = 0.7 self.goal.pose.orientation.w = 0.7 self.yaw = pi / 2 self.block = 'X -5.0Y -22.0' #self.coord = {} self.counter = -1 self.counter0 = 1 self.counter1 = 0 self.start = time.time() - 5
def make_ogrid(self, np_arr, size, center): np_center = np.array(center) np_origin = np.append((np_center - size / 2)[:2], 0) origin = mil_tools.numpy_quat_pair_to_pose(np_origin, [0, 0, 0, 1]) ogrid = OccupancyGrid() ogrid.header = mil_tools.make_header(frame='enu') ogrid.info.origin = origin ogrid.info.width = size / self.resolution ogrid.info.height = size / self.resolution ogrid.info.resolution = self.resolution ogrid.data = np.clip(np_arr.flatten() - 1, -100, 100).astype(np.int8) return ogrid
def update_map(grid, update): xgrid = OccupancyGrid() xgrid.header.stamp = update.header.stamp xgrid.header.frame_id = update.header.frame_id xgrid.info = grid.info xgrid.data = grid.data[:] i = 0 for yi in range(update.height): for xi in range(update.width): index = (update.y+yi)*xgrid.info.width + xi + update.x xgrid.data[index] = update.data[i] i+=1 return xgrid
def get_message(self): if self.grid is None: fprint("Ogrid was requested but no ogrid was found. Using blank.", msg_color='yellow') self.grid = np.zeros((self.height / self.resolution, self.width / self.resolution)) ogrid = OccupancyGrid() ogrid.header = mil_tools.make_header(frame="enu") ogrid.info.resolution = self.resolution ogrid.info.height, ogrid.info.width = self.grid.shape ogrid.info.origin = self.origin grid = np.copy(self.grid) ogrid.data = np.clip(grid.flatten() - 1, -100, 100).astype(np.int8) return ogrid
def publish_map(self,data,map_ori): data[np.where(data==1)]=100 data[np.where(data==-1)]=0 mapg=OccupancyGrid() mapg.header.frame_id = "map" mapg.header.stamp = rospy.Time.now() mapg.info.resolution=self.map_reso mapg.info.width=len(data) mapg.info.height=len(data[0]) mapg.info.origin=self.origin mapg.info.origin.position.x=map_ori[0] mapg.info.origin.position.y=map_ori[1] mapg.data=list(data.T.reshape(mapg.info.width*mapg.info.height,1)[:,0]) #T? self.map_pub.publish(mapg)
def publish_grid(self): ''' Take the occupancy grid and send it out over ros with timestamps and whatnot. ''' t = rospy.Time.now() header = Header(stamp=t, frame_id='/map') # Populate occ grid msg occ_msg = OccupancyGrid() occ_msg.header = header occ_msg.info = self.meta_data # Make sure values don't go out of range occ_grid = self.searched + self.markers - 1 occ_msg.data = np.clip(occ_grid.flatten(), -1, 100) self.occ_grid_pub.publish(occ_msg)
def __init__(self): # CONSTANT for Control varialbe range ---------------------------------------------------# self.MAX_speed = 5.5 # [m/s] self.MIN_speed = 0 # [m/s] self.MAX_steer = 2000 / float(71) # [degree] self.MIN_steer = -2000 / float(71) # [degree] self.MAX_brake = 150 self.MIN_brake = 1 self.NO_CONTROL = 0 self.EMERGENCY_BRAKE = 1 self.NORMAL_S = 2 # Normal path following mode #----------------------------------------------------------------------------------------# # Comunication rate self.pub_rate = 1.0 # [Hz] #IMPORTANT!!! Main control rate self.access_wait_rate = 500 # [Hz] # Map varialbe self.latest_map = OccupancyGrid() # Last updated map self.cur_map = OccupancyGrid() # Curent map of car self.upsign_map = False # if updated map is different, then 1 # Hardware constant self.L_wheelbase = 1.02 # [m] distance between two wheel axis self.L_rear = 0.51 # [m] distance between cm and rear wheel # Control variable self.control = Control() # My order to the car self.control_mode = self.NO_CONTROL self.writing_state = 0 self.control_count = 0 self.control_buff = [] self.control_time_buff = [] self.control_buff_size = 600 #TODO: Tune this! self.look_ahead = 20 #TODO: Tune this! now we choose 20th point as look_ahead_point self.regular_speed = 2 #TODO[m/s]: Tune this! self.k_gain = 0.28 self.look_ahead_point = PoseStamped # Input variable self.cur_state = Control() # Current state of car self.upsign_state = False # if updated state is different, then 1 self.latest_path = Path() # Last updated path self.cur_path = Path() # Current path of car self.upsign_path = False # if updated path is different, then 1
def update_grid(self, msg): xyz_array = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(msg) #print(xyz_array) #pose = self._tf_buffer.lookup_transform(self._fixed_frame, self._sensor_frame, rospy.Time()) #ros_pcl transform_pointcloud might be more efficient than transforming eadh point for i in range(len(xyz_array)): if xyz_array[i, 2] <= HEIGHT_THRESHOLD: grid_point = self.PointToVoxel(xyz_array[i, 0], xyz_array[i, 1]) self._map += self._occupied_update g = OccupancyGrid() g.data = (self.LogOddsToProbability(self._map) * 100).astype(np.int8) self._grid_pub.publish(g)
def initiateMapMsg(self):# written by Ambjorn Waldum map_msg = OccupancyGrid() map_msg.header.frame_id = 'manta/odom' resolution = 0.1 width = 500 height = 500 map_msg.info.resolution = resolution map_msg.info.width = width map_msg.info.height = height map_msg.data = np.arange(width*height, dtype=np.int8) map_msg.info.origin.position.x = - width // 2 * resolution map_msg.info.origin.position.y = - height // 2 * resolution map_msg.header.stamp = rospy.Time.now() return map_msg
def handle_og_request(self, og_request): og = self.cleaningManager.mapManager.occupancy_grid OG = OccupancyGrid() altered_og = [] for val in og: if val == -1: altered_og.append(val) else: altered_og.append(int(val * 100 - 70)) OG.data = altered_og OG.info.width = int(math.sqrt(len(og))) OG.info.height = int(math.sqrt(len(og))) OG.info.resolution = (2 * X_MAX) / OG_WIDTH #OG.info.origin = self.__get_og_origin() zed_quat = self.cleaningManager.mapManager.mapMaker.orientation zed_euler = self.quaternion_to_euler(zed_quat[0], zed_quat[1], zed_quat[2], zed_quat[3]) o_quat = self.euler_to_quaternion(zed_euler[2], zed_euler[1], zed_euler[0] + math.radians(180)) visible_corners = self.cleaningManager.mapManager.get_visible_area_corners( self.cleaningManager.mapManager.mapMaker.translation, self.cleaningManager.mapManager.mapMaker.orientation) og_origin = Pose() og_origin.position.x = visible_corners[0][0] og_origin.position.y = visible_corners[0][1] og_origin.orientation.x = o_quat[0] og_origin.orientation.y = o_quat[1] og_origin.orientation.z = o_quat[2] og_origin.orientation.w = o_quat[3] OG.info.origin = og_origin baseBotPose = Pose() baseBotPose.position.x = self.cleaningManager.mapManager.mapMaker.translation[ 0] baseBotPose.position.y = self.cleaningManager.mapManager.mapMaker.translation[ 1] baseBotPose.position.z = self.cleaningManager.mapManager.mapMaker.translation[ 2] baseBotPose.orientation.x = self.cleaningManager.mapManager.mapMaker.orientation[ 0] baseBotPose.orientation.y = self.cleaningManager.mapManager.mapMaker.orientation[ 1] baseBotPose.orientation.z = self.cleaningManager.mapManager.mapMaker.orientation[ 2] baseBotPose.orientation.w = self.cleaningManager.mapManager.mapMaker.orientation[ 3] return [baseBotPose, OG]
def test_astar(self): global_planner = GlobalPlanner() # Info for testing a star alone nmap = numpy.array([[0, 0, 254, 254, 0, 0], [0, 0, 100, 100, 0, 0], [0, 0, 0, 0, 0, 0], [0, 0, 50, 50, 0, 0], [0, 0, 50, 50, 0, 0]]) start_coord = (0, 0) end_coord = (4, 5) # Info for testing make_plan occupancy_grid = OccupancyGrid() occupancy_grid.header.frame_id = "/map" occupancy_grid.info.resolution = 0.5 occupancy_grid.info.width = 6 occupancy_grid.info.height = 5 occupancy_grid.data = nmap.flatten() start_pose = PoseStamped() start_pose.pose.position.x = float(start_coord[0]) start_pose.pose.position.y = float(start_coord[1]) end_pose = PoseStamped() end_pose.pose.position.x = ( float(end_coord[0]) + 0.5) * occupancy_grid.info.resolution # 2.25 end_pose.pose.position.y = ( float(end_coord[1]) + 0.5) * occupancy_grid.info.resolution # 2.75 # Results and check path = global_planner.astar(nmap, start_coord, end_coord) ros_path = global_planner.make_plan(occupancy_grid, start_pose, end_pose) # global_planner.print_path(nmap, path) # Uncomment to ease debugging expected_path = [(0, 0), (1, 0), (2, 0), (3, 0), (4, 0), (4, 1), (4, 2), (4, 3), (4, 4), (4, 5)] expected_ros_path = [(0, 0), (0.75, 0.25), (1.25, 0.25), (1.75, 0.25), (2.25, 0.25), (2.25, 0.75), (2.25, 1.25), (2.25, 1.75), (2.25, 2.25), (2.25, 2.75)] self.assertEqual(path, expected_path) for pose, expected_position in zip(ros_path.poses, expected_ros_path): # print(pose) # Uncomment to ease debugging self.assertEqual(pose.pose.position.x, expected_position[0]) self.assertEqual(pose.pose.position.y, expected_position[1])
def pub_occupancy(self): occupancy = [] for row in self.obstacle_map.log_prob_map: for i in row: try: occupancy.append(math.exp(i) * 100) except OverflowError: rospy.logwarn('OverflowError: %s', i) occupancy.append(100) msg = OccupancyGrid() msg.info.resolution = self.grid_size msg.info.width = self.width msg.info.height = self.height msg.data = occupancy self.occupancy_pub.publish(msg)
def map_callback(self,msg): """ receives new map info and inflates the map """ old_map = np.array(msg.data) side_length = int(round(np.sqrt(old_map.shape[0]))) old_map = old_map.reshape((side_length, side_length)) print(old_map) new_map = grey_dilation(old_map, size=(13,13)) new_map = new_map.reshape(side_length**2).tolist() print(len(new_map)) new_msg = OccupancyGrid() new_msg.data = new_map new_msg.info = msg.info self.pub.publish(new_msg)
def initialize_occ_grid(frame_id="odom", res=1.0, width=100, height=100, origin=Pose(Point(0.0, 0.0, 0.0), Quaternion(0.0, 0.0, 0.0, 1.0))): occ_grid = OG() occ_grid.header.frame_id = frame_id occ_grid.info.resolution = res occ_grid.info.width = width occ_grid.info.height = height occ_grid.info.origin = origin occ_grid.data = occ_grid.info.width * occ_grid.info.height * [50] return occ_grid
def publish_test_map(self, points, metadata, map_header): #Broken, about to fix. """ For testing purposes, publishes a map highlighting certain points. points is a list of tuples (x, y) in the map's coordinate system. """ test_map = np.zeros((metadata.height, metadata.width),dtype = np.uint8) for x, y in points: test_map[y, x] = 100 test_map_msg = OccupancyGrid() test_map_msg.header = map_header test_map_msg.header.stamp = self.get_clock().now().to_msg() test_map_msg.info = metadata test_map_msg.data = list(np.ravel(test_map)) self.test_map_pub.publish(test_map_msg)
def configure_msg(self): if not self.mapIsParsed: data = self.parse_and_configure_map() msg = OccupancyGrid() msg.header.frame_id = self.yml['image'] msg.header.stamp = self.get_clock().now().to_msg() msg.info.map_load_time = self.get_clock().now().to_msg() msg.info.origin.position.x = self.yml['origin'][0] msg.info.origin.position.y = self.yml['origin'][1] msg.info.resolution = round(self.yml['resolution'], 4) msg.info.height = self.map_shape[0] msg.info.width = self.map_shape[1] msg.data = data.tolist() return msg
def getMapSrv(self): map_ = OccupancyGrid() map_.header.stamp = rospy.Time.now() map_.header.frame_id = 'map' map.info.resolution = self.ground_resolution map_.info.width = self.width map_.info.height = self.height map_.info.origin = Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1)) flat_map = self.map.reshape((self.self.map.size, )) map_.data = list(np.round(flat_map)) return GetMapResponse(map_)