PreferencesDialogROS.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, 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 
30 #include <QDir>
31 #include <QFileInfo>
32 #include <QSettings>
33 #include <QHBoxLayout>
34 #include <QTimer>
35 #include <QLabel>
37 #include <QMessageBox>
38 #include <ros/exceptions.h>
39 #include <rtabmap/utilite/UStl.h>
40 
41 using namespace rtabmap;
42 
43 PreferencesDialogROS::PreferencesDialogROS(const QString & configFile) :
44  configFile_(configFile)
45 {
46 
47 }
48 
50 {
51  QFile::remove(getTmpIniFilePath());
52 }
53 
55 {
56  if(configFile_.isEmpty())
57  {
58  return PreferencesDialog::getIniFilePath();
59  }
60  return configFile_;
61 }
62 
64 {
65  return QDir::homePath()+"/.ros/"+QFileInfo(configFile_).fileName()+".tmp";
66 }
67 
68 void PreferencesDialogROS::readCameraSettings(const QString & filePath)
69 {
70  this->setInputRate(0);
71 }
72 
74 {
75  return tr("Reading parameters from the ROS server...");
76 }
77 
78 bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
79 {
80  QString path = getIniFilePath();
81  if(!filePath.isEmpty())
82  {
83  path = filePath;
84  }
85 
86  ros::NodeHandle nh;
87  ROS_INFO("%s", this->getParamMessage().toStdString().c_str());
88  bool validParameters = true;
89  int readCount = 0;
91  // remove Odom parameters
92  for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end();)
93  {
94  if(iter->first.find("Odom") == 0)
95  {
96  parameters.erase(iter++);
97  }
98  else
99  {
100  ++iter;
101  }
102  }
103  for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i)
104  {
105  if(i->first.compare(rtabmap::Parameters::kRtabmapWorkingDirectory()) == 0)
106  {
107  // use working directory of the GUI, not the one on rosparam server
108  QSettings settings(path, QSettings::IniFormat);
109  settings.beginGroup("Core");
110  QString value = settings.value(rtabmap::Parameters::kRtabmapWorkingDirectory().c_str(), "").toString();
111  if(!value.isEmpty() && QDir(value).exists())
112  {
113  this->setParameter(rtabmap::Parameters::kRtabmapWorkingDirectory(), value.toStdString());
114  }
115  else
116  {
117  // use default one
118  char * rosHomePath = getenv("ROS_HOME");
119  std::string workingDir = rosHomePath?rosHomePath:(QDir::homePath()+"/.ros").toStdString();
120  this->setParameter(rtabmap::Parameters::kRtabmapWorkingDirectory(), workingDir);
121  }
122  settings.endGroup();
123  }
124  else
125  {
126  std::string value;
127  if(nh.getParam(i->first,value))
128  {
129  PreferencesDialog::setParameter(i->first, value);
130  ++readCount;
131  }
132  else
133  {
134  ROS_WARN("Parameter %s not found", i->first.c_str());
135  validParameters = false;
136  }
137  }
138  }
139 
140  ROS_INFO("Parameters read = %d", readCount);
141 
142  if(validParameters)
143  {
144  ROS_INFO("Parameters successfully read.");
145  }
146  else
147  {
148  if(this->isVisible())
149  {
150  QString warning = tr("Failed to get some RTAB-Map parameters from ROS server, the rtabmap node may be not started or some parameters won't work...");
151  ROS_WARN("%s", warning.toStdString().c_str());
152  QMessageBox::warning(this, tr("Can't read parameters from ROS server."), warning);
153  }
154  return false;
155  }
156  return true;
157 }
158 
159 void PreferencesDialogROS::writeCoreSettings(const QString & filePath) const
160 {
161  QString path = getIniFilePath();
162  if(!filePath.isEmpty())
163  {
164  path = filePath;
165  }
166 
167  if(QFile::exists(path))
168  {
169  rtabmap::ParametersMap parameters = this->getAllParameters();
170 
171  std::string workingDir = uValue(parameters, Parameters::kRtabmapWorkingDirectory(), std::string(""));
172 
173  if(!workingDir.empty())
174  {
175  //Just update GUI working directory
176  QSettings settings(path, QSettings::IniFormat);
177  settings.beginGroup("Core");
178  settings.remove("");
179  settings.setValue(Parameters::kRtabmapWorkingDirectory().c_str(), workingDir.c_str());
180  settings.endGroup();
181  }
182  }
183 }
184 
void setParameter(const std::string &key, const std::string &value)
rtabmap::ParametersMap getAllParameters() const
std::map< std::string, std::string > ParametersMap
virtual void writeCoreSettings(const QString &filePath) const
virtual bool readCoreSettings(const QString &filePath)
#define ROS_WARN(...)
#define ROS_INFO(...)
virtual QString getTmpIniFilePath() const
static const ParametersMap & getDefaultParameters()
virtual QString getParamMessage()
bool getParam(const std::string &key, std::string &s) const
virtual QString getIniFilePath() const
void setInputRate(double value)
PreferencesDialogROS(const QString &configFile)
virtual void readCameraSettings(const QString &filePath)
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:42:19