mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-08 10:47:46 +08:00
ported rtabmap_ros_pkg_split to ros2
This commit is contained in:
matlabbe
@@ -0,0 +1,140 @@
|
||||
/*
|
||||
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 <ros/ros.h>
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
#include <nodelet/nodelet.h>
|
||||
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <sensor_msgs/image_encodings.h>
|
||||
#include <stereo_msgs/DisparityImage.h>
|
||||
|
||||
#include <image_transport/image_transport.h>
|
||||
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
|
||||
namespace rtabmap_util
|
||||
{
|
||||
|
||||
class DisparityToDepth : public nodelet::Nodelet
|
||||
{
|
||||
public:
|
||||
DisparityToDepth() {}
|
||||
|
||||
virtual ~DisparityToDepth(){}
|
||||
|
||||
private:
|
||||
virtual void onInit()
|
||||
{
|
||||
ros::NodeHandle & nh = getNodeHandle();
|
||||
ros::NodeHandle & pnh = getPrivateNodeHandle();
|
||||
|
||||
image_transport::ImageTransport it(nh);
|
||||
pub32f_ = it.advertise("depth", 1);
|
||||
pub16u_ = it.advertise("depth_raw", 1);
|
||||
sub_ = nh.subscribe("disparity", 1, &DisparityToDepth::callback, this);
|
||||
}
|
||||
|
||||
void callback(const stereo_msgs::DisparityImageConstPtr& disparityMsg)
|
||||
{
|
||||
if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0)
|
||||
{
|
||||
NODELET_ERROR("Input type must be disparity=32FC1");
|
||||
return;
|
||||
}
|
||||
|
||||
bool publish32f = pub32f_.getNumSubscribers();
|
||||
bool publish16u = pub16u_.getNumSubscribers();
|
||||
|
||||
if(publish32f || publish16u)
|
||||
{
|
||||
// sensor_msgs::image_encodings::TYPE_32FC1
|
||||
cv::Mat disparity(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast<uchar*>(disparityMsg->image.data.data()));
|
||||
|
||||
cv::Mat depth32f;
|
||||
cv::Mat depth16u;
|
||||
if(publish32f)
|
||||
{
|
||||
depth32f = cv::Mat::zeros(disparity.rows, disparity.cols, CV_32F);
|
||||
}
|
||||
if(publish16u)
|
||||
{
|
||||
depth16u = cv::Mat::zeros(disparity.rows, disparity.cols, CV_16U);
|
||||
}
|
||||
for (int i = 0; i < disparity.rows; i++)
|
||||
{
|
||||
for (int j = 0; j < disparity.cols; j++)
|
||||
{
|
||||
float disparity_value = disparity.at<float>(i,j);
|
||||
if (disparity_value > disparityMsg->min_disparity && disparity_value < disparityMsg->max_disparity)
|
||||
{
|
||||
// baseline * focal / disparity
|
||||
float depth = disparityMsg->T * disparityMsg->f / disparity_value;
|
||||
if(publish32f)
|
||||
{
|
||||
depth32f.at<float>(i,j) = depth;
|
||||
}
|
||||
if(publish16u)
|
||||
{
|
||||
depth16u.at<unsigned short>(i,j) = (unsigned short)(depth*1000.0f);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(publish32f)
|
||||
{
|
||||
// convert to ROS sensor_msg::Image
|
||||
cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_32FC1, depth32f);
|
||||
sensor_msgs::Image depthMsg;
|
||||
cvDepth.toImageMsg(depthMsg);
|
||||
|
||||
//publish the message
|
||||
pub32f_.publish(depthMsg);
|
||||
}
|
||||
|
||||
if(publish16u)
|
||||
{
|
||||
// convert to ROS sensor_msg::Image
|
||||
cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_16UC1, depth16u);
|
||||
sensor_msgs::Image depthMsg;
|
||||
cvDepth.toImageMsg(depthMsg);
|
||||
|
||||
//publish the message
|
||||
pub16u_.publish(depthMsg);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
image_transport::Publisher pub32f_;
|
||||
image_transport::Publisher pub16u_;
|
||||
ros::Subscriber sub_;
|
||||
};
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(rtabmap_util::DisparityToDepth, nodelet::Nodelet);
|
||||
}
|
||||
@@ -0,0 +1,122 @@
|
||||
/*
|
||||
Copyright (c) 2010-2019, 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 <ros/ros.h>
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
#include <nodelet/nodelet.h>
|
||||
#include <sensor_msgs/Imu.h>
|
||||
#include <tf/transform_broadcaster.h>
|
||||
#include <tf/LinearMath/Matrix3x3.h>
|
||||
#include <tf/transform_listener.h>
|
||||
|
||||
namespace rtabmap_util
|
||||
{
|
||||
|
||||
class ImuToTF : public nodelet::Nodelet
|
||||
{
|
||||
public:
|
||||
ImuToTF() :
|
||||
fixedFrameId_("odom"),
|
||||
waitForTransformDuration_(0.1)
|
||||
{}
|
||||
|
||||
virtual ~ImuToTF()
|
||||
{
|
||||
}
|
||||
|
||||
private:
|
||||
virtual void onInit()
|
||||
{
|
||||
ros::NodeHandle & nh = getNodeHandle();
|
||||
ros::NodeHandle & pnh = getPrivateNodeHandle();
|
||||
|
||||
pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
|
||||
pnh.param("base_frame_id", baseFrameId_, baseFrameId_);
|
||||
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
|
||||
NODELET_INFO("fixed_frame_id: %s", fixedFrameId_.c_str());
|
||||
NODELET_INFO("base_frame_id: %s", baseFrameId_.c_str());
|
||||
|
||||
sub_ = nh.subscribe<sensor_msgs::Imu>("imu/data", 1, &ImuToTF::imuCallback, this);
|
||||
}
|
||||
|
||||
void imuCallback(const sensor_msgs::ImuConstPtr & msg)
|
||||
{
|
||||
tf::Quaternion q;
|
||||
tf::quaternionMsgToTF(msg->orientation, q);
|
||||
tf::StampedTransform st;
|
||||
st.setRotation(q);
|
||||
|
||||
st.frame_id_ = fixedFrameId_;
|
||||
st.stamp_ = msg->header.stamp;
|
||||
|
||||
if(!baseFrameId_.empty() &&
|
||||
baseFrameId_.compare(msg->header.frame_id) != 0)
|
||||
{
|
||||
try
|
||||
{
|
||||
std::string errorMsg;
|
||||
if(!tfListener_.waitForTransform(baseFrameId_, msg->header.frame_id, msg->header.stamp, ros::Duration(waitForTransformDuration_), ros::Duration(0.01), &errorMsg))
|
||||
{
|
||||
NODELET_ERROR("Could not get transform from %s to %s after %f seconds (for stamp=%f)! Error=\"%s\".",
|
||||
baseFrameId_.c_str(), msg->header.frame_id.c_str(), 0.1, msg->header.stamp.toSec(), errorMsg.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
tf::StampedTransform tmp;
|
||||
tfListener_.lookupTransform(msg->header.frame_id, baseFrameId_, msg->header.stamp, tmp);
|
||||
tf::Transform t = tmp.inverse()*st*tmp;
|
||||
st.setRotation(t.getRotation());
|
||||
st.child_frame_id_ = baseFrameId_;
|
||||
}
|
||||
catch(tf::TransformException & ex)
|
||||
{
|
||||
NODELET_ERROR("(getting transform %s -> %s) %s", baseFrameId_.c_str(), msg->header.frame_id.c_str(), ex.what());
|
||||
return;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
st.child_frame_id_ = msg->header.frame_id;
|
||||
}
|
||||
st.setOrigin(tf::Vector3(0,0,0));
|
||||
|
||||
pub_.sendTransform(st);
|
||||
}
|
||||
|
||||
private:
|
||||
ros::Subscriber sub_;
|
||||
tf::TransformBroadcaster pub_;
|
||||
std::string fixedFrameId_;
|
||||
std::string baseFrameId_;
|
||||
tf::TransformListener tfListener_;
|
||||
double waitForTransformDuration_;
|
||||
};
|
||||
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(rtabmap_util::ImuToTF, nodelet::Nodelet);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,106 @@
|
||||
|
||||
#include <rtabmap_util/lidar_deskewing.hpp>
|
||||
|
||||
#include <laser_geometry/laser_geometry.hpp>
|
||||
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
|
||||
namespace rtabmap_util
|
||||
{
|
||||
|
||||
LidarDeskewing::LidarDeskewing(const rclcpp::NodeOptions & options) :
|
||||
Node("lidar_deskewing", options),
|
||||
waitForTransformDuration_(0.01),
|
||||
slerp_(false)
|
||||
{
|
||||
tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
|
||||
//auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
|
||||
// this->get_node_base_interface(),
|
||||
// this->get_node_timers_interface());
|
||||
//tfBuffer_->setCreateTimerInterface(timer_interface);
|
||||
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
|
||||
|
||||
int queueSize = 5;
|
||||
int qos = 0;
|
||||
queueSize = this->declare_parameter("queue_size", queueSize);
|
||||
qos = this->declare_parameter("qos", qos);
|
||||
fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
|
||||
waitForTransformDuration_ = this->declare_parameter("wait_for_transform", waitForTransformDuration_);
|
||||
slerp_ = this->declare_parameter("slerp", slerp_);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), " fixed_frame_id: %s", fixedFrameId_.c_str());
|
||||
RCLCPP_INFO(this->get_logger(), " wait_for_transform: %fs", waitForTransformDuration_);
|
||||
RCLCPP_INFO(this->get_logger(), " slerp: %s", slerp_?"true":"false");
|
||||
|
||||
if(fixedFrameId_.empty())
|
||||
{
|
||||
RCLCPP_FATAL(this->get_logger(), "fixed_frame_id parameter cannot be empty!");
|
||||
}
|
||||
|
||||
subScan_ = create_subscription<sensor_msgs::msg::LaserScan>("input_scan", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&LidarDeskewing::callbackScan, this, std::placeholders::_1));
|
||||
subCloud_ = create_subscription<sensor_msgs::msg::PointCloud2>("input_cloud", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&LidarDeskewing::callbackCloud, this, std::placeholders::_1));
|
||||
|
||||
pubScan_ = create_publisher<sensor_msgs::msg::PointCloud2>(std::string(subScan_->get_topic_name()) + "/deskewed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
|
||||
pubCloud_ = create_publisher<sensor_msgs::msg::PointCloud2>(std::string(subCloud_->get_topic_name()) + "/deskewed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
|
||||
}
|
||||
|
||||
LidarDeskewing::~LidarDeskewing()
|
||||
{
|
||||
}
|
||||
|
||||
void LidarDeskewing::callbackScan(const sensor_msgs::msg::LaserScan::ConstSharedPtr msg)
|
||||
{
|
||||
// make sure the frame of the laser is updated during the whole scan time
|
||||
rtabmap::Transform tmpT = rtabmap_conversions::getTransform(
|
||||
msg->header.frame_id,
|
||||
fixedFrameId_,
|
||||
msg->header.stamp,
|
||||
rclcpp::Time(msg->header.stamp.sec, msg->header.stamp.nanosec) + rclcpp::Duration::from_seconds(msg->ranges.size()*msg->time_increment),
|
||||
*tfBuffer_,
|
||||
waitForTransformDuration_);
|
||||
if(tmpT.isNull())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
sensor_msgs::msg::PointCloud2 scanOut;
|
||||
laser_geometry::LaserProjection projection;
|
||||
projection.transformLaserScanToPointCloud(fixedFrameId_, *msg, scanOut, *tfBuffer_);
|
||||
|
||||
rtabmap::Transform t = rtabmap_conversions::getTransform(msg->header.frame_id, scanOut.header.frame_id, msg->header.stamp, *tfBuffer_, waitForTransformDuration_);
|
||||
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(), msg->header.frame_id.c_str(), rtabmap_conversions::timestampFromROS(msg->header.stamp));
|
||||
return;
|
||||
}
|
||||
|
||||
sensor_msgs::msg::PointCloud2 scanOutDeskewed;
|
||||
rtabmap_conversions::transformPointCloud(t.toEigen4f(), scanOut, scanOutDeskewed);
|
||||
pubScan_->publish(scanOutDeskewed);
|
||||
}
|
||||
|
||||
void LidarDeskewing::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
|
||||
{
|
||||
sensor_msgs::msg::PointCloud2 msgDeskewed;
|
||||
if(rtabmap_conversions::deskew(*msg, msgDeskewed, fixedFrameId_, *tfBuffer_, waitForTransformDuration_, slerp_))
|
||||
{
|
||||
pubCloud_->publish(msgDeskewed);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Just republish the msg to not breakdown downstream
|
||||
// A warning should be already shown (see deskew() source code)
|
||||
RCLCPP_WARN(this->get_logger(), "deskewing failed! returning possible skewed cloud!");
|
||||
pubCloud_->publish(*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_util::LidarDeskewing)
|
||||
@@ -0,0 +1,270 @@
|
||||
/*
|
||||
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_util/obstacles_detection.hpp>
|
||||
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <pcl/filters/filter.h>
|
||||
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
|
||||
#include "rtabmap/utilite/UStl.h"
|
||||
|
||||
namespace rtabmap_util
|
||||
{
|
||||
|
||||
ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) :
|
||||
Node("obstacles_detection", options),
|
||||
frameId_("base_link"),
|
||||
waitForTransform_(0.2),
|
||||
mapFrameProjection_(rtabmap::Parameters::defaultGridMapFrameProjection()),
|
||||
warned_(false)
|
||||
{
|
||||
ULogger::setType(ULogger::kTypeConsole);
|
||||
ULogger::setLevel(ULogger::kWarning);
|
||||
|
||||
frameId_ = this->declare_parameter("frame_id", frameId_);
|
||||
mapFrameId_ = this->declare_parameter("map_frame_id", mapFrameId_);
|
||||
waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
|
||||
int qos = 0;
|
||||
qos = this->declare_parameter("qos", qos);
|
||||
|
||||
rtabmap::ParametersMap gridParameters = rtabmap::Parameters::getDefaultParameters("Grid");
|
||||
for(rtabmap::ParametersMap::iterator iter=gridParameters.begin(); iter!=gridParameters.end(); ++iter)
|
||||
{
|
||||
std::string vStr = declare_parameter(iter->first, iter->second);
|
||||
if(vStr.compare(iter->second) != 0)
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
|
||||
iter->second = vStr;
|
||||
}
|
||||
}
|
||||
|
||||
UASSERT(uContains(gridParameters, rtabmap::Parameters::kGridMapFrameProjection()));
|
||||
mapFrameProjection_ = uStr2Bool(gridParameters.at(rtabmap::Parameters::kGridMapFrameProjection()));
|
||||
if(mapFrameProjection_ && mapFrameId_.empty())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "obstacles_detection: Parameter \"%s\" is true but map_frame_id is not set!", rtabmap::Parameters::kGridMapFrameProjection().c_str());
|
||||
}
|
||||
|
||||
grid_.parseParameters(gridParameters);
|
||||
|
||||
tfBuffer_ = std::make_shared< tf2_ros::Buffer >(this->get_clock());
|
||||
tfListener_ = std::make_shared< tf2_ros::TransformListener >(*tfBuffer_);
|
||||
|
||||
groundPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("ground", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
|
||||
obstaclesPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("obstacles", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
|
||||
projObstaclesPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("proj_obstacles", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
|
||||
|
||||
cloudSub_ = create_subscription<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&ObstaclesDetection::callback, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
|
||||
|
||||
void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg)
|
||||
{
|
||||
rclcpp::Time time = now();
|
||||
|
||||
if (groundPub_->get_subscription_count() == 0 && obstaclesPub_->get_subscription_count() == 0 && projObstaclesPub_->get_subscription_count() == 0)
|
||||
{
|
||||
// no one wants the results
|
||||
return;
|
||||
}
|
||||
|
||||
rtabmap::Transform localTransform = rtabmap::Transform::getIdentity();
|
||||
localTransform = rtabmap_conversions::getTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, *tfBuffer_, waitForTransform_);
|
||||
if(localTransform.isNull())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to get transform between %s and %s frames", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
rtabmap::Transform pose = rtabmap::Transform::getIdentity();
|
||||
if(!mapFrameId_.empty())
|
||||
{
|
||||
pose = rtabmap_conversions::getTransform(mapFrameId_, frameId_, cloudMsg->header.stamp, *tfBuffer_, waitForTransform_);
|
||||
if(pose.isNull())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to get transform between %s and %s frames", mapFrameId_.c_str(), frameId_.c_str());
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height,
|
||||
uFormat("data=%d row_step=%d height=%d", cloudMsg->data.size(), cloudMsg->row_step, cloudMsg->height).c_str());
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
pcl::fromROSMsg(*cloudMsg, *inputCloud);
|
||||
if(inputCloud->isOrganized())
|
||||
{
|
||||
std::vector<int> indices;
|
||||
pcl::removeNaNFromPointCloud(*inputCloud, *inputCloud, indices);
|
||||
}
|
||||
else if(!inputCloud->is_dense && inputCloud->height == 1)
|
||||
{
|
||||
if(!warned_)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "Detected possible wrong format of point cloud \"%s\", it is "
|
||||
"indicated that it is not dense, but there is only one row. "
|
||||
"Assuming it is dense... This message will only appear once.", cloudSub_->get_topic_name());
|
||||
warned_ = true;
|
||||
}
|
||||
inputCloud->is_dense = true;
|
||||
}
|
||||
|
||||
//Common variables for all strategies
|
||||
pcl::IndicesPtr ground, obstacles;
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloudWithoutFlatSurfaces(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
|
||||
if(inputCloud->size())
|
||||
{
|
||||
inputCloud = rtabmap::util3d::transformPointCloud(inputCloud, localTransform);
|
||||
|
||||
pcl::IndicesPtr flatObstacles(new std::vector<int>);
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = grid_.segmentCloud<pcl::PointXYZ>(
|
||||
inputCloud,
|
||||
pcl::IndicesPtr(new std::vector<int>),
|
||||
pose,
|
||||
cv::Point3f(localTransform.x(), localTransform.y(), localTransform.z()),
|
||||
ground,
|
||||
obstacles,
|
||||
&flatObstacles);
|
||||
|
||||
if(cloud->size() && ((ground.get() && ground->size()) || (obstacles.get() && obstacles->size())))
|
||||
{
|
||||
if(groundPub_->get_subscription_count() &&
|
||||
ground.get() && ground->size())
|
||||
{
|
||||
pcl::copyPointCloud(*cloud, *ground, *groundCloud);
|
||||
}
|
||||
|
||||
if((obstaclesPub_->get_subscription_count() || projObstaclesPub_->get_subscription_count()) &&
|
||||
obstacles.get() && obstacles->size())
|
||||
{
|
||||
// remove flat obstacles from obstacles
|
||||
std::set<int> flatObstaclesSet;
|
||||
if(projObstaclesPub_->get_subscription_count())
|
||||
{
|
||||
flatObstaclesSet.insert(flatObstacles->begin(), flatObstacles->end());
|
||||
}
|
||||
|
||||
obstaclesCloud->resize(obstacles->size());
|
||||
obstaclesCloudWithoutFlatSurfaces->resize(obstacles->size());
|
||||
|
||||
int oi=0;
|
||||
for(unsigned int i=0; i<obstacles->size(); ++i)
|
||||
{
|
||||
obstaclesCloud->points[i] = cloud->at(obstacles->at(i));
|
||||
if(flatObstaclesSet.size() == 0 ||
|
||||
flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end())
|
||||
{
|
||||
obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i];
|
||||
obstaclesCloudWithoutFlatSurfaces->points[oi].z = 0;
|
||||
++oi;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
obstaclesCloudWithoutFlatSurfaces->resize(oi);
|
||||
}
|
||||
|
||||
if(!localTransform.isIdentity() || !pose.isIdentity())
|
||||
{
|
||||
//transform back in topic frame for 3d clouds and base frame for 2d clouds
|
||||
|
||||
float roll, pitch, yaw;
|
||||
pose.getEulerAngles(roll, pitch, yaw);
|
||||
rtabmap::Transform t = rtabmap::Transform(0,0, mapFrameProjection_?pose.z():0, roll, pitch, 0);
|
||||
|
||||
if(obstaclesCloudWithoutFlatSurfaces->size() && !pose.isIdentity())
|
||||
{
|
||||
obstaclesCloudWithoutFlatSurfaces = rtabmap::util3d::transformPointCloud(obstaclesCloudWithoutFlatSurfaces, t.inverse());
|
||||
}
|
||||
|
||||
t = (t*localTransform).inverse();
|
||||
if(groundCloud->size())
|
||||
{
|
||||
groundCloud = rtabmap::util3d::transformPointCloud(groundCloud, t);
|
||||
}
|
||||
if(obstaclesCloud->size())
|
||||
{
|
||||
obstaclesCloud = rtabmap::util3d::transformPointCloud(obstaclesCloud, t);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "obstacles_detection: Input cloud is empty! (%d x %d, is_dense=%d)", cloudMsg->width, cloudMsg->height, cloudMsg->is_dense?1:0);
|
||||
}
|
||||
|
||||
if(groundPub_->get_subscription_count())
|
||||
{
|
||||
sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
|
||||
pcl::toROSMsg(*groundCloud, *rosCloud);
|
||||
rosCloud->header = cloudMsg->header;
|
||||
|
||||
//publish the message
|
||||
groundPub_->publish(std::move(rosCloud));
|
||||
}
|
||||
|
||||
if(obstaclesPub_->get_subscription_count())
|
||||
{
|
||||
sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
|
||||
pcl::toROSMsg(*obstaclesCloud, *rosCloud);
|
||||
rosCloud->header = cloudMsg->header;
|
||||
|
||||
//publish the message
|
||||
obstaclesPub_->publish(std::move(rosCloud));
|
||||
}
|
||||
|
||||
if(projObstaclesPub_->get_subscription_count())
|
||||
{
|
||||
sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
|
||||
pcl::toROSMsg(*obstaclesCloudWithoutFlatSurfaces, *rosCloud);
|
||||
rosCloud->header.stamp = cloudMsg->header.stamp;
|
||||
rosCloud->header.frame_id = frameId_;
|
||||
|
||||
//publish the message
|
||||
projObstaclesPub_->publish(std::move(rosCloud));
|
||||
}
|
||||
|
||||
RCLCPP_DEBUG(this->get_logger(), "Obstacles segmentation time = %f s", (now() - time).seconds());
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#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_util::ObstaclesDetection)
|
||||
@@ -0,0 +1,419 @@
|
||||
/*
|
||||
Copyright (c) 2010-2019, 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_util/point_cloud_aggregator.hpp>
|
||||
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
#include <rtabmap/core/util3d_filtering.h>
|
||||
|
||||
namespace rtabmap_util
|
||||
{
|
||||
|
||||
PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options) :
|
||||
Node("point_cloud_aggregator", options),
|
||||
warningThread_(0),
|
||||
callbackCalled_(false),
|
||||
exactSync4_(0),
|
||||
approxSync4_(0),
|
||||
exactSync3_(0),
|
||||
approxSync3_(0),
|
||||
exactSync2_(0),
|
||||
approxSync2_(0),
|
||||
waitForTransform_(0.1),
|
||||
xyzOutput_(false)
|
||||
{
|
||||
tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
|
||||
//auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
|
||||
// this->get_node_base_interface(),
|
||||
// this->get_node_timers_interface());
|
||||
//tfBuffer_->setCreateTimerInterface(timer_interface);
|
||||
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
|
||||
|
||||
int queueSize = 5;
|
||||
int count = 2;
|
||||
bool approx=true;
|
||||
double approxSyncMaxInterval = 0.0;
|
||||
int qos=0;
|
||||
queueSize = this->declare_parameter("queue_size", queueSize);
|
||||
qos = this->declare_parameter("qos", qos);
|
||||
frameId_ = this->declare_parameter("frame_id", frameId_);
|
||||
fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
|
||||
approx = this->declare_parameter("approx_sync", approx);
|
||||
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
|
||||
count = this->declare_parameter("count", count);
|
||||
waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
|
||||
xyzOutput_ = this->declare_parameter("xyz_output", xyzOutput_);
|
||||
|
||||
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("combined_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
|
||||
|
||||
cloudSub_1_.subscribe(this, "cloud1", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
cloudSub_2_.subscribe(this, "cloud2", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
|
||||
std::string subscribedTopicsMsg;
|
||||
if(count == 4)
|
||||
{
|
||||
cloudSub_3_.subscribe(this, "cloud3", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
cloudSub_4_.subscribe(this, "cloud4", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
if(approx)
|
||||
{
|
||||
approxSync4_ = new message_filters::Synchronizer<ApproxSync4Policy>(ApproxSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
|
||||
if(approxSyncMaxInterval > 0.0)
|
||||
approxSync4_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
approxSync4_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds4_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
|
||||
}
|
||||
else
|
||||
{
|
||||
exactSync4_ = new message_filters::Synchronizer<ExactSync4Policy>(ExactSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
|
||||
exactSync4_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds4_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(),
|
||||
approx?"approx":"exact",
|
||||
approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
|
||||
cloudSub_1_.getTopic().c_str(),
|
||||
cloudSub_2_.getTopic().c_str(),
|
||||
cloudSub_3_.getTopic().c_str(),
|
||||
cloudSub_4_.getTopic().c_str());
|
||||
}
|
||||
else if(count == 3)
|
||||
{
|
||||
cloudSub_3_.subscribe(this, "cloud3", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
if(approx)
|
||||
{
|
||||
approxSync3_ = new message_filters::Synchronizer<ApproxSync3Policy>(ApproxSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
|
||||
if(approxSyncMaxInterval > 0.0)
|
||||
approxSync3_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
approxSync3_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds3_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
}
|
||||
else
|
||||
{
|
||||
exactSync3_ = new message_filters::Synchronizer<ExactSync3Policy>(ExactSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
|
||||
exactSync3_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds3_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",
|
||||
this->get_name(),
|
||||
approx?"approx":"exact",
|
||||
approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
|
||||
cloudSub_1_.getTopic().c_str(),
|
||||
cloudSub_2_.getTopic().c_str(),
|
||||
cloudSub_3_.getTopic().c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
if(approx)
|
||||
{
|
||||
approxSync2_ = new message_filters::Synchronizer<ApproxSync2Policy>(ApproxSync2Policy(queueSize), cloudSub_1_, cloudSub_2_);
|
||||
if(approxSyncMaxInterval > 0.0)
|
||||
approxSync2_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
approxSync2_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds2_callback, this, std::placeholders::_1, std::placeholders::_2));
|
||||
}
|
||||
else
|
||||
{
|
||||
exactSync2_ = new message_filters::Synchronizer<ExactSync2Policy>(ExactSync2Policy(queueSize), cloudSub_1_, cloudSub_2_);
|
||||
exactSync2_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds2_callback, this, std::placeholders::_1, std::placeholders::_2));
|
||||
}
|
||||
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s",
|
||||
this->get_name(),
|
||||
approx?"approx":"exact",
|
||||
approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
|
||||
cloudSub_1_.getTopic().c_str(),
|
||||
cloudSub_2_.getTopic().c_str());
|
||||
}
|
||||
|
||||
|
||||
warningThread_ = new std::thread([&](){
|
||||
rclcpp::Rate r(1.0/5.0);
|
||||
while(!callbackCalled_)
|
||||
{
|
||||
r.sleep();
|
||||
if(!callbackCalled_)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "%s: Did not receive data since 5 seconds! Make sure the input topics are "
|
||||
"published (\"$ rostopic hz my_topic\") and the timestamps in their "
|
||||
"header are set. %s%s",
|
||||
this->get_name(),
|
||||
approx?"":"Parameter \"approx_sync\" is false, which means that input "
|
||||
"topics should have all the exact timestamp for the callback to be called.",
|
||||
subscribedTopicsMsg.c_str());
|
||||
}
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
PointCloudAggregator::~PointCloudAggregator()
|
||||
{
|
||||
delete exactSync4_;
|
||||
delete approxSync4_;
|
||||
delete exactSync3_;
|
||||
delete approxSync3_;
|
||||
delete exactSync2_;
|
||||
delete approxSync2_;
|
||||
|
||||
if(warningThread_)
|
||||
{
|
||||
callbackCalled_=true;
|
||||
warningThread_->join();
|
||||
delete warningThread_;
|
||||
}
|
||||
}
|
||||
|
||||
void PointCloudAggregator::clouds4_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_1,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_2,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_3,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_4)
|
||||
{
|
||||
std::vector<sensor_msgs::msg::PointCloud2::ConstSharedPtr> clouds;
|
||||
clouds.push_back(cloudMsg_1);
|
||||
clouds.push_back(cloudMsg_2);
|
||||
clouds.push_back(cloudMsg_3);
|
||||
clouds.push_back(cloudMsg_4);
|
||||
|
||||
combineClouds(clouds);
|
||||
}
|
||||
void PointCloudAggregator::clouds3_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_1,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_2,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_3)
|
||||
{
|
||||
std::vector<sensor_msgs::msg::PointCloud2::ConstSharedPtr> clouds;
|
||||
clouds.push_back(cloudMsg_1);
|
||||
clouds.push_back(cloudMsg_2);
|
||||
clouds.push_back(cloudMsg_3);
|
||||
|
||||
combineClouds(clouds);
|
||||
}
|
||||
void PointCloudAggregator::clouds2_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_1,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_2)
|
||||
{
|
||||
std::vector<sensor_msgs::msg::PointCloud2::ConstSharedPtr> clouds;
|
||||
clouds.push_back(cloudMsg_1);
|
||||
clouds.push_back(cloudMsg_2);
|
||||
|
||||
combineClouds(clouds);
|
||||
}
|
||||
void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::PointCloud2::ConstSharedPtr> & cloudMsgs)
|
||||
{
|
||||
callbackCalled_ = true;
|
||||
UASSERT(cloudMsgs.size() > 1);
|
||||
if(cloudPub_->get_subscription_count())
|
||||
{
|
||||
pcl::PCLPointCloud2::Ptr output(new pcl::PCLPointCloud2);
|
||||
|
||||
std::string frameId = frameId_;
|
||||
if(!frameId.empty() && frameId.compare(cloudMsgs[0]->header.frame_id) != 0)
|
||||
{
|
||||
sensor_msgs::msg::PointCloud2 tmp;
|
||||
rtabmap::Transform t = rtabmap_conversions::getTransform(frameId, cloudMsgs[0]->header.frame_id, cloudMsgs[0]->header.stamp, *tfBuffer_, waitForTransform_);
|
||||
if(t.isNull())
|
||||
{
|
||||
return;
|
||||
}
|
||||
rtabmap_conversions::transformPointCloud(t.toEigen4f(), *cloudMsgs[0], tmp);
|
||||
pcl_conversions::toPCL(tmp, *output);
|
||||
}
|
||||
else
|
||||
{
|
||||
pcl_conversions::toPCL(*cloudMsgs[0], *output);
|
||||
frameId = cloudMsgs[0]->header.frame_id;
|
||||
}
|
||||
|
||||
if(xyzOutput_ && !output->data.empty())
|
||||
{
|
||||
// convert only if not already XYZ cloud
|
||||
bool hasField[4] = {false};
|
||||
for(size_t i=0; i<output->fields.size(); ++i)
|
||||
{
|
||||
if(output->fields[i].name.compare("x") == 0)
|
||||
{
|
||||
hasField[0] = true;
|
||||
}
|
||||
else if(output->fields[i].name.compare("y") == 0)
|
||||
{
|
||||
hasField[1] = true;
|
||||
}
|
||||
else if(output->fields[i].name.compare("z") == 0)
|
||||
{
|
||||
hasField[2] = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
hasField[3] = true; // other
|
||||
break;
|
||||
}
|
||||
}
|
||||
if(hasField[0] && hasField[1] && hasField[2] && !hasField[3])
|
||||
{
|
||||
// do nothing, already XYZ
|
||||
}
|
||||
else
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ> cloudxyz;
|
||||
pcl::fromPCLPointCloud2(*output, cloudxyz);
|
||||
pcl::toPCLPointCloud2(cloudxyz, *output);
|
||||
}
|
||||
}
|
||||
|
||||
for(unsigned int i=1; i<cloudMsgs.size(); ++i)
|
||||
{
|
||||
rtabmap::Transform cloudDisplacement;
|
||||
if(!fixedFrameId_.empty() &&
|
||||
cloudMsgs[0]->header.stamp != cloudMsgs[i]->header.stamp)
|
||||
{
|
||||
// approx sync
|
||||
cloudDisplacement = rtabmap_conversions::getTransform(
|
||||
frameId, //sourceTargetFrame
|
||||
fixedFrameId_, //fixedFrame
|
||||
cloudMsgs[i]->header.stamp, //stampSource
|
||||
cloudMsgs[0]->header.stamp, //stampTarget
|
||||
*tfBuffer_,
|
||||
waitForTransform_);
|
||||
}
|
||||
|
||||
pcl::PCLPointCloud2::Ptr cloud2(new pcl::PCLPointCloud2);
|
||||
if(frameId.compare(cloudMsgs[i]->header.frame_id) != 0)
|
||||
{
|
||||
sensor_msgs::msg::PointCloud2 tmp;
|
||||
rtabmap::Transform t = rtabmap_conversions::getTransform(frameId, cloudMsgs[i]->header.frame_id, cloudMsgs[i]->header.stamp, *tfBuffer_, waitForTransform_);
|
||||
rtabmap_conversions::transformPointCloud(t.toEigen4f(), *cloudMsgs[i], tmp);
|
||||
if(!cloudDisplacement.isNull())
|
||||
{
|
||||
sensor_msgs::msg::PointCloud2 tmp2;
|
||||
rtabmap_conversions::transformPointCloud(cloudDisplacement.toEigen4f(), tmp, tmp2);
|
||||
pcl_conversions::toPCL(tmp2, *cloud2);
|
||||
}
|
||||
else
|
||||
{
|
||||
pcl_conversions::toPCL(tmp, *cloud2);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(!cloudDisplacement.isNull())
|
||||
{
|
||||
sensor_msgs::msg::PointCloud2 tmp;
|
||||
rtabmap_conversions::transformPointCloud(cloudDisplacement.toEigen4f(), *cloudMsgs[i], tmp);
|
||||
pcl_conversions::toPCL(tmp, *cloud2);
|
||||
}
|
||||
else
|
||||
{
|
||||
pcl_conversions::toPCL(*cloudMsgs[i], *cloud2);
|
||||
}
|
||||
}
|
||||
|
||||
if(!cloud2->is_dense)
|
||||
{
|
||||
// remove nans
|
||||
cloud2 = rtabmap::util3d::removeNaNFromPointCloud(cloud2);
|
||||
}
|
||||
|
||||
if(xyzOutput_ && !cloud2->data.empty())
|
||||
{
|
||||
// convert only if not already XYZ cloud
|
||||
bool hasField[4] = {false};
|
||||
for(size_t i=0; i<cloud2->fields.size(); ++i)
|
||||
{
|
||||
if(cloud2->fields[i].name.compare("x") == 0)
|
||||
{
|
||||
hasField[0] = true;
|
||||
}
|
||||
else if(cloud2->fields[i].name.compare("y") == 0)
|
||||
{
|
||||
hasField[1] = true;
|
||||
}
|
||||
else if(cloud2->fields[i].name.compare("z") == 0)
|
||||
{
|
||||
hasField[2] = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
hasField[3] = true; // other
|
||||
break;
|
||||
}
|
||||
}
|
||||
if(hasField[0] && hasField[1] && hasField[2] && !hasField[3])
|
||||
{
|
||||
// do nothing, already XYZ
|
||||
}
|
||||
else
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ> cloudxyz;
|
||||
pcl::fromPCLPointCloud2(*cloud2, cloudxyz);
|
||||
pcl::toPCLPointCloud2(cloudxyz, *cloud2);
|
||||
}
|
||||
}
|
||||
|
||||
if(output->data.empty())
|
||||
{
|
||||
output = cloud2;
|
||||
}
|
||||
else if(!cloud2->data.empty())
|
||||
{
|
||||
|
||||
if(output->fields.size() != cloud2->fields.size())
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "%s: Input topics don't have all the "
|
||||
"same number of fields (cloud1=%d, cloud%d=%d), concatenation "
|
||||
"may fails. You can enable \"xyz_output\" option "
|
||||
"to convert all inputs to XYZ.",
|
||||
get_name(),
|
||||
(int)output->fields.size(),
|
||||
i+1,
|
||||
(int)output->fields.size());
|
||||
}
|
||||
|
||||
pcl::PCLPointCloud2::Ptr tmp_output(new pcl::PCLPointCloud2);
|
||||
#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
|
||||
pcl::concatenate(*output, *cloud2, *tmp_output);
|
||||
#else
|
||||
pcl::concatenatePointCloud(*output, *cloud2, *tmp_output);
|
||||
#endif
|
||||
//Make sure row_step is the sum of both
|
||||
tmp_output->row_step = tmp_output->width * tmp_output->point_step;
|
||||
output = tmp_output;
|
||||
}
|
||||
}
|
||||
|
||||
sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
|
||||
pcl_conversions::moveFromPCL(*output, *rosCloud);
|
||||
rosCloud->header.stamp = cloudMsgs[0]->header.stamp;
|
||||
rosCloud->header.frame_id = frameId;
|
||||
cloudPub_->publish(std::move(rosCloud));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#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_util::PointCloudAggregator)
|
||||
@@ -0,0 +1,521 @@
|
||||
/*
|
||||
Copyright (c) 2010-2019, 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_util/point_cloud_assembler.hpp>
|
||||
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <pcl/io/pcd_io.h>
|
||||
#include <pcl/filters/voxel_grid.h>
|
||||
#include <pcl/filters/radius_outlier_removal.h>
|
||||
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
#include <rtabmap_msgs/msg/odom_info.hpp>
|
||||
#include <rtabmap/core/util3d.h>
|
||||
#include <rtabmap/core/util3d_filtering.h>
|
||||
#include <rtabmap/core/Version.h>
|
||||
|
||||
namespace rtabmap_util
|
||||
{
|
||||
|
||||
PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
|
||||
Node("point_cloud_assembler", options),
|
||||
warningThread_(0),
|
||||
callbackCalled_(false),
|
||||
exactSync_(0),
|
||||
exactInfoSync_(0),
|
||||
maxClouds_(0),
|
||||
skipClouds_(0),
|
||||
cloudsSkipped_(0),
|
||||
circularBuffer_(false),
|
||||
linearUpdate_(0),
|
||||
angularUpdate_(0),
|
||||
assemblingTime_(0),
|
||||
waitForTransform_(0.1),
|
||||
rangeMin_(0),
|
||||
rangeMax_(0),
|
||||
voxelSize_(0),
|
||||
noiseRadius_(0),
|
||||
noiseMinNeighbors_(5),
|
||||
removeZ_(false),
|
||||
fixedFrameId_("odom"),
|
||||
frameId_("")
|
||||
{
|
||||
tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
|
||||
//auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
|
||||
// this->get_node_base_interface(),
|
||||
// this->get_node_timers_interface());
|
||||
//tfBuffer_->setCreateTimerInterface(timer_interface);
|
||||
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
|
||||
|
||||
int queueSize = 5;
|
||||
int qos = 0;
|
||||
bool subscribeOdomInfo = false;
|
||||
|
||||
queueSize = this->declare_parameter("queue_size", queueSize);
|
||||
qos = this->declare_parameter("qos", qos);
|
||||
int qosOdom = this->declare_parameter("qos_odom", qos);
|
||||
fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
|
||||
frameId_ = this->declare_parameter("frame_id", frameId_);
|
||||
maxClouds_ = this->declare_parameter("max_clouds", maxClouds_);
|
||||
assemblingTime_ = this->declare_parameter("assembling_time", assemblingTime_);
|
||||
skipClouds_ = this->declare_parameter("skip_clouds", skipClouds_);
|
||||
circularBuffer_ = this->declare_parameter("circular_buffer", circularBuffer_);
|
||||
linearUpdate_ = this->declare_parameter("linear_update", linearUpdate_);
|
||||
angularUpdate_ = this->declare_parameter("angular_update", angularUpdate_);
|
||||
waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
|
||||
rangeMin_ = this->declare_parameter("range_min", rangeMin_);
|
||||
rangeMax_ = this->declare_parameter("range_max", rangeMax_);
|
||||
voxelSize_ = this->declare_parameter("voxel_size", voxelSize_);
|
||||
noiseRadius_ = this->declare_parameter("noise_radius", noiseRadius_);
|
||||
noiseMinNeighbors_ = this->declare_parameter("noise_min_neighbors", noiseMinNeighbors_);
|
||||
removeZ_ = this->declare_parameter("remove_z", removeZ_);
|
||||
subscribeOdomInfo = this->declare_parameter("subscribe_odom_info", subscribeOdomInfo);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "%s: queue_size=%d", get_name(), queueSize);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: qos=%d", get_name(), qos);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: qos_odom=%d", get_name(), qosOdom);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: fixed_frame_id=%s", get_name(), fixedFrameId_.c_str());
|
||||
RCLCPP_INFO(this->get_logger(), "%s: frame_id=%s", get_name(), frameId_.c_str());
|
||||
RCLCPP_INFO(this->get_logger(), "%s: max_clouds=%d", get_name(), maxClouds_);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: assembling_time=%fs", get_name(), assemblingTime_);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: skip_clouds=%d", get_name(), skipClouds_);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: circular_buffer=%s", get_name(), circularBuffer_?"true":"false");
|
||||
RCLCPP_INFO(this->get_logger(), "%s: linear_update=%f m", get_name(), linearUpdate_);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: angular_update=%f rad", get_name(), angularUpdate_);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: wait_for_transform=%f", get_name(), waitForTransform_);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: range_min=%f", get_name(), rangeMin_);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: range_max=%f", get_name(), rangeMax_);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: voxel_size=%fm", get_name(), voxelSize_);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: noise_radius=%fm", get_name(), noiseRadius_);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: noise_min_neighbors=%d", get_name(), noiseMinNeighbors_);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: remove_z=%s", get_name(), removeZ_?"true":"false");
|
||||
|
||||
if(maxClouds_==0 && assemblingTime_ ==0.0)
|
||||
{
|
||||
RCLCPP_ERROR(get_logger(), "point_cloud_assembler: max_cloud or assembling_time parameters should be set!");
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
cloudsSkipped_ = skipClouds_;
|
||||
|
||||
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("assembled_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
|
||||
|
||||
if(!fixedFrameId_.empty())
|
||||
{
|
||||
cloudSub_ = create_subscription<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&PointCloudAssembler::callbackCloud, this, std::placeholders::_1));
|
||||
subscribedTopicsMsg_ = uFormat("\n%s subscribed to %s",
|
||||
get_name(),
|
||||
cloudSub_->get_topic_name());
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
syncCloudSub_.subscribe(this, "cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
syncOdomSub_.subscribe(this, "odom", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosOdom).get_rmw_qos_profile());
|
||||
syncOdomInfoSub_.subscribe(this, "odom_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosOdom).get_rmw_qos_profile());
|
||||
exactInfoSync_ = new message_filters::Synchronizer<syncInfoPolicy>(syncInfoPolicy(queueSize), syncCloudSub_, syncOdomSub_, syncOdomInfoSub_);
|
||||
exactInfoSync_->registerCallback(std::bind(&rtabmap_util::PointCloudAssembler::callbackCloudOdomInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (exact sync):\n %s,\n %s",
|
||||
get_name(),
|
||||
syncCloudSub_.getTopic().c_str(),
|
||||
syncOdomSub_.getTopic().c_str(),
|
||||
syncOdomInfoSub_.getTopic().c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
syncCloudSub_.subscribe(this, "cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
syncOdomSub_.subscribe(this, "odom", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosOdom).get_rmw_qos_profile());
|
||||
exactSync_ = new message_filters::Synchronizer<syncPolicy>(syncPolicy(queueSize), syncCloudSub_, syncOdomSub_);
|
||||
exactSync_->registerCallback(std::bind(&rtabmap_util::PointCloudAssembler::callbackCloudOdom, this, std::placeholders::_1, std::placeholders::_2));
|
||||
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (exact sync):\n %s,\n %s",
|
||||
get_name(),
|
||||
syncCloudSub_.getTopic().c_str(),
|
||||
syncOdomSub_.getTopic().c_str());
|
||||
}
|
||||
|
||||
warningThread_ = new std::thread([&](){
|
||||
rclcpp::Rate r(1.0/5.0);
|
||||
while(!callbackCalled_)
|
||||
{
|
||||
r.sleep();
|
||||
if(!callbackCalled_)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(),
|
||||
"%s: Did not receive data since 5 seconds! Make sure the input topics are "
|
||||
"published (\"$ rostopic hz my_topic\") and the timestamps in their "
|
||||
"header are set. %s",
|
||||
get_name(),
|
||||
subscribedTopicsMsg_.c_str());
|
||||
}
|
||||
}
|
||||
});
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg_.c_str());
|
||||
}
|
||||
|
||||
PointCloudAssembler::~PointCloudAssembler()
|
||||
{
|
||||
delete exactSync_;
|
||||
delete exactInfoSync_;
|
||||
|
||||
if(warningThread_)
|
||||
{
|
||||
callbackCalled_=true;
|
||||
warningThread_->join();
|
||||
delete warningThread_;
|
||||
}
|
||||
}
|
||||
|
||||
void PointCloudAssembler::callbackCloudOdom(
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg,
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg)
|
||||
{
|
||||
callbackCalled_ = true;
|
||||
rtabmap::Transform odom = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose);
|
||||
if(!odom.isNull())
|
||||
{
|
||||
fixedFrameId_ = odomMsg->header.frame_id;
|
||||
callbackCloud(cloudMsg);
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "Reseting point cloud assembler as null odometry has been received.");
|
||||
clouds_.clear();
|
||||
}
|
||||
}
|
||||
|
||||
sensor_msgs::msg::PointCloud2 removeField(const sensor_msgs::msg::PointCloud2 & input, const std::string & field)
|
||||
{
|
||||
sensor_msgs::msg::PointCloud2 output;
|
||||
int offset = 0;
|
||||
std::vector<int> inputFieldIndex;
|
||||
for(size_t i=0; i<input.fields.size(); ++i)
|
||||
{
|
||||
if(input.fields[i].name.compare(field) == 0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
else
|
||||
{
|
||||
sensor_msgs::msg::PointField outputField = input.fields[i];
|
||||
outputField.offset = offset;
|
||||
offset += outputField.count * rtabmap_conversions::sizeOfPointField(outputField.datatype);
|
||||
output.fields.push_back(outputField);
|
||||
inputFieldIndex.push_back(i);
|
||||
}
|
||||
}
|
||||
output.header = input.header;
|
||||
output.height = input.height;
|
||||
output.width = input.width;
|
||||
output.is_bigendian = input.is_bigendian;
|
||||
output.is_dense = input.is_dense;
|
||||
output.point_step = offset;
|
||||
output.row_step = output.width * output.point_step;
|
||||
output.data.resize(output.height*output.row_step);
|
||||
int total = output.height*output.width;
|
||||
for(int i=0; i<total; ++i)
|
||||
{
|
||||
// for each point, copy fields
|
||||
int oi = i*output.point_step;
|
||||
int pi = i*input.point_step;
|
||||
for(size_t j=0;j<output.fields.size(); ++j)
|
||||
{
|
||||
memcpy(&output.data[oi + output.fields[j].offset],
|
||||
&input.data[pi + input.fields[inputFieldIndex[j]].offset],
|
||||
output.fields[j].count * rtabmap_conversions::sizeOfPointField(output.fields[j].datatype));
|
||||
}
|
||||
}
|
||||
return output;
|
||||
}
|
||||
|
||||
void PointCloudAssembler::callbackCloudOdomInfo(
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg,
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
callbackCalled_ = true;
|
||||
rtabmap::Transform odom = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose);
|
||||
if(!odom.isNull())
|
||||
{
|
||||
if(odomInfoMsg->key_frame_added)
|
||||
{
|
||||
fixedFrameId_ = odomMsg->header.frame_id;
|
||||
callbackCloud(cloudMsg);
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "Skipping non keyframe...");
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "Reseting point cloud assembler as null odometry has been received.");
|
||||
clouds_.clear();
|
||||
}
|
||||
}
|
||||
|
||||
void PointCloudAssembler::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg)
|
||||
{
|
||||
if(cloudPub_->get_subscription_count())
|
||||
{
|
||||
UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height,
|
||||
uFormat("data=%d row_step=%d height=%d", cloudMsg->data.size(), cloudMsg->row_step, cloudMsg->height).c_str());
|
||||
|
||||
if(skipClouds_<=0 || cloudsSkipped_ >= skipClouds_)
|
||||
{
|
||||
cloudsSkipped_ = 0;
|
||||
|
||||
rtabmap::Transform pose = rtabmap_conversions::getTransform(
|
||||
fixedFrameId_, //fromFrame
|
||||
cloudMsg->header.frame_id, //toFrame
|
||||
cloudMsg->header.stamp,
|
||||
*tfBuffer_,
|
||||
waitForTransform_);
|
||||
|
||||
if(pose.isNull())
|
||||
{
|
||||
RCLCPP_ERROR(get_logger(), "Cloud not transform all clouds! Resetting...");
|
||||
clouds_.clear();
|
||||
return;
|
||||
}
|
||||
|
||||
bool isMoving = true;
|
||||
if(!previousPose_.isNull() && (linearUpdate_>0 || angularUpdate_>0))
|
||||
{
|
||||
rtabmap::Transform delta = previousPose_.inverse()*pose;
|
||||
float roll, pitch, yaw;
|
||||
delta.getEulerAngles(roll, pitch, yaw);
|
||||
isMoving = fabs(delta.x()) > linearUpdate_ ||
|
||||
fabs(delta.y()) > linearUpdate_ ||
|
||||
fabs(delta.z()) > linearUpdate_ ||
|
||||
(angularUpdate_>0.0f && (
|
||||
fabs(roll) > angularUpdate_ ||
|
||||
fabs(pitch) > angularUpdate_ ||
|
||||
fabs(yaw) > angularUpdate_));
|
||||
}
|
||||
|
||||
pcl::PCLPointCloud2::Ptr newCloud(new pcl::PCLPointCloud2);
|
||||
if(rangeMin_ > 0.0 || rangeMax_ > 0.0 || voxelSize_ > 0.0f)
|
||||
{
|
||||
pcl_conversions::toPCL(*cloudMsg, *newCloud);
|
||||
rtabmap::LaserScan scan = rtabmap::util3d::laserScanFromPointCloud(*newCloud);
|
||||
scan = rtabmap::util3d::commonFiltering(scan, 1, rangeMin_, rangeMax_, voxelSize_);
|
||||
#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
|
||||
std::uint64_t stamp = newCloud->header.stamp;
|
||||
#else
|
||||
pcl::uint64_t stamp = newCloud->header.stamp;
|
||||
#endif
|
||||
newCloud = rtabmap::util3d::laserScanToPointCloud2(scan, pose);
|
||||
newCloud->header.stamp = stamp;
|
||||
}
|
||||
else
|
||||
{
|
||||
sensor_msgs::msg::PointCloud2 output;
|
||||
rtabmap_conversions::transformPointCloud(pose.toEigen4f(), *cloudMsg, output);
|
||||
pcl_conversions::toPCL(output, *newCloud);
|
||||
}
|
||||
|
||||
if(!newCloud->is_dense)
|
||||
{
|
||||
// remove nans
|
||||
newCloud = rtabmap::util3d::removeNaNFromPointCloud(newCloud);
|
||||
}
|
||||
|
||||
clouds_.push_back(newCloud);
|
||||
|
||||
#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
|
||||
bool reachedMaxSize =
|
||||
((int)clouds_.size() >= maxClouds_ && maxClouds_ > 0)
|
||||
||
|
||||
((*newCloud).header.stamp >= clouds_.front()->header.stamp + static_cast<std::uint64_t>(assemblingTime_*1000000.0) && assemblingTime_ > 0.0);
|
||||
#else
|
||||
bool reachedMaxSize =
|
||||
((int)clouds_.size() >= maxClouds_ && maxClouds_ > 0)
|
||||
||
|
||||
((*newCloud).header.stamp >= clouds_.front()->header.stamp + static_cast<pcl::uint64_t>(assemblingTime_*1000000.0) && assemblingTime_ > 0.0);
|
||||
#endif
|
||||
|
||||
if( circularBuffer_ || reachedMaxSize )
|
||||
{
|
||||
pcl::PCLPointCloud2Ptr assembled(new pcl::PCLPointCloud2);
|
||||
for(std::list<pcl::PCLPointCloud2::Ptr>::iterator iter=clouds_.begin(); iter!=clouds_.end(); ++iter)
|
||||
{
|
||||
if(assembled->data.empty())
|
||||
{
|
||||
*assembled = *(*iter);
|
||||
}
|
||||
else
|
||||
{
|
||||
pcl::PCLPointCloud2Ptr assembledTmp(new pcl::PCLPointCloud2);
|
||||
#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
|
||||
pcl::concatenate(*assembled, *(*iter), *assembledTmp);
|
||||
#else
|
||||
pcl::concatenatePointCloud(*assembled, *(*iter), *assembledTmp);
|
||||
#endif
|
||||
//Make sure row_step is the sum of both
|
||||
assembledTmp->row_step = assembled->row_step + (*iter)->row_step;
|
||||
assembled = assembledTmp;
|
||||
}
|
||||
}
|
||||
|
||||
sensor_msgs::msg::PointCloud2 rosCloud;
|
||||
if(voxelSize_>0.0)
|
||||
{
|
||||
// estimate if there would be an overflow
|
||||
int x_idx=-1, y_idx=-1, z_idx=-1;
|
||||
for (std::size_t d = 0; d < assembled->fields.size (); ++d)
|
||||
{
|
||||
if (assembled->fields[d].name.compare("x")==0)
|
||||
x_idx = d;
|
||||
if (assembled->fields[d].name.compare("y")==0)
|
||||
y_idx = d;
|
||||
if (assembled->fields[d].name.compare("z")==0)
|
||||
z_idx = d;
|
||||
}
|
||||
bool overflow = false;
|
||||
if(x_idx>=0 && y_idx>=0 && z_idx>=0) {
|
||||
Eigen::Vector4f min_p, max_p;
|
||||
pcl::getMinMax3D(assembled, x_idx, y_idx, z_idx, min_p, max_p);
|
||||
float inverseVoxelSize = 1.0f/voxelSize_;
|
||||
std::int64_t dx = static_cast<std::int64_t>((max_p[0] - min_p[0]) * inverseVoxelSize)+1;
|
||||
std::int64_t dy = static_cast<std::int64_t>((max_p[1] - min_p[1]) * inverseVoxelSize)+1;
|
||||
std::int64_t dz = static_cast<std::int64_t>((max_p[2] - min_p[2]) * inverseVoxelSize)+1;
|
||||
|
||||
if ((dx*dy*dz) > static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::max()))
|
||||
{
|
||||
overflow = true;
|
||||
}
|
||||
}
|
||||
if(overflow)
|
||||
{
|
||||
rtabmap::LaserScan scan = rtabmap::util3d::laserScanFromPointCloud(*assembled);
|
||||
scan = rtabmap::util3d::commonFiltering(scan, 1, 0, 0, voxelSize_);
|
||||
#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
|
||||
std::uint64_t stamp = assembled->header.stamp;
|
||||
#else
|
||||
pcl::uint64_t stamp = assembled->header.stamp;
|
||||
#endif
|
||||
assembled = rtabmap::util3d::laserScanToPointCloud2(scan);
|
||||
assembled->header.stamp = stamp;
|
||||
}
|
||||
else
|
||||
{
|
||||
pcl::VoxelGrid<pcl::PCLPointCloud2> filter;
|
||||
filter.setLeafSize(voxelSize_, voxelSize_, voxelSize_);
|
||||
filter.setInputCloud(assembled);
|
||||
pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2);
|
||||
filter.filter(*output);
|
||||
assembled = output;
|
||||
}
|
||||
}
|
||||
if(noiseRadius_>0.0 && noiseMinNeighbors_>0)
|
||||
{
|
||||
pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2> filter;
|
||||
filter.setRadiusSearch(noiseRadius_);
|
||||
filter.setMinNeighborsInRadius(noiseMinNeighbors_);
|
||||
filter.setInputCloud(assembled);
|
||||
pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2);
|
||||
filter.filter(*output);
|
||||
assembled = output;
|
||||
}
|
||||
pcl_conversions::moveFromPCL(*assembled, rosCloud);
|
||||
rtabmap::Transform t = pose;
|
||||
if(!frameId_.empty())
|
||||
{
|
||||
// transform in target frame_id instead of sensor frame
|
||||
t = rtabmap_conversions::getTransform(
|
||||
fixedFrameId_, //fromFrame
|
||||
frameId_, //toFrame
|
||||
cloudMsg->header.stamp,
|
||||
*tfBuffer_,
|
||||
waitForTransform_);
|
||||
if(t.isNull())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Cloud not transform back assembled clouds in target frame \"%s\"! Resetting...", frameId_.c_str());
|
||||
clouds_.clear();
|
||||
return;
|
||||
}
|
||||
}
|
||||
rtabmap_conversions::transformPointCloud(t.toEigen4f().inverse(), rosCloud, rosCloud);
|
||||
|
||||
if(removeZ_)
|
||||
{
|
||||
rosCloud = removeField(rosCloud, "z");
|
||||
}
|
||||
|
||||
rosCloud.header = cloudMsg->header;
|
||||
if(!frameId_.empty())
|
||||
{
|
||||
rosCloud.header.frame_id = frameId_;
|
||||
}
|
||||
cloudPub_->publish(rosCloud);
|
||||
if(circularBuffer_)
|
||||
{
|
||||
if(!isMoving)
|
||||
{
|
||||
clouds_.pop_back();
|
||||
}
|
||||
else
|
||||
{
|
||||
previousPose_ = pose;
|
||||
if(reachedMaxSize)
|
||||
{
|
||||
clouds_.pop_front();
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
clouds_.clear();
|
||||
previousPose_.setNull();
|
||||
}
|
||||
}
|
||||
else if(!isMoving)
|
||||
{
|
||||
clouds_.pop_back();
|
||||
}
|
||||
else
|
||||
{
|
||||
previousPose_ = pose;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
++cloudsSkipped_;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#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_util::PointCloudAssembler)
|
||||
@@ -0,0 +1,345 @@
|
||||
/*
|
||||
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_util/point_cloud_xyz.hpp>
|
||||
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
|
||||
#include <image_geometry/pinhole_camera_model.h>
|
||||
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
|
||||
#include "rtabmap/core/util2d.h"
|
||||
#include "rtabmap/core/util3d.h"
|
||||
#include "rtabmap/core/util3d_filtering.h"
|
||||
#include "rtabmap/core/util3d_surface.h"
|
||||
#include "rtabmap/utilite/UConversion.h"
|
||||
#include "rtabmap/utilite/UStl.h"
|
||||
|
||||
namespace rtabmap_util
|
||||
{
|
||||
|
||||
PointCloudXYZ::PointCloudXYZ(const rclcpp::NodeOptions & options) :
|
||||
Node("point_cloud_xyz", options),
|
||||
maxDepth_(0.0),
|
||||
minDepth_(0.0),
|
||||
voxelSize_(0.0),
|
||||
decimation_(1),
|
||||
noiseFilterRadius_(0.0),
|
||||
noiseFilterMinNeighbors_(5),
|
||||
normalK_(0),
|
||||
normalRadius_(0.0),
|
||||
filterNaNs_(false),
|
||||
approxSyncDepth_(0),
|
||||
approxSyncDisparity_(0),
|
||||
exactSyncDepth_(0),
|
||||
exactSyncDisparity_(0)
|
||||
{
|
||||
int queueSize = 10;
|
||||
int qos = 0;
|
||||
bool approxSync = true;
|
||||
std::string roiStr;
|
||||
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);
|
||||
qos = this->declare_parameter("qos", qos);
|
||||
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
|
||||
maxDepth_ = this->declare_parameter("max_depth", maxDepth_);
|
||||
minDepth_ = this->declare_parameter("min_depth", minDepth_);
|
||||
voxelSize_ = this->declare_parameter("voxel_size", voxelSize_);
|
||||
decimation_ = this->declare_parameter("decimation", decimation_);
|
||||
noiseFilterRadius_ = this->declare_parameter("noise_filter_radius", noiseFilterRadius_);
|
||||
noiseFilterMinNeighbors_ = this->declare_parameter("noise_filter_min_neighbors", noiseFilterMinNeighbors_);
|
||||
normalK_ = this->declare_parameter("normal_k", normalK_);
|
||||
normalRadius_ = this->declare_parameter("normal_radius", normalRadius_);
|
||||
filterNaNs_ = this->declare_parameter("filter_nans", filterNaNs_);
|
||||
roiStr = this->declare_parameter("roi_ratios", roiStr);
|
||||
|
||||
//parse roi (region of interest)
|
||||
roiRatios_.resize(4, 0);
|
||||
if(!roiStr.empty())
|
||||
{
|
||||
std::list<std::string> strValues = uSplit(roiStr, ' ');
|
||||
if(strValues.size() != 4)
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
std::vector<float> tmpValues(4);
|
||||
unsigned int i=0;
|
||||
for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
|
||||
{
|
||||
tmpValues[i] = uStr2Float(*jter);
|
||||
++i;
|
||||
}
|
||||
|
||||
if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
|
||||
tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
|
||||
tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
|
||||
tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
|
||||
{
|
||||
roiRatios_ = tmpValues;
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Approximate time sync = %s", approxSync?"true":"false");
|
||||
|
||||
if(approxSync)
|
||||
{
|
||||
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
|
||||
if(approxSyncMaxInterval > 0.0)
|
||||
approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
approxSyncDepth_->registerCallback(std::bind(&PointCloudXYZ::callback, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
|
||||
if(approxSyncMaxInterval > 0.0)
|
||||
approxSyncDisparity_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
approxSyncDisparity_->registerCallback(std::bind(&PointCloudXYZ::callbackDisparity, this, std::placeholders::_1, std::placeholders::_2));
|
||||
}
|
||||
else
|
||||
{
|
||||
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
|
||||
exactSyncDepth_->registerCallback(std::bind(&PointCloudXYZ::callback, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
|
||||
exactSyncDisparity_->registerCallback(std::bind(&PointCloudXYZ::callbackDisparity, this, std::placeholders::_1, std::placeholders::_2));
|
||||
}
|
||||
|
||||
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
|
||||
|
||||
image_transport::TransportHints hints(this);
|
||||
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
cameraInfoSub_.subscribe(this, "depth/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
|
||||
|
||||
disparitySub_.subscribe(this, "disparity/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
disparityCameraInfoSub_.subscribe(this, "disparity/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
|
||||
}
|
||||
|
||||
PointCloudXYZ::~PointCloudXYZ()
|
||||
{
|
||||
delete approxSyncDepth_;
|
||||
delete approxSyncDisparity_;
|
||||
delete exactSyncDepth_;
|
||||
delete exactSyncDisparity_;
|
||||
}
|
||||
void PointCloudXYZ::callback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
|
||||
{
|
||||
if(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 &&
|
||||
depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 &&
|
||||
depthMsg->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0)
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Input type depth=32FC1,16UC1,MONO16");
|
||||
return;
|
||||
}
|
||||
|
||||
if(cloudPub_->get_subscription_count())
|
||||
{
|
||||
rclcpp::Time time = now();
|
||||
|
||||
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depthMsg);
|
||||
|
||||
rtabmap::CameraModel model = rtabmap_conversions::cameraModelFromROS(*cameraInfo);
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
|
||||
|
||||
cv::Mat depth = imageDepthPtr->image;
|
||||
if( roiRatios_.size() == 4 &&
|
||||
((roiRatios_[0] > 0.0f && roiRatios_[0] <= 1.0f) ||
|
||||
(roiRatios_[1] > 0.0f && roiRatios_[1] <= 1.0f) ||
|
||||
(roiRatios_[2] > 0.0f && roiRatios_[2] <= 1.0f) ||
|
||||
(roiRatios_[3] > 0.0f && roiRatios_[3] <= 1.0f)))
|
||||
{
|
||||
cv::Rect roiDepth = rtabmap::util2d::computeRoi(depth, roiRatios_);
|
||||
cv::Rect roiRgb;
|
||||
if(model.imageWidth() && model.imageHeight())
|
||||
{
|
||||
roiRgb = rtabmap::util2d::computeRoi(model.imageSize(), roiRatios_);
|
||||
}
|
||||
if( roiDepth.width%decimation_==0 &&
|
||||
roiDepth.height%decimation_==0 &&
|
||||
(roiRgb.width != 0 ||
|
||||
(roiRgb.width%decimation_==0 &&
|
||||
roiRgb.height%decimation_==0)))
|
||||
{
|
||||
depth = cv::Mat(depth, roiDepth);
|
||||
if(model.imageWidth() != 0 && model.imageHeight() != 0)
|
||||
{
|
||||
model = model.roi(roiRgb);
|
||||
}
|
||||
else
|
||||
{
|
||||
model = model.roi(roiDepth);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
|
||||
"dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
|
||||
"by decimation parameter (%d). Ignoring ROI ratios...",
|
||||
roiRatios_[0],
|
||||
roiRatios_[1],
|
||||
roiRatios_[2],
|
||||
roiRatios_[3],
|
||||
roiDepth.width,
|
||||
roiDepth.height,
|
||||
roiRgb.width,
|
||||
roiRgb.height,
|
||||
decimation_);
|
||||
}
|
||||
}
|
||||
|
||||
pcl::IndicesPtr indices(new std::vector<int>);
|
||||
pclCloud = rtabmap::util3d::cloudFromDepth(
|
||||
depth,
|
||||
model,
|
||||
decimation_,
|
||||
maxDepth_,
|
||||
minDepth_,
|
||||
indices.get());
|
||||
processAndPublish(pclCloud, indices, depthMsg->header);
|
||||
RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyz from depth time = %f s", (now() - time).seconds());
|
||||
}
|
||||
}
|
||||
|
||||
void PointCloudXYZ::callbackDisparity(
|
||||
const stereo_msgs::msg::DisparityImage::ConstSharedPtr disparityMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
|
||||
{
|
||||
if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 &&
|
||||
disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0)
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Input type must be disparity=32FC1 or 16SC1");
|
||||
return;
|
||||
}
|
||||
|
||||
cv::Mat disparity;
|
||||
if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0)
|
||||
{
|
||||
disparity = cv::Mat(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast<uchar*>(disparityMsg->image.data.data()));
|
||||
}
|
||||
else
|
||||
{
|
||||
disparity = cv::Mat(disparityMsg->image.height, disparityMsg->image.width, CV_16SC1, const_cast<uchar*>(disparityMsg->image.data.data()));
|
||||
}
|
||||
|
||||
if(cloudPub_->get_subscription_count())
|
||||
{
|
||||
rclcpp::Time time = now();
|
||||
|
||||
cv::Rect roi = rtabmap::util2d::computeRoi(disparity, roiRatios_);
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
|
||||
rtabmap::CameraModel leftModel = rtabmap_conversions::cameraModelFromROS(*cameraInfo);
|
||||
UASSERT(disparity.cols == leftModel.imageWidth() && disparity.rows == leftModel.imageHeight());
|
||||
rtabmap::StereoCameraModel stereoModel(disparityMsg->f, disparityMsg->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), disparityMsg->t);
|
||||
pcl::IndicesPtr indices(new std::vector<int>);
|
||||
pclCloud = rtabmap::util3d::cloudFromDisparity(
|
||||
cv::Mat(disparity, roi),
|
||||
stereoModel,
|
||||
decimation_,
|
||||
maxDepth_,
|
||||
minDepth_,
|
||||
indices.get());
|
||||
|
||||
processAndPublish(pclCloud, indices, disparityMsg->header);
|
||||
RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyz from disparity time = %f s", (now() - time).seconds());
|
||||
}
|
||||
}
|
||||
|
||||
void PointCloudXYZ::processAndPublish(pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud, pcl::IndicesPtr & indices, const std_msgs::msg::Header & header)
|
||||
{
|
||||
if(indices->size() && voxelSize_ > 0.0)
|
||||
{
|
||||
pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_);
|
||||
pclCloud->is_dense = true;
|
||||
}
|
||||
|
||||
// Do radius filtering after voxel filtering ( a lot faster)
|
||||
if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
|
||||
{
|
||||
if(pclCloud->is_dense)
|
||||
{
|
||||
indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
|
||||
}
|
||||
else
|
||||
{
|
||||
indices = rtabmap::util3d::radiusFiltering(pclCloud, indices, noiseFilterRadius_, noiseFilterMinNeighbors_);
|
||||
}
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
pcl::copyPointCloud(*pclCloud, *indices, *tmp);
|
||||
pclCloud = tmp;
|
||||
}
|
||||
|
||||
sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
|
||||
if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && (normalK_ > 0 || normalRadius_ > 0.0f))
|
||||
{
|
||||
//compute normals
|
||||
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);
|
||||
pcl::PointCloud<pcl::PointNormal>::Ptr pclCloudNormal(new pcl::PointCloud<pcl::PointNormal>);
|
||||
pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal);
|
||||
if(filterNaNs_)
|
||||
{
|
||||
pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal);
|
||||
}
|
||||
pcl::toROSMsg(*pclCloudNormal, *rosCloud);
|
||||
}
|
||||
else
|
||||
{
|
||||
if(filterNaNs_ && !pclCloud->is_dense)
|
||||
{
|
||||
pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
|
||||
}
|
||||
pcl::toROSMsg(*pclCloud, *rosCloud);
|
||||
}
|
||||
rosCloud->header.stamp = header.stamp;
|
||||
rosCloud->header.frame_id = header.frame_id;
|
||||
|
||||
//publish the message
|
||||
cloudPub_->publish(std::move(rosCloud));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#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_util::PointCloudXYZ)
|
||||
|
||||
@@ -0,0 +1,508 @@
|
||||
/*
|
||||
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_util/point_cloud_xyzrgb.hpp>
|
||||
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
|
||||
#include <image_geometry/pinhole_camera_model.h>
|
||||
#include <image_geometry/stereo_camera_model.h>
|
||||
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
|
||||
#include "rtabmap/core/util2d.h"
|
||||
#include "rtabmap/core/util3d.h"
|
||||
#include "rtabmap/core/util3d_filtering.h"
|
||||
#include "rtabmap/core/util3d_surface.h"
|
||||
#include "rtabmap/utilite/UConversion.h"
|
||||
#include "rtabmap/utilite/UStl.h"
|
||||
|
||||
namespace rtabmap_util
|
||||
{
|
||||
|
||||
PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) :
|
||||
Node("point_cloud_xyzrgb", options),
|
||||
maxDepth_(0.0),
|
||||
minDepth_(0.0),
|
||||
voxelSize_(0.0),
|
||||
decimation_(1),
|
||||
noiseFilterRadius_(0.0),
|
||||
noiseFilterMinNeighbors_(5),
|
||||
normalK_(0),
|
||||
normalRadius_(0.0),
|
||||
filterNaNs_(false),
|
||||
approxSyncDepth_(0),
|
||||
approxSyncDisparity_(0),
|
||||
approxSyncStereo_(0),
|
||||
exactSyncDepth_(0),
|
||||
exactSyncDisparity_(0),
|
||||
exactSyncStereo_(0)
|
||||
{
|
||||
bool approxSync = true;
|
||||
std::string roiStr;
|
||||
int queueSize = 10;
|
||||
int qos = 0;
|
||||
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);
|
||||
qos = this->declare_parameter("qos", qos);
|
||||
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
|
||||
maxDepth_ = this->declare_parameter("max_depth", maxDepth_);
|
||||
minDepth_ = this->declare_parameter("min_depth", minDepth_);
|
||||
voxelSize_ = this->declare_parameter("voxel_size", voxelSize_);
|
||||
decimation_ = this->declare_parameter("decimation", decimation_);
|
||||
noiseFilterRadius_ = this->declare_parameter("noise_filter_radius", noiseFilterRadius_);
|
||||
noiseFilterMinNeighbors_ = this->declare_parameter("noise_filter_min_neighbors", noiseFilterMinNeighbors_);
|
||||
normalK_ = this->declare_parameter("normal_k", normalK_);
|
||||
normalRadius_ = this->declare_parameter("normal_radius", normalRadius_);
|
||||
filterNaNs_ = this->declare_parameter("filter_nans", filterNaNs_);
|
||||
roiStr = this->declare_parameter("roi_ratios", roiStr);
|
||||
|
||||
//parse roi (region of interest)
|
||||
roiRatios_.resize(4, 0);
|
||||
if(!roiStr.empty())
|
||||
{
|
||||
std::list<std::string> strValues = uSplit(roiStr, ' ');
|
||||
if(strValues.size() != 4)
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
std::vector<float> tmpValues(4);
|
||||
unsigned int i=0;
|
||||
for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
|
||||
{
|
||||
tmpValues[i] = uStr2Float(*jter);
|
||||
++i;
|
||||
}
|
||||
|
||||
if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
|
||||
tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
|
||||
tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
|
||||
tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
|
||||
{
|
||||
roiRatios_ = tmpValues;
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// StereoBM parameters
|
||||
stereoBMParameters_ = rtabmap::Parameters::getDefaultParameters("StereoBM");
|
||||
for(rtabmap::ParametersMap::iterator iter=stereoBMParameters_.begin(); iter!=stereoBMParameters_.end(); ++iter)
|
||||
{
|
||||
std::string vStr = declare_parameter(iter->first, iter->second);
|
||||
if(vStr.compare(iter->second)!=0)
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
|
||||
iter->second = vStr;
|
||||
}
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Approximate time sync = %s", approxSync?"true":"false");
|
||||
|
||||
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
|
||||
|
||||
rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&PointCloudXYZRGB::rgbdImageCallback, this, std::placeholders::_1));
|
||||
|
||||
if(approxSync)
|
||||
{
|
||||
|
||||
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
|
||||
if(approxSyncMaxInterval > 0.0)
|
||||
approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
approxSyncDepth_->registerCallback(std::bind(&PointCloudXYZRGB::depthCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
|
||||
approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
|
||||
if(approxSyncMaxInterval > 0.0)
|
||||
approxSyncDisparity_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
approxSyncDisparity_->registerCallback(std::bind(&PointCloudXYZRGB::disparityCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
|
||||
approxSyncStereo_ = new message_filters::Synchronizer<MyApproxSyncStereoPolicy>(MyApproxSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
|
||||
if(approxSyncMaxInterval > 0.0)
|
||||
approxSyncStereo_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
approxSyncStereo_->registerCallback(std::bind(&PointCloudXYZRGB::stereoCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
|
||||
}
|
||||
else
|
||||
{
|
||||
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
|
||||
exactSyncDepth_->registerCallback(std::bind(&PointCloudXYZRGB::depthCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
|
||||
exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
|
||||
exactSyncDisparity_->registerCallback(std::bind(&PointCloudXYZRGB::disparityCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
|
||||
exactSyncStereo_ = new message_filters::Synchronizer<MyExactSyncStereoPolicy>(MyExactSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
|
||||
exactSyncStereo_->registerCallback(std::bind(&PointCloudXYZRGB::stereoCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
|
||||
}
|
||||
|
||||
image_transport::TransportHints hints(this);
|
||||
imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
|
||||
|
||||
imageDisparitySub_.subscribe(this, "disparity", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
|
||||
imageLeft_.subscribe(this, "left/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
imageRight_.subscribe(this, "right/image", 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());
|
||||
}
|
||||
|
||||
PointCloudXYZRGB::~PointCloudXYZRGB()
|
||||
{
|
||||
delete approxSyncDepth_;
|
||||
delete approxSyncDisparity_;
|
||||
delete approxSyncStereo_;
|
||||
delete exactSyncDepth_;
|
||||
delete exactSyncDisparity_;
|
||||
delete exactSyncStereo_;
|
||||
}
|
||||
|
||||
void PointCloudXYZRGB::depthCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr image,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageDepth,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
|
||||
{
|
||||
if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
|
||||
image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
|
||||
image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
|
||||
image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
|
||||
image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
|
||||
image->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
|
||||
image->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
|
||||
image->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
|
||||
!(imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
|
||||
imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 ||
|
||||
imageDepth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
|
||||
return;
|
||||
}
|
||||
|
||||
if(cloudPub_->get_subscription_count())
|
||||
{
|
||||
rclcpp::Time time = now();
|
||||
|
||||
cv_bridge::CvImageConstPtr imagePtr;
|
||||
if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
|
||||
{
|
||||
imagePtr = cv_bridge::toCvShare(image);
|
||||
}
|
||||
else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
|
||||
image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
|
||||
{
|
||||
imagePtr = cv_bridge::toCvShare(image, "mono8");
|
||||
}
|
||||
else
|
||||
{
|
||||
imagePtr = cv_bridge::toCvShare(image, "bgr8");
|
||||
}
|
||||
|
||||
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
|
||||
|
||||
rtabmap::CameraModel model = rtabmap_conversions::cameraModelFromROS(*cameraInfo);
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
|
||||
|
||||
cv::Mat rgb = imagePtr->image;
|
||||
cv::Mat depth = imageDepthPtr->image;
|
||||
if( roiRatios_.size() == 4 &&
|
||||
((roiRatios_[0] > 0.0f && roiRatios_[0] <= 1.0f) ||
|
||||
(roiRatios_[1] > 0.0f && roiRatios_[1] <= 1.0f) ||
|
||||
(roiRatios_[2] > 0.0f && roiRatios_[2] <= 1.0f) ||
|
||||
(roiRatios_[3] > 0.0f && roiRatios_[3] <= 1.0f)))
|
||||
{
|
||||
cv::Rect roiDepth = rtabmap::util2d::computeRoi(depth, roiRatios_);
|
||||
cv::Rect roiRgb = rtabmap::util2d::computeRoi(rgb, roiRatios_);
|
||||
if( roiDepth.width%decimation_==0 &&
|
||||
roiDepth.height%decimation_==0 &&
|
||||
roiRgb.width%decimation_==0 &&
|
||||
roiRgb.height%decimation_==0)
|
||||
{
|
||||
depth = cv::Mat(depth, roiDepth);
|
||||
rgb = cv::Mat(rgb, roiRgb);
|
||||
model = model.roi(roiRgb);
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
|
||||
"dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
|
||||
"by decimation parameter (%d). Ignoring ROI ratios...",
|
||||
roiRatios_[0],
|
||||
roiRatios_[1],
|
||||
roiRatios_[2],
|
||||
roiRatios_[3],
|
||||
roiDepth.width,
|
||||
roiDepth.height,
|
||||
roiRgb.width,
|
||||
roiRgb.height,
|
||||
decimation_);
|
||||
}
|
||||
}
|
||||
|
||||
pcl::IndicesPtr indices(new std::vector<int>);
|
||||
pclCloud = rtabmap::util3d::cloudFromDepthRGB(
|
||||
rgb,
|
||||
depth,
|
||||
model,
|
||||
decimation_,
|
||||
maxDepth_,
|
||||
minDepth_,
|
||||
indices.get());
|
||||
|
||||
processAndPublish(pclCloud, indices, imagePtr->header);
|
||||
|
||||
RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyzrgb from RGB-D time = %f s", (now() - time).seconds());
|
||||
}
|
||||
}
|
||||
|
||||
void PointCloudXYZRGB::disparityCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr image,
|
||||
const stereo_msgs::msg::DisparityImage::ConstSharedPtr imageDisparity,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr imagePtr;
|
||||
if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
|
||||
{
|
||||
imagePtr = cv_bridge::toCvShare(image);
|
||||
}
|
||||
else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
|
||||
image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
|
||||
{
|
||||
imagePtr = cv_bridge::toCvShare(image, "mono8");
|
||||
}
|
||||
else
|
||||
{
|
||||
imagePtr = cv_bridge::toCvShare(image, "bgr8");
|
||||
}
|
||||
|
||||
if(imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 &&
|
||||
imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0)
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Input type must be disparity=32FC1 or 16SC1");
|
||||
return;
|
||||
}
|
||||
|
||||
cv::Mat disparity;
|
||||
if(imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0)
|
||||
{
|
||||
disparity = cv::Mat(imageDisparity->image.height, imageDisparity->image.width, CV_32FC1, const_cast<uchar*>(imageDisparity->image.data.data()));
|
||||
}
|
||||
else
|
||||
{
|
||||
disparity = cv::Mat(imageDisparity->image.height, imageDisparity->image.width, CV_16SC1, const_cast<uchar*>(imageDisparity->image.data.data()));
|
||||
}
|
||||
|
||||
if(cloudPub_->get_subscription_count())
|
||||
{
|
||||
rclcpp::Time time = now();
|
||||
|
||||
cv::Rect roi = rtabmap::util2d::computeRoi(disparity, roiRatios_);
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
|
||||
rtabmap::CameraModel leftModel = rtabmap_conversions::cameraModelFromROS(*cameraInfo);
|
||||
UASSERT(disparity.cols == leftModel.imageWidth() && disparity.rows == leftModel.imageHeight());
|
||||
UASSERT(imagePtr->image.cols == leftModel.imageWidth() && imagePtr->image.rows == leftModel.imageHeight());
|
||||
rtabmap::StereoCameraModel stereoModel(imageDisparity->f, imageDisparity->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), imageDisparity->t);
|
||||
pcl::IndicesPtr indices(new std::vector<int>);
|
||||
pclCloud = rtabmap::util3d::cloudFromDisparityRGB(
|
||||
cv::Mat(imagePtr->image, roi),
|
||||
cv::Mat(disparity, roi),
|
||||
stereoModel,
|
||||
decimation_,
|
||||
maxDepth_,
|
||||
minDepth_,
|
||||
indices.get());
|
||||
|
||||
processAndPublish(pclCloud, indices, imageDisparity->header);
|
||||
|
||||
RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyzrgb from disparity time = %f s", (now() - time).seconds());
|
||||
}
|
||||
}
|
||||
|
||||
void PointCloudXYZRGB::stereoCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageLeft,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageRight,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr camInfoLeft,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr camInfoRight)
|
||||
{
|
||||
if(!(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
|
||||
imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
|
||||
imageLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
|
||||
imageLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
|
||||
imageLeft->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
|
||||
imageLeft->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0) ||
|
||||
!(imageRight->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
|
||||
imageRight->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
|
||||
imageRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
|
||||
imageRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
|
||||
imageRight->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
|
||||
imageRight->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (enc=%s)", imageLeft->encoding.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
if(cloudPub_->get_subscription_count())
|
||||
{
|
||||
rclcpp::Time time = now();
|
||||
|
||||
cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
|
||||
if(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
|
||||
imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
|
||||
{
|
||||
ptrLeftImage = cv_bridge::toCvShare(imageLeft, "mono8");
|
||||
}
|
||||
else
|
||||
{
|
||||
ptrLeftImage = cv_bridge::toCvShare(imageLeft, "bgr8");
|
||||
}
|
||||
ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8");
|
||||
|
||||
if(roiRatios_[0]!=0.0f || roiRatios_[1]!=0.0f || roiRatios_[2]!=0.0f || roiRatios_[3]!=0.0f)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "\"roi_ratios\" set but ignored for stereo images.");
|
||||
}
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
|
||||
pcl::IndicesPtr indices(new std::vector<int>);
|
||||
pclCloud = rtabmap::util3d::cloudFromStereoImages(
|
||||
ptrLeftImage->image,
|
||||
ptrRightImage->image,
|
||||
rtabmap_conversions::stereoCameraModelFromROS(*camInfoLeft, *camInfoRight),
|
||||
decimation_,
|
||||
maxDepth_,
|
||||
minDepth_,
|
||||
indices.get(),
|
||||
stereoBMParameters_);
|
||||
|
||||
processAndPublish(pclCloud, indices, imageLeft->header);
|
||||
|
||||
RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyzrgb from stereo time = %f s", (now() - time).seconds());
|
||||
}
|
||||
}
|
||||
|
||||
void PointCloudXYZRGB::rgbdImageCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image)
|
||||
{
|
||||
if(cloudPub_->get_subscription_count())
|
||||
{
|
||||
rclcpp::Time time = now();
|
||||
|
||||
rtabmap::SensorData data = rtabmap_conversions::rgbdImageFromROS(image);
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
|
||||
pcl::IndicesPtr indices(new std::vector<int>);
|
||||
if(data.isValid())
|
||||
{
|
||||
pclCloud = rtabmap::util3d::cloudRGBFromSensorData(
|
||||
data,
|
||||
decimation_,
|
||||
maxDepth_,
|
||||
minDepth_,
|
||||
indices.get(),
|
||||
stereoBMParameters_,
|
||||
roiRatios_);
|
||||
|
||||
processAndPublish(pclCloud, indices, image->header);
|
||||
}
|
||||
|
||||
RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyzrgb from rgbd_image time = %f s", (now() - time).seconds());
|
||||
}
|
||||
}
|
||||
|
||||
void PointCloudXYZRGB::processAndPublish(
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr & pclCloud,
|
||||
pcl::IndicesPtr & indices,
|
||||
const std_msgs::msg::Header & header)
|
||||
{
|
||||
if(indices->size() && voxelSize_ > 0.0)
|
||||
{
|
||||
pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_);
|
||||
pclCloud->is_dense = true;
|
||||
}
|
||||
|
||||
// Do radius filtering after voxel filtering ( a lot faster)
|
||||
if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
|
||||
{
|
||||
if(pclCloud->is_dense)
|
||||
{
|
||||
indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
|
||||
}
|
||||
else
|
||||
{
|
||||
indices = rtabmap::util3d::radiusFiltering(pclCloud, indices, noiseFilterRadius_, noiseFilterMinNeighbors_);
|
||||
}
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
|
||||
pcl::copyPointCloud(*pclCloud, *indices, *tmp);
|
||||
pclCloud = tmp;
|
||||
}
|
||||
|
||||
sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
|
||||
if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && (normalK_ > 0 || normalRadius_ > 0.0f))
|
||||
{
|
||||
//compute normals
|
||||
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);
|
||||
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pclCloudNormal(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
|
||||
pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal);
|
||||
if(filterNaNs_)
|
||||
{
|
||||
pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal);
|
||||
}
|
||||
pcl::toROSMsg(*pclCloudNormal, *rosCloud);
|
||||
}
|
||||
else
|
||||
{
|
||||
if(filterNaNs_ && !pclCloud->is_dense)
|
||||
{
|
||||
pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
|
||||
}
|
||||
pcl::toROSMsg(*pclCloud, *rosCloud);
|
||||
}
|
||||
rosCloud->header.stamp = header.stamp;
|
||||
rosCloud->header.frame_id = header.frame_id;
|
||||
|
||||
//publish the message
|
||||
cloudPub_->publish(std::move(rosCloud));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#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_util::PointCloudXYZRGB)
|
||||
|
||||
@@ -0,0 +1,269 @@
|
||||
/*
|
||||
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_util/pointcloud_to_depthimage.hpp>
|
||||
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
#include <rtabmap/core/util3d.h>
|
||||
#include <rtabmap/core/util2d.h>
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
|
||||
#include <sensor_msgs/image_encodings.hpp>
|
||||
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
|
||||
namespace rtabmap_util
|
||||
{
|
||||
|
||||
PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & options) :
|
||||
Node("pointcloud_to_depthimage", options),
|
||||
waitForTransform_(0.1),
|
||||
fillHolesSize_ (0),
|
||||
fillHolesError_(0.1),
|
||||
fillIterations_(1),
|
||||
decimation_(1),
|
||||
upscale_(false),
|
||||
upscaleDepthErrorRatio_(0.02),
|
||||
approxSync_(0),
|
||||
exactSync_(0)
|
||||
{
|
||||
tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
|
||||
//auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
|
||||
// this->get_node_base_interface(),
|
||||
// this->get_node_timers_interface());
|
||||
//tfBuffer_->setCreateTimerInterface(timer_interface);
|
||||
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
|
||||
|
||||
int queueSize = 10;
|
||||
int qos = 0;
|
||||
bool approx = true;
|
||||
queueSize = this->declare_parameter("queue_size", queueSize);
|
||||
qos = this->declare_parameter("qos", qos);
|
||||
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
|
||||
fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
|
||||
waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
|
||||
fillHolesSize_ = this->declare_parameter("fill_holes_size", fillHolesSize_);
|
||||
fillHolesError_ = this->declare_parameter("fill_holes_error", fillHolesError_);
|
||||
fillIterations_ = this->declare_parameter("fill_iterations", fillIterations_);
|
||||
decimation_ = this->declare_parameter("decimation", decimation_);
|
||||
approx = this->declare_parameter("approx", approx);
|
||||
upscale_ = this->declare_parameter("upscale", upscale_);
|
||||
upscaleDepthErrorRatio_ = this->declare_parameter("upscale_depth_error_ratio", upscaleDepthErrorRatio_);
|
||||
|
||||
if(fixedFrameId_.empty() && approx)
|
||||
{
|
||||
RCLCPP_FATAL(this->get_logger(), "fixed_frame_id should be set when using approximate "
|
||||
"time synchronization (approx=true)! If the robot "
|
||||
"is moving, it could be \"odom\". If not moving, it "
|
||||
"could be \"base_link\".");
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Params:");
|
||||
RCLCPP_INFO(this->get_logger(), " approx=%s", approx?"true":"false");
|
||||
RCLCPP_INFO(this->get_logger(), " queue_size=%d", queueSize);
|
||||
RCLCPP_INFO(this->get_logger(), " fixed_frame_id=%s", fixedFrameId_.c_str());
|
||||
RCLCPP_INFO(this->get_logger(), " wait_for_transform=%fs", waitForTransform_);
|
||||
RCLCPP_INFO(this->get_logger(), " fill_holes_size=%d pixels (0=disabled)", fillHolesSize_);
|
||||
RCLCPP_INFO(this->get_logger(), " fill_holes_error=%f", fillHolesError_);
|
||||
RCLCPP_INFO(this->get_logger(), " fill_iterations=%d", fillIterations_);
|
||||
RCLCPP_INFO(this->get_logger(), " decimation=%d", decimation_);
|
||||
RCLCPP_INFO(this->get_logger(), " upscale=%s (upscale_depth_error_ratio=%f)", upscale_?"true":"false", upscaleDepthErrorRatio_);
|
||||
|
||||
auto node = rclcpp::Node::make_shared(this->get_name());
|
||||
image_transport::ImageTransport it(node);
|
||||
depthImage16Pub_ = image_transport::create_camera_publisher(node.get(), "image_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); // 16 bits unsigned in mm
|
||||
depthImage32Pub_ = image_transport::create_camera_publisher(node.get(), "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());// 32 bits float in meters
|
||||
pointCloudTransformedPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud_transformed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
|
||||
cameraInfo16Pub_ = create_publisher<sensor_msgs::msg::CameraInfo>(depthImage16Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo));
|
||||
cameraInfo32Pub_ = create_publisher<sensor_msgs::msg::CameraInfo>(depthImage32Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo));
|
||||
|
||||
if(approx)
|
||||
{
|
||||
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_);
|
||||
approxSync_->registerCallback(std::bind(&PointCloudToDepthImage::callback, this, std::placeholders::_1, std::placeholders::_2));
|
||||
}
|
||||
else
|
||||
{
|
||||
fixedFrameId_.clear();
|
||||
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_);
|
||||
exactSync_->registerCallback(std::bind(&PointCloudToDepthImage::callback, this, std::placeholders::_1, std::placeholders::_2));
|
||||
}
|
||||
|
||||
pointCloudSub_.subscribe(this, "cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
cameraInfoSub_.subscribe(this, "camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
|
||||
}
|
||||
|
||||
PointCloudToDepthImage::~PointCloudToDepthImage()
|
||||
{
|
||||
delete approxSync_;
|
||||
delete exactSync_;
|
||||
}
|
||||
|
||||
void PointCloudToDepthImage::callback(
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointCloud2Msg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
if(depthImage32Pub_.getNumSubscribers() > 0 || depthImage16Pub_.getNumSubscribers() > 0)
|
||||
{
|
||||
double cloudStamp = rtabmap_conversions::timestampFromROS(pointCloud2Msg->header.stamp);
|
||||
double infoStamp = rtabmap_conversions::timestampFromROS(cameraInfoMsg->header.stamp);
|
||||
|
||||
rtabmap::Transform cloudDisplacement = rtabmap::Transform::getIdentity();
|
||||
if(!fixedFrameId_.empty())
|
||||
{
|
||||
// approx sync
|
||||
cloudDisplacement = rtabmap_conversions::getTransform(
|
||||
pointCloud2Msg->header.frame_id,
|
||||
fixedFrameId_,
|
||||
cameraInfoMsg->header.stamp,
|
||||
pointCloud2Msg->header.stamp,
|
||||
*tfBuffer_,
|
||||
waitForTransform_);
|
||||
}
|
||||
|
||||
if(cloudDisplacement.isNull())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
rtabmap::Transform cloudToCamera = rtabmap_conversions::getTransform(
|
||||
pointCloud2Msg->header.frame_id,
|
||||
cameraInfoMsg->header.frame_id,
|
||||
cameraInfoMsg->header.stamp,
|
||||
*tfBuffer_,
|
||||
waitForTransform_);
|
||||
|
||||
if(cloudToCamera.isNull())
|
||||
{
|
||||
return;
|
||||
}
|
||||
rtabmap::Transform localTransform = cloudDisplacement*cloudToCamera;
|
||||
|
||||
rtabmap::CameraModel model = rtabmap_conversions::cameraModelFromROS(*cameraInfoMsg, localTransform);
|
||||
sensor_msgs::msg::CameraInfo cameraInfoMsgOut = *cameraInfoMsg;
|
||||
|
||||
if(decimation_ > 1)
|
||||
{
|
||||
if(model.imageWidth()%decimation_ == 0 && model.imageHeight()%decimation_ == 0)
|
||||
{
|
||||
float scale = 1.0f/float(decimation_);
|
||||
model = model.scaled(scale);
|
||||
rtabmap_conversions::cameraModelToROS(model, cameraInfoMsgOut);
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "decimation (%d) not valid for image size %dx%d",
|
||||
decimation_,
|
||||
model.imageWidth(),
|
||||
model.imageHeight());
|
||||
}
|
||||
}
|
||||
|
||||
UASSERT_MSG(pointCloud2Msg->data.size() == pointCloud2Msg->row_step*pointCloud2Msg->height,
|
||||
uFormat("data=%d row_step=%d height=%d", pointCloud2Msg->data.size(), pointCloud2Msg->row_step, pointCloud2Msg->height).c_str());
|
||||
|
||||
pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
|
||||
pcl_conversions::toPCL(*pointCloud2Msg, *cloud);
|
||||
|
||||
cv_bridge::CvImage depthImage;
|
||||
|
||||
if(cloud->data.empty())
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "Received an empty cloud on topic \"%s\"! A depth image with all zeros is returned.", pointCloudSub_.getTopic().c_str());
|
||||
depthImage.image = cv::Mat::zeros(model.imageSize(), CV_32FC1);
|
||||
}
|
||||
else
|
||||
{
|
||||
depthImage.image = rtabmap::util3d::projectCloudToCamera(model.imageSize(), model.K(), cloud, model.localTransform());
|
||||
|
||||
if(fillHolesSize_ > 0 && fillIterations_ > 0)
|
||||
{
|
||||
for(int i=0; i<fillIterations_;++i)
|
||||
{
|
||||
depthImage.image = rtabmap::util2d::fillDepthHoles(depthImage.image, fillHolesSize_, fillHolesError_);
|
||||
}
|
||||
|
||||
if(pointCloudTransformedPub_->get_subscription_count()>0)
|
||||
{
|
||||
sensor_msgs::msg::PointCloud2 pointCloud2Out;
|
||||
rtabmap_conversions::transformPointCloud(model.localTransform().inverse().toEigen4f(), *pointCloud2Msg, pointCloud2Out);
|
||||
pointCloud2Out.header = cameraInfoMsg->header;
|
||||
pointCloudTransformedPub_->publish(pointCloud2Out);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
depthImage.header = cameraInfoMsg->header;
|
||||
|
||||
if(decimation_>1 && upscale_)
|
||||
{
|
||||
depthImage.image = rtabmap::util2d::interpolate(depthImage.image, decimation_, upscaleDepthErrorRatio_);
|
||||
}
|
||||
|
||||
if(depthImage32Pub_.getNumSubscribers())
|
||||
{
|
||||
depthImage.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
|
||||
depthImage32Pub_.publish(depthImage.toImageMsg(), cameraInfoMsg);
|
||||
if(cameraInfo32Pub_->get_subscription_count())
|
||||
{
|
||||
cameraInfo32Pub_->publish(cameraInfoMsgOut);
|
||||
}
|
||||
}
|
||||
|
||||
if(depthImage16Pub_.getNumSubscribers())
|
||||
{
|
||||
depthImage.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
|
||||
depthImage.image = rtabmap::util2d::cvtDepthFromFloat(depthImage.image);
|
||||
depthImage16Pub_.publish(depthImage.toImageMsg(), cameraInfoMsg);
|
||||
if(cameraInfo16Pub_->get_subscription_count())
|
||||
{
|
||||
cameraInfo16Pub_->publish(cameraInfoMsgOut);
|
||||
}
|
||||
}
|
||||
|
||||
if( cloudStamp != rtabmap_conversions::timestampFromROS(pointCloud2Msg->header.stamp) ||
|
||||
infoStamp != rtabmap_conversions::timestampFromROS(cameraInfoMsg->header.stamp))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Input stamps changed between the beginning and the end of the callback! Make "
|
||||
"sure the node publishing the topics doesn't override the same data after publishing them. A "
|
||||
"solution is to use this node within another nodelet manager. Stamps: "
|
||||
"cloud=%f->%f info=%f->%f",
|
||||
cloudStamp, rtabmap_conversions::timestampFromROS(pointCloud2Msg->header.stamp),
|
||||
infoStamp, rtabmap_conversions::timestampFromROS(cameraInfoMsg->header.stamp));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#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_util::PointCloudToDepthImage)
|
||||
@@ -0,0 +1,162 @@
|
||||
/*
|
||||
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_util/rgbd_relay.hpp"
|
||||
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <sensor_msgs/msg/compressed_image.hpp>
|
||||
#include <sensor_msgs/msg/camera_info.hpp>
|
||||
#include <sensor_msgs/image_encodings.hpp>
|
||||
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
|
||||
#include "rtabmap_conversions/MsgConversion.h"
|
||||
|
||||
#include "rtabmap/core/Compression.h"
|
||||
#include "rtabmap/utilite/UConversion.h"
|
||||
|
||||
namespace rtabmap_util
|
||||
{
|
||||
|
||||
RGBDRelay::RGBDRelay(const rclcpp::NodeOptions & options) :
|
||||
Node("rgbd_relay", options),
|
||||
compress_(false),
|
||||
uncompress_(false)
|
||||
{
|
||||
int qos = 0;
|
||||
qos = this->declare_parameter("qos", qos);
|
||||
compress_ = this->declare_parameter("compress", compress_);
|
||||
uncompress_ = this->declare_parameter("uncompress", uncompress_);
|
||||
|
||||
rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&RGBDRelay::callback, this, std::placeholders::_1));
|
||||
rgbdImagePub_ = create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image_relay", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
|
||||
}
|
||||
|
||||
void RGBDRelay::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) const
|
||||
{
|
||||
if(rgbdImagePub_->get_subscription_count())
|
||||
{
|
||||
if(!compress_ && !uncompress_)
|
||||
{
|
||||
//just republish it
|
||||
rgbdImagePub_->publish(*input);
|
||||
return;
|
||||
}
|
||||
|
||||
auto output = std::make_unique<rtabmap_msgs::msg::RGBDImage>();
|
||||
output->header = input->header;
|
||||
output->rgb_camera_info = input->rgb_camera_info;
|
||||
output->depth_camera_info = input->depth_camera_info;
|
||||
output->key_points = input->key_points;
|
||||
output->points = input->points;
|
||||
output->descriptors = input->descriptors;
|
||||
output->global_descriptor = input->global_descriptor;
|
||||
|
||||
rtabmap::StereoCameraModel stereoModel = rtabmap_conversions::stereoCameraModelFromROS(input->rgb_camera_info, input->depth_camera_info, rtabmap::Transform::getIdentity());
|
||||
|
||||
if(compress_)
|
||||
{
|
||||
if(!input->rgb_compressed.data.empty())
|
||||
{
|
||||
// already compressed, just copy pointer
|
||||
output->rgb_compressed = input->rgb_compressed;
|
||||
}
|
||||
else if(!input->rgb.data.empty())
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb = cv_bridge::toCvShare(input->rgb, input);
|
||||
rgb->toCompressedImageMsg(output->rgb_compressed, cv_bridge::JPG);
|
||||
}
|
||||
|
||||
if(!input->depth_compressed.data.empty())
|
||||
{
|
||||
// already compressed, just copy pointer
|
||||
output->depth_compressed = input->depth_compressed;
|
||||
}
|
||||
else if(!input->depth.data.empty())
|
||||
{
|
||||
if(stereoModel.isValidForProjection())
|
||||
{
|
||||
// right stereo image
|
||||
cv_bridge::CvImageConstPtr imageRightPtr = cv_bridge::toCvShare(input->depth, input);
|
||||
imageRightPtr->toCompressedImageMsg(output->depth_compressed, cv_bridge::JPG);
|
||||
}
|
||||
else
|
||||
{
|
||||
// depth image
|
||||
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(input->depth, input);
|
||||
output->depth_compressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
|
||||
output->depth_compressed.format = "png";
|
||||
}
|
||||
}
|
||||
}
|
||||
if(uncompress_)
|
||||
{
|
||||
if(!input->rgb.data.empty())
|
||||
{
|
||||
// already raw, just copy pointer
|
||||
output->rgb = input->rgb;
|
||||
}
|
||||
if(!input->rgb_compressed.data.empty())
|
||||
{
|
||||
cv_bridge::toCvCopy(input->rgb_compressed)->toImageMsg(output->rgb);
|
||||
}
|
||||
|
||||
if(!input->depth.data.empty())
|
||||
{
|
||||
// already raw, just copy pointer
|
||||
output->depth = input->depth;
|
||||
}
|
||||
else if(input->depth_compressed.format.compare("jpg")==0)
|
||||
{
|
||||
// right stereo image
|
||||
cv_bridge::toCvCopy(input->depth_compressed)->toImageMsg(output->depth);
|
||||
}
|
||||
else
|
||||
{
|
||||
// dpeth image
|
||||
auto cvImg = std::make_unique<cv_bridge::CvImage>();
|
||||
cvImg->header = input->depth_compressed.header;
|
||||
cvImg->image = rtabmap::uncompressImage(input->depth_compressed.data);
|
||||
UASSERT(cvImg->image.empty() || cvImg->image.type() == CV_32FC1 || cvImg->image.type() == CV_16UC1);
|
||||
cvImg->encoding = cvImg->image.empty()?"":cvImg->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
|
||||
cvImg->toImageMsg(output->depth);
|
||||
}
|
||||
}
|
||||
|
||||
rgbdImagePub_->publish(std::move(output));
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#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_util::RGBDRelay)
|
||||
@@ -0,0 +1,112 @@
|
||||
/*
|
||||
Copyright (c) 2010-2022, 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_util/rgbd_split.hpp>
|
||||
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
|
||||
namespace rtabmap_util
|
||||
{
|
||||
|
||||
RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) :
|
||||
Node("rgbd_split", options)
|
||||
{
|
||||
int queueSize = 10;
|
||||
int qos = 0;
|
||||
queueSize = this->declare_parameter("queue_size", queueSize);
|
||||
qos = this->declare_parameter("qos", qos);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
|
||||
|
||||
rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&RGBDSplit::callback, this, std::placeholders::_1));
|
||||
|
||||
auto node = rclcpp::Node::make_shared(this->get_name());
|
||||
image_transport::ImageTransport it(node);
|
||||
rgbPub_ = image_transport::create_camera_publisher(node.get(), std::string(rgbdImageSub_->get_topic_name()) + "/rgb", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
depthPub_ = image_transport::create_camera_publisher(node.get(), std::string(rgbdImageSub_->get_topic_name()) + "/depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
}
|
||||
|
||||
|
||||
void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) const
|
||||
{
|
||||
if(rgbPub_.getNumSubscribers())
|
||||
{
|
||||
sensor_msgs::msg::Image outputImage;
|
||||
sensor_msgs::msg::CameraInfo outputCameraInfo;
|
||||
outputImage.header = outputCameraInfo.header = input->header;
|
||||
outputCameraInfo = input->rgb_camera_info;
|
||||
|
||||
if(!input->rgb.data.empty())
|
||||
{
|
||||
// already raw, just copy pointer
|
||||
outputImage = input->rgb;
|
||||
}
|
||||
else if(!input->rgb_compressed.data.empty())
|
||||
{
|
||||
#ifdef CV_BRIDGE_HYDRO
|
||||
ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
|
||||
#else
|
||||
cv_bridge::toCvCopy(input->rgb_compressed)->toImageMsg(outputImage);
|
||||
#endif
|
||||
}
|
||||
rgbPub_.publish(outputImage, outputCameraInfo);
|
||||
}
|
||||
|
||||
if(depthPub_.getNumSubscribers())
|
||||
{
|
||||
sensor_msgs::msg::Image outputImage;
|
||||
sensor_msgs::msg::CameraInfo outputCameraInfo;
|
||||
outputCameraInfo = input->depth_camera_info;
|
||||
|
||||
if(!input->depth.data.empty())
|
||||
{
|
||||
// already raw, just copy pointer
|
||||
outputImage = input->depth;
|
||||
}
|
||||
else if(!input->depth_compressed.data.empty())
|
||||
{
|
||||
#ifdef CV_BRIDGE_HYDRO
|
||||
ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
|
||||
#else
|
||||
cv_bridge::toCvCopy(input->depth_compressed)->toImageMsg(outputImage);
|
||||
#endif
|
||||
}
|
||||
outputImage.header = outputCameraInfo.header = input->header;
|
||||
depthPub_.publish(outputImage, outputCameraInfo);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#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_util::RGBDSplit)
|
||||
|
||||
Reference in New Issue
Block a user