예제 #1
0
def xyz_point_cloud():
    msg = PointCloud2()
    msg.is_dense = True
    msg.is_bigendian = True
    # Unordered point cloud
    msg.height = 1
    msg.point_step = 12  # 32bit float x, y, and z

    x_field = PointField()
    x_field.name = 'x'
    x_field.offset = 0
    x_field.datatype = PointField.FLOAT32
    x_field.count = 1

    y_field = PointField()
    y_field.name = 'y'
    y_field.offset = 4
    y_field.datatype = PointField.FLOAT32
    y_field.count = 1

    z_field = PointField()
    z_field.name = 'z'
    z_field.offset = 8
    z_field.datatype = PointField.FLOAT32
    z_field.count = 1

    msg.fields.extend((x_field, y_field, z_field))

    return msg
예제 #2
0
def pointCloud2Show():
	rospy.init_node('point_cloud_publisher',anonymous = True)
	pub = rospy.Publisher('/cloud',PointCloud2,queue_size = 50)
	#rospy.init_node('point_cloud_publisher',anonymous = True)
	rate = rospy.Rate(10)
	clouds_old = csv.reader(open('/home/cheney/LearnLog/ROS/hello_rospy/src/beginner_tutorials/scripts/laser.csv','r'))
	clouds = []
	for cloud in clouds_old:
		cloud = map(float,cloud)
		clouds.append(cloud)
	num_points = len(clouds)

	while not rospy.is_shutdown():

		fields = []
		x = PointField()
		x.datatype = 7
		x.name = 'x'
		x.count = 1
		fields.append(x)

		y = PointField()
		y.datatype = 7
		y.name = 'y'
		y.count = 1
		fields.append(y)

		z = PointField()
		z.datatype = 7
		z.name = 'z'
		z.count = 1
		fields.append(z)

		inte = PointField()
		inte.datatype = 7
		inte.name = 'intensity'
		inte.count = 1
		fields.append(inte)

		points = []
		for i in range(num_points):
			alpha = (clouds[i][0]/180.0) * math.pi
			lamda = (clouds[i][1]/180.0) * math.pi
			distance = clouds[i][2]
			intensity = clouds[i][3]
			#not for sure if coordinate is right
			zz = distance * math.sin(lamda)
			yy = distance * math.cos(lamda) * math.cos(alpha)
			xx = distance * math.cos(lamda) * math.cos(alpha)
			points.append([xx,yy,zz,intensity])
			

		header = Header()
		header.stamp = rospy.Time.now()
		header.frame_id = 'sensor_frame'
		cloud2 = point_cloud2.create_cloud(header , fields , points)
		pub.publish(cloud2)
		rate.sleep()
		print 'done'
예제 #3
0
def msg_construct(data):
    print"decode"
    list = data.split(',',20)
    Cloud = PointCloud2()
    Cloud.header.seq = int(list[1])#uint32
    Cloud.header.frame_id = str(list[0])
    Cloud.header.stamp = rospy.Time.now()
    Cloud.height = int(list[2])#uint32
    Cloud.width = int(list[3])#uint32

    Fieldx = PointField()
    Fieldx.name = "x"
    Fieldx.offset = int(list[4])#uint32
    Fieldx.datatype = int(list[5])
    Fieldx.count = int(list[6])
    Cloud.fields.append(Fieldx)

    Fieldy = PointField()
    Fieldy.name = "y"
    Fieldy.offset = int(list[7])#uint32
    Fieldy.datatype = int(list[8])
    Fieldy.count = int(list[9])
    Cloud.fields.append(Fieldy)

    Fieldz= PointField()
    Fieldz.name = "z"
    Fieldz.offset = int(list[10])#uint32
    Fieldz.datatype = int(list[11])
    Fieldz.count = int(list[12])
    Cloud.fields.append(Fieldz)

    Fieldi = PointField()
    Fieldi.name = "intensity"
    Fieldi.offset = int(list[13])#uint32
    Fieldi.datatype = int(list[14])
    Fieldi.count = int(list[15])
    Cloud.fields.append(Fieldi)

    if list[16]=="false":
        Cloud.is_bigendian = bool(0)
    else:
        Cloud.is_bigendian = bool(1)
    Cloud.point_step = int(list[17])#uint32
    Cloud.row_step = int(list[18])#uint32
    if list[19]=="false":
        Cloud.is_dense = bool(0)
    else:
        Cloud.is_dense = bool(1)

    count = Cloud.height*Cloud.row_step

    data_ = list[20]
    for i in range(0,count):
        Cloud.data+=data_[i]
    #print str(Cloud)
    print "save points from seq:",Cloud.header.seq
    return Cloud
예제 #4
0
    def make_empty_cloud(self, frame_id, max_range, rotate_points):
        irange = range(-314, 315)
        jrange = range(-314, 315)
        msg = PointCloud2()
        msg.header.frame_id = frame_id
        msg.height = 1
        if rotate_points:
            msg.width = len(irange) * len(jrange)
        else:
            msg.width = len(irange)

        msg_fieldx = PointField()
        msg_fieldy = PointField()
        msg_fieldz = PointField()
        msg_fieldx.name = 'x'
        msg_fieldx.offset = 0
        msg_fieldx.datatype = 7
        msg_fieldx.count = 1
        msg_fieldy.name = 'y'
        msg_fieldy.offset = 4
        msg_fieldy.datatype = 7
        msg_fieldy.count = 1
        msg_fieldz.name = 'z'
        msg_fieldz.offset = 8
        msg_fieldz.datatype = 7
        msg_fieldz.count = 1

        msg.fields = [msg_fieldx, msg_fieldy, msg_fieldz]
        msg.point_step = 16
        msg.row_step = msg.point_step * msg.width
        msg.is_dense = True

        points = []
        for i in irange:
            x = max_range * math.cos(i * 0.005)
            y = max_range * math.sin(i * 0.005)
            if rotate_points:
                for j in jrange:
                    # x rotation
                    #            x = max_range * math.cos(i*0.005)
                    #            y = max_range * math.sin(i*0.005) * math.cos(j * 0.005)
                    #            z = -1 * max_range * math.sin(i*0.005) * math.sin(j * 0.005)
                    # y rotation
                    points.append(
                        pack('<ffff', x * math.cos(j * 0.005), y,
                             -1 * x * math.sin(j * 0.005), 0.0))
            else:
                points.append(pack('<ffff', x, y, 0.0, 0.0))

        msg.data = ''.join(points)
        return msg
예제 #5
0
def create_point_cloud_msg(points):
    point_cloud_msg = PointCloud2()
    point_cloud_msg.header.frame_id = 'depth_sensor'
    point_cloud_msg.height = 1
    # point_cloud_msg.width = len(points)
    point_cloud_msg.width = points.shape[0]
    point_cloud_msg.fields = []
    for i, name in enumerate(['x', 'y', 'z']):
        field = PointField()
        field.name = name
        field.offset = i * struct.calcsize('f')
        field.datatype = 7
        field.count = 1
        point_cloud_msg.fields.append(field)
    point_cloud_msg.is_bigendian = sys.byteorder == 'little'
    point_cloud_msg.point_step = 3 * struct.calcsize('f')
    point_cloud_msg.row_step = point_cloud_msg.point_step * point_cloud_msg.width
    point_cloud_msg.is_dense = True
    points_flat = points.flatten()
    # points_flat = np.empty(len(points) * 3, dtype=np.float)
    # for i in xrange(len(points)):
    #     points_flat[3 * i + 0] = float(points[i][0])
    #     points_flat[3 * i + 1] = float(points[i][1])
    #     points_flat[3 * i + 2] = float(points[i][2])
    #     # print('{} {} {}'.format(*[points[i][j] for j in xrange(3)]))
    # # for point in points:
    # #     points_flat.append(float(point[0]))
    # #     points_flat.append(float(point[1]))
    # #     points_flat.append(float(point[2]))
    point_cloud_msg.data = struct.pack("={}f".format(len(points_flat)),
                                       *points_flat)
    return point_cloud_msg
 def _create_point_cloud_msg(self,
                             point_cloud,
                             frame_id='depth_sensor',
                             timestamp=None):
     if timestamp is None:
         timestamp = rospy.Time.now()
     point_cloud_msg = PointCloud2()
     point_cloud_msg.header.stamp = timestamp
     point_cloud_msg.header.frame_id = frame_id
     point_cloud_msg.height = 1
     # point_cloud_msg.width = len(points)
     point_cloud_msg.width = point_cloud.shape[0]
     point_cloud_msg.fields = []
     for i, name in enumerate(['x', 'y', 'z']):
         field = PointField()
         field.name = name
         field.offset = i * struct.calcsize('f')
         field.datatype = PointField.FLOAT32
         field.count = 1
         point_cloud_msg.fields.append(field)
     point_cloud_msg.is_bigendian = sys.byteorder == 'little'
     point_cloud_msg.point_step = 3 * struct.calcsize('f')
     point_cloud_msg.row_step = point_cloud_msg.point_step * point_cloud_msg.width
     point_cloud_msg.is_dense = True
     points_flat = point_cloud.flatten()
     point_cloud_msg.data = struct.pack("={}f".format(len(points_flat)),
                                        *points_flat)
     return point_cloud_msg
예제 #7
0
def point_cloud2_numpy_to_ros(point_cloud, frame_id=None, timestamp=None):
    """Convert xyz numpy array to ROS PointCloud2 message"""
    assert (isinstance(point_cloud, np.ndarray))
    point_cloud = np.asarray(point_cloud, np.float32)
    point_cloud_msg = PointCloud2()
    if timestamp is None:
        timestamp = rospy.Time.now()
    point_cloud_msg.header.stamp = timestamp
    if frame_id is not None:
        point_cloud_msg.header.frame_id = frame_id
    point_cloud_msg.height = 1
    point_cloud_msg.width = point_cloud.shape[0]
    point_cloud_msg.fields = []
    offset = 0
    for i, name in enumerate(['x', 'y', 'z']):
        field = PointField()
        field.name = name
        field.offset = offset
        offset += np.float32().itemsize
        field.datatype = PointField.FLOAT32
        field.count = 1
        point_cloud_msg.fields.append(field)
    point_cloud_msg.is_bigendian = sys.byteorder == 'little'
    point_cloud_msg.point_step = offset
    point_cloud_msg.row_step = point_cloud_msg.point_step * point_cloud_msg.width
    point_cloud_msg.is_dense = True
    points_flat = point_cloud.flatten()
    point_cloud_msg.data = struct.pack("={}f".format(len(points_flat)),
                                       *points_flat)
    return point_cloud_msg
def xyzs2pc2(xyzs,stamp,seq,frame_id):
    pc2Data = PointCloud2()
    if len(xyzs) != 0:
        if type(xyzs[0][0]) == np.float64:
            xyzs = np.array(xyzs,dtype=np.float32)
        
        pc2Data.is_dense = False
        pc2Data.is_bigendian = False
        pc2Data.height = 1
        pc2Data.width = len(xyzs)
        pc2Data.header.frame_id = frame_id
        pc2Data.header.seq = seq
        pc2Data.header.stamp = stamp;
        for i in range(3):
            pc2Field = PointField()
            if i == 0:
                pc2Field.name = 'x'
            elif i == 1:
                pc2Field.name = 'y'
            elif i == 2:
                pc2Field.name = 'z'
            else:
                pc2Field.name = 'unknown'
            pc2Field.count = 1
            #if type(xyzs[0][0]) == np.float64:
            #    pc2Field.datatype = 8 # FLOAT64
            #    dataLen = 8 # FLOAT64
            if type(xyzs[0][0]) == np.float32:
                pc2Field.datatype = 7 # FLOAT32
                dataLen = 4 # FLOAT32
            else:
                assert False, "unknown datatype"
            
            pc2Field.offset = i*dataLen
            pc2Data.fields.append(pc2Field)
        pc2Data.point_step = i*dataLen + dataLen;
        pc2Data.row_step = pc2Data.point_step * pc2Data.width;
        
        data_buf = xyzs.flatten().tostring()
        
        assert len(data_buf)/float(dataLen)/3.0 == len(xyzs), "len(data_buf)/4.0/3.0 = "+np.str(len(data_buf)/4.0/3.0)+" but len(xyzs) = "+np.str(len(xyzs))
        pc2Data.data = data_buf
    return pc2Data
예제 #9
0
파일: numpy_pc2.py 프로젝트: kasertim/pypcd
def arr_to_fields(cloud_arr):
    '''Convert a numpy record datatype into a list of PointFields.
    '''
    fields = []
    for field_name in cloud_arr.dtype.names:
        np_field_type, field_offset = cloud_arr.dtype.fields[field_name]
        pf = PointField()
        pf.name = field_name
        pf.datatype = nptype_to_pftype[np_field_type]
        pf.offset = field_offset
        pf.count = 1 # is this ever more than one?
        fields.append(pf)
    return fields
예제 #10
0
def setup_pc2msg():
    msg = PointCloud2()
    
    f1 = PointField()
    f2 = PointField()
    f3 = PointField()
    f4 = PointField()
    
    #msg.header.frame_id = "usb_cam"
    
    msg.height = 1
    #msg.width = 3
    msg.point_step = 20
    #msg.row_step = 30
    
    f1.name = "x"
    f1.offset = 0
    f1.datatype = 7
    f1.count = 1
    
    f2.name = "y"
    f2.offset = 4
    f2.datatype = 7
    f2.count = 1
    
    f3.name = "z"
    f3.offset = 8
    f3.datatype = 7
    f3.count = 1
     
    f4.name = "rgb"
    f4.offset = 16
    f4.datatype = 7
    f4.count = 1
    
    msg.fields = [f1, f2, f3, f4]
    msg.is_dense = False
    
    return msg
예제 #11
0
def arr_to_fields(cloud_arr):
    '''Convert a numpy record datatype into a list of PointFields.
    '''
    fields = []
    for field_name in cloud_arr.dtype.names:
        np_field_type, field_offset = cloud_arr.dtype.fields[field_name]
        pf = PointField()
        pf.name = field_name
        pf.datatype = nptype_to_pftype[np_field_type]
        pf.offset = field_offset
        pf.count = 1  # is this ever more than one?
        fields.append(pf)
    return fields
예제 #12
0
def make_xyz_float32_fields():
    fields = []
    names = ['x', 'y', 'z']
    offset = 0
    for name in names:
        field = PointField()
        field.name = name
        field.offset = offset
        field.datatype = 7
        field.count = 1
        fields.append(field)
        # add offset
        offset += 4
    return fields
예제 #13
0
def arr_to_fields(cloud_arr):
    '''!
    Конвертирует список элементов numpy в список PointField 
    @param cloud_arr: список элементов numpy
    @return: возвращает список типа PointField
    '''
    fields = []
    for field_name in cloud_arr.dtype.names:
        np_field_type, field_offset = cloud_arr.dtype.fields[field_name]
        pf = PointField()
        pf.name = field_name
        pf.datatype = nptype_to_pftype[np_field_type]
        pf.offset = field_offset
        pf.count = 1 # is this ever more than one?
        fields.append(pf)
    return fields
def talker():
    pub = rospy.Publisher('chatter', PointCloud2, queue_size=10)
    rospy.init_node('talker', anonymous=True)
    rate = rospy.Rate(100000) # 10hz - go through the loop 10 times in a second 
    
    # PointCloud2
    msg_to_publish = PointCloud2()

    # PointField
    point_field = PointField()

    # generate data 
    data = []
    for i in range (256):
        data.append(i)

    counter = 1
    while not rospy.is_shutdown():  
        str_to_publish = "Message %d"% (counter)

        point_field.name = str_to_publish
        point_field.offset = 0
        point_field.datatype = 7
        point_field.count = 1

        # Header
        msg_to_publish.header.stamp = rospy.get_rostime()
        msg_to_publish.header.seq = counter

        msg_to_publish.height = 1
        msg_to_publish.width = 1
        msg_to_publish.fields = [point_field]
        msg_to_publish.is_bigendian = True
        msg_to_publish.point_step = 32
        msg_to_publish.row_step = 32

        # Data
        msg_to_publish.data = data
        msg_to_publish.is_dense = False

        rospy.loginfo(msg_to_publish)
        pub.publish(msg_to_publish)

        counter+=1
        if counter == 101:
            break
        rate.sleep()
예제 #15
0
 def point_cloud_2(cls, msg, obj):
     obj.header = decode.header(msg, obj.header, "")
     obj.height = msg["height"]
     obj.width = msg["width"]
     for i in range(msg['_length']):
         pf = PointField()
         pf.name = msg['%s_name' % i]
         pf.offset = msg['%s_offset' % i]
         pf.datatype = msg['%s_datatype' % i]
         pf.count = msg['%s_count' % i]
         obj.fields.append(pf)
     obj.is_bigedian = msg['is_bigedian']
     obj.point_step = msg['point_step']
     obj.row_step = msg['row_step']
     obj.data = msg['data']
     obj.is_dense = msg['is_dense']
     return(obj)
예제 #16
0
def dtype_to_fields(dtype):
    '''Convert a numpy record datatype into a list of PointFields.
    '''
    fields = []
    for field_name in dtype.names:
        np_field_type, field_offset = dtype.fields[field_name]
        pf = PointField()
        pf.name = field_name
        if np_field_type.subdtype:
            item_dtype, shape = np_field_type.subdtype
            pf.count = int(np.prod(shape))
            np_field_type = item_dtype
        else:
            pf.count = 1

        pf.datatype = nptype_to_pftype[np_field_type]
        pf.offset = field_offset
        fields.append(pf)
    return fields
예제 #17
0
def dtype_to_fields(dtype):
    '''Convert a numpy record datatype into a list of PointFields.
    '''
    fields = []
    for field_name in dtype.names:
        np_field_type, field_offset = dtype.fields[field_name]
        pf = PointField()
        pf.name = field_name
        if np_field_type.subdtype:
            item_dtype, shape = np_field_type.subdtype
            pf.count = np.prod(shape)
            np_field_type = item_dtype
        else:
            pf.count = 1

        pf.datatype = nptype_to_pftype[np_field_type]
        pf.offset = field_offset
        fields.append(pf)
    return fields
예제 #18
0
def combineAndPublish():
    global changes_pc2, map_pc2, view_pc2, new_stuff
    if changes_pc2 is not None and map_pc2 is not None and view_pc2 is not None:
        # Visualization priority:
        # 1. Changes
        # 2. Viewed areas
        # 3. Map

        # Changes
        cc = list(pc2.read_points(changes_pc2, field_names=("x", "y", "z")))
        cc = [[k[0][0],k[0][1],k[0][2],k[1]] for k in list(zip(cc, [changes_colour for x in cc]))]

        # Viewed area
        vc = list(pc2.read_points(view_pc2, field_names=("x", "y", "z", "rgb")))
        #  vc = [k for k in vc if k not in cc]

        # Map
        mc = list(pc2.read_points(map_pc2, field_names=("x", "y", "z")))
        #  mc = [[k[0][0],k[0][1],k[0][2],k[1]] for k in list(zip(mc, [map_colour for x in mc])) if k not in cc and k not in map(lambda x:(x[0],x[1],x[2]),vc)]
        mc = [[k[0][0],k[0][1],k[0][2],k[1]] for k in list(zip(mc, [map_colour for x in mc]))] 

        # NOTE
        # Filtering duplicate points is too time-consuming to do
        # When (if) size becomes a problem maybe it's worth investigating
        # (Changing the order of merging keeps indirectly applies the colour priority)
        z = mc+vc+cc

        rgbfield = PointField()
        rgbfield.name = "rgb"
        rgbfield.offset = 16
        rgbfield.datatype = 7
        rgbfield.count = 1
        fields = view_pc2.fields

        cpub.publish(pc2.create_cloud(changes_pc2.header, fields, z))
        new_stuff = False
예제 #19
0
파일: io.py 프로젝트: cburbridge/python_pcd
def read_pcd(filename, cloud_header=None, get_tf=True):
    if not os.path.isfile(filename):
        raise Exception("[read_pcd] File does not exist.")
    string_array =  lambda x: x.split()
    float_array  =  lambda x: [float(j) for j in x.split()]
    int_array  =  lambda x: [int(j) for j in x.split()]
    word =  lambda x: x.strip()
    headers =  [("VERSION", float),
               ("FIELDS", string_array),
               ("SIZE", int_array),
               ("TYPE", string_array),
               ("COUNT", int_array),
               ("WIDTH", int),
               ("HEIGHT", int),
               ("VIEWPOINT", float_array),
               ("POINTS", int),
               ("DATA", word)]
    header = {}
    with open(filename, "r") as pcdfile:
        while len(headers) > 0:
            line = pcdfile.readline()
            if line == "":
                raise Exception("[read_pcd] EOF reached while looking for headers.")
            f, v = line.split(" ", 1)
            if f.startswith("#"):
                continue
            if f not in zip(*headers)[0]:
                raise Exception("[read_pcd] Field '{}' not known or duplicate.".format(f))
            func =  headers[zip(*headers)[0].index(f)][1]
            header[f] = func(v)
            headers.remove((f, func))
        data =  pcdfile.read()
    # Check the number of points
    if header["VERSION"] != 0.7:
        raise Exception("[read_pcd] only PCD version 0.7 is understood.")
    if header["DATA"] != "binary":
        raise Exception("[read_pcd] Only binary .pcd files are readable.")
    if header["WIDTH"] * header["HEIGHT"] != header["POINTS"]:
        raise Exception("[read_pcd] POINTS count does not equal WIDTH*HEIGHT")
    
    cloud = PointCloud2()
    cloud.point_step = sum([size * count
                            for size, count in zip(header["SIZE"], header["COUNT"])])
    cloud.height = header["HEIGHT"]
    cloud.width = header["WIDTH"]
    cloud.row_step = cloud.width * cloud.point_step
    cloud.is_bigendian = False
    if cloud.row_step * cloud.height > len(data):
        raise Exception("[read_pcd] Data size mismatch.")
    offset = 0
    for field, size, type, count in zip(header["FIELDS"],
                                        header["SIZE"],
                                        header["TYPE"],
                                        header["COUNT"]):
        
        if field != "_":
            pass
        pf =  PointField()
        pf.count = count
        pf.offset = offset
        pf.name = field
        pf.datatype = size_type_to_datatype(size, type)
        cloud.fields.append(pf)
        offset += size * count
        
    cloud.data = data[0:cloud.row_step * cloud.height]
    if cloud_header is not None:
        cloud.header = header
    else:
        cloud.header.frame_id = "/pcd_cloud"
        
    if get_tf:
        tf = Transform()
        tf.translation.x, tf.translation.y, tf.translation.z =  header["VIEWPOINT"][0:3]
        tf.rotation.w, tf.rotation.x, tf.rotation.y, tf.rotation.z =  header["VIEWPOINT"][3:]
        
        return cloud, tf
    
    return cloud
예제 #20
0
def main(args):
    #prepare the listener
    global socket
    socket.connect("tcp://" + ip + ":" + port)
    socket.setsockopt_string(zmq.SUBSCRIBE, topic)

    #setup some opencv windows
    cv2.namedWindow("Listener Left Camera")
    cv2.namedWindow("Listener Right Camera")

    #Start the main loop
    while True:

        message_in = socket.recv(10 * 1024)
        #For some reason I have to make a deep copy of the message. Otherwise, when, the second time I received the message I got a truncated message error. This solves it so I did not worry about it anymore.
        message = copy.deepcopy(message_in)

        #Deserialization or unmarshalling
        #We use message[4:] because we know the first four bytes are
        #"777 " and we only want to give the data to the parser (not the topic name
        sd.ParseFromString(message[4:])

        #Getting the left image
        cv_left_image = np.zeros(
            (sd.left_image.height, sd.left_image.width, 3), np.uint8)
        cv_left_image.data = sd.left_image.data

        #Getting the right image
        cv_right_image = np.zeros(
            (sd.right_image.height, sd.right_image.width, 3), np.uint8)
        cv_right_image.data = sd.right_image.data

        #Getting the point cloud
        point_cloud_msg = PointCloud2()
        point_cloud_msg.height = sd.point_cloud.height
        point_cloud_msg.width = sd.point_cloud.width

        for field in sd.point_cloud.fields:
            point_field = PointField()
            point_field.name = field.name
            point_field.offset = field.offset
            point_field.datatype = field.datatype
            point_field.count = field.count

            point_cloud_msg.fields.append(point_field)

        point_cloud_msg.is_bigendian = sd.point_cloud.is_bigendian
        point_cloud_msg.is_bigendian = sd.point_cloud.is_bigendian
        point_cloud_msg.point_step = sd.point_cloud.point_step
        point_cloud_msg.row_step = sd.point_cloud.row_step
        point_cloud_msg.data = sd.point_cloud.data

        #Getting the NavSatFix
        nav_sat_fix = NavSatFix()
        nav_sat_fix.latitude = sd.nav_sat_fix.latitude
        nav_sat_fix.longitude = sd.nav_sat_fix.longitude
        nav_sat_fix.altitude = sd.nav_sat_fix.altitude
        nav_sat_fix.status.status = sd.nav_sat_fix.status.status
        nav_sat_fix.status.service = sd.nav_sat_fix.status.service

        #Getting the Odometry
        odometry = Odometry()
        #and then we can copy every field ... no need in this example

        #---------------------------------
        #Visualizing the received data
        #---------------------------------

        print("Received new message with stamp:\n" + str(sd.header.stamp))

        print(
            "First 10 points x,y,z and rgb (packed in float32) values (just for debug)"
        )
        count = 0
        for p in pc2.read_points(point_cloud_msg,
                                 field_names=("x", "y", "z", "rgb"),
                                 skip_nans=True):
            print " x : %f  y: %f  z: %f rgb: %f" % (p[0], p[1], p[2], p[3])
            count = count + 1
            if count > 10:
                break

        print("GPS data:")
        print("Latitude =" + str(nav_sat_fix.latitude))
        print("Longitude =" + str(nav_sat_fix.longitude))
        print("Altitude =" + str(nav_sat_fix.altitude))
        print("Status =" + str(nav_sat_fix.status.status))
        print("Service =" + str(nav_sat_fix.status.service))

        print("Odometry data (only position for example):")
        print("odom.pose.pose.position.x =" +
              str(sd.odometry.pose.pose.position.x))
        print("odom.pose.pose.position.y =" +
              str(sd.odometry.pose.pose.position.y))
        print("odom.pose.pose.position.z =" +
              str(sd.odometry.pose.pose.position.z))

        cv2.imshow("Listener Left Camera", cv_left_image)
        cv2.imshow("Listener Right Camera", cv_right_image)
        cv2.waitKey(30)
예제 #21
0
파일: dataset_io.py 프로젝트: mfkiwl/atom
def read_pcd(filename, cloud_header=None, get_tf=True):
    if not os.path.isfile(filename):
        raise Exception("[read_pcd] File does not exist.")
    string_array = lambda x: x.split()
    float_array = lambda x: [float(j) for j in x.split()]
    int_array = lambda x: [int(j) for j in x.split()]
    word = lambda x: x.strip()
    headers = [("VERSION", float),
               ("FIELDS", string_array), ("SIZE", int_array),
               ("TYPE", string_array), ("COUNT", int_array), ("WIDTH", int),
               ("HEIGHT", int), ("VIEWPOINT", float_array), ("POINTS", int),
               ("DATA", word)]
    header = {}
    with open(filename, "r") as pcdfile:
        while len(headers) > 0:
            line = pcdfile.readline()
            if line == "":
                raise Exception(
                    "[read_pcd] EOF reached while looking for headers.")
            f, v = line.split(" ", 1)
            if f.startswith("#"):
                continue
            if f not in zip(*headers)[0]:
                raise Exception(
                    "[read_pcd] Field '{}' not known or duplicate.".format(f))
            func = headers[zip(*headers)[0].index(f)][1]
            header[f] = func(v)
            headers.remove((f, func))
        data = pcdfile.read()
    # Check the number of points
    if header["VERSION"] != 0.7:
        raise Exception("[read_pcd] only PCD version 0.7 is understood.")
    if header["DATA"] != "binary":
        raise Exception("[read_pcd] Only binary .pcd files are readable.")
    if header["WIDTH"] * header["HEIGHT"] != header["POINTS"]:
        raise Exception("[read_pcd] POINTS count does not equal WIDTH*HEIGHT")

    cloud = PointCloud2()
    cloud.point_step = sum(
        [size * count for size, count in zip(header["SIZE"], header["COUNT"])])
    cloud.height = header["HEIGHT"]
    cloud.width = header["WIDTH"]
    cloud.row_step = cloud.width * cloud.point_step
    cloud.is_bigendian = False
    if cloud.row_step * cloud.height > len(data):
        raise Exception("[read_pcd] Data size mismatch.")
    offset = 0
    for field, size, type, count in zip(header["FIELDS"], header["SIZE"],
                                        header["TYPE"], header["COUNT"]):

        if field != "_":
            pf = PointField()
            pf.count = count
            pf.offset = offset
            pf.name = field
            pf.datatype = size_type_to_datatype(size, type)
            cloud.fields.append(pf)
            offset += size * count

    cloud.data = data[0:cloud.row_step * cloud.height]
    if cloud_header is not None:
        # cloud.header = header
        cloud.header = cloud_header
        # print('This is it, header is ' + str(cloud_header))
    else:
        cloud.header.frame_id = "/pcd_cloud"

    if get_tf:
        tf = Transform()
        tf.translation.x, tf.translation.y, tf.translation.z = header[
            "VIEWPOINT"][0:3]
        tf.rotation.w, tf.rotation.x, tf.rotation.y, tf.rotation.z = header[
            "VIEWPOINT"][3:]

        return cloud, tf

    return cloud
예제 #22
0
def handlePointCloud2(cloud):
    global cloud_timestamps, cloud_timestampsd, last_tc
    try:
        c = list(pc2.read_points(cloud, field_names=("x", "y", "z")))

        rgbc = [[k[0][0], k[0][1], k[0][2], k[1]]
                for k in list(zip(c, [0.0 for x in c]))]

        rgbfield = PointField()
        rgbfield.name = "rgb"
        rgbfield.offset = 16
        rgbfield.datatype = 7
        rgbfield.count = 1
        fields = cloud.fields + [rgbfield]

        cloud.header.frame_id = cloud.header.frame_id if cloud.header.frame_id[
            0] != "/" else cloud.header.frame_id[1:]

        tf = lookupTF(fov_msg.header.frame_id, cloud.header.frame_id)

        if not cl_mapping:
            if np.shape(cloud_timestamps)[0] != np.shape(c)[0]:
                if np.size(cloud_timestamps) > 0:
                    rospy.logwarn(
                        "Re-initializing pc2 viewed area due to pointcloud shape incompatibility..."
                    )
                else:
                    rospy.loginfo("Initializing pc2 viewed area...")
                cloud_timestamps = np.full(np.shape(c)[0], 0.0)
        else:
            if np.size(cloud_timestampsd) == 0:
                rospy.loginfo("Initializing pc2 viewed area...")
            cloud_timestampsd = {
                k: 0.0 if k not in cloud_timestampsd else cloud_timestampsd[k]
                for k in c
            }

        t = rospy.Time.now()
        if last_tc is not None:
            for i in range(len(rgbc)):
                if (not cl_mapping and cloud_timestamps[i] < 1) or (
                        cl_mapping and cloud_timestampsd[c[i]] < 1):
                    p = PoseStamped()
                    p.header.frame_id = fov_msg.header.frame_id
                    p.header.stamp = rospy.Time.now()
                    p.pose.position.x = rgbc[i][0]
                    p.pose.position.y = rgbc[i][1]
                    p.pose.position.z = rgbc[i][2]
                    p.pose.orientation.w = 1

                    pctf = tf2_geometry_msgs.do_transform_pose(p, tf)

                    if inFOV(pctf.pose.position.x, pctf.pose.position.y):
                        if max_time > 0:
                            dt = (t - last_tc).to_sec()
                            r = math.sqrt(
                                pctf.pose.position.x * pctf.pose.position.x +
                                pctf.pose.position.y * pctf.pose.position.y)
                            if not cl_mapping:
                                cloud_timestamps[i] = min(
                                    cloud_timestamps[i] +
                                    va_func(dt, r) / float(max_time), 1.0)
                            else:
                                cloud_timestampsd[c[i]] = min(
                                    cloud_timestampsd[c[i]] +
                                    va_func(dt, r) / float(max_time), 1.0)
                        else:
                            if not cl_mapping:
                                cloud_timestamps[i] = 1.0
                            else:
                                cloud_timestampsd[c[i]] = 1.0
                rgb = [0, 0, 0]
                if not cl_mapping:
                    rgb[rgb_channeli] = int(cloud_timestamps[i] * 255)
                    rgb = struct.unpack(
                        "f",
                        struct.pack("i", int('%02x%02x%02x' % tuple(rgb),
                                             16)))[0]
                    rgbc[i][3] = default_colour if cloud_timestamps[
                        i] == 0 else rgb
                else:
                    rgb[rgb_channeli] = int(cloud_timestampsd[c[i]] * 255)
                    rgb = struct.unpack(
                        "f",
                        struct.pack("i", int('%02x%02x%02x' % tuple(rgb),
                                             16)))[0]
                    rgbc[i][3] = default_colour if cloud_timestampsd[
                        c[i]] == 0 else rgb
                # Hacky way to check points due to my faulty GPU
                # Uncomment below lines for debugging using point translation instead of colour
                # ---
                #  if not cl_mapping:
                #  rgbc[i][0] = 10 - cloud_timestamps[i] * 10 + c[i][0]
                #  rgbc[i][1] = 10 - cloud_timestamps[i] * 10 + c[i][1]
                #  else:
                #  rgbc[i][0] = 10 - cloud_timestampsd[c[i]] * 10 + c[i][0]
                #  rgbc[i][1] = 10 - cloud_timestampsd[c[i]] * 10 + c[i][1]
                # ---
            pc2_area_pub.publish(pc2.create_cloud(cloud.header, fields, rgbc))

        last_tc = t
    except Exception as e:
        rospy.logerr(e)
예제 #23
0
def main(args):

    #prepare the listener
    global socket
    socket.connect("tcp://" + ip + ":" + port)
    socket.setsockopt_string(zmq.SUBSCRIBE, topic )

    #setup some opencv windows
    cv2.namedWindow("Listener Left Camera")
    cv2.namedWindow("Listener Right Camera")

    #Start the main loop
    while True:

        message_in = socket.recv(10*1024)
        #For some reason I have to make a deep copy of the message. Otherwise, when, the second time I received the message I got a truncated message error. This solves it so I did not worry about it anymore.
        message = copy.deepcopy(message_in)
        
        #Deserialization or unmarshalling
        #We use message[4:] because we know the first four bytes are
        #"777 " and we only want to give the data to the parser (not the topic name
        sd.ParseFromString(message[4:])
        
        #Getting the left image
        cv_left_image = np.zeros((sd.left_image.height, sd.left_image.width, 3), np.uint8)
        cv_left_image.data = sd.left_image.data
        
        #Getting the right image
        cv_right_image = np.zeros((sd.right_image.height, sd.right_image.width, 3), np.uint8)
        cv_right_image.data = sd.right_image.data

        #Getting the point cloud
        point_cloud_msg = PointCloud2()
        point_cloud_msg.height = sd.point_cloud.height
        point_cloud_msg.width = sd.point_cloud.width

        for field in sd.point_cloud.fields:
            point_field = PointField()
            point_field.name = field.name
            point_field.offset = field.offset
            point_field.datatype = field.datatype
            point_field.count = field.count

            point_cloud_msg.fields.append(point_field)

        point_cloud_msg.is_bigendian = sd.point_cloud.is_bigendian
        point_cloud_msg.is_bigendian = sd.point_cloud.is_bigendian
        point_cloud_msg.point_step = sd.point_cloud.point_step
        point_cloud_msg.row_step = sd.point_cloud.row_step
        point_cloud_msg.data = sd.point_cloud.data

        #Getting the NavSatFix
        nav_sat_fix = NavSatFix()
        nav_sat_fix.latitude = sd.nav_sat_fix.latitude
        nav_sat_fix.longitude = sd.nav_sat_fix.longitude
        nav_sat_fix.altitude = sd.nav_sat_fix.altitude
        nav_sat_fix.status.status = sd.nav_sat_fix.status.status
        nav_sat_fix.status.service = sd.nav_sat_fix.status.service

        #Getting the Odometry
        odometry = Odometry()
        #and then we can copy every field ... no need in this example


        #---------------------------------
        #Visualizing and /or printing the received data
        #---------------------------------
        print("Received new message with stamp:\n" + str(sd.header.stamp))
    
        print("First 10 points x,y,z and rgb (packed in float32) values (just for debug)")
        count = 0
        for p in pc2.read_points(point_cloud_msg, field_names = ("x", "y", "z", "rgb"), skip_nans=True):
            print " x : %f  y: %f  z: %f rgb: %f" %(p[0],p[1],p[2],p[3])
            count = count + 1
            if count > 10:
                break

        print("GPS data:")
        print("Latitude =" + str(nav_sat_fix.latitude))
        print("Longitude =" + str(nav_sat_fix.longitude))
        print("Altitude =" + str(nav_sat_fix.altitude))
        print("Status =" + str(nav_sat_fix.status.status))
        print("Service =" + str(nav_sat_fix.status.service))

        print("Odometry data (only position for example):")
        print("odom.pose.pose.position.x =" + str(sd.odometry.pose.pose.position.x))
        print("odom.pose.pose.position.y =" + str(sd.odometry.pose.pose.position.y))
        print("odom.pose.pose.position.z =" + str(sd.odometry.pose.pose.position.z))

        cv2.imshow("Listener Left Camera", cv_left_image)
        cv2.imshow("Listener Right Camera", cv_right_image)
        cv2.waitKey(30)
예제 #24
0
#!/usr/bin/env python

# This program publishes a pointcloud2 which has much shorter data
# than what is described by width, height, and point_step.  RViz
# should catch this and show an error instead of crashing.

import rospy
from sensor_msgs.msg import PointCloud2
from sensor_msgs.msg import PointField

rospy.init_node('bad_pointcloud_publisher')

pfx = PointField()
pfx.name = 'x'
pfx.offset = 0
pfx.datatype = 7
pfx.count = 1

pfy = PointField()
pfy.name = 'y'
pfy.offset = 4
pfy.datatype = 7
pfy.count = 1

pfz = PointField()
pfz.name = 'z'
pfz.offset = 8
pfz.datatype = 7
pfz.count = 1

pc = PointCloud2()
예제 #25
0
파일: send_bad_pc2.py 프로젝트: CURG/rviz
#!/usr/bin/env python

# This program publishes a pointcloud2 which has much shorter data
# than what is described by width, height, and point_step.  RViz
# should catch this and show an error instead of crashing.

import rospy
from sensor_msgs.msg import PointCloud2
from sensor_msgs.msg import PointField

rospy.init_node( 'bad_pointcloud_publisher' )

pfx = PointField()
pfx.name = 'x'
pfx.offset = 0
pfx.datatype = 7
pfx.count = 1

pfy = PointField()
pfy.name = 'y'
pfy.offset = 4
pfy.datatype = 7
pfy.count = 1

pfz = PointField()
pfz.name = 'z'
pfz.offset = 8
pfz.datatype = 7
pfz.count = 1

pc = PointCloud2()
예제 #26
0
#!/usr/bin/env python

# This program publishes a pointcloud2 which has much shorter data
# than what is described by width, height, and point_step.  RViz
# should catch this and show an error instead of crashing.

import rospy
from sensor_msgs.msg import PointCloud2
from sensor_msgs.msg import PointField

rospy.init_node("bad_pointcloud_publisher")

pfx = PointField()
pfx.name = "x"
pfx.offset = 0
pfx.datatype = 7
pfx.count = 1

pfy = PointField()
pfy.name = "y"
pfy.offset = 4
pfy.datatype = 7
pfy.count = 1

pfz = PointField()
pfz.name = "z"
pfz.offset = 8
pfz.datatype = 7
pfz.count = 1

pc = PointCloud2()