From f136749e130def88c6c7cd0bb31bfa36b037e0dc Mon Sep 17 00:00:00 2001 From: Adam Serafin Date: Thu, 19 Sep 2024 16:16:47 +0200 Subject: [PATCH] Jazzy 2.10.1 (#595) --- .github/workflows/main.workflow.yml | 10 +- depthai-ros/CHANGELOG.rst | 14 +- depthai-ros/CMakeLists.txt | 2 +- depthai-ros/package.xml | 2 +- depthai_bridge/CMakeLists.txt | 2 +- depthai_bridge/package.xml | 2 +- depthai_descriptions/CMakeLists.txt | 2 +- depthai_descriptions/package.xml | 2 +- depthai_examples/CMakeLists.txt | 2 +- depthai_examples/package.xml | 2 +- depthai_filters/CMakeLists.txt | 2 +- depthai_filters/package.xml | 2 +- depthai_ros_driver/CMakeLists.txt | 2 +- depthai_ros_driver/config/sr_poe_rgbd.yaml | 5 + depthai_ros_driver/package.xml | 2 +- depthai_ros_driver/src/camera.cpp | 21 +- .../src/dai_nodes/sensors/img_pub.cpp | 1 + depthai_ros_driver/src/dai_nodes/stereo.cpp | 403 ------------------ .../src/param_handlers/tof_param_handler.cpp | 2 +- depthai_ros_msgs/CMakeLists.txt | 2 +- depthai_ros_msgs/package.xml | 2 +- 21 files changed, 48 insertions(+), 436 deletions(-) delete mode 100644 depthai_ros_driver/src/dai_nodes/stereo.cpp diff --git a/.github/workflows/main.workflow.yml b/.github/workflows/main.workflow.yml index 126fde76..d8f4f742 100644 --- a/.github/workflows/main.workflow.yml +++ b/.github/workflows/main.workflow.yml @@ -10,6 +10,7 @@ on: - galactic - foxy - iron + - jazzy tags: - 'v*' pull_request: @@ -19,13 +20,14 @@ on: - galactic - foxy - iron + - jazzy env: - ROS_DISTRO: iron + ROS_DISTRO: jazzy jobs: clang-format-lint: name: ament_clang_format - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 strategy: fail-fast: false steps: @@ -45,7 +47,7 @@ jobs: depthai_ros linting: name: ament_${{ matrix.linter }} - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 strategy: fail-fast: false matrix: @@ -67,7 +69,7 @@ jobs: docker-build: name: Build and Upload to Docker Hub - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 strategy: fail-fast: false env: diff --git a/depthai-ros/CHANGELOG.rst b/depthai-ros/CHANGELOG.rst index bdf0faaf..a46eab1b 100644 --- a/depthai-ros/CHANGELOG.rst +++ b/depthai-ros/CHANGELOG.rst @@ -2,9 +2,14 @@ Changelog for package depthai-ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.1 (2024-09-18) +------------------- +* Fix ToF synced publishing +* Add camera_info publishing when publishing compressed images +* Catch errors when starting the device + 2.10.0 (2024-08-29) ------------------- -## What's Changed * Adding stl files for SR and LR models by @danilo-pejovic in https://github.com/luxonis/depthai-ros/pull/491 * No imu fix Humble by @Serafadam in https://github.com/luxonis/depthai-ros/pull/500 * Tracking converter for ROS2 Humble by @daniqsilva25 in https://github.com/luxonis/depthai-ros/pull/505 @@ -20,13 +25,6 @@ Changelog for package depthai-ros * WLS fix humble by @Serafadam in https://github.com/luxonis/depthai-ros/pull/582 * Syncing & RS updates Humble by @Serafadam in https://github.com/luxonis/depthai-ros/pull/586 -## New Contributors -* @r4hul77 made their first contribution in https://github.com/luxonis/depthai-ros/pull/507 -* @Nibanovic made their first contribution in https://github.com/luxonis/depthai-ros/pull/511 -* @destogl made their first contribution in https://github.com/luxonis/depthai-ros/pull/524 - -**Full Changelog**: https://github.com/luxonis/depthai-ros/compare/v2.9.0-humble...v2.10.0-humble - 2.9.0 (2024-01-24) ------------------- diff --git a/depthai-ros/CMakeLists.txt b/depthai-ros/CMakeLists.txt index 770691b9..9684655d 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.0 LANGUAGES CXX C) +project(depthai-ros VERSION 2.10.1 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) diff --git a/depthai-ros/package.xml b/depthai-ros/package.xml index a9a262b3..82fd8f15 100644 --- a/depthai-ros/package.xml +++ b/depthai-ros/package.xml @@ -1,7 +1,7 @@ depthai-ros - 2.10.0 + 2.10.1 The depthai-ros package diff --git a/depthai_bridge/CMakeLists.txt b/depthai_bridge/CMakeLists.txt index 292b365c..7bba9586 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.0 LANGUAGES CXX C) +project(depthai_bridge VERSION 2.10.1 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 4b34b45e..9148a6dc 100644 --- a/depthai_bridge/package.xml +++ b/depthai_bridge/package.xml @@ -1,7 +1,7 @@ depthai_bridge - 2.10.0 + 2.10.1 The depthai_bridge package Adam Serafin diff --git a/depthai_descriptions/CMakeLists.txt b/depthai_descriptions/CMakeLists.txt index 7cbf15a2..ad2dc15b 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.0) +project(depthai_descriptions VERSION 2.10.1) 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 3f3eeff2..a8c064d1 100644 --- a/depthai_descriptions/package.xml +++ b/depthai_descriptions/package.xml @@ -1,7 +1,7 @@ depthai_descriptions - 2.10.0 + 2.10.1 The depthai_descriptions package Adam Serafin diff --git a/depthai_examples/CMakeLists.txt b/depthai_examples/CMakeLists.txt index 3505ea40..afc582d0 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.0 LANGUAGES CXX C) +project(depthai_examples VERSION 2.10.1 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 d317c671..38327126 100644 --- a/depthai_examples/package.xml +++ b/depthai_examples/package.xml @@ -1,7 +1,7 @@ depthai_examples - 2.10.0 + 2.10.1 The depthai_examples package diff --git a/depthai_filters/CMakeLists.txt b/depthai_filters/CMakeLists.txt index a792b2a4..4349a21a 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.0 LANGUAGES CXX C) +project(depthai_filters VERSION 2.10.1 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/package.xml b/depthai_filters/package.xml index 057bf37e..1890e840 100644 --- a/depthai_filters/package.xml +++ b/depthai_filters/package.xml @@ -2,7 +2,7 @@ depthai_filters - 2.10.0 + 2.10.1 Depthai filters package Adam Serafin MIT diff --git a/depthai_ros_driver/CMakeLists.txt b/depthai_ros_driver/CMakeLists.txt index a33a8ef6..23bf624e 100644 --- a/depthai_ros_driver/CMakeLists.txt +++ b/depthai_ros_driver/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.22) -project(depthai_ros_driver VERSION 2.10.0) +project(depthai_ros_driver VERSION 2.10.1) set(CMAKE_POSITION_INDEPENDENT_CODE ON) set(CMAKE_BUILD_SHARED_LIBS ON) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) diff --git a/depthai_ros_driver/config/sr_poe_rgbd.yaml b/depthai_ros_driver/config/sr_poe_rgbd.yaml index 350dd6d7..242723af 100644 --- a/depthai_ros_driver/config/sr_poe_rgbd.yaml +++ b/depthai_ros_driver/config/sr_poe_rgbd.yaml @@ -6,5 +6,10 @@ i_enable_ir: false i_nn_type: none i_pipeline_type: rgbtof + pipeline_gen: + i_enable_sync: true right: i_low_bandwidth: true + i_synced: true + tof: + i_synced: true diff --git a/depthai_ros_driver/package.xml b/depthai_ros_driver/package.xml index 553755f9..d17a5ac7 100644 --- a/depthai_ros_driver/package.xml +++ b/depthai_ros_driver/package.xml @@ -2,7 +2,7 @@ depthai_ros_driver - 2.10.0 + 2.10.1 Depthai ROS Monolithic node. Adam Serafin diff --git a/depthai_ros_driver/src/camera.cpp b/depthai_ros_driver/src/camera.cpp index 4ca0da14..574ef7bd 100644 --- a/depthai_ros_driver/src/camera.cpp +++ b/depthai_ros_driver/src/camera.cpp @@ -15,7 +15,7 @@ Camera::Camera(const rclcpp::NodeOptions& options) : rclcpp::Node("camera", opti startTimer = this->create_wall_timer(std::chrono::seconds(1), [this]() { ph = std::make_unique(shared_from_this(), "camera"); ph->declareParams(); - onConfigure(); + start(); startTimer->cancel(); }); rclcpp::on_shutdown([this]() { stop(); }); @@ -76,11 +76,20 @@ void Camera::diagCB(const diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg) } void Camera::start() { - RCLCPP_INFO(get_logger(), "Starting camera."); - if(!camRunning) { - onConfigure(); - } else { - RCLCPP_INFO(get_logger(), "Camera already running!."); + bool success = false; + while(!success) { + try { + RCLCPP_INFO(this->get_logger(), "Starting camera."); + if(!camRunning) { + onConfigure(); + } else { + RCLCPP_INFO(this->get_logger(), "Camera already running!."); + } + success = true; + } catch(const std::exception& e) { + RCLCPP_ERROR(this->get_logger(), "Exception occurred: %s. Retry", e.what()); + camRunning = false; + } } } 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 70f9fb28..02e0e9c1 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp @@ -185,6 +185,7 @@ void ImagePublisher::publish(std::shared_ptr img) { } else { ffmpegPub->publish(std::move(img->ffmpegPacket)); } + infoPub->publish(std::move(img->info)); } else { if(ipcEnabled && (!pubConfig.lazyPub || detectSubscription(imgPub, infoPub))) { imgPub->publish(std::move(img->image)); diff --git a/depthai_ros_driver/src/dai_nodes/stereo.cpp b/depthai_ros_driver/src/dai_nodes/stereo.cpp deleted file mode 100644 index 47b4fe66..00000000 --- a/depthai_ros_driver/src/dai_nodes/stereo.cpp +++ /dev/null @@ -1,403 +0,0 @@ -#include "depthai_ros_driver/dai_nodes/stereo.hpp" - -#include "camera_info_manager/camera_info_manager.hpp" -#include "cv_bridge/cv_bridge.hpp" -#include "depthai/device/DataQueue.hpp" -#include "depthai/device/DeviceBase.hpp" -#include "depthai/pipeline/Pipeline.hpp" -#include "depthai/pipeline/node/StereoDepth.hpp" -#include "depthai/pipeline/node/VideoEncoder.hpp" -#include "depthai/pipeline/node/XLinkOut.hpp" -#include "depthai_bridge/ImageConverter.hpp" -#include "depthai_ros_driver/dai_nodes/nn/nn_helpers.hpp" -#include "depthai_ros_driver/dai_nodes/nn/spatial_nn_wrapper.hpp" -#include "depthai_ros_driver/dai_nodes/sensors/feature_tracker.hpp" -#include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" -#include "depthai_ros_driver/dai_nodes/sensors/sensor_wrapper.hpp" -#include "depthai_ros_driver/param_handlers/stereo_param_handler.hpp" -#include "depthai_ros_driver/utils.hpp" -#include "image_transport/camera_publisher.hpp" -#include "image_transport/image_transport.hpp" -#include "rclcpp/node.hpp" - -namespace depthai_ros_driver { -namespace dai_nodes { -Stereo::Stereo(const std::string& daiNodeName, - rclcpp::Node* node, - std::shared_ptr pipeline, - std::shared_ptr device, - dai::CameraBoardSocket leftSocket, - dai::CameraBoardSocket rightSocket) - : BaseNode(daiNodeName, node, pipeline) { - RCLCPP_DEBUG(node->get_logger(), "Creating node %s", daiNodeName.c_str()); - setNames(); - ph = std::make_unique(node, daiNodeName); - auto alignSocket = dai::CameraBoardSocket::CAM_A; - if(device->getDeviceName() == "OAK-D-SR") { - alignSocket = dai::CameraBoardSocket::CAM_C; - } - ph->updateSocketsFromParams(leftSocket, rightSocket, alignSocket); - auto features = device->getConnectedCameraFeatures(); - for(auto f : features) { - if(f.socket == leftSocket) { - leftSensInfo = f; - } else if(f.socket == rightSocket) { - rightSensInfo = f; - } else { - continue; - } - } - RCLCPP_DEBUG(node->get_logger(), - "Creating stereo node with left sensor %s and right sensor %s", - utils::getSocketName(leftSensInfo.socket).c_str(), - utils::getSocketName(rightSensInfo.socket).c_str()); - left = std::make_unique(utils::getSocketName(leftSensInfo.socket), node, pipeline, device, leftSensInfo.socket, false); - right = std::make_unique(utils::getSocketName(rightSensInfo.socket), node, pipeline, device, rightSensInfo.socket, false); - stereoCamNode = pipeline->create(); - ph->declareParams(stereoCamNode); - setXinXout(pipeline); - left->link(stereoCamNode->left); - right->link(stereoCamNode->right); - - if(ph->getParam("i_enable_spatial_nn")) { - if(ph->getParam("i_spatial_nn_source") == "left") { - nnNode = std::make_unique(getName() + "_spatial_nn", getROSNode(), pipeline, leftSensInfo.socket); - left->link(nnNode->getInput(static_cast(dai_nodes::nn_helpers::link_types::SpatialNNLinkType::input)), - static_cast(dai_nodes::link_types::RGBLinkType::preview)); - } else { - nnNode = std::make_unique(getName() + "_spatial_nn", getROSNode(), pipeline, rightSensInfo.socket); - right->link(nnNode->getInput(static_cast(dai_nodes::nn_helpers::link_types::SpatialNNLinkType::input)), - static_cast(dai_nodes::link_types::RGBLinkType::preview)); - } - stereoCamNode->depth.link(nnNode->getInput(static_cast(dai_nodes::nn_helpers::link_types::SpatialNNLinkType::inputDepth))); - } - - RCLCPP_DEBUG(node->get_logger(), "Node %s created", daiNodeName.c_str()); -} -Stereo::~Stereo() = default; -void Stereo::setNames() { - stereoQName = getName() + "_stereo"; - leftRectQName = getName() + "_left_rect"; - rightRectQName = getName() + "_right_rect"; -} - -void Stereo::setXinXout(std::shared_ptr pipeline) { - if(ph->getParam("i_publish_topic")) { - xoutStereo = pipeline->create(); - xoutStereo->setStreamName(stereoQName); - if(ph->getParam("i_low_bandwidth")) { - stereoEnc = sensor_helpers::createEncoder(pipeline, ph->getParam("i_low_bandwidth_quality")); - stereoCamNode->disparity.link(stereoEnc->input); - stereoEnc->bitstream.link(xoutStereo->input); - } else { - if(ph->getParam("i_output_disparity")) { - stereoCamNode->disparity.link(xoutStereo->input); - } else { - stereoCamNode->depth.link(xoutStereo->input); - } - } - } - if(ph->getParam("i_publish_left_rect") || ph->getParam("i_publish_synced_rect_pair")) { - xoutLeftRect = pipeline->create(); - xoutLeftRect->setStreamName(leftRectQName); - if(ph->getParam("i_left_rect_low_bandwidth")) { - leftRectEnc = sensor_helpers::createEncoder(pipeline, ph->getParam("i_left_rect_low_bandwidth_quality")); - stereoCamNode->rectifiedLeft.link(leftRectEnc->input); - leftRectEnc->bitstream.link(xoutLeftRect->input); - } else { - stereoCamNode->rectifiedLeft.link(xoutLeftRect->input); - } - } - - if(ph->getParam("i_publish_right_rect") || ph->getParam("i_publish_synced_rect_pair")) { - xoutRightRect = pipeline->create(); - xoutRightRect->setStreamName(rightRectQName); - if(ph->getParam("i_right_rect_low_bandwidth")) { - rightRectEnc = sensor_helpers::createEncoder(pipeline, ph->getParam("i_right_rect_low_bandwidth_quality")); - stereoCamNode->rectifiedRight.link(rightRectEnc->input); - rightRectEnc->bitstream.link(xoutRightRect->input); - } else { - stereoCamNode->rectifiedRight.link(xoutRightRect->input); - } - } - - if(ph->getParam("i_left_rect_enable_feature_tracker")) { - featureTrackerLeftR = std::make_unique(leftSensInfo.name + std::string("_rect_feature_tracker"), getROSNode(), pipeline); - - stereoCamNode->rectifiedLeft.link(featureTrackerLeftR->getInput()); - } - - if(ph->getParam("i_right_rect_enable_feature_tracker")) { - featureTrackerRightR = std::make_unique(rightSensInfo.name + std::string("_rect_feature_tracker"), getROSNode(), pipeline); - stereoCamNode->rectifiedRight.link(featureTrackerRightR->getInput()); - } -} - -void Stereo::setupRectQueue(std::shared_ptr device, - dai::CameraFeatures& sensorInfo, - const std::string& queueName, - std::unique_ptr& conv, - std::shared_ptr& im, - std::shared_ptr& q, - rclcpp::Publisher::SharedPtr pub, - rclcpp::Publisher::SharedPtr infoPub, - image_transport::CameraPublisher& pubIT, - bool isLeft) { - auto sensorName = utils::getSocketName(sensorInfo.socket); - auto tfPrefix = getTFPrefix(sensorName); - conv = std::make_unique(tfPrefix + "_camera_optical_frame", false, ph->getParam("i_get_base_device_timestamp")); - conv->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); - bool lowBandwidth = ph->getParam(isLeft ? "i_left_rect_low_bandwidth" : "i_right_rect_low_bandwidth"); - if(lowBandwidth) { - conv->convertFromBitstream(dai::RawImgFrame::Type::GRAY8); - } - bool addOffset = ph->getParam(isLeft ? "i_left_rect_add_exposure_offset" : "i_right_rect_add_exposure_offset"); - if(addOffset) { - auto offset = static_cast(ph->getParam(isLeft ? "i_left_rect_exposure_offset" : "i_right_rect_exposure_offset")); - conv->addExposureOffset(offset); - } - im = std::make_shared(getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + sensorName).get(), - "/rect"); - if(ph->getParam("i_reverse_stereo_socket_order")) { - conv->reverseStereoSocketOrder(); - } - auto info = sensor_helpers::getCalibInfo(getROSNode()->get_logger(), - *conv, - device, - sensorInfo.socket, - ph->getOtherNodeParam(sensorName, "i_width"), - ph->getOtherNodeParam(sensorName, "i_height")); - for(auto& d : info.d) { - d = 0.0; - } - - for(auto& r : info.r) { - r = 0.0; - } - info.r[0] = info.r[4] = info.r[8] = 1.0; - - im->setCameraInfo(info); - - q = device->getOutputQueue(queueName, ph->getOtherNodeParam(sensorName, "i_max_q_size"), false); - - // if publish synced pair is set to true then we skip individual publishing of left and right rectified frames - bool addCallback = !ph->getParam("i_publish_synced_rect_pair"); - - if(ipcEnabled()) { - pub = getROSNode()->create_publisher("~/" + sensorName + "/image_rect", 10); - infoPub = getROSNode()->create_publisher("~/" + getName() + "/camera_info", 10); - if(addCallback) { - q->addCallback(std::bind(sensor_helpers::splitPub, - std::placeholders::_1, - std::placeholders::_2, - *conv, - pub, - infoPub, - im, - ph->getParam("i_enable_lazy_publisher"))); - } - } else { - pubIT = image_transport::create_camera_publisher(getROSNode(), "~/" + sensorName + "/image_rect"); - if(addCallback) { - q->addCallback(std::bind( - sensor_helpers::cameraPub, std::placeholders::_1, std::placeholders::_2, *conv, pubIT, im, ph->getParam("i_enable_lazy_publisher"))); - } - } -} - -void Stereo::setupLeftRectQueue(std::shared_ptr device) { - setupRectQueue(device, leftSensInfo, leftRectQName, leftRectConv, leftRectIM, leftRectQ, leftRectPub, leftRectInfoPub, leftRectPubIT, true); -} - -void Stereo::setupRightRectQueue(std::shared_ptr device) { - setupRectQueue(device, rightSensInfo, rightRectQName, rightRectConv, rightRectIM, rightRectQ, rightRectPub, rightRectInfoPub, rightRectPubIT, false); -} - -void Stereo::setupStereoQueue(std::shared_ptr device) { - std::string tfPrefix; - if(ph->getParam("i_align_depth")) { - tfPrefix = getTFPrefix(ph->getParam("i_socket_name")); - } else { - tfPrefix = getTFPrefix(utils::getSocketName(rightSensInfo.socket).c_str()); - } - stereoConv = std::make_unique(tfPrefix + "_camera_optical_frame", false, ph->getParam("i_get_base_device_timestamp")); - stereoConv->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); - if(ph->getParam("i_low_bandwidth")) { - stereoConv->convertFromBitstream(dai::RawImgFrame::Type::RAW8); - } - if(ph->getParam("i_add_exposure_offset")) { - auto offset = static_cast(ph->getParam("i_exposure_offset")); - stereoConv->addExposureOffset(offset); - } - if(ph->getParam("i_reverse_stereo_socket_order")) { - stereoConv->reverseStereoSocketOrder(); - } - if(ph->getParam("i_enable_alpha_scaling")) { - stereoConv->setAlphaScaling(ph->getParam("i_alpha_scaling")); - } - stereoIM = std::make_shared( - getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + getName()).get(), "/" + getName()); - auto info = sensor_helpers::getCalibInfo(getROSNode()->get_logger(), - *stereoConv, - device, - static_cast(ph->getParam("i_board_socket_id")), - ph->getParam("i_width"), - ph->getParam("i_height")); - auto calibHandler = device->readCalibration(); - if(!ph->getParam("i_output_disparity")) { - if(ph->getParam("i_reverse_stereo_socket_order")) { - stereoConv->convertDispToDepth(calibHandler.getBaselineDistance(leftSensInfo.socket, rightSensInfo.socket, false)); - } else { - stereoConv->convertDispToDepth(calibHandler.getBaselineDistance(rightSensInfo.socket, leftSensInfo.socket, false)); - } - } - // remove distortion if alpha scaling is not enabled - if(!ph->getParam("i_enable_alpha_scaling")) { - for(auto& d : info.d) { - d = 0.0; - } - for(auto& r : info.r) { - r = 0.0; - } - info.r[0] = info.r[4] = info.r[8] = 1.0; - } - - stereoIM->setCameraInfo(info); - stereoQ = device->getOutputQueue(stereoQName, ph->getParam("i_max_q_size"), false); - if(ipcEnabled()) { - stereoPub = getROSNode()->create_publisher("~/" + getName() + "/image_raw", 10); - stereoInfoPub = getROSNode()->create_publisher("~/" + getName() + "/camera_info", 10); - stereoQ->addCallback(std::bind(sensor_helpers::splitPub, - std::placeholders::_1, - std::placeholders::_2, - *stereoConv, - stereoPub, - stereoInfoPub, - stereoIM, - ph->getParam("i_enable_lazy_publisher"))); - } else { - stereoPubIT = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/image_raw"); - stereoQ->addCallback(std::bind(sensor_helpers::cameraPub, - std::placeholders::_1, - std::placeholders::_2, - *stereoConv, - stereoPubIT, - stereoIM, - ph->getParam("i_enable_lazy_publisher"))); - } -} - -void Stereo::syncTimerCB() { - auto left = leftRectQ->get(); - auto right = rightRectQ->get(); - if(left->getSequenceNum() != right->getSequenceNum()) { - RCLCPP_WARN(getROSNode()->get_logger(), "Left and right rectified frames are not synchronized!"); - } else { - bool lazyPub = ph->getParam("i_enable_lazy_publisher"); - if(ipcEnabled() && rclcpp::ok() - && (!lazyPub || sensor_helpers::detectSubscription(leftRectPub, leftRectInfoPub) - || sensor_helpers::detectSubscription(rightRectPub, rightRectInfoPub))) { - auto leftInfo = leftRectIM->getCameraInfo(); - auto leftRawMsg = leftRectConv->toRosMsgRawPtr(left); - leftInfo.header = leftRawMsg.header; - auto rightInfo = rightRectIM->getCameraInfo(); - auto rightRawMsg = rightRectConv->toRosMsgRawPtr(right); - rightRawMsg.header.stamp = leftRawMsg.header.stamp; - rightInfo.header = rightRawMsg.header; - sensor_msgs::msg::CameraInfo::UniquePtr leftInfoMsg = std::make_unique(leftInfo); - sensor_msgs::msg::Image::UniquePtr leftMsg = std::make_unique(leftRawMsg); - sensor_msgs::msg::CameraInfo::UniquePtr rightInfoMsg = std::make_unique(rightInfo); - sensor_msgs::msg::Image::UniquePtr rightMsg = std::make_unique(rightRawMsg); - leftRectPub->publish(std::move(leftMsg)); - leftRectInfoPub->publish(std::move(leftInfoMsg)); - rightRectPub->publish(std::move(rightMsg)); - rightRectInfoPub->publish(std::move(rightInfoMsg)); - } else if(!ipcEnabled() && rclcpp::ok() && (!lazyPub || leftRectPubIT.getNumSubscribers() > 0 || rightRectPubIT.getNumSubscribers() > 0)) { - auto leftInfo = leftRectIM->getCameraInfo(); - auto leftRawMsg = leftRectConv->toRosMsgRawPtr(left); - leftInfo.header = leftRawMsg.header; - auto rightInfo = rightRectIM->getCameraInfo(); - auto rightRawMsg = rightRectConv->toRosMsgRawPtr(right); - rightRawMsg.header.stamp = leftRawMsg.header.stamp; - rightInfo.header = rightRawMsg.header; - leftRectPubIT.publish(leftRawMsg, leftInfo); - rightRectPubIT.publish(rightRawMsg, rightInfo); - } - } -} - -void Stereo::setupQueues(std::shared_ptr device) { - left->setupQueues(device); - right->setupQueues(device); - if(ph->getParam("i_publish_topic")) { - setupStereoQueue(device); - } - if(ph->getParam("i_publish_left_rect") || ph->getParam("i_publish_synced_rect_pair")) { - setupLeftRectQueue(device); - } - if(ph->getParam("i_publish_right_rect") || ph->getParam("i_publish_synced_rect_pair")) { - setupRightRectQueue(device); - } - if(ph->getParam("i_publish_synced_rect_pair")) { - int timerPeriod = 1000.0 / ph->getOtherNodeParam(leftSensInfo.name, "i_fps"); - RCLCPP_INFO(getROSNode()->get_logger(), "Setting up stereo pair sync timer with period %d ms based on left sensor FPS.", timerPeriod); - syncTimer = getROSNode()->create_wall_timer(std::chrono::milliseconds(timerPeriod), std::bind(&Stereo::syncTimerCB, this)); - } - if(ph->getParam("i_left_rect_enable_feature_tracker")) { - featureTrackerLeftR->setupQueues(device); - } - if(ph->getParam("i_right_rect_enable_feature_tracker")) { - featureTrackerRightR->setupQueues(device); - } - if(ph->getParam("i_enable_spatial_nn")) { - nnNode->setupQueues(device); - } -} -void Stereo::closeQueues() { - left->closeQueues(); - right->closeQueues(); - if(ph->getParam("i_publish_topic")) { - stereoQ->close(); - } - if(ph->getParam("i_publish_left_rect")) { - leftRectQ->close(); - } - if(ph->getParam("i_publish_right_rect")) { - rightRectQ->close(); - } - if(ph->getParam("i_publish_synced_rect_pair")) { - syncTimer->cancel(); - leftRectQ->close(); - rightRectQ->close(); - } - if(ph->getParam("i_left_rect_enable_feature_tracker")) { - featureTrackerLeftR->closeQueues(); - } - if(ph->getParam("i_right_rect_enable_feature_tracker")) { - featureTrackerRightR->closeQueues(); - } - if(ph->getParam("i_enable_spatial_nn")) { - nnNode->closeQueues(); - } -} - -void Stereo::link(dai::Node::Input in, int /*linkType*/) { - stereoCamNode->depth.link(in); -} - -dai::Node::Input Stereo::getInput(int linkType) { - if(linkType == static_cast(link_types::StereoLinkType::left)) { - return stereoCamNode->left; - } else if(linkType == static_cast(link_types::StereoLinkType::right)) { - return stereoCamNode->right; - } else { - throw std::runtime_error("Wrong link type specified!"); - } -} - -void Stereo::updateParams(const std::vector& params) { - ph->setRuntimeParams(params); -} - -} // namespace dai_nodes -} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/src/param_handlers/tof_param_handler.cpp b/depthai_ros_driver/src/param_handlers/tof_param_handler.cpp index 1e43852d..1ccc3306 100644 --- a/depthai_ros_driver/src/param_handlers/tof_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/tof_param_handler.cpp @@ -19,7 +19,7 @@ ToFParamHandler::ToFParamHandler(std::shared_ptr node, const std:: ToFParamHandler::~ToFParamHandler() = default; void ToFParamHandler::declareParams(std::shared_ptr cam, std::shared_ptr tof) { declareAndLogParam("i_publish_topic", true); - declareAndLogParam("i_synced", true); + declareAndLogParam("i_synced", false); declareAndLogParam("i_low_bandwidth", false); declareAndLogParam("i_low_bandwidth_profile", 4); declareAndLogParam("i_low_bandwidth_bitrate", 0); diff --git a/depthai_ros_msgs/CMakeLists.txt b/depthai_ros_msgs/CMakeLists.txt index 4011f812..8c3b48bc 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.0) +project(depthai_ros_msgs VERSION 2.10.1) if(POLICY CMP0057) cmake_policy(SET CMP0057 NEW) diff --git a/depthai_ros_msgs/package.xml b/depthai_ros_msgs/package.xml index 5bebaec3..2803de5e 100644 --- a/depthai_ros_msgs/package.xml +++ b/depthai_ros_msgs/package.xml @@ -1,7 +1,7 @@ depthai_ros_msgs - 2.10.0 + 2.10.1 Package to keep interface independent of the driver Adam Serafin