Removing image_view node and replacing with image_view that loads nodelet. (#479)

Author: Joshua Whitley

image_view/src/nodes/image_view.cpp

@@ -1,7 +1,7 @@
 /*********************************************************************
 * Software License Agreement (BSD License)
 *
-*  Copyright (c) 2008, Willow Garage, Inc.
+*  Copyright (c) 2008, 2019 Willow Garage, Inc., Joshua Whitley
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
@@ -31,197 +31,22 @@
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
-#include <image_view/ImageViewConfig.h>
 
 #include <ros/ros.h>
-#include <image_transport/image_transport.h>
-#include <dynamic_reconfigure/server.h>
+#include <nodelet/loader.h>
 
-#include <cv_bridge/cv_bridge.h>
-#include <opencv2/highgui/highgui.hpp>
-
-#include <boost/format.hpp>
-#include <boost/thread.hpp>
-#include <boost/filesystem.hpp>
-
-int g_count;
-cv::Mat g_last_image;
-boost::format g_filename_format;
-boost::mutex g_image_mutex;
-std::string g_window_name;
-bool g_gui;
-ros::Publisher g_pub;
-bool g_do_dynamic_scaling;
-int g_colormap;
-double g_min_image_value;
-double g_max_image_value;
-
-void reconfigureCb(image_view::ImageViewConfig &config, uint32_t level)
-{
-  boost::mutex::scoped_lock lock(g_image_mutex);
-  g_do_dynamic_scaling = config.do_dynamic_scaling;
-  g_colormap = config.colormap;
-  g_min_image_value = config.min_image_value;
-  g_max_image_value = config.max_image_value;
-}
-
-void imageCb(const sensor_msgs::ImageConstPtr& msg)
-{
-  boost::mutex::scoped_lock lock(g_image_mutex);
-
-  // Convert to OpenCV native BGR color
-  cv_bridge::CvImageConstPtr cv_ptr;
-  try {
-    cv_bridge::CvtColorForDisplayOptions options;
-    options.do_dynamic_scaling = g_do_dynamic_scaling;
-    options.colormap = g_colormap;
-    // Set min/max value for scaling to visualize depth/float image.
-    if (g_min_image_value == g_max_image_value) {
-      // Not specified by rosparam, then set default value.
-      // Because of current sensor limitation, we use 10m as default of max range of depth
-      // with consistency to the configuration in rqt_image_view.
-      options.min_image_value = 0;
-      if (msg->encoding == "32FC1") {
-        options.max_image_value = 10;  // 10 [m]
-      } else if (msg->encoding == "16UC1") {
-        options.max_image_value = 10 * 1000;  // 10 * 1000 [mm]
-      }
-    } else {
-      options.min_image_value = g_min_image_value;
-      options.max_image_value = g_max_image_value;
-    }
-    cv_ptr = cv_bridge::cvtColorForDisplay(cv_bridge::toCvShare(msg), "", options);
-    g_last_image = cv_ptr->image;
-  } catch (cv_bridge::Exception& e) {
-    ROS_ERROR_THROTTLE(30, "Unable to convert '%s' image for display: '%s'",
-                       msg->encoding.c_str(), e.what());
-  }
-  if (g_gui && !g_last_image.empty()) {
-    const cv::Mat &image = g_last_image;
-    cv::imshow(g_window_name, image);
-    cv::waitKey(1);
-  }
-  if (g_pub.getNumSubscribers() > 0) {
-    g_pub.publish(cv_ptr);
-  }
-}
-
-static void mouseCb(int event, int x, int y, int flags, void* param)
-{
-  if (event == cv::EVENT_LBUTTONDOWN) {
-    ROS_WARN_ONCE("Left-clicking no longer saves images. Right-click instead.");
-    return;
-  } else if (event != cv::EVENT_RBUTTONDOWN) {
-    return;
-  }
-
-  boost::mutex::scoped_lock lock(g_image_mutex);
-
-  const cv::Mat &image = g_last_image;
-
-  if (image.empty()) {
-    ROS_WARN("Couldn't save image, no data!");
-    return;
-  }
-
-  std::string filename = (g_filename_format % g_count).str();
-  if (cv::imwrite(filename, image)) {
-    ROS_INFO("Saved image %s", filename.c_str());
-    g_count++;
-  } else {
-    boost::filesystem::path full_path = boost::filesystem::complete(filename);
-    ROS_ERROR_STREAM("Failed to save image. Have permission to write there?: " << full_path);
-  }
-}
-
-static void guiCb(const ros::WallTimerEvent&)
-{
-    // Process pending GUI events and return immediately
-    cv::waitKey(1);
-}
-
-int main(int argc, char **argv)
+int main(int argc, char** argv)
 {
-  ros::init(argc, argv, "image_view", ros::init_options::AnonymousName);
-  if (ros::names::remap("image") == "image") {
-    ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
-             "\t$ rosrun image_view image_view image:=<image topic> [transport]");
-  }
-
-  ros::NodeHandle nh;
-  ros::NodeHandle local_nh("~");
-  ros::WallTimer gui_timer;
-
-  // Default window name is the resolved topic name
-  std::string topic = nh.resolveName("image");
-  local_nh.param("window_name", g_window_name, topic);
-  local_nh.param("gui", g_gui, true);  // gui/no_gui mode
-
-  if (g_gui) {
-    std::string format_string;
-    local_nh.param("filename_format", format_string, std::string("frame%04i.jpg"));
-    g_filename_format.parse(format_string);
-
-    // Handle window size
-    bool autosize;
-    local_nh.param("autosize", autosize, false);
-    cv::namedWindow(g_window_name, autosize ? (cv::WINDOW_AUTOSIZE | cv::WINDOW_KEEPRATIO | cv::WINDOW_GUI_EXPANDED) : 0);
-    cv::setMouseCallback(g_window_name, &mouseCb);
-
-    if(autosize == false)
-    {
-      if(local_nh.hasParam("width") && local_nh.hasParam("height"))
-      {
-        int width;
-        local_nh.getParam("width", width);
-        int height;
-        local_nh.getParam("height", height);
-        cv::resizeWindow(g_window_name, width, height);
-      }
-    }
-
-    // Since cv::startWindowThread() triggers a crash in cv::waitKey()
-    // if OpenCV is compiled against GTK, we call cv::waitKey() from
-    // the ROS event loop periodically, instead.
-    /*cv::startWindowThread();*/
-    gui_timer = local_nh.createWallTimer(ros::WallDuration(0.1), guiCb);
-  }
+  ros::init(argc, argv, "image_view");
 
-  // Handle transport
-  // priority:
-  //    1. command line argument
-  //    2. rosparam '~image_transport'
-  std::string transport;
-  local_nh.param("image_transport", transport, std::string("raw"));
-  ros::V_string myargv;
-  ros::removeROSArgs(argc, argv, myargv);
-  for (size_t i = 1; i < myargv.size(); ++i) {
-    if (myargv[i][0] != '-') {
-      transport = myargv[i];
-      break;
-    }
-  }
-  ROS_INFO_STREAM("Using transport \"" << transport << "\"");
-  image_transport::ImageTransport it(nh);
-  image_transport::TransportHints hints(transport, ros::TransportHints(), local_nh);
-  image_transport::Subscriber sub = it.subscribe(topic, 1, imageCb, hints);
-  g_pub = local_nh.advertise<sensor_msgs::Image>("output", 1);
+  nodelet::Loader nodelet;
+  nodelet::M_string remap(ros::names::getRemappings());
+  nodelet::V_string nargv;
 
-  dynamic_reconfigure::Server<image_view::ImageViewConfig> srv;
-  dynamic_reconfigure::Server<image_view::ImageViewConfig>::CallbackType f =
-    boost::bind(&reconfigureCb, _1, _2);
-  srv.setCallback(f);
+  std::string nodelet_name = ros::this_node::getName();
+  nodelet.load(nodelet_name, "image_view/image", remap, nargv);
 
   ros::spin();
 
-  if (g_gui) {
-    cv::destroyWindow(g_window_name);
-  }
-  // The publisher is a global variable, and therefore its scope exceeds those
-  // of the node handles in main(). Unfortunately, this will cause a crash
-  // when the publisher tries to shut down and all node handles are gone
-  // already. Therefore, we shut down the publisher now and avoid the annoying
-  // mutex assertion.
-  g_pub.shutdown();
   return 0;
 }