Add update_bundle and remove_bundle services

apriltags2_ros/CMakeLists.txt

@@ -19,6 +19,7 @@ find_package(catkin REQUIRED COMPONENTS
 
 find_package(Eigen REQUIRED)
 find_package(OpenCV REQUIRED)
+find_package(yaml-cpp REQUIRED)
 
 # Set the build type.  Options are:
 #  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
@@ -51,6 +52,8 @@ add_message_files(
 add_service_files(
   FILES
   AnalyzeSingleImage.srv
+  RemoveBundle.srv
+  UpdateBundle.srv
 )
 
 generate_messages(
@@ -74,6 +77,7 @@ include_directories(include
   ${catkin_INCLUDE_DIRS}
   ${Eigen_INCLUDE_DIRS}
   ${OpenCV_INCLUDE_DIRS}
+  ${YAML_CPP_INCLUDE_DIRS}
 )
 
 add_library(common src/common_functions.cpp)
@@ -82,6 +86,7 @@ target_link_libraries(common
   ${catkin_LIBRARIES}
   ${Eigen_LIBRARIES}
   ${OpenCV_LIBRARIES}
+  ${YAML_CPP_LIBRARIES}
 )
 
 add_library(continuous_detector src/continuous_detector.cpp)

apriltags2_ros/include/apriltags2_ros/common_functions.h

@@ -44,10 +44,12 @@
 #ifndef APRILTAGS2_ROS_COMMON_FUNCTIONS_H
 #define APRILTAGS2_ROS_COMMON_FUNCTIONS_H
 
-#include <string>
+#include <list>
+#include <map>
+#include <mutex>
 #include <sstream>
+#include <string>
 #include <vector>
-#include <map>
 
 #include <ros/ros.h>
 #include <ros/console.h>
@@ -61,6 +63,7 @@
 #include <image_transport/image_transport.h>
 #include <sensor_msgs/image_encodings.h>
 #include <tf/transform_broadcaster.h>
+#include <yaml-cpp/yaml.h>
 
 #include "apriltags2_ros/AprilTagDetection.h"
 #include "apriltags2_ros/AprilTagDetectionArray.h"
@@ -91,7 +94,7 @@ class StandaloneTagDescription
  public:
   StandaloneTagDescription() {};
   StandaloneTagDescription(int id, double size,
-                           std::string &frame_name) :
+                           const std::string &frame_name) :
       id_(id),
       size_(size),
       frame_name_(frame_name) {}
@@ -112,6 +115,7 @@ class TagBundleDescription
  public:
   std::map<int, int > id2idx_; // (id2idx_[<tag ID>]=<index in tags_>) mapping
 
+  TagBundleDescription() {};
   TagBundleDescription(std::string name) :
       name_(name) {}
 
@@ -125,6 +129,8 @@ class TagBundleDescription
   }
 
   std::string name () const { return name_; }
+  void setName(const std::string & name) {name_ = name;};
+
   // Get IDs of bundle member tags
   std::vector<int> bundleIds () {
     std::vector<int> ids;
@@ -160,6 +166,9 @@ class TagDetector
   // Remove detections of tags with the same ID
   void removeDuplicates();
 
+  std::string nextAvailableBundleName();
+
+  bool parseYamlBundle(const YAML::Node & bundle_node, TagBundleDescription & bundle_description);
   // AprilTags 2 code's attributes
   std::string family_;
   int border_;
@@ -178,7 +187,8 @@ class TagDetector
 
   // Other members
   std::map<int, StandaloneTagDescription> standalone_tag_descriptions_;
-  std::vector<TagBundleDescription > tag_bundle_descriptions_;
+  std::list<TagBundleDescription > tag_bundle_descriptions_;
+  std::mutex tag_bundle_descriptions_mutex_;
   bool remove_duplicates_;
   bool run_quietly_;
   bool publish_tf_;
@@ -193,8 +203,13 @@ class TagDetector
   // Store standalone and bundle tag descriptions
   std::map<int, StandaloneTagDescription> parseStandaloneTags(
       XmlRpc::XmlRpcValue& standalone_tag_descriptions);
-  std::vector<TagBundleDescription > parseTagBundles(
+  std::list<TagBundleDescription > parseTagBundles(
       XmlRpc::XmlRpcValue& tag_bundles);
+  /** Add or update one or more bundles
+   */
+  void updateBundle(const std::string & bundles_description);
+  void removeBundle(const std::string & bundle_name);
+
   double xmlRpcGetDouble(
       XmlRpc::XmlRpcValue& xmlValue, std::string field) const;
   double xmlRpcGetDoubleWithDefault(

apriltags2_ros/include/apriltags2_ros/continuous_detector.h

@@ -33,7 +33,7 @@
  * Wrapper class of TagDetector class which calls TagDetector::detectTags on
  * each newly arrived image published by a camera.
  *
- * $Revision: 1.0 $
+ * $Revision: 1.1 $
  * $Date: 2017/12/17 13:25:52 $
  * $Author: dmalyuta $
  *
@@ -43,12 +43,14 @@
 #ifndef APRILTAGS2_ROS_CONTINUOUS_DETECTOR_H
 #define APRILTAGS2_ROS_CONTINUOUS_DETECTOR_H
 
-#include "apriltags2_ros/common_functions.h"
-
 #include <memory>
 
 #include <nodelet/nodelet.h>
 
+#include <apriltags2_ros/common_functions.h>
+#include <apriltags2_ros/RemoveBundle.h>
+#include <apriltags2_ros/UpdateBundle.h>
+
 namespace apriltags2_ros
 {
 
@@ -61,6 +63,10 @@ class ContinuousDetector: public nodelet::Nodelet
   void imageCallback(const sensor_msgs::ImageConstPtr& image_rect,
                      const sensor_msgs::CameraInfoConstPtr& camera_info);
 
+  /* Services to add/update or remove a bundle. */
+  bool updateBundle(UpdateBundle::Request & req, UpdateBundle::Response & res);
+  bool removeBundle(RemoveBundle::Request & req, RemoveBundle::Response & res);
+
  private:
   std::shared_ptr<TagDetector> tag_detector_;
   bool draw_tag_detections_image_;
@@ -70,6 +76,8 @@ class ContinuousDetector: public nodelet::Nodelet
   image_transport::CameraSubscriber camera_image_subscriber_;
   image_transport::Publisher tag_detections_image_publisher_;
   ros::Publisher tag_detections_publisher_;
+  ros::ServiceServer update_bundle_server_;
+  ros::ServiceServer remove_bundle_server_;
 };
 
 } // namespace apriltags2_ros

apriltags2_ros/package.xml

@@ -39,6 +39,7 @@
   <build_depend>eigen</build_depend>
   <build_depend>libopencv-dev</build_depend>
   <build_depend>cmake_modules</build_depend>
+  <build_depend>libyaml-cpp-dev</build_depend>
 
   <run_depend>tf</run_depend>
   <run_depend>apriltags2</run_depend>
@@ -54,6 +55,7 @@
   <run_depend>pluginlib</run_depend>
   <run_depend>eigen</run_depend>
   <run_depend>libopencv-dev</run_depend>
+  <run_depend>libyaml-cpp-dev</run_depend>
 
   <export>
     <nodelet plugin="${prefix}/nodelet_plugins.xml" />