Add ROS output for the computer vision

time_stamper_cv/include/configuration.h

@@ -41,10 +41,28 @@ struct LedRowConfig {
 typedef std::map<std::string, LedRowConfig> LedRowConfigRepository;
 
 struct ImageProcessorConfig {
+  inline static constexpr const char *TOP_ROW{"TopRow"};
+  inline static constexpr const char *BOTTOM_ROW{"BottomRow"};
+
   int tolerance{10};
   cv::SimpleBlobDetector::Params params;
   LedRowConfigRepository led_row_config {
-      {"BottomRow", {{0, 0}, {6, 0}, 16}},
-      {"TopRow", {{0, 17}, {6, 0}, 10}}
+      {BOTTOM_ROW, {{0, 0}, {6, 0}, 16}},
+      {TOP_ROW, {{0, 17}, {6, 0}, 10}}
   };
+
+  /**
+   * Get row names
+   * @return string vector
+   */
+ public:
+  std::vector<std::string> rows() const {
+    std::vector<std::string> res{};
+
+    std::map<std::string, LedRowConfig>::const_iterator it;
+    for (it = led_row_config.begin() ; it != led_row_config.end(); it++) {
+      res.push_back(it->first);
+    }
+    return res;
+  }
 };

time_stamper_cv/include/convex_shape.h

@@ -1,7 +1,9 @@
 #pragma once
 #include <vector>
 #include <functional>
+
 #include <opencv2/core/types.hpp>
+
 #include "common.h"
 
 /**

time_stamper_cv/include/image_processor.h

@@ -1,10 +1,14 @@
 #pragma once
+#include <vector>
+
 #include "opencv2/opencv.hpp"
+#include "cv_bridge/cv_bridge.h"
+
 #include "convex_shape.h"
 #include "configuration.h"
-#include "cv_bridge/cv_bridge.h"
 #include "led_state_parser.h"
 #include "keypoint_detector.h"
+#include "time_stamper_cv/Ledstate.h"
 
 class ImageProcessor {
  public:
@@ -27,6 +31,11 @@ class ImageProcessor {
    */
   void setVisualization(bool visualization);
 
+  /**
+   * Fills in message to be published without header
+   */
+  void fillInLedStateMessage(time_stamper_cv::Ledstate *led_msg);
+
   /**
    * Default destructor
    */
@@ -41,6 +50,7 @@ class ImageProcessor {
   std::shared_ptr<KeyPointDetector> detector_;
   std::shared_ptr<ConvexShape> convex_shape_{};
   std::shared_ptr<LedStateParser> led_parser_{};
+  ImageProcessorConfig cfg_;
 
   bool visualization_{false};
   static constexpr const char *OPENCV_WINDOW{"Visualization"};

time_stamper_cv/include/keypoint_detector.h

@@ -1,7 +1,10 @@
 #pragma once
-#include "opencv2/opencv.hpp"
+
 #include <functional>
 
+#include "opencv2/opencv.hpp"
+
+
 class KeyPointDetector {
  public:
   explicit KeyPointDetector(cv::SimpleBlobDetector::Params params);

time_stamper_cv/include/led_detection_node.h

@@ -1,7 +1,9 @@
 #pragma once
+
 #include "ros/ros.h"
 #include "cv_bridge/cv_bridge.h"
 #include "sensor_msgs/Image.h"
+
 #include "image_processor.h"
 #include "configuration.h"
 
@@ -38,5 +40,6 @@ class LedDetectionNode {
   ros::NodeHandle nh_private_{"~"};
   __attribute__((unused)) ros::Subscriber img_sub_{};
   ros::Publisher img_pub_{};
-  std::shared_ptr<ImageProcessor> calibration_{};
+  std::shared_ptr<ImageProcessor> image_processor_{};
+  ros::Publisher led_state_pub_;
 };

time_stamper_cv/include/led_state_parser.h

@@ -1,6 +1,8 @@
 #pragma once
-#include "common.h"
+
 #include "opencv2/opencv.hpp"
+
+#include "common.h"
 #include "configuration.h"
 
 class LedStateParser {

time_stamper_cv/msg/Ledstate.msg

@@ -0,0 +1,5 @@
+Header header
+bool is_valid
+int32 counter
+bool[] binary_state
+float64[] intensity

time_stamper_cv/package.xml

@@ -13,4 +13,6 @@
     <depend>cv_bridge</depend>
     <depend>sensor_msgs</depend>
     <depend>image_undistort</depend>
+    <build_depend>message_generation</build_depend>
+    <exec_depend>message_runtime</exec_depend>
 </package>

time_stamper_cv/src/convex_shape.cpp

@@ -1,5 +1,6 @@
-#include "convex_shape.h"
 #include "opencv2/opencv.hpp"
+
+#include "convex_shape.h"
 #include "trigonometry.h"
 
 ConvexShape::ConvexShape(const int tolerance)

time_stamper_cv/src/image_processor.cpp

@@ -1,7 +1,9 @@
-#include "image_processor.h"
 #include "ros/ros.h"
 
-ImageProcessor::ImageProcessor(const ImageProcessorConfig &cfg) {
+#include "image_processor.h"
+#include <time_stamper_cv/Ledstate.h>
+
+ImageProcessor::ImageProcessor(const ImageProcessorConfig &cfg) : cfg_(cfg) {
   convex_shape_ = std::make_shared<ConvexShape>(ConvexShape(cfg.tolerance));
   detector_ = std::make_shared<KeyPointDetector>(KeyPointDetector(cfg.params));
   led_parser_ = std::make_shared<LedStateParser>(LedStateParser(cfg.led_row_config));
@@ -30,14 +32,18 @@ cv_bridge::CvImage ImageProcessor::process(const sensor_msgs::Image &image) {
   if (convex_shape_->isShapeValid()) {
     led_parser_->processImage(input_mat);
 
+    std::vector<std::string> rows = cfg_.rows();
     //Get inverted homography, so we know the position of each LED even if it's turned off.
-    led_parser_->transformLedRow("BottomRow", convex_shape_->getInvHomography());
-    led_parser_->transformLedRow("TopRow", convex_shape_->getInvHomography());
+    std::for_each(rows.begin(), rows.end(), [this] (const std::string& name) {
+      led_parser_->transformLedRow(name, convex_shape_->getInvHomography());
+    });
 
     /* When the first and last led in the top row are on, the state of the bottom row is changing.
        The frame has to be skipped. */
-    if (led_parser_->isLedOn("TopRow", 0) && led_parser_->isLedOn("TopRow", 9)) {
-      ROS_INFO("Skipped invalid LED pos");
+    int last_led_in_row = cfg_.led_row_config.at(ImageProcessorConfig::TOP_ROW).amount - 1;
+    if (led_parser_->isLedOn(ImageProcessorConfig::TOP_ROW, 0)
+        && led_parser_->isLedOn(ImageProcessorConfig::TOP_ROW, last_led_in_row)) {
+      ROS_DEBUG("Skipped invalid LED pos");
     }
   }
 
@@ -69,28 +75,30 @@ void ImageProcessor::visualize(const cv::Mat &visualization_mat) const {
   std::string counter_text("Counter: ");
 
   if (convex_shape_->isShapeValid()) {
-    int bottom_row_binary_value = led_parser_->getBinaryValue("BottomRow");
+    int bottom_row_binary_value = led_parser_->getBinaryValue(ImageProcessorConfig::BOTTOM_ROW);
     shape_text += "Valid";
     counter_text += std::to_string(bottom_row_binary_value);
 
-    for (const auto &led: led_parser_->getLedRow("BottomRow")) {
-      cv::Point2f led_pos = LedStateParser::normalize(led);
-      cv::circle(visualization_mat, led_pos, (int) 10, cv::Scalar(255, 0, 0));
-    }
+    std::vector<std::string> rows = cfg_.rows();
+    std::for_each(rows.begin(), rows.end(), [this, &visualization_mat](const std::string& name) {
+      for (const auto &led : led_parser_->getLedRow(name)) {
+        cv::Point2f led_pos = LedStateParser::normalize(led);
+        cv::circle(visualization_mat, led_pos, (int) 10, cv::Scalar(255, 0, 0));
+      }
+    });
 
-    for (const auto &led: led_parser_->getLedRow("TopRow")) {
-      cv::Point2f led_pos = LedStateParser::normalize(led);
-      cv::circle(visualization_mat, led_pos, (int) 10, cv::Scalar(255, 0, 0));
-    }
   } else {
     shape_text += "Invalid";
     counter_text += "---";
   }
 
-  cv::putText(visualization_mat, shape_text, cv::Point(s.width * 0.05, s.height * 0.85),
-              cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(255, 0, 0));
-  cv::putText(visualization_mat, counter_text, cv::Point(s.width * 0.05, s.height * 0.9),
-              cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(255, 0, 0));
+  //Print text on screen row by row
+  double height_multiplier = 0.85;
+  for (const auto &text: {shape_text, counter_text}) {
+    cv::putText(visualization_mat, text, cv::Point((int) (s.width * 0.05), (int) (s.height * height_multiplier)),
+                cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(255, 0, 0));
+    height_multiplier += 0.05;
+  }
   cv::imshow(OPENCV_WINDOW, visualization_mat);
   cv::waitKey(1);
 }
@@ -110,3 +118,17 @@ void ImageProcessor::visualizeCorners(const cv::Mat &visualization_mat, PointAng
 void ImageProcessor::log(const std::string &message) {
   ROS_INFO("%s", message.c_str());
 }
+
+void ImageProcessor::fillInLedStateMessage(time_stamper_cv::Ledstate* msg) {
+  msg->counter = led_parser_->getBinaryValue(ImageProcessorConfig::BOTTOM_ROW);
+  msg->is_valid = convex_shape_->isShapeValid();
+
+  std::vector<std::string> rows = cfg_.rows();
+  std::for_each(rows.begin(), rows.end(), [this, &msg] (const std::string& name) {
+    for (int i = 0; i < led_parser_->getLedRow(name).size(); i++) {
+      double brightness = led_parser_->getLedBrightness(name, i);
+      msg->intensity.push_back(brightness);
+      msg->binary_state.push_back(led_parser_->isLedOn(name, i));
+    }
+  });
+}

time_stamper_cv/src/keypoint_detector.cpp

@@ -1,6 +1,7 @@
-#include "keypoint_detector.h"
 #include "ros/ros.h"
 
+#include "keypoint_detector.h"
+
 KeyPointDetector::KeyPointDetector(cv::SimpleBlobDetector::Params params) {
   detector_ = cv::SimpleBlobDetector::create(params);
 }

time_stamper_cv/src/led_detection_node.cpp

@@ -2,12 +2,13 @@
 #include "trigonometry.h"
 
 LedDetectionNode::LedDetectionNode() {
-  calibration_ = std::make_shared<ImageProcessor>(ImageProcessor(getConfiguration()));
+  image_processor_ = std::make_shared<ImageProcessor>(ImageProcessor(getConfiguration()));
 }
 
 void LedDetectionNode::init() {
-  img_pub_ = nh_.advertise<sensor_msgs::Image>("time_stamper_cv_image", 1);
-  calibration_->setVisualization(nh_private_.param("show_visualization", true));
+  img_pub_ = nh_.advertise<sensor_msgs::Image>("timestamper/image", 1);
+  led_state_pub_ = nh_.advertise<time_stamper_cv::Ledstate>("timestamper/led_state", 1);
+  image_processor_->setVisualization(nh_private_.param("show_visualization", true));
 }
 
 void LedDetectionNode::start() {
@@ -16,7 +17,14 @@ void LedDetectionNode::start() {
 }
 
 void LedDetectionNode::callbackRawImage(const sensor_msgs::Image &image) const {
-  cv_bridge::CvImage out_msg = calibration_->process(image);
+  ROS_INFO_ONCE("Received first image");
+  cv_bridge::CvImage out_msg = image_processor_->process(image);
+
+  time_stamper_cv::Ledstate led_msg;
+  image_processor_->fillInLedStateMessage(&led_msg);
+  led_msg.header = image.header;
+  led_state_pub_.publish(led_msg);
+
   img_pub_.publish(out_msg.toImageMsg());
 }
 

time_stamper_cv/src/led_state_parser.cpp

@@ -1,6 +1,7 @@
-#include "led_state_parser.h"
 #include <utility>
 
+#include "led_state_parser.h"
+
 LedStateParser::LedStateParser(LedRowConfigRepository  led_row_config_, int image_crop_size)
     : led_row_configs_(std::move(led_row_config_)), size_(image_crop_size) {
 
@@ -11,6 +12,9 @@ LedStateParser::LedStateParser(LedRowConfigRepository  led_row_config_, int imag
              cv::Scalar(255, 255, 255), -1);
 
   kernel_normalized_ = 1.0 / (kernel / 255);
+  for (auto const& cfg : led_row_configs_) {
+    led_rows_.insert({cfg.first, generateLedRow(cfg.second)});
+  }
 }
 
 void LedStateParser::processImage(const cv::Mat &image) {
@@ -60,7 +64,7 @@ double LedStateParser::getLedBrightness(const std::string& led_row_name, int ind
       (int) (led_pos.y - (size_ / 2.0)));
 
   if ((begin.y + size_) > image_.rows || begin.x + size_ > image_.cols ||
-      (begin.x < 0 && begin.y < 0)) {
+      (begin.x < 0 || begin.y < 0)) {
     return -1;
   }
 

time_stamper_cv/src/main.cpp

@@ -1,4 +1,5 @@
 #include "ros/ros.h"
+
 #include "led_detection_node.h"
 
 int main(int argc, char **argv) {