Jul-26-2023, 08:19 PM
Yup ---- you make sense - i have followed and the program is now working much harder....i cleared up some typos aswell:
A_Fish_Detecting_LIDAR_Program.py
A_Fish_Detecting_LIDAR_Program.py
################################################################################## #!/usr/bin/env python3 ################################################################################## import rospy from sensor_msgs.msg import LaserScan import laser_geometry import laser_geometry.laser_geometry as lg import math import numpy as np import time from sensor_msgs.msg import PointCloud from geometry_msgs.msg import Point32 import std_msgs.msg import sys from sensor_msgs.msg import PointCloud2 import std_msgs.msg 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.https://python-forum.io/images/smilies/shy.pngFLOAT32 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) cloud = pc2.read_points_list(cloud) return cloud ################################################################################## #################################################################################### ################################################################################## def generator(): #while true: msg = laserscan() print ('laserscan: '+str(msg)) cloud = LaserProjection(msg) print ('laserprojection in mem: '+str(cloud)) t = 0 xyz = [] print (str(cloud.read())) for vaper in cloud: vaper = cloud.readline() t = t + 1 xyz = xyz.append(vaper) return xyz ################################################################################## ###### plotting ################################################################# ################################################################################## xyz = generator() def plotter(): # 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 ######################################### def plot_updater(): cloud = generator() ax.relim() ax.autoscale() cloud.set_xdata(cloud[0]) cloud.set_ydata(cloud[1]) cloud.set_3d_properties(cloud[2]) return ax, cloud fig = plotter() # instantiate figure and plot ######################################## plot_updater() rospy.spin()