Merge pull request #769 from uf-mil/image_helper

Created C++ Version of Image Helpers

mil_common/utils/mil_tools/CMakeLists.txt

@@ -10,6 +10,7 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   cv_bridge
   rosbag
+  image_transport
 )
 
 find_package(Eigen3 REQUIRED)
@@ -23,6 +24,7 @@ catkin_package(
   CATKIN_DEPENDS
     roscpp
     cv_bridge
+    image_transport
   DEPENDS
 )
 
@@ -39,12 +41,13 @@ include_directories(
     ${catkin_INCLUDE_DIRS}
     ${Boost_INCLUDE_DIRS}
     ${EIGEN_INCLUDE_DIRS}
+    ${OpenCV_INCLUDE_DIRS}
 )
 
 add_library(mil_tools
   src/mil_tools/mil_tools.cpp
+  src/mil_tools/image_publisher.cpp
+  include/mil_tools/image_subscriber.hpp
 )
-target_include_directories(mil_tools PUBLIC include ${EIGEN3_INCLUDE_DIRS})
-target_link_libraries(mil_tools ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-
-
+target_include_directories(mil_tools PUBLIC include ${EIGEN3_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
+target_link_libraries(mil_tools ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${OpenCV_LIBRARIES})

mil_common/utils/mil_tools/include/mil_tools/image_publisher.hpp

@@ -0,0 +1,17 @@
+#include <cv_bridge/cv_bridge.h>
+#include <image_transport/image_transport.h>
+#include <ros/ros.h>
+#include <opencv2/highgui/highgui.hpp>
+
+class ImagePublisher
+{
+public:
+  ImagePublisher(const std::string& topic, const std::string& encoding = "bgr8", int queue_size = 1);
+  void publish(cv::Mat& image_arg);
+
+private:
+  std::string encoding_;
+  image_transport::Publisher pub_;
+  ros::NodeHandle nh_;
+  image_transport::ImageTransport it_;
+};

mil_common/utils/mil_tools/include/mil_tools/image_subscriber.hpp

@@ -0,0 +1,115 @@
+#ifndef IMAGE_SUBSCRIBER_H_
+#define IMAGE_SUBSCRIBER_H_
+
+#include <cv_bridge/cv_bridge.h>
+#include <image_geometry/pinhole_camera_model.h>
+#include <image_transport/image_transport.h>
+#include <ros/ros.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/Image.h>
+#include <std_msgs/Time.h>
+#include <boost/optional.hpp>
+#include <functional>
+#include <opencv2/opencv.hpp>
+#include <type_traits>
+
+class ImageSubscriber
+{
+public:
+  template <typename T>
+  ImageSubscriber(const std::string& topic, void (T::*func)(cv::Mat&), T* a, const std::string& encoding = "bgr8",
+                  int queue_size = 1)
+    : encoding_(encoding), queue_size_(queue_size), it_(nh_)
+  {
+    image_subscriber_ = it_.subscribe(topic, queue_size, &ImageSubscriber::imageCallback, this);
+    function_ = std::bind(func, a, std::placeholders::_1);
+    camera_info_sub_ = nh_.subscribe(getInfoTopic(topic), queue_size, &ImageSubscriber::infoCallback, this);
+  }
+
+  ImageSubscriber(const std::string& topic, void (*func)(cv::Mat&), const std::string& encoding = "bgr8",
+                  int queue_size = 1)
+    : encoding_(encoding), queue_size_(queue_size), it_(nh_)
+  {
+    image_subscriber_ = it_.subscribe(topic, queue_size, &ImageSubscriber::imageCallback, this);
+    function_ = func;
+    camera_info_sub_ = nh_.subscribe(getInfoTopic(topic), queue_size, &ImageSubscriber::infoCallback, this);
+  }
+
+  std::string getInfoTopic(const std::string& input)
+  {
+    int location = input.rfind('/');
+    if (location == input.length() - 1)
+      location = input.rfind('/', input.length() - 2);
+    return input.substr(0, location - 1) + "/camera_info";
+  }
+
+  boost::optional<sensor_msgs::CameraInfo> waitForCameraInfo(unsigned int timeout = 10)
+  {
+    ROS_WARN("Blocking -- waiting at most %d seconds for camera info.", timeout);
+
+    auto time = ros::Duration(timeout);
+    time.sleep();
+    auto start_time = ros::Time::now();
+
+    while (ros::Time::now() - start_time < time && ros::ok())
+    {
+      if (camera_info_.has_value())
+      {
+        ROS_INFO("Camera info found!");
+        return camera_info_.get();
+      }
+      ros::Duration(0.2).sleep();
+    }
+
+    ROS_ERROR("Camera info not found.");
+    return boost::none;
+  }
+
+  bool waitForCameraModel(image_geometry::PinholeCameraModel& pinhole_camera, unsigned int timeout = 10)
+  {
+    auto msg = waitForCameraInfo(timeout);
+    if (!msg)
+      return false;
+    pinhole_camera.fromCameraInfo(msg);
+    return true;
+  }
+
+  void infoCallback(const sensor_msgs::CameraInfo& info)
+  {
+    camera_info_sub_.shutdown();
+    camera_info_.emplace(info);
+  }
+
+  void imageCallback(const sensor_msgs::ImageConstPtr& image)
+  {
+    try
+    {
+      last_image_time_ = image.header.stamp;
+      function_(cv_bridge::toCvCopy(image, encoding_)->image);
+    }
+    catch (cv_bridge::Exception& e)
+    {
+      ROS_ERROR("Could not convert from '%s' to '%s'.", image->encoding, encoding_);
+    }
+  }
+
+  boost::optional<std_msgs::Time> getLastImageTime()
+  {
+    return camera_info_;
+  }
+
+private:
+  std::string encoding_;
+  int queue_size_;
+
+  std::function<void(cv::Mat&)> function_;
+
+  ros::NodeHandle nh_;
+  image_transport::ImageTransport it_(nh_);
+  image_transport::Subscriber image_subscriber_;
+
+  ros::Subscriber camera_info_sub_;
+  boost::optional<sensor_msgs::CameraInfo> camera_info_;
+  boost::optional<std_msgs::Time> last_image_time_;
+};
+#endif  // IMAGE_SUBSCRIBER_H_

mil_common/utils/mil_tools/package.xml

@@ -14,6 +14,8 @@
   <run_depend>rospy</run_depend>
   <build_depend>cv_bridge</build_depend>
   <run_depend>cv_bridge</run_depend>
+  <build_depend>image_transport</build_depend>
+  <run_depend>image_transport</run_depend>
   <build_depend>cmake_modules</build_depend>
   <build_depend>python-tqdm</build_depend>
   <run_depend>python-tqdm</run_depend>

mil_common/utils/mil_tools/src/mil_tools/image_publisher.cpp

@@ -0,0 +1,14 @@
+#include <mil_tools/image_publisher.hpp>
+
+ImagePublisher::ImagePublisher(const std::string& topic, const std::string& encoding, int queue_size) : it_(nh_)
+{
+  this->encoding_ = encoding;
+  pub_ = it_.advertise(topic, queue_size);
+}
+void ImagePublisher::publish(cv::Mat& image_arg)
+{
+  std_msgs::Header header;
+  header.stamp = ros::Time::now();
+  sensor_msgs::ImagePtr msg = cv_bridge::CvImage(header, encoding_, image_arg).toImageMsg();
+  pub_.publish(msg);
+}