From e1c132c060e2d3ce0072502ccf03b4b3f57289c5 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Mon, 13 Feb 2023 16:23:47 +0000 Subject: [PATCH 1/6] calibration port --- README.md | 13 ++- depthai-ros/CHANGELOG.rst | 4 + 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 | 12 +-- depthai_examples/CMakeLists.txt | 2 +- depthai_examples/package.xml | 2 +- .../src/stereo_inertial_publisher.cpp | 15 +-- depthai_ros_driver/CMakeLists.txt | 3 +- .../config/calibration/left.yaml | 26 ++++++ .../config/calibration/rgb.yaml | 26 ++++++ .../config/calibration/right.yaml | 26 ++++++ .../config/rviz/calibration.yaml | 8 ++ .../dai_nodes/base_node.hpp | 3 + .../dai_nodes/sensors/mono.hpp | 4 +- .../dai_nodes/sensors/rgb.hpp | 4 +- .../dai_nodes/sensors/sensor_helpers.hpp | 27 ++++++ .../depthai_ros_driver/dai_nodes/stereo.hpp | 5 +- .../launch/calibration.launch.py | 80 ++++++++++++++++ depthai_ros_driver/package.xml | 3 +- depthai_ros_driver/src/camera.cpp | 4 +- .../src/dai_nodes/sensors/mono.cpp | 54 +++++------ .../src/dai_nodes/sensors/rgb.cpp | 93 +++++++++---------- .../src/dai_nodes/sensors/sensor_helpers.cpp | 65 +++++++++++++ depthai_ros_driver/src/dai_nodes/stereo.cpp | 67 +++++++------ .../src/param_handlers/mono_param_handler.cpp | 1 + .../src/param_handlers/rgb_param_handler.cpp | 1 + .../param_handlers/stereo_param_handler.cpp | 2 +- depthai_ros_msgs/CMakeLists.txt | 2 +- depthai_ros_msgs/package.xml | 2 +- 32 files changed, 423 insertions(+), 139 deletions(-) create mode 100644 depthai_ros_driver/config/calibration/left.yaml create mode 100644 depthai_ros_driver/config/calibration/rgb.yaml create mode 100644 depthai_ros_driver/config/calibration/right.yaml create mode 100644 depthai_ros_driver/config/rviz/calibration.yaml create mode 100644 depthai_ros_driver/launch/calibration.launch.py create mode 100644 depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp diff --git a/README.md b/README.md index e9e591ed..ffba31a3 100644 --- a/README.md +++ b/README.md @@ -92,7 +92,7 @@ The following setup procedure assumes you have cmake version >= 3.10.2 and OpenC 4. `cd ../..` 5. `rosdep install --from-paths src --ignore-src -r -y` 6. `source /opt/ros//setup.bash` -7. `catkin_make` (For ROS1) `colcon build` (for ROS2) +7. `catkin_make_isolated` (For ROS1) `MAKEFLAGS="-j1 -l1" colcon build` (for ROS2) 8. `source devel/setup.bash` (For ROS1) & `source install/setup.bash` (for ROS2) **Note** If you are using a lower end PC or RPi, standard building may take a lot of RAM and clog your PC. To avoid that, you can use `build.sh` command from your workspace (it just wraps colcon commands): @@ -156,6 +156,17 @@ See `low_bandwidth.yaml` file for example parameters for all streams ##### **OAK D PRO W** To properly align with depth, you need to set `rgb.i_resolution` parameter to `720` (see `config/oak_d_w_pro.yaml`). +#### Recalibration +If you want to use other calibration values than the ones provided by the device, you can do it in following ways: +* Use `set_camera_info` services available for each of the image streams +* Use `i_calibration_file` parameter available to point to the calibration file. **Note** camera name must start with `/`, so for example `/rgb`. See `depthai_ros_driver/config/calibration` for example calibration files. `calibration.launch` file is provided to start up a ROS camera calibrator node in both monocular and stereo configurations. +Calibration file syntax (from `camera_info_manager`): +``` + - file:///full/path/to/local/file.yaml + - file:///full/path/to/videre/file.ini + - package://camera_info_manager/tests/test_calibration.yaml + - package://ros_package_name/calibrations/camera3.yaml +``` ## Executing an example ### ROS1 diff --git a/depthai-ros/CHANGELOG.rst b/depthai-ros/CHANGELOG.rst index 49d20422..840212d6 100644 --- a/depthai-ros/CHANGELOG.rst +++ b/depthai-ros/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package depthai-ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.6.3 (2023-02-10) +___________ +* Camera calibration updates + 2.6.2 (2023-02-01) ___________ * Fixed timestamp in SpatialDetector diff --git a/depthai-ros/CMakeLists.txt b/depthai-ros/CMakeLists.txt index e880558e..53efdd07 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.6.2 LANGUAGES CXX C) +project(depthai-ros VERSION 2.6.3 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) diff --git a/depthai-ros/package.xml b/depthai-ros/package.xml index 35701c13..e8d7ec62 100644 --- a/depthai-ros/package.xml +++ b/depthai-ros/package.xml @@ -1,7 +1,7 @@ depthai-ros - 2.6.2 + 2.6.3 The depthai-ros package diff --git a/depthai_bridge/CMakeLists.txt b/depthai_bridge/CMakeLists.txt index 070fb808..b7820313 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.6.2 LANGUAGES CXX C) +project(depthai_bridge VERSION 2.6.3 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 7342a744..d7d6254c 100644 --- a/depthai_bridge/package.xml +++ b/depthai_bridge/package.xml @@ -1,7 +1,7 @@ depthai_bridge - 2.6.2 + 2.6.3 The depthai_bridge package Sachin Guruswamy diff --git a/depthai_bridge/src/ImageConverter.cpp b/depthai_bridge/src/ImageConverter.cpp index 9ad662f8..9ce4da9d 100644 --- a/depthai_bridge/src/ImageConverter.cpp +++ b/depthai_bridge/src/ImageConverter.cpp @@ -209,13 +209,13 @@ void ImageConverter::toDaiMsg(const ImageMsgs::Image& inMsg, dai::ImgFrame& outD *ImageMsgs::Image& opMsg) to cross verify.. **/ /* #ifdef IS_ROS2 - TimePoint ts(std::chrono::seconds((int)inMsg.header.stamp.seconds ()) + std::chrono::nanoseconds(inMsg.header.stamp.nanoseconds())); - #else - TimePoint ts(std::chrono::seconds((int)inMsg.header.stamp.toSec()) + std::chrono::nanoseconds(inMsg.header.stamp.toNSec())); - #endif + TimePoint ts(std::chrono::seconds((int)inMsg.header.stamp.seconds ()) + std::chrono::nanoseconds(inMsg.header.stamp.nanoseconds())); + #else + TimePoint ts(std::chrono::seconds((int)inMsg.header.stamp.toSec()) + std::chrono::nanoseconds(inMsg.header.stamp.toNSec())); + #endif - outData.setTimestamp(ts); - outData.setSequenceNum(inMsg.header.seq); */ + outData.setTimestamp(ts); + outData.setSequenceNum(inMsg.header.seq); */ outData.setWidth(inMsg.width); outData.setHeight(inMsg.height); outData.setType(revEncodingIter->first); diff --git a/depthai_examples/CMakeLists.txt b/depthai_examples/CMakeLists.txt index 2dc456ca..cc455b12 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.6.2 LANGUAGES CXX C) +project(depthai_examples VERSION 2.6.3 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 6ae2a5ee..03bcb516 100644 --- a/depthai_examples/package.xml +++ b/depthai_examples/package.xml @@ -1,7 +1,7 @@ depthai_examples - 2.6.2 + 2.6.3 The depthai_examples package diff --git a/depthai_examples/src/stereo_inertial_publisher.cpp b/depthai_examples/src/stereo_inertial_publisher.cpp index ca648bf8..d1924be0 100644 --- a/depthai_examples/src/stereo_inertial_publisher.cpp +++ b/depthai_examples/src/stereo_inertial_publisher.cpp @@ -190,20 +190,21 @@ std::tuple createPipeline(bool enableDepth, } if(rgbWidth > stereoWidth || rgbHeight > stereoHeight) { - DEPTHAI_ROS_WARN_STREAM( - "DEPTHAI", - "RGB Camera resolution is heigher than the configured stereo resolution. Upscaling the stereo depth/disparity to match RGB camera resolution."); + DEPTHAI_ROS_WARN_STREAM("DEPTHAI", + "RGB Camera resolution is heigher than the configured stereo resolution. Upscaling the " + "stereo depth/disparity to match RGB camera resolution."); } else if(rgbWidth > stereoWidth || rgbHeight > stereoHeight) { DEPTHAI_ROS_WARN_STREAM("DEPTHAI", - "RGB Camera resolution is heigher than the configured stereo resolution. Downscaling the stereo depth/disparity to match " + "RGB Camera resolution is heigher than the configured stereo resolution. Downscaling the " + "stereo depth/disparity to match " "RGB camera resolution."); } if(enableSpatialDetection) { if(previewWidth > rgbWidth or previewHeight > rgbHeight) { - DEPTHAI_ROS_ERROR_STREAM( - "DEPTHAI", - "Preview Image size should be smaller than the scaled resolution. Please adjust the scale parameters or the preview size accordingly."); + DEPTHAI_ROS_ERROR_STREAM("DEPTHAI", + "Preview Image size should be smaller than the scaled resolution. Please adjust the " + "scale parameters or the preview size accordingly."); throw std::runtime_error("Invalid Image Size"); } diff --git a/depthai_ros_driver/CMakeLists.txt b/depthai_ros_driver/CMakeLists.txt index a2728844..af9c0c46 100644 --- a/depthai_ros_driver/CMakeLists.txt +++ b/depthai_ros_driver/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(depthai_ros_driver VERSION 2.6.2) +project(depthai_ros_driver VERSION 2.6.3) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -19,6 +19,7 @@ ament_auto_add_library( src/dai_nodes/sensors/rgb.cpp src/dai_nodes/sensors/mono.cpp src/dai_nodes/sensors/camera_sensor.cpp + src/dai_nodes/sensors/sensor_helpers.cpp src/dai_nodes/stereo.cpp src/dai_nodes/nn/nn_wrapper.cpp src/dai_nodes/nn/spatial_nn_wrapper.cpp diff --git a/depthai_ros_driver/config/calibration/left.yaml b/depthai_ros_driver/config/calibration/left.yaml new file mode 100644 index 00000000..d0504261 --- /dev/null +++ b/depthai_ros_driver/config/calibration/left.yaml @@ -0,0 +1,26 @@ +image_width: 1280 +image_height: 720 +camera_name: /left +camera_matrix: + rows: 3 + cols: 3 + data: [800.40414, 0. , 638.37063, + 0. , 799.68286, 360.37479, + 0. , 0. , 1. ] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [0.107416, -0.285425, -0.005807, -0.001530, 0.000000] +rectification_matrix: + rows: 3 + cols: 3 + data: [ 0.99961855, -0.02124419, 0.01764781, + 0.02115037, 0.99976125, 0.00548633, + -0.01776014, -0.00511098, 0.99982921] +projection_matrix: + rows: 3 + cols: 4 + data: [833.04045, 0. , 616.04111, 0. , + 0. , 833.04045, 359.7206 , 0. , + 0. , 0. , 1. , 0. ] diff --git a/depthai_ros_driver/config/calibration/rgb.yaml b/depthai_ros_driver/config/calibration/rgb.yaml new file mode 100644 index 00000000..e093f776 --- /dev/null +++ b/depthai_ros_driver/config/calibration/rgb.yaml @@ -0,0 +1,26 @@ +image_width: 1280 +image_height: 720 +camera_name: /rgb +camera_matrix: + rows: 3 + cols: 3 + data: [1007.03765, 0. , 693.05655, + 0. , 1007.59267, 356.9163 , + 0. , 0. , 1. ] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [0.080921, -0.244505, 0.006909, 0.015070, 0.000000] +rectification_matrix: + rows: 3 + cols: 3 + data: [1., 0., 0., + 0., 1., 0., + 0., 0., 1.] +projection_matrix: + rows: 3 + cols: 4 + data: [ 984.22742, 0. , 720.52955, 0. , + 0. , 1022.02051, 359.96275, 0. , + 0. , 0. , 1. , 0. ] diff --git a/depthai_ros_driver/config/calibration/right.yaml b/depthai_ros_driver/config/calibration/right.yaml new file mode 100644 index 00000000..e99384f1 --- /dev/null +++ b/depthai_ros_driver/config/calibration/right.yaml @@ -0,0 +1,26 @@ +image_width: 1280 +image_height: 720 +camera_name: /right +camera_matrix: + rows: 3 + cols: 3 + data: [801.10787, 0. , 661.27048, + 0. , 802.27862, 362.70452, + 0. , 0. , 1. ] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [0.027691, -0.084910, -0.002828, 0.001984, 0.000000] +rectification_matrix: + rows: 3 + cols: 3 + data: [ 0.99912007, -0.00732194, 0.04129749, + 0.00754073, 0.99995833, -0.00514455, + -0.0412581 , 0.00545144, 0.99913365] +projection_matrix: + rows: 3 + cols: 4 + data: [ 833.04045, 0. , 616.04111, -242.47655, + 0. , 833.04045, 359.7206 , 0. , + 0. , 0. , 1. , 0. ] diff --git a/depthai_ros_driver/config/rviz/calibration.yaml b/depthai_ros_driver/config/rviz/calibration.yaml new file mode 100644 index 00000000..1c1c98b4 --- /dev/null +++ b/depthai_ros_driver/config/rviz/calibration.yaml @@ -0,0 +1,8 @@ +/oak: + camera: + i_pipeline_type: RGBStereo + i_nn_type: none + left: + i_publish_topic: true + right: + i_publish_topic: true \ No newline at end of file diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp index 79bdaafd..60f36750 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp @@ -36,6 +36,9 @@ class BaseNode { std::string getName() { return baseDAINodeName; }; + std::string getTFPrefix(const std::string& frameName = "") { + return std::string(getROSNode()->get_name()) + "_" + frameName; + } private: rclcpp::Node* baseNode; diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/mono.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/mono.hpp index 8b4769a4..df6c9023 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/mono.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/mono.hpp @@ -1,5 +1,6 @@ #pragma once +#include "camera_info_manager/camera_info_manager.hpp" #include "depthai/depthai.hpp" #include "depthai_bridge/ImageConverter.hpp" #include "depthai_ros_driver/dai_nodes/base_node.hpp" @@ -29,10 +30,9 @@ class Mono : public BaseNode { void closeQueues() override; private: - void monoQCB(const std::string& name, const std::shared_ptr& data); std::unique_ptr imageConverter; image_transport::CameraPublisher monoPub; - sensor_msgs::msg::CameraInfo monoInfo; + std::shared_ptr infoManager; std::shared_ptr monoCamNode; std::shared_ptr videoEnc; std::unique_ptr ph; diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/rgb.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/rgb.hpp index d3a48a08..7f1eb161 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/rgb.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/rgb.hpp @@ -1,5 +1,6 @@ #pragma once +#include "camera_info_manager/camera_info_manager.hpp" #include "depthai/depthai.hpp" #include "depthai_bridge/ImageConverter.hpp" #include "depthai_ros_driver/dai_nodes/base_node.hpp" @@ -30,10 +31,9 @@ class RGB : public BaseNode { void closeQueues() override; private: - void colorQCB(const std::string& name, const std::shared_ptr& data); std::unique_ptr imageConverter; image_transport::CameraPublisher rgbPub, previewPub; - sensor_msgs::msg::CameraInfo rgbInfo, previewInfo; + std::shared_ptr infoManager, previewInfoManager; std::shared_ptr colorCamNode; std::shared_ptr videoEnc; std::unique_ptr ph; 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 27dcfebe..7060bade 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 @@ -1,8 +1,14 @@ #pragma once +#include #include #include +#include "camera_info_manager/camera_info_manager.hpp" +#include "depthai_bridge/ImageConverter.hpp" +#include "image_transport/camera_publisher.hpp" +#include "sensor_msgs/msg/camera_info.hpp" + namespace depthai_ros_driver { namespace dai_nodes { namespace link_types { @@ -86,6 +92,27 @@ inline std::vector availableSensors{ {"AR0234", {"1200P"}, true}, {"IMX582", {"48mp", "12mp", "4k"}, true}, }; + +void imgCB(const std::string& /*name*/, + const std::shared_ptr& data, + dai::ros::ImageConverter& converter, + image_transport::CameraPublisher& pub, + std::shared_ptr infoManager); +void compressedImgCB(const std::string& /*name*/, + const std::shared_ptr& data, + dai::ros::ImageConverter& converter, + image_transport::CameraPublisher& pub, + std::shared_ptr infoManager, + dai::RawImgFrame::Type dataType); +sensor_msgs::msg::CameraInfo getCalibInfo(const rclcpp::Logger& logger, + dai::ros::ImageConverter& converter, + std::shared_ptr device, + dai::CameraBoardSocket socket, + int width = 0, + int height = 0); +std::shared_ptr createEncoder(std::shared_ptr pipeline, + int quality, + dai::VideoEncoderProperties::Profile profile = dai::VideoEncoderProperties::Profile::MJPEG); } // namespace sensor_helpers } // namespace dai_nodes } // namespace depthai_ros_driver \ No newline at end of file diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/stereo.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/stereo.hpp index 51edb526..65a5960d 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/stereo.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/stereo.hpp @@ -1,8 +1,10 @@ #pragma once +#include "camera_info_manager/camera_info_manager.hpp" #include "depthai/depthai.hpp" #include "depthai_bridge/ImageConverter.hpp" #include "depthai_ros_driver/dai_nodes/base_node.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" #include "depthai_ros_driver/param_handlers/stereo_param_handler.hpp" #include "image_transport/camera_publisher.hpp" #include "image_transport/image_transport.hpp" @@ -27,10 +29,9 @@ class Stereo : public BaseNode { void closeQueues() override; private: - void stereoQCB(const std::string& name, const std::shared_ptr& data); std::unique_ptr imageConverter; image_transport::CameraPublisher stereoPub; - sensor_msgs::msg::CameraInfo stereoInfo; + std::shared_ptr infoManager; std::shared_ptr stereoCamNode; std::shared_ptr videoEnc; std::unique_ptr left; diff --git a/depthai_ros_driver/launch/calibration.launch.py b/depthai_ros_driver/launch/calibration.launch.py new file mode 100644 index 00000000..a13dc4d0 --- /dev/null +++ b/depthai_ros_driver/launch/calibration.launch.py @@ -0,0 +1,80 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer, Node +from launch_ros.descriptions import ComposableNode + + +def launch_setup(context, *args, **kwargs): + name = LaunchConfiguration("name").perform(context) + type = LaunchConfiguration("type").perform(context) + mono_args = ["--size", "8x6", + "--square", "0.108 ", + "--camera_name", "oak/rgb", + "--no-service-check"] + stereo_args = ["--size", "8x6", + "--square", "0.108 ", + "--camera_name", "/rgb", + "--approximate", "0.1", + "--camera_name", "oak", + "left_camera", "left", + "right_camera", "right", + "--no-service-check"] + mono_remappings = [('/image', '{}/rgb/image_raw'.format(name))] + stereo_remappings = [('/left', '{}/left/image_raw'.format(name)), + ('/right', '{}/right/image_raw'.format(name))] + args = [] + remappings = [] + if type=="mono": + args = mono_args + remappings = mono_remappings + else: + args = stereo_args + remappings = stereo_remappings + + return [ + ComposableNodeContainer( + name=name+"_container", + namespace="", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + ComposableNode( + package="depthai_ros_driver", + plugin="depthai_ros_driver::Camera", + name=name, + parameters=[{"camera.i_pipeline_type": "RGBStereo", + "camera.i_nn_type": "none", + "right.i_publish_topic": True, + "left.i_publish_topic": True}], + ), + ], + output="screen", + ), + Node( + name="calibrator", + namespace="", + package="camera_calibration", + executable="cameracalibrator", + arguments=args, + remappings=remappings + ) + ] + + +def generate_launch_description(): + depthai_prefix = get_package_share_directory("depthai_ros_driver") + declared_arguments = [ + DeclareLaunchArgument("name", default_value="oak"), + DeclareLaunchArgument("size", default_value="8x6"), + DeclareLaunchArgument("square", default_value="0.108"), + DeclareLaunchArgument("type", default_value="mono") + + ] + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) diff --git a/depthai_ros_driver/package.xml b/depthai_ros_driver/package.xml index 7a75f85a..0d16df6e 100644 --- a/depthai_ros_driver/package.xml +++ b/depthai_ros_driver/package.xml @@ -2,7 +2,7 @@ depthai_ros_driver - 2.6.2 + 2.6.3 Depthai ROS Monolithic node. Adam Serafin Sachin Guruswamy @@ -12,6 +12,7 @@ ament_cmake ament_cmake_auto + camera_calibration rclcpp rclcpp_components sensor_msgs diff --git a/depthai_ros_driver/src/camera.cpp b/depthai_ros_driver/src/camera.cpp index a1fac541..3e9bc845 100644 --- a/depthai_ros_driver/src/camera.cpp +++ b/depthai_ros_driver/src/camera.cpp @@ -119,7 +119,9 @@ void Camera::startDevice() { auto speed = usbStrings[static_cast(device->getUsbSpeed())]; RCLCPP_INFO(this->get_logger(), "USB SPEED: %s", speed.c_str()); } else { - RCLCPP_INFO(this->get_logger(), "PoE camera detected. Consider enabling low bandwidth for specific image topics (see readme)."); + RCLCPP_INFO(this->get_logger(), + "PoE camera detected. Consider enabling low bandwidth for specific image topics (see " + "readme)."); } } diff --git a/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp b/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp index 9bdff576..8b1931dd 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp @@ -32,9 +32,7 @@ void Mono::setXinXout(std::shared_ptr pipeline) { xoutMono = pipeline->create(); xoutMono->setStreamName(monoQName); if(ph->getParam(getROSNode(), "i_low_bandwidth")) { - videoEnc = pipeline->create(); - videoEnc->setQuality(ph->getParam(getROSNode(), "i_low_bandwidth_quality")); - videoEnc->setProfile(dai::VideoEncoderProperties::Profile::MJPEG); + videoEnc = sensor_helpers::createEncoder(pipeline, ph->getParam(getROSNode(), "i_low_bandwidth_quality")); monoCamNode->out.link(videoEnc->input); videoEnc->bitstream.link(xoutMono->input); } else { @@ -49,21 +47,34 @@ void Mono::setXinXout(std::shared_ptr pipeline) { void Mono::setupQueues(std::shared_ptr device) { if(ph->getParam(getROSNode(), "i_publish_topic")) { monoQ = device->getOutputQueue(monoQName, ph->getParam(getROSNode(), "i_max_q_size"), false); - monoQ->addCallback(std::bind(&Mono::monoQCB, this, std::placeholders::_1, std::placeholders::_2)); - auto tfPrefix = std::string(getROSNode()->get_name()) + "_" + getName(); + auto tfPrefix = getTFPrefix(getName()); imageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); monoPub = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/image_raw"); - auto calibHandler = device->readCalibration(); - try { - monoInfo = imageConverter->calibrationToCameraInfo(calibHandler, - static_cast(ph->getParam(getROSNode(), "i_board_socket_id")), - ph->getParam(getROSNode(), "i_width"), - ph->getParam(getROSNode(), "i_height")); - } catch(std::runtime_error& e) { - RCLCPP_ERROR(getROSNode()->get_logger(), "No calibration! Publishing empty camera_info."); + infoManager = std::make_shared( + getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + getName()).get(), "/" + getName()); + if(ph->getParam(getROSNode(), "i_calibration_file").empty()) { + infoManager->setCameraInfo(sensor_helpers::getCalibInfo(getROSNode()->get_logger(), + *imageConverter, + device, + static_cast(ph->getParam(getROSNode(), "i_board_socket_id")), + ph->getParam(getROSNode(), "i_width"), + ph->getParam(getROSNode(), "i_height"))); + } else { + infoManager->loadCameraInfo(ph->getParam(getROSNode(), "i_calibration_file")); + } + if(ph->getParam(getROSNode(), "i_low_bandwidth")) { + monoQ->addCallback(std::bind(sensor_helpers::compressedImgCB, + std::placeholders::_1, + std::placeholders::_2, + *imageConverter, + monoPub, + infoManager, + dai::RawImgFrame::Type::GRAY8)); + } else { + monoQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *imageConverter, monoPub, infoManager)); } + controlQ = device->getInputQueue(controlQName); } - controlQ = device->getInputQueue(controlQName); } void Mono::closeQueues() { if(ph->getParam(getROSNode(), "i_publish_topic")) { @@ -72,21 +83,6 @@ void Mono::closeQueues() { controlQ->close(); } -void Mono::monoQCB(const std::string& /*name*/, const std::shared_ptr& data) { - auto img = std::dynamic_pointer_cast(data); - std::deque deq; - if(ph->getParam(getROSNode(), "i_low_bandwidth")) - imageConverter->toRosMsgFromBitStream(img, deq, dai::RawImgFrame::Type::GRAY8, monoInfo); - else - imageConverter->toRosMsg(img, deq); - while(deq.size() > 0) { - auto currMsg = deq.front(); - monoInfo.header = currMsg.header; - monoPub.publish(currMsg, monoInfo); - deq.pop_front(); - } -} - void Mono::link(const dai::Node::Input& in, int /*linkType*/) { monoCamNode->out.link(in); } diff --git a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp index 0772562d..8ee2df8d 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp @@ -35,9 +35,7 @@ void RGB::setXinXout(std::shared_ptr pipeline) { if(ph->getParam(getROSNode(), "i_low_bandwidth")) { RCLCPP_INFO(getROSNode()->get_logger(), "POE"); - videoEnc = pipeline->create(); - videoEnc->setQuality(ph->getParam(getROSNode(), "i_low_bandwidth_quality")); - videoEnc->setProfile(dai::VideoEncoderProperties::Profile::MJPEG); + videoEnc = sensor_helpers::createEncoder(pipeline, ph->getParam(getROSNode(), "i_low_bandwidth_quality")); colorCamNode->video.link(videoEnc->input); videoEnc->bitstream.link(xoutColor->input); } else { @@ -57,36 +55,55 @@ void RGB::setXinXout(std::shared_ptr pipeline) { } void RGB::setupQueues(std::shared_ptr device) { - auto calibHandler = device->readCalibration(); if(ph->getParam(getROSNode(), "i_publish_topic")) { - auto tfPrefix = std::string(getROSNode()->get_name()) + "_" + getName(); + auto tfPrefix = getTFPrefix(getName()); + infoManager = std::make_shared( + getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + getName()).get(), "/" + getName()); imageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); - colorQ = device->getOutputQueue(ispQName, ph->getParam(getROSNode(), "i_max_q_size"), false); - colorQ->addCallback(std::bind(&RGB::colorQCB, this, std::placeholders::_1, std::placeholders::_2)); + if(ph->getParam(getROSNode(), "i_calibration_file").empty()) { + infoManager->setCameraInfo(sensor_helpers::getCalibInfo(getROSNode()->get_logger(), + *imageConverter, + device, + static_cast(ph->getParam(getROSNode(), "i_board_socket_id")), + ph->getParam(getROSNode(), "i_width"), + ph->getParam(getROSNode(), "i_height"))); + } else { + infoManager->loadCameraInfo(ph->getParam(getROSNode(), "i_calibration_file")); + } rgbPub = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/image_raw"); - - if(ph->getParam(getROSNode(), "i_enable_preview")) { - previewQ = device->getOutputQueue(previewQName, ph->getParam(getROSNode(), "i_max_q_size"), false); - previewQ->addCallback(std::bind(&RGB::colorQCB, this, std::placeholders::_1, std::placeholders::_2)); - previewPub = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/preview/image_raw"); - try { - previewInfo = imageConverter->calibrationToCameraInfo(calibHandler, - static_cast(ph->getParam(getROSNode(), "i_board_socket_id")), - ph->getParam(getROSNode(), "i_preview_size"), - ph->getParam(getROSNode(), "i_preview_size")); - } catch(std::runtime_error& e) { - RCLCPP_ERROR(getROSNode()->get_logger(), "No calibration! Publishing empty camera_info."); - } - }; - try { - rgbInfo = imageConverter->calibrationToCameraInfo(calibHandler, - static_cast(ph->getParam(getROSNode(), "i_board_socket_id")), - ph->getParam(getROSNode(), "i_width"), - ph->getParam(getROSNode(), "i_height")); - } catch(std::runtime_error& e) { - RCLCPP_ERROR(getROSNode()->get_logger(), "No calibration! Publishing empty camera_info."); + colorQ = device->getOutputQueue(ispQName, ph->getParam(getROSNode(), "i_max_q_size"), false); + if(ph->getParam(getROSNode(), "i_low_bandwidth")) { + colorQ->addCallback(std::bind(sensor_helpers::compressedImgCB, + std::placeholders::_1, + std::placeholders::_2, + *imageConverter, + rgbPub, + infoManager, + dai::RawImgFrame::Type::BGR888i)); + } else { + colorQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *imageConverter, rgbPub, infoManager)); } } + if(ph->getParam(getROSNode(), "i_enable_preview")) { + previewQ = device->getOutputQueue(previewQName, ph->getParam(getROSNode(), "i_max_q_size"), false); + previewPub = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/preview/image_raw"); + previewInfoManager = std::make_shared( + getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + previewQName).get(), previewQName); + auto tfPrefix = getTFPrefix(getName()); + imageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); + if(ph->getParam(getROSNode(), "i_calibration_file").empty()) { + previewInfoManager->setCameraInfo( + sensor_helpers::getCalibInfo(getROSNode()->get_logger(), + *imageConverter, + device, + static_cast(ph->getParam(getROSNode(), "i_board_socket_id")), + ph->getParam(getROSNode(), "i_preview_size"), + ph->getParam(getROSNode(), "i_preview_size"))); + } else { + infoManager->loadCameraInfo(ph->getParam(getROSNode(), "i_calibration_file")); + } + previewQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *imageConverter, previewPub, previewInfoManager)); + }; controlQ = device->getInputQueue(controlQName); } @@ -100,26 +117,6 @@ void RGB::closeQueues() { controlQ->close(); } -void RGB::colorQCB(const std::string& name, const std::shared_ptr& data) { - auto img = std::dynamic_pointer_cast(data); - std::deque deq; - if(ph->getParam(getROSNode(), "i_low_bandwidth")) - imageConverter->toRosMsgFromBitStream(img, deq, dai::RawImgFrame::Type::BGR888i, rgbInfo); - else - imageConverter->toRosMsg(img, deq); - while(deq.size() > 0) { - auto currMsg = deq.front(); - if(name == ispQName) { - rgbInfo.header = currMsg.header; - rgbPub.publish(currMsg, rgbInfo); - } else { - previewInfo.header = currMsg.header; - previewPub.publish(currMsg, previewInfo); - } - deq.pop_front(); - } -} - void RGB::link(const dai::Node::Input& in, int linkType) { if(linkType == static_cast(link_types::RGBLinkType::video)) { colorCamNode->video.link(in); diff --git a/depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp b/depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp new file mode 100644 index 00000000..8ff0ac2c --- /dev/null +++ b/depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp @@ -0,0 +1,65 @@ +#include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" + +namespace depthai_ros_driver { +namespace dai_nodes { +namespace sensor_helpers { + +void compressedImgCB(const std::string& /*name*/, + const std::shared_ptr& data, + dai::ros::ImageConverter& converter, + image_transport::CameraPublisher& pub, + std::shared_ptr infoManager, + dai::RawImgFrame::Type dataType) { + auto img = std::dynamic_pointer_cast(data); + std::deque deq; + auto info = infoManager->getCameraInfo(); + converter.toRosMsgFromBitStream(img, deq, dataType, info); + while(deq.size() > 0) { + auto currMsg = deq.front(); + info.header = currMsg.header; + pub.publish(currMsg, info); + deq.pop_front(); + } +} +void imgCB(const std::string& /*name*/, + const std::shared_ptr& data, + dai::ros::ImageConverter& converter, + image_transport::CameraPublisher& pub, + std::shared_ptr infoManager) { + auto img = std::dynamic_pointer_cast(data); + std::deque deq; + auto info = infoManager->getCameraInfo(); + converter.toRosMsg(img, deq); + while(deq.size() > 0) { + auto currMsg = deq.front(); + info.header = currMsg.header; + pub.publish(currMsg, info); + deq.pop_front(); + } +} +sensor_msgs::msg::CameraInfo getCalibInfo(const rclcpp::Logger& logger, + dai::ros::ImageConverter& converter, + std::shared_ptr device, + dai::CameraBoardSocket socket, + int width, + int height) { + sensor_msgs::msg::CameraInfo info; + auto calibHandler = device->readCalibration(); + + try { + info = converter.calibrationToCameraInfo(calibHandler, socket, width, height); + } catch(std::runtime_error& e) { + RCLCPP_ERROR(logger, "No calibration! Publishing empty camera_info."); + } + return info; +} +std::shared_ptr createEncoder(std::shared_ptr pipeline, int quality, dai::VideoEncoderProperties::Profile profile) { + auto enc = pipeline->create(); + enc->setQuality(quality); + enc->setProfile(profile); + return enc; +} + +} // namespace sensor_helpers +} // namespace dai_nodes +} // namespace depthai_ros_driver \ No newline at end of file diff --git a/depthai_ros_driver/src/dai_nodes/stereo.cpp b/depthai_ros_driver/src/dai_nodes/stereo.cpp index da309ea0..67c5d7a4 100644 --- a/depthai_ros_driver/src/dai_nodes/stereo.cpp +++ b/depthai_ros_driver/src/dai_nodes/stereo.cpp @@ -31,9 +31,7 @@ void Stereo::setXinXout(std::shared_ptr pipeline) { xoutStereo->setStreamName(stereoQName); if(ph->getParam(getROSNode(), "i_low_bandwidth")) { RCLCPP_INFO(getROSNode()->get_logger(), "POE"); - videoEnc = pipeline->create(); - videoEnc->setQuality(ph->getParam(getROSNode(), "i_low_bandwidth_quality")); - videoEnc->setProfile(dai::VideoEncoderProperties::Profile::MJPEG); + videoEnc = sensor_helpers::createEncoder(pipeline, ph->getParam(getROSNode(), "i_low_bandwidth_quality")); stereoCamNode->disparity.link(videoEnc->input); videoEnc->bitstream.link(xoutStereo->input); } else { @@ -45,25 +43,48 @@ void Stereo::setupQueues(std::shared_ptr device) { left->setupQueues(device); right->setupQueues(device); stereoQ = device->getOutputQueue(stereoQName, ph->getParam(getROSNode(), "i_max_q_size"), false); - std::string frameName; + std::string tfPrefix; if(ph->getParam(getROSNode(), "i_align_depth")) { - frameName = "rgb"; + tfPrefix = getTFPrefix("rgb"); } else { - frameName = "right"; + tfPrefix = getTFPrefix("right"); } - auto tfPrefix = std::string(getROSNode()->get_name()) + "_" + frameName; imageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); - stereoQ->addCallback(std::bind(&Stereo::stereoQCB, this, std::placeholders::_1, std::placeholders::_2)); + stereoPub = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/image_raw"); + infoManager = std::make_shared( + getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + getName()).get(), "/" + getName()); + auto info = sensor_helpers::getCalibInfo(getROSNode()->get_logger(), + *imageConverter, + device, + static_cast(ph->getParam(getROSNode(), "i_board_socket_id")), + ph->getParam(getROSNode(), "i_width"), + ph->getParam(getROSNode(), "i_height")); auto calibHandler = device->readCalibration(); - try { - stereoInfo = imageConverter->calibrationToCameraInfo(calibHandler, - static_cast(ph->getParam(getROSNode(), "i_board_socket_id")), - ph->getParam(getROSNode(), "i_width"), - ph->getParam(getROSNode(), "i_height")); - stereoInfo.p[3] = calibHandler.getBaselineDistance() * 10.0; // baseline in mm - } catch(std::runtime_error& e) { - RCLCPP_ERROR(getROSNode()->get_logger(), "No calibration! Publishing empty camera_info."); + info.p[3] = calibHandler.getBaselineDistance() * 10.0; // baseline in mm + infoManager->setCameraInfo(info); + + if(ph->getParam(getROSNode(), "i_low_bandwidth")) { + if(ph->getParam(getROSNode(), "i_output_disparity")) { + stereoQ->addCallback(std::bind(sensor_helpers::compressedImgCB, + std::placeholders::_1, + std::placeholders::_2, + *imageConverter, + stereoPub, + infoManager, + dai::RawImgFrame::Type::GRAY8)); + } else { + // converting disp->depth + stereoQ->addCallback(std::bind(sensor_helpers::compressedImgCB, + std::placeholders::_1, + std::placeholders::_2, + *imageConverter, + stereoPub, + infoManager, + dai::RawImgFrame::Type::RAW8)); + } + } else { + stereoQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *imageConverter, stereoPub, infoManager)); } } void Stereo::closeQueues() { @@ -71,20 +92,6 @@ void Stereo::closeQueues() { right->closeQueues(); stereoQ->close(); } -void Stereo::stereoQCB(const std::string& /*name*/, const std::shared_ptr& data) { - auto img = std::dynamic_pointer_cast(data); - std::deque deq; - if(ph->getParam(getROSNode(), "i_low_bandwidth")) - imageConverter->toRosMsgFromBitStream(img, deq, dai::RawImgFrame::Type::RAW8, stereoInfo); - else - imageConverter->toRosMsg(img, deq); - while(deq.size() > 0) { - auto currMsg = deq.front(); - stereoInfo.header = currMsg.header; - stereoPub.publish(currMsg, stereoInfo); - deq.pop_front(); - } -} void Stereo::link(const dai::Node::Input& in, int /*linkType*/) { stereoCamNode->depth.link(in); diff --git a/depthai_ros_driver/src/param_handlers/mono_param_handler.cpp b/depthai_ros_driver/src/param_handlers/mono_param_handler.cpp index 8962194a..fecd404e 100644 --- a/depthai_ros_driver/src/param_handlers/mono_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/mono_param_handler.cpp @@ -22,6 +22,7 @@ void MonoParamHandler::declareParams( declareAndLogParam(node, "i_low_bandwidth", false); declareAndLogParam(node, "i_low_bandwidth_quality", 50); declareAndLogParam(node, "i_board_socket_id", static_cast(socket)); + declareAndLogParam(node, "i_calibration_file", ""); monoCam->setBoardSocket(socket); monoCam->setFps(declareAndLogParam(node, "i_fps", 30.0)); diff --git a/depthai_ros_driver/src/param_handlers/rgb_param_handler.cpp b/depthai_ros_driver/src/param_handlers/rgb_param_handler.cpp index 0aea3346..1124f9fd 100644 --- a/depthai_ros_driver/src/param_handlers/rgb_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/rgb_param_handler.cpp @@ -30,6 +30,7 @@ void RGBParamHandler::declareParams(rclcpp::Node* node, declareAndLogParam(node, "i_low_bandwidth", false); declareAndLogParam(node, "i_low_bandwidth_quality", 50); declareAndLogParam(node, "i_board_socket_id", static_cast(socket)); + declareAndLogParam(node, "i_calibration_file", ""); colorCam->setBoardSocket(socket); colorCam->setFps(declareAndLogParam(node, "i_fps", 30.0)); size_t preview_size = declareAndLogParam(node, "i_preview_size", 416); 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 a93533a3..13b00580 100644 --- a/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp @@ -63,8 +63,8 @@ void StereoParamHandler::declareParams(rclcpp::Node* node, std::shared_ptr(node, "i_temporal_filter_persistency", "VALID_2_IN_LAST_4")); } + config.postProcessing.speckleFilter.enable = declareAndLogParam(node, "i_enable_speckle_filter", false); if(config.postProcessing.speckleFilter.enable) { - config.postProcessing.speckleFilter.enable = declareAndLogParam(node, "i_enable_speckle_filter", false); config.postProcessing.speckleFilter.speckleRange = declareAndLogParam(node, "i_speckle_filter_speckle_range", 50); } config.postProcessing.spatialFilter.enable = declareAndLogParam(node, "i_enable_spatial_filter", false); diff --git a/depthai_ros_msgs/CMakeLists.txt b/depthai_ros_msgs/CMakeLists.txt index 165a8e0c..bbd70de4 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.6.2) +project(depthai_ros_msgs VERSION 2.6.3) if(POLICY CMP0057) cmake_policy(SET CMP0057 NEW) diff --git a/depthai_ros_msgs/package.xml b/depthai_ros_msgs/package.xml index 515464e4..10ca6932 100644 --- a/depthai_ros_msgs/package.xml +++ b/depthai_ros_msgs/package.xml @@ -1,7 +1,7 @@ depthai_ros_msgs - 2.6.2 + 2.6.3 Package to keep interface independent of the driver Sachin Guruswamy From c1c242ea59e44daf08d8b844fbf79633b63a37f2 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 15 Feb 2023 13:47:14 +0000 Subject: [PATCH 2/6] clang tidy update --- .../dai_nodes/sensors/sensor_helpers.hpp | 17 ++++------------- .../param_handlers/nn_param_handler.hpp | 8 ++++---- .../src/dai_nodes/nn/mobilenet.cpp | 4 ++-- .../src/dai_nodes/nn/nn_wrapper.cpp | 4 ++-- .../src/dai_nodes/nn/segmentation.cpp | 4 ++-- .../src/dai_nodes/nn/spatial_mobilenet.cpp | 2 +- .../src/dai_nodes/nn/spatial_nn_wrapper.cpp | 4 ++-- .../src/dai_nodes/nn/spatial_yolo.cpp | 2 +- .../src/dai_nodes/sensors/camera_sensor.cpp | 4 ++-- .../src/dai_nodes/sensors/imu.cpp | 2 +- .../src/dai_nodes/sensors/mono.cpp | 2 +- .../src/dai_nodes/sensors/rgb.cpp | 2 +- .../src/dai_nodes/sensors/sensor_helpers.cpp | 14 +++++++++++++- depthai_ros_driver/src/dai_nodes/stereo.cpp | 2 +- .../src/param_handlers/camera_param_handler.cpp | 4 ++-- .../src/param_handlers/imu_param_handler.cpp | 6 +++--- .../src/param_handlers/mono_param_handler.cpp | 2 +- .../src/param_handlers/nn_param_handler.cpp | 12 ++++++------ .../src/param_handlers/rgb_param_handler.cpp | 2 +- .../src/param_handlers/stereo_param_handler.cpp | 2 +- 20 files changed, 51 insertions(+), 48 deletions(-) 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 7060bade..93b6d774 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 @@ -76,22 +76,13 @@ struct ImageSensor { width = 6000; break; } + default: { + throw std::runtime_error("Resolution not supported!"); + } } } }; -inline std::vector availableSensors{ - {"IMX378", {"12mp", "4k"}, true}, - {"OV9282", {"800P", "720p", "400p"}, false}, - {"OV9782", {"800P", "720p", "400p"}, true}, - {"OV9281", {"800P", "720p", "400p"}, true}, - {"IMX214", {"13mp", "12mp", "4k", "1080p"}, true}, - {"OV7750", {"480P", "400p"}, false}, - {"OV7251", {"480P", "400p"}, false}, - {"IMX477", {"12mp", "4k", "1080p"}, true}, - {"IMX577", {"12mp", "4k", "1080p"}, true}, - {"AR0234", {"1200P"}, true}, - {"IMX582", {"48mp", "12mp", "4k"}, true}, -}; +extern std::vector availableSensors; void imgCB(const std::string& /*name*/, const std::shared_ptr& data, 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 72671bfc..fbe784a5 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 @@ -32,14 +32,14 @@ class NNParamHandler : public BaseParamHandler { void setNNParams(rclcpp::Node* node, nlohmann::json data, std::shared_ptr nn); template - void setSpatialParams(rclcpp::Node* node, nlohmann::json data, std::shared_ptr nn) { + void setSpatialParams(std::shared_ptr nn) { nn->setBoundingBoxScaleFactor(0.5); nn->setDepthLowerThreshold(100); nn->setDepthUpperThreshold(10000); } template - void setYoloParams(rclcpp::Node* node, nlohmann::json data, std::shared_ptr nn) { + void setYoloParams(nlohmann::json data, std::shared_ptr nn) { auto metadata = data["nn_config"]["NN_specific_metadata"]; int num_classes = 80; if(metadata.contains("classes")) { @@ -81,7 +81,7 @@ class NNParamHandler : public BaseParamHandler { if(data.contains("model") && data.contains("nn_config")) { auto modelPath = getModelPath(data); declareAndLogParam(node, "i_model_path", modelPath); - setImageManip(node, modelPath, imageManip); + setImageManip(modelPath, imageManip); nn->setBlobPath(modelPath); nn->setNumPoolFrames(declareAndLogParam(node, "i_num_pool_frames", 4)); nn->setNumInferenceThreads(declareAndLogParam(node, "i_num_inference_threads", 2)); @@ -94,7 +94,7 @@ class NNParamHandler : public BaseParamHandler { dai::CameraControl setRuntimeParams(rclcpp::Node* node, const std::vector& params) override; private: - void setImageManip(rclcpp::Node* node, const std::string& model_path, std::shared_ptr imageManip); + void setImageManip(const std::string& model_path, std::shared_ptr imageManip); std::string getModelPath(const nlohmann::json& data); std::unordered_map nnFamilyMap; }; diff --git a/depthai_ros_driver/src/dai_nodes/nn/mobilenet.cpp b/depthai_ros_driver/src/dai_nodes/nn/mobilenet.cpp index 57085393..3b3624fa 100644 --- a/depthai_ros_driver/src/dai_nodes/nn/mobilenet.cpp +++ b/depthai_ros_driver/src/dai_nodes/nn/mobilenet.cpp @@ -59,11 +59,11 @@ void Mobilenet::mobilenetCB(const std::string& /*name*/, const std::shared_ptrout.link(in); } -dai::Node::Input Mobilenet::getInput(int linkType) { +dai::Node::Input Mobilenet::getInput(int /*linkType*/) { return imageManip->inputImage; } diff --git a/depthai_ros_driver/src/dai_nodes/nn/nn_wrapper.cpp b/depthai_ros_driver/src/dai_nodes/nn/nn_wrapper.cpp index 02d94548..b53642d3 100644 --- a/depthai_ros_driver/src/dai_nodes/nn/nn_wrapper.cpp +++ b/depthai_ros_driver/src/dai_nodes/nn/nn_wrapper.cpp @@ -29,10 +29,10 @@ NNWrapper::NNWrapper(const std::string& daiNodeName, rclcpp::Node* node, std::sh } RCLCPP_DEBUG(node->get_logger(), "Base node %s created", daiNodeName.c_str()); -}; +} void NNWrapper::setNames() {} -void NNWrapper::setXinXout(std::shared_ptr pipeline) {} +void NNWrapper::setXinXout(std::shared_ptr /*pipeline*/) {} void NNWrapper::setupQueues(std::shared_ptr device) { nnNode->setupQueues(device); diff --git a/depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp b/depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp index 677bd942..9d5d0ca1 100644 --- a/depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp +++ b/depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp @@ -77,11 +77,11 @@ cv::Mat Segmentation::decodeDeeplab(cv::Mat mat) { } return colors; } -void Segmentation::link(const dai::Node::Input& in, int linkType) { +void Segmentation::link(const dai::Node::Input& in, int /*linkType*/) { segNode->out.link(in); } -dai::Node::Input Segmentation::getInput(int linkType) { +dai::Node::Input Segmentation::getInput(int /*linkType*/) { return imageManip->inputImage; } diff --git a/depthai_ros_driver/src/dai_nodes/nn/spatial_mobilenet.cpp b/depthai_ros_driver/src/dai_nodes/nn/spatial_mobilenet.cpp index dfdc87cc..894e70ed 100644 --- a/depthai_ros_driver/src/dai_nodes/nn/spatial_mobilenet.cpp +++ b/depthai_ros_driver/src/dai_nodes/nn/spatial_mobilenet.cpp @@ -57,7 +57,7 @@ void SpatialMobilenet::mobilenetCB(const std::string& /*name*/, const std::share } } -void SpatialMobilenet::link(const dai::Node::Input& in, int linkType) { +void SpatialMobilenet::link(const dai::Node::Input& in, int /*linkType*/) { mobileNode->out.link(in); } diff --git a/depthai_ros_driver/src/dai_nodes/nn/spatial_nn_wrapper.cpp b/depthai_ros_driver/src/dai_nodes/nn/spatial_nn_wrapper.cpp index fd9b4e77..c9705bb4 100644 --- a/depthai_ros_driver/src/dai_nodes/nn/spatial_nn_wrapper.cpp +++ b/depthai_ros_driver/src/dai_nodes/nn/spatial_nn_wrapper.cpp @@ -28,10 +28,10 @@ SpatialNNWrapper::SpatialNNWrapper(const std::string& daiNodeName, rclcpp::Node* } RCLCPP_DEBUG(node->get_logger(), "Base node %s created", daiNodeName.c_str()); -}; +} void SpatialNNWrapper::setNames() {} -void SpatialNNWrapper::setXinXout(std::shared_ptr pipeline) {} +void SpatialNNWrapper::setXinXout(std::shared_ptr /*pipeline*/) {} void SpatialNNWrapper::setupQueues(std::shared_ptr device) { nnNode->setupQueues(device); diff --git a/depthai_ros_driver/src/dai_nodes/nn/spatial_yolo.cpp b/depthai_ros_driver/src/dai_nodes/nn/spatial_yolo.cpp index 95e78ac9..5779f9f3 100644 --- a/depthai_ros_driver/src/dai_nodes/nn/spatial_yolo.cpp +++ b/depthai_ros_driver/src/dai_nodes/nn/spatial_yolo.cpp @@ -56,7 +56,7 @@ void SpatialYolo::yoloCB(const std::string& /*name*/, const std::shared_ptrout.link(in); } diff --git a/depthai_ros_driver/src/dai_nodes/sensors/camera_sensor.cpp b/depthai_ros_driver/src/dai_nodes/sensors/camera_sensor.cpp index ec5887c1..02dede43 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/camera_sensor.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/camera_sensor.cpp @@ -32,10 +32,10 @@ CameraSensor::CameraSensor(const std::string& daiNodeName, } RCLCPP_DEBUG(node->get_logger(), "Base node %s created", daiNodeName.c_str()); -}; +} void CameraSensor::setNames() {} -void CameraSensor::setXinXout(std::shared_ptr pipeline) {} +void CameraSensor::setXinXout(std::shared_ptr /*pipeline*/) {} void CameraSensor::setupQueues(std::shared_ptr device) { sensorNode->setupQueues(device); diff --git a/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp b/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp index 0c98b2fa..5c2f572a 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp @@ -13,7 +13,7 @@ Imu::Imu(const std::string& daiNodeName, rclcpp::Node* node, std::shared_ptrdeclareParams(node, imuNode); setXinXout(pipeline); RCLCPP_DEBUG(node->get_logger(), "Node %s created", daiNodeName.c_str()); -}; +} void Imu::setNames() { imuQName = getName() + "_imu"; } diff --git a/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp b/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp index 8b1931dd..3c62c47b 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp @@ -21,7 +21,7 @@ Mono::Mono(const std::string& daiNodeName, ph->declareParams(node, monoCamNode, socket, sensor, publish); setXinXout(pipeline); RCLCPP_DEBUG(node->get_logger(), "Node %s created", daiNodeName.c_str()); -}; +} void Mono::setNames() { monoQName = getName() + "_mono"; controlQName = getName() + "_control"; diff --git a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp index 8ee2df8d..c53ca0f4 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp @@ -21,7 +21,7 @@ RGB::RGB(const std::string& daiNodeName, ph->declareParams(node, colorCamNode, socket, sensor, publish); setXinXout(pipeline); RCLCPP_DEBUG(node->get_logger(), "Node %s created", daiNodeName.c_str()); -}; +} void RGB::setNames() { ispQName = getName() + "_isp"; previewQName = getName() + "_preview"; 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 8ff0ac2c..e790fa49 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp @@ -3,7 +3,19 @@ namespace depthai_ros_driver { namespace dai_nodes { namespace sensor_helpers { - +std::vector availableSensors{ + {"IMX378", {"12mp", "4k"}, true}, + {"OV9282", {"800P", "720p", "400p"}, false}, + {"OV9782", {"800P", "720p", "400p"}, true}, + {"OV9281", {"800P", "720p", "400p"}, true}, + {"IMX214", {"13mp", "12mp", "4k", "1080p"}, true}, + {"OV7750", {"480P", "400p"}, false}, + {"OV7251", {"480P", "400p"}, false}, + {"IMX477", {"12mp", "4k", "1080p"}, true}, + {"IMX577", {"12mp", "4k", "1080p"}, true}, + {"AR0234", {"1200P"}, true}, + {"IMX582", {"48mp", "12mp", "4k"}, true}, +}; void compressedImgCB(const std::string& /*name*/, const std::shared_ptr& data, dai::ros::ImageConverter& converter, diff --git a/depthai_ros_driver/src/dai_nodes/stereo.cpp b/depthai_ros_driver/src/dai_nodes/stereo.cpp index 67c5d7a4..535ffba4 100644 --- a/depthai_ros_driver/src/dai_nodes/stereo.cpp +++ b/depthai_ros_driver/src/dai_nodes/stereo.cpp @@ -21,7 +21,7 @@ Stereo::Stereo(const std::string& daiNodeName, rclcpp::Node* node, std::shared_p left->link(stereoCamNode->left); right->link(stereoCamNode->right); RCLCPP_DEBUG(node->get_logger(), "Node %s created", daiNodeName.c_str()); -}; +} void Stereo::setNames() { stereoQName = getName() + "_stereo"; } diff --git a/depthai_ros_driver/src/param_handlers/camera_param_handler.cpp b/depthai_ros_driver/src/param_handlers/camera_param_handler.cpp index 4f1a6826..0de367e0 100644 --- a/depthai_ros_driver/src/param_handlers/camera_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/camera_param_handler.cpp @@ -13,7 +13,7 @@ CameraParamHandler::CameraParamHandler(const std::string& name) : BaseParamHandl {"SUPER", dai::UsbSpeed::SUPER}, {"SUPER_PLUS", dai::UsbSpeed::SUPER_PLUS}, }; -}; +} CameraParamHandler::~CameraParamHandler() = default; dai::UsbSpeed CameraParamHandler::getUSBSpeed(rclcpp::Node* node) { @@ -30,7 +30,7 @@ void CameraParamHandler::declareParams(rclcpp::Node* node) { declareAndLogParam(node, "i_laser_dot_brightness", 800, getRangedIntDescriptor(0, 1200)); declareAndLogParam(node, "i_floodlight_brightness", 0, getRangedIntDescriptor(0, 1500)); } -dai::CameraControl CameraParamHandler::setRuntimeParams(rclcpp::Node* node, const std::vector& params) { +dai::CameraControl CameraParamHandler::setRuntimeParams(rclcpp::Node* /*node*/, const std::vector& /*params*/) { dai::CameraControl ctrl; return ctrl; } diff --git a/depthai_ros_driver/src/param_handlers/imu_param_handler.cpp b/depthai_ros_driver/src/param_handlers/imu_param_handler.cpp index ffb5a4ed..d6292c9b 100644 --- a/depthai_ros_driver/src/param_handlers/imu_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/imu_param_handler.cpp @@ -5,16 +5,16 @@ namespace depthai_ros_driver { namespace param_handlers { -ImuParamHandler::ImuParamHandler(const std::string& name) : BaseParamHandler(name){}; +ImuParamHandler::ImuParamHandler(const std::string& name) : BaseParamHandler(name) {} ImuParamHandler::~ImuParamHandler() = default; -void ImuParamHandler::declareParams(rclcpp::Node* node, std::shared_ptr imu) { +void ImuParamHandler::declareParams(rclcpp::Node* /*node*/, std::shared_ptr imu) { imu->enableIMUSensor(dai::IMUSensor::ACCELEROMETER_RAW, 400); imu->enableIMUSensor(dai::IMUSensor::GYROSCOPE_RAW, 400); // imu->enableIMUSensor(dai::IMUSensor::ROTATION_VECTOR, 400); imu->setBatchReportThreshold(1); imu->setMaxBatchReports(10); } -dai::CameraControl ImuParamHandler::setRuntimeParams(rclcpp::Node* node, const std::vector& params) { +dai::CameraControl ImuParamHandler::setRuntimeParams(rclcpp::Node* /*node*/, const std::vector& /*params*/) { dai::CameraControl ctrl; return ctrl; } diff --git a/depthai_ros_driver/src/param_handlers/mono_param_handler.cpp b/depthai_ros_driver/src/param_handlers/mono_param_handler.cpp index fecd404e..9fab6f0e 100644 --- a/depthai_ros_driver/src/param_handlers/mono_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/mono_param_handler.cpp @@ -13,7 +13,7 @@ MonoParamHandler::MonoParamHandler(const std::string& name) : BaseParamHandler(n {"720", dai::MonoCameraProperties::SensorResolution::THE_720_P}, {"800", dai::MonoCameraProperties::SensorResolution::THE_800_P}, }; -}; +} MonoParamHandler::~MonoParamHandler() = default; void MonoParamHandler::declareParams( rclcpp::Node* node, std::shared_ptr monoCam, dai::CameraBoardSocket socket, dai_nodes::sensor_helpers::ImageSensor, bool publish) { diff --git a/depthai_ros_driver/src/param_handlers/nn_param_handler.cpp b/depthai_ros_driver/src/param_handlers/nn_param_handler.cpp index 0162e981..5a86cb13 100644 --- a/depthai_ros_driver/src/param_handlers/nn_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/nn_param_handler.cpp @@ -16,7 +16,7 @@ NNParamHandler::NNParamHandler(const std::string& name) : BaseParamHandler(name) {"mobilenet", nn::NNFamily::Mobilenet}, {"YOLO", nn::NNFamily::Yolo}, }; -}; +} NNParamHandler::~NNParamHandler() = default; nn::NNFamily NNParamHandler::getNNFamily(rclcpp::Node* node) { std::string config_path = ament_index_cpp::get_package_share_directory("depthai_ros_driver") + "/config/nn/"; @@ -71,7 +71,7 @@ void NNParamHandler::setNNParams(rclcpp::Node* node, nlohmann::json data, std::s if(!labels.empty()) { declareAndLogParam>(node, "i_label_map", labels); } - setSpatialParams(node, data, nn); + setSpatialParams(nn); } void NNParamHandler::setNNParams(rclcpp::Node* node, nlohmann::json data, std::shared_ptr nn) { float conf_threshold = 0.5; @@ -83,9 +83,9 @@ void NNParamHandler::setNNParams(rclcpp::Node* node, nlohmann::json data, std::s if(!labels.empty()) { declareAndLogParam>(node, "i_label_map", labels); } - setSpatialParams(node, data, nn); + setSpatialParams(nn); if(data["nn_config"].contains("NN_specific_metadata")) { - setYoloParams(node, data, nn); + setYoloParams(data, nn); } } @@ -100,11 +100,11 @@ void NNParamHandler::setNNParams(rclcpp::Node* node, nlohmann::json data, std::s declareAndLogParam>(node, "i_label_map", labels); } if(data["nn_config"].contains("NN_specific_metadata")) { - setYoloParams(node, data, nn); + setYoloParams(data, nn); } } -void NNParamHandler::setImageManip(rclcpp::Node* node, const std::string& model_path, std::shared_ptr imageManip) { +void NNParamHandler::setImageManip(const std::string& model_path, std::shared_ptr imageManip) { auto blob = dai::OpenVINO::Blob(model_path); auto first_info = blob.networkInputs.begin(); auto input_size = first_info->second.dims[0]; diff --git a/depthai_ros_driver/src/param_handlers/rgb_param_handler.cpp b/depthai_ros_driver/src/param_handlers/rgb_param_handler.cpp index 1124f9fd..9dca235b 100644 --- a/depthai_ros_driver/src/param_handlers/rgb_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/rgb_param_handler.cpp @@ -17,7 +17,7 @@ RGBParamHandler::RGBParamHandler(const std::string& name) : BaseParamHandler(nam {"4000x3000", dai::ColorCameraProperties::SensorResolution::THE_4000X3000}, {"5312X6000", dai::ColorCameraProperties::SensorResolution::THE_5312X6000}, {"48_MP", dai::ColorCameraProperties::SensorResolution::THE_48_MP}}; -}; +} RGBParamHandler::~RGBParamHandler() = default; void RGBParamHandler::declareParams(rclcpp::Node* node, std::shared_ptr colorCam, 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 13b00580..14d3f7a8 100644 --- a/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp @@ -85,7 +85,7 @@ void StereoParamHandler::declareParams(rclcpp::Node* node, std::shared_ptrinitialConfig.set(config); } -dai::CameraControl StereoParamHandler::setRuntimeParams(rclcpp::Node* node, const std::vector& params) { +dai::CameraControl StereoParamHandler::setRuntimeParams(rclcpp::Node* /*node*/, const std::vector& /*params*/) { dai::CameraControl ctrl; return ctrl; } From 83050db08919d2fc6b932901f15e311ea73fff46 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 15 Feb 2023 13:51:57 +0000 Subject: [PATCH 3/6] launch file minor fix --- depthai_ros_driver/launch/calibration.launch.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/depthai_ros_driver/launch/calibration.launch.py b/depthai_ros_driver/launch/calibration.launch.py index a13dc4d0..6e3d9cd0 100644 --- a/depthai_ros_driver/launch/calibration.launch.py +++ b/depthai_ros_driver/launch/calibration.launch.py @@ -11,12 +11,14 @@ def launch_setup(context, *args, **kwargs): name = LaunchConfiguration("name").perform(context) type = LaunchConfiguration("type").perform(context) - mono_args = ["--size", "8x6", - "--square", "0.108 ", + size = LaunchConfiguration("size").perform(context) + square = LaunchConfiguration("square").perform(context) + mono_args = ["--size", size, + "--square", square, "--camera_name", "oak/rgb", "--no-service-check"] - stereo_args = ["--size", "8x6", - "--square", "0.108 ", + stereo_args = ["--size", size, + "--square", square, "--camera_name", "/rgb", "--approximate", "0.1", "--camera_name", "oak", From 4ea12d0cf5fa8f651856ebcb8e3ab8cf2024ce41 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 15 Feb 2023 14:21:56 +0000 Subject: [PATCH 4/6] update config file --- depthai_ros_driver/config/camera.yaml | 54 ++++++++++++++----- .../src/dai_nodes/sensors/rgb.cpp | 6 +-- 2 files changed, 43 insertions(+), 17 deletions(-) diff --git a/depthai_ros_driver/config/camera.yaml b/depthai_ros_driver/config/camera.yaml index 840ce5b7..b4a84cce 100644 --- a/depthai_ros_driver/config/camera.yaml +++ b/depthai_ros_driver/config/camera.yaml @@ -12,8 +12,11 @@ i_usb_speed: SUPER_PLUS left: i_board_socket_id: 1 + i_calibration_file: '' i_fps: 30.0 i_height: 720 + i_low_bandwidth: false + i_low_bandwidth_quality: 50 i_max_q_size: 30 i_publish_topic: false i_resolution: '720' @@ -45,16 +48,46 @@ - train - tvmonitor i_max_q_size: 30 - i_nn_config_path: depthai_ros_driver/mobilenet + i_model_path: /workspaces/ros_2/install/depthai_examples/share/depthai_examples/resources/mobilenet-ssd_openvino_2021.2_6shave.blob + i_nn_config_path: /workspaces/ros_2/install/depthai_ros_driver/share/depthai_ros_driver/config/nn/mobilenet.json i_num_inference_threads: 2 i_num_pool_frames: 4 + oak: + rgb: + image_raw: + format: jpeg + jpeg_quality: 95 + png_level: 3 + tiff: + res_unit: inch + xdpi: -1 + ydpi: -1 + stereo: + image_raw: + format: jpeg + jpeg_quality: 95 + png_level: 3 + tiff: + res_unit: inch + xdpi: -1 + ydpi: -1 + qos_overrides: + /parameter_events: + publisher: + depth: 1000 + durability: volatile + history: keep_last + reliability: reliable rgb: i_board_socket_id: 0 + i_calibration_file: '' i_enable_preview: false i_fps: 30.0 i_height: 720 i_interleaved: false i_keep_preview_aspect_ratio: true + i_low_bandwidth: false + i_low_bandwidth_quality: 50 i_max_q_size: 30 i_preview_size: 416 i_publish_topic: true @@ -70,8 +103,11 @@ r_whitebalance: 3300 right: i_board_socket_id: 2 + i_calibration_file: '' i_fps: 30.0 i_height: 720 + i_low_bandwidth: false + i_low_bandwidth_quality: 50 i_max_q_size: 30 i_publish_topic: false i_resolution: '720' @@ -83,30 +119,22 @@ i_align_depth: true i_bilateral_sigma: 0 i_board_socket_id: 0 - i_decimation_filter_decimation_factor: 1 - i_decimation_filter_decimation_mode: PIXEL_SKIPPING i_depth_filter_size: 5 i_depth_preset: HIGH_ACCURACY i_enable_decimation_filter: false i_enable_distortion_correction: false i_enable_spatial_filter: false + i_enable_speckle_filter: false i_enable_temporal_filter: false i_enable_threshold_filter: false i_extended_disp: false i_height: 720 + i_low_bandwidth: false + i_low_bandwidth_quality: 50 i_lr_check: true i_lrc_threshold: 10 i_max_q_size: 30 i_rectify_edge_fill_color: 0 - i_spatial_filter_alpha: 0.5 - i_spatial_filter_delta: 20 - i_spatial_filter_hole_filling_radius: 2 - i_spatial_filter_iterations: 1 i_stereo_conf_threshold: 255 - i_temporal_filter_alpha: 0.4000000059604645 - i_temporal_filter_delta: 20 - i_temporal_filter_persistency: VALID_2_IN_LAST_4 - i_threshold_filter_max_range: 15000 - i_threshold_filter_min_range: 400 i_width: 1280 - use_sim_time: false + use_sim_time: false \ No newline at end of file diff --git a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp index c53ca0f4..a9efdda2 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp @@ -33,22 +33,20 @@ void RGB::setXinXout(std::shared_ptr pipeline) { xoutColor = pipeline->create(); xoutColor->setStreamName(ispQName); if(ph->getParam(getROSNode(), "i_low_bandwidth")) { - RCLCPP_INFO(getROSNode()->get_logger(), "POE"); - videoEnc = sensor_helpers::createEncoder(pipeline, ph->getParam(getROSNode(), "i_low_bandwidth_quality")); colorCamNode->video.link(videoEnc->input); videoEnc->bitstream.link(xoutColor->input); } else { colorCamNode->isp.link(xoutColor->input); } - if(ph->getParam(getROSNode(), "i_enable_preview")) { + } + if(ph->getParam(getROSNode(), "i_enable_preview")) { xoutPreview = pipeline->create(); xoutPreview->setStreamName(previewQName); xoutPreview->input.setQueueSize(2); xoutPreview->input.setBlocking(false); colorCamNode->preview.link(xoutPreview->input); } - } xinControl = pipeline->create(); xinControl->setStreamName(controlQName); xinControl->out.link(colorCamNode->inputControl); From a8429481b50e43c2e2faddcbeae30ac381f7175f Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 15 Feb 2023 15:01:02 +0000 Subject: [PATCH 5/6] update testing --- depthai_ros_driver/CMakeLists.txt | 12 ---------- .../launch/calibration.launch.py | 23 +++++++++---------- depthai_ros_driver/package.xml | 4 ---- 3 files changed, 11 insertions(+), 28 deletions(-) diff --git a/depthai_ros_driver/CMakeLists.txt b/depthai_ros_driver/CMakeLists.txt index af9c0c46..66cee7f7 100644 --- a/depthai_ros_driver/CMakeLists.txt +++ b/depthai_ros_driver/CMakeLists.txt @@ -46,18 +46,6 @@ target_link_libraries( rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::Camera") ament_export_include_directories(include) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) install( diff --git a/depthai_ros_driver/launch/calibration.launch.py b/depthai_ros_driver/launch/calibration.launch.py index 6e3d9cd0..98f682a2 100644 --- a/depthai_ros_driver/launch/calibration.launch.py +++ b/depthai_ros_driver/launch/calibration.launch.py @@ -14,23 +14,23 @@ def launch_setup(context, *args, **kwargs): size = LaunchConfiguration("size").perform(context) square = LaunchConfiguration("square").perform(context) mono_args = ["--size", size, - "--square", square, - "--camera_name", "oak/rgb", - "--no-service-check"] + "--square", square, + "--camera_name", "oak/rgb", + "--no-service-check"] stereo_args = ["--size", size, - "--square", square, - "--camera_name", "/rgb", - "--approximate", "0.1", - "--camera_name", "oak", - "left_camera", "left", - "right_camera", "right", - "--no-service-check"] + "--square", square, + "--camera_name", "/rgb", + "--approximate", "0.1", + "--camera_name", "oak", + "left_camera", "left", + "right_camera", "right", + "--no-service-check"] mono_remappings = [('/image', '{}/rgb/image_raw'.format(name))] stereo_remappings = [('/left', '{}/left/image_raw'.format(name)), ('/right', '{}/right/image_raw'.format(name))] args = [] remappings = [] - if type=="mono": + if type == "mono": args = mono_args remappings = mono_remappings else: @@ -68,7 +68,6 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): - depthai_prefix = get_package_share_directory("depthai_ros_driver") declared_arguments = [ DeclareLaunchArgument("name", default_value="oak"), DeclareLaunchArgument("size", default_value="8x6"), diff --git a/depthai_ros_driver/package.xml b/depthai_ros_driver/package.xml index 0d16df6e..4653a9ca 100644 --- a/depthai_ros_driver/package.xml +++ b/depthai_ros_driver/package.xml @@ -6,7 +6,6 @@ Depthai ROS Monolithic node. Adam Serafin Sachin Guruswamy - Adam Serafin MIT @@ -26,9 +25,6 @@ image_pipeline depthai depthai_bridge - ament_lint_auto - ament_lint_common - ament_cmake From 2a41a2fa679fc290fc0ad94de31d80bdab00b4da Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 15 Feb 2023 17:17:08 +0000 Subject: [PATCH 6/6] added usb port id --- README.md | 2 +- depthai-ros/CHANGELOG.rst | 1 + depthai_ros_driver/config/camera.yaml | 1 + .../include/depthai_ros_driver/camera.hpp | 1 + depthai_ros_driver/src/camera.cpp | 43 ++++++++++++------- .../param_handlers/camera_param_handler.cpp | 1 + 6 files changed, 32 insertions(+), 17 deletions(-) diff --git a/README.md b/README.md index ffba31a3..9ec9b6d5 100644 --- a/README.md +++ b/README.md @@ -126,7 +126,7 @@ As for the parameters themselves, there are a few crucial ones that decide on ho This tells the camera whether it should load stereo components. Default set to `RGBD`. * `camera.i_nn_type` can be either `none`, `rgb` or `spatial`. This is responsible for whether the NN that we load should also take depth information (and for example provide detections in 3D format). Default set to `spatial` -* `camera.i_mx_id`/`camera.i_ip` are for connecting to a specific camera. If not set, it automatically connects to the next available device. +* `camera.i_mx_id`/`camera.i_ip`/`camera.i_usb_port_id` are for connecting to a specific camera. If not set, it automatically connects to the next available device. You can get those parameters from logs by running the default launch file. * `nn.i_nn_config_path` represents path to JSON that contains information on what type of NN to load, and what parameters to use. Currently we provide options to load MobileNet, Yolo and Segmentation (not in spatial) models. To see their example configs, navigate to `depthai_ros_driver/config/nn`. Defaults to `mobilenet.json` from `depthai_ros_driver` To use provided example NN's, you can set the path to: diff --git a/depthai-ros/CHANGELOG.rst b/depthai-ros/CHANGELOG.rst index 840212d6..3b71b349 100644 --- a/depthai-ros/CHANGELOG.rst +++ b/depthai-ros/CHANGELOG.rst @@ -5,6 +5,7 @@ Changelog for package depthai-ros 2.6.3 (2023-02-10) ___________ * Camera calibration updates +* Option to connect to the device via USB port id 2.6.2 (2023-02-01) ___________ diff --git a/depthai_ros_driver/config/camera.yaml b/depthai_ros_driver/config/camera.yaml index b4a84cce..af4150d5 100644 --- a/depthai_ros_driver/config/camera.yaml +++ b/depthai_ros_driver/config/camera.yaml @@ -9,6 +9,7 @@ i_mx_id: '' i_nn_type: spatial i_pipeline_type: RGBD + i_usb_port_id: '' i_usb_speed: SUPER_PLUS left: i_board_socket_id: 1 diff --git a/depthai_ros_driver/include/depthai_ros_driver/camera.hpp b/depthai_ros_driver/include/depthai_ros_driver/camera.hpp index 3164118d..bdb55683 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/camera.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/camera.hpp @@ -32,5 +32,6 @@ class Camera : public rclcpp::Node { std::shared_ptr pipeline; std::shared_ptr device; std::vector> daiNodes; + bool camRunning = false; }; } // namespace depthai_ros_driver \ No newline at end of file diff --git a/depthai_ros_driver/src/camera.cpp b/depthai_ros_driver/src/camera.cpp index 3e9bc845..a2af2149 100644 --- a/depthai_ros_driver/src/camera.cpp +++ b/depthai_ros_driver/src/camera.cpp @@ -70,49 +70,60 @@ void Camera::setupQueues() { } void Camera::startDevice() { - dai::DeviceInfo info; - auto mxid = ph->getParam(this, "i_mx_id"); - auto ip = ph->getParam(this, "i_ip"); - if(!mxid.empty()) { - } else if(!ip.empty()) { - } rclcpp::Rate r(1.0); - bool cam_running = false; - std::vector availableDevices = dai::Device::getAllAvailableDevices(); - while(!cam_running) { + while(!camRunning) { + auto mxid = ph->getParam(this, "i_mx_id"); + auto ip = ph->getParam(this, "i_ip"); + auto usb_id = ph->getParam(this, "i_usb_port_id"); try { - dai::UsbSpeed speed = ph->getUSBSpeed(this); - if(mxid.empty() && ip.empty()) { + if(mxid.empty() && ip.empty() && usb_id.empty()) { RCLCPP_INFO(this->get_logger(), "No ip/mxid specified, connecting to the next available device."); device = std::make_shared(); + camRunning = true; } else { + std::vector availableDevices = dai::Device::getAllAvailableDevices(); + if(availableDevices.size() == 0) { + throw std::runtime_error("No devices detected!"); + } + dai::UsbSpeed speed = ph->getUSBSpeed(this); for(const auto& info : availableDevices) { if(!mxid.empty() && info.getMxId() == mxid) { RCLCPP_INFO(this->get_logger(), "Connecting to the camera using mxid: %s", mxid.c_str()); if(info.state == X_LINK_UNBOOTED || info.state == X_LINK_BOOTLOADER) { device = std::make_shared(info, speed); + camRunning = true; } else if(info.state == X_LINK_BOOTED) { - throw std::runtime_error("Device with mxid %s is already booted in different process."); + throw std::runtime_error("Device is already booted in different process."); } } else if(!ip.empty() && info.name == ip) { RCLCPP_INFO(this->get_logger(), "Connecting to the camera using ip: %s", ip.c_str()); if(info.state == X_LINK_UNBOOTED || info.state == X_LINK_BOOTLOADER) { device = std::make_shared(info); + camRunning = true; + } else if(info.state == X_LINK_BOOTED) { + throw std::runtime_error("Device is already booted in different process."); + } + } else if(!usb_id.empty() && info.name == usb_id) { + RCLCPP_INFO(this->get_logger(), "Connecting to the camera using USB ID: %s", usb_id.c_str()); + if(info.state == X_LINK_UNBOOTED || info.state == X_LINK_BOOTLOADER) { + device = std::make_shared(info); + camRunning = true; } else if(info.state == X_LINK_BOOTED) { - throw std::runtime_error("Device with ip %s is already booted in different process."); + throw std::runtime_error("Device is already booted in different process."); } + } else { + RCLCPP_INFO(this->get_logger(), "Device info: MXID: %s, Name: %s", info.getMxId().c_str(), info.name.c_str()); + throw std::runtime_error("Unable to connect to the device, check if parameters match with given info."); } } } - cam_running = true; } catch(const std::runtime_error& e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); } r.sleep(); } - auto deviceName = device->getMxId(); - RCLCPP_INFO(this->get_logger(), "Camera %s connected!", deviceName.c_str()); + RCLCPP_INFO(this->get_logger(), "Camera with MXID: %s and Name: %s connected!", device->getMxId().c_str(), device->getDeviceInfo().name.c_str()); auto protocol = device->getDeviceInfo().getXLinkDeviceDesc().protocol; if(protocol != XLinkProtocol_t::X_LINK_TCP_IP) { diff --git a/depthai_ros_driver/src/param_handlers/camera_param_handler.cpp b/depthai_ros_driver/src/param_handlers/camera_param_handler.cpp index 0de367e0..3aa257b7 100644 --- a/depthai_ros_driver/src/param_handlers/camera_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/camera_param_handler.cpp @@ -27,6 +27,7 @@ void CameraParamHandler::declareParams(rclcpp::Node* node) { declareAndLogParam(node, "i_usb_speed", "SUPER_PLUS"); declareAndLogParam(node, "i_mx_id", ""); declareAndLogParam(node, "i_ip", ""); + declareAndLogParam(node, "i_usb_port_id", ""); declareAndLogParam(node, "i_laser_dot_brightness", 800, getRangedIntDescriptor(0, 1200)); declareAndLogParam(node, "i_floodlight_brightness", 0, getRangedIntDescriptor(0, 1500)); }