Python OccupancyGrid.header方法代碼示例

# 需要導入模塊: from nav_msgs.msg import OccupancyGrid [as 別名]
# 或者: from nav_msgs.msg.OccupancyGrid import header [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 header [as 別名]
    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

# 需要導入模塊: from nav_msgs.msg import OccupancyGrid [as 別名]
# 或者: from nav_msgs.msg.OccupancyGrid import header [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 header [as 別名]
    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

# 需要導入模塊: from nav_msgs.msg import OccupancyGrid [as 別名]
# 或者: from nav_msgs.msg.OccupancyGrid import header [as 別名]
    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

# 需要導入模塊: from nav_msgs.msg import OccupancyGrid [as 別名]
# 或者: from nav_msgs.msg.OccupancyGrid import header [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 header [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