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
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'
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
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
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 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
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
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
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
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
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
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
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()
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)
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
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
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
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)
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
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)
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)
# 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()
# 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()
