[fix] add qr_detector

qr_detector/CHANGELOG.rst

@@ -0,0 +1,8 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package qr_detector
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.0.0 (2017-05-01)
+------------------
+* QR detection wrapper on zbar implemented
+* Contributors: Michal Drwiega

qr_detector/CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 2.8.11)
+project(qr_detector)
+
+set(CMAKE_BUILD_TYPE Release)
+set(CMAKE_CXX_FLAGS "-std=c++11 -Wall ${CMAKE_CXX_FLAGS}")
+
+find_package(catkin REQUIRED COMPONENTS
+             roscpp nodelet sensor_msgs cv_bridge image_transport)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES qr_detector_nodelet
+  CATKIN_DEPENDS roscpp nodelet sensor_msgs cv_bridge image_transport
+)
+
+find_library(ZBAR_LIBRARIES NAMES zbar)
+
+include_directories(include ${catkin_INCLUDE_DIRS})
+
+add_library(qr_detector_nodelet src/qr_detector_nodelet.cpp src/detector.cpp)
+target_link_libraries(qr_detector_nodelet zbar ${catkin_LIBRARIES})
+
+install(TARGETS qr_detector_nodelet
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+	LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+install(DIRECTORY launch
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+install(FILES nodelets.xml
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

qr_detector/LICENSE

@@ -0,0 +1,27 @@
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2017-, Michal Drwiega (drwiega.michal@gmail.com)
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     1. Redistributions of source code must retain the above copyright
+ *        notice, this list of conditions and the following disclaimer.
+ *     2. Redistributions in binary form must reproduce the above copyright
+ *        notice, this list of conditions and the following disclaimer in the
+ *        documentation and/or other materials provided with the distribution.
+ *     3. Neither the name of the copyright holder nor the names of its
+ *        contributors may be used to endorse or promote products derived
+ *        from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+ * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

qr_detector/README.md

@@ -0,0 +1,11 @@
+## qr_detector
+The QR codes detector based on zbar library (http://zbar.sourceforge.net), dedicated to ROS systems.
+
+Installation of zbar library on Ubuntu
+`sudo apt install libzbar-dev`
+
+Subscribes:
+- **/image** (sensor_msgs/Image) - the topic with RGB images which contains QR codes.
+
+Publishes:
+- **/qr_codes** (std_msgs/String) - message from each detected QR code is published as a string.

qr_detector/include/qr_detector/detector.h

@@ -0,0 +1,37 @@
+#pragma once
+
+#include <string>
+#include <vector>
+#include <opencv2/opencv.hpp>
+#include "zbar.h"
+
+namespace qr_detector {
+
+struct Tag {
+  std::string message;
+  std::vector<cv::Point> polygon;
+};
+
+using Tags = std::vector<Tag>;
+
+/**
+ * Main QR codes detector class
+ */
+class Detector {
+ public:
+  Detector();
+  ~Detector() = default;
+
+  /**
+   * Detects tags in image.
+   *
+   * @param[in] image - image with qr tags
+   * @param[in] timeout - max time for tags detection in ms
+   */
+  Tags detect(const cv::Mat& image, size_t timeout = 100);
+
+ protected:
+  zbar::ImageScanner scanner_;
+};
+
+}

qr_detector/include/qr_detector/qr_detector_nodelet.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <image_transport/image_transport.h>
+
+#include "qr_detector/detector.h"
+
+namespace qr_detector {
+
+class QrDetectorNodelet : public nodelet::Nodelet {
+ public:
+  QrDetectorNodelet();
+  virtual ~QrDetectorNodelet();
+
+private:
+  void onInit() override;
+  void connectCallback();
+  void disconnectCallback();
+  void imageCallback(const sensor_msgs::ImageConstPtr& image);
+
+  ros::NodeHandle nh_;
+  image_transport::ImageTransport it_;
+  image_transport::Subscriber img_subscriber_;
+  ros::Publisher tags_publisher_;
+  Detector detector_;
+};
+
+}

qr_detector/launch/detector.launch

@@ -0,0 +1,24 @@
+<launch>
+    <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
+        <param name="video_device" value="/dev/video0" />
+        <param name="image_width" value="640" />
+        <param name="image_height" value="480" />
+        <param name="pixel_format" value="yuyv" />
+        <param name="camera_frame_id" value="usb_cam" />
+        <param name="io_method" value="mmap"/>
+    </node>
+
+    <node pkg="nodelet"
+          type="nodelet"
+          name="qr_detector"
+          args="standalone qr_detector/qr_detector_nodelet">
+        <remap from="image" to="usb_cam/image_raw"/>
+    </node>
+
+    <node
+        pkg="rqt_image_view"
+        type="rqt_image_view"
+        name="rqt_image_view"
+        output="screen"
+    />    
+</launch>

qr_detector/nodelets.xml

@@ -0,0 +1,10 @@
+<library path="lib/libqr_detector_nodelet">
+
+    <class name="qr_detector/qr_detector_nodelet"
+           type="qr_detector::QrDetectorNodelet"
+           base_class_type="nodelet::Nodelet">
+        <description>
+            QR detector nodelet.
+        </description>
+    </class>
+</library>

qr_detector/package.xml

@@ -0,0 +1,24 @@
+<package format="2">
+  <name>qr_detector</name>
+  <version>1.0.0</version>
+  <description>
+  QR codes detector based on zbar library (http://zbar.sourceforge.net)
+  </description>
+  <author email="drwiega.michal@gmail.com">Michal Drwiega (http://www.mdrwiega.com)</author>
+  <maintainer email="drwiega.michal@gmail.com">Michal Drwiega</maintainer>
+  <license>BSD</license>
+  <url type="website">http://ros.org/wiki/qr_detector</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>roscpp</depend>
+  <depend>nodelet</depend>
+  <depend>zbar</depend>
+  <depend>sensor_msgs</depend>
+  <depend>cv_bridge</depend>
+  <depend>image_transport</depend>
+
+  <export>
+    <nodelet plugin="${prefix}/nodelets.xml"/>
+  </export>
+</package>

qr_detector/src/detector.cpp

@@ -0,0 +1,38 @@
+#include "qr_detector/detector.h"
+
+#include <cv_bridge/cv_bridge.h>
+
+namespace qr_detector {
+
+Detector::Detector() : scanner_()
+{
+  scanner_.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);
+}
+
+Tags Detector::detect(const cv::Mat& image, size_t timeout)
+{
+  cv::Mat grayImg;
+  cv::cvtColor(image, grayImg, CV_BGR2GRAY);
+
+  const auto width = image.cols;
+  const auto height = image.rows;
+
+  zbar::Image img(width, height, "Y800", grayImg.data, width * height);
+  scanner_.scan(img);
+
+  Tags tags;
+  for (auto s = img.symbol_begin(); s != img.symbol_end(); ++s)
+  {
+    Tag tag;
+    tag.message = s->get_data();
+
+    for(int i = 0; i < s->get_location_size(); i++) {
+      tag.polygon.push_back(cv::Point(s->get_location_x(i), s->get_location_y(i)));
+    }
+    tags.push_back(tag);
+  }
+
+  return tags;
+}
+
+}

qr_detector/src/qr_detector_nodelet.cpp

@@ -0,0 +1,71 @@
+#include "qr_detector/qr_detector_nodelet.h"
+
+#include "pluginlib/class_list_macros.h"
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/image_encodings.h>
+#include "std_msgs/String.h"
+
+PLUGINLIB_EXPORT_CLASS(qr_detector::QrDetectorNodelet, nodelet::Nodelet);
+
+namespace qr_detector {
+
+QrDetectorNodelet::QrDetectorNodelet()
+    : it_(nh_)
+{ }
+
+QrDetectorNodelet::~QrDetectorNodelet()
+{
+  img_subscriber_.shutdown();
+}
+
+void QrDetectorNodelet::onInit()
+{
+  nh_ = getNodeHandle();
+
+  tags_publisher_ = nh_.advertise<std_msgs::String>("qr_codes", 10,
+                                                  std::bind(&QrDetectorNodelet::connectCallback, this),
+                                                  std::bind(&QrDetectorNodelet::disconnectCallback, this));
+
+  NODELET_INFO_STREAM("Initializing nodelet... [" << nh_.getNamespace() << "]");
+}
+
+void QrDetectorNodelet::connectCallback()
+{
+  if (!img_subscriber_ && tags_publisher_.getNumSubscribers() > 0)
+  {
+    NODELET_INFO("Connecting to image topic.");
+    img_subscriber_ = it_.subscribe("image", 1, &QrDetectorNodelet::imageCallback, this);
+  }
+}
+
+void QrDetectorNodelet::disconnectCallback()
+{
+  if (tags_publisher_.getNumSubscribers() == 0)
+  {
+    NODELET_INFO("Unsubscribing from image topic.");
+    img_subscriber_.shutdown();
+  }
+}
+
+void QrDetectorNodelet::imageCallback(const sensor_msgs::ImageConstPtr &image)
+{
+  cv_bridge::CvImageConstPtr cv_image;
+
+  try {
+    cv_image = cv_bridge::toCvShare(image, sensor_msgs::image_encodings::BGR8);
+  }
+  catch (cv_bridge::Exception& e) {
+    ROS_ERROR("cv_bridge exception: %s", e.what());
+    return;
+  }
+
+  auto tags = detector_.detect(cv_image->image, 10);
+  for (auto& tag : tags)
+  {
+    std_msgs::String msg;
+    msg.data = tag.message;
+    tags_publisher_.publish(msg);
+  }
+}
+
+}