feat(slam): add rtabmap_ros
This commit is contained in:
X-lanni
@@ -0,0 +1,98 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "rtabmap_viz/GuiWrapper.h"
|
||||
#include "rtabmap/utilite/ULogger.h"
|
||||
|
||||
#include <QApplication>
|
||||
#include <rtabmap/gui/MainWindow.h>
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
#include <signal.h>
|
||||
|
||||
QApplication * app = 0;
|
||||
|
||||
void my_handler(int){
|
||||
UINFO("rtabmap_viz: ctrl-c catched! Exiting Qt app...");
|
||||
app->exit(-1);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
UINFO("Starting node...");
|
||||
|
||||
ULogger::setType(ULogger::kTypeConsole);
|
||||
ULogger::setLevel(ULogger::kWarning);
|
||||
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
app = new QApplication(argc, argv);
|
||||
app->connect( app, SIGNAL( lastWindowClosed() ), app, SLOT( quit() ) );
|
||||
|
||||
std::vector<std::string> arguments;
|
||||
for(int i=1;i<argc;++i)
|
||||
{
|
||||
arguments.push_back(argv[i]);
|
||||
}
|
||||
|
||||
int r;
|
||||
{
|
||||
rclcpp::NodeOptions options;
|
||||
options.arguments(arguments);
|
||||
auto node = std::make_shared<rtabmap_viz::GuiWrapper>(options);
|
||||
|
||||
// Catch ctrl-c to close the gui
|
||||
// (Place this after QApplication's constructor)
|
||||
struct sigaction sigIntHandler;
|
||||
sigIntHandler.sa_handler = my_handler;
|
||||
sigemptyset(&sigIntHandler.sa_mask);
|
||||
sigIntHandler.sa_flags = 0;
|
||||
sigaction(SIGINT, &sigIntHandler, NULL);
|
||||
|
||||
// Here start the ROS events loop
|
||||
rclcpp::executors::SingleThreadedExecutor executor; //Use 1 thread
|
||||
executor.add_node(node);
|
||||
auto spin_executor = [&executor]() {
|
||||
executor.spin();
|
||||
};
|
||||
|
||||
// Launch executer
|
||||
std::thread execution_thread(spin_executor);
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "rtabmap_viz started.");
|
||||
// Now wait for application to finish
|
||||
r = app->exec();// MUST be called by the Main Thread
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "rtabmap_viz stopping spinner...");
|
||||
rclcpp::shutdown();
|
||||
execution_thread.join();
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "rtabmap_viz: All done! Closing...");
|
||||
}
|
||||
delete app;
|
||||
|
||||
return r;
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,213 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "rtabmap_viz/PreferencesDialogROS.h"
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
#include <QDir>
|
||||
#include <QFileInfo>
|
||||
#include <QSettings>
|
||||
#include <QHBoxLayout>
|
||||
#include <QTimer>
|
||||
#include <QLabel>
|
||||
#include <rtabmap/core/RtabmapEvent.h>
|
||||
#include <QMessageBox>
|
||||
#include <rtabmap/utilite/UStl.h>
|
||||
#include <rtabmap/utilite/UTimer.h>
|
||||
#include <rtabmap/utilite/UThread.h>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
PreferencesDialogROS::PreferencesDialogROS(rclcpp::Node * node, const QString & configFile, const std::string & rtabmapNodeName) :
|
||||
configFile_(configFile),
|
||||
node_(node),
|
||||
rtabmapNodeName_(rtabmapNodeName)
|
||||
{
|
||||
UASSERT(node_);
|
||||
}
|
||||
|
||||
PreferencesDialogROS::~PreferencesDialogROS()
|
||||
{
|
||||
QFile::remove(getTmpIniFilePath());
|
||||
}
|
||||
|
||||
QString PreferencesDialogROS::getIniFilePath() const
|
||||
{
|
||||
if(configFile_.isEmpty())
|
||||
{
|
||||
return PreferencesDialog::getIniFilePath();
|
||||
}
|
||||
return configFile_;
|
||||
}
|
||||
|
||||
QString PreferencesDialogROS::getTmpIniFilePath() const
|
||||
{
|
||||
return QDir::homePath()+"/.ros/"+QFileInfo(configFile_).fileName()+".tmp";
|
||||
}
|
||||
|
||||
void PreferencesDialogROS::readRtabmapNodeParameters()
|
||||
{
|
||||
readCoreSettings(getTmpIniFilePath());
|
||||
}
|
||||
|
||||
void PreferencesDialogROS::readCameraSettings(const QString &)
|
||||
{
|
||||
this->setInputRate(0);
|
||||
}
|
||||
|
||||
QString PreferencesDialogROS::getParamMessage()
|
||||
{
|
||||
return tr("Reading parameters from the ROS server...");
|
||||
}
|
||||
|
||||
bool PreferencesDialogROS::hasAllParameters()
|
||||
{
|
||||
auto client = std::make_shared<rclcpp::AsyncParametersClient>(node_, rtabmapNodeName_);
|
||||
return client->service_is_ready();
|
||||
}
|
||||
|
||||
bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
|
||||
{
|
||||
QString path = getIniFilePath();
|
||||
if(!filePath.isEmpty())
|
||||
{
|
||||
path = filePath;
|
||||
}
|
||||
|
||||
char nodeName[42];
|
||||
snprintf(
|
||||
nodeName, sizeof(nodeName), "rtabmap_viz_param_client_%zx",
|
||||
reinterpret_cast<size_t>(this)
|
||||
);
|
||||
auto node = std::make_shared<rclcpp::Node>(nodeName);
|
||||
RCLCPP_INFO(node_->get_logger(), "%s", this->getParamMessage().toStdString().c_str());
|
||||
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
|
||||
// remove Odom parameters
|
||||
for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end();)
|
||||
{
|
||||
if(iter->first.find("Odom") == 0)
|
||||
{
|
||||
parameters.erase(iter++);
|
||||
}
|
||||
else
|
||||
{
|
||||
++iter;
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<std::string> rosParameters;
|
||||
|
||||
for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i)
|
||||
{
|
||||
if(i->first.compare(rtabmap::Parameters::kRtabmapWorkingDirectory()) == 0)
|
||||
{
|
||||
// use working directory of the GUI, not the one on rosparam server
|
||||
QSettings settings(path, QSettings::IniFormat);
|
||||
settings.beginGroup("Core");
|
||||
QString value = settings.value(rtabmap::Parameters::kRtabmapWorkingDirectory().c_str(), "").toString();
|
||||
if(!value.isEmpty() && QDir(value).exists())
|
||||
{
|
||||
this->setParameter(rtabmap::Parameters::kRtabmapWorkingDirectory(), value.toStdString());
|
||||
}
|
||||
else
|
||||
{
|
||||
// use default one
|
||||
char * rosHomePath = getenv("ROS_HOME");
|
||||
std::string workingDir = rosHomePath?rosHomePath:(QDir::homePath()+"/.ros").toStdString();
|
||||
this->setParameter(rtabmap::Parameters::kRtabmapWorkingDirectory(), workingDir);
|
||||
}
|
||||
settings.endGroup();
|
||||
}
|
||||
else
|
||||
{
|
||||
rosParameters.push_back(i->first);
|
||||
}
|
||||
}
|
||||
|
||||
auto client = std::make_shared<rclcpp::AsyncParametersClient>(node, rtabmapNodeName_);
|
||||
if (!client->wait_for_service(std::chrono::seconds(5))) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Can't call rtabmap parameters service, is the node running?");
|
||||
}
|
||||
int readCount = 0;
|
||||
if(client->service_is_ready())
|
||||
{
|
||||
auto parameters = client->get_parameters(rosParameters);
|
||||
if (rclcpp::spin_until_future_complete(node, parameters, std::chrono::seconds(5)) ==
|
||||
rclcpp::FutureReturnCode::SUCCESS)
|
||||
{
|
||||
for (auto & parameter : parameters.get()) {
|
||||
const std::string & key = parameter.get_name();
|
||||
std::string value = parameter.value_to_string();
|
||||
PreferencesDialog::setParameter(key, value);
|
||||
++readCount;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "Parameters read = %d", readCount);
|
||||
|
||||
if(readCount>0)
|
||||
{
|
||||
RCLCPP_INFO(node_->get_logger(), "Parameters successfully read.");
|
||||
}
|
||||
else
|
||||
{
|
||||
if(this->isVisible())
|
||||
{
|
||||
QString warning = tr("Failed to get RTAB-Map parameters from ROS server, the rtabmap node may be not started or some parameters won't work...");
|
||||
RCLCPP_WARN(node_->get_logger(), "%s", warning.toStdString().c_str());
|
||||
QMessageBox::warning(this, tr("Can't read parameters from ROS server."), warning);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void PreferencesDialogROS::writeCoreSettings(const QString & filePath) const
|
||||
{
|
||||
QString path = getIniFilePath();
|
||||
if(!filePath.isEmpty())
|
||||
{
|
||||
path = filePath;
|
||||
}
|
||||
|
||||
if(QFile::exists(path))
|
||||
{
|
||||
rtabmap::ParametersMap parameters = this->getAllParameters();
|
||||
|
||||
std::string workingDir = uValue(parameters, Parameters::kRtabmapWorkingDirectory(), std::string(""));
|
||||
|
||||
if(!workingDir.empty())
|
||||
{
|
||||
//Just update GUI working directory
|
||||
QSettings settings(path, QSettings::IniFormat);
|
||||
settings.beginGroup("Core");
|
||||
settings.remove("");
|
||||
settings.setValue(Parameters::kRtabmapWorkingDirectory().c_str(), workingDir.c_str());
|
||||
settings.endGroup();
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user