IMU publisher thread using too much CPU

[omar-abugharbiyeh]

Describe the bug

When launching the Azure Kinect driver it appears to be using 100% of the available CPU even without any connected cameras.

To Reproduce this problem seems to happen on the foxy-devel branch when cloning, building, and then launching the driver. I have managed to narrow down the reason slightly and I've noticed that this only happens when the IMU publisher thread starts. I am running it through docker but I dont think that has any effect on the issue.

Logs

[INFO] [launch]: All log files can be found below /root/.ros/log/2021-10-12-08-41-39-821603-logi-XMG-laptop-171497
[INFO] [launch]: Default logging verbosity is set to INFO
Robot description xacro_file : /code/ros2_ws/install/azure_kinect_ros_driver/share/azure_kinect_ros_driver/urdf/azure_kinect.urdf.xacro
Robot description urdf_path : /code/ros2_ws/install/azure_kinect_ros_driver/share/azure_kinect_ros_driver/urdf/azure_kinect.urdf
[INFO] [node-1]: process started with pid [171505]
[INFO] [robot_state_publisher-2]: process started with pid [171507]
[INFO] [joint_state_publisher-3]: process started with pid [171509]
[robot_state_publisher-2] Parsing robot urdf xml string.
[robot_state_publisher-2] Link camera_body had 0 children
[robot_state_publisher-2] Link camera_visor had 0 children
[robot_state_publisher-2] [INFO] [1634020899.927570814] [robot_state_publisher]: got segment camera_base
[robot_state_publisher-2] [INFO] [1634020899.927664747] [robot_state_publisher]: got segment camera_body
[robot_state_publisher-2] [INFO] [1634020899.927676689] [robot_state_publisher]: got segment camera_visor
[node-1] [INFO] [1634020899.997823250] [k4a_ros_device_node]: K4A Parameters:
[node-1] [INFO] [1634020899.997992888] [k4a_ros_device_params_node]: sensor_sn - std::string : 
[node-1] [INFO] [1634020899.998029693] [k4a_ros_device_params_node]: depth_enabled - bool : 1
[node-1] [INFO] [1634020899.998043032] [k4a_ros_device_params_node]: depth_unit - std::string : 16UC1
[node-1] [INFO] [1634020899.998056302] [k4a_ros_device_params_node]: depth_mode - std::string : WFOV_UNBINNED
[node-1] [INFO] [1634020899.998068873] [k4a_ros_device_params_node]: color_enabled - bool : 1
[node-1] [INFO] [1634020899.998081513] [k4a_ros_device_params_node]: color_format - std::string : bgra
[node-1] [INFO] [1634020899.998094364] [k4a_ros_device_params_node]: color_resolution - std::string : 1536P
[node-1] [INFO] [1634020899.998106725] [k4a_ros_device_params_node]: fps - int : 5
[node-1] [INFO] [1634020899.998351021] [k4a_ros_device_params_node]: point_cloud - bool : 1
[node-1] [INFO] [1634020899.998369878] [k4a_ros_device_params_node]: rgb_point_cloud - bool : 1
[node-1] [INFO] [1634020899.998383217] [k4a_ros_device_params_node]: point_cloud_in_depth_frame - bool : 0
[node-1] [INFO] [1634020899.998395718] [k4a_ros_device_params_node]: tf_prefix - std::string : 
[node-1] [INFO] [1634020899.998408429] [k4a_ros_device_params_node]: recording_file - std::string : 
[node-1] [INFO] [1634020899.998421419] [k4a_ros_device_params_node]: recording_loop_enabled - bool : 0
[node-1] [INFO] [1634020899.998433780] [k4a_ros_device_params_node]: body_tracking_enabled - bool : 0
[node-1] [INFO] [1634020899.998462414] [k4a_ros_device_params_node]: body_tracking_smoothing_factor - float : 0
[node-1] [INFO] [1634020899.998473798] [k4a_ros_device_params_node]: imu_rate_target - int : 0
[node-1] [INFO] [1634020899.998486788] [k4a_ros_device_params_node]: rescale_ir_to_mono8 - bool : 0
[node-1] [INFO] [1634020899.998499079] [k4a_ros_device_params_node]: ir_mono8_scaling_factor - float : 1
[node-1] [INFO] [1634020899.998511581] [k4a_ros_device_params_node]: wired_sync_mode - int : 0
[node-1] [INFO] [1634020899.998524361] [k4a_ros_device_params_node]: subordinate_delay_off_master_usec - int : 0
[node-1] [INFO] [1634020900.005179298] [k4a_ros_device_node]: Found 0 sensors
[node-1] [INFO] [1634020900.005711539] [k4a_ros_device_node]: No serial number provided: picking first sensor
[node-1] [ERROR] [1634020900.006162837] [k4a_ros_device_node]: Failed to open a K4A device. Cannot continue.
[node-1] [INFO] [1634020900.006505955] [k4a_ros_device_params_node]: Setting wired sync mode: 0
[node-1] [INFO] [1634020900.006825118] [k4a_ros_device_params_node]: Setting subordinate delay: 0
[node-1] [INFO] [1634020900.007125774] [k4a_ros_device_params_node]: Setting RGB Camera Format: bgra
[node-1] [INFO] [1634020900.007426500] [k4a_ros_device_params_node]: Setting RGB Camera Resolution: 1536P
[node-1] [INFO] [1634020900.007735187] [k4a_ros_device_params_node]: Setting Depth Camera Mode: WFOV_UNBINNED
[node-1] [INFO] [1634020900.008050090] [k4a_ros_device_params_node]: Setting Camera FPS: 5
[node-1] [INFO] [1634020900.008361431] [k4a_ros_device_params_node]: Using default IMU rate. Setting to maximum: 1600 Hz.
[node-1] [INFO] [1634020900.008694841] [k4a_ros_device_params_node]: Setting Target IMU rate to 1600 (desired: 1600)
[node-1] [INFO] [1634020900.009120159] [k4a_bridge]: K4A Started
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[robot_state_publisher-2] [INFO] [1634020908.743874972] [rclcpp]: signal_handler(signal_value=2)
[node-1] [INFO] [1634020908.743855487] [rclcpp]: signal_handler(signal_value=2)
[node-1] [INFO] [1634020908.744749559] [k4a_bridge]: ROS Exit Started
[node-1] [INFO] [1634020908.744787691] [k4a_ros_device_node]: Joining camera publisher thread
[node-1] [INFO] [1634020908.745021021] [k4a_ros_device_node]: Camera publisher thread joined
[node-1] [INFO] [1634020908.745049166] [k4a_ros_device_node]: Joining IMU publisher thread
[node-1] [INFO] [1634020908.745059782] [k4a_ros_device_node]: IMU publisher thread joined
[node-1] [INFO] [1634020908.754288445] [k4a_bridge]: ROS Exit
[node-1] [INFO] [1634020908.754389711] [k4a_bridge]: ROS Shutdown complete
[node-1] [INFO] [1634020908.754394460] [k4a_bridge]: Finished ros bridge main
[INFO] [robot_state_publisher-2]: process has finished cleanly [pid 171507]
[INFO] [node-1]: process has finished cleanly [pid 171505]
[INFO] [joint_state_publisher-3]: process has finished cleanly [pid 171509]

Desktop:

• OS: Ubuntu
• Version Ubuntu 20.04.3 LTS

[omar-abugharbiyeh]

it seems that the issue was that the while loop in imuPublisherThread is missing a loop_rate.sleep()