Source code for simulation.utils.ros_base.transform_node
"""ROS transform base node class with tf2 transform interface."""
from typing import Optional
import rospy
import tf2_ros
from simulation.utils.geometry import Transform
from simulation.utils.ros_base.node_base import NodeBase
[docs]class TransformNode(NodeBase):
"""Extends NodeBase with a tf2 transform handler.
Args:
name (str): Name of the node
parameter_cache_time (int) = 1: Duration for which parameters will be cached
for performance
log_level (int) = rospy.INFO: Loglevel with which the node works.
"""
def __init__(
self, *, name: str, parameter_cache_time: float = 1, log_level: int = rospy.INFO
):
super().__init__(
name=name, parameter_cache_time=parameter_cache_time, log_level=log_level
)
self.__tf2 = tf2_ros.Buffer()
tf2_ros.TransformListener(self.__tf2)
[docs] def get_transformation(
self,
target_frame: str,
source_frame: str,
timestamp: rospy.rostime.Time,
timeout: rospy.rostime.Duration = rospy.Duration(0.1),
) -> Optional[Transform]:
"""tf2 transform handler.
Arguments:
target_frame (str): Name of the target frame
source_frame (str): Name of the source frame
timestamp (rospy.rostime.Time): The time in the buffer
timeout (rospy.rostime.Duration): Lookup timeout
Returns:
Returns the transformation.
"""
try:
tf_transform = self.__tf2.lookup_transform(
target_frame, source_frame, timestamp, timeout
)
return Transform(tf_transform.transform)
except Exception as e:
rospy.logerr(f"Could not lookup transform for message {e}")
return