Laser scan subscriber repeatedly consumes first laser scan message

asked 2020-08-28 01:23:49 -0500

JeffR1992 gravatar image

updated 2020-08-28 22:36:20 -0500

I'm writing a simple mapper in python, that uses rospy to subscribe to odom and laser scan messages, which are used to update an occupancy grid map that is then published for use in rviz. The odometry subscriber and callback work as expected (i.e. when the robot moves in the world, I can see this change in the occupancy grid map when testing in rviz). However, the laser scan subscriber and callback only seem to consume the first laser scan that is published, and then re-consumes this same first laser scan ad infinitum (i.e. no matter where the robot moves in the world, the occupancy grid map always gets updated with the first laser scan that was published).

Since I'm unable to post an rviz screenshot of what the occupancy grid map looks like (the forum complains that I don't have enough points to post an image), I've posted the screenshot to imgur:

Thus, what I'm trying to figure out is why my odometry subscriber and callback is working correctly, but my lidar subscriber and callback is not. Any help would be greatly appreciated.

The following code is used to achieve the above explanation:

import rospy
import numpy as np
import matplotlib.pyplot as plt

from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from nav_msgs.msg import OccupancyGrid
from geometry_msgs.msg import Quaternion

from mapper import *

def quaternion_to_yaw(q: Quaternion) -> float:
    return np.arctan2(2.0*(q.w*q.z + q.x*q.y), 1.0 - 2.0*(q.y*q.y + q.z*q.z))

class MapperNode:
    def __init__(self, pose: Pose, mapper: Mapper):
        self.pose = pose
        self.mapper = mapper

        self.lidar_sub = rospy.Subscriber("scan", LaserScan, self.lidar_callback)
        self.odom_sub = rospy.Subscriber("odom", Odometry, self.odometry_callback)
        self.map_pub = rospy.Publisher("map", OccupancyGrid, queue_size=1)

    def lidar_callback(self, scan: LaserScan):
        scan_ranges = np.array(scan.ranges)
        self.mapper.update_map(self.pose, scan_ranges, scan.angle_min, scan.angle_increment)        

    def odometry_callback(self, odom: Odometry):
        x = odom.pose.pose.position.x
        y = odom.pose.pose.position.y
        theta = quaternion_to_yaw(odom.pose.pose.orientation)
        self.pose = Pose(x, y, theta)

    def publish_map(self):
        occupancy_grid_map = OccupancyGrid()

        occupancy_grid_map.header.stamp =
        occupancy_grid_map.header.frame_id = "map" = self.mapper.num_rows = self.mapper.num_cols = self.mapper.resolution = -self.mapper.grid_origin.x*self.mapper.resolution = -self.mapper.grid_origin.y*self.mapper.resolution = 0.0 = 1.0 = 0.0 = 0.0 = 0.0

        # rviz expects a 1-dimensional int array with probability values ranging from 0 to 100        = (100*self.mapper.probability_map().ravel()).astype(int, copy=False)


def main():
    rospy.init_node("mapper_node", anonymous=True)

    pose = Pose(0.0, 0.0, 0.0)

    x_min = -100.0
    x_max ...
edit retag flag offensive close merge delete


Can you show your TF tree please ? The screenshot you gave complains about the tf map so if your tfs are not connected it's normal to have this kind of behavior.

Delb gravatar image Delb  ( 2020-08-28 04:33:18 -0500 )edit

just for completeness, have you confirmed the laser scan messages are not repeating?

billy gravatar image billy  ( 2020-08-28 17:16:47 -0500 )edit

Hi Delb, I've edited my original post to include the tf tree (although it might be a little difficult to see all the detail in png format).

Hi Billy, regarding the laser scan messages, if I check what's being published in the terminal with rostopic echo scan, I can see the laser range values changing as I move the robot around the world. However, if I try print(scan_ranges) in the lidar_callback() function, I don't see any changes to the laser range values (i.e. they're always the same range values from the first scan message that was received by the callback).

JeffR1992 gravatar image JeffR1992  ( 2020-08-28 22:29:11 -0500 )edit

I performed some more tests today, and noticed that if I only have print(scan_ranges) in the lidar_callback() function (i.e. I remove the update_map() and publish_map() calls), then the values printed in scan_ranges change as I move the robot around its environment. However, when I include the update_map() function, that's when print(scan_ranges) shows no change in the values contained in scan_ranges.

Does this mean that the update_map() function isn't finishing in time for new scan messages to be processed? To test this, I placed a print statement at the end of the update_map() function and it reaches this print statement every time lidar_callback() is triggered, so it's left me scratching my head a bit.

JeffR1992 gravatar image JeffR1992  ( 2020-08-30 18:52:12 -0500 )edit

What is the function update_map ? Maybe it's taking way too much time to execute and you are actually printing old values. You should check the timestamp of the messages, also try to look the scan topic statistics to see if there are dropped message ( I would also consider profiling your node to see if you are spending too much time in the function update_map

Delb gravatar image Delb  ( 2020-08-31 03:03:16 -0500 )edit