From ebc6d6ff1fbd79829c71d4609a581043774c30e0 Mon Sep 17 00:00:00 2001 From: Adam Serafin Date: Fri, 2 Dec 2022 10:00:45 +0100 Subject: [PATCH] Foxy dockerfile (#165) * separate branch for humble * humble dockerfile update * galactic dockerfile update * foxy dockerfile * Galactic dockerfile (#167) * separate branch for humble * humble dockerfile update * update readme * galactic dockerfile update * updated dockerfile and workflow * formatting * typo * update versions * typo * try different naming scheme * add required ros distro * typo * latest to 22.04 * separate clang format * different argument format * update lint version * update setup ros as well * lint cmakelists * update workflow * remove gcc10 from dockerfile * Revert "remove gcc10 from dockerfile" This reverts commit ca4d011a9c473ea42a51e338a951437608ae29ab. * remove gcc10 in dockerfile * Humble dockerfile (#166) * humble dockerfile update * update readme * updated dockerfile and workflow * formatting * typo * update versions * typo * try different naming scheme * add required ros distro * typo * latest to 22.04 * separate clang format * different argument format * update lint version * update setup ros as well * lint cmakelists * update workflow * remove gcc10 in dockerfile * use env variables for distro * update readme * changing platform syntax * remove platform var * syntax change again * syntax again * reword * reword * minor typo * add colcon sequential arg * rewording * change platform * minor arg change * cmakelists format * minor typo --- .github/workflows/main.workflow.yml | 182 +++++++------ .zshrc | 8 + Dockerfile | 23 ++ README.md | 73 +++--- depthai-ros/CMakeLists.txt | 2 +- depthai_bridge/CMakeLists.txt | 57 ++-- .../depthai_bridge/BridgePublisher.hpp | 12 +- .../include/depthai_bridge/ImageConverter.hpp | 9 +- .../include/depthai_bridge/ImuConverter.hpp | 8 +- .../include/depthai_bridge/depthaiUtility.hpp | 82 +++--- depthai_bridge/src/ImageConverter.cpp | 1 - depthai_bridge/src/ImuConverter.cpp | 1 - .../src/SpatialDetectionConverter.cpp | 1 - depthai_examples/CMakeLists.txt | 18 +- depthai_examples/src/crop_control_service.cpp | 233 ++++++++--------- depthai_examples/src/depth_crop_control.cpp | 92 ++++--- depthai_examples/src/rgb_publisher.cpp | 45 ++-- depthai_examples/src/rgb_stereo_node.cpp | 244 +++++++++--------- depthai_examples/src/rgb_video_subscriber.cpp | 13 +- .../src/stereo_inertial_publisher.cpp | 151 ++++++----- depthai_examples/src/stereo_publisher.cpp | 200 +++++++------- depthai_ros_msgs/CMakeLists.txt | 4 +- depthai_ros_msgs/package.xml | 3 +- entrypoint.sh | 7 + 24 files changed, 720 insertions(+), 749 deletions(-) create mode 100644 .zshrc create mode 100644 Dockerfile create mode 100755 entrypoint.sh diff --git a/.github/workflows/main.workflow.yml b/.github/workflows/main.workflow.yml index 5c3aa2e9..38dee474 100644 --- a/.github/workflows/main.workflow.yml +++ b/.github/workflows/main.workflow.yml @@ -6,117 +6,115 @@ on: push: branches: - main - - devel + - humble + - galactic + - foxy tags: - 'v*' pull_request: branches: - main - - devel + - humble + - galactic + - foxy +env: + ROS_DISTRO: foxy jobs: - - style: + clang-format-lint: + name: ament_clang_format runs-on: ubuntu-20.04 - + strategy: + fail-fast: false steps: - - uses: actions/checkout@v2 - - - name: Clang-Format lint - - uses: DoozyX/clang-format-lint-action@v0.13 + - uses: actions/checkout@v2.3.4 + - uses: ros-tooling/setup-ros@0.4.1 + with: + required-ros-distributions: ${{ env.ROS_DISTRO }} + - uses: ros-tooling/action-ros2-lint@0.1.3 with: - source: '.' - extensions: 'h,hpp,c,cpp' - clangFormatVersion: 10 - - ROS1-build: - runs-on: ${{ matrix.os }} + distribution: ${{ env.ROS_DISTRO }} + linter: clang_format + arguments: --config ./.clang-format + package-name: | + depthai_bridge + depthai_examples + depthai_ros_msgs + depthai_ros + linting: + name: ament_${{ matrix.linter }} + runs-on: ubuntu-20.04 strategy: + fail-fast: false matrix: - os: [ubuntu-18.04, ubuntu-20.04] - + linter: [xmllint, pep257, lint_cmake] steps: - - uses: actions/checkout@v2 - - - name: Melodic build - if: matrix.os == 'ubuntu-18.04' - uses: ros-tooling/setup-ros@v0.2 - with: - required-ros-distributions: melodic - - - name: Noetic build - if: matrix.os == 'ubuntu-20.04' - uses: ros-tooling/setup-ros@v0.2 - with: - required-ros-distributions: noetic - - - name: Installing libusb - run: sudo apt-get install libusb-1.0-0-dev - - - name: Installing DepthAi Core - run: sudo wget -qO- https://raw.githubusercontent.com/luxonis/depthai-ros/$GITHUB_SHA/install_dependencies.sh | sudo bash - - - name: Build depthai-bridge melodic - if: matrix.os == 'ubuntu-18.04' - uses: ros-tooling/action-ros-ci@v0.2 - with: - # vcs-repo-file-url: "${{ github.workspace }}/.github/workflows/examples.repos" - # package-name: depthai_examples - target-ros1-distro: melodic - skip-tests: true - - - name: Build depthai-bridge noetic - if: matrix.os == 'ubuntu-20.04' - uses: ros-tooling/action-ros-ci@v0.2 - with: - # vcs-repo-file-url: "${{ github.workspace }}/.github/workflows/examples.repos" - # package-name: depthai_examples - target-ros1-distro: noetic - skip-tests: true - + - uses: actions/checkout@v2.3.4 + - uses: ros-tooling/setup-ros@0.4.1 + with: + required-ros-distributions: ${{ env.ROS_DISTRO }} + - uses: ros-tooling/action-ros2-lint@0.1.3 + with: + distribution: ${{ env.ROS_DISTRO }} + linter: ${{ matrix.linter }} + package-name: | + depthai_bridge + depthai_examples + depthai_ros_msgs + depthai_ros - ROS2-build: + docker-build: + name: Build and Upload to Docker Hub runs-on: ubuntu-20.04 strategy: - matrix: - ros_distribution: [foxy] - + fail-fast: false + env: + PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'luxonis/depthai-ros') && startsWith(github.ref, 'refs/tags/v') }} steps: - - uses: actions/checkout@v2 - - - name: ${{ matrix.ros_distribution }} build - uses: ros-tooling/setup-ros@v0.2 - with: - required-ros-distributions: ${{ matrix.ros_distribution }} + - name: Checkout + uses: actions/checkout@v3 - - name: Installing libusb - run: sudo apt-get install libusb-1.0-0-dev + - name: Set up QEMU + uses: docker/setup-qemu-action@v2 - - name: Installing DepthAi Core - run: sudo wget -qO- https://raw.githubusercontent.com/luxonis/depthai-ros/$GITHUB_SHA/install_dependencies.sh | sudo bash - - - name: Build depthai-bridge ${{ matrix.ros_distribution }} - uses: ros-tooling/action-ros-ci@v0.2 - with: - # vcs-repo-file-url: "${{ github.workspace }}/.github/workflows/examples.repos" - # package-name: depthai_examples - target-ros2-distro: ${{ matrix.ros_distribution }} - skip-tests: true + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v2 + - name: Login to DockerHub + uses: docker/login-action@v1 + if: env.PUSH == 'true' + with: + username: ${{ secrets.DOCKERHUB_USER }} + password: ${{ secrets.DOCKERHUB_PASS }} + - name: Get Version + if: env.PUSH == 'true' + id: vars + run: echo ::set-output name=short_ref::${GITHUB_REF#refs/*/} + - name: Build + uses: docker/build-push-action@v3 + if: env.PUSH == 'false' + with: + build-args: | + ROS_DISTRO=${{ env.ROS_DISTRO }} + USE_RVIZ=1 + platforms: | + amd64 + arm64 + no-cache: true - # build_: - # runs-on: ubuntu-18.04 - # steps: - # - uses: actions/checkout@v2 - # - uses: ros-tooling/setup-ros@v0.2 - # with: - # required-ros-distributions: melodic - # - run: sudo wget -qO- https://raw.githubusercontent.com/luxonis/depthai-ros/noetic-devel/install_dependencies.sh | sudo bash - # - uses: ros-tooling/action-ros-ci@v0.2 - # with: - # # vcs-repo-file-url: "${{ github.workspace }}/.github/workflows/examples.repos" - # # package-name: depthai_examples - # target-ros1-distro: melodic - # skip-tests: true + - name: Build and Push + uses: docker/build-push-action@v3 + if: env.PUSH == 'true' + with: + build-args: | + ROS_DISTRO=${{ env.ROS_DISTRO }} + USE_RVIZ=1 + platforms: | + amd64 + arm64 + push: ${{ env.PUSH }} + no-cache: true + tags: | + luxonis/depthai-ros:${{ env.ROS_DISTRO }}-${{ steps.vars.outputs.short_ref }} + luxonis/depthai-ros:${{ env.ROS_DISTRO }}-latest diff --git a/.zshrc b/.zshrc new file mode 100644 index 00000000..d6151f6f --- /dev/null +++ b/.zshrc @@ -0,0 +1,8 @@ +export ZSH="$HOME/.oh-my-zsh" + +ZSH_THEME="robbyrussell" + +plugins=(git) + +source $ZSH/oh-my-zsh.sh + diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 00000000..a14a42d2 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,23 @@ +ARG ROS_DISTRO=humble +FROM ros:${ROS_DISTRO}-ros-base +ARG USE_RVIZ +ARG BUILD_SEQUENTIAL=0 +ENV DEBIAN_FRONTEND=noninteractive +RUN apt-get update \ + && apt-get -y install --no-install-recommends software-properties-common git libusb-1.0-0-dev wget zsh python3-colcon-common-extensions + +ENV DEBIAN_FRONTEND=dialog +RUN sh -c "$(wget https://raw.github.com/ohmyzsh/ohmyzsh/master/tools/install.sh -O -)" + +ENV WS=/ws +RUN mkdir -p $WS/src +COPY ./ .$WS/src/depthai_ros +RUN cd .$WS/ && rosdep install --from-paths src --ignore-src -y +RUN if [ "$BUILD_SEQUENTIAL" = "1" ] ; then cd .$WS/ && . /opt/ros/${ROS_DISTRO}/setup.sh && colcon build --executor sequential --cmake-args -DCMAKE_BUILD_TYPE=Release ; else cd .$WS/ && . /opt/ros/${ROS_DISTRO}/setup.sh && colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release; fi +RUN if [ "$USE_RVIZ" = "1" ] ; then echo "RVIZ ENABLED" && sudo apt install -y ros-${ROS_DISTRO}-rviz2 ros-${ROS_DISTRO}-rviz-imu-plugin ; else echo "RVIZ NOT ENABLED"; fi +RUN echo "if [ -f ${WS}/install/setup.zsh ]; then source ${WS}/install/setup.zsh; fi" >> $HOME/.zshrc +RUN echo 'eval "$(register-python-argcomplete3 ros2)"' >> $HOME/.zshrc +RUN echo 'eval "$(register-python-argcomplete3 colcon)"' >> $HOME/.zshrc +RUN echo "if [ -f ${WS}/install/setup.bash ]; then source ${WS}/install/setup.bash; fi" >> $HOME/.bashrc +ENTRYPOINT [ "/ws/src/depthai_ros/entrypoint.sh" ] +CMD ["zsh"] \ No newline at end of file diff --git a/README.md b/README.md index 60484edf..22593519 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,12 @@ -# depthai-ros -main branch supports ROS Melodic, ROS Noetic, ROS2 Foxy & Galactic. Might also work on kinetic too. +# Depthai ROS Repository +Hi and welcome to the main depthai-ros respository! +Supported ROS versions: +- Noetic +- Galactic +- Humble +For development check out respective git branches. ### Install from ros binaries @@ -13,6 +18,32 @@ sudo udevadm control --reload-rules && sudo udevadm trigger Install depthai-ros. (Available for Noetic, foxy, galactic and humble) `sudo apt install ros--depthai-ros` +## Docker +You can additionally build and run docker images on your local machine. To do that, add USB rules as in above step, clone the repository and inside it run (it matters on which branch you are on): +``` +docker build --build-arg USE_RVIZ=1 -t depthai_ros . +``` +If you find out that you run out of RAM during building, you can also set `BUILD_SEQUENTIAL=1` to build packages one at a time, it should take longer, but use less RAM. + +`RUN_RVIZ` arg means rviz will be installed inside docker. If you want to run it you need to also execute following command (you'll have to do it again after restarting your PC): +``` +xhost +local:docker +``` + +Then you can run your image in following way: +``` +docker run -it -v /dev/:/dev/ --privileged -e DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix depthai_ros +``` +will run an interactive docker session. +### Running on ROS1 +``` +docker run -it -v /dev/:/dev/ --privileged -e DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix depthai_ros roslaunch depthai_examples stereo_inertial_node.launch +``` +Will only start `stereo_inertial_node` launch file (you can try different commands). +### Running on ROS2 +``` +docker run -it -v /dev/:/dev/ --privileged -e DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix depthai_ros roslaunch depthai_examples stereo_inertial_node.launch.py +``` ## Install from source ### Install dependencies @@ -42,42 +73,6 @@ The following setup procedure assumes you have cmake version >= 3.10.2 and OpenC 7. `catkin_make` (For ROS1) `colcon build` (for ROS2) 8. `source devel/setup.bash` (For ROS1) & `source install/setup.bash` (for ROS2) - - - - - ## Executing an example ### ROS1 @@ -132,4 +127,4 @@ ros2 launch depthai_examples mobile_publisher.launch.py camera_model:=OAK-D-LITE ### Users can write Custom converters and plug them in for bridge Publisher. If there a standard Message or usecase for which we have not provided a ros msg or - converter feel free to create a issue or reach out to us on our discord community. We would be happy to add more. + converter feel free to create a issue or reach out to us on our discord community. We would be happy to add more. \ No newline at end of file diff --git a/depthai-ros/CMakeLists.txt b/depthai-ros/CMakeLists.txt index ae0b7ca9..efbcc0d4 100644 --- a/depthai-ros/CMakeLists.txt +++ b/depthai-ros/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required (VERSION 3.10.2) # CMake version in Ubuntu 18.04 LTS +cmake_minimum_required(VERSION 3.10.2) # CMake version in Ubuntu 18.04 LTS project(depthai-ros VERSION 2.5.3 LANGUAGES CXX C) diff --git a/depthai_bridge/CMakeLists.txt b/depthai_bridge/CMakeLists.txt index ca772619..25dfb6ff 100644 --- a/depthai_bridge/CMakeLists.txt +++ b/depthai_bridge/CMakeLists.txt @@ -1,5 +1,5 @@ -cmake_minimum_required (VERSION 3.10.2) # CMake version in Ubuntu 18.04 LTS -set (CMAKE_POSITION_INDEPENDENT_CODE ON) +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.5.3 LANGUAGES CXX C) @@ -39,16 +39,16 @@ find_package(std_msgs REQUIRED) find_package(vision_msgs REQUIRED) set(dependencies - camera_info_manager - cv_bridge - depthai_ros_msgs - image_transport - rclcpp - sensor_msgs - stereo_msgs - std_msgs - vision_msgs - ) + camera_info_manager + cv_bridge + depthai_ros_msgs + image_transport + rclcpp + sensor_msgs + stereo_msgs + std_msgs + vision_msgs +) include_directories( include @@ -56,7 +56,7 @@ include_directories( ) -FILE(GLOB LIB_SRC +file(GLOB LIB_SRC "src/DisparityConverter.cpp" "src/ImageConverter.cpp" "src/ImgDetectionConverter.cpp" @@ -72,18 +72,19 @@ target_compile_definitions(${PROJECT_NAME} PUBLIC IS_ROS2) message(STATUS "--------------------------$ENV{ROS_DISTRO} ---") message(STATUS $ENV{ROS_DISTRO}) -if($ENV{ROS_DISTRO} STREQUAL "galactic" OR ($ENV{ROS_DISTRO} STREQUAL "rolling" AND "${ubuntu_version}" STREQUAL "20.04\n")) +if($ENV{ROS_DISTRO} STREQUAL "galactic" OR +($ENV{ROS_DISTRO} STREQUAL "rolling" AND "${ubuntu_version}" STREQUAL "20.04\n")) target_compile_definitions(${PROJECT_NAME} PRIVATE IS_GALACTIC) endif() -if($ENV{ROS_DISTRO} STREQUAL "humble" ) +if($ENV{ROS_DISTRO} STREQUAL "humble") target_compile_definitions(${PROJECT_NAME} PRIVATE IS_HUMBLE) endif() -target_link_libraries(${PROJECT_NAME} - depthai::core +target_link_libraries(${PROJECT_NAME} + depthai::core opencv_imgproc - opencv_highgui) + opencv_highgui) ament_export_targets(depthai_bridgeTargets HAS_LIBRARY_TARGET) @@ -92,25 +93,25 @@ install(DIRECTORY include/ ) install(TARGETS depthai_bridge - EXPORT depthai_bridgeTargets + EXPORT depthai_bridgeTargets ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin INCLUDES DESTINATION include - ) +) install(EXPORT depthai_bridgeTargets DESTINATION share/${PROJECT_NAME}/cmake) - install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch - FILES_MATCHING PATTERN "*.py") - install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME}) - install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch + FILES_MATCHING PATTERN "*.py") +install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) - ament_export_include_directories(include) - ament_export_libraries(depthai_bridge) - ament_export_dependencies(${dependencies}) +ament_export_include_directories(include) +ament_export_libraries(depthai_bridge) +ament_export_dependencies(${dependencies}) - ament_package() +ament_package() diff --git a/depthai_bridge/include/depthai_bridge/BridgePublisher.hpp b/depthai_bridge/include/depthai_bridge/BridgePublisher.hpp index 726c5986..39836643 100644 --- a/depthai_bridge/include/depthai_bridge/BridgePublisher.hpp +++ b/depthai_bridge/include/depthai_bridge/BridgePublisher.hpp @@ -1,22 +1,18 @@ #pragma once +#include #include +#include #include #include #include #include "depthai/depthai.hpp" - - -#include -#include - #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/camera_info.hpp" #include "sensor_msgs/msg/image.hpp" #include "std_msgs/msg/header.hpp" - namespace dai { namespace ros { @@ -26,7 +22,6 @@ namespace ImageMsgs = sensor_msgs::msg; using ImagePtr = ImageMsgs::Image::SharedPtr; namespace rosOrigin = ::rclcpp; - template class BridgePublisher { public: @@ -67,7 +62,6 @@ class BridgePublisher { */ typename rclcpp::Publisher::SharedPtr advertise(int queueSize, std::false_type); - BridgePublisher(const BridgePublisher& other); void addPublisherCallback(); @@ -170,8 +164,6 @@ std::shared_ptr BridgePublisher::adv return std::make_shared(_it.advertise(_rosTopic, queueSize)); } - - template void BridgePublisher::daiCallback(std::string name, std::shared_ptr data) { // std::cout << "In callback " << name << std::endl; diff --git a/depthai_bridge/include/depthai_bridge/ImageConverter.hpp b/depthai_bridge/include/depthai_bridge/ImageConverter.hpp index 983e6fa2..214b01aa 100644 --- a/depthai_bridge/include/depthai_bridge/ImageConverter.hpp +++ b/depthai_bridge/include/depthai_bridge/ImageConverter.hpp @@ -12,10 +12,10 @@ #include #include - #include "rclcpp/rclcpp.hpp" - #include "sensor_msgs/msg/camera_info.hpp" - #include "sensor_msgs/msg/image.hpp" - #include "std_msgs/msg/header.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "std_msgs/msg/header.hpp" namespace dai { @@ -63,7 +63,6 @@ class ImageConverter { std::chrono::time_point _steadyBaseTime; rclcpp::Time _rosBaseTime; - }; } // namespace ros diff --git a/depthai_bridge/include/depthai_bridge/ImuConverter.hpp b/depthai_bridge/include/depthai_bridge/ImuConverter.hpp index b2f94989..93396ddd 100644 --- a/depthai_bridge/include/depthai_bridge/ImuConverter.hpp +++ b/depthai_bridge/include/depthai_bridge/ImuConverter.hpp @@ -7,10 +7,8 @@ #include #include - - #include "rclcpp/rclcpp.hpp" - #include "sensor_msgs/msg/imu.hpp" - +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/imu.hpp" namespace dai { @@ -19,7 +17,6 @@ namespace ros { namespace ImuMsgs = sensor_msgs::msg; using ImuPtr = ImuMsgs::Imu::SharedPtr; - enum class ImuSyncMethod { COPY, LINEAR_INTERPOLATE_GYRO, LINEAR_INTERPOLATE_ACCEL }; class ImuConverter { @@ -41,7 +38,6 @@ class ImuConverter { ImuSyncMethod _syncMode; std::chrono::time_point _steadyBaseTime; rclcpp::Time _rosBaseTime; - }; } // namespace ros diff --git a/depthai_bridge/include/depthai_bridge/depthaiUtility.hpp b/depthai_bridge/include/depthai_bridge/depthaiUtility.hpp index 9451024f..744edae9 100644 --- a/depthai_bridge/include/depthai_bridge/depthaiUtility.hpp +++ b/depthai_bridge/include/depthai_bridge/depthaiUtility.hpp @@ -10,45 +10,44 @@ namespace ros { enum LogLevel { DEBUG, INFO, WARN, ERROR, FATAL }; - #define DEPTHAI_ROS_LOG_STREAM(loggerName, level, isOnce, args) \ - switch(level) { \ - case dai::ros::LogLevel::DEBUG: \ - if(isOnce) { \ - RCLCPP_DEBUG_STREAM_ONCE(rclcpp::get_logger(loggerName), args); \ - } else { \ - RCLCPP_DEBUG_STREAM(rclcpp::get_logger(loggerName), args); \ - } \ - break; \ - case dai::ros::LogLevel::INFO: \ - if(isOnce) { \ - RCLCPP_INFO_STREAM_ONCE(rclcpp::get_logger(loggerName), args); \ - } else { \ - RCLCPP_INFO_STREAM(rclcpp::get_logger(loggerName), args); \ - } \ - break; \ - case dai::ros::LogLevel::WARN: \ - if(isOnce) { \ - RCLCPP_WARN_STREAM_ONCE(rclcpp::get_logger(loggerName), args); \ - } else { \ - RCLCPP_WARN_STREAM(rclcpp::get_logger(loggerName), args); \ - } \ - break; \ - case dai::ros::LogLevel::ERROR: \ - if(isOnce) { \ - RCLCPP_ERROR_STREAM_ONCE(rclcpp::get_logger(loggerName), args); \ - } else { \ - RCLCPP_ERROR_STREAM(rclcpp::get_logger(loggerName), args); \ - } \ - break; \ - case dai::ros::LogLevel::FATAL: \ - if(isOnce) { \ - RCLCPP_FATAL_STREAM_ONCE(rclcpp::get_logger(loggerName), args); \ - } else { \ - RCLCPP_FATAL_STREAM(rclcpp::get_logger(loggerName), args); \ - } \ - break; \ - } - +#define DEPTHAI_ROS_LOG_STREAM(loggerName, level, isOnce, args) \ + switch(level) { \ + case dai::ros::LogLevel::DEBUG: \ + if(isOnce) { \ + RCLCPP_DEBUG_STREAM_ONCE(rclcpp::get_logger(loggerName), args); \ + } else { \ + RCLCPP_DEBUG_STREAM(rclcpp::get_logger(loggerName), args); \ + } \ + break; \ + case dai::ros::LogLevel::INFO: \ + if(isOnce) { \ + RCLCPP_INFO_STREAM_ONCE(rclcpp::get_logger(loggerName), args); \ + } else { \ + RCLCPP_INFO_STREAM(rclcpp::get_logger(loggerName), args); \ + } \ + break; \ + case dai::ros::LogLevel::WARN: \ + if(isOnce) { \ + RCLCPP_WARN_STREAM_ONCE(rclcpp::get_logger(loggerName), args); \ + } else { \ + RCLCPP_WARN_STREAM(rclcpp::get_logger(loggerName), args); \ + } \ + break; \ + case dai::ros::LogLevel::ERROR: \ + if(isOnce) { \ + RCLCPP_ERROR_STREAM_ONCE(rclcpp::get_logger(loggerName), args); \ + } else { \ + RCLCPP_ERROR_STREAM(rclcpp::get_logger(loggerName), args); \ + } \ + break; \ + case dai::ros::LogLevel::FATAL: \ + if(isOnce) { \ + RCLCPP_FATAL_STREAM_ONCE(rclcpp::get_logger(loggerName), args); \ + } else { \ + RCLCPP_FATAL_STREAM(rclcpp::get_logger(loggerName), args); \ + } \ + break; \ + } // DEBUG stream macros on top of ROS logger #define DEPTHAI_ROS_DEBUG_STREAM(loggerName, args) DEPTHAI_ROS_LOG_STREAM(loggerName, dai::ros::LogLevel::DEBUG, false, args) @@ -76,8 +75,8 @@ enum LogLevel { DEBUG, INFO, WARN, ERROR, FATAL }; #define DEPTHAI_ROS_FATAL_STREAM_ONCE(loggerName, args) DEPTHAI_ROS_LOG_STREAM(loggerName, dai::ros::LogLevel::FATAL, true, args) inline rclcpp::Time getFrameTime(rclcpp::Time rclBaseTime, - std::chrono::time_point steadyBaseTime, - std::chrono::time_point currTimePoint) { + std::chrono::time_point steadyBaseTime, + std::chrono::time_point currTimePoint) { auto elapsedTime = currTimePoint - steadyBaseTime; // uint64_t nSec = rosBaseTime.toNSec() + std::chrono::duration_cast(elapsedTime).count(); auto rclStamp = rclBaseTime + elapsedTime; @@ -85,7 +84,6 @@ inline rclcpp::Time getFrameTime(rclcpp::Time rclBaseTime, return rclStamp; } - template T lerp(const T& a, const T& b, const double t) { return a * (1.0 - t) + b * t; diff --git a/depthai_bridge/src/ImageConverter.cpp b/depthai_bridge/src/ImageConverter.cpp index 305d2add..b24f8759 100644 --- a/depthai_bridge/src/ImageConverter.cpp +++ b/depthai_bridge/src/ImageConverter.cpp @@ -23,7 +23,6 @@ std::unordered_map ImageConverter::planarEn ImageConverter::ImageConverter(bool interleaved) : _daiInterleaved(interleaved), _steadyBaseTime(std::chrono::steady_clock::now()) { _rosBaseTime = rclcpp::Clock().now(); - } ImageConverter::ImageConverter(const std::string frameName, bool interleaved) diff --git a/depthai_bridge/src/ImuConverter.cpp b/depthai_bridge/src/ImuConverter.cpp index 1c42b821..65c4c13f 100644 --- a/depthai_bridge/src/ImuConverter.cpp +++ b/depthai_bridge/src/ImuConverter.cpp @@ -13,7 +13,6 @@ ImuConverter::ImuConverter(const std::string& frameName, ImuSyncMethod syncMode, _sequenceNum(0), _steadyBaseTime(std::chrono::steady_clock::now()) { _rosBaseTime = rclcpp::Clock().now(); - } void ImuConverter::FillImuData_LinearInterpolation(std::vector& imuPackets, std::deque& imuMsgs) { diff --git a/depthai_bridge/src/SpatialDetectionConverter.cpp b/depthai_bridge/src/SpatialDetectionConverter.cpp index 450ca4c4..cd42304c 100644 --- a/depthai_bridge/src/SpatialDetectionConverter.cpp +++ b/depthai_bridge/src/SpatialDetectionConverter.cpp @@ -8,7 +8,6 @@ SpatialDetectionConverter::SpatialDetectionConverter(std::string frameName, int void SpatialDetectionConverter::toRosMsg(std::shared_ptr inNetData, std::deque& opDetectionMsgs) { - auto tstamp = inNetData->getTimestamp(); SpatialMessages::SpatialDetectionArray opDetectionMsg; diff --git a/depthai_examples/CMakeLists.txt b/depthai_examples/CMakeLists.txt index 3d3d5fe2..d0c5cc05 100644 --- a/depthai_examples/CMakeLists.txt +++ b/depthai_examples/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required (VERSION 3.10.2) # CMake version in Ubuntu 18.04 LTS +cmake_minimum_required(VERSION 3.10.2) # CMake version in Ubuntu 18.04 LTS project(depthai_examples VERSION 2.5.3 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) @@ -22,7 +22,7 @@ set(tiny_yolo_v4_blob_name "yolov4_tiny_coco_416x416_openvino_2021.4_6shave_bgr. set(mobilenet_blob_name "mobilenet-ssd_openvino_2021.2_6shave.blob") set(mobilenet_blob "${PROJECT_SOURCE_DIR}/resources/${mobilenet_blob_name}") -file(DOWNLOAD "https://artifacts.luxonis.com/artifactory/luxonis-depthai-data-local/network/mobilenet-ssd_openvino_2021.2_6shave.blob" +file(DOWNLOAD "https://artifacts.luxonis.com/artifactory/luxonis-depthai-data-local/network/mobilenet-ssd_openvino_2021.2_6shave.blob" ${mobilenet_blob} TIMEOUT 60 # seconds EXPECTED_HASH SHA1=f0e14978b3f77a4f93b9f969cd39e58bb7aef490 @@ -73,15 +73,15 @@ include_directories( macro(dai_add_node_ros2 node_name node_src) add_executable("${node_name}" "${node_src}") - target_link_libraries("${node_name}" - depthai::core + target_link_libraries("${node_name}" + depthai::core opencv_imgproc opencv_highgui) - ament_target_dependencies("${node_name}" - ${dependencies}) + ament_target_dependencies("${node_name}" + ${dependencies}) -endmacro() +endmacro() dai_add_node_ros2(crop_control_service src/crop_control_service.cpp) @@ -92,7 +92,7 @@ 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(stereo_node src/stereo_publisher.cpp) dai_add_node_ros2(yolov4_spatial_node src/yolov4_spatial_publisher.cpp) - + target_compile_definitions(mobilenet_node PRIVATE BLOB_NAME="${mobilenet_blob_name}") target_compile_definitions(yolov4_spatial_node PRIVATE BLOB_NAME="${tiny_yolo_v4_blob_name}") target_compile_definitions(stereo_inertial_node PRIVATE BLOB_NAME="${tiny_yolo_v4_blob_name}") @@ -117,6 +117,6 @@ install(TARGETS stereo_node yolov4_spatial_node DESTINATION lib/${PROJECT_NAME}) - + ament_package() diff --git a/depthai_examples/src/crop_control_service.cpp b/depthai_examples/src/crop_control_service.cpp index ac9bc8e1..eca0220d 100644 --- a/depthai_examples/src/crop_control_service.cpp +++ b/depthai_examples/src/crop_control_service.cpp @@ -1,141 +1,124 @@ -#include "rclcpp/rclcpp.hpp" -#include #include -#include #include + +#include +#include + +#include "rclcpp/rclcpp.hpp" using namespace std::chrono_literals; static constexpr float stepSize = 0.02; -void boundAdjuster(double &value) { - if (value < 0) { - std::cout << "values should always be greater than 0." << std::endl; - std::cout << "Resetting to 0." << std::endl; - value = 0; - } else if (value > 1) { - std::cout << "values should always be less than 1." << std::endl; - std::cout << "Resetting to 1." << std::endl; - value = 1; - } - return; +void boundAdjuster(double& value) { + if(value < 0) { + std::cout << "values should always be greater than 0." << std::endl; + std::cout << "Resetting to 0." << std::endl; + value = 0; + } else if(value > 1) { + std::cout << "values should always be less than 1." << std::endl; + std::cout << "Resetting to 1." << std::endl; + value = 1; + } + return; } -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("crop_control_service"); - std::string serviceName; - node->declare_parameter("service_name", "crop_control_srv"); - node->get_parameter("service_name", serviceName); +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("crop_control_service"); + std::string serviceName; + node->declare_parameter("service_name", "crop_control_srv"); + node->get_parameter("service_name", serviceName); - rclcpp::Client::SharedPtr client = - node->create_client( - serviceName); - - auto request = std::make_shared(); - request->top_left.x = 0.2; - request->top_left.y = 0.2; - request->bottom_right.x = 0.2; - request->bottom_right.y = 0.2; + rclcpp::Client::SharedPtr client = node->create_client(serviceName); - while (!client->wait_for_service(1s)) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), - "Interrupted while waiting for the service. Exiting."); - return 0; - } - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "service not available, waiting again..."); - } - std::cout << "Use the following keys to control the cropping region" - << std::endl; - std::cout << " Q/W -> Increment/Decrement the topleft X position" - << std::endl; - std::cout << " A/S -> Increment/Decrement the topleft Y position" - << std::endl; - std::cout << " E/R -> Increment/Decrement the bottomright X position" - << std::endl; - std::cout << " D/F -> Increment/Decrement the bottomright Y position" - << std::endl; - std::cout << " Preess ctrl+D to exit." << std::endl; - char c; - bool sendSignal = false; + auto request = std::make_shared(); + request->top_left.x = 0.2; + request->top_left.y = 0.2; + request->bottom_right.x = 0.2; + request->bottom_right.y = 0.2; - while (rclcpp::ok()) { - c = std::tolower(getchar()); - switch (c) { - case 'w': - request->top_left.x -= stepSize; - boundAdjuster(request->top_left.x); - sendSignal = true; - break; - case 'q': - request->top_left.x += stepSize; - boundAdjuster(request->top_left.x); - sendSignal = true; - break; - case 'a': - request->top_left.y += stepSize; - boundAdjuster(request->top_left.y); - sendSignal = true; - break; - case 's': - request->top_left.y -= stepSize; - boundAdjuster(request->top_left.y); - sendSignal = true; - break; - case 'e': - request->bottom_right.x += stepSize; - boundAdjuster(request->bottom_right.x); - sendSignal = true; - break; - case 'r': - request->bottom_right.x -= stepSize; - boundAdjuster(request->bottom_right.x); - sendSignal = true; - break; - case 'd': - request->bottom_right.y += stepSize; - boundAdjuster(request->bottom_right.y); - sendSignal = true; - break; - case 'f': - request->bottom_right.y -= stepSize; - boundAdjuster(request->bottom_right.y); - sendSignal = true; - break; - default: - // TODO(sachin): Use RCLCPP_INFO instead of cout. - std::cout << " Entered Invalid Key..!!!" << std::endl; - std::cout << " Use the following keys to control the cropping region" - << std::endl; - std::cout << " Q/W -> Increment/Decrement the topleft X position" - << std::endl; - std::cout << " A/S -> Increment/Decrement the topleft Y position" - << std::endl; - std::cout << " E/R -> Increment/Decrement the bottomright X position" - << std::endl; - std::cout << " D/F -> Increment/Decrement the bottomright Y position" - << std::endl; - std::cout << " Preess ctrl+D to exit." << std::endl; + while(!client->wait_for_service(1s)) { + if(!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + return 0; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again..."); } + std::cout << "Use the following keys to control the cropping region" << std::endl; + std::cout << " Q/W -> Increment/Decrement the topleft X position" << std::endl; + std::cout << " A/S -> Increment/Decrement the topleft Y position" << std::endl; + std::cout << " E/R -> Increment/Decrement the bottomright X position" << std::endl; + std::cout << " D/F -> Increment/Decrement the bottomright Y position" << std::endl; + std::cout << " Preess ctrl+D to exit." << std::endl; + char c; + bool sendSignal = false; + + while(rclcpp::ok()) { + c = std::tolower(getchar()); + switch(c) { + case 'w': + request->top_left.x -= stepSize; + boundAdjuster(request->top_left.x); + sendSignal = true; + break; + case 'q': + request->top_left.x += stepSize; + boundAdjuster(request->top_left.x); + sendSignal = true; + break; + case 'a': + request->top_left.y += stepSize; + boundAdjuster(request->top_left.y); + sendSignal = true; + break; + case 's': + request->top_left.y -= stepSize; + boundAdjuster(request->top_left.y); + sendSignal = true; + break; + case 'e': + request->bottom_right.x += stepSize; + boundAdjuster(request->bottom_right.x); + sendSignal = true; + break; + case 'r': + request->bottom_right.x -= stepSize; + boundAdjuster(request->bottom_right.x); + sendSignal = true; + break; + case 'd': + request->bottom_right.y += stepSize; + boundAdjuster(request->bottom_right.y); + sendSignal = true; + break; + case 'f': + request->bottom_right.y -= stepSize; + boundAdjuster(request->bottom_right.y); + sendSignal = true; + break; + default: + // TODO(sachin): Use RCLCPP_INFO instead of cout. + std::cout << " Entered Invalid Key..!!!" << std::endl; + std::cout << " Use the following keys to control the cropping region" << std::endl; + std::cout << " Q/W -> Increment/Decrement the topleft X position" << std::endl; + std::cout << " A/S -> Increment/Decrement the topleft Y position" << std::endl; + std::cout << " E/R -> Increment/Decrement the bottomright X position" << std::endl; + std::cout << " D/F -> Increment/Decrement the bottomright Y position" << std::endl; + std::cout << " Preess ctrl+D to exit." << std::endl; + } - if (sendSignal) { - std::cout << "Top left Position -> (" << request->top_left.x << ", " - << request->top_left.y << ")" << std::endl; - std::cout << "Bottion right Position -> (" << request->bottom_right.x - << ", " << request->bottom_right.y << ")" << std::endl; - auto result = client->async_send_request(request); - if (rclcpp::spin_until_future_complete(node, result) == - rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Status: %ld", - result.get()->status); - } else { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), - "Failed to call service crop_control_srv"); - } - sendSignal = false; + if(sendSignal) { + std::cout << "Top left Position -> (" << request->top_left.x << ", " << request->top_left.y << ")" << std::endl; + std::cout << "Bottion right Position -> (" << request->bottom_right.x << ", " << request->bottom_right.y << ")" << std::endl; + auto result = client->async_send_request(request); + if(rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Status: %ld", result.get()->status); + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service crop_control_srv"); + } + sendSignal = false; + } } - } - return 0; + return 0; } \ No newline at end of file diff --git a/depthai_examples/src/depth_crop_control.cpp b/depthai_examples/src/depth_crop_control.cpp index 4663244b..12b2666a 100644 --- a/depthai_examples/src/depth_crop_control.cpp +++ b/depthai_examples/src/depth_crop_control.cpp @@ -2,33 +2,29 @@ * This example shows usage of depth camera in crop mode with the possibility to move the crop. * Use 'WASD' in order to do it. */ -#include "rclcpp/rclcpp.hpp" - +#include +#include +#include #include #include - -#include #include -#include -#include #include "depthai/depthai.hpp" - +#include "rclcpp/rclcpp.hpp" // Step size ('W','A','S','D' controls) static constexpr float stepSize = 0.02; std::shared_ptr configQueue; -void cropDepthImage(depthai_ros_msgs::srv::NormalizedImageCrop::Request request, depthai_ros_msgs::srv::NormalizedImageCrop::Response response){ +void cropDepthImage(depthai_ros_msgs::srv::NormalizedImageCrop::Request request, depthai_ros_msgs::srv::NormalizedImageCrop::Response response) { dai::ImageManipConfig cfg; cfg.setCropRect(request.topLeft.x, request.topLeft.y, request.bottomRight.x, request.bottomRight.y); configQueue->send(cfg); response->status = true; - return; + return; } int main() { - rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("depth_crop_control"); std::string cameraName, monoResolution; @@ -39,20 +35,20 @@ int main() { node->declare_parameter("lrcheck", true); node->declare_parameter("extended", false); node->declare_parameter("subpixel", true); - node->declare_parameter("confidence", 200); - node->declare_parameter("LRchecktresh", 5); - node->declare_parameter("monoResolution", "400p"); + node->declare_parameter("confidence", 200); + node->declare_parameter("LRchecktresh", 5); + node->declare_parameter("monoResolution", "400p"); node->get_parameter("tf_prefix", cameraName); - node->get_parameter("lrcheck", lrcheck); - node->get_parameter("extended", extended); - node->get_parameter("subpixel", subpixel); - node->get_parameter("confidence", confidence); + node->get_parameter("lrcheck", lrcheck); + node->get_parameter("extended", extended); + node->get_parameter("subpixel", subpixel); + node->get_parameter("confidence", confidence); node->get_parameter("LRchecktresh", LRchecktresh); node->get_parameter("monoResolution", monoResolution); rclcpp::Service::SharedPtr service = - node->create_service("crop_control_srv", &cropDepthImage); + node->create_service("crop_control_srv", &cropDepthImage); // ros::ServiceServer service = n.advertiseService("crop_control_srv", cropDepthImage); @@ -76,26 +72,25 @@ int main() { dai::Point2f bottomRight(0.8, 0.8); int monoWidth, monoHeight; - dai::node::MonoCamera::Properties::SensorResolution monoRes; - if(monoResolution == "720p"){ - monoRes = dai::node::MonoCamera::Properties::SensorResolution::THE_720_P; - monoWidth = 1280; + dai::node::MonoCamera::Properties::SensorResolution monoRes; + if(monoResolution == "720p") { + monoRes = dai::node::MonoCamera::Properties::SensorResolution::THE_720_P; + monoWidth = 1280; monoHeight = 720; - }else if(monoResolution == "400p" ){ - monoRes = dai::node::MonoCamera::Properties::SensorResolution::THE_400_P; - monoWidth = 640; + } else if(monoResolution == "400p") { + monoRes = dai::node::MonoCamera::Properties::SensorResolution::THE_400_P; + monoWidth = 640; monoHeight = 400; - }else if(monoResolution == "800p" ){ - monoRes = dai::node::MonoCamera::Properties::SensorResolution::THE_800_P; - monoWidth = 1280; + } else if(monoResolution == "800p") { + monoRes = dai::node::MonoCamera::Properties::SensorResolution::THE_800_P; + monoWidth = 1280; monoHeight = 800; - }else if(monoResolution == "480p" ){ - monoRes = dai::node::MonoCamera::Properties::SensorResolution::THE_480_P; - monoWidth = 640; + } else if(monoResolution == "480p") { + monoRes = dai::node::MonoCamera::Properties::SensorResolution::THE_480_P; + monoWidth = 640; monoHeight = 480; - }else{ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), - "Invalid parameter. -> monoResolution: %s", resolution.c_str()); + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Invalid parameter. -> monoResolution: %s", resolution.c_str()); throw std::runtime_error("Invalid mono camera resolution."); } @@ -130,26 +125,27 @@ int main() { auto calibrationHandler = device.readCalibration(); auto boardName = calibrationHandler.getEepromData().boardName; - if (monoHeight > 480 && boardName == "OAK-D-LITE") { + if(monoHeight > 480 && boardName == "OAK-D-LITE") { monoWidth = 640; monoHeight = 480; } dai::rosBridge::ImageConverter depthConverter(cameraName + "_right_camera_optical_frame", true); // TODO(sachin): Modify the calibration based on crop from service - auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, monoWidth, monoHeight); - - dai::rosBridge::BridgePublisher depthPublish(depthQueue, - node, - std::string("stereo/depth"), - std::bind(&dai::rosBridge::ImageConverter::toRosMsg, - &depthConverter, // since the converter has the same frame name - // and image type is also same we can reuse it - std::placeholders::_1, - std::placeholders::_2) , - 30, - rightCameraInfo, - "stereo"); + auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, monoWidth, monoHeight); + + dai::rosBridge::BridgePublisher depthPublish( + depthQueue, + node, + std::string("stereo/depth"), + std::bind(&dai::rosBridge::ImageConverter::toRosMsg, + &depthConverter, // since the converter has the same frame name + // and image type is also same we can reuse it + std::placeholders::_1, + std::placeholders::_2), + 30, + rightCameraInfo, + "stereo"); depthPublish.addPublisherCallback(); rclcpp::spin(node); diff --git a/depthai_examples/src/rgb_publisher.cpp b/depthai_examples/src/rgb_publisher.cpp index e3926247..2b28fdc2 100644 --- a/depthai_examples/src/rgb_publisher.cpp +++ b/depthai_examples/src/rgb_publisher.cpp @@ -1,23 +1,24 @@ -#include "rclcpp/rclcpp.hpp" - -#include #include +#include + +#include "rclcpp/rclcpp.hpp" // #include "utility.hpp" -#include #include +#include // Inludes common necessary includes for development using depthai library -#include "depthai/depthai.hpp" #include #include -dai::Pipeline createPipeline(){ +#include "depthai/depthai.hpp" + +dai::Pipeline createPipeline() { dai::Pipeline pipeline; auto colorCam = pipeline.create(); auto xlinkOut = pipeline.create(); xlinkOut->setStreamName("video"); - + colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); colorCam->setInterleaved(false); @@ -26,41 +27,39 @@ dai::Pipeline createPipeline(){ return pipeline; } -int main(int argc, char** argv){ - +int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("rgb_node"); std::string tfPrefix; std::string cameraParamUri = "package://depthai_examples/params/camera"; - + node->declare_parameter("tf_prefix", "oak"); node->declare_parameter("camera_param_uri", cameraParamUri); - + node->get_parameter("tf_prefix", tfPrefix); node->get_parameter("camera_param_uri", cameraParamUri); dai::Pipeline pipeline = createPipeline(); dai::Device device(pipeline); std::shared_ptr imgQueue = device.getOutputQueue("video", 30, false); - + std::string color_uri = cameraParamUri + "/" + "color.yaml"; dai::rosBridge::ImageConverter rgbConverter(tfPrefix + "_rgb_camera_optical_frame", false); dai::rosBridge::BridgePublisher rgbPublish(imgQueue, - node, - std::string("color/image"), - std::bind(&dai::rosBridge::ImageConverter::toRosMsg, - &rgbConverter, // since the converter has the same frame name - // and image type is also same we can reuse it - std::placeholders::_1, - std::placeholders::_2) , - 30, - color_uri, - "color"); + node, + std::string("color/image"), + std::bind(&dai::rosBridge::ImageConverter::toRosMsg, + &rgbConverter, // since the converter has the same frame name + // and image type is also same we can reuse it + std::placeholders::_1, + std::placeholders::_2), + 30, + color_uri, + "color"); rgbPublish.addPublisherCallback(); rclcpp::spin(node); return 0; } - diff --git a/depthai_examples/src/rgb_stereo_node.cpp b/depthai_examples/src/rgb_stereo_node.cpp index d621a69b..275072ad 100644 --- a/depthai_examples/src/rgb_stereo_node.cpp +++ b/depthai_examples/src/rgb_stereo_node.cpp @@ -1,58 +1,64 @@ -#include "rclcpp/rclcpp.hpp" - -#include -#include -#include #include +#include #include +#include +#include + +#include "rclcpp/rclcpp.hpp" // #include "utility.hpp" -#include #include +#include // Inludes common necessary includes for development using depthai library -#include "depthai/depthai.hpp" - #include #include -using namespace std::chrono_literals; +#include "depthai/depthai.hpp" -std::tuple createPipeline(bool lrcheck, bool extended, bool subpixel, int confidence, int LRchecktresh, - bool useVideo, bool usePreview, int previewWidth, int previewHeight, - std::string mResolution, std::string cResolution){ +using namespace std::chrono_literals; +std::tuple createPipeline(bool lrcheck, + bool extended, + bool subpixel, + int confidence, + int LRchecktresh, + bool useVideo, + bool usePreview, + int previewWidth, + int previewHeight, + std::string mResolution, + std::string cResolution) { dai::Pipeline pipeline; - auto monoLeft = pipeline.create(); - auto monoRight = pipeline.create(); - - auto stereo = pipeline.create(); - auto xoutDepth = pipeline.create(); + auto monoLeft = pipeline.create(); + auto monoRight = pipeline.create(); + + auto stereo = pipeline.create(); + auto xoutDepth = pipeline.create(); xoutDepth->setStreamName("depth"); - + int width, height; - dai::node::MonoCamera::Properties::SensorResolution monoResolution; - if(mResolution == "720p"){ - monoResolution = dai::node::MonoCamera::Properties::SensorResolution::THE_720_P; - width = 1280; + dai::node::MonoCamera::Properties::SensorResolution monoResolution; + if(mResolution == "720p") { + monoResolution = dai::node::MonoCamera::Properties::SensorResolution::THE_720_P; + width = 1280; height = 720; - }else if(mResolution == "400p" ){ - monoResolution = dai::node::MonoCamera::Properties::SensorResolution::THE_400_P; - width = 640; + } else if(mResolution == "400p") { + monoResolution = dai::node::MonoCamera::Properties::SensorResolution::THE_400_P; + width = 640; height = 400; - }else if(mResolution == "800p" ){ - monoResolution = dai::node::MonoCamera::Properties::SensorResolution::THE_800_P; - width = 1280; + } else if(mResolution == "800p") { + monoResolution = dai::node::MonoCamera::Properties::SensorResolution::THE_800_P; + width = 1280; height = 800; - }else if(mResolution == "480p" ){ - monoResolution = dai::node::MonoCamera::Properties::SensorResolution::THE_480_P; - width = 640; + } else if(mResolution == "480p") { + monoResolution = dai::node::MonoCamera::Properties::SensorResolution::THE_480_P; + width = 640; height = 480; - }else{ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), - "Invalid parameter. -> monoResolution: %s", mResolution.c_str()); + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Invalid parameter. -> monoResolution: %s", mResolution.c_str()); throw std::runtime_error("Invalid mono camera resolution."); } @@ -64,7 +70,7 @@ std::tuple createPipeline(bool lrcheck, bool extended, // StereoDepth stereo->initialConfig.setConfidenceThreshold(confidence); - stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout + stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout stereo->initialConfig.setLeftRightCheckThreshold(LRchecktresh); stereo->setLeftRightCheck(lrcheck); stereo->setExtendedDisparity(extended); @@ -79,19 +85,19 @@ std::tuple createPipeline(bool lrcheck, bool extended, auto colorCam = pipeline.create(); dai::ColorCameraProperties::SensorResolution colorResolution; - if (cResolution == "1080p"){ + if(cResolution == "1080p") { colorResolution = dai::ColorCameraProperties::SensorResolution::THE_1080_P; - }else if (cResolution == "4K"){ + } else if(cResolution == "4K") { colorResolution = dai::ColorCameraProperties::SensorResolution::THE_4_K; } colorCam->setResolution(colorResolution); - if (cResolution == "1080p"){ + if(cResolution == "1080p") { colorCam->setVideoSize(1920, 1080); - }else{ + } else { colorCam->setVideoSize(3840, 2160); } - + colorCam->setPreviewSize(previewWidth, previewHeight); colorCam->setInterleaved(false); colorCam->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); @@ -100,27 +106,25 @@ std::tuple createPipeline(bool lrcheck, bool extended, auto xlinkPreviewOut = pipeline.create(); xlinkPreviewOut->setStreamName("preview"); - if(usePreview){ + if(usePreview) { colorCam->preview.link(xlinkPreviewOut->input); } - + auto xlinkVideoOut = pipeline.create(); xlinkVideoOut->setStreamName("video"); xlinkVideoOut->input.setQueueSize(1); - if(useVideo){ + if(useVideo) { colorCam->video.link(xlinkVideoOut->input); } return std::make_tuple(pipeline, width, height); } - -int main(int argc, char** argv){ - +int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("rgb_stereo_node"); - + std::string tfPrefix, monoResolution, colorResolution; bool lrcheck, extended, subpixel; bool useVideo, usePreview, useDepth; @@ -131,9 +135,9 @@ int main(int argc, char** argv){ node->declare_parameter("lrcheck", true); node->declare_parameter("extended", false); node->declare_parameter("subpixel", true); - node->declare_parameter("confidence", 200); - node->declare_parameter("LRchecktresh", 5); - node->declare_parameter("monoResolution", "720p"); + node->declare_parameter("confidence", 200); + node->declare_parameter("LRchecktresh", 5); + node->declare_parameter("monoResolution", "720p"); node->declare_parameter("colorResolution", "1080p"); node->declare_parameter("useVideo", true); node->declare_parameter("usePreview", false); @@ -144,10 +148,10 @@ int main(int argc, char** argv){ node->declare_parameter("floodLightmA", 0.0f); node->get_parameter("tf_prefix", tfPrefix); - node->get_parameter("lrcheck", lrcheck); - node->get_parameter("extended", extended); - node->get_parameter("subpixel", subpixel); - node->get_parameter("confidence", confidence); + node->get_parameter("lrcheck", lrcheck); + node->get_parameter("extended", extended); + node->get_parameter("subpixel", subpixel); + node->get_parameter("confidence", confidence); node->get_parameter("LRchecktresh", LRchecktresh); node->get_parameter("monoResolution", monoResolution); node->get_parameter("colorResolution", colorResolution); @@ -160,23 +164,21 @@ int main(int argc, char** argv){ node->get_parameter("floodLightmA", floodLightmA); int colorWidth, colorHeight; - if(colorResolution == "1080p"){ + if(colorResolution == "1080p") { colorWidth = 1920; colorHeight = 1080; - }else if (colorResolution == "4K"){ + } else if(colorResolution == "4K") { colorWidth = 3840; colorHeight = 2160; - }else{ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), - "Invalid parameter. -> colorResolution: %s", colorResolution.c_str()); + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Invalid parameter. -> colorResolution: %s", colorResolution.c_str()); throw std::runtime_error("Invalid color camera resolution."); } dai::Pipeline pipeline; int monoWidth, monoHeight; - std::tie(pipeline, monoWidth, monoHeight) = createPipeline(lrcheck, extended, subpixel, confidence , LRchecktresh, - useVideo, usePreview, previewWidth, previewHeight, - monoResolution, colorResolution); + std::tie(pipeline, monoWidth, monoHeight) = createPipeline( + lrcheck, extended, subpixel, confidence, LRchecktresh, useVideo, usePreview, previewWidth, previewHeight, monoResolution, colorResolution); dai::Device device(pipeline); auto videoQueue = device.getOutputQueue("video", 30, false); @@ -186,30 +188,26 @@ int main(int argc, char** argv){ auto calibrationHandler = device.readCalibration(); auto boardName = calibrationHandler.getEepromData().boardName; - if (monoHeight > 480 && boardName == "OAK-D-LITE") { + if(monoHeight > 480 && boardName == "OAK-D-LITE") { monoWidth = 640; monoHeight = 480; } - #ifdef IS_GALACTIC - // Parameter events for OAK-D-PRO + // Parameter events for OAK-D-PRO std::shared_ptr param_subscriber; std::shared_ptr dot_cb_handle, flood_cb_handle; - auto cb = [node, &device](const rclcpp::Parameter & p) { - if (p.get_name() == std::string("dotProjectormA")) - { + auto cb = [node, &device](const rclcpp::Parameter& p) { + if(p.get_name() == std::string("dotProjectormA")) { RCLCPP_INFO(node->get_logger(), "Updating Dot Projector current to %f", p.as_double()); device.setIrLaserDotProjectorBrightness(static_cast(p.as_double())); - } - else if (p.get_name() == std::string("floodLightmA")) - { + } else if(p.get_name() == std::string("floodLightmA")) { RCLCPP_INFO(node->get_logger(), "Updating Flood Light current to %f", p.as_double()); device.setIrFloodLightBrightness(static_cast(p.as_double())); } }; - if (boardName.find("PRO") != std::string::npos) { + if(boardName.find("PRO") != std::string::npos) { param_subscriber = std::make_shared(node); dot_cb_handle = param_subscriber->add_parameter_callback("dotProjectormA", cb); @@ -223,24 +221,22 @@ int main(int argc, char** argv){ float dotProjectormATemp, floodLightmATemp; node->get_parameter("dotProjectormA", dotProjectormATemp); node->get_parameter("floodLightmA", floodLightmATemp); - if (dotProjectormATemp != dotProjectormA){ + if(dotProjectormATemp != dotProjectormA) { dotProjectormA = dotProjectormATemp; RCLCPP_INFO(node->get_logger(), "Updating Dot Projector current to %f", dotProjectormA); device.setIrLaserDotProjectorBrightness(static_cast(dotProjectormA)); } - if (floodLightmATemp != floodLightmA){ + if(floodLightmATemp != floodLightmA) { floodLightmA = floodLightmATemp; RCLCPP_INFO(node->get_logger(), "Updating Flood Light current to %f", floodLightmA); device.setIrFloodLightBrightness(static_cast(floodLightmA)); } }; - if (boardName.find("PRO") != std::string::npos) { + if(boardName.find("PRO") != std::string::npos) { timer = node->create_wall_timer(500ms, cb); } -#endif - - +#endif std::unique_ptr> depthPublish, rgbPreviewPublish, rgbPublish; @@ -251,67 +247,64 @@ int main(int argc, char** argv){ auto previewCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, previewWidth, previewHeight); auto videoCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, colorWidth, colorHeight); - if(useDepth) - { - depthPublish = std::make_unique>(stereoQueue, - node, - std::string("stereo/depth"), - std::bind(&dai::rosBridge::ImageConverter::toRosMsg, - &depthConverter, // since the converter has the same frame name - // and image type is also same we can reuse it - std::placeholders::_1, - std::placeholders::_2) , - 30, - stereoCameraInfo, - "stereo"); + if(useDepth) { + depthPublish = std::make_unique>( + stereoQueue, + node, + std::string("stereo/depth"), + std::bind(&dai::rosBridge::ImageConverter::toRosMsg, + &depthConverter, // since the converter has the same frame name + // and image type is also same we can reuse it + std::placeholders::_1, + std::placeholders::_2), + 30, + stereoCameraInfo, + "stereo"); } - if(usePreview) - { - rgbPreviewPublish = std::make_unique>(previewQueue, - node, - std::string("color/preview/image"), - std::bind(&dai::rosBridge::ImageConverter::toRosMsg, - &rgbConverter, // since the converter has the same frame name - // and image type is also same we can reuse it - std::placeholders::_1, - std::placeholders::_2) , - 30, - previewCameraInfo, - "color/preview"); + if(usePreview) { + rgbPreviewPublish = std::make_unique>( + previewQueue, + node, + std::string("color/preview/image"), + std::bind(&dai::rosBridge::ImageConverter::toRosMsg, + &rgbConverter, // since the converter has the same frame name + // and image type is also same we can reuse it + std::placeholders::_1, + std::placeholders::_2), + 30, + previewCameraInfo, + "color/preview"); } - if(useVideo) - { - rgbPublish = std::make_unique>(videoQueue, - node, - std::string("color/video/image"), - std::bind(&dai::rosBridge::ImageConverter::toRosMsg, - &rgbConverter, // since the converter has the same frame name - // and image type is also same we can reuse it - std::placeholders::_1, - std::placeholders::_2) , - 30, - videoCameraInfo, - "color/video"); + if(useVideo) { + rgbPublish = std::make_unique>( + videoQueue, + node, + std::string("color/video/image"), + std::bind(&dai::rosBridge::ImageConverter::toRosMsg, + &rgbConverter, // since the converter has the same frame name + // and image type is also same we can reuse it + std::placeholders::_1, + std::placeholders::_2), + 30, + videoCameraInfo, + "color/video"); } - if(useDepth) - { - depthPublish->addPublisherCallback(); // addPublisherCallback works only when the dataqueue is non blocking. + if(useDepth) { + depthPublish->addPublisherCallback(); // addPublisherCallback works only when the dataqueue is non blocking. } - if(usePreview) - { + if(usePreview) { rgbPreviewPublish->addPublisherCallback(); } - if(useVideo) - { + if(useVideo) { rgbPublish->addPublisherCallback(); } - // We can add the rectified frames also similar to these publishers. + // We can add the rectified frames also similar to these publishers. // Left them out so that users can play with it by adding and removing rclcpp::spin(node); @@ -319,4 +312,3 @@ int main(int argc, char** argv){ return 0; } - diff --git a/depthai_examples/src/rgb_video_subscriber.cpp b/depthai_examples/src/rgb_video_subscriber.cpp index 0772485e..6cd67b2b 100644 --- a/depthai_examples/src/rgb_video_subscriber.cpp +++ b/depthai_examples/src/rgb_video_subscriber.cpp @@ -1,25 +1,24 @@ -#include "rclcpp/rclcpp.hpp" +#include +#include #include -#include -#include +#include "rclcpp/rclcpp.hpp" // Inludes common necessary includes for development using depthai library dai::rosBridge::ImageConverter inputConverter(true); -void rgbCallback(const sensor_msgs::msg::Image::SharedPtr rgbImageMsg){ +void rgbCallback(const sensor_msgs::msg::Image::SharedPtr rgbImageMsg) { cv::Mat rgbImage = inputConverter.rosMsgtoCvMat(*rgbImageMsg); cv::imshow("video", rgbImage); cv::waitKey(1); return; } -int main(int argc, char** argv){ - +int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("rgb_subscriber_node"); - + rclcpp::Subscription::SharedPtr sub = node->create_subscription("rgb_image", 5, &rgbCallback); rclcpp::spin(node); rclcpp::shutdown(); diff --git a/depthai_examples/src/stereo_inertial_publisher.cpp b/depthai_examples/src/stereo_inertial_publisher.cpp index 26d85f4a..ca648bf8 100644 --- a/depthai_examples/src/stereo_inertial_publisher.cpp +++ b/depthai_examples/src/stereo_inertial_publisher.cpp @@ -1,12 +1,12 @@ #include #include +#include #include #include #include -#include -#include #include +#include #include #include "rclcpp/rclcpp.hpp" @@ -15,11 +15,10 @@ #include #include #include -#include #include +#include #include - #include "depthai/depthai.hpp" std::vector usbStrings = {"UNKNOWN", "LOW", "FULL", "HIGH", "SUPER", "SUPER_PLUS"}; @@ -39,7 +38,7 @@ std::tuple createPipeline(bool enableDepth, std::string rgbResolutionStr, int rgbScaleNumerator, int rgbScaleDinominator, - int previewWidth, + int previewWidth, int previewHeight, bool syncNN, std::string nnPath) { @@ -149,56 +148,62 @@ std::tuple createPipeline(bool enableDepth, camRgb->isp.link(xoutRgb->input); // std::cout << (rgbWidth % 2 == 0 && rgbHeight % 3 == 0) << std::endl; - // assert(("Needs Width to be multiple of 2 and height to be multiple of 3 since the Image is NV12 format here.", (rgbWidth % 2 == 0 && rgbHeight % 3 == 0))); - if(rgbWidth % 16 != 0) { + // assert(("Needs Width to be multiple of 2 and height to be multiple of 3 since the Image is NV12 format here.", (rgbWidth % 2 == 0 && rgbHeight % 3 == + // 0))); + if(rgbWidth % 16 != 0) { if(rgbResolution == dai::node::ColorCamera::Properties::SensorResolution::THE_12_MP) { - DEPTHAI_ROS_ERROR_STREAM("DEPTHAI","RGB Camera width should be multiple of 16. Please choose a different scaling factor." - << std::endl - << "Here are the scalng options that works for 12MP with depth aligned" << std::endl - << "4056 x 3040 * 2/13 --> 624 x 468" << std::endl - << "4056 x 3040 * 2/39 --> 208 x 156" << std::endl - << "4056 x 3040 * 2/51 --> 160 x 120" << std::endl - << "4056 x 3040 * 4/13 --> 1248 x 936" << std::endl - << "4056 x 3040 * 4/26 --> 624 x 468" << std::endl - << "4056 x 3040 * 4/29 --> 560 x 420" << std::endl - << "4056 x 3040 * 4/35 --> 464 x 348" << std::endl - << "4056 x 3040 * 4/39 --> 416 x 312" << std::endl - << "4056 x 3040 * 6/13 --> 1872 x 1404" << std::endl - << "4056 x 3040 * 6/39 --> 624 x 468" << std::endl - << "4056 x 3040 * 7/25 --> 1136 x 852" << std::endl - << "4056 x 3040 * 8/26 --> 1248 x 936" << std::endl - << "4056 x 3040 * 8/39 --> 832 x 624" << std::endl - << "4056 x 3040 * 8/52 --> 624 x 468" << std::endl - << "4056 x 3040 * 8/58 --> 560 x 420" << std::endl - << "4056 x 3040 * 10/39 --> 1040 x 780" << std::endl - << "4056 x 3040 * 10/59 --> 688 x 516" << std::endl - << "4056 x 3040 * 12/17 --> 2864 x 2146" << std::endl - << "4056 x 3040 * 12/26 --> 1872 x 1404" << std::endl - << "4056 x 3040 * 12/39 --> 1248 x 936" << std::endl - << "4056 x 3040 * 13/16 --> 3296 x 2470" << std::endl - << "4056 x 3040 * 14/39 --> 1456 x 1092" << std::endl - << "4056 x 3040 * 14/50 --> 1136 x 852" << std::endl - << "4056 x 3040 * 14/53 --> 1072 x 804" << std::endl - << "4056 x 3040 * 16/39 --> 1664 x 1248" << std::endl - << "4056 x 3040 * 16/52 --> 1248 x 936" << std::endl); + DEPTHAI_ROS_ERROR_STREAM("DEPTHAI", + "RGB Camera width should be multiple of 16. Please choose a different scaling factor." + << std::endl + << "Here are the scalng options that works for 12MP with depth aligned" << std::endl + << "4056 x 3040 * 2/13 --> 624 x 468" << std::endl + << "4056 x 3040 * 2/39 --> 208 x 156" << std::endl + << "4056 x 3040 * 2/51 --> 160 x 120" << std::endl + << "4056 x 3040 * 4/13 --> 1248 x 936" << std::endl + << "4056 x 3040 * 4/26 --> 624 x 468" << std::endl + << "4056 x 3040 * 4/29 --> 560 x 420" << std::endl + << "4056 x 3040 * 4/35 --> 464 x 348" << std::endl + << "4056 x 3040 * 4/39 --> 416 x 312" << std::endl + << "4056 x 3040 * 6/13 --> 1872 x 1404" << std::endl + << "4056 x 3040 * 6/39 --> 624 x 468" << std::endl + << "4056 x 3040 * 7/25 --> 1136 x 852" << std::endl + << "4056 x 3040 * 8/26 --> 1248 x 936" << std::endl + << "4056 x 3040 * 8/39 --> 832 x 624" << std::endl + << "4056 x 3040 * 8/52 --> 624 x 468" << std::endl + << "4056 x 3040 * 8/58 --> 560 x 420" << std::endl + << "4056 x 3040 * 10/39 --> 1040 x 780" << std::endl + << "4056 x 3040 * 10/59 --> 688 x 516" << std::endl + << "4056 x 3040 * 12/17 --> 2864 x 2146" << std::endl + << "4056 x 3040 * 12/26 --> 1872 x 1404" << std::endl + << "4056 x 3040 * 12/39 --> 1248 x 936" << std::endl + << "4056 x 3040 * 13/16 --> 3296 x 2470" << std::endl + << "4056 x 3040 * 14/39 --> 1456 x 1092" << std::endl + << "4056 x 3040 * 14/50 --> 1136 x 852" << std::endl + << "4056 x 3040 * 14/53 --> 1072 x 804" << std::endl + << "4056 x 3040 * 16/39 --> 1664 x 1248" << std::endl + << "4056 x 3040 * 16/52 --> 1248 x 936" << std::endl); } else { - DEPTHAI_ROS_ERROR_STREAM("DEPTHAI","RGB Camera width should be multiple of 16. Please choose a different scaling factor."); + DEPTHAI_ROS_ERROR_STREAM("DEPTHAI", "RGB Camera width should be multiple of 16. Please choose a different scaling factor."); } throw std::runtime_error("Adjust RGB Camaera scaling."); } if(rgbWidth > stereoWidth || rgbHeight > stereoHeight) { - DEPTHAI_ROS_WARN_STREAM("DEPTHAI", + DEPTHAI_ROS_WARN_STREAM( + "DEPTHAI", "RGB Camera resolution is heigher than the configured stereo resolution. Upscaling the stereo depth/disparity to match RGB camera resolution."); } else if(rgbWidth > stereoWidth || rgbHeight > stereoHeight) { - DEPTHAI_ROS_WARN_STREAM("DEPTHAI", - "RGB Camera resolution is heigher than the configured stereo resolution. Downscaling the stereo depth/disparity to match RGB camera resolution."); + DEPTHAI_ROS_WARN_STREAM("DEPTHAI", + "RGB Camera resolution is heigher than the configured stereo resolution. Downscaling the stereo depth/disparity to match " + "RGB camera resolution."); } if(enableSpatialDetection) { - if (previewWidth > rgbWidth or previewHeight > rgbHeight) { - DEPTHAI_ROS_ERROR_STREAM("DEPTHAI", "Preview Image size should be smaller than the scaled resolution. Please adjust the scale parameters or the preview size accordingly."); + if(previewWidth > rgbWidth or previewHeight > rgbHeight) { + DEPTHAI_ROS_ERROR_STREAM( + "DEPTHAI", + "Preview Image size should be smaller than the scaled resolution. Please adjust the scale parameters or the preview size accordingly."); throw std::runtime_error("Invalid Image Size"); } @@ -308,11 +313,11 @@ int main(int argc, char** argv) { node->declare_parameter("manualExposure", false); node->declare_parameter("expTime", 20000); node->declare_parameter("sensIso", 800); - - node->declare_parameter("rgbScaleNumerator", 2); + + node->declare_parameter("rgbScaleNumerator", 2); node->declare_parameter("rgbScaleDinominator", 3); - node->declare_parameter("previewWidth", 416); - node->declare_parameter("previewHeight", 416); + node->declare_parameter("previewWidth", 416); + node->declare_parameter("previewHeight", 416); node->declare_parameter("angularVelCovariance", 0.02); node->declare_parameter("linearAccelCovariance", 0.0); @@ -320,13 +325,11 @@ int main(int argc, char** argv) { node->declare_parameter("detectionClassesCount", 80); node->declare_parameter("syncNN", true); - node->declare_parameter("enableDotProjector", false); node->declare_parameter("enableFloodLight", false); node->declare_parameter("dotProjectormA", 200.0); node->declare_parameter("floodLightmA", 200.0); - // updating parameters if defined in launch file. node->get_parameter("mxId", mxId); @@ -358,7 +361,6 @@ int main(int argc, char** argv) { node->get_parameter("previewWidth", previewWidth); node->get_parameter("previewHeight", previewHeight); - node->get_parameter("angularVelCovariance", angularVelCovariance); node->get_parameter("linearAccelCovariance", linearAccelCovariance); node->get_parameter("enableSpatialDetection", enableSpatialDetection); @@ -380,7 +382,7 @@ int main(int argc, char** argv) { node->get_parameter("nnName", nnName); } nnPath = resourceBaseFolder + "/" + nnName; - + if(mode == "depth") { enableDepth = true; } else { @@ -392,25 +394,25 @@ int main(int argc, char** argv) { dai::Pipeline pipeline; int width, height; bool isDeviceFound = false; - std::tie(pipeline, width, height) = createPipeline(enableDepth, - enableSpatialDetection, - lrcheck, - extended, - subpixel, - rectify, - depth_aligned, - stereo_fps, - confidence, - LRchecktresh, - detectionClassesCount, - monoResolution, - rgbResolution, - rgbScaleNumerator, - rgbScaleDinominator, - previewWidth, - previewHeight, - syncNN, - nnPath); + std::tie(pipeline, width, height) = createPipeline(enableDepth, + enableSpatialDetection, + lrcheck, + extended, + subpixel, + rectify, + depth_aligned, + stereo_fps, + confidence, + LRchecktresh, + detectionClassesCount, + monoResolution, + rgbResolution, + rgbScaleNumerator, + rgbScaleDinominator, + previewWidth, + previewHeight, + syncNN, + nnPath); std::shared_ptr device; std::vector availableDevices = dai::Device::getAllAvailableDevices(); @@ -443,12 +445,11 @@ int main(int argc, char** argv) { std::cout << "Device USB status: " << usbStrings[static_cast(device->getUsbSpeed())] << std::endl; } - // Apply camera controls auto controlQueue = device->getInputQueue("control"); - //Set manual exposure - if(manualExposure){ + // Set manual exposure + if(manualExposure) { dai::CameraControl ctrl; ctrl.setManualExposure(expTime, sensIso); controlQueue->send(ctrl); @@ -470,7 +471,6 @@ int main(int argc, char** argv) { height = 480; } - if(boardName.find("PRO") != std::string::npos) { if(enableDotProjector) { device->setIrLaserDotProjectorBrightness(dotProjectormA); @@ -497,13 +497,12 @@ int main(int argc, char** argv) { "imu"); imuPublish.addPublisherCallback(); - + // auto leftCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::LEFT, monoWidth, monoHeight); // auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, monoWidth, monoHeight); // const std::string leftPubName = rectify ? std::string("left/image_rect") : std::string("left/image_raw"); // const std::string rightPubName = rectify ? std::string("right/image_rect") : std::string("right/image_raw"); - dai::rosBridge::ImageConverter rgbConverter(tfPrefix + "_rgb_camera_optical_frame", false); if(enableDepth) { auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, width, height); @@ -567,7 +566,7 @@ int main(int argc, char** argv) { } else { auto leftCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::LEFT, width, height); auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, width, height); - + auto leftQueue = device->getOutputQueue("left", 30, false); auto rightQueue = device->getOutputQueue("right", 30, false); dai::rosBridge::BridgePublisher leftPublish( diff --git a/depthai_examples/src/stereo_publisher.cpp b/depthai_examples/src/stereo_publisher.cpp index 3e45f0a1..356f888e 100644 --- a/depthai_examples/src/stereo_publisher.cpp +++ b/depthai_examples/src/stereo_publisher.cpp @@ -1,62 +1,61 @@ -#include "rclcpp/rclcpp.hpp" - -#include +#include #include -#include +#include +#include #include #include -#include -#include +#include + +#include "rclcpp/rclcpp.hpp" // Inludes common necessary includes for development using depthai library -#include "depthai/depthai.hpp" #include -#include #include +#include +#include "depthai/depthai.hpp" -std::tuple createPipeline(bool withDepth, bool lrcheck, bool extended, bool subpixel, int confidence, int LRchecktresh, std::string resolution){ +std::tuple createPipeline( + bool withDepth, bool lrcheck, bool extended, bool subpixel, int confidence, int LRchecktresh, std::string resolution) { dai::Pipeline pipeline; - dai::node::MonoCamera::Properties::SensorResolution monoResolution; - auto monoLeft = pipeline.create(); - auto monoRight = pipeline.create(); - auto xoutLeft = pipeline.create(); - auto xoutRight = pipeline.create(); - auto stereo = pipeline.create(); - auto xoutDepth = pipeline.create(); + dai::node::MonoCamera::Properties::SensorResolution monoResolution; + auto monoLeft = pipeline.create(); + auto monoRight = pipeline.create(); + auto xoutLeft = pipeline.create(); + auto xoutRight = pipeline.create(); + auto stereo = pipeline.create(); + auto xoutDepth = pipeline.create(); // XLinkOut xoutLeft->setStreamName("left"); xoutRight->setStreamName("right"); - if (withDepth) { + if(withDepth) { xoutDepth->setStreamName("depth"); - } - else { + } else { xoutDepth->setStreamName("disparity"); } int width, height; - if(resolution == "720p"){ - monoResolution = dai::node::MonoCamera::Properties::SensorResolution::THE_720_P; - width = 1280; + if(resolution == "720p") { + monoResolution = dai::node::MonoCamera::Properties::SensorResolution::THE_720_P; + width = 1280; height = 720; - }else if(resolution == "400p" ){ - monoResolution = dai::node::MonoCamera::Properties::SensorResolution::THE_400_P; - width = 640; + } else if(resolution == "400p") { + monoResolution = dai::node::MonoCamera::Properties::SensorResolution::THE_400_P; + width = 640; height = 400; - }else if(resolution == "800p" ){ - monoResolution = dai::node::MonoCamera::Properties::SensorResolution::THE_800_P; - width = 1280; + } else if(resolution == "800p") { + monoResolution = dai::node::MonoCamera::Properties::SensorResolution::THE_800_P; + width = 1280; height = 800; - }else if(resolution == "480p" ){ - monoResolution = dai::node::MonoCamera::Properties::SensorResolution::THE_480_P; - width = 640; + } else if(resolution == "480p") { + monoResolution = dai::node::MonoCamera::Properties::SensorResolution::THE_480_P; + width = 640; height = 480; - }else{ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), - "Invalid parameter. -> monoResolution: %s", resolution.c_str()); + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Invalid parameter. -> monoResolution: %s", resolution.c_str()); throw std::runtime_error("Invalid mono camera resolution."); } @@ -68,7 +67,7 @@ std::tuple createPipeline(bool withDepth, bool lrcheck, // StereoDepth stereo->initialConfig.setConfidenceThreshold(confidence); - stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout + stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout stereo->initialConfig.setLeftRightCheckThreshold(LRchecktresh); stereo->setLeftRightCheck(lrcheck); stereo->setExtendedDisparity(extended); @@ -81,21 +80,19 @@ std::tuple createPipeline(bool withDepth, bool lrcheck, stereo->rectifiedLeft.link(xoutLeft->input); stereo->rectifiedRight.link(xoutRight->input); - if(withDepth){ + if(withDepth) { stereo->depth.link(xoutDepth->input); - } - else{ + } else { stereo->disparity.link(xoutDepth->input); } return std::make_tuple(pipeline, width, height); } -int main(int argc, char** argv){ - +int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("stereo_node"); - + std::string tfPrefix, mode, monoResolution; bool lrcheck, extended, subpixel, enableDepth; int confidence, LRchecktresh; @@ -107,23 +104,22 @@ int main(int argc, char** argv){ node->declare_parameter("lrcheck", true); node->declare_parameter("extended", false); node->declare_parameter("subpixel", true); - node->declare_parameter("confidence", 200); - node->declare_parameter("LRchecktresh", 5); - node->declare_parameter("monoResolution", "720p"); - - node->get_parameter("tf_prefix", tfPrefix); - node->get_parameter("mode", mode); - node->get_parameter("lrcheck", lrcheck); - node->get_parameter("extended", extended); - node->get_parameter("subpixel", subpixel); - node->get_parameter("confidence", confidence); - node->get_parameter("LRchecktresh", LRchecktresh); + node->declare_parameter("confidence", 200); + node->declare_parameter("LRchecktresh", 5); + node->declare_parameter("monoResolution", "720p"); + + node->get_parameter("tf_prefix", tfPrefix); + node->get_parameter("mode", mode); + node->get_parameter("lrcheck", lrcheck); + node->get_parameter("extended", extended); + node->get_parameter("subpixel", subpixel); + node->get_parameter("confidence", confidence); + node->get_parameter("LRchecktresh", LRchecktresh); node->get_parameter("monoResolution", monoResolution); - if(mode == "depth"){ + if(mode == "depth") { enableDepth = true; - } - else{ + } else { enableDepth = false; } @@ -132,78 +128,72 @@ int main(int argc, char** argv){ auto leftQueue = device.getOutputQueue("left", 30, false); auto rightQueue = device.getOutputQueue("right", 30, false); std::shared_ptr stereoQueue; - if (enableDepth) { + if(enableDepth) { stereoQueue = device.getOutputQueue("depth", 30, false); - }else{ + } else { stereoQueue = device.getOutputQueue("disparity", 30, false); } auto calibrationHandler = device.readCalibration(); auto boardName = calibrationHandler.getEepromData().boardName; - if (monoHeight > 480 && boardName == "OAK-D-LITE") { + if(monoHeight > 480 && boardName == "OAK-D-LITE") { monoWidth = 640; monoHeight = 480; } dai::rosBridge::ImageConverter converter(tfPrefix + "_left_camera_optical_frame", true); - auto leftCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::LEFT, monoWidth, monoHeight); - dai::rosBridge::BridgePublisher leftPublish(leftQueue, - node, - std::string("left/image_rect"), - std::bind(&dai::rosBridge::ImageConverter::toRosMsg, - &converter, - std::placeholders::_1, - std::placeholders::_2) , - 30, - leftCameraInfo, - "left"); + auto leftCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::LEFT, monoWidth, monoHeight); + dai::rosBridge::BridgePublisher leftPublish( + leftQueue, + node, + std::string("left/image_rect"), + std::bind(&dai::rosBridge::ImageConverter::toRosMsg, &converter, std::placeholders::_1, std::placeholders::_2), + 30, + leftCameraInfo, + "left"); leftPublish.addPublisherCallback(); dai::rosBridge::ImageConverter rightconverter(tfPrefix + "_right_camera_optical_frame", true); - auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, monoWidth, monoHeight); - - dai::rosBridge::BridgePublisher rightPublish(rightQueue, - node, - std::string("right/image_rect"), - std::bind(&dai::rosBridge::ImageConverter::toRosMsg, - &rightconverter, - std::placeholders::_1, - std::placeholders::_2) , - 30, - rightCameraInfo, - "right"); + auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, monoWidth, monoHeight); + + dai::rosBridge::BridgePublisher rightPublish( + rightQueue, + node, + std::string("right/image_rect"), + std::bind(&dai::rosBridge::ImageConverter::toRosMsg, &rightconverter, std::placeholders::_1, std::placeholders::_2), + 30, + rightCameraInfo, + "right"); rightPublish.addPublisherCallback(); - if(mode == "depth"){ - dai::rosBridge::BridgePublisher depthPublish(stereoQueue, - node, - std::string("stereo/depth"), - std::bind(&dai::rosBridge::ImageConverter::toRosMsg, - &rightconverter, // since the converter has the same frame name - // and image type is also same we can reuse it - std::placeholders::_1, - std::placeholders::_2) , - 30, - rightCameraInfo, - "stereo"); + if(mode == "depth") { + dai::rosBridge::BridgePublisher depthPublish( + stereoQueue, + node, + std::string("stereo/depth"), + std::bind(&dai::rosBridge::ImageConverter::toRosMsg, + &rightconverter, // since the converter has the same frame name + // and image type is also same we can reuse it + std::placeholders::_1, + std::placeholders::_2), + 30, + rightCameraInfo, + "stereo"); depthPublish.addPublisherCallback(); rclcpp::spin(node); - } - else{ + } else { dai::rosBridge::DisparityConverter dispConverter(tfPrefix + "_right_camera_optical_frame", 880, 7.5, 20, 2000); - dai::rosBridge::BridgePublisher dispPublish(stereoQueue, - node, - std::string("stereo/disparity"), - std::bind(&dai::rosBridge::DisparityConverter::toRosMsg, - &dispConverter, - std::placeholders::_1, - std::placeholders::_2) , - 30, - rightCameraInfo, - "stereo"); + dai::rosBridge::BridgePublisher dispPublish( + stereoQueue, + node, + std::string("stereo/disparity"), + std::bind(&dai::rosBridge::DisparityConverter::toRosMsg, &dispConverter, std::placeholders::_1, std::placeholders::_2), + 30, + rightCameraInfo, + "stereo"); dispPublish.addPublisherCallback(); rclcpp::spin(node); } diff --git a/depthai_ros_msgs/CMakeLists.txt b/depthai_ros_msgs/CMakeLists.txt index 6c56c7dd..8e8800b0 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 +cmake_minimum_required(VERSION 3.10.2) # CMake version in Ubuntu 18.04 LTS -project (depthai_ros_msgs VERSION 2.5.3) +project(depthai_ros_msgs VERSION 2.5.3) if(POLICY CMP0057) cmake_policy(SET CMP0057 NEW) diff --git a/depthai_ros_msgs/package.xml b/depthai_ros_msgs/package.xml index 2159c194..c051021a 100644 --- a/depthai_ros_msgs/package.xml +++ b/depthai_ros_msgs/package.xml @@ -8,8 +8,7 @@ Sachin Guruswamy MIT - - catkin + ament_cmake builtin_interfaces diff --git a/entrypoint.sh b/entrypoint.sh new file mode 100755 index 00000000..6a57bc77 --- /dev/null +++ b/entrypoint.sh @@ -0,0 +1,7 @@ +#!/bin/bash +set -e + +# setup ros environment +source "/opt/ros/$ROS_DISTRO/setup.bash" +source "/ws/install/setup.bash" +exec "$@" \ No newline at end of file