diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md new file mode 100644 index 00000000..06987a25 --- /dev/null +++ b/.github/pull_request_template.md @@ -0,0 +1,15 @@ +## Overview +Author: + +## Issue +Issue link (if present): +Issue description: +Related PRs + +## Changes +ROS distro: +List of changes: + +## Testing +Hardware used: +Depthai library version: diff --git a/README.md b/README.md index 3c949c40..1394aa9b 100644 --- a/README.md +++ b/README.md @@ -185,6 +185,26 @@ As for the parameters themselves, there are a few crucial ones that decide on ho * `Depth` - Publishes only depth stream, no NN available * `CamArray` - Publishes streams for all detected sensors, no NN available This tells the camera whether it should load stereo components. Default set to `RGBD`. +It is also possible to create a custom pipeline since all types are defined as plugins. + +To do that, you can create a custom package (let's say `test_plugins`), create an executable in that package (`test_plugins.cpp`). Inside that file, define a cusom plugin that inherits from `depthai_ros_driver::pipeline_gen::BasePipeline` and overrides `createPipeline` method. + +After that export plugin, for example: + +```c++ +#include +PLUGINLIB_EXPORT_CLASS(test_plugins::Test, depthai_ros_driver::pipeline_gen::BasePipeline) +``` +Add plugin definition: +```xml + + + Test Pipeline. + + +``` + +Now you can use created plugin as pipeline, just set `camera.i_pipeline_type` to `test_plugins::Test`. * `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`/`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. diff --git a/depthai-ros/CHANGELOG.rst b/depthai-ros/CHANGELOG.rst index d7a61e1b..22e5e1a3 100644 --- a/depthai-ros/CHANGELOG.rst +++ b/depthai-ros/CHANGELOG.rst @@ -2,7 +2,15 @@ Changelog for package depthai-ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.7.2 (20230-5-08) +2.7.3 (20230-06-16) + +* Pipeline generation as a plugin +* Fixed bounding box generation issue +* Stereo rectified streams publishing +* Camera trigger mechanisms +* Brightness filter + +2.7.2 (20230-05-08) * IMU improvements 2.7.1 (2023-03-29) diff --git a/depthai-ros/CMakeLists.txt b/depthai-ros/CMakeLists.txt index d69c7aa3..890f0ec9 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.2 LANGUAGES CXX C) +project(depthai-ros VERSION 2.7.3 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) diff --git a/depthai-ros/package.xml b/depthai-ros/package.xml index e3f6e0e6..8b354deb 100644 --- a/depthai-ros/package.xml +++ b/depthai-ros/package.xml @@ -1,7 +1,7 @@ depthai-ros - 2.7.2 + 2.7.3 The depthai-ros package diff --git a/depthai_bridge/CMakeLists.txt b/depthai_bridge/CMakeLists.txt index 41041145..e3d077e9 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.2 LANGUAGES CXX C) +project(depthai_bridge VERSION 2.7.3 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) diff --git a/depthai_bridge/include/depthai_bridge/ImuConverter.hpp b/depthai_bridge/include/depthai_bridge/ImuConverter.hpp index e74a6f99..74f8a471 100644 --- a/depthai_bridge/include/depthai_bridge/ImuConverter.hpp +++ b/depthai_bridge/include/depthai_bridge/ImuConverter.hpp @@ -32,7 +32,8 @@ class ImuConverter { double angular_velocity_cov = 0.0, double rotation_cov = 0.0, double magnetic_field_cov = 0.0, - bool enable_rotation = false); + bool enable_rotation = false, + bool getBaseDeviceTimestamp = false); ~ImuConverter(); void toRosMsg(std::shared_ptr inData, std::deque& outImuMsgs); void toRosDaiMsg(std::shared_ptr inData, std::deque& outImuMsgs); @@ -112,6 +113,7 @@ class ImuConverter { ImuSyncMethod _syncMode; std::chrono::time_point _steadyBaseTime; rclcpp::Time _rosBaseTime; + bool _getBaseDeviceTimestamp; void fillImuMsg(dai::IMUReportAccelerometer report, ImuMsgs::Imu& msg); void fillImuMsg(dai::IMUReportGyroscope report, ImuMsgs::Imu& msg); @@ -124,7 +126,7 @@ class ImuConverter { void fillImuMsg(dai::IMUReportMagneticField report, depthai_ros_msgs::msg::ImuWithMagneticField& msg); template - void CreateUnitMessage(I first, S second, T third, F fourth, M& msg, dai::Timestamp timestamp) { + void CreateUnitMessage(I first, S second, T third, F fourth, M& msg, std::chrono::_V2::steady_clock::time_point timestamp) { fillImuMsg(first, msg); fillImuMsg(second, msg); fillImuMsg(third, msg); @@ -132,7 +134,7 @@ class ImuConverter { msg.header.frame_id = _frameName; - msg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, timestamp.get()); + msg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, timestamp); } template @@ -162,7 +164,12 @@ class ImuConverter { const double alpha = diff.count() / dt; I interp = lerpImu(interp0, interp1, alpha); M msg; - CreateUnitMessage(interp, currSecond, currThird, currFourth, msg, currSecond.timestamp); + std::chrono::_V2::steady_clock::time_point tstamp; + if(_getBaseDeviceTimestamp) + tstamp = currSecond.getTimestampDevice(); + else + tstamp = currSecond.getTimestamp(); + CreateUnitMessage(interp, currSecond, currThird, currFourth, msg, tstamp); imuMsgs.push_back(msg); second.pop_front(); third.pop_front(); diff --git a/depthai_bridge/package.xml b/depthai_bridge/package.xml index 49e75867..e84f6e96 100644 --- a/depthai_bridge/package.xml +++ b/depthai_bridge/package.xml @@ -1,7 +1,7 @@ depthai_bridge - 2.7.2 + 2.7.3 The depthai_bridge package Sachin Guruswamy diff --git a/depthai_bridge/src/ImuConverter.cpp b/depthai_bridge/src/ImuConverter.cpp index baa937bd..54eea829 100644 --- a/depthai_bridge/src/ImuConverter.cpp +++ b/depthai_bridge/src/ImuConverter.cpp @@ -13,7 +13,8 @@ ImuConverter::ImuConverter(const std::string& frameName, double angular_velocity_cov, double rotation_cov, double magnetic_field_cov, - bool enable_rotation) + bool enable_rotation, + bool getBaseDeviceTimestamp) : _frameName(frameName), _syncMode(syncMode), _linear_accel_cov(linear_accel_cov), @@ -22,7 +23,8 @@ ImuConverter::ImuConverter(const std::string& frameName, _magnetic_field_cov(magnetic_field_cov), _enable_rotation(enable_rotation), _sequenceNum(0), - _steadyBaseTime(std::chrono::steady_clock::now()) { + _steadyBaseTime(std::chrono::steady_clock::now()), + _getBaseDeviceTimestamp(getBaseDeviceTimestamp) { _rosBaseTime = rclcpp::Clock().now(); } @@ -91,7 +93,13 @@ void ImuConverter::toRosMsg(std::shared_ptr inData, std::dequepackets[i].rotationVector; auto magn = inData->packets[i].magneticField; ImuMsgs::Imu msg; - CreateUnitMessage(accel, gyro, rot, magn, msg, accel.timestamp); + std::chrono::_V2::steady_clock::time_point tstamp; + if(_getBaseDeviceTimestamp) + tstamp = accel.getTimestampDevice(); + else + tstamp = accel.getTimestamp(); + + CreateUnitMessage(accel, gyro, rot, magn, msg, tstamp); outImuMsgs.push_back(msg); } } @@ -107,7 +115,13 @@ void ImuConverter::toRosDaiMsg(std::shared_ptr inData, std::deque< auto rot = inData->packets[i].rotationVector; auto magn = inData->packets[i].magneticField; depthai_ros_msgs::msg::ImuWithMagneticField msg; - CreateUnitMessage(accel, gyro, rot, magn, msg, accel.timestamp); + std::chrono::_V2::steady_clock::time_point tstamp; + if(_getBaseDeviceTimestamp) + tstamp = accel.getTimestampDevice(); + else + tstamp = accel.getTimestamp(); + + CreateUnitMessage(accel, gyro, rot, magn, msg, tstamp); outImuMsgs.push_back(msg); } } diff --git a/depthai_descriptions/CMakeLists.txt b/depthai_descriptions/CMakeLists.txt index 541f4678..cdc65c8f 100644 --- a/depthai_descriptions/CMakeLists.txt +++ b/depthai_descriptions/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(depthai_descriptions VERSION 2.7.2) +project(depthai_descriptions VERSION 2.7.3) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/depthai_descriptions/launch/urdf.launch b/depthai_descriptions/launch/urdf.launch deleted file mode 100644 index 2d9dc6af..00000000 --- a/depthai_descriptions/launch/urdf.launch +++ /dev/null @@ -1,33 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/depthai_descriptions/package.xml b/depthai_descriptions/package.xml index 2e74de72..474adbb5 100644 --- a/depthai_descriptions/package.xml +++ b/depthai_descriptions/package.xml @@ -1,7 +1,7 @@ depthai_descriptions - 2.7.2 + 2.7.3 The depthai_descriptions package Sachin Guruswamy diff --git a/depthai_descriptions/urdf/include/depthai_macro.urdf.xacro b/depthai_descriptions/urdf/include/depthai_macro.urdf.xacro index fc1c9e6f..df541ec7 100644 --- a/depthai_descriptions/urdf/include/depthai_macro.urdf.xacro +++ b/depthai_descriptions/urdf/include/depthai_macro.urdf.xacro @@ -24,7 +24,6 @@ - diff --git a/depthai_descriptions/urdf/models/OAK-D-PRO-W.stl b/depthai_descriptions/urdf/models/OAK-D-PRO-W.stl new file mode 100644 index 00000000..8540d645 Binary files /dev/null and b/depthai_descriptions/urdf/models/OAK-D-PRO-W.stl differ diff --git a/depthai_examples/CMakeLists.txt b/depthai_examples/CMakeLists.txt index c5a097b4..f343ebff 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.2 LANGUAGES CXX C) +project(depthai_examples VERSION 2.7.3 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) diff --git a/depthai_examples/launch/stereo_inertial_node.launch.py b/depthai_examples/launch/stereo_inertial_node.launch.py index 1cc34900..ee5f24c7 100644 --- a/depthai_examples/launch/stereo_inertial_node.launch.py +++ b/depthai_examples/launch/stereo_inertial_node.launch.py @@ -153,6 +153,7 @@ def generate_launch_description(): 'cam_yaw', default_value=cam_yaw, description='Yaw orientation of the camera with respect to the base frame.') + declare_lrcheck_cmd = DeclareLaunchArgument( 'lrcheck', @@ -298,6 +299,7 @@ def generate_launch_description(): 'enableRviz', default_value=enableRviz, description='When True create a RVIZ window.') + urdf_launch = IncludeLaunchDescription( diff --git a/depthai_examples/package.xml b/depthai_examples/package.xml index 965e7122..bee9333f 100644 --- a/depthai_examples/package.xml +++ b/depthai_examples/package.xml @@ -1,7 +1,7 @@ depthai_examples - 2.7.2 + 2.7.3 The depthai_examples package diff --git a/depthai_filters/CMakeLists.txt b/depthai_filters/CMakeLists.txt index d40d3406..85c10140 100644 --- a/depthai_filters/CMakeLists.txt +++ b/depthai_filters/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(depthai_filters VERSION 2.7.2 LANGUAGES CXX C) +project(depthai_filters VERSION 2.7.3 LANGUAGES CXX C) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/depthai_filters/package.xml b/depthai_filters/package.xml index 4a7143c2..691417dd 100644 --- a/depthai_filters/package.xml +++ b/depthai_filters/package.xml @@ -2,7 +2,7 @@ depthai_filters - 2.7.2 + 2.7.3 Depthai filters package Adam Serafin MIT diff --git a/depthai_ros_driver/CMakeLists.txt b/depthai_ros_driver/CMakeLists.txt index 07622d61..73723316 100644 --- a/depthai_ros_driver/CMakeLists.txt +++ b/depthai_ros_driver/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.22) -project(depthai_ros_driver VERSION 2.7.2) +project(depthai_ros_driver VERSION 2.7.3) set(CMAKE_POSITION_INDEPENDENT_CODE ON) set(CMAKE_BUILD_SHARED_LIBS ON) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) @@ -52,6 +52,7 @@ depthai rclcpp rclcpp_components std_srvs +pluginlib ) @@ -103,7 +104,8 @@ target_link_libraries( add_library( ${PROJECT_NAME} SHARED src/camera.cpp - src/pipeline_generator.cpp + src/pipeline/pipeline_generator.cpp + src/pipeline/base_types.cpp ) ament_target_dependencies(${PROJECT_NAME} ${CAM_DEPS}) @@ -116,18 +118,28 @@ target_link_libraries( rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::Camera") +pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) ament_export_include_directories(include) +ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(${COMMON_DEPS} ${SENSOR_DEPS} ${NN_DEPS} ${CAM_DEPS}) + install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) -install( - TARGETS - ${PROJECT_NAME} ${COMMON_LIB_NAME} ${SENSOR_LIB_NAME} ${NN_LIB_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin +install(TARGETS ${PROJECT_NAME} ${SENSOR_LIB_NAME} +${NN_LIB_NAME} +${COMMON_LIB_NAME} +EXPORT ${PROJECT_NAME}Targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include ) +install(EXPORT ${PROJECT_NAME}Targets + DESTINATION share/${PROJECT_NAME}/cmake) + ament_python_install_package(${PROJECT_NAME}) # Install Python executables install( @@ -139,6 +151,7 @@ install( ament_export_include_directories( include ) + install( DIRECTORY include/ DESTINATION include diff --git a/depthai_ros_driver/config/camera.yaml b/depthai_ros_driver/config/camera.yaml index 1082cb2a..2ebd09db 100644 --- a/depthai_ros_driver/config/camera.yaml +++ b/depthai_ros_driver/config/camera.yaml @@ -18,6 +18,7 @@ i_acc_freq: 400 i_acc_cov: 0.0 i_batch_report_threshold: 1 + i_get_base_device_timestamp: false i_enable_rotation: false i_gyro_cov: 0.0 i_gyro_freq: 400 @@ -31,7 +32,7 @@ left: i_board_socket_id: 1 i_calibration_file: '' - i_get_base_device_timestamp: true + i_get_base_device_timestamp: false i_fps: 30.0 i_height: 720 i_low_bandwidth: false @@ -53,7 +54,7 @@ rgb: i_board_socket_id: 0 i_simulate_from_topic: false - i_get_base_device_timestamp: true + i_get_base_device_timestamp: false i_disable_node: false i_calibration_file: '' i_enable_preview: false @@ -82,7 +83,7 @@ right: i_board_socket_id: 2 i_calibration_file: '' - i_get_base_device_timestamp: true + i_get_base_device_timestamp: false i_fps: 30.0 i_height: 720 i_low_bandwidth: false @@ -98,7 +99,7 @@ r_set_man_exposure: false stereo: i_align_depth: true - i_get_base_device_timestamp: true + i_get_base_device_timestamp: false i_output_disparity: false i_bilateral_sigma: 0 i_board_socket_id: 0 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 e222d53c..f190f64b 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 @@ -43,9 +43,19 @@ class Detection : public BaseNode { void setupQueues(std::shared_ptr device) override { nnQ = device->getOutputQueue(nnQName, ph->getParam("i_max_q_size"), false); auto tfPrefix = getTFPrefix("rgb"); + int width; + int height; + if(ph->getParam("i_disable_resize")) { + width = getROSNode()->get_parameter("rgb.i_preview_size").as_int(); + height = getROSNode()->get_parameter("rgb.i_preview_size").as_int(); + } + else{ + width = imageManip->initialConfig.getResizeConfig().width; + height = imageManip->initialConfig.getResizeConfig().height; + } detConverter = std::make_unique(tfPrefix + "_camera_optical_frame", - imageManip->initialConfig.getResizeConfig().width, - imageManip->initialConfig.getResizeConfig().height, + width, + height, false, ph->getParam("i_get_base_device_timestamp")); detPub = getROSNode()->template create_publisher("~/" + getName() + "/detections", 10); @@ -59,9 +69,9 @@ class Detection : public BaseNode { infoManager->setCameraInfo(sensor_helpers::getCalibInfo(getROSNode()->get_logger(), *imageConverter, device, - dai::CameraBoardSocket::RGB, - imageManip->initialConfig.getResizeWidth(), - imageManip->initialConfig.getResizeWidth())); + static_cast(ph->getParam("i_board_socket_id")), + width, + height)); ptPub = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/passthrough/image_raw"); ptQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *imageConverter, ptPub, infoManager)); 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 9f4a8c2b..a1241d01 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 @@ -45,9 +45,19 @@ class SpatialDetection : public BaseNode { void setupQueues(std::shared_ptr device) override { nnQ = device->getOutputQueue(nnQName, ph->getParam("i_max_q_size"), false); auto tfPrefix = getTFPrefix("rgb"); + int width; + int height; + if(ph->getParam("i_disable_resize")) { + width = getROSNode()->get_parameter("rgb.i_preview_size").as_int(); + height = getROSNode()->get_parameter("rgb.i_preview_size").as_int(); + } + else{ + width = imageManip->initialConfig.getResizeConfig().width; + height = imageManip->initialConfig.getResizeConfig().height; + } detConverter = std::make_unique(tfPrefix + "_camera_optical_frame", - imageManip->initialConfig.getResizeConfig().width, - imageManip->initialConfig.getResizeConfig().height, + width, + height, false, ph->getParam("i_get_base_device_timestamp")); nnQ->addCallback(std::bind(&SpatialDetection::spatialCB, this, std::placeholders::_1, std::placeholders::_2)); @@ -61,19 +71,18 @@ class SpatialDetection : public BaseNode { ptInfoMan->setCameraInfo(sensor_helpers::getCalibInfo(getROSNode()->get_logger(), *ptImageConverter, device, - dai::CameraBoardSocket::RGB, - imageManip->initialConfig.getResizeWidth(), - imageManip->initialConfig.getResizeWidth())); + static_cast(ph->getParam("i_board_socket_id")), + width, + height)); ptPub = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/passthrough/image_raw"); ptQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *ptImageConverter, ptPub, ptInfoMan)); } if(ph->getParam("i_enable_passthrough_depth")) { - dai::CameraBoardSocket socket = dai::CameraBoardSocket::RGB; + dai::CameraBoardSocket socket = static_cast(getROSNode()->get_parameter("stereo.i_board_socket_id").as_int()); if(!getROSNode()->get_parameter("stereo.i_align_depth").as_bool()) { tfPrefix = getTFPrefix("right"); - socket = dai::CameraBoardSocket::RIGHT; }; ptDepthQ = device->getOutputQueue(ptDepthQName, ph->getParam("i_max_q_size"), false); ptDepthImageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); diff --git a/depthai_ros_driver/include/depthai_ros_driver/pipeline/base_pipeline.hpp b/depthai_ros_driver/include/depthai_ros_driver/pipeline/base_pipeline.hpp new file mode 100644 index 00000000..2959e68b --- /dev/null +++ b/depthai_ros_driver/include/depthai_ros_driver/pipeline/base_pipeline.hpp @@ -0,0 +1,62 @@ +#pragma once + +#include +#include +#include +#include + +#include "depthai_ros_driver/dai_nodes/base_node.hpp" +#include "depthai_ros_driver/dai_nodes/nn/nn_helpers.hpp" +#include "depthai_ros_driver/dai_nodes/nn/nn_wrapper.hpp" +#include "depthai_ros_driver/dai_nodes/nn/spatial_nn_wrapper.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" +#include "depthai_ros_driver/dai_nodes/stereo.hpp" + +namespace dai { +class Pipeline; +class Device; +} // namespace dai + +namespace rclcpp { +class Node; +} + +namespace depthai_ros_driver { +namespace pipeline_gen { +enum class NNType { None, RGB, Spatial }; +class BasePipeline { + public: + ~BasePipeline() = default; + std::unique_ptr createNN(rclcpp::Node* node, std::shared_ptr pipeline, dai_nodes::BaseNode& daiNode) { + auto nn = std::make_unique("nn", node, pipeline); + daiNode.link(nn->getInput(), static_cast(dai_nodes::link_types::RGBLinkType::preview)); + return nn; + } + std::unique_ptr createSpatialNN(rclcpp::Node* node, + std::shared_ptr pipeline, + dai_nodes::BaseNode& daiNode, + dai_nodes::BaseNode& daiStereoNode) { + auto nn = std::make_unique("nn", node, pipeline); + daiNode.link(nn->getInput(static_cast(dai_nodes::nn_helpers::link_types::SpatialNNLinkType::input)), + static_cast(dai_nodes::link_types::RGBLinkType::preview)); + daiStereoNode.link(nn->getInput(static_cast(dai_nodes::nn_helpers::link_types::SpatialNNLinkType::inputDepth))); + return nn; + } + + virtual std::vector> createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& nnType) = 0; + + protected: + BasePipeline(){}; + const std::string alphabet = "abcdefghijklmnopqrstuvwxyz"; + std::unordered_map nnTypeMap = { + {"", NNType::None}, + {"NONE", NNType::None}, + {"RGB", NNType::RGB}, + {"SPATIAL", NNType::Spatial}, + }; +}; +} // namespace pipeline_gen +} // namespace depthai_ros_driver \ No newline at end of file diff --git a/depthai_ros_driver/include/depthai_ros_driver/pipeline/base_types.hpp b/depthai_ros_driver/include/depthai_ros_driver/pipeline/base_types.hpp new file mode 100644 index 00000000..a78c6333 --- /dev/null +++ b/depthai_ros_driver/include/depthai_ros_driver/pipeline/base_types.hpp @@ -0,0 +1,65 @@ +#pragma once + +#include +#include +#include +#include + +#include "depthai_ros_driver/dai_nodes/base_node.hpp" +#include "depthai_ros_driver/pipeline/base_pipeline.hpp" + +namespace dai { +class Pipeline; +class Device; +} // namespace dai + +namespace rclcpp { +class Node; +} + +namespace depthai_ros_driver { +namespace pipeline_gen { +class RGB : public BasePipeline { + public: + std::vector> createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& nnType) override; +}; +class RGBD : public BasePipeline { + public: + std::vector> createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& nnType) override; +}; +class RGBStereo : public BasePipeline { + public: + std::vector> createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& nnType) override; +}; +class Stereo : public BasePipeline { + public: + std::vector> createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& nnType) override; +}; +class Depth : public BasePipeline { + public: + std::vector> createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& nnType) override; +}; +class CamArray : public BasePipeline { + public: + std::vector> createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& nnType) override; +}; +} // namespace pipeline_gen +} // namespace depthai_ros_driver \ No newline at end of file diff --git a/depthai_ros_driver/include/depthai_ros_driver/pipeline_generator.hpp b/depthai_ros_driver/include/depthai_ros_driver/pipeline/pipeline_generator.hpp similarity index 68% rename from depthai_ros_driver/include/depthai_ros_driver/pipeline_generator.hpp rename to depthai_ros_driver/include/depthai_ros_driver/pipeline/pipeline_generator.hpp index 6a9ff91c..ff7fdefd 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/pipeline_generator.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/pipeline/pipeline_generator.hpp @@ -19,17 +19,11 @@ class Node; namespace depthai_ros_driver { namespace pipeline_gen { enum class PipelineType { RGB, RGBD, RGBStereo, Stereo, Depth, CamArray }; -enum class NNType { None, RGB, Spatial }; + class PipelineGenerator { public: - PipelineGenerator(){}; ~PipelineGenerator() = default; PipelineType validatePipeline(rclcpp::Node* node, PipelineType type, int sensorNum); - std::unique_ptr createNN(rclcpp::Node* node, std::shared_ptr pipeline, dai_nodes::BaseNode& daiNode); - std::unique_ptr createSpatialNN(rclcpp::Node* node, - std::shared_ptr pipeline, - dai_nodes::BaseNode& daiNode, - dai_nodes::BaseNode& daiStereoNode); std::vector> createPipeline(rclcpp::Node* node, std::shared_ptr device, std::shared_ptr pipeline, @@ -37,20 +31,19 @@ class PipelineGenerator { const std::string& nnType, bool enableImu); - private: + protected: + std::unordered_map pluginTypeMap{{"RGB", "depthai_ros_driver::pipeline_gen::RGB"}, + {"RGBD", "depthai_ros_driver::pipeline_gen::RGBD"}, + {"RGBSTEREO", "depthai_ros_driver::pipeline_gen::RGBStereo"}, + {"STEREO", "depthai_ros_driver::pipeline_gen::Stereo"}, + {"DEPTH", "depthai_ros_driver::pipeline_gen::Depth"}, + {"CAMARRAY", "depthai_ros_driver::pipeline_gen::CamArray"}}; std::unordered_map pipelineTypeMap{{"RGB", PipelineType::RGB}, {"RGBD", PipelineType::RGBD}, {"RGBSTEREO", PipelineType::RGBStereo}, {"STEREO", PipelineType::Stereo}, {"DEPTH", PipelineType::Depth}, {"CAMARRAY", PipelineType::CamArray}}; - std::unordered_map nnTypeMap = { - {"", NNType::None}, - {"NONE", NNType::None}, - {"RGB", NNType::RGB}, - {"SPATIAL", NNType::Spatial}, - }; - const std::string alphabet = "abcdefghijklmnopqrstuvwxyz"; }; } // namespace pipeline_gen } // namespace depthai_ros_driver \ No newline at end of file diff --git a/depthai_ros_driver/include/depthai_ros_driver/utils.hpp b/depthai_ros_driver/include/depthai_ros_driver/utils.hpp index b99730ec..b370dd3b 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/utils.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/utils.hpp @@ -19,7 +19,7 @@ T getValFromMap(const std::string& name, const std::unordered_mapfirst << "\n"; } - throw std::runtime_error(stream.str()); + throw std::out_of_range(stream.str()); } } std::string getUpperCaseStr(const std::string& string); diff --git a/depthai_ros_driver/launch/rtabmap.launch.py b/depthai_ros_driver/launch/rtabmap.launch.py index 47849a77..bd36584a 100644 --- a/depthai_ros_driver/launch/rtabmap.launch.py +++ b/depthai_ros_driver/launch/rtabmap.launch.py @@ -59,8 +59,8 @@ def launch_setup(context, *args, **kwargs): target_container=name+"_container", composable_node_descriptions=[ ComposableNode( - package='rtabmap_ros', - plugin='rtabmap_ros::RGBDOdometry', + package='rtabmap_odom', + plugin='rtabmap_odom::RGBDOdometry', name='rgbd_odometry', parameters=parameters, remappings=remappings, @@ -72,8 +72,8 @@ def launch_setup(context, *args, **kwargs): target_container=name+"_container", composable_node_descriptions=[ ComposableNode( - package='rtabmap_ros', - plugin='rtabmap_ros::CoreWrapper', + package='rtabmap_slam', + plugin='rtabmap_slam::CoreWrapper', name='rtabmap', parameters=parameters, remappings=remappings, @@ -82,8 +82,8 @@ def launch_setup(context, *args, **kwargs): ), Node( - package="rtabmap_ros", - executable="rtabmapviz", + package="rtabmap_viz", + executable="rtabmap_viz", output="screen", parameters=parameters, remappings=remappings, diff --git a/depthai_ros_driver/package.xml b/depthai_ros_driver/package.xml index d37f2a4d..fa2adb1e 100644 --- a/depthai_ros_driver/package.xml +++ b/depthai_ros_driver/package.xml @@ -2,7 +2,7 @@ depthai_ros_driver - 2.7.2 + 2.7.3 Depthai ROS Monolithic node. Adam Serafin Sachin Guruswamy @@ -28,6 +28,7 @@ depthai_descriptions depthai_ros_msgs depthai_examples + pluginlib ament_cmake diff --git a/depthai_ros_driver/plugins.xml b/depthai_ros_driver/plugins.xml new file mode 100644 index 00000000..1dfd7d40 --- /dev/null +++ b/depthai_ros_driver/plugins.xml @@ -0,0 +1,20 @@ + + + RGB Pipeline. + + + RGBD Pipeline. + + + RGBStereo Pipeline. + + + Stereo Pipeline. + + + Depth Pipeline. + + + CamArray Pipeline. + + \ No newline at end of file diff --git a/depthai_ros_driver/src/camera.cpp b/depthai_ros_driver/src/camera.cpp index f6c56620..e0d94a72 100644 --- a/depthai_ros_driver/src/camera.cpp +++ b/depthai_ros_driver/src/camera.cpp @@ -5,7 +5,7 @@ #include "ament_index_cpp/get_package_share_directory.hpp" #include "depthai/device/Device.hpp" #include "depthai/pipeline/Pipeline.hpp" -#include "depthai_ros_driver/pipeline_generator.hpp" +#include "depthai_ros_driver/pipeline/pipeline_generator.hpp" namespace depthai_ros_driver { diff --git a/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp b/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp index 034dc95c..e4c27e2e 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp @@ -44,7 +44,8 @@ void Imu::setupQueues(std::shared_ptr device) { ph->getParam("i_gyro_cov"), ph->getParam("i_rot_cov"), ph->getParam("i_mag_cov"), - ph->getParam("i_enable_rotation")); + ph->getParam("i_enable_rotation"), + ph->getParam("i_get_base_device_timestamp")); param_handlers::imu::ImuMsgType msgType = ph->getMsgType(); switch(msgType) { case param_handlers::imu::ImuMsgType::IMU: { 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 8dda80cd..879817e9 100644 --- a/depthai_ros_driver/src/param_handlers/imu_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/imu_param_handler.cpp @@ -18,6 +18,7 @@ void ImuParamHandler::declareParams(std::shared_ptr imu, const s }; imuMessagetTypeMap = { {"IMU", imu::ImuMsgType::IMU}, {"IMU_WITH_MAG", imu::ImuMsgType::IMU_WITH_MAG}, {"IMU_WITH_MAG_SPLIT", imu::ImuMsgType::IMU_WITH_MAG_SPLIT}}; + declareAndLogParam("i_get_base_device_timestamp", false); declareAndLogParam("i_message_type", "IMU"); declareAndLogParam("i_sync_method", "LINEAR_INTERPOLATE_ACCEL"); declareAndLogParam("i_acc_cov", 0.0); diff --git a/depthai_ros_driver/src/pipeline/base_types.cpp b/depthai_ros_driver/src/pipeline/base_types.cpp new file mode 100644 index 00000000..6432fb72 --- /dev/null +++ b/depthai_ros_driver/src/pipeline/base_types.cpp @@ -0,0 +1,155 @@ +#include "depthai_ros_driver/pipeline/base_types.hpp" + +#include "depthai/device/Device.hpp" +#include "depthai/pipeline/Pipeline.hpp" +#include "depthai_ros_driver/dai_nodes/base_node.hpp" +#include "depthai_ros_driver/dai_nodes/nn/nn_helpers.hpp" +#include "depthai_ros_driver/dai_nodes/nn/nn_wrapper.hpp" +#include "depthai_ros_driver/dai_nodes/nn/spatial_nn_wrapper.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/imu.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/sensor_wrapper.hpp" +#include "depthai_ros_driver/dai_nodes/stereo.hpp" +#include "depthai_ros_driver/pipeline/base_pipeline.hpp" +#include "depthai_ros_driver/utils.hpp" +#include "rclcpp/node.hpp" + +namespace depthai_ros_driver { +namespace pipeline_gen { + +std::vector> RGB::createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& nnType) { + std::string nTypeUpCase = utils::getUpperCaseStr(nnType); + auto nType = utils::getValFromMap(nTypeUpCase, nnTypeMap); + + std::vector> daiNodes; + auto rgb = std::make_unique("rgb", node, pipeline, device, dai::CameraBoardSocket::RGB); + switch(nType) { + case NNType::None: + break; + case NNType::RGB: { + auto nn = createNN(node, pipeline, *rgb); + daiNodes.push_back(std::move(nn)); + break; + } + case NNType::Spatial: { + RCLCPP_WARN(node->get_logger(), "Spatial NN selected, but configuration is RGB."); + } + default: + break; + } + daiNodes.push_back(std::move(rgb)); + return daiNodes; +} +std::vector> RGBD::createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& nnType) { + std::string nTypeUpCase = utils::getUpperCaseStr(nnType); + auto nType = utils::getValFromMap(nTypeUpCase, nnTypeMap); + + std::vector> daiNodes; + auto rgb = std::make_unique("rgb", node, pipeline, device, dai::CameraBoardSocket::RGB); + auto stereo = std::make_unique("stereo", node, pipeline, device); + switch(nType) { + case NNType::None: + break; + case NNType::RGB: { + auto nn = createNN(node, pipeline, *rgb); + daiNodes.push_back(std::move(nn)); + break; + } + case NNType::Spatial: { + auto nn = createSpatialNN(node, pipeline, *rgb, *stereo); + daiNodes.push_back(std::move(nn)); + break; + } + default: + break; + } + daiNodes.push_back(std::move(rgb)); + daiNodes.push_back(std::move(stereo)); + return daiNodes; +} +std::vector> RGBStereo::createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& nnType) { + std::string nTypeUpCase = utils::getUpperCaseStr(nnType); + auto nType = utils::getValFromMap(nTypeUpCase, nnTypeMap); + + std::vector> daiNodes; + auto rgb = std::make_unique("rgb", node, pipeline, device, dai::CameraBoardSocket::RGB); + auto left = std::make_unique("left", node, pipeline, device, dai::CameraBoardSocket::LEFT); + auto right = std::make_unique("right", node, pipeline, device, dai::CameraBoardSocket::RIGHT); + switch(nType) { + case NNType::None: + break; + case NNType::RGB: { + auto nn = createNN(node, pipeline, *rgb); + daiNodes.push_back(std::move(nn)); + break; + } + case NNType::Spatial: { + RCLCPP_WARN(node->get_logger(), "Spatial NN selected, but configuration is RGBStereo."); + } + default: + break; + } + daiNodes.push_back(std::move(rgb)); + daiNodes.push_back(std::move(left)); + daiNodes.push_back(std::move(right)); + return daiNodes; +} +std::vector> Stereo::createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& /*nnType*/) { + std::vector> daiNodes; + auto left = std::make_unique("left", node, pipeline, device, dai::CameraBoardSocket::LEFT); + auto right = std::make_unique("right", node, pipeline, device, dai::CameraBoardSocket::RIGHT); + daiNodes.push_back(std::move(left)); + daiNodes.push_back(std::move(right)); + return daiNodes; +} +std::vector> Depth::createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& /*nnType*/) { + std::vector> daiNodes; + auto stereo = std::make_unique("stereo", node, pipeline, device); + daiNodes.push_back(std::move(stereo)); + return daiNodes; +} +std::vector> CamArray::createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& /*nnType*/) { + std::vector> daiNodes; + int i = 0; + int j = 0; + for(auto& sensor : device->getCameraSensorNames()) { + // append letter for greater sensor number + if(i % alphabet.size() == 0) { + j++; + } + std::string nodeName(j, alphabet[i % alphabet.size()]); + auto daiNode = std::make_unique(nodeName, node, pipeline, device, sensor.first); + daiNodes.push_back(std::move(daiNode)); + i++; + }; + return daiNodes; +} +} // namespace pipeline_gen +} // namespace depthai_ros_driver + +#include + +PLUGINLIB_EXPORT_CLASS(depthai_ros_driver::pipeline_gen::RGB, depthai_ros_driver::pipeline_gen::BasePipeline) +PLUGINLIB_EXPORT_CLASS(depthai_ros_driver::pipeline_gen::RGBD, depthai_ros_driver::pipeline_gen::BasePipeline) +PLUGINLIB_EXPORT_CLASS(depthai_ros_driver::pipeline_gen::RGBStereo, depthai_ros_driver::pipeline_gen::BasePipeline) +PLUGINLIB_EXPORT_CLASS(depthai_ros_driver::pipeline_gen::Stereo, depthai_ros_driver::pipeline_gen::BasePipeline) +PLUGINLIB_EXPORT_CLASS(depthai_ros_driver::pipeline_gen::Depth, depthai_ros_driver::pipeline_gen::BasePipeline) +PLUGINLIB_EXPORT_CLASS(depthai_ros_driver::pipeline_gen::CamArray, depthai_ros_driver::pipeline_gen::BasePipeline) \ No newline at end of file diff --git a/depthai_ros_driver/src/pipeline/pipeline_generator.cpp b/depthai_ros_driver/src/pipeline/pipeline_generator.cpp new file mode 100644 index 00000000..bf0b6747 --- /dev/null +++ b/depthai_ros_driver/src/pipeline/pipeline_generator.cpp @@ -0,0 +1,71 @@ +#include "depthai_ros_driver/pipeline/pipeline_generator.hpp" + +#include "depthai/device/Device.hpp" +#include "depthai/pipeline/Pipeline.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/imu.hpp" +#include "depthai_ros_driver/pipeline/base_pipeline.hpp" +#include "depthai_ros_driver/utils.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/node.hpp" + +namespace depthai_ros_driver { +namespace pipeline_gen { +std::vector> PipelineGenerator::createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& pipelineType, + const std::string& nnType, + bool enableImu) { + RCLCPP_INFO(node->get_logger(), "Pipeline type: %s", pipelineType.c_str()); + std::string pluginType = pipelineType; + std::vector> daiNodes; + // Check if one of the default types. + try { + std::string pTypeUpCase = utils::getUpperCaseStr(pipelineType); + auto pType = utils::getValFromMap(pTypeUpCase, pipelineTypeMap); + pType = validatePipeline(node, pType, device->getCameraSensorNames().size()); + pluginType = utils::getValFromMap(pTypeUpCase, pluginTypeMap); + } catch(std::out_of_range& e) { + RCLCPP_DEBUG(node->get_logger(), "Pipeline type [%s] not found in base types, trying to load as a plugin.", pipelineType.c_str()); + } + pluginlib::ClassLoader pipelineLoader("depthai_ros_driver", "depthai_ros_driver::pipeline_gen::BasePipeline"); + + try { + std::shared_ptr pipelinePlugin = pipelineLoader.createSharedInstance(pluginType); + daiNodes = pipelinePlugin->createPipeline(node, device, pipeline, nnType); + } catch(pluginlib::PluginlibException& ex) { + RCLCPP_ERROR(node->get_logger(), "The plugin failed to load for some reason. Error: %s\n", ex.what()); + } + + if(enableImu) { + if(device->getConnectedIMU()== "NONE") { + RCLCPP_WARN(node->get_logger(), "IMU enabled but not available!"); + } else { + auto imu = std::make_unique("imu", node, pipeline, device); + daiNodes.push_back(std::move(imu)); + } + } + + RCLCPP_INFO(node->get_logger(), "Finished setting up pipeline."); + return daiNodes; +} + +PipelineType PipelineGenerator::validatePipeline(rclcpp::Node* node, PipelineType type, int sensorNum) { + if(sensorNum == 1) { + if(type != PipelineType::RGB) { + RCLCPP_ERROR(node->get_logger(), "Wrong pipeline chosen for camera as it has only one sensor. Switching to RGB."); + return PipelineType::RGB; + } + } else if(sensorNum == 2) { + if(type != PipelineType::Stereo && type != PipelineType::Depth) { + RCLCPP_ERROR(node->get_logger(), "Wrong pipeline chosen for camera as it has only stereo pair. Switching to Stereo."); + return PipelineType::Stereo; + } + } else if(sensorNum > 3 && type != PipelineType::CamArray) { + RCLCPP_ERROR(node->get_logger(), "For cameras with more than three sensors you can only use CamArray. Switching to CamArray."); + return PipelineType::CamArray; + } + return type; +} +} // namespace pipeline_gen +} // namespace depthai_ros_driver \ No newline at end of file diff --git a/depthai_ros_driver/src/pipeline_generator.cpp b/depthai_ros_driver/src/pipeline_generator.cpp deleted file mode 100644 index 11d9eb86..00000000 --- a/depthai_ros_driver/src/pipeline_generator.cpp +++ /dev/null @@ -1,170 +0,0 @@ -#include "depthai_ros_driver/pipeline_generator.hpp" - -#include "depthai/device/Device.hpp" -#include "depthai/pipeline/Pipeline.hpp" -#include "depthai_ros_driver/dai_nodes/nn/nn_helpers.hpp" -#include "depthai_ros_driver/dai_nodes/nn/nn_wrapper.hpp" -#include "depthai_ros_driver/dai_nodes/nn/spatial_nn_wrapper.hpp" -#include "depthai_ros_driver/dai_nodes/sensors/imu.hpp" -#include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" -#include "depthai_ros_driver/dai_nodes/sensors/sensor_wrapper.hpp" -#include "depthai_ros_driver/dai_nodes/stereo.hpp" -#include "depthai_ros_driver/utils.hpp" -#include "rclcpp/node.hpp" - -namespace depthai_ros_driver { -namespace pipeline_gen { -std::vector> PipelineGenerator::createPipeline(rclcpp::Node* node, - std::shared_ptr device, - std::shared_ptr pipeline, - const std::string& pipelineType, - const std::string& nnType, - bool enableImu) { - RCLCPP_INFO(node->get_logger(), "Pipeline type: %s", pipelineType.c_str()); - std::string pTypeUpCase = utils::getUpperCaseStr(pipelineType); - std::string nTypeUpCase = utils::getUpperCaseStr(nnType); - auto pType = utils::getValFromMap(pTypeUpCase, pipelineTypeMap); - pType = validatePipeline(node, pType, device->getCameraSensorNames().size()); - auto nType = utils::getValFromMap(nTypeUpCase, nnTypeMap); - std::vector> daiNodes; - switch(pType) { - case PipelineType::RGB: { - auto rgb = std::make_unique("rgb", node, pipeline, device, dai::CameraBoardSocket::RGB); - switch(nType) { - case NNType::None: - break; - case NNType::RGB: { - auto nn = createNN(node, pipeline, *rgb); - daiNodes.push_back(std::move(nn)); - break; - } - case NNType::Spatial: { - RCLCPP_WARN(node->get_logger(), "Spatial NN selected, but configuration is RGB."); - } - default: - break; - } - daiNodes.push_back(std::move(rgb)); - break; - } - - case PipelineType::RGBD: { - auto rgb = std::make_unique("rgb", node, pipeline, device, dai::CameraBoardSocket::RGB); - auto stereo = std::make_unique("stereo", node, pipeline, device); - switch(nType) { - case NNType::None: - break; - case NNType::RGB: { - auto nn = createNN(node, pipeline, *rgb); - daiNodes.push_back(std::move(nn)); - break; - } - case NNType::Spatial: { - auto nn = createSpatialNN(node, pipeline, *rgb, *stereo); - daiNodes.push_back(std::move(nn)); - break; - } - default: - break; - } - daiNodes.push_back(std::move(rgb)); - daiNodes.push_back(std::move(stereo)); - break; - } - case PipelineType::RGBStereo: { - auto rgb = std::make_unique("rgb", node, pipeline, device, dai::CameraBoardSocket::RGB); - auto left = std::make_unique("left", node, pipeline, device, dai::CameraBoardSocket::LEFT); - auto right = std::make_unique("right", node, pipeline, device, dai::CameraBoardSocket::RIGHT); - switch(nType) { - case NNType::None: - break; - case NNType::RGB: { - auto nn = createNN(node, pipeline, *rgb); - daiNodes.push_back(std::move(nn)); - break; - } - case NNType::Spatial: { - RCLCPP_WARN(node->get_logger(), "Spatial NN selected, but configuration is RGBStereo."); - } - default: - break; - } - daiNodes.push_back(std::move(rgb)); - daiNodes.push_back(std::move(left)); - daiNodes.push_back(std::move(right)); - break; - } - case PipelineType::Stereo: { - auto left = std::make_unique("left", node, pipeline, device, dai::CameraBoardSocket::LEFT); - auto right = std::make_unique("right", node, pipeline, device, dai::CameraBoardSocket::RIGHT); - daiNodes.push_back(std::move(left)); - daiNodes.push_back(std::move(right)); - break; - } - case PipelineType::Depth: { - auto stereo = std::make_unique("stereo", node, pipeline, device); - daiNodes.push_back(std::move(stereo)); - break; - } - case PipelineType::CamArray: { - int i = 0; - int j = 0; - for(auto& sensor : device->getCameraSensorNames()) { - // append letter for greater sensor number - if(i % alphabet.size() == 0) { - j++; - } - std::string nodeName(j, alphabet[i % alphabet.size()]); - auto daiNode = std::make_unique(nodeName, node, pipeline, device, sensor.first); - daiNodes.push_back(std::move(daiNode)); - i++; - }; - break; - } - default: { - std::string configuration = pipelineType; - throw std::runtime_error("UNKNOWN PIPELINE TYPE SPECIFIED/CAMERA DOESN'T SUPPORT GIVEN PIPELINE. Configuration: " + configuration); - } - } - if(enableImu) { - auto imu = std::make_unique("imu", node, pipeline, device); - daiNodes.push_back(std::move(imu)); - } - - RCLCPP_INFO(node->get_logger(), "Finished setting up pipeline."); - return daiNodes; -} -std::unique_ptr PipelineGenerator::createNN(rclcpp::Node* node, std::shared_ptr pipeline, dai_nodes::BaseNode& daiNode) { - auto nn = std::make_unique("nn", node, pipeline); - daiNode.link(nn->getInput(), static_cast(dai_nodes::link_types::RGBLinkType::preview)); - return nn; -} -std::unique_ptr PipelineGenerator::createSpatialNN(rclcpp::Node* node, - std::shared_ptr pipeline, - dai_nodes::BaseNode& daiNode, - dai_nodes::BaseNode& daiStereoNode) { - auto nn = std::make_unique("nn", node, pipeline); - daiNode.link(nn->getInput(static_cast(dai_nodes::nn_helpers::link_types::SpatialNNLinkType::input)), - static_cast(dai_nodes::link_types::RGBLinkType::preview)); - daiStereoNode.link(nn->getInput(static_cast(dai_nodes::nn_helpers::link_types::SpatialNNLinkType::inputDepth))); - return nn; -} -PipelineType PipelineGenerator::validatePipeline(rclcpp::Node* node, PipelineType type, int sensorNum) { - if(sensorNum == 1) { - if(type != PipelineType::RGB) { - RCLCPP_ERROR(node->get_logger(), "Wrong pipeline chosen for camera as it has only one sensor. Switching to RGB."); - return PipelineType::RGB; - } - } else if(sensorNum == 2) { - if(type != PipelineType::Stereo && type != PipelineType::Depth) { - RCLCPP_ERROR(node->get_logger(), "Wrong pipeline chosen for camera as it has only stereo pair. Switching to Stereo."); - return PipelineType::Stereo; - } - } else if(sensorNum > 3 && type != PipelineType::CamArray) { - RCLCPP_ERROR(node->get_logger(), "For cameras with more than three sensors you can only use CamArray. Switching to CamArray."); - return PipelineType::CamArray; - } - return type; -} -} // namespace pipeline_gen -} // namespace depthai_ros_driver \ No newline at end of file diff --git a/depthai_ros_msgs/CMakeLists.txt b/depthai_ros_msgs/CMakeLists.txt index cba6e91f..7cc85967 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.2) +project(depthai_ros_msgs VERSION 2.7.3) if(POLICY CMP0057) cmake_policy(SET CMP0057 NEW) diff --git a/depthai_ros_msgs/package.xml b/depthai_ros_msgs/package.xml index 16dd76d7..4b5ac275 100644 --- a/depthai_ros_msgs/package.xml +++ b/depthai_ros_msgs/package.xml @@ -1,7 +1,7 @@ depthai_ros_msgs - 2.7.2 + 2.7.3 Package to keep interface independent of the driver Sachin Guruswamy