Merge PR IntelRealSense#2849 from Arun-Prasad-V: Create /imu topic on…

…ly when motion streams enabled

README.md

@@ -304,6 +304,22 @@ User can set the camera name and camera namespace, to distinguish between camera
   - This param also depends on **publish_tf** param
     - If **publish_tf:=false**, then no TFs will be published, even if **tf_publish_rate** is >0.0 Hz
     - If **publish_tf:=true** and **tf_publish_rate** set to >0.0 Hz, then dynamic TFs will be published at the specified rate
+- **unite_imu_method**:
+  - For the D400 cameras with built in IMU components, below 2 unrelated streams (each with it's own frequency) will be created:
+    - *gyro* - which shows angular velocity 
+    - *accel* - which shows linear acceleration. 
+  - Both streams will publish data to its corresponding topics:
+    - '/camera/camera/gyro/sample' & '/camera/camera/accel/sample'
+    - Though both topics are of same message type 'sensor_msgs::Imu', only their relevant fields are filled out.
+  - A new topic called **imu** will be created, when both *accel* and *gyro* streams are enabled and the param *unite_imu_method* set to > 0.
+    - Data from both accel and gyro are combined and published to this topic
+    - All the fields of the Imu message are filled out.
+    - It will be published at the rate of the gyro.
+  - `unite_imu_method` param supports below values:
+    - 0 -> **none**: no imu topic
+    - 1 -> **copy**: Every gyro message will be attached by the last accel message.
+    - 2 -> **linear_interpolation**: Every gyro message will be attached by an accel message which is interpolated to gyro's timestamp.
+  - Note: When the param *unite_imu_method* is dynamically updated, re-enable either gyro or accel stream for the change to take effect.
 
 #### Parameters that cannot be changed in runtime:
 - **serial_no**:
@@ -339,18 +355,6 @@ User can set the camera name and camera namespace, to distinguish between camera
   - For example: `initial_reset:=true`
 - **base_frame_id**: defines the frame_id all static transformations refers to.
 - **odom_frame_id**: defines the origin coordinate system in ROS convention (X-Forward, Y-Left, Z-Up). pose topic defines the pose relative to that system.
-
-- **unite_imu_method**:
-  - D400 cameras have built in IMU components which produce 2 unrelated streams, each with it's own frequency: 
-    - *gyro* - which shows angular velocity 
-    - *accel* which shows linear acceleration. 
-  - By default, 2 corresponding topics are available, each with only the relevant fields of the message sensor_msgs::Imu are filled out.
-  - Setting *unite_imu_method* creates a new topic, *imu*, that replaces the default *gyro* and *accel* topics.
-    - The *imu* topic is published at the rate of the gyro.
-    - All the fields of the Imu message under the *imu* topic are filled out. 
-  - `unite_imu_method` parameter supported values are [0-2] meaning:  [0 -> None, 1 -> Copy, 2 -> Linear_ interpolation] when:
-    - **linear_interpolation**: Every gyro message is attached by the an accel message interpolated to the gyro's timestamp.
-    - **copy**: Every gyro message is attached by the last accel message.
 - **clip_distance**:
   - Remove from the depth image all values above a given value (meters). Disable by giving negative value (default)
   - For example: `clip_distance:=1.5`

realsense2_camera/include/base_realsense_node.h

@@ -256,7 +256,6 @@ namespace realsense2_camera
         void startDiagnosticsUpdater();
         void monitoringProfileChanges();
         void publish_temperature();
-        void setupFiltersPublishers();
         void setAvailableSensors();
         void setCallbackFunctions();
         void updateSensors();
@@ -315,6 +314,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;

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),
@@ -416,7 +418,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);
@@ -499,7 +501,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,
@@ -613,7 +616,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)

realsense2_camera/src/parameters.cpp

@@ -135,6 +135,8 @@ void BaseRealSenseNode::setDynamicParams()
                             [this](const rclcpp::Parameter& parameter)
                             {
                                 _imu_sync_method = imu_sync_method(parameter.get_value<int>());
+                                ROS_WARN("For the 'unite_imu_method' param update to take effect, "
+                                         "re-enable either gyro or accel stream.");
                             }, crnt_descriptor);
     _parameters_names.push_back(param_name);
 }

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);
         }
@@ -272,6 +269,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(),
@@ -307,6 +309,14 @@ 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))
+    {
+        rmw_qos_profile_t qos = _use_intra_process ? qos_string_to_qos(DEFAULT_QOS) : qos_string_to_qos(HID_QOS);
+
+        _synced_imu_publisher = std::make_shared<SyncedImuPublisher>(_node.create_publisher<sensor_msgs::msg::Imu>("~/imu", 
+                                                        rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos)));
+    }
+
 }
 
 void BaseRealSenseNode::startRGBDPublisherIfNeeded()