diff --git a/depthai-ros/CHANGELOG.rst b/depthai-ros/CHANGELOG.rst index ff2db0ce..d9bc91f8 100644 --- a/depthai-ros/CHANGELOG.rst +++ b/depthai-ros/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package depthai-ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.5 (2025-01-09) +------------------- +* Fix low bandwidth issues +* New stereo filters +* Diagnostics update +* Fix IR calculation + +2.10.4 (2024-11-07) +------------------- +* Fix rectified topic names +* Fix pointcloud launch +* Add sensor parameters for max autoexposure, sharpness, luma and chroma denoise + 2.10.3 (2024-10-14) ------------------- * Allow setting USB speed without specifying device information diff --git a/depthai-ros/CMakeLists.txt b/depthai-ros/CMakeLists.txt index 5e018685..4c61f29c 100644 --- a/depthai-ros/CMakeLists.txt +++ b/depthai-ros/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required (VERSION 3.10.2) # CMake version in Ubuntu 18.04 LTS -project(depthai-ros VERSION 2.10.3 LANGUAGES CXX C) +project(depthai-ros VERSION 2.10.5 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) diff --git a/depthai-ros/package.xml b/depthai-ros/package.xml index 52c0aa59..81ae8e8e 100644 --- a/depthai-ros/package.xml +++ b/depthai-ros/package.xml @@ -1,7 +1,7 @@ depthai-ros - 2.10.3 + 2.10.5 The depthai-ros package Adam Serafin diff --git a/depthai_bridge/CMakeLists.txt b/depthai_bridge/CMakeLists.txt index 6a37ae5a..bd36c4c5 100644 --- a/depthai_bridge/CMakeLists.txt +++ b/depthai_bridge/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required (VERSION 3.10.2) # CMake version in Ubuntu 18.04 LTS set (CMAKE_POSITION_INDEPENDENT_CODE ON) -project(depthai_bridge VERSION 2.10.3 LANGUAGES CXX C) +project(depthai_bridge VERSION 2.10.5 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) @@ -12,6 +12,9 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON) if(POLICY CMP0057) cmake_policy(SET CMP0057 NEW) endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Wno-deprecated-declarations -Wno-maybe-uninitialized -Wno-reorder -Wno-unused-parameter) +endif() set(opencv_version 4) find_package(OpenCV ${opencv_version} QUIET COMPONENTS imgproc highgui calib3d) diff --git a/depthai_bridge/package.xml b/depthai_bridge/package.xml index 9785c452..818cd8e0 100644 --- a/depthai_bridge/package.xml +++ b/depthai_bridge/package.xml @@ -1,7 +1,7 @@ depthai_bridge - 2.10.3 + 2.10.5 The depthai_bridge package Adam Serafin diff --git a/depthai_bridge/src/TFPublisher.cpp b/depthai_bridge/src/TFPublisher.cpp index f1a11335..ca4ab9cf 100644 --- a/depthai_bridge/src/TFPublisher.cpp +++ b/depthai_bridge/src/TFPublisher.cpp @@ -246,7 +246,7 @@ std::string TFPublisher::getURDF() { ROS_DEBUG("Xacro command: %s", cmd.c_str()); std::array buffer; std::string result; - std::unique_ptr pipe(popen(cmd.c_str(), "r"), pclose); + std::unique_ptr pipe(popen(cmd.c_str(), "r"), pclose); if(!pipe) { throw std::runtime_error("popen() failed!"); } diff --git a/depthai_descriptions/CMakeLists.txt b/depthai_descriptions/CMakeLists.txt index da509eb0..109f1f17 100644 --- a/depthai_descriptions/CMakeLists.txt +++ b/depthai_descriptions/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.10.2) -project(depthai_descriptions VERSION 2.10.3 LANGUAGES CXX C) +project(depthai_descriptions VERSION 2.10.5 LANGUAGES CXX C) find_package(catkin REQUIRED diff --git a/depthai_descriptions/package.xml b/depthai_descriptions/package.xml index 6b0cca7a..17cfc046 100644 --- a/depthai_descriptions/package.xml +++ b/depthai_descriptions/package.xml @@ -1,7 +1,7 @@ depthai_descriptions - 2.10.3 + 2.10.5 The depthai_descriptions package Adam Serafin diff --git a/depthai_examples/CMakeLists.txt b/depthai_examples/CMakeLists.txt index f19740a5..280a088b 100644 --- a/depthai_examples/CMakeLists.txt +++ b/depthai_examples/CMakeLists.txt @@ -1,15 +1,17 @@ cmake_minimum_required (VERSION 3.10.2) # CMake version in Ubuntu 18.04 LTS -project(depthai_examples VERSION 2.10.3 LANGUAGES CXX C) +project(depthai_examples VERSION 2.10.5 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) -add_compile_options(-g) ## is used, also find other catkin packages if(POLICY CMP0057) cmake_policy(SET CMP0057 NEW) endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Wno-deprecated-declarations -Wno-maybe-uninitialized) +endif() set(_opencv_version 4) find_package(OpenCV 4 QUIET COMPONENTS imgproc highgui) diff --git a/depthai_examples/package.xml b/depthai_examples/package.xml index 657b2faa..7bd7ac7b 100644 --- a/depthai_examples/package.xml +++ b/depthai_examples/package.xml @@ -1,7 +1,7 @@ depthai_examples - 2.10.3 + 2.10.5 The depthai_examples package Adam Serafin diff --git a/depthai_examples/src/rgb_stereo_node.cpp b/depthai_examples/src/rgb_stereo_node.cpp index 6930474f..d5c0d6d4 100644 --- a/depthai_examples/src/rgb_stereo_node.cpp +++ b/depthai_examples/src/rgb_stereo_node.cpp @@ -118,7 +118,6 @@ int main(int argc, char** argv) { auto stereoQueue = device.getOutputQueue("depth", 30, false); auto previewQueue = device.getOutputQueue("video", 30, false); - bool latched_cam_info = true; std::string stereo_uri = camera_param_uri + "/" + "right.yaml"; std::string color_uri = camera_param_uri + "/" + "color.yaml"; diff --git a/depthai_filters/CMakeLists.txt b/depthai_filters/CMakeLists.txt index fb3de7cc..741045ad 100644 --- a/depthai_filters/CMakeLists.txt +++ b/depthai_filters/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.10.2) -project(depthai_filters VERSION 2.10.3 LANGUAGES CXX C) +project(depthai_filters VERSION 2.10.5 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) diff --git a/depthai_filters/package.xml b/depthai_filters/package.xml index fbd23329..01c63c8e 100644 --- a/depthai_filters/package.xml +++ b/depthai_filters/package.xml @@ -1,7 +1,7 @@ depthai_filters - 2.10.3 + 2.10.5 The depthai_filters package Adam Serafin diff --git a/depthai_ros_driver/CMakeLists.txt b/depthai_ros_driver/CMakeLists.txt index 1aad54a7..03b677b1 100644 --- a/depthai_ros_driver/CMakeLists.txt +++ b/depthai_ros_driver/CMakeLists.txt @@ -1,18 +1,17 @@ cmake_minimum_required(VERSION 3.16.3) -project(depthai_ros_driver VERSION 2.10.3) +project(depthai_ros_driver VERSION 2.10.5) if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif() if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 11) + set(CMAKE_CXX_STANDARD 14) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wno-deprecated-declarations -Wno-maybe-uninitialized -Wno-reorder -Wno-unused-parameter) endif() set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) -add_compile_options(-g) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(CMAKE_POSITION_INDEPENDENT_CODE ON) set(BUILD_SHARED_LIBS ON) diff --git a/depthai_ros_driver/include/depthai_ros_driver/camera.hpp b/depthai_ros_driver/include/depthai_ros_driver/camera.hpp index b58f054b..32316ee4 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/camera.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/camera.hpp @@ -19,6 +19,10 @@ namespace depthai_ros_driver { using Trigger = std_srvs::Trigger; class Camera : public nodelet::Nodelet { public: + /** + * @brief Destructor of the class Camera. Stops the device and destroys the pipeline. + */ + ~Camera(); void onInit() override; /** * @brief Creates the pipeline and starts the device. Also sets up parameter callback and services. @@ -92,4 +96,4 @@ class Camera : public nodelet::Nodelet { double laserDotBrightness; std::unique_ptr tfPub; }; -} // namespace depthai_ros_driver \ No newline at end of file +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp index 0e1c838c..637c4484 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp @@ -53,6 +53,7 @@ extern const std::unordered_map fSyncModeMap; extern const std::unordered_map cameraImageOrientationMap; bool rsCompabilityMode(ros::NodeHandle node); +std::string tfPrefix(ros::NodeHandle node); std::string getSocketName(ros::NodeHandle node, dai::CameraBoardSocket socket); std::string getNodeName(ros::NodeHandle node, NodeNameEnum name); void basicCameraPub(const std::string& /*name*/, diff --git a/depthai_ros_driver/include/depthai_ros_driver/utils.hpp b/depthai_ros_driver/include/depthai_ros_driver/utils.hpp index 0dbcfa00..168e94c6 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/utils.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/utils.hpp @@ -47,6 +47,7 @@ struct ImgConverterConfig { bool getBaseDeviceTimestamp = false; bool updateROSBaseTimeOnRosMsg = false; bool lowBandwidth = false; + bool isStereo = false; dai::RawImgFrame::Type encoding = dai::RawImgFrame::Type::BGR888i; bool addExposureOffset = false; dai::CameraExposureOffset expOffset = dai::CameraExposureOffset::START; diff --git a/depthai_ros_driver/package.xml b/depthai_ros_driver/package.xml index 7b121871..ee4035c2 100644 --- a/depthai_ros_driver/package.xml +++ b/depthai_ros_driver/package.xml @@ -2,7 +2,7 @@ depthai_ros_driver - 2.10.3 + 2.10.5 Depthai ROS Monolithic node. Adam Serafin Adam Serafin diff --git a/depthai_ros_driver/src/camera.cpp b/depthai_ros_driver/src/camera.cpp index d61f802a..d87e19b8 100644 --- a/depthai_ros_driver/src/camera.cpp +++ b/depthai_ros_driver/src/camera.cpp @@ -1,5 +1,7 @@ #include "depthai_ros_driver/camera.hpp" +#include + #include #include @@ -52,6 +54,10 @@ void Camera::onInit() { diagSub = pNH.subscribe("/diagnostics", 1, &Camera::diagCB, this); } +Camera::~Camera() { + stop(); +} + void Camera::diagCB(const diagnostic_msgs::DiagnosticArray::ConstPtr& msg) { for(const auto& status : msg->status) { std::string nodeletName = pNH.getNamespace() + "_nodelet_manager"; @@ -241,7 +247,7 @@ void Camera::startDevice() { } } else if(!ip.empty() && info.name == ip) { ROS_INFO("Connecting to the camera using ip: %s", ip.c_str()); - if(info.state == X_LINK_UNBOOTED || info.state == X_LINK_BOOTLOADER) { + if(info.state == X_LINK_UNBOOTED || info.state == X_LINK_BOOTLOADER || info.state == X_LINK_ANY_STATE) { device = std::make_shared(info); camRunning = true; } else if(info.state == X_LINK_BOOTED) { diff --git a/depthai_ros_driver/src/dai_nodes/base_node.cpp b/depthai_ros_driver/src/dai_nodes/base_node.cpp index 72608be7..b42797ad 100644 --- a/depthai_ros_driver/src/dai_nodes/base_node.cpp +++ b/depthai_ros_driver/src/dai_nodes/base_node.cpp @@ -32,7 +32,7 @@ std::string BaseNode::getSocketName(dai::CameraBoardSocket socket) { return sensor_helpers::getSocketName(getROSNode(), socket); } std::string BaseNode::getTFPrefix(const std::string& frameName) { - auto prefix = std::string(getROSNode().getNamespace()) + "_" + frameName; + auto prefix = sensor_helpers::tfPrefix(getROSNode()) + "_" + frameName; prefix.erase(0, 1); return prefix; } @@ -48,14 +48,14 @@ dai::Node::Input BaseNode::getInput(int /*linkType*/) { } dai::Node::Input BaseNode::getInputByName(const std::string& /*name*/) { throw(std::runtime_error("getInputByName() not implemented")); -}; +} void BaseNode::closeQueues() { throw(std::runtime_error("closeQueues() not implemented")); } std::shared_ptr BaseNode::setupXout(std::shared_ptr pipeline, const std::string& name) { return utils::setupXout(pipeline, name); -}; +} std::shared_ptr BaseNode::setupOutput(std::shared_ptr pipeline, const std::string& qName, @@ -63,7 +63,7 @@ std::shared_ptr BaseNode::setupOutput(std::share bool isSynced, const utils::VideoEncoderConfig& encoderConfig) { return std::make_shared(getROSNode(), pipeline, qName, nodeLink, isSynced, encoderConfig); -}; +} void BaseNode::setNames() { throw(std::runtime_error("setNames() not implemented")); @@ -82,7 +82,7 @@ void BaseNode::link(dai::Node::Input /*in*/, int /*linkType = 0*/) { } std::vector> BaseNode::getPublishers() { return std::vector>(); -}; +} void BaseNode::updateParams(parametersConfig& /*config*/) { return; diff --git a/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp b/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp index 811aebdf..f784be82 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp @@ -21,7 +21,7 @@ ImagePublisher::ImagePublisher(ros::NodeHandle node, std::function linkFunc, bool synced, const utils::VideoEncoderConfig& encoderConfig) - : node(node), encConfig(encoderConfig), qName(qName), synced(synced), it(node) { + : node(node), it(node), encConfig(encoderConfig), qName(qName), synced(synced) { if(!synced) { xout = utils::setupXout(pipeline, qName); } @@ -89,7 +89,7 @@ void ImagePublisher::createImageConverter(std::shared_ptr device) { if(convConfig.alphaScalingEnabled) { converter->setAlphaScaling(convConfig.alphaScaling); } - if(convConfig.outputDisparity) { + if(convConfig.isStereo && !convConfig.outputDisparity) { auto calHandler = device->readCalibration(); double baseline = calHandler.getBaselineDistance(pubConfig.leftSocket, pubConfig.rightSocket, false); if(convConfig.reverseSocketOrder) { @@ -105,8 +105,10 @@ std::shared_ptr ImagePublisher::createEncoder(std::shar auto enc = pipeline->create(); enc->setQuality(encoderConfig.quality); enc->setProfile(encoderConfig.profile); - enc->setBitrate(encoderConfig.bitrate); - enc->setKeyframeFrequency(encoderConfig.frameFreq); + if(encoderConfig.profile != dai::VideoEncoderProperties::Profile::MJPEG) { + enc->setBitrate(encoderConfig.bitrate); + enc->setKeyframeFrequency(encoderConfig.frameFreq); + } return enc; } void ImagePublisher::createInfoManager(std::shared_ptr device) { @@ -122,10 +124,10 @@ void ImagePublisher::createInfoManager(std::shared_ptr device) { } else { infoManager->loadCameraInfo(pubConfig.calibrationFile); } -}; +} ImagePublisher::~ImagePublisher() { closeQueue(); -}; +} void ImagePublisher::closeQueue() { if(dataQ) dataQ->close(); diff --git a/depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp b/depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp index 4c26755c..cae50f19 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp @@ -95,6 +95,15 @@ bool rsCompabilityMode(ros::NodeHandle node) { node.getParam("camera_i_rs_compat", compat); return compat; } +std::string tfPrefix(ros::NodeHandle node) { + bool pubTF = false; + std::string tfPrefix = node.getNamespace(); + node.getParam("camera_i_publish_tf_from_calibration", pubTF); + if(pubTF) { + node.getParam("camera_i_tf_base_frame", tfPrefix); + } + return tfPrefix; +} std::string getNodeName(ros::NodeHandle node, NodeNameEnum name) { if(rsCompabilityMode(node)) { return rsNodeNameMap.at(name); diff --git a/depthai_ros_driver/src/dai_nodes/sensors/stereo.cpp b/depthai_ros_driver/src/dai_nodes/sensors/stereo.cpp index 2a9f2e0c..cac6c2e5 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/stereo.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/stereo.cpp @@ -75,8 +75,9 @@ void Stereo::setNames() { void Stereo::setXinXout(std::shared_ptr pipeline) { bool outputDisparity = ph->getParam("i_output_disparity"); + bool lowBandwidth = ph->getParam("i_low_bandwidth"); std::function stereoLinkChoice; - if(outputDisparity) { + if(outputDisparity || lowBandwidth) { stereoLinkChoice = [&](auto input) { stereoCamNode->disparity.link(input); }; } else { stereoLinkChoice = [&](auto input) { stereoCamNode->depth.link(input); }; @@ -87,7 +88,7 @@ void Stereo::setXinXout(std::shared_ptr pipeline) { encConf.bitrate = ph->getParam("i_low_bandwidth_bitrate"); encConf.frameFreq = ph->getParam("i_low_bandwidth_frame_freq"); encConf.quality = ph->getParam("i_low_bandwidth_quality"); - encConf.enabled = ph->getParam("i_low_bandwidth"); + encConf.enabled = lowBandwidth; stereoPub = setupOutput(pipeline, stereoQName, stereoLinkChoice, ph->getParam("i_synced"), encConf); } @@ -188,6 +189,7 @@ void Stereo::setupStereoQueue(std::shared_ptr device) { convConfig.alphaScaling = ph->getParam("i_alpha_scaling"); } convConfig.outputDisparity = ph->getParam("i_output_disparity"); + convConfig.isStereo = true; utils::ImgPublisherConfig pubConf; pubConf.daiNodeName = getName(); diff --git a/depthai_ros_driver/src/dai_nodes/sys_logger.cpp b/depthai_ros_driver/src/dai_nodes/sys_logger.cpp index 3a4a1bdd..6bb15adc 100644 --- a/depthai_ros_driver/src/dai_nodes/sys_logger.cpp +++ b/depthai_ros_driver/src/dai_nodes/sys_logger.cpp @@ -72,7 +72,22 @@ void SysLogger::produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& auto logData = loggerQ->get(std::chrono::seconds(5), timeout); if(!timeout) { stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "System Information"); - stat.add("System Information", sysInfoToString(*logData)); + auto sysInfo = *logData; + stat.add("Leon CSS CPU Usage", sysInfo.leonCssCpuUsage.average * 100); + stat.add("Leon MSS CPU Usage", sysInfo.leonMssCpuUsage.average * 100); + stat.add("Ddr Memory Usage", sysInfo.ddrMemoryUsage.used / (1024.0f * 1024.0f)); + stat.add("Ddr Memory Total", sysInfo.ddrMemoryUsage.total / (1024.0f * 1024.0f)); + stat.add("Cmx Memory Usage", sysInfo.cmxMemoryUsage.used / (1024.0f * 1024.0f)); + stat.add("Cmx Memory Total", sysInfo.cmxMemoryUsage.total); + stat.add("Leon CSS Memory Usage", sysInfo.leonCssMemoryUsage.used / (1024.0f * 1024.0f)); + stat.add("Leon CSS Memory Total", sysInfo.leonCssMemoryUsage.total / (1024.0f * 1024.0f)); + stat.add("Leon MSS Memory Usage", sysInfo.leonMssMemoryUsage.used / (1024.0f * 1024.0f)); + stat.add("Leon MSS Memory Total", sysInfo.leonMssMemoryUsage.total / (1024.0f * 1024.0f)); + stat.add("Average Chip Temperature", sysInfo.chipTemperature.average); + stat.add("Leon CSS Chip Temperature", sysInfo.chipTemperature.css); + stat.add("Leon MSS Chip Temperature", sysInfo.chipTemperature.mss); + stat.add("UPA Chip Temperature", sysInfo.chipTemperature.upa); + stat.add("DSS Chip Temperature", sysInfo.chipTemperature.dss); } else { stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "No Data"); } diff --git a/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp b/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp index 4b950402..a96f3029 100644 --- a/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp @@ -75,19 +75,19 @@ void SensorParamHandler::declareParams(std::shared_ptr mo } monoCam->setImageOrientation( utils::getValFromMap(declareAndLogParam("i_sensor_img_orientation", "AUTO"), dai_nodes::sensor_helpers::cameraImageOrientationMap)); - int expLimit = declareAndLogParam("r_auto_exposure_limit", 1000); + int expLimit = declareAndLogParam("r_auto_exposure_limit", 1000); if(declareAndLogParam("r_set_auto_exposure_limit", false)) { monoCam->initialControl.setAutoExposureLimit(expLimit); } - int sharpness = declareAndLogParam("r_sharpness", 1); + int sharpness = declareAndLogParam("r_sharpness", 1); if(declareAndLogParam("r_set_sharpness", false)) { monoCam->initialControl.setSharpness(sharpness); } - int chromaDenoise = declareAndLogParam("r_chroma_denoise", 1); + int chromaDenoise = declareAndLogParam("r_chroma_denoise", 1); if(declareAndLogParam("r_set_chroma_denoise", false)) { monoCam->initialControl.setChromaDenoise(chromaDenoise); } - int lumaDenoise = declareAndLogParam("r_luma_denoise", 1); + int lumaDenoise = declareAndLogParam("r_luma_denoise", 1); if(declareAndLogParam("r_set_luma_denoise", false)) { monoCam->initialControl.setLumaDenoise(lumaDenoise); } @@ -182,19 +182,19 @@ void SensorParamHandler::declareParams(std::shared_ptr c } colorCam->setImageOrientation( utils::getValFromMap(declareAndLogParam("i_sensor_img_orientation", "AUTO"), dai_nodes::sensor_helpers::cameraImageOrientationMap)); - int expLimit = declareAndLogParam("r_auto_exposure_limit", 1000); + int expLimit = declareAndLogParam("r_auto_exposure_limit", 1000); if(declareAndLogParam("r_set_auto_exposure_limit", false)) { colorCam->initialControl.setAutoExposureLimit(expLimit); } - int sharpness = declareAndLogParam("r_sharpness", 1); + int sharpness = declareAndLogParam("r_sharpness", 1); if(declareAndLogParam("r_set_sharpness", false)) { colorCam->initialControl.setSharpness(sharpness); } - int chromaDenoise = declareAndLogParam("r_chroma_denoise", 1); + int chromaDenoise = declareAndLogParam("r_chroma_denoise", 1); if(declareAndLogParam("r_set_chroma_denoise", false)) { colorCam->initialControl.setChromaDenoise(chromaDenoise); } - int lumaDenoise = declareAndLogParam("r_luma_denoise", 1); + int lumaDenoise = declareAndLogParam("r_luma_denoise", 1); if(declareAndLogParam("r_set_luma_denoise", false)) { colorCam->initialControl.setLumaDenoise(lumaDenoise); } diff --git a/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp b/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp index e6bed223..2bb7b4cd 100644 --- a/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp @@ -8,10 +8,12 @@ namespace depthai_ros_driver { namespace param_handlers { StereoParamHandler::StereoParamHandler(ros::NodeHandle node, const std::string& name) : BaseParamHandler(node, name) { - depthPresetMap = { - {"HIGH_ACCURACY", dai::node::StereoDepth::PresetMode::HIGH_ACCURACY}, - {"HIGH_DENSITY", dai::node::StereoDepth::PresetMode::HIGH_DENSITY}, - }; + depthPresetMap = {{"HIGH_ACCURACY", dai::node::StereoDepth::PresetMode::HIGH_ACCURACY}, + {"HIGH_DENSITY", dai::node::StereoDepth::PresetMode::HIGH_DENSITY}, + {"DEFAULT", dai::node::StereoDepth::PresetMode::DEFAULT}, + {"FACE", dai::node::StereoDepth::PresetMode::FACE}, + {"HIGH_DETAIL", dai::node::StereoDepth::PresetMode::HIGH_DETAIL}, + {"ROBOTICS", dai::node::StereoDepth::PresetMode::ROBOTICS}}; disparityWidthMap = { {"DISPARITY_64", dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_64}, {"DISPARITY_96", dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_96}, @@ -50,7 +52,7 @@ void StereoParamHandler::updateSocketsFromParams(dai::CameraBoardSocket& left, d StereoParamHandler::~StereoParamHandler() = default; void StereoParamHandler::declareParams(std::shared_ptr stereo) { declareAndLogParam("i_max_q_size", 30); - declareAndLogParam("i_low_bandwidth", false); + bool lowBandwidth = declareAndLogParam("i_low_bandwidth", false); declareAndLogParam("i_low_bandwidth_quality", 50); declareAndLogParam("i_low_bandwidth_profile", 4); declareAndLogParam("i_low_bandwidth_frame_freq", 30); @@ -128,34 +130,38 @@ void StereoParamHandler::declareParams(std::shared_ptr s stereo->initialConfig.setLeftRightCheckThreshold(declareAndLogParam("i_lrc_threshold", 10)); stereo->initialConfig.setMedianFilter(static_cast(declareAndLogParam("i_depth_filter_size", 5))); stereo->initialConfig.setConfidenceThreshold(declareAndLogParam("i_stereo_conf_threshold", 240)); - if(declareAndLogParam("i_subpixel", true)) { + if(declareAndLogParam("i_subpixel", true) && !lowBandwidth) { stereo->initialConfig.setSubpixel(true); stereo->initialConfig.setSubpixelFractionalBits(declareAndLogParam("i_subpixel_fractional_bits", 3)); + } else { + stereo->initialConfig.setSubpixel(false); + ROS_INFO("Subpixel disabled due to low bandwidth mode"); } stereo->setRectifyEdgeFillColor(declareAndLogParam("i_rectify_edge_fill_color", 0)); if(declareAndLogParam("i_enable_alpha_scaling", false)) { stereo->setAlphaScaling(declareAndLogParam("i_alpha_scaling", 0.0)); } - auto config = stereo->initialConfig.get(); + dai::RawStereoDepthConfig config = stereo->initialConfig.get(); config.costMatching.disparityWidth = utils::getValFromMap(declareAndLogParam("i_disparity_width", "DISPARITY_96"), disparityWidthMap); stereo->setExtendedDisparity(declareAndLogParam("i_extended_disp", false)); config.costMatching.enableCompanding = declareAndLogParam("i_enable_companding", false); - config.postProcessing.temporalFilter.enable = declareAndLogParam("i_enable_temporal_filter", false); - if(config.postProcessing.temporalFilter.enable) { + if(declareAndLogParam("i_enable_temporal_filter", false)) { + config.postProcessing.temporalFilter.enable = true; config.postProcessing.temporalFilter.alpha = declareAndLogParam("i_temporal_filter_alpha", 0.4); config.postProcessing.temporalFilter.delta = declareAndLogParam("i_temporal_filter_delta", 20); config.postProcessing.temporalFilter.persistencyMode = utils::getValFromMap(declareAndLogParam("i_temporal_filter_persistency", "VALID_2_IN_LAST_4"), temporalPersistencyMap); } - config.postProcessing.speckleFilter.enable = declareAndLogParam("i_enable_speckle_filter", false); - if(config.postProcessing.speckleFilter.enable) { + if(declareAndLogParam("i_enable_speckle_filter", false)) { + config.postProcessing.speckleFilter.enable = true; config.postProcessing.speckleFilter.speckleRange = declareAndLogParam("i_speckle_filter_speckle_range", 50); } if(declareAndLogParam("i_enable_disparity_shift", false)) { config.algorithmControl.disparityShift = declareAndLogParam("i_disparity_shift", 0); } - config.postProcessing.spatialFilter.enable = declareAndLogParam("i_enable_spatial_filter", false); - if(config.postProcessing.spatialFilter.enable) { + + if(declareAndLogParam("i_enable_spatial_filter", false)) { + config.postProcessing.spatialFilter.enable = true; config.postProcessing.spatialFilter.holeFillingRadius = declareAndLogParam("i_spatial_filter_hole_filling_radius", 2); config.postProcessing.spatialFilter.alpha = declareAndLogParam("i_spatial_filter_alpha", 0.5); config.postProcessing.spatialFilter.delta = declareAndLogParam("i_spatial_filter_delta", 20); diff --git a/depthai_ros_driver/src/utils.cpp b/depthai_ros_driver/src/utils.cpp index e8a151b3..21ec69f3 100644 --- a/depthai_ros_driver/src/utils.cpp +++ b/depthai_ros_driver/src/utils.cpp @@ -17,6 +17,6 @@ std::shared_ptr setupXout(std::shared_ptr pi xout->input.setWaitForMessage(false); xout->input.setQueueSize(1); return xout; -}; +} } // namespace utils } // namespace depthai_ros_driver diff --git a/depthai_ros_msgs/CMakeLists.txt b/depthai_ros_msgs/CMakeLists.txt index 0aa2b1e5..11824be9 100644 --- a/depthai_ros_msgs/CMakeLists.txt +++ b/depthai_ros_msgs/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required (VERSION 3.10.2) # CMake version in Ubuntu 18.04 LTS -project (depthai_ros_msgs VERSION 2.10.3) +project (depthai_ros_msgs VERSION 2.10.5) if(POLICY CMP0057) cmake_policy(SET CMP0057 NEW) diff --git a/depthai_ros_msgs/package.xml b/depthai_ros_msgs/package.xml index ca79bf31..5e38110d 100644 --- a/depthai_ros_msgs/package.xml +++ b/depthai_ros_msgs/package.xml @@ -1,7 +1,7 @@ depthai_ros_msgs - 2.10.3 + 2.10.5 Package to keep interface independent of the driver Adam Serafin