diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 5a9008c056..27024b14f8 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/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 _static_tf_broadcaster; std::shared_ptr _dynamic_tf_broadcaster; @@ -271,7 +272,8 @@ namespace realsense2_camera stream_index_pair _pointcloud_texture; PipelineSyncer _syncer; rs2::asynchronous_syncer _asyncer; - std::shared_ptr _align_depth_filter, _colorizer_filter; + std::shared_ptr _colorizer_filter; + std::shared_ptr _align_depth_filter; std::shared_ptr _pc_filter; std::vector> _filters; std::vector _dev_sensors; @@ -290,6 +292,7 @@ namespace realsense2_camera std::shared_ptr _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 _diagnostics_updater; rs2::stream_profile _base_profile; diff --git a/realsense2_camera/include/named_filter.h b/realsense2_camera/include/named_filter.h index 83a5abedc6..ba55d26545 100644 --- a/realsense2_camera/include/named_filter.h +++ b/realsense2_camera/include/named_filter.h @@ -57,4 +57,11 @@ namespace realsense2_camera rclcpp::Publisher::SharedPtr _pointcloud_publisher; std::string _pointcloud_qos; }; + + class AlignDepthFilter : public NamedFilter + { + public: + AlignDepthFilter(std::shared_ptr filter, std::function update_align_depth_func, + std::shared_ptr parameters, rclcpp::Logger logger, bool is_enabled = false); + }; } diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 2cafb18dc3..25be0240b8 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/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(std::make_shared(), _parameters, _logger)); _filters.push_back(std::make_shared(std::make_shared(), _parameters, _logger)); _filters.push_back(std::make_shared(std::make_shared(false), _parameters, _logger)); - _align_depth_filter = std::make_shared(std::make_shared(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 update_align_depth_func = [this](const rclcpp::Parameter&){ + { + std::lock_guard lock_guard(_profile_changes_mutex); + _is_align_depth_changed = true; + } + _cv_mpc.notify_one(); + }; + _align_depth_filter = std::make_shared(std::make_shared(RS2_STREAM_COLOR), update_align_depth_func, _parameters, _logger); _filters.push_back(_align_depth_filter); + _colorizer_filter = std::make_shared(std::make_shared(), _parameters, _logger); _filters.push_back(_colorizer_filter); + _pc_filter = std::make_shared(std::make_shared(), _node, _parameters, _logger); _filters.push_back(_pc_filter); } diff --git a/realsense2_camera/src/named_filter.cpp b/realsense2_camera/src/named_filter.cpp index 871c9497c3..3f0493e57a 100644 --- a/realsense2_camera/src/named_filter.cpp +++ b/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 filter, + std::function update_align_depth_func, + std::shared_ptr 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"); +} diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 8acc937832..3b3d458b56 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -32,11 +32,10 @@ void BaseRealSenseNode::monitoringProfileChanges() { int time_interval(10000); std::function func = [this, time_interval](){ - std::mutex mu; - std::unique_lock lock(mu); + std::unique_lock 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 multiple_message_callback_function = [this](rs2::frame frame){multiple_message_callback(frame, _imu_sync_method);}; - std::function update_sensor_func = [this](){_is_profile_changed = true; _cv_mpc.notify_one();}; + std::function update_sensor_func = [this](){ + { + std::lock_guard lock_guard(_profile_changes_mutex); + _is_profile_changed = true; + } + _cv_mpc.notify_one(); + }; std::function hardware_reset_func = [this](){hardwareResetRequest();}; @@ -219,7 +225,7 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi _info_publisher[sip] = _node.create_publisher(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 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 active_profiles = sensor->get_active_streams(); sensor->stop();