Ejemplo n.º 1
0
    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)
Ejemplo n.º 2
0
        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)
Ejemplo n.º 3
0
    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
Ejemplo n.º 4
0
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
Ejemplo n.º 5
0
    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
Ejemplo n.º 6
0
        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)
Ejemplo n.º 7
0
    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
Ejemplo n.º 8
0
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 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
Ejemplo n.º 10
0
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)
Ejemplo n.º 11
0
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 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)
Ejemplo n.º 13
0
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)
Ejemplo n.º 14
0
 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
Ejemplo n.º 15
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()
Ejemplo n.º 16
0
def ExpandMap(occupancyGrid):

	lowerResGrid = OccupancyGrid(occupancyGrid.header, occupancyGrid.info, [])

	oldWidth = occupancyGrid.info.width

	lowerResGrid.info.resolution = .1
	lowerResGrid.info.width = int(math.floor(float(occupancyGrid.info.width)/2))
	lowerResGrid.info.height = int(math.floor(float(occupancyGrid.info.height)/2))

	width = lowerResGrid.info.width
	height = lowerResGrid.info.height

	lowerResGrid.data = [-1]*width*height

	for i in range (0, height):
		for j in range (0, width):
			if occupancyGrid.data[(j*2) + (oldWidth * i*2)] >= 1 or \
				occupancyGrid.data[(j*2) + (oldWidth * ((i*2)+1))] >= 1 or \
				occupancyGrid.data[(j*2)+1 + (oldWidth * i*2)] >= 1 or \
				occupancyGrid.data[(j*2)+1 + (oldWidth * ((i*2)+1))] >= 1:
				lowerResGrid.data[j + (width * i)] = 100
			elif occupancyGrid.data[(j*2) + (oldWidth * i*2)] == 0 and \
				occupancyGrid.data[(j*2) + (oldWidth * ((i*2)+1))] == 0 and \
				occupancyGrid.data[(j*2)+1 + (oldWidth * i*2)] == 0 and \
				occupancyGrid.data[(j*2)+1 + (oldWidth * ((i*2)+1))] == 0:
				lowerResGrid.data[j + (width * i)] = 0
			else:
				lowerResGrid.data[j + (width * i)] = -1

	expandedGrid = OccupancyGrid(lowerResGrid.header, lowerResGrid.info, lowerResGrid.data)

	expandedData = list(lowerResGrid.data)

	print "expanding"
	for i in range (0, height):
		for j in range (0, width):
			if (lowerResGrid.data[j + (width * i)] >= 1):
				for k in range (j - 2, j + 3):
					for l in range (i - 2, i + 3):
						if (k > 0 and k < width and l > 0 and l < height):
							expandedData[k + (width * l)] = 100

	expandedGrid.data = tuple(expandedData)
	print "expanded"
	return expandedGrid, lowerResGrid
Ejemplo n.º 17
0
def image2ism(imgConfidence, imgClass, header):

    if useTfForExtrinsic:
        headFrame = header.frame_id
        headFrame = str.join(
            '/',
            [strPart for strPart in headFrame.split('/') if strPart is not ''
             ])  # Complicated wat to remove the first '/' if it exists.

        trans = tfBuffer.lookup_transform(targetFrameId, headFrame,
                                          rospy.Time())
        ipm.update_extrinsic_from_tf_transform(trans)

    if ipm.isExtrinsicUpdated == False:
        ipm.update_extrinsic(cam_roll, cam_pitch, cam_yaw, pCamera)

    for objectType in pubOutputTopics.keys():
        classNumber = outputTopicsNumber[objectType]
        bwClass = imgClass == classNumber
        imgConfidenceClass = np.zeros_like(imgConfidence)
        imgConfidenceClass[bwClass] = imgConfidence[bwClass]

        resample_input
        if resample_input == 1.0:
            cv_image = imgConfidenceClass
        else:
            cv_image = cv2.resize(imgConfidenceClass,
                                  None,
                                  fx=resample_input,
                                  fy=resample_input,
                                  interpolation=cv2.INTER_NEAREST)

        imDimIn = cv_image.shape[0:2]
        if ipm.isHomographyUpdated == False:
            pRayStarts, pDst, rHorizon, rHorizonTrue, pSrc, pDstOut = ipm.update_homography(
                imDimIn)

        # A grid is only published when the homography is updated.
        if ipm.isHomographyUpdated == True:
            grid = ipm.makePerspectiveMapping(cv_image, match2Grid=match2Grid)

            grid_msg = OccupancyGrid()
            grid_msg.header = header
            #grid_msg.header.stamp = rospy.Time.now()
            grid_msg.header.frame_id = targetFrameId
            grid_msg.info.resolution = grid_resolution
            grid_msg.info.width = grid.shape[1]
            grid_msg.info.height = grid.shape[0]
            origin_x = ipm.distToMapping
            origin_y = -float(grid.shape[0]) * grid_resolution / 2
            origin_z = 0.0
            grid_msg.info.origin = Pose(Point(origin_x, origin_y, origin_z),
                                        Quaternion(0, 0, 0, 1))
            grid_msg.data = np.flipud(grid).flatten()

            pubOutputTopics[objectType].publish(grid_msg)
Ejemplo n.º 18
0
    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)
Ejemplo n.º 19
0
    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)
Ejemplo n.º 20
0
    def get_message(self):
        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(), -100, 100).astype(np.int8)

        return ogrid
Ejemplo n.º 21
0
def publishMap(worldmap, topicName, publisher, x, y, rot):
    msg = OccupancyGrid()
    msg.header.stamp = rospy.Time.now()
    msg.header.frame_id = topicName
    msg.info.resolution = 0.05
    msg.info.width = worldmap.shape[0]
    msg.info.height = worldmap.shape[1]
    msg.info.origin.orientation = Quaternion(0, 0, 0, 1)
    msg.info.origin.position.x = -x * msg.info.resolution
    msg.info.origin.position.y = -y * msg.info.resolution
    msg.data = 100 / (1 + np.exp(-worldmap))
    msg.data[worldmap == 0] = -1
    msg.data = msg.data.T.astype(np.int8).ravel()
    publisher.publish(msg)
    br = tf.TransformBroadcaster()
    br.sendTransform(
        (0, 0, 0),
        tf.transformations.quaternion_from_euler(0, 0, -rot - np.pi / 2),
        rospy.Time.now(), topicName, "base_link")
Ejemplo n.º 22
0
    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
Ejemplo n.º 23
0
 def get_exploration_occupancy_grid(self):
     msg = OccupancyGrid()
     msg.header = make_header("/map")
     msg.info = self.map_info
     buff = self.exploration_buffer.astype(np.int8)
     buff[buff == 0] = 100
     buff[buff == 1] = 50
     msg.data = buff.reshape(self.map_info.height *
                             self.map_info.width).tolist()
     return msg
Ejemplo n.º 24
0
    def publish_change_grid(self):
	change_grid = OccupancyGrid()
	change_grid.header.stamp = rospy.Time.now()
	change_grid.header.frame_id = "map"
	change_grid.info.resolution = self.height/self.division
	change_grid.info.width = self.division
	change_grid.info.height = self.division
	change_grid.info.origin.orientation.w = 1
	change_grid.data = [(self.change_detected[x][y])*25 for y in range(self.division) for x in range(self.division)]
	self.pub_change_grid.publish(change_grid)
Ejemplo n.º 25
0
    def pub_grid(self, *args):
        grid = self.grid_drawer.img

        ogrid = OccupancyGrid()
        ogrid.header.frame_id = '/world'
        ogrid.info = self.map_meta_data
        ogrid.data = np.subtract(np.flipud(grid).flatten(),
                                 1).astype(np.int8).tolist()

        self.ogrid_pub.publish(ogrid)
Ejemplo n.º 26
0
def mapMask(map1):
    map2 = OccupancyGrid()
    map2.header = map1.header
    map2.info = map1.info
    global offs
    offs = map1.info.width
    map2.info.origin.position.x = 0
    map2.info.origin.position.y = 0
    map2.data = [0 if x == -1 else x for x in map1.data]
    pubM.publish(map2)
Ejemplo n.º 27
0
    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)
Ejemplo n.º 28
0
    def pub_ogrid(self, *args):
        ogrid = OccupancyGrid()
        ogrid.header = navigator_tools.make_header(frame="enu")
        ogrid.info = self.map_metadata

        # Over saturate and clip to get range of [-1, 1]
        self.grid = np.clip(self.grid * 1000, -1, 1)
        ogrid.data = self.grid.flatten().astype(np.int8).tolist()

        self.ogrid_pub.publish(ogrid)
Ejemplo n.º 29
0
def mappublisher(m,height, width, resolution,morigin):
    msg = OccupancyGrid()
    msg.header.frame_id='map'
    msg.info.resolution = resolution
    msg.info.width      = math.ceil(width/resolution)
    msg.info.height     = math.ceil(height/resolution)
    msg.info.origin.position.x=-morigin[0]
    msg.info.origin.position.y=-morigin[1]
    msg.data=m  
    mappub.publish(msg)
 def publish_map(self):
     g = OccupancyGrid()
     g.header.frame_id = "/cartographer_map"
     g.info.resolution = self.resolution
     g.info.width = self.width
     g.info.height = self.height
     g.info.origin = self.true_origin
     g.info.map_load_time = rospy.Time.now()
     g.data = self.MAP
     self.map_pub.publish(g)
Ejemplo n.º 31
0
	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)
Ejemplo n.º 32
0
def callback(data):
    # tempolary hardcode the parameter
    x_lim = -10
    y_lim = -10
    x_width = 20
    y_width = 20
    resol = 0.1

    map_meta = data.info
    N = int(x_width / resol) + 1
    temp = np.ones((N, N)) * 50

    dimh = MultiArrayDimension()
    dimw = dimh
    dimh.size = data.info.height
    dimh.label = 'height'
    dimh.stride = 48
    dimw.size = data.info.width
    dimw.label = 'width'
    dimw.stride = 64

    datanp = np.array(data.data, dtype=float)
    datanp = datanp
    datanp[datanp < 0] = 50
    for i in range(data.info.height):
        for j in range(data.info.width):
            idx_x = round(
                N / 2 -
                map_meta.height / 2)  #-data.info.origin.position.x/0.1)
            idx_y = round(N / 2 - -data.info.origin.position.x / 0.1)
            # temp[int(idx_x + i),int(idx_y + j)] = datanp[(i-1)*data.info.width+j]
    for i in range(80):
        for j in range(60):
            temp[int(N / 2 - 40 + i), int(N / 2 - 30 + j)] = 0.1

    map = OccupancyGrid()
    map.data = temp.flatten()
    map.info.width = int(N)
    map.info.height = int(N)
    map.info.resolution = 0.1
    map.info.origin = map_meta.origin
    # data.data
    pub = rospy.Publisher('/TheMap', Float64MultiArray, queue_size=10)
    pub_map = rospy.Publisher('/map_fixed', OccupancyGrid, queue_size=10)
    a = Float64MultiArray()
    a.layout.dim.append(dimh)
    a.layout.dim.append(dimw)
    a.layout.data_offset = 0
    a.data = temp.flatten() / 100.
    pub_map.publish(map)
    pub.publish(a)
    br = tf.TransformBroadcaster()
    br.sendTransform((-map_meta.origin.position.x - 10,
                      -map_meta.origin.position.y - 10, 0.0),
                     (0.0, 0.0, 0.0, 1.0), rospy.Time.now(), "map", "world")
Ejemplo n.º 33
0
 def publish_map(self):
     (h, w) = self.explore_map.shape
     map_msg = OccupancyGrid()
     map_msg.info.resolution = UNIT
     map_msg.info.width = w
     map_msg.info.height = h
     map_msg.info.origin.position.x = -self.x0
     map_msg.info.origin.position.y = -self.y0
     map_msg.data = (1 + 97 * self.explore_map.reshape(
         (h * w, ))).astype(np.int8)
     self.pub_map.publish(map_msg)
Ejemplo n.º 34
0
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)
Ejemplo n.º 35
0
def numpyToOcc(grid, res):
	msg = OccupancyGrid()
	msg.header.frame_id = "map"
	msg.data = grid.ravel()
	msg.info = MapMetaData()
	msg.info.height = grid.shape[0]
	msg.info.width = grid.shape[1]
	msg.info.resolution = res
	msg.info.origin = Pose(Point(0, 0, 0), Quaternion(0,0,0,1))

	return msg
Ejemplo n.º 36
0
    def publish_crowd_risk_model(self, crowd_risk_model):
	risk_grid = OccupancyGrid()
	risk_grid.header.stamp = rospy.Time.now()
	risk_grid.header.frame_id = "map"
	risk_grid.info.resolution = self.height/self.division
	risk_grid.info.width = self.division
	risk_grid.info.height = self.division
	risk_grid.info.origin.orientation.w = 1
	risk_list = [crowd_risk_model[x][y] for x in range(self.division) for y in range(self.division)]
	risk_grid.data = self.normalize(risk_list, 0, 100)  
	self.pub_risk_grid.publish(risk_grid)
Ejemplo n.º 37
0
def numpyToOcc(grid, origMsg):
	msg = OccupancyGrid()
	msg.header.frame_id = "map"
	msg.data = grid.ravel()
	msg.info = MapMetaData()
	msg.info.height = grid.shape[0]
	msg.info.width = grid.shape[1]
	msg.info.resolution = 0.05
	msg.info.origin = origMsg.info.origin

	return msg
Ejemplo n.º 38
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
Ejemplo n.º 39
0
    def numpyToOcc(self, grid):
        msg = OccupancyGrid()
        msg.header.frame_id = "base_link"
        msg.data = grid.ravel()
        msg.info = MapMetaData()
        msg.info.height = self.h
        msg.info.width = self.w
        msg.info.resolution = self.res
        msg.info.origin = Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1))

        return msg
Ejemplo n.º 40
0
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)
Ejemplo n.º 41
0
 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
Ejemplo n.º 42
0
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
Ejemplo n.º 43
0
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)
Ejemplo n.º 44
0
    def to_message(self):
        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))

        flat_grid = self.grid.reshape((self.grid.size,)) * 100
        grid_msg.data = list(np.round(flat_grid))
        return grid_msg
Ejemplo n.º 45
0
def create_test_occupancy_grid():
    my_test_occupancy_grid = OccupancyGrid()

    # test map meta data
    my_test_occupancy_grid.info.width = 3
    my_test_occupancy_grid.info.height = 3
    my_test_occupancy_grid.info.resolution = 1.0

    # test map obstacles
    my_test_occupancy_grid.data = [0, 100, 0, 0, 100, 0, 0, 0, 0]

    plot_all_the_trajectories(my_test_occupancy_grid)
Ejemplo n.º 46
0
def restrict(res, data):
    d = len(data)
    d = int(round(sqrt(d)))
    msg = OccupancyGrid()
    msg.header.frame_id = "map"
    msg.info.resolution = res
    msg.info.width = d
    msg.info.height = d
    msg.info.origin.position.x = -12.825
    msg.info.origin.position.y = -12.825
    msg.data = data
    restPub.publish(msg)
Ejemplo n.º 47
0
def create_test_occupancy_grid():
    my_test_occupancy_grid = OccupancyGrid()

    # test map meta data
    my_test_occupancy_grid.info.width = 3
    my_test_occupancy_grid.info.height = 3
    my_test_occupancy_grid.info.resolution = 1.0

    # test map obstacles
    my_test_occupancy_grid.data = [0, 100, 0, 0, 100, 0, 0, 0, 0]

    do_stuff_with_map(my_test_occupancy_grid)
Ejemplo n.º 48
0
	def create_map(self):
		test_map = OccupancyGrid()
		#Default config
		test_map.info.resolution = .1	#m/cell
		test_map.info.width = 200		#cells
		test_map.info.height = 200		#cells
		test_map.data = []
		#Initialize
		p0 = self.calculateLogOddsProbability(self._l0)*100
		for i in xrange(0, test_map.info.width*test_map.info.height):
			test_map.data.append( p0 )
		return test_map
Ejemplo n.º 49
0
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
Ejemplo n.º 50
0
 def ConvertMap(self, map_msg):
     #Convert an OccupancyGrid map message into the internal representation.  This allocates a map_t and returns it.
     map = OccupancyGrid()
     map.data = list(copy.deepcopy(map_msg.data))
     for i in range(map_msg.info.width * map_msg.info.height):
         if map_msg.data[i] == 0:
             map.data[i] = -1
         elif map_msg.data[i] == 100:
             map.data[i] = 1
         else:
             map.data[i] = 0
     return map
    def run(self):
        """
        Starts ros loop
        """

        while not rospy.is_shutdown():
            if self.costmap is not None:
                costmap_msgs = OccupancyGrid()
                costmap_msgs.info = self.metadata
                costmap_msgs.data = np.ravel(self.costmap).tolist()
                self.pub.publish(costmap_msgs)
            self.rate.sleep()
Ejemplo n.º 52
0
    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
Ejemplo n.º 53
0
 def genDummy(self):
     g = OccupancyGrid()
     g.info.resolution = 0.05
     g.info.width = 256
     g.info.height = 128
     tiles = []
     for x in range(0, 512):
         for y in range(0, 512):
             tiles.append(Int8(round(np.random.random(1)[0] * 0.51) * 100))
     g.data = tiles
     print("generated occupancy grid")
     return g
 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
Ejemplo n.º 55
0
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
Ejemplo n.º 56
0
    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
Ejemplo n.º 57
0
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
Ejemplo n.º 58
0
    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
Ejemplo n.º 59
0
 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)