ros2/find_object_2d_node.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include "CameraROS.h"
29 #include "FindObjectROS.h"
30 
31 #include <QApplication>
32 #include <QDir>
33 #include "find_object/MainWindow.h"
34 #include "ParametersToolBox.h"
35 #include "find_object/Settings.h"
36 #include <signal.h>
37 
38 void my_handler_gui(int s){
39  QApplication::closeAllWindows();
40  QApplication::quit();
41 }
42 void my_handler(int s){
43  QCoreApplication::quit();
44 }
45 
46 void setupQuitSignal(bool gui)
47 {
48  // Catch ctrl-c to close the gui
49  struct sigaction sigIntHandler;
50  if(gui)
51  {
52  sigIntHandler.sa_handler = my_handler_gui;
53  }
54  else
55  {
56  sigIntHandler.sa_handler = my_handler;
57  }
58  sigemptyset(&sigIntHandler.sa_mask);
59  sigIntHandler.sa_flags = 0;
60  sigaction(SIGINT, &sigIntHandler, NULL);
61 }
62 
63 class FindObjectNode : public rclcpp::Node {
64 
65 public:
67  rclcpp::Node("find_object_2d"),
68  findObjectROS_(0),
69  camera_(0),
70  gui_(true)
71  {
72  std::string objectsPath;
73  std::string sessionPath;
74  std::string settingsPath = QDir::homePath().append("/.ros/find_object_2d.ini").toStdString();
75  bool subscribeDepth = false;
76 
77  gui_ = this->declare_parameter("gui", gui_);
78  objectsPath = this->declare_parameter("objects_path", objectsPath);
79  sessionPath = this->declare_parameter("session_path", sessionPath);
80  settingsPath = this->declare_parameter("settings_path", settingsPath);
81  subscribeDepth = this->declare_parameter("subscribe_depth", subscribeDepth);
82 
83  RCLCPP_INFO(this->get_logger(), "gui=%d", gui_?1:0);
84  RCLCPP_INFO(this->get_logger(), "objects_path=%s", objectsPath.c_str());
85  RCLCPP_INFO(this->get_logger(), "session_path=%s", sessionPath.c_str());
86  RCLCPP_INFO(this->get_logger(), "settings_path=%s", settingsPath.c_str());
87  RCLCPP_INFO(this->get_logger(), "subscribe_depth = %s", subscribeDepth?"true":"false");
88 
89  if(settingsPath.empty())
90  {
91  settingsPath = QDir::homePath().append("/.ros/find_object_2d.ini").toStdString();
92  }
93  else
94  {
95  if(!sessionPath.empty())
96  {
97  RCLCPP_WARN(this->get_logger(), "\"settings_path\" parameter is ignored when \"session_path\" is set.");
98  }
99 
100  QString path = settingsPath.c_str();
101  if(path.contains('~'))
102  {
103  path.replace('~', QDir::homePath());
104  settingsPath = path.toStdString();
105  }
106  }
107 
108  // Load settings, should be loaded before creating other objects
109  find_object::Settings::init(settingsPath.c_str());
110 
111  findObjectROS_ = new FindObjectROS(this);
112  if(!sessionPath.empty())
113  {
114  if(!objectsPath.empty())
115  {
116  RCLCPP_WARN(this->get_logger(), "\"objects_path\" parameter is ignored when \"session_path\" is set.");
117  }
118  if(!findObjectROS_->loadSession(sessionPath.c_str()))
119  {
120  RCLCPP_ERROR(this->get_logger(), "Failed to load session \"%s\"", sessionPath.c_str());
121  }
122  }
123  else if(!objectsPath.empty())
124  {
125  QString path = objectsPath.c_str();
126  if(path.contains('~'))
127  {
128  path.replace('~', QDir::homePath());
129  }
130  if(!findObjectROS_->loadObjects(path))
131  {
132  RCLCPP_ERROR(this->get_logger(), "No objects loaded from path \"%s\"", path.toStdString().c_str());
133  }
134  }
135  camera_ = new CameraROS(subscribeDepth, this);
136 
137  // Catch ctrl-c to close the gui
139  }
140 
141  void exec(int argc, char ** argv, std::shared_ptr<rclcpp::Node> node)
142  {
143  camera_->setupExecutor(node);
144  if(gui_)
145  {
146  QApplication app(argc, argv);
147  find_object::MainWindow mainWindow(findObjectROS_, camera_); // take ownership
148 
149  QObject::connect(
150  &mainWindow,
151  SIGNAL(objectsFound(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)),
153  SLOT(publish(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)));
154 
155  QStringList topics = camera_->subscribedTopics();
156  if(topics.size() == 1)
157  {
158  mainWindow.setSourceImageText(mainWindow.tr(
159  "<qt>Find-Object subscribed to <b>%1</b> topic.<br/>"
160  "You can remap the topic when starting the node: <br/>\"rosrun find_object_2d find_object_2d image:=your/image/topic\".<br/>"
161  "</qt>").arg(topics.first()));
162  }
163  else if(topics.size() == 3)
164  {
165  mainWindow.setSourceImageText(mainWindow.tr(
166  "<qt>Find-Object subscribed to : <br/> <b>%1</b> <br/> <b>%2</b> <br/> <b>%3</b><br/>"
167  "</qt>").arg(topics.at(0)).arg(topics.at(1)).arg(topics.at(2)));
168  }
169  mainWindow.show();
170  app.connect( &app, SIGNAL( lastWindowClosed() ), &app, SLOT( quit() ) );
171 
172  // mainWindow has ownership
173  findObjectROS_ = 0;
174  camera_ = 0;
175 
176  // loop
177  mainWindow.startProcessing();
178  app.exec();
180  }
181  else
182  {
183  QCoreApplication app(argc, argv);
184 
185  // connect stuff:
186  QObject::connect(camera_, SIGNAL(imageReceived(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), findObjectROS_, SLOT(detect(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)));
187 
188  //loop
189  camera_->start();
190  app.exec();
191 
192  delete camera_;
193  camera_=0;
194  delete findObjectROS_;
195  findObjectROS_=0;
196  }
197  }
198 
199  virtual ~FindObjectNode()
200  {
201  delete findObjectROS_;
202  delete camera_;
203  }
204 
205 private:
208  bool gui_;
209 };
210 
211 int main(int argc, char** argv)
212 {
213  rclcpp::init(argc, argv);
214  auto node = std::make_shared<FindObjectNode>();
215  node->exec(argc, argv, node);
216  rclcpp::shutdown();
217 }
app
int main(int argc, char **argv)
virtual bool start()
std::string settingsPath
void setupExecutor(std::shared_ptr< rclcpp::Node > node)
FindObjectROS * findObjectROS_
void setupQuitSignal(bool gui)
QStringList subscribedTopics() const
void setSourceImageText(const QString &text)
Definition: MainWindow.cpp:364
void my_handler_gui(int s)
void my_handler(int s)
void exec(int argc, char **argv, std::shared_ptr< rclcpp::Node > node)
static ParametersMap init(const QString &fileName)
Definition: Settings.cpp:101
static void saveSettings(const QString &fileName=QString())
Definition: Settings.cpp:288


find_object_2d
Author(s): Mathieu Labbe
autogenerated on Mon Dec 12 2022 03:20:09