def add_transform(self, target, source, cb, wait=False): """ If wait is true, this function may block until the action client has connected. """ target = TLC.sanitize_frame(target) source = TLC.sanitize_frame(source) key = TLC.key_from_transform(target, source) if key in self.transforms: if rospy.get_time() - self.last_update < 1.0: return self.transforms[key] = {'cb': cb, 'sub': TLM.Subscription(target, source)} self.last_update = rospy.get_time() self._update_transforms(wait=wait)
def query_transform(self, target, source, cb): target = TLC.sanitize_frame(target) source = TLC.sanitize_frame(source) key = TLC.key_from_transform(target, source) if key in self.cbs: if rospy.get_time() - self.ts[key] < 1.0: return self.cbs[key] = cb self.ts[key] = rospy.get_time() goal = TLM.TfLookupGoal(target, source, rospy.Time(0.0)) try: self.ghs.append(self.al_client.send_goal(goal, transition_cb=self._al_cb)) except rospy.ROSException as e: rospy.logfatal("Failed to send goal: %s", e.message)
def query_transform(self, target, source, cb): target = TLC.sanitize_frame(target) source = TLC.sanitize_frame(source) key = TLC.key_from_transform(target, source) if key in self.cbs: if rospy.get_time() - self.ts[key] < 1.0: return self.cbs[key] = cb self.ts[key] = rospy.get_time() goal = TLM.TfLookupGoal(target, source, rospy.Time(0.0)) try: self.ghs.append( self.al_client.send_goal(goal, transition_cb=self._al_cb)) except rospy.ROSException as e: rospy.logfatal("Failed to send goal: %s", e.message)