def add_ros_cloud_template(self):
# XYZ RGB point cloud2
self.rgb_cloud = PointCloud2()
self.rgb_cloud.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)
]
self.rgb_cloud.point_step = 16
self.rgb_cloud.is_bigendian = False
self.rgb_cloud.height = 1
self.rgb_cloud.is_dense = True
# XYZ point cloud 2
self.cloud = PointCloud2()
self.cloud.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1)
]
self.cloud.point_step = 12
self.cloud.is_bigendian = False
self.cloud.height = 1
self.cloud.is_dense = True
def objDetection(req):
print('[Object Detection] Receive object name: ' + req.obj_name)
rep = ObjDetectionWithNameResponse()
rep.input_pc = PointCloud2()
pt1 = Point()
pt1.x = 0
pt1.y = 0
pt1.z = 0
pt2 = Point()
pt2.x = 50
pt2.y = 50
pt2.z = 0
rep.bbox_corner1 = pt1
rep.bbox_corner2 = pt2
pc = PointCloud2()
pc.height = 480
pc.width = 640
rep.input_pc = pc
rospy.sleep(1)
print('[Object Detection] Return pointcloud and bounding boxes:')
print(' pointcloud = (width, height) = ({}, {})'.format(
rep.input_pc.width, rep.input_pc.height))
print(' bbox_corner1 = (x, y, z) = ({}, {}, {})'.format(
rep.bbox_corner1.x, rep.bbox_corner1.y, rep.bbox_corner1.z))
print(' bbox_corner2 = (x, y, z) = ({}, {}, {})'.format(
rep.bbox_corner2.x, rep.bbox_corner2.y, rep.bbox_corner2.z))
return rep
def load_visible_vg(self, filename):
if self.scene.scene_type == SceneType.LIVE:
# raise RuntimeError("Live scene not updated after multiobject refactor")
try:
pt_msg = PointCloud2()
with (self.scene.get_save_path() / filename).open('rb') as f:
pt_msg.deserialize(f.read())
if len(self.robot_views) != 1:
raise RuntimeError(
"Live scene with multiple objects, yet single latest segmented point found"
)
self.robot_views[0].voxelize_visible_element(pt_msg)
except FileNotFoundError as e:
print(
"Single latest segmeented points not found. You are probably doing this after the refactor"
)
for i in range(self.scene.num_objects):
filename = f"{self.scene.name}_latest_segmented_pts_{i}.msg"
pt_msg = PointCloud2()
with (self.scene.get_save_path() / filename).open('rb') as f:
pt_msg.deserialize(f.read())
self.robot_views[i].voxelize_visible_element(pt_msg)
# self.robot_views[i].get_visible_element(save_file=save_path)
elif self.scene.scene_type == SceneType.SIMULATION:
raise RuntimeError(
"What are you doing loading the visible_vg from a simulation scene?"
)
else:
raise RuntimeError("Unknown scene type")
def array_to_pointcloud2(cloud_arr,
stamp=None,
frame_id=None,
merge_rgb=False):
'''Converts a numpy record array to a sensor_msgs.msg.PointCloud2.
'''
if merge_rgb:
cloud_arr = merge_rgb_fields(cloud_arr)
# make it 2d (even if height will be 1)
cloud_arr = np.atleast_2d(cloud_arr)
cloud_msg = PointCloud2()
if stamp is not None:
cloud_msg.header.stamp = stamp
if frame_id is not None:
cloud_msg.header.frame_id = frame_id
cloud_msg.height = cloud_arr.shape[0]
cloud_msg.width = cloud_arr.shape[1]
cloud_msg.fields = arr_to_fields(cloud_arr)
cloud_msg.is_bigendian = False # assumption
cloud_msg.point_step = cloud_arr.dtype.itemsize
cloud_msg.row_step = cloud_msg.point_step * cloud_arr.shape[1]
cloud_msg.is_dense = all([
np.isfinite(cloud_arr[fname]).all() for fname in cloud_arr.dtype.names
])
cloud_msg.data = cloud_arr.tostring()
return cloud_msg
def xyz_array_to_pointcloud2(points_sum, 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
print("cluster points shape:", points_sum.shape)
# if len(points_sum.shape) == 3:
msg.height = 1
msg.width = points_sum.shape[1] * points_sum.shape[0]
# msg.width = points_sum.shape[1]
# else:
# msg.height = 1
# msg.width = len(points_sum)
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)
]
msg.is_bigendian = False
msg.point_step = 4
msg.row_step = points_sum.shape[1]
msg.is_dense = int(np.isfinite(points_sum).all())
# msg.is_dense = 0
print("msg.is_dense:", msg.is_dense)
# msg.data = np.asarray(points_sum, np.float32).tostring()
msg.data = points_sum.astype(np.float32).tobytes()
# print("convert msg.data:", msg.data)
return msg
def createImageEdges(self, image, drone3dPosInMap, stamp):
##Show image edges as 3D Points
imgHeight, imgWidth, channels = image.shape
# upperleft
ptul2D = [1, 1]
# upperleft
ptur2D = [1, imgHeight]
# upperleft
ptll2D = [imgWidth, 1]
# upperleft
ptlr2D = [imgWidth, imgHeight]
# Create Points which mark the edges of the camerapic in the world (RVIZ)
ptul3D = imagePointTransformations.getPixel3DPosOnWater(ptul2D, drone3dPosInMap, self.camModel, stamp)
# upperright
ptur3D = imagePointTransformations.getPixel3DPosOnWater(ptur2D, drone3dPosInMap, self.camModel, stamp)
# lowerleft
ptll3D = imagePointTransformations.getPixel3DPosOnWater(ptll2D, drone3dPosInMap, self.camModel, stamp)
# lowerright
ptlr3D = imagePointTransformations.getPixel3DPosOnWater(ptlr2D, drone3dPosInMap, self.camModel, stamp)
pixelProjection3DPoints = [ptul3D, ptur3D, ptll3D, ptlr3D]
header = Header()
header.frame_id = "map"
header.stamp = stamp
image3dProjection = PointCloud2()
image3dProjection = pc2.create_cloud_xyz32(header, pixelProjection3DPoints)
self.picProjPub.publish(image3dProjection)
def publish_filtered_rscan(self, pts_filtered, radar_msg):
rscan_filtered = PointCloud2()
rscan_filtered.header = radar_msg.header
rscan_filtered.height = 1
rscan_filtered.width = pts_filtered.shape[0]
rscan_filtered.is_bigendian = False
rscan_filtered.point_step = 24
rscan_filtered.row_step = rscan_filtered.width * rscan_filtered.point_step
rscan_filtered.is_dense = True
## this is not being done correctly...
rscan_filtered.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('range', 16, PointField.FLOAT32, 1),
PointField('doppler', 20, PointField.FLOAT32, 1),
]
data = np.reshape(pts_filtered,
(pts_filtered.shape[0] * pts_filtered.shape[1], ))
rscan_filtered.data = data.tostring()
self.pc_pub.publish(rscan_filtered)
def pcl_to_ros(pcl_array):
""" Converts a pcl PointXYZRGB to a ROS PointCloud2 message
Args:
pcl_array (PointCloud_PointXYZRGB): A PCL XYZRGB point cloud
Returns:
PointCloud2: A ROS point cloud
"""
ros_msg = PointCloud2()
ros_msg.header.stamp = rospy.Time.now()
ros_msg.header.frame_id = "world"
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="rgb",
offset=16,
datatype=PointField.FLOAT32, count=1))
ros_msg.is_bigendian = False
ros_msg.point_step = 32
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:
s = struct.pack('>f', data[3])
i = struct.unpack('>l', s)[0]
pack = ctypes.c_uint32(i).value
r = (pack & 0x00FF0000) >> 16
g = (pack & 0x0000FF00) >> 8
b = (pack & 0x000000FF)
buffer.append(struct.pack('ffffBBBBIII', data[0], data[1], data[2], 1.0, b, g, r, 0, 0, 0, 0))
# print(buffer)
ros_msg.data = b"".join(buffer)
# ros_msg.data = str.join(buffer)
# buffer = np.atleast_2d(buffer)
# ros_msg.data = buffer.tostring()
# ros_msg.data = str(buffer)
# print(ros_msg.data)
return ros_msg
def __init__(self):
rospy.init_node("PC")
self._pub = rospy.Publisher("PointCloud", PointCloud2, queue_size=1)
self._sub = rospy.Subscriber("/angles", Int16, self.callback)
self.angle = Int16()
self.data = PointCloud2()
self.once = True
def npz_pcl_dense(npz=None, ts=None, frame_id=None):
points = npz["pcloud_points"]
reflectance = npz["pcloud_attr.reflectance"]
colors = np.stack([reflectance, reflectance, reflectance], axis=1)
assert (points.shape == colors.shape)
msg = PointCloud2()
msg.header.frame_id = frame_id
msg.header.stamp.secs = divmod(ts, 1000000)[0] #stamp in micro secs
msg.header.stamp.nsecs = divmod(ts, 1000000)[1] * 1000 # nano secs
msg.width = points.shape[0]
msg.height = 1
msg.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('r', 12, PointField.FLOAT32, 1),
PointField('g', 16, PointField.FLOAT32, 1),
PointField('b', 20, PointField.FLOAT32, 1)
]
msg.is_bigendian = False
msg.point_step = 24
msg.row_step = msg.point_step * msg.width
msg.is_dense = True
data_array = np.array(np.hstack([points, colors]), dtype=np.float32)
msg.data = data_array.tostring()
return msg
def make_points_msg(header, xyz):
pts = PointCloud2()
pts.header = header
pts.height = SENSOR_HEIGHT_PIXELS
pts.width = SENSOR_WIDTH_PIXELS
pts.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),
]
pts.is_bigendian = False
pts.point_step = 20
pts.row_step = 4480
xyz32 = xyz.astype(np.float32)
xyz32[np.isnan(xyz32)] = 0.0
buf = io.BytesIO()
for row in xyz32:
# write xyz values (12 bytes)
buf.write(struct.pack("<fff", *row))
# pad with 8 bytes of 0 to replicate original point_step of 20
buf.write(struct.pack("<q", 0))
pts.data = buf.getvalue()
pts.is_dense = True
return pts
def publish_points(points, publisher, frame_id=None, stamp=None):
"""
Publish point cloud.
:param points:
:param publisher:
:param frame_id:
:param stamp:
:return:
"""
if not points.dtype == np.float32:
rospy.logwarn(
"Point cloud type not float32, may not be visualized by Rviz.")
msg = PointCloud2()
if stamp:
msg.header.stamp = stamp
if frame_id:
msg.header.frame_id = frame_id
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 * len(points)
msg.is_dense = int(np.isfinite(points).all())
msg.data = points.tostring()
publisher.publish(msg)
def xyzi_array_to_pointcloud2(points, stamp=None, frame_id=None, seq=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 seq:
msg.header.seq = seq
else:
N = len(points)
xyzi = np.array(np.hstack([points]), dtype=np.float32)
msg.height = 1
msg.width = N
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 = msg.point_step * N
msg.is_dense = True
msg.data = xyzi.tostring()
return msg
def publisher():
global g_pub, g_msg
sys = CameraSystem()
dev = sys.scan()
cam = sys.connect(dev[0])
if cam.isInitialized() == False:
print('Fail init')
#
g_pub = rospy.Publisher('cloud_point', PointCloud2, queue_size=10)
rospy.init_node('voxel_publisher', anonymous=True)
g_msg = PointCloud2()
g_msg.header.frame_id = 'base_link'
g_msg.height = 240
g_msg.width = 320
g_msg.fields.append(PointField(name='x', offset=0, datatype=7, count=1))
g_msg.fields.append(PointField(name='y', offset=4, datatype=7, count=1))
g_msg.fields.append(PointField(name='z', offset=8, datatype=7, count=1))
g_msg.point_step = 12
g_msg.row_step = 320 * 12
#
cam.registerCallback(3, cb)
ret, rate = cam.getFrameRate()
if ret == False:
print("Fail")
print("fr:{}".format(rate.numerator))
time.sleep(2) # necessory
cam.start()
rospy.spin()
def pcl_to_ros(pcl_array):
ros_msg = PointCloud2()
ros_msg.header.stamp = rospy.Time.now()
ros_msg.header.frame_id = "world"
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.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]
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)
ground_points_pub.publish(ros_msg)
def pcl_dense_msg(cls,
points=None,
reflectance=None,
ts=None,
frame_id=None):
colors = np.stack([reflectance, reflectance, reflectance], axis=1)
assert (points.shape == colors.shape)
msg = PointCloud2()
cls.set_ros_msg_header(ros_msg=msg, ts=ts, frame_id=frame_id)
msg.width = points.shape[0]
msg.height = 1
msg.fields = msg.fields = cls.get_pcl_fields()
msg.is_bigendian = False
msg.point_step = 24
msg.row_step = msg.point_step * msg.width
msg.is_dense = True
data_array = np.array(np.hstack([points, colors]), dtype=np.float32)
msg.data = data_array.tostring()
return msg
def custom_publish(self, points, stamp=None, frame_id=None):
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
#addPointField(msg_pointcloud, format_str.c_str(), 1, sensor_msgs::PointField::UINT32, msg_pointcloud.point_step);
msg.row_step = points.shape[0] * points.shape[1] #*msg.point_step
msg.is_dense = int(np.isfinite(points).all())
msg.data = np.asarray(points, np.float32).tostring()
self.publish(msg)
"""
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(
self,
points,
frame_id=None,
stamp=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 run(self):
"""
The run function for this step
Args:
Yields:
chuck_approach_pose (geometry_msgs/PoseStamped) : approach pose for impedance control with the chuck
.. seealso::
:meth:`task_executor.abstract_step.AbstractStep.run`
"""
rospy.loginfo("Action {}: Detecting schunk.".format(self.name))
self._stopped = False
# Ask for the schunk detector
stub_tf = Transform()
stub_pcl = PointCloud2()
stub = TemplateMatchRequest()
stub.initial_estimate = stub_tf
stub.target_cloud = stub_pcl
chuck_approach_pose = self._detect_schunk_srv(stub).template_pose
self.notify_service_called(
DetectSchunkAction.DETECT_SCHUNK_SERVICE_NAME)
yield self.set_running() # Check the status of the server
if self._stopped:
yield self.set_preempted(action=self.name,
srv=self._detect_schunk_srv.resolved_name,
chuck_approach_pose=chuck_approach_pose)
else:
yield self.set_succeeded(chuck_approach_pose=chuck_approach_pose)
def default(self, ci='unused'):
if not self.component_instance.capturing:
return # press [Space] key to enable capturing
points = self.data['points']
width = self.component_instance.image_width * self.component_instance.image_height
size = len(points)
pc2 = PointCloud2()
pc2.header = self.get_ros_header()
pc2.height = 1
pc2.width = width
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 = int(size / width)
pc2.row_step = size
# memoryview from PyMemoryView_FromMemory() implements the buffer interface
pc2.data = bytes(points)
self.publish(pc2)
self.send_transform_robot()
def processing_xyzi(self):
for i in range(self.len_files):
print("[+] {} th file name : {} ".format(i, self.npy_files[i]))
bin_points = np.load(os.path.join(self.npy_path,
self.npy_files[i]))
bin_points_pred = np.load(
os.path.join(self.npy_path_pred, self.npy_files_pred[i]))
pc2_msg = PointCloud2()
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'velodyne_link'
h, w, _ = bin_points.shape # 64, 512 ,6
cloud_points = []
for j in range(h): # 64
for k in range(w): # 512
cloud_points.append(
list(
np.append(bin_points[j, :, :3][k],
bin_points_pred[j][k])))
pc2_msg = self.create_cloud_xyzi32(header, cloud_points)
# ed: /velodyne_points_npy publish
self.velo_pub.publish(pc2_msg)
self.loop_rate.sleep()
if rospy.is_shutdown():
return
def to_cloud_msg(points, intensities=None, frame=None, stamp=None):
"""Convert list of unstructured points to a PointCloud2 message.
Args:
points: Point coordinates as array of shape (N,3).
colors: Colors as array of shape (N,3).
frame
stamp
"""
msg = PointCloud2()
msg.header.frame_id = frame
msg.header.stamp = stamp or rospy.Time.now()
msg.height = 1
msg.width = points.shape[0]
msg.is_bigendian = False
msg.is_dense = False
msg.fields = [
PointField("x", 0, PointField.FLOAT32, 1),
PointField("y", 4, PointField.FLOAT32, 1),
PointField("z", 8, PointField.FLOAT32, 1),
]
msg.point_step = 12
data = points
if intensities is not None:
msg.fields.append(PointField("intensity", 12, PointField.FLOAT32, 1))
msg.point_step += 4
data = np.hstack([points, intensities])
msg.row_step = msg.point_step * points.shape[0]
msg.data = data.astype(np.float32).tostring()
return msg
def processing_xyz(self):
for i in range(self.len_files):
print("[+] {} th file name : {} ".format(
i, self.npy_files_list_xyz[i]))
bin_points = np.fromfile(os.path.join(self.npy_path,
self.npy_files_list_xyz[i]),
dtype=np.float32).reshape(-1, 4)
pc2_msg = PointCloud2()
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'velodyne_link'
cloud_points = []
for j in range(len(bin_points)):
if (bin_points[j][0] >= 0.1 and bin_points[j][1] != 0
and bin_points[j][2] <= 5 and bin_points[j][3] < 10):
cloud_points.append(list(bin_points[j, :3]))
pc2_msg = pcl2.create_cloud_xyz32(header, cloud_points)
# ed: /velodyne_points_npy publish
self.velo_pub.publish(pc2_msg)
self.loop_rate.sleep()
if rospy.is_shutdown():
return
def xyzarray_to_pc2(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 = PointField.FLOAT32
dtype = np.float32
itemsize = np.dtype(dtype).itemsize
data = points.astype(dtype).tobytes()
fields = [
PointField(name=n, offset=i * itemsize, datatype=ros_dtype, count=1)
for i, n in enumerate('xyz')
]
header = parent_frame.header #std_msgs.Header(frame_id=parent_frame, stamp=rospy.Time.now())
return PointCloud2(header=header,
height=1,
width=points.shape[0],
is_dense=False,
is_bigendian=False,
fields=fields,
point_step=(itemsize * 3),
row_step=(itemsize * 3 * points.shape[0]),
data=data)
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 create_cloud(header, fields, points):
"""
Create a L{sensor_msgs.msg.PointCloud2} message.
@param header: The point cloud header.
@type header: L{std_msgs.msg.Header}
@param fields: The point cloud fields.
@type fields: iterable of L{sensor_msgs.msg.PointField}
@param points: The point cloud points.
@type points: list of iterables, i.e. one iterable for each point, with the
elements of each iterable being the values of the fields for
that point (in the same order as the fields parameter)
@return: The point cloud.
@rtype: L{sensor_msgs.msg.PointCloud2}
"""
cloud_struct = struct.Struct(_get_struct_fmt(False, fields))
buff = ctypes.create_string_buffer(cloud_struct.size * len(points))
point_step, pack_into = cloud_struct.size, cloud_struct.pack_into
offset = 0
for p in points:
pack_into(buff, offset, *p)
offset += point_step
return PointCloud2(header=header,
height=1,
width=len(points),
is_dense=False,
is_bigendian=False,
fields=fields,
point_step=cloud_struct.size,
row_step=cloud_struct.size * len(points),
data=buff.raw)
def __init__(self):
self.pointcloud = PointCloud2()
self.rate = 1
self.start = 0
return
def simulate_mbes(self, mbes_ac):
# Find particle's mbes pose without broadcasting/listening to tf transforms
particle_tf = Transform()
particle_tf.translation = self.pose.position
particle_tf.rotation = self.pose.orientation
mat_part = matrix_from_tf(particle_tf)
trans_mat = np.dot(mat_part, self.mbes_tf_mat)
trans = TransformStamped()
trans.transform.translation.x = translation_from_matrix(trans_mat)[0]
trans.transform.translation.y = translation_from_matrix(trans_mat)[1]
trans.transform.translation.z = translation_from_matrix(trans_mat)[2]
trans.transform.rotation = Quaternion(
*quaternion_from_matrix(trans_mat))
# Build MbesSimGoal to send to action server
mbes_goal = MbesSimGoal()
mbes_goal.mbes_pose.header.frame_id = self.map_frame
# mbes_goal.mbes_pose.child_frame_id = self.mbes_frame_id # The particles will be in a child frame to the map
mbes_goal.mbes_pose.header.stamp = rospy.Time.now()
mbes_goal.mbes_pose.transform = trans.transform
# Get result from action server
mbes_ac.send_goal(mbes_goal)
mbes_ac.wait_for_result()
mbes_res = mbes_ac.get_result()
# Pack result into PointCloud2
mbes_pcloud = PointCloud2()
mbes_pcloud = mbes_res.sim_mbes
mbes_pcloud.header.frame_id = self.map_frame
return mbes_pcloud
def publish_image_as_pointcloud(point_cloud_publisher: rospy.Publisher,
message: PointCloud2):
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
message.header = h
message.header.frame_id = 'img2rviz'
point_cloud_publisher.publish(message)
