Robotics StackExchange | Archived questions

How to use subscriber in ROS node

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 w>0 or h>0:

                roi = RegionOfInterest()

                roi.x_offset = x

                roi.y_offset = y

                roi.width = w
                roi.height = h
                self.ROI.publish(roi)
                ''' displaying'''

            cv2.imshow("Image window", cv_image) 

        cv2.imshow("mask", mask) 

        cv2.imshow("resulatant", res)           
            cv2.waitKey(3)

def stop():
    cmd = rospy.Publisher("/quadrotor/rotorcraftvelocity", Twist)   
    motion = Twist()
    motion.linear.x = +0.0
    motion.linear.y = +0.0
    motion.linear.z = +0.0
    cmd.publish(motion) 
    print 'stop'
def up():
    cmd = rospy.Publisher("/quadrotor/rotorcraftvelocity", Twist)
    motion = Twist()
    motion.linear.z = +0.1
    cmd.publish(motion)         
    print 'up'

def find(argv):
    vn = test_vision_node_fly()
    print('ar')
    print('yes')
def where(msg):
    global x
    global y
    global z
    x = msg.pose.position.x
    y = msg.pose.position.y
    z = msg.pose.position.z
    if z < 2.5 and y <= -1 and y > -1.09:
        up()
        find(sys.argv)
        #stop() 
        print('1')
        print(z)
def pilot():
    rospy.init_node("pilot")

    #rospy.Subscriber("/quadrotor/videocamera/camera_info", CameraInfo, getCameraInfo)  
    rospy.Subscriber("/quadrotor/pose", PoseStamped, where)

    print('hi') 

    rospy.spin() # this will block untill you hit Ctrl+C
if __name__ == '__main__':
        pilot()

But I am getting the following error and Is there any other approach to that ... here I am try to find a color object. I want my bot to simultaneously move and find the object ! Can anybody please tell me how i can do that ! Thanks in advance !

Asked by jashanvir on 2014-06-10 09:20:21 UTC

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.

Asked by Thomas D on 2014-06-10 10:52:40 UTC

Answers

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.

Asked by ahendrix on 2014-06-13 12:25:58 UTC

Comments

Thanks a lot ! It really helps me !

Asked by jashanvir on 2014-06-16 04:25:32 UTC

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 '

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.

Asked by anonymous8676 on 2014-06-13 17:28:38 UTC

Comments