Jul-26-2023, 09:29 PM
updated Code:
a "one shot graph"----realtime otw...
a "one shot graph"----realtime otw...
##################################################################################
#!/usr/bin/env python3
##################################################################################
import rospy
from sensor_msgs.msg import LaserScan
import laser_geometry.laser_geometry as lg
import math
import numpy as np
import time
import sensor_msgs.point_cloud2 as pcl2
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import axes3d
import mpl_toolkits.mplot3d
##################################################################################
###### rospy #################################################################
##################################################################################
rospy.init_node('fishviewer')
##################################################################################
###### LaserScan #################################################################
##################################################################################
def laserscan():
msg = LaserScan()
msg.angle_min = -1.57
msg.angle_min = -1.57
msg.angle_max = 1.57
msg.angle_increment = 0.1
msg.time_increment = 0.001
msg.scan_time = 0.05
msg.range_min = 0.0
msg.range_max = 10.0
msg.ranges = [1.0] * int((msg.angle_max - msg.angle_min) / msg.angle_increment)
msg.intensities = [1] * len(msg.ranges)
return msg
##################################################################################
##### LaserProjection -- convert scan to point cloud 2 ###############################################################################
##################################################################################
class LaserProjection:
LASER_SCAN_INVALID = -1.0
LASER_SCAN_MIN_RANGE = -2.0
LASER_SCAN_MAX_RANGE = -3.0
class ChannelOption:
NONE = 0x00 # Enable no channels
INTENSITY = 0x01 # Enable "intensities" channel
INDEX = 0x02 # Enable "index" channel
DISTANCE = 0x04 # Enable "distances" channel
TIMESTAMP = 0x08 # Enable "stamps" channel
VIEWPOINT = 0x10 # Enable "viewpoint" channel
DEFAULT = (INTENSITY | INDEX)
def __init__(self,msg):
self.__angle_min = 0.0
self.__angle_max = 0.0
self.__cos_sin_map = np.array([[]])
def projectLaser(self, msg,
range_cutoff=-1.0, channel_options=ChannelOption.DEFAULT):
return self.__projectLaser(msg, range_cutoff, channel_options)
def __projectLaser(self, msg, range_cutoff, channel_options):
N = len(msg.ranges)
ranges = np.array(msg.ranges)
if (self.__cos_sin_map.shape[1] != N or
self.__angle_min != msg.angle_min or
self.__angle_max != msg.angle_max):
rospy.logdebug("No precomputed map given. Computing one.")
self.__angle_min = msg.angle_min
self.__angle_max = msg.angle_max
angles = msg.angle_min + np.arange(N) * msg.angle_increment
self.__cos_sin_map = np.array([np.cos(angles), np.sin(angles)])
output = ranges * self.__cos_sin_map
# Set the output cloud accordingly
cloud = PointCloud2()
fields = [pcl2.PointField() for _ in range(3)]
fields[0].name = "x"
fields[0].offset = 0
fields[0].datatype = pcl2.PointField.FLOAT32
fields[0].count = 1
fields[1].name = "y"
fields[1].offset = 4
fields[1].datatype = pcl2.PointField.FLOAT32
fields[1].count = 1
fields[2].name = "z"
fields[2].offset = 8
fields[2].datatype = pcl2.PointField.FLOAT32
fields[2].count = 1
idx_intensity = idx_index = idx_distance = idx_timestamp = -1
idx_vpx = idx_vpy = idx_vpz = -1
offset = 12
if (channel_options & self.ChannelOption.INTENSITY and
len(msg.intensities) > 0):
field_size = len(fields)
fields.append(pcl2.PointField())
fields[field_size].name = "intensity"
fields[field_size].datatype = pcl2.PointField.FLOAT32
fields[field_size].offset = offset
fields[field_size].count = 1
offset += 4
idx_intensity = field_size
if channel_options & self.ChannelOption.INDEX:
field_size = len(fields)
fields.append(pcl2.PointField())
fields[field_size].name = "index"
fields[field_size].datatype = pcl2.PointField.INT32
fields[field_size].offset = offset
fields[field_size].count = 1
offset += 4
idx_index = field_size
if channel_options & self.ChannelOption.DISTANCE:
field_size = len(fields)
fields.append(pcl2.PointField())
fields[field_size].name = "distances"
fields[field_size].datatype = pcl2.PointField.FLOAT32
fields[field_size].offset = offset
fields[field_size].count = 1
offset += 4
idx_distance = field_size
if channel_options & self.ChannelOption.TIMESTAMP:
field_size = len(fields)
fields.append(pcl2.PointField())
fields[field_size].name = "stamps"
fields[field_size].datatype = pcl2.PointField.FLOAT32
fields[field_size].offset = offset
fields[field_size].count = 1
offset += 4
idx_timestamp = field_size
if channel_options & self.ChannelOption.VIEWPOINT:
field_size = len(fields)
fields.extend([pcl2.PointField() for _ in range(3)])
fields[field_size].name = "vp_x"
fields[field_size].datatype = pcl2.PointField.FLOAT32
fields[field_size].offset = offset
fields[field_size].count = 1
offset += 4
idx_vpx = field_size
field_size += 1
fields[field_size].name = "vp_y"
fields[field_size].datatype = pcl2.PointField.FLOAT32
fields[field_size].offset = offset
fields[field_size].count = 1
offset += 4
idx_vpy = field_size
field_size += 1
fields[field_size].name = "vp_z"
fields[field_size].datatype = pcl2.PointField.FLOAT32
fields[field_size].offset = offset
fields[field_size].count = 1
offset += 4
idx_vpz = field_size
if range_cutoff < 0:
range_cutoff = msg.range_max
else:
range_cutoff = min(range_cutoff, msg.range_max)
points = []
for i in range(N):
ri = msg.ranges[i]
if ri < range_cutoff and ri >= msg.range_min:
point = output[:, i].tolist()
point.append(0)
if idx_intensity != -1:
point.append(msg.intensities[i])
if idx_index != -1:
point.append(i)
if idx_distance != -1:
point.append(msg.ranges[i])
if idx_timestamp != -1:
point.append(i * msg.time_increment)
if idx_vpx != -1 and idx_vpy != -1 and idx_vpz != -1:
point.extend([0 for _ in range(3)])
points.append(point)
cloud = pcl2.create_cloud(msg.header, fields, points)
x = []
y = []
z = []
xyz = []
for vapor in pcl2.read_points(msg, skip_nans=True):
pt_x = vapor[0]
x = x.append(pt_x)
pt_y = vapor[1]
y = y.append(pt_y)
pt_z = vapor[2]
z = z.append(pt_z)
xyz = x, y, z
return xyz
##################################################################################
####################################################################################
##################################################################################
def generator():
msg = laserscan()
xyz = LaserProjection(msg)
return xyz
##################################################################################
###### plotting #################################################################
##################################################################################
def plotter(xyz):
# Creating figure
fig = plt.figure(figsize = (16, 9))
ax = plt.axes(projection ="3d")
ax.grid(b = True, color ='grey',
linestyle ='-.', linewidth = 0.3,
alpha = 0.2)
my_cmap = plt.get_cmap('hsv')
sctt = ax.scatter3D(xyz[0], xyz[1], xyz[2],
alpha = 0.8,
c = (xyz[0] + xyz[1] + xyz[2]),
cmap = my_cmap,
marker ='0(c='+xyz[2]+' depth')
plt.title("Lidar Fish Viewer")
ax.set_zlabel('DEPTH', fontweight ='bold')
fig.colorbar(sctt, ax = ax, shrink = 0.5, aspect = 5)
plt.show()
return fig,ax,sctt
#########################################
xyz = generator()
print ('points:'+str(xyz))https://python-forum.io/images/smilies/arrow.png
time.sleep(9)
plotter(xyz)
rospy.spin()
