0

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()
Adid
  • 1,504
  • 3
  • 13
AmirulJ
  • 118
  • 1
  • 11
  • What is the error? – Adid Jan 05 '22 at 08:44
  • its said that the matrix rows is not the same size @Adid – AmirulJ Jan 05 '22 at 08:46
  • Could you edit your post to include the full error message? – Adid Jan 05 '22 at 08:47
  • I dont know why I cant edit this question. when I press edit, this message pops out "Approve, reject, or improve this suggested edit.' @Adid – AmirulJ Jan 05 '22 at 10:37
  • Instead I'll share here the error `x2 = matrix('x22 x22(2:360;1) x22(3:360;1:2) x22(4:360;1:3) x22(5:360;1:4)') File "/usr/lib/python2.7/dist-packages/numpy/matrixlib/defmatrix.py", line 267, in __new__ data = _convert_from_string(data) File "/usr/lib/python2.7/dist-packages/numpy/matrixlib/defmatrix.py", line 61, in _convert_from_string raise ValueError("Rows not the same size.") ValueError: Rows not the same size.` Thank you! @Adid – AmirulJ Jan 05 '22 at 10:38
  • Hi @Adid can you help me with this question https://stackoverflow.com/q/70601911/16594158 – AmirulJ Jan 06 '22 at 02:50

0 Answers0