Python OccupancyGrid.info方法代码示例

# 需要导入模块: from nav_msgs.msg import OccupancyGrid [as 别名]
# 或者: from nav_msgs.msg.OccupancyGrid import info [as 别名]
    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)

# 需要导入模块: from nav_msgs.msg import OccupancyGrid [as 别名]
# 或者: from nav_msgs.msg.OccupancyGrid import info [as 别名]
    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

# 需要导入模块: from nav_msgs.msg import OccupancyGrid [as 别名]
# 或者: from nav_msgs.msg.OccupancyGrid import info [as 别名]
    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)

# 需要导入模块: from nav_msgs.msg import OccupancyGrid [as 别名]
# 或者: from nav_msgs.msg.OccupancyGrid import info [as 别名]
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)

# 需要导入模块: from nav_msgs.msg import OccupancyGrid [as 别名]
# 或者: from nav_msgs.msg.OccupancyGrid import info [as 别名]
    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)

# 需要导入模块: from nav_msgs.msg import OccupancyGrid [as 别名]
# 或者: from nav_msgs.msg.OccupancyGrid import info [as 别名]
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

# 需要导入模块: from nav_msgs.msg import OccupancyGrid [as 别名]
# 或者: from nav_msgs.msg.OccupancyGrid import info [as 别名]
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

# 需要导入模块: from nav_msgs.msg import OccupancyGrid [as 别名]
# 或者: from nav_msgs.msg.OccupancyGrid import info [as 别名]
 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)

# 需要导入模块: from nav_msgs.msg import OccupancyGrid [as 别名]
# 或者: from nav_msgs.msg.OccupancyGrid import info [as 别名]
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

# 需要导入模块: from nav_msgs.msg import OccupancyGrid [as 别名]
# 或者: from nav_msgs.msg.OccupancyGrid import info [as 别名]
def numpy_to_occupancy_grid(arr, info=None):
	if not len(arr.shape) == 2:
		raise TypeError('Array must be 2D')
	if not arr.dtype == np.int8:
		raise TypeError('Array must be of int8s')

	grid = OccupancyGrid()
	if isinstance(arr, np.ma.MaskedArray):
		# We assume that the masked value are already -1, for speed
		arr = arr.data
	grid.data = arr.ravel()
	grid.info = info or MapMetaData()
	grid.info.height = arr.shape[0]
	grid.info.width = arr.shape[1]

	return grid

# 需要导入模块: from nav_msgs.msg import OccupancyGrid [as 别名]
# 或者: from nav_msgs.msg.OccupancyGrid import info [as 别名]
def publishMap():
    meta = MapMetaData()
    meta.width = 10 
    meta.height = 10
    meta.resolution = 0.2
    for c in range(-1,100):
        map = OccupancyGrid()
        map.header.frame_id = 'map'
        for i in range(0,100):
            val = c + i
            if val > 100:
                val = val - 101
            map.data.append(val)
        map.info = meta
        mapPub.publish(map)
        publishGrid()

# 需要导入模块: from nav_msgs.msg import OccupancyGrid [as 别名]
# 或者: from nav_msgs.msg.OccupancyGrid import info [as 别名]
def mapCallback(msg):
	print "Got map"
	global expanded_map_pub
	expansion_factor = int(math.ceil(rospy.get_param('expand_by', 1)/msg.info.resolution))
	print expansion_factor
	height = msg.info.height
	width = msg.info.width
	new_map = OccupancyGrid()
	new_map.info = msg.info
	new_map.data = [0 for x in range(width*height)]
	for cell in range(0, width*height):
		new_map.data[cell] = msg.data[cell]
		for n in getNeighbors(msg, cell, expansion_factor):
			if n > 50:
				new_map.data[cell] = 100
				break
	print 'Publishing new map'
	expanded_map_pub.publish(new_map)

# 需要导入模块: from nav_msgs.msg import OccupancyGrid [as 别名]
# 或者: from nav_msgs.msg.OccupancyGrid import info [as 别名]
def publishMapArray(msg):
    global mapMetaData, header
    msg[msg==5]=50
    msg[msg==256]=0
    processedMap = OccupancyGrid()
    data =[]
    for i in range(len(msg)):
        #processedMap.data.append(msg[i])    
        for j in range(len(msg[i])):
            data.append(int(msg[i][j]))
    processedMap.info = mapMetaData
    processedMap.header = header
    processedMap.data=data
    #processedMap = OccupancyGrid(header,mapMetaData,data)
    pub = rospy.Publisher('ProcessedMap', OccupancyGrid, queue_size = 10)
    emptyMsg = OccupancyGrid()
    pub.publish(processedMap)
    print "publishing..."
    pub.publish(processedMap)
    msg[msg==50]=5
    msg[msg==0]=256

# 需要导入模块: from nav_msgs.msg import OccupancyGrid [as 别名]
# 或者: from nav_msgs.msg.OccupancyGrid import info [as 别名]
def publishMapArray(msg):
    global mapMetaData, header, oldMap
    msg[msg==5]=50
    msg[msg==256]=0
    processedMap = OccupancyGrid()
    data =[]
    for i in range(len(msg)):
        #processedMap.data.append(msg[i])    
        for j in range(len(msg[i])):
            data.append(int(msg[i][j]))
    processedMap.info = mapMetaData
    processedMap.header = header
    processedMap.data=data
    pub = rospy.Publisher('ProcessedMap', OccupancyGrid, queue_size = 10)
    rate = rospy.Rate(10) # 10hz
    while (not rospy.is_shutdown() and oldMap):
        str_log = "message sent at: %s" % rospy.get_time()
        rospy.loginfo(hello_str)
        pub.publish(processedMap)
        rate.sleep()
    print "publishing..."
    pub.publish(processedMap)
    msg[msg==50]=5
    msg[msg==0]=256

# 需要导入模块: from nav_msgs.msg import OccupancyGrid [as 别名]
# 或者: from nav_msgs.msg.OccupancyGrid import info [as 别名]
    pose_sub = rospy.Subscriber('current_pose', PoseStamped , poseCallback)

    generate_vel_yaw_profile()

    static_map = get_static_map()
    print static_map.info

    r = rospy.Rate(100)
    tStart = rospy.Time.now().to_sec()
    while not rospy.is_shutdown():
        time_passed = rospy.Time.now().to_sec() - tStart

        obs_pv_arrays = PoseVelArray()
        ma = MarkerArray()
        obs_map = OccupancyGrid()
        obs_map.info = static_map.info
        obs_map.header.frame_id = 'map'
        for i in range(obs_map.info.height*obs_map.info.width):
            obs_map.data.append(0)

        for i in range(len(obstacles_list)):
            m = Marker()

            m.header.frame_id='map'
            m.scale.x=1;
            m.scale.y=1;
            m.scale.z=0.1;
            m.type = Marker.SPHERE
            m.color.a = 1.0

            m.id = i