image_view.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 #include <image_view/ImageViewConfig.h>
00035 
00036 #include <ros/ros.h>
00037 #include <image_transport/image_transport.h>
00038 #include <dynamic_reconfigure/server.h>
00039 
00040 #include <cv_bridge/cv_bridge.h>
00041 #include <opencv2/highgui/highgui.hpp>
00042 
00043 #include <boost/format.hpp>
00044 #include <boost/thread.hpp>
00045 #include <boost/filesystem.hpp>
00046 
00047 int g_count;
00048 cv::Mat g_last_image;
00049 boost::format g_filename_format;
00050 boost::mutex g_image_mutex;
00051 std::string g_window_name;
00052 bool g_gui;
00053 ros::Publisher g_pub;
00054 bool g_do_dynamic_scaling;
00055 int g_colormap;
00056 double g_min_image_value;
00057 double g_max_image_value;
00058 
00059 void reconfigureCb(image_view::ImageViewConfig &config, uint32_t level)
00060 {
00061   boost::mutex::scoped_lock lock(g_image_mutex);
00062   g_do_dynamic_scaling = config.do_dynamic_scaling;
00063   g_colormap = config.colormap;
00064   g_min_image_value = config.min_image_value;
00065   g_max_image_value = config.max_image_value;
00066 }
00067 
00068 void imageCb(const sensor_msgs::ImageConstPtr& msg)
00069 {
00070   boost::mutex::scoped_lock lock(g_image_mutex);
00071 
00072   // Convert to OpenCV native BGR color
00073   cv_bridge::CvImageConstPtr cv_ptr;
00074   try {
00075     cv_bridge::CvtColorForDisplayOptions options;
00076     options.do_dynamic_scaling = g_do_dynamic_scaling;
00077     options.colormap = g_colormap;
00078     // Set min/max value for scaling to visualize depth/float image.
00079     if (g_min_image_value == g_max_image_value) {
00080       // Not specified by rosparam, then set default value.
00081       // Because of current sensor limitation, we use 10m as default of max range of depth
00082       // with consistency to the configuration in rqt_image_view.
00083       options.min_image_value = 0;
00084       if (msg->encoding == "32FC1") {
00085         options.max_image_value = 10;  // 10 [m]
00086       } else if (msg->encoding == "16UC1") {
00087         options.max_image_value = 10 * 1000;  // 10 * 1000 [mm]
00088       }
00089     } else {
00090       options.min_image_value = g_min_image_value;
00091       options.max_image_value = g_max_image_value;
00092     }
00093     cv_ptr = cv_bridge::cvtColorForDisplay(cv_bridge::toCvShare(msg), "", options);
00094     g_last_image = cv_ptr->image;
00095   } catch (cv_bridge::Exception& e) {
00096     ROS_ERROR_THROTTLE(30, "Unable to convert '%s' image for display: '%s'",
00097                        msg->encoding.c_str(), e.what());
00098   }
00099   if (g_gui && !g_last_image.empty()) {
00100     const cv::Mat &image = g_last_image;
00101     cv::imshow(g_window_name, image);
00102     cv::waitKey(3);
00103   }
00104   if (g_pub.getNumSubscribers() > 0) {
00105     g_pub.publish(cv_ptr);
00106   }
00107 }
00108 
00109 static void mouseCb(int event, int x, int y, int flags, void* param)
00110 {
00111   if (event == cv::EVENT_LBUTTONDOWN) {
00112     ROS_WARN_ONCE("Left-clicking no longer saves images. Right-click instead.");
00113     return;
00114   } else if (event != cv::EVENT_RBUTTONDOWN) {
00115     return;
00116   }
00117 
00118   boost::mutex::scoped_lock lock(g_image_mutex);
00119 
00120   const cv::Mat &image = g_last_image;
00121 
00122   if (image.empty()) {
00123     ROS_WARN("Couldn't save image, no data!");
00124     return;
00125   }
00126 
00127   std::string filename = (g_filename_format % g_count).str();
00128   if (cv::imwrite(filename, image)) {
00129     ROS_INFO("Saved image %s", filename.c_str());
00130     g_count++;
00131   } else {
00132     boost::filesystem::path full_path = boost::filesystem::complete(filename);
00133     ROS_ERROR_STREAM("Failed to save image. Have permission to write there?: " << full_path);
00134   }
00135 }
00136 
00137 
00138 int main(int argc, char **argv)
00139 {
00140   ros::init(argc, argv, "image_view", ros::init_options::AnonymousName);
00141   if (ros::names::remap("image") == "image") {
00142     ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
00143              "\t$ rosrun image_view image_view image:=<image topic> [transport]");
00144   }
00145 
00146   ros::NodeHandle nh;
00147   ros::NodeHandle local_nh("~");
00148 
00149   // Default window name is the resolved topic name
00150   std::string topic = nh.resolveName("image");
00151   local_nh.param("window_name", g_window_name, topic);
00152   local_nh.param("gui", g_gui, true);  // gui/no_gui mode
00153 
00154   if (g_gui) {
00155     std::string format_string;
00156     local_nh.param("filename_format", format_string, std::string("frame%04i.jpg"));
00157     g_filename_format.parse(format_string);
00158 
00159     // Handle window size
00160     bool autosize;
00161     local_nh.param("autosize", autosize, false);
00162     cv::namedWindow(g_window_name, autosize ? (CV_WINDOW_AUTOSIZE | CV_WINDOW_KEEPRATIO | CV_GUI_EXPANDED) : 0);
00163     cv::setMouseCallback(g_window_name, &mouseCb);
00164 
00165     if(autosize == false)
00166     {
00167       if(local_nh.hasParam("width") && local_nh.hasParam("height"))
00168       {
00169         int width;
00170         local_nh.getParam("width", width);
00171         int height;
00172         local_nh.getParam("height", height);
00173         cv::resizeWindow(g_window_name, width, height);
00174       }
00175     }
00176 
00177     // Start the OpenCV window thread so we don't have to waitKey() somewhere
00178     cv::startWindowThread();
00179   }
00180 
00181   // Handle transport
00182   // priority:
00183   //    1. command line argument
00184   //    2. rosparam '~image_transport'
00185   std::string transport;
00186   local_nh.param("image_transport", transport, std::string("raw"));
00187   ros::V_string myargv;
00188   ros::removeROSArgs(argc, argv, myargv);
00189   for (size_t i = 1; i < myargv.size(); ++i) {
00190     if (myargv[i][0] != '-') {
00191       transport = myargv[i];
00192       break;
00193     }
00194   }
00195   ROS_INFO_STREAM("Using transport \"" << transport << "\"");
00196   image_transport::ImageTransport it(nh);
00197   image_transport::TransportHints hints(transport, ros::TransportHints(), local_nh);
00198   image_transport::Subscriber sub = it.subscribe(topic, 1, imageCb, hints);
00199   g_pub = local_nh.advertise<sensor_msgs::Image>("output", 1);
00200 
00201   dynamic_reconfigure::Server<image_view::ImageViewConfig> srv;
00202   dynamic_reconfigure::Server<image_view::ImageViewConfig>::CallbackType f =
00203     boost::bind(&reconfigureCb, _1, _2);
00204   srv.setCallback(f);
00205 
00206   ros::spin();
00207 
00208   if (g_gui) {
00209     cv::destroyWindow(g_window_name);
00210   }
00211   return 0;
00212 }


image_view
Author(s): Patrick Mihelich
autogenerated on Wed May 1 2019 02:51:52