def run(self):
r = rospy.Rate(5)
while not rospy.is_shutdown():
try:
r.sleep()
except rospy.exceptions.ROSTimeMovedBackwardsException, e:
self.lock.acquire()
rospy.logdebug(str(e))
self.stack = ['__none']
self.selected = None
self.requesting = None
self.mux_select('__none')
self.lock.release()
continue
now = rostime.get_rostime()
for topic, last in self.last_msg.items():
if (not last.is_zero()) and ((now - last).to_sec() > 2):
# auto release lock
self.lock.acquire()
while self.requesting:
sleep(0.010)
rospy.logwarn('Timeout release of request for "%s".',
topic)
if self.stack[-1] == topic:
self.stack.pop()
self.requesting = self.stack[-1]
self.mux_select(self.requesting)
else:
self.stack.remove(topic)
self.last_msg[topic] = rostime.Time(0)
self.lock.release()
def cmdvel_cb(self, msg): now = rostime.get_rostime() #print(now), " ", if (now-self.last_request).to_sec()>0.5: self.priority_acquire(self.output_topic) self.last_request = now self.cmdvel_pub.publish(msg) self.last_msg = now
def cmdvel_cb(self, msg): now = rostime.get_rostime() #print(now), " ", if (now-self.last_request).to_sec()>0.5: self.priority_acquire(self.output_topic) self.last_request = now self.cmdvel_pub.publish(msg) self.last_msg = now
def run(self): r = rospy.Rate(5) while not rospy.is_shutdown(): r.sleep() if (not self.last_msg.is_zero()) and\ (rostime.get_rostime()-self.last_msg).to_sec()>0.5: self.last_request = rostime.Time(0) self.last_msg = rostime.Time(0) self.priority_release(self.output_topic)
def run(self): r = rospy.Rate(5) while not rospy.is_shutdown(): r.sleep() if (not self.last_msg.is_zero()) and\ (rostime.get_rostime()-self.last_msg).to_sec()>0.5: self.last_request = rostime.Time(0) self.last_msg = rostime.Time(0) self.priority_release(self.output_topic)
def selected_cb(self, msg):
rospy.loginfo('Selected: "%s"', msg.data)
self.selected = msg.data
if self.requesting and self.selected != self.requesting:
rospy.logerr('Requesting "%s" but selected is "%s"!',
self.requesting, self.selected)
self.requesting = None
if self.stack[-1] != self.selected:
self.stack.append(self.selected)
if msg.data != '__none':
self.last_msg[msg.data] = rostime.get_rostime()
def selected_cb(self, msg):
rospy.loginfo('Selected: "%s"', msg.data)
self.selected = msg.data
if self.requesting and self.selected!=self.requesting:
rospy.logerr('Requesting "%s" but selected is "%s"!',
self.requesting, self.selected)
self.requesting = None
if self.stack[-1]!=self.selected:
self.stack.append(self.selected)
if msg.data != '__none':
self.last_msg[msg.data] = rostime.get_rostime()
def run(self):
r = rospy.Rate(5)
while not rospy.is_shutdown():
r.sleep()
now = rostime.get_rostime()
for topic, last in self.last_msg.items():
if (not last.is_zero()) and ((now-last).to_sec()>2):
# auto release lock
self.lock.acquire()
while self.requesting:
sleep(0.010)
rospy.logwarn('Timeout release of request for "%s".', topic)
if self.stack[-1] == topic:
self.stack.pop()
self.requesting = self.stack[-1]
self.mux_select(self.requesting)
else:
self.stack.remove(topic)
self.last_msg[topic] = rostime.Time(0)
self.lock.release()
def cb(msg):
if topic in self.stack:
self.last_msg[topic] = rostime.get_rostime()
def cb(msg): if topic in self.stack: self.last_msg[topic] = rostime.get_rostime()
