fix updateSensors and IMU callbacks

IntelRealSense · Aug 3, 2022 · 3f1f51b · 3f1f51b
1 parent 725168d
commit 3f1f51b
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 1 deletion.
diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp
@@ -404,6 +404,13 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame)
 
     auto stream_index = (stream == GYRO.first)?GYRO:ACCEL;
     rclcpp::Time t(frameSystemTimeSec(frame));
+
+    if(_imu_publishers.find(stream_index) == _imu_publishers.end())
+    {
+        ROS_DEBUG("Received IMU callback while topic does not exist");
+        return;
+    }
+
     if (0 != _imu_publishers[stream_index]->get_subscription_count())
     {
         auto imu_msg = sensor_msgs::msg::Imu();

diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp
@@ -296,7 +296,12 @@ void BaseRealSenseNode::updateSensors()
             std::vector<stream_profile> wanted_profiles;
 
             bool is_profile_changed(sensor->getUpdatedProfiles(wanted_profiles));
-            if (is_profile_changed || _is_align_depth_changed)
+            bool is_video_sensor = (sensor->is<rs2::depth_sensor>() || sensor->is<rs2::color_sensor>() || sensor->is<rs2::fisheye_sensor>());          
+
+            // do all updates if profile has been changed, or if the align depth filter status has been changed
+            // and we are on a video sensor. TODO: explore better options to monitor and update changes
+            // without resetting the whole sensors and topics.
+            if (is_profile_changed || (_is_align_depth_changed && is_video_sensor))
             {
                 std::vector<stream_profile> active_profiles = sensor->get_active_streams();
                 if(is_profile_changed)