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