feat(slam): add rtabmap_ros
This commit is contained in:
X-lanni
@@ -0,0 +1,139 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(rtabmap_odom)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# To suppress PCL_ROOT warning
|
||||
if(POLICY CMP0074)
|
||||
cmake_policy(SET CMP0074 NEW)
|
||||
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
|
||||
)
|
||||
endif()
|
||||
|
||||
find_package(ament_cmake_ros REQUIRED)
|
||||
find_package(cv_bridge REQUIRED)
|
||||
find_package(image_geometry REQUIRED)
|
||||
find_package(laser_geometry REQUIRED)
|
||||
find_package(message_filters REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(pcl_conversions REQUIRED)
|
||||
find_package(pcl_ros REQUIRED)
|
||||
find_package(pluginlib REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_components REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(rtabmap_conversions REQUIRED)
|
||||
find_package(rtabmap_msgs REQUIRED)
|
||||
find_package(rtabmap_util REQUIRED)
|
||||
find_package(rtabmap_sync REQUIRED)
|
||||
|
||||
include_directories(
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/include
|
||||
)
|
||||
|
||||
SET(Libraries
|
||||
cv_bridge
|
||||
image_geometry
|
||||
laser_geometry
|
||||
message_filters
|
||||
nav_msgs
|
||||
pcl_conversions
|
||||
pcl_ros
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
rtabmap_conversions
|
||||
rtabmap_msgs
|
||||
rtabmap_util
|
||||
rtabmap_sync
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
SET(rtabmap_odom_lib_src
|
||||
src/OdometryROS.cpp
|
||||
)
|
||||
|
||||
SET(rtabmap_odom_plugins_lib_src
|
||||
src/nodelets/rgbd_odometry.cpp
|
||||
src/nodelets/stereo_odometry.cpp
|
||||
src/nodelets/icp_odometry.cpp
|
||||
)
|
||||
|
||||
############################
|
||||
## Declare a cpp library
|
||||
############################
|
||||
add_library(rtabmap_odom SHARED ${rtabmap_odom_lib_src})
|
||||
target_include_directories(rtabmap_odom
|
||||
PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
)
|
||||
add_library(rtabmap_odom_plugins SHARED ${rtabmap_odom_plugins_lib_src})
|
||||
|
||||
ament_target_dependencies(rtabmap_odom ${Libraries})
|
||||
ament_target_dependencies(rtabmap_odom_plugins ${Libraries})
|
||||
|
||||
target_link_libraries(rtabmap_odom_plugins rtabmap_odom)
|
||||
|
||||
rclcpp_components_register_nodes(rtabmap_odom_plugins "rtabmap_odom::RGBDOdometry")
|
||||
rclcpp_components_register_nodes(rtabmap_odom_plugins "rtabmap_odom::StereoOdometry")
|
||||
rclcpp_components_register_nodes(rtabmap_odom_plugins "rtabmap_odom::ICPOdometry")
|
||||
|
||||
|
||||
add_executable(rtabmap_rgbd_odometry src/RGBDOdometryNode.cpp)
|
||||
ament_target_dependencies(rtabmap_rgbd_odometry ${Libraries})
|
||||
target_link_libraries(rtabmap_rgbd_odometry rtabmap_odom_plugins)
|
||||
set_target_properties(rtabmap_rgbd_odometry PROPERTIES OUTPUT_NAME "rgbd_odometry")
|
||||
|
||||
add_executable(rtabmap_stereo_odometry src/StereoOdometryNode.cpp)
|
||||
ament_target_dependencies(rtabmap_stereo_odometry ${Libraries})
|
||||
target_link_libraries(rtabmap_stereo_odometry rtabmap_odom_plugins)
|
||||
set_target_properties(rtabmap_stereo_odometry PROPERTIES OUTPUT_NAME "stereo_odometry")
|
||||
|
||||
add_executable(rtabmap_icp_odometry src/ICPOdometryNode.cpp)
|
||||
ament_target_dependencies(rtabmap_icp_odometry ${Libraries})
|
||||
target_link_libraries(rtabmap_icp_odometry rtabmap_odom_plugins)
|
||||
set_target_properties(rtabmap_icp_odometry PROPERTIES OUTPUT_NAME "icp_odometry")
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
ament_export_dependencies(${Libraries})
|
||||
ament_export_include_directories(include)
|
||||
ament_export_targets(${PROJECT_NAME}) # To include downstream with targets
|
||||
ament_export_libraries(rtabmap_odom rtabmap_odom_plugins) # To include downstream without targets
|
||||
|
||||
install(TARGETS
|
||||
rtabmap_odom
|
||||
rtabmap_odom_plugins
|
||||
EXPORT ${PROJECT_NAME}
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
)
|
||||
|
||||
install(TARGETS
|
||||
rtabmap_rgbd_odometry
|
||||
rtabmap_icp_odometry
|
||||
rtabmap_stereo_odometry
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(DIRECTORY include/
|
||||
DESTINATION include
|
||||
FILES_MATCHING PATTERN "*.h"
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@@ -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.
|
||||
*/
|
||||
|
||||
#ifndef ODOMETRYROS_H_
|
||||
#define ODOMETRYROS_H_
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include <tf2_ros/transform_broadcaster.h>
|
||||
#include <tf2_ros/buffer.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
|
||||
#include <diagnostic_updater/diagnostic_updater.hpp>
|
||||
|
||||
#include <std_srvs/srv/empty.hpp>
|
||||
#include <std_msgs/msg/header.hpp>
|
||||
#include <nav_msgs/msg/odometry.hpp>
|
||||
#include <sensor_msgs/msg/imu.hpp>
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
|
||||
#include <rtabmap_msgs/msg/odom_info.hpp>
|
||||
#include <rtabmap_msgs/msg/rgbd_image.hpp>
|
||||
#include <rtabmap_msgs/srv/reset_pose.hpp>
|
||||
#include <rtabmap/core/SensorData.h>
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
#include <rtabmap/utilite/UThread.h>
|
||||
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
#include "rtabmap_util/ULogToRosout.h"
|
||||
#include "rtabmap_sync/SyncDiagnostic.h"
|
||||
|
||||
namespace rtabmap {
|
||||
class Odometry;
|
||||
}
|
||||
|
||||
namespace rtabmap_odom {
|
||||
|
||||
class OdometryROS : public rclcpp::Node, public UThread
|
||||
{
|
||||
|
||||
public:
|
||||
explicit OdometryROS(const rclcpp::NodeOptions & options);
|
||||
explicit OdometryROS(const std::string & name, const rclcpp::NodeOptions & options);
|
||||
virtual ~OdometryROS();
|
||||
|
||||
void processData(rtabmap::SensorData & data, const std_msgs::msg::Header & header);
|
||||
|
||||
void resetOdom(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
|
||||
void resetToPose(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_msgs::srv::ResetPose::Request>, std::shared_ptr<rtabmap_msgs::srv::ResetPose::Response>);
|
||||
void pause(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
|
||||
void resume(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
|
||||
void setLogDebug(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
|
||||
void setLogInfo(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
|
||||
void setLogWarn(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
|
||||
void setLogError(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
|
||||
|
||||
const std::string & frameId() const {return frameId_;}
|
||||
const std::string & odomFrameId() const {return odomFrameId_;}
|
||||
const std::string & guessFrameId() const {return guessFrameId_;}
|
||||
const rtabmap::ParametersMap & parameters() const {return parameters_;}
|
||||
bool isPaused() const {return paused_;}
|
||||
|
||||
protected:
|
||||
void init(bool stereoParams, bool visParams, bool icpParams);
|
||||
rmw_qos_reliability_policy_t qos() const {return qos_;}
|
||||
void initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync, const std::string & subscribedTopic = "");
|
||||
|
||||
virtual void flushCallbacks() {};
|
||||
tf2_ros::Buffer & tfBuffer() {return *tfBuffer_;}
|
||||
const double & waitForTransform() const {return waitForTransform_;}
|
||||
rtabmap::Transform velocityGuess() const;
|
||||
double previousStamp() const {return previousStamp_;}
|
||||
virtual void postProcessData(const rtabmap::SensorData & /*data*/, const std_msgs::msg::Header & /*header*/) const {}
|
||||
|
||||
private:
|
||||
|
||||
virtual void mainLoop();
|
||||
virtual void mainLoopKill();
|
||||
virtual void updateParameters(rtabmap::ParametersMap &) {}
|
||||
virtual void onOdomInit() {}
|
||||
|
||||
void callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg);
|
||||
void reset(const rtabmap::Transform & pose = rtabmap::Transform::getIdentity());
|
||||
|
||||
protected:
|
||||
rclcpp::CallbackGroup::SharedPtr dataCallbackGroup_;
|
||||
void tick(const rclcpp::Time & stamp);
|
||||
|
||||
private:
|
||||
rtabmap::Odometry * odometry_;
|
||||
|
||||
// parameters
|
||||
std::string frameId_;
|
||||
std::string odomFrameId_;
|
||||
std::string groundTruthFrameId_;
|
||||
std::string groundTruthBaseFrameId_;
|
||||
std::string guessFrameId_;
|
||||
double guessMinTranslation_;
|
||||
double guessMinRotation_;
|
||||
double guessMinTime_;
|
||||
bool publishTf_;
|
||||
double waitForTransform_;
|
||||
bool publishNullWhenLost_;
|
||||
bool publishCompressedSensorData_;
|
||||
rmw_qos_reliability_policy_t qos_;
|
||||
rtabmap::ParametersMap parameters_;
|
||||
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odomPub_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::OdomInfo>::SharedPtr odomInfoPub_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::OdomInfo>::SharedPtr odomInfoLitePub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr odomLocalMap_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr odomLocalScanMap_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr odomLastFrame_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr odomRgbdImagePub_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::SensorData>::SharedPtr odomSensorDataPub_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::SensorData>::SharedPtr odomSensorDataFeaturesPub_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::SensorData>::SharedPtr odomSensorDataCompressedPub_;
|
||||
|
||||
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr resetSrv_;
|
||||
rclcpp::Service<rtabmap_msgs::srv::ResetPose>::SharedPtr resetToPoseSrv_;
|
||||
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr pauseSrv_;
|
||||
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr resumeSrv_;
|
||||
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr setLogDebugSrv_;
|
||||
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr setLogInfoSrv_;
|
||||
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr setLogWarnSrv_;
|
||||
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr setLogErrorSrv_;
|
||||
|
||||
std::shared_ptr<tf2_ros::TransformBroadcaster> tfBroadcaster_;
|
||||
std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tfListener_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imuSub_;
|
||||
rclcpp::CallbackGroup::SharedPtr imuCallbackGroup_;
|
||||
|
||||
// Safe-threading
|
||||
UMutex imuMutex_;
|
||||
UMutex dataMutex_;
|
||||
USemaphore dataReady_;
|
||||
rtabmap::SensorData dataToProcess_;
|
||||
std_msgs::msg::Header dataHeaderToProcess_;
|
||||
bool bufferedDataToProcess_;
|
||||
|
||||
bool paused_;
|
||||
int resetCountdown_;
|
||||
int resetCurrentCount_;
|
||||
bool stereoParams_;
|
||||
bool visParams_;
|
||||
bool icpParams_;
|
||||
rtabmap::Transform guess_;
|
||||
rtabmap::Transform guessPreviousPose_;
|
||||
double previousStamp_;
|
||||
double previousClockTime_;
|
||||
double expectedUpdateRate_;
|
||||
double maxUpdateRate_;
|
||||
double minUpdateRate_;
|
||||
std::string compressionImgFormat_;
|
||||
bool compressionParallelized_;
|
||||
int odomStrategy_;
|
||||
bool waitIMUToinit_;
|
||||
bool alwaysCheckImuTf_;
|
||||
bool imuProcessed_;
|
||||
int processedMsgs_;
|
||||
int droppedMsgs_;
|
||||
std::map<double, sensor_msgs::msg::Imu::ConstSharedPtr> imus_;
|
||||
std::string configPath_;
|
||||
rtabmap::Transform initialPose_;
|
||||
rtabmap::Transform imuLocalTransform_;
|
||||
|
||||
rtabmap_util::ULogToRosout ulogToRosout_;
|
||||
|
||||
class OdomStatusTask : public diagnostic_updater::DiagnosticTask
|
||||
{
|
||||
public:
|
||||
OdomStatusTask();
|
||||
void setStatus(bool isLost, int processedMsgs, int droppedMsgs);
|
||||
void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
|
||||
private:
|
||||
bool lost_;
|
||||
bool dataReceived_;
|
||||
int processedMsgs_;
|
||||
int droppedMsgs_;
|
||||
};
|
||||
OdomStatusTask statusDiagnostic_;
|
||||
std::unique_ptr<rtabmap_sync::SyncDiagnostic> syncDiagnostic_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,88 @@
|
||||
/*
|
||||
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 "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include <rtabmap_odom/OdometryROS.h>
|
||||
#include <rtabmap_odom/visibility.h>
|
||||
|
||||
//#include <pluginlib/class_list_macros.h>
|
||||
//#include <pluginlib/class_loader.hpp>
|
||||
|
||||
#include <sensor_msgs/msg/laser_scan.hpp>
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
|
||||
//#include "rtabmap_ros/PluginInterface.h"
|
||||
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
namespace rtabmap_odom
|
||||
{
|
||||
|
||||
class ICPOdometry : public rtabmap_odom::OdometryROS
|
||||
{
|
||||
public:
|
||||
RTABMAP_ODOM_PUBLIC
|
||||
explicit ICPOdometry(const rclcpp::NodeOptions & options);
|
||||
|
||||
virtual ~ICPOdometry();
|
||||
|
||||
private:
|
||||
virtual void updateParameters(rtabmap::ParametersMap &);
|
||||
virtual void onOdomInit();
|
||||
|
||||
void callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scanMsg);
|
||||
void callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr pointCloudMsg);
|
||||
|
||||
protected:
|
||||
virtual void flushCallbacks();
|
||||
void postProcessData(const SensorData & data, const std_msgs::msg::Header & header) const;
|
||||
|
||||
private:
|
||||
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_sub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr filtered_scan_pub_;
|
||||
int scanCloudMaxPoints_;
|
||||
bool scanCloudIs2d_;
|
||||
int scanDownsamplingStep_;
|
||||
double scanRangeMin_;
|
||||
double scanRangeMax_;
|
||||
double scanVoxelSize_;
|
||||
int scanNormalK_;
|
||||
double scanNormalRadius_;
|
||||
double scanNormalGroundUp_;
|
||||
bool deskewing_;
|
||||
bool deskewingSlerp_;
|
||||
//std::vector<std::shared_ptr<rtabmap_ros::PluginInterface> > plugins_;
|
||||
//pluginlib::ClassLoader<rtabmap_ros::PluginInterface> plugin_loader_;
|
||||
bool scanReceived_ = false;
|
||||
bool cloudReceived_ = false;
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,156 @@
|
||||
/*
|
||||
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 "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include <rtabmap_odom/OdometryROS.h>
|
||||
#include <rtabmap_odom/visibility.h>
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
|
||||
#include <image_transport/image_transport.hpp>
|
||||
#include <image_transport/subscriber_filter.hpp>
|
||||
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <rtabmap_msgs/msg/rgbd_image.hpp>
|
||||
#include <rtabmap_msgs/msg/rgbd_images.hpp>
|
||||
|
||||
#ifdef PRE_ROS_IRON
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#else
|
||||
#include <cv_bridge/cv_bridge.hpp>
|
||||
#endif
|
||||
|
||||
namespace rtabmap_odom
|
||||
{
|
||||
|
||||
class RGBDOdometry : public rtabmap_odom::OdometryROS
|
||||
{
|
||||
public:
|
||||
RTABMAP_ODOM_PUBLIC
|
||||
explicit RGBDOdometry(const rclcpp::NodeOptions & options);
|
||||
virtual ~RGBDOdometry();
|
||||
|
||||
private:
|
||||
virtual void updateParameters(rtabmap::ParametersMap & parameters);
|
||||
virtual void onOdomInit();
|
||||
|
||||
void commonCallback(
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & rgbImages,
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & depthImages,
|
||||
const std::vector<sensor_msgs::msg::CameraInfo>& cameraInfos);
|
||||
|
||||
void callback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr image,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depth,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo);
|
||||
|
||||
void callbackRGBDX(
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images);
|
||||
|
||||
void callbackRGBD(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image);
|
||||
|
||||
void callbackRGBD2(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2);
|
||||
|
||||
void callbackRGBD3(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3);
|
||||
|
||||
void callbackRGBD4(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4);
|
||||
|
||||
void callbackRGBD5(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5);
|
||||
|
||||
void callbackRGBD6(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6);
|
||||
|
||||
protected:
|
||||
virtual void flushCallbacks();
|
||||
|
||||
private:
|
||||
image_transport::SubscriberFilter image_mono_sub_;
|
||||
image_transport::SubscriberFilter image_depth_sub_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> info_sub_;
|
||||
|
||||
rclcpp::Subscription<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdSub_;
|
||||
rclcpp::Subscription<rtabmap_msgs::msg::RGBDImages>::SharedPtr rgbdxSub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image1_sub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image2_sub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image3_sub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image4_sub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image5_sub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image6_sub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo> MyApproxSyncPolicy;
|
||||
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
|
||||
typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo> MyExactSyncPolicy;
|
||||
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
|
||||
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync2Policy;
|
||||
message_filters::Synchronizer<MyApproxSync2Policy> * approxSync2_;
|
||||
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync2Policy;
|
||||
message_filters::Synchronizer<MyExactSync2Policy> * exactSync2_;
|
||||
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync3Policy;
|
||||
message_filters::Synchronizer<MyApproxSync3Policy> * approxSync3_;
|
||||
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync3Policy;
|
||||
message_filters::Synchronizer<MyExactSync3Policy> * exactSync3_;
|
||||
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync4Policy;
|
||||
message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_;
|
||||
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync4Policy;
|
||||
message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
|
||||
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync5Policy;
|
||||
message_filters::Synchronizer<MyApproxSync5Policy> * approxSync5_;
|
||||
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync5Policy;
|
||||
message_filters::Synchronizer<MyExactSync5Policy> * exactSync5_;
|
||||
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync6Policy;
|
||||
message_filters::Synchronizer<MyApproxSync6Policy> * approxSync6_;
|
||||
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync6Policy;
|
||||
message_filters::Synchronizer<MyExactSync6Policy> * exactSync6_;
|
||||
int topicQueueSize_;
|
||||
int syncQueueSize_;
|
||||
bool keepColor_;
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,155 @@
|
||||
/*
|
||||
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 "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include <rtabmap_odom/OdometryROS.h>
|
||||
#include <rtabmap_odom/visibility.h>
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
|
||||
#include <image_transport/image_transport.hpp>
|
||||
#include <image_transport/subscriber_filter.hpp>
|
||||
|
||||
#ifdef PRE_ROS_IRON
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#else
|
||||
#include <cv_bridge/cv_bridge.hpp>
|
||||
#endif
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <rtabmap_msgs/msg/rgbd_image.hpp>
|
||||
#include <rtabmap_msgs/msg/rgbd_images.hpp>
|
||||
|
||||
namespace rtabmap_odom
|
||||
{
|
||||
|
||||
class StereoOdometry : public rtabmap_odom::OdometryROS
|
||||
{
|
||||
public:
|
||||
RTABMAP_ODOM_PUBLIC
|
||||
StereoOdometry(const rclcpp::NodeOptions & options);
|
||||
virtual ~StereoOdometry();
|
||||
|
||||
private:
|
||||
virtual void updateParameters(rtabmap::ParametersMap & parameters);
|
||||
virtual void onOdomInit();
|
||||
|
||||
void commonCallback(
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & leftImages,
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & rightImages,
|
||||
const std::vector<sensor_msgs::msg::CameraInfo>& leftCameraInfos,
|
||||
const std::vector<sensor_msgs::msg::CameraInfo>& rightCameraInfos);
|
||||
|
||||
void callback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageRectLeft,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageRectRight,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight);
|
||||
|
||||
void callbackRGBD(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image);
|
||||
void callbackRGBDX(
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images);
|
||||
void callbackRGBD2(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2);
|
||||
void callbackRGBD3(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3);
|
||||
void callbackRGBD4(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4);
|
||||
void callbackRGBD5(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5);
|
||||
void callbackRGBD6(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6);
|
||||
|
||||
|
||||
protected:
|
||||
virtual void flushCallbacks();
|
||||
|
||||
private:
|
||||
image_transport::SubscriberFilter imageRectLeft_;
|
||||
image_transport::SubscriberFilter imageRectRight_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoLeft_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoRight_;
|
||||
|
||||
rclcpp::Subscription<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdSub_;
|
||||
rclcpp::Subscription<rtabmap_msgs::msg::RGBDImages>::SharedPtr rgbdxSub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image1_sub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image2_sub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image3_sub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image4_sub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image5_sub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image6_sub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo> MyApproxSyncPolicy;
|
||||
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
|
||||
typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo> MyExactSyncPolicy;
|
||||
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
|
||||
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync2Policy;
|
||||
message_filters::Synchronizer<MyApproxSync2Policy> * approxSync2_;
|
||||
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync2Policy;
|
||||
message_filters::Synchronizer<MyExactSync2Policy> * exactSync2_;
|
||||
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync3Policy;
|
||||
message_filters::Synchronizer<MyApproxSync3Policy> * approxSync3_;
|
||||
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync3Policy;
|
||||
message_filters::Synchronizer<MyExactSync3Policy> * exactSync3_;
|
||||
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync4Policy;
|
||||
message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_;
|
||||
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync4Policy;
|
||||
message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
|
||||
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync5Policy;
|
||||
message_filters::Synchronizer<MyApproxSync5Policy> * approxSync5_;
|
||||
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync5Policy;
|
||||
message_filters::Synchronizer<MyExactSync5Policy> * exactSync5_;
|
||||
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync6Policy;
|
||||
message_filters::Synchronizer<MyApproxSync6Policy> * approxSync6_;
|
||||
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync6Policy;
|
||||
message_filters::Synchronizer<MyExactSync6Policy> * exactSync6_;
|
||||
|
||||
int topicQueueSize_;
|
||||
int syncQueueSize_;
|
||||
bool keepColor_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,58 @@
|
||||
// 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_ODOM__VISIBILITY_CONTROL_H_
|
||||
#define RTABMAP_ODOM__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_ODOM_EXPORT __attribute__ ((dllexport))
|
||||
#define RTABMAP_ODOM_IMPORT __attribute__ ((dllimport))
|
||||
#else
|
||||
#define RTABMAP_ODOM_EXPORT __declspec(dllexport)
|
||||
#define RTABMAP_ODOM_IMPORT __declspec(dllimport)
|
||||
#endif
|
||||
#ifdef RTABMAP_ODOM_BUILDING_DLL
|
||||
#define RTABMAP_ODOM_PUBLIC RTABMAP_ODOM_EXPORT
|
||||
#else
|
||||
#define RTABMAP_ODOM_PUBLIC RTABMAP_ODOM_IMPORT
|
||||
#endif
|
||||
#define RTABMAP_ODOM_PUBLIC_TYPE RTABMAP_ODOM_PUBLIC
|
||||
#define RTABMAP_ODOM_LOCAL
|
||||
#else
|
||||
#define RTABMAP_ODOM_EXPORT __attribute__ ((visibility("default")))
|
||||
#define RTABMAP_ODOM_IMPORT
|
||||
#if __GNUC__ >= 4
|
||||
#define RTABMAP_ODOM_PUBLIC __attribute__ ((visibility("default")))
|
||||
#define RTABMAP_ODOM_LOCAL __attribute__ ((visibility("hidden")))
|
||||
#else
|
||||
#define RTABMAP_ODOM_PUBLIC
|
||||
#define RTABMAP_ODOM_LOCAL
|
||||
#endif
|
||||
#define RTABMAP_ODOM_PUBLIC_TYPE
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // RTABMAP_ODOM__VISIBILITY_CONTROL_H_
|
||||
@@ -0,0 +1,37 @@
|
||||
<?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_odom</name>
|
||||
<version>0.22.0</version>
|
||||
<description>RTAB-Map's odometry 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>image_geometry</depend>
|
||||
<depend>laser_geometry</depend>
|
||||
<depend>message_filters</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>pcl_conversions</depend>
|
||||
<depend>pcl_ros</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_components</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>rtabmap_conversions</depend>
|
||||
<depend>rtabmap_msgs</depend>
|
||||
<depend>rtabmap_util</depend>
|
||||
<depend>rtabmap_sync</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
|
||||
</package>
|
||||
@@ -0,0 +1,79 @@
|
||||
/*
|
||||
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_odom/icp_odometry.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ULogger::setType(ULogger::kTypeConsole);
|
||||
ULogger::setLevel(ULogger::kWarning);
|
||||
|
||||
// process "--params" argument
|
||||
std::vector<std::string> arguments;
|
||||
for(int i=1;i<argc;++i)
|
||||
{
|
||||
if(strcmp(argv[i], "--params") == 0)
|
||||
{
|
||||
rtabmap::ParametersMap parametersOdom = rtabmap::Parameters::getDefaultOdometryParameters(false, false, true);
|
||||
for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
|
||||
{
|
||||
std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
|
||||
std::cout <<
|
||||
str <<
|
||||
std::setw(60 - str.size()) <<
|
||||
" [" <<
|
||||
rtabmap::Parameters::getDescription(iter->first).c_str() <<
|
||||
"]" <<
|
||||
std::endl;
|
||||
}
|
||||
UWARN("Node will now exit after showing default odometry parameters because "
|
||||
"argument \"--params\" is detected!");
|
||||
exit(0);
|
||||
}
|
||||
else if(strcmp(argv[i], "--udebug") == 0)
|
||||
{
|
||||
ULogger::setLevel(ULogger::kDebug);
|
||||
}
|
||||
else if(strcmp(argv[i], "--uinfo") == 0)
|
||||
{
|
||||
ULogger::setLevel(ULogger::kInfo);
|
||||
}
|
||||
arguments.push_back(argv[i]);
|
||||
}
|
||||
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::NodeOptions options;
|
||||
options.arguments(arguments);
|
||||
auto node = std::make_shared<rtabmap_odom::ICPOdometry>(options);
|
||||
rclcpp::executors::MultiThreadedExecutor executor;
|
||||
executor.add_node(node);
|
||||
executor.spin();
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,85 @@
|
||||
/*
|
||||
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_odom/rgbd_odometry.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#ifdef RTABMAP_PYTHON
|
||||
#include <rtabmap/core/PythonInterface.h>
|
||||
#endif
|
||||
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ULogger::setType(ULogger::kTypeConsole);
|
||||
ULogger::setLevel(ULogger::kWarning);
|
||||
|
||||
// process "--params" argument
|
||||
std::vector<std::string> arguments;
|
||||
for(int i=1;i<argc;++i)
|
||||
{
|
||||
if(strcmp(argv[i], "--params") == 0)
|
||||
{
|
||||
rtabmap::ParametersMap parametersOdom = rtabmap::Parameters::getDefaultOdometryParameters(false);
|
||||
for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
|
||||
{
|
||||
std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
|
||||
std::cout <<
|
||||
str <<
|
||||
std::setw(60 - str.size()) <<
|
||||
" [" <<
|
||||
rtabmap::Parameters::getDescription(iter->first).c_str() <<
|
||||
"]" <<
|
||||
std::endl;
|
||||
}
|
||||
UWARN("Node will now exit after showing default odometry parameters because "
|
||||
"argument \"--params\" is detected!");
|
||||
exit(0);
|
||||
}
|
||||
else if(strcmp(argv[i], "--udebug") == 0)
|
||||
{
|
||||
ULogger::setLevel(ULogger::kDebug);
|
||||
}
|
||||
else if(strcmp(argv[i], "--uinfo") == 0)
|
||||
{
|
||||
ULogger::setLevel(ULogger::kInfo);
|
||||
}
|
||||
arguments.push_back(argv[i]);
|
||||
}
|
||||
|
||||
#ifdef RTABMAP_PYTHON
|
||||
rtabmap::PythonInterface pythonInterface;
|
||||
#endif
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::NodeOptions options;
|
||||
options.arguments(arguments);
|
||||
auto node = std::make_shared<rtabmap_odom::RGBDOdometry>(options);
|
||||
rclcpp::executors::MultiThreadedExecutor executor;
|
||||
executor.add_node(node);
|
||||
executor.spin();
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,85 @@
|
||||
/*
|
||||
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_odom/stereo_odometry.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#ifdef RTABMAP_PYTHON
|
||||
#include <rtabmap/core/PythonInterface.h>
|
||||
#endif
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ULogger::setType(ULogger::kTypeConsole);
|
||||
ULogger::setLevel(ULogger::kWarning);
|
||||
|
||||
// process "--params" argument
|
||||
std::vector<std::string> arguments;
|
||||
for(int i=1;i<argc;++i)
|
||||
{
|
||||
if(strcmp(argv[i], "--params") == 0)
|
||||
{
|
||||
rtabmap::ParametersMap parametersOdom = rtabmap::Parameters::getDefaultOdometryParameters(true);
|
||||
for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
|
||||
{
|
||||
std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
|
||||
std::cout <<
|
||||
str <<
|
||||
std::setw(60 - str.size()) <<
|
||||
" [" <<
|
||||
rtabmap::Parameters::getDescription(iter->first).c_str() <<
|
||||
"]" <<
|
||||
std::endl;
|
||||
}
|
||||
UWARN("Node will now exit after showing default odometry parameters because "
|
||||
"argument \"--params\" is detected!");
|
||||
exit(0);
|
||||
}
|
||||
else if(strcmp(argv[i], "--udebug") == 0)
|
||||
{
|
||||
ULogger::setLevel(ULogger::kDebug);
|
||||
}
|
||||
else if(strcmp(argv[i], "--uinfo") == 0)
|
||||
{
|
||||
ULogger::setLevel(ULogger::kInfo);
|
||||
}
|
||||
arguments.push_back(argv[i]);
|
||||
|
||||
}
|
||||
|
||||
#ifdef RTABMAP_PYTHON
|
||||
rtabmap::PythonInterface pythonInterface;
|
||||
#endif
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::NodeOptions options;
|
||||
options.arguments(arguments);
|
||||
auto node = std::make_shared<rtabmap_odom::StereoOdometry>(options);
|
||||
rclcpp::executors::MultiThreadedExecutor executor;
|
||||
executor.add_node(node);
|
||||
executor.spin();
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,842 @@
|
||||
/*
|
||||
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_odom/icp_odometry.hpp>
|
||||
|
||||
#include <laser_geometry/laser_geometry.hpp>
|
||||
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
|
||||
#include "rtabmap_conversions/MsgConversion.h"
|
||||
|
||||
#include <rtabmap/core/util3d.h>
|
||||
#include <rtabmap/core/util3d_surface.h>
|
||||
#include <rtabmap/core/util3d_transforms.h>
|
||||
#include <rtabmap/core/util3d_filtering.h>
|
||||
#include <rtabmap/core/util2d.h>
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <rtabmap/utilite/UStl.h>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
namespace rtabmap_odom
|
||||
{
|
||||
|
||||
ICPOdometry::ICPOdometry(const rclcpp::NodeOptions & options) :
|
||||
OdometryROS("icp_odometry", options),
|
||||
scanCloudMaxPoints_(-1),
|
||||
scanCloudIs2d_(false),
|
||||
scanDownsamplingStep_(1),
|
||||
scanRangeMin_(0),
|
||||
scanRangeMax_(0),
|
||||
scanVoxelSize_(0.0),
|
||||
scanNormalK_(0),
|
||||
scanNormalRadius_(0.0),
|
||||
scanNormalGroundUp_(0.0),
|
||||
deskewing_(false),
|
||||
deskewingSlerp_(false),
|
||||
scanReceived_(false),
|
||||
cloudReceived_(false)
|
||||
{
|
||||
OdometryROS::init(false, false, true);
|
||||
}
|
||||
|
||||
ICPOdometry::~ICPOdometry()
|
||||
{
|
||||
}
|
||||
|
||||
void ICPOdometry::onOdomInit()
|
||||
{
|
||||
scanCloudMaxPoints_ = this->declare_parameter("scan_cloud_max_points", scanCloudMaxPoints_);
|
||||
scanCloudIs2d_ = this->declare_parameter("scan_cloud_is_2d", scanCloudIs2d_);
|
||||
scanDownsamplingStep_ = this->declare_parameter("scan_downsampling_step", scanDownsamplingStep_);
|
||||
scanRangeMin_ = this->declare_parameter("scan_range_min", scanRangeMin_);
|
||||
scanRangeMax_ = this->declare_parameter("scan_range_max", scanRangeMax_);
|
||||
scanVoxelSize_ = this->declare_parameter("scan_voxel_size", scanVoxelSize_);
|
||||
scanNormalK_ = this->declare_parameter("scan_normal_k", scanNormalK_);
|
||||
scanNormalRadius_ = this->declare_parameter("scan_normal_radius", scanNormalRadius_);
|
||||
scanNormalGroundUp_ = this->declare_parameter("scan_normal_ground_up", scanNormalGroundUp_);
|
||||
deskewing_ = this->declare_parameter("deskewing", deskewing_);
|
||||
deskewingSlerp_ = this->declare_parameter("deskewing_slerp", deskewingSlerp_);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "IcpOdometry: qos = %d", (int)qos());
|
||||
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
|
||||
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_cloud_is_2d = %s", scanCloudIs2d_?"true":"false");
|
||||
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_downsampling_step = %d", scanDownsamplingStep_);
|
||||
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_range_min = %f m", scanRangeMin_);
|
||||
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_range_max = %f m", scanRangeMax_);
|
||||
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_voxel_size = %f m", scanVoxelSize_);
|
||||
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_k = %d", scanNormalK_);
|
||||
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_radius = %f m", scanNormalRadius_);
|
||||
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_ground_up = %f", scanNormalGroundUp_);
|
||||
RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing = %s", deskewing_?"true":"false");
|
||||
RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing_slerp = %s", deskewingSlerp_?"true":"false");
|
||||
|
||||
rclcpp::SubscriptionOptions options;
|
||||
options.callback_group = dataCallbackGroup_;
|
||||
|
||||
scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1), options);
|
||||
cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1), options);
|
||||
|
||||
filtered_scan_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>("odom_filtered_input_scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()));
|
||||
|
||||
initDiagnosticMsg(uFormat("\n%s subscribed to %s and %s (make sure only one of this topic is published, otherwise remap one to a dummy topic name).",
|
||||
get_name(),
|
||||
scan_sub_->get_topic_name(),
|
||||
cloud_sub_->get_topic_name()), true);
|
||||
}
|
||||
|
||||
void ICPOdometry::updateParameters(ParametersMap & parameters)
|
||||
{
|
||||
//make sure we are using Reg/Strategy=0
|
||||
ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
|
||||
if(iter != parameters.end() && iter->second.compare("1") != 0)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "ICP odometry works only with \"Reg/Strategy\"=1. Ignoring value %s.", iter->second.c_str());
|
||||
}
|
||||
uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "1"));
|
||||
|
||||
iter = parameters.find(Parameters::kIcpDownsamplingStep());
|
||||
if(iter != parameters.end())
|
||||
{
|
||||
int value = uStr2Int(iter->second);
|
||||
if(value > 1)
|
||||
{
|
||||
if(!this->has_parameter("scan_downsampling_step"))
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_downsampling_step\" for convenience. \"%s\" is set to 1.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
|
||||
scanDownsamplingStep_ = value;
|
||||
iter->second = "1";
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_downsampling_step\" are set.", iter->first.c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
iter = parameters.find(Parameters::kIcpRangeMin());
|
||||
if(iter != parameters.end())
|
||||
{
|
||||
float value = uStr2Float(iter->second);
|
||||
if(value != 0.0f)
|
||||
{
|
||||
if(!this->has_parameter("scan_range_min"))
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_range_min\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
|
||||
scanRangeMin_ = value;
|
||||
iter->second = "0";
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_range_min\" are set.", iter->first.c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
iter = parameters.find(Parameters::kIcpRangeMax());
|
||||
if(iter != parameters.end())
|
||||
{
|
||||
float value = uStr2Float(iter->second);
|
||||
if(value != 0.0f)
|
||||
{
|
||||
if(!this->has_parameter("scan_range_max"))
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_range_max\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
|
||||
scanRangeMax_ = value;
|
||||
iter->second = "0";
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_range_max\" are set.", iter->first.c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
iter = parameters.find(Parameters::kIcpVoxelSize());
|
||||
if(iter != parameters.end())
|
||||
{
|
||||
float value = uStr2Float(iter->second);
|
||||
if(value != 0.0f)
|
||||
{
|
||||
if(!this->has_parameter("scan_voxel_size"))
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_voxel_size\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
|
||||
scanVoxelSize_ = value;
|
||||
iter->second = "0";
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_voxel_size\" are set.", iter->first.c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(this->has_parameter("scan_voxel_size"))
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_voxel_size is set (%f), setting %s to 0", scanVoxelSize_, Parameters::kIcpVoxelSize().c_str());
|
||||
parameters.insert(ParametersPair(Parameters::kIcpVoxelSize(), "0"));
|
||||
}
|
||||
iter = parameters.find(Parameters::kIcpPointToPlaneK());
|
||||
if(iter != parameters.end())
|
||||
{
|
||||
int value = uStr2Int(iter->second);
|
||||
if(value != 0)
|
||||
{
|
||||
if(!this->has_parameter("scan_normal_k"))
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_k\" for convenience.", iter->second.c_str(), iter->first.c_str());
|
||||
scanNormalK_ = value;
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_k is set (%d), setting %s to same value.", scanNormalK_, Parameters::kIcpPointToPlaneK().c_str());
|
||||
iter->second = uNumber2Str(scanNormalK_);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(this->has_parameter("scan_normal_k"))
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_k is set (%d), setting %s to same value.", scanNormalK_, Parameters::kIcpPointToPlaneK().c_str());
|
||||
parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneK(), uNumber2Str(scanNormalK_)));
|
||||
}
|
||||
iter = parameters.find(Parameters::kIcpPointToPlaneRadius());
|
||||
if(iter != parameters.end())
|
||||
{
|
||||
float value = uStr2Float(iter->second);
|
||||
if(value != 0.0f)
|
||||
{
|
||||
if(!this->has_parameter("scan_normal_radius"))
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_radius\" for convenience.", iter->second.c_str(), iter->first.c_str());
|
||||
scanNormalRadius_ = value;
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_radius is set (%f), setting %s to same value.", scanNormalRadius_, Parameters::kIcpPointToPlaneRadius().c_str());
|
||||
iter->second = uNumber2Str(scanNormalK_);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(this->has_parameter("scan_normal_radius"))
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_radius is set (%f), setting %s to same value.", scanNormalRadius_, Parameters::kIcpPointToPlaneRadius().c_str());
|
||||
parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneRadius(), uNumber2Str(scanNormalRadius_)));
|
||||
}
|
||||
iter = parameters.find(Parameters::kIcpPointToPlaneGroundNormalsUp());
|
||||
if(iter != parameters.end())
|
||||
{
|
||||
float value = uStr2Float(iter->second);
|
||||
if(value != 0.0f)
|
||||
{
|
||||
if(!this->has_parameter("scan_normal_ground_up"))
|
||||
{
|
||||
RCLCPP_WARN(get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_ground_up\" for convenience.", iter->second.c_str(), iter->first.c_str());
|
||||
scanNormalGroundUp_ = value;
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_ground_up is set (%f), setting %s to same value.", scanNormalGroundUp_, Parameters::kIcpPointToPlaneGroundNormalsUp().c_str());
|
||||
iter->second = uNumber2Str(scanNormalK_);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(this->has_parameter("scan_normal_ground_up"))
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_ground_up is set (%f), setting %s to same value.", scanNormalGroundUp_, Parameters::kIcpPointToPlaneGroundNormalsUp().c_str());
|
||||
parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(scanNormalGroundUp_)));
|
||||
}
|
||||
}
|
||||
|
||||
void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scanMsg)
|
||||
{
|
||||
if(cloudReceived_)
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "%s is already receiving clouds on \"%s\", but also "
|
||||
"just received a scan on \"%s\". Both subscribers cannot be "
|
||||
"used at the same time! Disabling scan subscriber.",
|
||||
get_name(), cloud_sub_->get_topic_name(), scan_sub_->get_topic_name());
|
||||
scan_sub_.reset();
|
||||
return;
|
||||
}
|
||||
|
||||
tick(scanMsg->header.stamp);
|
||||
|
||||
scanReceived_ = true;
|
||||
if(this->isPaused())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// make sure the frame of the laser is updated too
|
||||
Transform localScanTransform = rtabmap_conversions::getTransform(this->frameId(),
|
||||
scanMsg->header.frame_id,
|
||||
scanMsg->header.stamp,
|
||||
tfBuffer(), waitForTransform());
|
||||
if(localScanTransform.isNull())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "TF of received laser scan topic at time %fs is not set, aborting odometry update.", rtabmap_conversions::timestampFromROS(scanMsg->header.stamp));
|
||||
return;
|
||||
}
|
||||
|
||||
//transform in frameId_ frame
|
||||
sensor_msgs::msg::PointCloud2 scanOut;
|
||||
laser_geometry::LaserProjection projection;
|
||||
if(deskewing_ && (!guessFrameId().empty() || (frameId().compare(scanMsg->header.frame_id) != 0)))
|
||||
{
|
||||
// make sure the frame of the laser is updated during the whole scan time
|
||||
rtabmap::Transform tmpT = rtabmap_conversions::getMovingTransform(
|
||||
scanMsg->header.frame_id,
|
||||
guessFrameId().empty()?frameId():guessFrameId(),
|
||||
scanMsg->header.stamp,
|
||||
rclcpp::Time(scanMsg->header.stamp.sec, scanMsg->header.stamp.nanosec) + rclcpp::Duration::from_seconds(scanMsg->ranges.size()*scanMsg->time_increment),
|
||||
this->tfBuffer(),
|
||||
this->waitForTransform());
|
||||
if(tmpT.isNull())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
projection.transformLaserScanToPointCloud(
|
||||
guessFrameId().empty()?frameId():guessFrameId(),
|
||||
*scanMsg,
|
||||
scanOut,
|
||||
this->tfBuffer(),
|
||||
-1.0f,
|
||||
laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp);
|
||||
|
||||
if(guessFrameId().empty() && previousStamp() > 0 && !velocityGuess().isNull())
|
||||
{
|
||||
// deskew with constant velocity model (we are in frameId)
|
||||
sensor_msgs::msg::PointCloud2 scanOutDeskewed;
|
||||
if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess()))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!");
|
||||
return;
|
||||
}
|
||||
scanOut = scanOutDeskewed;
|
||||
}
|
||||
|
||||
rtabmap::Transform t = rtabmap_conversions::getTransform(scanMsg->header.frame_id, scanOut.header.frame_id, scanMsg->header.stamp, tfBuffer(), waitForTransform());
|
||||
if(t.isNull())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
|
||||
scanOut.header.frame_id.c_str(), scanMsg->header.frame_id.c_str(), rtabmap_conversions::timestampFromROS(scanMsg->header.stamp));
|
||||
return;
|
||||
}
|
||||
|
||||
sensor_msgs::msg::PointCloud2 scanOutDeskewed;
|
||||
rtabmap_conversions::transformPointCloud(t.toEigen4f(), scanOut, scanOutDeskewed);
|
||||
scanOutDeskewed.header.frame_id = scanMsg->header.frame_id;
|
||||
scanOut = scanOutDeskewed;
|
||||
}
|
||||
else
|
||||
{
|
||||
projection.projectLaser(*scanMsg, scanOut, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp);
|
||||
|
||||
if(deskewing_ && previousStamp() > 0 && !velocityGuess().isNull())
|
||||
{
|
||||
// deskew with constant velocity model
|
||||
sensor_msgs::msg::PointCloud2 scanOutDeskewed;
|
||||
if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess()))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!");
|
||||
return;
|
||||
}
|
||||
scanOut = scanOutDeskewed;
|
||||
}
|
||||
}
|
||||
|
||||
bool hasIntensity = false;
|
||||
for(unsigned int i=0; i<scanOut.fields.size(); ++i)
|
||||
{
|
||||
if(scanOut.fields[i].name.compare("intensity") == 0)
|
||||
{
|
||||
if(scanOut.fields[i].datatype == sensor_msgs::msg::PointField::FLOAT32)
|
||||
{
|
||||
hasIntensity = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
static bool warningShown = false;
|
||||
if(!warningShown)
|
||||
{
|
||||
RCLCPP_WARN(get_logger(), "The input scan cloud has an \"intensity\" field "
|
||||
"but the datatype (%d) is not supported. Intensity will be ignored. "
|
||||
"This message is only shown once.", scanOut.fields[i].datatype);
|
||||
warningShown = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZI>::Ptr pclScanI(new pcl::PointCloud<pcl::PointXYZI>);
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
|
||||
if(hasIntensity)
|
||||
{
|
||||
pcl::fromROSMsg(scanOut, *pclScanI);
|
||||
pclScanI->is_dense = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
pcl::fromROSMsg(scanOut, *pclScan);
|
||||
pclScan->is_dense = true;
|
||||
}
|
||||
|
||||
LaserScan scan;
|
||||
int maxLaserScans = (int)scanMsg->ranges.size();
|
||||
if(!pclScan->empty() || !pclScanI->empty())
|
||||
{
|
||||
if(scanDownsamplingStep_ > 1)
|
||||
{
|
||||
if(hasIntensity)
|
||||
{
|
||||
pclScanI = util3d::downsample(pclScanI, scanDownsamplingStep_);
|
||||
}
|
||||
else
|
||||
{
|
||||
pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
|
||||
}
|
||||
maxLaserScans /= scanDownsamplingStep_;
|
||||
}
|
||||
if(scanVoxelSize_ > 0.0f)
|
||||
{
|
||||
float pointsBeforeFiltering;
|
||||
float pointsAfterFiltering;
|
||||
if(hasIntensity)
|
||||
{
|
||||
pointsBeforeFiltering = (float)pclScanI->size();
|
||||
pclScanI = util3d::voxelize(pclScanI, scanVoxelSize_);
|
||||
pointsAfterFiltering = (float)pclScanI->size();
|
||||
}
|
||||
else
|
||||
{
|
||||
pointsBeforeFiltering = (float)pclScan->size();
|
||||
pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
|
||||
pointsAfterFiltering = (float)pclScan->size();
|
||||
}
|
||||
float ratio = pointsAfterFiltering / pointsBeforeFiltering;
|
||||
maxLaserScans = int(float(maxLaserScans) * ratio);
|
||||
}
|
||||
if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
|
||||
{
|
||||
//compute normals
|
||||
pcl::PointCloud<pcl::Normal>::Ptr normals;
|
||||
if(scanVoxelSize_ > 0.0f)
|
||||
{
|
||||
if(hasIntensity)
|
||||
{
|
||||
normals = util3d::computeNormals2D(pclScanI, scanNormalK_, scanNormalRadius_);
|
||||
}
|
||||
else
|
||||
{
|
||||
normals = util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(hasIntensity)
|
||||
{
|
||||
normals = util3d::computeFastOrganizedNormals2D(pclScanI, scanNormalK_, scanNormalRadius_);
|
||||
}
|
||||
else
|
||||
{
|
||||
normals = util3d::computeFastOrganizedNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
|
||||
}
|
||||
}
|
||||
pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanINormal;
|
||||
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal;
|
||||
if(hasIntensity)
|
||||
{
|
||||
pclScanINormal.reset(new pcl::PointCloud<pcl::PointXYZINormal>);
|
||||
pcl::concatenateFields(*pclScanI, *normals, *pclScanINormal);
|
||||
scan = util3d::laserScan2dFromPointCloud(*pclScanINormal);
|
||||
}
|
||||
else
|
||||
{
|
||||
pclScanNormal.reset(new pcl::PointCloud<pcl::PointNormal>);
|
||||
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
|
||||
scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(hasIntensity)
|
||||
{
|
||||
scan = util3d::laserScan2dFromPointCloud(*pclScanI);
|
||||
}
|
||||
else
|
||||
{
|
||||
scan = util3d::laserScan2dFromPointCloud(*pclScan);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(scanRangeMin_ > 0 || scanRangeMax_ > 0)
|
||||
{
|
||||
scan = util3d::rangeFiltering(scan, scanRangeMin_, scanRangeMax_);
|
||||
}
|
||||
|
||||
rtabmap::SensorData data(
|
||||
LaserScan(scan,
|
||||
maxLaserScans,
|
||||
scanRangeMax_>0&&scanRangeMax_<scanMsg->range_max?scanRangeMax_:scanMsg->range_max,
|
||||
localScanTransform),
|
||||
cv::Mat(),
|
||||
cv::Mat(),
|
||||
CameraModel(),
|
||||
0,
|
||||
rtabmap_conversions::timestampFromROS(scanMsg->header.stamp));
|
||||
|
||||
this->processData(data, scanMsg->header);
|
||||
}
|
||||
|
||||
void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr pointCloudMsg)
|
||||
{
|
||||
UASSERT_MSG(pointCloudMsg->data.size() == pointCloudMsg->row_step*pointCloudMsg->height,
|
||||
uFormat("data=%d row_step=%d height=%d", pointCloudMsg->data.size(), pointCloudMsg->row_step, pointCloudMsg->height).c_str());
|
||||
|
||||
if(scanReceived_)
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "%s is already receiving scans on \"%s\", but also "
|
||||
"just received a cloud on \"%s\". Both subscribers cannot be "
|
||||
"used at the same time! Disabling cloud subscriber.",
|
||||
this->get_name(), scan_sub_->get_topic_name(), cloud_sub_->get_topic_name());
|
||||
cloud_sub_.reset();
|
||||
return;
|
||||
}
|
||||
|
||||
tick(pointCloudMsg->header.stamp);
|
||||
|
||||
cloudReceived_ = true;
|
||||
if(this->isPaused())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
std::shared_ptr<sensor_msgs::msg::PointCloud2> cloudMsg(new sensor_msgs::msg::PointCloud2);
|
||||
*cloudMsg = *pointCloudMsg;
|
||||
|
||||
rtabmap::Transform localScanTransform = rtabmap_conversions::getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp, this->tfBuffer(), this->waitForTransform());
|
||||
if(localScanTransform.isNull())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "TF of received scan cloud at time %fs is not set, aborting rtabmap update.", rtabmap_conversions::timestampFromROS(cloudMsg->header.stamp));
|
||||
return;
|
||||
}
|
||||
|
||||
if(deskewing_)
|
||||
{
|
||||
if(!guessFrameId().empty())
|
||||
{
|
||||
// deskew with TF
|
||||
if(!rtabmap_conversions::deskew(*pointCloudMsg, *cloudMsg, guessFrameId(), tfBuffer(), waitForTransform(), deskewingSlerp_))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!");
|
||||
return;
|
||||
}
|
||||
}
|
||||
else if(previousStamp() > 0 && !velocityGuess().isNull())
|
||||
{
|
||||
// deskew with constant velocity model
|
||||
bool alreadyInBaseFrame = frameId().compare(pointCloudMsg->header.frame_id) == 0;
|
||||
std::shared_ptr<sensor_msgs::msg::PointCloud2> cloudInBaseFrame;
|
||||
std::shared_ptr<sensor_msgs::msg::PointCloud2> cloudPtr = cloudMsg;
|
||||
if(!alreadyInBaseFrame)
|
||||
{
|
||||
// transform in base frame
|
||||
rtabmap::Transform t = rtabmap_conversions::getTransform(frameId(), pointCloudMsg->header.frame_id, pointCloudMsg->header.stamp, tfBuffer(), waitForTransform());
|
||||
if(t.isNull())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Cannot transform cloud from \"%s\" frame to \"%s\" frame at time %fs.",
|
||||
pointCloudMsg->header.frame_id.c_str(), frameId().c_str(), rtabmap_conversions::timestampFromROS(pointCloudMsg->header.stamp));
|
||||
return;
|
||||
}
|
||||
|
||||
cloudInBaseFrame.reset(new sensor_msgs::msg::PointCloud2);
|
||||
rtabmap_conversions::transformPointCloud(t.toEigen4f(), *pointCloudMsg, *cloudInBaseFrame);
|
||||
cloudPtr = cloudInBaseFrame;
|
||||
}
|
||||
|
||||
std::shared_ptr<sensor_msgs::msg::PointCloud2> cloudDeskewed(new sensor_msgs::msg::PointCloud2);
|
||||
if(!rtabmap_conversions::deskew(*cloudPtr, *cloudDeskewed, previousStamp(), velocityGuess()))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!");
|
||||
return;
|
||||
}
|
||||
|
||||
if(!alreadyInBaseFrame)
|
||||
{
|
||||
// put back in scan frame
|
||||
rtabmap::Transform t = rtabmap_conversions::getTransform(pointCloudMsg->header.frame_id, frameId(), pointCloudMsg->header.stamp, tfBuffer(), waitForTransform());
|
||||
if(t.isNull())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Cannot transform cloud from \"%s\" frame to \"%s\" frame at time %fs.",
|
||||
frameId().c_str(), pointCloudMsg->header.frame_id.c_str(), rtabmap_conversions::timestampFromROS(pointCloudMsg->header.stamp));
|
||||
return;
|
||||
}
|
||||
rtabmap_conversions::transformPointCloud(t.toEigen4f(), *cloudDeskewed, *cloudMsg);
|
||||
}
|
||||
else
|
||||
{
|
||||
cloudMsg = cloudDeskewed;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
LaserScan scan;
|
||||
bool hasNormals = false;
|
||||
bool hasIntensity = false;
|
||||
bool is3D = false;
|
||||
for(unsigned int i=0; i<cloudMsg->fields.size(); ++i)
|
||||
{
|
||||
if(scanVoxelSize_ == 0.0f && cloudMsg->fields[i].name.compare("normal_x") == 0)
|
||||
{
|
||||
hasNormals = true;
|
||||
}
|
||||
if(cloudMsg->fields[i].name.compare("z") == 0 && !scanCloudIs2d_)
|
||||
{
|
||||
is3D = true;
|
||||
}
|
||||
if(cloudMsg->fields[i].name.compare("intensity") == 0)
|
||||
{
|
||||
if(cloudMsg->fields[i].datatype == sensor_msgs::msg::PointField::FLOAT32)
|
||||
{
|
||||
hasIntensity = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
static bool warningShown = false;
|
||||
if(!warningShown)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "The input scan cloud has an \"intensity\" field "
|
||||
"but the datatype (%d) is not supported. Intensity will be ignored. "
|
||||
"This message is only shown once.", cloudMsg->fields[i].datatype);
|
||||
warningShown = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(cloudMsg->height > 1) // organized cloud
|
||||
{
|
||||
if(scanCloudMaxPoints_ == -1)
|
||||
{
|
||||
scanCloudMaxPoints_ = cloudMsg->height * cloudMsg->width;
|
||||
RCLCPP_WARN(this->get_logger(), "IcpOdometry: \"scan_cloud_max_points\" is not set but input "
|
||||
"cloud is not dense, for convenience it will be set to %d (%dx%d)",
|
||||
scanCloudMaxPoints_, cloudMsg->width, cloudMsg->height);
|
||||
}
|
||||
else if(scanCloudMaxPoints_ > 0 && scanCloudMaxPoints_ < int(cloudMsg->height * cloudMsg->width))
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "IcpOdometry: \"scan_cloud_max_points\" is set to %d but input "
|
||||
"cloud is not dense and has a size of %d (%dx%d), setting to this later size.",
|
||||
scanCloudMaxPoints_, cloudMsg->width *cloudMsg->height, cloudMsg->width, cloudMsg->height);
|
||||
scanCloudMaxPoints_ = cloudMsg->width *cloudMsg->height;
|
||||
}
|
||||
}
|
||||
if(scanCloudMaxPoints_ == -1)
|
||||
{
|
||||
scanCloudMaxPoints_ = 0;
|
||||
}
|
||||
int maxLaserScans = scanCloudMaxPoints_;
|
||||
|
||||
if(hasNormals && hasIntensity)
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZINormal>);
|
||||
pcl::fromROSMsg(*cloudMsg, *pclScan);
|
||||
if(pclScan->size() && scanDownsamplingStep_ > 1)
|
||||
{
|
||||
pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
|
||||
if(pclScan->height>1)
|
||||
{
|
||||
maxLaserScans = pclScan->height * pclScan->width;
|
||||
}
|
||||
else
|
||||
{
|
||||
maxLaserScans /= scanDownsamplingStep_;
|
||||
}
|
||||
}
|
||||
scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan);
|
||||
}
|
||||
else if(hasNormals)
|
||||
{
|
||||
pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
|
||||
pcl::fromROSMsg(*cloudMsg, *pclScan);
|
||||
if(pclScan->size() && scanDownsamplingStep_ > 1)
|
||||
{
|
||||
pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
|
||||
if(pclScan->height>1)
|
||||
{
|
||||
maxLaserScans = pclScan->height * pclScan->width;
|
||||
}
|
||||
else
|
||||
{
|
||||
maxLaserScans /= scanDownsamplingStep_;
|
||||
}
|
||||
}
|
||||
scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan);
|
||||
}
|
||||
else if(hasIntensity)
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
|
||||
pcl::fromROSMsg(*cloudMsg, *pclScan);
|
||||
if(pclScan->size() && scanDownsamplingStep_ > 1)
|
||||
{
|
||||
pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
|
||||
if(pclScan->height>1)
|
||||
{
|
||||
maxLaserScans = pclScan->height * pclScan->width;
|
||||
}
|
||||
else
|
||||
{
|
||||
maxLaserScans /= scanDownsamplingStep_;
|
||||
}
|
||||
}
|
||||
if(!pclScan->is_dense)
|
||||
{
|
||||
pclScan = util3d::removeNaNFromPointCloud(pclScan);
|
||||
}
|
||||
|
||||
if(pclScan->size())
|
||||
{
|
||||
if(scanVoxelSize_ > 0.0f)
|
||||
{
|
||||
float pointsBeforeFiltering = (float)pclScan->size();
|
||||
pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
|
||||
float ratio = float(pclScan->size()) / pointsBeforeFiltering;
|
||||
maxLaserScans = int(float(maxLaserScans) * ratio);
|
||||
}
|
||||
if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
|
||||
{
|
||||
//compute normals
|
||||
pcl::PointCloud<pcl::Normal>::Ptr normals = is3D?
|
||||
util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_):
|
||||
util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
|
||||
pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointXYZINormal>);
|
||||
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
|
||||
scan = is3D?util3d::laserScanFromPointCloud(*pclScanNormal):util3d::laserScan2dFromPointCloud(*pclScanNormal);
|
||||
}
|
||||
else
|
||||
{
|
||||
scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
pcl::fromROSMsg(*cloudMsg, *pclScan);
|
||||
if(pclScan->size() && scanDownsamplingStep_ > 1)
|
||||
{
|
||||
pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
|
||||
if(pclScan->height>1)
|
||||
{
|
||||
maxLaserScans = pclScan->height * pclScan->width;
|
||||
}
|
||||
else
|
||||
{
|
||||
maxLaserScans /= scanDownsamplingStep_;
|
||||
}
|
||||
|
||||
}
|
||||
if(!pclScan->is_dense)
|
||||
{
|
||||
pclScan = util3d::removeNaNFromPointCloud(pclScan);
|
||||
}
|
||||
|
||||
if(pclScan->size())
|
||||
{
|
||||
if(scanVoxelSize_ > 0.0f)
|
||||
{
|
||||
float pointsBeforeFiltering = (float)pclScan->size();
|
||||
pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
|
||||
float ratio = float(pclScan->size()) / pointsBeforeFiltering;
|
||||
maxLaserScans = int(float(maxLaserScans) * ratio);
|
||||
}
|
||||
if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
|
||||
{
|
||||
//compute normals
|
||||
pcl::PointCloud<pcl::Normal>::Ptr normals = is3D?
|
||||
util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_):
|
||||
util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
|
||||
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
|
||||
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
|
||||
scan = is3D?util3d::laserScanFromPointCloud(*pclScanNormal):util3d::laserScan2dFromPointCloud(*pclScanNormal);
|
||||
}
|
||||
else
|
||||
{
|
||||
scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
LaserScan laserScan(scan,
|
||||
maxLaserScans,
|
||||
0,
|
||||
localScanTransform);
|
||||
if(scanRangeMin_ > 0 || scanRangeMax_ > 0)
|
||||
{
|
||||
laserScan = util3d::rangeFiltering(laserScan, scanRangeMin_, scanRangeMax_);
|
||||
}
|
||||
if(!laserScan.isEmpty() && laserScan.hasNormals() && !laserScan.is2d() && scanNormalGroundUp_)
|
||||
{
|
||||
laserScan = util3d::adjustNormalsToViewPoint(laserScan, Eigen::Vector3f(0,0,10), (float)scanNormalGroundUp_);
|
||||
}
|
||||
|
||||
rtabmap::SensorData data(
|
||||
laserScan,
|
||||
cv::Mat(),
|
||||
cv::Mat(),
|
||||
CameraModel(),
|
||||
0,
|
||||
rtabmap_conversions::timestampFromROS(cloudMsg->header.stamp));
|
||||
|
||||
this->processData(data, cloudMsg->header);
|
||||
}
|
||||
|
||||
void ICPOdometry::flushCallbacks()
|
||||
{
|
||||
// flush callbacks
|
||||
}
|
||||
|
||||
void ICPOdometry::postProcessData(const SensorData & data, const std_msgs::msg::Header & header) const
|
||||
{
|
||||
if(filtered_scan_pub_->get_subscription_count())
|
||||
{
|
||||
sensor_msgs::msg::PointCloud2::UniquePtr msg(new sensor_msgs::msg::PointCloud2);
|
||||
pcl_conversions::fromPCL(*rtabmap::util3d::laserScanToPointCloud2(data.laserScanRaw()), *msg);
|
||||
msg->header = header;
|
||||
filtered_scan_pub_->publish(std::move(msg));
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
|
||||
// Register the component with class_loader.
|
||||
// This acts as a sort of entry point, allowing the component to be discoverable when its library
|
||||
// is being loaded into a running process.
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_odom::ICPOdometry)
|
||||
@@ -0,0 +1,918 @@
|
||||
/*
|
||||
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_odom/rgbd_odometry.hpp>
|
||||
|
||||
#ifdef PRE_ROS_IRON
|
||||
#include <image_geometry/stereo_camera_model.h>
|
||||
#else
|
||||
#include <image_geometry/stereo_camera_model.hpp>
|
||||
#endif
|
||||
|
||||
#include <sensor_msgs/image_encodings.hpp>
|
||||
|
||||
#include "rtabmap_conversions/MsgConversion.h"
|
||||
#include <rtabmap_msgs/msg/rgbd_images.hpp>
|
||||
|
||||
#include <rtabmap/core/util3d.h>
|
||||
#include <rtabmap/core/util2d.h>
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <rtabmap/utilite/UStl.h>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
namespace rtabmap_odom
|
||||
{
|
||||
|
||||
RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) :
|
||||
OdometryROS("rgbd_odometry", options),
|
||||
approxSync_(0),
|
||||
exactSync_(0),
|
||||
approxSync2_(0),
|
||||
exactSync2_(0),
|
||||
approxSync3_(0),
|
||||
exactSync3_(0),
|
||||
approxSync4_(0),
|
||||
exactSync4_(0),
|
||||
approxSync5_(0),
|
||||
exactSync5_(0),
|
||||
approxSync6_(0),
|
||||
exactSync6_(0),
|
||||
topicQueueSize_(10),
|
||||
syncQueueSize_(5),
|
||||
keepColor_(false)
|
||||
{
|
||||
OdometryROS::init(false, true, false);
|
||||
}
|
||||
|
||||
RGBDOdometry::~RGBDOdometry()
|
||||
{
|
||||
delete approxSync_;
|
||||
delete exactSync_;
|
||||
delete approxSync2_;
|
||||
delete exactSync2_;
|
||||
delete approxSync3_;
|
||||
delete exactSync3_;
|
||||
delete approxSync4_;
|
||||
delete exactSync4_;
|
||||
delete approxSync5_;
|
||||
delete exactSync5_;
|
||||
delete approxSync6_;
|
||||
delete exactSync6_;
|
||||
}
|
||||
|
||||
void RGBDOdometry::onOdomInit()
|
||||
{
|
||||
int rgbdCameras = 1;
|
||||
bool approxSync = true;
|
||||
bool subscribeRGBD = false;
|
||||
double approxSyncMaxInterval = 0.0;
|
||||
approxSync = this->declare_parameter("approx_sync", approxSync);
|
||||
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
|
||||
topicQueueSize_ = this->declare_parameter("topic_queue_size", topicQueueSize_);
|
||||
int queueSize = this->declare_parameter("queue_size", -1);
|
||||
if(queueSize != -1)
|
||||
{
|
||||
syncQueueSize_ = queueSize;
|
||||
RCLCPP_WARN(this->get_logger(), "Parameter \"queue_size\" has been renamed "
|
||||
"to \"sync_queue_size\" and will be removed "
|
||||
"in future versions! The value (%d) is copied to "
|
||||
"\"sync_queue_size\".", syncQueueSize_);
|
||||
}
|
||||
syncQueueSize_ = this->declare_parameter("sync_queue_size", syncQueueSize_);
|
||||
int qosCamInfo = this->declare_parameter("qos_camera_info", (int)qos());
|
||||
subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD);
|
||||
rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras);
|
||||
if(rgbdCameras < 0)
|
||||
{
|
||||
rgbdCameras = 0;
|
||||
}
|
||||
keepColor_ = this->declare_parameter("keep_color", keepColor_);
|
||||
std::string rgbdTransport = this->declare_parameter("rgb_transport", std::string("raw"));
|
||||
std::string depthTransport = this->declare_parameter("depth_transport", std::string("raw"));
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync = %s", approxSync?"true":"false");
|
||||
if(approxSync)
|
||||
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
|
||||
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: topic_queue_size = %d", topicQueueSize_);
|
||||
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: sync_queue_size = %d", syncQueueSize_);
|
||||
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos = %d", (int)qos());
|
||||
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos_camera_info = %d", qosCamInfo);
|
||||
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
|
||||
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: rgbd_cameras = %d", rgbdCameras);
|
||||
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: keep_color = %s", keepColor_?"true":"false");
|
||||
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: rgb_transport = %s", rgbdTransport.c_str());
|
||||
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: depth_transport = %s", depthTransport.c_str());
|
||||
|
||||
rclcpp::SubscriptionOptions options;
|
||||
options.callback_group = dataCallbackGroup_;
|
||||
|
||||
std::string subscribedTopic;
|
||||
std::string subscribedTopicsMsg;
|
||||
if(subscribeRGBD)
|
||||
{
|
||||
if(rgbdCameras >= 2)
|
||||
{
|
||||
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
if(rgbdCameras >= 3)
|
||||
{
|
||||
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
}
|
||||
if(rgbdCameras >= 4)
|
||||
{
|
||||
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
}
|
||||
if(rgbdCameras >= 5)
|
||||
{
|
||||
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
}
|
||||
if(rgbdCameras >= 6)
|
||||
{
|
||||
rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
}
|
||||
|
||||
if(rgbdCameras == 2)
|
||||
{
|
||||
if(approxSync)
|
||||
{
|
||||
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
|
||||
MyApproxSync2Policy(syncQueueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_);
|
||||
if(approxSyncMaxInterval > 0.0)
|
||||
approxSync2_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
approxSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
|
||||
}
|
||||
else
|
||||
{
|
||||
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
|
||||
MyExactSync2Policy(syncQueueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_);
|
||||
exactSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
|
||||
}
|
||||
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s",
|
||||
get_name(),
|
||||
approxSync?"approx":"exact",
|
||||
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
|
||||
rgbd_image1_sub_.getSubscriber()->get_topic_name(),
|
||||
rgbd_image2_sub_.getSubscriber()->get_topic_name());
|
||||
}
|
||||
else if(rgbdCameras == 3)
|
||||
{
|
||||
if(approxSync)
|
||||
{
|
||||
approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
|
||||
MyApproxSync3Policy(syncQueueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_,
|
||||
rgbd_image3_sub_);
|
||||
if(approxSyncMaxInterval > 0.0)
|
||||
approxSync3_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
approxSync3_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
}
|
||||
else
|
||||
{
|
||||
exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
|
||||
MyExactSync3Policy(syncQueueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_,
|
||||
rgbd_image3_sub_);
|
||||
exactSync3_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
}
|
||||
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s",
|
||||
get_name(),
|
||||
approxSync?"approx":"exact",
|
||||
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
|
||||
rgbd_image1_sub_.getSubscriber()->get_topic_name(),
|
||||
rgbd_image2_sub_.getSubscriber()->get_topic_name(),
|
||||
rgbd_image3_sub_.getSubscriber()->get_topic_name());
|
||||
}
|
||||
else if(rgbdCameras == 4)
|
||||
{
|
||||
if(approxSync)
|
||||
{
|
||||
approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
|
||||
MyApproxSync4Policy(syncQueueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_,
|
||||
rgbd_image3_sub_,
|
||||
rgbd_image4_sub_);
|
||||
if(approxSyncMaxInterval > 0.0)
|
||||
approxSync4_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
approxSync4_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
|
||||
}
|
||||
else
|
||||
{
|
||||
exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
|
||||
MyExactSync4Policy(syncQueueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_,
|
||||
rgbd_image3_sub_,
|
||||
rgbd_image4_sub_);
|
||||
exactSync4_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
|
||||
}
|
||||
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s,\n %s",
|
||||
get_name(),
|
||||
approxSync?"approx":"exact",
|
||||
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
|
||||
rgbd_image1_sub_.getSubscriber()->get_topic_name(),
|
||||
rgbd_image2_sub_.getSubscriber()->get_topic_name(),
|
||||
rgbd_image3_sub_.getSubscriber()->get_topic_name(),
|
||||
rgbd_image4_sub_.getSubscriber()->get_topic_name());
|
||||
}
|
||||
else if(rgbdCameras == 5)
|
||||
{
|
||||
if(approxSync)
|
||||
{
|
||||
approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
|
||||
MyApproxSync5Policy(syncQueueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_,
|
||||
rgbd_image3_sub_,
|
||||
rgbd_image4_sub_,
|
||||
rgbd_image5_sub_);
|
||||
if(approxSyncMaxInterval > 0.0)
|
||||
approxSync5_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
approxSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
|
||||
}
|
||||
else
|
||||
{
|
||||
exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
|
||||
MyExactSync5Policy(syncQueueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_,
|
||||
rgbd_image3_sub_,
|
||||
rgbd_image4_sub_,
|
||||
rgbd_image5_sub_);
|
||||
exactSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
|
||||
}
|
||||
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s",
|
||||
get_name(),
|
||||
approxSync?"approx":"exact",
|
||||
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
|
||||
rgbd_image1_sub_.getSubscriber()->get_topic_name(),
|
||||
rgbd_image2_sub_.getSubscriber()->get_topic_name(),
|
||||
rgbd_image3_sub_.getSubscriber()->get_topic_name(),
|
||||
rgbd_image4_sub_.getSubscriber()->get_topic_name(),
|
||||
rgbd_image5_sub_.getSubscriber()->get_topic_name());
|
||||
}
|
||||
else if(rgbdCameras == 6)
|
||||
{
|
||||
if(approxSync)
|
||||
{
|
||||
approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
|
||||
MyApproxSync6Policy(syncQueueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_,
|
||||
rgbd_image3_sub_,
|
||||
rgbd_image4_sub_,
|
||||
rgbd_image5_sub_,
|
||||
rgbd_image6_sub_);
|
||||
if(approxSyncMaxInterval > 0.0)
|
||||
approxSync6_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
approxSync6_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
|
||||
}
|
||||
else
|
||||
{
|
||||
exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
|
||||
MyExactSync6Policy(syncQueueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_,
|
||||
rgbd_image3_sub_,
|
||||
rgbd_image4_sub_,
|
||||
rgbd_image5_sub_,
|
||||
rgbd_image6_sub_);
|
||||
exactSync6_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
|
||||
}
|
||||
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s",
|
||||
get_name(),
|
||||
approxSync?"approx":"exact",
|
||||
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
|
||||
rgbd_image1_sub_.getTopic().c_str(),
|
||||
rgbd_image2_sub_.getTopic().c_str(),
|
||||
rgbd_image3_sub_.getTopic().c_str(),
|
||||
rgbd_image4_sub_.getTopic().c_str(),
|
||||
rgbd_image5_sub_.getTopic().c_str(),
|
||||
rgbd_image6_sub_.getTopic().c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_FATAL(this->get_logger(),
|
||||
"%s doesn't support more than 6 cameras (rgbd_cameras=%d) with "
|
||||
"internal synchronization interface, set rgbd_cameras=0 and use "
|
||||
"rgbd_images input topic instead for more cameras (for which "
|
||||
"rgbdx_sync node can sync up to 8 cameras).",
|
||||
get_name(), rgbdCameras);
|
||||
}
|
||||
}
|
||||
else if(rgbdCameras == 0)
|
||||
{
|
||||
rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBDX, this, std::placeholders::_1), options);
|
||||
|
||||
subscribedTopic = rgbdxSub_->get_topic_name();
|
||||
subscribedTopicsMsg = uFormat("\n%s subscribed to:\n %s",
|
||||
get_name(),
|
||||
rgbdxSub_->get_topic_name());
|
||||
}
|
||||
else
|
||||
{
|
||||
rgbdSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1), options);
|
||||
|
||||
subscribedTopic = rgbdSub_->get_topic_name();
|
||||
subscribedTopicsMsg =
|
||||
uFormat("\n%s subscribed to:\n %s",
|
||||
get_name(),
|
||||
rgbdSub_->get_topic_name());
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
image_transport::TransportHints rgb_hints(this, "raw", "rgb_transport");
|
||||
image_transport::TransportHints depth_hints(this, "raw", "depth_transport");
|
||||
|
||||
std::string rgb_topic = get_node_base_interface()->resolve_topic_or_service_name(
|
||||
"rgb/image", false, false
|
||||
);
|
||||
std::string depth_topic = get_node_base_interface()->resolve_topic_or_service_name(
|
||||
"depth/image", false, false
|
||||
);
|
||||
|
||||
image_mono_sub_.subscribe(this, rgb_topic, rgb_hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
image_depth_sub_.subscribe(this, depth_topic, depth_hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
info_sub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
|
||||
|
||||
if(approxSync)
|
||||
{
|
||||
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
|
||||
if(approxSyncMaxInterval > 0.0)
|
||||
approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
approxSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
}
|
||||
else
|
||||
{
|
||||
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
|
||||
exactSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
}
|
||||
|
||||
subscribedTopic = image_mono_sub_.getSubscriber().getTopic();
|
||||
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s, topic_queue_size=%d, sync_queue_size=%d):\n %s,\n %s,\n %s",
|
||||
get_name(),
|
||||
approxSync?"approx":"exact",
|
||||
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
|
||||
topicQueueSize_,
|
||||
syncQueueSize_,
|
||||
image_mono_sub_.getSubscriber().getTopic().c_str(),
|
||||
image_depth_sub_.getSubscriber().getTopic().c_str(),
|
||||
info_sub_.getSubscriber()->get_topic_name());
|
||||
}
|
||||
initDiagnosticMsg(subscribedTopicsMsg, approxSync, subscribedTopic);
|
||||
}
|
||||
|
||||
void RGBDOdometry::updateParameters(ParametersMap & parameters)
|
||||
{
|
||||
//make sure we are using Reg/Strategy=0
|
||||
ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
|
||||
if(iter != parameters.end() && iter->second.compare("0") != 0)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "RGBD odometry works only with \"Reg/Strategy\"=0. Ignoring value %s.", iter->second.c_str());
|
||||
}
|
||||
uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0"));
|
||||
|
||||
int estimationType = Parameters::defaultVisEstimationType();
|
||||
Parameters::parse(parameters, Parameters::kVisEstimationType(), estimationType);
|
||||
int rgbdCameras = 1;
|
||||
bool subscribeRGBD = false;
|
||||
this->get_parameter("subscribe_rgbd", subscribeRGBD);
|
||||
this->get_parameter("rgbd_cameras", rgbdCameras);
|
||||
if(subscribeRGBD && rgbdCameras> 1 && estimationType>0)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "Setting \"%s\" parameter to 0 (%d is not supported "
|
||||
"for multi-cameras) as \"subscribe_rgbd\" is "
|
||||
"true and \"rgbd_cameras\">1. Set \"%s\" to 0 to suppress this warning.",
|
||||
Parameters::kVisEstimationType().c_str(),
|
||||
estimationType,
|
||||
Parameters::kVisEstimationType().c_str());
|
||||
uInsert(parameters, ParametersPair(Parameters::kVisEstimationType(), "0"));
|
||||
}
|
||||
}
|
||||
|
||||
void RGBDOdometry::commonCallback(
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & rgbImages,
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & depthImages,
|
||||
const std::vector<sensor_msgs::msg::CameraInfo>& cameraInfos)
|
||||
{
|
||||
UASSERT(rgbImages.size() > 0 && rgbImages.size() == depthImages.size() && rgbImages.size() == cameraInfos.size());
|
||||
rclcpp::Time higherStamp;
|
||||
int imageWidth = rgbImages[0]->image.cols;
|
||||
int imageHeight = rgbImages[0]->image.rows;
|
||||
int depthWidth = depthImages[0]->image.cols;
|
||||
int depthHeight = depthImages[0]->image.rows;
|
||||
|
||||
UASSERT_MSG(
|
||||
imageWidth/depthWidth == imageHeight/depthHeight,
|
||||
uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
|
||||
|
||||
int cameraCount = rgbImages.size();
|
||||
cv::Mat rgb;
|
||||
cv::Mat depth;
|
||||
std::vector<rtabmap::CameraModel> cameraModels;
|
||||
for(unsigned int i=0; i<rgbImages.size(); ++i)
|
||||
{
|
||||
if(!(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
|
||||
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
|
||||
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
|
||||
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
|
||||
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
|
||||
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
|
||||
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
|
||||
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
|
||||
!(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
|
||||
depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
|
||||
depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8 and "
|
||||
"image_depth=32FC1,16UC1,mono16. Current rgb=%s and depth=%s",
|
||||
rgbImages[i]->encoding.c_str(),
|
||||
depthImages[i]->encoding.c_str());
|
||||
return;
|
||||
}
|
||||
UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight,
|
||||
uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
|
||||
imageWidth,
|
||||
rgbImages[i]->image.cols,
|
||||
imageHeight,
|
||||
rgbImages[i]->image.rows).c_str());
|
||||
UASSERT_MSG(depthImages[i]->image.cols == depthWidth && depthImages[i]->image.rows == depthHeight,
|
||||
uFormat("depthWidth=%d vs %d depthHeight=%d vs %d",
|
||||
depthWidth,
|
||||
depthImages[i]->image.cols,
|
||||
depthHeight,
|
||||
depthImages[i]->image.rows).c_str());
|
||||
|
||||
rclcpp::Time stamp = rtabmap_conversions::timestampFromROS(rgbImages[i]->header.stamp)>rtabmap_conversions::timestampFromROS(depthImages[i]->header.stamp)?rgbImages[i]->header.stamp:depthImages[i]->header.stamp;
|
||||
|
||||
if(i == 0)
|
||||
{
|
||||
higherStamp = stamp;
|
||||
}
|
||||
else if(stamp > higherStamp)
|
||||
{
|
||||
higherStamp = stamp;
|
||||
}
|
||||
|
||||
Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), rgbImages[i]->header.frame_id, stamp, tfBuffer(), waitForTransform());
|
||||
if(localTransform.isNull())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if(i>0)
|
||||
{
|
||||
double stampDiff = fabs(rtabmap_conversions::timestampFromROS(rgbImages[i]->header.stamp) - rtabmap_conversions::timestampFromROS(rgbImages[i-1]->header.stamp));
|
||||
if(stampDiff > 1.0/60.0)
|
||||
{
|
||||
static bool warningShown = false;
|
||||
if(!warningShown)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "The time difference between cameras %d and %d is "
|
||||
"high (diff=%fs, cam%d=%fs, cam%d=%fs). You may want "
|
||||
"to set approx_sync_max_interval to reject bad synchronizations or use "
|
||||
"approx_sync=false if streams have all the exact same timestamp. This "
|
||||
"message is only printed once.",
|
||||
i-1, i,
|
||||
stampDiff,
|
||||
i-1, rtabmap_conversions::timestampFromROS(rgbImages[i-1]->header.stamp),
|
||||
i, rtabmap_conversions::timestampFromROS(rgbImages[i]->header.stamp));
|
||||
warningShown = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
cv_bridge::CvImageConstPtr ptrImage = rgbImages[i];
|
||||
if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
|
||||
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
|
||||
{
|
||||
if(keepColor_ && rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
|
||||
{
|
||||
ptrImage = cv_bridge::cvtColor(rgbImages[i], "bgr8");
|
||||
}
|
||||
else
|
||||
{
|
||||
ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8");
|
||||
}
|
||||
}
|
||||
|
||||
cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
|
||||
|
||||
// initialize
|
||||
if(rgb.empty())
|
||||
{
|
||||
rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
|
||||
}
|
||||
if(depth.empty())
|
||||
{
|
||||
depth = cv::Mat(depthHeight, depthWidth*cameraCount, ptrDepth->image.type());
|
||||
}
|
||||
|
||||
if(ptrImage->image.type() == rgb.type())
|
||||
{
|
||||
ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Some RGB images are not the same type! %d vs %d", ptrImage->image.type(), rgb.type());
|
||||
return;
|
||||
}
|
||||
|
||||
if(ptrDepth->image.type() == depth.type())
|
||||
{
|
||||
ptrDepth->image.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Some Depth images are not the same type! %d vs %d", ptrDepth->image.type(), depth.type());
|
||||
return;
|
||||
}
|
||||
|
||||
cameraModels.push_back(rtabmap_conversions::cameraModelFromROS(cameraInfos[i], localTransform));
|
||||
}
|
||||
|
||||
rtabmap::SensorData data(
|
||||
rgb,
|
||||
depth,
|
||||
cameraModels,
|
||||
0,
|
||||
rtabmap_conversions::timestampFromROS(higherStamp));
|
||||
|
||||
std_msgs::msg::Header header;
|
||||
header.stamp = higherStamp;
|
||||
header.frame_id = rgbImages.size()==1?rgbImages[0]->header.frame_id:"";
|
||||
this->processData(data, header);
|
||||
}
|
||||
|
||||
void RGBDOdometry::callback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr image,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depth,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
|
||||
{
|
||||
tick(image->header.stamp);
|
||||
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
|
||||
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(1);
|
||||
std::vector<sensor_msgs::msg::CameraInfo> infoMsgs;
|
||||
imageMsgs[0] = cv_bridge::toCvShare(image);
|
||||
depthMsgs[0] = cv_bridge::toCvShare(depth);
|
||||
infoMsgs.push_back(*cameraInfo);
|
||||
|
||||
double stampDiff = fabs(rtabmap_conversions::timestampFromROS(image->header.stamp) - rtabmap_conversions::timestampFromROS(depth->header.stamp));
|
||||
if(stampDiff > 0.020)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "The time difference between rgb and depth frames is "
|
||||
"high (diff=%fs, rgb=%fs, depth=%fs). You may want "
|
||||
"to set approx_sync_max_interval lower than 0.02s to reject spurious bad synchronizations or use "
|
||||
"approx_sync=false if streams have all the exact same timestamp.",
|
||||
stampDiff,
|
||||
rtabmap_conversions::timestampFromROS(image->header.stamp),
|
||||
rtabmap_conversions::timestampFromROS(depth->header.stamp));
|
||||
}
|
||||
|
||||
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
|
||||
}
|
||||
}
|
||||
|
||||
void RGBDOdometry::callbackRGBDX(
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images)
|
||||
{
|
||||
tick(images->header.stamp);
|
||||
|
||||
if(!this->isPaused())
|
||||
{
|
||||
if(images->rgbd_images.empty())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Input topic \"%s\" doesn't contain any image(s)!", rgbdxSub_->get_topic_name());
|
||||
return;
|
||||
}
|
||||
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(images->rgbd_images.size());
|
||||
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(images->rgbd_images.size());
|
||||
std::vector<sensor_msgs::msg::CameraInfo> infoMsgs;
|
||||
for(size_t i=0; i<images->rgbd_images.size(); ++i)
|
||||
{
|
||||
rtabmap_conversions::toCvShare(images->rgbd_images[i], images, imageMsgs[i], depthMsgs[i]);
|
||||
infoMsgs.push_back(images->rgbd_images[i].rgb_camera_info);
|
||||
}
|
||||
|
||||
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
|
||||
}
|
||||
}
|
||||
|
||||
void RGBDOdometry::callbackRGBD(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image)
|
||||
{
|
||||
tick(image->header.stamp);
|
||||
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
|
||||
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(1);
|
||||
std::vector<sensor_msgs::msg::CameraInfo> infoMsgs;
|
||||
rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
|
||||
infoMsgs.push_back(image->rgb_camera_info);
|
||||
|
||||
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
|
||||
}
|
||||
}
|
||||
|
||||
void RGBDOdometry::callbackRGBD2(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2)
|
||||
{
|
||||
tick(image->header.stamp);
|
||||
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2);
|
||||
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(2);
|
||||
std::vector<sensor_msgs::msg::CameraInfo> infoMsgs;
|
||||
rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
|
||||
rtabmap_conversions::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
|
||||
infoMsgs.push_back(image->rgb_camera_info);
|
||||
infoMsgs.push_back(image2->rgb_camera_info);
|
||||
|
||||
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
|
||||
}
|
||||
}
|
||||
|
||||
void RGBDOdometry::callbackRGBD3(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3)
|
||||
{
|
||||
tick(image->header.stamp);
|
||||
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(3);
|
||||
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(3);
|
||||
std::vector<sensor_msgs::msg::CameraInfo> infoMsgs;
|
||||
rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
|
||||
rtabmap_conversions::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
|
||||
rtabmap_conversions::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
|
||||
infoMsgs.push_back(image->rgb_camera_info);
|
||||
infoMsgs.push_back(image2->rgb_camera_info);
|
||||
infoMsgs.push_back(image3->rgb_camera_info);
|
||||
|
||||
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
|
||||
}
|
||||
}
|
||||
|
||||
void RGBDOdometry::callbackRGBD4(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4)
|
||||
{
|
||||
tick(image->header.stamp);
|
||||
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(4);
|
||||
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(4);
|
||||
std::vector<sensor_msgs::msg::CameraInfo> infoMsgs;
|
||||
rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
|
||||
rtabmap_conversions::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
|
||||
rtabmap_conversions::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
|
||||
rtabmap_conversions::toCvShare(image4, imageMsgs[3], depthMsgs[3]);
|
||||
infoMsgs.push_back(image->rgb_camera_info);
|
||||
infoMsgs.push_back(image2->rgb_camera_info);
|
||||
infoMsgs.push_back(image3->rgb_camera_info);
|
||||
infoMsgs.push_back(image4->rgb_camera_info);
|
||||
|
||||
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
|
||||
}
|
||||
}
|
||||
|
||||
void RGBDOdometry::callbackRGBD5(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5)
|
||||
{
|
||||
tick(image->header.stamp);
|
||||
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(5);
|
||||
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(5);
|
||||
std::vector<sensor_msgs::msg::CameraInfo> infoMsgs;
|
||||
rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
|
||||
rtabmap_conversions::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
|
||||
rtabmap_conversions::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
|
||||
rtabmap_conversions::toCvShare(image4, imageMsgs[3], depthMsgs[3]);
|
||||
rtabmap_conversions::toCvShare(image5, imageMsgs[4], depthMsgs[4]);
|
||||
infoMsgs.push_back(image->rgb_camera_info);
|
||||
infoMsgs.push_back(image2->rgb_camera_info);
|
||||
infoMsgs.push_back(image3->rgb_camera_info);
|
||||
infoMsgs.push_back(image4->rgb_camera_info);
|
||||
infoMsgs.push_back(image5->rgb_camera_info);
|
||||
|
||||
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
|
||||
}
|
||||
}
|
||||
|
||||
void RGBDOdometry::callbackRGBD6(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6)
|
||||
{
|
||||
tick(image->header.stamp);
|
||||
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(6);
|
||||
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(6);
|
||||
std::vector<sensor_msgs::msg::CameraInfo> infoMsgs;
|
||||
rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
|
||||
rtabmap_conversions::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
|
||||
rtabmap_conversions::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
|
||||
rtabmap_conversions::toCvShare(image4, imageMsgs[3], depthMsgs[3]);
|
||||
rtabmap_conversions::toCvShare(image5, imageMsgs[4], depthMsgs[4]);
|
||||
rtabmap_conversions::toCvShare(image6, imageMsgs[5], depthMsgs[5]);
|
||||
infoMsgs.push_back(image->rgb_camera_info);
|
||||
infoMsgs.push_back(image2->rgb_camera_info);
|
||||
infoMsgs.push_back(image3->rgb_camera_info);
|
||||
infoMsgs.push_back(image4->rgb_camera_info);
|
||||
infoMsgs.push_back(image5->rgb_camera_info);
|
||||
infoMsgs.push_back(image6->rgb_camera_info);
|
||||
|
||||
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
|
||||
}
|
||||
}
|
||||
|
||||
void RGBDOdometry::flushCallbacks()
|
||||
{
|
||||
// flush callbacks
|
||||
if(approxSync_)
|
||||
{
|
||||
delete approxSync_;
|
||||
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
|
||||
approxSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
}
|
||||
if(exactSync_)
|
||||
{
|
||||
delete exactSync_;
|
||||
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
|
||||
exactSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
}
|
||||
if(approxSync2_)
|
||||
{
|
||||
delete approxSync2_;
|
||||
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
|
||||
MyApproxSync2Policy(syncQueueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_);
|
||||
approxSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
|
||||
}
|
||||
if(exactSync2_)
|
||||
{
|
||||
delete exactSync2_;
|
||||
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
|
||||
MyExactSync2Policy(syncQueueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_);
|
||||
exactSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
|
||||
}
|
||||
if(approxSync3_)
|
||||
{
|
||||
delete approxSync3_;
|
||||
approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
|
||||
MyApproxSync3Policy(syncQueueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_,
|
||||
rgbd_image3_sub_);
|
||||
approxSync3_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
}
|
||||
if(exactSync3_)
|
||||
{
|
||||
delete exactSync3_;
|
||||
exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
|
||||
MyExactSync3Policy(syncQueueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_,
|
||||
rgbd_image3_sub_);
|
||||
exactSync3_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
}
|
||||
if(approxSync4_)
|
||||
{
|
||||
delete approxSync4_;
|
||||
approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
|
||||
MyApproxSync4Policy(syncQueueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_,
|
||||
rgbd_image3_sub_,
|
||||
rgbd_image4_sub_);
|
||||
approxSync4_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
|
||||
}
|
||||
if(exactSync4_)
|
||||
{
|
||||
delete exactSync4_;
|
||||
exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
|
||||
MyExactSync4Policy(syncQueueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_,
|
||||
rgbd_image3_sub_,
|
||||
rgbd_image4_sub_);
|
||||
exactSync4_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
|
||||
}
|
||||
if(approxSync5_)
|
||||
{
|
||||
delete approxSync5_;
|
||||
approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
|
||||
MyApproxSync5Policy(syncQueueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_,
|
||||
rgbd_image3_sub_,
|
||||
rgbd_image4_sub_,
|
||||
rgbd_image5_sub_);
|
||||
approxSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
|
||||
}
|
||||
if(exactSync5_)
|
||||
{
|
||||
delete exactSync5_;
|
||||
exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
|
||||
MyExactSync5Policy(syncQueueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_,
|
||||
rgbd_image3_sub_,
|
||||
rgbd_image4_sub_,
|
||||
rgbd_image5_sub_);
|
||||
exactSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
|
||||
}
|
||||
if(approxSync6_)
|
||||
{
|
||||
delete approxSync6_;
|
||||
approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
|
||||
MyApproxSync6Policy(syncQueueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_,
|
||||
rgbd_image3_sub_,
|
||||
rgbd_image4_sub_,
|
||||
rgbd_image5_sub_,
|
||||
rgbd_image6_sub_);
|
||||
approxSync6_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
|
||||
}
|
||||
if(exactSync6_)
|
||||
{
|
||||
delete exactSync6_;
|
||||
exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
|
||||
MyExactSync6Policy(syncQueueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_,
|
||||
rgbd_image3_sub_,
|
||||
rgbd_image4_sub_,
|
||||
rgbd_image5_sub_,
|
||||
rgbd_image6_sub_);
|
||||
exactSync6_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
|
||||
// Register the component with class_loader.
|
||||
// This acts as a sort of entry point, allowing the component to be discoverable when its library
|
||||
// is being loaded into a running process.
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_odom::RGBDOdometry)
|
||||
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user