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 and publish it to the new topic as shown below. But there are an error when I run the coding. The error is
May I know why? 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
x2 = np.array(x1,dtype=float)
#input 1
x1_xoffset = 0.1656566113
x1_gain = 0.599818245108751
x1_ymin = -1
#layer 1
b1 = matrix ('-326.80027082597098342;-98.990085738415288574;83.870582554028558775;-72.368630055558369918;-0.7092849798153298968;57.269078212651997717;44.310310623213219117;-31.707975756385888388;-17.911078072875334044;-4.6394718363325218036')
IW1_1 = matrix ('337.00974203853263589;112.59115212423027685;-110.55255345932357613;113.22379022448127728;1.3919097878355670694;-110.07199451465535844;-110.63343956448693461;112.90214029452344846;111.7555578995358303;114.3437445151164269')
#layer 2
b2 = matrix ('-2.978824073708045072e-05')
LW2_1 = matrix ('0.030307489543199929438 0.060616873246115143825 -0.060618376238034066272 0.060619146713019073092 -0.00016486744836432447401 -0.060619845018730772468 -0.060619496805093056602 0.060618473675156582525 0.060617195348181586445 0.060615509952774870861')
# Output 1
y1_ymin = -1
y1_gain = 0.606060606060606
y1_xoffset = 0.2
#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()
scan.ranges = y2
pub.publish(scan)
rospy.init_node("ann_publisher")
sub = rospy.Subscriber('/scan', LaserScan, callback)
pub = rospy.Publisher('/scan_new', LaserScan, queue_size=1)
rate = rospy.Rate(2)
scan = LaserScan()
rospy.spin()
The reason you're not seeing output is because the message field ranges
has the wrong type when trying to publish it out. y1
is a numpy.matrix
and when you call tolist()
it doesn't flatten it out to a 1D array, it returns a an array that contains your list. This is where the red output text is coming from. To fix the problem do:
y1 = ((a2-y1_ymin)/(y1_gain))+y1_xoffset
y2 = y1.tolist()[0]