Ask Your Question
0

How to use subscriber in ROS node

asked 2014-06-10 09:20:21 -0600

jashanvir gravatar image

updated 2014-06-13 06:00:23 -0600

Hello ! I am working on ROS and morse ! I have written a ROS node which is subscribing to the sensors of the robot. Lets say sub1 is for position and sub2 is for camera; pub1 for motion and pub2 for height

Now when I subscribe to the position of the robot and stays inside that function. and wont come out of it ! I want to do something like this sub1 - get the position; pub1 - move a small distance; pub2- maintain a height; stop the motion and check if the object is in the camera view; if object found ; store the position and come out of it; pub3 - give the postion; if object NOT found; go back to sub1 and get the new position and impart motion again;

Here is what I am working on (the sample code):-

class test_vision_node_fly:
    def __init__(self):
        self.image_pub = rospy.Publisher("quadrotor/videocamera1/camera_info",Image)
        self.ROI = rospy.Publisher("roi", RegionOfInterest)
        rospy.sleep(1)
        self.cv_window_name = "Image window"
        cv2.namedWindow(self.cv_window_name, 0)
        cv2.namedWindow("trackbars" , 0) 

            self.bridge = CvBridge()

            self.image_sub = rospy.Subscriber("quadrotor/videocamera1/image",Image,self.callback)

        rospy.loginfo("Waiting for image topics...")

    def on_trackbar(self,x):
            global H_MAX
            global H_MIN
            global S_MAX
            global S_MIN
            global V_MAX
            global V_MIN

        H_MIN = cv2.getTrackbarPos('H_MIN', 'trackbars')
            H_MAX = cv2.getTrackbarPos('H_MAX', 'trackbars')  
            S_MAX = cv2.getTrackbarPos('S_MAX', 'trackbars')
            S_MIN = cv2.getTrackbarPos('S_MIN', 'trackbars')
            V_MIN = cv2.getTrackbarPos('V_MIN', 'trackbars')
            V_MAX = cv2.getTrackbarPos('V_MAX', 'trackbars')

            pass   
    def callback(self,data):

        """ Convert the raw image to OpenCV format using the convert_image() helper function """
            cv_image = self.convert_image(data)
        # Convert the image to a Numpy array since most cv2 functions
            # require Numpy arrays.
            cv_image = np.array(cv_image, dtype=np.uint8)
         "" Apply the CamShift algorithm using the do_camshift() helper function """
            cv_image = self.do_camshift(cv_image)

            """ Refresh the displayed image """
            cv2.imshow(self.cv_window_name, cv_image)

    def convert_image(self, ros_image):  
        try:
                cv_image = self.bridge.imgmsg_to_cv2(ros_image, "bgr8")
            print('q')
            return cv_image
            except CvBridgeError, e:
                print e
    def do_camshift(self, cv_image):            
            ''' converting bgr to hsv ''' 

            hsv=cv2.cvtColor(cv_image,cv2.COLOR_BGR2HSV) 

        '''creating trackbars'''
        cv2.createTrackbar('H_MIN', 'trackbars', H_MIN, H_MAX, self.on_trackbar) 
        cv2.createTrackbar('H_MAX', 'trackbars', H_MAX, H_MAX, self.on_trackbar)
            cv2.createTrackbar('S_MIN', 'trackbars', S_MIN, S_MAX, self.on_trackbar) 
        cv2.createTrackbar('S_MAX', 'trackbars', S_MAX, S_MAX, self.on_trackbar)    
        cv2.createTrackbar('V_MIN', 'trackbars', V_MIN, V_MAX, self.on_trackbar)    
            cv2.createTrackbar('V_MAX', 'trackbars', V_MAX, V_MAX, self.on_trackbar)

            ''' threshdolding '''
        COLOR_MIN = np.array([H_MIN, S_MIN, V_MIN],dtype=np.uint8)

            COLOR_MAX = np.array([H_MAX, S_MAX, V_MAX],dtype=np.uint8)

            mask=cv2.inRange(hsv, COLOR_MIN, COLOR_MAX)

            new_mask = mask.copy()

        # Bitwise-AND mask and original image
            res = cv2.bitwise_and(cv_image,cv_image, mask= mask)

        #removing noise 
        kernel = np.ones((12,12),np.uint8)

            new_mask = cv2.morphologyEx(new_mask, cv2.MORPH_CLOSE, kernel)

            new_mask = cv2.morphologyEx(new_mask, cv2.MORPH_OPEN, kernel)

        contours, hierarchy = cv2.findContours(new_mask,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)

        # Draw contours on the original image
            cv2.drawContours(cv_image, contours, -1, (0,0,0), 3)

        x,y,w,h = cv2.boundingRect(contours[-1])

        cv2.rectangle(cv_image,(x,y),(x+w,y+h),(0,254,0),3)

        if x>0 or y>0 or ...
(more)
edit retag flag offensive close merge delete

Comments

If you add a minimal working example of the code you are trying to get running it will be easier for others to help.

Thomas D gravatar imageThomas D ( 2014-06-10 10:52:40 -0600 )edit

2 Answers

Sort by ยป oldest newest most voted
1

answered 2014-06-13 12:25:58 -0600

ahendrix gravatar image

You're creating your publishers and immediately publishing to them, which usually results in a race condition where subscribers don't receive the first message that you've published. (see this answer for a more detailed explanation).

you should instead create your publishers once, during node startup, and them publish commands to them later.

edit flag offensive delete link more

Comments

Thanks a lot ! It really helps me !

jashanvir gravatar imagejashanvir ( 2014-06-16 04:25:32 -0600 )edit
0

answered 2014-06-13 17:28:38 -0600

cmeaclem gravatar image

I usually start debugging publisher/subscriber issues with 'rostopic'. First check that your topic is actually being published by calling 'rostopic list' and 'rostopic echo <topic name="">'

Secondly you can check if your subscriber is working by publishing to it using 'rostopic pub' and observe if the program functions as expected.

This should help you identify if the issue is with the send or receive.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2014-06-10 09:20:21 -0600

Seen: 600 times

Last updated: Jun 13 '14