feat(slam): add rtabmap_ros
This commit is contained in:
X-lanni
@@ -0,0 +1,80 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(rtabmap_viz)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64")
|
||||
# issues #1285 #1288
|
||||
find_library(
|
||||
builtin_interfaces__rosidl_generator_c_LIB NAMES builtin_interfaces__rosidl_generator_c
|
||||
PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib"
|
||||
NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED
|
||||
)
|
||||
find_library(
|
||||
rcutils_LIB NAMES rcutils
|
||||
PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib"
|
||||
NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED
|
||||
)
|
||||
find_library(
|
||||
crypto_LIB NAMES crypto
|
||||
PATHS "/usr/lib/${CMAKE_SYSTEM_PROCESSOR}-linux-gnu"
|
||||
NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED
|
||||
)
|
||||
endif()
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(cv_bridge REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(std_srvs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(rtabmap_msgs REQUIRED)
|
||||
find_package(rtabmap_sync REQUIRED)
|
||||
find_package(tf2 REQUIRED)
|
||||
|
||||
find_package(RTABMap COMPONENTS gui REQUIRED)
|
||||
|
||||
include_directories(
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/include
|
||||
)
|
||||
|
||||
# libraries
|
||||
SET(Libraries
|
||||
cv_bridge
|
||||
geometry_msgs
|
||||
rclcpp
|
||||
std_msgs
|
||||
std_srvs
|
||||
nav_msgs
|
||||
rtabmap_msgs
|
||||
rtabmap_sync
|
||||
tf2
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
add_executable(rtabmap_viz src/GuiNode.cpp src/GuiWrapper.cpp src/PreferencesDialogROS.cpp include/${PROJECT_NAME}/PreferencesDialogROS.h)
|
||||
ament_target_dependencies(rtabmap_viz ${Libraries})
|
||||
SET_TARGET_PROPERTIES(
|
||||
rtabmap_viz
|
||||
PROPERTIES
|
||||
AUTOUIC ON
|
||||
AUTOMOC ON
|
||||
AUTORCC ON
|
||||
)
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
install(TARGETS
|
||||
rtabmap_viz
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,169 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef GUIWRAPPER_H_
|
||||
#define GUIWRAPPER_H_
|
||||
|
||||
#include <rtabmap_viz/visibility.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp/executor.hpp>
|
||||
#include "rtabmap_msgs/msg/info.hpp"
|
||||
#include "rtabmap_msgs/msg/map_data.hpp"
|
||||
#include "rtabmap_msgs/msg/odom_info.hpp"
|
||||
#include "rtabmap_msgs/msg/goal.hpp"
|
||||
#include "rtabmap/utilite/UEventsHandler.h"
|
||||
#include "rtabmap/core/Transform.h"
|
||||
|
||||
#include <tf2_ros/buffer.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
|
||||
#include <geometry_msgs/msg/twist_stamped.hpp>
|
||||
#include <nav_msgs/msg/path.hpp>
|
||||
#include <std_msgs/msg/bool.hpp>
|
||||
#include <std_msgs/msg/int32_multi_array.hpp>
|
||||
#include "rtabmap_msgs/srv/get_map.hpp"
|
||||
|
||||
#include <rtabmap_sync/CommonDataSubscriber.h>
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
class MainWindow;
|
||||
class PreferencesDialog;
|
||||
}
|
||||
|
||||
class QApplication;
|
||||
|
||||
namespace rtabmap_viz {
|
||||
|
||||
class GuiWrapper : public rclcpp::Node, public UEventsHandler, public rtabmap_sync::CommonDataSubscriber
|
||||
{
|
||||
public:
|
||||
RTABMAP_VIZ_PUBLIC
|
||||
explicit GuiWrapper(const rclcpp::NodeOptions & options);
|
||||
virtual ~GuiWrapper();
|
||||
|
||||
protected:
|
||||
virtual bool handleEvent(UEvent * anEvent);
|
||||
|
||||
private:
|
||||
void infoMapCallback(const rtabmap_msgs::msg::Info::ConstSharedPtr infoMsg, const rtabmap_msgs::msg::MapData::ConstSharedPtr mapMsg);
|
||||
void infoCallback(const rtabmap_msgs::msg::Info::ConstSharedPtr infoMsg);
|
||||
void goalPathCallback(const rtabmap_msgs::msg::Goal::ConstSharedPtr goalMsg, const nav_msgs::msg::Path::ConstSharedPtr pathMsg);
|
||||
void goalReachedCallback(const std_msgs::msg::Bool::ConstSharedPtr value);
|
||||
|
||||
virtual void commonMultiCameraCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
|
||||
const std::vector<sensor_msgs::msg::CameraInfo> & cameraInfoMsgs,
|
||||
const std::vector<sensor_msgs::msg::CameraInfo> & depthCameraInfoMsgs,
|
||||
const sensor_msgs::msg::LaserScan & scanMsg,
|
||||
const sensor_msgs::msg::PointCloud2 & scan3dMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
|
||||
const std::vector<rtabmap_msgs::msg::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_msgs::msg::GlobalDescriptor>(),
|
||||
const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPoints = std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> >(),
|
||||
const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3d = std::vector<std::vector<rtabmap_msgs::msg::Point3f> >(),
|
||||
const std::vector<cv::Mat> & localDescriptors = std::vector<cv::Mat>());
|
||||
virtual void commonStereoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
|
||||
const cv_bridge::CvImageConstPtr& leftImageMsg,
|
||||
const cv_bridge::CvImageConstPtr& rightImageMsg,
|
||||
const sensor_msgs::msg::CameraInfo& leftCamInfoMsg,
|
||||
const sensor_msgs::msg::CameraInfo& rightCamInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan & scan2dMsg,
|
||||
const sensor_msgs::msg::PointCloud2 & scan3dMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
|
||||
const std::vector<rtabmap_msgs::msg::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_msgs::msg::GlobalDescriptor>(),
|
||||
const std::vector<rtabmap_msgs::msg::KeyPoint> & localKeyPoints = std::vector<rtabmap_msgs::msg::KeyPoint>(),
|
||||
const std::vector<rtabmap_msgs::msg::Point3f> & localPoints3d = std::vector<rtabmap_msgs::msg::Point3f>(),
|
||||
const cv::Mat & localDescriptors = cv::Mat());
|
||||
virtual void commonLaserScanCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
|
||||
const sensor_msgs::msg::LaserScan & scan2dMsg,
|
||||
const sensor_msgs::msg::PointCloud2 & scan3dMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
|
||||
const rtabmap_msgs::msg::GlobalDescriptor & globalDescriptor = rtabmap_msgs::msg::GlobalDescriptor());
|
||||
|
||||
virtual void commonOdomCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg);
|
||||
|
||||
virtual void commonSensorDataCallback(
|
||||
const rtabmap_msgs::msg::SensorData::ConstSharedPtr & sensorDataMsg,
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr & odomInfoMsg);
|
||||
|
||||
void defaultCallback(const nav_msgs::msg::Odometry::SharedPtr & odomMsg);
|
||||
|
||||
void processRequestedMap(const rtabmap_msgs::msg::MapData & map);
|
||||
bool callEmptyService(const std::string & name);
|
||||
bool callMapDataService(const std::string & name, bool global, bool optimized, bool graphOnly);
|
||||
|
||||
private:
|
||||
rtabmap::PreferencesDialog * prefDialog_;
|
||||
rtabmap::MainWindow * mainWindow_;
|
||||
std::string cameraNodeName_;
|
||||
double lastOdomInfoUpdateTime_;
|
||||
std::string rtabmapNodeName_;
|
||||
|
||||
// odometry subscription stuffs
|
||||
std::string frameId_;
|
||||
std::string odomFrameId_;
|
||||
double waitForTransform_;
|
||||
bool odomSensorSync_;
|
||||
double maxOdomUpdateRate_;
|
||||
std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tfListener_;
|
||||
|
||||
rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr republishNodeDataPub_;
|
||||
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::Info> infoTopic_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::MapData> mapDataTopic_;
|
||||
rclcpp::Subscription<rtabmap_msgs::msg::Info>::SharedPtr infoOnlyTopic_;
|
||||
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::Goal> goalTopic_;
|
||||
message_filters::Subscriber<nav_msgs::msg::Path> pathTopic_;
|
||||
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr goalReachedTopic_;
|
||||
|
||||
typedef message_filters::sync_policies::ExactTime<
|
||||
rtabmap_msgs::msg::Info,
|
||||
rtabmap_msgs::msg::MapData> MyInfoMapSyncPolicy;
|
||||
message_filters::Synchronizer<MyInfoMapSyncPolicy> * infoMapSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ExactTime<
|
||||
rtabmap_msgs::msg::Goal,
|
||||
nav_msgs::msg::Path> MyGoalPathSyncPolicy;
|
||||
message_filters::Synchronizer<MyGoalPathSyncPolicy> * goalPathSync_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* GUIWRAPPER_H_ */
|
||||
@@ -0,0 +1,65 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef PREFERENCESDIALOGROS_H_
|
||||
#define PREFERENCESDIALOGROS_H_
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp/executor.hpp>
|
||||
#include <rtabmap/gui/PreferencesDialog.h>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
class PreferencesDialogROS : public PreferencesDialog
|
||||
{
|
||||
Q_OBJECT
|
||||
public:
|
||||
PreferencesDialogROS(rclcpp::Node * node, const QString & configFile, const std::string & rtabmapNodeName);
|
||||
virtual ~PreferencesDialogROS();
|
||||
|
||||
virtual QString getIniFilePath() const;
|
||||
virtual QString getTmpIniFilePath() const;
|
||||
bool hasAllParameters();
|
||||
|
||||
public Q_SLOTS:
|
||||
void readRtabmapNodeParameters();
|
||||
|
||||
protected:
|
||||
virtual QString getParamMessage();
|
||||
|
||||
virtual void readCameraSettings(const QString & filePath);
|
||||
virtual bool readCoreSettings(const QString & filePath);
|
||||
virtual void writeCameraSettings(const QString &) const {}
|
||||
virtual void writeCoreSettings(const QString & filePath) const;
|
||||
|
||||
private:
|
||||
QString configFile_;
|
||||
rclcpp::Node * node_;
|
||||
std::string rtabmapNodeName_;
|
||||
};
|
||||
|
||||
#endif /* PREFERENCESDIALOGROS_H_ */
|
||||
@@ -0,0 +1,59 @@
|
||||
// Copyright 2016 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef RTABMAP_VIZ__VISIBILITY_CONTROL_H_
|
||||
#define RTABMAP_VIZ__VISIBILITY_CONTROL_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
|
||||
// https://gcc.gnu.org/wiki/Visibility
|
||||
|
||||
#if defined _WIN32 || defined __CYGWIN__
|
||||
#ifdef __GNUC__
|
||||
#define RTABMAP_VIZ_EXPORT __attribute__ ((dllexport))
|
||||
#define RTABMAP_VIZ_IMPORT __attribute__ ((dllimport))
|
||||
#else
|
||||
#define RTABMAP_VIZ_EXPORT __declspec(dllexport)
|
||||
#define RTABMAP_VIZ_IMPORT __declspec(dllimport)
|
||||
#endif
|
||||
#ifdef RTABMAP_VIZ_BUILDING_DLL
|
||||
#define RTABMAP_VIZ_PUBLIC RTABMAP_VIZ_EXPORT
|
||||
#else
|
||||
#define RTABMAP_VIZ_PUBLIC RTABMAP_VIZ_IMPORT
|
||||
#endif
|
||||
#define RTABMAP_VIZ_PUBLIC_TYPE RTABMAP_VIZ_PUBLIC
|
||||
#define RTABMAP_VIZ_LOCAL
|
||||
#else
|
||||
#define RTABMAP_VIZ_EXPORT __attribute__ ((visibility("default")))
|
||||
#define RTABMAP_VIZ_IMPORT
|
||||
#if __GNUC__ >= 4
|
||||
#define RTABMAP_VIZ_PUBLIC __attribute__ ((visibility("default")))
|
||||
#define RTABMAP_VIZ_LOCAL __attribute__ ((visibility("hidden")))
|
||||
#else
|
||||
#define RTABMAP_VIZ_PUBLIC
|
||||
#define RTABMAP_VIZ_LOCAL
|
||||
#endif
|
||||
#define RTABMAP_VIZ_PUBLIC_TYPE
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // RTABMAP_VIZ__VISIBILITY_CONTROL_H_
|
||||
|
||||
@@ -0,0 +1,31 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>rtabmap_viz</name>
|
||||
<version>0.22.0</version>
|
||||
<description>RTAB-Map's visualization package.</description>
|
||||
<maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
|
||||
<author>Mathieu Labbe</author>
|
||||
<license>BSD</license>
|
||||
<url type="bugtracker">https://github.com/introlab/rtabmap_ros/issues</url>
|
||||
<url type="repository">https://github.com/introlab/rtabmap_ros</url>
|
||||
|
||||
<buildtool_depend>ament_cmake_ros</buildtool_depend>
|
||||
|
||||
<build_depend>ros_environment</build_depend>
|
||||
|
||||
<depend>cv_bridge</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>std_srvs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>rtabmap_msgs</depend>
|
||||
<depend>rtabmap_sync</depend>
|
||||
<depend>tf2</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
|
||||
</package>
|
||||
@@ -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