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

How to get consecutive images with 3 seconds delay

asked 2023-05-08 02:20:48 -0500

Furki gravatar image

Hello, I am trying to read two consecutive images from my rostopic using rospy.Subscriber to compare the orientation between these pictures but using the code below, I found out two images I get are the same every time so I am trying to find out if there is a way to get the two images with delay, more specifically, getting the second image 3 seconds after I get the first image. I tried putting time.sleep() between functions but it wouldn't work out.

import cv2
import numpy as np
import rospy
import time
import os


from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

def cam_callback(msg):
          path = '/home/furki/img'
          try:
     # Converting ROS Image message to OpenCV2
             cv2_img1  = bridge1.imgmsg_to_cv2(msg,"bgr8")
          except CvBridgeError as e:
             print(e)
          else:
          # Save your OpenCV2 image as a jpeg
             img1 = cv2.imwrite(os.path.join(path, f'img1.jpeg'), cv2_img1)

def cam_callback2(msg):
          path = '/home/furki/img'
          try:
     # Converting ROS Image message to OpenCV2
             cv2_img2 = bridge.imgmsg_to_cv2(msg,"bgr8")
          except CvBridgeError as e:
             print(e)
          else:
          # Save your OpenCV2 image as a jpeg
             img2 = cv2.imwrite(os.path.join(path, f'img2.jpeg'), cv2_img2)


rospy.init_node("orientation_feature_detection_py")
image_topic = '/s500/usb_cam/image_raw'
rate = rospy.Rate(1)


rospy.Subscriber(image_topic, Image, cam_callback)
rate.sleep()
rospy.Subscriber(image_topic, Image, cam_callback2)
rate.sleep()

# Reading the two images

img1 = cv2.imread('/home/furki/img/img1.jpeg')

img2 = cv2.imread('/home/furki/img/img2.jpeg')
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2023-05-08 08:00:33 -0500

lucasw gravatar image

updated 2023-05-08 08:02:23 -0500

Each callback is getting called with every message published on image_topic after the callback is created, so cam_callback gets all the images and overwrites img1.jpep repeatedly, then a second later cam_callback2 becomes active and starts overwriting img2.jpeg repeatedly, then both callbacks are overwriting the two different files with the same image repeatedly until the node exits.

Try putting loginfos in the callbacks to see this more clearly:

import rospy
from std_msgs.msg import Float32


def callback1(msg):
    rospy.loginfo(f"callback1: {msg}")


def callback2(msg):
    rospy.loginfo(f"callback2: {msg}")


rospy.init_node("dual_callbacks")
rospy.Subscriber("test", Float32, callback1)
rospy.sleep(3.0)
rospy.Subscriber("test", Float32, callback2)
rospy.sleep(1.0)

run that node, then run this one:

import rospy
from std_msgs.msg import Float32


rospy.init_node("increment")
pub = rospy.Publisher("test", Float32, queue_size=2)
rate = rospy.Rate(3.0)
value = 0.0
while not rospy.is_shutdown():
    pub.publish(Float32(value))
    value += 1.0
    rate.sleep()

and you'll see output like this from the callback node:

[I] [1683550340.629]: callback1: data: 11.0
[I] [1683550340.962]: callback1: data: 12.0
[I] [1683550341.295]: callback1: data: 13.0
[I] [1683550341.628]: callback1: data: 14.0
[I] [1683550341.629]: callback2: data: 14.0
[I] [1683550341.961]: callback1: data: 15.0
[I] [1683550341.962]: callback2: data: 15.0
[I] [1683550342.294]: callback1: data: 16.0
[I] [1683550342.295]: callback2: data: 16.0

If the callbacks were unregistered or ignored later messages after receiving a message the first time you would get the desired effect:

import rospy
from std_msgs.msg import Float32


def callback1(msg):
    rospy.loginfo(f"callback1: {msg}")
    sub1.unregister()


def callback2(msg):
    rospy.loginfo(f"callback2: {msg}")
    sub2.unregister()


rospy.init_node("dual_callbacks")
sub1 = rospy.Subscriber("test", Float32, callback1)
rospy.sleep(3.0)
sub2 = rospy.Subscriber("test", Float32, callback2)
rospy.sleep(1.0)
edit flag offensive delete link more

Comments

Thank you :)

Furki gravatar image Furki  ( 2023-05-08 08:48:32 -0500 )edit

Question Tools

Stats

Asked: 2023-05-08 02:20:48 -0500

Seen: 87 times

Last updated: May 08 '23