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.dIR_BR_callback)
dIR_BL_sub=rospy.Subscriber('dIR_BL', Range, warning.dIR_BL_callback)
dIR_front_sub=rospy.Subscriber('dIR_front', Range, warning.dIR_front_callback)
sonic_sub=rospy.Subscriber('ultrasound', Range, warning.sonic_callback)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
warn_pub.publish(flags.data)
rate.sleep()
if __name__=='__main__':
try:
main()
except rospy.ROSInterruptException:
pass
↧