diff --git a/depthai-ros/CHANGELOG.rst b/depthai-ros/CHANGELOG.rst index 8e3c0c97..6edfe81d 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 3da4853a..7c913b18 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 5b9d07ee..4fcf6ffb 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 diff --git a/depthai_bridge/CMakeLists.txt b/depthai_bridge/CMakeLists.txt index caa37fad..2b577df7 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) diff --git a/depthai_bridge/package.xml b/depthai_bridge/package.xml index 7e440bc7..13bab0c1 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 @@ -14,7 +14,6 @@ libboost-dev rclcpp - ament_index_cpp cv_bridge camera_info_manager depthai diff --git a/depthai_bridge/src/ImageConverter.cpp b/depthai_bridge/src/ImageConverter.cpp index c666b398..d6fe1a91 100644 --- a/depthai_bridge/src/ImageConverter.cpp +++ b/depthai_bridge/src/ImageConverter.cpp @@ -142,55 +142,6 @@ ImageMsgs::Image ImageConverter::toRosMsgRawPtr(std::shared_ptr i return outImageMsg; } - if(fromBitstream) { - std::string encoding; - int decodeFlags; - int channels; - cv::Mat output; - switch(srcType) { - case dai::RawImgFrame::Type::BGR888i: { - encoding = sensor_msgs::image_encodings::BGR8; - decodeFlags = cv::IMREAD_COLOR; - channels = CV_8UC3; - break; - } - case dai::RawImgFrame::Type::GRAY8: { - encoding = sensor_msgs::image_encodings::MONO8; - decodeFlags = cv::IMREAD_GRAYSCALE; - channels = CV_8UC1; - break; - } - case dai::RawImgFrame::Type::RAW8: { - encoding = sensor_msgs::image_encodings::TYPE_16UC1; - decodeFlags = cv::IMREAD_ANYDEPTH; - channels = CV_16UC1; - break; - } - default: { - std::cout << frameName << static_cast(srcType) << std::endl; - throw(std::runtime_error("Converted type not supported!")); - } - } - - output = cv::imdecode(cv::Mat(inData->getData()), decodeFlags); - - // converting disparity - if(dispToDepth) { - auto factor = std::abs(baseline * 10) * info.p[0]; - cv::Mat depthOut = cv::Mat(cv::Size(output.cols, output.rows), CV_16UC1); - depthOut.forEach([&output, &factor](uint16_t& pixel, const int* position) -> void { - auto disp = output.at(position); - if(disp == 0) - pixel = 0; - else - pixel = factor / disp; - }); - output = depthOut.clone(); - } - cv_bridge::CvImage(header, encoding, output).toImageMsg(outImageMsg); - return outImageMsg; - } - if(planarEncodingEnumMap.find(inData->getType()) != planarEncodingEnumMap.end()) { // cv::Mat inImg = inData->getCvFrame(); cv::Mat mat, output; @@ -537,6 +488,7 @@ ImageMsgs::CameraInfo ImageConverter::calibrationToCameraInfo( std::copy(stereoIntrinsics[i].begin(), stereoIntrinsics[i].end(), stereoFlatIntrinsics.begin() + 4 * i); stereoFlatIntrinsics[(4 * i) + 3] = 0; } + // Check stereo socket order dai::CameraBoardSocket stereoSocketFirst = calibHandler.getStereoLeftCameraId(); dai::CameraBoardSocket stereoSocketSecond = calibHandler.getStereoRightCameraId(); diff --git a/depthai_bridge/src/ImuConverter.cpp b/depthai_bridge/src/ImuConverter.cpp index 49bc9eba..e6cf7933 100644 --- a/depthai_bridge/src/ImuConverter.cpp +++ b/depthai_bridge/src/ImuConverter.cpp @@ -143,4 +143,4 @@ void ImuConverter::toRosDaiMsg(std::shared_ptr inData, std::deque< } } // namespace ros -} // namespace dai +} // namespace dai \ No newline at end of file diff --git a/depthai_bridge/src/TFPublisher.cpp b/depthai_bridge/src/TFPublisher.cpp index 8e52744e..3bc1f470 100644 --- a/depthai_bridge/src/TFPublisher.cpp +++ b/depthai_bridge/src/TFPublisher.cpp @@ -242,7 +242,7 @@ std::string TFPublisher::getURDF() { RCLCPP_DEBUG(logger, "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 2d751133..bc03ec82 100644 --- a/depthai_descriptions/CMakeLists.txt +++ b/depthai_descriptions/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(depthai_descriptions VERSION 2.10.3) +project(depthai_descriptions VERSION 2.10.5) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/depthai_descriptions/package.xml b/depthai_descriptions/package.xml index 27f85f05..7128de43 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 48d76b9a..d4655d93 100644 --- a/depthai_examples/CMakeLists.txt +++ b/depthai_examples/CMakeLists.txt @@ -1,5 +1,5 @@ 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) diff --git a/depthai_examples/package.xml b/depthai_examples/package.xml index 879d4861..8d3bf94e 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 diff --git a/depthai_filters/CMakeLists.txt b/depthai_filters/CMakeLists.txt index cdac3a5d..e70aa597 100644 --- a/depthai_filters/CMakeLists.txt +++ b/depthai_filters/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(depthai_filters VERSION 2.10.3 LANGUAGES CXX C) +project(depthai_filters VERSION 2.10.5 LANGUAGES CXX C) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/depthai_filters/config/usb_cam_overlay.yaml b/depthai_filters/config/usb_cam_overlay.yaml index 70eac219..508f47a9 100644 --- a/depthai_filters/config/usb_cam_overlay.yaml +++ b/depthai_filters/config/usb_cam_overlay.yaml @@ -9,4 +9,4 @@ i_simulated_topic_name: /image_raw i_low_bandwidth: true nn: - i_enable_passthrough: true \ No newline at end of file + i_enable_passthrough: true diff --git a/depthai_filters/include/depthai_filters/feature_tracker_overlay.hpp b/depthai_filters/include/depthai_filters/feature_tracker_overlay.hpp index 028bd9e3..92119f6d 100644 --- a/depthai_filters/include/depthai_filters/feature_tracker_overlay.hpp +++ b/depthai_filters/include/depthai_filters/feature_tracker_overlay.hpp @@ -42,4 +42,5 @@ class FeatureTrackerOverlay : public rclcpp::Node { std::unordered_set trackedIDs; std::unordered_map> trackedFeaturesPath; }; -} // namespace depthai_filters + +} // namespace depthai_filters \ No newline at end of file diff --git a/depthai_filters/launch/example_det2d_overlay.launch.py b/depthai_filters/launch/example_det2d_overlay.launch.py index 41827cbe..478708ac 100644 --- a/depthai_filters/launch/example_det2d_overlay.launch.py +++ b/depthai_filters/launch/example_det2d_overlay.launch.py @@ -26,11 +26,11 @@ def launch_setup(context, *args, **kwargs): composable_node_descriptions=[ ComposableNode( package="depthai_filters", + name="detection_overlay", plugin="depthai_filters::Detection2DOverlay", - name=name+"detection_overlay", remappings=[('rgb/preview/image_raw', name+'/nn/passthrough/image_raw'), ('nn/detections', name+'/nn/detections')], - parameters=[params_file] + parameters=[params_file], ), ], ), diff --git a/depthai_filters/package.xml b/depthai_filters/package.xml index 1cc2c127..418d56be 100644 --- a/depthai_filters/package.xml +++ b/depthai_filters/package.xml @@ -2,7 +2,7 @@ depthai_filters - 2.10.3 + 2.10.5 Depthai filters package Adam Serafin MIT diff --git a/depthai_filters/src/detection2d_overlay.cpp b/depthai_filters/src/detection2d_overlay.cpp index 53e8a4e0..a42e1c6e 100644 --- a/depthai_filters/src/detection2d_overlay.cpp +++ b/depthai_filters/src/detection2d_overlay.cpp @@ -19,7 +19,8 @@ void Detection2DOverlay::onInit() { void Detection2DOverlay::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& preview, const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections) { - cv::Mat previewMat = utils::msgToMat(this->get_logger(), preview, sensor_msgs::image_encodings::BGR8); + cv::Mat previewMat = utils::msgToMat(this->get_logger(), preview, sensor_msgs::image_encodings::BGR8); + auto blue = cv::Scalar(255, 0, 0); for(auto& detection : detections->detections) { diff --git a/depthai_filters/src/feature_tracker_overlay.cpp b/depthai_filters/src/feature_tracker_overlay.cpp index 6edefc6b..88e91314 100644 --- a/depthai_filters/src/feature_tracker_overlay.cpp +++ b/depthai_filters/src/feature_tracker_overlay.cpp @@ -73,3 +73,4 @@ void FeatureTrackerOverlay::drawFeatures(cv::Mat& img) { } } // namespace depthai_filters #include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(depthai_filters::FeatureTrackerOverlay); \ No newline at end of file diff --git a/depthai_filters/src/features_3d.cpp b/depthai_filters/src/features_3d.cpp index 53f2fb59..708f3258 100644 --- a/depthai_filters/src/features_3d.cpp +++ b/depthai_filters/src/features_3d.cpp @@ -66,3 +66,4 @@ void Features3D::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& depth, } // namespace depthai_filters #include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(depthai_filters::Features3D); \ No newline at end of file diff --git a/depthai_filters/src/spatial_bb.cpp b/depthai_filters/src/spatial_bb.cpp index 3ec2fbe8..5ebe2363 100644 --- a/depthai_filters/src/spatial_bb.cpp +++ b/depthai_filters/src/spatial_bb.cpp @@ -145,3 +145,4 @@ void SpatialBB::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& preview } // namespace depthai_filters #include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(depthai_filters::SpatialBB); \ No newline at end of file diff --git a/depthai_ros_driver/CMakeLists.txt b/depthai_ros_driver/CMakeLists.txt index d17b8a9c..7731edff 100644 --- a/depthai_ros_driver/CMakeLists.txt +++ b/depthai_ros_driver/CMakeLists.txt @@ -1,11 +1,11 @@ cmake_minimum_required(VERSION 3.22) -project(depthai_ros_driver VERSION 2.10.3) +project(depthai_ros_driver VERSION 2.10.5) set(CMAKE_POSITION_INDEPENDENT_CODE ON) set(CMAKE_BUILD_SHARED_LIBS ON) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 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) endif() # Default to C99 if(NOT CMAKE_C_STANDARD) diff --git a/depthai_ros_driver/config/pcl.yaml b/depthai_ros_driver/config/pcl.yaml index 1ea1fbb1..2466e9ef 100644 --- a/depthai_ros_driver/config/pcl.yaml +++ b/depthai_ros_driver/config/pcl.yaml @@ -6,4 +6,5 @@ i_align_depth: true i_board_socket_id: 2 i_subpixel: true - i_publish_right_rect: true \ No newline at end of file + i_right_rect_publish_topic: true + i_right_rect_synced: false diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp index a84b8041..ebff5159 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp @@ -55,9 +55,6 @@ class SpatialDetection : public BaseNode { if(ph->getParam("i_disable_resize")) { width = ph->getOtherNodeParam(socketName, "i_preview_width"); height = ph->getOtherNodeParam(socketName, "i_preview_height"); - } else if(ph->getParam("i_desqueeze_output")) { - width = ph->getOtherNodeParam(socketName, "i_width"); - height = ph->getOtherNodeParam(socketName, "i_height"); } else { width = imageManip->initialConfig.getResizeConfig().width; height = imageManip->initialConfig.getResizeConfig().height; 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 d468e749..230b79d5 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 @@ -56,6 +56,7 @@ extern const std::unordered_map fSyncModeMap; extern const std::unordered_map cameraImageOrientationMap; bool rsCompabilityMode(std::shared_ptr node); +std::string tfPrefix(std::shared_ptr node); std::string getSocketName(std::shared_ptr node, dai::CameraBoardSocket socket); std::string getNodeName(std::shared_ptr node, NodeNameEnum name); void basicCameraPub(const std::string& /*name*/, diff --git a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/imu_param_handler.hpp b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/imu_param_handler.hpp index 42fe6d98..be882eed 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/imu_param_handler.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/imu_param_handler.hpp @@ -6,7 +6,6 @@ #include "depthai-shared/properties/IMUProperties.hpp" #include "depthai/pipeline/datatype/CameraControl.hpp" -#include "depthai-shared/properties/IMUProperties.hpp" #include "depthai_bridge/ImuConverter.hpp" #include "depthai_ros_driver/param_handlers/base_param_handler.hpp" diff --git a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/nn_param_handler.hpp b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/nn_param_handler.hpp index 9e287363..f16c8d46 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/nn_param_handler.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/nn_param_handler.hpp @@ -40,7 +40,7 @@ class NNParamHandler : public BaseParamHandler { template void declareParams(std::shared_ptr nn, std::shared_ptr imageManip) { declareAndLogParam("i_disable_resize", false); - declareAndLogParam("i_desqueeze_output", false); + declareAndLogParam("i_desqueeze_output", false); declareAndLogParam("i_enable_passthrough", false); declareAndLogParam("i_enable_passthrough_depth", false); declareAndLogParam("i_get_base_device_timestamp", false); diff --git a/depthai_ros_driver/include/depthai_ros_driver/utils.hpp b/depthai_ros_driver/include/depthai_ros_driver/utils.hpp index ed01b594..168e94c6 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/utils.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/utils.hpp @@ -4,11 +4,6 @@ #include #include #include -#include - -namespace dai { -enum class CameraBoardSocket; -} // namespace dai #include "depthai-shared/common/CameraBoardSocket.hpp" #include "depthai-shared/datatype/RawImgFrame.hpp" @@ -52,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; @@ -71,7 +67,7 @@ struct ImgPublisherConfig { dai::CameraBoardSocket rightSocket = dai::CameraBoardSocket::CAM_C; std::string calibrationFile = ""; std::string topicSuffix = "/image_raw"; - std::string infoSuffix = ""; + std::string infoSuffix = ""; std::string compressedTopicSuffix = "/image_raw/compressed"; std::string infoMgrSuffix = ""; bool rectified = false; diff --git a/depthai_ros_driver/launch/camera.launch.py b/depthai_ros_driver/launch/camera.launch.py index 5f3a8d87..83bb4ca8 100644 --- a/depthai_ros_driver/launch/camera.launch.py +++ b/depthai_ros_driver/launch/camera.launch.py @@ -143,8 +143,8 @@ def launch_setup(context, *args, **kwargs): }, } parameter_overrides["depth"] = { - "i_publish_left_rect": True, - "i_publish_right_rect": True, + "i_left_rect_publish_topic": True, + "i_right_rect_publish_topic": True, } tf_params = {} diff --git a/depthai_ros_driver/package.xml b/depthai_ros_driver/package.xml index 7d7f8ee6..372b0b56 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 diff --git a/depthai_ros_driver/plugins.xml b/depthai_ros_driver/plugins.xml index 1de38c58..17dbbea7 100644 --- a/depthai_ros_driver/plugins.xml +++ b/depthai_ros_driver/plugins.xml @@ -29,4 +29,4 @@ RGB + ToF Pipeline. - + \ No newline at end of file diff --git a/depthai_ros_driver/src/camera.cpp b/depthai_ros_driver/src/camera.cpp index fe01bfee..8ccfc568 100644 --- a/depthai_ros_driver/src/camera.cpp +++ b/depthai_ros_driver/src/camera.cpp @@ -20,7 +20,9 @@ Camera::Camera(const rclcpp::NodeOptions& options) : rclcpp::Node("camera", opti }); rclcpp::on_shutdown([this]() { stop(); }); } -Camera::~Camera() = default; +Camera::~Camera() { + stop(); +} void Camera::onConfigure() { getDeviceType(); createPipeline(); @@ -116,7 +118,6 @@ void Camera::saveCalib() { savePath << "/tmp/" << device->getMxId().c_str() << "_calibration.json"; RCLCPP_INFO(get_logger(), "Saving calibration to: %s", savePath.str().c_str()); calibHandler.eepromToJsonFile(savePath.str()); - auto json = calibHandler.eepromToJson(); } void Camera::loadCalib(const std::string& path) { @@ -287,13 +288,13 @@ rcl_interfaces::msg::SetParametersResult Camera::parameterCB(const std::vectorgetFullParamName("i_laser_dot_brightness")) { float laserdotBrightness = float(p.get_value()); if(laserdotBrightness > 1.0) { - laserdotBrightness = 1200.0 / laserdotBrightness; + laserdotBrightness = laserdotBrightness / 1200.0; } device->setIrLaserDotProjectorIntensity(laserdotBrightness); } else if(p.get_name() == ph->getFullParamName("i_floodlight_brightness")) { float floodlightBrightness = float(p.get_value()); if(floodlightBrightness > 1.0) { - floodlightBrightness = 1500.0 / floodlightBrightness; + floodlightBrightness = floodlightBrightness / 1500.0; } device->setIrFloodLightIntensity(floodlightBrightness); } diff --git a/depthai_ros_driver/src/dai_nodes/base_node.cpp b/depthai_ros_driver/src/dai_nodes/base_node.cpp index b6376bfa..8000348f 100644 --- a/depthai_ros_driver/src/dai_nodes/base_node.cpp +++ b/depthai_ros_driver/src/dai_nodes/base_node.cpp @@ -46,7 +46,7 @@ std::string BaseNode::getSocketName(dai::CameraBoardSocket socket) { } std::string BaseNode::getTFPrefix(const std::string& frameName) { - return std::string(getROSNode()->get_name()) + "_" + frameName; + return sensor_helpers::tfPrefix(getROSNode()) + "_" + frameName; } std::string BaseNode::getOpticalTFPrefix(const std::string& frameName) { 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 2d861911..0e8220c4 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp @@ -99,7 +99,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) { @@ -115,8 +115,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) { diff --git a/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp b/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp index 9174e854..36fb35c6 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp @@ -42,8 +42,7 @@ void Mono::setXinXout(std::shared_ptr pipeline) { encConfig.quality = ph->getParam("i_low_bandwidth_quality"); encConfig.enabled = ph->getParam("i_low_bandwidth"); - imagePublisher = setupOutput( - pipeline, monoQName, [&](auto input) { monoCamNode->out.link(input); }, ph->getParam("i_synced"), encConfig); + imagePublisher = setupOutput(pipeline, monoQName, [&](auto input) { monoCamNode->out.link(input); }, ph->getParam("i_synced"), encConfig); } xinControl = pipeline->create(); xinControl->setStreamName(controlQName); 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 82a976e1..f43ecf33 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp @@ -63,6 +63,13 @@ const std::unordered_map NodeNameMap = { bool rsCompabilityMode(std::shared_ptr node) { return node->get_parameter("camera.i_rs_compat").as_bool(); } + +std::string tfPrefix(std::shared_ptr node) { + if(node->get_parameter("camera.i_publish_tf_from_calibration").as_bool()) { + return node->get_parameter("camera.i_tf_base_frame").as_string(); + } + return node->get_name(); +} std::string getNodeName(std::shared_ptr node, NodeNameEnum name) { if(rsCompabilityMode(node)) { return rsNodeNameMap.at(name); @@ -141,11 +148,6 @@ sensor_msgs::msg::CameraInfo getCalibInfo(const rclcpp::Logger& logger, return info; } -bool detectSubscription(const rclcpp::Publisher::SharedPtr& pub, - const rclcpp::Publisher::SharedPtr& infoPub) { - return (pub->get_subscription_count() > 0 || pub->get_intra_process_subscription_count() > 0 || infoPub->get_subscription_count() > 0 - || infoPub->get_intra_process_subscription_count() > 0); -} } // namespace sensor_helpers } // namespace dai_nodes } // namespace depthai_ros_driver diff --git a/depthai_ros_driver/src/dai_nodes/sensors/stereo.cpp b/depthai_ros_driver/src/dai_nodes/sensors/stereo.cpp index ba57490c..66848add 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/stereo.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/stereo.cpp @@ -80,8 +80,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); }; @@ -92,7 +93,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); } @@ -155,7 +156,7 @@ void Stereo::setupRectQueue(std::shared_ptr device, pubConfig.width = ph->getOtherNodeParam(sensorName, "i_width"); pubConfig.height = ph->getOtherNodeParam(sensorName, "i_height"); pubConfig.topicName = "~/" + sensorName; - pubConfig.topicSuffix = rsCompabilityMode() ? "/image_rect_raw" : "/image_raw"; + pubConfig.topicSuffix = rsCompabilityMode() ? "/image_rect_raw" : "/image_rect"; pubConfig.maxQSize = ph->getOtherNodeParam(sensorName, "i_max_q_size"); pubConfig.socket = sensorInfo.socket; pubConfig.infoMgrSuffix = "rect"; @@ -193,11 +194,12 @@ 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(); pubConf.topicName = "~/" + getName(); - pubConf.topicSuffix = rsCompabilityMode() ? "/image_rect_raw" : "/image_raw"; + pubConf.topicSuffix = rsCompabilityMode() ? "/image_rect_raw" : "/image_raw"; pubConf.rectified = !convConfig.alphaScalingEnabled; pubConf.width = ph->getParam("i_width"); pubConf.height = ph->getParam("i_height"); diff --git a/depthai_ros_driver/src/dai_nodes/sys_logger.cpp b/depthai_ros_driver/src/dai_nodes/sys_logger.cpp index cf4dd7b3..85cb94f6 100644 --- a/depthai_ros_driver/src/dai_nodes/sys_logger.cpp +++ b/depthai_ros_driver/src/dai_nodes/sys_logger.cpp @@ -33,7 +33,7 @@ void SysLogger::setXinXout(std::shared_ptr pipeline) { void SysLogger::setupQueues(std::shared_ptr device) { loggerQ = device->getOutputQueue(loggerQName, 8, false); updater = std::make_shared(getROSNode()); - updater->setHardwareID(getROSNode()->get_name() + std::string("_") + device->getMxId() + std::string("_") + device->getDeviceName()); + updater->setHardwareID(getROSNode()->get_fully_qualified_name() + std::string("_") + device->getMxId() + std::string("_") + device->getDeviceName()); updater->add("sys_logger", std::bind(&SysLogger::produceDiagnostics, this, std::placeholders::_1)); } @@ -69,7 +69,22 @@ void SysLogger::produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& auto logData = loggerQ->get(std::chrono::seconds(5), timeout); if(!timeout) { stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "System Information"); - stat.add("System Information", sysInfoToString(*logData)); + const dai::SystemInformation& 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::msg::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 0b9fe0dd..24744099 100644 --- a/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp @@ -79,6 +79,22 @@ 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); + if(declareAndLogParam("r_set_auto_exposure_limit", false)) { + monoCam->initialControl.setAutoExposureLimit(expLimit); + } + int sharpness = declareAndLogParam("r_sharpness", 1); + if(declareAndLogParam("r_set_sharpness", false)) { + monoCam->initialControl.setSharpness(sharpness); + } + 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); + if(declareAndLogParam("r_set_luma_denoise", false)) { + monoCam->initialControl.setLumaDenoise(lumaDenoise); + } } void SensorParamHandler::declareParams(std::shared_ptr colorCam, dai_nodes::sensor_helpers::ImageSensor sensor, bool publish) { declareAndLogParam("i_publish_topic", publish); @@ -178,6 +194,22 @@ 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); + if(declareAndLogParam("r_set_auto_exposure_limit", false)) { + colorCam->initialControl.setAutoExposureLimit(expLimit); + } + int sharpness = declareAndLogParam("r_sharpness", 1); + if(declareAndLogParam("r_set_sharpness", false)) { + colorCam->initialControl.setSharpness(sharpness); + } + 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); + if(declareAndLogParam("r_set_luma_denoise", false)) { + colorCam->initialControl.setLumaDenoise(lumaDenoise); + } } dai::CameraControl SensorParamHandler::setRuntimeParams(const std::vector& params) { dai::CameraControl ctrl; @@ -216,6 +248,38 @@ dai::CameraControl SensorParamHandler::setRuntimeParams(const std::vector("r_set_man_whitebalance")) { ctrl.setManualWhiteBalance(p.get_value()); } + } else if(p.get_name() == getFullParamName("r_set_auto_exposure_limit")) { + if(p.get_value()) { + ctrl.setAutoExposureLimit(getParam("r_auto_exposure_limit")); + } + } else if(p.get_name() == getFullParamName("r_auto_exposure_limit")) { + if(getParam("r_set_auto_exposure_limit")) { + ctrl.setAutoExposureLimit(p.get_value()); + } + } else if(p.get_name() == getFullParamName("r_set_sharpness")) { + if(p.get_value()) { + ctrl.setSharpness(getParam("r_sharpness")); + } + } else if(p.get_name() == getFullParamName("r_sharpness")) { + if(getParam("r_set_sharpness")) { + ctrl.setSharpness(p.get_value()); + } + } else if(p.get_name() == getFullParamName("r_set_chroma_denoise")) { + if(p.get_value()) { + ctrl.setChromaDenoise(getParam("r_chroma_denoise")); + } + } else if(p.get_name() == getFullParamName("r_chroma_denoise")) { + if(getParam("r_set_chroma_denoise")) { + ctrl.setChromaDenoise(p.get_value()); + } + } else if(p.get_name() == getFullParamName("r_set_luma_denoise")) { + if(p.get_value()) { + ctrl.setLumaDenoise(getParam("r_luma_denoise")); + } + } else if(p.get_name() == getFullParamName("r_luma_denoise")) { + if(getParam("r_set_luma_denoise")) { + ctrl.setLumaDenoise(p.get_value()); + } } } return ctrl; 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 e2bede44..c5cd93c3 100644 --- a/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp @@ -1,5 +1,7 @@ #include "depthai_ros_driver/param_handlers/stereo_param_handler.hpp" +#include + #include "depthai-shared/common/CameraFeatures.hpp" #include "depthai/pipeline/datatype/StereoDepthConfig.hpp" #include "depthai/pipeline/node/StereoDepth.hpp" @@ -10,10 +12,12 @@ namespace depthai_ros_driver { namespace param_handlers { StereoParamHandler::StereoParamHandler(std::shared_ptr 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}, @@ -54,7 +58,7 @@ void StereoParamHandler::updateSocketsFromParams(dai::CameraBoardSocket& left, d 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); @@ -82,7 +86,7 @@ void StereoParamHandler::declareParams(std::shared_ptr s declareAndLogParam("i_left_rect_add_exposure_offset", false); declareAndLogParam("i_left_rect_exposure_offset", 0); declareAndLogParam("i_left_rect_enable_feature_tracker", false); - declareAndLogParam("i_left_rect_synced", true); + declareAndLogParam("i_left_rect_synced", false); declareAndLogParam("i_left_rect_publish_compressed", false); declareAndLogParam("i_right_rect_publish_topic", false); @@ -95,7 +99,7 @@ void StereoParamHandler::declareParams(std::shared_ptr s declareAndLogParam("i_right_rect_enable_feature_tracker", false); declareAndLogParam("i_right_rect_add_exposure_offset", false); declareAndLogParam("i_right_rect_exposure_offset", 0); - declareAndLogParam("i_right_rect_synced", true); + declareAndLogParam("i_right_rect_synced", false); declareAndLogParam("i_right_rect_publish_compressed", false); declareAndLogParam("i_enable_spatial_nn", false); @@ -136,34 +140,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); + RCLCPP_INFO(getROSNode()->get_logger(), "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_msgs/CMakeLists.txt b/depthai_ros_msgs/CMakeLists.txt index b1b99c90..d027a765 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 f7efee68..f455901d 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