def __init__(self):
print("Initalized")
self.Yl = 21
self.Cbl = 77
self.Crl = 83
self.Yh = 88
self.Cbh = 150
self.Crh = 146
self.clr = None
self.depth = None
self.cnt = 0
self.bridge = CvBridge()
self.h = std_msgs.msg.Header()
self.h.stamp = rospy.Time.now()
self.h.frame_id = "map"
self.pub = rospy.Publisher('pcl', PointCloud2, queue_size=10)
self.ptcld = None
self.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1)
]
def xyz_array_to_pointcloud2(self, points, limit):
'''
Create a sensor_msgs.PointCloud2 from an array of points.
'''
if limit > 0: points = points[-limit:]
msg = PointCloud2()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "base_link"
if len(points.shape) == 3:
msg.height = points.shape[1]
msg.width = points.shape[0]
else:
msg.height = 1
msg.width = len(points)
msg.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1)
]
msg.is_bigendian = False
msg.point_step = 12
msg.row_step = 12 * points.shape[0]
msg.is_dense = int(np.isfinite(points).all())
msg.data = np.asarray(points, np.float32).tostring()
return msg
def xyz_array_to_pointcloud2(points, stamp=None, frame_id=None):
'''
Create a sensor_msgs.PointCloud2 from an array
of points.
'''
msg = PointCloud2()
if stamp:
msg.header.stamp = stamp
if frame_id:
msg.header.frame_id = frame_id
if len(points.shape) == 3:
msg.height = points.shape[1]
msg.width = points.shape[0]
else:
msg.height = 1
msg.width = len(points)
msg.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1)
]
msg.is_bigendian = False
msg.point_step = 12
msg.row_step = 12 * points.shape[0]
msg.is_dense = int(np.isfinite(points).all())
msg.data = np.asarray(points, np.float32).tostring()
return msg
def pub_cube(self, lim=8):
points = []
for i in range(lim):
for j in range(lim):
for k in range(lim):
x = float(i) / lim
y = float(j) / lim
z = float(k) / lim
pt = [x, y, z, 0]
r = int(x * 255.0)
g = int(y * 255.0)
b = int(z * 255.0)
a = 255
# print r, g, b, a
rgb = struct.unpack('I', struct.pack('BBBB', b, g, r,
a))[0]
# print hex(rgb)
pt[3] = rgb
points.append(pt)
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('rgb', 12, PointField.UINT32, 1),
# PointField('rgba', 12, PointField.UINT32, 1),
]
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = "map"
pc2 = point_cloud2.create_cloud(header, fields, points)
pc2.header.stamp = rospy.Time.now()
self.pub.publish(pc2)
def default(self, ci='unused'):
if not self.component_instance.capturing:
return # press [Space] key to enable capturing
# This message holds a collection of N-dimensional points, which may
# contain additional information such as normals, intensity, etc. The
# point data is stored as a binary blob, its layout described by the
# contents of the "fields" array.
# The point cloud data may be organized 2d (image-like) or 1d
# (unordered). Point clouds organized as 2d images may be produced by
# camera depth sensors such as stereo or time-of-flight.
pc2 = PointCloud2()
# Time of sensor data acquisition, and the coordinate frame ID (for 3d
# points).
pc2.header = self.get_ros_header()
# 2D structure of the point cloud. If the cloud is unordered, height is
# 1 and width is the length of the point cloud.
pc2.height = 1
pc2.width = self.data['nb_points']
# Describes the channels and their layout in the binary data blob.
pc2.fields = [PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1)]
pc2.is_dense = True # True if there are no invalid points
pc2.is_bigendian = False # Is this data bigendian?
pc2.point_step = 12 # Length of a point in bytes
pc2.row_step = len(self.data['points']) # Length of a row in bytes
# Actual point data, size is (row_step*height)
# memoryview from PyMemoryView_FromMemory() implements the buffer interface
pc2.data = bytes(self.data['points'])
self.publish_with_robot_transform(pc2)
def sensor_data_updated(self, carla_lidar_measurement):
"""
Function to transform the a received lidar measurement into a ROS point cloud message
:param carla_lidar_measurement: carla lidar measurement object
:type carla_lidar_measurement: carla.LidarMeasurement
"""
header = self.get_msg_header()
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('intensity', 12, PointField.FLOAT32, 1),
]
lidar_data = numpy.fromstring(carla_lidar_measurement.raw_data,
dtype=numpy.float32)
lidar_data = numpy.reshape(lidar_data,
(int(lidar_data.shape[0] / 4), 4))
# we take the oposite of y axis
# (as lidar point are express in left handed coordinate system, and ros need right handed)
lidar_data[:, 1] *= -1
point_cloud_msg = create_cloud(header, fields, lidar_data)
self.publish_message(self.get_topic_prefix() + "/point_cloud",
point_cloud_msg)
def default(self, ci='unused'):
if not self.component_instance.capturing:
return # press [Space] key to enable capturing
points = self.data['points']
size = len(points)
pc2 = PointCloud2()
pc2.header = self.get_ros_header()
pc2.height = 1
pc2.width = int(size / 12)
pc2.is_dense = False
pc2.is_bigendian = False
pc2.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1)
]
pc2.point_step = 12
pc2.row_step = size
# memoryview from PyMemoryView_FromMemory() implements the buffer interface
pc2.data = bytes(points)
self.publish(pc2)
self.send_transform_robot()
def _publish_pointcloud(self, carla_lidar_measurement):
"""
Function to transform the a received lidar measurement into a ROS point cloud message
:param carla_lidar_measurement: carla lidar measurement object
:type carla_lidar_measurement: carla.LidarMeasurement
"""
header = Header()
set_timestamp(header, carla_lidar_measurement.timestamp)
header.frame_id = self._lidar_frame
fields = [
PointField("x", 0, PointField.FLOAT32, 1),
PointField("y", 4, PointField.FLOAT32, 1),
PointField("z", 8, PointField.FLOAT32, 1),
PointField("intensity", 12, PointField.FLOAT32, 1),
]
lidar_data = (
np.frombuffer(carla_lidar_measurement.raw_data, dtype=np.float32)
.reshape([-1, 4])
.copy()
)
# we take the oposite of y axis
# (as lidar point are express in left handed coordinate system, and ros need right handed)
lidar_data[:, 1] *= -1
point_cloud_msg = create_cloud(header, fields, lidar_data)
self.publish("lidar", point_cloud_msg)
def points_to_pointcloud2(points, frame_id="base_radar_link"):
'''
Create a sensor_msgs.PointCloud2 from an array
of points.
'''
msg = PointCloud2()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = frame_id
if len(points.shape) == 3:
msg.height = points.shape[1]
msg.width = points.shape[0]
else:
msg.height = 1
msg.width = points.shape[0]
msg.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('intensity', 16, PointField.FLOAT32, 1)]
msg.is_bigendian = False
msg.point_step = 32
msg.row_step = 32*points.shape[0]
msg.is_dense = int(np.isfinite(points).all())
msg.data = points.astype(np.float32).tobytes()
return msg
def __init__(self):
# Save the point cloud in the listener callback
self.received_ros_cloud = None
self.open3d_pointcloud = None
self.time_stamps = list() # save all the time stamp
self.position = list()
rospy.init_node('liver_pose', anonymous=True)
self.pub = rospy.Publisher('pose', Pose, queue_size=1)
#for publishing static point cloud
self.pub_point = rospy.Publisher('static_pointcloud',
PointCloud2,
queue_size=1)
self.FIELDS_XYZ = [
PointField(name='x',
offset=0,
datatype=PointField.FLOAT32,
count=1),
PointField(name='y',
offset=4,
datatype=PointField.FLOAT32,
count=1),
PointField(name='z',
offset=8,
datatype=PointField.FLOAT32,
count=1),
]
self.header = Header()
self.header.frame_id = "camera_link"
self.pos = np.zeros((3, 1))
def xyz_array_to_point_cloud_msg(points, timestamp=None):
"""
Please refer to this ros answer about the usage of point cloud message:
https://answers.ros.org/question/234455/pointcloud2-and-pointfield/
:param points:
:param header:
:return:
"""
header = Header()
header.frame_id = 'map'
if timestamp is None:
timestamp = rospy.Time().now()
header.stamp = timestamp
msg = PointCloud2()
msg.header = header
msg.width = points.shape[0]
msg.height = points.shape[1]
msg.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
]
msg.is_bigendian = False
msg.point_step = 12
msg.row_step = msg.point_step * msg.width
# organized clouds are non-dense, since we have to use std::numeric_limits<float>::quiet_NaN()
# to fill all x/y/z value; un-organized clouds are dense, since we can filter out unfavored ones
msg.is_dense = False
xyz = points.astype(np.float32)
msg.data = xyz.tostring()
return msg
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 save_velo_data(bag, kitti, velo_frame_id, topic):
print("Exporting velodyne data")
velo_path = os.path.join(kitti.data_path, 'velodyne_points')
velo_data_dir = os.path.join(velo_path, 'data')
velo_filenames = sorted(os.listdir(velo_data_dir))
with open(os.path.join(velo_path, 'timestamps.txt')) as f:
lines = f.readlines()
velo_datetimes = []
for line in lines:
if len(line) == 1:
continue
dt = datetime.strptime(line[:-4], '%Y-%m-%d %H:%M:%S.%f')
velo_datetimes.append(dt)
iterable = zip(velo_datetimes, velo_filenames)
bar = progressbar.ProgressBar()
count = 0
for dt, filename in bar(iterable):
if dt is None:
continue
velo_filename = os.path.join(velo_data_dir, filename)
# read binary data
scan = (np.fromfile(velo_filename, dtype=np.float32)).reshape(-1, 4)
# get ring channel
depth = np.linalg.norm(scan, 2, axis=1)
pitch = np.arcsin(scan[:, 2] / depth) # arcsin(z, depth)
fov_down = -24.8 / 180.0 * np.pi
fov = (abs(-24.8) + abs(2.0)) / 180.0 * np.pi
proj_y = (pitch + abs(fov_down)) / fov # in [0.0, 1.0]
proj_y *= 64 # in [0.0, H]
proj_y = np.floor(proj_y)
proj_y = np.minimum(64 - 1, proj_y)
proj_y = np.maximum(0, proj_y).astype(np.int32) # in [0,H-1]
proj_y = proj_y.reshape(-1, 1)
scan = np.concatenate((scan, proj_y), axis=1)
# create header
header = Header()
header.frame_id = velo_frame_id
header.stamp = rospy.Time.from_sec(
float(datetime.strftime(dt, "%s.%f")))
# fill pcl msg
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('intensity', 12, PointField.FLOAT32, 1),
PointField('ring', 16, PointField.UINT16, 1)
]
pcl_msg = pcl2.create_cloud(header, fields, scan)
pcl_msg.is_dense = True
# print(pcl_msg)
bag.write(topic, pcl_msg, t=pcl_msg.header.stamp)
def generate_pointcloud2_data(self, x, y, z):
points = []
lim = 8
pointN = len(x)
for i in range(pointN):
px = x[i]
py = y[i]
pz = z[i]
r = 255
g = 165
b = 0
a = 255
rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0]
pt = [px, py, pz, rgb]
points.append(pt)
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
# PointField('rgb', 12, PointField.UINT32, 1),
PointField('rgba', 12, PointField.UINT32, 1),
]
header = Header()
header.frame_id = "world"
pc2 = point_cloud2.create_cloud(header, fields, points)
return pc2
def callback(message):
nmeasures = len(message.ranges)
rospy.loginfo('I received "' + Fore.RED + str(nmeasures) + Fore.RESET +
' measurements')
# Create point list
points = []
i = 0
for range in message.ranges:
theta = message.angle_min + i * message.angle_increment
x = range * cos(theta)
y = range * sin(theta)
z = 0
pt = [x, y, z]
points.append(pt)
i = +1
# Message config
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1)
]
header = Header()
header.frame_id = message.header.frame_id
pc2 = point_cloud2.create_cloud(header, fields, points)
pub.publish(pc2)
def _callback(self, data):
"""The _callback method parses images, projects it into pseudo-LiDAR point
clouds and publishes it.
Args:
data (ros imgmsg message): contains input image
Returns: None
"""
depth_img = self.br.imgmsg_to_cv2(data)
cloud = self.PL.project_PL(depth_img)
# convert cloud to PCL2 msg
dt = datetime.now()
# create header
header = Header()
header.frame_id = 'velodyne'
header.stamp = rospy.Time.from_sec(
float(datetime.strftime(dt, "%s.%f")))
# fill pcl msg
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('i', 12, PointField.FLOAT32, 1)
]
# create pcl2 cloud
pcl_msg = _pcl2.create_cloud(header, fields, cloud)
self._publish(pcl_msg)
def gen_pcl(num_points, scale, spin):
points = []
for i in range(num_points):
#progress = i / num_points
#val = progress * scale
#sv = math.sin(val + spin)
#cv = math.cos(val + spin)
x = (random.random() - 0.5)
y = (random.random() - 0.5)
z = random.random()
points.append([x, y, z, 8.0, 1.0])
return point_cloud2.create_cloud(
header=Header(
stamp=rospy.Time.now(),
frame_id="map",
),
fields=[
PointField("x", 0, PointField.FLOAT32, 1),
PointField("y", 4, PointField.FLOAT32, 1),
PointField("z", 8, PointField.FLOAT32, 1),
PointField("intensity", 16, PointField.FLOAT32, 1),
PointField("ring", 20, PointField.FLOAT32, 1),
],
points=points,
)
"""
def img_cb(self, rgb_data, depth_data):
cv_image = self.bridge.imgmsg_to_cv2(rgb_data, "bgr8")
print cv_image.shape
cv_depthimage = self.bridge.imgmsg_to_cv2(depth_data, "32FC1")
cv_depthimage2 = np.array(cv_depthimage, dtype=np.float32)
print cv_depthimage2.shape
for x in range(cv_depthimage2.shape[0]):
for y in range(cv_depthimage2.shape[1]):
zc = cv_depthimage2[x][y]
#print zc
position = self.getXYZ(y, x, zc*0.001000)
#print position
b = cv_image[x][y][0]
g = cv_image[x][y][1]
r = cv_image[x][y][2]
a = 255
rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0]
pt = [position[0],position[1],position[2],rgb]
self.points.append(pt)
header = Header()
header.stamp = rospy.Time.now()
header.frame_id ="camera_color_optical_frame"
#header.frame_id = "X1/rgbd_camera_frame_optical"
fields = [PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1), PointField('rgb', 12, PointField.UINT32, 1)]
pointcloud = point_cloud2.create_cloud(header, fields, self.points)
self.pc_pub.publish(pointcloud)
self.points = []
def pointcloud2withrgb(self, points, colors, stamp, frame_id, seq):
points_array = []
for i in range(points.shape[0]):
x, y, z = points[i]
color = colors[i] * 255
color = color.astype(int)
hex_r = (0xff & color[0]) << 16
hex_g = (0xff & color[1]) << 8
hex_b = (0xff & color[2])
hex_rgb = hex_r | hex_g | hex_b
float_rgb = struct.unpack('f', struct.pack('i', hex_rgb))[0]
# r, g, b = color.astype(int)
# a = 0
# rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0]
pt = [x, y, z, float_rgb]
points_array.append(pt)
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('rgb', 12, PointField.FLOAT32, 1),
]
header = Header()
header.frame_id = frame_id
msg = pc2.create_cloud(header, fields, points_array)
msg.header.stamp = stamp
return msg
def create_cloud_xyzil32(self, header, points):
fields = [PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('intensity', 12, PointField.FLOAT32, 1),
PointField('label', 16, PointField.FLOAT32, 1)]
return pc2.create_cloud(header, fields, points)
def pcl_to_ros(pcl_array):
ros_msg = PointCloud2()
ros_msg.header.stamp = rospy.Time.now()
ros_msg.header.frame_id = "pandar"
ros_msg.height = 1
ros_msg.width = pcl_array.size
ros_msg.fields.append(
PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1))
ros_msg.fields.append(
PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1))
ros_msg.fields.append(
PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1))
ros_msg.fields.append(
PointField(name="i", offset=12, datatype=PointField.FLOAT32, count=1))
ros_msg.is_bigendian = False
ros_msg.point_step = 12
ros_msg.row_step = ros_msg.point_step * ros_msg.width * ros_msg.height
ros_msg.is_dense = False
buffer = []
for data in pcl_array:
float_rgb = struct.unpack('f', struct.pack('i', 0xff0000))[0]
#print(data[3])
try:
buffer.append(struct.pack('fff', data[0], data[1], data[2]))
except Exception as e:
print(e)
else:
pass
ros_msg.data = "".join(buffer)
return ros_msg
def setUp(self):
self.point_cloud_in = point_cloud2.PointCloud2()
self.point_cloud_in.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1)
]
self.point_cloud_in.point_step = 4 * 3
self.point_cloud_in.height = 1
# we add two points (with x, y, z to the cloud)
self.point_cloud_in.width = 2
self.point_cloud_in.row_step = self.point_cloud_in.point_step * self.point_cloud_in.width
points = [1, 2, 0, 10, 20, 30]
self.point_cloud_in.data = struct.pack('%sf' % len(points), *points)
self.transform_translate_xyz_300 = TransformStamped()
self.transform_translate_xyz_300.transform.translation.x = 300
self.transform_translate_xyz_300.transform.translation.y = 300
self.transform_translate_xyz_300.transform.translation.z = 300
self.transform_translate_xyz_300.transform.rotation.w = 1 # no rotation so we only set w
assert (list(point_cloud2.read_points(self.point_cloud_in)) == [
(1.0, 2.0, 0.0), (10.0, 20.0, 30.0)
])
def sensor_data_updated(self, carla_dvs_event_array):
"""
Function to transform the received DVS event array into a ROS message
:param carla_dvs_event_array: dvs event array object
:type carla_image: carla.DVSEventArray
"""
super(DVSCamera, self).sensor_data_updated(carla_dvs_event_array)
header = self.get_msg_header(timestamp=carla_dvs_event_array.timestamp)
fields = [
PointField(name='x', offset=0, datatype=PointField.UINT16,
count=1),
PointField(name='y', offset=2, datatype=PointField.UINT16,
count=1),
PointField(name='t',
offset=4,
datatype=PointField.FLOAT64,
count=1),
PointField(name='pol',
offset=12,
datatype=PointField.INT8,
count=1)
]
dvs_events_msg = create_cloud(header, fields,
self._dvs_events.tolist())
self.dvs_camera_publisher.publish(dvs_events_msg)
def setUp(self):
self.point_cloud_in = point_cloud2.PointCloud2()
self.point_cloud_in.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('index', 12, PointField.INT32, 1)
]
self.point_cloud_in.point_step = 4 * 4
self.point_cloud_in.height = 1
# we add two points (with x, y, z to the cloud)
self.point_cloud_in.width = 2
self.point_cloud_in.row_step = self.point_cloud_in.point_step * self.point_cloud_in.width
self.points = [(1.0, 2.0, 0.0, 123), (10.0, 20.0, 30.0, 456)]
for point in self.points:
self.point_cloud_in.data += struct.pack('3fi', *point)
self.transform_translate_xyz_300 = TransformStamped()
self.transform_translate_xyz_300.transform.translation.x = self.TRANSFORM_OFFSET_DISTANCE
self.transform_translate_xyz_300.transform.translation.y = self.TRANSFORM_OFFSET_DISTANCE
self.transform_translate_xyz_300.transform.translation.z = self.TRANSFORM_OFFSET_DISTANCE
self.transform_translate_xyz_300.transform.rotation.w = 1 # no rotation so we only set w
assert (list(point_cloud2.read_points(
self.point_cloud_in)) == self.points)
def pub_data(self, points, intensity, data_type='map'):
#msg = self.surface_to_pcl(data,.1)
msg = PointCloud2()
buf = []
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'map'
msg.height = points.shape[0]
msg.width = 1
msg.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('intensity', 12, PointField.FLOAT32, 1)
]
msg.is_bigendian = False
msg.point_step = 16
msg.row_step = 16 * msg.height
msg.is_dense = True
if (intensity is None):
points = np.hstack([points, np.zeros([points.shape[0], 1])])
else:
points = np.hstack([points, intensity.reshape([-1, 1])])
msg.data = np.asarray(points, np.float32).tostring()
if (data_type == 'map'):
self.map_pub.publish(msg)
else:
self.raw_pub.publish(msg)
def talker(pc_path):
pub = rospy.Publisher('/point_cloud', PointCloud2, queue_size=10)
rospy.init_node('pc_publisher', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
pc = np.load(pc_path)+0.1
points = pc.reshape(-1,3)
points[:,1] = points[:,1]+0.1
msg = PointCloud2()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "base"
msg.height = 1
msg.width = points.shape[0]
msg.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1)
]
msg.is_bigendian = False
msg.point_step = 12
msg.row_step = msg.point_step * points.shape[0]
msg.is_dense = False
msg.data = points.astype(np.float32).tobytes()
pub.publish(msg)
print('send')
rate.sleep()
def create_cloud_xyz32(header, points):
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1)
]
return create_cloud(header, fields, points)
def __publish_point_cloud_data(self, stamp):
data = self._wb_device.getPointCloud(data_type='buffer')
if data:
msg = PointCloud2()
msg.header.stamp = stamp
msg.header.frame_id = self._frame_id
msg.height = 1
msg.width = self._wb_device.getNumberOfPoints()
msg.point_step = 20
msg.row_step = 20 * self._wb_device.getNumberOfPoints()
msg.is_dense = False
msg.fields = [
PointField(name='x',
offset=0,
datatype=PointField.FLOAT32,
count=1),
PointField(name='y',
offset=4,
datatype=PointField.FLOAT32,
count=1),
PointField(name='z',
offset=8,
datatype=PointField.FLOAT32,
count=1)
]
msg.is_bigendian = False
# We pass `data` directly to we avoid using `data` setter.
# Otherwise ROS2 converts data to `array.array` which slows down the simulation as it copies memory internally.
# Both, `bytearray` and `array.array`, implement Python buffer protocol, so we should not see unpredictable
# behavior.
# deepcode ignore W0212: Avoid conversion from `bytearray` to `array.array`.
msg._data = data
self.__publisher.publish(msg)
def publish_point_cloud(particle_publisher, cloud_points, frame_id, stamp):
header = Header()
header.stamp = stamp
header.frame_id = frame_id
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1)
]
if cloud_points.shape[1] == 4:
fields.append(PointField('intensity', 8, PointField.FLOAT32, 1))
buff = np.ascontiguousarray(cloud_points,
dtype=np.float32).ravel().tobytes()
cloud = PointCloud2(header=header,
height=1,
width=cloud_points.shape[0],
is_dense=False,
is_bigendian=False,
fields=fields,
point_step=len(fields) * 4,
row_step=len(fields) * 4 * cloud_points.shape[1],
data=buff)
particle_publisher.publish(cloud)
def get_pointcloud(self):
point_data = []
for i, point3d in enumerate(self.est_points3d):
# Create the point cloud data
a = 255
rgb = struct.unpack(
'I',
struct.pack('BBBB', self.color3d[i][2], self.color3d[i][1],
self.color3d[i][0], a))[0]
pt = [point3d[0], point3d[1], point3d[2], rgb]
point_data.append(pt)
# Define the point cloud metadata
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('rgba', 12, PointField.UINT32, 1),
]
# Create the message and return
header = Header()
header.frame_id = "map"
pc2 = point_cloud2.create_cloud(header, fields, point_data)
self.est_points3d = np.array(self.est_points3d)
return pc2
