00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #include <opencv/cv.h>
00040 #include <opencv/highgui.h>
00041
00042 #include <ros/ros.h>
00043 #include <sensor_msgs/Image.h>
00044 #include <cv_bridge/cv_bridge.h>
00045 #include <image_transport/image_transport.h>
00046
00047 #include <boost/thread.hpp>
00048 #include <boost/format.hpp>
00049
00050 #ifdef HAVE_GTK
00051 #include <gtk/gtk.h>
00052
00053
00054
00055
00056 static void destroy(GtkWidget *widget, gpointer data)
00057 {
00058 ros::shutdown();
00059 }
00060 #endif
00061
00062 class ImageView
00063 {
00064 private:
00065 image_transport::Subscriber sub_;
00066
00067 sensor_msgs::ImageConstPtr last_msg_;
00068 boost::mutex image_mutex_;
00069
00070 std::string window_name_;
00071 boost::format filename_format_;
00072 int count_;
00073 IplImage ipl_image_;
00074
00075 public:
00076 ImageView(const ros::NodeHandle& nh, const std::string& transport)
00077 : filename_format_(""), count_(0)
00078 {
00079 std::string topic = nh.resolveName("image");
00080 ros::NodeHandle local_nh("~");
00081 local_nh.param("window_name", window_name_, topic);
00082
00083 bool autosize;
00084 local_nh.param("autosize", autosize, false);
00085
00086 std::string format_string;
00087 local_nh.param("filename_format", format_string, std::string("frame%04i.jpg"));
00088 filename_format_.parse(format_string);
00089
00090 const char* name = window_name_.c_str();
00091 cvNamedWindow(name, autosize ? CV_WINDOW_AUTOSIZE : 0);
00092 cvSetMouseCallback(name, &ImageView::mouse_cb, this);
00093 #ifdef HAVE_GTK
00094 GtkWidget *widget = GTK_WIDGET( cvGetWindowHandle(name) );
00095 g_signal_connect(widget, "destroy", G_CALLBACK(destroy), NULL);
00096 #endif
00097 cvStartWindowThread();
00098
00099 image_transport::ImageTransport it(nh);
00100 sub_ = it.subscribe(topic, 1, &ImageView::image_cb, this, transport);
00101 }
00102
00103 ~ImageView()
00104 {
00105 cvDestroyWindow(window_name_.c_str());
00106 }
00107
00108 void image_cb(const sensor_msgs::ImageConstPtr& msg)
00109 {
00110 boost::lock_guard<boost::mutex> guard(image_mutex_);
00111
00112
00113 last_msg_ = msg;
00114
00115
00116
00117 if (msg->encoding.find("bayer") != std::string::npos)
00118 boost::const_pointer_cast<sensor_msgs::Image>(msg)->encoding = "mono8";
00119
00120 cv_bridge::CvImagePtr cv_ptr;
00121 try
00122 {
00123 cv_ptr = cv_bridge::toCvCopy(msg, "passthrough");
00124 }
00125 catch (cv_bridge::Exception& e)
00126 {
00127 ROS_ERROR("cv_bridge exception: %s", e.what());
00128 return;
00129 }
00130
00131
00132 ipl_image_ = cv_ptr->image;
00133 ROS_INFO("received image with %d channels", ipl_image_.nChannels);
00134
00135 IplImage* alpha = cvCreateImage(cvGetSize(&ipl_image_), IPL_DEPTH_8U, 1);
00136 IplImage* b = cvCreateImage(cvGetSize(&ipl_image_), IPL_DEPTH_8U, 1);
00137 IplImage* g = cvCreateImage(cvGetSize(&ipl_image_), IPL_DEPTH_8U, 1);
00138 IplImage* r = cvCreateImage(cvGetSize(&ipl_image_), IPL_DEPTH_8U, 1);
00139 cvSplit(&ipl_image_, b,g,r,alpha);
00140
00141 double alpha_value = 0.7;
00142 cvAddWeighted(b, alpha_value, alpha, 1.0 - alpha_value, 0.0, b);
00143 cvAddWeighted(g, alpha_value, alpha, 1.0 - alpha_value, 0.0, g);
00144 cvAddWeighted(r, alpha_value, alpha, 1.0 - alpha_value, 0.0, r);
00145
00146 cvMerge(b,g,r,NULL, &ipl_image_);
00147
00148 cvShowImage(window_name_.c_str(),&ipl_image_);
00149 cvReleaseImage(&alpha);
00150 cvReleaseImage(&b);
00151 cvReleaseImage(&g);
00152 cvReleaseImage(&r);
00153 }
00154
00155 static void mouse_cb(int event, int x, int y, int flags, void* param)
00156 {
00157 if (event != CV_EVENT_LBUTTONDOWN)
00158 return;
00159
00160 ImageView *iv = (ImageView*)param;
00161 boost::lock_guard<boost::mutex> guard(iv->image_mutex_);
00162
00163 IplImage *image = &iv->ipl_image_;
00164 if (image) {
00165 std::string filename = (iv->filename_format_ % iv->count_).str();
00166 cvSaveImage(filename.c_str(), image);
00167 ROS_INFO("Saved image %s", filename.c_str());
00168 iv->count_++;
00169 } else {
00170 ROS_WARN("Couldn't save image, no data!");
00171 }
00172 }
00173 };
00174
00175 int main(int argc, char **argv)
00176 {
00177 ros::init(argc, argv, "image_view", ros::init_options::AnonymousName);
00178 ros::NodeHandle n;
00179 if (n.resolveName("image") == "/image") {
00180 ROS_WARN("image_view: image has not been remapped! Typical command-line usage:\n"
00181 "\t$ ./image_view image:=<image topic> [transport]");
00182 }
00183
00184 ImageView view(n, (argc > 1) ? argv[1] : "raw");
00185
00186 ros::spin();
00187
00188 return 0;
00189 }