ros2 / ros1_bridge

ROS 2 package that provides bidirectional communication between ROS 1 and ROS 2
Apache License 2.0
421 stars 274 forks source link

<Frame> passed to lookupTransform argument t arget_frame does not exist #389

Open songanz opened 1 year ago

songanz commented 1 year ago

Bug report

Required Info:

Steps to reproduce issue

  1. Start any gazebo simulator you have which contains at least two frames on a ROS1 docker
    1. Set the use_sim_time to be "true" in that simulator
    2. I am particularly using this environment from CMU: cmu_env with roslaunch vehicle_simulator system_campus.launch
  2. Start the ros1_bridge in a ROS2 docker
    export ROS_MASTER_URI=http://localhost:11311
    ros2 run ros1_bridge dynamic_bridge --bridge-all-topics --ros-args -p use_sim_time:=true
  3. Set up tf_listener in ROS2

    
    class TFListener(Node):
    def __init__(self) -> None:
        super().__init__('cam_sem_seg')
        """ parameters """
        self.declare_parameters(
            namespace='cam_sem_seg_py',
            parameters=[
                ('use_sim_time', None),
                ('image_topic', None),
                ('camera_info', None),
                ('lidar_topic', None),
                ('camera_frame', None),
                ('lidar_frame', None),
            ]
        )
        use_sim_time = self.get_parameter('cam_sem_seg_py.use_sim_time')
        # set use_sim_time to be true in your launch
        use_sim_time = rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, use_sim_time.value)  
        self.set_parameters([use_sim_time])
    
        """ ros """        
        """ sub/pub """
        (image_topic, camera_info, lidar_topic) = self.get_parameters(
            ['cam_sem_seg_py.image_topic', 'cam_sem_seg_py.camera_info', 'cam_sem_seg_py.lidar_topic'])
    
        self.img_sub = message_filters.Subscriber(self, Image, image_topic.value)
        self.info_sub = message_filters.Subscriber(self, CameraInfo, camera_info.value)
        self.lidar_sub = message_filters.Subscriber(self, PointCloud2, lidar_topic.value)
    
        ts = message_filters.ApproximateTimeSynchronizer([self.img_sub, self.info_sub, self.lidar_sub], 10, 0.04)
        ts.registerCallback(self.call_back)
    
        # TF listener
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        (self.camera_frame, self.lidar_frame) = self.get_parameters(
            ['cam_sem_seg_py.camera_frame', 'cam_sem_seg_py.lidar_frame'])
    
        self.pub = self.create_publisher(PointCloud2, "/lidar_label", 10)
    
    def call_back(self, img_msg, info_sub, lidar_msg):
        # load intrinsic
        self.intrinsic = np.array(info_sub.p).reshape(3,4)  # if error, double check the camera_info
    
        # get extrinsic
        try:
            lid2cam_T = self.tf_buffer.lookup_transform(
                self.camera_frame.value,  # To
                self.lidar_frame.value,  # from
                self.get_clock().now())
            self.extrinsic = np.dot(lid2cam_T, self.intrinsic)
    
        except TransformException as ex:
            print('use_sim_time: ', self.get_parameters(['use_sim_time'])[0].value)
            print('self.get_clock().now(): ', self.get_clock().now())
            print('rclpy.time.Time(): ', rclpy.time.Time())
            self.get_logger().info(
                f'Could not transform {self.camera_frame.value} to {self.lidar_frame.value}: {ex}')
            return
        """ Process the image and lidar messages """
        ......
        self.pub.publish(pc2)
4. Launch file for the tf_listener
```python
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    package_name='your_name'
    your_node = Node(
        package= package_name,
        namespace= 'your_namespace',
        executable= 'your_executable',
        name= package_name,
        parameters=[
            {'use_sim_time': use_sim_time},
            {'image_topic': '/camera/image'},
            {'camera_info': '/camera/camera_info'},
            {'lidar_topic': '/sensor_scan'},
            {'camera_frame':'camera'},
            {'lidar_frame': 'sensor_at_scan'},
        ],
        emulate_tty=True
    )
    return LaunchDescription([your_node])
  1. On ROS2 docker terminal: ros2 run tf2_ros tf2_echo <reference_frame> <target_frame> You will see it cannot find the frame first, and then it finds the frame with a completely different timestamp. image
  2. In the meantime, the tf_listener is showing: image which means even though I use self.get_clock.now() (it is the sim_time) in the lookup_transform() function, it still cannot find the frame.

Expected behavior

Find the frame and get the TF correctly

Actual behavior

As described in the "Steps to reproduce issue" part

Additional information

NAN

songanz commented 1 year ago

Any comment on this issue?