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()