diff --git a/aloha_ws/src/apriltag_ros/.github/build_and_test.yml b/aloha_ws/src/apriltag_ros/.github/build_and_test.yml new file mode 100644 index 00000000..26d157ca --- /dev/null +++ b/aloha_ws/src/apriltag_ros/.github/build_and_test.yml @@ -0,0 +1,48 @@ +name: ROS2 CI + +on: + pull_request: + branches: + - 'ros2' + push: + branches: + - 'ros2' + +jobs: + test_environment: + runs-on: [ubuntu-latest] + strategy: + fail-fast: false + matrix: + ros_distribution: + - foxy + - galactic + - rolling + include: + # Foxy Fitzroy (June 2020 - May 2023) + - docker_image: rostooling/setup-ros-docker:ubuntu-focal-ros-foxy-ros-base-latest + ros_distribution: foxy + ros_version: 2 + # Galactic Geochelone (May 2021 - November 2022) + - docker_image: rostooling/setup-ros-docker:ubuntu-focal-ros-galactic-ros-base-latest + ros_distribution: galactic + ros_version: 2 + # Rolling Ridley (June 2020 - Present) + - docker_image: rostooling/setup-ros-docker:ubuntu-focal-ros-rolling-ros-base-latest + ros_distribution: rolling + ros_version: 2 + container: + image: ${{ matrix.docker_image }} + steps: + - name: setup directories + run: mkdir -p ros_ws/src + - name: checkout + uses: actions/checkout@v2 + with: + path: ros_ws/src + - name: build and test + uses: ros-tooling/action-ros-ci@master + with: + package-name: apriltag_ros + target-ros2-distro: ${{ matrix.ros_distribution }} + vcs-repo-file-url: "" diff --git a/aloha_ws/src/apriltag_ros/.github/workflows/ros_ci.yml b/aloha_ws/src/apriltag_ros/.github/workflows/ros_ci.yml new file mode 100644 index 00000000..83d7fd48 --- /dev/null +++ b/aloha_ws/src/apriltag_ros/.github/workflows/ros_ci.yml @@ -0,0 +1,24 @@ +name: ROS-CI +on: [push, pull_request] + +jobs: + CI: + strategy: + matrix: + env: + - {ROS_DISTRO: melodic} + - {ROS_DISTRO: noetic} + env: + CCACHE_DIR: /github/home/.ccache # Enable ccache + PRERELEASE: true + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + # This step will fetch/store the directory used by ccache before/after the ci run + - uses: actions/cache@v2 + with: + path: ${{ env.CCACHE_DIR }} + key: ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }} + # Run industrial_ci + - uses: 'ros-industrial/industrial_ci@master' + env: ${{ matrix.env }} diff --git a/aloha_ws/src/apriltag_ros/.gitignore b/aloha_ws/src/apriltag_ros/.gitignore new file mode 100644 index 00000000..c5b0f2dd --- /dev/null +++ b/aloha_ws/src/apriltag_ros/.gitignore @@ -0,0 +1,3 @@ +**/matlab_rosbag-0.5.0-linux64/ +**/*.bag +.vscode/ diff --git a/aloha_ws/src/apriltag_ros/.travis.yml b/aloha_ws/src/apriltag_ros/.travis.yml new file mode 100644 index 00000000..26924b05 --- /dev/null +++ b/aloha_ws/src/apriltag_ros/.travis.yml @@ -0,0 +1,21 @@ +# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). +# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) + +language: generic # optional, just removes the language badge +services: + - docker +cache: + directories: + - $HOME/.ccache +git: + quiet: true # optional, silences the cloning of the target repository +env: + global: + - CCACHE_DIR=$HOME/.ccache # enables C/C++ caching in industrial_ci + matrix: # each line is a job + - ROS_DISTRO="melodic" + - ROS_DISTRO="noetic" +install: + - git clone --quiet --depth 1 https://github.com/ros-industrial/industrial_ci.git .industrial_ci +script: + - .industrial_ci/travis.sh diff --git a/aloha_ws/src/apriltag_ros/CONTRIBUTING.md b/aloha_ws/src/apriltag_ros/CONTRIBUTING.md new file mode 100644 index 00000000..5c47685a --- /dev/null +++ b/aloha_ws/src/apriltag_ros/CONTRIBUTING.md @@ -0,0 +1,3 @@ +Any contribution that you make to this repository will +be under the BSD 2-Clause Simplified License, as dictated by that +[license](https://opensource.org/licenses/BSD-2-Clause). diff --git a/aloha_ws/src/apriltag_ros/LICENSE b/aloha_ws/src/apriltag_ros/LICENSE new file mode 100644 index 00000000..23c69410 --- /dev/null +++ b/aloha_ws/src/apriltag_ros/LICENSE @@ -0,0 +1,27 @@ +All rights reserved. + +Software License Agreement (BSD 2-Clause Simplified License) + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above + copyright notice, this list of conditions and the following + disclaimer in the documentation and/or other materials provided + with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/aloha_ws/src/apriltag_ros/README.md b/aloha_ws/src/apriltag_ros/README.md new file mode 100644 index 00000000..988140ec --- /dev/null +++ b/aloha_ws/src/apriltag_ros/README.md @@ -0,0 +1,91 @@ +# apriltag_ros + +`apriltag_ros` is a Robot Operating System (ROS) wrapper of the [AprilTag 3 visual fiducial detector](https://april.eecs.umich.edu/software/apriltag.html). For details and tutorials, please see the [ROS wiki](http://wiki.ros.org/apriltag_ros). + +`apriltag_ros` depends on the latest release of the [AprilTag library](https://github.com/AprilRobotics/apriltag). Clone it into your catkin workspace before building. + +**Authors**: Danylo Malyuta, Wolfgang Merkt + +**Maintainers**: [Danylo Malyuta](mailto:danylo.malyuta@gmail.com) ([Autonomous Control Laboratory](https://www.aa.washington.edu/research/acl), University of Washington), [Wolfgang Merkt](https://github.com/wxmerkt) + +## Quickstart + +Starting with a working ROS installation (Kinetic and Melodic are supported): +``` +export ROS_DISTRO=melodic # Set this to your distro, e.g. kinetic or melodic +source /opt/ros/$ROS_DISTRO/setup.bash # Source your ROS distro +mkdir -p ~/catkin_ws/src # Make a new workspace +cd ~/catkin_ws/src # Navigate to the source space +git clone https://github.com/AprilRobotics/apriltag.git # Clone Apriltag library +git clone https://github.com/AprilRobotics/apriltag_ros.git # Clone Apriltag ROS wrapper +cd ~/catkin_ws # Navigate to the workspace +rosdep install --from-paths src --ignore-src -r -y # Install any missing packages +catkin build # Build all packages in the workspace (catkin_make_isolated will work also) +``` +See the [ROS wiki](http://wiki.ros.org/apriltag_ros) for details and tutorials. + +## Tag Size Definition + +For a correct depth estimation (and hence the correct full pose) it is necessary to specify the tag size in config/tags.yaml correctly. In the [Wiki for the AprilTag Library](https://github.com/AprilRobotics/apriltag/wiki/AprilTag-User-Guide#pose-estimation) the correct interpretation of the term "tag size" is explained. The size is defined by the length of the black/white border between the complete black and white rectangle of any tag type. Note that for apriltag3 marker families this does not in fact correspond to the outside of the marker. + +Below is a visualization of the tag size (red arrow) to be specified for the most common tag classes: +![Tag Size Guide](./apriltag_ros/docs/tag_size_guide.svg) + +## Contributing + +Pull requests are welcome! Especially for the following areas: + +- Publishing of the AprilTag 3 algorithm intermediate images over a ROS image topic (which AprilTag 3 already generates when `tag_debug==1`) +- Conversion of the bundle calibration script from MATLAB to Python +- Extend calibration to support calibrating tags that cannot appear simultaneously with the master tag, but do appear simultaneously with other tags which themselves or via a similar relationship appear with the master tag (e.g. a bundle with the geometry of a cube - if the master is on one face, tags on the opposite face cannot currently be calibrated). This is basically "transform chaining" and potentially allows calibration of bundles with arbitrary geometry as long as a transform chain exists from any tag to the master tag +- Supporting multiple tag family detection (currently all tags have to be of the same family). This means calling the detector once for each family. Because the core AprilTag 2 algorithm is the performance bottleneck, detection of `n` tag families will possibly decrease performance by `1/n` (tbd if this still holds for v3) + +## Changelog + +- In March 2019, the code was upgraded to AprilTag 3 and as thus the options `refine_pose`, `refine_decode`, and `black_border` were removed. + +## Copyright + +The source code in `apriltag_ros/` is original code that is the ROS wrapper itself, see the [LICENSE](https://github.com/AprilRobotics/apriltag_ros/blob/526b9455121ae0bb6b4c1c3db813f0fbdf78393c/LICENSE). It is inspired by [apriltags_ros](https://github.com/RIVeR-Lab/apriltags_ros) and provides a superset of its functionalities. + +If you use this code, please kindly inform [Danylo Malyuta](mailto:danylo.malyuta@gmail.com) (to maintain a list here of research works that have benefited from the code) and cite: + +- D. Malyuta, C. Brommer, D. Hentzen, T. Stastny, R. Siegwart, and R. Brockers, “[Long-duration fully autonomous operation of rotorcraft unmanned aerial systems for remote-sensing data acquisition](https://onlinelibrary.wiley.com/doi/abs/10.1002/rob.21898),” Journal of Field Robotics, p. arXiv:1908.06381, Aug. 2019. +- C. Brommer, D. Malyuta, D. Hentzen, and R. Brockers, “[Long-duration autonomy for small rotorcraft UAS including recharging](https://ieeexplore.ieee.org/document/8594111),” in IEEE/RSJ International Conference on Intelligent Robots and Systems, IEEE, p. arXiv:1810.05683, oct 2018. +- J. Wang and E. Olson, "[AprilTag 2: Efficient and robust fiducial detection](http://ieeexplore.ieee.org/document/7759617/)," in ''Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)'', October 2016. + +``` +@article{Malyuta2019, + doi = {10.1002/rob.21898}, + url = {https://doi.org/10.1002/rob.21898}, + pages = {arXiv:1908.06381}, + year = {2019}, + month = aug, + publisher = {Wiley}, + author = {Danylo Malyuta and Christian Brommer and Daniel Hentzen and Thomas Stastny and Roland Siegwart and Roland Brockers}, + title = {Long-duration fully autonomous operation of rotorcraft unmanned aerial systems for remote-sensing data acquisition}, + journal = {Journal of Field Robotics} +} +@inproceedings{Brommer2018, + doi = {10.1109/iros.2018.8594111}, + url = {https://doi.org/10.1109/iros.2018.8594111}, + pages = {arXiv:1810.05683}, + year = {2018}, + month = {oct}, + publisher = {{IEEE}}, + author = {Christian Brommer and Danylo Malyuta and Daniel Hentzen and Roland Brockers}, + title = {Long-Duration Autonomy for Small Rotorcraft {UAS} Including Recharging}, + booktitle = {{IEEE}/{RSJ} International Conference on Intelligent Robots and Systems} +} +@inproceedings{Wang2016, + author = {Wang, John and Olson, Edwin}, + booktitle = {2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, + doi = {10.1109/IROS.2016.7759617}, + isbn = {978-1-5090-3762-9}, + month = {oct}, + pages = {4193--4198}, + publisher = {IEEE}, + title = {{AprilTag 2: Efficient and robust fiducial detection}}, + year = {2016} +} +``` diff --git a/aloha_ws/src/apriltag_ros/apriltag_ros/CHANGELOG.rst b/aloha_ws/src/apriltag_ros/apriltag_ros/CHANGELOG.rst new file mode 100644 index 00000000..54420d41 --- /dev/null +++ b/aloha_ws/src/apriltag_ros/apriltag_ros/CHANGELOG.rst @@ -0,0 +1,43 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package apriltag_ros +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +3.2.1 (2022-04-05) +------------------ +* Fixed propagation of apriltag and Eigen headers and libraries (`#124 `_) +* Drop old C++11 as it breaks with new log4cxx. +* Contributors: Jochen Sprickerhof, Remo Diethelm, Wolfgang Merkt + +3.2.0 (2022-03-10) +------------------ +* Add transport hint option (`#108 `_) +* Move to using the apriltag CMake target (`#104 `_) +* Set the tag's parent frame to the camera optical frame (`#101 `_) +* Fix bug in K matrix in single_image_client (`#103 `_) +* Add configurable max_hamming_distance for the AprilTag Detector (`#93 `_) +* Introduce lazy processing for ContinuousDetector (`#80 `_) +* Contributors: Akshay Prasad, Amal Nanavati, Christian Rauch, Hongzhuo Liang, Wolfgang Merkt + +3.1.2 (2020-07-15) +------------------ +* Add support for tagCircle21h7, tagCircle49h12 (`#69 `_) +* Add support for tagCustom48h12 (`#65 `_) +* Contributors: Anthony Biviano, Kyle Saltmarsh, Wolfgang Merkt, kai wu + +3.1.1 (2019-10-07) +------------------ +* Add support for tagStandard41h12 and tagStandard52h13 (`#63 `_, `#59 `_). +* Add gray image input support (`#58 `_). +* Contributors: Alexander Reimann, Moritz Zimmermann, Samuel Bachmann, Wolfgang Merkt + +3.1.0 (2019-05-25) +------------------ +* Prepare for release (3.1) and fix catkin_lint errors +* Upgrade to AprilTag 3, fix installation, and performance improvements (`#43 `_) + Upgrade to AprilTag 3, fix installation, and performance improvements +* Rename package to apriltag_ros + - Relates to https://github.com/AprilRobotics/apriltag_ros/issues/45 +* Contributors: Wolfgang Merkt + +1.0.0 (2018-05-14) +------------------ diff --git a/aloha_ws/src/apriltag_ros/apriltag_ros/CMakeLists.txt b/aloha_ws/src/apriltag_ros/apriltag_ros/CMakeLists.txt new file mode 100644 index 00000000..815eaded --- /dev/null +++ b/aloha_ws/src/apriltag_ros/apriltag_ros/CMakeLists.txt @@ -0,0 +1,127 @@ +cmake_minimum_required(VERSION 3.5) +project(apriltag_ros) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(Eigen3 REQUIRED) +find_package(OpenCV REQUIRED) +find_package(apriltag REQUIRED) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +# We default to 'Release' if none was specified +if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + message(STATUS "Setting build type to 'Release' as none was specified.") + set(CMAKE_BUILD_TYPE Release CACHE STRING "Choose the build type" FORCE) + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Coverage" "Debug" "Release" "MinSizeRel" "RelWithDebInfo") +endif() + +add_compile_options( + "-O3" "-funsafe-loop-optimizations" "-fsee" "-funroll-loops" "-fno-math-errno" + "-funsafe-math-optimizations" "-ffinite-math-only" "-fno-signed-zeros") + +# Note: These options have been turned off to allow for binary releases - +# in local builds, they can be reactivated to achieve higher performance. +# if("${CMAKE_SYSTEM_PROCESSOR}" MATCHES "x86_64" OR "x86_32") +# message("enabling msse2 for x86_64 or x86_32 architecture") +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native -msse2 ") +# endif() +# if("${CMAKE_SYSTEM_PROCESSOR}" MATCHES "arm") +# message("enabling -mfpu=neon -mfloat-abi=softfp for ARM architecture") +# set(CMAKE_CXX_FLAGS +# "${CMAKE_CXX_FLAGS} -march=armv7-a -mcpu=cortex-a9 -mfpu=neon -mtune=cortex-a9 -mvectorize-with-neon-quad -ffast-math ") +# endif() + + +find_package(rosidl_default_generators REQUIRED) +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/AprilTagDetection.msg" + "msg/AprilTagDetectionArray.msg" + "srv/AnalyzeSingleImage.srv" + DEPENDENCIES + geometry_msgs sensor_msgs std_msgs +) + +include_directories(include + ${EIGEN3_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} +) + +ament_auto_add_library(${PROJECT_NAME}_common SHARED + src/common_functions.cpp +) +target_link_libraries(${PROJECT_NAME}_common + ${OpenCV_LIBRARIES} + apriltag::apriltag +) +if(${tf2_geometry_msgs_VERSION} VERSION_GREATER_EQUAL 0.18.0) + target_compile_definitions(${PROJECT_NAME}_common PRIVATE + USE_TF2_GEOMETRY_MSGS_HPP_HEADER=1 + ) +endif() + +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(${PROJECT_NAME}_common + ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target( + cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(${PROJECT_NAME}_common "${cpp_typesupport_target}") +endif() + +ament_auto_add_library(${PROJECT_NAME}_continuous_detector SHARED + src/continuous_detector.cpp +) +target_link_libraries(${PROJECT_NAME}_continuous_detector + ${PROJECT_NAME}_common +) +rclcpp_components_register_node(${PROJECT_NAME}_continuous_detector + PLUGIN "apriltag_ros::ContinuousDetector" + EXECUTABLE ${PROJECT_NAME}_continuous_detector_node +) + +ament_auto_add_library(${PROJECT_NAME}_single_image_detector SHARED + src/single_image_detector.cpp +) +target_link_libraries(${PROJECT_NAME}_single_image_detector + ${PROJECT_NAME}_common +) +rclcpp_components_register_node(${PROJECT_NAME}_single_image_detector + PLUGIN "apriltag_ros::SingleImageDetector" + EXECUTABLE ${PROJECT_NAME}_single_image_detector_node +) + +ament_auto_add_library(${PROJECT_NAME}_single_image_client SHARED + src/single_image_client.cpp +) +rclcpp_components_register_node(${PROJECT_NAME}_single_image_client + PLUGIN "apriltag_ros::SingleImageClient" + EXECUTABLE ${PROJECT_NAME}_single_image_client_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + config + launch + scripts +) diff --git a/aloha_ws/src/apriltag_ros/apriltag_ros/config/settings.param.yaml b/aloha_ws/src/apriltag_ros/apriltag_ros/config/settings.param.yaml new file mode 100644 index 00000000..744a1424 --- /dev/null +++ b/aloha_ws/src/apriltag_ros/apriltag_ros/config/settings.param.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + # AprilTag 3 code parameters + # Find descriptions in apriltag/include/apriltag.h:struct apriltag_detector + # apriltag/include/apriltag.h:struct apriltag_family + tag_family: 'tag36h11' # options: tagStandard52h13, tagStandard41h12, tag36h11, tag25h9, tag16h5, tagCustom48h12, tagCircle21h7, tagCircle49h12 + tag_threads: 2 # default: 2 + tag_decimate: 1.0 # default: 1.0 + tag_blur: 0.0 # default: 0.0 + tag_refine_edges: 1 # default: 1 + tag_debug: 0 # default: 0 + max_hamming_dist: 2 # default: 2 (Tunable parameter with 2 being a good choice - values >=3 consume large amounts of memory. Choose the largest value possible.) + # Other parameters + publish_tf: true # default: false + transport_hint: "raw" # default: raw, see http://wiki.ros.org/image_transport#Known_Transport_Packages for options diff --git a/aloha_ws/src/apriltag_ros/apriltag_ros/config/tags.param.yaml b/aloha_ws/src/apriltag_ros/apriltag_ros/config/tags.param.yaml new file mode 100644 index 00000000..d51be003 --- /dev/null +++ b/aloha_ws/src/apriltag_ros/apriltag_ros/config/tags.param.yaml @@ -0,0 +1,59 @@ +/**: + ros__parameters: + # # Definitions of tags to detect + # + # ## General remarks + # + # - All length in meters + # - Ellipsis (...) signifies that the previous element can be repeated multiple times. + # + # ## Standalone tag definitions + # ### Remarks + # + # + # ### Syntax + # + # standalone_tags: + # tag_names: ["TAG_NAME", ...] + # TAG_NAME: + # id: ID + # size: SIZE + # ... + # + standalone_tags: + tag_names: ["tag_3"] + tag_3: + id: 3 + size: 1.0 + + # ## Tag bundle definitions + # ### Remarks + # + # - x, y, z have default values of 0 thus they are optional + # - qw has default value of 1 and qx, qy, qz have default values of 0 thus they are optional + # + # ### Syntax + # + # tag_bundles: + # bundle_names: ["CUSTOM_BUNDLE_NAME", ...] + # CUSTOM_BUNDLE_NAME: + # layout: + # ids: [ID, ...] + # ID: + # size: SIZE + # x: X_POS + # y: Y_POS + # z: Z_POS + # qw: QUAT_W_VAL + # qx: QUAT_X_VAL + # qy: QUAT_Y_VAL + # qz: QUAT_Z_VAL + # ... + # ... + tag_bundles: + bundle_names: ["bundle_0"] + bundle_0: + layout: + ids: [3] + 3: + size: 1.0 diff --git a/aloha_ws/src/apriltag_ros/apriltag_ros/config/tags.yaml b/aloha_ws/src/apriltag_ros/apriltag_ros/config/tags.yaml new file mode 100644 index 00000000..d24f148d --- /dev/null +++ b/aloha_ws/src/apriltag_ros/apriltag_ros/config/tags.yaml @@ -0,0 +1,48 @@ +# # Definitions of tags to detect +# +# ## General remarks +# +# - All length in meters +# - 'size' refers to the length of the shared border between solid black and solid white rectangle. +# See README.md or https://github.com/AprilRobotics/apriltag/wiki/AprilTag-User-Guide#pose-estimation for details. +# - Ellipsis (...) signifies that the previous element can be repeated multiple times. +# +# ## Standalone tag definitions +# ### Remarks +# +# - name is optional +# +# ### Syntax +# +# standalone_tags: +# [ +# {id: ID, size: SIZE, name: NAME}, +# ... +# ] +standalone_tags: + [ + ] +# ## Tag bundle definitions +# ### Remarks +# +# - name is optional +# - x, y, z have default values of 0 thus they are optional +# - qw has default value of 1 and qx, qy, qz have default values of 0 thus they are optional +# +# ### Syntax +# +# tag_bundles: +# [ +# { +# name: 'CUSTOM_BUNDLE_NAME', +# layout: +# [ +# {id: ID, size: SIZE, x: X_POS, y: Y_POS, z: Z_POS, qw: QUAT_W_VAL, qx: QUAT_X_VAL, qy: QUAT_Y_VAL, qz: QUAT_Z_VAL}, +# ... +# ] +# }, +# ... +# ] +tag_bundles: + [ + ] diff --git a/aloha_ws/src/apriltag_ros/apriltag_ros/docs/tag_size_guide.svg b/aloha_ws/src/apriltag_ros/apriltag_ros/docs/tag_size_guide.svg new file mode 100644 index 00000000..2dbda28d --- /dev/null +++ b/aloha_ws/src/apriltag_ros/apriltag_ros/docs/tag_size_guide.svg @@ -0,0 +1,224 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + image/svg+xml + + + + + + + + + Marker Size definition for apriltag_ros + + + + + + diff --git a/aloha_ws/src/apriltag_ros/apriltag_ros/include/apriltag_ros/common_functions.hpp b/aloha_ws/src/apriltag_ros/apriltag_ros/include/apriltag_ros/common_functions.hpp new file mode 100644 index 00000000..32f8ff24 --- /dev/null +++ b/aloha_ws/src/apriltag_ros/apriltag_ros/include/apriltag_ros/common_functions.hpp @@ -0,0 +1,242 @@ +// Copyright (c) 2017, California Institute of Technology. +// All rights reserved. +// +// Software License Agreement (BSD 2-Clause Simplified License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// The views and conclusions contained in the software and documentation are +// those of the authors and should not be interpreted as representing official +// policies, either expressed or implied, of the California Institute of +// Technology. +/** + ** common_functions.hpp ******************************************************* + * + * Wrapper classes for AprilTag standalone and bundle detection. Main function + * is TagDetector::detectTags which wraps the call to core AprilTag 2 + * algorithm, apriltag_detector_detect(). + * + * $Revision: 1.0 $ + * $Date: 2017/12/17 13:23:14 $ + * $Author: dmalyuta $ + * + * Originator: Danylo Malyuta, JPL + ******************************************************************************/ + +#ifndef APRILTAG_ROS__COMMON_FUNCTIONS_HPP_ +#define APRILTAG_ROS__COMMON_FUNCTIONS_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include "eigen3/Eigen/Dense" +#include "eigen3/Eigen/Geometry" + +#include "opencv2/opencv.hpp" +#include "opencv2/imgproc/imgproc.hpp" +#include "opencv2/core/core.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "cv_bridge/cv_bridge.h" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "image_transport/image_transport.hpp" +#include "sensor_msgs/image_encodings.hpp" +#include "tf2_ros/transform_broadcaster.h" + +#include "apriltag_ros/msg/april_tag_detection.hpp" +#include "apriltag_ros/msg/april_tag_detection_array.hpp" + +namespace apriltag_ros +{ + +// Stores the properties of a tag member of a bundle +struct TagBundleMember +{ + int id; // Payload ID + double size; // [m] Side length + cv::Matx44d T_oi; // Rigid transform from tag i frame to bundle origin frame +}; + +class StandaloneTagDescription +{ +public: + StandaloneTagDescription() {} + StandaloneTagDescription( + int id, double size, + const std::string & frame_name) + : id_(id), + size_(size), + frame_name_(frame_name) {} + + double size() {return size_;} + int id() {return id_;} + std::string & frame_name() {return frame_name_;} + +private: + // Tag description + int id_; + double size_; + std::string frame_name_; +}; + +class TagBundleDescription +{ +public: + std::map id2idx_; // (id2idx_[]=) mapping + + explicit TagBundleDescription(std::string name) + : name_(name) {} + + void addMemberTag(int id, double size, cv::Matx44d T_oi) + { + TagBundleMember member; + member.id = id; + member.size = size; + member.T_oi = T_oi; + tags_.push_back(member); + id2idx_[id] = tags_.size() - 1; + } + + std::string name() const {return name_;} + // Get IDs of bundle member tags + std::vector bundleIds() + { + std::vector ids; + for (unsigned int i = 0; i < tags_.size(); i++) { + ids.push_back(tags_[i].id); + } + return ids; + } + // Get sizes of bundle member tags + std::vector bundleSizes() + { + std::vector sizes; + for (unsigned int i = 0; i < tags_.size(); i++) { + sizes.push_back(tags_[i].size); + } + return sizes; + } + int memberID(int tagID) {return tags_[id2idx_[tagID]].id;} + double memberSize(int tagID) {return tags_[id2idx_[tagID]].size;} + cv::Matx44d memberT_oi(int tagID) {return tags_[id2idx_[tagID]].T_oi;} + +private: + // Bundle description + std::string name_; + std::vector tags_; +}; + +class TagDetector +{ +private: + // Detections sorting + static int idComparison(const void * first, const void * second); + + // Remove detections of tags with the same ID + void removeDuplicates(); + + // AprilTag 2 code's attributes + std::string family_; + int threads_; + double decimate_; + double blur_; + int refine_edges_; + int debug_; + int max_hamming_distance_ = 2; // Tunable, but really, 2 is a good choice. Values of >=3 + // consume prohibitively large amounts of memory, and otherwise + // you want the largest value possible. + + // AprilTag 2 objects + apriltag_family_t * tf_; + apriltag_detector_t * td_; + zarray_t * detections_; + + // Node objects + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_{rclcpp::get_logger("tag_detector")}; + + // Other members + std::map standalone_tag_descriptions_; + std::vector tag_bundle_descriptions_; + bool remove_duplicates_; + bool run_quietly_; + bool publish_tf_; + std::unique_ptr tf_pub_; + +public: + explicit TagDetector(rclcpp::Node * node); + ~TagDetector(); + + // Store standalone and bundle tag descriptions + std::map parseStandaloneTags(rclcpp::Node * node); + std::vector parseTagBundles(rclcpp::Node * node); + + bool findStandaloneTagDescription( + int id, StandaloneTagDescription * & descriptionContainer, + bool printWarning = true); + + geometry_msgs::msg::PoseWithCovarianceStamped makeTagPose( + const Eigen::Matrix4d & transform, + const Eigen::Quaternion rot_quaternion, + const std_msgs::msg::Header & header); + + // Detect tags in an image + apriltag_ros::msg::AprilTagDetectionArray detectTags( + const cv_bridge::CvImagePtr & image, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info); + + // Get the pose of the tag in the camera frame + // Returns homogeneous transformation matrix [R,t;[0 0 0 1]] which + // takes a point expressed in the tag frame to the same point + // expressed in the camera frame. As usual, R is the (passive) + // rotation from the tag frame to the camera frame and t is the + // vector from the camera frame origin to the tag frame origin, + // expressed in the camera frame. + Eigen::Matrix4d getRelativeTransform( + std::vector objectPoints, + std::vector imagePoints, + double fx, double fy, double cx, double cy) const; + + void addImagePoints( + apriltag_detection_t * detection, + std::vector & imagePoints) const; + void addObjectPoints( + double s, cv::Matx44d T_oi, + std::vector & objectPoints) const; + + // Draw the detected tags' outlines and payload values on the image + void drawDetections(cv_bridge::CvImagePtr image); + + bool get_publish_tf() const {return publish_tf_;} +}; + +} // namespace apriltag_ros + +#endif // APRILTAG_ROS__COMMON_FUNCTIONS_HPP_ diff --git a/aloha_ws/src/apriltag_ros/apriltag_ros/include/apriltag_ros/continuous_detector.hpp b/aloha_ws/src/apriltag_ros/apriltag_ros/include/apriltag_ros/continuous_detector.hpp new file mode 100644 index 00000000..f25cda85 --- /dev/null +++ b/aloha_ws/src/apriltag_ros/apriltag_ros/include/apriltag_ros/continuous_detector.hpp @@ -0,0 +1,81 @@ +// Copyright (c) 2017, California Institute of Technology. +// All rights reserved. +// +// Software License Agreement (BSD 2-Clause Simplified License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// The views and conclusions contained in the software and documentation are +// those of the authors and should not be interpreted as representing official +// policies, either expressed or implied, of the California Institute of +// Technology. +/** + ** continuous_detector.hpp **************************************************** + * + * Wrapper class of TagDetector class which calls TagDetector::detectTags on + * each newly arrived image published by a camera. + * + * $Revision: 1.0 $ + * $Date: 2017/12/17 13:25:52 $ + * $Author: dmalyuta $ + * + * Originator: Danylo Malyuta, JPL + ******************************************************************************/ + +#ifndef APRILTAG_ROS__CONTINUOUS_DETECTOR_HPP_ +#define APRILTAG_ROS__CONTINUOUS_DETECTOR_HPP_ + +#include + +#include "apriltag_ros/msg/april_tag_detection_array.hpp" +#include "apriltag_ros/common_functions.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace apriltag_ros +{ + +class ContinuousDetector : public rclcpp::Node +{ +public: + explicit ContinuousDetector(const rclcpp::NodeOptions & node_options); + + void imageCallback( + const sensor_msgs::msg::Image::ConstSharedPtr & image_rect, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info); + +private: + std::unique_ptr tag_detector_; + bool draw_tag_detections_image_; + cv_bridge::CvImagePtr cv_image_; + + image_transport::CameraSubscriber camera_image_subscriber_; + image_transport::Publisher tag_detections_image_publisher_; + rclcpp::Publisher::SharedPtr + tag_detections_publisher_; +}; + +} // namespace apriltag_ros + +#endif // APRILTAG_ROS__CONTINUOUS_DETECTOR_HPP_ diff --git a/aloha_ws/src/apriltag_ros/apriltag_ros/include/apriltag_ros/single_image_detector.hpp b/aloha_ws/src/apriltag_ros/apriltag_ros/include/apriltag_ros/single_image_detector.hpp new file mode 100644 index 00000000..7e4fa807 --- /dev/null +++ b/aloha_ws/src/apriltag_ros/apriltag_ros/include/apriltag_ros/single_image_detector.hpp @@ -0,0 +1,81 @@ +// Copyright (c) 2017, California Institute of Technology. +// All rights reserved. +// +// Software License Agreement (BSD 2-Clause Simplified License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// The views and conclusions contained in the software and documentation are +// those of the authors and should not be interpreted as representing official +// policies, either expressed or implied, of the California Institute of +// Technology. +/** + ** single_image_detector.hpp ************************************************** + * + * Wrapper class of TagDetector class which calls TagDetector::detectTags on a + * an image stored at a specified load path and stores the output at a specified + * save path. + * + * $Revision: 1.0 $ + * $Date: 2017/12/17 13:33:40 $ + * $Author: dmalyuta $ + * + * Originator: Danylo Malyuta, JPL + ******************************************************************************/ + +#ifndef APRILTAG_ROS__SINGLE_IMAGE_DETECTOR_HPP_ +#define APRILTAG_ROS__SINGLE_IMAGE_DETECTOR_HPP_ + +#include + +#include "apriltag_ros/common_functions.hpp" +#include "apriltag_ros/srv/analyze_single_image.hpp" + +namespace apriltag_ros +{ + +class SingleImageDetector : public rclcpp::Node +{ +private: + std::unique_ptr tag_detector_; + rclcpp::Service::SharedPtr + single_image_analysis_service_; + + rclcpp::Publisher::SharedPtr + tag_detections_publisher_; + +public: + explicit SingleImageDetector(const rclcpp::NodeOptions & node_options); + + // The function which provides the single image analysis service + bool analyzeImage( + const std::shared_ptr request_header, + apriltag_ros::srv::AnalyzeSingleImage::Request::SharedPtr request, + apriltag_ros::srv::AnalyzeSingleImage::Response::SharedPtr response); +}; + +} // namespace apriltag_ros + +#endif // APRILTAG_ROS__SINGLE_IMAGE_DETECTOR_HPP_ diff --git a/aloha_ws/src/apriltag_ros/apriltag_ros/launch/continuous_detection.launch.xml b/aloha_ws/src/apriltag_ros/apriltag_ros/launch/continuous_detection.launch.xml new file mode 100644 index 00000000..a8906060 --- /dev/null +++ b/aloha_ws/src/apriltag_ros/apriltag_ros/launch/continuous_detection.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/aloha_ws/src/apriltag_ros/apriltag_ros/launch/single_image_client.launch.xml b/aloha_ws/src/apriltag_ros/apriltag_ros/launch/single_image_client.launch.xml new file mode 100644 index 00000000..f68c5d08 --- /dev/null +++ b/aloha_ws/src/apriltag_ros/apriltag_ros/launch/single_image_client.launch.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aloha_ws/src/apriltag_ros/apriltag_ros/launch/single_image_detector.launch.xml b/aloha_ws/src/apriltag_ros/apriltag_ros/launch/single_image_detector.launch.xml new file mode 100644 index 00000000..78ad2fb3 --- /dev/null +++ b/aloha_ws/src/apriltag_ros/apriltag_ros/launch/single_image_detector.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + diff --git a/aloha_ws/src/apriltag_ros/apriltag_ros/msg/AprilTagDetection.msg b/aloha_ws/src/apriltag_ros/apriltag_ros/msg/AprilTagDetection.msg new file mode 100644 index 00000000..b0c68785 --- /dev/null +++ b/aloha_ws/src/apriltag_ros/apriltag_ros/msg/AprilTagDetection.msg @@ -0,0 +1,14 @@ +# Tag ID(s). If a standalone tag, this is a vector of size 1. If a tag bundle, +# this is a vector containing the IDs of each tag in the bundle. +int32[] id + +# Tag size(s). If a standalone tag, this is a vector of size 1. If a tag bundle, +# this is a vector containing the sizes of each tag in the bundle, in the same +# order as the IDs above. +float64[] size + +# Pose in the camera frame, obtained from homography transform. If a standalone +# tag, the homography is from the four tag corners. If a tag bundle, the +# homography is from at least the four corners of one member tag and at most the +# four corners of all member tags. +geometry_msgs/PoseWithCovarianceStamped pose \ No newline at end of file diff --git a/aloha_ws/src/apriltag_ros/apriltag_ros/msg/AprilTagDetectionArray.msg b/aloha_ws/src/apriltag_ros/apriltag_ros/msg/AprilTagDetectionArray.msg new file mode 100644 index 00000000..7b453854 --- /dev/null +++ b/aloha_ws/src/apriltag_ros/apriltag_ros/msg/AprilTagDetectionArray.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +AprilTagDetection[] detections diff --git a/aloha_ws/src/apriltag_ros/apriltag_ros/package.xml b/aloha_ws/src/apriltag_ros/apriltag_ros/package.xml new file mode 100644 index 00000000..cb20c4da --- /dev/null +++ b/aloha_ws/src/apriltag_ros/apriltag_ros/package.xml @@ -0,0 +1,53 @@ + + + + apriltag_ros + 3.2.1 + + A ROS wrapper of the AprilTag 3 visual fiducial detection + algorithm. Provides full access to the core AprilTag 3 algorithm's + customizations and makes the tag detection image and detected tags' poses + available over ROS topics (including tf). The core AprilTag 3 algorithm is + extended to allow the detection of tag bundles and a bundle calibration + script is provided (bundle detection is more accurate than single tag + detection). Continuous (camera image stream) and single image detector nodes + are available. + + + Danylo Malyuta + Wolfgang Merkt + + BSD + + http://www.ros.org/wiki/apriltag_ros + https://github.com/AprilRobotics/apriltag_ros/issues + https://github.com/AprilRobotics/apriltag_ros + + Danylo Malyuta + + ament_cmake_auto + rosidl_default_generators + + apriltag + geometry_msgs + image_transport + image_geometry + rclcpp + rclcpp_components + sensor_msgs + std_msgs + cv_bridge + tf2_geometry_msgs + tf2_ros + eigen + libopencv-dev + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/aloha_ws/src/apriltag_ros/apriltag_ros/scripts/analyze_image b/aloha_ws/src/apriltag_ros/apriltag_ros/scripts/analyze_image new file mode 100755 index 00000000..dfc92af2 --- /dev/null +++ b/aloha_ws/src/apriltag_ros/apriltag_ros/scripts/analyze_image @@ -0,0 +1,8 @@ +#!/bin/bash +# +# Detect AprilTag in a single image. Usage: +# +# ./analyze_image.sh +# + +ros2 launch apriltag_ros single_image_client.launch.xml image_load_path:="$1" image_save_path:="$2" diff --git a/aloha_ws/src/apriltag_ros/apriltag_ros/scripts/calibrate_bundle.m b/aloha_ws/src/apriltag_ros/apriltag_ros/scripts/calibrate_bundle.m new file mode 100644 index 00000000..252d0b0e --- /dev/null +++ b/aloha_ws/src/apriltag_ros/apriltag_ros/scripts/calibrate_bundle.m @@ -0,0 +1,341 @@ +% Copyright (c) 2017, California Institute of Technology. +% All rights reserved. +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are met: +% +% 1. Redistributions of source code must retain the above copyright notice, +% this list of conditions and the following disclaimer. +% 2. Redistributions in binary form must reproduce the above copyright notice, +% this list of conditions and the following disclaimer in the documentation +% and/or other materials provided with the distribution. +% +% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% POSSIBILITY OF SUCH DAMAGE. +% +% The views and conclusions contained in the software and documentation are +% those of the authors and should not be interpreted as representing official +% policies, either expressed or implied, of the California Institute of +% Technology. +% +%%% calibrate_bundle.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% +% Script to determine AprilTag bundle relative poses to a "master" tag. +% +% Instructions: +% Record a bagfile of the /tag_detections topic where you steadily +% point the camera at the AprilTag bundle such that all the bundle's +% individual tags are visible at least once at some point (the more the +% better). Run the script, then copy the printed output into the tag.yaml +% configuration file of apriltag_ros. +% +% $Revision: 1.0 $ +% $Date: 2017/12/17 13:37:34 $ +% $Author: dmalyuta $ +% +% Originator: Danylo Malyuta, JPL +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%% User inputs + +% Relative directory of calibration bagfile +calibration_file = 'data/calibration.bag'; + +% Bundle name +bundle_name = 'my_bundle'; + +% Master tag's ID +master_id = 0; + +%% Make sure matlab_rosbag is installed + +if ~exist('matlab_rosbag-0.5.0-linux64','file') + websave('matlab_rosbag', ... + ['https://github.com/bcharrow/matlab_rosbag/releases/' ... + 'download/v0.5/matlab_rosbag-0.5.0-linux64.zip']); + unzip('matlab_rosbag'); + delete matlab_rosbag.zip +end +addpath('matlab_rosbag-0.5.0-linux64'); + +%% Load the tag detections bagfile + +bag = ros.Bag.load(calibration_file); +tag_msg = bag.readAll('/tag_detections'); + +clear tag_data; +N = numel(tag_msg); +t0 = getBagTime(tag_msg{1}); +for i = 1:N + tag_data.t(i) = getBagTime(tag_msg{i})-t0; + for j = 1:numel(tag_msg{i}.detections) + detection = tag_msg{i}.detections(j); + if numel(detection.id)>1 + % Can only use standalone tag detections for calibration! + % The math allows for bundles too (e.g. bundle composed of + % bundles) but the code does not, and it's not that useful + % anyway + warning_str = 'Skipping tag bundle detection with IDs'; + for k = 1:numel(detection.id) + warning_str = sprintf('%s %d',warning_str,detection.id(k)); + end + warning(warning_str); + continue; + end + tag_data.detection(i).id(j) = detection.id; + tag_data.detection(i).size(j) = detection.size; + % Tag position with respect to camera frame + tag_data.detection(i).p(:,j) = detection.pose.pose.pose.position; + % Tag orientation with respect to camera frame + % [w;x;y;z] format + tag_data.detection(i).q(:,j) = ... + detection.pose.pose.pose.orientation([4,1,2,3]); + end +end + +%% Compute the measured poses of each tag relative to the master tag + +master_size = []; % Size of the master tag + +% IDs, sizes, relative positions and orientations of detected tags other +% than master +other_ids = []; +other_sizes = []; +rel_p = {}; +rel_q = {}; + +createT = @(p,q) [quat2rotmat(q) p; zeros(1,3) 1]; +invertT = @(T) [T(1:3,1:3)' -T(1:3,1:3)'*T(1:3,4); zeros(1,3) 1]; + +N = numel(tag_data.detection); +for i = 1:N + this = tag_data.detection(i); + + mi = find(this.id == master_id); + if isempty(mi) + % Master not detected in this detection, so this particular + % detection is useless + continue; + end + + % Get the master tag's rigid body transform to the camera frame + T_cm = createT(this.p(:,mi), this.q(:,mi)); + + % Get the rigid body transform of every other tag to the camera frame + for j = 1:numel(this.id) + % Skip the master, but get its size first + if isempty(master_size) + master_size = this.size(j); + end + % We already have the rigid body transform from the master tag to + % the camera frame (T_cm) + if j == mi + continue; + end + + % Add ID to detected IDs, if not already there + id = this.id(j); + if ~ismember(id, other_ids) + other_ids(end+1) = id; + other_sizes(end+1) = this.size(j); + rel_p{end+1} = []; + rel_q{end+1} = []; + end + + % Find the index in other_ids corresponding to this tag + k = find(other_ids == id); + assert(numel(k) == 1, ... + 'Tag ID must appear exactly once in the other_ids array'); + + % Get this tag's rigid body transform to the camera frame + T_cj = createT(this.p(:,j), this.q(:,j)); + + % Deduce this tag's rigid body transform to the master tag's frame + T_mj = invertT(T_cm)*T_cj; + + % Save the relative position and orientation of this tag to the + % master tag + rel_p{k}(:,end+1) = T_mj(1:3,4); + rel_q{k}(:,end+1) = rotmat2quat(T_mj); + end +end + +assert(~isempty(master_size), ... + sprintf('Master tag with ID %d not found in detections', master_id)); + +%% Compute (geometric) median position of each tag in master tag frame + +geometricMedianCost = @(x,y) sum(sqrt(sum((x-y).^2))); + +options = optimset('MaxIter',1000,'MaxFunEvals',1000, ... + 'Algorithm','interior-point', ... + 'TolFun', 1e-6, 'TolX', 1e-6); + +M = numel(rel_p); +rel_p_median = nan(3, numel(other_ids)); +for i = 1:M + % Compute the mean position as the initial value for the minimization + % problem + p_0 = mean(rel_p{i},2); + + % Compute the geometric median + [rel_p_median(:,i),~,exitflag] = ... + fminsearch(@(x) geometricMedianCost(rel_p{i}, x), p_0, options); + assert(exitflag == 1, ... + sprintf(['Geometric median minimization did ' ... + 'not converge (exitflag %d)'], exitflag)); +end + +%% Compute the average orientation of each tag with respect to the master tag + +rel_q_mean = nan(4, numel(other_ids)); +for i = 1:M + % Use the method in Landis et al. "Averaging Quaternions", JGCD 2007 + + % Check the sufficient uniqueness condition + % TODO this is a computational bottleness - without this check, script + % returns much faster. Any way to speed up this double-for-loop? + error_angle{i} = []; + for j = 1:size(rel_q{i},2) + q_1 = rel_q{i}(:,j); + for k = 1:size(rel_q{i},2) + if j==k + continue; + end + q_2 = rel_q{i}(:,k); + q_error = quatmult(quatinv(q_1),q_2); + % Saturate to valid acos range, which prevents imaginary output + % from acos due to q_error_w being infinitesimaly (to numerical + % precision) outside of valid [-1,1] range + q_error_w = min(1,max(q_error(1),-1)); + error_angle{i}(end+1) = 2*acos(q_error_w); + if 2*acos(q_error_w) >= pi/2 + warning(['Quaternion pair q_%u and q_%u for tag ID %u ' ... + 'are more than 90 degrees apart!'], ... + j,k,other_ids(i)); + end + end + end + + % Average quaternion method + Q = rel_q{i}; + [V, D] = eig(Q*Q.'); + [~,imax] = max(diag(D)); % Get the largest eigenvalue + rel_q_mean(:,i) = V(:,imax); % Corresponding eigenvector + if rel_q_mean(1,i) < 0 + rel_q_mean(:,i) = -rel_q_mean(:,i); % Ensure w positive + end +end + +%% Print output to paste in tags.yaml + +% Head + master tag +fprintf([ ... +'tag_bundles:\n' ... +' [\n' ... +' {\n' ... +' name: ''%s'',\n' ... +' layout:\n' ... +' [\n'], bundle_name); + +% All other tags detected at least once together with master tag +for i = 0:numel(other_ids) + newline = ','; + if i == numel(other_ids) + newline = ''; + end + if i == 0 + fprintf(' {id: %d, size: %.2f, x: %.4f, y: %.4f, z: %.4f, qw: %.4f, qx: %.4f, qy: %.4f, qz: %.4f}%s\n', ... + master_id, master_size, 0, 0, 0, 1, 0, 0, 0, newline); + else + fprintf(' {id: %d, size: %.2f, x: %.4f, y: %.4f, z: %.4f, qw: %.4f, qx: %.4f, qy: %.4f, qz: %.4f}%s\n', ... + other_ids(i), other_sizes(i), rel_p_median(1,i), ... + rel_p_median(2,i), rel_p_median(3,i), rel_q_mean(1,i), ... + rel_q_mean(2,i), rel_q_mean(3,i), rel_q_mean(4,i), newline); + end +end + +% Tail +fprintf([ ... +' ]\n'... +' }\n'... +' ]\n']); + +%% Local functions + +function t = getBagTime(bagfile) + t = double(bagfile.header.stamp.sec)+ ... + double(bagfile.header.stamp.nsec)/1e9; +end + +function R = quat2rotmat(q) + % Creates an ACTIVE rotation matrix from a quaternion + w = q(1); + x = q(2); + y = q(3); + z = q(4); + + R = [1-2*(y^2+z^2) 2*(x*y-w*z) 2*(x*z+w*y) + 2*(x*y+w*z) 1-2*(x^2+z^2) 2*(y*z-w*x) + 2*(x*z-w*y) 2*(y*z+w*x) 1-2*(x^2+y^2)]; +end + +function q = rotmat2quat(R) + % Adapted for MATLAB from + % http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ + tr = R(1,1) + R(2,2) + R(3,3); + + if tr > 0 + S = sqrt(tr+1.0) * 2; % S=4*qw + qw = 0.25 * S; + qx = (R(3,2) - R(2,3)) / S; + qy = (R(1,3) - R(3,1)) / S; + qz = (R(2,1) - R(1,2)) / S; + elseif (R(1,1) > R(2,2)) && (R(1,1) > R(3,3)) + S = sqrt(1.0 + R(1,1) - R(2,2) - R(3,3)) * 2; % S=4*qx + qw = (R(3,2) - R(2,3)) / S; + qx = 0.25 * S; + qy = (R(1,2) + R(2,1)) / S; + qz = (R(1,3) + R(3,1)) / S; + elseif (R(2,2) > R(3,3)) + S = sqrt(1.0 + R(2,2) - R(1,1) - R(3,3)) * 2; % S=4*qy + qw = (R(1,3) - R(3,1)) / S; + qx = (R(1,2) + R(2,1)) / S; + qy = 0.25 * S; + qz = (R(2,3) + R(3,2)) / S; + else + S = sqrt(1.0 + R(3,3) - R(1,1) - R(2,2)) * 2; % S=4*qz + qw = (R(2,1) - R(1,2)) / S; + qx = (R(1,3) + R(3,1)) / S; + qy = (R(2,3) + R(3,2)) / S; + qz = 0.25 * S; + end + + q = [qw qx qy qz]'; +end + +function c = quatmult(a,b) + % More humanly understandable version: + % Omegaa = [a((1)) -a((2):(4)).' + % a((2):(4)) a((1))*eye((3))-[0 -a((4)) a((3)); a((4)) 0 -a((2));-a((3)) a((2)) 0]]; + % c = Omegaa*b; + % More optimized version: + c_w = a(1)*b(1) - a(2)*b(2) - a(3)*b(3) - a(4)*b(4); + c_x = a(1)*b(2) + a(2)*b(1) - a(3)*b(4) + a(4)*b(3); + c_y = a(1)*b(3) + a(3)*b(1) + a(2)*b(4) - a(4)*b(2); + c_z = a(1)*b(4) - a(2)*b(3) + a(3)*b(2) + a(4)*b(1); + c = [c_w; c_x; c_y; c_z]; +end + +function qinv = quatinv(q) + qinv = [q(1); -q(2:4)]; +end diff --git a/aloha_ws/src/apriltag_ros/apriltag_ros/src/common_functions.cpp b/aloha_ws/src/apriltag_ros/apriltag_ros/src/common_functions.cpp new file mode 100644 index 00000000..29798bfe --- /dev/null +++ b/aloha_ws/src/apriltag_ros/apriltag_ros/src/common_functions.cpp @@ -0,0 +1,649 @@ +// Copyright (c) 2017, California Institute of Technology. +// All rights reserved. +// +// Software License Agreement (BSD 2-Clause Simplified License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// The views and conclusions contained in the software and documentation are +// those of the authors and should not be interpreted as representing official +// policies, either expressed or implied, of the California Institute of +// Technology. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "apriltag_ros/common_functions.hpp" +#include "image_geometry/pinhole_camera_model.h" +#include "rcpputils/asserts.hpp" +#if USE_TF2_GEOMETRY_MSGS_HPP_HEADER +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#else +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#endif + +#include "common/homography.h" + +namespace apriltag_ros +{ + +TagDetector::TagDetector(rclcpp::Node * node) +: family_(node->declare_parameter("tag_family", "tag36h11")), + threads_(node->declare_parameter("tag_threads", 4)), + decimate_(node->declare_parameter("tag_decimate", 1.0)), + blur_(node->declare_parameter("tag_blur", 0.0)), + refine_edges_(node->declare_parameter("tag_refine_edges", 1)), + debug_(node->declare_parameter("tag_debug", 0)), + max_hamming_distance_(node->declare_parameter("max_hamming_dist", 2)), + clock_(node->get_clock()), + standalone_tag_descriptions_(parseStandaloneTags(node)), + tag_bundle_descriptions_(parseTagBundles(node)), + remove_duplicates_(node->declare_parameter("remove_duplicates", true)), + publish_tf_(node->declare_parameter("publish_tf", false)), + tf_pub_(std::make_unique(node)) +{ + // Define the tag family whose tags should be searched for in the camera + // images + if (family_ == "tagStandard52h13") { + tf_ = tagStandard52h13_create(); + } else if (family_ == "tagStandard41h12") { + tf_ = tagStandard41h12_create(); + } else if (family_ == "tag36h11") { + tf_ = tag36h11_create(); + } else if (family_ == "tag25h9") { + tf_ = tag25h9_create(); + } else if (family_ == "tag16h5") { + tf_ = tag16h5_create(); + } else if (family_ == "tagCustom48h12") { + tf_ = tagCustom48h12_create(); + } else if (family_ == "tagCircle21h7") { + tf_ = tagCircle21h7_create(); + } else if (family_ == "tagCircle49h12") { + tf_ = tagCircle49h12_create(); + } else { + RCLCPP_WARN(logger_, "Invalid tag family specified! Aborting"); + exit(1); + } + + // Create the AprilTag 2 detector + td_ = apriltag_detector_create(); + apriltag_detector_add_family_bits(td_, tf_, max_hamming_distance_); + td_->quad_decimate = static_cast(decimate_); + td_->quad_sigma = static_cast(blur_); + td_->nthreads = threads_; + td_->debug = debug_; + td_->refine_edges = refine_edges_; + + detections_ = nullptr; +} + +// destructor +TagDetector::~TagDetector() +{ + // free memory associated with tag detector + apriltag_detector_destroy(td_); + + // Free memory associated with the array of tag detections + if (detections_) { + apriltag_detections_destroy(detections_); + } + + // free memory associated with tag family + if (family_ == "tagStandard52h13") { + tagStandard52h13_destroy(tf_); + } else if (family_ == "tagStandard41h12") { + tagStandard41h12_destroy(tf_); + } else if (family_ == "tag36h11") { + tag36h11_destroy(tf_); + } else if (family_ == "tag25h9") { + tag25h9_destroy(tf_); + } else if (family_ == "tag16h5") { + tag16h5_destroy(tf_); + } else if (family_ == "tagCustom48h12") { + tagCustom48h12_destroy(tf_); + } else if (family_ == "tagCircle21h7") { + tagCircle21h7_destroy(tf_); + } else if (family_ == "tagCircle49h12") { + tagCircle49h12_destroy(tf_); + } +} + +apriltag_ros::msg::AprilTagDetectionArray TagDetector::detectTags( + const cv_bridge::CvImagePtr & image, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info) +{ + // Convert image to AprilTag code's format + cv::Mat gray_image; + if (image->image.channels() == 1) { + gray_image = image->image; + } else { + cv::cvtColor(image->image, gray_image, CV_BGR2GRAY); + } + image_u8_t apriltag_image{ + gray_image.cols, // width + gray_image.rows, // height + gray_image.cols, // stride + gray_image.data // buf + }; + + image_geometry::PinholeCameraModel camera_model; + camera_model.fromCameraInfo(camera_info); + + // Get camera intrinsic properties for rectified image. + double fx = camera_model.fx(); // focal length in camera x-direction [px] + double fy = camera_model.fy(); // focal length in camera y-direction [px] + double cx = camera_model.cx(); // optical center x-coordinate [px] + double cy = camera_model.cy(); // optical center y-coordinate [px] + + // Run AprilTag 2 algorithm on the image + if (detections_) { + apriltag_detections_destroy(detections_); + detections_ = NULL; + } + detections_ = apriltag_detector_detect(td_, &apriltag_image); + + // If remove_dulpicates_ is set to true, then duplicate tags are not allowed. + // Thus any duplicate tag IDs visible in the scene must include at least 1 + // erroneous detection. Remove any tags with duplicate IDs to ensure removal + // of these erroneous detections + if (remove_duplicates_) { + removeDuplicates(); + } + + // Compute the estimated translation and rotation individually for each + // detected tag + apriltag_ros::msg::AprilTagDetectionArray tag_detection_array; + std::vector detection_names; + tag_detection_array.header = image->header; + std::map> bundleObjectPoints; + std::map> bundleImagePoints; + for (int i = 0; i < zarray_size(detections_); i++) { + // Get the i-th detected tag + apriltag_detection_t * detection; + zarray_get(detections_, i, &detection); + + // Bootstrap this for loop to find this tag's description amongst + // the tag bundles. If found, add its points to the bundle's set of + // object-image corresponding points (tag corners) for cv::solvePnP. + // Don't yet run cv::solvePnP on the bundles, though, since we're still in + // the process of collecting all the object-image corresponding points + int tagID = detection->id; + bool is_part_of_bundle = false; + for (unsigned int j = 0; j < tag_bundle_descriptions_.size(); j++) { + // Iterate over the registered bundles + TagBundleDescription bundle = tag_bundle_descriptions_[j]; + + if (bundle.id2idx_.find(tagID) != bundle.id2idx_.end()) { + // This detected tag belongs to the j-th tag bundle (its ID was found in + // the bundle description) + is_part_of_bundle = true; + std::string bundleName = bundle.name(); + + //===== Corner points in the world frame coordinates + double s = bundle.memberSize(tagID) / 2; + addObjectPoints( + s, bundle.memberT_oi(tagID), + bundleObjectPoints[bundleName]); + + //===== Corner points in the image frame coordinates + addImagePoints(detection, bundleImagePoints[bundleName]); + } + } + + // Find this tag's description amongst the standalone tags + // Print warning when a tag was found that is neither part of a + // bundle nor standalone (thus it is a tag in the environment + // which the user specified no description for, or Apriltags + // misdetected a tag (bad ID or a false positive)). + StandaloneTagDescription * standaloneDescription; + if (!findStandaloneTagDescription( + tagID, standaloneDescription, + !is_part_of_bundle)) + { + continue; + } + + //================================================================= + // The remainder of this for loop is concerned with standalone tag + // poses! + double tag_size = standaloneDescription->size(); + + // Get estimated tag pose in the camera frame. + // + // Note on frames: + // The raw AprilTag 2 uses the following frames: + // - camera frame: looking from behind the camera (like a + // photographer), x is right, y is up and z is towards you + // (i.e. the back of camera) + // - tag frame: looking straight at the tag (oriented correctly), + // x is right, y is down and z is away from you (into the tag). + // But we want: + // - camera frame: looking from behind the camera (like a + // photographer), x is right, y is down and z is straight + // ahead + // - tag frame: looking straight at the tag (oriented correctly), + // x is right, y is up and z is towards you (out of the tag). + // Using these frames together with cv::solvePnP directly avoids + // AprilTag 2's frames altogether. + // TODO(someone) solvePnP[Ransac] better? + std::vector standaloneTagObjectPoints; + std::vector standaloneTagImagePoints; + addObjectPoints(tag_size / 2, cv::Matx44d::eye(), standaloneTagObjectPoints); + addImagePoints(detection, standaloneTagImagePoints); + Eigen::Matrix4d transform = getRelativeTransform( + standaloneTagObjectPoints, + standaloneTagImagePoints, + fx, fy, cx, cy); + Eigen::Matrix3d rot = transform.block(0, 0, 3, 3); + Eigen::Quaternion rot_quaternion(rot); + + geometry_msgs::msg::PoseWithCovarianceStamped tag_pose = + makeTagPose(transform, rot_quaternion, image->header); + + // Add the detection to the back of the tag detection array + apriltag_ros::msg::AprilTagDetection tag_detection; + tag_detection.pose = tag_pose; + tag_detection.id.push_back(detection->id); + tag_detection.size.push_back(tag_size); + tag_detection_array.detections.push_back(tag_detection); + detection_names.push_back(standaloneDescription->frame_name()); + } + + //================================================================= + // Estimate bundle origin pose for each bundle in which at least one + // member tag was detected + + for (unsigned int j = 0; j < tag_bundle_descriptions_.size(); j++) { + // Get bundle name + std::string bundleName = tag_bundle_descriptions_[j].name(); + + std::map>::iterator it = + bundleObjectPoints.find(bundleName); + if (it != bundleObjectPoints.end()) { + // Some member tags of this bundle were detected, get the bundle's + // position! + TagBundleDescription & bundle = tag_bundle_descriptions_[j]; + + Eigen::Matrix4d transform = + getRelativeTransform( + bundleObjectPoints[bundleName], + bundleImagePoints[bundleName], fx, fy, cx, cy); + Eigen::Matrix3d rot = transform.block(0, 0, 3, 3); + Eigen::Quaternion rot_quaternion(rot); + + geometry_msgs::msg::PoseWithCovarianceStamped bundle_pose = + makeTagPose(transform, rot_quaternion, image->header); + + // Add the detection to the back of the tag detection array + apriltag_ros::msg::AprilTagDetection tag_detection; + tag_detection.pose = bundle_pose; + tag_detection.id = bundle.bundleIds(); + tag_detection.size = bundle.bundleSizes(); + tag_detection_array.detections.push_back(tag_detection); + detection_names.push_back(bundle.name()); + } + } + + // If set, publish the transform /tf topic + if (publish_tf_) { + for (unsigned int i = 0; i < tag_detection_array.detections.size(); i++) { + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.header.frame_id = image->header.frame_id; + transform_stamped.child_frame_id = detection_names[i]; + transform_stamped.header.stamp = tag_detection_array.detections[i].pose.header.stamp; + + const auto & pose = tag_detection_array.detections[i].pose.pose.pose; + transform_stamped.transform.translation.x = pose.position.x; + transform_stamped.transform.translation.y = pose.position.y; + transform_stamped.transform.translation.z = pose.position.z; + + tf2::Quaternion tf_quaternion; + tf2::fromMsg(pose.orientation, tf_quaternion); + transform_stamped.transform.rotation.x = tf_quaternion.x(); + transform_stamped.transform.rotation.y = tf_quaternion.y(); + transform_stamped.transform.rotation.z = tf_quaternion.z(); + transform_stamped.transform.rotation.w = tf_quaternion.w(); + + tf_pub_->sendTransform(transform_stamped); + } + } + + return tag_detection_array; +} + +int TagDetector::idComparison(const void * first, const void * second) +{ + int id1 = (reinterpret_cast(first))->id; + int id2 = (reinterpret_cast(second))->id; + return (id1 < id2) ? -1 : ((id1 == id2) ? 0 : 1); +} + +void TagDetector::removeDuplicates() +{ + zarray_sort(detections_, &idComparison); + int count = 0; + bool duplicate_detected = false; + while (true) { + if (count > zarray_size(detections_) - 1) { + // The entire detection set was parsed + return; + } + apriltag_detection_t * detection; + zarray_get(detections_, count, &detection); + int id_current = detection->id; + // Default id_next value of -1 ensures that if the last detection + // is a duplicated tag ID, it will get removed + int id_next = -1; + if (count < zarray_size(detections_) - 1) { + zarray_get(detections_, count + 1, &detection); + id_next = detection->id; + } + if (id_current == id_next || (id_current != id_next && duplicate_detected)) { + duplicate_detected = true; + // Remove the current tag detection from detections array + int shuffle = 0; + zarray_remove_index(detections_, count, shuffle); + if (id_current != id_next) { + RCLCPP_WARN_STREAM( + logger_, + "Pruning tag ID " << id_current << " because it " + "appears more than once in the image."); + duplicate_detected = false; // Reset + } + continue; + } else { + count++; + } + } +} + +void TagDetector::addObjectPoints( + double s, cv::Matx44d T_oi, std::vector & objectPoints) const +{ + // Add to object point vector the tag corner coordinates in the bundle frame + // Going counterclockwise starting from the bottom left corner + objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0) * cv::Vec4d(-s, -s, 0, 1)); + objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0) * cv::Vec4d(s, -s, 0, 1)); + objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0) * cv::Vec4d(s, s, 0, 1)); + objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0) * cv::Vec4d(-s, s, 0, 1)); +} + +void TagDetector::addImagePoints( + apriltag_detection_t * detection, + std::vector & imagePoints) const +{ + // Add to image point vector the tag corners in the image frame + // Going counterclockwise starting from the bottom left corner + double tag_x[4] = {-1, 1, 1, -1}; + double tag_y[4] = {1, 1, -1, -1}; // Negated because AprilTag tag local + // frame has y-axis pointing DOWN + // while we use the tag local frame + // with y-axis pointing UP + for (int i = 0; i < 4; i++) { + // Homography projection taking tag local frame coordinates to image pixels + double im_x, im_y; + homography_project(detection->H, tag_x[i], tag_y[i], &im_x, &im_y); + imagePoints.push_back(cv::Point2d(im_x, im_y)); + } +} + +Eigen::Matrix4d TagDetector::getRelativeTransform( + std::vector objectPoints, + std::vector imagePoints, + double fx, double fy, double cx, double cy) const +{ + // perform Perspective-n-Point camera pose estimation using the + // above 3D-2D point correspondences + cv::Mat rvec, tvec; + cv::Matx33d cameraMatrix(fx, 0, cx, + 0, fy, cy, + 0, 0, 1); + cv::Vec4f distCoeffs(0, 0, 0, 0); // distortion coefficients + // TODO(someone): Perhaps something like SOLVEPNP_EPNP would be faster? Would + // need to first check WHAT is a bottleneck in this code, and only + // do this if PnP solution is the bottleneck. + cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec); + cv::Matx33d R; + cv::Rodrigues(rvec, R); + Eigen::Matrix3d wRo; + wRo << R(0, 0), R(0, 1), R(0, 2), R(1, 0), R(1, 1), R(1, 2), R(2, 0), R(2, 1), R(2, 2); + + Eigen::Matrix4d T; // homogeneous transformation matrix + T.topLeftCorner(3, 3) = wRo; + T.col(3).head(3) << + tvec.at(0), tvec.at(1), tvec.at(2); + T.row(3) << 0, 0, 0, 1; + return T; +} + +geometry_msgs::msg::PoseWithCovarianceStamped TagDetector::makeTagPose( + const Eigen::Matrix4d & transform, + const Eigen::Quaternion rot_quaternion, + const std_msgs::msg::Header & header) +{ + geometry_msgs::msg::PoseWithCovarianceStamped pose; + pose.header = header; + //===== Position and orientation + pose.pose.pose.position.x = transform(0, 3); + pose.pose.pose.position.y = transform(1, 3); + pose.pose.pose.position.z = transform(2, 3); + pose.pose.pose.orientation.x = rot_quaternion.x(); + pose.pose.pose.orientation.y = rot_quaternion.y(); + pose.pose.pose.orientation.z = rot_quaternion.z(); + pose.pose.pose.orientation.w = rot_quaternion.w(); + return pose; +} + +void TagDetector::drawDetections(cv_bridge::CvImagePtr image) +{ + for (int i = 0; i < zarray_size(detections_); i++) { + apriltag_detection_t * det; + zarray_get(detections_, i, &det); + + // Check if this ID is present in config/tags.yaml + // Check if is part of a tag bundle + int tagID = det->id; + bool is_part_of_bundle = false; + for (unsigned int j = 0; j < tag_bundle_descriptions_.size(); j++) { + TagBundleDescription bundle = tag_bundle_descriptions_[j]; + if (bundle.id2idx_.find(tagID) != bundle.id2idx_.end()) { + is_part_of_bundle = true; + break; + } + } + // If not part of a bundle, check if defined as a standalone tag + StandaloneTagDescription * standaloneDescription; + if (!is_part_of_bundle && + !findStandaloneTagDescription(tagID, standaloneDescription, false)) + { + // Neither a standalone tag nor part of a bundle, so this is a "rogue" + // tag, skip it. + continue; + } + + // Draw tag outline with edge colors green, blue, blue, red + // (going counter-clockwise, starting from lower-left corner in + // tag coords). cv::Scalar(Blue, Green, Red) format for the edge + // colors! + line( + image->image, cv::Point(static_cast(det->p[0][0]), static_cast(det->p[0][1])), + cv::Point(static_cast(det->p[1][0]), static_cast(det->p[1][1])), + cv::Scalar(0, 0xff, 0)); // green + line( + image->image, cv::Point(static_cast(det->p[0][0]), static_cast(det->p[0][1])), + cv::Point(static_cast(det->p[3][0]), static_cast(det->p[3][1])), + cv::Scalar(0, 0, 0xff)); // red + line( + image->image, cv::Point(static_cast(det->p[1][0]), static_cast(det->p[1][1])), + cv::Point(static_cast(det->p[2][0]), static_cast(det->p[2][1])), + cv::Scalar(0xff, 0, 0)); // blue + line( + image->image, cv::Point(static_cast(det->p[2][0]), static_cast(det->p[2][1])), + cv::Point(static_cast(det->p[3][0]), static_cast(det->p[3][1])), + cv::Scalar(0xff, 0, 0)); // blue + + // Print tag ID in the middle of the tag + std::stringstream ss; + ss << det->id; + cv::String text = ss.str(); + int fontface = cv::FONT_HERSHEY_SCRIPT_SIMPLEX; + double fontscale = 0.5; + int baseline; + cv::Size textsize = cv::getTextSize( + text, fontface, + fontscale, 2, &baseline); + cv::putText( + image->image, text, + cv::Point( + static_cast(det->c[0] - textsize.width / 2), + static_cast(det->c[1] + textsize.height / 2)), + fontface, fontscale, cv::Scalar(0xff, 0x99, 0), 2); + } +} + +// Parse standalone tag descriptions +std::map TagDetector::parseStandaloneTags(rclcpp::Node * node) +{ + // Create map that will be filled by the function and returned in the end + std::map descriptions; + const auto tag_names = node->declare_parameter( + "standalone_tags.tag_names", + std::vector()); + // Loop through all tag descriptions + for (const auto & name : tag_names) { + const auto id = node->declare_parameter( + "standalone_tags." + name + ".id" + ); + const auto size = node->declare_parameter( + "standalone_tags." + name + ".size" + ); + + StandaloneTagDescription description(id, size, name); + RCLCPP_INFO_STREAM( + logger_, + "Loaded tag config: " << id << ", size: " << + size << ", frame_name: " << name.c_str()); + // Add this tag's description to map of descriptions + descriptions.insert(std::make_pair(id, description)); + } + + return descriptions; +} + +// parse tag bundle descriptions +std::vector TagDetector::parseTagBundles(rclcpp::Node * node) +{ + std::vector descriptions; + const auto bundle_names = node->declare_parameter( + "tag_bundles.bundle_names", + std::vector()); + + // Loop through all tag bundle descritions + for (const auto & name : bundle_names) { + TagBundleDescription bundle_i(name); + RCLCPP_INFO( + logger_, "Loading tag bundle '%s'", bundle_i.name().c_str()); + + const auto layout_ids = node->declare_parameter( + "tag_bundles." + name + ".layout.ids", + std::vector()); + + // Loop through each member tag of the bundle + for (const auto id : layout_ids) { + const auto prefix = "tag_bundles." + name + ".layout." + std::to_string(id) + "."; + const auto size = node->declare_parameter(prefix + "size"); + + // Make sure that if this tag was specified also as standalone, + // then the sizes match + StandaloneTagDescription * standaloneDescription; + if (findStandaloneTagDescription(id, standaloneDescription, false)) { + rcpputils::assert_true(size == standaloneDescription->size()); + } + + // Get this tag's pose with respect to the bundle origin + const auto x = node->declare_parameter(prefix + "x", 0.0); + const auto y = node->declare_parameter(prefix + "y", 0.0); + const auto z = node->declare_parameter(prefix + "z", 0.0); + const auto qw = node->declare_parameter(prefix + "qw", 1.0); + const auto qx = node->declare_parameter(prefix + "qx", 0.0); + const auto qy = node->declare_parameter(prefix + "qy", 0.0); + const auto qz = node->declare_parameter(prefix + "qz", 0.0); + Eigen::Quaterniond q_tag(qw, qx, qy, qz); + q_tag.normalize(); + Eigen::Matrix3d R_oi = q_tag.toRotationMatrix(); + + // Build the rigid transform from tag_j to the bundle origin + cv::Matx44d T_mj(R_oi(0, 0), R_oi(0, 1), R_oi(0, 2), x, + R_oi(1, 0), R_oi(1, 1), R_oi(1, 2), y, + R_oi(2, 0), R_oi(2, 1), R_oi(2, 2), z, + 0, 0, 0, 1); + + // Register the tag member + bundle_i.addMemberTag(id, size, T_mj); + RCLCPP_INFO_STREAM( + logger_, + "id: " << id << ", size: " << size << ", " << + "p = [" << x << "," << y << "," << z << "], " << + "q = [" << qw << "," << qx << "," << qy << "," << + qz << "]"); + } + descriptions.push_back(bundle_i); + } + return descriptions; +} + +bool TagDetector::findStandaloneTagDescription( + int id, StandaloneTagDescription * & descriptionContainer, bool printWarning) +{ + std::map::iterator description_itr = + standalone_tag_descriptions_.find(id); + if (description_itr == standalone_tag_descriptions_.end()) { + if (printWarning) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 10000, + "Requested description of standalone tag ID [%d]," + " but no description was found...", id); + } + return false; + } + descriptionContainer = &(description_itr->second); + return true; +} + +} // namespace apriltag_ros diff --git a/aloha_ws/src/apriltag_ros/apriltag_ros/src/continuous_detector.cpp b/aloha_ws/src/apriltag_ros/apriltag_ros/src/continuous_detector.cpp new file mode 100644 index 00000000..15a39d1d --- /dev/null +++ b/aloha_ws/src/apriltag_ros/apriltag_ros/src/continuous_detector.cpp @@ -0,0 +1,103 @@ +// Copyright (c) 2017, California Institute of Technology. +// All rights reserved. +// +// Software License Agreement (BSD 2-Clause Simplified License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// The views and conclusions contained in the software and documentation are +// those of the authors and should not be interpreted as representing official +// policies, either expressed or implied, of the California Institute of +// Technology. + +#include "apriltag_ros/continuous_detector.hpp" + +#include + +namespace apriltag_ros +{ +ContinuousDetector::ContinuousDetector(const rclcpp::NodeOptions & node_options) +: Node("continuous_detector", node_options) +{ + tag_detector_ = std::make_unique(this); + draw_tag_detections_image_ = declare_parameter( + "publish_tag_detections_image", false); + + const auto transport_hint = declare_parameter("transport_hint", "raw"); + + camera_image_subscriber_ = image_transport::create_camera_subscription( + this, "~/image_rect", + std::bind( + &ContinuousDetector::imageCallback, + this, std::placeholders::_1, std::placeholders::_2), transport_hint); + tag_detections_publisher_ = create_publisher( + "~/tag_detections", 1); + if (draw_tag_detections_image_) { + tag_detections_image_publisher_ = image_transport::create_publisher( + this, "~/tag_detections_image"); + } +} + +void ContinuousDetector::imageCallback( + const sensor_msgs::msg::Image::ConstSharedPtr & image_rect, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info) +{ + // Lazy updates: + // When there are no subscribers _and_ when tf is not published, + // skip detection. + if (tag_detections_publisher_->get_subscription_count() == 0 && + tag_detections_publisher_->get_intra_process_subscription_count() == 0 && + tag_detections_image_publisher_.getNumSubscribers() == 0 && + !tag_detector_->get_publish_tf()) + { + // ROS_INFO_STREAM("No subscribers and no tf publishing, skip processing."); + return; + } + + // Convert ROS's sensor_msgs::Image to cv_bridge::CvImagePtr in order to run + // AprilTag 2 on the iamge + try { + cv_image_ = cv_bridge::toCvCopy(image_rect, image_rect->encoding); + } catch (const cv_bridge::Exception & e) { + RCLCPP_ERROR(get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + + // Publish detected tags in the image by AprilTag 2 + tag_detections_publisher_->publish( + tag_detector_->detectTags(cv_image_, camera_info)); + + // Publish the camera image overlaid by outlines of the detected tags and + // their payload values + if (draw_tag_detections_image_) { + tag_detector_->drawDetections(cv_image_); + tag_detections_image_publisher_.publish(cv_image_->toImageMsg()); + } +} + +} // namespace apriltag_ros + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(apriltag_ros::ContinuousDetector) diff --git a/aloha_ws/src/apriltag_ros/apriltag_ros/src/single_image_client.cpp b/aloha_ws/src/apriltag_ros/apriltag_ros/src/single_image_client.cpp new file mode 100644 index 00000000..0c3a96ac --- /dev/null +++ b/aloha_ws/src/apriltag_ros/apriltag_ros/src/single_image_client.cpp @@ -0,0 +1,109 @@ +// Copyright (c) 2017, California Institute of Technology. +// All rights reserved. +// +// Software License Agreement (BSD 2-Clause Simplified License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// The views and conclusions contained in the software and documentation are +// those of the authors and should not be interpreted as representing official +// policies, either expressed or implied, of the California Institute of +// Technology. + +#include +#include + +#include "apriltag_ros/common_functions.hpp" +#include "apriltag_ros/srv/analyze_single_image.hpp" + +namespace apriltag_ros +{ +class SingleImageClient : public rclcpp::Node +{ +public: + explicit SingleImageClient(const rclcpp::NodeOptions & node_options); +}; + +SingleImageClient::SingleImageClient( + const rclcpp::NodeOptions & node_options) +: rclcpp::Node("single_image_client", node_options) +{ + using std::chrono_literals::operator""s; + + auto client = create_client( + "single_image_tag_detection"); + + while (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(get_logger(), "Interrupted while waiting for service."); + rclcpp::shutdown(); + return; + } + RCLCPP_INFO(get_logger(), "Waiting for service..."); + } + + // Get the request parameters + auto request = std::make_shared(); + request->full_path_where_to_get_image = declare_parameter("image_load_path"); + request->full_path_where_to_save_image = declare_parameter("image_save_path"); + // Replicate sensors_msgs/CameraInfo message (must be up-to-date with the + // analyzed image!) + request->camera_info.distortion_model = "plumb_bob"; + // Intrinsic camera matrix for the raw (distorted) images + const auto fx = declare_parameter("fx"); + const auto cx = declare_parameter("cx"); + const auto fy = declare_parameter("fy"); + const auto cy = declare_parameter("cy"); + request->camera_info.k[0] = fx; + request->camera_info.k[2] = cx; + request->camera_info.k[4] = fy; + request->camera_info.k[5] = cy; + request->camera_info.k[8] = 1.0; + request->camera_info.p[0] = fx; + request->camera_info.p[2] = cx; + request->camera_info.p[5] = fy; + request->camera_info.p[6] = cy; + request->camera_info.p[10] = 1.0; + + auto response = client->async_send_request(request); + + if (rclcpp::spin_until_future_complete( + get_node_base_interface(), response) == rclcpp::FutureReturnCode::SUCCESS) + { + // use parameter run_quielty=false in order to have the service + // print out the tag position and orientation + if (response.get()->tag_detections.detections.size() == 0) { + RCLCPP_WARN_STREAM(get_logger(), "No detected tags!"); + } + } else { + RCLCPP_ERROR(get_logger(), "Failed to call service single_image_tag_detection"); + } + + rclcpp::shutdown(); +} +} // namespace apriltag_ros + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(apriltag_ros::SingleImageClient) diff --git a/aloha_ws/src/apriltag_ros/apriltag_ros/src/single_image_detector.cpp b/aloha_ws/src/apriltag_ros/apriltag_ros/src/single_image_detector.cpp new file mode 100644 index 00000000..f173347a --- /dev/null +++ b/aloha_ws/src/apriltag_ros/apriltag_ros/src/single_image_detector.cpp @@ -0,0 +1,109 @@ +// Copyright (c) 2017, California Institute of Technology. +// All rights reserved. +// +// Software License Agreement (BSD 2-Clause Simplified License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// The views and conclusions contained in the software and documentation are +// those of the authors and should not be interpreted as representing official +// policies, either expressed or implied, of the California Institute of +// Technology. + +#include + +#include "apriltag_ros/single_image_detector.hpp" + +#include "opencv2/highgui/highgui.hpp" + +namespace apriltag_ros +{ + +SingleImageDetector::SingleImageDetector(const rclcpp::NodeOptions & node_options) +: Node("single_image_detector", node_options) +{ + using std::placeholders::_1; + using std::placeholders::_2; + using std::placeholders::_3; + + tag_detector_ = std::make_unique(this); + // Advertise the single image analysis service + single_image_analysis_service_ = create_service( + "single_image_tag_detection", + std::bind(&SingleImageDetector::analyzeImage, this, _1, _2, _3)); + tag_detections_publisher_ = create_publisher( + "tag_detections", 1); + RCLCPP_INFO_STREAM(get_logger(), "Ready to do tag detection on single images"); +} + +bool SingleImageDetector::analyzeImage( + [[ maybe_unused ]] const std::shared_ptr request_header, + apriltag_ros::srv::AnalyzeSingleImage::Request::SharedPtr request, + apriltag_ros::srv::AnalyzeSingleImage::Response::SharedPtr response) +{ + RCLCPP_INFO(get_logger(), "[ Summoned to analyze image ]"); + RCLCPP_INFO_STREAM( + get_logger(), + "Image load path: " << request->full_path_where_to_get_image); + RCLCPP_INFO_STREAM( + get_logger(), + "Image save path: " << request->full_path_where_to_save_image); + + // Read the image + cv::Mat image = cv::imread( + request->full_path_where_to_get_image, + cv::IMREAD_COLOR); + if (!image.data) { + // Cannot read image + RCLCPP_ERROR_STREAM( + get_logger(), + "Could not read image " << request->full_path_where_to_get_image); + return false; + } + + // Detect tags in the image + auto loaded_image = std::make_shared( + std_msgs::msg::Header{}, "bgr8", image); + loaded_image->header.frame_id = "camera"; + response->tag_detections = tag_detector_->detectTags( + loaded_image, std::make_shared(request->camera_info)); + + // Publish detected tags (AprilTagDetectionArray, basically an array of + // geometry_msgs/PoseWithCovarianceStamped) + tag_detections_publisher_->publish(response->tag_detections); + + // Save tag detections image + tag_detector_->drawDetections(loaded_image); + cv::imwrite(request->full_path_where_to_save_image, loaded_image->image); + + RCLCPP_INFO(get_logger(), "Done!\n"); + + return true; +} + +} // namespace apriltag_ros + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(apriltag_ros::SingleImageDetector) diff --git a/aloha_ws/src/apriltag_ros/apriltag_ros/srv/AnalyzeSingleImage.srv b/aloha_ws/src/apriltag_ros/apriltag_ros/srv/AnalyzeSingleImage.srv new file mode 100644 index 00000000..09a2e2a6 --- /dev/null +++ b/aloha_ws/src/apriltag_ros/apriltag_ros/srv/AnalyzeSingleImage.srv @@ -0,0 +1,14 @@ +# Service which takes in: +# +# full_path_to_image : full path to a .jpg image +# +# and returns: +# +# pose : the pose of the tag in the camera frame +# tag_detection_image : an image with the detected tag's border highlighted and payload value printed + +string full_path_where_to_get_image +string full_path_where_to_save_image +sensor_msgs/CameraInfo camera_info +--- +apriltag_ros/AprilTagDetectionArray tag_detections