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

Image Subscriber Lags (despite queue_size=1 and buff_size=2**30)

asked 2021-12-10 16:39:18 -0500

jdastoor3 gravatar image

updated 2022-03-03 09:34:47 -0500

lucasw gravatar image

Hi,

I have been working on a Python class that will allow me to spawn an object in gazebo, take an image of the object, and then delete that object, for a given number of objects that I provide. I am able to spawn the objects, and have written a subscriber that is able to take images using a sensor. However, the images that are saved are not being taken in the order I am asking them to. Starting the script, I seem to be getting images out of order. For example, if I set it to spawn 5 objects without deleting for clarity, and take an image each time a new object is spawned, I could have 1 image without any objects, 3 images with 2 objects present, and the last image with 3 objects present. Sometimes I even get images with objects in positions that were set the last time I ran the script.

I thought this was because my publisher and subscriber were working at different rates, so looking at the answers on this forum I added queue_size=1 and buff_size = 2***30, to make sure that it was taking the last image that was sent. However, the problem is still there and nothing has changed. I have also tried increasing the time delay which has not helped, and was trying to find a way to delete the subscriber queue each time so that the latest message was definitely the one being read, but I have been unable to do so. I am not sure how to solve this and was wondering if anyone had a solution to this or an idea of why this was happening? I have attached my code below. Thanks!

import numpy as np
import rospy, tf, time
from gazebo_msgs.srv import SpawnModel, DeleteModel, GetModelState
from geometry_msgs.msg import *
from take_photo import ImageNode
# ROS Image message
from sensor_msgs.msg import Image
# ROS Image message -> OpenCV2 image converter
from cv_bridge import CvBridge, CvBridgeError
# OpenCV2 for saving an image
import cv2
import os

class ImageCollection():
    def __init__(self, number_spawn, model_path, time_delay=3.0):
        print("Waiting for gazebo services...")
        #rospy.init_node("spawn_products_in_bins")
        rospy.wait_for_service("gazebo/delete_model")
        rospy.wait_for_service("gazebo/spawn_sdf_model")
        rospy.wait_for_service("gazebo/get_model_state")
        print("Got it.")
        self.delete_model = rospy.ServiceProxy("gazebo/delete_model", DeleteModel)
        self.spawn_model = rospy.ServiceProxy("gazebo/spawn_sdf_model", SpawnModel)
        self.model_coordinates = rospy.ServiceProxy("gazebo/get_model_state", GetModelState)

        # Instantiate CvBridge
        self.bridge = CvBridge()
        self.image_number = 0
        self.number_spawn = number_spawn
        self.time_delay = time_delay
        self.model_path = model_path

        # Create a directory to store images
        self.folder_path = "Gazebo_Images"
        try:
            os.mkdir(self.folder_path)
        except:
            print("Folder already exists, appending to that folder...")

        # Initialize the node
        rospy.init_node('image_listener')
        # Define your image topic
        self.image_topic = "/wamv/sensors/cameras/front_left_camera/image_raw"
        # Set up your subscriber and define its callback
        # rospy.Subscriber(image_topic, Image, self.image_callback)
        # rospy.spin()

    def spawn(self):

        with open(self.model_path, "r") as f:
            product_xml = f.read()

        # Determine a random position for the buoy (function has not been shown here but is just a random generator)
        bin_x, bin_y ...
(more)
edit retag flag offensive close merge delete

Comments

Hi @jdastoor3 your question is clear. Unfortunately I don’t know the answer as I’ll need time to study it more. However why not use a rosbag to save your pictures instead of trying to handle in the node?

osilva gravatar image osilva  ( 2021-12-10 17:45:02 -0500 )edit

I was unaware that was a possibility. Would that avoid the issues I have been having? I found this script, would that be a good place to start? Sorry, I am new to ROS.

jdastoor3 gravatar image jdastoor3  ( 2021-12-10 17:53:36 -0500 )edit

Yes a good place and search for ‘rosbag tutorials’. It will be a lot faster

osilva gravatar image osilva  ( 2021-12-10 18:03:57 -0500 )edit

Take a look at this thread, hope it helps: https://stackoverflow.com/questions/3...

osilva gravatar image osilva  ( 2021-12-10 18:35:16 -0500 )edit

1 Answer

Sort by » oldest newest most voted
1

answered 2021-12-11 09:00:29 -0500

Mike Scheutzow gravatar image

updated 2021-12-11 09:13:31 -0500

Your code is doing many things which lead to bad performance.

  1. You should not be doing any rospy calls until you have done the rospy.init_node().

  2. If you want good performance, you can not do slow stuff inside a subscribe callback: don't sleep, don't call services, don't wait for services, don't write a lot of data to files, don't convert the image data into some other format.

  3. One design strategy many have used successfully is to simply have the callback save the msg and then return. Then in your main loop, you keep checking to see if a msg is available, then process it when one shows up. In the main loop it is OK to do all the slow things I listed in item (2).

  4. If fact, you must sleep in main loop because you want to limit how many times per second the main loop executes. Typically a rospy.Rate object is used to implement this.

  5. If you sleep in main loop, use rospy.sleep(), not time.sleep().

edit flag offensive delete link more

Comments

So essentially I should just save the msg that callback receives, and create a main function which has the code currently in callback? Where would I put the rospy.Rate object? Is that right after I start the subscriber? And I don't need the rospy.spin() right after I call the subscriber right?

jdastoor3 gravatar image jdastoor3  ( 2021-12-11 14:14:30 -0500 )edit

Here is example of how to use rospy.Rate() in a main loop. Yes, the while loop replaces the rospy.spin() statement.

http://wiki.ros.org/rospy/Overview/Ti...

And you should rethink that 3 second sleep that you have in there.

Mike Scheutzow gravatar image Mike Scheutzow  ( 2021-12-11 18:51:36 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2021-12-10 16:39:18 -0500

Seen: 339 times

Last updated: Dec 11 '21