diff --git a/apriltag_tools/README.md b/apriltag_tools/README.md index f83256b..ca882d0 100644 --- a/apriltag_tools/README.md +++ b/apriltag_tools/README.md @@ -12,6 +12,8 @@ This node runs an apriltag detector on a bag with images and produces an output - Parameters: - ``in_bag``: path of input bag - ``image_topic``: name of image topic on which to run the AprilTag detector + - ``image_tag_topic``: output topic for images with detected AprilTags shown + - ``tag_topic``: output topic for detected tags - ``out_bag``: path of output bag Example: diff --git a/apriltag_tools/src/detect_from_bag.cpp b/apriltag_tools/src/detect_from_bag.cpp index 5a34b50..cf2439f 100644 --- a/apriltag_tools/src/detect_from_bag.cpp +++ b/apriltag_tools/src/detect_from_bag.cpp @@ -56,6 +56,10 @@ int main(int argc, char ** argv) auto dummy_node = std::make_shared("dummy_node"); const std::string image_topic = dummy_node->declare_parameter("image_topic", ""); + const std::string image_tag_topic = + dummy_node->declare_parameter("image_tag_topic", "image_tags"); + const std::string tag_topic = + dummy_node->declare_parameter("tag_topic", "tags"); if (image_topic.empty()) { RCLCPP_ERROR(get_logger(), "must specify image_topic parameter!"); rclcpp::shutdown(); @@ -77,15 +81,14 @@ int main(int argc, char ** argv) std::make_shared(detector_options); exec.add_node(detector_node); - const std::vector draw_remap = { "--ros-args", "--remap", "image:=" + image_topic, "--remap", - "tags:=" + image_topic + "/tags", + "tags:=" + image_topic + "/" + tag_topic, "--remap", - "image_tags:=" + image_topic + "/image_tags"}; + "image_tags:=" + image_topic + "/" + image_tag_topic}; rclcpp::NodeOptions draw_options; draw_options.arguments(draw_remap); @@ -117,8 +120,8 @@ int main(int argc, char ** argv) const auto topic_set = player_node->getTopics(); std::vector topics; std::copy(topic_set.begin(), topic_set.end(), std::back_inserter(topics)); - topics.push_back(image_topic + "/tags"); - topics.push_back(image_topic + "/image_tags"); + topics.push_back(image_topic + "/" + tag_topic); + topics.push_back(image_topic + "/" + image_tag_topic); const std::string out_uri = detector_node->get_parameter_or("out_bag", std::string());