Play back t265 bags through rs_from_file.launch

[jeffdelmerico]

I initially raised this as a comment in #1789 but it's a different enough question that I'm creating a separate issue here.

I recorded the streams from two devices simultaneously through the viewer (t265 and d415), producing two bag files encoded in the realsense format. I would like to use rs_from_file.launch to play these bags back so that the data from the streams is published into ROS. This seems to be the purpose of rs_from_file.launch, since it launches a realsesnse2_camera nodelet, which creates a pipeline from the realsense-encoded bag to simulate a live camera stream. Running rs_from_file.launch on the d415 bag file successfully republishes the data into ROS, but my attempts with the t265 bag produce the following errors:

[ INFO] [1617910087.390691404]: Initializing nodelet with 8 worker threads.
[ INFO] [1617910087.495343081]: RealSense ROS v2.2.22
[ INFO] [1617910087.495368724]: Built with LibRealSense v2.42.0
[ INFO] [1617910087.495378151]: Running with LibRealSense v2.42.0
[ INFO] [1617910087.512972061]: publish topics from rosbag file: /path/to/bagfile.bag
[ERROR] [1617910099.596389027]: Exception: Could not find requested sensor type!

Is it possible to play back t265 streams that were recorded through the viewer using this launch file? If not, I would appreciate suggestions on how to implement this feature.

[doronhi]

Hi @jeffdelmerico I started looking into it and reproduced the issue. It seems like an issue with identifying the pose sensor from a playback device. I hope to have an update soon.

[jeffdelmerico]

Thanks for your response! Please let me know if I can provide further information from my side or test any changes.

[doronhi]

I opened an inner ticket for librealsense. Tracked as DSO-16975. I hope it will be attended soon.

In the meanwhile I can offer a workaround: In base_realsense_node.cpp, line 931 replace: else if (sensor.is<rs2::pose_sensor>()) with: else if (module_name == "Tracking Module")

[jeffdelmerico]

Thank you very much for your response. I will try the workaround for now, and monitor the other ticket. In the meantime, should I leave this one open until the other is resolved?

[doronhi]

Yes, you can leave this one open until this it is resolved. The other one, #1789, can be closed, I think.

[jeffdelmerico]

Great, thanks!

[victorll998]

@doronhi I tried the workaround above and weirdly I still get the same result.

To test the workaround, I removed realsense2_camera from ROS distribution and installed the realsense2_camera from the source after I changed the line of code.

Please let me know if I missed anything or did something in the wrong way.

 SUMMARY
========

PARAMETERS
 * /rosdistro: melodic
 * /rosversion: 1.14.10
 * /t265/realsense2_camera/accel_fps: 0
 * /t265/realsense2_camera/accel_frame_id: t265_accel_frame
 * /t265/realsense2_camera/accel_optical_frame_id: t265_accel_optica...
 * /t265/realsense2_camera/align_depth: False
 * /t265/realsense2_camera/aligned_depth_to_color_frame_id: t265_aligned_dept...
 * /t265/realsense2_camera/aligned_depth_to_fisheye1_frame_id: t265_aligned_dept...
 * /t265/realsense2_camera/aligned_depth_to_fisheye2_frame_id: t265_aligned_dept...
 * /t265/realsense2_camera/aligned_depth_to_fisheye_frame_id: t265_aligned_dept...
 * /t265/realsense2_camera/aligned_depth_to_infra1_frame_id: t265_aligned_dept...
 * /t265/realsense2_camera/aligned_depth_to_infra2_frame_id: t265_aligned_dept...
 * /t265/realsense2_camera/allow_no_texture_points: False
 * /t265/realsense2_camera/base_frame_id: t265_link
 * /t265/realsense2_camera/calib_odom_file: 
 * /t265/realsense2_camera/clip_distance: -1.0
 * /t265/realsense2_camera/color_fps: 30
 * /t265/realsense2_camera/color_frame_id: t265_color_frame
 * /t265/realsense2_camera/color_height: 480
 * /t265/realsense2_camera/color_optical_frame_id: t265_color_optica...
 * /t265/realsense2_camera/color_width: 640
 * /t265/realsense2_camera/confidence_fps: 30
 * /t265/realsense2_camera/confidence_height: 480
 * /t265/realsense2_camera/confidence_width: 640
 * /t265/realsense2_camera/depth_fps: 30
 * /t265/realsense2_camera/depth_frame_id: t265_depth_frame
 * /t265/realsense2_camera/depth_height: 480
 * /t265/realsense2_camera/depth_optical_frame_id: t265_depth_optica...
 * /t265/realsense2_camera/depth_width: 640
 * /t265/realsense2_camera/device_type: 
 * /t265/realsense2_camera/enable_accel: False
 * /t265/realsense2_camera/enable_color: True
 * /t265/realsense2_camera/enable_confidence: True
 * /t265/realsense2_camera/enable_depth: True
 * /t265/realsense2_camera/enable_fisheye1: False
 * /t265/realsense2_camera/enable_fisheye2: False
 * /t265/realsense2_camera/enable_fisheye: False
 * /t265/realsense2_camera/enable_gyro: False
 * /t265/realsense2_camera/enable_infra1: False
 * /t265/realsense2_camera/enable_infra2: False
 * /t265/realsense2_camera/enable_infra: False
 * /t265/realsense2_camera/enable_pointcloud: False
 * /t265/realsense2_camera/enable_pose: False
 * /t265/realsense2_camera/enable_sync: False
 * /t265/realsense2_camera/filters: 
 * /t265/realsense2_camera/fisheye1_frame_id: t265_fisheye1_frame
 * /t265/realsense2_camera/fisheye1_optical_frame_id: t265_fisheye1_opt...
 * /t265/realsense2_camera/fisheye2_frame_id: t265_fisheye2_frame
 * /t265/realsense2_camera/fisheye2_optical_frame_id: t265_fisheye2_opt...
 * /t265/realsense2_camera/fisheye_fps: 30
 * /t265/realsense2_camera/fisheye_frame_id: t265_fisheye_frame
 * /t265/realsense2_camera/fisheye_height: 0
 * /t265/realsense2_camera/fisheye_optical_frame_id: t265_fisheye_opti...
 * /t265/realsense2_camera/fisheye_width: 0
 * /t265/realsense2_camera/gyro_fps: 0
 * /t265/realsense2_camera/gyro_frame_id: t265_gyro_frame
 * /t265/realsense2_camera/gyro_optical_frame_id: t265_gyro_optical...
 * /t265/realsense2_camera/imu_optical_frame_id: t265_imu_optical_...
 * /t265/realsense2_camera/infra1_frame_id: t265_infra1_frame
 * /t265/realsense2_camera/infra1_optical_frame_id: t265_infra1_optic...
 * /t265/realsense2_camera/infra2_frame_id: t265_infra2_frame
 * /t265/realsense2_camera/infra2_optical_frame_id: t265_infra2_optic...
 * /t265/realsense2_camera/infra_fps: 30
 * /t265/realsense2_camera/infra_height: 480
 * /t265/realsense2_camera/infra_rgb: False
 * /t265/realsense2_camera/infra_width: 640
 * /t265/realsense2_camera/initial_reset: False
 * /t265/realsense2_camera/json_file_path: 
 * /t265/realsense2_camera/linear_accel_cov: 0.01
 * /t265/realsense2_camera/odom_frame_id: t265_odom_frame
 * /t265/realsense2_camera/ordered_pc: False
 * /t265/realsense2_camera/pointcloud_texture_index: 0
 * /t265/realsense2_camera/pointcloud_texture_stream: RS2_STREAM_COLOR
 * /t265/realsense2_camera/pose_frame_id: t265_pose_frame
 * /t265/realsense2_camera/pose_optical_frame_id: t265_pose_optical...
 * /t265/realsense2_camera/publish_odom_tf: True
 * /t265/realsense2_camera/publish_tf: True
 * /t265/realsense2_camera/rosbag_filename: /home/parallels/D...
 * /t265/realsense2_camera/serial_no: 
 * /t265/realsense2_camera/stereo_module/exposure/1: 7500
 * /t265/realsense2_camera/stereo_module/exposure/2: 1
 * /t265/realsense2_camera/stereo_module/gain/1: 16
 * /t265/realsense2_camera/stereo_module/gain/2: 16
 * /t265/realsense2_camera/tf_publish_rate: 0.0
 * /t265/realsense2_camera/topic_odom_in: t265/odom_in
 * /t265/realsense2_camera/unite_imu_method: none
 * /t265/realsense2_camera/usb_port_id: 

NODES
  /t265/
    realsense2_camera (nodelet/nodelet)
    t265_manager (nodelet/nodelet)

auto-starting new master
process[master]: started with pid [12638]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 9bc4ab24-a272-11eb-9269-001c42b717af
process[rosout-1]: started with pid [12649]
started core service [/rosout]
process[t265/t265_manager-2]: started with pid [12659]
process[t265/realsense2_camera-3]: started with pid [12660]
[ INFO] [1618989869.632563083]: Initializing nodelet with 8 worker threads.
[ INFO] [1618989869.706370876]: RealSense ROS v2.2.23
[ INFO] [1618989869.706412378]: Built with LibRealSense v2.44.0
[ INFO] [1618989869.706430688]: Running with LibRealSense v2.44.0
[ INFO] [1618989869.719871692]: publish topics from rosbag file: /home/parallels/Documents/20210421_161336.bag
[ERROR] [1618989869.763143413]: Exception: Could not find requested sensor type!

---

Update:

I tested again with some of the previous bag files that were recorded through realsense-viewer and it turned out to be worked. My guess of the error above is caused by the questionable bag file through viewer when I tried to record three devices. A close look you will find this bag file has some topics that shouldn't exist for t265. But when I used rs-rosbag-inspector to check this bag file it doesn't display any topics that shouldn't exist as rs_from_file.launch has shown above. I wonder what are these topics and where are these topics come from.

[jeffdelmerico]

Unfortunately, that workaround still resulted in the same error for me. However, with some more debugging, it looks like the exception is triggered from rs_device.hpp:58 when the wheel odometer variable (_wo_snr) is set in the initializer list of the T265RealsenseNode constructor.

So for me, the process never reaches the point where it calls publishTopics and can hit that fix, but instead the exception gets thrown when the factory tries to initialize the T265RealsenseNode here, and then is caught here.