I would like to improve the LDS-01 sensor of the turtlebot3 by subscribing the message.ranges, modify the messange.ranges by applying some algorithm to the message and publish it to the new topic as shown below. But there are an error when I run the coding.
Before this, I have run the same coding as How to subscribe "/scan" topic, modify the messages and publish to the new topic? and there is no error to run this code. I think this is because the algorithm is only compute one by one of the message.ranges which is from 1; 2; 3; until 360 degree. It means that the matrix of the message.ranges is 360 x 1.
But for the coding below, the algorithm needs to compute five value at one times which is 1 2 3 4 5; 2 3 4 5 6; 3 4 5 6 7; until 360 1 2 3 4 degree again since lds-01 sensor is 360 degree. It means that the matrix of the message.ranges needs to change from 360 x 1 to 360 x 5. But there is an error when I run the coding below.
How to solve this problem and is there anyone can help with my coding? Thank you in advance!
#! /usr/bin/env python
import rospy
from sensor_msgs.msg import LaserScan
from numpy import *
import numpy as np
def callback(msg):
x1 = msg.ranges
x22 = np.array(x1,dtype=float)
x2 = matrix('x22 x22(2:360;1) x22(3:360;1:2) x22(4:360;1:3) x22(5:360;1:4)')
#input 1
x1_xoffset = matrix ('0;0;0;0;0')
x1_gain = matrix ('0.0307564548089385;0.0381097564300047;0.0308794477170062;0.0306353793381491;0.0324886282550904')
x1_ymin = -1
#layer 1
b1 = matrix ('-3.0022961030147494732;1.1284344283916103446;10.262689092808290781;-0.29108190341738160445;1.6910182135862883435;-1.4322186646812866684;0.52038673092297094147;0.18373415980376742174;-1.341864158161501841;8.9924977389929185989;-6.5122750450398561028;1.610769930183955978;-2.7072823580198375204;-3.071861929436806804;2.089532698983309178')
IW1_1 = matrix ('1.1763831014323093971 -0.022205589873535169082 1.3591906653400733784 0.35171992356252657075 -0.090715245174806585782;-0.33137985382807932933 0.7589349385846153595 -0.79263062237043191427 -3.0721179898719421786 -0.48109484027945054185;-2.0429705214887010634 -1.4215783235189332068 -2.2199535121273483718 5.6383715387809294484 9.3956079158810990037;-1.0582400260177218243 2.2756497752355073771 -1.4894441143471608413 -0.61366692742938355742 -1.7609538885010909137;0.32284523401768933093 -2.3716027653122901953 2.9198002838288754646 1.6727926692167303102 1.4016343126251111784;0.64284592426311937263 1.9670290884990859759 -0.8613791137489327232 -0.76088524923761946539 -0.5846670508779477915;-1.0662823986094978057 -0.38913892204807792874 -1.9927038658686935246 -0.89079948781370843491 -0.062953202997214421921;1.3836374075598649735 0.82293783803825726331 -1.0449519484800251501 2.2182027529674033239 1.9552752800894290797;2.1909981502076187887 -2.7456420582638014771 -1.0375844062452397321 -1.8757795009105702189 -0.76052914811528571359;5.0141311791138925003 3.8600626154505972565 3.7213564064231658968 3.1907998160592647707 -5.4753580388207421237;-7.3738127265306703251 0.81690452601147378608 -1.848533301150358632 1.4523073997719444517 -0.66541668998457348394;-0.41953258579903940362 -1.8045840188098498658 1.3538831558038899594 0.82232112784155764196 -0.036636531083722792546;-0.28307053443826624139 0.97238373514558673616 -0.44631585409406204779 1.395531629393004236 -0.91460564492552842708;-0.24800624331256537758 -0.12091709836330821748 0.57989534406924081456 1.2276541890871852658 -0.80248852007095816674;-0.027878630077996850029 -1.4092029396990795043 -1.5425528632120226735 -1.6945480178338363508 -0.066421346305691020273')
#layer 2
b2 = matrix ('-3.3718607680138723559')
LW2_1 = matrix ('1.1577237841084386805 3.1188616770417478818 11.377170228092280624 2.0941520852461419366 -5.8125506022667261519 1.8927565783053732495 2.1715202069861394563 -3.9047604811323477492 -2.558429670659956745 8.1105881568708060314 -7.0980360301043301519 -3.1203137130883740191 0.91759847912706682393 2.0087541762502567622 1.2751834061996529801')
# Output 1
y1_ymin = -1
y1_gain = 2.85714285714286
y1_xoffset = 0
#input 1
xp1 = ((x2 - x1_xoffset)*(x1_gain))+x1_ymin
#layer 1
n1 = IW1_1*xp1+b1
a1 = 2/(1+exp(-2*n1))-1
#layer 2
a2 = LW2_1*a1+b2
# Output 1
y1 = ((a2-y1_ymin)/(y1_gain))+y1_xoffset
y2 = y1.tolist()[0]
scan.header = msg.header
scan.angle_min = msg.angle_min
scan.angle_max = msg.angle_max
scan.angle_increment = msg.angle_increment
scan.time_increment = msg.time_increment
scan.scan_time = msg.scan_time
scan.range_min = msg.range_min
scan.range_max = msg.range_max
scan.ranges = y2
scan.intensities = msg.intensities
pub.publish(scan)
rospy.init_node("ann_realdata_publisher")
sub = rospy.Subscriber('/scan', LaserScan, callback)
pub = rospy.Publisher('/scan_realdata', LaserScan, queue_size=1)
rate = rospy.Rate(2)
scan = LaserScan()
rospy.spin()