ROS image subscriber wont stop subscribing
Hi,
I have a project where this node subscribe to two topics. an image camera topic and a topic that gives input
what i want to do is if the topic input is a '1' than it will not subscribe to the camera, if its a '2', it will subscribe to the camera and take a picture. However, it seems that after i click on '2' and than back to '1', my camera will continue to take pictures.
code:
#!/usr/bin/env python
from __future__ import print_function
import roslib
import sys
import rospy
import cv2
from std_msgs.msg import String, Int32
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import os
import numpy as np
import argparse
import time
def test(data):
bridge = CvBridge()
try:
cv_image = bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
pub = rospy.Publisher('depthoutput2',String,queue_size=10)
#cv_image = cv2.imread ('toilet_image_1.jpg')
image = cv_image
image = cv2.resize(image, (300,300),fx=0,fy=0, interpolation = cv2.INTER_CUBIC)
cv2.imwrite('image_photo_taken.jpg', image)
image = cv2.imread('image_photo_taken.jpg')
cv2.imshow("Image window",image)
cv2.waitKey(1)
def callback1 (data):
movement = data.data()
if movement ==1 :
print ('stop')
elif movement ==2 :
print('take picture')
image_sub = rospy.Subscriber("/camera/rgb/image_raw",Image,test)
else:
print ('error')
def main():
rospy.init_node ('yolo',anonymous = True)
rospy.Subscriber('depthoutput', Int32, callback1)
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
cv2.destroyAllWindows()
if __name__ == '__main__':
main()
Asked by loguna on 2019-06-12 03:12:49 UTC
Answers
I think I can see why your code is behaving in this way. While your callback is waiting for the user to press a key the image subscriber is still operating and queuing up messages to process, so that when the callback finishes the next image is immediately processed and so on in a loop.
To be clear, do you want the node to take a single picture when you press 1 or a sequence of pictures until you press 2?
If you just want to take a single picture then you should make your subscriber object image_sub
a global initially set to None
Then in the image callback call the image_sub.unregister()
function so that only a single callback will be executed until the subscriber is re-created again.
If you want to continuously take pictures until 2 is pressed then you'll want to create the subscriber with a queue size of 1:
image_sub = rospy.Subscriber("/camera/rgb/image_raw",Image,test, queue_size=1)
And move the call to image_sub.unregister()
into the if movement ==1:
case.
Hope this helps.
Asked by PeteBlackerThe3rd on 2019-06-12 05:59:19 UTC
Comments
If the idea is really to only retrieve a single message from a topic, I would suggest to use rospy.wait_for_message(..)
. See #q292582 and the other linked Q&As.
Asked by gvdhoorn on 2019-06-12 06:25:50 UTC
Comments