From a5e7b641e3a0a5ab435a689745c0510fd7571eaa Mon Sep 17 00:00:00 2001 From: Adam Serafin Date: Mon, 26 Jun 2023 16:21:50 +0200 Subject: [PATCH] Add spatial bounding boxes in Noetic (#338) --- depthai-ros/CHANGELOG.rst | 6 +- depthai-ros/CMakeLists.txt | 2 +- depthai-ros/package.xml | 2 +- depthai_bridge/CMakeLists.txt | 2 +- depthai_bridge/package.xml | 2 +- depthai_bridge/src/ImageConverter.cpp | 3 + depthai_bridge/src/ImuConverter.cpp | 3 + .../src/SpatialDetectionConverter.cpp | 4 +- depthai_descriptions/CMakeLists.txt | 2 +- depthai_descriptions/package.xml | 2 +- depthai_examples/CMakeLists.txt | 2 +- depthai_examples/package.xml | 2 +- depthai_filters/CMakeLists.txt | 7 +- depthai_filters/config/spatial_bb.yaml | 9 ++ .../depthai_filters/detection2d_overlay.hpp | 6 +- .../depthai_filters/segmentation_overlay.hpp | 6 +- .../include/depthai_filters/spatial_bb.hpp | 36 +++++ .../include/depthai_filters/utils.hpp | 1 + depthai_filters/launch/spatial_bb.launch | 19 +++ depthai_filters/nodelet_plugins.xml | 21 ++- depthai_filters/package.xml | 3 +- depthai_filters/src/detection2d_overlay.cpp | 1 + depthai_filters/src/segmentation_overlay.cpp | 1 + depthai_filters/src/spatial_bb.cpp | 151 ++++++++++++++++++ depthai_filters/src/utils.cpp | 7 + depthai_ros_driver/cfg/parameters.cfg | 10 ++ .../dai_nodes/nn/detection.hpp | 2 + .../dai_nodes/nn/spatial_detection.hpp | 3 + depthai_ros_driver/package.xml | 2 +- .../src/dai_nodes/sensors/imu.cpp | 2 +- .../src/dai_nodes/sensors/mono.cpp | 1 + .../src/dai_nodes/sensors/rgb.cpp | 1 + depthai_ros_driver/src/dai_nodes/stereo.cpp | 4 +- .../param_handlers/stereo_param_handler.cpp | 7 +- depthai_ros_msgs/CMakeLists.txt | 2 +- depthai_ros_msgs/package.xml | 2 +- 36 files changed, 299 insertions(+), 37 deletions(-) create mode 100644 depthai_filters/config/spatial_bb.yaml create mode 100644 depthai_filters/include/depthai_filters/spatial_bb.hpp create mode 100644 depthai_filters/launch/spatial_bb.launch create mode 100644 depthai_filters/src/spatial_bb.cpp diff --git a/depthai-ros/CHANGELOG.rst b/depthai-ros/CHANGELOG.rst index 14279097..edb4acb9 100644 --- a/depthai-ros/CHANGELOG.rst +++ b/depthai-ros/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package depthai-ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.4 (2023-06-26) +------------------- +* ROS time update +* Minor bugfixes + 2.7.3 (2023-06-16) ------------------- * Pipeline generation as a plugin @@ -10,7 +15,6 @@ Changelog for package depthai-ros * Camera trigger mechanisms * Brightness filter - 2.7.2 (2023-5-08) ------------------- * IMU improvements diff --git a/depthai-ros/CMakeLists.txt b/depthai-ros/CMakeLists.txt index a091d06b..f57eb7fa 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.7.3 LANGUAGES CXX C) +project(depthai-ros VERSION 2.7.4 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) diff --git a/depthai-ros/package.xml b/depthai-ros/package.xml index b3dc50a0..7d23703a 100644 --- a/depthai-ros/package.xml +++ b/depthai-ros/package.xml @@ -1,7 +1,7 @@ depthai-ros - 2.7.3 + 2.7.4 The depthai-ros package diff --git a/depthai_bridge/CMakeLists.txt b/depthai_bridge/CMakeLists.txt index de561eef..cb73f9ee 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.7.3 LANGUAGES CXX C) +project(depthai_bridge VERSION 2.7.4 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 b78d2744..2f0a979d 100644 --- a/depthai_bridge/package.xml +++ b/depthai_bridge/package.xml @@ -1,7 +1,7 @@ depthai_bridge - 2.7.3 + 2.7.4 The depthai_bridge package Sachin Guruswamy diff --git a/depthai_bridge/src/ImageConverter.cpp b/depthai_bridge/src/ImageConverter.cpp index c989dc61..1fbd6242 100644 --- a/depthai_bridge/src/ImageConverter.cpp +++ b/depthai_bridge/src/ImageConverter.cpp @@ -42,6 +42,9 @@ void ImageConverter::toRosMsgFromBitStream(std::shared_ptr inData std::deque& outImageMsgs, dai::RawImgFrame::Type type, const sensor_msgs::CameraInfo& info) { + if(_updateRosBaseTimeOnToRosMsg) { + updateRosBaseTime(); + } std::chrono::_V2::steady_clock::time_point tstamp; if(_getBaseDeviceTimestamp) tstamp = inData->getTimestampDevice(); diff --git a/depthai_bridge/src/ImuConverter.cpp b/depthai_bridge/src/ImuConverter.cpp index ad340822..326bced1 100644 --- a/depthai_bridge/src/ImuConverter.cpp +++ b/depthai_bridge/src/ImuConverter.cpp @@ -113,6 +113,9 @@ void ImuConverter::toRosMsg(std::shared_ptr inData, std::deque inData, std::deque& outImuMsgs) { + if(_updateRosBaseTimeOnToRosMsg) { + updateRosBaseTime(); + } if(_syncMode != ImuSyncMethod::COPY) { FillImuData_LinearInterpolation(inData->packets, outImuMsgs); } else { diff --git a/depthai_bridge/src/SpatialDetectionConverter.cpp b/depthai_bridge/src/SpatialDetectionConverter.cpp index 6b170cbc..fa796115 100644 --- a/depthai_bridge/src/SpatialDetectionConverter.cpp +++ b/depthai_bridge/src/SpatialDetectionConverter.cpp @@ -77,7 +77,9 @@ void SpatialDetectionConverter::toRosMsg(std::shared_ptr inNetData, std::deque& opDetectionMsgs) { - // setting the header + if(_updateRosBaseTimeOnToRosMsg) { + updateRosBaseTime(); + } std::chrono::_V2::steady_clock::time_point tstamp; if(_getBaseDeviceTimestamp) tstamp = inNetData->getTimestampDevice(); diff --git a/depthai_descriptions/CMakeLists.txt b/depthai_descriptions/CMakeLists.txt index 518dfd66..12b551b9 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.7.3 LANGUAGES CXX C) +project(depthai_descriptions VERSION 2.7.4 LANGUAGES CXX C) find_package(catkin REQUIRED diff --git a/depthai_descriptions/package.xml b/depthai_descriptions/package.xml index 854c8372..188540dd 100644 --- a/depthai_descriptions/package.xml +++ b/depthai_descriptions/package.xml @@ -1,7 +1,7 @@ depthai_descriptions - 2.7.3 + 2.7.4 The depthai_descriptions package Adam Serafin diff --git a/depthai_examples/CMakeLists.txt b/depthai_examples/CMakeLists.txt index 52371849..38166a8f 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.7.3 LANGUAGES CXX C) +project(depthai_examples VERSION 2.7.4 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 e18ff31b..f027e60c 100644 --- a/depthai_examples/package.xml +++ b/depthai_examples/package.xml @@ -1,7 +1,7 @@ depthai_examples - 2.7.3 + 2.7.4 The depthai_examples package diff --git a/depthai_filters/CMakeLists.txt b/depthai_filters/CMakeLists.txt index 62de84f1..c7009179 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.7.3 LANGUAGES CXX C) +project(depthai_filters VERSION 2.7.4 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) @@ -22,7 +22,7 @@ endif() ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure message_filters sensor_msgs vision_msgs cv_bridge image_transport nodelet) +find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure message_filters sensor_msgs vision_msgs visualization_msgs cv_bridge image_transport nodelet) ## Generate dynamic reconfigure parameters in the 'cfg' folder @@ -34,7 +34,7 @@ generate_dynamic_reconfigure_options( catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS roscpp dynamic_reconfigure message_filters sensor_msgs vision_msgs cv_bridge image_transport nodelet + CATKIN_DEPENDS roscpp dynamic_reconfigure message_filters sensor_msgs vision_msgs visualization_msgs cv_bridge image_transport nodelet DEPENDS OpenCV ) @@ -55,6 +55,7 @@ add_library(${PROJECT_NAME} SHARED src/detection2d_overlay.cpp src/segmentation_overlay.cpp src/wls_filter.cpp + src/spatial_bb.cpp src/utils.cpp ) diff --git a/depthai_filters/config/spatial_bb.yaml b/depthai_filters/config/spatial_bb.yaml new file mode 100644 index 00000000..a36b6e35 --- /dev/null +++ b/depthai_filters/config/spatial_bb.yaml @@ -0,0 +1,9 @@ +/oak: + camera_i_nn_type: spatial + rgb_i_enable_preview: true + rgb_i_keep_preview_aspect_ratio: false + nn_i_enable_passthrough: true + nn_i_disable_resize: true + stereo_i_subpixel: true +/spatial_bb_node: + desqueeze: true diff --git a/depthai_filters/include/depthai_filters/detection2d_overlay.hpp b/depthai_filters/include/depthai_filters/detection2d_overlay.hpp index b2277df2..2c3f26e2 100644 --- a/depthai_filters/include/depthai_filters/detection2d_overlay.hpp +++ b/depthai_filters/include/depthai_filters/detection2d_overlay.hpp @@ -22,8 +22,8 @@ class Detection2DOverlay : public nodelet::Nodelet { typedef message_filters::sync_policies::ApproximateTime syncPolicy; std::unique_ptr> sync; ros::Publisher overlayPub; - const std::vector labelMap = {"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", - "car", "cat", "chair", "cow", "diningtable", "dog", "horse", - "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; + std::vector labelMap = {"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", + "car", "cat", "chair", "cow", "diningtable", "dog", "horse", + "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; }; } // namespace depthai_filters \ No newline at end of file diff --git a/depthai_filters/include/depthai_filters/segmentation_overlay.hpp b/depthai_filters/include/depthai_filters/segmentation_overlay.hpp index b788bc78..63fb9ded 100644 --- a/depthai_filters/include/depthai_filters/segmentation_overlay.hpp +++ b/depthai_filters/include/depthai_filters/segmentation_overlay.hpp @@ -21,8 +21,8 @@ class SegmentationOverlay : public nodelet::Nodelet { typedef message_filters::sync_policies::ApproximateTime syncPolicy; std::unique_ptr> sync; ros::Publisher overlayPub; - const std::vector labelMap = {"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", - "car", "cat", "chair", "cow", "diningtable", "dog", "horse", - "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; + std::vector labelMap = {"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", + "car", "cat", "chair", "cow", "diningtable", "dog", "horse", + "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; }; } // namespace depthai_filters \ No newline at end of file diff --git a/depthai_filters/include/depthai_filters/spatial_bb.hpp b/depthai_filters/include/depthai_filters/spatial_bb.hpp new file mode 100644 index 00000000..e5be4949 --- /dev/null +++ b/depthai_filters/include/depthai_filters/spatial_bb.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include "image_transport/image_transport.h" +#include "message_filters/subscriber.h" +#include "message_filters/sync_policies/approximate_time.h" +#include "message_filters/synchronizer.h" +#include "nodelet/nodelet.h" +#include "ros/ros.h" +#include "sensor_msgs/CameraInfo.h" +#include "sensor_msgs/Image.h" +#include "vision_msgs/Detection3DArray.h" +#include "visualization_msgs/MarkerArray.h" + +namespace depthai_filters { +class SpatialBB : public nodelet::Nodelet { + public: + void onInit() override; + + void overlayCB(const sensor_msgs::ImageConstPtr& preview, + const sensor_msgs::CameraInfoConstPtr& info, + const vision_msgs::Detection3DArrayConstPtr& detections); + + message_filters::Subscriber previewSub; + message_filters::Subscriber infoSub; + message_filters::Subscriber detSub; + + typedef message_filters::sync_policies::ApproximateTime syncPolicy; + std::unique_ptr> sync; + ros::Publisher overlayPub; + ros::Publisher markerPub; + 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 diff --git a/depthai_filters/include/depthai_filters/utils.hpp b/depthai_filters/include/depthai_filters/utils.hpp index 02be9357..11cc1df4 100644 --- a/depthai_filters/include/depthai_filters/utils.hpp +++ b/depthai_filters/include/depthai_filters/utils.hpp @@ -6,5 +6,6 @@ namespace depthai_filters { namespace utils { cv::Mat msgToMat(const sensor_msgs::ImageConstPtr& img, const std::string& encoding); +void addTextToFrame(cv::Mat& frame, const std::string& text, int x, int y); } // namespace utils } // namespace depthai_filters \ No newline at end of file diff --git a/depthai_filters/launch/spatial_bb.launch b/depthai_filters/launch/spatial_bb.launch new file mode 100644 index 00000000..7d816a9c --- /dev/null +++ b/depthai_filters/launch/spatial_bb.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/depthai_filters/nodelet_plugins.xml b/depthai_filters/nodelet_plugins.xml index 384fc673..4146322e 100644 --- a/depthai_filters/nodelet_plugins.xml +++ b/depthai_filters/nodelet_plugins.xml @@ -1,17 +1,22 @@ - + Detection 2D Overlay. - + - - + + Segmentation Overlay. - + - - + + WLS Filter. - + + + + + Spatial Bounding Box publisher. + \ No newline at end of file diff --git a/depthai_filters/package.xml b/depthai_filters/package.xml index 14ea7fa7..5e869716 100644 --- a/depthai_filters/package.xml +++ b/depthai_filters/package.xml @@ -1,7 +1,7 @@ depthai_filters - 2.7.3 + 2.7.4 The depthai_filters package Adam Serafin @@ -20,6 +20,7 @@ image_pipeline nodelet libopencv-dev + visualization_msgs diff --git a/depthai_filters/src/detection2d_overlay.cpp b/depthai_filters/src/detection2d_overlay.cpp index a353c5d3..dbc1876a 100644 --- a/depthai_filters/src/detection2d_overlay.cpp +++ b/depthai_filters/src/detection2d_overlay.cpp @@ -14,6 +14,7 @@ void Detection2DOverlay::onInit() { previewSub.subscribe(pNH, "/rgb/preview/image_raw", 1); detSub.subscribe(pNH, "/nn/detections", 1); sync = std::make_unique>(syncPolicy(10), previewSub, detSub); + pNH.getParam("label_map", labelMap); sync->registerCallback(std::bind(&Detection2DOverlay::overlayCB, this, std::placeholders::_1, std::placeholders::_2)); overlayPub = pNH.advertise("overlay", 10); } diff --git a/depthai_filters/src/segmentation_overlay.cpp b/depthai_filters/src/segmentation_overlay.cpp index e1b082a5..e9fd3fb3 100644 --- a/depthai_filters/src/segmentation_overlay.cpp +++ b/depthai_filters/src/segmentation_overlay.cpp @@ -13,6 +13,7 @@ void SegmentationOverlay::onInit() { previewSub.subscribe(pNH, "/rgb/preview/image_raw", 1); segSub.subscribe(pNH, "/nn/image_raw", 1); sync = std::make_unique>(syncPolicy(10), previewSub, segSub); + pNH.getParam("label_map", labelMap); sync->registerCallback(std::bind(&SegmentationOverlay::overlayCB, this, std::placeholders::_1, std::placeholders::_2)); overlayPub = pNH.advertise("overlay", 10); } diff --git a/depthai_filters/src/spatial_bb.cpp b/depthai_filters/src/spatial_bb.cpp new file mode 100644 index 00000000..902fa825 --- /dev/null +++ b/depthai_filters/src/spatial_bb.cpp @@ -0,0 +1,151 @@ +#include "depthai_filters/spatial_bb.hpp" + +#include +#include + +#include "cv_bridge/cv_bridge.h" +#include "depthai_filters/utils.hpp" +#include "geometry_msgs/Point32.h" +#include "nodelet/nodelet.h" +#include "pluginlib/class_list_macros.h" + +namespace depthai_filters { +void SpatialBB::onInit() { + auto pNH = getPrivateNodeHandle(); + previewSub.subscribe(pNH, "/rgb/preview/image_raw", 1); + infoSub.subscribe(pNH, "/stereo/camera_info", 1); + detSub.subscribe(pNH, "/nn/spatial_detections", 1); + sync = std::make_unique>(syncPolicy(10), previewSub, infoSub, detSub); + pNH.getParam("label_map", labelMap); + pNH.getParam("desqueeze", desqueeze); + sync->registerCallback(std::bind(&SpatialBB::overlayCB, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + overlayPub = pNH.advertise("overlay", 10); + markerPub = pNH.advertise("spatial_bb", 10); +} + +void SpatialBB::overlayCB(const sensor_msgs::ImageConstPtr& preview, + const sensor_msgs::CameraInfoConstPtr& info, + const vision_msgs::Detection3DArrayConstPtr& detections) { + cv::Mat previewMat = utils::msgToMat(preview, sensor_msgs::image_encodings::BGR8); + auto blue = cv::Scalar(255, 0, 0); + + double ratioY = double(info->height) / double(previewMat.rows); + double ratioX; + int offsetX; + if(desqueeze) { + ratioX = double(info->width) / double(previewMat.cols); + offsetX = 0; + } else { + ratioX = ratioY; + offsetX = (info->width - info->height) / 2.0; + } + int offsetY = 0; + visualization_msgs::MarkerArray marker_array; + double fx = info->K[0]; + double fy = info->K[4]; + double cx = info->K[2]; + double cy = info->K[5]; + int id = 0; + 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; + auto y1 = detection.bbox.center.position.y - detections->detections[0].bbox.size.y / 2.0; + auto y2 = detection.bbox.center.position.y + detections->detections[0].bbox.size.y / 2.0; + + cv::rectangle(previewMat, cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)), blue); + auto labelStr = labelMap[detection.results[0].id]; + utils::addTextToFrame(previewMat, labelStr, x1 + 10, y1 + 10); + auto confidence = detection.results[0].score; + std::stringstream confStr; + confStr << std::fixed << std::setprecision(2) << confidence * 100; + utils::addTextToFrame(previewMat, confStr.str(), x1 + 10, y1 + 40); + + std::stringstream depthX; + depthX << "X: " << detection.results[0].pose.pose.position.x << " mm"; + utils::addTextToFrame(previewMat, depthX.str(), x1 + 10, y1 + 60); + + std::stringstream depthY; + depthY << "Y: " << detection.results[0].pose.pose.position.y << " mm"; + utils::addTextToFrame(previewMat, depthY.str(), x1 + 10, y1 + 75); + std::stringstream depthZ; + depthZ << "Z: " << detection.results[0].pose.pose.position.z << " mm"; + utils::addTextToFrame(previewMat, depthZ.str(), x1 + 10, y1 + 90); + + // Marker publishing + const auto& bbox = detection.bbox; + auto bbox_size_x = bbox.size.x * ratioX; + auto bbox_size_y = bbox.size.y * ratioY; + + auto bbox_center_x = bbox.center.position.x * ratioX + offsetX; + auto bbox_center_y = bbox.center.position.y * ratioY + offsetY; + visualization_msgs::Marker box_marker; + box_marker.header.frame_id = info->header.frame_id; + box_marker.header.stamp = info->header.stamp; + box_marker.ns = "detections"; + box_marker.id = id++; + box_marker.type = visualization_msgs::Marker::LINE_STRIP; + box_marker.action = visualization_msgs::Marker::ADD; + + box_marker.scale.x = 0.05; // Line width + box_marker.color.g = 1.0; + box_marker.color.a = 1.0; + + // Define bbox corner points in depth image frame + geometry_msgs::Point32 corners[4]; + corners[0].x = bbox_center_x - bbox_size_x / 2.0; + corners[0].y = bbox_center_y - bbox_size_y / 2.0; + corners[1].x = bbox_center_x + bbox_size_x / 2.0; + corners[1].y = bbox_center_y - bbox_size_y / 2.0; + corners[2].x = bbox_center_x + bbox_size_x / 2.0; + corners[2].y = bbox_center_y + bbox_size_y / 2.0; + corners[3].x = bbox_center_x - bbox_size_x / 2.0; + corners[3].y = bbox_center_y + bbox_size_y / 2.0; + // The polygon points are a rectangle, so we need 5 points to close the loop + box_marker.points.resize(5); + for(int i = 0; i < 4; ++i) { + auto& point = corners[i]; + point.z = detection.results[0].pose.pose.position.z; + box_marker.points[i].x = (point.x - cx) * point.z / fx; + box_marker.points[i].y = (point.y - cy) * point.z / fy; + box_marker.points[i].z = point.z; // assuming depth_val is in millimeters + } + // Repeat the first point to close the loop + box_marker.points[4] = box_marker.points[0]; + marker_array.markers.push_back(box_marker); + + // Create a text marker for the label + visualization_msgs::Marker text_marker; + text_marker.header.frame_id = info->header.frame_id; + text_marker.header.stamp = info->header.stamp; + text_marker.ns = "detections_label"; + text_marker.id = id++; + text_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; + text_marker.action = visualization_msgs::Marker::ADD; + + text_marker.scale.z = 0.3; // Text size + text_marker.color.r = 1.0; + text_marker.color.g = 1.0; + text_marker.color.b = 1.0; + text_marker.color.a = 1.0; + + // Position the text above the bounding box + text_marker.pose.position.x = box_marker.points[0].x; + text_marker.pose.position.y = box_marker.points[0].y; + text_marker.pose.position.z = box_marker.points[0].z + 0.1; // Adjust this value to position the text above the box + + // Set the text to the detection label + text_marker.text = labelStr; + marker_array.markers.push_back(text_marker); + } + markerPub.publish(marker_array); + sensor_msgs::Image outMsg; + cv_bridge::CvImage(preview->header, sensor_msgs::image_encodings::BGR8, previewMat).toImageMsg(outMsg); + + overlayPub.publish(outMsg); +} +} // namespace depthai_filters + +#include + +// watch the capitalization carefully +PLUGINLIB_EXPORT_CLASS(depthai_filters::SpatialBB, nodelet::Nodelet) diff --git a/depthai_filters/src/utils.cpp b/depthai_filters/src/utils.cpp index f4c83fe5..a01e3ef3 100644 --- a/depthai_filters/src/utils.cpp +++ b/depthai_filters/src/utils.cpp @@ -16,5 +16,12 @@ cv::Mat msgToMat(const sensor_msgs::ImageConstPtr& img, const std::string& encod } return mat; } +void addTextToFrame(cv::Mat& frame, const std::string& text, int x, int y) { + auto white = cv::Scalar(255, 255, 255); + auto black = cv::Scalar(0, 0, 0); + + cv::putText(frame, text, cv::Point(x, y), cv::FONT_HERSHEY_TRIPLEX, 0.5, white, 3); + cv::putText(frame, text, cv::Point(x, y), cv::FONT_HERSHEY_TRIPLEX, 0.5, black); +} } // namespace utils } // namespace depthai_filters \ No newline at end of file diff --git a/depthai_ros_driver/cfg/parameters.cfg b/depthai_ros_driver/cfg/parameters.cfg index 84f55953..4d46e9c0 100755 --- a/depthai_ros_driver/cfg/parameters.cfg +++ b/depthai_ros_driver/cfg/parameters.cfg @@ -49,6 +49,7 @@ camera.add("left_i_get_base_device_timestamp", bool_t, 0, "Use base device times camera.add("left_i_isp_num", int_t, 0, "ISP scale numerator", 2) camera.add("left_i_isp_den", int_t, 0, "ISP scale denominator", 3) camera.add("left_i_output_isp", bool_t, 0, "Output ISP instead of video", True) +camera.add("left_i_update_ros_base_time_on_ros_msg", bool_t, 0 ,"Whether to update ROS time with new messages", False) camera.add("right_i_board_socket_id", int_t, 0, "Sensor board socket id", 2) camera.add("right_i_calibration_file", str_t, 0, "Path to calibration file", "") @@ -76,6 +77,7 @@ camera.add("right_i_get_base_device_timestamp", bool_t, 0, "Use base device time camera.add("right_i_isp_num", int_t, 0, "ISP scale numerator", 2) camera.add("right_i_isp_den", int_t, 0, "ISP scale denominator", 3) camera.add("right_i_output_isp", bool_t, 0, "Output ISP instead of video", False) +camera.add("right_i_update_ros_base_time_on_ros_msg", bool_t, 0 ,"Whether to update ROS time with new messages", False) camera.add("rgb_i_board_socket_id", int_t, 0, "Sensor board socket id", 0) @@ -106,6 +108,7 @@ camera.add("rgb_i_get_base_device_timestamp", bool_t, 0, "Use base device timest camera.add("rgb_i_isp_num", int_t, 0, "ISP scale numerator", 2) camera.add("rgb_i_isp_den", int_t, 0, "ISP scale denominator", 3) camera.add("rgb_i_output_isp", bool_t, 0, "Output ISP instead of video", False) +camera.add("rgb_i_update_ros_base_time_on_ros_msg", bool_t, 0 ,"Whether to update ROS time with new messages", False) camera.add("stereo_i_align_depth", bool_t, 0, "Whether to align depth to RGB", True) @@ -122,6 +125,8 @@ camera.add("stereo_i_brightness_filter_max_brightness", int_t, 0, "Brightness fi camera.add("stereo_i_enable_companding", bool_t, 0, "Enable companding", False) camera.add("stereo_i_enable_decimation_filter", bool_t, 0, "Enable decimation filter", False) camera.add("stereo_i_enable_distortion_correction", bool_t, 0, "Enable distortion correction", True) +camera.add("stereo_i_enable_speckle_filter", bool_t, 0, "Enable speckle filter", False) +camera.add("stereo_i_speckle_filter_max_range", int_t, 0, "Speckle filter max range", 50) camera.add("stereo_i_enable_spatial_filter", bool_t, 0, "Enable spatial filter", False) camera.add("stereo_i_enable_temporal_filter", bool_t, 0, "Enable temporal filter", False) camera.add("stereo_i_enable_threshold_filter", bool_t, 0, "Enable threshold filter", False) @@ -135,9 +140,11 @@ camera.add("stereo_i_max_q_size", int_t, 0, "Internal queue size", 30) camera.add("stereo_i_publish_left_rect", bool_t, 0, "Publish rectified left", False) camera.add("stereo_i_left_rect_low_bandwidth", bool_t, 0, "Low bandwidth for rectified left", False) camera.add("stereo_i_left_rect_low_bandwidth_quality", int_t, 0, "Low bandwidth left quality", 50) +camera.add("stereo_i_left_rect_update_ros_base_time_on_ros_msg", bool_t, 0 ,"Whether to update ROS time with new messages", False) camera.add("stereo_i_publish_right_rect", bool_t, 0, "Publish rectified right", False) camera.add("stereo_i_right_rect_low_bandwidth", bool_t, 0, "Low bandwidth for rectified right", False) camera.add("stereo_i_right_rect_low_bandwidth_quality", int_t, 0, "Low bandwidth right quality", 50) +camera.add("stereo_i_right_rect_update_ros_base_time_on_ros_msg", bool_t, 0 ,"Whether to update ROS time with new messages", False) camera.add("stereo_i_publish_topic", bool_t, 0, "Publish stereo topic", True) camera.add("stereo_i_rectify_edge_fill_color", int_t, 0, "Rectify edge fill color", 0) camera.add("stereo_i_spatial_filter_alpha", double_t, 0, "Spatial filter alpha", 0.5) @@ -158,11 +165,13 @@ camera.add("stereo_i_subpixel_fractional_bits", int_t, 0, "Subpixel fractional b camera.add("stereo_i_set_input_size", bool_t, 0, "Set stereo input size", False) camera.add("stereo_i_input_width", int_t, 0, "Input image width", 1280) camera.add("stereo_i_input_height", int_t, 0, "Input image height", 720) +camera.add("stereo_i_update_ros_base_time_on_ros_msg", bool_t, 0 ,"Whether to update ROS time with new messages", False) camera.add("nn_i_nn_config_path", str_t, 0, "NN JSON Config path", "depthai_ros_driver/mobilenet") camera.add("nn_i_disable_resize", bool_t, 0, "Disable ImageManip input", True) camera.add("nn_i_enable_passthrough", bool_t, 0, "Publish passthrough", False) camera.add("nn_i_enable_passthrough_depth", bool_t, 0, "Publish passthrough depth", False) +camera.add("nn_i_update_ros_base_time_on_ros_msg", bool_t, 0 ,"Whether to update ROS time with new messages", False) camera.add("imu_i_max_q_size", int_t, 0, "Internal queue size", 30) camera.add("imu_i_acc_freq", int_t, 0, "Accelerometer frequency", 500) @@ -179,6 +188,7 @@ camera.add("imu_i_message_type", str_t, 0, "ROS Publisher type", "IMU") camera.add("imu_i_rot_cov", double_t, 0, "Rotation covariance", -1.0) camera.add("imu_i_rot_freq", int_t, 0, "Rotation frequency", 100) camera.add("imu_i_sync_method", str_t, 0, "Imu sync method", "COPY") +camera.add("imu_i_update_ros_base_time_on_ros_msg", bool_t, 0 ,"Whether to update ROS time with new messages", False) 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 c4f7b873..c39a6967 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 @@ -55,12 +55,14 @@ class Detection : public BaseNode { } detConverter = std::make_unique( tfPrefix + "_camera_optical_frame", width, height, false, ph->getParam("i_get_base_device_timestamp")); + detConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); detPub = getROSNode().template advertise(getName() + "/detections", 10); nnQ->addCallback(std::bind(&Detection::detectionCB, this, std::placeholders::_1, std::placeholders::_2)); if(ph->getParam("i_enable_passthrough")) { ptQ = device->getOutputQueue(ptQName, ph->getParam("i_max_q_size"), false); imageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); + imageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); infoManager = std::make_shared(ros::NodeHandle(getROSNode(), getName()), "/" + getName()); infoManager->setCameraInfo(sensor_helpers::getCalibInfo(*imageConverter, device, dai::CameraBoardSocket::RGB, width, height)); 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 ae32431b..46175bbe 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 @@ -58,12 +58,14 @@ class SpatialDetection : public BaseNode { } detConverter = std::make_unique( tfPrefix + "_camera_optical_frame", width, height, false, ph->getParam("i_get_base_device_timestamp")); + detConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); nnQ->addCallback(std::bind(&SpatialDetection::spatialCB, this, std::placeholders::_1, std::placeholders::_2)); detPub = getROSNode().template advertise(getName() + "/spatial_detections", 10); if(ph->getParam("i_enable_passthrough")) { ptQ = device->getOutputQueue(ptQName, ph->getParam("i_max_q_size"), false); ptImageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); + ptImageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); ptInfoMan = std::make_shared(ros::NodeHandle(getROSNode(), getName()), "/" + getName()); ptInfoMan->setCameraInfo(sensor_helpers::getCalibInfo(*ptImageConverter, device, dai::CameraBoardSocket::RGB, width, height)); @@ -81,6 +83,7 @@ class SpatialDetection : public BaseNode { }; ptDepthQ = device->getOutputQueue(ptDepthQName, ph->getParam("i_max_q_size"), false); ptDepthImageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); + ptDepthImageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); ptDepthInfoMan = std::make_shared(ros::NodeHandle(getROSNode(), getName()), "/" + getName()); int width = ph->getOtherNodeParam("stereo", "i_width"); int height = ph->getOtherNodeParam("stereo", "i_height"); diff --git a/depthai_ros_driver/package.xml b/depthai_ros_driver/package.xml index c7374d09..30c133a8 100644 --- a/depthai_ros_driver/package.xml +++ b/depthai_ros_driver/package.xml @@ -2,7 +2,7 @@ depthai_ros_driver - 2.7.3 + 2.7.4 Depthai ROS Monolithic node. Adam Serafin Sachin Guruswamy diff --git a/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp b/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp index 1e0af79a..306b8c66 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp @@ -47,7 +47,7 @@ void Imu::setupQueues(std::shared_ptr device) { ph->getParam("i_rot_cov"), ph->getParam("i_mag_cov"), ph->getParam("i_enable_rotation")); - + imuConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); param_handlers::imu::ImuMsgType msgType = ph->getMsgType(); switch(msgType) { case param_handlers::imu::ImuMsgType::IMU: { diff --git a/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp b/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp index 41db4dca..07e6b8b3 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp @@ -59,6 +59,7 @@ void Mono::setupQueues(std::shared_ptr device) { monoQ = device->getOutputQueue(monoQName, ph->getParam("i_max_q_size"), false); auto tfPrefix = getTFPrefix(getName()); imageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); + imageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); monoPub = it.advertiseCamera(getName() + "/image_raw", 1); infoManager = std::make_shared(ros::NodeHandle(getROSNode(), getName()), "/" + getName()); if(ph->getParam("i_calibration_file").empty()) { diff --git a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp index da657a56..5257c630 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp @@ -72,6 +72,7 @@ void RGB::setupQueues(std::shared_ptr device) { auto tfPrefix = getTFPrefix(getName()); infoManager = std::make_shared(ros::NodeHandle(getROSNode(), getName()), "/" + getName()); imageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); + imageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); if(ph->getParam("i_calibration_file").empty()) { infoManager->setCameraInfo(sensor_helpers::getCalibInfo(*imageConverter, device, diff --git a/depthai_ros_driver/src/dai_nodes/stereo.cpp b/depthai_ros_driver/src/dai_nodes/stereo.cpp index 534d924c..7c358926 100644 --- a/depthai_ros_driver/src/dai_nodes/stereo.cpp +++ b/depthai_ros_driver/src/dai_nodes/stereo.cpp @@ -90,6 +90,7 @@ void Stereo::setupLeftRectQueue(std::shared_ptr device) { leftRectQ = device->getOutputQueue(leftRectQName, ph->getOtherNodeParam(leftSensInfo.name, "i_max_q_size"), false); auto tfPrefix = getTFPrefix(leftSensInfo.name); leftRectConv = std::make_unique(tfPrefix + "_camera_optical_frame", false, ph->getParam("i_get_base_device_timestamp")); + leftRectConv->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_left_rect_update_ros_base_time_on_ros_msg")); leftRectIM = std::make_shared(ros::NodeHandle(getROSNode(), leftSensInfo.name), "/" + leftSensInfo.name + "/rect"); auto info = sensor_helpers::getCalibInfo(*leftRectConv, device, @@ -114,6 +115,7 @@ void Stereo::setupRightRectQueue(std::shared_ptr device) { rightRectQ = device->getOutputQueue(rightRectQName, ph->getOtherNodeParam(rightSensInfo.name, "i_max_q_size"), false); auto tfPrefix = getTFPrefix(rightSensInfo.name); rightRectConv = std::make_unique(tfPrefix + "_camera_optical_frame", false, ph->getParam("i_get_base_device_timestamp")); + rightRectConv->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_right_rect_update_ros_base_time_on_ros_msg")); rightRectIM = std::make_shared(ros::NodeHandle(getROSNode(), rightSensInfo.name), "/" + rightSensInfo.name + "/rect"); auto info = sensor_helpers::getCalibInfo(*rightRectConv, @@ -144,7 +146,7 @@ void Stereo::setupStereoQueue(std::shared_ptr device) { tfPrefix = getTFPrefix(rightSensInfo.name); } stereoConv = std::make_unique(tfPrefix + "_camera_optical_frame", false); - + stereoConv->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); stereoPub = it.advertiseCamera(getName() + "/image_raw", 1); stereoIM = std::make_shared(ros::NodeHandle(getROSNode(), getName()), "/" + getName()); auto info = sensor_helpers::getCalibInfo(*stereoConv, 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 0f96b1ea..4439f570 100644 --- a/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp @@ -71,9 +71,8 @@ void StereoParamHandler::declareParams(std::shared_ptr s stereo->setRectifyEdgeFillColor(getParam("i_rectify_edge_fill_color")); auto config = stereo->initialConfig.get(); config.costMatching.disparityWidth = utils::getValFromMap(getParam("i_disparity_width", "DISPARITY_96"), disparityWidthMap); - if(!config.algorithmControl.enableExtended) { - config.costMatching.enableCompanding = getParam("i_enable_companding", false); - } + stereo->setExtendedDisparity(getParam("i_extended_disp")); + config.costMatching.enableCompanding = getParam("i_enable_companding", false); config.postProcessing.temporalFilter.enable = getParam("i_enable_temporal_filter"); if(config.postProcessing.temporalFilter.enable) { config.postProcessing.temporalFilter.alpha = getParam("i_temporal_filter_alpha"); @@ -81,8 +80,8 @@ void StereoParamHandler::declareParams(std::shared_ptr s config.postProcessing.temporalFilter.persistencyMode = utils::getValFromMap(getParam("i_temporal_filter_persistency"), temporalPersistencyMap); } + config.postProcessing.speckleFilter.enable = getParam("i_enable_speckle_filter"); if(config.postProcessing.speckleFilter.enable) { - config.postProcessing.speckleFilter.enable = getParam("i_enable_speckle_filter"); config.postProcessing.speckleFilter.speckleRange = getParam("i_speckle_filter_speckle_range"); } config.postProcessing.spatialFilter.enable = getParam("i_enable_spatial_filter"); diff --git a/depthai_ros_msgs/CMakeLists.txt b/depthai_ros_msgs/CMakeLists.txt index bcd37c14..20e4916b 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.7.3) +project (depthai_ros_msgs VERSION 2.7.4) if(POLICY CMP0057) cmake_policy(SET CMP0057 NEW) diff --git a/depthai_ros_msgs/package.xml b/depthai_ros_msgs/package.xml index 1b1681f2..a206ab5f 100644 --- a/depthai_ros_msgs/package.xml +++ b/depthai_ros_msgs/package.xml @@ -1,7 +1,7 @@ depthai_ros_msgs - 2.7.3 + 2.7.4 Package to keep interface independent of the driver Sachin Guruswamy