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 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()