Converting python 2 URG lidar filter to python 3 in ROS

Question

I am trying to convert some python 2 code into python 3 in order to get a lidar sensor to work properly and only display 180 degrees instead of 270. I have code that worked previously in python 2 but now that I am trying to run it in python 3 I am having issues. I am using ROS. When I run my launch code everything seems to run smoothly and I get no errors; however, when I look in RVIZ I can see that the sensor is still scanning in 270 degrees instead of the 180 that I want. Does anyone have any clue as to what is not working? Here is the code:

#!/usr/bin/env python3

import math
import rospy
from sensor_msgs.msg import LaserScan
from sensor_msgs.msg import Range
from std_msgs.msg import Float64
from std_msgs.msg import Float64MultiArray

ID_0DEG = 180 #180
ID_90DEG = 540 #540
ID_180DEG = 900 #900
#ID_DIFF = ID_180DEG - ID_0DEG
#RAD_PER_ID = math.pi/ID_DIFF
Y_TH = 0.2
X_TH1 = 1.0
X_TH0 = 0.2

STEP2 = 10
AREA_ANGLE = 180.0 #degree

class DoFilter:
    def __init__(self):

        rospy.Subscriber("scan", LaserScan, self.call_scan)
        self.pub_scan = rospy.Publisher("scan_filtered", LaserScan, queue_size=1)
        self.pub_detect = rospy.Publisher("detect_obstacle", Float64MultiArray, queue_size=1)
        self.pub_range = rospy.Publisher("polar_scatter", Range, queue_size=1)
        self.detect = Float64MultiArray() #Float64()
        self.newdata = LaserScan()
        self.detect.data = [1.0,1.0]
        self.range = Range()

    def call_scan(self, data):

        #self.newdata = data
        #self.newdata.ranges = list(data.ranges)
        #self.newdata.intensities = list(data.intensities)
        #self.detect.data = 0.0

        num_filter = ID_180DEG - ID_0DEG + 1
        self.newdata.header = data.header
        self.newdata.angle_min = -math.pi / 2.0 #data.angle_min
        self.newdata.angle_max =  math.pi / 2.0 #data.angle_max
        self.newdata.angle_increment = data.angle_increment
        self.newdata.time_increment = data.time_increment
        self.newdata.scan_time = data.scan_time
        self.newdata.range_min = data.range_min
        self.newdata.range_max = data.range_max
        self.newdata.ranges = list(range(num_filter)) 
        self.newdata.intensities = list(range(num_filter))
        for x in range(0,num_filter):
            self.newdata.ranges[x] = data.ranges[x + ID_0DEG]
            self.newdata.intensities[x] = data.intensities[x + ID_0DEG]
        self.pub_scan.publish(self.newdata)

        #print(len(data.ranges),len(self.newdata.ranges))

        #self.detect.data = [1.0,1.0]
        #for x in range(0,num_filter):
        #    a = math.pi * x / (num_filter - 1)
        #    lx = self.newdata.ranges[x] * math.sin(a)
        #    ly = self.newdata.ranges[x] * math.cos(a)
        #    if abs(lx) < X_TH1 and abs(ly) < Y_TH:
        #        lxn = (lx - X_TH0) / X_TH1
        #        if lxn < self.detect.data[0]:
        #            self.detect.data = [lxn,ly/Y_TH]

        for x in range(0,num_filter,STEP2):
            self.range.field_of_view = int (AREA_ANGLE - x * AREA_ANGLE / num_filter)
            self.range.range = self.newdata.ranges[x]
            self.pub_range.publish(self.range)     

        #print(self.detect.data)
        #self.pub_detect.publish(self.detect)

if __name__ == '__main__':
    rospy.init_node('urg_filter_node', anonymous=False)
    rospy.loginfo ('Start urg_filter')
    lidar = DoFilter()
    rospy.spin()

And here is my the launch file that calls it:

Any help would be greatly appreciated!

Converting python 2 URG lidar filter to python 3 in ROS

0 Answers