Nelle's profile - activity

Really confused here. I want to delay certain publishing of messages to a topic for about 1-2 seconds. Can rospy.sleep help? My code already works fine, but I need the delay since I cannot put any delays in the Arduino/rosserial because it gives a lost sync error. Can someone please help? I've been doing this for 2 weeks and no solution so far. Here is my code.

#!/usr/bin/env python


import rospy
import time

from std_msgs.msg import Int32  
from sensor_msgs.msg import Range

flags = Int32()

class warning_flag():
    def __init__(self):
        self.aIR_FR = None
        self.aIR_FL = None
        self.dIR_front = None
        self.dIR_BR = None
        self.dIR_BL = None
        self.sonic = None

    def aIR_FR_callback(self, aIR_FR_msg):
        print "Analog IR Right Range: %s" % aIR_FR_msg.range
        self.aIR_FR = aIR_FR_msg.range
        self.warn()

    def aIR_FL_callback(self, aIR_FL_msg):
        print "Analog IR Left Range: %s" % aIR_FL_msg.range
        self.aIR_FL = aIR_FL_msg.range
        self.warn()

    def dIR_BL_callback(self, dIR_BL_msg):
        print "Digital IR Left Range: %s" % dIR_BL_msg.range
        self.dIR_BL = dIR_BL_msg.range
        self.warn()

    def dIR_BR_callback(self, dIR_BR_msg):
        print "Digital IR Right Range: %s" % dIR_BR_msg.range
        self.dIR_BR = dIR_BR_msg.range
        self.warn()

    def dIR_front_callback(self, dIR_front_msg):
        print "Digital IR Range: %s" % dIR_front_msg.range
        self.dIR_front = dIR_front_msg.range
        self.warn()

    def sonic_callback(self, sonic_msg):
        print "Ultrasonic Range: %s" % sonic_msg.range
        self.sonic = sonic_msg.range    
        self.warn()

    def warn(self):
        sonic_zone = 0.3
        aIR_FR_zone = 0.2
        aIR_FL_zone = 0.2
        dIR_front_zone = 0
        dIR_BR_zone = 1
        dIR_BL_zone = 1

        if (self.aIR_FR > aIR_FR_zone and self.aIR_FL <= aIR_FL_zone):
            print "Turn right"
            flags.data = 2

    elif (self.aIR_FR <= aIR_FR_zone and self.aIR_FL > aIR_FL_zone):
            print "Turn Left"
            flags.data = 3

        elif (self.aIR_FR <= aIR_FR_zone and self.aIR_FL <= aIR_FL_zone):
            if ((self.sonic > sonic_zone and self.dIR_front != dIR_front_zone) or
        (self.sonic <= sonic_zone and self.dIR_front != dIR_front_zone)):    
                if (self.dIR_BR != dIR_BR_zone and self.dIR_BL != dIR_BL_zone):
                    print "Stop"
                    flags.data = 5
                else:    
                    if (self.aIR_FR > self.aIR_FL):
                        print "Turn Right"
                        flags.data = 2
                    else:
                        print "Turn Left"
                        flags.data = 3

        elif ((self.sonic > sonic_zone and self.dIR_front == dIR_front_zone) or
        (self.sonic <= sonic_zone and self.dIR_front == dIR_front_zone)):
                if (self.dIR_BR == dIR_BR_zone and self.dIR_BL == dIR_BL_zone):
                    print "Back off"
                    flags.data = 4
            #put delay here
            if (self.aIR_FR > self.aIR_FL):
                       print "Turn Right"
                        flags.data = 2
                    else:
                        print "Turn Left"
                        flags.data = 3
                else:    
                    if (self.aIR_FR > self.aIR_FL):
                        print "Turn Right"
                        flags.data = 2
                    else:
                        print "Turn Left"
                        flags.data = 3

        elif (self.aIR_FR > aIR_FR_zone and self.aIR_FL > aIR_FL_zone):
            if (self.sonic > sonic_zone and self.dIR_front == dIR_front_zone):
        print "Advance"
        flags.data = 1                
        else:   
        if (self.dIR_BR == dIR_BR_zone and self.dIR_BL == dIR_BL_zone):
                    print "Back off"
                    flags.data = 4
            #put delay here
            if (self.aIR_FR > self.aIR_FL):
                        print "Turn Right"
                        flags.data = 2
                    else:
                        print "Turn Left"
                        flags.data = 3
                else:    
                    if (self.aIR_FR > self.aIR_FL):
                        print "Turn Right"
                        flags.data = 2
                    else:
                        print "Turn Left"
                        flags.data = 3

def main():
    rospy.init_node('sensor_pub_sub_node')
    warn_pub=rospy.Publisher('Warnings', Int32, queue_size=1000)
    warning = warning_flag()
    aIR_FR_sub=rospy.Subscriber('aIR_FR', Range, warning.aIR_FR_callback)
    aIR_FL_sub=rospy.Subscriber('aIR_FL', Range, warning.aIR_FL_callback)
    dIR_BR_sub=rospy.Subscriber('dIR_BR', Range, warning ...