diff --git a/depthai-ros/CHANGELOG.rst b/depthai-ros/CHANGELOG.rst index a46eab1b..9c0a9d09 100644 --- a/depthai-ros/CHANGELOG.rst +++ b/depthai-ros/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package depthai-ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.2 (2024-09-26) +------------------- +* Fix Stereo K matrix publishing +* Fix socket ID for NN detections +* Remove catching errors when starting the device since it introduced undefined behavior + 2.10.1 (2024-09-18) ------------------- * Fix ToF synced publishing diff --git a/depthai-ros/CMakeLists.txt b/depthai-ros/CMakeLists.txt index 9684655d..f2766769 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.1 LANGUAGES CXX C) +project(depthai-ros VERSION 2.10.2 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) diff --git a/depthai-ros/package.xml b/depthai-ros/package.xml index 82fd8f15..4cf4aac6 100644 --- a/depthai-ros/package.xml +++ b/depthai-ros/package.xml @@ -1,7 +1,7 @@ depthai-ros - 2.10.1 + 2.10.2 The depthai-ros package diff --git a/depthai_bridge/CMakeLists.txt b/depthai_bridge/CMakeLists.txt index 7bba9586..9c199c09 100644 --- a/depthai_bridge/CMakeLists.txt +++ b/depthai_bridge/CMakeLists.txt @@ -1,10 +1,11 @@ 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.1 LANGUAGES CXX C) +project(depthai_bridge VERSION 2.10.2 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # Update the policy setting to avoid an error when loading the ament_cmake package # at the current cmake version level diff --git a/depthai_bridge/package.xml b/depthai_bridge/package.xml index 9148a6dc..8d974ff3 100644 --- a/depthai_bridge/package.xml +++ b/depthai_bridge/package.xml @@ -1,7 +1,7 @@ depthai_bridge - 2.10.1 + 2.10.2 The depthai_bridge package Adam Serafin diff --git a/depthai_descriptions/CMakeLists.txt b/depthai_descriptions/CMakeLists.txt index ad2dc15b..f524c03f 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.1) +project(depthai_descriptions VERSION 2.10.2) 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 a8c064d1..5a190525 100644 --- a/depthai_descriptions/package.xml +++ b/depthai_descriptions/package.xml @@ -1,7 +1,7 @@ depthai_descriptions - 2.10.1 + 2.10.2 The depthai_descriptions package Adam Serafin diff --git a/depthai_examples/CMakeLists.txt b/depthai_examples/CMakeLists.txt index afc582d0..98269251 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.1 LANGUAGES CXX C) +project(depthai_examples VERSION 2.10.2 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 38327126..35723f24 100644 --- a/depthai_examples/package.xml +++ b/depthai_examples/package.xml @@ -1,7 +1,7 @@ depthai_examples - 2.10.1 + 2.10.2 The depthai_examples package diff --git a/depthai_filters/CMakeLists.txt b/depthai_filters/CMakeLists.txt index 4349a21a..98c0d2f3 100644 --- a/depthai_filters/CMakeLists.txt +++ b/depthai_filters/CMakeLists.txt @@ -1,10 +1,11 @@ cmake_minimum_required(VERSION 3.8) -project(depthai_filters VERSION 2.10.1 LANGUAGES CXX C) +project(depthai_filters VERSION 2.10.2 LANGUAGES CXX C) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # find dependencies find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() diff --git a/depthai_filters/config/detection.yaml b/depthai_filters/config/detection.yaml index c777b9a3..6b47f74e 100644 --- a/depthai_filters/config/detection.yaml +++ b/depthai_filters/config/detection.yaml @@ -6,3 +6,6 @@ i_enable_preview: true nn: i_enable_passthrough: true +/detection_overlay: + ros__parameters: + desqueeze: true diff --git a/depthai_filters/include/depthai_filters/detection2d_overlay.hpp b/depthai_filters/include/depthai_filters/detection2d_overlay.hpp index 2b79ef75..70631573 100644 --- a/depthai_filters/include/depthai_filters/detection2d_overlay.hpp +++ b/depthai_filters/include/depthai_filters/detection2d_overlay.hpp @@ -14,17 +14,19 @@ class Detection2DOverlay : public rclcpp::Node { explicit Detection2DOverlay(const rclcpp::NodeOptions& options); void onInit(); - void overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& preview, const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections); + void overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& preview, const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info, const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections); message_filters::Subscriber previewSub; + message_filters::Subscriber infoSub; message_filters::Subscriber detSub; - typedef message_filters::sync_policies::ApproximateTime syncPolicy; + typedef message_filters::sync_policies::ApproximateTime syncPolicy; std::unique_ptr> sync; rclcpp::Publisher::SharedPtr overlayPub; std::vector labelMap = {"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", "car", "cat", "chair", "cow", "diningtable", "dog", "horse", "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; + bool desqueeze = false; }; -} // namespace depthai_filters \ No newline at end of file +} // namespace depthai_filters diff --git a/depthai_filters/launch/example_det2d_overlay.launch.py b/depthai_filters/launch/example_det2d_overlay.launch.py index 08d26f88..5546c9f5 100644 --- a/depthai_filters/launch/example_det2d_overlay.launch.py +++ b/depthai_filters/launch/example_det2d_overlay.launch.py @@ -27,8 +27,11 @@ def launch_setup(context, *args, **kwargs): ComposableNode( package="depthai_filters", plugin="depthai_filters::Detection2DOverlay", + name=name+"detection_overlay", remappings=[('rgb/preview/image_raw', name+'/nn/passthrough/image_raw'), - ('nn/detections', name+'/nn/detections')] + ('rgb/preview/camera_info', name+'/nn/passthrough/camera_info'), + ('nn/detections', name+'/nn/detections')], + parameters=[params_file] ), ], ), @@ -46,4 +49,4 @@ def generate_launch_description(): return LaunchDescription( declared_arguments + [OpaqueFunction(function=launch_setup)] - ) \ No newline at end of file + ) diff --git a/depthai_filters/package.xml b/depthai_filters/package.xml index 1890e840..25698c16 100644 --- a/depthai_filters/package.xml +++ b/depthai_filters/package.xml @@ -2,7 +2,7 @@ depthai_filters - 2.10.1 + 2.10.2 Depthai filters package Adam Serafin MIT diff --git a/depthai_filters/src/detection2d_overlay.cpp b/depthai_filters/src/detection2d_overlay.cpp index c6f667ab..baf52369 100644 --- a/depthai_filters/src/detection2d_overlay.cpp +++ b/depthai_filters/src/detection2d_overlay.cpp @@ -10,19 +10,42 @@ Detection2DOverlay::Detection2DOverlay(const rclcpp::NodeOptions& options) : rcl } void Detection2DOverlay::onInit() { previewSub.subscribe(this, "rgb/preview/image_raw"); + infoSub.subscribe(this, "rgb/preview/camera_info"); detSub.subscribe(this, "nn/detections"); - sync = std::make_unique>(syncPolicy(10), previewSub, detSub); - sync->registerCallback(std::bind(&Detection2DOverlay::overlayCB, this, std::placeholders::_1, std::placeholders::_2)); + sync = std::make_unique>(syncPolicy(10), previewSub, infoSub, detSub); + sync->registerCallback(std::bind(&Detection2DOverlay::overlayCB, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); overlayPub = this->create_publisher("overlay", 10); labelMap = this->declare_parameter>("label_map", labelMap); + desqueeze = this->declare_parameter("desqueeze", desqueeze); } void Detection2DOverlay::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& preview, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info, 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); + double ratioX = 1.0; + double ratioY = 1.0; + int offsetX = 0; + double offsetY = 0; + // if preview size is less than camera info size + if(previewMat.rows < info->height || previewMat.cols < info->width) { + ratioY = double(info->height) / double(previewMat.rows); + if(desqueeze) { + ratioX = double(info->width) / double(previewMat.cols); + } else { + ratioX = ratioY; + offsetX = (info->width - info->height) / 2.0; + } + } else { + ratioY = double(previewMat.rows) / double(info->height); + if(desqueeze) { + ratioX = double(previewMat.cols) / double(info->width); + } else { + ratioX = double(previewMat.cols) / double(info->width); + } + } for(auto& detection : detections->detections) { auto x1 = detection.bbox.center.position.x - detections->detections[0].bbox.size_x / 2.0; auto x2 = detection.bbox.center.position.x + detections->detections[0].bbox.size_x / 2.0; @@ -30,6 +53,10 @@ void Detection2DOverlay::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr auto y2 = detection.bbox.center.position.y + detections->detections[0].bbox.size_y / 2.0; auto labelStr = labelMap[stoi(detection.results[0].hypothesis.class_id)]; auto confidence = detection.results[0].hypothesis.score; + x1 = x1 * ratioX + offsetX; + x2 = x2 * ratioX + offsetX; + y1 = y1 * ratioY + offsetY; + y2 = y2 * ratioY + offsetY; utils::addTextToFrame(previewMat, labelStr, x1 + 10, y1 + 20); std::stringstream confStr; confStr << std::fixed << std::setprecision(2) << confidence * 100; @@ -44,4 +71,4 @@ void Detection2DOverlay::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr } // namespace depthai_filters #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(depthai_filters::Detection2DOverlay); \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(depthai_filters::Detection2DOverlay); diff --git a/depthai_ros_driver/CMakeLists.txt b/depthai_ros_driver/CMakeLists.txt index 23bf624e..dcfc4c6b 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.1) +project(depthai_ros_driver VERSION 2.10.2) set(CMAKE_POSITION_INDEPENDENT_CODE ON) set(CMAKE_BUILD_SHARED_LIBS ON) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp index 3986f6c4..c343a876 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp @@ -84,9 +84,11 @@ class Detection : public BaseNode { convConf.updateROSBaseTimeOnRosMsg = ph->getParam("i_update_ros_base_time_on_ros_msg"); utils::ImgPublisherConfig pubConf; + pubConf.width = width; + pubConf.height = height; pubConf.daiNodeName = getName(); - pubConf.topicName = "~/" + getName(); - pubConf.topicSuffix = "passthrough"; + pubConf.topicName = "~/" + getName() + "/passthrough"; + pubConf.infoSuffix = "/passthrough"; pubConf.socket = static_cast(ph->getParam("i_board_socket_id")); ptPub->setup(device, convConf, pubConf); 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 f22d1986..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 @@ -76,8 +76,8 @@ class SpatialDetection : public BaseNode { pubConf.width = width; pubConf.height = height; pubConf.daiNodeName = getName(); - pubConf.topicName = "~/" + getName(); - pubConf.topicSuffix = "passthrough"; + pubConf.topicName = "~/" + getName() + "/passthrough"; + pubConf.infoSuffix = "/passthrough"; pubConf.socket = static_cast(ph->getParam("i_board_socket_id")); ptPub->setup(device, convConf, pubConf); @@ -97,8 +97,8 @@ class SpatialDetection : public BaseNode { pubConf.width = ph->getOtherNodeParam(sensor_helpers::getNodeName(getROSNode(), sensor_helpers::NodeNameEnum::Stereo), "i_width"); pubConf.height = ph->getOtherNodeParam(sensor_helpers::getNodeName(getROSNode(), sensor_helpers::NodeNameEnum::Stereo), "i_height"); pubConf.daiNodeName = getName(); - pubConf.topicName = "~/" + getName(); - pubConf.topicSuffix = "/passthrough_depth"; + pubConf.topicName = "~/" + getName() + "/passthrough_depth"; + pubConf.infoSuffix = "/passthrough_depth"; pubConf.socket = socket; ptDepthPub->setup(device, convConf, pubConf); diff --git a/depthai_ros_driver/include/depthai_ros_driver/utils.hpp b/depthai_ros_driver/include/depthai_ros_driver/utils.hpp index c83444d7..ed01b594 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/utils.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/utils.hpp @@ -71,6 +71,7 @@ struct ImgPublisherConfig { dai::CameraBoardSocket rightSocket = dai::CameraBoardSocket::CAM_C; std::string calibrationFile = ""; std::string topicSuffix = "/image_raw"; + std::string infoSuffix = ""; std::string compressedTopicSuffix = "/image_raw/compressed"; std::string infoMgrSuffix = ""; bool rectified = false; diff --git a/depthai_ros_driver/package.xml b/depthai_ros_driver/package.xml index d17a5ac7..dca5ce86 100644 --- a/depthai_ros_driver/package.xml +++ b/depthai_ros_driver/package.xml @@ -2,7 +2,7 @@ depthai_ros_driver - 2.10.1 + 2.10.2 Depthai ROS Monolithic node. Adam Serafin diff --git a/depthai_ros_driver/src/camera.cpp b/depthai_ros_driver/src/camera.cpp index 574ef7bd..69d77088 100644 --- a/depthai_ros_driver/src/camera.cpp +++ b/depthai_ros_driver/src/camera.cpp @@ -76,20 +76,11 @@ void Camera::diagCB(const diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg) } void Camera::start() { - 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; - } + RCLCPP_INFO(this->get_logger(), "Starting camera."); + if(!camRunning) { + onConfigure(); + } else { + RCLCPP_INFO(this->get_logger(), "Camera already running!."); } } 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 02e0e9c1..2d861911 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp @@ -69,10 +69,12 @@ void ImagePublisher::setup(std::shared_ptr device, const utils::Img ffmpegPub = node->create_publisher( pubConfig.topicName + pubConfig.compressedTopicSuffix, rclcpp::QoS(10), pubOptions); } - infoPub = node->create_publisher(pubConfig.topicName + "/camera_info", rclcpp::QoS(10), pubOptions); + infoPub = + node->create_publisher(pubConfig.topicName + pubConfig.infoSuffix + "/camera_info", rclcpp::QoS(10), pubOptions); } else if(ipcEnabled) { imgPub = node->create_publisher(pubConfig.topicName + pubConfig.topicSuffix, rclcpp::QoS(10), pubOptions); - infoPub = node->create_publisher(pubConfig.topicName + "/camera_info", rclcpp::QoS(10), pubOptions); + infoPub = + node->create_publisher(pubConfig.topicName + pubConfig.infoSuffix + "/camera_info", rclcpp::QoS(10), pubOptions); } else { imgPubIT = image_transport::create_camera_publisher(node.get(), pubConfig.topicName + pubConfig.topicSuffix); } @@ -124,7 +126,6 @@ void ImagePublisher::createInfoManager(std::shared_ptr device) { auto info = sensor_helpers::getCalibInfo(node->get_logger(), converter, device, pubConfig.socket, pubConfig.width, pubConfig.height); if(pubConfig.rectified) { std::fill(info.d.begin(), info.d.end(), 0.0); - std::fill(info.k.begin(), info.k.end(), 0.0); info.r[0] = info.r[4] = info.r[8] = 1.0; } infoManager->setCameraInfo(info); diff --git a/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp b/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp index 6693d579..1bd98b0b 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp @@ -33,9 +33,9 @@ SensorWrapper::SensorWrapper(const std::string& daiNodeName, converter = std::make_unique(true); setNames(); setXinXout(pipeline); - socketID = ph->getParam("i_board_socket_id"); } + socketID = ph->getParam("i_board_socket_id"); if(ph->getParam("i_disable_node") && ph->getParam("i_simulate_from_topic")) { RCLCPP_INFO(getROSNode()->get_logger(), "Disabling node %s, pipeline data taken from topic.", getName().c_str()); } else { 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 23cc3866..0b9fe0dd 100644 --- a/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp @@ -30,7 +30,7 @@ void SensorParamHandler::declareCommonParams(dai::CameraBoardSocket socket) { declareAndLogParam("i_simulated_topic_name", ""); declareAndLogParam("i_disable_node", false); declareAndLogParam("i_get_base_device_timestamp", false); - socketID = static_cast(declareAndLogParam("i_board_socket_id", static_cast(socket), 0)); + socketID = static_cast(declareAndLogParam("i_board_socket_id", static_cast(socket), false)); declareAndLogParam("i_update_ros_base_time_on_ros_msg", false); declareAndLogParam("i_enable_feature_tracker", false); declareAndLogParam("i_enable_nn", false); diff --git a/depthai_ros_msgs/CMakeLists.txt b/depthai_ros_msgs/CMakeLists.txt index 8c3b48bc..45aec171 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.1) +project(depthai_ros_msgs VERSION 2.10.2) if(POLICY CMP0057) cmake_policy(SET CMP0057 NEW) diff --git a/depthai_ros_msgs/package.xml b/depthai_ros_msgs/package.xml index 2803de5e..615b9272 100644 --- a/depthai_ros_msgs/package.xml +++ b/depthai_ros_msgs/package.xml @@ -1,7 +1,7 @@ depthai_ros_msgs - 2.10.1 + 2.10.2 Package to keep interface independent of the driver Adam Serafin