diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index eecad960..4d042ec2 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -295,6 +295,11 @@ class OBCameraNode { std::shared_ptr& response); void setSYNCHostimeCallback(const std::shared_ptr& request, std::shared_ptr& response); + + void setDecimationFilterEnableCallback( + const std::shared_ptr& request, + std::shared_ptr& response); + bool toggleSensor(const stream_index_pair& stream_index, bool enabled, std::string& msg); void saveImageCallback(const std::shared_ptr& request, @@ -448,6 +453,7 @@ class OBCameraNode { rclcpp::Service::SharedPtr set_reset_timestamp_srv_; rclcpp::Service::SharedPtr set_interleaver_laser_sync_srv_; rclcpp::Service::SharedPtr set_sync_host_time_srv_; + rclcpp::Service::SharedPtr set_decimation_filter_enable_srv_; bool enable_sync_output_accel_gyro_ = false; bool publish_tf_ = false; diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp index b8163066..95a99b7b 100644 --- a/orbbec_camera/src/ros_service.cpp +++ b/orbbec_camera/src/ros_service.cpp @@ -184,6 +184,11 @@ void OBCameraNode::setupCameraCtrlServices() { std::shared_ptr response) { setSYNCHostimeCallback(request, response); }); + set_decimation_filter_enable_srv_ = node_->create_service( + "set_decimation_filter_enable", [this](const std::shared_ptr request, + std::shared_ptr response) { + setDecimationFilterEnableCallback(request, response); + }); } void OBCameraNode::setExposureCallback(const std::shared_ptr& request, @@ -836,4 +841,33 @@ void OBCameraNode::setSYNCHostimeCallback( response->success = false; } } +void OBCameraNode::setDecimationFilterEnableCallback( + const std::shared_ptr& request, + std::shared_ptr& response) { + try { + enable_decimation_filter_ = request->data; + if (enable_decimation_filter_) { + decimation_filter_scale_ = node_->get_parameter("decimation_filter_scale").as_int(); + if (decimation_filter_scale_ == -1) { + RCLCPP_WARN(logger_, "Please configure the parameter 'decimation_filter_scale'"); + return; + } + } else { + decimation_filter_scale_ = -1; + } + setupDepthPostProcessFilter(); + node_->set_parameter(rclcpp::Parameter("enable_decimation_filter", enable_decimation_filter_)); + node_->set_parameter(rclcpp::Parameter("decimation_filter_scale", decimation_filter_scale_)); + response->success = true; + } catch (const ob::Error& e) { + response->message = e.getMessage(); + response->success = false; + } catch (const std::exception& e) { + response->message = e.what(); + response->success = false; + } catch (...) { + response->message = "unknown error"; + response->success = false; + } +} } // namespace orbbec_camera