From 4e52524676dc393f3ae537aeffb8103635b8fba1 Mon Sep 17 00:00:00 2001 From: Sergio Bellido Date: Wed, 23 Mar 2022 08:17:55 +0100 Subject: [PATCH 1/4] Added some functionalities, pending for checks --- importRosbag/messageTypes/common.py | 3 ++ .../messageTypes/sensor_msgs_PointCloud2.py | 48 ++++++++++++++++--- 2 files changed, 45 insertions(+), 6 deletions(-) diff --git a/importRosbag/messageTypes/common.py b/importRosbag/messageTypes/common.py index f3925ea..1d0706f 100644 --- a/importRosbag/messageTypes/common.py +++ b/importRosbag/messageTypes/common.py @@ -43,6 +43,9 @@ def unpackRosUint32(data, ptr): def unpackRosUint8(data, ptr): return unpack('=B', data[ptr:ptr+1])[0], ptr+1 +def unpackRosUint16(data, ptr): + return unpack('=B', data[ptr:ptr+2])[0], ptr+2 + def unpackRosString(data, ptr): stringLen = unpack('=L', data[ptr:ptr+4])[0] ptr += 4 diff --git a/importRosbag/messageTypes/sensor_msgs_PointCloud2.py b/importRosbag/messageTypes/sensor_msgs_PointCloud2.py index 02d027a..1a10439 100644 --- a/importRosbag/messageTypes/sensor_msgs_PointCloud2.py +++ b/importRosbag/messageTypes/sensor_msgs_PointCloud2.py @@ -34,7 +34,7 @@ import numpy as np from .common import unpackRosString, unpackRosUint8, unpackRosUint32, \ - unpackRosTimestamp + unpackRosTimestamp, unpackRosFloat32, unpackRosUint16 def importTopic(msgs, **kwargs): ''' @@ -52,6 +52,12 @@ def importTopic(msgs, **kwargs): disable_bar = kwargs.get('disable_bar') tsByMessage = [] pointsByMessage = [] + timeByMessage = [] + reflectivityByMessage = [] + ringByMessage = [] + noiseByMessage = [] + rangesByMessage = [] + for msg in tqdm(msgs, disable=disable_bar): data = msg['data'] @@ -78,23 +84,53 @@ def importTopic(msgs, **kwargs): rowStep, ptr = unpackRosUint32(data, ptr) numPoints = width * height - points = np.empty((numPoints, 3), dtype=np.float32) + points = np.empty((numPoints, 4), dtype=np.float32) + time_arr = np.empty((numPoints,1), dtype=np.uint32) + reflectivity = np.empty((numPoints, 1), dtype=np.uint16) + ring = np.empty((numPoints, 1), dtype=np.uint8) + noise = np.empty((numPoints, 1), dtype=np.uint16) + ranges = np.empty((numPoints, 1), dtype=np.uint32) + arraySize, ptr = unpackRosUint32(data, ptr) # assert arraySize = width*height for x in range(width): for y in range(height): - points[x*height + y, :] = np.frombuffer(data[ptr:ptr+12], dtype=np.float32) + points[x*height + y, :] = np.frombuffer(data[ptr:ptr+16], dtype=np.float32) + time_arr[x*height + y, 0] = np.frombuffer(data[ptr+16:ptr+16+4], dtype=np.uint32) + reflectivity[x*height + y, 0] = np.frombuffer(data[ptr+20:ptr+20+2], dtype=np.uint16) + ring[x*height + y, 0] = np.frombuffer(data[ptr+22:ptr+22+1], dtype=np.uint8) + noise[x*height + y, 0] = np.frombuffer(data[ptr+23:ptr+23+2], dtype=np.uint16) + ranges[x*height + y, 0] = np.frombuffer(data[ptr+25:ptr+25+4], dtype=np.uint32) + ptr += pointStep + pointsByMessage.append(points) tsByMessage.append(np.ones((numPoints), dtype=np.float64) * ts) + timeByMessage.append(time_arr) + reflectivityByMessage.append(reflectivity) + ringByMessage.append(ring) + noiseByMessage.append(noise) + rangesByMessage.append(ranges) + if not pointsByMessage: # None of the messages contained any points return None - points = np.concatenate(pointsByMessage) - ts = np.concatenate(tsByMessage) - + points = np.concatenate(pointsByMessage) + ts = np.concatenate(tsByMessage) + time_internal = np.concatenate(timeByMessage) + reflectivity = np.concatenate(reflectivityByMessage) + ring = np.concatenate(ringByMessage) + noise = np.concatenate(noiseByMessage) + ranges = np.concatenate(rangesByMessage) + + # Crop arrays to number of events outDict = { 'ts': ts, 'point': points, + 'time': time_internal, + 'reflectivity': reflectivity, + 'ring': ring, + 'noise': noise, + 'ranges': ranges, } return outDict From 022a5bfa5d227893f5603985c7c3981e62ca4955 Mon Sep 17 00:00:00 2001 From: sergio bellido jimenez Date: Fri, 25 Mar 2022 15:28:45 +0100 Subject: [PATCH 2/4] Added functionalities for the variable fields at pointcloud2 msgs --- importRosbag/importRosbag.py | 7 +- importRosbag/messageTypes/common.py | 8 + .../messageTypes/sensor_msgs_PointCloud2.py | 28 +- .../sensor_msgs_PointCloud2_testSBJ.py | 249 ++++++++++++++++++ 4 files changed, 281 insertions(+), 11 deletions(-) create mode 100644 importRosbag/messageTypes/sensor_msgs_PointCloud2_testSBJ.py diff --git a/importRosbag/importRosbag.py b/importRosbag/importRosbag.py index 0fbf1c0..5f4d042 100644 --- a/importRosbag/importRosbag.py +++ b/importRosbag/importRosbag.py @@ -40,6 +40,7 @@ from struct import error as structError from tqdm import tqdm +import time # Local imports from .messageTypes.common import unpackHeader @@ -53,12 +54,14 @@ from .messageTypes.sensor_msgs_CameraInfo import importTopic as import_sensor_msgs_CameraInfo from .messageTypes.sensor_msgs_Image import importTopic as import_sensor_msgs_Image from .messageTypes.sensor_msgs_Imu import importTopic as import_sensor_msgs_Imu -from .messageTypes.sensor_msgs_PointCloud2 import importTopic as import_sensor_msgs_PointCloud2 +#from .messageTypes.sensor_msgs_PointCloud2 import importTopic as import_sensor_msgs_PointCloud2 +from .messageTypes.sensor_msgs_PointCloud2_testSBJ import importTopic as import_sensor_msgs_PointCloud2 from .messageTypes.tf_tfMessage import importTopic as import_tf_tfMessage import logging def importTopic(topic, **kwargs): + time_start = time.time() msgs = topic['msgs'] topicType = topic['type'].replace('/','_') if topicType == 'dvs_msgs_EventArray': topicDict = import_dvs_msgs_EventArray(msgs, **kwargs) @@ -76,6 +79,8 @@ def importTopic(topic, **kwargs): return None if topicDict: topicDict['rosbagType'] = topic['type'] + elapsed_time = time.time() - time_start + print("Tiempo transcurrido: {} segundos".format(elapsed_time)) return topicDict def readFile(filePathOrName): diff --git a/importRosbag/messageTypes/common.py b/importRosbag/messageTypes/common.py index 1d0706f..cf4f19a 100644 --- a/importRosbag/messageTypes/common.py +++ b/importRosbag/messageTypes/common.py @@ -56,6 +56,14 @@ def unpackRosString(data, ptr): ptr += stringLen return outStr, ptr +def unpackStringData(data, stringLen, ptr): + try: + outStr = data[ptr:ptr+stringLen].decode('utf-8') + except UnicodeDecodeError: + outStr = 'UnicodeDecodeError' + ptr += stringLen + return outStr, ptr + def unpackRosFloat64Array(data, num, ptr): return np.frombuffer(data[ptr:ptr+num*8], dtype=np.float64), ptr+num*8 diff --git a/importRosbag/messageTypes/sensor_msgs_PointCloud2.py b/importRosbag/messageTypes/sensor_msgs_PointCloud2.py index 1a10439..a3255bf 100644 --- a/importRosbag/messageTypes/sensor_msgs_PointCloud2.py +++ b/importRosbag/messageTypes/sensor_msgs_PointCloud2.py @@ -32,7 +32,7 @@ from tqdm import tqdm import numpy as np - +import time from .common import unpackRosString, unpackRosUint8, unpackRosUint32, \ unpackRosTimestamp, unpackRosFloat32, unpackRosUint16 @@ -52,6 +52,7 @@ def importTopic(msgs, **kwargs): disable_bar = kwargs.get('disable_bar') tsByMessage = [] pointsByMessage = [] + intensityByMessage = [] timeByMessage = [] reflectivityByMessage = [] ringByMessage = [] @@ -59,7 +60,6 @@ def importTopic(msgs, **kwargs): rangesByMessage = [] for msg in tqdm(msgs, disable=disable_bar): - data = msg['data'] ptr = 0 seq, ptr = unpackRosUint32(data, ptr) @@ -84,7 +84,8 @@ def importTopic(msgs, **kwargs): rowStep, ptr = unpackRosUint32(data, ptr) numPoints = width * height - points = np.empty((numPoints, 4), dtype=np.float32) + points = np.empty((numPoints, 3), dtype=np.float32) + intensity = np.empty((numPoints, 1), dtype=np.float32) time_arr = np.empty((numPoints,1), dtype=np.uint32) reflectivity = np.empty((numPoints, 1), dtype=np.uint16) ring = np.empty((numPoints, 1), dtype=np.uint8) @@ -95,17 +96,21 @@ def importTopic(msgs, **kwargs): # assert arraySize = width*height for x in range(width): for y in range(height): - points[x*height + y, :] = np.frombuffer(data[ptr:ptr+16], dtype=np.float32) - time_arr[x*height + y, 0] = np.frombuffer(data[ptr+16:ptr+16+4], dtype=np.uint32) - reflectivity[x*height + y, 0] = np.frombuffer(data[ptr+20:ptr+20+2], dtype=np.uint16) - ring[x*height + y, 0] = np.frombuffer(data[ptr+22:ptr+22+1], dtype=np.uint8) - noise[x*height + y, 0] = np.frombuffer(data[ptr+23:ptr+23+2], dtype=np.uint16) - ranges[x*height + y, 0] = np.frombuffer(data[ptr+25:ptr+25+4], dtype=np.uint32) + points[x*height+y, :] = np.frombuffer(data[ptr:ptr+12], dtype=np.float32) + intensity[x*height+y, 0] = np.frombuffer(data[ptr+16:ptr+20], dtype=np.float32) + #intensity, ptr_delete = unpackRosFloat32(data, ptr+12) + time_arr[x*height+y, 0] = np.frombuffer(data[ptr+20:ptr+24], dtype=np.uint32) + reflectivity[x*height+y, 0] = np.frombuffer(data[ptr+24:ptr+26], dtype=np.uint16) + ring[x*height+y, 0] = np.frombuffer(data[ptr+26:ptr+27], dtype=np.uint8) + noise[x*height+y, 0] = np.frombuffer(data[ptr+27:ptr+29], dtype=np.uint16) + ranges[x*height+y, 0] = np.frombuffer(data[ptr+29:ptr+33], dtype=np.uint32) ptr += pointStep pointsByMessage.append(points) - tsByMessage.append(np.ones((numPoints), dtype=np.float64) * ts) + tsByMessage.append(np.ones(numPoints, dtype=np.float64) * ts) + intensityByMessage.append(intensity) + #intensityByMessage.append(np.ones(numPoints, dtype=np.float64) * intensity) timeByMessage.append(time_arr) reflectivityByMessage.append(reflectivity) ringByMessage.append(ring) @@ -116,6 +121,7 @@ def importTopic(msgs, **kwargs): return None points = np.concatenate(pointsByMessage) ts = np.concatenate(tsByMessage) + intensity = np.concatenate(intensityByMessage) time_internal = np.concatenate(timeByMessage) reflectivity = np.concatenate(reflectivityByMessage) ring = np.concatenate(ringByMessage) @@ -123,10 +129,12 @@ def importTopic(msgs, **kwargs): ranges = np.concatenate(rangesByMessage) + # Crop arrays to number of events outDict = { 'ts': ts, 'point': points, + 'intensity': intensity, 'time': time_internal, 'reflectivity': reflectivity, 'ring': ring, diff --git a/importRosbag/messageTypes/sensor_msgs_PointCloud2_testSBJ.py b/importRosbag/messageTypes/sensor_msgs_PointCloud2_testSBJ.py new file mode 100644 index 0000000..7e163a1 --- /dev/null +++ b/importRosbag/messageTypes/sensor_msgs_PointCloud2_testSBJ.py @@ -0,0 +1,249 @@ +# -*- coding: utf-8 -*- + +""" +Copyright (C) 2019 Event-driven Perception for Robotics +Authors: Sim Bamford +This program is free software: you can redistribute it and/or modify it under +the terms of the GNU General Public License as published by the Free Software +Foundation, either version 3 of the License, or (at your option) any later version. +This program is distributed in the hope that it will be useful, but WITHOUT ANY +WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A +PARTICULAR PURPOSE. See the GNU General Public License for more details. +You should have received a copy of the GNU General Public License along with +this program. If not, see . + +Intended as part of importRosbag. + +The importTopic function receives a list of messages and returns +a dict with one field for each data field in the message, where the field +will contain an appropriate iterable to contain the interpretted contents of each message. +In some cases, static info is repeated in each message; in which case a field may not contain an iterable. + +This function imports the ros message type defined at: +http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html + +For simplicity, we're currently directly unpacking the format that we are +encountering in the data, which is x,y,z,_,rgb,_,_,_ +each as 32-bit little-endian floats +""" + +# %% + + +from tqdm import tqdm +import numpy as np +import time + +from .common import unpackRosString, unpackRosUint8, unpackRosUint32, unpackRosTimestamp, unpackRosFloat32, \ + unpackRosUint16, unpackStringData + + +def importTopic(msgs, **kwargs): + ''' + ros message is defined here: + http://docs.ros.org/api/geometry_msgs/html/msg/PoseStamped.html + the result is are np arrays of float64 for: + rotQ (4 cols, quaternion) + angV (3 cols) + acc (3 cols) + mag (3 cols) + temp (1 cols) - but I'll probably ignore this to start with + ''' + # tempAll = np.zeros((sizeOfArray, 1), dtype=np.float64) + # for msg in tqdm(msgs, position=0, leave=True): + disable_bar = kwargs.get('disable_bar') + DTYPE_VAR = { + '1': '=i1', # INT8 + '2': '=u1', # UINT8 + '3': '=i2', # INT16 + '4': '=u2', # UINT16 + '5': '=i4', # INT32 + '6': '=u4', # UINT32 + '7': '=f4', # FLOAT32 + '8': '=f8', # FLOAT64 + } + dtype_header_1 = { + 'names': ['sequence_id', 'timestamp_seconds', 'timestamp_nseconds', # List of Strings + 'frame_len'], + 'formats': ['=u4','=u4','=u4','=u4'], # List of Strings + 'offsets': [0, 4, 8, 12], # List of INT + 'itemsize': 16 # INT + } + dtype_header_2 = { + 'names': ['height', 'width', 'fieldSize'], + 'formats': [ '=u4', '=u4', '=u4'], # List of Strings + 'offsets': [0, 4, 8], # List of INT + 'itemsize': 12 # INT + } + dtype_header_3 = { + 'names': ['is_Bigendian', 'PointStep', 'RowStep', 'data_size'], + 'formats': [ '=u1', '=u4', '=u4','=u4'], # List of Strings + 'offsets': [0, 1, 5, 9], # List of INT + 'itemsize': 13 # INT + } + dtype_fields = { + 'names': [], # List of Strings + 'formats': [], # List of Strings + 'offsets': [], # List of INT + 'itemsize': 0, + 'count': [] # List of INT + } + + tsByMessage_2 = [] + pointsByMessage_2 = [] + + ''' #Del codigo anterior + tsByMessage = [] + pointsByMessage = [] + intensityByMessage = [] + timeByMessage = [] + reflectivityByMessage = [] + ringByMessage = [] + noiseByMessage = [] + rangesByMessage = []''' + + for msg in tqdm(msgs, disable=disable_bar): + '''time_start = time.time()''' + data = msg['data'] + ptr = 0 + pointer = 0 + # Made with numpy from buffer + header_1 = np.frombuffer(data[pointer:pointer + dtype_header_1['itemsize']], dtype=dtype_header_1) + timestamp = np.float64(header_1['timestamp_seconds'][0])+np.float64(header_1['timestamp_nseconds'][0])*0.000000001 + + pointer += dtype_header_1['itemsize'] + frame_len = header_1['frame_len'][0].astype(int) + frame_id, pointer = unpackStringData(data, frame_len, pointer) + header_2 = np.frombuffer(data[pointer:pointer + dtype_header_2['itemsize']], dtype=dtype_header_2) + pointer += dtype_header_2['itemsize'] + + #Asignar las variables de array a independientes + height = header_2['height'][0] + width = header_2['width'][0] + fieldSize = header_2['fieldSize'][0] + + #print('type of fieldSize: {},{}'.format(type(header_2['fieldSize'][0]),len(header_2['fieldSize']))) + if width > 0 and height > 0: + for element in range(fieldSize): + name, pointer = unpackRosString(data, pointer) + offset = np.frombuffer(data[pointer:pointer+4], dtype='=u4') + pointer += 4 + datatype = np.frombuffer(data[pointer:pointer+1], dtype='=u1') + pointer += 1 + count = np.frombuffer(data[pointer:pointer + 4], dtype='=u4') + pointer += 4 + #APPEND TO THE DICTIONARY ('dtype_fields') + dtype_fields['names'].append(name) + dtype_fields['offsets'].append(offset[0]) + dtype_fields['formats'].append(str(DTYPE_VAR[str(datatype[0])])) #Pasarlo por el diccionario + dtype_fields['count'].append(count[0]) + + header_3 = np.frombuffer(data[pointer:pointer + dtype_header_3['itemsize']], dtype=dtype_header_3) + pointer += dtype_header_3['itemsize'] + + PointStep = header_3['PointStep'][0] + dtype_fields['itemsize'] = PointStep + numPoints = width * height + data_array = np.empty((numPoints,1),dtype=dtype_fields) + #array = np.frombuffer(data[pointer:pointer + dtype_fields['itemsize']], dtype=dtype_fields) + for x in range(width): + for y in range(height): + data_array[x * height + y, :] = np.frombuffer(data[pointer:pointer+dtype_fields['itemsize']], dtype=dtype_fields) + pointer += PointStep + + pointsByMessage_2.append(data_array) + tsByMessage_2.append(np.ones(numPoints, dtype=np.float64) * timestamp) #Meter aquí la traspuesta de la matriz??? + + if not pointsByMessage_2: # None of the messages contained any points + return None + #pass + data_array = np.concatenate(pointsByMessage_2) + timestamp = np.concatenate(tsByMessage_2) + '''elapsed_time = time.time() - time_start + print("Tiempo transcurrido: {} segundos".format(elapsed_time))''' + # Crop arrays to number of events + outDict = { + 'ts': timestamp, + 'point': data_array, + } + return outDict + + ''' #----------------------------------------------------------------- + seq, ptr = unpackRosUint32(data, ptr) + ts, ptr = unpackRosTimestamp(data, ptr) + frame_id, ptr = unpackRosString(data, ptr) + height, ptr = unpackRosUint32(data, ptr) + width, ptr = unpackRosUint32(data, ptr) + + if width > 0 and height > 0: + + arraySize, ptr = unpackRosUint32(data, ptr) + for element in range(arraySize): + # Move through the field definitions - we'll ignore these + # until we encounter a file that uses a different set + name, ptr = unpackRosString(data, ptr) + offset, ptr = unpackRosUint32(data, ptr) + datatype, ptr = unpackRosUint8(data, ptr) + count, ptr = unpackRosUint32(data, ptr) + + isBigendian, ptr = unpackRosUint8(data, ptr) + pointStep, ptr = unpackRosUint32(data, ptr) + rowStep, ptr = unpackRosUint32(data, ptr) + + numPoints = width * height + points = np.empty((numPoints, 3), dtype=np.float32) + intensity = np.empty((numPoints, 1), dtype=np.float32) + time_arr = np.empty((numPoints, 1), dtype=np.uint32) + reflectivity = np.empty((numPoints, 1), dtype=np.uint16) + ring = np.empty((numPoints, 1), dtype=np.uint8) + noise = np.empty((numPoints, 1), dtype=np.uint16) + ranges = np.empty((numPoints, 1), dtype=np.uint32) + + arraySize, ptr = unpackRosUint32(data, ptr) + # assert arraySize = width*height + for x in range(width): + for y in range(height): + points[x * height + y, :] = np.frombuffer(data[ptr:ptr + 12], dtype=np.float32) + intensity[x * height + y, 0] = np.frombuffer(data[ptr + 16:ptr + 20], dtype=np.float32) + # intensity, ptr_delete = unpackRosFloat32(data, ptr+12) + time_arr[x * height + y, 0] = np.frombuffer(data[ptr + 20:ptr + 24], dtype=np.uint32) + reflectivity[x * height + y, 0] = np.frombuffer(data[ptr + 24:ptr + 26], dtype=np.uint16) + ring[x * height + y, 0] = np.frombuffer(data[ptr + 26:ptr + 27], dtype=np.uint8) + noise[x * height + y, 0] = np.frombuffer(data[ptr + 27:ptr + 29], dtype=np.uint16) + ranges[x * height + y, 0] = np.frombuffer(data[ptr + 29:ptr + 33], dtype=np.uint32) + + ptr += pointStep + + pointsByMessage.append(points) + tsByMessage.append(np.ones(numPoints, dtype=np.float64) * ts) + intensityByMessage.append(intensity) + # intensityByMessage.append(np.ones(numPoints, dtype=np.float64) * intensity) + timeByMessage.append(time_arr) + reflectivityByMessage.append(reflectivity) + ringByMessage.append(ring) + noiseByMessage.append(noise) + rangesByMessage.append(ranges) + + if not pointsByMessage: # None of the messages contained any points + return None + points = np.concatenate(pointsByMessage) + ts = np.concatenate(tsByMessage) + intensity = np.concatenate(intensityByMessage) + time_internal = np.concatenate(timeByMessage) + reflectivity = np.concatenate(reflectivityByMessage) + ring = np.concatenate(ringByMessage) + noise = np.concatenate(noiseByMessage) + ranges = np.concatenate(rangesByMessage) + + # Crop arrays to number of events + outDict = { + 'ts': ts, + 'point': points, + 'intensity': intensity, + 'time': time_internal, + 'reflectivity': reflectivity, + 'ring': ring, + 'noise': noise, + 'ranges': ranges, + } + return outDict''' From 0ea015fc512ee7ca0d3dddf4f856896eabe7dc86 Mon Sep 17 00:00:00 2001 From: Sergio Bellido Date: Mon, 28 Mar 2022 15:57:38 +0200 Subject: [PATCH 3/4] Solved bug of nameValue error --- importRosbag/importRosbag.py | 1 + .../sensor_msgs_PointCloud2_testSBJ.py | 183 +++++------------- 2 files changed, 45 insertions(+), 139 deletions(-) diff --git a/importRosbag/importRosbag.py b/importRosbag/importRosbag.py index 5f4d042..aa1d4c6 100644 --- a/importRosbag/importRosbag.py +++ b/importRosbag/importRosbag.py @@ -182,6 +182,7 @@ def rekeyConnsByTopic(connDict): def importRosbag(filePathOrName, **kwargs): global disable_bar + kwargs.setdefault('log', 'ERROR') disable_bar = kwargs.get('disable_bar') loglevel = kwargs.get('log') numeric_level = getattr(logging, loglevel.upper(), None) diff --git a/importRosbag/messageTypes/sensor_msgs_PointCloud2_testSBJ.py b/importRosbag/messageTypes/sensor_msgs_PointCloud2_testSBJ.py index 7e163a1..3f44b99 100644 --- a/importRosbag/messageTypes/sensor_msgs_PointCloud2_testSBJ.py +++ b/importRosbag/messageTypes/sensor_msgs_PointCloud2_testSBJ.py @@ -22,9 +22,8 @@ This function imports the ros message type defined at: http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html -For simplicity, we're currently directly unpacking the format that we are -encountering in the data, which is x,y,z,_,rgb,_,_,_ -each as 32-bit little-endian floats +In this new version of the pointcloud2 extract, we can automated unpacking +all the fields that are included in pointcloud2 message in the format that is required. """ # %% @@ -41,16 +40,11 @@ def importTopic(msgs, **kwargs): ''' ros message is defined here: - http://docs.ros.org/api/geometry_msgs/html/msg/PoseStamped.html - the result is are np arrays of float64 for: - rotQ (4 cols, quaternion) - angV (3 cols) - acc (3 cols) - mag (3 cols) - temp (1 cols) - but I'll probably ignore this to start with + http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html + the result is a dict variable with + - timestamp + - point structure (include all field that declares in pointcloud2 msg) ''' - # tempAll = np.zeros((sizeOfArray, 1), dtype=np.float64) - # for msg in tqdm(msgs, position=0, leave=True): disable_bar = kwargs.get('disable_bar') DTYPE_VAR = { '1': '=i1', # INT8 @@ -65,67 +59,56 @@ def importTopic(msgs, **kwargs): dtype_header_1 = { 'names': ['sequence_id', 'timestamp_seconds', 'timestamp_nseconds', # List of Strings 'frame_len'], - 'formats': ['=u4','=u4','=u4','=u4'], # List of Strings - 'offsets': [0, 4, 8, 12], # List of INT - 'itemsize': 16 # INT + 'formats': ['=u4','=u4','=u4','=u4'], # List of Strings + 'offsets': [0, 4, 8, 12], # List of INT + 'itemsize': 16 # INT } dtype_header_2 = { - 'names': ['height', 'width', 'fieldSize'], - 'formats': [ '=u4', '=u4', '=u4'], # List of Strings - 'offsets': [0, 4, 8], # List of INT - 'itemsize': 12 # INT + 'names': ['height', 'width', 'fieldSize'], # List of Strings + 'formats': [ '=u4', '=u4', '=u4'], # List of Strings + 'offsets': [0, 4, 8], # List of INT + 'itemsize': 12 # INT } dtype_header_3 = { - 'names': ['is_Bigendian', 'PointStep', 'RowStep', 'data_size'], - 'formats': [ '=u1', '=u4', '=u4','=u4'], # List of Strings - 'offsets': [0, 1, 5, 9], # List of INT - 'itemsize': 13 # INT + 'names': ['is_Bigendian', 'PointStep', 'RowStep', 'data_size'], # List of Strings + 'formats': [ '=u1', '=u4', '=u4','=u4'], # List of Strings + 'offsets': [0, 1, 5, 9], # List of INT + 'itemsize': 13 # INT } dtype_fields = { - 'names': [], # List of Strings - 'formats': [], # List of Strings - 'offsets': [], # List of INT - 'itemsize': 0, - 'count': [] # List of INT + 'names': [], # List of Strings + 'formats': [], # List of Strings + 'offsets': [], # List of INT + 'itemsize': 0, # INT + 'count': [] # List of INT } tsByMessage_2 = [] pointsByMessage_2 = [] - ''' #Del codigo anterior - tsByMessage = [] - pointsByMessage = [] - intensityByMessage = [] - timeByMessage = [] - reflectivityByMessage = [] - ringByMessage = [] - noiseByMessage = [] - rangesByMessage = []''' - for msg in tqdm(msgs, disable=disable_bar): - '''time_start = time.time()''' data = msg['data'] - ptr = 0 pointer = 0 - # Made with numpy from buffer + # Reading first header (numpy) header_1 = np.frombuffer(data[pointer:pointer + dtype_header_1['itemsize']], dtype=dtype_header_1) timestamp = np.float64(header_1['timestamp_seconds'][0])+np.float64(header_1['timestamp_nseconds'][0])*0.000000001 - pointer += dtype_header_1['itemsize'] + # Unpacking frame_id with unknown dimension frame_len = header_1['frame_len'][0].astype(int) frame_id, pointer = unpackStringData(data, frame_len, pointer) + # Reading second header (numpy) header_2 = np.frombuffer(data[pointer:pointer + dtype_header_2['itemsize']], dtype=dtype_header_2) pointer += dtype_header_2['itemsize'] - #Asignar las variables de array a independientes + #Assign variables height = header_2['height'][0] width = header_2['width'][0] fieldSize = header_2['fieldSize'][0] - #print('type of fieldSize: {},{}'.format(type(header_2['fieldSize'][0]),len(header_2['fieldSize']))) + #Reading fields of pointcloud2 if width > 0 and height > 0: for element in range(fieldSize): - name, pointer = unpackRosString(data, pointer) + names, pointer = unpackRosString(data, pointer) offset = np.frombuffer(data[pointer:pointer+4], dtype='=u4') pointer += 4 datatype = np.frombuffer(data[pointer:pointer+1], dtype='=u1') @@ -133,9 +116,9 @@ def importTopic(msgs, **kwargs): count = np.frombuffer(data[pointer:pointer + 4], dtype='=u4') pointer += 4 #APPEND TO THE DICTIONARY ('dtype_fields') - dtype_fields['names'].append(name) + dtype_fields['names'].append(names) dtype_fields['offsets'].append(offset[0]) - dtype_fields['formats'].append(str(DTYPE_VAR[str(datatype[0])])) #Pasarlo por el diccionario + dtype_fields['formats'].append(str(DTYPE_VAR[str(datatype[0])])) dtype_fields['count'].append(count[0]) header_3 = np.frombuffer(data[pointer:pointer + dtype_header_3['itemsize']], dtype=dtype_header_3) @@ -144,8 +127,8 @@ def importTopic(msgs, **kwargs): PointStep = header_3['PointStep'][0] dtype_fields['itemsize'] = PointStep numPoints = width * height - data_array = np.empty((numPoints,1),dtype=dtype_fields) - #array = np.frombuffer(data[pointer:pointer + dtype_fields['itemsize']], dtype=dtype_fields) + + data_array = np.empty((numPoints, 1), dtype=dtype_fields) for x in range(width): for y in range(height): data_array[x * height + y, :] = np.frombuffer(data[pointer:pointer+dtype_fields['itemsize']], dtype=dtype_fields) @@ -153,97 +136,19 @@ def importTopic(msgs, **kwargs): pointsByMessage_2.append(data_array) tsByMessage_2.append(np.ones(numPoints, dtype=np.float64) * timestamp) #Meter aquí la traspuesta de la matriz??? + #Reset dtype_fields variables for the next msg + dtype_fields['names'] = [] + dtype_fields['offsets'] = [] + dtype_fields['formats'] = [] + dtype_fields['count'] = [] - if not pointsByMessage_2: # None of the messages contained any points - return None - #pass - data_array = np.concatenate(pointsByMessage_2) - timestamp = np.concatenate(tsByMessage_2) - '''elapsed_time = time.time() - time_start - print("Tiempo transcurrido: {} segundos".format(elapsed_time))''' - # Crop arrays to number of events - outDict = { - 'ts': timestamp, - 'point': data_array, - } - return outDict - - ''' #----------------------------------------------------------------- - seq, ptr = unpackRosUint32(data, ptr) - ts, ptr = unpackRosTimestamp(data, ptr) - frame_id, ptr = unpackRosString(data, ptr) - height, ptr = unpackRosUint32(data, ptr) - width, ptr = unpackRosUint32(data, ptr) - - if width > 0 and height > 0: - - arraySize, ptr = unpackRosUint32(data, ptr) - for element in range(arraySize): - # Move through the field definitions - we'll ignore these - # until we encounter a file that uses a different set - name, ptr = unpackRosString(data, ptr) - offset, ptr = unpackRosUint32(data, ptr) - datatype, ptr = unpackRosUint8(data, ptr) - count, ptr = unpackRosUint32(data, ptr) - - isBigendian, ptr = unpackRosUint8(data, ptr) - pointStep, ptr = unpackRosUint32(data, ptr) - rowStep, ptr = unpackRosUint32(data, ptr) - - numPoints = width * height - points = np.empty((numPoints, 3), dtype=np.float32) - intensity = np.empty((numPoints, 1), dtype=np.float32) - time_arr = np.empty((numPoints, 1), dtype=np.uint32) - reflectivity = np.empty((numPoints, 1), dtype=np.uint16) - ring = np.empty((numPoints, 1), dtype=np.uint8) - noise = np.empty((numPoints, 1), dtype=np.uint16) - ranges = np.empty((numPoints, 1), dtype=np.uint32) - - arraySize, ptr = unpackRosUint32(data, ptr) - # assert arraySize = width*height - for x in range(width): - for y in range(height): - points[x * height + y, :] = np.frombuffer(data[ptr:ptr + 12], dtype=np.float32) - intensity[x * height + y, 0] = np.frombuffer(data[ptr + 16:ptr + 20], dtype=np.float32) - # intensity, ptr_delete = unpackRosFloat32(data, ptr+12) - time_arr[x * height + y, 0] = np.frombuffer(data[ptr + 20:ptr + 24], dtype=np.uint32) - reflectivity[x * height + y, 0] = np.frombuffer(data[ptr + 24:ptr + 26], dtype=np.uint16) - ring[x * height + y, 0] = np.frombuffer(data[ptr + 26:ptr + 27], dtype=np.uint8) - noise[x * height + y, 0] = np.frombuffer(data[ptr + 27:ptr + 29], dtype=np.uint16) - ranges[x * height + y, 0] = np.frombuffer(data[ptr + 29:ptr + 33], dtype=np.uint32) - - ptr += pointStep - - pointsByMessage.append(points) - tsByMessage.append(np.ones(numPoints, dtype=np.float64) * ts) - intensityByMessage.append(intensity) - # intensityByMessage.append(np.ones(numPoints, dtype=np.float64) * intensity) - timeByMessage.append(time_arr) - reflectivityByMessage.append(reflectivity) - ringByMessage.append(ring) - noiseByMessage.append(noise) - rangesByMessage.append(ranges) - - if not pointsByMessage: # None of the messages contained any points + if not pointsByMessage_2: # None of the messages contained any points return None - points = np.concatenate(pointsByMessage) - ts = np.concatenate(tsByMessage) - intensity = np.concatenate(intensityByMessage) - time_internal = np.concatenate(timeByMessage) - reflectivity = np.concatenate(reflectivityByMessage) - ring = np.concatenate(ringByMessage) - noise = np.concatenate(noiseByMessage) - ranges = np.concatenate(rangesByMessage) - + data_array = np.concatenate(pointsByMessage_2) + timestamp = np.concatenate(tsByMessage_2) # Crop arrays to number of events outDict = { - 'ts': ts, - 'point': points, - 'intensity': intensity, - 'time': time_internal, - 'reflectivity': reflectivity, - 'ring': ring, - 'noise': noise, - 'ranges': ranges, - } - return outDict''' + 'ts': timestamp, + 'point': data_array, + } + return outDict From 19222ab3de163f7eca306d41a2dc12d44b75626d Mon Sep 17 00:00:00 2001 From: Sergio Bellido Date: Thu, 31 Mar 2022 13:33:28 +0200 Subject: [PATCH 4/4] Clean and comment code for PR --- .idea/.gitignore | 3 + .idea/importRosbag.iml | 12 ++ .idea/inspectionProfiles/Project_Default.xml | 14 ++ .../inspectionProfiles/profiles_settings.xml | 6 + .idea/misc.xml | 7 + .idea/modules.xml | 8 + .idea/vcs.xml | 6 + importRosbag/importRosbag.py | 3 +- .../messageTypes/sensor_msgs_PointCloud2.py | 202 ++++++++++-------- .../sensor_msgs_PointCloud2_testSBJ.py | 154 ------------- 10 files changed, 164 insertions(+), 251 deletions(-) create mode 100644 .idea/.gitignore create mode 100644 .idea/importRosbag.iml create mode 100644 .idea/inspectionProfiles/Project_Default.xml create mode 100644 .idea/inspectionProfiles/profiles_settings.xml create mode 100644 .idea/misc.xml create mode 100644 .idea/modules.xml create mode 100644 .idea/vcs.xml delete mode 100644 importRosbag/messageTypes/sensor_msgs_PointCloud2_testSBJ.py diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..26d3352 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,3 @@ +# Default ignored files +/shelf/ +/workspace.xml diff --git a/.idea/importRosbag.iml b/.idea/importRosbag.iml new file mode 100644 index 0000000..cf07444 --- /dev/null +++ b/.idea/importRosbag.iml @@ -0,0 +1,12 @@ + + + + + + + + + + \ No newline at end of file diff --git a/.idea/inspectionProfiles/Project_Default.xml b/.idea/inspectionProfiles/Project_Default.xml new file mode 100644 index 0000000..e03dac5 --- /dev/null +++ b/.idea/inspectionProfiles/Project_Default.xml @@ -0,0 +1,14 @@ + + + + \ No newline at end of file diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000..91a2157 --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,7 @@ + + + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml new file mode 100644 index 0000000..2523385 --- /dev/null +++ b/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml new file mode 100644 index 0000000..94a25f7 --- /dev/null +++ b/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/importRosbag/importRosbag.py b/importRosbag/importRosbag.py index aa1d4c6..97d5a03 100644 --- a/importRosbag/importRosbag.py +++ b/importRosbag/importRosbag.py @@ -54,8 +54,7 @@ from .messageTypes.sensor_msgs_CameraInfo import importTopic as import_sensor_msgs_CameraInfo from .messageTypes.sensor_msgs_Image import importTopic as import_sensor_msgs_Image from .messageTypes.sensor_msgs_Imu import importTopic as import_sensor_msgs_Imu -#from .messageTypes.sensor_msgs_PointCloud2 import importTopic as import_sensor_msgs_PointCloud2 -from .messageTypes.sensor_msgs_PointCloud2_testSBJ import importTopic as import_sensor_msgs_PointCloud2 +from .messageTypes.sensor_msgs_PointCloud2 import importTopic as import_sensor_msgs_PointCloud2 from .messageTypes.tf_tfMessage import importTopic as import_tf_tfMessage import logging diff --git a/importRosbag/messageTypes/sensor_msgs_PointCloud2.py b/importRosbag/messageTypes/sensor_msgs_PointCloud2.py index a3255bf..5f23b16 100644 --- a/importRosbag/messageTypes/sensor_msgs_PointCloud2.py +++ b/importRosbag/messageTypes/sensor_msgs_PointCloud2.py @@ -22,123 +22,135 @@ This function imports the ros message type defined at: http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html -For simplicity, we're currently directly unpacking the format that we are -encountering in the data, which is x,y,z,_,rgb,_,_,_ -each as 32-bit little-endian floats +In this new version of the pointcloud2 extract, we can automated unpacking +all the fields that are included in pointcloud2 message in the format that is required. """ -#%% +# %% from tqdm import tqdm import numpy as np import time -from .common import unpackRosString, unpackRosUint8, unpackRosUint32, \ - unpackRosTimestamp, unpackRosFloat32, unpackRosUint16 + +from .common import unpackRosString, unpackRosUint8, unpackRosUint32, unpackRosTimestamp, unpackRosFloat32, \ + unpackRosUint16, unpackStringData + def importTopic(msgs, **kwargs): ''' ros message is defined here: - http://docs.ros.org/api/geometry_msgs/html/msg/PoseStamped.html - the result is are np arrays of float64 for: - rotQ (4 cols, quaternion) - angV (3 cols) - acc (3 cols) - mag (3 cols) - temp (1 cols) - but I'll probably ignore this to start with + http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html + the result is a variable dimension dictionary with + - timestamp + - point structure (include all field that declares in pointcloud2 msg) ''' - #tempAll = np.zeros((sizeOfArray, 1), dtype=np.float64) - #for msg in tqdm(msgs, position=0, leave=True): disable_bar = kwargs.get('disable_bar') - tsByMessage = [] - pointsByMessage = [] - intensityByMessage = [] - timeByMessage = [] - reflectivityByMessage = [] - ringByMessage = [] - noiseByMessage = [] - rangesByMessage = [] + DTYPE_VAR = { + '1': '=i1', # INT8 + '2': '=u1', # UINT8 + '3': '=i2', # INT16 + '4': '=u2', # UINT16 + '5': '=i4', # INT32 + '6': '=u4', # UINT32 + '7': '=f4', # FLOAT32 + '8': '=f8', # FLOAT64 + } + dtype_header_1 = { + 'names': ['sequence_id', 'timestamp_seconds', 'timestamp_nseconds', # List of Strings + 'frame_len'], + 'formats': ['=u4','=u4','=u4','=u4'], # List of Strings + 'offsets': [0, 4, 8, 12], # List of INT + 'itemsize': 16 # INT + } + dtype_header_2 = { + 'names': ['height', 'width', 'fieldSize'], # List of Strings + 'formats': [ '=u4', '=u4', '=u4'], # List of Strings + 'offsets': [0, 4, 8], # List of INT + 'itemsize': 12 # INT + } + dtype_header_3 = { + 'names': ['is_Bigendian', 'PointStep', 'RowStep', 'data_size'], # List of Strings + 'formats': [ '=u1', '=u4', '=u4','=u4'], # List of Strings + 'offsets': [0, 1, 5, 9], # List of INT + 'itemsize': 13 # INT + } + dtype_fields = { + 'names': [], # List of Strings + 'formats': [], # List of Strings + 'offsets': [], # List of INT + 'itemsize': 0, # INT + 'count': [] # List of INT + } + + tsByMessage_2 = [] + pointsByMessage_2 = [] for msg in tqdm(msgs, disable=disable_bar): data = msg['data'] ptr = 0 - seq, ptr = unpackRosUint32(data, ptr) - ts, ptr = unpackRosTimestamp(data, ptr) - frame_id, ptr = unpackRosString(data, ptr) - height, ptr = unpackRosUint32(data, ptr) - width, ptr = unpackRosUint32(data, ptr) - + # Reading first header (with numpy) + header_1 = np.frombuffer(data[ptr:ptr + dtype_header_1['itemsize']], dtype=dtype_header_1) + timestamp = np.float64(header_1['timestamp_seconds'][0])+np.float64(header_1['timestamp_nseconds'][0])*0.000000001 + ptr += dtype_header_1['itemsize'] + # Unpacking frame_id with unknown dimension + frame_len = header_1['frame_len'][0].astype(int) + frame_id, ptr = unpackStringData(data, frame_len, ptr) + # Reading second header + header_2 = np.frombuffer(data[ptr:ptr + dtype_header_2['itemsize']], dtype=dtype_header_2) + ptr += dtype_header_2['itemsize'] + + #Assign variables + height = header_2['height'][0] + width = header_2['width'][0] + fieldSize = header_2['fieldSize'][0] + + #Reading fields of pointcloud2 if width > 0 and height > 0: - - arraySize, ptr = unpackRosUint32(data, ptr) - for element in range(arraySize): - # Move through the field definitions - we'll ignore these - # until we encounter a file that uses a different set - name, ptr = unpackRosString(data, ptr) - offset, ptr = unpackRosUint32(data, ptr) - datatype, ptr = unpackRosUint8(data, ptr) - count, ptr = unpackRosUint32(data, ptr) - - isBigendian, ptr = unpackRosUint8(data, ptr) - pointStep, ptr = unpackRosUint32(data, ptr) - rowStep, ptr = unpackRosUint32(data, ptr) - + for element in range(fieldSize): + names, ptr = unpackRosString(data, ptr) + offset = np.frombuffer(data[ptr:ptr+4], dtype='=u4') + ptr += 4 + datatype = np.frombuffer(data[ptr:ptr+1], dtype='=u1') + ptr += 1 + count = np.frombuffer(data[ptr:ptr + 4], dtype='=u4') + ptr += 4 + #APPEND TO THE DICTIONARY ('dtype_fields') + dtype_fields['names'].append(names) + dtype_fields['offsets'].append(offset[0]) + dtype_fields['formats'].append(str(DTYPE_VAR[str(datatype[0])])) + dtype_fields['count'].append(count[0]) + # Reading third header + header_3 = np.frombuffer(data[ptr:ptr + dtype_header_3['itemsize']], dtype=dtype_header_3) + ptr += dtype_header_3['itemsize'] + + #Assign variables + PointStep = header_3['PointStep'][0] + dtype_fields['itemsize'] = PointStep numPoints = width * height - points = np.empty((numPoints, 3), dtype=np.float32) - intensity = np.empty((numPoints, 1), dtype=np.float32) - time_arr = np.empty((numPoints,1), dtype=np.uint32) - reflectivity = np.empty((numPoints, 1), dtype=np.uint16) - ring = np.empty((numPoints, 1), dtype=np.uint8) - noise = np.empty((numPoints, 1), dtype=np.uint16) - ranges = np.empty((numPoints, 1), dtype=np.uint32) - - arraySize, ptr = unpackRosUint32(data, ptr) - # assert arraySize = width*height + + #Read pointcloud2 data with datatype obtained before + data_array = np.empty((numPoints, 1), dtype=dtype_fields) for x in range(width): - for y in range(height): - points[x*height+y, :] = np.frombuffer(data[ptr:ptr+12], dtype=np.float32) - intensity[x*height+y, 0] = np.frombuffer(data[ptr+16:ptr+20], dtype=np.float32) - #intensity, ptr_delete = unpackRosFloat32(data, ptr+12) - time_arr[x*height+y, 0] = np.frombuffer(data[ptr+20:ptr+24], dtype=np.uint32) - reflectivity[x*height+y, 0] = np.frombuffer(data[ptr+24:ptr+26], dtype=np.uint16) - ring[x*height+y, 0] = np.frombuffer(data[ptr+26:ptr+27], dtype=np.uint8) - noise[x*height+y, 0] = np.frombuffer(data[ptr+27:ptr+29], dtype=np.uint16) - ranges[x*height+y, 0] = np.frombuffer(data[ptr+29:ptr+33], dtype=np.uint32) - - ptr += pointStep - - pointsByMessage.append(points) - tsByMessage.append(np.ones(numPoints, dtype=np.float64) * ts) - intensityByMessage.append(intensity) - #intensityByMessage.append(np.ones(numPoints, dtype=np.float64) * intensity) - timeByMessage.append(time_arr) - reflectivityByMessage.append(reflectivity) - ringByMessage.append(ring) - noiseByMessage.append(noise) - rangesByMessage.append(ranges) - - if not pointsByMessage: # None of the messages contained any points + for y in range(height): + data_array[x * height + y, :] = np.frombuffer(data[ptr:ptr+dtype_fields['itemsize']], dtype=dtype_fields) + ptr += PointStep + + pointsByMessage_2.append(data_array) + tsByMessage_2.append(np.ones(numPoints, dtype=np.float64) * timestamp) #Meter aquí la traspuesta de la matriz??? + #Reset dtype_fields variables for the next msg + dtype_fields['names'] = [] + dtype_fields['offsets'] = [] + dtype_fields['formats'] = [] + dtype_fields['count'] = [] + + if not pointsByMessage_2: # None of the messages contained any points return None - points = np.concatenate(pointsByMessage) - ts = np.concatenate(tsByMessage) - intensity = np.concatenate(intensityByMessage) - time_internal = np.concatenate(timeByMessage) - reflectivity = np.concatenate(reflectivityByMessage) - ring = np.concatenate(ringByMessage) - noise = np.concatenate(noiseByMessage) - ranges = np.concatenate(rangesByMessage) - - - + data_array = np.concatenate(pointsByMessage_2) + timestamp = np.concatenate(tsByMessage_2) # Crop arrays to number of events outDict = { - 'ts': ts, - 'point': points, - 'intensity': intensity, - 'time': time_internal, - 'reflectivity': reflectivity, - 'ring': ring, - 'noise': noise, - 'ranges': ranges, + 'ts': timestamp, + 'point': data_array, } return outDict diff --git a/importRosbag/messageTypes/sensor_msgs_PointCloud2_testSBJ.py b/importRosbag/messageTypes/sensor_msgs_PointCloud2_testSBJ.py deleted file mode 100644 index 3f44b99..0000000 --- a/importRosbag/messageTypes/sensor_msgs_PointCloud2_testSBJ.py +++ /dev/null @@ -1,154 +0,0 @@ -# -*- coding: utf-8 -*- - -""" -Copyright (C) 2019 Event-driven Perception for Robotics -Authors: Sim Bamford -This program is free software: you can redistribute it and/or modify it under -the terms of the GNU General Public License as published by the Free Software -Foundation, either version 3 of the License, or (at your option) any later version. -This program is distributed in the hope that it will be useful, but WITHOUT ANY -WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A -PARTICULAR PURPOSE. See the GNU General Public License for more details. -You should have received a copy of the GNU General Public License along with -this program. If not, see . - -Intended as part of importRosbag. - -The importTopic function receives a list of messages and returns -a dict with one field for each data field in the message, where the field -will contain an appropriate iterable to contain the interpretted contents of each message. -In some cases, static info is repeated in each message; in which case a field may not contain an iterable. - -This function imports the ros message type defined at: -http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html - -In this new version of the pointcloud2 extract, we can automated unpacking -all the fields that are included in pointcloud2 message in the format that is required. -""" - -# %% - - -from tqdm import tqdm -import numpy as np -import time - -from .common import unpackRosString, unpackRosUint8, unpackRosUint32, unpackRosTimestamp, unpackRosFloat32, \ - unpackRosUint16, unpackStringData - - -def importTopic(msgs, **kwargs): - ''' - ros message is defined here: - http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html - the result is a dict variable with - - timestamp - - point structure (include all field that declares in pointcloud2 msg) - ''' - disable_bar = kwargs.get('disable_bar') - DTYPE_VAR = { - '1': '=i1', # INT8 - '2': '=u1', # UINT8 - '3': '=i2', # INT16 - '4': '=u2', # UINT16 - '5': '=i4', # INT32 - '6': '=u4', # UINT32 - '7': '=f4', # FLOAT32 - '8': '=f8', # FLOAT64 - } - dtype_header_1 = { - 'names': ['sequence_id', 'timestamp_seconds', 'timestamp_nseconds', # List of Strings - 'frame_len'], - 'formats': ['=u4','=u4','=u4','=u4'], # List of Strings - 'offsets': [0, 4, 8, 12], # List of INT - 'itemsize': 16 # INT - } - dtype_header_2 = { - 'names': ['height', 'width', 'fieldSize'], # List of Strings - 'formats': [ '=u4', '=u4', '=u4'], # List of Strings - 'offsets': [0, 4, 8], # List of INT - 'itemsize': 12 # INT - } - dtype_header_3 = { - 'names': ['is_Bigendian', 'PointStep', 'RowStep', 'data_size'], # List of Strings - 'formats': [ '=u1', '=u4', '=u4','=u4'], # List of Strings - 'offsets': [0, 1, 5, 9], # List of INT - 'itemsize': 13 # INT - } - dtype_fields = { - 'names': [], # List of Strings - 'formats': [], # List of Strings - 'offsets': [], # List of INT - 'itemsize': 0, # INT - 'count': [] # List of INT - } - - tsByMessage_2 = [] - pointsByMessage_2 = [] - - for msg in tqdm(msgs, disable=disable_bar): - data = msg['data'] - pointer = 0 - # Reading first header (numpy) - header_1 = np.frombuffer(data[pointer:pointer + dtype_header_1['itemsize']], dtype=dtype_header_1) - timestamp = np.float64(header_1['timestamp_seconds'][0])+np.float64(header_1['timestamp_nseconds'][0])*0.000000001 - pointer += dtype_header_1['itemsize'] - # Unpacking frame_id with unknown dimension - frame_len = header_1['frame_len'][0].astype(int) - frame_id, pointer = unpackStringData(data, frame_len, pointer) - # Reading second header (numpy) - header_2 = np.frombuffer(data[pointer:pointer + dtype_header_2['itemsize']], dtype=dtype_header_2) - pointer += dtype_header_2['itemsize'] - - #Assign variables - height = header_2['height'][0] - width = header_2['width'][0] - fieldSize = header_2['fieldSize'][0] - - #Reading fields of pointcloud2 - if width > 0 and height > 0: - for element in range(fieldSize): - names, pointer = unpackRosString(data, pointer) - offset = np.frombuffer(data[pointer:pointer+4], dtype='=u4') - pointer += 4 - datatype = np.frombuffer(data[pointer:pointer+1], dtype='=u1') - pointer += 1 - count = np.frombuffer(data[pointer:pointer + 4], dtype='=u4') - pointer += 4 - #APPEND TO THE DICTIONARY ('dtype_fields') - dtype_fields['names'].append(names) - dtype_fields['offsets'].append(offset[0]) - dtype_fields['formats'].append(str(DTYPE_VAR[str(datatype[0])])) - dtype_fields['count'].append(count[0]) - - header_3 = np.frombuffer(data[pointer:pointer + dtype_header_3['itemsize']], dtype=dtype_header_3) - pointer += dtype_header_3['itemsize'] - - PointStep = header_3['PointStep'][0] - dtype_fields['itemsize'] = PointStep - numPoints = width * height - - data_array = np.empty((numPoints, 1), dtype=dtype_fields) - for x in range(width): - for y in range(height): - data_array[x * height + y, :] = np.frombuffer(data[pointer:pointer+dtype_fields['itemsize']], dtype=dtype_fields) - pointer += PointStep - - pointsByMessage_2.append(data_array) - tsByMessage_2.append(np.ones(numPoints, dtype=np.float64) * timestamp) #Meter aquí la traspuesta de la matriz??? - #Reset dtype_fields variables for the next msg - dtype_fields['names'] = [] - dtype_fields['offsets'] = [] - dtype_fields['formats'] = [] - dtype_fields['count'] = [] - - if not pointsByMessage_2: # None of the messages contained any points - return None - data_array = np.concatenate(pointsByMessage_2) - timestamp = np.concatenate(tsByMessage_2) - # Crop arrays to number of events - outDict = { - 'ts': timestamp, - 'point': data_array, - } - return outDict