diff --git a/orbbec_camera/include/orbbec_camera/utils.h b/orbbec_camera/include/orbbec_camera/utils.h index 1f6e6c89..b74ef056 100644 --- a/orbbec_camera/include/orbbec_camera/utils.h +++ b/orbbec_camera/include/orbbec_camera/utils.h @@ -188,6 +188,8 @@ std::string ObDeviceTypeToString(const OBDeviceType& type); rmw_qos_profile_t getRMWQosProfileFromString(const std::string& str_qos); +std::string getRMWQosProfileDescription(const rmw_qos_profile_t& qos_profile); + bool isOpenNIDevice(int pid); OB_DEPTH_PRECISION_LEVEL depthPrecisionLevelFromString( diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 410000a2..97c0dc20 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -5446,14 +5446,8 @@ void OBCameraNode::setupImagePublisher(const stream_index_pair &stream_index) { image_publishers_[stream_index] = std::make_shared(*node_, topic, image_qos_profile); } - std::string depth; - if (image_qos_profile.history == RMW_QOS_POLICY_HISTORY_KEEP_LAST) { - depth = ", depth=" + std::to_string(image_qos_profile.depth); - } - RCLCPP_INFO_STREAM( - logger_, topic << " QoS: reliability=" << magic_enum::enum_name(image_qos_profile.reliability) - << ", durability=" << magic_enum::enum_name(image_qos_profile.durability) - << ", history=" << magic_enum::enum_name(image_qos_profile.history) << depth); + RCLCPP_INFO_STREAM(logger_, + topic << " QoS: " << getRMWQosProfileDescription(image_qos_profile)); if (is_mjpg_color_stream) { compressed_image_publishers_[stream_index] = diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp index 0a73d234..0b95972f 100644 --- a/orbbec_camera/src/ros_service.cpp +++ b/orbbec_camera/src/ros_service.cpp @@ -478,7 +478,7 @@ void OBCameraNode::getColorQueueStatsCallback( }; }; - nlohmann::json queues; + nlohmann::json queues = nlohmann::json::object(); uint64_t overflow_count = 0; const bool reset = request->data; if (enable_stream_[COLOR] || reset) { diff --git a/orbbec_camera/src/utils.cpp b/orbbec_camera/src/utils.cpp index 4f4aef43..819c1f84 100644 --- a/orbbec_camera/src/utils.cpp +++ b/orbbec_camera/src/utils.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include "orbbec_camera/utils.h" #include #include "orbbec_camera/constants.h" @@ -566,6 +567,29 @@ rmw_qos_profile_t getRMWQosProfileFromString(const std::string &str_qos) { } } +std::string getRMWQosProfileDescription(const rmw_qos_profile_t &qos_profile) { + const auto short_qos_name = [](auto policy) { + auto name = magic_enum::enum_name(policy); + constexpr size_t prefix_size = sizeof("RMW_QOS_POLICY_") - 1; + if (name.size() <= prefix_size) { + return name; + } + name.remove_prefix(prefix_size); + const auto separator = name.find('_'); + if (separator < name.size()) { + name.remove_prefix(separator + 1); + } + return name; + }; + + std::string history(short_qos_name(qos_profile.history)); + if (qos_profile.history == RMW_QOS_POLICY_HISTORY_KEEP_LAST) { + history += "(" + std::to_string(qos_profile.depth) + ")"; + } + return std::string(short_qos_name(qos_profile.reliability)) + "/" + + std::string(short_qos_name(qos_profile.durability)) + "/" + history; +} + bool isOpenNIDevice(int pid) { static const std::vector OPENNI_DEVICE_PIDS = { 0x0300, 0x0301, 0x0400, 0x0401, 0x0402, 0x0403, 0x0404, 0x0407, 0x0601, 0x060b, 0x060e,