diff --git a/config/scan_params copy.yaml b/config/scan_params copy.yaml new file mode 100644 index 00000000..a5bdc0a8 --- /dev/null +++ b/config/scan_params copy.yaml @@ -0,0 +1,9 @@ +rplidar_params: + fov: 240 + rotation: 0 + range: {min: 0.05, max: 30.0} + # reduction_method: '1-Pooling' + # output_samples: 1195 + reduction_method: 'None' + output_samples: 'None' + intensities: 47 diff --git a/config/scan_params.yaml b/config/scan_params.yaml new file mode 100644 index 00000000..972188c5 --- /dev/null +++ b/config/scan_params.yaml @@ -0,0 +1,34 @@ +rplidar_params: + fov: 240 + rotation: 0 + range: {min: 0.14, max: 30.0} + # reduction_method: '1-Pooling' + # output_samples: 1195 + reduction_method: 'none' + output_samples: 'None' + intensities: 47 + + #reduction_method only admit "[max/min/mean/(int)]-pooling", "resample", or "None" + #----------------------------------------------------------------------------------------------------# + # reducion method 1 + # reduction_method: 'resample' + # resample_interval: 2 # Mantém cada segundo ponto de dados + # interpolation_method: "linear" # Usa interpolação linear / suavização moderada / pode n capturar variações complexas + # interpolation_method: "spline" # Usa interpolação spline / suavização avançada / captura variações precisa e suaves / mais custoso computacionalmente. + # interpolation_method: "nearest" # Usa interpolação por vizinho mais próximo / Mantém os valores originais / resulta em dados menos suaves, aparencia em blocos + + # Comentários: + # - Intervalo pequeno (2) mantém mais detalhes dos dados originais. + # - Reduz levemente a quantidade de dados, melhorando a eficiência sem perder muita precisão. + # - Adequado para pré-processamento e visualização inicial, mantendo uma representação detalhada. + #----------------------------------------------------------------------------------------------------# + # reduction method 2 + # reduction_method: 'mean(2)-pooling' + # pooling_region_size: 2 # Agrupa pontos em regiões de 2x2 + # pooling_method: "average" # Usa averaje pooling + + + # Comentários: + # - Seleciona o valor médio dentro de cada bloco 2x2. + # - Suaviza os dados, útil para obter uma representação geral suave. + # - Esse metodo substitui 2 blocos vizinho em apena 1 bloco que representa os dois, diminuindo a quantidade de informação diff --git a/launch/rplidar_a1.launch b/launch/rplidar_a1.launch deleted file mode 100644 index 592bc771..00000000 --- a/launch/rplidar_a1.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/launch/rplidar_a2m12.launch b/launch/rplidar_a2m12.launch deleted file mode 100644 index 056b7aa5..00000000 --- a/launch/rplidar_a2m12.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/launch/rplidar_a2m7.launch b/launch/rplidar_a2m7.launch deleted file mode 100644 index 056b7aa5..00000000 --- a/launch/rplidar_a2m7.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/launch/rplidar_a3.launch b/launch/rplidar_a3.launch deleted file mode 100644 index f8ac8450..00000000 --- a/launch/rplidar_a3.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/launch/rplidar_c1.launch b/launch/rplidar_c1.launch deleted file mode 100644 index 54684880..00000000 --- a/launch/rplidar_c1.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/launch/rplidar_s1.launch b/launch/rplidar_s1.launch deleted file mode 100644 index 26668545..00000000 --- a/launch/rplidar_s1.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/launch/rplidar_s1_tcp.launch b/launch/rplidar_s1_tcp.launch deleted file mode 100644 index a1e6b4bd..00000000 --- a/launch/rplidar_s1_tcp.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/launch/rplidar_s2.launch b/launch/rplidar_s2-origin.launch similarity index 74% rename from launch/rplidar_s2.launch rename to launch/rplidar_s2-origin.launch index a10baed3..071ddd4e 100644 --- a/launch/rplidar_s2.launch +++ b/launch/rplidar_s2-origin.launch @@ -2,9 +2,9 @@ - + - + diff --git a/launch/rplidar_s2_mapping.launch b/launch/rplidar_s2_mapping.launch new file mode 100644 index 00000000..3f8a0be4 --- /dev/null +++ b/launch/rplidar_s2_mapping.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/launch/rplidar_s2_navigation.launch b/launch/rplidar_s2_navigation.launch new file mode 100644 index 00000000..5e0f8f14 --- /dev/null +++ b/launch/rplidar_s2_navigation.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/rplidar_s2e.launch b/launch/rplidar_s2e.launch deleted file mode 100644 index cec1491c..00000000 --- a/launch/rplidar_s2e.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/launch/rplidar_s3.launch b/launch/rplidar_s3.launch deleted file mode 100644 index a10baed3..00000000 --- a/launch/rplidar_s3.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/launch/rplidar_t1.launch b/launch/rplidar_t1.launch deleted file mode 100644 index 31d22e2c..00000000 --- a/launch/rplidar_t1.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/launch/test_rplidar.launch b/launch/test_rplidar.launch deleted file mode 100644 index afe4aa7e..00000000 --- a/launch/test_rplidar.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/launch/test_rplidar_a3.launch b/launch/test_rplidar_a3.launch deleted file mode 100644 index 45541ebe..00000000 --- a/launch/test_rplidar_a3.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/launch/view_rplidar_a1.launch b/launch/view_rplidar_a1.launch deleted file mode 100644 index 183a6f6e..00000000 --- a/launch/view_rplidar_a1.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - diff --git a/launch/view_rplidar_a2m12.launch b/launch/view_rplidar_a2m12.launch deleted file mode 100644 index 20cb3800..00000000 --- a/launch/view_rplidar_a2m12.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - diff --git a/launch/view_rplidar_a2m7.launch b/launch/view_rplidar_a2m7.launch deleted file mode 100644 index 82f746a7..00000000 --- a/launch/view_rplidar_a2m7.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - diff --git a/launch/view_rplidar_a2m8.launch b/launch/view_rplidar_a2m8.launch deleted file mode 100644 index bff8b1b1..00000000 --- a/launch/view_rplidar_a2m8.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - diff --git a/launch/view_rplidar_a3.launch b/launch/view_rplidar_a3.launch deleted file mode 100644 index 010f6d6e..00000000 --- a/launch/view_rplidar_a3.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - diff --git a/launch/view_rplidar_c1.launch b/launch/view_rplidar_c1.launch deleted file mode 100644 index 4979ff71..00000000 --- a/launch/view_rplidar_c1.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - diff --git a/launch/view_rplidar_s1.launch b/launch/view_rplidar_s1.launch deleted file mode 100644 index e3c3127e..00000000 --- a/launch/view_rplidar_s1.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - diff --git a/launch/view_rplidar_s1_tcp.launch b/launch/view_rplidar_s1_tcp.launch deleted file mode 100644 index 88b011df..00000000 --- a/launch/view_rplidar_s1_tcp.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - diff --git a/launch/view_rplidar_s2.launch b/launch/view_rplidar_s2.launch deleted file mode 100644 index 65bf1c86..00000000 --- a/launch/view_rplidar_s2.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - diff --git a/launch/view_rplidar_s2e.launch b/launch/view_rplidar_s2e.launch deleted file mode 100644 index 064cc185..00000000 --- a/launch/view_rplidar_s2e.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - diff --git a/launch/view_rplidar_s3.launch b/launch/view_rplidar_s3.launch deleted file mode 100644 index 6a375d04..00000000 --- a/launch/view_rplidar_s3.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - diff --git a/launch/view_rplidar_t1.launch b/launch/view_rplidar_t1.launch deleted file mode 100644 index e07878ca..00000000 --- a/launch/view_rplidar_t1.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - diff --git a/scripts/__pycache__/pooling.cpython-38.pyc b/scripts/__pycache__/pooling.cpython-38.pyc new file mode 100644 index 00000000..db6ca2cc Binary files /dev/null and b/scripts/__pycache__/pooling.cpython-38.pyc differ diff --git a/scripts/__pycache__/resample.cpython-38.pyc b/scripts/__pycache__/resample.cpython-38.pyc new file mode 100644 index 00000000..a5ce88a6 Binary files /dev/null and b/scripts/__pycache__/resample.cpython-38.pyc differ diff --git a/scripts/create_udev_rules.sh b/scripts/create_udev_rules.sh old mode 100755 new mode 100644 diff --git a/scripts/delete_udev_rules.sh b/scripts/delete_udev_rules.sh old mode 100755 new mode 100644 diff --git a/scripts/down_sample.py b/scripts/down_sample.py new file mode 100644 index 00000000..e761afba --- /dev/null +++ b/scripts/down_sample.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python3 +import rospy +from adc_raw.msg import Mcp3208_data +from sensor_msgs.msg import ChannelFloat32 +from sensor_msgs.msg import LaserScan +import tf2_ros +from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3, TransformStamped + +import numpy as np +class DownSampler(): + def __init__(self,name): + self.name=name + self.rospy=rospy + self.rospy.init_node(self.name,anonymous=True) + self.rospy.loginfo("[%s] Starting Node ",self.name) + self.initParams() + self.initVariables() + self.initSubscribers() + self.initPublishers() + self.main() + + def initParams(self): + self.acquisition_rate = self.rospy.get_param("laser/acquisition_rate",10) + return + + def initVariables(self): + self.rate = self.rospy.Rate(self.acquisition_rate) + self.laser_output_message = LaserScan() + self.output_size = self.rospy.get_param("rplidar_params/output_samples",1195) + self.output_scan_array = [] + self.output_intensities_array=[] + self.samples_spacing = (int(3585/self.output_size)) + # print(self.samples_spacing) + self.flag = False + return + + def initSubscribers(self): + self.lidar_sub = self.rospy.Subscriber("/raw_scan", LaserScan, self.lidarCallback, queue_size=2) + return + + def initPublishers(self): + self.pubLaser = self.rospy.Publisher("/scan",LaserScan,queue_size=2) + + + # Let's initialize the transform broadcaster and the message to broadcast + self.lidar_broadcaster = tf2_ros.TransformBroadcaster() + self.lidar_transform = TransformStamped() + self.lidar_transform.header.frame_id = "rplidara2_sensor_link" + self.lidar_transform.child_frame_id = "base_link" + #self.pub_laserRaw2 = self.rospy.Publisher(self.publish_topic2,ChannelFloat32,queue_size=10) + return + + def lidarCallback(self,msg): + self.output_scan_array=[] + self.output_intensities_array=[] + # print(len(msg.ranges)) + # self.output_scan_array[0]=msg.ranges[0] + for i in range(0,len(msg.ranges),3): + self.output_scan_array.append(msg.ranges[i]) + self.output_intensities_array.append(msg.intensities[i]) + + self.output_scan_array=np.array(self.output_scan_array) + # print(self.output_scan_array.size) + + self.laser_output_message.header=msg.header + self.laser_output_message.header.frame_id="rplidara2_sensor_link" + self.laser_output_message.header.stamp = self.rospy.Time.now() + self.laser_output_message.ranges=self.output_scan_array + self.laser_output_message.intensities=self.output_intensities_array + self.laser_output_message.angle_min=msg.angle_min + self.laser_output_message.angle_max=msg.angle_max + self.laser_output_message.range_min=msg.range_min + self.laser_output_message.range_min=msg.range_max + self.laser_output_message.angle_increment=3*msg.angle_increment + self.laser_output_message.scan_time=3*msg.scan_time + self.laser_output_message.time_increment=3*msg.time_increment + + + + + # self.odom_transform.header.stamp = self.rospy.Time.now() + # self.odom_transform.transform.translation.x = self.pos_x + # self.odom_transform.transform.translation.y = self.pos_y + # self.odom_transform.transform.translation.z = 0.0 + # self.odom_transform.transform.rotation.x = odom_quat[0] + # self.odom_transform.transform.rotation.y = odom_quat[1] + # self.odom_transform.transform.rotation.z = odom_quat[2] + # self.odom_transform.transform.rotation.w = odom_quat[3] + + # self.lidar_broadcaster.sendTransform(self.lidar_transform) + self.pubLaser.publish(self.laser_output_message) + + + + def main(self): + while not self.rospy.is_shutdown(): + self.rospy.spin() +if __name__ == '__main__': + try: + DownSamplerNode = DownSampler('DownSampler_NODE') + except rospy.ROSInterruptException: + pass diff --git a/scripts/pooling.py b/scripts/pooling.py new file mode 100644 index 00000000..d5539e7d --- /dev/null +++ b/scripts/pooling.py @@ -0,0 +1,110 @@ +#!/usr/bin/python +from numpy import array, mean, inf, isfinite +from numba import jit + + +@jit(nopython=True) +def max_pooling(input_array, value, array_len, function=max): + return [function(input_array[pos:pos + value]) for pos in range(0, array_len, value)] + + +@jit(nopython=True) +def min_pooling(input_array, value, array_len, function=min): + return [function(input_array[pos:pos + value]) for pos in range(0, array_len, value)] + + +@jit(nopython=True) +def mean_pooling(input_array, value, array_len, function=mean): + return [function(input_array[pos:pos + value]) for pos in range(0, array_len, value)] + + +@jit(nopython=True) +def max_zwi_pooling(input_array, value, array_len): + return [max(input_array[pos:pos + value][isfinite(input_array[pos:pos + value])]) + if isfinite(input_array[pos:pos + value]).any() else inf for pos in range(0, array_len, value)] + + +@jit(nopython=True) +def min_zwi_pooling(input_array, value, array_len): + return [min(input_array[pos:pos + value][isfinite(input_array[pos:pos + value])]) + if isfinite(input_array[pos:pos + value]).any() else inf for pos in range(0, array_len, value)] + + +@jit(nopython=True) +def mean_zwi_pooling(input_array, value, array_len): + return [mean(input_array[pos:pos + value][isfinite(input_array[pos:pos + value])]) + if isfinite(input_array[pos:pos + value]).any() else inf for pos in range(0, array_len, value)] + + +@jit(nopython=False) +def function_pooling(input_array, value, array_len, function): + return [function(input_array[pos:pos + value]) for pos in range(0, array_len, value)] + + +@jit(nopython=False) +def zwi_function_pooling(input_array, value, array_len, function): # zero weight inf + return [function(input_array[pos:pos + value][isfinite(input_array[pos:pos + value])]) + if isfinite(input_array[pos:pos + value]).any() else inf for pos in range(0, array_len, value)] + + +@jit(nopython=True) +def position_pooling(input_array, position, sections): + return [input_array[pos + position - 1] for pos in sections] + + +def pooling(input_array, value, method=None, zero_weight_inf=True, use_jit=True): + array_len = len(input_array) + if array_len % value: + raise TypeError('input_array length % pooling value > 0') + + if type(method) is int: + if method > value: + raise TypeError('method > value') + if use_jit: + return position_pooling(input_array, value, array_len, method) + return [input_array[pos + method - 1] for pos in range(0, array_len, value)] + + if zero_weight_inf: + if use_jit: + if method is max: + return max_zwi_pooling(input_array, value, array_len) + if method is min: + return min_zwi_pooling(input_array, value, array_len) + if method is mean: + return mean_zwi_pooling(input_array, value, array_len) + + return zwi_function_pooling(input_array, value, array_len, function=method) + if use_jit: + if method is max: + return max_pooling(input_array, value, array_len) + if method is min: + return min_pooling(input_array, value, array_len) + if method is mean: + return mean_pooling(input_array, value, array_len) + return function_pooling(input_array, value, array_len, function=method) + + +if __name__ == '__main__': + from functools import wraps + from time import process_time + + def timing(function): + @wraps(function) + def wrap(*args, **kw): + ts = process_time() + result = function(*args, **kw) + print(f'[{function.__name__}] took: {1000 * (process_time() - ts)} ms') + return result + + return wrap + + @timing + def test(): + return position_pooling(a, 2, 3200, 1) + + a = array(200 * [20.11221, 200.233525, -5.52335, 23.43434, -0.00976, inf, 119.2, 100.0, 120.12, 32.43, inf, inf, -120.34, 12.56565, 9.78665, 23.0890]) + + for i in range(1, 10): + pooled = test() + # print(pooled) + print(f'input_len: [{len(a)}], output_len: [{len(pooled)}]') diff --git a/scripts/resample.py b/scripts/resample.py new file mode 100644 index 00000000..c1d520be --- /dev/null +++ b/scripts/resample.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python3 +from math import isinf, inf +from numpy import empty +from numba import jit + + +@jit(nopython=True) +def resample(data, new_size): + size = data.size + result_data = empty(new_size, data.dtype) + carry = 0 + old_pos = 0 + for new_pos in range(new_size): + bill = carry + while old_pos*new_size - new_pos*size < size: + if isinf(data[old_pos]): + bill = inf + old_pos += 1 + continue + bill += data[old_pos] + old_pos += 1 + if isinf(bill): + result_data[new_pos] = bill + continue + carry = (old_pos-(new_pos+1)*size/new_size)*data[old_pos-1] + bill -= carry + result_data[new_pos] = bill*new_size/size + return result_data + + +if __name__ == '__main__': + from functools import wraps + from time import process_time + from numpy import array + + def timing(function): + @wraps(function) + def wrap(*args, **kw): + ts = process_time() + result = function(*args, **kw) + print(f'[{function.__name__}] took: {1000 * (process_time() - ts)} ms') + return result + + return wrap + + @timing + def test(): + return resample(a, 1600) + + a = array(200 * [20.11221, 200.233525, -5.52335, 23.43434, -0.00976, inf, 119.2, 100.0, 120.12, 32.43, inf, inf, -120.34, 12.56565, 9.78665, 23.0890]) + + for i in range(1, 10): + resampled = test() + # print(resampled) + print(f'input_len: [{len(a)}], output_len: [{len(resampled)}]') diff --git a/scripts/rplidar_scan.py b/scripts/rplidar_scan.py new file mode 100644 index 00000000..d6c6c4e2 --- /dev/null +++ b/scripts/rplidar_scan.py @@ -0,0 +1,309 @@ +#!/usr/bin/env python3 +import rospy +from sensor_msgs.msg import LaserScan, ChannelFloat32 +from std_msgs.msg import String +from numpy import concatenate +from copy import deepcopy +from numpy import array as nparray +from math import pi, radians +from pooling import * +from resample import * +import time + + +class RPLIDAR_SCAN: + def __init__(self, name='RPLIDAR_SCAN', autostart=True, rate=11, log_level=rospy.INFO): + self.name = name + self.rospy = rospy + self.rospy.init_node(self.name, anonymous=True, log_level=log_level) + self.rospy.loginfo("[%s] Starting Node ", self.name) + self.rate = self.rospy.Rate(rate) + + self.initParams() + self.initVariables() + self.initPublishers() + self.initSubscribers() + + if autostart: + self.start() + + def initParams(self): + self.lidar_intensities = float(rospy.get_param('rplidar_params/intensities')) + self.output_samples_length = rospy.get_param('rplidar_params/output_samples') + self.reduction_method = str(rospy.get_param('rplidar_params/reduction_method')).lower() + self.lidar_range = dict(rospy.get_param('rplidar_params/range')) + self.lidar_fov = int(rospy.get_param('rplidar_params/fov')) + self.lidar_rotation = int(rospy.get_param('rplidar_params/rotation')) + self.mask_value = inf + self.reduction = None + self.length_ratio = None + self.reduction_position = None + self.lidar_raw_ranges_length = None + self.reduce_fixed_params = None + + if self.lidar_intensities < 0: + self.rospy.logerr(f'[{self.name}] lidar_intensities cant admit negative values') + raise TypeError(f'[{self.name}] lidar_intensities cant admit negative values') + + try: + if self.output_samples_length < 0: + self.rospy.logerr(f'[{self.name}] output_samples_length cant admit negative values') + raise TypeError(f'[{self.name}] output_samples_length cant admit negative values') + except TypeError or ValueError: + self.rospy.logwarn(f'[{self.name}] [output_samples] not informed or invalid, value will be set to [raw_samples_length] and [reduction_method] set to [None]') + self.output_samples_length = None + self.reduction_method = None + + if self.lidar_range['min'] < 0 or self.lidar_range['max'] < 0: + self.rospy.logerr(f'[{self.name}] lidar_range cant admit negative values') + raise TypeError(f'[{self.name}] lidar_range cant admit negative values') + if self.lidar_fov < 0 or self.lidar_fov > 360: + self.rospy.logerr(f'[{self.name}] lidar_fov only admit values between 0~360') + raise TypeError(f'[{self.name}] lidar_fov only admit values between 0~360') + if self.lidar_rotation < -180 or self.lidar_rotation > 180: + self.rospy.logerr(f'[{self.name}] lidar_rotation only admit values between -180~180') + raise TypeError(f'[{self.name}] lidar_rotation only admit values between -180~180') + + def defineParams(self, range_samples): + self.lidar_raw_ranges_length = len(range_samples) + self.rospy.loginfo( + f'[{self.name}] ##### rplidar samples read! [{self.lidar_raw_ranges_length}] raw samples! #####') + + # auto set samples if None + if not self.output_samples_length: + self.output_samples_length = self.lidar_raw_ranges_length + self.length_ratio = self.lidar_raw_ranges_length / self.output_samples_length + + if not (0 < self.lidar_fov < 360) or not self.output_samples_length: + self.min_mask_index, self.max_mask_index = None, None + self.mask_values_array = nparray(tuple()) + else: + # define indexes to mask method + self.min_mask_index = round(self.output_samples_length * (radians(self.lidar_fov / (4 * pi)))) + self.max_mask_index = round(self.output_samples_length * (1 - radians(self.lidar_fov / (4 * pi)))) + self.mask_values_array = nparray(int(self.max_mask_index - self.min_mask_index) * [self.mask_value]) + + # this section define parameters and function to match selected reduction method + if not self.reduction_method or 'none' in self.reduction_method: + # não estou conseguindo chavear esses parametros ao carregá-los então estou modificando aqui. Ass: AJ + self.reduction_method = 'none' #8,54% de erro em 2000 amostras com 6% de corte emax:10.52A1 + # self.reduction_method = 'resample' #8,95% de erro em 2000 amostras com 6% de corte emax:9.44A16 + # self.reduction_method = 'pooling' # não consegui validar + + + else: + if not('pooling' in self.reduction_method or 'resample' in self.reduction_method): + self.rospy.logerr(f'[{self.name}] reduction_method only admit "[max/min/mean/(int)]-pooling", "resample", or "None"') + raise TypeError(f'[{self.name}] reduction_method only admit "[max/min/mean/(int)]-pooling", "resample", or "None"') + + try: + # resample method + if 'resample' in self.reduction_method: + self.reduction = resample + self.reduce_fixed_params = tuple([self.output_samples_length]) + + # cases bellow refer to max/min/mean pooling method + elif 'max' in self.reduction_method: + self.reduction = max_pooling + self.reduce_fixed_params = (self.length_ratio, self.lidar_raw_ranges_length) + elif 'min' in self.reduction_method: + self.reduction = min_pooling + self.reduce_fixed_params = (self.length_ratio, self.lidar_raw_ranges_length) + elif 'mean' in self.reduction_method: + self.reduction = mean_pooling + self.reduce_fixed_params = (self.length_ratio, self.lidar_raw_ranges_length) + print("POOOOLING MEDIO") + + # next case refer to position pooling method + elif type(int(self.reduction_method.split('-')[0])) is int: + self.reduction = position_pooling + self.reduction_position = int(self.reduction_method.split('-')[0]) + self.reduction_sections = nparray(range(0, self.lidar_raw_ranges_length, int(self.length_ratio))) + self.reduce_fixed_params = (self.reduction_position, self.reduction_sections) + + else: + raise TypeError + + except TypeError: + TypeError(f'[{self.name}] pooling reduction method need [max/min/mean/(int)] information') + + self.rospy.loginfo(f'[{self.name}] * PARAMETERS *\n' + f' * fov: [{self.lidar_fov}]\n' + f' * min_mask_index: [{self.min_mask_index}]\n' + f' * max_mask_index: [{self.max_mask_index}]\n' + f' * output_samples_length: [{self.output_samples_length}]\n' + f' * fov_samples(length): [{self.output_samples_length - len(self.mask_values_array)}]\n' + f' * mask_samples(length): [{len(self.mask_values_array)}]\n' + f' * reduction_method: {self.reduction_method}') + + self.lidar_redata.angle_min = -pi + radians(self.lidar_rotation) + self.lidar_redata.angle_max = pi + radians(self.lidar_rotation) + self.lidar_redata.angle_increment = 2 * pi / self.output_samples_length + self.lidar_redata.time_increment = self.lidar_raw_msg.time_increment * self.length_ratio + + self.lidar_redata.range_min = self.lidar_range['min'] + self.lidar_redata.range_max = self.lidar_range['max'] + #self.lidar_redata.intensities = nparray(self.output_samples_length * [self.lidar_intensities]) + self.lidar_redata.intensities = self.lidar_intensities + + if self.reduction_position: + if self.length_ratio % 1 > 0: + self.rospy.logerr( + f'[{self.name}] input_array length [{self.lidar_raw_ranges_length}] % pooling value [{self.length_ratio}] > 0') + raise TypeError(f'[{self.name}] input_array length % pooling value > 0') + if self.reduction_position > self.length_ratio: + self.rospy.logerr( + f'[{self.name}] reduction_position [{self.reduction_position}] > length_ratio value [{self.length_ratio}]') + raise TypeError( + f'[{self.name}] reduction_position [{self.reduction_position}] > length_ratio value [{self.length_ratio}]') + + def initVariables(self): + self.lidar_raw_msg = LaserScan() + self.lidar_redata = LaserScan() + self.callback_flag = False + self.running_flag = False + self.reading_params_flag = False + self.ready_flag = False + self.length_error_flag = False + + def initPublishers(self): + self.lidar_pub = self.rospy.Publisher('/scan_unfiltered', LaserScan, queue_size=15) + + def initSubscribers(self): + self.lidar_sub = self.rospy.Subscriber("/raw_scan", LaserScan, self.lidarCallback, queue_size=15) + self.lidar_params_sub = self.rospy.Subscriber('/scan_params', String, self.lidarParamsCallback, queue_size=1) + + def lidarCallback(self, msg): + if any(msg.ranges): + self.lidar_raw_msg = msg + self.lidar_raw_ranges = deepcopy(self.lidar_raw_msg.ranges) + self.callback_flag = True + if any(msg.intensities): + self.lidar_intensities = msg.intensities + + def lidarParamsCallback(self, msg): + # this function update parameters thought a string message callback + # parameters and variables are reloaded and the /scan change dynamically + if msg: + params = msg.data.split(' ') + if '-i' in params: # 'intensities' + self.rospy.set_param('rplidar_params/intensities', float(params[1 + params.index('-i')])) + self.rospy.loginfo(f'[{self.name}] "rplidar_params/intensities" set to "{self.rospy.get_param("rplidar_params/intensities")}"') + if '-s' in params: # 'samples' + self.rospy.set_param('rplidar_params/output_samples', (params[1 + params.index('-s')])) + self.rospy.loginfo(f'[{self.name}] "rplidar_params/output_samples" set to "{self.rospy.get_param("rplidar_params/output_samples")}"') + if '-m' in params: # 'reduction method' + self.rospy.set_param('rplidar_params/reduction_method', str(params[1 + params.index('-m')])) + self.rospy.loginfo(f'[{self.name}] "rplidar_params/reduction_method" set to "{self.rospy.get_param("rplidar_params/reduction_method")}"') + if '-min' in params: # 'range["min"]' + self.rospy.set_param('rplidar_params/range/min', float(params[1 + params.index('-min')])) + self.rospy.loginfo(f'[{self.name}] "rplidar_params/range/min" set to "{self.rospy.get_param("rplidar_params/range/min")}"') + if '-max' in params: # 'range["max"]' + self.rospy.set_param('rplidar_params/range/max', float(params[1 + params.index('-max')])) + self.rospy.loginfo(f'[{self.name}] "rplidar_params/range/max" set to "{self.rospy.get_param("rplidar_params/range/max")}"') + if '-f' in params: # 'fov' + self.rospy.set_param('rplidar_params/fov', int(params[1 + params.index('-f')])) + self.rospy.loginfo(f'[{self.name}] "rplidar_params/fov" set to "{self.rospy.get_param("rplidar_params/fov")}"') + if '-r' in params: # 'rotation' + self.rospy.set_param('rplidar_params/rotation', int(params[1 + params.index('-r')])) + self.rospy.loginfo(f'[{self.name}] "rplidar_params/rotation" set to "{self.rospy.get_param("rplidar_params/rotation")}"') + + self.softReload() + + def start(self): + self.running_flag = True + self.main() + + def softReload(self): + self.rospy.logwarn(f'[{self.name}] waiting end of current loop to reload, delay for first /scan publish expected') + # while self.callback_flag: + # continue + self.running_flag = False + self.initVariables() + self.initParams() + self.start() + + def main(self): + while not self.rospy.is_shutdown(): + if not self.callback_flag or not self.running_flag: + # commit this sleep to achieve minimal delay, but maybe overrate comparisons + # self.rate.sleep() + continue + + # setup parameters on start every reload + if not self.reading_params_flag and not self.ready_flag: + self.reading_params_flag = True + self.defineParams(self.lidar_raw_ranges) + self.ready_flag = True + # this section only will run again if a problem occur + # proceed to error detection, reduction, mask and publish looping + + if not self.ready_flag: + self.rate.sleep() + continue + + # this section prevent error related to inconsistent raw sampling number + if self.length_ratio and self.output_samples_length > self.lidar_raw_ranges_length and not self.length_error_flag: + self.output_samples_length = self.lidar_raw_ranges_length + self.length_ratio, self.reduction_position = 1, 1 + + self.rospy.logwarn( + f'[{self.name}] output_samples_length is bigger then raw_samples, ' + f'[{self.name}] output_samples_length set to max_value for this method [{self.lidar_raw_ranges_length}]') + self.rate.sleep() + continue + + # this section recover from error related to inconsistent raw sampling number + if self.output_samples_length == self.lidar_raw_ranges_length and self.length_error_flag: + self.length_error_flag = False + self.output_samples_length = int(rospy.get_param('rplidar_params/output_samples')) + self.rospy.logwarn(f'[{self.name}] output_samples_length returned to user defined length [{self.output_samples_length}]') + self.softReload() + self.rate.sleep() + continue + + # synchronize frame updates for mapping and navigation + self.lidar_redata.header = self.lidar_raw_msg.header + + # reduce and mask out fov values + if self.reduction: + reduced_ranges = self.reduction(nparray(self.lidar_raw_ranges), *self.reduce_fixed_params) + self.lidar_redata.ranges = concatenate((reduced_ranges[0:self.min_mask_index], self.mask_values_array, reduced_ranges[self.max_mask_index:-1])) + elif any(self.mask_values_array): + self.lidar_redata.ranges = concatenate((self.lidar_raw_ranges[0:self.min_mask_index], self.mask_values_array, self.lidar_raw_ranges[self.max_mask_index:-1])) + else: + self.lidar_redata.ranges = self.lidar_raw_msg.ranges + + # mask range + for sample_index in range(len(self.lidar_redata.ranges)): + if not (self.lidar_range['min'] < self.lidar_redata.ranges[sample_index] < self.lidar_range['max']): + self.lidar_redata.ranges[sample_index] = inf + + # publish section + # self.lidar_pub.publish(self.lidar_redata) + + try: + # if time.time()%1 >0.9: + # self.rospy.logerr("overflow error") + # self.softReload() + # continue + self.lidar_pub.publish(self.lidar_redata) + except OverflowError: # probably this error is associated with high main rate + self.rospy.logerr(f'[{self.name}] OverflowError: float too large to pack with f format') + self.softReload() + continue + + # sweet dreams + self.rate.sleep() + self.callback_flag = False + self.__exit__() + + def __exit__(self): + quit() + + +if __name__ == '__main__': + try: + rplidar_scan = RPLIDAR_SCAN('RPLIDAR_SCAN') + except rospy.ROSInterruptException or KeyboardInterrupt: + quit() diff --git a/scripts/rplidar_scan_orin.py b/scripts/rplidar_scan_orin.py new file mode 100644 index 00000000..cb4d53db --- /dev/null +++ b/scripts/rplidar_scan_orin.py @@ -0,0 +1,301 @@ +#!/usr/bin/env python3 +import rospy +from sensor_msgs.msg import LaserScan, ChannelFloat32 +from std_msgs.msg import String +from numpy import concatenate +from copy import deepcopy +from numpy import array as nparray +from math import pi, radians +from pooling import * +from resample import * +import time + + +class RPLIDAR_SCAN: + def __init__(self, name='RPLIDAR_SCAN', autostart=True, rate=10, log_level=rospy.INFO): + self.name = name + self.rospy = rospy + self.rospy.init_node(self.name, anonymous=True, log_level=log_level) + self.rospy.loginfo("[%s] Starting Node ", self.name) + self.rate = self.rospy.Rate(rate) + + self.initParams() + self.initVariables() + self.initPublishers() + self.initSubscribers() + + if autostart: + self.start() + + def initParams(self): + self.lidar_intensities = float(rospy.get_param('rplidar_params/intensities')) + self.output_samples_length = rospy.get_param('rplidar_params/output_samples') + self.reduction_method = str(rospy.get_param('rplidar_params/reduction_method')).lower() + self.lidar_range = dict(rospy.get_param('rplidar_params/range')) + self.lidar_fov = int(rospy.get_param('rplidar_params/fov')) + self.lidar_rotation = int(rospy.get_param('rplidar_params/rotation')) + self.mask_value = inf + self.reduction = None + self.length_ratio = None + self.reduction_position = None + self.lidar_raw_ranges_length = None + self.reduce_fixed_params = None + + if self.lidar_intensities < 0: + self.rospy.logerr(f'[{self.name}] lidar_intensities cant admit negative values') + raise TypeError(f'[{self.name}] lidar_intensities cant admit negative values') + + try: + if self.output_samples_length < 0: + self.rospy.logerr(f'[{self.name}] output_samples_length cant admit negative values') + raise TypeError(f'[{self.name}] output_samples_length cant admit negative values') + except TypeError or ValueError: + self.rospy.logwarn(f'[{self.name}] [output_samples] not informed or invalid, value will be set to [raw_samples_length] and [reduction_method] set to [None]') + self.output_samples_length = None + self.reduction_method = None + + if self.lidar_range['min'] < 0 or self.lidar_range['max'] < 0: + self.rospy.logerr(f'[{self.name}] lidar_range cant admit negative values') + raise TypeError(f'[{self.name}] lidar_range cant admit negative values') + if self.lidar_fov < 0 or self.lidar_fov > 360: + self.rospy.logerr(f'[{self.name}] lidar_fov only admit values between 0~360') + raise TypeError(f'[{self.name}] lidar_fov only admit values between 0~360') + if self.lidar_rotation < -180 or self.lidar_rotation > 180: + self.rospy.logerr(f'[{self.name}] lidar_rotation only admit values between -180~180') + raise TypeError(f'[{self.name}] lidar_rotation only admit values between -180~180') + + def defineParams(self, range_samples): + self.lidar_raw_ranges_length = len(range_samples) + self.rospy.loginfo( + f'[{self.name}] ##### rplidar samples read! [{self.lidar_raw_ranges_length}] raw samples! #####') + + # auto set samples if None + if not self.output_samples_length: + self.output_samples_length = self.lidar_raw_ranges_length + self.length_ratio = self.lidar_raw_ranges_length / self.output_samples_length + + if not (0 < self.lidar_fov < 360) or not self.output_samples_length: + self.min_mask_index, self.max_mask_index = None, None + self.mask_values_array = nparray(tuple()) + else: + # define indexes to mask method + self.min_mask_index = round(self.output_samples_length * (radians(self.lidar_fov / (4 * pi)))) + self.max_mask_index = round(self.output_samples_length * (1 - radians(self.lidar_fov / (4 * pi)))) + self.mask_values_array = nparray(int(self.max_mask_index - self.min_mask_index) * [self.mask_value]) + + # this section define parameters and function to match selected reduction method + if not self.reduction_method or 'none' in self.reduction_method: + self.reduction_method = None + else: + if not('pooling' in self.reduction_method or 'resample' in self.reduction_method): + self.rospy.logerr(f'[{self.name}] reduction_method only admit "[max/min/mean/(int)]-pooling", "resample", or "None"') + raise TypeError(f'[{self.name}] reduction_method only admit "[max/min/mean/(int)]-pooling", "resample", or "None"') + + try: + # resample method + if 'resample' in self.reduction_method: + self.reduction = resample + self.reduce_fixed_params = tuple([self.output_samples_length]) + + # cases bellow refer to max/min/mean pooling method + elif 'max' in self.reduction_method: + self.reduction = max_pooling + self.reduce_fixed_params = (self.length_ratio, self.lidar_raw_ranges_length) + elif 'min' in self.reduction_method: + self.reduction = min_pooling + self.reduce_fixed_params = (self.length_ratio, self.lidar_raw_ranges_length) + elif 'mean' in self.reduction_method: + self.reduction = mean_pooling + self.reduce_fixed_params = (self.length_ratio, self.lidar_raw_ranges_length) + + # next case refer to position pooling method + elif type(int(self.reduction_method.split('-')[0])) is int: + self.reduction = position_pooling + self.reduction_position = int(self.reduction_method.split('-')[0]) + self.reduction_sections = nparray(range(0, self.lidar_raw_ranges_length, int(self.length_ratio))) + self.reduce_fixed_params = (self.reduction_position, self.reduction_sections) + + else: + raise TypeError + + except TypeError: + TypeError(f'[{self.name}] pooling reduction method need [max/min/mean/(int)] information') + + self.rospy.loginfo(f'[{self.name}] * PARAMETERS *\n' + f' * fov: [{self.lidar_fov}]\n' + f' * min_mask_index: [{self.min_mask_index}]\n' + f' * max_mask_index: [{self.max_mask_index}]\n' + f' * output_samples_length: [{self.output_samples_length}]\n' + f' * fov_samples(length): [{self.output_samples_length - len(self.mask_values_array)}]\n' + f' * mask_samples(length): [{len(self.mask_values_array)}]\n' + f' * reduction_method: {self.reduction_method}') + + self.lidar_redata.angle_min = -pi + radians(self.lidar_rotation) + self.lidar_redata.angle_max = pi + radians(self.lidar_rotation) + self.lidar_redata.angle_increment = 2 * pi / self.output_samples_length + self.lidar_redata.time_increment = self.lidar_raw_msg.time_increment * self.length_ratio + + self.lidar_redata.range_min = self.lidar_range['min'] + self.lidar_redata.range_max = self.lidar_range['max'] + self.lidar_redata.intensities = nparray(self.output_samples_length * [self.lidar_intensities]) + + if self.reduction_position: + if self.length_ratio % 1 > 0: + self.rospy.logerr( + f'[{self.name}] input_array length [{self.lidar_raw_ranges_length}] % pooling value [{self.length_ratio}] > 0') + raise TypeError(f'[{self.name}] input_array length % pooling value > 0') + if self.reduction_position > self.length_ratio: + self.rospy.logerr( + f'[{self.name}] reduction_position [{self.reduction_position}] > length_ratio value [{self.length_ratio}]') + raise TypeError( + f'[{self.name}] reduction_position [{self.reduction_position}] > length_ratio value [{self.length_ratio}]') + + def initVariables(self): + self.lidar_raw_msg = LaserScan() + self.lidar_redata = LaserScan() + self.callback_flag = False + self.running_flag = False + self.reading_params_flag = False + self.ready_flag = False + self.length_error_flag = False + + def initPublishers(self): + self.lidar_pub = self.rospy.Publisher('/scan', LaserScan, queue_size=2) + + def initSubscribers(self): + self.lidar_sub = self.rospy.Subscriber("/raw_scan", LaserScan, self.lidarCallback, queue_size=2) + self.lidar_params_sub = self.rospy.Subscriber('/scan_params', String, self.lidarParamsCallback, queue_size=1) + + def lidarCallback(self, msg): + if any(msg.ranges): + self.lidar_raw_msg = msg + self.lidar_raw_ranges = deepcopy(self.lidar_raw_msg.ranges) + self.callback_flag = True + return + + def lidarParamsCallback(self, msg): + # this function update parameters thought a string message callback + # parameters and variables are reloaded and the /scan change dynamically + if msg: + params = msg.data.split(' ') + if '-i' in params: # 'intensities' + self.rospy.set_param('rplidar_params/intensities', float(params[1 + params.index('-i')])) + self.rospy.loginfo(f'[{self.name}] "rplidar_params/intensities" set to "{self.rospy.get_param("rplidar_params/intensities")}"') + if '-s' in params: # 'samples' + self.rospy.set_param('rplidar_params/output_samples', (params[1 + params.index('-s')])) + self.rospy.loginfo(f'[{self.name}] "rplidar_params/output_samples" set to "{self.rospy.get_param("rplidar_params/output_samples")}"') + if '-m' in params: # 'reduction method' + self.rospy.set_param('rplidar_params/reduction_method', str(params[1 + params.index('-m')])) + self.rospy.loginfo(f'[{self.name}] "rplidar_params/reduction_method" set to "{self.rospy.get_param("rplidar_params/reduction_method")}"') + if '-min' in params: # 'range["min"]' + self.rospy.set_param('rplidar_params/range/min', float(params[1 + params.index('-min')])) + self.rospy.loginfo(f'[{self.name}] "rplidar_params/range/min" set to "{self.rospy.get_param("rplidar_params/range/min")}"') + if '-max' in params: # 'range["max"]' + self.rospy.set_param('rplidar_params/range/max', float(params[1 + params.index('-max')])) + self.rospy.loginfo(f'[{self.name}] "rplidar_params/range/max" set to "{self.rospy.get_param("rplidar_params/range/max")}"') + if '-f' in params: # 'fov' + self.rospy.set_param('rplidar_params/fov', int(params[1 + params.index('-f')])) + self.rospy.loginfo(f'[{self.name}] "rplidar_params/fov" set to "{self.rospy.get_param("rplidar_params/fov")}"') + if '-r' in params: # 'rotation' + self.rospy.set_param('rplidar_params/rotation', int(params[1 + params.index('-r')])) + self.rospy.loginfo(f'[{self.name}] "rplidar_params/rotation" set to "{self.rospy.get_param("rplidar_params/rotation")}"') + + self.softReload() + + def start(self): + self.running_flag = True + self.main() + + def softReload(self): + self.rospy.logwarn(f'[{self.name}] waiting end of current loop to reload, delay for first /scan publish expected') + # while self.callback_flag: + # continue + self.running_flag = False + self.initVariables() + self.initParams() + self.start() + + def main(self): + while not self.rospy.is_shutdown(): + if not self.callback_flag or not self.running_flag: + # commit this sleep to achieve minimal delay, but maybe overrate comparisons + # self.rate.sleep() + continue + + # setup parameters on start every reload + if not self.reading_params_flag and not self.ready_flag: + self.reading_params_flag = True + self.defineParams(self.lidar_raw_ranges) + self.ready_flag = True + # this section only will run again if a problem occur + # proceed to error detection, reduction, mask and publish looping + + if not self.ready_flag: + self.rate.sleep() + continue + + # this section prevent error related to inconsistent raw sampling number + if self.length_ratio and self.output_samples_length > self.lidar_raw_ranges_length and not self.length_error_flag: + self.output_samples_length = self.lidar_raw_ranges_length + self.length_ratio, self.reduction_position = 1, 1 + + self.rospy.logwarn( + f'[{self.name}] output_samples_length is bigger then raw_samples, ' + f'[{self.name}] output_samples_length set to max_value for this method [{self.lidar_raw_ranges_length}]') + self.rate.sleep() + continue + + # this section recover from error related to inconsistent raw sampling number + if self.output_samples_length == self.lidar_raw_ranges_length and self.length_error_flag: + self.length_error_flag = False + self.output_samples_length = int(rospy.get_param('rplidar_params/output_samples')) + self.rospy.logwarn(f'[{self.name}] output_samples_length returned to user defined length [{self.output_samples_length}]') + self.softReload() + self.rate.sleep() + continue + + # synchronize frame updates for mapping and navigation + self.lidar_redata.header = self.lidar_raw_msg.header + + # reduce and mask out fov values + if self.reduction: + reduced_ranges = self.reduction(nparray(self.lidar_raw_ranges), *self.reduce_fixed_params) + self.lidar_redata.ranges = concatenate((reduced_ranges[0:self.min_mask_index], self.mask_values_array, reduced_ranges[self.max_mask_index:-1])) + elif any(self.mask_values_array): + self.lidar_redata.ranges = concatenate((self.lidar_raw_ranges[0:self.min_mask_index], self.mask_values_array, self.lidar_raw_ranges[self.max_mask_index:-1])) + else: + self.lidar_redata.ranges = self.lidar_raw_msg.ranges + + # mask range + for sample_index in range(len(self.lidar_redata.ranges)): + if not (self.lidar_range['min'] < self.lidar_redata.ranges[sample_index] < self.lidar_range['max']): + self.lidar_redata.ranges[sample_index] = inf + + # publish section + # self.lidar_pub.publish(self.lidar_redata) + + try: + # if time.time()%1 >0.9: + # self.rospy.logerr("overflow error") + # self.softReload() + # continue + self.lidar_pub.publish(self.lidar_redata) + except OverflowError: # probably this error is associated with high main rate + self.rospy.logerr(f'[{self.name}] OverflowError: float too large to pack with f format') + self.softReload() + continue + + # sweet dreams + self.rate.sleep() + self.callback_flag = False + self.__exit__() + + def __exit__(self): + quit() + + +if __name__ == '__main__': + try: + rplidar_scan = RPLIDAR_SCAN('RPLIDAR_SCAN') + except rospy.ROSInterruptException or KeyboardInterrupt: + quit() diff --git a/scripts/scan_filter_V1.0.py b/scripts/scan_filter_V1.0.py new file mode 100644 index 00000000..0d5c3690 --- /dev/null +++ b/scripts/scan_filter_V1.0.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 + +#Esse script verifica se a proporção de amostras com valor inf em uma mensagem do lidar é maior que o aceitável, caso for maior, a mensagem anterior será republicada. +#Como utilizamos apenas 66% da visão do lidar, o valor mínimo de proporção é 34%, onde qualquer valor inf a mais além do pré-ajustado na mensagem fará com que a mesma seja ignorada. +import rospy +import numpy as np +from sensor_msgs.msg import LaserScan + +class scanFilter: + def __init__(self): + self.rospy = rospy + rospy.init_node("scan_filter_node") + self.initVariables() + self.initSubscribers() + self.initPublishers() + self.main() + + def initVariables(self): + self.last_scan = LaserScan() + self.proportion = 0.50 + self.deleted_counter = 0 + self.general_counter = 0 + self.erro_percentil = 0 + + def initSubscribers(self): + self.indices_sub = rospy.Subscriber("/scan_unfiltered", LaserScan, self.lidar_callback) + + def initPublishers(self): + self.filtered_scan_pub = rospy.Publisher("/scan", LaserScan, queue_size=50) + + def lidar_callback(self, msg): + scan_data = np.array(msg.ranges) + + + inf_count = np.sum(np.isinf(scan_data)) + total_count = len(scan_data) + + + if ((inf_count/total_count) < self.proportion): + self.last_scan = msg + self.filtered_scan_pub.publish(self.last_scan) + self.general_counter = self.general_counter + 1 + + else: + if self.last_scan is not None: + self.last_scan.header.stamp = self.rospy.Time.now() + self.filtered_scan_pub.publish(self.last_scan) + self.deleted_counter = self.deleted_counter + 1 + self.general_counter = self.general_counter + 1 + print("numero de mensagens excluidas: ", self.deleted_counter) + self.erro_percentil = (self.deleted_counter/self.general_counter)*100 + print("percentual de erro: ",self.erro_percentil) + + + + def main(self): + while not self.rospy.is_shutdown(): + + + rospy.spin() + +if __name__ == '__main__': + try: + scanFilter() + except rospy.ROSInterruptException: + pass \ No newline at end of file diff --git a/scripts/scan_filter_V1.1.py b/scripts/scan_filter_V1.1.py new file mode 100644 index 00000000..301b2b5c --- /dev/null +++ b/scripts/scan_filter_V1.1.py @@ -0,0 +1,70 @@ +#!/usr/bin/env python3 + +import rospy +import numpy as np +from sensor_msgs.msg import LaserScan +import threading +import time + +class scanFilter: + def __init__(self): + self.rospy = rospy + rospy.init_node("scan_filter_node") + self.initVariables() + self.initSubscribers() + self.initPublishers() + + # Iniciar a thread para imprimir erro_percentil a cada 10 segundos + self.print_thread = threading.Thread(target=self.print_variable_every_10_seconds) + self.print_thread.daemon = True + self.print_thread.start() + + self.main() + + def initVariables(self): + self.last_scan = LaserScan() + self.proportion = 0.70 + self.deleted_counter = 0 + self.general_counter = 0 + self.erro_percentil = 0 + + def initSubscribers(self): + self.indices_sub = rospy.Subscriber("/scan_unfiltered", LaserScan, self.lidar_callback) + + def initPublishers(self): + self.filtered_scan_pub = rospy.Publisher("/scan", LaserScan, queue_size=50) + + def lidar_callback(self, msg): + scan_data = np.array(msg.ranges) + + inf_count = np.sum(np.isinf(scan_data)) + total_count = len(scan_data) + + if (inf_count / total_count) < self.proportion: + self.last_scan = msg + self.filtered_scan_pub.publish(self.last_scan) + self.general_counter += 1 + else: + if self.last_scan is not None: + self.last_scan.header.stamp = self.rospy.Time.now() + self.filtered_scan_pub.publish(self.last_scan) + self.deleted_counter += 1 + self.general_counter += 1 + # print("numero de mensagens excluidas: ", self.deleted_counter) + self.erro_percentil = (self.deleted_counter / self.general_counter) * 100 + # print("percentual de erro: ", self.erro_percentil) + + def print_variable_every_10_seconds(self): + while not rospy.is_shutdown(): + print("percentual de erro: ", self.erro_percentil) + time.sleep(10) + + def main(self): + while not self.rospy.is_shutdown(): + rospy.spin() + +if __name__ == '__main__': + try: + scanFilter() + except rospy.ROSInterruptException: + pass diff --git a/scripts/scan_filter_interpolator_V1.0.py b/scripts/scan_filter_interpolator_V1.0.py new file mode 100644 index 00000000..88f8091b --- /dev/null +++ b/scripts/scan_filter_interpolator_V1.0.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python3 + +import rospy +import numpy as np +from sensor_msgs.msg import LaserScan +from scipy.interpolate import interp1d + +class scanFilter: + def __init__(self): + self.rospy = rospy + rospy.init_node("scan_filter_node") + self.initVariables() + self.initSubscribers() + self.initPublishers() + self.main() + + def initVariables(self): + self.last_scan = LaserScan() + self.proportion = 0.5 + self.regions = [(0,658), (1315,1972)] + + def initSubscribers(self): + self.indices_sub = rospy.Subscriber("/scan_unfiltered", LaserScan, self.lidar_callback) + + def initPublishers(self): + self.filtered_scan_pub = rospy.Publisher("/scan", LaserScan, queue_size=50) + + def lidar_callback(self, msg): + scan_data = np.array(msg.ranges) + print("resultado antes da interpolação: ", msg.ranges[95:125]) + inf_count = np.sum(np.isinf(scan_data)) + total_count = len(scan_data) + + if ((inf_count/total_count) < self.proportion): + self.last_scan = msg + + self.last_scan.ranges = self.lidar_interpolator(scan_data) + print("resultado depois da interpolação: ", self.last_scan.ranges[95:125]) + self.filtered_scan_pub.publish(self.last_scan) + + else: + if self.last_scan is not None: + self.filtered_scan_pub.publish(self.last_scan) + + def lidar_interpolator(self, ranges): + + for region in self.regions: + start_index, end_index = region + + region_ranges = ranges[start_index:end_index] + + valid = np.isfinite(region_ranges) + + invalid = ~valid + + if np.sum(valid) == 0: + continue + + interpolate_array = interp1d(np.where(valid)[0], region_ranges[valid], kind = 'linear', bounds_error=False, fill_value='extrapolate') + + region_ranges[invalid] = interpolate_array(np.where(invalid)[0]) + + ranges[start_index:end_index] = region_ranges + + return ranges + + def main(self): + while not self.rospy.is_shutdown(): + + rospy.spin() + +if __name__ == '__main__': + try: + scanFilter() + except rospy.ROSInterruptException: + pass \ No newline at end of file diff --git a/scripts/scan_filter_interpolator_V1.1.py b/scripts/scan_filter_interpolator_V1.1.py new file mode 100644 index 00000000..196010d6 --- /dev/null +++ b/scripts/scan_filter_interpolator_V1.1.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 + +import rospy +import numpy as np +from sensor_msgs.msg import LaserScan +from scipy.interpolate import interp1d +from math import pi, radians + +class scanFilter: + def __init__(self): + self.rospy = rospy + rospy.init_node("scan_filter_node") + self.initVariables() + self.initSubscribers() + self.initPublishers() + self.main() + + def initVariables(self): + self.last_scan = LaserScan() + self.proportion = 0.5 + self.lidar_fov = 240 + self.regions = [(0,0),(0,0)] + + + def initSubscribers(self): + self.indices_sub = rospy.Subscriber("/scan_unfiltered", LaserScan, self.lidar_callback) + + def initPublishers(self): + self.filtered_scan_pub = rospy.Publisher("/scan", LaserScan, queue_size=50) + + def lidar_callback(self, msg): + scan_data = np.array(msg.ranges) + + inf_count = np.sum(np.isinf(scan_data)) + total_count = len(scan_data) + + min_mask_index = round(total_count * (radians(self.lidar_fov / (4 * pi)))) + max_mask_index = round(total_count * (1 - radians(self.lidar_fov / (4 * pi)))) + + print("o valor de min_mask é: ", min_mask_index) + print("o valor de max_mask é: ", max_mask_index) + + self.regions = [(0,min_mask_index), (max_mask_index,total_count)] + + print("As regiões sao: ", self.regions) + + if ((inf_count/total_count) < self.proportion): + self.last_scan = msg + + self.last_scan.ranges = self.lidar_interpolator(scan_data) + + self.filtered_scan_pub.publish(self.last_scan) + + else: + if self.last_scan is not None: + self.filtered_scan_pub.publish(self.last_scan) + + def lidar_interpolator(self, ranges): + + for region in self.regions: + start_index, end_index = region + + region_ranges = ranges[start_index:end_index] + + valid = np.isfinite(region_ranges) + + invalid = ~valid + + if np.sum(valid) == 0: + continue + + interpolate_array = interp1d(np.where(valid)[0], region_ranges[valid], kind = 'linear', bounds_error=False, fill_value='extrapolate') + + region_ranges[invalid] = interpolate_array(np.where(invalid)[0]) + + ranges[start_index:end_index] = region_ranges + + return ranges + + def main(self): + while not self.rospy.is_shutdown(): + + rospy.spin() + +if __name__ == '__main__': + try: + scanFilter() + except rospy.ROSInterruptException: + pass \ No newline at end of file diff --git a/scripts/scan_filter_regressor_V1.0.py b/scripts/scan_filter_regressor_V1.0.py new file mode 100644 index 00000000..8635a3d8 --- /dev/null +++ b/scripts/scan_filter_regressor_V1.0.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python3 + +import rospy +import numpy as np +from sensor_msgs.msg import LaserScan +from math import pi, radians +from sklearn.impute import KNNImputer + +class scanFilter: + def __init__(self): + self.rospy = rospy + rospy.init_node("scan_filter_node") + self.initVariables() + self.initSubscribers() + self.initPublishers() + self.main() + + def initVariables(self): + self.last_scan = LaserScan() + self.proportion = 0.5 + self.lidar_fov = 240 + self.regions = [(0,0),(0,0)] + + + def initSubscribers(self): + self.indices_sub = rospy.Subscriber("/scan_unfiltered", LaserScan, self.lidar_callback) + + def initPublishers(self): + self.filtered_scan_pub = rospy.Publisher("/scan", LaserScan, queue_size=50) + + def lidar_callback(self, msg): + scan_data = np.array(msg.ranges) + + inf_count = np.sum(np.isinf(scan_data)) + total_count = len(scan_data) + + min_mask_index = round(total_count * (radians(self.lidar_fov / (4 * pi)))) + max_mask_index = round(total_count * (1 - radians(self.lidar_fov / (4 * pi)))) + + self.regions = [(0,min_mask_index), (max_mask_index,total_count)] + + if ((inf_count/total_count) < self.proportion): + self.last_scan = msg + # print("Antes da regressão: ", scan_data[90:130]) + + self.last_scan.ranges = self.lidar_regressor(scan_data) + + # print("Depois da regressão: ", self.last_scan[90:130]) + + self.filtered_scan_pub.publish(self.last_scan) + + else: + if self.last_scan is not None: + self.filtered_scan_pub.publish(self.last_scan) + + def lidar_regressor(self, ranges): + + for region in self.regions: + start_index, end_index = region + + region_ranges = ranges[start_index:end_index].reshape(-1,1) + + region_ranges[~np.isfinite(region_ranges)] = np.nan + + imputer = KNNImputer(n_neighbors=5) + interpolated_region = imputer.fit_transform(region_ranges) + + ranges[start_index:end_index] = interpolated_region.ravel() + + return ranges + + def main(self): + while not self.rospy.is_shutdown(): + + rospy.spin() + +if __name__ == '__main__': + try: + scanFilter() + except rospy.ROSInterruptException: + pass \ No newline at end of file diff --git a/src/node.cpp b/src/node.cpp index 6639b82e..cf20afdf 100644 --- a/src/node.cpp +++ b/src/node.cpp @@ -239,7 +239,7 @@ int main(int argc, char * argv[]) { float max_distance; double scan_frequency; ros::NodeHandle nh; - ros::Publisher scan_pub = nh.advertise("scan", 1000); + ros::Publisher scan_pub = nh.advertise("raw_scan", 1000); ros::NodeHandle nh_private("~"); nh_private.param("channel_type", channel_type, "serial"); nh_private.param("tcp_ip", tcp_ip, "192.168.0.7");