def lookup_transform(
self,
target_frame: str,
source_frame: str,
time: Time,
timeout: Duration = Duration()
) -> TransformStamped:
"""
Get the transform from the source frame to the target frame.
:param target_frame: Name of the frame to transform into.
:param source_frame: Name of the input frame.
:param time: The time at which to get the transform (0 will get the latest).
:param timeout: Time to wait for the target frame to become available.
:return: The transform between the frames.
"""
if isinstance(time, builtin_interfaces.msg.Time):
source_time = Time.from_msg(time)
warnings.warn(
'Passing a builtin_interfaces.msg.Time argument is deprecated, and will be removed in the near future. '
'Use rclpy.time.Time instead.')
elif isinstance(time, Time):
source_time = time
else:
raise TypeError('Must pass a rclpy.time.Time object.')
goal = LookupTransform.Goal()
goal.target_frame = target_frame
goal.source_frame = source_frame
goal.source_time = source_time.to_msg()
goal.timeout = timeout.to_msg()
goal.advanced = False
return self.__process_goal(goal)
def lookup_transform_full(self,
target_frame,
target_time,
source_frame,
source_time,
fixed_frame,
timeout=Duration()):
"""
Get the transform from the source frame to the target frame using the advanced API.
:param target_frame: Name of the frame to transform into.
:param target_time: The time to transform to. (0 will get the latest)
:param source_frame: Name of the input frame.
:param source_time: The time at which source_frame will be evaluated. (0 will get the latest)
:param fixed_frame: Name of the frame to consider constant in time.
:param timeout: (Optional) Time to wait for the target frame to become available.
:return: The transform between the frames.
:rtype: :class:`geometry_msgs.msg.TransformStamped`
"""
goal = LookupTransform.Goal()
goal.target_frame = target_frame
goal.source_frame = source_frame
goal.source_time = source_time.to_msg()
goal.timeout = timeout.to_msg()
goal.target_time = target_time.to_msg()
goal.fixed_frame = fixed_frame
goal.advanced = True
return self.__process_goal(goal)
def lookup_transform(self, target_frame, source_frame, time, timeout=Duration()):
"""
Get the transform from the source frame to the target frame.
:param target_frame: Name of the frame to transform into.
:param source_frame: Name of the input frame.
:param time: The time at which to get the transform. (0 will get the latest)
:param timeout: (Optional) Time to wait for the target frame to become available.
:return: The transform between the frames.
:rtype: :class:`geometry_msgs.msg.TransformStamped`
"""
goal = LookupTransform.Goal()
goal.target_frame = target_frame
goal.source_frame = source_frame
goal.source_time = time.to_msg()
goal.timeout = timeout.to_msg()
goal.advanced = False
return self.__process_goal(goal)
def execute_callback(self, goal_handle):
response = LookupTransform.Result()
response.transform = TransformStamped()
response.error = TF2Error()
try:
if not goal_handle.request.advanced:
transform = self.buffer_core.lookup_transform_core(
target_frame=goal_handle.request.target_frame,
source_frame=goal_handle.request.source_frame,
time=goal_handle.request.source_time)
else:
transform = self.buffer_core.lookup_transform_full_core(
target_frame=goal_handle.request.target_frame,
source_frame=goal_handle.request.source_frame,
source_time=goal_handle.request.source_time,
target_time=goal_handle.request.target_time,
fixed_frame=goal_handle.request.fixed_frame)
response.transform = transform
except LookupException as e:
response.error.error = TF2Error.LOOKUP_ERROR
return response
def execute_goal_callback(self, goal_handle):
print('execute_goal_callback')
goal_handle.set_succeeded()
return LookupTransform.Result()