From 15d848ae5cd3fb810eb55576f022e15b06dbb55e Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 6 Sep 2023 08:22:09 +0000 Subject: [PATCH] update to v280 --- README.md | 41 +++ depthai-ros/CHANGELOG.rst | 14 + depthai-ros/CMakeLists.txt | 2 +- depthai-ros/package.xml | 3 +- depthai_bridge/CMakeLists.txt | 6 +- .../depthai_bridge/BridgePublisher.hpp | 29 +- .../include/depthai_bridge/ImageConverter.hpp | 13 +- .../include/depthai_bridge/TFPublisher.hpp | 87 ++++++ .../TrackedFeaturesConverter.hpp | 55 ++++ depthai_bridge/package.xml | 3 +- depthai_bridge/src/ImageConverter.cpp | 186 ++++++------ depthai_bridge/src/TFPublisher.cpp | 261 +++++++++++++++++ .../src/TrackedFeaturesConverter.cpp | 51 ++++ depthai_descriptions/CMakeLists.txt | 2 +- depthai_descriptions/launch/urdf_launch.py | 78 +++-- depthai_descriptions/package.xml | 2 +- .../urdf/base_descr.urdf.xacro | 21 ++ .../urdf/include/base_macro.urdf.xacro | 78 +++++ .../urdf/include/depthai_macro.urdf.xacro | 185 +++++------- depthai_examples/CMakeLists.txt | 4 +- depthai_examples/package.xml | 2 +- .../src/feature_tracker_publisher.cpp | 100 +++++++ depthai_filters/CMakeLists.txt | 8 +- depthai_filters/config/feature_tracker.yaml | 6 + .../feature_tracker_overlay.hpp | 46 +++ .../include/depthai_filters/features_3d.hpp | 36 +++ .../launch/example_feature_3d.launch.py | 51 ++++ .../launch/example_feature_tracker.launch.py | 51 ++++ depthai_filters/launch/spatial_bb.launch.py | 5 +- depthai_filters/package.xml | 3 +- .../src/feature_tracker_overlay.cpp | 76 +++++ depthai_filters/src/features_3d.cpp | 69 +++++ depthai_ros_driver/CMakeLists.txt | 8 +- depthai_ros_driver/config/camera.yaml | 6 +- depthai_ros_driver/config/pcl.yaml | 3 +- .../include/depthai_ros_driver/camera.hpp | 50 +++- .../dai_nodes/base_node.hpp | 36 ++- .../dai_nodes/nn/detection.hpp | 45 ++- .../dai_nodes/nn/spatial_detection.hpp | 4 +- .../dai_nodes/sensors/feature_tracker.hpp | 58 ++++ .../dai_nodes/sensors/mono.hpp | 6 +- .../dai_nodes/sensors/rgb.hpp | 5 +- .../dai_nodes/sensors/sensor_helpers.hpp | 34 ++- .../dai_nodes/sensors/sensor_wrapper.hpp | 2 +- .../depthai_ros_driver/dai_nodes/stereo.hpp | 38 ++- .../dai_nodes/sys_logger.hpp | 42 +++ .../param_handlers/base_param_handler.hpp | 2 +- .../feature_tracker_param_handler.hpp | 34 +++ .../param_handlers/sensor_param_handler.hpp | 1 + .../param_handlers/stereo_param_handler.hpp | 7 +- .../pipeline/base_pipeline.hpp | 1 - .../pipeline/pipeline_generator.hpp | 21 ++ .../include/depthai_ros_driver/utils.hpp | 7 + depthai_ros_driver/launch/camera.launch.py | 55 +++- .../launch/pointcloud.launch.py | 5 +- depthai_ros_driver/launch/rgbd_pcl.launch.py | 1 - depthai_ros_driver/package.xml | 6 +- depthai_ros_driver/src/camera.cpp | 103 +++++-- .../src/dai_nodes/base_node.cpp | 32 ++ .../src/dai_nodes/nn/segmentation.cpp | 2 +- .../src/dai_nodes/sensors/feature_tracker.cpp | 84 ++++++ .../src/dai_nodes/sensors/mono.cpp | 35 ++- .../src/dai_nodes/sensors/rgb.cpp | 55 +++- .../src/dai_nodes/sensors/sensor_helpers.cpp | 171 ++++------- .../src/dai_nodes/sensors/sensor_wrapper.cpp | 12 +- depthai_ros_driver/src/dai_nodes/stereo.cpp | 273 +++++++++++++----- .../src/dai_nodes/sys_logger.cpp | 81 ++++++ .../param_handlers/camera_param_handler.cpp | 15 + .../feature_tracker_param_handler.cpp | 30 ++ .../src/param_handlers/imu_param_handler.cpp | 2 +- .../src/param_handlers/nn_param_handler.cpp | 12 +- .../param_handlers/sensor_param_handler.cpp | 60 +++- .../param_handlers/stereo_param_handler.cpp | 78 +++-- .../src/pipeline/base_types.cpp | 18 +- .../src/pipeline/pipeline_generator.cpp | 4 +- depthai_ros_driver/src/utils.cpp | 17 ++ depthai_ros_msgs/CMakeLists.txt | 4 +- depthai_ros_msgs/msg/TrackedFeature.msg | 10 + depthai_ros_msgs/msg/TrackedFeatures.msg | 2 + depthai_ros_msgs/package.xml | 2 +- 80 files changed, 2560 insertions(+), 593 deletions(-) create mode 100644 depthai_bridge/include/depthai_bridge/TFPublisher.hpp create mode 100644 depthai_bridge/include/depthai_bridge/TrackedFeaturesConverter.hpp create mode 100644 depthai_bridge/src/TFPublisher.cpp create mode 100644 depthai_bridge/src/TrackedFeaturesConverter.cpp create mode 100644 depthai_descriptions/urdf/base_descr.urdf.xacro create mode 100644 depthai_descriptions/urdf/include/base_macro.urdf.xacro create mode 100644 depthai_examples/src/feature_tracker_publisher.cpp create mode 100644 depthai_filters/config/feature_tracker.yaml create mode 100644 depthai_filters/include/depthai_filters/feature_tracker_overlay.hpp create mode 100644 depthai_filters/include/depthai_filters/features_3d.hpp create mode 100644 depthai_filters/launch/example_feature_3d.launch.py create mode 100644 depthai_filters/launch/example_feature_tracker.launch.py create mode 100644 depthai_filters/src/feature_tracker_overlay.cpp create mode 100644 depthai_filters/src/features_3d.cpp create mode 100644 depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/feature_tracker.hpp create mode 100644 depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sys_logger.hpp create mode 100644 depthai_ros_driver/include/depthai_ros_driver/param_handlers/feature_tracker_param_handler.hpp create mode 100644 depthai_ros_driver/src/dai_nodes/sensors/feature_tracker.cpp create mode 100644 depthai_ros_driver/src/dai_nodes/sys_logger.cpp create mode 100644 depthai_ros_driver/src/param_handlers/feature_tracker_param_handler.cpp create mode 100644 depthai_ros_msgs/msg/TrackedFeature.msg create mode 100644 depthai_ros_msgs/msg/TrackedFeatures.msg diff --git a/README.md b/README.md index b3aa80f4..844825b0 100644 --- a/README.md +++ b/README.md @@ -141,6 +141,31 @@ This runs your camera as a ROS2 Component and gives you the ability to customize Paramerers that begin with `r_` can be freely modified during runtime, for example with rqt. Parameters that begin with `i_` are set when camera is initializing, to change them you have to call `stop` and `start` services. This can be used to hot swap NNs during runtime, changing resolutions, etc. Below you can see some examples: + +#### Publishing TFs from extrinsics + +By default, camera transforms are published from default URDF descriptions based on CAD models. This can be overriden by using TFPublisher class from `depthai_bridge`, which based on Device's camera calibration data republishes the description with updated information. To enable this behavior in `depthai_ros_driver`, you can use following parameters: +- `camera.i_publish_tf_from_calibration` - setting this to true launches TFPublisher + +Then you can set following arguments: +- `camera.i_tf_camera_name` - if not set, defaults to the node name +- `camera.i_tf_camera_model` - if not set, it will be automatically detected. If the node is unable to detect STL file for the camera it is set to `OAK-D`. To explicitly set it in `camera.launch.py`, set `override_cam_model:=true` +- `camera.i_tf_base_frame` +- `camera.i_tf_parent_frame` +- `camera.i_tf_cam_pos_x` +- `camera.i_tf_cam_pos_y` +- `camera.i_tf_cam_pos_z` +- `camera.i_tf_cam_roll` +- `camera.i_tf_cam_pitch` +- `camera.i_tf_cam_yaw` + +When using `camera.launch.py`, you can set `pass_tf_args_as_params:=true` so that TF arguments are used to fill those parameters. For example `ros2 launch depthai_ros_driver camera.launch.py pass_tf_args_as_params:=true parent_frame:=map cam_pos_x:=1.0 imu_from_descr:=true` + +It is also possible to set custom URDF path (for now only absolute path works) and custom xacro arguments using `camera.i_tf_custom_urdf_path` and `camera.i_tf_custom_xacro_args`. Please note that robot_state_publisher must be running. + +**NOTE ON IMU EXTRINSICS** +If your camera has uncalibrated IMU, a warning will be shown, and IMU will be published with zero rotation and translation. You can override this behavior by setting `camera.i_tf_imu_from_descr`: true. This will publish default IMU extrinsics from URDF based on camera model. + #### Setting RGB parameters By default RGB camera outputs `ISP` frame. To set custom width and height of output image, you can set `i_isp_num` and `i_isp_den` which scale image dimensions (2 and 3 by default, so from 1920x1080 to 1280x720), note for RGBD alignment to work resulting width and height must be divisible by 16. @@ -149,6 +174,19 @@ Additionally you can set `i.output_isp: false` to use `video` output and set cus ![](docs/param_rgb.gif) #### Setting Stereo parameters ![](docs/param_stereo.gif) +##### Depth alignment +When setting `stereo.i_align_depth: true`, stereo output is aligned to board socket specified by `stereo.i_board_socket_id` parameter (by default 0/CAM_A) + +You can enable rectified Stereo streams by setting, for example in the case of right stream `i_publish_right_rect: true`. You can also set `i_publish_synced_rect_pair: true` to get both images with the same timestamps. + +##### Custom Sensor sockets + +Configuration of which sensors are used for computing stereo pair can be done either programatically, by specifying them in a constructor of a Stereo node (for example when building a custom pipeline), or via parameters - `stereo.i_left_socket_id`/`stereo.i_right_socket_id`. Please note that currently if you want to use rgb/center socket instead of one of the given pairs you will need to build a custom pipeline for that. + +#### Feature Tracker + +Each sensor node (and rectified streams from Stereo node) has the option to add FeatureTracker node, which publishes `depthai_ros_msgs/msg/TrackedFeatures` messages. +To enable features on, for example rgb node, set `rgb: i_enable_feature_tracker: true`. To enable publishing on rectified streams, set for example `stereo: i_left_rect_enable_feature_tracker` #### Setting IMU parameters Parameters: @@ -260,6 +298,9 @@ Available filters: `ros2 launch depthai_filters example_seg_overlay.launch.py` - WLS filter - stereo depth filter that smooths out overall depth image based on disparity data. It subscribes to `stereo/image_raw` and `left/image raw` topics. Parameters needed to enable it - `left.i_publish_topic`, `stereo.i_output_disparity` an example can be seen by running `ros2 launch depthai_filters example_wls_filter.launch.py` +- SpatialBB - publishes bounding boxes as 3D line Markers based on spatial detections coming from driver node +- FeatureTrackerOverlay - publishes Tracked Features overlay based on features and images coming from the driver +- Features3D - uses depth image to republish features as 3D pointcloud ### Using external sources for NN inference or Stereo Depth diff --git a/depthai-ros/CHANGELOG.rst b/depthai-ros/CHANGELOG.rst index 7af7d5e7..49baa52c 100644 --- a/depthai-ros/CHANGELOG.rst +++ b/depthai-ros/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package depthai-ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2023-09-01) +------------------- +* Add camera image orientation param +* Performance update +* Feature tracker +* Handle USB speed when usb id is specified +* Change misleading error to a clearer message +* Watchdog +* Depth alignment update +* Synced stereo streams +* Lazy Publishing +* Urdf loader +* Add exposure offset + 2.7.5 (2023-08-07) ------------------- * IMU sync fix diff --git a/depthai-ros/CMakeLists.txt b/depthai-ros/CMakeLists.txt index 28e63452..17607a09 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.5 LANGUAGES CXX C) +project(depthai-ros VERSION 2.8.0 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) diff --git a/depthai-ros/package.xml b/depthai-ros/package.xml index 2be1cd74..85100343 100644 --- a/depthai-ros/package.xml +++ b/depthai-ros/package.xml @@ -1,7 +1,7 @@ depthai-ros - 2.7.5 + 2.8.0 The depthai-ros package @@ -19,6 +19,7 @@ depthai_examples depthai_ros_driver depthai_descriptions + depthai_filters ament_cmake diff --git a/depthai_bridge/CMakeLists.txt b/depthai_bridge/CMakeLists.txt index 6930d4df..6e66bca9 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.5 LANGUAGES CXX C) +project(depthai_bridge VERSION 2.8.0 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) @@ -37,6 +37,7 @@ find_package(sensor_msgs REQUIRED) find_package(stereo_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(vision_msgs REQUIRED) +find_package(tf2_ros REQUIRED) set(dependencies camera_info_manager @@ -48,6 +49,7 @@ set(dependencies stereo_msgs std_msgs vision_msgs + tf2_ros ) include_directories( @@ -62,6 +64,8 @@ file(GLOB LIB_SRC "src/ImgDetectionConverter.cpp" "src/SpatialDetectionConverter.cpp" "src/ImuConverter.cpp" +"src/TFPublisher.cpp" +"src/TrackedFeaturesConverter.cpp" ) add_library(${PROJECT_NAME} SHARED ${LIB_SRC}) diff --git a/depthai_bridge/include/depthai_bridge/BridgePublisher.hpp b/depthai_bridge/include/depthai_bridge/BridgePublisher.hpp index 99f793bb..5f6e5939 100644 --- a/depthai_bridge/include/depthai_bridge/BridgePublisher.hpp +++ b/depthai_bridge/include/depthai_bridge/BridgePublisher.hpp @@ -34,7 +34,8 @@ class BridgePublisher { std::shared_ptr node, std::string rosTopic, ConvertFunc converter, - rclcpp::QoS qosSetting = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + rclcpp::QoS qosSetting = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + bool lazyPublisher = true); BridgePublisher(std::shared_ptr daiMessageQueue, std::shared_ptr node, @@ -42,7 +43,8 @@ class BridgePublisher { ConvertFunc converter, size_t qosHistoryDepth, std::string cameraParamUri = "", - std::string cameraName = ""); + std::string cameraName = "", + bool lazyPublisher = true); BridgePublisher(std::shared_ptr daiMessageQueue, std::shared_ptr node, @@ -50,7 +52,8 @@ class BridgePublisher { ConvertFunc converter, size_t qosHistoryDepth, ImageMsgs::CameraInfo cameraInfoData, - std::string cameraName); + std::string cameraName, + bool lazyPublisher = true); /** * Tag Dispacher function to to overload the Publisher to ImageTransport Publisher @@ -95,6 +98,7 @@ class BridgePublisher { std::unique_ptr _camInfoManager; bool _isCallbackAdded = false; bool _isImageMessage = false; // used to enable camera info manager + bool _lazyPublisher = true; }; template @@ -105,8 +109,9 @@ BridgePublisher::BridgePublisher(std::shared_ptr node, std::string rosTopic, ConvertFunc converter, - rclcpp::QoS qosSetting) - : _daiMessageQueue(daiMessageQueue), _node(node), _converter(converter), _it(node), _rosTopic(rosTopic) { + rclcpp::QoS qosSetting, + bool lazyPublisher) + : _daiMessageQueue(daiMessageQueue), _node(node), _converter(converter), _it(node), _rosTopic(rosTopic), _lazyPublisher(lazyPublisher) { _rosPublisher = _node->create_publisher(_rosTopic, qosSetting); } @@ -117,14 +122,16 @@ BridgePublisher::BridgePublisher(std::shared_ptr{}); } @@ -135,14 +142,16 @@ BridgePublisher::BridgePublisher(std::shared_ptr{}); } @@ -222,7 +231,7 @@ void BridgePublisher::publishHelper(std::shared_ptr inDa } mainSubCount = _node->count_subscribers(_rosTopic); - if(mainSubCount > 0 || infoSubCount > 0) { + if(!_lazyPublisher || (mainSubCount > 0 || infoSubCount > 0)) { _converter(inDataPtr, opMsgs); while(opMsgs.size()) { diff --git a/depthai_bridge/include/depthai_bridge/ImageConverter.hpp b/depthai_bridge/include/depthai_bridge/ImageConverter.hpp index 6df9253c..6287fbc9 100644 --- a/depthai_bridge/include/depthai_bridge/ImageConverter.hpp +++ b/depthai_bridge/include/depthai_bridge/ImageConverter.hpp @@ -55,12 +55,12 @@ class ImageConverter { _updateRosBaseTimeOnToRosMsg = update; } - void toRosMsgFromBitStream(std::shared_ptr inData, - std::deque& outImageMsgs, - dai::RawImgFrame::Type type, - const sensor_msgs::msg::CameraInfo& info); + void convertFromBitstream(dai::RawImgFrame::Type srcType); + void addExposureOffset(dai::CameraExposureOffset& offset); + void convertDispToDepth(); void toRosMsg(std::shared_ptr inData, std::deque& outImageMsgs); + ImageMsgs::Image toRosMsgRawPtr(std::shared_ptr inData, const sensor_msgs::msg::CameraInfo& info = sensor_msgs::msg::CameraInfo()); ImagePtr toRosMsgPtr(std::shared_ptr inData); void toDaiMsg(const ImageMsgs::Image& inMsg, dai::ImgFrame& outData); @@ -95,6 +95,11 @@ class ImageConverter { int64_t _totalNsChange{0}; // Whether to update the ROS base time on each message conversion bool _updateRosBaseTimeOnToRosMsg{false}; + dai::RawImgFrame::Type _srcType; + bool _fromBitstream = false; + bool _convertDispToDepth = false; + bool _addExpOffset = false; + dai::CameraExposureOffset _expOffset; }; } // namespace ros diff --git a/depthai_bridge/include/depthai_bridge/TFPublisher.hpp b/depthai_bridge/include/depthai_bridge/TFPublisher.hpp new file mode 100644 index 00000000..01ef7a34 --- /dev/null +++ b/depthai_bridge/include/depthai_bridge/TFPublisher.hpp @@ -0,0 +1,87 @@ +#pragma once +#include "depthai-shared/common/CameraFeatures.hpp" +#include "depthai/device/CalibrationHandler.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "nlohmann/json.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/parameter_client.hpp" +#include "tf2_ros/static_transform_broadcaster.h" + +namespace dai { +namespace ros { +class TFPublisher { + public: + explicit TFPublisher(rclcpp::Node* node, + const dai::CalibrationHandler& calHandler, + const std::vector& camFeatures, + const std::string& camName, + const std::string& camModel, + const std::string& baseFrame, + const std::string& parentFrame, + const std::string& camPosX, + const std::string& camPosY, + const std::string& camPosZ, + const std::string& camRoll, + const std::string& camPitch, + const std::string& camYaw, + const std::string& imuFromDescr, + const std::string& customURDFLocation, + const std::string& customXacroArgs); + /* + * @brief Obtain URDF description by running Xacro with provided arguments. + */ + std::string getURDF(); + geometry_msgs::msg::Quaternion quatFromRotM(nlohmann::json rotMatrix); + geometry_msgs::msg::Vector3 transFromExtr(nlohmann::json translation); + + private: + /* + * @brief Converts model name to one of the available model families + */ + void convertModelName(); + /* + * @brief Prepare arguments for xacro command. If custom URDF location is not provided, check if model name is available in depthai_descriptions package. + */ + std::string prepareXacroArgs(); + /* + * @brief Get URDF description and set it as a parameter for robot_state_publisher + */ + void publishDescription(); + /* + * @brief Publish camera transforms ("standard" and optical) based on calibration data. + * Frame names are based on socket names and use following convention: [base_frame]_[socket_name]_camera_frame and + * [base_frame]_[socket_name]_camera_optical_frame + */ + void publishCamTransforms(nlohmann::json camData, rclcpp::Node* node); + /* + * @brief Publish IMU transform based on calibration data. + * Frame name is based on IMU name and uses following convention: [base_frame]_imu_frame. + * If IMU extrinsics are not set, warning is printed out and imu frame is published with zero translation and rotation. + */ + void publishImuTransform(nlohmann::json json, rclcpp::Node* node); + /* + * @brief Check if model STL file is available in depthai_descriptions package. + */ + bool modelNameAvailable(); + std::string getCamSocketName(int socketNum); + std::unique_ptr _paramClient; + std::shared_ptr _tfPub; + std::string _camName; + std::string _camModel; + std::string _baseFrame; + std::string _parentFrame; + std::string _camPosX; + std::string _camPosY; + std::string _camPosZ; + std::string _camRoll; + std::string _camPitch; + std::string _camYaw; + std::string _imuFromDescr; + std::string _customURDFLocation; + std::string _customXacroArgs; + std::vector _camFeatures; + rclcpp::Logger _logger; +}; +} // namespace ros +} // namespace dai \ No newline at end of file diff --git a/depthai_bridge/include/depthai_bridge/TrackedFeaturesConverter.hpp b/depthai_bridge/include/depthai_bridge/TrackedFeaturesConverter.hpp new file mode 100644 index 00000000..f8c5bb80 --- /dev/null +++ b/depthai_bridge/include/depthai_bridge/TrackedFeaturesConverter.hpp @@ -0,0 +1,55 @@ +#pragma once + +#include +#include +#include +#include + +#include "depthai/pipeline/datatype/TrackedFeatures.hpp" +#include "rclcpp/time.hpp" + +namespace dai { + +namespace ros { + +class TrackedFeaturesConverter { + public: + // DetectionConverter() = default; + TrackedFeaturesConverter(std::string frameName, bool getBaseDeviceTimestamp = false); + ~TrackedFeaturesConverter(); + + /** + * @brief Handles cases in which the ROS time shifts forward or backward + * Should be called at regular intervals or on-change of ROS time, depending + * on monitoring. + * + */ + void updateRosBaseTime(); + + /** + * @brief Commands the converter to automatically update the ROS base time on message conversion based on variable + * + * @param update: bool whether to automatically update the ROS base time on message conversion + */ + void setUpdateRosBaseTimeOnToRosMsg(bool update = true) { + _updateRosBaseTimeOnToRosMsg = update; + } + + void toRosMsg(std::shared_ptr inFeatures, std::deque& featureMsgs); + + private: + const std::string _frameName; + std::chrono::time_point _steadyBaseTime; + rclcpp::Time _rosBaseTime; + bool _getBaseDeviceTimestamp; + // For handling ROS time shifts and debugging + int64_t _totalNsChange{0}; + // Whether to update the ROS base time on each message conversion + bool _updateRosBaseTimeOnToRosMsg{false}; +}; + +} // namespace ros + +namespace rosBridge = ros; + +} // namespace dai diff --git a/depthai_bridge/package.xml b/depthai_bridge/package.xml index 9002320b..efad0950 100644 --- a/depthai_bridge/package.xml +++ b/depthai_bridge/package.xml @@ -1,7 +1,7 @@ depthai_bridge - 2.7.5 + 2.8.0 The depthai_bridge package Sachin Guruswamy @@ -25,6 +25,7 @@ std_msgs stereo_msgs vision_msgs + tf2_ros robot_state_publisher xacro diff --git a/depthai_bridge/src/ImageConverter.cpp b/depthai_bridge/src/ImageConverter.cpp index 3ab623b8..0bc2d1a5 100644 --- a/depthai_bridge/src/ImageConverter.cpp +++ b/depthai_bridge/src/ImageConverter.cpp @@ -40,74 +40,32 @@ void ImageConverter::updateRosBaseTime() { updateBaseTime(_steadyBaseTime, _rosBaseTime, _totalNsChange); } -void ImageConverter::toRosMsgFromBitStream(std::shared_ptr inData, - std::deque& outImageMsgs, - dai::RawImgFrame::Type type, - const sensor_msgs::msg::CameraInfo& info) { - if(_updateRosBaseTimeOnToRosMsg) { - updateRosBaseTime(); - } - std::chrono::_V2::steady_clock::time_point tstamp; - if(_getBaseDeviceTimestamp) - tstamp = inData->getTimestampDevice(); - else - tstamp = inData->getTimestamp(); +void ImageConverter::convertFromBitstream(dai::RawImgFrame::Type srcType) { + _fromBitstream = true; + _srcType = srcType; +} - ImageMsgs::Image outImageMsg; - StdMsgs::Header header; - header.frame_id = _frameName; - header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, tstamp); - std::string encoding; - int decodeFlags; - cv::Mat output; - switch(type) { - case dai::RawImgFrame::Type::BGR888i: { - encoding = sensor_msgs::image_encodings::BGR8; - decodeFlags = cv::IMREAD_COLOR; - break; - } - case dai::RawImgFrame::Type::GRAY8: { - encoding = sensor_msgs::image_encodings::MONO8; - decodeFlags = cv::IMREAD_GRAYSCALE; - break; - } - case dai::RawImgFrame::Type::RAW8: { - encoding = sensor_msgs::image_encodings::TYPE_16UC1; - decodeFlags = cv::IMREAD_GRAYSCALE; - break; - } - default: { - throw(std::runtime_error("Converted type not supported!")); - } - } +void ImageConverter::convertDispToDepth() { + _convertDispToDepth = true; +} - output = cv::imdecode(cv::Mat(1, inData->getData().size(), CV_8UC1, inData->getData().data()), decodeFlags); - - // converting disparity - if(type == dai::RawImgFrame::Type::RAW8) { - auto factor = (info.k[0] * info.p[3]); - cv::Mat depthOut = cv::Mat(cv::Size(output.cols, output.rows), CV_16UC1); - depthOut.forEach([&output, &factor](short& pixel, const int* position) -> void { - auto disp = output.at(position); - if(disp == 0) - pixel = 0; - else - pixel = factor / disp; - }); - output = depthOut.clone(); - } - cv_bridge::CvImage(header, encoding, output).toImageMsg(outImageMsg); - outImageMsgs.push_back(outImageMsg); - return; +void ImageConverter::addExposureOffset(dai::CameraExposureOffset& offset) { + _expOffset = offset; + _addExpOffset = true; } -void ImageConverter::toRosMsg(std::shared_ptr inData, std::deque& outImageMsgs) { +ImageMsgs::Image ImageConverter::toRosMsgRawPtr(std::shared_ptr inData, const sensor_msgs::msg::CameraInfo& info) { if(_updateRosBaseTimeOnToRosMsg) { updateRosBaseTime(); } std::chrono::_V2::steady_clock::time_point tstamp; if(_getBaseDeviceTimestamp) - tstamp = inData->getTimestampDevice(); + if(_addExpOffset) + tstamp = inData->getTimestampDevice(_expOffset); + else + tstamp = inData->getTimestampDevice(); + else if(_addExpOffset) + tstamp = inData->getTimestamp(_expOffset); else tstamp = inData->getTimestamp(); ImageMsgs::Image outImageMsg; @@ -116,6 +74,55 @@ void ImageConverter::toRosMsg(std::shared_ptr inData, std::deque< header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, tstamp); + if(_fromBitstream) { + std::string encoding; + int decodeFlags; + int channels; + cv::Mat output; + switch(_srcType) { + case dai::RawImgFrame::Type::BGR888i: { + encoding = sensor_msgs::image_encodings::BGR8; + decodeFlags = cv::IMREAD_COLOR; + channels = CV_8UC3; + break; + } + case dai::RawImgFrame::Type::GRAY8: { + encoding = sensor_msgs::image_encodings::MONO8; + decodeFlags = cv::IMREAD_GRAYSCALE; + channels = CV_8UC1; + break; + } + case dai::RawImgFrame::Type::RAW8: { + encoding = sensor_msgs::image_encodings::TYPE_16UC1; + decodeFlags = cv::IMREAD_ANYDEPTH; + channels = CV_16UC1; + break; + } + default: { + std::cout << _frameName << static_cast(_srcType) << std::endl; + throw(std::runtime_error("Converted type not supported!")); + } + } + + output = cv::imdecode(cv::Mat(inData->getData()), decodeFlags); + + // converting disparity + if(_convertDispToDepth) { + auto factor = std::abs(info.p[3]) * 10000; + cv::Mat depthOut = cv::Mat(cv::Size(output.cols, output.rows), CV_16UC1); + depthOut.forEach([&output, &factor](uint16_t& pixel, const int* position) -> void { + auto disp = output.at(position); + if(disp == 0) + pixel = 0; + else + pixel = factor / disp; + }); + output = depthOut.clone(); + } + cv_bridge::CvImage(header, encoding, output).toImageMsg(outImageMsg); + return outImageMsg; + } + if(planarEncodingEnumMap.find(inData->getType()) != planarEncodingEnumMap.end()) { // cv::Mat inImg = inData->getCvFrame(); cv::Mat mat, output; @@ -142,22 +149,20 @@ void ImageConverter::toRosMsg(std::shared_ptr inData, std::deque< switch(inData->getType()) { case dai::RawImgFrame::Type::RGB888p: { cv::Size s(inData->getWidth(), inData->getHeight()); - std::vector channels; - // RGB - channels.push_back(cv::Mat(s, CV_8UC1, inData->getData().data() + s.area() * 2)); - channels.push_back(cv::Mat(s, CV_8UC1, inData->getData().data() + s.area() * 1)); - channels.push_back(cv::Mat(s, CV_8UC1, inData->getData().data() + s.area() * 0)); - cv::merge(channels, output); + cv::Mat m1 = cv::Mat(s, CV_8UC1, inData->getData().data() + s.area() * 2); + cv::Mat m2 = cv::Mat(s, CV_8UC1, inData->getData().data() + s.area() * 1); + cv::Mat m3 = cv::Mat(s, CV_8UC1, inData->getData().data() + s.area() * 0); + cv::Mat channels[3] = {m1, m2, m3}; + cv::merge(channels, 3, output); } break; case dai::RawImgFrame::Type::BGR888p: { cv::Size s(inData->getWidth(), inData->getHeight()); - std::vector channels; - // BGR - channels.push_back(cv::Mat(s, CV_8UC1, inData->getData().data() + s.area() * 0)); - channels.push_back(cv::Mat(s, CV_8UC1, inData->getData().data() + s.area() * 1)); - channels.push_back(cv::Mat(s, CV_8UC1, inData->getData().data() + s.area() * 2)); - cv::merge(channels, output); + cv::Mat m1 = cv::Mat(s, CV_8UC1, inData->getData().data() + s.area() * 0); + cv::Mat m2 = cv::Mat(s, CV_8UC1, inData->getData().data() + s.area() * 1); + cv::Mat m3 = cv::Mat(s, CV_8UC1, inData->getData().data() + s.area() * 2); + cv::Mat channels[3] = {m1, m2, m3}; + cv::merge(channels, 3, output); } break; case dai::RawImgFrame::Type::YUV420p: @@ -188,19 +193,25 @@ void ImageConverter::toRosMsg(std::shared_ptr inData, std::deque< outImageMsg.is_bigendian = true; size_t size = inData->getData().size(); - outImageMsg.data.resize(size); - unsigned char* imageMsgDataPtr = reinterpret_cast(&outImageMsg.data[0]); - unsigned char* daiImgData = reinterpret_cast(inData->getData().data()); - - // TODO(Sachin): Try using assign since it is a vector - // img->data.assign(packet.data->cbegin(), packet.data->cend()); - memcpy(imageMsgDataPtr, daiImgData, size); + outImageMsg.data.reserve(size); + outImageMsg.data = std::move(inData->getData()); } + return outImageMsg; +} + +void ImageConverter::toRosMsg(std::shared_ptr inData, std::deque& outImageMsgs) { + auto outImageMsg = toRosMsgRawPtr(inData); outImageMsgs.push_back(outImageMsg); return; } -// TODO(sachin): Not tested +ImagePtr ImageConverter::toRosMsgPtr(std::shared_ptr inData) { + auto msg = toRosMsgRawPtr(inData); + + ImagePtr ptr = std::make_shared(msg); + return ptr; +} + void ImageConverter::toDaiMsg(const ImageMsgs::Image& inMsg, dai::ImgFrame& outData) { std::unordered_map::iterator revEncodingIter; if(_daiInterleaved) { @@ -208,7 +219,7 @@ void ImageConverter::toDaiMsg(const ImageMsgs::Image& inMsg, dai::ImgFrame& outD return pair.second == inMsg.encoding; }); if(revEncodingIter == encodingEnumMap.end()) - std::runtime_error( + throw std::runtime_error( "Unable to find DAI encoding for the corresponding " "sensor_msgs::image.encoding stream"); @@ -247,15 +258,6 @@ void ImageConverter::toDaiMsg(const ImageMsgs::Image& inMsg, dai::ImgFrame& outD outData.setType(revEncodingIter->first); } -ImagePtr ImageConverter::toRosMsgPtr(std::shared_ptr inData) { - std::deque msgQueue; - toRosMsg(inData, msgQueue); - auto msg = msgQueue.front(); - - ImagePtr ptr = std::make_shared(msg); - return ptr; -} - void ImageConverter::planarToInterleaved(const std::vector& srcData, std::vector& destData, int w, int h, int numPlanes, int bpp) { if(numPlanes == 3) { // optimization (cache) @@ -272,7 +274,7 @@ void ImageConverter::planarToInterleaved(const std::vector& srcData, st destData[i * 3 + 2] = r; } } else { - std::runtime_error( + throw std::runtime_error( "If you encounter the scenario where you need this " "please create an issue on github"); } @@ -300,7 +302,7 @@ void ImageConverter::interleavedToPlanar(const std::vector& srcData, st // destData[i*3+2] = r; // } } else { - std::runtime_error( + throw std::runtime_error( "If you encounter the scenario where you need this " "please create an issue on github"); } @@ -314,7 +316,7 @@ cv::Mat ImageConverter::rosMsgtoCvMat(ImageMsgs::Image& inMsg) { cv::cvtColor(nv_frame, rgb, cv::COLOR_YUV2BGR_NV12); return rgb; } else { - std::runtime_error("This frature is still WIP"); + throw std::runtime_error("This frature is still WIP"); return rgb; } } diff --git a/depthai_bridge/src/TFPublisher.cpp b/depthai_bridge/src/TFPublisher.cpp new file mode 100644 index 00000000..ae9bd691 --- /dev/null +++ b/depthai_bridge/src/TFPublisher.cpp @@ -0,0 +1,261 @@ +#include "depthai_bridge/TFPublisher.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "nlohmann/json.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/LinearMath/Matrix3x3.h" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace dai { +namespace ros { +TFPublisher::TFPublisher(rclcpp::Node* node, + const dai::CalibrationHandler& calHandler, + const std::vector& camFeatures, + const std::string& camName, + const std::string& camModel, + const std::string& baseFrame, + const std::string& parentFrame, + const std::string& camPosX, + const std::string& camPosY, + const std::string& camPosZ, + const std::string& camRoll, + const std::string& camPitch, + const std::string& camYaw, + const std::string& imuFromDescr, + const std::string& customURDFLocation, + const std::string& customXacroArgs) + : _camName(camName), + _camModel(camModel), + _baseFrame(baseFrame), + _parentFrame(parentFrame), + _camPosX(camPosX), + _camPosY(camPosY), + _camPosZ(camPosZ), + _camRoll(camRoll), + _camPitch(camPitch), + _camYaw(camYaw), + _camFeatures(camFeatures), + _imuFromDescr(imuFromDescr), + _customURDFLocation(customURDFLocation), + _customXacroArgs(customXacroArgs), + _logger(node->get_logger()) { + _tfPub = std::make_shared(node); + + _paramClient = std::make_unique(node, _camName + std::string("_state_publisher")); + + auto json = calHandler.eepromToJson(); + auto camData = json["cameraData"]; + publishDescription(); + publishCamTransforms(camData, node); + if(_imuFromDescr != "true") { + publishImuTransform(json, node); + } +} + +void TFPublisher::publishDescription() { + auto urdf = getURDF(); + auto robotDescr = rclcpp::Parameter("robot_description", urdf); + auto result = _paramClient->set_parameters({robotDescr}); + RCLCPP_INFO(_logger, "Published URDF"); +} + +void TFPublisher::publishCamTransforms(nlohmann::json camData, rclcpp::Node* node) { + for(auto& cam : camData) { + geometry_msgs::msg::TransformStamped ts; + geometry_msgs::msg::TransformStamped opticalTS; + ts.header.stamp = node->get_clock()->now(); + opticalTS.header.stamp = ts.header.stamp; + auto extrinsics = cam[1]["extrinsics"]; + + ts.transform.rotation = quatFromRotM(extrinsics["rotationMatrix"]); + ts.transform.translation = transFromExtr(extrinsics["translation"]); + + std::string name = getCamSocketName(cam[0]); + ts.child_frame_id = _baseFrame + std::string("_") + name + std::string("_camera_frame"); + // check if the camera is at the end of the chain + if(extrinsics["toCameraSocket"] != -1) { + ts.header.frame_id = _baseFrame + std::string("_") + getCamSocketName(extrinsics["toCameraSocket"].get()) + std::string("_camera_frame"); + } else { + ts.header.frame_id = _baseFrame; + ts.transform.rotation.w = 1.0; + ts.transform.rotation.x = 0.0; + ts.transform.rotation.y = 0.0; + ts.transform.rotation.z = 0.0; + } + // rotate optical fransform + opticalTS.child_frame_id = _baseFrame + std::string("_") + name + std::string("_camera_optical_frame"); + opticalTS.header.frame_id = ts.child_frame_id; + opticalTS.transform.rotation.w = 0.5; + opticalTS.transform.rotation.x = -0.5; + opticalTS.transform.rotation.y = 0.5; + opticalTS.transform.rotation.z = -0.5; + _tfPub->sendTransform(ts); + _tfPub->sendTransform(opticalTS); + } +} +void TFPublisher::publishImuTransform(nlohmann::json json, rclcpp::Node* node) { + geometry_msgs::msg::TransformStamped ts; + ts.header.stamp = node->get_clock()->now(); + auto imuExtr = json["imuExtrinsics"]; + if(imuExtr["toCameraSocket"] != -1) { + ts.header.frame_id = _baseFrame + std::string("_") + getCamSocketName(imuExtr["toCameraSocket"].get()) + std::string("_camera_frame"); + } else { + ts.header.frame_id = _baseFrame; + } + ts.child_frame_id = _baseFrame + std::string("_imu_frame"); + + ts.transform.rotation = quatFromRotM(imuExtr["rotationMatrix"]); + ts.transform.translation = transFromExtr(imuExtr["translation"]); + bool zeroTrans = ts.transform.translation.x == 0 && ts.transform.translation.y == 0 && ts.transform.translation.z == 0; + bool zeroRot = ts.transform.rotation.w == 1 && ts.transform.rotation.x == 0 && ts.transform.rotation.y == 0 && ts.transform.rotation.z == 0; + if(zeroTrans || zeroRot) { + RCLCPP_WARN(_logger, "IMU extrinsics appear to be default. Check if the IMU is calibrated."); + } + _tfPub->sendTransform(ts); +} + +std::string TFPublisher::getCamSocketName(int socketNum) { + std::string name; + for(auto& cam : _camFeatures) { + if(cam.socket == static_cast(socketNum)) { + if(cam.name == "color" || cam.name == "center") { + name = "rgb"; + } else { + name = cam.name; + } + return name; + } + } + throw std::runtime_error("Camera socket not found"); +} + +geometry_msgs::msg::Vector3 TFPublisher::transFromExtr(nlohmann::json translation) { + geometry_msgs::msg::Vector3 trans; + // optical coordinates to ROS + trans.x = translation["y"].get() / -100.0; + trans.y = translation["x"].get() / -100.0; + trans.z = translation["z"].get() / 100.0; + return trans; +} +geometry_msgs::msg::Quaternion TFPublisher::quatFromRotM(nlohmann::json rotMatrix) { + tf2::Matrix3x3 m(rotMatrix[0][0], + rotMatrix[0][1], + rotMatrix[0][2], + + rotMatrix[1][0], + rotMatrix[1][1], + rotMatrix[1][2], + + rotMatrix[2][0], + rotMatrix[2][1], + rotMatrix[2][2]); + + tf2::Quaternion q; + m.getRotation(q); + geometry_msgs::msg::Quaternion msg_quat = tf2::toMsg(q); + return msg_quat; +} + +bool TFPublisher::modelNameAvailable() { + std::string path = ament_index_cpp::get_package_share_directory("depthai_descriptions") + "/urdf/models/"; + DIR* dir; + struct dirent* ent; + convertModelName(); + if((dir = opendir(path.c_str())) != NULL) { + while((ent = readdir(dir)) != NULL) { + auto name = std::string(ent->d_name); + RCLCPP_DEBUG(_logger, "Found model: %s", name.c_str()); + if(name == _camModel + ".stl") { + return true; + } + } + closedir(dir); + } else { + throw std::runtime_error("Could not open depthai_descriptions package directory"); + } + return false; +} + +std::string TFPublisher::prepareXacroArgs() { + if(!_customURDFLocation.empty() || !modelNameAvailable()) { + RCLCPP_ERROR( + _logger, + "Model name %s not found in depthai_descriptions package. If camera model is autodetected, please notify developers. Using default model: OAK-D", + _camModel.c_str()); + _camModel = "OAK-D"; + } + + std::string xacroArgs = "camera_name:=" + _camName; + xacroArgs += " camera_model:=" + _camModel; + xacroArgs += " base_frame:=" + _baseFrame; + xacroArgs += " parent_frame:=" + _parentFrame; + xacroArgs += " cam_pos_x:=" + _camPosX; + xacroArgs += " cam_pos_y:=" + _camPosY; + xacroArgs += " cam_pos_z:=" + _camPosZ; + xacroArgs += " cam_roll:=" + _camRoll; + xacroArgs += " cam_pitch:=" + _camPitch; + xacroArgs += " cam_yaw:=" + _camYaw; + xacroArgs += " has_imu:=" + _imuFromDescr; + return xacroArgs; +} + +void TFPublisher::convertModelName() { + if(_camModel.find("OAK-D-PRO-POE") != std::string::npos || _camModel.find("OAK-D-PRO-W-POE") != std::string::npos + || _camModel.find("OAK-D-S2-POE") != std::string::npos) { + _camModel = "OAK-D-POE"; + } else if(_camModel.find("OAK-D-LITE") != std::string::npos) { + _camModel = "OAK-D-PRO"; + } else if(_camModel.find("OAK-D-S2") != std::string::npos) { + _camModel = "OAK-D-PRO"; + } else if(_camModel.find("OAK-D-PRO-W") != std::string::npos) { + _camModel = "OAK-D-PRO"; + } else if(_camModel.find("OAK-D-PRO") != std::string::npos) { + _camModel = "OAK-D-PRO"; + } else if(_camModel.find("OAK-D-POE")) { + _camModel = "OAK-D-POE"; + } else if(_camModel.find("OAK-D") != std::string::npos) { + _camModel = "OAK-D"; + } else { + RCLCPP_WARN(_logger, "Unable to match model name: %s to available model family.", _camModel.c_str()); + } +} + +std::string TFPublisher::getURDF() { + std::string args, path; + if(_customXacroArgs.empty()) { + args = prepareXacroArgs(); + } else { + args = _customXacroArgs; + } + if(_customURDFLocation.empty()) { + path = ament_index_cpp::get_package_share_directory("depthai_descriptions") + "/urdf/base_descr.urdf.xacro "; + } else { + path = _customURDFLocation + " "; + } + std::string cmd = "xacro " + path + args; + RCLCPP_DEBUG(_logger, "Xacro command: %s", cmd.c_str()); + std::array buffer; + std::string result; + std::unique_ptr pipe(popen(cmd.c_str(), "r"), pclose); + if(!pipe) { + throw std::runtime_error("popen() failed!"); + } + while(fgets(buffer.data(), buffer.size(), pipe.get()) != nullptr) { + result += buffer.data(); + } + return result; +} +} // namespace ros +} // namespace dai \ No newline at end of file diff --git a/depthai_bridge/src/TrackedFeaturesConverter.cpp b/depthai_bridge/src/TrackedFeaturesConverter.cpp new file mode 100644 index 00000000..cb9e40c1 --- /dev/null +++ b/depthai_bridge/src/TrackedFeaturesConverter.cpp @@ -0,0 +1,51 @@ +#include "depthai_bridge/TrackedFeaturesConverter.hpp" + +#include "depthai_bridge/depthaiUtility.hpp" + +namespace dai { + +namespace ros { + +TrackedFeaturesConverter::TrackedFeaturesConverter(std::string frameName, bool getBaseDeviceTimestamp) + : _frameName(frameName), _steadyBaseTime(std::chrono::steady_clock::now()), _getBaseDeviceTimestamp(getBaseDeviceTimestamp) { + _rosBaseTime = rclcpp::Clock().now(); +} + +TrackedFeaturesConverter::~TrackedFeaturesConverter() = default; + +void TrackedFeaturesConverter::updateRosBaseTime() { + updateBaseTime(_steadyBaseTime, _rosBaseTime, _totalNsChange); +} + +void TrackedFeaturesConverter::toRosMsg(std::shared_ptr inFeatures, std::deque& featureMsgs) { + if(_updateRosBaseTimeOnToRosMsg) { + updateRosBaseTime(); + } + std::chrono::_V2::steady_clock::time_point tstamp; + if(_getBaseDeviceTimestamp) + tstamp = inFeatures->getTimestampDevice(); + else + tstamp = inFeatures->getTimestamp(); + + depthai_ros_msgs::msg::TrackedFeatures msg; + + msg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, tstamp); + msg.header.frame_id = _frameName; + msg.features.resize(inFeatures->trackedFeatures.size()); + + for(const auto& feature : inFeatures->trackedFeatures) { + depthai_ros_msgs::msg::TrackedFeature ft; + ft.header = msg.header; + ft.position.x = feature.position.x; + ft.position.y = feature.position.y; + ft.age = feature.age; + ft.id = feature.id; + ft.harris_score = feature.harrisScore; + ft.tracking_error = feature.trackingError; + msg.features.emplace_back(ft); + } + featureMsgs.push_back(msg); +} + +} // namespace ros +} // namespace dai \ No newline at end of file diff --git a/depthai_descriptions/CMakeLists.txt b/depthai_descriptions/CMakeLists.txt index ab4b4278..6e6c8916 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.5) +project(depthai_descriptions VERSION 2.8.0) 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.py b/depthai_descriptions/launch/urdf_launch.py index c60cfa45..039ddfaa 100644 --- a/depthai_descriptions/launch/urdf_launch.py +++ b/depthai_descriptions/launch/urdf_launch.py @@ -1,38 +1,39 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, OpaqueFunction -from launch_ros.actions import Node +from launch_ros.actions import Node, LoadComposableNodes +from launch_ros.descriptions import ComposableNode from launch.substitutions import LaunchConfiguration, Command +from launch.conditions import IfCondition, UnlessCondition from ament_index_python.packages import get_package_share_directory import os -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch_ros.actions import Node -from launch.substitutions import LaunchConfiguration, Command -from ament_index_python.packages import get_package_share_directory -import os def launch_setup(context, *args, **kwargs): bringup_dir = get_package_share_directory('depthai_descriptions') xacro_path = os.path.join(bringup_dir, 'urdf', 'depthai_descr.urdf.xacro') - camera_model = LaunchConfiguration('camera_model', default = 'OAK-D') - tf_prefix = LaunchConfiguration('tf_prefix', default = 'oak') - base_frame = LaunchConfiguration('base_frame', default = 'oak-d_frame') - parent_frame = LaunchConfiguration('parent_frame', default = 'oak-d-base-frame') - cam_pos_x = LaunchConfiguration('cam_pos_x', default = '0.0') - cam_pos_y = LaunchConfiguration('cam_pos_y', default = '0.0') - cam_pos_z = LaunchConfiguration('cam_pos_z', default = '0.0') - cam_roll = LaunchConfiguration('cam_roll', default = '1.5708') - cam_pitch = LaunchConfiguration('cam_pitch', default = '0.0') - cam_yaw = LaunchConfiguration('cam_yaw', default = '1.5708') - namespace = LaunchConfiguration('namespace', default = '') - - - rsp_node = Node( + camera_model = LaunchConfiguration('camera_model', default='OAK-D') + tf_prefix = LaunchConfiguration('tf_prefix', default='oak') + base_frame = LaunchConfiguration('base_frame', default='oak-d_frame') + parent_frame = LaunchConfiguration( + 'parent_frame', default='oak-d-base-frame') + cam_pos_x = LaunchConfiguration('cam_pos_x', default='0.0') + cam_pos_y = LaunchConfiguration('cam_pos_y', default='0.0') + cam_pos_z = LaunchConfiguration('cam_pos_z', default='0.0') + cam_roll = LaunchConfiguration('cam_roll', default='1.5708') + cam_pitch = LaunchConfiguration('cam_pitch', default='0.0') + cam_yaw = LaunchConfiguration('cam_yaw', default='1.5708') + namespace = LaunchConfiguration('namespace', default='') + use_composition = LaunchConfiguration('use_composition', default='false') + + name = LaunchConfiguration('tf_prefix').perform(context) + + return [ + Node( package='robot_state_publisher', + condition=UnlessCondition(use_composition), executable='robot_state_publisher', - name=LaunchConfiguration('tf_prefix').perform(context)+"_state_publisher", + name=name+'_state_publisher', namespace=namespace, parameters=[{'robot_description': Command( [ @@ -48,9 +49,32 @@ def launch_setup(context, *args, **kwargs): 'cam_pitch:=', cam_pitch, ' ', 'cam_yaw:=', cam_yaw ])}] - ) + ), + LoadComposableNodes( + target_container=name+"_container", + condition=IfCondition(use_composition), + composable_node_descriptions=[ + ComposableNode( + package='robot_state_publisher', + plugin='robot_state_publisher::RobotStatePublisher', + name=name+'_state_publisher', + parameters=[{'robot_description': Command( + [ + 'xacro', ' ', xacro_path, ' ', + 'camera_name:=', tf_prefix, ' ', + 'camera_model:=', camera_model, ' ', + 'base_frame:=', base_frame, ' ', + 'parent_frame:=', parent_frame, ' ', + 'cam_pos_x:=', cam_pos_x, ' ', + 'cam_pos_y:=', cam_pos_y, ' ', + 'cam_pos_z:=', cam_pos_z, ' ', + 'cam_roll:=', cam_roll, ' ', + 'cam_pitch:=', cam_pitch, ' ', + 'cam_yaw:=', cam_yaw + ])}] + )]) + ] - return [rsp_node] def generate_launch_description(): declared_arguments = [ @@ -106,7 +130,11 @@ def generate_launch_description(): DeclareLaunchArgument( 'cam_yaw', default_value='0.0', - description='Yaw orientation of the camera with respect to the base frame.') + description='Yaw orientation of the camera with respect to the base frame.'), + DeclareLaunchArgument( + 'use_composition', + default_value='false', + description='Use composition to start the robot_state_publisher node. Default value will be false') ] return LaunchDescription( diff --git a/depthai_descriptions/package.xml b/depthai_descriptions/package.xml index 0a7f3123..f82814aa 100644 --- a/depthai_descriptions/package.xml +++ b/depthai_descriptions/package.xml @@ -1,7 +1,7 @@ depthai_descriptions - 2.7.5 + 2.8.0 The depthai_descriptions package Sachin Guruswamy diff --git a/depthai_descriptions/urdf/base_descr.urdf.xacro b/depthai_descriptions/urdf/base_descr.urdf.xacro new file mode 100644 index 00000000..e5d04601 --- /dev/null +++ b/depthai_descriptions/urdf/base_descr.urdf.xacro @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/depthai_descriptions/urdf/include/base_macro.urdf.xacro b/depthai_descriptions/urdf/include/base_macro.urdf.xacro new file mode 100644 index 00000000..e46e3bca --- /dev/null +++ b/depthai_descriptions/urdf/include/base_macro.urdf.xacro @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/depthai_descriptions/urdf/include/depthai_macro.urdf.xacro b/depthai_descriptions/urdf/include/depthai_macro.urdf.xacro index df541ec7..d16b7f43 100644 --- a/depthai_descriptions/urdf/include/depthai_macro.urdf.xacro +++ b/depthai_descriptions/urdf/include/depthai_macro.urdf.xacro @@ -1,119 +1,76 @@ - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + cam_roll cam_pitch cam_yaw r:=0.8 g:=0.8 b:=0.8 a:=0.8 "> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + \ No newline at end of file diff --git a/depthai_examples/CMakeLists.txt b/depthai_examples/CMakeLists.txt index 84dc5876..5acb9a8f 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.5 LANGUAGES CXX C) +project(depthai_examples VERSION 2.8.0 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) @@ -98,6 +98,7 @@ dai_add_node_ros2(crop_control_service src/crop_control_service.cpp) dai_add_node_ros2(mobilenet_node src/mobilenet_publisher.cpp) dai_add_node_ros2(rgb_stereo_node src/rgb_stereo_node.cpp) dai_add_node_ros2(stereo_inertial_node src/stereo_inertial_publisher.cpp) +dai_add_node_ros2(feature_tracker src/feature_tracker_publisher.cpp) dai_add_node_ros2(stereo_node src/stereo_publisher.cpp) dai_add_node_ros2(yolov4_spatial_node src/yolov4_spatial_publisher.cpp) @@ -123,6 +124,7 @@ install(TARGETS stereo_inertial_node stereo_node yolov4_spatial_node + feature_tracker DESTINATION lib/${PROJECT_NAME}) ament_package() diff --git a/depthai_examples/package.xml b/depthai_examples/package.xml index 37140550..cfba823a 100644 --- a/depthai_examples/package.xml +++ b/depthai_examples/package.xml @@ -1,7 +1,7 @@ depthai_examples - 2.7.5 + 2.8.0 The depthai_examples package diff --git a/depthai_examples/src/feature_tracker_publisher.cpp b/depthai_examples/src/feature_tracker_publisher.cpp new file mode 100644 index 00000000..ca30d94b --- /dev/null +++ b/depthai_examples/src/feature_tracker_publisher.cpp @@ -0,0 +1,100 @@ +#include +#include +#include +#include + +#include "depthai_ros_msgs/msg/tracked_features.hpp" +#include "rclcpp/rclcpp.hpp" + +// Inludes common necessary includes for development using depthai library +#include "depthai/device/DataQueue.hpp" +#include "depthai/device/Device.hpp" +#include "depthai/pipeline/Pipeline.hpp" +#include "depthai/pipeline/node/ColorCamera.hpp" +#include "depthai/pipeline/node/FeatureTracker.hpp" +#include "depthai/pipeline/node/MonoCamera.hpp" +#include "depthai/pipeline/node/StereoDepth.hpp" +#include "depthai/pipeline/node/XLinkIn.hpp" +#include "depthai/pipeline/node/XLinkOut.hpp" +#include "depthai_bridge/BridgePublisher.hpp" +#include "depthai_bridge/ImageConverter.hpp" +#include "depthai_bridge/TrackedFeaturesConverter.hpp" + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("feature_tracker"); + + dai::Pipeline pipeline; + + // Define sources and outputs + auto monoLeft = pipeline.create(); + auto monoRight = pipeline.create(); + auto featureTrackerLeft = pipeline.create(); + auto featureTrackerRight = pipeline.create(); + + auto xoutTrackedFeaturesLeft = pipeline.create(); + auto xoutTrackedFeaturesRight = pipeline.create(); + + xoutTrackedFeaturesLeft->setStreamName("trackedFeaturesLeft"); + xoutTrackedFeaturesRight->setStreamName("trackedFeaturesRight"); + + // Properties + monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); + monoLeft->setCamera("left"); + monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); + monoRight->setCamera("right"); + + // Linking + monoLeft->out.link(featureTrackerLeft->inputImage); + featureTrackerLeft->outputFeatures.link(xoutTrackedFeaturesLeft->input); + + monoRight->out.link(featureTrackerRight->inputImage); + featureTrackerRight->outputFeatures.link(xoutTrackedFeaturesRight->input); + + // By default the least mount of resources are allocated + // increasing it improves performance when optical flow is enabled + auto numShaves = 2; + auto numMemorySlices = 2; + featureTrackerLeft->setHardwareResources(numShaves, numMemorySlices); + featureTrackerRight->setHardwareResources(numShaves, numMemorySlices); + + auto featureTrackerConfig = featureTrackerRight->initialConfig.get(); + + std::shared_ptr device; + std::vector availableDevices = dai::Device::getAllAvailableDevices(); + + std::cout << "Listing available devices..." << std::endl; + device = std::make_shared(pipeline); + auto outputFeaturesLeftQueue = device->getOutputQueue("trackedFeaturesLeft", 8, false); + auto outputFeaturesRightQueue = device->getOutputQueue("trackedFeaturesRight", 8, false); + std::string tfPrefix = "oak"; + dai::rosBridge::TrackedFeaturesConverter leftConverter(tfPrefix + "_left_camera_optical_frame", true); + + dai::rosBridge::TrackedFeaturesConverter rightConverter(tfPrefix + "_right_camera_optical_frame", true); + + dai::rosBridge::BridgePublisher featuresPubL( + outputFeaturesLeftQueue, + node, + std::string("features_left"), + std::bind(&dai::rosBridge::TrackedFeaturesConverter::toRosMsg, &leftConverter, std::placeholders::_1, std::placeholders::_2), + 30, + "", + "features_left"); + + featuresPubL.addPublisherCallback(); + + dai::rosBridge::BridgePublisher featuresPubR( + outputFeaturesRightQueue, + node, + std::string("features_right"), + std::bind(&dai::rosBridge::TrackedFeaturesConverter::toRosMsg, &rightConverter, std::placeholders::_1, std::placeholders::_2), + 30, + "", + "features_right"); + + featuresPubR.addPublisherCallback(); + std::cout << "Ready." << std::endl; + rclcpp::spin(node); + + return 0; +} diff --git a/depthai_filters/CMakeLists.txt b/depthai_filters/CMakeLists.txt index 9a7c9327..148f4ad4 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.5 LANGUAGES CXX C) +project(depthai_filters VERSION 2.8.0 LANGUAGES CXX C) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -18,6 +18,8 @@ ament_auto_add_library( src/segmentation_overlay.cpp src/spatial_bb.cpp src/wls_filter.cpp + src/feature_tracker_overlay.cpp + src/features_3d.cpp src/utils.cpp ) ament_target_dependencies(${PROJECT_NAME} ${DEPENDENCIES}) @@ -29,7 +31,9 @@ rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::Detection2DOv rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::SegmentationOverlay") rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::WLSFilter") rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::SpatialBB") -ament_export_include_directories(include) +rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::FeatureTrackerOverlay") +rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::Features3D") + install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) install( TARGETS diff --git a/depthai_filters/config/feature_tracker.yaml b/depthai_filters/config/feature_tracker.yaml new file mode 100644 index 00000000..c5db4849 --- /dev/null +++ b/depthai_filters/config/feature_tracker.yaml @@ -0,0 +1,6 @@ +/oak: + ros__parameters: + camera: + i_nn_type: none + rgb: + i_enable_feature_tracker: true \ No newline at end of file diff --git a/depthai_filters/include/depthai_filters/feature_tracker_overlay.hpp b/depthai_filters/include/depthai_filters/feature_tracker_overlay.hpp new file mode 100644 index 00000000..026e38c1 --- /dev/null +++ b/depthai_filters/include/depthai_filters/feature_tracker_overlay.hpp @@ -0,0 +1,46 @@ +#pragma once + +#include "cv_bridge/cv_bridge.h" +#include "depthai_ros_msgs/msg/tracked_features.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "message_filters/subscriber.h" +#include "message_filters/sync_policies/approximate_time.h" +#include "message_filters/synchronizer.h" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/image.hpp" + +namespace depthai_filters { + +class FeatureTrackerOverlay : public rclcpp::Node { + public: + explicit FeatureTrackerOverlay(const rclcpp::NodeOptions& options); + void onInit(); + + void overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& img, const depthai_ros_msgs::msg::TrackedFeatures::ConstSharedPtr& detections); + + message_filters::Subscriber imgSub; + message_filters::Subscriber featureSub; + + typedef message_filters::sync_policies::ApproximateTime syncPolicy; + std::unique_ptr> sync; + rclcpp::Publisher::SharedPtr overlayPub; + + using featureIdType = decltype(geometry_msgs::msg::Point::x); + + private: + void trackFeaturePath(std::vector& features); + + void drawFeatures(cv::Mat& img); + + int circleRadius = 2; + int maxTrackedFeaturesPathLength = 30; + + cv::Scalar lineColor = cv::Scalar(200, 0, 200); + cv::Scalar pointColor = cv::Scalar(0, 0, 255); + + int trackedFeaturesPathLength = 10; + std::unordered_set trackedIDs; + std::unordered_map> trackedFeaturesPath; +}; + +} // namespace depthai_filters \ No newline at end of file diff --git a/depthai_filters/include/depthai_filters/features_3d.hpp b/depthai_filters/include/depthai_filters/features_3d.hpp new file mode 100644 index 00000000..ef1641e5 --- /dev/null +++ b/depthai_filters/include/depthai_filters/features_3d.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include "depthai_ros_msgs/msg/tracked_features.hpp" +#include "message_filters/subscriber.h" +#include "message_filters/sync_policies/approximate_time.h" +#include "message_filters/synchronizer.h" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +namespace depthai_filters { +class Features3D : public rclcpp::Node { + public: + explicit Features3D(const rclcpp::NodeOptions& options); + void onInit(); + + void overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& depth, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info, + const depthai_ros_msgs::msg::TrackedFeatures::ConstSharedPtr& features); + + message_filters::Subscriber depthSub; + message_filters::Subscriber featureSub; + message_filters::Subscriber infoSub; + + typedef message_filters::sync_policies::ApproximateTime + syncPolicy; + std::unique_ptr> sync; + rclcpp::Publisher::SharedPtr overlayPub; + rclcpp::Publisher::SharedPtr pclPub; + float getDepthAt(int x, int y, const sensor_msgs::msg::Image::ConstSharedPtr& depth_image); + bool desqueeze = false; +}; + +} // namespace depthai_filters \ No newline at end of file diff --git a/depthai_filters/launch/example_feature_3d.launch.py b/depthai_filters/launch/example_feature_3d.launch.py new file mode 100644 index 00000000..b1dd16af --- /dev/null +++ b/depthai_filters/launch/example_feature_3d.launch.py @@ -0,0 +1,51 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode + + +def launch_setup(context, *args, **kwargs): + params_file = LaunchConfiguration("params_file") + depthai_prefix = get_package_share_directory("depthai_ros_driver") + name = LaunchConfiguration('name').perform(context) + + return [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(depthai_prefix, 'launch', 'camera.launch.py')), + launch_arguments={"name": name, + "params_file": params_file}.items()), + + LoadComposableNodes( + target_container=name+"_container", + composable_node_descriptions=[ + ComposableNode( + package="depthai_filters", + name="feature_overlay", + plugin="depthai_filters::Features3D", + remappings=[('stereo/image_raw', name+'/stereo/image_raw'), + ('stereo/camera_info', name+'/stereo/camera_info'), + ('feature_tracker/tracked_features', name+'/rgb_feature_tracker/tracked_features')] + ), + ], + ), + + ] + + +def generate_launch_description(): + depthai_filters_prefix = get_package_share_directory("depthai_filters") + + declared_arguments = [ + DeclareLaunchArgument("name", default_value="oak"), + DeclareLaunchArgument("params_file", default_value=os.path.join(depthai_filters_prefix, 'config', 'feature_tracker.yaml')), + ] + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) \ No newline at end of file diff --git a/depthai_filters/launch/example_feature_tracker.launch.py b/depthai_filters/launch/example_feature_tracker.launch.py new file mode 100644 index 00000000..974e6782 --- /dev/null +++ b/depthai_filters/launch/example_feature_tracker.launch.py @@ -0,0 +1,51 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode + + +def launch_setup(context, *args, **kwargs): + params_file = LaunchConfiguration("params_file") + depthai_prefix = get_package_share_directory("depthai_ros_driver") + name = LaunchConfiguration('name').perform(context) + + return [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(depthai_prefix, 'launch', 'camera.launch.py')), + launch_arguments={"name": name, + "params_file": params_file}.items()), + + LoadComposableNodes( + target_container=name+"_container", + composable_node_descriptions=[ + ComposableNode( + package="depthai_filters", + name="feature_overlay_rgb", + plugin="depthai_filters::FeatureTrackerOverlay", + remappings=[('rgb/preview/image_raw', name+'/rgb/image_raw'), + ('feature_tracker/tracked_features', name+'/rgb_feature_tracker/tracked_features'), + ('overlay', 'overlay_rgb')] + ) + ], + ), + + ] + + +def generate_launch_description(): + depthai_filters_prefix = get_package_share_directory("depthai_filters") + + declared_arguments = [ + DeclareLaunchArgument("name", default_value="oak"), + DeclareLaunchArgument("params_file", default_value=os.path.join(depthai_filters_prefix, 'config', 'feature_tracker.yaml')), + ] + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) \ No newline at end of file diff --git a/depthai_filters/launch/spatial_bb.launch.py b/depthai_filters/launch/spatial_bb.launch.py index b8ccdef5..158add17 100644 --- a/depthai_filters/launch/spatial_bb.launch.py +++ b/depthai_filters/launch/spatial_bb.launch.py @@ -28,10 +28,13 @@ def launch_setup(context, *args, **kwargs): ComposableNode( package="depthai_filters", plugin="depthai_filters::SpatialBB", + name="spatial_bb_node", remappings=[ ('stereo/camera_info', name+'/stereo/camera_info'), ('nn/spatial_detections', name+'/nn/spatial_detections'), - ('rgb/preview/image_raw', name+'/rgb/preview/image_raw')] + ('rgb/preview/image_raw', name+'/rgb/preview/image_raw'), + ], + parameters=[params_file], ), ], ), diff --git a/depthai_filters/package.xml b/depthai_filters/package.xml index a4684998..e834f431 100644 --- a/depthai_filters/package.xml +++ b/depthai_filters/package.xml @@ -2,7 +2,7 @@ depthai_filters - 2.7.5 + 2.8.0 Depthai filters package Adam Serafin MIT @@ -18,6 +18,7 @@ cv_bridge image_transport visualization_msgs + depthai_ros_msgs ament_cmake diff --git a/depthai_filters/src/feature_tracker_overlay.cpp b/depthai_filters/src/feature_tracker_overlay.cpp new file mode 100644 index 00000000..107e0c58 --- /dev/null +++ b/depthai_filters/src/feature_tracker_overlay.cpp @@ -0,0 +1,76 @@ +#include "depthai_filters/feature_tracker_overlay.hpp" + +#include "cv_bridge/cv_bridge.h" +#include "depthai_filters/utils.hpp" + +namespace depthai_filters { + +FeatureTrackerOverlay::FeatureTrackerOverlay(const rclcpp::NodeOptions& options) : rclcpp::Node("feature_overlay", options) { + onInit(); +} +void FeatureTrackerOverlay::onInit() { + imgSub.subscribe(this, "rgb/preview/image_raw"); + featureSub.subscribe(this, "feature_tracker/tracked_features"); + sync = std::make_unique>(syncPolicy(10), imgSub, featureSub); + sync->registerCallback(std::bind(&FeatureTrackerOverlay::overlayCB, this, std::placeholders::_1, std::placeholders::_2)); + overlayPub = this->create_publisher("overlay", 10); +} + +void FeatureTrackerOverlay::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& img, + const depthai_ros_msgs::msg::TrackedFeatures::ConstSharedPtr& features) { + cv::Mat imgMat = utils::msgToMat(this->get_logger(), img, sensor_msgs::image_encodings::BGR8); + std::vector f = features->features; + trackFeaturePath(f); + drawFeatures(imgMat); + sensor_msgs::msg::Image outMsg; + cv_bridge::CvImage(img->header, sensor_msgs::image_encodings::BGR8, imgMat).toImageMsg(outMsg); + + overlayPub->publish(outMsg); +} + +void FeatureTrackerOverlay::trackFeaturePath(std::vector& features) { + std::unordered_set newTrackedIDs; + for(auto& currentFeature : features) { + auto currentID = currentFeature.id; + newTrackedIDs.insert(currentID); + + if(!trackedFeaturesPath.count(currentID)) { + trackedFeaturesPath.insert({currentID, std::deque()}); + } + std::deque& path = trackedFeaturesPath.at(currentID); + + path.push_back(currentFeature.position); + while(path.size() > std::max(1, trackedFeaturesPathLength)) { + path.pop_front(); + } + } + + std::unordered_set featuresToRemove; + for(auto& oldId : trackedIDs) { + if(!newTrackedIDs.count(oldId)) { + featuresToRemove.insert(oldId); + } + } + + for(auto& id : featuresToRemove) { + trackedFeaturesPath.erase(id); + } + + trackedIDs = newTrackedIDs; +} +void FeatureTrackerOverlay::drawFeatures(cv::Mat& img) { + for(auto& featurePath : trackedFeaturesPath) { + std::deque& path = featurePath.second; + unsigned int j = 0; + for(j = 0; j < path.size() - 1; j++) { + auto src = cv::Point(path[j].x, path[j].y); + auto dst = cv::Point(path[j + 1].x, path[j + 1].y); + cv::line(img, src, dst, lineColor, 1, cv::LINE_AA, 0); + } + + cv::circle(img, cv::Point(path[j].x, path[j].y), circleRadius, pointColor, -1, cv::LINE_AA, 0); + } +} +} // namespace depthai_filters +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(depthai_filters::FeatureTrackerOverlay); \ No newline at end of file diff --git a/depthai_filters/src/features_3d.cpp b/depthai_filters/src/features_3d.cpp new file mode 100644 index 00000000..314d1ba2 --- /dev/null +++ b/depthai_filters/src/features_3d.cpp @@ -0,0 +1,69 @@ +#include "depthai_filters/features_3d.hpp" + +#include "cv_bridge/cv_bridge.h" +#include "depthai_filters/utils.hpp" +#include "geometry_msgs/msg/point32.hpp" +#include "opencv2/opencv.hpp" +#include "sensor_msgs/point_cloud2_iterator.hpp" + +namespace depthai_filters { + +Features3D::Features3D(const rclcpp::NodeOptions& options) : rclcpp::Node("features3d", options) { + onInit(); +} +void Features3D::onInit() { + depthSub.subscribe(this, "stereo/image_raw"); + infoSub.subscribe(this, "stereo/camera_info"); + featureSub.subscribe(this, "feature_tracker/tracked_features"); + sync = std::make_unique>(syncPolicy(10), depthSub, infoSub, featureSub); + sync->registerCallback(std::bind(&Features3D::overlayCB, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + pclPub = this->create_publisher("features", 10); + overlayPub = this->create_publisher("overlay", 10); + desqueeze = this->declare_parameter("desqueeze", false); +} +float Features3D::getDepthAt(int x, int y, const sensor_msgs::msg::Image::ConstSharedPtr& depth_image) { + // Assuming depth image is of encoding type 16UC1 (16-bit depth, unsigned) + int row_step = depth_image->step; // bytes per row + int pixel_index = y * row_step + x * 2; // 2 bytes per pixel for 16-bit image + + // Get the 16-bit depth value + uint16_t depth_raw = *(uint16_t*)(&depth_image->data[pixel_index]); + + // Convert to meters or appropriate unit (this depends on your depth sensor configuration) + // Often, raw values are in millimeters, so convert to meters. + float depth_meters = depth_raw * 0.001; + + return depth_meters; +} +void Features3D::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& depth, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info, + const depthai_ros_msgs::msg::TrackedFeatures::ConstSharedPtr& features) { + sensor_msgs::msg::PointCloud2 cloud; + cloud.header.frame_id = info->header.frame_id; // Set this to your camera's frame + cloud.header.stamp = this->get_clock()->now(); + cloud.height = 1; + cloud.width = features->features.size(); // assuming features is your vector of 2D points + sensor_msgs::PointCloud2Modifier pcd_modifier(cloud); + pcd_modifier.setPointCloud2FieldsByString(1, "xyz"); + sensor_msgs::PointCloud2Iterator out_x(cloud, "x"); + sensor_msgs::PointCloud2Iterator out_y(cloud, "y"); + sensor_msgs::PointCloud2Iterator out_z(cloud, "z"); + double fx = info->k[0]; + double fy = info->k[4]; + double cx = info->k[2]; + double cy = info->k[5]; + for(const auto& feature : features->features) { + float depthVal = getDepthAt(feature.position.x, feature.position.y, depth); // Define this function based on your depth image structure + *out_x = (feature.position.x - cx) * depthVal / fx; + *out_y = (feature.position.y - cy) * depthVal / fy; + *out_z = depthVal; + ++out_x; + ++out_y; + ++out_z; + } + pclPub->publish(cloud); +} + +} // namespace depthai_filters +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(depthai_filters::Features3D); \ No newline at end of file diff --git a/depthai_ros_driver/CMakeLists.txt b/depthai_ros_driver/CMakeLists.txt index 71502987..27e84c2a 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.5) +project(depthai_ros_driver VERSION 2.8.0) set(CMAKE_POSITION_INDEPENDENT_CODE ON) set(CMAKE_BUILD_SHARED_LIBS ON) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) @@ -28,6 +28,8 @@ depthai_bridge image_transport rclcpp sensor_msgs +diagnostic_updater +diagnostic_msgs ) set(SENSOR_DEPS @@ -63,11 +65,13 @@ add_library( ${COMMON_LIB_NAME} SHARED src/utils.cpp src/dai_nodes/base_node.cpp + src/dai_nodes/sys_logger.cpp src/dai_nodes/sensors/sensor_helpers.cpp # TODO: Figure out different place for this src/param_handlers/camera_param_handler.cpp src/param_handlers/imu_param_handler.cpp src/param_handlers/nn_param_handler.cpp src/param_handlers/sensor_param_handler.cpp + src/param_handlers/feature_tracker_param_handler.cpp src/param_handlers/stereo_param_handler.cpp ) @@ -79,6 +83,7 @@ add_library( src/dai_nodes/sensors/imu.cpp src/dai_nodes/sensors/rgb.cpp src/dai_nodes/sensors/mono.cpp + src/dai_nodes/sensors/feature_tracker.cpp src/dai_nodes/sensors/sensor_wrapper.cpp src/dai_nodes/stereo.cpp ) @@ -116,7 +121,6 @@ target_link_libraries( ${COMMON_LIB_NAME} ) - rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::Camera") pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) ament_export_include_directories(include) diff --git a/depthai_ros_driver/config/camera.yaml b/depthai_ros_driver/config/camera.yaml index 2ebd09db..1cfe524a 100644 --- a/depthai_ros_driver/config/camera.yaml +++ b/depthai_ros_driver/config/camera.yaml @@ -24,7 +24,7 @@ i_gyro_freq: 400 i_mag_cov: 0.0 i_mag_freq: 100 - i_max_batch_reports: 10 + i_max_batch_reports: 5 i_message_type: IMU i_rot_cov: -1.0 i_rot_freq: 400 @@ -108,7 +108,7 @@ i_disparity_width: DISPARITY_96 i_enable_companding: false i_enable_decimation_filter: false - i_enable_distortion_correction: true + i_enable_distortion_correction: false i_enable_spatial_filter: false i_enable_speckle_filter: false i_enable_temporal_filter: false @@ -124,7 +124,7 @@ i_lrc_threshold: 10 i_max_q_size: 30 i_rectify_edge_fill_color: 0 - i_stereo_conf_threshold: 255 + i_stereo_conf_threshold: 240 i_set_input_size: false i_input_width: 1280 i_input_height: 720 diff --git a/depthai_ros_driver/config/pcl.yaml b/depthai_ros_driver/config/pcl.yaml index 1db745e8..6fcddb92 100644 --- a/depthai_ros_driver/config/pcl.yaml +++ b/depthai_ros_driver/config/pcl.yaml @@ -2,8 +2,7 @@ ros__parameters: camera: i_nn_type: none - right: - i_publish_topic: true stereo: i_align_depth: false i_subpixel: true + i_publish_right_rect: true \ No newline at end of file diff --git a/depthai_ros_driver/include/depthai_ros_driver/camera.hpp b/depthai_ros_driver/include/depthai_ros_driver/camera.hpp index 02694e3b..12376b38 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/camera.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/camera.hpp @@ -4,8 +4,10 @@ #include #include +#include "depthai_bridge/TFPublisher.hpp" #include "depthai_ros_driver/dai_nodes/base_node.hpp" #include "depthai_ros_driver/param_handlers/camera_param_handler.hpp" +#include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "rclcpp/node.hpp" #include "std_srvs/srv/trigger.hpp" @@ -19,32 +21,74 @@ using Trigger = std_srvs::srv::Trigger; class Camera : public rclcpp::Node { public: explicit Camera(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + /* + * @brief Destructor of the class Camera. Stops the device and destroys the pipeline. + */ + ~Camera(); + /* + * @brief Creates the pipeline and starts the device. Also sets up parameter callback and services. + */ void onConfigure(); private: + /* + * @brief Print information about the device type. + */ void getDeviceType(); + /* + * @brief Create the pipeline by using PipelineGenerator. + */ void createPipeline(); - void loadNodes(); + /* + * @brief Connect either to a first available device or to a device with a specific USB port, MXID or IP. Loops continuously until a device is found. + */ void startDevice(); - void rgbPipeline(); + /* + * @brief Sets up the queues and creates publishers for the nodes in the pipeline. + */ void setupQueues(); + /* + * @brief Sets IR floodlight and dot pattern projector. + */ void setIR(); + /* + * @brief Saves pipeline as a json to a file. + */ void savePipeline(); + /* + * @brief Saves calibration data to a json file. + */ void saveCalib(); + /* + * @brief Loads calibration data from a path. + * @param path Path to the calibration file. + */ void loadCalib(const std::string& path); rcl_interfaces::msg::SetParametersResult parameterCB(const std::vector& params); OnSetParametersCallbackHandle::SharedPtr paramCBHandle; std::unique_ptr ph; rclcpp::Service::SharedPtr startSrv, stopSrv, savePipelineSrv, saveCalibSrv; + rclcpp::Subscription::SharedPtr diagSub; + /* + * Closes all the queues, clears the configured BaseNodes, stops the pipeline and resets the device. + */ + void stop(); + /* + * Runs onConfigure(); + */ + void start(); + void restart(); + void diagCB(const diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg); + void startCB(const Trigger::Request::SharedPtr /*req*/, Trigger::Response::SharedPtr res); void stopCB(const Trigger::Request::SharedPtr /*req*/, Trigger::Response::SharedPtr res); void saveCalibCB(const Trigger::Request::SharedPtr /*req*/, Trigger::Response::SharedPtr res); void savePipelineCB(const Trigger::Request::SharedPtr /*req*/, Trigger::Response::SharedPtr res); - std::vector usbStrings = {"UNKNOWN", "LOW", "FULL", "HIGH", "SUPER", "SUPER_PLUS"}; std::shared_ptr pipeline; std::shared_ptr device; std::vector> daiNodes; bool camRunning = false; + std::unique_ptr tfPub; }; } // namespace depthai_ros_driver \ 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 fa8c71bc..b3ab1f9c 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 @@ -19,25 +19,57 @@ namespace depthai_ros_driver { namespace dai_nodes { class BaseNode { public: + /* + * @brief Constructor of the class BaseNode. Creates a node in the pipeline. + * + * @param[in] daiNodeName The dai node name + * @param node The node + * @param pipeline The pipeline + */ BaseNode(const std::string& daiNodeName, rclcpp::Node* node, std::shared_ptr pipeline); virtual ~BaseNode(); - virtual void updateParams(const std::vector& params) = 0; - virtual void link(dai::Node::Input in, int linkType = 0) = 0; + virtual void updateParams(const std::vector& params); + virtual void link(dai::Node::Input in, int linkType = 0); virtual dai::Node::Input getInput(int linkType = 0); virtual void setupQueues(std::shared_ptr device) = 0; + /* + * @brief Sets the names of the queues. + */ virtual void setNames() = 0; + /* + * @brief Link inputs and outputs. + * + * @param pipeline The pipeline + */ virtual void setXinXout(std::shared_ptr pipeline) = 0; virtual void closeQueues() = 0; void setNodeName(const std::string& daiNodeName); void setROSNodePointer(rclcpp::Node* node); + /* + * @brief Gets the ROS node pointer. + * + * @return The ROS node pointer. + */ rclcpp::Node* getROSNode(); + /* + * @brief Gets the name of the node. + * + * @return The name. + */ std::string getName(); + /* + * @brief Append ROS node name to the frameName given. + * + * @param[in] frameName The frame name + */ std::string getTFPrefix(const std::string& frameName = ""); + bool ipcEnabled(); private: rclcpp::Node* baseNode; std::string baseDAINodeName; + bool intraProcessEnabled; }; } // 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/nn/detection.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp index 26728ad9..ec3a390b 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 @@ -27,6 +27,14 @@ namespace nn { template class Detection : public BaseNode { public: + /* + * @brief Constructor of the class Detection. Creates a DetectionNetwork node in the pipeline. Also creates an ImageManip node in the pipeline. + * The ImageManip node is used to resize the input frames to the size required by the DetectionNetwork node. + * + * @param[in] daiNodeName The dai node name + * @param node The node + * @param pipeline The pipeline + */ Detection(const std::string& daiNodeName, rclcpp::Node* node, std::shared_ptr pipeline) : BaseNode(daiNodeName, node, pipeline) { RCLCPP_DEBUG(node->get_logger(), "Creating node %s", daiNodeName.c_str()); setNames(); @@ -39,7 +47,12 @@ class Detection : public BaseNode { setXinXout(pipeline); } ~Detection() = default; - + /* + * @brief Sets up the queues for the DetectionNetwork node and the ImageManip node. Also sets up the publishers for the DetectionNetwork node and the + * ImageManip node. + * + * @param device The device + */ void setupQueues(std::shared_ptr device) override { nnQ = device->getOutputQueue(nnQName, ph->getParam("i_max_q_size"), false); auto tfPrefix = getTFPrefix("rgb"); @@ -74,22 +87,41 @@ class Detection : public BaseNode { 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)); + ptQ->addCallback(std::bind(sensor_helpers::basicCameraPub, std::placeholders::_1, std::placeholders::_2, *imageConverter, ptPub, infoManager)); } }; + /* + * @brief Links the input of the DetectionNetwork node to the output of the ImageManip node. + * + * @param[in] in The input of the DetectionNetwork node + * @param[in] linkType The link type (not used) + */ void link(dai::Node::Input in, int /*linkType*/) override { detectionNode->out.link(in); }; + /* + * @brief Gets the input of the DetectionNetwork node. + * + * @param[in] linkType The link type (not used) + * + * @return The input of the DetectionNetwork node. + */ dai::Node::Input getInput(int /*linkType*/) override { if(ph->getParam("i_disable_resize")) { return detectionNode->input; } return imageManip->inputImage; }; + void setNames() override { nnQName = getName() + "_nn"; ptQName = getName() + "_pt"; }; + /* + * @brief Sets the XLinkOut for the DetectionNetwork node and the ImageManip node. Additionally enables the passthrough. + * + * @param pipeline The pipeline + */ void setXinXout(std::shared_ptr pipeline) override { xoutNN = pipeline->create(); xoutNN->setStreamName(nnQName); @@ -100,6 +132,9 @@ class Detection : public BaseNode { detectionNode->passthrough.link(xoutPT->input); } }; + /* + * @brief Closes the queues for the DetectionNetwork node and the passthrough. + */ void closeQueues() override { nnQ->close(); if(ph->getParam("i_enable_passthrough")) { @@ -112,6 +147,12 @@ class Detection : public BaseNode { }; private: + /* + * @brief Callback for the DetectionNetwork node. Converts the ImgDetections to Detection2DArray and publishes it. + * + * @param[in] name The name of the stream + * @param[in] data The DAI data + */ void detectionCB(const std::string& /*name*/, const std::shared_ptr& data) { auto inDet = std::dynamic_pointer_cast(data); std::deque deq; 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 5cba6324..ea82cece 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp @@ -76,7 +76,7 @@ class SpatialDetection : public BaseNode { 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)); + ptQ->addCallback(std::bind(sensor_helpers::basicCameraPub, std::placeholders::_1, std::placeholders::_2, *ptImageConverter, ptPub, ptInfoMan)); } if(ph->getParam("i_enable_passthrough_depth")) { @@ -98,7 +98,7 @@ class SpatialDetection : public BaseNode { ptDepthPub = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/passthrough_depth/image_raw"); ptDepthQ->addCallback( - std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *ptDepthImageConverter, ptDepthPub, ptDepthInfoMan)); + std::bind(sensor_helpers::basicCameraPub, std::placeholders::_1, std::placeholders::_2, *ptDepthImageConverter, ptDepthPub, ptDepthInfoMan)); } }; void link(dai::Node::Input in, int /*linkType = 0*/) override { diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/feature_tracker.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/feature_tracker.hpp new file mode 100644 index 00000000..746d6cc5 --- /dev/null +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/feature_tracker.hpp @@ -0,0 +1,58 @@ +#pragma once + +#include "depthai_ros_driver/dai_nodes/base_node.hpp" +#include "depthai_ros_msgs/msg/tracked_features.hpp" +#include "rclcpp/publisher.hpp" + +namespace dai { +class Pipeline; +class Device; +class DataOutputQueue; +class ADatatype; +namespace node { +class FeatureTracker; +class XLinkOut; +} // namespace node +namespace ros { +class TrackedFeaturesConverter; +} +} // namespace dai + +namespace rclcpp { +class Node; +class Parameter; +} // namespace rclcpp + +namespace depthai_ros_driver { +namespace param_handlers { +class FeatureTrackerParamHandler; +} +namespace dai_nodes { + +class FeatureTracker : public BaseNode { + public: + explicit FeatureTracker(const std::string& daiNodeName, rclcpp::Node* node, std::shared_ptr pipeline); + ~FeatureTracker(); + void updateParams(const std::vector& params) override; + void setupQueues(std::shared_ptr device) override; + void link(dai::Node::Input in, int linkType = 0) override; + dai::Node::Input getInput(int linkType = 0) override; + void setNames() override; + void setXinXout(std::shared_ptr pipeline) override; + void closeQueues() override; + void getParentName(const std::string& fullName); + + private: + std::unique_ptr featureConverter; + void featureQCB(const std::string& name, const std::shared_ptr& data); + rclcpp::Publisher::SharedPtr featurePub; + std::shared_ptr featureNode; + std::unique_ptr ph; + std::shared_ptr featureQ; + std::shared_ptr xoutFeature; + std::string featureQName; + std::string parentName; +}; + +} // 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/sensors/mono.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/mono.hpp index 76bf1bcb..5621dada 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 @@ -4,6 +4,8 @@ #include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" #include "image_transport/camera_publisher.hpp" #include "image_transport/image_transport.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/image.hpp" namespace dai { class Pipeline; @@ -55,7 +57,9 @@ class Mono : public BaseNode { private: std::unique_ptr imageConverter; - image_transport::CameraPublisher monoPub; + image_transport::CameraPublisher monoPubIT; + rclcpp::Publisher::SharedPtr monoPub; + rclcpp::Publisher::SharedPtr infoPub; std::shared_ptr infoManager; std::shared_ptr monoCamNode; std::shared_ptr videoEnc; 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 8414105c..864a993e 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 @@ -4,6 +4,7 @@ #include "image_transport/camera_publisher.hpp" #include "image_transport/image_transport.hpp" #include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/image.hpp" namespace dai { class Pipeline; @@ -60,7 +61,9 @@ class RGB : public BaseNode { private: std::unique_ptr imageConverter; - image_transport::CameraPublisher rgbPub, previewPub; + image_transport::CameraPublisher rgbPubIT, previewPubIT; + rclcpp::Publisher::SharedPtr rgbPub, previewPub; + rclcpp::Publisher::SharedPtr rgbInfoPub, previewInfoPub; std::shared_ptr infoManager, previewInfoManager; std::shared_ptr colorCamNode; std::shared_ptr videoEnc; 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 06c2b7e6..09608c75 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 @@ -38,24 +38,32 @@ enum class RGBLinkType { video, isp, preview }; namespace sensor_helpers { struct ImageSensor { std::string name; + std::string defaultResolution; std::vector allowedResolutions; bool color; - void getSizeFromResolution(const dai::ColorCameraProperties::SensorResolution& res, int& width, int& height); }; extern std::vector availableSensors; -void imgCB(const std::string& /*name*/, - const std::shared_ptr& data, - dai::ros::ImageConverter& converter, - image_transport::CameraPublisher& pub, - std::shared_ptr infoManager); +void basicCameraPub(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); +void cameraPub(const std::string& /*name*/, + const std::shared_ptr& data, + dai::ros::ImageConverter& converter, + image_transport::CameraPublisher& pub, + std::shared_ptr infoManager, + bool lazyPub = true); + +void splitPub(const std::string& /*name*/, + const std::shared_ptr& data, + dai::ros::ImageConverter& converter, + rclcpp::Publisher::SharedPtr imgPub, + rclcpp::Publisher::SharedPtr infoPub, + std::shared_ptr infoManager, + bool lazyPub = true); sensor_msgs::msg::CameraInfo getCalibInfo(const rclcpp::Logger& logger, dai::ros::ImageConverter& converter, @@ -66,6 +74,8 @@ sensor_msgs::msg::CameraInfo getCalibInfo(const rclcpp::Logger& logger, std::shared_ptr createEncoder(std::shared_ptr pipeline, int quality, dai::VideoEncoderProperties::Profile profile = dai::VideoEncoderProperties::Profile::MJPEG); +bool detectSubscription(const rclcpp::Publisher::SharedPtr& pub, + const rclcpp::Publisher::SharedPtr& infoPub); } // 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/sensors/sensor_wrapper.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_wrapper.hpp index 7e6f45a0..080c5700 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_wrapper.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_wrapper.hpp @@ -53,7 +53,7 @@ class SensorWrapper : public BaseNode { private: void subCB(const sensor_msgs::msg::Image& img); - std::unique_ptr sensorNode; + std::unique_ptr sensorNode, featureTrackerNode; std::unique_ptr ph; std::unique_ptr converter; rclcpp::Subscription::SharedPtr sub; 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 c3d7f062..bcc09024 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 @@ -5,14 +5,19 @@ #include #include "depthai-shared/common/CameraBoardSocket.hpp" +#include "depthai-shared/common/CameraFeatures.hpp" #include "depthai_ros_driver/dai_nodes/sensors/sensor_wrapper.hpp" #include "image_transport/camera_publisher.hpp" #include "image_transport/image_transport.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/image.hpp" + namespace dai { class Pipeline; class Device; class DataOutputQueue; class ADatatype; +class ImgFrame; namespace node { class StereoDepth; class XLinkOut; @@ -42,19 +47,14 @@ namespace link_types { enum class StereoLinkType { left, right }; }; -struct StereoSensorInfo { - std::string name; - dai::CameraBoardSocket socket; -}; - class Stereo : public BaseNode { public: explicit Stereo(const std::string& daiNodeName, rclcpp::Node* node, std::shared_ptr pipeline, std::shared_ptr device, - StereoSensorInfo leftInfo = StereoSensorInfo{"left", dai::CameraBoardSocket::CAM_B}, - StereoSensorInfo rightInfo = StereoSensorInfo{"right", dai::CameraBoardSocket::CAM_C}); + dai::CameraBoardSocket leftSocket = dai::CameraBoardSocket::CAM_B, + dai::CameraBoardSocket rightSocket = dai::CameraBoardSocket::CAM_C); ~Stereo(); void updateParams(const std::vector& params) override; void setupQueues(std::shared_ptr device) override; @@ -68,18 +68,38 @@ class Stereo : public BaseNode { void setupStereoQueue(std::shared_ptr device); void setupLeftRectQueue(std::shared_ptr device); void setupRightRectQueue(std::shared_ptr device); + void setupRectQueue(std::shared_ptr device, + dai::CameraFeatures& sensorInfo, + const std::string& queueName, + std::unique_ptr& conv, + std::shared_ptr& im, + std::shared_ptr& q, + rclcpp::Publisher::SharedPtr pub, + rclcpp::Publisher::SharedPtr infoPub, + image_transport::CameraPublisher& pubIT, + bool isLeft); + /* + * This callback is used to synchronize left and right rectified frames + * It is called every 10ms and it publishes the frames if they are synchronized + * If they are not synchronized, it prints a warning message + */ + void syncTimerCB(); std::unique_ptr stereoConv, leftRectConv, rightRectConv; - image_transport::CameraPublisher stereoPub, leftRectPub, rightRectPub; + image_transport::CameraPublisher stereoPubIT, leftRectPubIT, rightRectPubIT; + rclcpp::Publisher::SharedPtr stereoPub, leftRectPub, rightRectPub; + rclcpp::Publisher::SharedPtr stereoInfoPub, leftRectInfoPub, rightRectInfoPub; std::shared_ptr stereoIM, leftRectIM, rightRectIM; std::shared_ptr stereoCamNode; std::shared_ptr stereoEnc, leftRectEnc, rightRectEnc; std::unique_ptr left; std::unique_ptr right; + std::unique_ptr featureTrackerLeftR, featureTrackerRightR; std::unique_ptr ph; std::shared_ptr stereoQ, leftRectQ, rightRectQ; std::shared_ptr xoutStereo, xoutLeftRect, xoutRightRect; std::string stereoQName, leftRectQName, rightRectQName; - StereoSensorInfo leftSensInfo, rightSensInfo; + dai::CameraFeatures leftSensInfo, rightSensInfo; + rclcpp::TimerBase::SharedPtr syncTimer; }; } // namespace dai_nodes diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sys_logger.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sys_logger.hpp new file mode 100644 index 00000000..ccebcc4a --- /dev/null +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sys_logger.hpp @@ -0,0 +1,42 @@ +#include "depthai/pipeline/datatype/SystemInformation.hpp" +#include "depthai_ros_driver/dai_nodes/base_node.hpp" +#include "diagnostic_updater/diagnostic_updater.hpp" +namespace dai { +class Pipeline; +class Device; +class DataOutputQueue; +class ADatatype; +namespace node { +class SystemLogger; +class XLinkOut; +class VideoEncoder; +} // namespace node +} // namespace dai +namespace rclcpp { +class Node; +class TimerBase; +} // namespace rclcpp + +namespace depthai_ros_driver { + +namespace dai_nodes { +class SysLogger : public BaseNode { + public: + SysLogger(const std::string& daiNodeName, rclcpp::Node* node, std::shared_ptr pipeline); + ~SysLogger(); + void setupQueues(std::shared_ptr device) override; + void setNames() override; + void setXinXout(std::shared_ptr pipeline) override; + void closeQueues() override; + + private: + std::string sysInfoToString(const dai::SystemInformation& sysInfo); + void produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat); + std::shared_ptr updater; + std::shared_ptr xoutLogger; + std::shared_ptr sysNode; + std::shared_ptr loggerQ; + std::string loggerQName; +}; +} // namespace dai_nodes +} // namespace depthai_ros_driver \ No newline at end of file diff --git a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/base_param_handler.hpp b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/base_param_handler.hpp index cb00f4a6..1faac56c 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/base_param_handler.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/base_param_handler.hpp @@ -41,7 +41,7 @@ class BaseParamHandler { return baseName + "." + paramName; } std::string getFullParamName(const std::string& daiNodeName, const std::string& paramName) { - std::string name = std::string(baseNode->get_namespace()) + "/" + daiNodeName + "." + paramName; + std::string name = daiNodeName + "." + paramName; return name; } diff --git a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/feature_tracker_param_handler.hpp b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/feature_tracker_param_handler.hpp new file mode 100644 index 00000000..14c8a9ab --- /dev/null +++ b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/feature_tracker_param_handler.hpp @@ -0,0 +1,34 @@ +#pragma once + +#include +#include +#include + +#include "depthai/pipeline/datatype/CameraControl.hpp" +#include "depthai/pipeline/datatype/FeatureTrackerConfig.hpp" +#include "depthai_ros_driver/param_handlers/base_param_handler.hpp" + +namespace dai { +namespace node { +class FeatureTracker; +} +} // namespace dai + +namespace rclcpp { +class Node; +class Parameter; +} // namespace rclcpp + +namespace depthai_ros_driver { +namespace param_handlers { + +class FeatureTrackerParamHandler : public BaseParamHandler { + public: + explicit FeatureTrackerParamHandler(rclcpp::Node* node, const std::string& name); + ~FeatureTrackerParamHandler(); + void declareParams(std::shared_ptr featureTracker); + dai::CameraControl setRuntimeParams(const std::vector& params) override; + std::unordered_map motionEstMap; +}; +} // namespace param_handlers +} // namespace depthai_ros_driver \ No newline at end of file diff --git a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/sensor_param_handler.hpp b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/sensor_param_handler.hpp index 87aa3ed2..5c47deaa 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/sensor_param_handler.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/sensor_param_handler.hpp @@ -43,6 +43,7 @@ class SensorParamHandler : public BaseParamHandler { std::unordered_map monoResolutionMap; std::unordered_map rgbResolutionMap; std::unordered_map fSyncModeMap; + std::unordered_map cameraImageOrientationMap; }; } // namespace param_handlers } // namespace depthai_ros_driver \ No newline at end of file diff --git a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/stereo_param_handler.hpp b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/stereo_param_handler.hpp index 6b0e9857..26182143 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/stereo_param_handler.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/stereo_param_handler.hpp @@ -9,6 +9,10 @@ #include "depthai/pipeline/node/StereoDepth.hpp" #include "depthai_ros_driver/param_handlers/base_param_handler.hpp" +namespace dai { +class CameraFeatures; +} + namespace rclcpp { class Node; class Parameter; @@ -20,8 +24,9 @@ class StereoParamHandler : public BaseParamHandler { public: explicit StereoParamHandler(rclcpp::Node* node, const std::string& name); ~StereoParamHandler(); - void declareParams(std::shared_ptr stereo, const std::string& rightName); + void declareParams(std::shared_ptr stereo, const std::vector& camFeatures); dai::CameraControl setRuntimeParams(const std::vector& params) override; + void updateSocketsFromParams(dai::CameraBoardSocket& left, dai::CameraBoardSocket& right); private: std::unordered_map depthPresetMap; 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 index 2959e68b..ffce5152 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/pipeline/base_pipeline.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/pipeline/base_pipeline.hpp @@ -50,7 +50,6 @@ class BasePipeline { protected: BasePipeline(){}; - const std::string alphabet = "abcdefghijklmnopqrstuvwxyz"; std::unordered_map nnTypeMap = { {"", NNType::None}, {"NONE", NNType::None}, diff --git a/depthai_ros_driver/include/depthai_ros_driver/pipeline/pipeline_generator.hpp b/depthai_ros_driver/include/depthai_ros_driver/pipeline/pipeline_generator.hpp index ff7fdefd..5904c4ef 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/pipeline/pipeline_generator.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/pipeline/pipeline_generator.hpp @@ -23,7 +23,28 @@ enum class PipelineType { RGB, RGBD, RGBStereo, Stereo, Depth, CamArray }; class PipelineGenerator { public: ~PipelineGenerator() = default; + /* + * @brief Validates the pipeline type. If the pipeline type is not valid for the number of sensors, it will be changed to the default type. + * + * @param node The node + * @param[in] type The type + * @param[in] sensorNum The sensor number + * + * @return The validated pipeline type. + */ PipelineType validatePipeline(rclcpp::Node* node, PipelineType type, int sensorNum); + /* + * @brief Creates the pipeline by using a plugin. Plugin types need to be of type depthai_ros_driver::pipeline_gen::BasePipeline. + * + * @param node The node + * @param device The device + * @param pipeline The pipeline + * @param[in] pipelineType The pipeline type name (plugin name or one of the default types) + * @param[in] nnType The neural network type (none, rgb, spatial) + * @param[in] enableImu Indicates if IMU is enabled + * + * @return Vector BaseNodes created. + */ std::vector> createPipeline(rclcpp::Node* node, std::shared_ptr device, std::shared_ptr pipeline, diff --git a/depthai_ros_driver/include/depthai_ros_driver/utils.hpp b/depthai_ros_driver/include/depthai_ros_driver/utils.hpp index b370dd3b..841cb6ef 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/utils.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/utils.hpp @@ -5,6 +5,12 @@ #include #include #include +#include + +namespace dai { +enum class CameraBoardSocket; +struct CameraFeatures; +} // namespace dai namespace depthai_ros_driver { namespace utils { @@ -23,5 +29,6 @@ T getValFromMap(const std::string& name, const std::unordered_map camFeatures); } // namespace utils } // namespace depthai_ros_driver \ No newline at end of file diff --git a/depthai_ros_driver/launch/camera.launch.py b/depthai_ros_driver/launch/camera.launch.py index 006e9be1..a51dd3f8 100644 --- a/depthai_ros_driver/launch/camera.launch.py +++ b/depthai_ros_driver/launch/camera.launch.py @@ -15,6 +15,8 @@ def launch_setup(context, *args, **kwargs): if(context.environment.get('DEPTHAI_DEBUG')=='1'): log_level='debug' + + urdf_launch_dir = os.path.join(get_package_share_directory('depthai_descriptions'), 'launch') params_file = LaunchConfiguration("params_file") @@ -29,7 +31,44 @@ def launch_setup(context, *args, **kwargs): cam_roll = LaunchConfiguration('cam_roll', default = '0.0') cam_pitch = LaunchConfiguration('cam_pitch', default = '0.0') cam_yaw = LaunchConfiguration('cam_yaw', default = '0.0') + use_composition = LaunchConfiguration('rsp_use_composition', default='true') + imu_from_descr = LaunchConfiguration('imu_from_descr', default='false') + pass_tf_args_as_params = LaunchConfiguration('pass_tf_args_as_params', default='false') + override_cam_model = LaunchConfiguration('override_cam_model', default='false') + + tf_params = {} + if(pass_tf_args_as_params.perform(context) == 'true'): + cam_model = '' + if override_cam_model.perform(context) == 'true': + cam_model = camera_model.perform(context) + tf_params = {'camera': { + 'i_publish_tf_from_calibration': True, + 'i_tf_tf_prefix': name, + 'i_tf_camera_model': cam_model, + 'i_tf_base_frame': name, + 'i_tf_parent_frame': parent_frame.perform(context), + 'i_tf_cam_pos_x': cam_pos_x.perform(context), + 'i_tf_cam_pos_y': cam_pos_y.perform(context), + 'i_tf_cam_pos_z': cam_pos_z.perform(context), + 'i_tf_cam_roll': cam_roll.perform(context), + 'i_tf_cam_pitch': cam_pitch.perform(context), + 'i_tf_cam_yaw': cam_yaw.perform(context), + 'i_tf_imu_from_descr': imu_from_descr.perform(context), + } + } + use_gdb = LaunchConfiguration('use_gdb', default = 'false') + use_valgrind = LaunchConfiguration('use_valgrind', default = 'false') + use_perf = LaunchConfiguration('use_perf', default = 'false') + + launch_prefix = '' + + if (use_gdb.perform(context) == 'true'): + launch_prefix += "gdb -ex run --args " + if (use_valgrind.perform(context) == 'true'): + launch_prefix += "valgrind --tool=callgrind" + if (use_perf.perform(context) == 'true'): + launch_prefix += "perf record -g --call-graph dwarf --output=perf.out.node_name.data --" return [ Node( condition=IfCondition(LaunchConfiguration("use_rviz").perform(context)), @@ -51,7 +90,8 @@ def launch_setup(context, *args, **kwargs): 'cam_pos_z': cam_pos_z, 'cam_roll': cam_roll, 'cam_pitch': cam_pitch, - 'cam_yaw': cam_yaw}.items()), + 'cam_yaw': cam_yaw, + 'use_composition': use_composition}.items()), ComposableNodeContainer( name=name+"_container", @@ -63,10 +103,11 @@ def launch_setup(context, *args, **kwargs): package="depthai_ros_driver", plugin="depthai_ros_driver::Camera", name=name, - parameters=[params_file], + parameters=[params_file, tf_params], ) ], arguments=['--ros-args', '--log-level', log_level], + prefix=[launch_prefix], output="both", ), @@ -79,6 +120,7 @@ def generate_launch_description(): declared_arguments = [ DeclareLaunchArgument("name", default_value="oak"), DeclareLaunchArgument("parent_frame", default_value="oak-d-base-frame"), + DeclareLaunchArgument("camera_model", default_value="OAK-D"), DeclareLaunchArgument("cam_pos_x", default_value="0.0"), DeclareLaunchArgument("cam_pos_y", default_value="0.0"), DeclareLaunchArgument("cam_pos_z", default_value="0.0"), @@ -87,7 +129,14 @@ def generate_launch_description(): DeclareLaunchArgument("cam_yaw", default_value="0.0"), DeclareLaunchArgument("params_file", default_value=os.path.join(depthai_prefix, 'config', 'camera.yaml')), DeclareLaunchArgument("use_rviz", default_value='false'), - DeclareLaunchArgument("rviz_config", default_value=os.path.join(depthai_prefix, "config", "rviz", "rgbd.rviz")) + DeclareLaunchArgument("rviz_config", default_value=os.path.join(depthai_prefix, "config", "rviz", "rgbd.rviz")), + DeclareLaunchArgument("rsp_use_composition", default_value='true'), + DeclareLaunchArgument("pass_tf_args_as_params", default_value='false', description='Enables TF publishing from camera calibration file.'), + DeclareLaunchArgument("imu_from_descr", default_value='false', description='Enables IMU publishing from URDF.'), + DeclareLaunchArgument("override_cam_model", default_value='false', description='Overrides camera model from calibration file.'), + DeclareLaunchArgument("use_gdb", default_value='false'), + DeclareLaunchArgument("use_valgrind", default_value='false'), + DeclareLaunchArgument("use_perf", default_value='false') ] return LaunchDescription( diff --git a/depthai_ros_driver/launch/pointcloud.launch.py b/depthai_ros_driver/launch/pointcloud.launch.py index 95775acb..b1e14448 100644 --- a/depthai_ros_driver/launch/pointcloud.launch.py +++ b/depthai_ros_driver/launch/pointcloud.launch.py @@ -39,10 +39,9 @@ def launch_setup(context, *args, **kwargs): package='depth_image_proc', plugin='depth_image_proc::PointCloudXyziNode', name='point_cloud_xyzi', - remappings=[('depth/image_rect', name+'/stereo/image_raw'), - ('intensity/image_rect', name+'/right/image_raw'), - ('intensity/camera_info', name+'/right/camera_info'), + ('intensity/image_rect', name+'/right/image_rect'), + ('intensity/camera_info', name+'/stereo/camera_info'), ('points', name+'/points') ]), ], diff --git a/depthai_ros_driver/launch/rgbd_pcl.launch.py b/depthai_ros_driver/launch/rgbd_pcl.launch.py index 2bb9c10b..6b50fa04 100644 --- a/depthai_ros_driver/launch/rgbd_pcl.launch.py +++ b/depthai_ros_driver/launch/rgbd_pcl.launch.py @@ -16,7 +16,6 @@ def launch_setup(context, *args, **kwargs): name = LaunchConfiguration('name').perform(context) rgb_topic_name = name+'/rgb/image_raw' - print(LaunchConfiguration('rectify_rgb').perform(context)) if LaunchConfiguration('rectify_rgb').perform(context)=='true': rgb_topic_name = name +'/rgb/image_rect' return [ diff --git a/depthai_ros_driver/package.xml b/depthai_ros_driver/package.xml index a137d208..e0bd5ca0 100644 --- a/depthai_ros_driver/package.xml +++ b/depthai_ros_driver/package.xml @@ -2,7 +2,7 @@ depthai_ros_driver - 2.7.5 + 2.8.0 Depthai ROS Monolithic node. Adam Serafin Sachin Guruswamy @@ -16,7 +16,6 @@ rclcpp_components sensor_msgs vision_msgs - diagnostic_msgs std_msgs std_srvs cv_bridge @@ -29,7 +28,8 @@ depthai_ros_msgs depthai_examples pluginlib - + diagnostic_updater + diagnostic_msgs ament_cmake diff --git a/depthai_ros_driver/src/camera.cpp b/depthai_ros_driver/src/camera.cpp index e0d94a72..b9f14844 100644 --- a/depthai_ros_driver/src/camera.cpp +++ b/depthai_ros_driver/src/camera.cpp @@ -5,7 +5,9 @@ #include "ament_index_cpp/get_package_share_directory.hpp" #include "depthai/device/Device.hpp" #include "depthai/pipeline/Pipeline.hpp" +#include "depthai_bridge/TFPublisher.hpp" #include "depthai_ros_driver/pipeline/pipeline_generator.hpp" +#include "diagnostic_msgs/msg/diagnostic_status.hpp" namespace depthai_ros_driver { @@ -14,6 +16,7 @@ Camera::Camera(const rclcpp::NodeOptions& options) : rclcpp::Node("camera", opti ph->declareParams(); onConfigure(); } +Camera::~Camera() = default; void Camera::onConfigure() { getDeviceType(); createPipeline(); @@ -25,15 +28,88 @@ void Camera::onConfigure() { stopSrv = this->create_service("~/stop_camera", std::bind(&Camera::stopCB, this, std::placeholders::_1, std::placeholders::_2)); savePipelineSrv = this->create_service("~/save_pipeline", std::bind(&Camera::savePipelineCB, this, std::placeholders::_1, std::placeholders::_2)); saveCalibSrv = this->create_service("~/save_calibration", std::bind(&Camera::saveCalibCB, this, std::placeholders::_1, std::placeholders::_2)); + + // If model name not set get one from the device + std::string camModel = ph->getParam("i_tf_camera_model"); + if(camModel.empty()) { + camModel = device->getDeviceName(); + } + + if(ph->getParam("i_publish_tf_from_calibration")) { + tfPub = std::make_unique(this, + device->readCalibration(), + device->getConnectedCameraFeatures(), + ph->getParam("i_tf_camera_name"), + camModel, + ph->getParam("i_tf_base_frame"), + ph->getParam("i_tf_parent_frame"), + ph->getParam("i_tf_cam_pos_x"), + ph->getParam("i_tf_cam_pos_y"), + ph->getParam("i_tf_cam_pos_z"), + ph->getParam("i_tf_cam_roll"), + ph->getParam("i_tf_cam_pitch"), + ph->getParam("i_tf_cam_yaw"), + ph->getParam("i_tf_imu_from_descr"), + ph->getParam("i_tf_custom_urdf_location"), + ph->getParam("i_tf_custom_xacro_args")); + } + diagSub = this->create_subscription("/diagnostics", 10, std::bind(&Camera::diagCB, this, std::placeholders::_1)); RCLCPP_INFO(this->get_logger(), "Camera ready!"); } +void Camera::diagCB(const diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg) { + for(const auto& status : msg->status) { + if(status.name == get_name() + std::string(": sys_logger")) { + if(status.level == diagnostic_msgs::msg::DiagnosticStatus::ERROR) { + RCLCPP_ERROR(this->get_logger(), "Camera diagnostics error: %s", status.message.c_str()); + restart(); + } + } + } +} + +void Camera::start() { + RCLCPP_INFO(this->get_logger(), "Starting camera."); + if(!camRunning) { + onConfigure(); + } else { + RCLCPP_INFO(this->get_logger(), "Camera already running!."); + } +} + +void Camera::stop() { + RCLCPP_INFO(this->get_logger(), "Stopping camera."); + if(camRunning) { + for(const auto& node : daiNodes) { + node->closeQueues(); + } + daiNodes.clear(); + device.reset(); + pipeline.reset(); + camRunning = false; + } else { + RCLCPP_INFO(this->get_logger(), "Camera already stopped!"); + } +} + +void Camera::restart() { + RCLCPP_ERROR(this->get_logger(), "Restarting camera"); + stop(); + start(); + if(camRunning) { + return; + } else { + RCLCPP_ERROR(this->get_logger(), "Restarting camera failed."); + } +} + void Camera::saveCalib() { auto calibHandler = device->readCalibration(); std::stringstream savePath; savePath << "/tmp/" << device->getMxId().c_str() << "_calibration.json"; RCLCPP_INFO(this->get_logger(), "Saving calibration to: %s", savePath.str().c_str()); calibHandler.eepromToJsonFile(savePath.str()); + auto json = calibHandler.eepromToJson(); } void Camera::loadCalib(const std::string& path) { @@ -62,27 +138,11 @@ void Camera::savePipelineCB(const Trigger::Request::SharedPtr /*req*/, Trigger:: } void Camera::startCB(const Trigger::Request::SharedPtr /*req*/, Trigger::Response::SharedPtr res) { - RCLCPP_INFO(this->get_logger(), "Starting camera."); - if(!camRunning) { - onConfigure(); - } else { - RCLCPP_INFO(this->get_logger(), "Camera already running!."); - } + start(); res->success = true; } void Camera::stopCB(const Trigger::Request::SharedPtr /*req*/, Trigger::Response::SharedPtr res) { - RCLCPP_INFO(this->get_logger(), "Stopping camera."); - if(camRunning) { - for(const auto& node : daiNodes) { - node->closeQueues(); - } - daiNodes.clear(); - device.reset(); - pipeline.reset(); - camRunning = false; - } else { - RCLCPP_INFO(this->get_logger(), "Camera already stopped!"); - } + stop(); res->success = true; } void Camera::getDeviceType() { @@ -124,7 +184,7 @@ void Camera::setupQueues() { void Camera::startDevice() { rclcpp::Rate r(1.0); - while(!camRunning) { + while(rclcpp::ok() && !camRunning) { auto mxid = ph->getParam("i_mx_id"); auto ip = ph->getParam("i_ip"); auto usb_id = ph->getParam("i_usb_port_id"); @@ -159,14 +219,13 @@ void Camera::startDevice() { } 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); + device = std::make_shared(info, speed); camRunning = true; } else if(info.state == X_LINK_BOOTED) { 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."); + RCLCPP_INFO(this->get_logger(), "Ignoring device info: MXID: %s, Name: %s", info.getMxId().c_str(), info.name.c_str()); } } } diff --git a/depthai_ros_driver/src/dai_nodes/base_node.cpp b/depthai_ros_driver/src/dai_nodes/base_node.cpp index 44948b38..c57fa733 100644 --- a/depthai_ros_driver/src/dai_nodes/base_node.cpp +++ b/depthai_ros_driver/src/dai_nodes/base_node.cpp @@ -1,5 +1,6 @@ #include "depthai_ros_driver/dai_nodes/base_node.hpp" +#include "depthai-shared/common/CameraBoardSocket.hpp" #include "depthai/device/Device.hpp" #include "depthai/pipeline/Pipeline.hpp" #include "rclcpp/node.hpp" @@ -9,6 +10,7 @@ namespace dai_nodes { BaseNode::BaseNode(const std::string& daiNodeName, rclcpp::Node* node, std::shared_ptr /*pipeline*/) { setNodeName(daiNodeName); setROSNodePointer(node); + intraProcessEnabled = node->get_node_options().use_intra_process_comms(); }; BaseNode::~BaseNode() = default; void BaseNode::setNodeName(const std::string& daiNodeName) { @@ -23,11 +25,41 @@ rclcpp::Node* BaseNode::getROSNode() { std::string BaseNode::getName() { return baseDAINodeName; }; + +bool BaseNode::ipcEnabled() { + return intraProcessEnabled; +} + std::string BaseNode::getTFPrefix(const std::string& frameName) { return std::string(getROSNode()->get_name()) + "_" + frameName; } + dai::Node::Input BaseNode::getInput(int /*linkType = 0*/) { throw(std::runtime_error("getInput() not implemented")); }; + +void BaseNode::closeQueues() { + throw(std::runtime_error("closeQueues() not implemented")); +}; + +void BaseNode::setNames() { + throw(std::runtime_error("setNames() not implemented")); +}; + +void BaseNode::setXinXout(std::shared_ptr /*pipeline*/) { + throw(std::runtime_error("setXinXout() not implemented")); +}; + +void BaseNode::setupQueues(std::shared_ptr /*device*/) { + throw(std::runtime_error("setupQueues() not implemented")); +}; + +void BaseNode::link(dai::Node::Input /*in*/, int /*linkType = 0*/) { + throw(std::runtime_error("link() not implemented")); +}; + +void BaseNode::updateParams(const std::vector& /*params*/) { + return; +}; } // namespace dai_nodes } // namespace depthai_ros_driver \ No newline at end of file diff --git a/depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp b/depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp index 91885caf..90872dbd 100644 --- a/depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp +++ b/depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp @@ -71,7 +71,7 @@ void Segmentation::setupQueues(std::shared_ptr device) { imageManip->initialConfig.getResizeWidth())); 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)); + ptQ->addCallback(std::bind(sensor_helpers::basicCameraPub, std::placeholders::_1, std::placeholders::_2, *imageConverter, ptPub, infoManager)); } } diff --git a/depthai_ros_driver/src/dai_nodes/sensors/feature_tracker.cpp b/depthai_ros_driver/src/dai_nodes/sensors/feature_tracker.cpp new file mode 100644 index 00000000..a66eedaf --- /dev/null +++ b/depthai_ros_driver/src/dai_nodes/sensors/feature_tracker.cpp @@ -0,0 +1,84 @@ +#include "depthai_ros_driver/dai_nodes/sensors/feature_tracker.hpp" + +#include "depthai/device/DataQueue.hpp" +#include "depthai/device/Device.hpp" +#include "depthai/pipeline/Pipeline.hpp" +#include "depthai/pipeline/node/FeatureTracker.hpp" +#include "depthai/pipeline/node/XLinkOut.hpp" +#include "depthai_bridge/TrackedFeaturesConverter.hpp" +#include "depthai_ros_driver/param_handlers/feature_tracker_param_handler.hpp" +#include "depthai_ros_driver/utils.hpp" +#include "depthai_ros_msgs/msg/tracked_features.hpp" +#include "rclcpp/node.hpp" + +namespace depthai_ros_driver { +namespace dai_nodes { +FeatureTracker::FeatureTracker(const std::string& daiNodeName, rclcpp::Node* node, std::shared_ptr pipeline) + : BaseNode(daiNodeName, node, pipeline) { + RCLCPP_DEBUG(node->get_logger(), "Creating node %s", daiNodeName.c_str()); + getParentName(daiNodeName); + setNames(); + featureNode = pipeline->create(); + ph = std::make_unique(node, daiNodeName); + ph->declareParams(featureNode); + setXinXout(pipeline); + RCLCPP_DEBUG(node->get_logger(), "Node %s created", daiNodeName.c_str()); +} +FeatureTracker::~FeatureTracker() = default; + +void FeatureTracker::getParentName(const std::string& fullName) { + auto endIdx = fullName.find("_"); + parentName = fullName.substr(0, endIdx); +} + +void FeatureTracker::setNames() { + featureQName = parentName + getName() + "_queue"; +} + +void FeatureTracker::setXinXout(std::shared_ptr pipeline) { + xoutFeature = pipeline->create(); + xoutFeature->setStreamName(featureQName); + featureNode->outputFeatures.link(xoutFeature->input); +} + +void FeatureTracker::setupQueues(std::shared_ptr device) { + featureQ = device->getOutputQueue(featureQName, ph->getParam("i_max_q_size"), false); + auto tfPrefix = getTFPrefix(parentName); + rclcpp::PublisherOptions options; + options.qos_overriding_options = rclcpp::QosOverridingOptions(); + featureConverter = std::make_unique(tfPrefix + "_frame", ph->getParam("i_get_base_device_timestamp")); + featureConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); + + featurePub = getROSNode()->create_publisher("~/" + getName() + "/tracked_features", 10, options); + featureQ->addCallback(std::bind(&FeatureTracker::featureQCB, this, std::placeholders::_1, std::placeholders::_2)); +} + +void FeatureTracker::closeQueues() { + featureQ->close(); +} + +void FeatureTracker::featureQCB(const std::string& /*name*/, const std::shared_ptr& data) { + auto featureData = std::dynamic_pointer_cast(data); + std::deque deq; + featureConverter->toRosMsg(featureData, deq); + while(deq.size() > 0) { + auto currMsg = deq.front(); + featurePub->publish(currMsg); + deq.pop_front(); + } +} + +void FeatureTracker::link(dai::Node::Input in, int /*linkType*/) { + featureNode->outputFeatures.link(in); +} + +dai::Node::Input FeatureTracker::getInput(int /*linkType*/) { + return featureNode->inputImage; +} + +void FeatureTracker::updateParams(const std::vector& params) { + ph->setRuntimeParams(params); +} + +} // namespace dai_nodes +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp b/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp index 7266a08b..22cb09bc 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp @@ -9,7 +9,9 @@ #include "depthai/pipeline/node/XLinkIn.hpp" #include "depthai/pipeline/node/XLinkOut.hpp" #include "depthai_bridge/ImageConverter.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" #include "depthai_ros_driver/param_handlers/sensor_param_handler.hpp" +#include "depthai_ros_driver/utils.hpp" #include "image_transport/camera_publisher.hpp" #include "image_transport/image_transport.hpp" #include "rclcpp/node.hpp" @@ -56,12 +58,18 @@ void Mono::setXinXout(std::shared_ptr pipeline) { void Mono::setupQueues(std::shared_ptr device) { if(ph->getParam("i_publish_topic")) { - monoQ = device->getOutputQueue(monoQName, ph->getParam("i_max_q_size"), false); - auto tfPrefix = getTFPrefix(getName()); + auto tfPrefix = getTFPrefix( + utils::getSocketName(static_cast(ph->getParam("i_board_socket_id")), device->getConnectedCameraFeatures())); imageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false, ph->getParam("i_get_base_device_timestamp")); imageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); - monoPub = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/image_raw"); + if(ph->getParam("i_low_bandwidth")) { + imageConverter->convertFromBitstream(dai::RawImgFrame::Type::GRAY8); + } + if(ph->getParam("i_add_exposure_offset")) { + auto offset = static_cast(ph->getParam("i_exposure_offset")); + imageConverter->addExposureOffset(offset); + } infoManager = std::make_shared( getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + getName()).get(), "/" + getName()); if(ph->getParam("i_calibration_file").empty()) { @@ -74,16 +82,29 @@ void Mono::setupQueues(std::shared_ptr device) { } else { infoManager->loadCameraInfo(ph->getParam("i_calibration_file")); } - if(ph->getParam("i_low_bandwidth")) { - monoQ->addCallback(std::bind(sensor_helpers::compressedImgCB, + monoQ = device->getOutputQueue(monoQName, ph->getParam("i_max_q_size"), false); + if(ipcEnabled()) { + RCLCPP_DEBUG(getROSNode()->get_logger(), "Enabling intra_process communication!"); + monoPub = getROSNode()->create_publisher("~/" + getName() + "/image_raw", 10); + infoPub = getROSNode()->create_publisher("~/" + getName() + "/camera_info", 10); + monoQ->addCallback(std::bind(sensor_helpers::splitPub, std::placeholders::_1, std::placeholders::_2, *imageConverter, monoPub, + infoPub, infoManager, - dai::RawImgFrame::Type::GRAY8)); + ph->getParam("i_enable_lazy_publisher"))); + } else { - monoQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *imageConverter, monoPub, infoManager)); + monoPubIT = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/image_raw"); + monoQ->addCallback(std::bind(sensor_helpers::cameraPub, + std::placeholders::_1, + std::placeholders::_2, + *imageConverter, + monoPubIT, + infoManager, + ph->getParam("i_enable_lazy_publisher"))); } } controlQ = device->getInputQueue(controlQName); diff --git a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp index 44fdcbd2..6794c732 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp @@ -11,6 +11,7 @@ #include "depthai_bridge/ImageConverter.hpp" #include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" #include "depthai_ros_driver/param_handlers/sensor_param_handler.hpp" +#include "depthai_ros_driver/utils.hpp" #include "image_transport/camera_publisher.hpp" #include "image_transport/image_transport.hpp" #include "rclcpp/node.hpp" @@ -21,7 +22,7 @@ RGB::RGB(const std::string& daiNodeName, rclcpp::Node* node, std::shared_ptr pipeline, dai::CameraBoardSocket socket = dai::CameraBoardSocket::CAM_A, - sensor_helpers::ImageSensor sensor = {"IMX378", {"12mp", "4k"}, true}, + sensor_helpers::ImageSensor sensor = {"IMX378", "4k", {"12mp", "4k"}, true}, bool publish = true) : BaseNode(daiNodeName, node, pipeline) { RCLCPP_DEBUG(node->get_logger(), "Creating node %s", daiNodeName.c_str()); @@ -68,12 +69,21 @@ void RGB::setXinXout(std::shared_ptr pipeline) { void RGB::setupQueues(std::shared_ptr device) { if(ph->getParam("i_publish_topic")) { - auto tfPrefix = getTFPrefix(getName()); + auto tfPrefix = getTFPrefix( + utils::getSocketName(static_cast(ph->getParam("i_board_socket_id")), device->getConnectedCameraFeatures())); 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, ph->getParam("i_get_base_device_timestamp")); imageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); + if(ph->getParam("i_low_bandwidth")) { + imageConverter->convertFromBitstream(dai::RawImgFrame::Type::BGR888i); + } + if(ph->getParam("i_add_exposure_offset")) { + auto offset = static_cast(ph->getParam("i_exposure_offset")); + imageConverter->addExposureOffset(offset); + } + if(ph->getParam("i_calibration_file").empty()) { infoManager->setCameraInfo(sensor_helpers::getCalibInfo(getROSNode()->get_logger(), *imageConverter, @@ -84,23 +94,33 @@ void RGB::setupQueues(std::shared_ptr device) { } else { infoManager->loadCameraInfo(ph->getParam("i_calibration_file")); } - rgbPub = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/image_raw"); colorQ = device->getOutputQueue(ispQName, ph->getParam("i_max_q_size"), false); - if(ph->getParam("i_low_bandwidth")) { - colorQ->addCallback(std::bind(sensor_helpers::compressedImgCB, + if(ipcEnabled()) { + rgbPub = getROSNode()->create_publisher("~/" + getName() + "/image_raw", 10); + rgbInfoPub = getROSNode()->create_publisher("~/" + getName() + "/camera_info", 10); + colorQ->addCallback(std::bind(sensor_helpers::splitPub, std::placeholders::_1, std::placeholders::_2, *imageConverter, rgbPub, + rgbInfoPub, infoManager, - dai::RawImgFrame::Type::BGR888i)); + ph->getParam("i_enable_lazy_publisher"))); + } else { - colorQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *imageConverter, rgbPub, infoManager)); + rgbPubIT = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/image_raw"); + colorQ->addCallback(std::bind(sensor_helpers::cameraPub, + std::placeholders::_1, + std::placeholders::_2, + *imageConverter, + rgbPubIT, + infoManager, + ph->getParam("i_enable_lazy_publisher"))); } } if(ph->getParam("i_enable_preview")) { previewQ = device->getOutputQueue(previewQName, ph->getParam("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()); @@ -114,9 +134,24 @@ void RGB::setupQueues(std::shared_ptr device) { ph->getParam("i_preview_size"), ph->getParam("i_preview_size"))); } else { - infoManager->loadCameraInfo(ph->getParam("i_calibration_file")); + previewInfoManager->loadCameraInfo(ph->getParam("i_calibration_file")); + } + if(ipcEnabled()) { + previewPubIT = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/preview/image_raw"); + previewQ->addCallback( + std::bind(sensor_helpers::basicCameraPub, std::placeholders::_1, std::placeholders::_2, *imageConverter, previewPubIT, previewInfoManager)); + } else { + previewPub = getROSNode()->create_publisher("~/" + getName() + "/preview/image_raw", 10); + previewInfoPub = getROSNode()->create_publisher("~/" + getName() + "/preview/camera_info", 10); + previewQ->addCallback(std::bind(sensor_helpers::splitPub, + std::placeholders::_1, + std::placeholders::_2, + *imageConverter, + previewPub, + previewInfoPub, + previewInfoManager, + ph->getParam("i_enable_lazy_publisher"))); } - previewQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *imageConverter, previewPub, previewInfoManager)); }; controlQ = device->getInputQueue(controlQName); } 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 35af62cd..9b7af0d8 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp @@ -9,121 +9,69 @@ namespace depthai_ros_driver { namespace dai_nodes { namespace sensor_helpers { -void ImageSensor::getSizeFromResolution(const dai::ColorCameraProperties::SensorResolution& res, int& width, int& height) { - switch(res) { - case dai::ColorCameraProperties::SensorResolution::THE_720_P: { - width = 1280; - height = 720; - break; - } - case dai::ColorCameraProperties::SensorResolution::THE_800_P: { - width = 1280; - height = 800; - break; - } - case dai::ColorCameraProperties::SensorResolution::THE_1080_P: { - width = 1920; - height = 1080; - break; - } - case dai::ColorCameraProperties::SensorResolution::THE_4_K: { - width = 3840; - height = 2160; - break; - } - case dai::ColorCameraProperties::SensorResolution::THE_12_MP: { - width = 4056; - height = 3040; - break; - } - case dai::ColorCameraProperties::SensorResolution::THE_1200_P: { - width = 1920; - height = 1200; - break; - } - case dai::ColorCameraProperties::SensorResolution::THE_5_MP: { - width = 2592; - height = 1944; - break; - } - case dai::ColorCameraProperties::SensorResolution::THE_13_MP: { - width = 4208; - height = 3120; - break; - } - case dai::ColorCameraProperties::SensorResolution::THE_4000X3000: { - width = 4000; - height = 3000; - break; - } - case dai::ColorCameraProperties::SensorResolution::THE_5312X6000: { - width = 5312; - height = 6000; - break; - } - case dai::ColorCameraProperties::SensorResolution::THE_48_MP: { - width = 8000; - height = 6000; - break; - } - case dai::ColorCameraProperties::SensorResolution::THE_1440X1080: { - width = 1440; - height = 1080; - break; - } - default: { - throw std::runtime_error("Resolution not supported!"); - } + +std::vector availableSensors{{"IMX378", "4k", {"12mp", "4k"}, true}, + {"OV9282", "800p", {"800p", "720p", "400p"}, false}, + {"OV9782", "800p", {"800p", "720p", "400p"}, true}, + {"OV9281", "800p", {"800p", "720p", "400p"}, true}, + {"IMX214", "1080p", {"13mp", "12mp", "4k", "1080p"}, true}, + {"IMX412", "1080p", {"13mp", "12mp", "4k", "1080p"}, true}, + {"OV7750", "480p", {"480p", "400p"}, false}, + {"OV7251", "480p", {"480p", "400p"}, false}, + {"IMX477", "1080p", {"12mp", "4k", "1080p"}, true}, + {"IMX577", "1080p", {"12mp", "4k", "1080p"}, true}, + {"AR0234", "1200p", {"1200p"}, true}, + {"IMX582", "4k", {"48mp", "12mp", "4k"}, true}, + {"LCM48", "4k", {"48mp", "12mp", "4k"}, true}}; + +void basicCameraPub(const std::string& /*name*/, + const std::shared_ptr& data, + dai::ros::ImageConverter& converter, + image_transport::CameraPublisher& pub, + std::shared_ptr infoManager) { + if(rclcpp::ok() && (pub.getNumSubscribers() > 0)) { + auto img = std::dynamic_pointer_cast(data); + auto info = infoManager->getCameraInfo(); + auto rawMsg = converter.toRosMsgRawPtr(img); + info.header = rawMsg.header; + pub.publish(rawMsg, info); } } -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}, - {"IMX412", {"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}, - {"LCM48", {"48mp", "12mp", "4k"}, true}, -}; -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 cameraPub(const std::string& /*name*/, + const std::shared_ptr& data, + dai::ros::ImageConverter& converter, + image_transport::CameraPublisher& pub, + std::shared_ptr infoManager, + bool lazyPub) { + if(rclcpp::ok() && (!lazyPub || pub.getNumSubscribers() > 0)) { + auto img = std::dynamic_pointer_cast(data); + auto info = infoManager->getCameraInfo(); + auto rawMsg = converter.toRosMsgRawPtr(img, info); + info.header = rawMsg.header; + pub.publish(rawMsg, info); } } -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(); + +void splitPub(const std::string& /*name*/, + const std::shared_ptr& data, + dai::ros::ImageConverter& converter, + rclcpp::Publisher::SharedPtr imgPub, + rclcpp::Publisher::SharedPtr infoPub, + std::shared_ptr infoManager, + bool lazyPub) { + if(rclcpp::ok() && (!lazyPub || detectSubscription(imgPub, infoPub))) { + auto img = std::dynamic_pointer_cast(data); + auto info = infoManager->getCameraInfo(); + auto rawMsg = converter.toRosMsgRawPtr(img, info); + info.header = rawMsg.header; + sensor_msgs::msg::CameraInfo::UniquePtr infoMsg = std::make_unique(info); + sensor_msgs::msg::Image::UniquePtr msg = std::make_unique(rawMsg); + imgPub->publish(std::move(msg)); + infoPub->publish(std::move(infoMsg)); } } + sensor_msgs::msg::CameraInfo getCalibInfo(const rclcpp::Logger& logger, dai::ros::ImageConverter& converter, std::shared_ptr device, @@ -146,6 +94,11 @@ std::shared_ptr createEncoder(std::shared_ptr::SharedPtr& pub, + const rclcpp::Publisher::SharedPtr& infoPub) { + return (pub->get_subscription_count() > 0 || pub->get_intra_process_subscription_count() > 0 || infoPub->get_subscription_count() > 0 + || infoPub->get_intra_process_subscription_count() > 0); +} } // 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/sensors/sensor_wrapper.cpp b/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp index 06566462..f91a8480 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp @@ -4,6 +4,7 @@ #include "depthai/pipeline/Pipeline.hpp" #include "depthai/pipeline/node/XLinkIn.hpp" #include "depthai_bridge/ImageConverter.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/feature_tracker.hpp" #include "depthai_ros_driver/dai_nodes/sensors/mono.hpp" #include "depthai_ros_driver/dai_nodes/sensors/rgb.hpp" #include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" @@ -58,7 +59,10 @@ SensorWrapper::SensorWrapper(const std::string& daiNodeName, sensorNode = std::make_unique(daiNodeName, node, pipeline, socket, (*sensorIt), publish); } } - + if(ph->getParam("i_enable_feature_tracker")) { + featureTrackerNode = std::make_unique(daiNodeName + std::string("_feature_tracker"), node, pipeline); + sensorNode->link(featureTrackerNode->getInput()); + } RCLCPP_DEBUG(node->get_logger(), "Base node %s created", daiNodeName.c_str()); } SensorWrapper::~SensorWrapper() = default; @@ -89,6 +93,9 @@ void SensorWrapper::setupQueues(std::shared_ptr device) { if(!ph->getParam("i_disable_node")) { sensorNode->setupQueues(device); } + if(ph->getParam("i_enable_feature_tracker")) { + featureTrackerNode->setupQueues(device); + } } void SensorWrapper::closeQueues() { if(ph->getParam("i_simulate_from_topic")) { @@ -97,6 +104,9 @@ void SensorWrapper::closeQueues() { if(!ph->getParam("i_disable_node")) { sensorNode->closeQueues(); } + if(ph->getParam("i_enable_feature_tracker")) { + featureTrackerNode->closeQueues(); + } } void SensorWrapper::link(dai::Node::Input in, int linkType) { diff --git a/depthai_ros_driver/src/dai_nodes/stereo.cpp b/depthai_ros_driver/src/dai_nodes/stereo.cpp index 49a681d7..c8e49fb7 100644 --- a/depthai_ros_driver/src/dai_nodes/stereo.cpp +++ b/depthai_ros_driver/src/dai_nodes/stereo.cpp @@ -9,6 +9,7 @@ #include "depthai/pipeline/node/VideoEncoder.hpp" #include "depthai/pipeline/node/XLinkOut.hpp" #include "depthai_bridge/ImageConverter.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/feature_tracker.hpp" #include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" #include "depthai_ros_driver/dai_nodes/sensors/sensor_wrapper.hpp" #include "depthai_ros_driver/param_handlers/stereo_param_handler.hpp" @@ -22,17 +23,27 @@ Stereo::Stereo(const std::string& daiNodeName, rclcpp::Node* node, std::shared_ptr pipeline, std::shared_ptr device, - StereoSensorInfo leftInfo, - StereoSensorInfo rightInfo) - : BaseNode(daiNodeName, node, pipeline), leftSensInfo(leftInfo), rightSensInfo(rightInfo) { + dai::CameraBoardSocket leftSocket, + dai::CameraBoardSocket rightSocket) + : BaseNode(daiNodeName, node, pipeline) { RCLCPP_DEBUG(node->get_logger(), "Creating node %s", daiNodeName.c_str()); setNames(); - stereoCamNode = pipeline->create(); - left = std::make_unique(leftInfo.name, node, pipeline, device, leftInfo.socket, false); - right = std::make_unique(rightInfo.name, node, pipeline, device, rightInfo.socket, false); - ph = std::make_unique(node, daiNodeName); - ph->declareParams(stereoCamNode, rightInfo.name); + ph->updateSocketsFromParams(leftSocket, rightSocket); + auto features = device->getConnectedCameraFeatures(); + for(auto f : features) { + if(f.socket == leftSocket) { + leftSensInfo = f; + } else if(f.socket == rightSocket) { + rightSensInfo = f; + } else { + continue; + } + } + stereoCamNode = pipeline->create(); + left = std::make_unique(leftSensInfo.name, node, pipeline, device, leftSensInfo.socket, false); + right = std::make_unique(rightSensInfo.name, node, pipeline, device, rightSensInfo.socket, false); + ph->declareParams(stereoCamNode, features); setXinXout(pipeline); left->link(stereoCamNode->left); right->link(stereoCamNode->right); @@ -61,7 +72,7 @@ void Stereo::setXinXout(std::shared_ptr pipeline) { } } } - if(ph->getParam("i_publish_left_rect")) { + if(ph->getParam("i_publish_left_rect") || ph->getParam("i_publish_synced_rect_pair")) { xoutLeftRect = pipeline->create(); xoutLeftRect->setStreamName(leftRectQName); if(ph->getParam("i_left_rect_low_bandwidth")) { @@ -73,7 +84,7 @@ void Stereo::setXinXout(std::shared_ptr pipeline) { } } - if(ph->getParam("i_publish_right_rect")) { + if(ph->getParam("i_publish_right_rect") || ph->getParam("i_publish_synced_rect_pair")) { xoutRightRect = pipeline->create(); xoutRightRect->setStreamName(rightRectQName); if(ph->getParam("i_right_rect_low_bandwidth")) { @@ -84,73 +95,110 @@ void Stereo::setXinXout(std::shared_ptr pipeline) { stereoCamNode->rectifiedRight.link(xoutRightRect->input); } } + + if(ph->getParam("i_left_rect_enable_feature_tracker")) { + featureTrackerLeftR = std::make_unique(leftSensInfo.name + std::string("_rect_feature_tracker"), getROSNode(), pipeline); + + stereoCamNode->rectifiedLeft.link(featureTrackerLeftR->getInput()); + } + + if(ph->getParam("i_right_rect_enable_feature_tracker")) { + featureTrackerRightR = std::make_unique(rightSensInfo.name + std::string("_rect_feature_tracker"), getROSNode(), pipeline); + stereoCamNode->rectifiedRight.link(featureTrackerRightR->getInput()); + } } -void Stereo::setupLeftRectQueue(std::shared_ptr device) { - leftRectQ = device->getOutputQueue(leftRectQName, ph->getOtherNodeParam(leftSensInfo.name, "i_max_q_size"), false); - auto tfPrefix = getTFPrefix(leftSensInfo.name); - leftRectConv = std::make_unique(tfPrefix + "_camera_optical_frame", false, ph->getParam("i_get_base_device_timestamp")); - leftRectConv->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); - leftRectIM = std::make_shared( - getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + leftSensInfo.name).get(), "/rect"); +void Stereo::setupRectQueue(std::shared_ptr device, + dai::CameraFeatures& sensorInfo, + const std::string& queueName, + std::unique_ptr& conv, + std::shared_ptr& im, + std::shared_ptr& q, + rclcpp::Publisher::SharedPtr pub, + rclcpp::Publisher::SharedPtr infoPub, + image_transport::CameraPublisher& pubIT, + bool isLeft) { + auto tfPrefix = getTFPrefix(sensorInfo.name); + conv = std::make_unique(tfPrefix + "_camera_optical_frame", false, ph->getParam("i_get_base_device_timestamp")); + conv->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); + bool lowBandwidth = ph->getParam(isLeft ? "i_left_rect_low_bandwidth" : "i_right_rect_low_bandwidth"); + if(lowBandwidth) { + conv->convertFromBitstream(dai::RawImgFrame::Type::GRAY8); + } + bool addOffset = ph->getParam(isLeft ? "i_left_rect_add_exposure_offset" : "i_right_rect_add_exposure_offset"); + if(addOffset) { + auto offset = static_cast(ph->getParam(isLeft ? "i_left_rect_exposure_offset" : "i_right_rect_exposure_offset")); + conv->addExposureOffset(offset); + } + im = std::make_shared( + getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + sensorInfo.name).get(), "/rect"); + auto info = sensor_helpers::getCalibInfo(getROSNode()->get_logger(), - *leftRectConv, + *conv, device, - leftSensInfo.socket, - ph->getOtherNodeParam(leftSensInfo.name, "i_width"), - ph->getOtherNodeParam(leftSensInfo.name, "i_height")); - leftRectIM->setCameraInfo(info); - leftRectPub = image_transport::create_camera_publisher(getROSNode(), "~/" + leftSensInfo.name + "/image_rect"); - dai::RawImgFrame::Type encType = dai::RawImgFrame::Type::GRAY8; - if(left->getSensorData().color) { - encType = dai::RawImgFrame::Type::BGR888i; - } - if(ph->getParam("i_left_rect_low_bandwidth")) { - leftRectQ->addCallback( - std::bind(sensor_helpers::compressedImgCB, std::placeholders::_1, std::placeholders::_2, *leftRectConv, leftRectPub, leftRectIM, encType)); + sensorInfo.socket, + ph->getOtherNodeParam(sensorInfo.name, "i_width"), + ph->getOtherNodeParam(sensorInfo.name, "i_height")); + for(auto& d : info.d) { + d = 0.0; + } + + im->setCameraInfo(info); + + q = device->getOutputQueue(queueName, ph->getOtherNodeParam(sensorInfo.name, "i_max_q_size"), false); + + // if publish synced pair is set to true then we skip individual publishing of left and right rectified frames + bool addCallback = !ph->getParam("i_publish_synced_rect_pair"); + + if(ipcEnabled()) { + pub = getROSNode()->create_publisher("~/" + sensorInfo.name + "/image_rect", 10); + infoPub = getROSNode()->create_publisher("~/" + getName() + "/camera_info", 10); + if(addCallback) { + q->addCallback(std::bind(sensor_helpers::splitPub, + std::placeholders::_1, + std::placeholders::_2, + *conv, + pub, + infoPub, + im, + ph->getParam("i_enable_lazy_publisher"))); + } } else { - leftRectQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *leftRectConv, leftRectPub, leftRectIM)); + pubIT = image_transport::create_camera_publisher(getROSNode(), "~/" + sensorInfo.name + "/image_rect"); + if(addCallback) { + q->addCallback(std::bind( + sensor_helpers::cameraPub, std::placeholders::_1, std::placeholders::_2, *conv, pubIT, im, ph->getParam("i_enable_lazy_publisher"))); + } } } +void Stereo::setupLeftRectQueue(std::shared_ptr device) { + setupRectQueue(device, leftSensInfo, leftRectQName, leftRectConv, leftRectIM, leftRectQ, leftRectPub, leftRectInfoPub, leftRectPubIT, true); +} + void Stereo::setupRightRectQueue(std::shared_ptr device) { - rightRectQ = device->getOutputQueue(rightRectQName, ph->getOtherNodeParam(rightSensInfo.name, "i_max_q_size"), false); - auto tfPrefix = getTFPrefix(rightSensInfo.name); - rightRectConv = std::make_unique(tfPrefix + "_camera_optical_frame", false, ph->getParam("i_get_base_device_timestamp")); - rightRectConv->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); - rightRectIM = std::make_shared( - getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + rightSensInfo.name).get(), "/rect"); - auto info = sensor_helpers::getCalibInfo(getROSNode()->get_logger(), - *rightRectConv, - device, - rightSensInfo.socket, - ph->getOtherNodeParam(rightSensInfo.name, "i_width"), - ph->getOtherNodeParam(rightSensInfo.name, "i_height")); - rightRectIM->setCameraInfo(info); - rightRectPub = image_transport::create_camera_publisher(getROSNode(), "~/" + rightSensInfo.name + "/image_rect"); - dai::RawImgFrame::Type encType = dai::RawImgFrame::Type::GRAY8; - if(right->getSensorData().color) { - encType = dai::RawImgFrame::Type::BGR888i; - } - if(ph->getParam("i_right_rect_low_bandwidth")) { - rightRectQ->addCallback( - std::bind(sensor_helpers::compressedImgCB, std::placeholders::_1, std::placeholders::_2, *rightRectConv, rightRectPub, rightRectIM, encType)); - } else { - rightRectQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *rightRectConv, rightRectPub, rightRectIM)); - } + setupRectQueue(device, rightSensInfo, rightRectQName, rightRectConv, rightRectIM, rightRectQ, rightRectPub, rightRectInfoPub, rightRectPubIT, false); } void Stereo::setupStereoQueue(std::shared_ptr device) { - stereoQ = device->getOutputQueue(stereoQName, ph->getParam("i_max_q_size"), false); std::string tfPrefix; if(ph->getParam("i_align_depth")) { - tfPrefix = getTFPrefix("rgb"); + tfPrefix = getTFPrefix(ph->getParam("i_socket_name")); } else { tfPrefix = getTFPrefix(rightSensInfo.name); } stereoConv = std::make_unique(tfPrefix + "_camera_optical_frame", false, ph->getParam("i_get_base_device_timestamp")); stereoConv->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); - stereoPub = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/image_raw"); + if(ph->getParam("i_low_bandwidth")) { + stereoConv->convertFromBitstream(dai::RawImgFrame::Type::RAW8); + } + if(!ph->getParam("i_output_disparity")) { + stereoConv->convertDispToDepth(); + } + if(ph->getParam("i_add_exposure_offset")) { + auto offset = static_cast(ph->getParam("i_exposure_offset")); + stereoConv->addExposureOffset(offset); + } stereoIM = std::make_shared( getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + getName()).get(), "/" + getName()); auto info = sensor_helpers::getCalibInfo(getROSNode()->get_logger(), @@ -160,31 +208,77 @@ void Stereo::setupStereoQueue(std::shared_ptr device) { ph->getParam("i_width"), ph->getParam("i_height")); auto calibHandler = device->readCalibration(); - info.p[3] = calibHandler.getBaselineDistance() * 10.0; // baseline in mm + + // remove distortion since image is rectified for(auto& d : info.d) { d = 0.0; } + for(auto& r : info.r) { + r = 0.0; + } + info.r[0] = info.r[4] = info.r[8] = 1.0; + info.p[3] = calibHandler.getBaselineDistance(leftSensInfo.socket, rightSensInfo.socket); stereoIM->setCameraInfo(info); + stereoQ = device->getOutputQueue(stereoQName, ph->getParam("i_max_q_size"), false); + if(ipcEnabled()) { + stereoPub = getROSNode()->create_publisher("~/" + getName() + "/image_raw", 10); + stereoInfoPub = getROSNode()->create_publisher("~/" + getName() + "/camera_info", 10); + stereoQ->addCallback(std::bind(sensor_helpers::splitPub, + std::placeholders::_1, + std::placeholders::_2, + *stereoConv, + stereoPub, + stereoInfoPub, + stereoIM, + ph->getParam("i_enable_lazy_publisher"))); + } else { + stereoPubIT = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/image_raw"); + stereoQ->addCallback(std::bind(sensor_helpers::cameraPub, + std::placeholders::_1, + std::placeholders::_2, + *stereoConv, + stereoPubIT, + stereoIM, + ph->getParam("i_enable_lazy_publisher"))); + } +} - if(ph->getParam("i_low_bandwidth")) { - if(ph->getParam("i_output_disparity")) { - stereoQ->addCallback(std::bind(sensor_helpers::compressedImgCB, - std::placeholders::_1, - std::placeholders::_2, - *stereoConv, - stereoPub, - stereoIM, - dai::RawImgFrame::Type::GRAY8)); - } else { - // converting disp->depth - stereoQ->addCallback(std::bind( - sensor_helpers::compressedImgCB, std::placeholders::_1, std::placeholders::_2, *stereoConv, stereoPub, stereoIM, dai::RawImgFrame::Type::RAW8)); - } +void Stereo::syncTimerCB() { + auto left = leftRectQ->get(); + auto right = rightRectQ->get(); + if(left->getSequenceNum() != right->getSequenceNum()) { + RCLCPP_WARN(getROSNode()->get_logger(), "Left and right rectified frames are not synchronized!"); } else { - if(ph->getParam("i_output_disparity")) { - stereoQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *stereoConv, stereoPub, stereoIM)); + bool lazyPub = ph->getParam("i_enable_lazy_publisher"); + if(ipcEnabled() && rclcpp::ok() + && (!lazyPub || sensor_helpers::detectSubscription(leftRectPub, leftRectInfoPub) + || sensor_helpers::detectSubscription(rightRectPub, rightRectInfoPub))) { + auto leftInfo = leftRectIM->getCameraInfo(); + auto leftRawMsg = leftRectConv->toRosMsgRawPtr(left); + leftInfo.header = leftRawMsg.header; + auto rightInfo = rightRectIM->getCameraInfo(); + auto rightRawMsg = rightRectConv->toRosMsgRawPtr(right); + rightRawMsg.header.stamp = leftRawMsg.header.stamp; + rightInfo.header = rightRawMsg.header; + sensor_msgs::msg::CameraInfo::UniquePtr leftInfoMsg = std::make_unique(leftInfo); + sensor_msgs::msg::Image::UniquePtr leftMsg = std::make_unique(leftRawMsg); + sensor_msgs::msg::CameraInfo::UniquePtr rightInfoMsg = std::make_unique(rightInfo); + sensor_msgs::msg::Image::UniquePtr rightMsg = std::make_unique(rightRawMsg); + leftRectPub->publish(std::move(leftMsg)); + leftRectInfoPub->publish(std::move(leftInfoMsg)); + rightRectPub->publish(std::move(rightMsg)); + rightRectInfoPub->publish(std::move(rightInfoMsg)); + } else if(!ipcEnabled() && rclcpp::ok() && (!lazyPub || leftRectPubIT.getNumSubscribers() > 0 || rightRectPubIT.getNumSubscribers() > 0)) { + auto leftInfo = leftRectIM->getCameraInfo(); + auto leftRawMsg = leftRectConv->toRosMsgRawPtr(left); + leftInfo.header = leftRawMsg.header; + auto rightInfo = rightRectIM->getCameraInfo(); + auto rightRawMsg = rightRectConv->toRosMsgRawPtr(right); + rightRawMsg.header.stamp = leftRawMsg.header.stamp; + rightInfo.header = rightRawMsg.header; + leftRectPubIT.publish(leftRawMsg, leftInfo); + rightRectPubIT.publish(rightRawMsg, rightInfo); } - stereoQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *stereoConv, stereoPub, stereoIM)); } } @@ -194,12 +288,23 @@ void Stereo::setupQueues(std::shared_ptr device) { if(ph->getParam("i_publish_topic")) { setupStereoQueue(device); } - if(ph->getParam("i_publish_left_rect")) { + if(ph->getParam("i_publish_left_rect") || ph->getParam("i_publish_synced_rect_pair")) { setupLeftRectQueue(device); } - if(ph->getParam("i_publish_right_rect")) { + if(ph->getParam("i_publish_right_rect") || ph->getParam("i_publish_synced_rect_pair")) { setupRightRectQueue(device); } + if(ph->getParam("i_publish_synced_rect_pair")) { + int timerPeriod = 1000.0 / ph->getOtherNodeParam(leftSensInfo.name, "i_fps"); + RCLCPP_INFO(getROSNode()->get_logger(), "Setting up stereo pair sync timer with period %d ms based on left sensor FPS.", timerPeriod); + syncTimer = getROSNode()->create_wall_timer(std::chrono::milliseconds(timerPeriod), std::bind(&Stereo::syncTimerCB, this)); + } + if(ph->getParam("i_left_rect_enable_feature_tracker")) { + featureTrackerLeftR->setupQueues(device); + } + if(ph->getParam("i_right_rect_enable_feature_tracker")) { + featureTrackerRightR->setupQueues(device); + } } void Stereo::closeQueues() { left->closeQueues(); @@ -207,12 +312,20 @@ void Stereo::closeQueues() { if(ph->getParam("i_publish_topic")) { stereoQ->close(); } - if(ph->getParam("i_publish_left_rect")) { + if(ph->getParam("i_publish_left_rect") || ph->getParam("i_publish_synced_rect_pair")) { + syncTimer->reset(); leftRectQ->close(); } - if(ph->getParam("i_publish_right_rect")) { + if(ph->getParam("i_publish_right_rect") || ph->getParam("i_publish_synced_rect_pair")) { + syncTimer->reset(); rightRectQ->close(); } + if(ph->getParam("i_left_rect_enable_feature_tracker")) { + featureTrackerLeftR->closeQueues(); + } + if(ph->getParam("i_right_rect_enable_feature_tracker")) { + featureTrackerRightR->closeQueues(); + } } void Stereo::link(dai::Node::Input in, int /*linkType*/) { diff --git a/depthai_ros_driver/src/dai_nodes/sys_logger.cpp b/depthai_ros_driver/src/dai_nodes/sys_logger.cpp new file mode 100644 index 00000000..c7231614 --- /dev/null +++ b/depthai_ros_driver/src/dai_nodes/sys_logger.cpp @@ -0,0 +1,81 @@ +#include "depthai_ros_driver/dai_nodes/sys_logger.hpp" + +#include "depthai/device/DataQueue.hpp" +#include "depthai/device/Device.hpp" +#include "depthai/pipeline/Pipeline.hpp" +#include "depthai/pipeline/datatype/SystemInformation.hpp" +#include "depthai/pipeline/node/SystemLogger.hpp" +#include "depthai/pipeline/node/XLinkOut.hpp" +#include "rclcpp/node.hpp" + +namespace depthai_ros_driver { +namespace dai_nodes { +SysLogger::SysLogger(const std::string& daiNodeName, rclcpp::Node* node, std::shared_ptr pipeline) : BaseNode(daiNodeName, node, pipeline) { + RCLCPP_DEBUG(node->get_logger(), "Creating node %s", daiNodeName.c_str()); + setNames(); + sysNode = pipeline->create(); + setXinXout(pipeline); + RCLCPP_DEBUG(node->get_logger(), "Node %s created", daiNodeName.c_str()); +} +SysLogger::~SysLogger() = default; + +void SysLogger::setNames() { + loggerQName = getName() + "_queue"; +} + +void SysLogger::setXinXout(std::shared_ptr pipeline) { + xoutLogger = pipeline->create(); + xoutLogger->setStreamName(loggerQName); + sysNode->out.link(xoutLogger->input); +} + +void SysLogger::setupQueues(std::shared_ptr device) { + loggerQ = device->getOutputQueue(loggerQName, 8, false); + updater = std::make_shared(getROSNode()); + updater->setHardwareID(getROSNode()->get_name() + std::string("_") + device->getMxId() + std::string("_") + device->getDeviceName()); + updater->add("sys_logger", std::bind(&SysLogger::produceDiagnostics, this, std::placeholders::_1)); +} + +void SysLogger::closeQueues() { + loggerQ->close(); +} + +std::string SysLogger::sysInfoToString(const dai::SystemInformation& sysInfo) { + std::stringstream ss; + ss << "System Information: " << std::endl; + ss << " Leon CSS CPU Usage: " << sysInfo.leonCssCpuUsage.average * 100 << std::endl; + ss << " Leon MSS CPU Usage: " << sysInfo.leonMssCpuUsage.average * 100 << std::endl; + ss << " Ddr Memory Usage: " << sysInfo.ddrMemoryUsage.used / (1024.0f * 1024.0f) << std::endl; + ss << " Ddr Memory Total: " << sysInfo.ddrMemoryUsage.total / (1024.0f * 1024.0f) << std::endl; + ss << " Cmx Memory Usage: " << sysInfo.cmxMemoryUsage.used / (1024.0f * 1024.0f) << std::endl; + ss << " Cmx Memory Total: " << sysInfo.cmxMemoryUsage.total << std::endl; + ss << " Leon CSS Memory Usage: " << sysInfo.leonCssMemoryUsage.used / (1024.0f * 1024.0f) << std::endl; + ss << " Leon CSS Memory Total: " << sysInfo.leonCssMemoryUsage.total / (1024.0f * 1024.0f) << std::endl; + ss << " Leon MSS Memory Usage: " << sysInfo.leonMssMemoryUsage.used / (1024.0f * 1024.0f) << std::endl; + ss << " Leon MSS Memory Total: " << sysInfo.leonMssMemoryUsage.total / (1024.0f * 1024.0f) << std::endl; + ss << " Average Chip Temperature: " << sysInfo.chipTemperature.average << std::endl; + ss << " Leon CSS Chip Temperature: " << sysInfo.chipTemperature.css << std::endl; + ss << " Leon MSS Chip Temperature: " << sysInfo.chipTemperature.mss << std::endl; + ss << " UPA Chip Temperature: " << sysInfo.chipTemperature.upa << std::endl; + ss << " DSS Chip Temperature: " << sysInfo.chipTemperature.dss << std::endl; + + return ss.str(); +} + +void SysLogger::produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat) { + try { + auto logData = loggerQ->tryGet(); + if(logData) { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "System Information"); + stat.add("System Information", sysInfoToString(*logData)); + } else { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "No Data"); + } + } catch(const std::exception& e) { + RCLCPP_ERROR(getROSNode()->get_logger(), "No data on logger queue!"); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, e.what()); + } +} + +} // namespace dai_nodes +} // namespace depthai_ros_driver 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 1d985e17..459406f2 100644 --- a/depthai_ros_driver/src/param_handlers/camera_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/camera_param_handler.cpp @@ -35,6 +35,21 @@ void CameraParamHandler::declareParams() { declareAndLogParam("i_external_calibration_path", ""); declareAndLogParam("i_laser_dot_brightness", 800, getRangedIntDescriptor(0, 1200)); declareAndLogParam("i_floodlight_brightness", 0, getRangedIntDescriptor(0, 1500)); + + declareAndLogParam("i_publish_tf_from_calibration", false); + declareAndLogParam("i_tf_camera_name", getROSNode()->get_name()); + declareAndLogParam("i_tf_camera_model", ""); + declareAndLogParam("i_tf_base_frame", "oak"); + declareAndLogParam("i_tf_parent_frame", "oak-d-base-frame"); + declareAndLogParam("i_tf_cam_pos_x", "0.0"); + declareAndLogParam("i_tf_cam_pos_y", "0.0"); + declareAndLogParam("i_tf_cam_pos_z", "0.0"); + declareAndLogParam("i_tf_cam_roll", "0.0"); + declareAndLogParam("i_tf_cam_pitch", "0.0"); + declareAndLogParam("i_tf_cam_yaw", "0.0"); + declareAndLogParam("i_tf_imu_from_descr", "false"); + declareAndLogParam("i_tf_custom_urdf_location", ""); + declareAndLogParam("i_tf_custom_xacro_args", ""); } dai::CameraControl CameraParamHandler::setRuntimeParams(const std::vector& /*params*/) { dai::CameraControl ctrl; diff --git a/depthai_ros_driver/src/param_handlers/feature_tracker_param_handler.cpp b/depthai_ros_driver/src/param_handlers/feature_tracker_param_handler.cpp new file mode 100644 index 00000000..8a93fc4f --- /dev/null +++ b/depthai_ros_driver/src/param_handlers/feature_tracker_param_handler.cpp @@ -0,0 +1,30 @@ +#include "depthai_ros_driver/param_handlers/feature_tracker_param_handler.hpp" + +#include "depthai/pipeline/node/FeatureTracker.hpp" +#include "depthai_ros_driver/utils.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/node.hpp" + +namespace depthai_ros_driver { +namespace param_handlers { +FeatureTrackerParamHandler::FeatureTrackerParamHandler(rclcpp::Node* node, const std::string& name) : BaseParamHandler(node, name) {} +FeatureTrackerParamHandler::~FeatureTrackerParamHandler() = default; +void FeatureTrackerParamHandler::declareParams(std::shared_ptr featureTracker) { + declareAndLogParam("i_get_base_device_timestamp", false); + + featureTracker->setHardwareResources(declareAndLogParam("i_num_shaves", 2), declareAndLogParam("i_num_memory_slices", 2)); + motionEstMap = {{"LUCAS_KANADE_OPTICAL_FLOW", dai::FeatureTrackerConfig::MotionEstimator::Type::LUCAS_KANADE_OPTICAL_FLOW}, + {"HW_MOTION_ESTIMATION", dai::FeatureTrackerConfig::MotionEstimator::Type::HW_MOTION_ESTIMATION} + + }; + auto config = featureTracker->initialConfig.get(); + config.motionEstimator.type = (motionEstMap.at(declareAndLogParam("i_motion_estimator", "LUCAS_KANADE_OPTICAL_FLOW"))); + featureTracker->initialConfig.set(config); +} + +dai::CameraControl FeatureTrackerParamHandler::setRuntimeParams(const std::vector& /*params*/) { + dai::CameraControl ctrl; + return ctrl; +} +} // namespace param_handlers +} // namespace depthai_ros_driver \ No newline at end of file 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 7dcdb3b1..d8593deb 100644 --- a/depthai_ros_driver/src/param_handlers/imu_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/imu_param_handler.cpp @@ -38,7 +38,7 @@ void ImuParamHandler::declareParams(std::shared_ptr imu, const s } imu->enableIMUSensor(dai::IMUSensor::ACCELEROMETER_RAW, declareAndLogParam("i_acc_freq", 400)); imu->enableIMUSensor(dai::IMUSensor::GYROSCOPE_RAW, declareAndLogParam("i_gyro_freq", 400)); - imu->setBatchReportThreshold(declareAndLogParam("i_batch_report_threshold", 1)); + imu->setBatchReportThreshold(declareAndLogParam("i_batch_report_threshold", 5)); imu->setMaxBatchReports(declareAndLogParam("i_max_batch_reports", 10)); } 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 9283f66c..4645091d 100644 --- a/depthai_ros_driver/src/param_handlers/nn_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/nn_param_handler.cpp @@ -112,10 +112,12 @@ void NNParamHandler::setNNParams(nlohmann::json data, std::shared_ptr imageManip) { auto blob = dai::OpenVINO::Blob(model_path); auto firstInfo = blob.networkInputs.begin(); - auto inputSize = firstInfo->second.dims[0]; - if(inputSize > 590) { + auto inputWidth = firstInfo->second.dims[0]; + auto inputHeight = firstInfo->second.dims[1]; + if(inputWidth > 590 || inputHeight > 590) { std::ostringstream stream; - stream << "Current network input size is too large to resize. Please set following parameters: rgb.i_preview_size: " << inputSize; + stream << "Current network input size is too large to resize. Please set following parameters: rgb.i_preview_width: " << inputWidth; + stream << ", rgb.i_preview_height: " << inputHeight; stream << " and nn.i_disable_resize to true"; throw std::runtime_error(stream.str()); } @@ -123,8 +125,8 @@ void NNParamHandler::setImageManip(const std::string& model_path, std::shared_pt imageManip->inputImage.setBlocking(false); imageManip->inputImage.setQueueSize(8); imageManip->setKeepAspectRatio(false); - RCLCPP_INFO(getROSNode()->get_logger(), "NN input size: %d x %d. Resizing input image in case of different dimensions.", inputSize, inputSize); - imageManip->initialConfig.setResize(inputSize, inputSize); + RCLCPP_INFO(getROSNode()->get_logger(), "NN input size: %d x %d. Resizing input image in case of different dimensions.", inputWidth, inputHeight); + imageManip->initialConfig.setResize(inputWidth, inputHeight); } std::string NNParamHandler::getModelPath(const nlohmann::json& data) { std::string modelPath; diff --git a/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp b/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp index 18a75206..088b4e08 100644 --- a/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp @@ -27,11 +27,22 @@ void SensorParamHandler::declareCommonParams() { declareAndLogParam("i_get_base_device_timestamp", false); declareAndLogParam("i_board_socket_id", 0); declareAndLogParam("i_update_ros_base_time_on_ros_msg", false); + declareAndLogParam("i_enable_feature_tracker", false); + declareAndLogParam("i_enable_lazy_publisher", true); + declareAndLogParam("i_add_exposure_offset", false); + declareAndLogParam("i_exposure_offset", 0); fSyncModeMap = { {"OFF", dai::CameraControl::FrameSyncMode::OFF}, {"OUTPUT", dai::CameraControl::FrameSyncMode::OUTPUT}, {"INPUT", dai::CameraControl::FrameSyncMode::INPUT}, }; + cameraImageOrientationMap = { + {"NORMAL", dai::CameraImageOrientation::NORMAL}, + {"ROTATE_180_DEG", dai::CameraImageOrientation::ROTATE_180_DEG}, + {"AUTO", dai::CameraImageOrientation::AUTO}, + {"HORIZONTAL_MIRROR", dai::CameraImageOrientation::HORIZONTAL_MIRROR}, + {"VERTICAL_FLIP", dai::CameraImageOrientation::VERTICAL_FLIP}, + }; } void SensorParamHandler::declareParams(std::shared_ptr monoCam, @@ -64,22 +75,26 @@ void SensorParamHandler::declareParams(std::shared_ptr mo if(declareAndLogParam("i_fsync_trigger", false)) { monoCam->initialControl.setExternalTrigger(declareAndLogParam("i_num_frames_burst", 1), declareAndLogParam("i_num_frames_discard", 0)); } + if(declareAndLogParam("i_set_isp3a_fps", false)) { + monoCam->setIsp3aFps(declareAndLogParam("i_isp3a_fps", 10)); + } + monoCam->setImageOrientation(utils::getValFromMap(declareAndLogParam("i_sensor_img_orientation", "NORMAL"), cameraImageOrientationMap)); } void SensorParamHandler::declareParams(std::shared_ptr colorCam, dai::CameraBoardSocket socket, dai_nodes::sensor_helpers::ImageSensor sensor, bool publish) { - rgbResolutionMap = {{"720", dai::ColorCameraProperties::SensorResolution::THE_720_P}, - {"1080", dai::ColorCameraProperties::SensorResolution::THE_1080_P}, - {"4K", dai::ColorCameraProperties::SensorResolution::THE_4_K}, - {"12MP", dai::ColorCameraProperties::SensorResolution::THE_12_MP}, - {"13MP", dai::ColorCameraProperties::SensorResolution::THE_13_MP}, - {"800", dai::ColorCameraProperties::SensorResolution::THE_800_P}, - {"1200", dai::ColorCameraProperties::SensorResolution::THE_1200_P}, + rgbResolutionMap = {{"720p", dai::ColorCameraProperties::SensorResolution::THE_720_P}, + {"1080p", dai::ColorCameraProperties::SensorResolution::THE_1080_P}, + {"4k", dai::ColorCameraProperties::SensorResolution::THE_4_K}, + {"12mp", dai::ColorCameraProperties::SensorResolution::THE_12_MP}, + {"13mp", dai::ColorCameraProperties::SensorResolution::THE_13_MP}, + {"800p", dai::ColorCameraProperties::SensorResolution::THE_800_P}, + {"1200p", dai::ColorCameraProperties::SensorResolution::THE_1200_P}, {"5MP", dai::ColorCameraProperties::SensorResolution::THE_5_MP}, {"4000x3000", dai::ColorCameraProperties::SensorResolution::THE_4000X3000}, {"5312X6000", dai::ColorCameraProperties::SensorResolution::THE_5312X6000}, - {"48_MP", dai::ColorCameraProperties::SensorResolution::THE_48_MP}, + {"48mp", dai::ColorCameraProperties::SensorResolution::THE_48_MP}, {"1440X1080", dai::ColorCameraProperties::SensorResolution::THE_1440X1080}}; declareAndLogParam("i_publish_topic", publish); declareAndLogParam("i_board_socket_id", static_cast(socket)); @@ -87,12 +102,25 @@ void SensorParamHandler::declareParams(std::shared_ptr c declareAndLogParam("i_enable_preview", false); colorCam->setBoardSocket(socket); colorCam->setFps(declareAndLogParam("i_fps", 30.0)); - size_t preview_size = declareAndLogParam("i_preview_size", 300); - colorCam->setPreviewSize(preview_size, preview_size); - auto resolution = utils::getValFromMap(declareAndLogParam("i_resolution", "1080"), rgbResolutionMap); - int width, height; - colorCam->setResolution(resolution); - sensor.getSizeFromResolution(colorCam->getResolution(), width, height); + int preview_size = declareAndLogParam("i_preview_size", 300); + int preview_width = declareAndLogParam("i_preview_width", preview_size); + int preview_height = declareAndLogParam("i_preview_height", preview_size); + colorCam->setPreviewSize(preview_width, preview_height); + auto resString = declareAndLogParam("i_resolution", sensor.defaultResolution); + + // if resolution not in allowed resolutions, use default + if(std::find(sensor.allowedResolutions.begin(), sensor.allowedResolutions.end(), resString) == sensor.allowedResolutions.end()) { + RCLCPP_WARN(getROSNode()->get_logger(), + "Resolution %s not supported by sensor %s. Using default resolution %s", + resString.c_str(), + sensor.name.c_str(), + sensor.defaultResolution.c_str()); + resString = sensor.defaultResolution; + } + + int width = colorCam->getResolutionWidth(); + int height = colorCam->getResolutionHeight(); + colorCam->setResolution(utils::getValFromMap(resString, rgbResolutionMap)); colorCam->setInterleaved(declareAndLogParam("i_interleaved", false)); if(declareAndLogParam("i_set_isp_scale", true)) { @@ -131,6 +159,10 @@ void SensorParamHandler::declareParams(std::shared_ptr c if(declareAndLogParam("i_fsync_trigger", false)) { colorCam->initialControl.setExternalTrigger(declareAndLogParam("i_num_frames_burst", 1), declareAndLogParam("i_num_frames_discard", 0)); } + if(declareAndLogParam("i_set_isp3a_fps", false)) { + colorCam->setIsp3aFps(declareAndLogParam("i_isp3a_fps", 10)); + } + colorCam->setImageOrientation(utils::getValFromMap(declareAndLogParam("i_sensor_img_orientation", "NORMAL"), cameraImageOrientationMap)); } dai::CameraControl SensorParamHandler::setRuntimeParams(const std::vector& params) { dai::CameraControl ctrl; 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 e2018de5..cf2fc505 100644 --- a/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp @@ -1,5 +1,6 @@ #include "depthai_ros_driver/param_handlers/stereo_param_handler.hpp" +#include "depthai-shared/common/CameraFeatures.hpp" #include "depthai/pipeline/datatype/StereoDepthConfig.hpp" #include "depthai/pipeline/node/StereoDepth.hpp" #include "depthai_ros_driver/utils.hpp" @@ -37,7 +38,20 @@ StereoParamHandler::StereoParamHandler(rclcpp::Node* node, const std::string& na } StereoParamHandler::~StereoParamHandler() = default; -void StereoParamHandler::declareParams(std::shared_ptr stereo, const std::string& rightName) { + +void StereoParamHandler::updateSocketsFromParams(dai::CameraBoardSocket& left, dai::CameraBoardSocket& right) { + int newLeftS = declareAndLogParam("i_left_socket_id", static_cast(left)); + int newRightS = declareAndLogParam("i_right_socket_id", static_cast(right)); + if(newLeftS != static_cast(left) || newRightS != static_cast(right)) { + RCLCPP_WARN(getROSNode()->get_logger(), "Left or right socket changed, updating stereo node"); + RCLCPP_WARN(getROSNode()->get_logger(), "Old left socket: %d, new left socket: %d", static_cast(left), newLeftS); + RCLCPP_WARN(getROSNode()->get_logger(), "Old right socket: %d, new right socket: %d", static_cast(right), newRightS); + } + left = static_cast(newLeftS); + right = static_cast(newRightS); +} + +void StereoParamHandler::declareParams(std::shared_ptr stereo, const std::vector& camFeatures) { declareAndLogParam("i_max_q_size", 30); declareAndLogParam("i_low_bandwidth", false); declareAndLogParam("i_low_bandwidth_quality", 50); @@ -45,56 +59,65 @@ void StereoParamHandler::declareParams(std::shared_ptr s declareAndLogParam("i_get_base_device_timestamp", false); declareAndLogParam("i_update_ros_base_time_on_ros_msg", false); declareAndLogParam("i_publish_topic", true); + declareAndLogParam("i_add_exposure_offset", false); + declareAndLogParam("i_exposure_offset", 0); + declareAndLogParam("i_enable_lazy_publisher", true); + declareAndLogParam("i_publish_synced_rect_pair", false); declareAndLogParam("i_publish_left_rect", false); declareAndLogParam("i_left_rect_low_bandwidth", false); declareAndLogParam("i_left_rect_low_bandwidth_quality", 50); + declareAndLogParam("i_left_rect_add_exposure_offset", false); + declareAndLogParam("i_left_rect_exposure_offset", 0); + declareAndLogParam("i_left_rect_enable_feature_tracker", false); declareAndLogParam("i_publish_right_rect", false); declareAndLogParam("i_right_rect_low_bandwidth", false); declareAndLogParam("i_right_rect_low_bandwidth_quality", 50); + declareAndLogParam("i_right_rect_enable_feature_tracker", false); + declareAndLogParam("i_right_rect_add_exposure_offset", false); + declareAndLogParam("i_right_rect_exposure_offset", 0); stereo->setLeftRightCheck(declareAndLogParam("i_lr_check", true)); int width = 1280; int height = 720; - dai::CameraBoardSocket socket = dai::CameraBoardSocket::CAM_C; + auto socket = static_cast(declareAndLogParam("i_board_socket_id", static_cast(dai::CameraBoardSocket::CAM_A))); + std::string socketName; if(declareAndLogParam("i_align_depth", true)) { + socketName = utils::getSocketName(socket, camFeatures); try { - width = getROSNode()->get_parameter("rgb.i_width").as_int(); - height = getROSNode()->get_parameter("rgb.i_height").as_int(); - socket = static_cast(getROSNode()->get_parameter("rgb.i_board_socket_id").as_int()); + width = getROSNode()->get_parameter(socketName + ".i_width").as_int(); + height = getROSNode()->get_parameter(socketName + ".i_height").as_int(); } catch(rclcpp::exceptions::ParameterNotDeclaredException& e) { - RCLCPP_ERROR(getROSNode()->get_logger(), "RGB parameters not set, defaulting to 1280x720 unless specified otherwise."); - } - - } else { - try { - width = getROSNode()->get_parameter(rightName + ".i_width").as_int(); - height = getROSNode()->get_parameter(rightName + ".i_height").as_int(); - socket = static_cast(getROSNode()->get_parameter(rightName + ".i_board_socket_id").as_int()); - } catch(rclcpp::exceptions::ParameterNotDeclaredException& e) { - RCLCPP_ERROR(getROSNode()->get_logger(), "Right parameters not set, defaulting to 1280x720 unless specified otherwise."); + RCLCPP_ERROR(getROSNode()->get_logger(), "%s parameters not set, defaulting to 1280x720 unless specified otherwise.", socketName.c_str()); } + declareAndLogParam("i_socket_name", socketName); + stereo->setDepthAlign(socket); } - declareAndLogParam("i_board_socket_id", static_cast(socket)); - stereo->setDepthAlign(socket); if(declareAndLogParam("i_set_input_size", false)) { stereo->setInputResolution(declareAndLogParam("i_input_width", 1280), declareAndLogParam("i_input_height", 720)); } - stereo->setOutputSize(declareAndLogParam("i_width", width), declareAndLogParam("i_height", height)); + width = declareAndLogParam("i_width", width); + height = declareAndLogParam("i_height", height); + stereo->setOutputSize(width, height); stereo->setDefaultProfilePreset(depthPresetMap.at(declareAndLogParam("i_depth_preset", "HIGH_ACCURACY"))); - stereo->enableDistortionCorrection(declareAndLogParam("i_enable_distortion_correction", true)); + if(declareAndLogParam("i_enable_distortion_correction", false)) { + stereo->enableDistortionCorrection(true); + } stereo->initialConfig.setBilateralFilterSigma(declareAndLogParam("i_bilateral_sigma", 0)); stereo->initialConfig.setLeftRightCheckThreshold(declareAndLogParam("i_lrc_threshold", 10)); stereo->initialConfig.setMedianFilter(static_cast(declareAndLogParam("i_depth_filter_size", 5))); - stereo->initialConfig.setConfidenceThreshold(declareAndLogParam("i_stereo_conf_threshold", 255)); + stereo->initialConfig.setConfidenceThreshold(declareAndLogParam("i_stereo_conf_threshold", 240)); if(declareAndLogParam("i_subpixel", false)) { stereo->initialConfig.setSubpixel(true); stereo->initialConfig.setSubpixelFractionalBits(declareAndLogParam("i_subpixel_fractional_bits", 3)); } stereo->setRectifyEdgeFillColor(declareAndLogParam("i_rectify_edge_fill_color", 0)); + if(declareAndLogParam("i_enable_alpha_scaling", false)) { + stereo->setAlphaScaling(declareAndLogParam("i_alpha_scaling", 0.0)); + } auto config = stereo->initialConfig.get(); config.costMatching.disparityWidth = utils::getValFromMap(declareAndLogParam("i_disparity_width", "DISPARITY_96"), disparityWidthMap); stereo->setExtendedDisparity(declareAndLogParam("i_extended_disp", false)); @@ -110,6 +133,9 @@ void StereoParamHandler::declareParams(std::shared_ptr s if(config.postProcessing.speckleFilter.enable) { config.postProcessing.speckleFilter.speckleRange = declareAndLogParam("i_speckle_filter_speckle_range", 50); } + if(declareAndLogParam("i_enable_disparity_shift", false)) { + config.algorithmControl.disparityShift = declareAndLogParam("i_disparity_shift", 0); + } config.postProcessing.spatialFilter.enable = declareAndLogParam("i_enable_spatial_filter", false); if(config.postProcessing.spatialFilter.enable) { config.postProcessing.spatialFilter.holeFillingRadius = declareAndLogParam("i_spatial_filter_hole_filling_radius", 2); @@ -129,6 +155,18 @@ void StereoParamHandler::declareParams(std::shared_ptr s config.postProcessing.decimationFilter.decimationMode = utils::getValFromMap(declareAndLogParam("i_decimation_filter_decimation_mode", "PIXEL_SKIPPING"), decimationModeMap); config.postProcessing.decimationFilter.decimationFactor = declareAndLogParam("i_decimation_filter_decimation_factor", 1); + int decimatedWidth = width / config.postProcessing.decimationFilter.decimationFactor; + int decimatedHeight = height / config.postProcessing.decimationFilter.decimationFactor; + RCLCPP_INFO(getROSNode()->get_logger(), + "Decimation filter enabled with decimation factor %d. Previous width and height: %d x %d, after decimation: %d x %d", + config.postProcessing.decimationFilter.decimationFactor, + width, + height, + decimatedWidth, + decimatedHeight); + stereo->setOutputSize(decimatedWidth, decimatedHeight); + declareAndLogParam("i_width", decimatedWidth, true); + declareAndLogParam("i_height", decimatedHeight, true); } stereo->initialConfig.set(config); } diff --git a/depthai_ros_driver/src/pipeline/base_types.cpp b/depthai_ros_driver/src/pipeline/base_types.cpp index 41ba908e..12c9752a 100644 --- a/depthai_ros_driver/src/pipeline/base_types.cpp +++ b/depthai_ros_driver/src/pipeline/base_types.cpp @@ -128,17 +128,15 @@ std::vector> CamArray::createPipeline(rclcp 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++; + + for(auto& feature : device->getConnectedCameraFeatures()) { + if(feature.name == "color") { + auto daiNode = std::make_unique("rgb", node, pipeline, device, feature.socket); + daiNodes.push_back(std::move(daiNode)); + } else { + auto daiNode = std::make_unique(feature.name, node, pipeline, device, feature.socket); + daiNodes.push_back(std::move(daiNode)); } - 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; } diff --git a/depthai_ros_driver/src/pipeline/pipeline_generator.cpp b/depthai_ros_driver/src/pipeline/pipeline_generator.cpp index d9a6be32..e9948a14 100644 --- a/depthai_ros_driver/src/pipeline/pipeline_generator.cpp +++ b/depthai_ros_driver/src/pipeline/pipeline_generator.cpp @@ -3,6 +3,7 @@ #include "depthai/device/Device.hpp" #include "depthai/pipeline/Pipeline.hpp" #include "depthai_ros_driver/dai_nodes/sensors/imu.hpp" +#include "depthai_ros_driver/dai_nodes/sys_logger.hpp" #include "depthai_ros_driver/pipeline/base_pipeline.hpp" #include "depthai_ros_driver/utils.hpp" #include "pluginlib/class_loader.hpp" @@ -45,7 +46,8 @@ std::vector> PipelineGenerator::createPipel daiNodes.push_back(std::move(imu)); } } - + auto sysLogger = std::make_unique("sys_logger", node, pipeline); + daiNodes.push_back(std::move(sysLogger)); RCLCPP_INFO(node->get_logger(), "Finished setting up pipeline."); return daiNodes; } diff --git a/depthai_ros_driver/src/utils.cpp b/depthai_ros_driver/src/utils.cpp index a82b02e9..6d50a2c5 100644 --- a/depthai_ros_driver/src/utils.cpp +++ b/depthai_ros_driver/src/utils.cpp @@ -1,4 +1,7 @@ #include "depthai_ros_driver/utils.hpp" + +#include "depthai-shared/common/CameraBoardSocket.hpp" +#include "depthai-shared/common/CameraFeatures.hpp" namespace depthai_ros_driver { namespace utils { std::string getUpperCaseStr(const std::string& string) { @@ -6,5 +9,19 @@ std::string getUpperCaseStr(const std::string& string) { for(auto& c : upper) c = toupper(c); return upper; } +std::string getSocketName(dai::CameraBoardSocket socket, std::vector camFeatures) { + std::string name; + for(auto& cam : camFeatures) { + if(cam.socket == socket) { + if(cam.name == "color" || cam.name == "center") { + name = "rgb"; + } else { + name = cam.name; + } + return name; + } + } + throw std::runtime_error("Camera socket not found"); +} } // namespace utils } // 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 9210b25b..0cea7ad0 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.5) +project(depthai_ros_msgs VERSION 2.8.0) if(POLICY CMP0057) cmake_policy(SET CMP0057 NEW) @@ -25,6 +25,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/HandLandmark.msg" "msg/HandLandmarkArray.msg" "msg/ImuWithMagneticField.msg" + "msg/TrackedFeature.msg" + "msg/TrackedFeatures.msg" # "msg/ImageMarker.msg" # "msg/ImageMarkerArray.msg" "msg/SpatialDetection.msg" diff --git a/depthai_ros_msgs/msg/TrackedFeature.msg b/depthai_ros_msgs/msg/TrackedFeature.msg new file mode 100644 index 00000000..f0aead6e --- /dev/null +++ b/depthai_ros_msgs/msg/TrackedFeature.msg @@ -0,0 +1,10 @@ +std_msgs/Header header + +geometry_msgs/Point position +uint32 id + +uint32 age + +float32 harris_score + +float32 tracking_error diff --git a/depthai_ros_msgs/msg/TrackedFeatures.msg b/depthai_ros_msgs/msg/TrackedFeatures.msg new file mode 100644 index 00000000..ab977295 --- /dev/null +++ b/depthai_ros_msgs/msg/TrackedFeatures.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +depthai_ros_msgs/TrackedFeature[] features \ No newline at end of file diff --git a/depthai_ros_msgs/package.xml b/depthai_ros_msgs/package.xml index 9b444079..da69c6ad 100644 --- a/depthai_ros_msgs/package.xml +++ b/depthai_ros_msgs/package.xml @@ -1,7 +1,7 @@ depthai_ros_msgs - 2.7.5 + 2.8.0 Package to keep interface independent of the driver Sachin Guruswamy