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/CvBridge.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 sensor_msgs::CvBridge img_bridge_;
00069 boost::mutex image_mutex_;
00070
00071 std::string window_name_;
00072 boost::format filename_format_;
00073 int count_;
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 if (img_bridge_.fromImage(*msg, "bgra8")){
00121 IplImage* ipl_image = img_bridge_.toIpl();
00122 ROS_INFO("received image with %d channels", ipl_image->nChannels);
00123
00124 IplImage* alpha = cvCreateImage(cvGetSize(ipl_image), IPL_DEPTH_8U, 1);
00125 IplImage* b = cvCreateImage(cvGetSize(ipl_image), IPL_DEPTH_8U, 1);
00126 IplImage* g = cvCreateImage(cvGetSize(ipl_image), IPL_DEPTH_8U, 1);
00127 IplImage* r = cvCreateImage(cvGetSize(ipl_image), IPL_DEPTH_8U, 1);
00128 cvSplit(ipl_image, b,g,r,alpha);
00129
00130 double alpha_value = 0.7;
00131 cvAddWeighted(b, alpha_value, alpha, 1.0 - alpha_value, 0.0, b);
00132 cvAddWeighted(g, alpha_value, alpha, 1.0 - alpha_value, 0.0, g);
00133 cvAddWeighted(r, alpha_value, alpha, 1.0 - alpha_value, 0.0, r);
00134
00135 cvMerge(b,g,r,NULL, ipl_image);
00136
00137 cvShowImage(window_name_.c_str(),ipl_image);
00138 cvReleaseImage(&alpha);
00139 cvReleaseImage(&b);
00140 cvReleaseImage(&g);
00141 cvReleaseImage(&r);
00142 }
00143 else
00144 ROS_ERROR("Unable to convert %s image to bgr8", msg->encoding.c_str());
00145 }
00146
00147 static void mouse_cb(int event, int x, int y, int flags, void* param)
00148 {
00149 if (event != CV_EVENT_LBUTTONDOWN)
00150 return;
00151
00152 ImageView *iv = (ImageView*)param;
00153 boost::lock_guard<boost::mutex> guard(iv->image_mutex_);
00154
00155 IplImage *image = iv->img_bridge_.toIpl();
00156 if (image) {
00157 std::string filename = (iv->filename_format_ % iv->count_).str();
00158 cvSaveImage(filename.c_str(), image);
00159 ROS_INFO("Saved image %s", filename.c_str());
00160 iv->count_++;
00161 } else {
00162 ROS_WARN("Couldn't save image, no data!");
00163 }
00164 }
00165 };
00166
00167 int main(int argc, char **argv)
00168 {
00169 ros::init(argc, argv, "image_view", ros::init_options::AnonymousName);
00170 ros::NodeHandle n;
00171 if (n.resolveName("image") == "/image") {
00172 ROS_WARN("image_view: image has not been remapped! Typical command-line usage:\n"
00173 "\t$ ./image_view image:=<image topic> [transport]");
00174 }
00175
00176 ImageView view(n, (argc > 1) ? argv[1] : "raw");
00177
00178 ros::spin();
00179
00180 return 0;
00181 }