laser_assemble.py 文件源码

def create_image_from_cloud(self, points):
        size = 100, 100, 1
        img = numpy.zeros(size, numpy.uint8)

        #tf = self.zarj_os.transform.lookup_transform('world', 'head', rospy.get_rostime())
        print points[0]
        #print self.zarj_os.transform.transform_from_world(points[0])
        for p in points:
            #tp = self.zarj_os.transform.transform_from_world(p)
            ipx = int((p.x * 100.0) + 50.0)
            ipy = int((p.y * 100.0) + 50.0)
            if ipx >= 0 and ipx < 100 and ipy >= 0 and ipy < 100:
                ipz = int(p.z * 100.0)
                if ipz > 255:
                    ipz = 255
                img[ipx, ipy] = ipz
        return img