feat(aloha_ws): Clone apriltag_ros

Branch: ros2-port Reference: https://github.com/Interbotix/apriltag_ros/tree/d726791a1bf03692e44c59ec094ff0e50c41a0ad
j3soon · Sep 5, 2024 · 6690a8b · 6690a8b
1 parent edf3f3e
commit 6690a8b
Show file tree

Hide file tree

Showing 29 changed files with 2,595 additions and 0 deletions.
diff --git a/aloha_ws/src/apriltag_ros/.github/build_and_test.yml 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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 <https://github.com/AprilRobotics/apriltag_ros/issues/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 <https://github.com/AprilRobotics/apriltag_ros/issues/108>`_)
+* Move to using the apriltag CMake target (`#104 <https://github.com/AprilRobotics/apriltag_ros/issues/104>`_)
+* Set the tag's parent frame to the camera optical frame (`#101 <https://github.com/AprilRobotics/apriltag_ros/issues/101>`_)
+* Fix bug in K matrix in single_image_client (`#103 <https://github.com/AprilRobotics/apriltag_ros/issues/103>`_)
+* Add configurable max_hamming_distance for the AprilTag Detector (`#93 <https://github.com/AprilRobotics/apriltag_ros/issues/93>`_)
+* Introduce lazy processing for ContinuousDetector (`#80 <https://github.com/AprilRobotics/apriltag_ros/issues/80>`_)
+* Contributors: Akshay Prasad, Amal Nanavati, Christian Rauch, Hongzhuo Liang, Wolfgang Merkt
+
+3.1.2 (2020-07-15)
+------------------
+* Add support for tagCircle21h7, tagCircle49h12 (`#69 <https://github.com/AprilRobotics/apriltag_ros/issues/69>`_)
+* Add support for tagCustom48h12 (`#65 <https://github.com/AprilRobotics/apriltag_ros/issues/65>`_)
+* Contributors: Anthony Biviano, Kyle Saltmarsh, Wolfgang Merkt, kai wu
+
+3.1.1 (2019-10-07)
+------------------
+* Add support for tagStandard41h12 and tagStandard52h13 (`#63 <https://github.com/AprilRobotics/apriltag_ros/issues/63>`_, `#59 <https://github.com/AprilRobotics/apriltag_ros/issues/59>`_).
+* Add gray image input support (`#58 <https://github.com/AprilRobotics/apriltag_ros/issues/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 <https://github.com/AprilRobotics/apriltag_ros/issues/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
@@ -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
+)