def point_cloud(self, points, parent_frame):
import sensor_msgs.msg as sensor_msgs
import std_msgs.msg as std_msgs
""" Creates a point cloud message.
Args:
points: Nx3 array of xyz positions.
parent_frame: frame in which the point cloud is defined
Returns:
sensor_msgs/PointCloud2 message
Code source:
https://gist.github.com/pgorczak/5c717baa44479fa064eb8d33ea4587e0
References:
http://docs.ros.org/melodic/api/sensor_msgs/html/msg/PointCloud2.html
http://docs.ros.org/melodic/api/sensor_msgs/html/msg/PointField.html
http://docs.ros.org/melodic/api/std_msgs/html/msg/Header.html
"""
# In a PointCloud2 message, the point cloud is stored as an byte
# array. In order to unpack it, we also include some parameters
# which desribes the size of each individual point.
if not len(points):
return sensor_msgs.PointCloud2()
ros_dtype = sensor_msgs.PointField.FLOAT32
dtype = np.float32
itemsize = np.dtype(dtype).itemsize # A 32-bit float takes 4 bytes.
data = points.astype(dtype).tobytes()
# The fields specify what the bytes represents. The first 4 bytes
# represents the x-coordinate, the next 4 the y-coordinate, etc.
fields = [
sensor_msgs.PointField(name=n,
offset=i * itemsize,
datatype=ros_dtype,
count=1) for i, n in enumerate('xyz')
]
# The PointCloud2 message also has a header which specifies which
# coordinate frame it is represented in.
header = std_msgs.Header(frame_id=parent_frame)
return sensor_msgs.PointCloud2(
header=header,
height=1,
width=points.shape[0],
is_dense=False,
is_bigendian=False,
fields=fields,
point_step=(itemsize *
3), # Every point consists of three float32s.
row_step=(itemsize * 3 * points.shape[0]),
data=data,
)
def makePointCloud2Msg(points, frame_time, parent_frame, pcd_format):
ros_dtype = sensor_msgs.PointField.FLOAT32
dtype = np.float32
itemsize = np.dtype(dtype).itemsize
data = points.astype(dtype).tobytes()
fields = [
sensor_msgs.PointField(name=n,
offset=i * itemsize,
datatype=ros_dtype,
count=1) for i, n in enumerate(pcd_format)
]
# header = std_msgs.Header(frame_id=parent_frame, stamp=rospy.Time.now())
header = std_msgs.Header(frame_id=parent_frame,
stamp=rospy.Time.from_sec(frame_time))
num_field = len(pcd_format)
return sensor_msgs.PointCloud2(header=header,
height=1,
width=points.shape[0],
is_dense=False,
is_bigendian=False,
fields=fields,
point_step=(itemsize * num_field),
row_step=(itemsize * num_field *
points.shape[0]),
data=data)
def to_PointCloud2(*args, **kwargs):
'''
converts the input to a sensor_msgs/PointCloud2 message
supported input:
- ROS PointCloud2 messages
- numpy structured array
NYI:
- ROS PointCloud messages
- iterable of ROS Point messages
- iterable of kdl Vectors
- iterable of iterables
- numpy array Nx3 or 3xN (if N == 3 then it is assumed Nx3)
keyword args:
- frame_id
- stamp
NOTE: mixing types is not supported
'''
if (len(args) > 0 and type(args[0]) == sensor_msgs.PointCloud2):
return copy.deepcopy(args[0])
pc2 = sensor_msgs.PointCloud2()
# assumed to be a numpy structured array
pts = args[0]
(pc2.fields, ptstep) = get_point_fields(pts.dtype)
if (len(pts.shape) == 1):
pc2.height = 1
pc2.width = pts.shape[0]
else:
pc2.height = pts.shape[0]
pc2.width = pts.shape[1]
pc2.is_bigendian = False
pc2.point_step = ptstep
pc2.row_step = ptstep * pc2.width
pc2.data = args[0].tostring()
pc2.is_dense = True
if ('frame_id' in kwargs.keys()):
pc2.header.frame_id = kwargs['frame_id']
if ('stamp' in kwargs.keys()):
pc2.header.stamp = kwargs['stamp']
return pc2
def _to_ros_msg(self):
val = self.value
msg = m.PointCloud2()
msg.header = self._get_header()
msg.height = val.height
msg.width = val.width
msg.fields = [
m.PointField(name='x', offset=0, datatype=7, count=1),
m.PointField(name='y', offset=4, datatype=7, count=1),
m.PointField(name='z', offset=8, datatype=7, count=1)
]
msg.is_bigendian = False
msg.point_step = 12
msg.row_step = val.width * msg.point_step
msg.is_dense = val.is_dense
msg.data = ''.join([struct.pack('fff', *p) for p in val.to_list()])
return msg
def publish_estimated_position(self,target,x,y):
'''
Publish the estimated target position using Particle Filter
'''
# make the 2D (xyzrgba) array
x = x.reshape([x.size,1])[::10]
y = y.reshape([y.size,1])[::10]
z = np.zeros(x.shape)
r = np.ones(x.shape)
g = np.zeros(x.shape)
b = np.zeros(x.shape)
a = np.zeros(x.shape)+0.3
points = np.concatenate((x,y,z,r,g,b,a),axis=1)
ros_dtype = sensor_msgs.PointField.FLOAT32
dtype = np.float32
itemsize = np.dtype(dtype).itemsize
data = points.astype(dtype).tobytes()
fields = [sensor_msgs.PointField(name=n, offset=i*itemsize, datatype=ros_dtype, count=1) for i, n in enumerate('xyzrgba')]
header = std_msgs.Header(frame_id='world_ned',stamp=rospy.Time.now())
pc2 = sensor_msgs.PointCloud2(
header=header,
height=1,
width=points.shape[0],
is_dense=False,
is_bigendian=False,
fields=fields,
point_step=(itemsize*7),
row_step=(itemsize*7*points.shape[0]),
data=data)
#Publish the particles positions as a point cloud
self.point_cloud.publish(pc2)
#Publish the estimated target posiiton
# Create the point message
point = PointStamped()
point.header.stamp = rospy.Time.now()
point.header.frame_id = "world_ned"
point.point.x = target.pf._x[0]
point.point.y = target.pf._x[2]
self.p_estimated_target_position.publish(point)
return
def point_cloud(points, parent_frame, has_rgb=False):
""" Creates a point cloud message.
Args:
points: Nx7 array of xyz positions (m) and rgba colors (0..1)
parent_frame: frame in which the point cloud is defined
Returns:
sensor_msgs/PointCloud2 message
"""
ros_dtype = sensor_msgs.PointField.FLOAT32
dtype = np.float32
itemsize = np.dtype(dtype).itemsize
data = points.astype(dtype).tobytes()
channels = 'xyzrgb'
steps = 6
pts_num = points.shape[0]
if not has_rgb:
channels = "xyz"
steps = 3
pts_num = points.shape[0]
fields = [
sensor_msgs.PointField(name=n,
offset=i * itemsize,
datatype=ros_dtype,
count=1) for i, n in enumerate(channels)
]
header = std_msgs.Header(frame_id=parent_frame, stamp=rospy.Time.now())
# print(f"datasize {len(points.astype(dtype).tobytes())} pts_num {pts_num} point_step {itemsize * steps}")
return sensor_msgs.PointCloud2(header=header,
height=1,
width=pts_num,
is_dense=False,
is_bigendian=False,
fields=fields,
point_step=(itemsize * steps),
row_step=(itemsize * steps * pts_num),
data=data)
def point_cloud(points, parent_frame):
""" Creates a point cloud message.
Args:
points: Nx7 array of xyz positions (m) and rgba colors (0..1)
parent_frame: frame in which the point cloud is defined
Returns:
sensor_msgs/PointCloud2 message
"""
ros_dtype = sensor_msgs.PointField.FLOAT32
dtype = np.float32
itemsize = np.dtype(dtype).itemsize
data = points.astype(dtype).tobytes()
fields = [
sensor_msgs.PointField('x', 0, ros_dtype, 1),
sensor_msgs.PointField('y', itemsize, ros_dtype, 1),
sensor_msgs.PointField('z', 2*itemsize, ros_dtype, 1),
sensor_msgs.PointField('intensity', 3*itemsize, ros_dtype, 1),
sensor_msgs.PointField('velocity', 4*itemsize, ros_dtype, 1)]
header = std_msgs.Header(frame_id=parent_frame, stamp=rospy.Time.now())
return sensor_msgs.PointCloud2(
header=header,
height=1,
width=points.shape[0],#number of points in the frame
is_dense=False,
is_bigendian=False,
fields=fields,
point_step=(itemsize * 5),
#point_step=32,
row_step=(itemsize * 5 * points.shape[0]),
#row_step=32*points.shape[0],
data=data
)
def xyzrgb2pc(xyz, bgr, frame_id):
xyz = np.asarray(xyz)
bgr = np.asarray(bgr)
assert xyz.shape == bgr.shape
if xyz.ndim == 2:
xyz = xyz[None, :, :]
bgr = bgr[None, :, :]
height = xyz.shape[0]
width = xyz.shape[1]
arr = np.empty((height, width, 8), dtype='float32')
arr[:, :, 0:3] = xyz
bgr1 = np.empty((height, width, 4), dtype='uint8')
bgr1[:, :, 0:3] = bgr
arr[:, :, 4] = bgr1.view(dtype='float32').reshape(height, width)
data = arr.tostring()
msg = sm.PointCloud2()
msg.data = data
msg.header.frame_id = frame_id
msg.fields = [
sm.PointField(name='x', offset=0, datatype=7, count=1),
sm.PointField(name='y', offset=4, datatype=7, count=1),
sm.PointField(name='z', offset=8, datatype=7, count=1),
sm.PointField(name='rgb', offset=16, datatype=7, count=1)
]
msg.is_dense = False
msg.width = width
msg.height = height
msg.header.stamp = rospy.Time.now()
msg.point_step = 32
msg.row_step = 32 * width
msg.is_bigendian = False
return msg
def xyzrgb2pc(xyz,bgr, frame_id):
arr = np.empty((480,640,8),dtype='float32')
arr[:,:,0:3] = xyz
bgr1 = np.empty((480,640,4),dtype='uint8')
bgr1[:,:,0:3] = bgr
arr[:,:,4] = bgr1.view(dtype='float32').reshape(480,640)
data = arr.tostring()
msg = sm.PointCloud2()
msg.data = data
msg.header.frame_id = frame_id
msg.header.stamp = rospy.Time.now()
msg.fields = [sm.PointField(name='x',offset=0,datatype=7,count=1),
sm.PointField(name='y',offset=4,datatype=7,count=1),
sm.PointField(name='z',offset=8,datatype=7,count=1),
sm.PointField(name='rgb',offset=16,datatype=7,count=1)]
msg.is_dense = False
msg.width=640
msg.height=480
msg.header.stamp = rospy.Time.now()
msg.point_step = 32
msg.row_step = 20480
msg.is_bigendian = False
return msg
def to_PointCloud2XYZ(*args, **kwargs):
'''
converts the input to a sensor_msgs/PointCloud2 message with fields x,y,z
supported input:
- numpy array Nx3 or 3xN (if N == 3 then it is assumed Nx3)
NYI:
- ROS PointCloud messages
- iterable of ROS Point messages
- iterable of kdl Vectors
- iterable of iterables
- numpy array Nx3 or 3xN (if N == 3 then it is assumed Nx3)
- numpy array Nx3 or 3xN (if N == 3 then it is assumed Nx3)
keyword args:
- frame_id
- stamp
NOTE: mixing types is not supported
'''
pc2 = sensor_msgs.PointCloud2()
# assumed to be a numpy array
pts = args[0]
if (type(pts) == np.ndarray):
# 3xN (N > 3)
if (max(pts.shape) > 3 and pts.shape[1] > pts.shape[0]):
pts = pts.T
# Nx3 or 3xN (N < 3)
elif (pts.shape != (3, 3) and pts.shape[0] == 3):
pts = pts.T
# pts should be Nx3 now
pc2.height = 1
pc2.width = pts.shape[0]
# format fields
pc2dtype = NUMPY_2_PC2_DATATYPE[pts.dtype.name]
offset = pts.dtype.itemsize
pc2.fields = [
sensor_msgs.PointField('x', 0, pc2dtype, 1),
sensor_msgs.PointField('y', offset, pc2dtype, 1),
sensor_msgs.PointField('z', 2 * offset, pc2dtype, 1)
]
pc2.is_bigendian = False
pc2.point_step = 3 * offset
pc2.row_step = pc2.width * pc2.point_step
pc2.data = pts.tostring()
pc2.is_dense = True
if ('frame_id' in kwargs.keys()):
pc2.header.frame_id = kwargs['frame_id']
if ('stamp' in kwargs.keys()):
pc2.header.stamp = kwargs['stamp']
return pc2