Merge pull request #1 from AprilRobotics/master

From Pull request AprilRobotics#93

.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

README.md

@@ -6,7 +6,7 @@
 
 **Authors**: Danylo Malyuta, Wolfgang Merkt
 
-**Maintainers**: [Danylo Malyuta](mailto:danylo.malyuta@gmail.com) (NASA Jet Propulsion Laboratory, California Institute of Technology), [Wolfgang Merkt](https://github.com/wxmerkt)
+**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
 
@@ -41,21 +41,34 @@ Pull requests are welcome! Especially for the following areas:
 
 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 cite:
+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, “[Navigation, Control and Mission Logic for Quadrotor Full-cycle Autonomy](https://www.research-collection.ethz.ch/handle/20.500.11850/248154),” Master thesis, Jet Propulsion Laboratory, 4800 Oak Grove Drive, Pasadena, CA 91109, USA, December 2017.
+- 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.
 
 ```
-@mastersthesis{malyuta:2017mt,
-  author = {Danylo Malyuta},
-  title = {{Guidance, Navigation, Control and Mission Logic for Quadrotor Full-cycle Autonomy}},
-  language = {english},
-  type = {Master thesis},
-  school = {Jet Propulsion Laboratory},
-  address = {4800 Oak Grove Drive, Pasadena, CA 91109, USA},
-  month = dec,
-  year = {2017}
+@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},

apriltag_ros/CHANGELOG.rst

@@ -0,0 +1,27 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package apriltag_ros
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+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)
+------------------

apriltag_ros/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(apriltag_ros)
 
 find_package(catkin REQUIRED COMPONENTS
@@ -30,7 +30,13 @@ link_directories(${apriltag_LIBDIR})
 #  Release        : w/o debug symbols, w/ optimization
 #  RelWithDebInfo : w/ debug symbols, w/ optimization
 #  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
-# set(CMAKE_BUILD_TYPE Release)
+# 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()
+
 
 set(CMAKE_CXX_STANDARD 11)
 add_compile_options("-O3" "-funsafe-loop-optimizations" "-fsee" "-funroll-loops" "-fno-math-errno" "-funsafe-math-optimizations" "-ffinite-math-only" "-fno-signed-zeros")

apriltag_ros/config/settings.yaml

@@ -1,11 +1,12 @@
 # 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: tag36h11, tag25h9, tag16h5, tagStandard41h12
+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

apriltag_ros/include/apriltag_ros/common_functions.h

@@ -168,6 +168,9 @@ class TagDetector
   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_;
@@ -232,6 +235,8 @@ class TagDetector
 
   // 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

apriltag_ros/include/apriltag_ros/continuous_detector.h

@@ -55,7 +55,9 @@ namespace apriltag_ros
 class ContinuousDetector: public nodelet::Nodelet
 {
  public:
-   ContinuousDetector();
+  ContinuousDetector() = default;
+  ~ContinuousDetector() = default;
+
   void onInit();
 
   void imageCallback(const sensor_msgs::ImageConstPtr& image_rect,

apriltag_ros/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>apriltag_ros</name>
-  <version>3.1.0</version>
+  <version>3.1.2</version>
   <description>
     A ROS wrapper of the AprilTag 3 visual fiducial detection
     algorithm. Provides full access to the core AprilTag 3 algorithm's

apriltag_ros/src/common_functions.cpp

@@ -33,10 +33,14 @@
 #include "image_geometry/pinhole_camera_model.h"
 
 #include "common/homography.h"
+#include "tagStandard52h13.h"
 #include "tagStandard41h12.h"
 #include "tag36h11.h"
 #include "tag25h9.h"
 #include "tag16h5.h"
+#include "tagCustom48h12.h"
+#include "tagCircle21h7.h"
+#include "tagCircle49h12.h"
 
 namespace apriltag_ros
 {
@@ -48,6 +52,7 @@ TagDetector::TagDetector(ros::NodeHandle pnh) :
     blur_(getAprilTagOption<double>(pnh, "tag_blur", 0.0)),
     refine_edges_(getAprilTagOption<int>(pnh, "tag_refine_edges", 1)),
     debug_(getAprilTagOption<int>(pnh, "tag_debug", 0)),
+    max_hamming_distance_(getAprilTagOption<int>(pnh, "max_hamming_dist", 2)),
     publish_tf_(getAprilTagOption<bool>(pnh, "publish_tf", false))
 {
   // Parse standalone tag descriptions specified by user (stored on ROS
@@ -102,7 +107,15 @@ TagDetector::TagDetector(ros::NodeHandle pnh) :
 
   // Define the tag family whose tags should be searched for in the camera
   // images
-  if (family_ == "tag36h11")
+  if (family_ == "tagStandard52h13")
+  {
+    tf_ = tagStandard52h13_create();
+  }
+  else if (family_ == "tagStandard41h12")
+  {
+    tf_ = tagStandard41h12_create();
+  }
+  else if (family_ == "tag36h11")
   {
     tf_ = tag36h11_create();
   }
@@ -114,9 +127,17 @@ TagDetector::TagDetector(ros::NodeHandle pnh) :
   {
     tf_ = tag16h5_create();
   }
-  else if (family_ == "tagStandard41h12")
+  else if (family_ == "tagCustom48h12")
   {
-    tf_ = tagStandard41h12_create();
+    tf_ = tagCustom48h12_create();
+  }
+  else if (family_ == "tagCircle21h7")
+  {
+    tf_ = tagCircle21h7_create();
+  }
+  else if (family_ == "tagCircle49h12")
+  {
+    tf_ = tagCircle49h12_create();
   }
   else
   {
@@ -126,7 +147,7 @@ TagDetector::TagDetector(ros::NodeHandle pnh) :
 
   // Create the AprilTag 2 detector
   td_ = apriltag_detector_create();
-  apriltag_detector_add_family(td_, tf_);
+  apriltag_detector_add_family_bits(td_, tf_, max_hamming_distance_);
   td_->quad_decimate = (float)decimate_;
   td_->quad_sigma = (float)blur_;
   td_->nthreads = threads_;
@@ -152,7 +173,15 @@ TagDetector::~TagDetector() {
   apriltag_detections_destroy(detections_);
 
   // free memory associated with tag family
-  if (family_ == "tag36h11")
+  if (family_ == "tagStandard52h13")
+  {
+    tagStandard52h13_destroy(tf_);
+  }
+  else if (family_ == "tagStandard41h12")
+  {
+    tagStandard41h12_destroy(tf_);
+  }
+  else if (family_ == "tag36h11")
   {
     tag36h11_destroy(tf_);
   }
@@ -164,9 +193,17 @@ TagDetector::~TagDetector() {
   {
     tag16h5_destroy(tf_);
   }
-  else if (family_ == "tagStandard41h12")
+  else if (family_ == "tagCustom48h12")
   {
-    tagStandard41h12_destroy(tf_);
+    tagCustom48h12_destroy(tf_);
+  }
+  else if (family_ == "tagCircle21h7")
+  {
+    tagCircle21h7_destroy(tf_);
+  }
+  else if (family_ == "tagCircle49h12")
+  {
+    tagCircle49h12_destroy(tf_);
   }
 }
 
@@ -266,7 +303,7 @@ AprilTagDetectionArray TagDetector::detectTags (
     if (!findStandaloneTagDescription(tagID, standaloneDescription,
                                       !is_part_of_bundle))
     {
-      continue; 
+      continue;
     }
 
     //=================================================================
@@ -301,7 +338,7 @@ AprilTagDetectionArray TagDetector::detectTags (
                                                      fx, fy, cx, cy);
     Eigen::Matrix3d rot = transform.block(0, 0, 3, 3);
     Eigen::Quaternion<double> rot_quaternion(rot);
-    
+
     geometry_msgs::PoseWithCovarianceStamped tag_pose =
         makeTagPose(transform, rot_quaternion, image->header);
 
@@ -645,14 +682,14 @@ std::vector<TagBundleDescription > TagDetector::parseTagBundles (
     }
     TagBundleDescription bundle_i(bundleName);
     ROS_INFO("Loading tag bundle '%s'",bundle_i.name().c_str());
-    
+
     ROS_ASSERT(bundle_description["layout"].getType() ==
                XmlRpc::XmlRpcValue::TypeArray);
     XmlRpc::XmlRpcValue& member_tags = bundle_description["layout"];
 
     // Loop through each member tag of the bundle
     for (int32_t j=0; j<member_tags.size(); j++)
-    {      
+    {
       ROS_ASSERT(member_tags[j].getType() == XmlRpc::XmlRpcValue::TypeStruct);
       XmlRpc::XmlRpcValue& tag = member_tags[j];
 
@@ -667,9 +704,9 @@ std::vector<TagBundleDescription > TagDetector::parseTagBundles (
       StandaloneTagDescription* standaloneDescription;
       if (findStandaloneTagDescription(id, standaloneDescription, false))
       {
-        ROS_ASSERT(size == standaloneDescription->size()); 
+        ROS_ASSERT(size == standaloneDescription->size());
       }
-      
+
       // Get this tag's pose with respect to the bundle origin
       double x  = xmlRpcGetDoubleWithDefault(tag, "x", 0.);
       double y  = xmlRpcGetDoubleWithDefault(tag, "y", 0.);

apriltag_ros/src/continuous_detector.cpp

@@ -37,11 +37,6 @@ PLUGINLIB_EXPORT_CLASS(apriltag_ros::ContinuousDetector, nodelet::Nodelet);
 
 namespace apriltag_ros
 {
-
-ContinuousDetector::ContinuousDetector ()
-{
-}
-
 void ContinuousDetector::onInit ()
 {
   ros::NodeHandle& nh = getNodeHandle();
@@ -68,6 +63,17 @@ void ContinuousDetector::imageCallback (
     const sensor_msgs::ImageConstPtr& image_rect,
     const sensor_msgs::CameraInfoConstPtr& camera_info)
 {
+  // Lazy updates:
+  // When there are no subscribers _and_ when tf is not published,
+  // skip detection.
+  if (tag_detections_publisher_.getNumSubscribers() == 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