search

IntelRealSense / realsense-ros

ROS Wrapper for Intel(R) RealSense(TM) Cameras
http://wiki.ros.org/RealSense
Apache License 2.0
2.54k stars 1.75k forks source link

cannot find "camera_imu_optical_frame" in TF #2560

Closed chivas1000 closed 1 year ago

chivas1000 commented 1 year ago

Hi, I'm using realsense ros2 wrapper, and I want to use the imu of d435i, when I open the imu and it can output /camera/imu in ros2 topic list and it can show properly but as to the TF relationship, it didn't output which I cannot find camera_imu_optical_frame in TF am I missing something?

ros2 topic echo /camera/imu header: stamp: sec: 1669193225 nanosec: 913198592 frame_id: camera_imu_optical_frame orientation: x: 0.0 y: 0.0 z: 0.0 w: 0.0 orientation_covariance:

my yaml file:

device_type: '' serial_no: '' usb_port_id: ''

rgb_camera: profile: '640x480x15' color_qos: "SENSOR_DATA"

depth_module: profile: '848x480x60' emitter_enabled: 1 emitter_on_off: true depth_qos: "SENSOR_DATA" depth_info_qos: "SYSTEM_DEFAULT"

infra_qos: "SENSOR_DATA"

enable_color: true enable_depth: true

enable_gyro: false

enable_accel: false

enable_infra1: true enable_infra2: true

imu

enable_gyro: true enable_accel: true unite_imu_method: 2 enable_sync: true

may need

hold_back_imu_for_frames

pointcloud: enable: false

pointcloud_texture_index: 0 pointcloud_texture_stream: RS2_STREAM_ANY

enable_sync: false

align_depth: false

chivas1000 commented 1 year ago

here is my output of launch:

nuc@nuc-NUC10i7FNK:~$ ros2 launch realsense2_camera nvr_debug.launch.py [INFO] [launch]: All log files can be found below /home/nuc/.ros/log/2022-11-23-19-10-55-137874-nuc-NUC10i7FNK-3005 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [component_container-1]: process started with pid [3018] [INFO] [static_transform_publisher-2]: process started with pid [3020] [static_transform_publisher-2] [INFO] [1669201855.313504411] [static_transform_publisher_pbHqw8JU1A5c4ZAc]: Spinning until killed publishing transform from 'base_link' to 'camera_link' [component_container-1] [INFO] [1669201855.546749999] [realsense_container]: Load Library: /home/nuc/ros2_ws/install/realsense2_camera/lib/librealsense2_camera.so [component_container-1] [INFO] [1669201855.674242834] [realsense_container]: Found class: rclcpp_components::NodeFactoryTemplate [component_container-1] [INFO] [1669201855.674280788] [realsense_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate [component_container-1] [INFO] [1669201857.597524924] [camera.camera]: RealSense ROS v4.51.1 [component_container-1] [INFO] [1669201857.597571861] [camera.camera]: Built with LibRealSense v2.51.1 [component_container-1] [INFO] [1669201857.597582539] [camera.camera]: Running with LibRealSense v2.51.1 [INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/camera/camera' in container '/realsense_container' [component_container-1] [INFO] [1669201857.600767956] [realsense_container]: Load Library: /home/nuc/ros2_ws/install/realsense_splitter/lib/librealsense_splitter_component.so [component_container-1] [INFO] [1669201857.607938210] [realsense_container]: Found class: rclcpp_components::NodeFactoryTemplate [component_container-1] [INFO] [1669201857.607960397] [realsense_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate [component_container-1] [INFO] [1669201857.610350112] [camera.realsense_splitter_node]: Creating a RealsenseSplitterNode(). [INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/camera/realsense_splitter_node' in container '/realsense_container' [component_container-1] [INFO] [1669201857.658131574] [camera.camera]: Device with serial number 135122070687 was found. [component_container-1] [component_container-1] [INFO] [1669201857.658177898] [camera.camera]: Device with physical ID /sys/devices/pci0000:00/0000:00:1c.0/0000:01:00.0/0000:02:02.0/0000:39:00.0/usb4/4-1/4-1:1.0/video4linux/video0 was found. [component_container-1] [INFO] [1669201857.658186417] [camera.camera]: Device with name Intel RealSense D435I was found. [component_container-1] [INFO] [1669201857.658413259] [camera.camera]: Device with port number 4-1 was found. [component_container-1] [INFO] [1669201857.658425655] [camera.camera]: Device USB type: 3.2

[component_container-1] [INFO] [1669201857.660344129] [camera.camera]: JSON file is not provided [component_container-1] [INFO] [1669201857.660373665] [camera.camera]: Device Name: Intel RealSense D435I [component_container-1] [INFO] [1669201857.660383203] [camera.camera]: Device Serial No: 135122070687 [component_container-1] [INFO] [1669201857.660389718] [camera.camera]: Device physical port: /sys/devices/pci0000:00/0000:00:1c.0/0000:01:00.0/0000:02:02.0/0000:39:00.0/usb4/4-1/4-1:1.0/video4linux/video0 [component_container-1] [INFO] [1669201857.660396155] [camera.camera]: Device FW version: 05.13.00.50 [component_container-1] [INFO] [1669201857.660401891] [camera.camera]: Device Product ID: 0x0B3A [component_container-1] [INFO] [1669201857.660407712] [camera.camera]: Sync Mode: On [component_container-1] [WARN] [1669201857.674633159] [camera.camera]: re-enable the stream for the change to take effect. [component_container-1] [WARN] [1669201857.674749088] [camera.camera]: re-enable the stream for the change to take effect. [component_container-1] [WARN] [1669201857.675260228] [camera.camera]: re-enable the stream for the change to take effect. [component_container-1] [WARN] [1669201857.683171625] [camera.camera]: re-enable the stream for the change to take effect. [component_container-1] [WARN] [1669201857.684017805] [camera.camera]: re-enable the stream for the change to take effect. [component_container-1] [INFO] [1669201857.692662333] [camera.camera]: Stopping Sensor: Stereo Module [component_container-1] [INFO] [1669201857.747004396] [camera.camera]: Starting Sensor: Stereo Module [component_container-1] [INFO] [1669201857.800765292] [camera.camera]: Open profile: stream_type: Infra(1), Format: Y8, Width: 848, Height: 480, FPS: 60 [component_container-1] [INFO] [1669201857.800811529] [camera.camera]: Open profile: stream_type: Infra(2), Format: Y8, Width: 848, Height: 480, FPS: 60 [component_container-1] [INFO] [1669201857.800820453] [camera.camera]: Open profile: stream_type: Depth(0), Format: Z16, Width: 848, Height: 480, FPS: 60 [component_container-1] [INFO] [1669201857.801033247] [camera.camera]: Stopping Sensor: RGB Camera [component_container-1] [INFO] [1669201857.801881131] [camera.camera]: Starting Sensor: RGB Camera [component_container-1] [INFO] [1669201857.803507061] [camera.camera]: Open profile: stream_type: Color(0), Format: RGB8, Width: 640, Height: 480, FPS: 15 [component_container-1] [INFO] [1669201857.803541993] [camera.camera]: Stopping Sensor: Motion Module [component_container-1] [INFO] [1669201857.805405332] [camera.camera]: Starting Sensor: Motion Module [component_container-1] [INFO] [1669201857.806861482] [camera.camera]: Open profile: stream_type: Accel(0)Format: MOTION_XYZ32F, FPS: 100 [component_container-1] [INFO] [1669201857.806883140] [camera.camera]: Open profile: stream_type: Gyro(0)Format: MOTION_XYZ32F, FPS: 200 [component_container-1] [INFO] [1669201857.807470721] [camera.camera]: RealSense Node Is Up! [component_container-1] [WARN] [1669201857.837939446] [camera.camera]:

MartyG-RealSense commented 1 year ago

Hi @chivas1000 There does not seem to be a problem with the publishing of all of the streams including accel and gyro.

Could you first test whether setting initial_reset to true in your yaml file to reset the camera at launch makes a difference, please?

chivas1000 commented 1 year ago

@MartyG-RealSense Hi, thanks for the reply, after adding initial_reset : true, the output is as follows, which it has two errors at the end: nuc@nuc-NUC10i7FNK:~$ ros2 launch realsense2_camera nvr_debug.launch.py [INFO] [launch]: All log files can be found below /home/nuc/.ros/log/2022-11-24-10-32-18-769920-nuc-NUC10i7FNK-3689 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [component_container-1]: process started with pid [3702] [INFO] [static_transform_publisher-2]: process started with pid [3704] [static_transform_publisher-2] [INFO] [1669257138.903907952] [static_transform_publisher_Kwywt9Sdr6KS2kki]: Spinning until killed publishing transform from 'base_link' to 'camera_link' [component_container-1] [INFO] [1669257139.135451001] [realsense_container]: Load Library: /home/nuc/ros2_ws/install/realsense2_camera/lib/librealsense2_camera.so [component_container-1] [INFO] [1669257139.196322043] [realsense_container]: Found class: rclcpp_components::NodeFactoryTemplate [component_container-1] [INFO] [1669257139.196368916] [realsense_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate [component_container-1] [INFO] [1669257141.117522479] [camera.camera]: RealSense ROS v4.51.1 [component_container-1] [INFO] [1669257141.117570424] [camera.camera]: Built with LibRealSense v2.51.1 [component_container-1] [INFO] [1669257141.117578801] [camera.camera]: Running with LibRealSense v2.51.1 [INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/camera/camera' in container '/realsense_container' [component_container-1] [INFO] [1669257141.119128145] [realsense_container]: Load Library: /home/nuc/ros2_ws/install/realsense_splitter/lib/librealsense_splitter_component.so [component_container-1] [INFO] [1669257141.120541258] [realsense_container]: Found class: rclcpp_components::NodeFactoryTemplate [component_container-1] [INFO] [1669257141.120558870] [realsense_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate [component_container-1] [INFO] [1669257141.122501454] [camera.realsense_splitter_node]: Creating a RealsenseSplitterNode(). [INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/camera/realsense_splitter_node' in container '/realsense_container' [component_container-1] [INFO] [1669257141.177017042] [camera.camera]: Device with serial number 135122070687 was found. [component_container-1] [component_container-1] [INFO] [1669257141.177064109] [camera.camera]: Device with physical ID /sys/devices/pci0000:00/0000:00:1c.0/0000:01:00.0/0000:02:02.0/0000:39:00.0/usb4/4-1/4-1:1.0/video4linux/video0 was found. [component_container-1] [INFO] [1669257141.177072500] [camera.camera]: Device with name Intel RealSense D435I was found. [component_container-1] [INFO] [1669257141.177273603] [camera.camera]: Device with port number 4-1 was found. [component_container-1] [INFO] [1669257141.177285875] [camera.camera]: Device USB type: 3.2 [component_container-1] [INFO] [1669257141.177293344] [camera.camera]: Resetting device... [component_container-1] [INFO] [1669257147.384513721] [camera.camera]: Device with serial number 135122070687 was found. [component_container-1] [component_container-1] [INFO] [1669257147.384562112] [camera.camera]: Device with physical ID /sys/devices/pci0000:00/0000:00:1c.0/0000:01:00.0/0000:02:02.0/0000:39:00.0/usb4/4-1/4-1:1.0/video4linux/video0 was found. [component_container-1] [INFO] [1669257147.384570258] [camera.camera]: Device with name Intel RealSense D435I was found. [component_container-1] [INFO] [1669257147.384745602] [camera.camera]: Device with port number 4-1 was found. [component_container-1] [INFO] [1669257147.384756779] [camera.camera]: Device USB type: 3.2

[component_container-1] [INFO] [1669257147.385932152] [camera.camera]: JSON file is not provided [component_container-1] [INFO] [1669257147.385951690] [camera.camera]: Device Name: Intel RealSense D435I [component_container-1] [INFO] [1669257147.385962739] [camera.camera]: Device Serial No: 135122070687 [component_container-1] [INFO] [1669257147.385972042] [camera.camera]: Device physical port: /sys/devices/pci0000:00/0000:00:1c.0/0000:01:00.0/0000:02:02.0/0000:39:00.0/usb4/4-1/4-1:1.0/video4linux/video0 [component_container-1] [INFO] [1669257147.385978212] [camera.camera]: Device FW version: 05.13.00.50 [component_container-1] [INFO] [1669257147.385983516] [camera.camera]: Device Product ID: 0x0B3A [component_container-1] [INFO] [1669257147.385988371] [camera.camera]: Sync Mode: On [component_container-1] [WARN] [1669257147.700995753] [camera.camera]: re-enable the stream for the change to take effect. [component_container-1] [WARN] [1669257147.701087235] [camera.camera]: re-enable the stream for the change to take effect. [component_container-1] [WARN] [1669257147.701515031] [camera.camera]: re-enable the stream for the change to take effect. [component_container-1] [WARN] [1669257147.708382640] [camera.camera]: re-enable the stream for the change to take effect. [component_container-1] [WARN] [1669257147.708968597] [camera.camera]: re-enable the stream for the change to take effect. [component_container-1] [INFO] [1669257147.714606923] [camera.camera]: Stopping Sensor: Stereo Module [component_container-1] [INFO] [1669257147.768014588] [camera.camera]: Starting Sensor: Stereo Module [component_container-1] [INFO] [1669257147.821525495] [camera.camera]: Open profile: stream_type: Infra(1), Format: Y8, Width: 848, Height: 480, FPS: 60 [component_container-1] [INFO] [1669257147.821578679] [camera.camera]: Open profile: stream_type: Infra(2), Format: Y8, Width: 848, Height: 480, FPS: 60 [component_container-1] [INFO] [1669257147.821588331] [camera.camera]: Open profile: stream_type: Depth(0), Format: Z16, Width: 848, Height: 480, FPS: 60 [component_container-1] [INFO] [1669257147.821849612] [camera.camera]: Stopping Sensor: RGB Camera [component_container-1] [INFO] [1669257147.822756410] [camera.camera]: Starting Sensor: RGB Camera [component_container-1] [INFO] [1669257147.824327015] [camera.camera]: Open profile: stream_type: Color(0), Format: RGB8, Width: 640, Height: 480, FPS: 15 [component_container-1] [INFO] [1669257147.824366454] [camera.camera]: Stopping Sensor: Motion Module [component_container-1] [WARN] [1669257147.863450739] [camera.camera]: [component_container-1] [WARN] [1669257147.863496509] [camera.camera]: frame's time domain is HARDWARE_CLOCK. Timestamps may reset periodically. [component_container-1] [INFO] [1669257147.923715307] [camera.camera]: Starting Sensor: Motion Module [component_container-1] [INFO] [1669257147.925429258] [camera.camera]: Open profile: stream_type: Accel(0)Format: MOTION_XYZ32F, FPS: 100 [component_container-1] [INFO] [1669257147.925451886] [camera.camera]: Open profile: stream_type: Gyro(0)Format: MOTION_XYZ32F, FPS: 200 [component_container-1] [INFO] [1669257147.926099846] [camera.camera]: RealSense Node Is Up! [component_container-1] [WARN] [1669257148.768799873] [camera.camera]: XXX Hardware Notification:Motion Module failure,1.66926e+12,Error,Hardware Error [component_container-1] [WARN] [1669257148.768869706] [camera.camera]: Hardware Notification:Motion Module failure,1.66926e+12,Error,Hardware Error

and then it still doesn't come out the "camera_imu_optical_frame": Screenshot from 2022-11-24 10-35-44

also, is that the ROS2 realsense wrapper wouldn't publish this TF relationship? since I saw this thread, that it has to use imu_filter_madgwick node to publish this frame https://github.com/IntelRealSense/realsense-ros/issues/2400

MartyG-RealSense commented 1 year ago

Does the TF publish if you remove the unite_imu_method instruction from your configuration file so that the IMU topics are published as separate 'accel' and 'gyro' topics instead of a single /imu topic?

MartyG-RealSense commented 1 year ago

Your TF issue also sounds like a recent case at https://github.com/IntelRealSense/realsense-ros/issues/2563

MartyG-RealSense commented 1 year ago

Hi @chivas1000 Do you have an update about this case that you can provide, please? Thanks!

chivas1000 commented 1 year ago

Hi @chivas1000 Do you have an update about this case that you can provide, please? Thanks!

Hi, in my case, actually I've use https://github.com/TaiyouKomazawa/ros2_imu_filter_madgwick to let it output the camera_imu_optical_frame, but actually in my vslam application, this frame is no need but camera_accel_optical_frame can be the imu frame needed.

in summary, for one who wants the camera_imu_optical_frame, just install imu_filter_madgwick package, since in realsense ROS wrapper it might not have the internal method for this united imu frame published as TF.

MartyG-RealSense commented 1 year ago

Thanks so much @chivas1000 for your helpful advice for others in the RealSense ROS community who are in this situation!

MartyG-RealSense commented 1 year ago

Hi @chivas1000 Do you require further assistance with this case, please? Thanks!

MartyG-RealSense commented 1 year ago

Case closed due to solution achieved and no further comments received.