ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

OccupancyGrid not getting published - Python.

asked 2020-01-01 02:03:45 -0500

ros-noob gravatar image

Hi, so I want to publish an occupancy grid of a map.png file I have.

After running this node, I do rostopic list but it doesn't show me any published map. Can someone tell what am I doing wrong?

#!/usr/bin/env python

import rospy 
import cv2 as cv
from nav_msgs.msg import *
from std_msgs.msg import *

grid = OccupancyGrid()
def load():
    img = cv.imread('map.png', 0)

    for i in range(len(img)):
        img[i] = (img[i] / 255) * 100

    grid.data = img
    pub.publish(grid)


def main():
    rospy.init_node('loadmap', anonymous = True)
    global pub
    pub = rospy.Publisher('/map', OccupancyGrid, queue_size = 10)
    load()
    rospy.spin

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-01-01 05:52:31 -0500

gvdhoorn gravatar image

updated 2020-01-01 05:53:30 -0500

It is most likely being published, but only once, and too fast after creating the Publisher. This doesn't leave any time for any potential Subscribers to actually notice it, register, prepare to receive the data and then actually receive it.

If you want to give "late joiners" a chance to receive the message -- and you don't publish periodically, or have messages changing contents often, you could try using a latched Publisher. From wiki/rospy/Overview/Publishers and Subscribers: rospy.Publisher initialization:

There are additional advanced options that allow you to configure the Publisher:

[..]

  • latch=False

    Enable 'latching' on the connection. When a connection is latched, a reference to the last message published is saved and sent to any future subscribers that connect. This is useful for slow-changing or static data like a map. The message is not copied! If you change the message object after publishing, the change message will be sent to future subscribers.

Note how publishing data "like a map" is given as one of the cases in which this could be useful.

edit flag offensive delete link more

Comments

There are quite a few Q&As about publishing too fast, or not "seeing any message" here on this site. To find them, use Google with site:answers.ros.org and your keywords.

gvdhoorn gravatar image gvdhoorn  ( 2020-01-01 05:53:14 -0500 )edit

That seems do it! Thanks a lot!

ros-noob gravatar image ros-noob  ( 2020-01-01 08:58:29 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2020-01-01 02:03:45 -0500

Seen: 1,648 times

Last updated: Jan 01 '20