find_object_2d_node.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include "CameraROS.h"
00029 #include "FindObjectROS.h"
00030 
00031 #include <QApplication>
00032 #include <QDir>
00033 #include "find_object/MainWindow.h"
00034 #include "ParametersToolBox.h"
00035 #include "find_object/Settings.h"
00036 #include <signal.h>
00037 
00038 using namespace find_object;
00039 
00040 bool gui;
00041 std::string settingsPath;
00042 
00043 void my_handler_gui(int s){
00044         QApplication::closeAllWindows();
00045         QApplication::quit();
00046 }
00047 void my_handler(int s){
00048         QCoreApplication::quit();
00049 }
00050 
00051 void setupQuitSignal(bool gui)
00052 {
00053         // Catch ctrl-c to close the gui
00054         struct sigaction sigIntHandler;
00055         if(gui)
00056         {
00057                 sigIntHandler.sa_handler = my_handler_gui;
00058         }
00059         else
00060         {
00061                 sigIntHandler.sa_handler = my_handler;
00062         }
00063         sigemptyset(&sigIntHandler.sa_mask);
00064         sigIntHandler.sa_flags = 0;
00065         sigaction(SIGINT, &sigIntHandler, NULL);
00066 }
00067 
00068 int main(int argc, char** argv)
00069 {
00070         ros::init(argc, argv, "find_object_2d");
00071 
00072         gui = true;
00073         std::string objectsPath;
00074         std::string sessionPath;
00075         settingsPath = QDir::homePath().append("/.ros/find_object_2d.ini").toStdString();
00076         bool subscribeDepth = false;
00077         std::string objFramePrefix = "object";
00078 
00079         ros::NodeHandle nh("~");
00080 
00081         nh.param("gui", gui, gui);
00082         nh.param("objects_path", objectsPath, objectsPath);
00083         nh.param("session_path", sessionPath, sessionPath);
00084         nh.param("settings_path", settingsPath, settingsPath);
00085         nh.param("subscribe_depth", subscribeDepth, subscribeDepth);
00086         nh.param("obj_frame_prefix", objFramePrefix, objFramePrefix);
00087 
00088         ROS_INFO("gui=%d", (int)gui);
00089         ROS_INFO("objects_path=%s", objectsPath.c_str());
00090         ROS_INFO("session_path=%s", sessionPath.c_str());
00091         ROS_INFO("settings_path=%s", settingsPath.c_str());
00092         ROS_INFO("subscribe_depth = %s", subscribeDepth?"true":"false");
00093         ROS_INFO("obj_frame_prefix = %s", objFramePrefix.c_str());
00094 
00095         if(settingsPath.empty())
00096         {
00097                 settingsPath = QDir::homePath().append("/.ros/find_object_2d.ini").toStdString();
00098         }
00099         else
00100         {
00101                 if(!sessionPath.empty())
00102                 {
00103                         ROS_WARN("\"settings_path\" parameter is ignored when \"session_path\" is set.");
00104                 }
00105 
00106                 QString path = settingsPath.c_str();
00107                 if(path.contains('~'))
00108                 {
00109                         path.replace('~', QDir::homePath());
00110                         settingsPath = path.toStdString();
00111                 }
00112         }
00113 
00114         // Load settings, should be loaded before creating other objects
00115         Settings::init(settingsPath.c_str());
00116 
00117         FindObjectROS * findObjectROS = new FindObjectROS(objFramePrefix);
00118         if(!sessionPath.empty())
00119         {
00120                 if(!objectsPath.empty())
00121                 {
00122                         ROS_WARN("\"objects_path\" parameter is ignored when \"session_path\" is set.");
00123                 }
00124                 if(!findObjectROS->loadSession(sessionPath.c_str()))
00125                 {
00126                         ROS_ERROR("Failed to load session \"%s\"", sessionPath.c_str());
00127                 }
00128         }
00129         else if(!objectsPath.empty())
00130         {
00131                 QString path = objectsPath.c_str();
00132                 if(path.contains('~'))
00133                 {
00134                         path.replace('~', QDir::homePath());
00135                 }
00136                 if(!findObjectROS->loadObjects(path))
00137                 {
00138                         ROS_ERROR("No objects loaded from path \"%s\"", path.toStdString().c_str());
00139                 }
00140         }
00141 
00142         CameraROS * camera = new CameraROS(subscribeDepth);
00143 
00144         QObject::connect(
00145                         camera,
00146                         SIGNAL(rosDataReceived(const std::string &, const ros::Time &, const cv::Mat &, float)),
00147                         findObjectROS,
00148                         SLOT(setDepthData(const std::string &, const ros::Time &, const cv::Mat &, float)));
00149 
00150         // Catch ctrl-c to close the gui
00151         setupQuitSignal(gui);
00152 
00153         if(gui)
00154         {
00155                 QApplication app(argc, argv);
00156                 MainWindow mainWindow(findObjectROS, camera); // take ownership
00157 
00158                 QObject::connect(
00159                                 &mainWindow,
00160                                 SIGNAL(objectsFound(const find_object::DetectionInfo &)),
00161                                 findObjectROS,
00162                                 SLOT(publish(const find_object::DetectionInfo &)));
00163 
00164                 QStringList topics = camera->subscribedTopics();
00165                 if(topics.size() == 1)
00166                 {
00167                         mainWindow.setSourceImageText(mainWindow.tr(
00168                                         "<qt>Find-Object subscribed to <b>%1</b> topic.<br/>"
00169                                         "You can remap the topic when starting the node: <br/>\"rosrun find_object_2d find_object_2d image:=your/image/topic\".<br/>"
00170                                         "</qt>").arg(topics.first()));
00171                 }
00172                 else if(topics.size() == 3)
00173                 {
00174                         mainWindow.setSourceImageText(mainWindow.tr(
00175                                         "<qt>Find-Object subscribed to : <br/> <b>%1</b> <br/> <b>%2</b> <br/> <b>%3</b><br/>"
00176                                         "</qt>").arg(topics.at(0)).arg(topics.at(1)).arg(topics.at(2)));
00177                 }
00178                 mainWindow.show();
00179                 app.connect( &app, SIGNAL( lastWindowClosed() ), &app, SLOT( quit() ) );
00180 
00181                 // loop
00182                 mainWindow.startProcessing();
00183                 app.exec();
00184                 Settings::saveSettings();
00185         }
00186         else
00187         {
00188                 QCoreApplication app(argc, argv);
00189 
00190                 // connect stuff:
00191                 QObject::connect(camera, SIGNAL(imageReceived(const cv::Mat &)), findObjectROS, SLOT(detect(const cv::Mat &)));
00192 
00193                 //loop
00194                 camera->start();
00195                 app.exec();
00196 
00197                 delete camera;
00198                 delete findObjectROS;
00199         }
00200         return 0;
00201 }


find_object_2d
Author(s): Mathieu Labbe
autogenerated on Thu Aug 27 2015 13:00:33