diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 81194f3..373021c 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -16,4 +16,4 @@ jobs: uses: ros-misc-utilities/ros_build_scripts/.github/workflows/ros2_recent_ci.yml@master with: repo: ${{ github.event.repository.name }} - vcs_url: https://raw.githubusercontent.com/${{ github.repository }}/master/${{ github.event.repository.name }}.repos + vcs_url: https://raw.githubusercontent.com/${{ github.repository }}/${{ github.head_ref || github.ref_name }}/${{ github.event.repository.name }}.repos diff --git a/apriltag_detector/include/apriltag_detector/detector_component.hpp b/apriltag_detector/include/apriltag_detector/detector_component.hpp index 2bae492..c48df79 100644 --- a/apriltag_detector/include/apriltag_detector/detector_component.hpp +++ b/apriltag_detector/include/apriltag_detector/detector_component.hpp @@ -41,6 +41,8 @@ class DetectorComponent : public rclcpp::Node const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); ~DetectorComponent(); auto getNumMessages() const { return (num_messages_); } + auto getNumTagsDetected() const { return (num_tags_detected_); } + void resetNumTagsDetected() { num_tags_detected_ = 0; } private: void subscribe(); @@ -55,6 +57,7 @@ class DetectorComponent : public rclcpp::Node std::string image_qos_profile_{"default"}; std::string in_transport_{"raw"}; std::size_t num_messages_{0}; + std::size_t num_tags_detected_{0}; pluginlib::ClassLoader detector_loader_; std::shared_ptr detector_; }; diff --git a/apriltag_detector/src/detector_component.cpp b/apriltag_detector/src/detector_component.cpp index fb0488b..cf3bbe7 100644 --- a/apriltag_detector/src/detector_component.cpp +++ b/apriltag_detector/src/detector_component.cpp @@ -160,6 +160,7 @@ void DetectorComponent::callback( std::make_unique(); detector_->detect(cvImg->image, array_msg.get()); array_msg->header = msg->header; + num_tags_detected_ += array_msg->detections.size(); detect_pub_->publish(std::move(array_msg)); } } diff --git a/apriltag_tools/src/detect_from_bag.cpp b/apriltag_tools/src/detect_from_bag.cpp index 6dab3e8..87a5b7c 100644 --- a/apriltag_tools/src/detect_from_bag.cpp +++ b/apriltag_tools/src/detect_from_bag.cpp @@ -61,6 +61,7 @@ int main(int argc, char ** argv) rclcpp::shutdown(); throw std::runtime_error("must specify image_topic parameter!"); } + dummy_node.reset(); // delete dummy node object rclcpp::executors::SingleThreadedExecutor exec; @@ -151,7 +152,9 @@ int main(int argc, char ** argv) exec.spin_some(); // for recording the last message RCLCPP_INFO_STREAM( - get_logger(), "num messages detected: " << detector_node->getNumMessages()); + get_logger(), "num messages found: " << detector_node->getNumMessages()); + RCLCPP_INFO_STREAM( + get_logger(), "num tags detected: " << detector_node->getNumTagsDetected()); RCLCPP_INFO(get_logger(), "detect_from_bag finished!"); rclcpp::shutdown(); return 0;