class LocalizationNode(object): # model in meters COEFFS = [-0.1416, 2.311, -14.76, 36.79] EVERY_MINUTE = 0.1667 EVERY_SECOND = 1 def __init__(self): rospy.init_node("position_publisher", anonymous=True) self.rate = rospy.Rate(LocalizationNode.EVERY_SECOND) self.extractParams() self.frame = "/target_" + str(self.tag) self.publisher = rospy.Publisher(self.frame, PointStamped, queue_size=10) self.model = Poly3(LocalizationNode.COEFFS) self.selector = BasestationSelector(self.basestations) self.ekf = EKF(self.tag, 1, 0.1, self.basestations, self.model, self.selector) self.miss_counter = 2 def extractParams(self): self.tag = rospy.get_param("/localization_node/tag") stations = rospy.get_param("/localization_node/basestations") self.buildBasestation(stations) def buildBasestation(self, stations): self.basestations = [] for station in stations: self.basestations.append(Basestation(station[0], float(station[1]), float(station[2]))) def publishPosition(self, position): msg = PointStamped() msg.header.stamp = rospy.Time.now() msg.header.frame_id = self.frame msg.point = Point(position[0], position[1], 0) self.publisher.publish(msg) def localizationLoop(self): while not rospy.is_shutdown(): try: if self.miss_counter >= 2: # after two misses it is assumed the target is not in range self.rate = rospy.Rate(LocalizationNode.EVERY_MINUTE) # once in a minute position = self.ekf.setInitialPositionToCloserBasestation() else: position = self.ekf.ekfIteration() self.publishPosition(position) self.rate = rospy.Rate(LocalizationNode.EVERY_SECOND) # normal rate self.miss_counter = 0 except NoMeasurementException: # no measurements means the target is not in range self.miss_counter += 1 self.rate.sleep()