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)