RemotePtam.cpp
Go to the documentation of this file.
00001 /*
00002 
00003 Copyright (c) 2008, Willow Garage, Inc.
00004 
00005 Copyright (c) 2011, Markus Achtelik, ASL, ETH Zurich, Switzerland
00006 You can contact the author at <markus dot achtelik at mavt dot ethz dot ch>
00007 
00008 All rights reserved.
00009 
00010 Redistribution and use in source and binary forms, with or without
00011 modification, are permitted provided that the following conditions are met:
00012  * Redistributions of source code must retain the above copyright
00013 notice, this list of conditions and the following disclaimer.
00014  * Redistributions in binary form must reproduce the above copyright
00015 notice, this list of conditions and the following disclaimer in the
00016 documentation and/or other materials provided with the distribution.
00017  * Neither the name of ETHZ-ASL nor the
00018 names of its contributors may be used to endorse or promote products
00019 derived from this software without specific prior written permission.
00020 
00021 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00022 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00023 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00024 DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY
00025 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00026 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00028 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00029 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00030 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00031 
00032  */
00033 
00034 #include <opencv2/imgproc/imgproc.hpp>
00035 #include <opencv2/highgui/highgui.hpp>
00036 
00037 #include <ros/ros.h>
00038 #include <sensor_msgs/Image.h>
00039 #include <sensor_msgs/image_encodings.h>
00040 #include <std_msgs/String.h>
00041 
00042 #include <cv_bridge/cv_bridge.h>
00043 #include <image_transport/image_transport.h>
00044 
00045 #ifdef HAVE_GTK
00046 #include <gtk/gtk.h>
00047 
00048 // Platform-specific workaround for #3026: image_view doesn't close when
00049 // closing image window. On platforms using GTK+ we connect this to the
00050 // window's "destroy" event so that image_view exits.
00051 static void destroyNode(GtkWidget *widget, gpointer data)
00052 {
00053   ros::shutdown();
00054 }
00055 
00056 #endif
00057 
00058 class RemotePtam
00059 {
00060 private:
00061   image_transport::Subscriber *sub_;
00062   std::string window_name_;
00063   std::string transport_;
00064   std::string topic_;
00065 
00066 public:
00067   RemotePtam(const ros::NodeHandle& nh, const std::string& _transport)
00068   {
00069     topic_ = "vslam/preview";
00070     ros::NodeHandle local_nh("~");
00071     local_nh.param("window_name", window_name_, topic_);
00072 
00073     transport_ = _transport;
00074 
00075     bool autosize;
00076     local_nh.param("autosize", autosize, false);
00077     cv::namedWindow(window_name_, autosize ? 1 : 0);
00078 
00079 #ifdef HAVE_GTK
00080     // Register appropriate handler for when user closes the display window
00081     GtkWidget *widget = GTK_WIDGET( cvGetWindowHandle(window_name_.c_str()) );
00082     g_signal_connect(widget, "destroy", G_CALLBACK(destroyNode), NULL);
00083 #endif
00084 
00085     sub_ = NULL;
00086     subscribe(nh);
00087   }
00088 
00089   ~RemotePtam()
00090   {
00091     unsubscribe();
00092     cv::destroyWindow(window_name_);
00093   }
00094 
00095   void image_cb(const sensor_msgs::ImageConstPtr& msg)
00096   {
00097     cv_bridge::CvImageConstPtr cv_ptr;
00098     try
00099     {
00100       cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8);
00101     }
00102     catch (cv_bridge::Exception& e)
00103     {
00104       ROS_ERROR("cv_bridge exception: %s", e.what());
00105       return;
00106     }
00107 
00108     cv::imshow(window_name_, cv_ptr->image);
00109   }
00110   void subscribe(const ros::NodeHandle& nh)
00111   {
00112     if (sub_ == NULL)
00113     {
00114       image_transport::ImageTransport it(nh);
00115       sub_ = new image_transport::Subscriber(it.subscribe(topic_, 1, &RemotePtam::image_cb, this, transport_));
00116     }
00117   }
00118 
00119   void unsubscribe()
00120   {
00121     if (sub_ != NULL)
00122     {
00123       delete sub_;
00124       sub_ = NULL;
00125     }
00126   }
00127 };
00128 
00129 int main(int argc, char **argv)
00130 {
00131   ros::init(argc, argv, "vslam_remote", ros::init_options::AnonymousName);
00132   ros::NodeHandle n;
00133 
00134   RemotePtam remote(n, (argc > 1) ? argv[1] : "raw");
00135 
00136   char key = 0;
00137 
00138   remote.subscribe(n);
00139   bool subscribed = true;
00140 
00141   ros::Publisher key_pub = n.advertise<std_msgs::String> ("vslam/key_pressed", 10);
00142   std_msgs::StringPtr msg(new std_msgs::String);
00143 
00144   while (ros::ok())
00145   {
00146     key = cvWaitKey(10);
00147 
00148     if (key == ' ')
00149     {
00150       std::cout << "Sending \"Space\" to ptam" << std::endl;
00151       msg->data = "Space";
00152       key_pub.publish(msg);
00153     }
00154     else if (key == 'r')
00155     {
00156       std::cout << "Sending \"r\" to ptam" << std::endl;
00157       msg->data = "r";
00158       key_pub.publish(msg);
00159     }
00160     else if (key == 'a')
00161     {
00162       std::cout << "Sending \"a\" to ptam" << std::endl;
00163       msg->data = "a";
00164       key_pub.publish(msg);
00165     }
00166     else if (key == 'q')
00167     {
00168       std::cout << "Sending \"q\" to ptam" << std::endl;
00169       msg->data = "q";
00170       key_pub.publish(msg);
00171     }
00172     else if (key == 's')
00173     {
00174       if (subscribed)
00175       {
00176         remote.unsubscribe();
00177         subscribed = false;
00178         std::cout << "unsubscribed" << std::endl;
00179       }
00180       else
00181       {
00182         remote.subscribe(n);
00183         subscribed = true;
00184         std::cout << "subscribed" << std::endl;
00185       }
00186     }
00187 
00188     ros::spinOnce();
00189   }
00190   return 0;
00191 }


ptam
Author(s): Markus Achtelik , Stephan Weiss , Simon Lynen
autogenerated on Sun Oct 5 2014 23:52:33