From 6bbfdb746f66b0d3ea224faf5eb0922b0d9668d1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ga=C3=ABl=20=C3=89corchard?= Date: Tue, 22 Jan 2019 10:13:09 +0100 Subject: [PATCH 1/2] Add update_bundle and remove_bundle services --- apriltags2_ros/CMakeLists.txt | 5 + .../include/apriltags2_ros/common_functions.h | 25 +- .../apriltags2_ros/continuous_detector.h | 14 +- apriltags2_ros/package.xml | 2 + apriltags2_ros/src/common_functions.cpp | 352 ++++++++++++++++-- apriltags2_ros/src/continuous_detector.cpp | 16 + apriltags2_ros/srv/RemoveBundle.srv | 3 + apriltags2_ros/srv/UpdateBundle.srv | 6 + 8 files changed, 377 insertions(+), 46 deletions(-) create mode 100644 apriltags2_ros/srv/RemoveBundle.srv create mode 100644 apriltags2_ros/srv/UpdateBundle.srv diff --git a/apriltags2_ros/CMakeLists.txt b/apriltags2_ros/CMakeLists.txt index f436f3f2..97fbc50a 100644 --- a/apriltags2_ros/CMakeLists.txt +++ b/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) diff --git a/apriltags2_ros/include/apriltags2_ros/common_functions.h b/apriltags2_ros/include/apriltags2_ros/common_functions.h index 078cde40..04834374 100644 --- a/apriltags2_ros/include/apriltags2_ros/common_functions.h +++ b/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 +#include +#include +#include #include +#include #include -#include #include #include @@ -61,6 +63,7 @@ #include #include #include +#include #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 id2idx_; // (id2idx_[]=) 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 bundleIds () { std::vector 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 standalone_tag_descriptions_; - std::vector tag_bundle_descriptions_; + std::list 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 parseStandaloneTags( XmlRpc::XmlRpcValue& standalone_tag_descriptions); - std::vector parseTagBundles( + std::list 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( diff --git a/apriltags2_ros/include/apriltags2_ros/continuous_detector.h b/apriltags2_ros/include/apriltags2_ros/continuous_detector.h index 435fee82..412e0de6 100644 --- a/apriltags2_ros/include/apriltags2_ros/continuous_detector.h +++ b/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 #include +#include +#include +#include + 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 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 diff --git a/apriltags2_ros/package.xml b/apriltags2_ros/package.xml index 9821c8c9..973413b6 100644 --- a/apriltags2_ros/package.xml +++ b/apriltags2_ros/package.xml @@ -39,6 +39,7 @@ eigen libopencv-dev cmake_modules + libyaml-cpp-dev tf apriltags2 @@ -54,6 +55,7 @@ pluginlib eigen libopencv-dev + libyaml-cpp-dev diff --git a/apriltags2_ros/src/common_functions.cpp b/apriltags2_ros/src/common_functions.cpp index 8e6f674c..196b0631 100644 --- a/apriltags2_ros/src/common_functions.cpp +++ b/apriltags2_ros/src/common_functions.cpp @@ -31,6 +31,10 @@ #include "apriltags2_ros/common_functions.h" #include "image_geometry/pinhole_camera_model.h" +#include +#include +#include +#include #include "common/homography.h" #include "tag36h11.h" @@ -87,6 +91,7 @@ TagDetector::TagDetector(ros::NodeHandle pnh) : { try { + std::lock_guard lock{tag_bundle_descriptions_mutex_}; tag_bundle_descriptions_ = parseTagBundles(tag_bundle_descriptions); } catch(XmlRpc::XmlRpcException e) @@ -243,11 +248,10 @@ AprilTagDetectionArray TagDetector::detectTags ( // 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 lock{tag_bundle_descriptions_mutex_}; + // Iterate over the registered bundles. + for (auto & bundle : tag_bundle_descriptions_) { - // 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 @@ -274,7 +278,7 @@ AprilTagDetectionArray TagDetector::detectTags ( if (!findStandaloneTagDescription(tagID, standaloneDescription, !is_part_of_bundle)) { - continue; + continue; } //================================================================= @@ -309,7 +313,7 @@ AprilTagDetectionArray TagDetector::detectTags ( fx, fy, cx, cy); Eigen::Matrix3d rot = transform.block(0, 0, 3, 3); Eigen::Quaternion rot_quaternion(rot); - + geometry_msgs::PoseWithCovarianceStamped tag_pose = makeTagPose(transform, rot_quaternion, image->header); @@ -326,36 +330,35 @@ AprilTagDetectionArray TagDetector::detectTags ( // Estimate bundle origin pose for each bundle in which at least one // member tag was detected - for (unsigned int j=0; j >::iterator it = - bundleObjectPoints.find(bundleName); - if (it != bundleObjectPoints.end()) + std::lock_guard lock{tag_bundle_descriptions_mutex_}; + for (auto & bundle : tag_bundle_descriptions_) { - // Some member tags of this bundle were detected, get the bundle's - // position! - TagBundleDescription& bundle = tag_bundle_descriptions_[j]; + // Get bundle name + std::string bundleName = bundle.name(); - Eigen::Matrix4d transform = + std::map >::iterator it = + bundleObjectPoints.find(bundleName); + if (it != bundleObjectPoints.end()) + { + 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); + bundleImagePoints[bundleName], fx, fy, cx, cy); + Eigen::Matrix3d rot = transform.block(0, 0, 3, 3); + Eigen::Quaternion rot_quaternion(rot); - geometry_msgs::PoseWithCovarianceStamped bundle_pose = + geometry_msgs::PoseWithCovarianceStamped bundle_pose = makeTagPose(transform, rot_quaternion, image->header); - // Add the detection to the back of the tag detection array - 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()); + // Add the detection to the back of the tag detection array + 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()); + } } } @@ -517,9 +520,9 @@ void TagDetector::drawDetections (cv_bridge::CvImagePtr image) // Check if is part of a tag bundle int tagID = det->id; bool is_part_of_bundle = false; - for (unsigned int j=0; j lock{tag_bundle_descriptions_mutex_}; + for (auto & bundle : tag_bundle_descriptions_) { - TagBundleDescription bundle = tag_bundle_descriptions_[j]; if (bundle.id2idx_.find(tagID) != bundle.id2idx_.end()) { is_part_of_bundle = true; @@ -625,13 +628,13 @@ std::map TagDetector::parseStandaloneTags ( } // parse tag bundle descriptions -std::vector TagDetector::parseTagBundles ( +std::list TagDetector::parseTagBundles ( XmlRpc::XmlRpcValue& tag_bundles) { - std::vector descriptions; + std::list descriptions; ROS_ASSERT(tag_bundles.getType() == XmlRpc::XmlRpcValue::TypeArray); - // Loop through all tag bundle descritions + // Loop through all tag bundle descriptions for (int32_t i=0; i 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 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.); @@ -708,6 +711,145 @@ std::vector TagDetector::parseTagBundles ( return descriptions; } +/** Add or update one or more bundles + * + * @param[in] bundles_description Tag bundle description in yaml format for one + * one more bundles. + * Must be a sequence, even with one bundle. + * Example: + * "[{name: b1, layout: [{id: 42, size: 0.05, + * x: 1, y: 2, z: 3, qw: 1, qx: 0, qy: 0, qz: 0}, + * {id: 43, size: 0.05, x: 1, y: 2, z: 3, + * qw: 1, qx: 0, qy: 0, qz: 0}]}, + * {layout: [{id: 44, size: 0.05, x: 1, y: 2, + * z: 3, qw: 1, qx: 0, qy: 0, qz: 0}, {id: 45, + * size: 0.05, x: 1, y: 2, z: 3, qw: 1, + * qx: 0, qy: 0, qz: 0}]}]". + * Due to a design choice of the underlying + * yaml-cpp library, there must be a space after + * field names, e.g. "id: 42" is correct, + * whereas "id:42" is not. + */ +void TagDetector::updateBundle(const std::string & bundles_description) +{ + YAML::Node bundles_node = YAML::Load(bundles_description); + if (!bundles_node.IsSequence()) + { + ROS_WARN("Error parsing bundles description, description is not a sequence"); + return; + } + + for (auto bundle_node : bundles_node) + { + if (!bundle_node.IsMap()) + { + ROS_WARN("Error parsing bundle description, bundle is not a map, ignoring this bundle"); + continue; + } + + TagBundleDescription bundle_description; + if (!parseYamlBundle(bundle_node, bundle_description)) + { + continue; + } + + /* We acquire the lock now, to avoid insertion of a bundle between getting + * the next available bundle name and the insertion of the current bundle. */ + std::lock_guard lock{tag_bundle_descriptions_mutex_}; + /* Remove a possibly already existing bundle. */ + const std::string bundle_name = bundle_description.name(); + const auto & bundle_with_same_name = std::find_if(tag_bundle_descriptions_.begin(), + tag_bundle_descriptions_.end(), + [bundle_name] (const TagBundleDescription bundle) {return bundle.name() == bundle_name;}); + if (bundle_with_same_name != tag_bundle_descriptions_.end()) + { + /* We already have a bundle with the same name, remove it. */ + ROS_WARN_STREAM("A bundle with the name \"" << bundle_name << "\" already existed and will be updated"); + tag_bundle_descriptions_.erase(bundle_with_same_name); + } + if (bundle_name.empty()) + { + bundle_description.setName(nextAvailableBundleName()); + ROS_WARN_STREAM("Bundle has no name, setting to \"" << bundle_description.name() << "\""); + } + + tag_bundle_descriptions_.push_back(bundle_description); + } + /* Print outs the current list of bundles. */ + if (tag_bundle_descriptions_.empty()) + { + ROS_DEBUG("No bundle in database"); + } + else + { + ROS_DEBUG("current bundle list:"); + for (auto & bundle : tag_bundle_descriptions_) + { + ROS_DEBUG_STREAM(" " << bundle.name()); + } + } +} + +/** Remove an existing bundle. + */ +void TagDetector::removeBundle(const std::string & bundle_name) +{ + std::lock_guard lock{tag_bundle_descriptions_mutex_}; + const auto & bundle_with_this_name = std::find_if(tag_bundle_descriptions_.begin(), + tag_bundle_descriptions_.end(), + [bundle_name] (const TagBundleDescription bundle) {return bundle.name() == bundle_name;}); + if (bundle_with_this_name != tag_bundle_descriptions_.end()) + { + /* We have a bundle with this name, remove it. */ + tag_bundle_descriptions_.erase(bundle_with_this_name); + } + else + { + ROS_DEBUG_STREAM("No bundle with the name \"" << bundle_name << "\" existed, doing nothing"); + } + /* Print outs the current list of bundles. */ + if (tag_bundle_descriptions_.empty()) + { + ROS_DEBUG("No bundle in database"); + } + else + { + ROS_DEBUG("current bundle list:"); + for (auto & bundle : tag_bundle_descriptions_) + { + ROS_DEBUG_STREAM(" " << bundle.name()); + } + } +} + +/** Returns the first available name with format "bundle_[0-9]+" + * + * This function does NOT lock tag_bundle_descriptions_mutex_. + */ +std::string TagDetector::nextAvailableBundleName() +{ + size_t i = 1; + while (true) + { + std::stringstream bundle_name_stream; + bundle_name_stream << "bundle_" << i; + bool free_slot = true; + for (auto & bundle : tag_bundle_descriptions_) + { + if (bundle_name_stream.str() == bundle.name()) + { + free_slot = false; + break; + } + } + if (free_slot) + { + return bundle_name_stream.str(); + } + i++; + } +} + double TagDetector::xmlRpcGetDouble (XmlRpc::XmlRpcValue& xmlValue, std::string field) const { @@ -766,4 +908,138 @@ bool TagDetector::findStandaloneTagDescription ( return true; } +bool TagDetector::parseYamlBundle(const YAML::Node & bundle_node, TagBundleDescription & bundle_description) +{ + std::string bundle_name; + if (bundle_node["name"]) + { + try + { + bundle_name = bundle_node["name"].as(); + } + catch (YAML::ParserException & e) + { + ROS_WARN_STREAM(e.msg); + return false; + } + } + bundle_description.setName(bundle_name); + + if (!bundle_node["layout"]) + { + ROS_WARN("layout must be provided, ignoring this bundle"); + return false; + } + + YAML::Node layout_node = bundle_node["layout"]; + + if (!layout_node.IsSequence()) + { + ROS_WARN("layout must be a sequence, ignoring this bundle"); + return false; + } + + /* Loop through each member tag of the bundle. */ + for (auto tag_node : layout_node) + { + if (!tag_node.IsMap()) + { + ROS_WARN("Member tag must be a map, ignoring this bundle"); + return false; + } + + if (!tag_node["id"] + or !tag_node["size"] + or !tag_node["x"] + or !tag_node["y"] + or !tag_node["z"] + or !tag_node["qx"] + or !tag_node["qy"] + or !tag_node["qz"] + or !tag_node["qw"]) + { + ROS_WARN("Member tag must have fields id, size, x, y, z, qx, qy, qz, qw, ignoring this bundle"); + return false; + } + + if (!tag_node["id"].IsScalar() + or !tag_node["size"].IsScalar() + or !tag_node["x"].IsScalar() + or !tag_node["y"].IsScalar() + or !tag_node["z"].IsScalar() + or !tag_node["qx"].IsScalar() + or !tag_node["qy"].IsScalar() + or !tag_node["qz"].IsScalar() + or !tag_node["qw"].IsScalar()) + { + ROS_WARN("Member tag fields must be scalars, ignoring this bundle"); + return false; + } + + int id; + try + { + id = tag_node["id"].as(); + } + catch (YAML::Exception & e) + { + ROS_WARN("Member tag field id must be a positive integer, ignoring this bundle"); + return false; + } + if (id < 0) + { + ROS_WARN("Member tag field id must be positive, ignoring this bundle"); + return false; + } + + double size; + double x; + double y; + double z; + double qx; + double qy; + double qz; + double qw; + try + { + size = tag_node["size"].as(); + x = tag_node["x"].as(); + y = tag_node["y"].as(); + z = tag_node["z"].as(); + qx = tag_node["qx"].as(); + qy = tag_node["qy"].as(); + qz = tag_node["qz"].as(); + qw = tag_node["qw"].as(); + } + catch (YAML::Exception & e) + { + ROS_WARN("Member tag fields size, x, y, z, qx, qy, qz, qw must be floats, ignoring this bundle"); + return false; + } + + /* Make sure that if the tag is also specified also as standalone, then + * the sizes match. */ + StandaloneTagDescription* standaloneDescription; + if (findStandaloneTagDescription(id, standaloneDescription, false) and + (size != standaloneDescription->size())) + { + ROS_WARN_STREAM("A tag with id \"" << id << "\" with different size already exists, ignoring this bundle"); + return false; + } + + Eigen::Quaterniond q_tag(qw, qx, qy, qz); + q_tag.normalize(); + Eigen::Matrix3d r_oi = q_tag.toRotationMatrix(); + + /* Build the rigid transform from this tag to the bundle origin. */ + cv::Matx44d t_oi(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); + + bundle_description.addMemberTag(id, size, t_oi); + } + return true; +} + } // namespace apriltags2_ros diff --git a/apriltags2_ros/src/continuous_detector.cpp b/apriltags2_ros/src/continuous_detector.cpp index 1cd4f3dd..d8a00a12 100644 --- a/apriltags2_ros/src/continuous_detector.cpp +++ b/apriltags2_ros/src/continuous_detector.cpp @@ -62,6 +62,9 @@ void ContinuousDetector::onInit () { tag_detections_image_publisher_ = it_->advertise("tag_detections_image", 1); } + + update_bundle_server_ = pnh.advertiseService("update_bundle", &ContinuousDetector::updateBundle, this); + remove_bundle_server_ = pnh.advertiseService("remove_bundle", &ContinuousDetector::removeBundle, this); } void ContinuousDetector::imageCallback ( @@ -94,4 +97,17 @@ void ContinuousDetector::imageCallback ( } } +bool ContinuousDetector::updateBundle(UpdateBundle::Request & req, UpdateBundle::Response & /* res */) +{ + tag_detector_->updateBundle(req.tag_bundles.data); + return true; +} + +bool ContinuousDetector::removeBundle(RemoveBundle::Request & req, RemoveBundle::Response & /* res */) +{ + tag_detector_->removeBundle(req.bundle_name.data); + return true; +} + + } // namespace apriltags2_ros diff --git a/apriltags2_ros/srv/RemoveBundle.srv b/apriltags2_ros/srv/RemoveBundle.srv new file mode 100644 index 00000000..476e7214 --- /dev/null +++ b/apriltags2_ros/srv/RemoveBundle.srv @@ -0,0 +1,3 @@ +# Name of the bundle to remove +std_msgs/String bundle_name +--- diff --git a/apriltags2_ros/srv/UpdateBundle.srv b/apriltags2_ros/srv/UpdateBundle.srv new file mode 100644 index 00000000..ac9437e1 --- /dev/null +++ b/apriltags2_ros/srv/UpdateBundle.srv @@ -0,0 +1,6 @@ +# Tag bundle description in yaml format. +# Example: +# "[{name: b1, layout: [{id: 42, size: 0.05, x: 1, y: 2, z: 3, qw: 1, qx: 0, qy: 0, qz: 0}, {id: 43, size: 0.05, x: 1, y: 2, z: 3, qw: 1, qx: 0, qy: 0, qz: 0}]},{layout: [{id: 44, size: 0.05, x: 1, y: 2, z: 3, qw: 1, qx: 0, qy: 0, qz: 0}, {id: 45, size: 0.05, x: 1, y: 2, z: 3, qw: 1, qx: 0, qy: 0, qz: 0}]}]" +# Due to a design choice of the underlying yaml-cpp library, there must be a space after field names, e.g. "id: 42" is correct, whereas "id:42" is not. +std_msgs/String tag_bundles +--- From 1a7e97885663a932261aefb6ac07405baa07deea Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ga=C3=ABl=20=C3=89corchard?= Date: Wed, 23 Jan 2019 14:17:42 +0100 Subject: [PATCH 2/2] Use string instead of std_msgs/String in services --- apriltags2_ros/src/continuous_detector.cpp | 4 ++-- apriltags2_ros/srv/RemoveBundle.srv | 2 +- apriltags2_ros/srv/UpdateBundle.srv | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/apriltags2_ros/src/continuous_detector.cpp b/apriltags2_ros/src/continuous_detector.cpp index d8a00a12..32271f82 100644 --- a/apriltags2_ros/src/continuous_detector.cpp +++ b/apriltags2_ros/src/continuous_detector.cpp @@ -99,13 +99,13 @@ void ContinuousDetector::imageCallback ( bool ContinuousDetector::updateBundle(UpdateBundle::Request & req, UpdateBundle::Response & /* res */) { - tag_detector_->updateBundle(req.tag_bundles.data); + tag_detector_->updateBundle(req.tag_bundles); return true; } bool ContinuousDetector::removeBundle(RemoveBundle::Request & req, RemoveBundle::Response & /* res */) { - tag_detector_->removeBundle(req.bundle_name.data); + tag_detector_->removeBundle(req.bundle_name); return true; } diff --git a/apriltags2_ros/srv/RemoveBundle.srv b/apriltags2_ros/srv/RemoveBundle.srv index 476e7214..16612249 100644 --- a/apriltags2_ros/srv/RemoveBundle.srv +++ b/apriltags2_ros/srv/RemoveBundle.srv @@ -1,3 +1,3 @@ # Name of the bundle to remove -std_msgs/String bundle_name +string bundle_name --- diff --git a/apriltags2_ros/srv/UpdateBundle.srv b/apriltags2_ros/srv/UpdateBundle.srv index ac9437e1..9a39a2de 100644 --- a/apriltags2_ros/srv/UpdateBundle.srv +++ b/apriltags2_ros/srv/UpdateBundle.srv @@ -2,5 +2,5 @@ # Example: # "[{name: b1, layout: [{id: 42, size: 0.05, x: 1, y: 2, z: 3, qw: 1, qx: 0, qy: 0, qz: 0}, {id: 43, size: 0.05, x: 1, y: 2, z: 3, qw: 1, qx: 0, qy: 0, qz: 0}]},{layout: [{id: 44, size: 0.05, x: 1, y: 2, z: 3, qw: 1, qx: 0, qy: 0, qz: 0}, {id: 45, size: 0.05, x: 1, y: 2, z: 3, qw: 1, qx: 0, qy: 0, qz: 0}]}]" # Due to a design choice of the underlying yaml-cpp library, there must be a space after field names, e.g. "id: 42" is correct, whereas "id:42" is not. -std_msgs/String tag_bundles +string tag_bundles ---