How to merge two laserscans?

asked 2017-10-11 12:17:53 -0500

krishna43 gravatar image

updated 2017-10-11 12:30:45 -0500

jayess gravatar image

Hello all,

I have two sensors (KInect and asus) and I am able to get laserscan data from both of them. I wrote a simple python script which subscribes to two scan topics and merges into a new scan topic. I wanted to use the newscan to build a map but it is not working as i expected as the newscan has some delay and whenever i try to use this newscan for gmapping it's throwing me an error saying that "extraplotaion error".

Has anyone tried this before? If yes, Can you please help me?

Here is my python script:

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import LaserScan

class extended_scan:

    def __init__(self):

        self.scan_pub = rospy.Publisher('/newScan', LaserScan, queue_size=0)
        self.scan1 = None
        self.scan2 = None

    def laser_1_callback(self, data1):
        # print data1
        # print 'sai'
        self.scan1 = data1

        return self.scan1

    def laser_2_callback(self, data2):
        # print data2
        # print 'sai1'
        self.scan2 = data2

        return self.scan2

    def newScan_pub(self):
        while not rospy.is_shutdown():
            rospy.Subscriber('/camera1/scan', LaserScan, self.laser_1_callback)
            rospy.Subscriber('/camera2/scan', LaserScan, self.laser_2_callback)

            newdata = LaserScan()
            if self.scan1 == None or self.scan2 == None:
                rospy.loginfo('got None data')
                newdata.header.seq = self.scan1.header.seq
                newdata.header.stamp = self.scan1.header.stamp
                newdata.header.frame_id = self.scan1.header.frame_id
                newdata.angle_min = self.scan1.angle_min
                newdata.angle_max = self.scan1.angle_max + -(self.scan2.angle_min) + self.scan2.angle_max
                newdata.angle_increment = self.scan1.angle_increment
                newdata.time_increment = self.scan1.time_increment
                newdata.scan_time = self.scan1.scan_time
                newdata.range_min = self.scan1.range_min
                newdata.range_max = self.scan1.range_max
                newdata.ranges = self.scan1.ranges + self.scan2.ranges
                newdata.intensities = self.scan1.intensities
                # print len(self.scan1.ranges)
                # print len(self.scan2.ranges)
                # print len(newdata.ranges)

                # rospy.spin()
if __name__ == '__main__':

    rospy.loginfo(' starting extended_scan_publishe node')

    ES = extended_scan()
    # rospy.spin()
edit retag flag offensive close merge delete


Can you please update your question with error copy and pasted from the terminal? Many times an Extrapolation Error can be from a networking issue (like clocks not being synced). Check out

jayess gravatar image jayess  ( 2017-10-11 12:33:20 -0500 )edit

This is not going to work. The laserscan message is not able to hold data from more than one sensor as the ranges data is simply a vector/list from angle_min to angle_max. Pretty easy to see that you can't shove two messages together into that format. Also, the times of the scans will differ ...

ufr3c_tjc gravatar image ufr3c_tjc  ( 2017-10-11 18:14:14 -0500 )edit

... which will affect the accuracy of the SLAM estimate. Sometimes packages allow you to feed in data from two different scanning sources. The transforms are then used to match the different sources against each other and against the SLAM map. Not sure about gmapping's ability to do this though.

ufr3c_tjc gravatar image ufr3c_tjc  ( 2017-10-11 18:16:44 -0500 )edit

So what I'm saying is, try mapping both /scan outputs from the Asus and Kinect into gmapping and see if it works.

ufr3c_tjc gravatar image ufr3c_tjc  ( 2017-10-11 18:18:35 -0500 )edit