mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-05 01:07:49 +08:00
ported rtabmap_ros_pkg_split to ros2
This commit is contained in:
matlabbe
@@ -0,0 +1,76 @@
|
||||
/*
|
||||
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);
|
||||
rclcpp::spin(std::make_shared<rtabmap_odom::ICPOdometry>(options));
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,76 @@
|
||||
/*
|
||||
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"
|
||||
|
||||
|
||||
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]);
|
||||
}
|
||||
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::NodeOptions options;
|
||||
options.arguments(arguments);
|
||||
rclcpp::spin(std::make_shared<rtabmap_odom::RGBDOdometry>(options));
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,76 @@
|
||||
/*
|
||||
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"
|
||||
|
||||
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]);
|
||||
|
||||
}
|
||||
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::NodeOptions options;
|
||||
options.arguments(arguments);
|
||||
rclcpp::spin(std::make_shared<rtabmap_odom::StereoOdometry>(options));
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,820 @@
|
||||
/*
|
||||
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_(0),
|
||||
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()
|
||||
{
|
||||
//plugins_.clear();
|
||||
}
|
||||
|
||||
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");
|
||||
|
||||
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));
|
||||
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));
|
||||
|
||||
filtered_scan_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>("odom_filtered_input_scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()));
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
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::getTransform(
|
||||
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(),
|
||||
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);
|
||||
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;
|
||||
}
|
||||
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(scanCloudMaxPoints_ == 0 && cloudMsg->height > 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(cloudMsg->height > 1 && 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;
|
||||
}
|
||||
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,763 @@
|
||||
/*
|
||||
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 <image_geometry/stereo_camera_model.h>
|
||||
|
||||
#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),
|
||||
queueSize_(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_;
|
||||
}
|
||||
|
||||
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);
|
||||
queueSize_ = this->declare_parameter("queue_size", queueSize_);
|
||||
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 = 1;
|
||||
}
|
||||
if(rgbdCameras > 5)
|
||||
{
|
||||
RCLCPP_FATAL(this->get_logger(), "Only 5 cameras maximum supported yet. Set 0 to use rgbd_images input (for which rgbdx_sync node can sync up to 8 cameras).");
|
||||
}
|
||||
keepColor_ = this->declare_parameter("keep_color", keepColor_);
|
||||
|
||||
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: queue_size = %d", queueSize_);
|
||||
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");
|
||||
|
||||
std::string subscribedTopicsMsg;
|
||||
if(subscribeRGBD)
|
||||
{
|
||||
if(rgbdCameras >= 2)
|
||||
{
|
||||
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
if(rgbdCameras >= 3)
|
||||
{
|
||||
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
}
|
||||
if(rgbdCameras >= 4)
|
||||
{
|
||||
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
}
|
||||
if(rgbdCameras >= 5)
|
||||
{
|
||||
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
}
|
||||
|
||||
if(rgbdCameras == 2)
|
||||
{
|
||||
if(approxSync)
|
||||
{
|
||||
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
|
||||
MyApproxSync2Policy(queueSize_),
|
||||
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(queueSize_),
|
||||
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(queueSize_),
|
||||
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(queueSize_),
|
||||
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(queueSize_),
|
||||
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(queueSize_),
|
||||
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(queueSize_),
|
||||
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(queueSize_),
|
||||
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 == 0)
|
||||
{
|
||||
rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBDX, this, std::placeholders::_1));
|
||||
|
||||
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(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1));
|
||||
|
||||
subscribedTopicsMsg =
|
||||
uFormat("\n%s subscribed to:\n %s",
|
||||
get_name(),
|
||||
rgbdSub_->get_topic_name());
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
image_transport::TransportHints hints(this);
|
||||
image_mono_sub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
image_depth_sub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
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(queueSize_), 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(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
|
||||
exactSync_->registerCallback(std::bind(&RGBDOdometry::callback, 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():"",
|
||||
image_mono_sub_.getSubscriber().getTopic().c_str(),
|
||||
image_depth_sub_.getSubscriber().getTopic().c_str(),
|
||||
info_sub_.getSubscriber()->get_topic_name());
|
||||
}
|
||||
this->startWarningThread(subscribedTopicsMsg, approxSync);
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
callbackCalled();
|
||||
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)
|
||||
{
|
||||
callbackCalled();
|
||||
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)
|
||||
{
|
||||
callbackCalled();
|
||||
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)
|
||||
{
|
||||
callbackCalled();
|
||||
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)
|
||||
{
|
||||
callbackCalled();
|
||||
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)
|
||||
{
|
||||
callbackCalled();
|
||||
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)
|
||||
{
|
||||
callbackCalled();
|
||||
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::flushCallbacks()
|
||||
{
|
||||
// flush callbacks
|
||||
if(approxSync_)
|
||||
{
|
||||
delete approxSync_;
|
||||
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), 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(queueSize_), 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(queueSize_),
|
||||
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(queueSize_),
|
||||
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(queueSize_),
|
||||
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(queueSize_),
|
||||
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(queueSize_),
|
||||
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(queueSize_),
|
||||
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(queueSize_),
|
||||
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(queueSize_),
|
||||
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));
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#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)
|
||||
@@ -0,0 +1,825 @@
|
||||
/*
|
||||
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 <sensor_msgs/image_encodings.hpp>
|
||||
|
||||
#include <image_geometry/stereo_camera_model.h>
|
||||
|
||||
#include "rtabmap_conversions/MsgConversion.h"
|
||||
#include <rtabmap_msgs/msg/rgbd_images.hpp>
|
||||
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
#include <rtabmap/utilite/UTimer.h>
|
||||
#include <rtabmap/utilite/UStl.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <rtabmap/core/Odometry.h>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
namespace rtabmap_odom
|
||||
{
|
||||
|
||||
StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) :
|
||||
rtabmap_odom::OdometryROS("stereo_odometry", options),
|
||||
approxSync_(0),
|
||||
exactSync_(0),
|
||||
approxSync2_(0),
|
||||
exactSync2_(0),
|
||||
approxSync3_(0),
|
||||
exactSync3_(0),
|
||||
approxSync4_(0),
|
||||
exactSync4_(0),
|
||||
queueSize_(5),
|
||||
keepColor_(false)
|
||||
{
|
||||
OdometryROS::init(true, true, false);
|
||||
}
|
||||
|
||||
StereoOdometry::~StereoOdometry()
|
||||
{
|
||||
delete approxSync_;
|
||||
delete exactSync_;
|
||||
}
|
||||
|
||||
void StereoOdometry::onOdomInit()
|
||||
{
|
||||
bool approxSync = false;
|
||||
bool subscribeRGBD = false;
|
||||
double approxSyncMaxInterval = 0.0;
|
||||
int rgbdCameras = 1;
|
||||
approxSync = this->declare_parameter("approx_sync", approxSync);
|
||||
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
|
||||
queueSize_ = this->declare_parameter("queue_size", queueSize_);
|
||||
int qosCamInfo = this->declare_parameter("qos_camera_info", (int)qos());
|
||||
subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD);
|
||||
rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras);
|
||||
keepColor_ = this->declare_parameter("keep_color", keepColor_);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "StereoOdometry: approx_sync = %s", approxSync?"true":"false");
|
||||
if(approxSync)
|
||||
RCLCPP_INFO(this->get_logger(), "StereoOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
|
||||
RCLCPP_INFO(this->get_logger(), "StereoOdometry: queue_size = %d", queueSize_);
|
||||
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(), "StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
|
||||
RCLCPP_INFO(this->get_logger(), "StereoOdometry: keep_color = %s", keepColor_?"true":"false");
|
||||
|
||||
std::string subscribedTopicsMsg;
|
||||
if(subscribeRGBD)
|
||||
{
|
||||
if(rgbdCameras >= 2)
|
||||
{
|
||||
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
if(rgbdCameras >= 3)
|
||||
{
|
||||
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
}
|
||||
if(rgbdCameras >= 4)
|
||||
{
|
||||
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
}
|
||||
|
||||
if(rgbdCameras == 2)
|
||||
{
|
||||
if(approxSync)
|
||||
{
|
||||
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
|
||||
MyApproxSync2Policy(queueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_);
|
||||
if(approxSyncMaxInterval > 0.0)
|
||||
approxSync2_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
approxSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
|
||||
}
|
||||
else
|
||||
{
|
||||
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
|
||||
MyExactSync2Policy(queueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_);
|
||||
exactSync2_->registerCallback(std::bind(&StereoOdometry::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_.getTopic().c_str(),
|
||||
rgbd_image2_sub_.getTopic().c_str());
|
||||
}
|
||||
else if(rgbdCameras == 3)
|
||||
{
|
||||
if(approxSync)
|
||||
{
|
||||
approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
|
||||
MyApproxSync3Policy(queueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_,
|
||||
rgbd_image3_sub_);
|
||||
if(approxSyncMaxInterval > 0.0)
|
||||
approxSync3_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
approxSync3_->registerCallback(std::bind(&StereoOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
}
|
||||
else
|
||||
{
|
||||
exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
|
||||
MyExactSync3Policy(queueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_,
|
||||
rgbd_image3_sub_);
|
||||
exactSync3_->registerCallback(std::bind(&StereoOdometry::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_.getTopic().c_str(),
|
||||
rgbd_image2_sub_.getTopic().c_str(),
|
||||
rgbd_image3_sub_.getTopic().c_str());
|
||||
}
|
||||
else if(rgbdCameras == 4)
|
||||
{
|
||||
if(approxSync)
|
||||
{
|
||||
approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
|
||||
MyApproxSync4Policy(queueSize_),
|
||||
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(&StereoOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
|
||||
}
|
||||
else
|
||||
{
|
||||
exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
|
||||
MyExactSync4Policy(queueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_,
|
||||
rgbd_image3_sub_,
|
||||
rgbd_image4_sub_);
|
||||
exactSync4_->registerCallback(std::bind(&StereoOdometry::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_.getTopic().c_str(),
|
||||
rgbd_image2_sub_.getTopic().c_str(),
|
||||
rgbd_image3_sub_.getTopic().c_str(),
|
||||
rgbd_image4_sub_.getTopic().c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_FATAL(this->get_logger(), "%s doesn't support more than 4 cameras (rgbd_cameras=%d) with internal synchronization interface, set rgbd_cameras=0 and use rgbd_images input topic instead for more cameras.", get_name(), rgbdCameras);
|
||||
}
|
||||
|
||||
}
|
||||
else if(rgbdCameras == 0)
|
||||
{
|
||||
rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBDX, this, std::placeholders::_1));
|
||||
|
||||
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(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1));
|
||||
|
||||
subscribedTopicsMsg =
|
||||
uFormat("\n%s subscribed to:\n %s",
|
||||
get_name(),
|
||||
rgbdSub_->get_topic_name());
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
image_transport::TransportHints hints(this);
|
||||
imageRectLeft_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
imageRectRight_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
cameraInfoLeft_.subscribe(this, "left/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
|
||||
cameraInfoRight_.subscribe(this, "right/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(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
|
||||
if(approxSyncMaxInterval>0.0)
|
||||
approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
approxSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
|
||||
}
|
||||
else
|
||||
{
|
||||
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
|
||||
exactSync_->registerCallback(std::bind(&StereoOdometry::callback, 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():"",
|
||||
imageRectLeft_.getTopic().c_str(),
|
||||
imageRectRight_.getTopic().c_str(),
|
||||
cameraInfoLeft_.getSubscriber()->get_topic_name(),
|
||||
cameraInfoRight_.getSubscriber()->get_topic_name());
|
||||
}
|
||||
|
||||
this->startWarningThread(subscribedTopicsMsg, approxSync);
|
||||
}
|
||||
|
||||
void StereoOdometry::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(), "Stereo odometry works only with \"Reg/Strategy\"=0. Ignoring value %s.", iter->second.c_str());
|
||||
}
|
||||
uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0"));
|
||||
}
|
||||
|
||||
void StereoOdometry::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)
|
||||
{
|
||||
UASSERT(leftImages.size() > 0 &&
|
||||
leftImages.size() == rightImages.size() &&
|
||||
leftImages.size() == leftCameraInfos.size() &&
|
||||
rightImages.size() == rightCameraInfos.size());
|
||||
rclcpp::Time higherStamp;
|
||||
int leftWidth = leftImages[0]->image.cols;
|
||||
int leftHeight = leftImages[0]->image.rows;
|
||||
int rightWidth = rightImages[0]->image.cols;
|
||||
int rightHeight = rightImages[0]->image.rows;
|
||||
|
||||
UASSERT_MSG(
|
||||
leftWidth == rightWidth && leftHeight == rightHeight,
|
||||
uFormat("left=%dx%d right=%dx%d", leftWidth, leftHeight, rightWidth, rightHeight).c_str());
|
||||
|
||||
int cameraCount = leftImages.size();
|
||||
cv::Mat left;
|
||||
cv::Mat right;
|
||||
std::vector<rtabmap::StereoCameraModel> cameraModels;
|
||||
for(unsigned int i=0; i<leftImages.size(); ++i)
|
||||
{
|
||||
if(!(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
|
||||
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
|
||||
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
|
||||
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
|
||||
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
|
||||
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
|
||||
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
|
||||
!(rightImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
|
||||
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
|
||||
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
|
||||
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
|
||||
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
|
||||
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
|
||||
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 recommended), received types are %s (left) and %s (right)",
|
||||
leftImages[i]->encoding.c_str(), rightImages[i]->encoding.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
rclcpp::Time stamp = rtabmap_conversions::timestampFromROS(leftImages[i]->header.stamp)>rtabmap_conversions::timestampFromROS(rightImages[i]->header.stamp)?leftImages[i]->header.stamp:rightImages[i]->header.stamp;
|
||||
|
||||
if(i == 0)
|
||||
{
|
||||
higherStamp = stamp;
|
||||
}
|
||||
else if(stamp > higherStamp)
|
||||
{
|
||||
higherStamp = stamp;
|
||||
}
|
||||
|
||||
Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), leftImages[i]->header.frame_id, stamp, tfBuffer(), waitForTransform());
|
||||
if(localTransform.isNull())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if(i>0)
|
||||
{
|
||||
double stampDiff = fabs(rtabmap_conversions::timestampFromROS(leftImages[i]->header.stamp) - rtabmap_conversions::timestampFromROS(leftImages[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(leftImages[i-1]->header.stamp),
|
||||
i, rtabmap_conversions::timestampFromROS(leftImages[i]->header.stamp));
|
||||
warningShown = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(!leftImages[i]->image.empty() && !rightImages[i]->image.empty())
|
||||
{
|
||||
bool alreadyRectified = true;
|
||||
Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified);
|
||||
rtabmap::Transform stereoTransform;
|
||||
if(!alreadyRectified)
|
||||
{
|
||||
if(rightCameraInfos[i].header.frame_id.empty() || leftCameraInfos[i].header.frame_id.empty())
|
||||
{
|
||||
if(rightCameraInfos[i].p[3] == 0.0 && leftCameraInfos[i].p[3] == 0)
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but the frame_id in one of the camera_info "
|
||||
"topic is empty, so TF between the cameras cannot be computed!",
|
||||
Parameters::kRtabmapImagesAlreadyRectified().c_str());
|
||||
return;
|
||||
}
|
||||
else
|
||||
{
|
||||
stereoTransform = rtabmap_conversions::getTransform(
|
||||
rightCameraInfos[i].header.frame_id,
|
||||
leftCameraInfos[i].header.frame_id,
|
||||
leftCameraInfos[i].header.stamp,
|
||||
tfBuffer(),
|
||||
waitForTransform());
|
||||
if(stereoTransform.isNull())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get TF between the two cameras! (between frames %s and %s)",
|
||||
Parameters::kRtabmapImagesAlreadyRectified().c_str(),
|
||||
rightCameraInfos[i].header.frame_id.c_str(),
|
||||
leftCameraInfos[i].header.frame_id.c_str());
|
||||
return;
|
||||
}
|
||||
else if(stereoTransform.isIdentity())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get a valid TF between the two cameras! "
|
||||
"Identity transform returned between left and right cameras. Verify that if TF between "
|
||||
"the cameras is valid: \"rosrun tf tf_echo %s %s\".",
|
||||
Parameters::kRtabmapImagesAlreadyRectified().c_str(),
|
||||
rightCameraInfos[i].header.frame_id.c_str(),
|
||||
leftCameraInfos[i].header.frame_id.c_str());
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
rtabmap::StereoCameraModel stereoModel = rtabmap_conversions::stereoCameraModelFromROS(leftCameraInfos[i], rightCameraInfos[i], localTransform, stereoTransform);
|
||||
|
||||
if( stereoModel.baseline() == 0 &&
|
||||
alreadyRectified &&
|
||||
!rightCameraInfos[i].header.frame_id.empty() &&
|
||||
!leftCameraInfos[i].header.frame_id.empty())
|
||||
{
|
||||
stereoTransform = rtabmap_conversions::getTransform(
|
||||
leftCameraInfos[i].header.frame_id,
|
||||
rightCameraInfos[i].header.frame_id,
|
||||
leftCameraInfos[i].header.stamp,
|
||||
tfBuffer(),
|
||||
waitForTransform());
|
||||
|
||||
if(!stereoTransform.isNull() && stereoTransform.x()>0)
|
||||
{
|
||||
static bool warned = false;
|
||||
if(!warned)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "Parameter %s is false but the frame_id in one of the "
|
||||
"camera_info topic is empty, so TF between the cameras cannot be "
|
||||
"computed! However, the baseline can be computed from the calibration, "
|
||||
"we will use this one instead of TF. This message is only printed once...",
|
||||
Parameters::kRtabmapImagesAlreadyRectified().c_str());
|
||||
warned = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
stereoTransform = rtabmap_conversions::getTransform(
|
||||
rightCameraInfos[i].header.frame_id,
|
||||
leftCameraInfos[i].header.frame_id,
|
||||
leftCameraInfos[i].header.stamp,
|
||||
tfBuffer(),
|
||||
waitForTransform());
|
||||
if(stereoTransform.isNull())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get TF between the two cameras! (between frames %s and %s)",
|
||||
Parameters::kRtabmapImagesAlreadyRectified().c_str(),
|
||||
rightCameraInfos[i].header.frame_id.c_str(),
|
||||
leftCameraInfos[i].header.frame_id.c_str());
|
||||
return;
|
||||
}
|
||||
else if(stereoTransform.isIdentity())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get a valid TF between the two cameras! "
|
||||
"Identity transform returned between left and right cameras. Verify that if TF between "
|
||||
"the cameras is valid: \"rosrun tf tf_echo %s %s\".",
|
||||
Parameters::kRtabmapImagesAlreadyRectified().c_str(),
|
||||
rightCameraInfos[i].header.frame_id.c_str(),
|
||||
leftCameraInfos[i].header.frame_id.c_str());
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
rtabmap::StereoCameraModel stereoModel = rtabmap_conversions::stereoCameraModelFromROS(leftCameraInfos[i], rightCameraInfos[i], localTransform, stereoTransform);
|
||||
|
||||
if( stereoModel.baseline() == 0 &&
|
||||
alreadyRectified &&
|
||||
!rightCameraInfos[i].header.frame_id.empty() &&
|
||||
!leftCameraInfos[i].header.frame_id.empty())
|
||||
{
|
||||
stereoTransform = rtabmap_conversions::getTransform(
|
||||
leftCameraInfos[i].header.frame_id,
|
||||
rightCameraInfos[i].header.frame_id,
|
||||
leftCameraInfos[i].header.stamp,
|
||||
tfBuffer(),
|
||||
waitForTransform());
|
||||
|
||||
if(!stereoTransform.isNull() && stereoTransform.x()>0)
|
||||
{
|
||||
static bool warned = false;
|
||||
if(!warned)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "Right camera info doesn't have Tx set but we are assuming that stereo images are already rectified (see %s parameter). While not "
|
||||
"recommended, we used TF to get the baseline (%s->%s = %fm) for convenience (e.g., D400 ir stereo issue). It is preferred to feed "
|
||||
"a valid right camera info if stereo images are already rectified. This message is only printed once...",
|
||||
rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str(),
|
||||
rightCameraInfos[i].header.frame_id.c_str(), leftCameraInfos[i].header.frame_id.c_str(), stereoTransform.x());
|
||||
warned = true;
|
||||
}
|
||||
stereoModel = rtabmap::StereoCameraModel(
|
||||
stereoModel.left().fx(),
|
||||
stereoModel.left().fy(),
|
||||
stereoModel.left().cx(),
|
||||
stereoModel.left().cy(),
|
||||
stereoTransform.x(),
|
||||
stereoModel.localTransform(),
|
||||
stereoModel.left().imageSize());
|
||||
}
|
||||
}
|
||||
if(alreadyRectified && stereoModel.baseline() <= 0)
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
|
||||
"setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline());
|
||||
return;
|
||||
}
|
||||
|
||||
if(stereoModel.baseline() > 10.0)
|
||||
{
|
||||
static bool shown = false;
|
||||
if(!shown)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "Detected baseline (%f m) is quite large! Is your "
|
||||
"right camera_info P(0,3) correctly set? Note that "
|
||||
"baseline=-P(0,3)/P(0,0). This warning is printed only once.",
|
||||
stereoModel.baseline());
|
||||
shown = true;
|
||||
}
|
||||
}
|
||||
cv_bridge::CvImageConstPtr ptrLeft = leftImages[i];
|
||||
if(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
|
||||
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
|
||||
{
|
||||
if(keepColor_ && leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
|
||||
{
|
||||
ptrLeft = cv_bridge::cvtColor(leftImages[i], "bgr8");
|
||||
}
|
||||
else
|
||||
{
|
||||
ptrLeft = cv_bridge::cvtColor(leftImages[i], "mono8");
|
||||
}
|
||||
}
|
||||
cv_bridge::CvImageConstPtr ptrRight = rightImages[i];
|
||||
if(rightImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
|
||||
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
|
||||
{
|
||||
ptrRight = cv_bridge::cvtColor(rightImages[i], "mono8");
|
||||
}
|
||||
|
||||
// initialize
|
||||
if(left.empty())
|
||||
{
|
||||
left = cv::Mat(leftHeight, leftWidth*cameraCount, ptrLeft->image.type());
|
||||
}
|
||||
if(right.empty())
|
||||
{
|
||||
right = cv::Mat(rightHeight, rightWidth*cameraCount, ptrRight->image.type());
|
||||
}
|
||||
|
||||
if(ptrLeft->image.type() == left.type())
|
||||
{
|
||||
ptrLeft->image.copyTo(cv::Mat(left, cv::Rect(i*leftWidth, 0, leftWidth, leftHeight)));
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Some left images are not the same type! %d vs %d", ptrLeft->image.type(), left.type());
|
||||
return;
|
||||
}
|
||||
|
||||
if(ptrRight->image.type() == right.type())
|
||||
{
|
||||
ptrRight->image.copyTo(cv::Mat(right, cv::Rect(i*rightWidth, 0, rightWidth, rightHeight)));
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Some right images are not the same type! %d vs %d", ptrRight->image.type(), right.type());
|
||||
return;
|
||||
}
|
||||
|
||||
cameraModels.push_back(stereoModel);
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Odom: input images empty?!?");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
rtabmap::SensorData data(
|
||||
left,
|
||||
right,
|
||||
cameraModels,
|
||||
0,
|
||||
rtabmap_conversions::timestampFromROS(higherStamp));
|
||||
|
||||
std_msgs::msg::Header header;
|
||||
header.stamp = higherStamp;
|
||||
header.frame_id = leftImages.size()==1?leftImages[0]->header.frame_id:"";
|
||||
this->processData(data, header);
|
||||
}
|
||||
|
||||
void StereoOdometry::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)
|
||||
{
|
||||
callbackCalled();
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
|
||||
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(1);
|
||||
std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
|
||||
std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
|
||||
leftMsgs[0] = cv_bridge::toCvShare(imageRectLeft);
|
||||
rightMsgs[0] = cv_bridge::toCvShare(imageRectRight);
|
||||
leftInfoMsgs.push_back(*cameraInfoLeft);
|
||||
rightInfoMsgs.push_back(*cameraInfoRight);
|
||||
|
||||
double stampDiff = fabs(rtabmap_conversions::timestampFromROS(imageRectLeft->header.stamp) - rtabmap_conversions::timestampFromROS(imageRectRight->header.stamp));
|
||||
if(stampDiff > 0.010)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "The time difference between left and right frames is "
|
||||
"high (diff=%fs, left=%fs, right=%fs). If your left and right cameras are hardware "
|
||||
"synchronized, use approx_sync:=false. Otherwise, you may want "
|
||||
"to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations.",
|
||||
stampDiff,
|
||||
rtabmap_conversions::timestampFromROS(imageRectLeft->header.stamp),
|
||||
rtabmap_conversions::timestampFromROS(imageRectRight->header.stamp));
|
||||
}
|
||||
|
||||
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
|
||||
}
|
||||
}
|
||||
|
||||
void StereoOdometry::callbackRGBD(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image)
|
||||
{
|
||||
callbackCalled();
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
|
||||
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(1);
|
||||
std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
|
||||
std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
|
||||
rtabmap_conversions::toCvShare(image, leftMsgs[0], rightMsgs[0]);
|
||||
leftInfoMsgs.push_back(image->rgb_camera_info);
|
||||
rightInfoMsgs.push_back(image->depth_camera_info);
|
||||
|
||||
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
|
||||
}
|
||||
}
|
||||
|
||||
void StereoOdometry::callbackRGBDX(
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images)
|
||||
{
|
||||
callbackCalled();
|
||||
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> leftMsgs(images->rgbd_images.size());
|
||||
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(images->rgbd_images.size());
|
||||
std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
|
||||
std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
|
||||
for(size_t i=0; i<images->rgbd_images.size(); ++i)
|
||||
{
|
||||
rtabmap_conversions::toCvShare(images->rgbd_images[i], images, leftMsgs[i], rightMsgs[i]);
|
||||
leftInfoMsgs.push_back(images->rgbd_images[i].rgb_camera_info);
|
||||
rightInfoMsgs.push_back(images->rgbd_images[i].depth_camera_info);
|
||||
}
|
||||
|
||||
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
|
||||
}
|
||||
}
|
||||
|
||||
void StereoOdometry::callbackRGBD2(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2)
|
||||
{
|
||||
callbackCalled();
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(2);
|
||||
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(2);
|
||||
std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
|
||||
std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
|
||||
rtabmap_conversions::toCvShare(image, leftMsgs[0], rightMsgs[0]);
|
||||
rtabmap_conversions::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
|
||||
leftInfoMsgs.push_back(image->rgb_camera_info);
|
||||
leftInfoMsgs.push_back(image2->rgb_camera_info);
|
||||
rightInfoMsgs.push_back(image->depth_camera_info);
|
||||
rightInfoMsgs.push_back(image2->depth_camera_info);
|
||||
|
||||
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
|
||||
}
|
||||
}
|
||||
|
||||
void StereoOdometry::callbackRGBD3(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3)
|
||||
{
|
||||
callbackCalled();
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(3);
|
||||
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(3);
|
||||
std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
|
||||
std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
|
||||
rtabmap_conversions::toCvShare(image, leftMsgs[0], rightMsgs[0]);
|
||||
rtabmap_conversions::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
|
||||
rtabmap_conversions::toCvShare(image3, leftMsgs[2], rightMsgs[2]);
|
||||
leftInfoMsgs.push_back(image->rgb_camera_info);
|
||||
leftInfoMsgs.push_back(image2->rgb_camera_info);
|
||||
leftInfoMsgs.push_back(image3->rgb_camera_info);
|
||||
rightInfoMsgs.push_back(image->depth_camera_info);
|
||||
rightInfoMsgs.push_back(image2->depth_camera_info);
|
||||
rightInfoMsgs.push_back(image3->depth_camera_info);
|
||||
|
||||
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
|
||||
}
|
||||
}
|
||||
|
||||
void StereoOdometry::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)
|
||||
{
|
||||
callbackCalled();
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(4);
|
||||
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(4);
|
||||
std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
|
||||
std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
|
||||
rtabmap_conversions::toCvShare(image, leftMsgs[0], rightMsgs[0]);
|
||||
rtabmap_conversions::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
|
||||
rtabmap_conversions::toCvShare(image3, leftMsgs[2], rightMsgs[2]);
|
||||
rtabmap_conversions::toCvShare(image4, leftMsgs[3], rightMsgs[3]);
|
||||
leftInfoMsgs.push_back(image->rgb_camera_info);
|
||||
leftInfoMsgs.push_back(image2->rgb_camera_info);
|
||||
leftInfoMsgs.push_back(image3->rgb_camera_info);
|
||||
leftInfoMsgs.push_back(image4->rgb_camera_info);
|
||||
rightInfoMsgs.push_back(image->depth_camera_info);
|
||||
rightInfoMsgs.push_back(image2->depth_camera_info);
|
||||
rightInfoMsgs.push_back(image3->depth_camera_info);
|
||||
rightInfoMsgs.push_back(image4->depth_camera_info);
|
||||
|
||||
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
|
||||
}
|
||||
}
|
||||
|
||||
void StereoOdometry::flushCallbacks()
|
||||
{
|
||||
//flush callbacks
|
||||
if(approxSync_)
|
||||
{
|
||||
delete approxSync_;
|
||||
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
|
||||
approxSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
|
||||
}
|
||||
if(exactSync_)
|
||||
{
|
||||
delete exactSync_;
|
||||
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
|
||||
exactSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
|
||||
}
|
||||
if(approxSync2_)
|
||||
{
|
||||
delete approxSync2_;
|
||||
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
|
||||
MyApproxSync2Policy(queueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_);
|
||||
approxSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
|
||||
}
|
||||
if(exactSync2_)
|
||||
{
|
||||
delete exactSync2_;
|
||||
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
|
||||
MyExactSync2Policy(queueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_);
|
||||
exactSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
|
||||
}
|
||||
if(approxSync3_)
|
||||
{
|
||||
delete approxSync3_;
|
||||
approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
|
||||
MyApproxSync3Policy(queueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_,
|
||||
rgbd_image3_sub_);
|
||||
approxSync3_->registerCallback(std::bind(&StereoOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
}
|
||||
if(exactSync3_)
|
||||
{
|
||||
delete exactSync3_;
|
||||
exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
|
||||
MyExactSync3Policy(queueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_,
|
||||
rgbd_image3_sub_);
|
||||
exactSync3_->registerCallback(std::bind(&StereoOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
}
|
||||
if(approxSync4_)
|
||||
{
|
||||
delete approxSync4_;
|
||||
approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
|
||||
MyApproxSync4Policy(queueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_,
|
||||
rgbd_image3_sub_,
|
||||
rgbd_image4_sub_);
|
||||
approxSync4_->registerCallback(std::bind(&StereoOdometry::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(queueSize_),
|
||||
rgbd_image1_sub_,
|
||||
rgbd_image2_sub_,
|
||||
rgbd_image3_sub_,
|
||||
rgbd_image4_sub_);
|
||||
exactSync4_->registerCallback(std::bind(&StereoOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#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::StereoOdometry)
|
||||
|
||||
Reference in New Issue
Block a user