i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-07 05:27:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat: add runtime stream profile service

Browse Source
This commit is contained in:
slz
2026-07-08 16:10:51 +08:00
parent fdc0d1fe9c
commit d59c78d750
6 changed files with 506 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+36
View File
@@ -19,6 +19,7 @@
#include <nlohmann/json.hpp>
#include <memory>
#include <optional>
#include <rclcpp/rclcpp.hpp>
#include <string>
#include <unordered_map>
@@ -63,6 +64,7 @@
#include "orbbec_camera_msgs/srv/set_string.hpp"
#include "orbbec_camera_msgs/srv/set_filter.hpp"
#include "orbbec_camera_msgs/srv/set_arrays.hpp"
#include "orbbec_camera_msgs/srv/set_stream_profile.hpp"
#include "orbbec_camera_msgs/srv/get_user_calib_params.hpp"
#include "orbbec_camera_msgs/srv/set_user_calib_params.hpp"
#include "orbbec_camera/constants.h"
@@ -122,6 +124,7 @@ using SetBool = std_srvs::srv::SetBool;
using GetBool = orbbec_camera_msgs::srv::GetBool;
using SetFilter = orbbec_camera_msgs::srv::SetFilter;
using SetArrays = orbbec_camera_msgs::srv::SetArrays;
using SetStreamProfile = orbbec_camera_msgs::srv::SetStreamProfile;
using SetUserCalibParams = orbbec_camera_msgs::srv::SetUserCalibParams;
using GetUserCalibParams = orbbec_camera_msgs::srv::GetUserCalibParams;
using DepthFilterState = orbbec_camera_msgs::msg::DepthFilterState;
@@ -238,6 +241,14 @@ class OBCameraNode {
double timestamp_ = -1; // in nanoseconds
};
struct PendingStreamProfile {
stream_index_pair stream_index;
int requested_width = 0;
int requested_height = 0;
int requested_fps = 0;
std::shared_ptr<ob::VideoStreamProfile> profile;
};
void setupDevices();
void loadConfigJson();
@@ -267,6 +278,24 @@ class OBCameraNode {
void setupProfiles();
std::shared_ptr<ob::VideoStreamProfile> selectVideoStreamProfile(
const stream_index_pair& stream_index, int width, int height, int fps, OBFormat format);
std::optional<stream_index_pair> getImageStreamByName(const std::string& stream_name) const;
bool validateStreamProfileRequest(const std::shared_ptr<SetStreamProfile::Request>& request,
std::vector<PendingStreamProfile>& pending_profiles,
std::string& message);
bool applyStreamProfiles(const std::vector<PendingStreamProfile>& pending_profiles,
std::string& message);
void clearColorFrameQueues();
void stopColorFrameThreads();
void setupImageBuffers();
void updateImageConfig(const stream_index_pair& stream_index);
void printSensorProfiles(const std::shared_ptr<ob::Sensor>& sensor);
@@ -277,6 +306,8 @@ class OBCameraNode {
void setupTopics();
void setupImagePublisher(const stream_index_pair& stream_index);
void setupPipelineConfig();
void setupDiagnosticUpdater();
@@ -475,6 +506,9 @@ class OBCameraNode {
void setAEStrategyCallback(const std::shared_ptr<SetString::Request>& request,
std::shared_ptr<SetString::Response>& response);
void setStreamProfileCallback(const std::shared_ptr<SetStreamProfile::Request>& request,
std::shared_ptr<SetStreamProfile::Response>& response);
void setUserCalibParamsCallback(const std::shared_ptr<SetUserCalibParams::Request>& request,
std::shared_ptr<SetUserCalibParams::Response>& response);
@@ -699,6 +733,7 @@ class OBCameraNode {
rclcpp::Service<SetInt32>::SharedPtr set_sync_io_voltage_level_srv_;
rclcpp::Service<orbbec_camera_msgs::srv::GetBool>::SharedPtr get_streams_enable_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_streams_enable_srv_;
rclcpp::Service<SetStreamProfile>::SharedPtr set_stream_profile_srv_;
rclcpp::Service<GetUserCalibParams>::SharedPtr get_user_calib_params_srv_;
rclcpp::Service<SetUserCalibParams>::SharedPtr set_user_calib_params_srv_;
rclcpp::Service<SetString>::SharedPtr set_ae_reference_stream_srv_;
@@ -839,6 +874,7 @@ class OBCameraNode {
// For color
std::queue<std::shared_ptr<ob::FrameSet>> color_frame_queue_;
std::shared_ptr<std::thread> colorFrameThread_ = nullptr;
std::atomic_bool stop_color_frame_threads_{false};
std::mutex color_frame_queue_lock_;
std::condition_variable color_frame_queue_cv_;