PR #2332 from Samer: Fix align_depth enable/disable

realsense2_camera/include/base_realsense_node.h

@@ -238,6 +238,7 @@ namespace realsense2_camera
         double _tf_publish_rate, _diagnostics_period;
         std::mutex _publish_tf_mutex;
         std::mutex _update_sensor_mutex;
+        std::mutex _profile_changes_mutex;
 
         std::shared_ptr<tf2_ros::StaticTransformBroadcaster> _static_tf_broadcaster;
         std::shared_ptr<tf2_ros::TransformBroadcaster> _dynamic_tf_broadcaster;
@@ -271,7 +272,8 @@ namespace realsense2_camera
         stream_index_pair _pointcloud_texture;
         PipelineSyncer _syncer;
         rs2::asynchronous_syncer _asyncer;
-        std::shared_ptr<NamedFilter> _align_depth_filter, _colorizer_filter;
+        std::shared_ptr<NamedFilter> _colorizer_filter;
+        std::shared_ptr<AlignDepthFilter> _align_depth_filter;
         std::shared_ptr<PointcloudFilter> _pc_filter;
         std::vector<std::shared_ptr<NamedFilter>> _filters;
         std::vector<rs2::sensor> _dev_sensors;
@@ -290,6 +292,7 @@ namespace realsense2_camera
         std::shared_ptr<std::thread> _monitoring_pc;   //pc = profile changes
         mutable std::condition_variable _cv_temp, _cv_mpc, _cv_tf;
         bool _is_profile_changed;
+        bool _is_align_depth_changed;
 
         std::shared_ptr<diagnostic_updater::Updater> _diagnostics_updater;
         rs2::stream_profile _base_profile;

realsense2_camera/include/named_filter.h

@@ -57,4 +57,11 @@ namespace realsense2_camera
             rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr _pointcloud_publisher;
             std::string _pointcloud_qos;
     };
+
+    class AlignDepthFilter : public NamedFilter
+    {
+        public:
+            AlignDepthFilter(std::shared_ptr<rs2::filter> filter, std::function<void(const rclcpp::Parameter&)> update_align_depth_func,
+                std::shared_ptr<Parameters> parameters, rclcpp::Logger logger, bool is_enabled = false);
+    };
 }

realsense2_camera/src/base_realsense_node.cpp

@@ -84,7 +84,8 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node,
     _use_intra_process(use_intra_process),
     _is_initialized_time_base(false),
     _sync_frames(SYNC_FRAMES),
-    _is_profile_changed(false)
+    _is_profile_changed(false),
+    _is_align_depth_changed(false)
 {
     if ( use_intra_process )
     {
@@ -157,10 +158,26 @@ void BaseRealSenseNode::setupFilters()
     _filters.push_back(std::make_shared<NamedFilter>(std::make_shared<rs2::temporal_filter>(), _parameters, _logger));
     _filters.push_back(std::make_shared<NamedFilter>(std::make_shared<rs2::hole_filling_filter>(), _parameters, _logger));
     _filters.push_back(std::make_shared<NamedFilter>(std::make_shared<rs2::disparity_transform>(false), _parameters, _logger));
-    _align_depth_filter = std::make_shared<NamedFilter>(std::make_shared<rs2::align>(RS2_STREAM_COLOR), _parameters, _logger);
+
+    /* 
+    update_align_depth_func is being used in the align depth filter for triggiring the thread that monitors profile
+    changes (_monitoring_pc) on every disable/enable of the align depth filter. This filter enablement/disablement affects
+    several topics creation/destruction, therefore, refreshing the topics is required similarly to what is done when turning on/off a sensor.
+    See BaseRealSenseNode::monitoringProfileChanges() as reference.
+    */ 
+    std::function<void(const rclcpp::Parameter&)> update_align_depth_func = [this](const rclcpp::Parameter&){
+        {
+            std::lock_guard<std::mutex> lock_guard(_profile_changes_mutex);
+            _is_align_depth_changed = true;
+        }
+        _cv_mpc.notify_one();
+    };
+    _align_depth_filter = std::make_shared<AlignDepthFilter>(std::make_shared<rs2::align>(RS2_STREAM_COLOR), update_align_depth_func, _parameters, _logger);
     _filters.push_back(_align_depth_filter);
+
     _colorizer_filter = std::make_shared<NamedFilter>(std::make_shared<rs2::colorizer>(), _parameters, _logger); 
     _filters.push_back(_colorizer_filter);
+
     _pc_filter = std::make_shared<PointcloudFilter>(std::make_shared<rs2::pointcloud>(), _node, _parameters, _logger);
     _filters.push_back(_pc_filter);
 }

realsense2_camera/src/named_filter.cpp

@@ -263,3 +263,14 @@ void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2:
             _pointcloud_publisher->publish(_msg_pointcloud);
     }
 }
+
+
+AlignDepthFilter::AlignDepthFilter(std::shared_ptr<rs2::filter> filter, 
+    std::function<void(const rclcpp::Parameter&)> update_align_depth_func,
+    std::shared_ptr<Parameters> parameters, rclcpp::Logger logger, bool is_enabled):
+    NamedFilter(filter, parameters, logger, is_enabled, false)
+{
+    _params.registerDynamicOptions(*(_filter.get()), "align_depth");
+    _params.getParameters()->setParamT("align_depth.enable", _is_enabled, update_align_depth_func);
+    _parameters_names.push_back("align_depth.enable");
+}

realsense2_camera/src/rs_node_setup.cpp

@@ -32,11 +32,10 @@ void BaseRealSenseNode::monitoringProfileChanges()
 {
     int time_interval(10000);
     std::function<void()> func = [this, time_interval](){
-        std::mutex mu;
-        std::unique_lock<std::mutex> lock(mu);
+        std::unique_lock<std::mutex> lock(_profile_changes_mutex);
         while(_is_running) {
-            _cv_mpc.wait_for(lock, std::chrono::milliseconds(time_interval), [&]{return (!_is_running || _is_profile_changed);});
-            if (_is_running && _is_profile_changed)
+            _cv_mpc.wait_for(lock, std::chrono::milliseconds(time_interval), [&]{return (!_is_running || _is_profile_changed || _is_align_depth_changed);});
+            if (_is_running && (_is_profile_changed || _is_align_depth_changed))
             {
                 ROS_DEBUG("Profile has changed");
                 try
@@ -48,6 +47,7 @@ void BaseRealSenseNode::monitoringProfileChanges()
                     ROS_ERROR_STREAM("Error updating the sensors: " << e.what());
                 }
                 _is_profile_changed = false;
+                _is_align_depth_changed = false;
             }
         }
     };
@@ -114,7 +114,13 @@ void BaseRealSenseNode::setAvailableSensors()
 
     std::function<void(rs2::frame)> multiple_message_callback_function = [this](rs2::frame frame){multiple_message_callback(frame, _imu_sync_method);};
 
-    std::function<void()> update_sensor_func = [this](){_is_profile_changed = true; _cv_mpc.notify_one();};
+    std::function<void()> update_sensor_func = [this](){
+        {
+            std::lock_guard<std::mutex> lock_guard(_profile_changes_mutex);
+            _is_profile_changed = true;
+        }
+        _cv_mpc.notify_one();
+    };
 
     std::function<void()> hardware_reset_func = [this](){hardwareResetRequest();};
 
@@ -219,7 +225,7 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& profi
             _info_publisher[sip] = _node.create_publisher<sensor_msgs::msg::CameraInfo>(camera_info.str(), 
                                     rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos));
 
-            if ((sip != DEPTH) && sip.second < 2)
+            if (_align_depth_filter->is_enabled() && (sip != DEPTH) && sip.second < 2)
             {
                 std::stringstream aligned_image_raw, aligned_camera_info;
                 aligned_image_raw << "aligned_depth_to_" << stream_name << "/image_raw";
@@ -289,8 +295,8 @@ void BaseRealSenseNode::updateSensors()
             // if active_profiles != wanted_profiles: stop sensor.
             std::vector<stream_profile> wanted_profiles;
 
-            bool is_changed(sensor->getUpdatedProfiles(wanted_profiles));
-            if (is_changed)
+            bool is_profile_changed(sensor->getUpdatedProfiles(wanted_profiles));
+            if (is_profile_changed || _is_align_depth_changed)
             {
                 std::vector<stream_profile> active_profiles = sensor->get_active_streams();
                 sensor->stop();