Create /imu topic only when motion streams enabled

Arun-Prasad-V · Aug 18, 2023 · e14ba75 · e14ba75
1 parent 25a1191
commit e14ba75
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 10 deletions.
diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h
@@ -252,7 +252,6 @@ namespace realsense2_camera
         void startDiagnosticsUpdater();
         void monitoringProfileChanges();
         void publish_temperature();
-        void setupFiltersPublishers();
         void setAvailableSensors();
         void setCallbackFunctions();
         void updateSensors();
@@ -311,6 +310,8 @@ namespace realsense2_camera
         bool _enable_rgbd;
         bool _is_color_enabled;
         bool _is_depth_enabled;
+        bool _is_accel_enabled;
+        bool _is_gyro_enabled;
         bool _pointcloud;
         bool _publish_odom_tf;
         imu_sync_method _imu_sync_method;

diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp
@@ -112,6 +112,8 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node,
     _enable_rgbd(ENABLE_RGBD),
     _is_color_enabled(false),
     _is_depth_enabled(false),
+    _is_accel_enabled(false),
+    _is_gyro_enabled(false),
     _pointcloud(false),
     _publish_odom_tf(false),
     _imu_sync_method(imu_sync_method::NONE),
@@ -381,7 +383,7 @@ void BaseRealSenseNode::imu_callback_sync(rs2::frame frame, imu_sync_method sync
         _is_initialized_time_base = setBaseTime(frame_time, frame.get_frame_timestamp_domain());
     }
 
-    if (0 != _synced_imu_publisher->getNumSubscribers())
+    if (_synced_imu_publisher && (0 != _synced_imu_publisher->getNumSubscribers()))
     {
         auto crnt_reading = *(reinterpret_cast<const float3*>(frame.get_data()));
         Eigen::Vector3d v(crnt_reading.x, crnt_reading.y, crnt_reading.z);
@@ -464,7 +466,8 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame)
 
 void BaseRealSenseNode::frame_callback(rs2::frame frame)
 {
-    _synced_imu_publisher->Pause();
+    if (_synced_imu_publisher)
+        _synced_imu_publisher->Pause();
     double frame_time = frame.get_timestamp();
 
     // We compute a ROS timestamp which is based on an initial ROS time at point of first frame,
@@ -576,7 +579,8 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame)
         }
         publishFrame(frame, t, sip, _images, _info_publishers, _image_publishers);
      }
-    _synced_imu_publisher->Resume();
+     if (_synced_imu_publisher)
+        _synced_imu_publisher->Resume();
 } // frame_callback
 
 void BaseRealSenseNode::multiple_message_callback(rs2::frame frame, imu_sync_method sync_method)

diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp
@@ -27,18 +27,12 @@ void BaseRealSenseNode::setup()
     setAvailableSensors();
     SetBaseStream();
     setupFilters();
-    setupFiltersPublishers();
     setCallbackFunctions();
     monitoringProfileChanges();
     updateSensors();
     publishServices();
 }
 
-void BaseRealSenseNode::setupFiltersPublishers()
-{
-    _synced_imu_publisher = std::make_shared<SyncedImuPublisher>(_node.create_publisher<sensor_msgs::msg::Imu>("imu", 5));
-}
-
 void BaseRealSenseNode::monitoringProfileChanges()
 {
     int time_interval(10000);
@@ -190,6 +184,9 @@ void BaseRealSenseNode::stopPublishers(const std::vector<stream_profile>& profil
         }
         else if (profile.is<rs2::motion_stream_profile>())
         {
+            _is_accel_enabled = false;
+            _is_gyro_enabled = false;
+            _synced_imu_publisher.reset();
             _imu_publishers.erase(sip);
             _imu_info_publishers.erase(sip);
         }
@@ -270,6 +267,11 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& profi
         }
         else if (profile.is<rs2::motion_stream_profile>())
         {
+            if(profile.stream_type() == RS2_STREAM_ACCEL)
+                _is_accel_enabled = true;
+            else if (profile.stream_type() == RS2_STREAM_GYRO)
+                _is_gyro_enabled = true;
+
             std::stringstream data_topic_name, info_topic_name;
             data_topic_name << stream_name << "/sample";
             _imu_publishers[sip] = _node.create_publisher<sensor_msgs::msg::Imu>(data_topic_name.str(),
@@ -305,6 +307,11 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& profi
                 rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(extrinsics_qos), extrinsics_qos), std::move(options));
         }
     }
+    if (_is_accel_enabled && _is_gyro_enabled && (_imu_sync_method > imu_sync_method::NONE))
+    {
+        _synced_imu_publisher = std::make_shared<SyncedImuPublisher>(_node.create_publisher<sensor_msgs::msg::Imu>("imu", 5));
+    }
+
 }
 
 void BaseRealSenseNode::startRGBDPublisherIfNeeded()