mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-06 01:37:46 +08:00
ported point_cloud_assembler node
This commit is contained in:
matlabbe
@@ -35,117 +35,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
|
||||
namespace pcl_ros {
|
||||
|
||||
void
|
||||
transformPointCloud (
|
||||
const Eigen::Matrix4f &transform,
|
||||
const sensor_msgs::msg::PointCloud2 &in,
|
||||
sensor_msgs::msg::PointCloud2 &out)
|
||||
{
|
||||
// Get X-Y-Z indices
|
||||
int x_idx = pcl::getFieldIndex (in, "x");
|
||||
int y_idx = pcl::getFieldIndex (in, "y");
|
||||
int z_idx = pcl::getFieldIndex (in, "z");
|
||||
|
||||
if (x_idx == -1 || y_idx == -1 || z_idx == -1)
|
||||
{
|
||||
UERROR ("Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format.");
|
||||
return;
|
||||
}
|
||||
|
||||
if (in.fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
|
||||
in.fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
|
||||
in.fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32)
|
||||
{
|
||||
UERROR ("X-Y-Z coordinates not floats. Currently only floats are supported.");
|
||||
return;
|
||||
}
|
||||
|
||||
// Check if distance is available
|
||||
int dist_idx = pcl::getFieldIndex (in, "distance");
|
||||
|
||||
// Copy the other data
|
||||
if (&in != &out)
|
||||
{
|
||||
out.header = in.header;
|
||||
out.height = in.height;
|
||||
out.width = in.width;
|
||||
out.fields = in.fields;
|
||||
out.is_bigendian = in.is_bigendian;
|
||||
out.point_step = in.point_step;
|
||||
out.row_step = in.row_step;
|
||||
out.is_dense = in.is_dense;
|
||||
out.data.resize (in.data.size ());
|
||||
// Copy everything as it's faster than copying individual elements
|
||||
memcpy (&out.data[0], &in.data[0], in.data.size ());
|
||||
}
|
||||
|
||||
Eigen::Array4i xyz_offset (in.fields[x_idx].offset, in.fields[y_idx].offset, in.fields[z_idx].offset, 0);
|
||||
|
||||
for (size_t i = 0; i < in.width * in.height; ++i)
|
||||
{
|
||||
Eigen::Vector4f pt (*(float*)&in.data[xyz_offset[0]], *(float*)&in.data[xyz_offset[1]], *(float*)&in.data[xyz_offset[2]], 1);
|
||||
Eigen::Vector4f pt_out;
|
||||
|
||||
bool max_range_point = false;
|
||||
int distance_ptr_offset = i*in.point_step + in.fields[dist_idx].offset;
|
||||
float* distance_ptr = (dist_idx < 0 ? NULL : (float*)(&in.data[distance_ptr_offset]));
|
||||
if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2]))
|
||||
{
|
||||
if (distance_ptr==NULL || !std::isfinite(*distance_ptr)) // Invalid point
|
||||
{
|
||||
pt_out = pt;
|
||||
}
|
||||
else // max range point
|
||||
{
|
||||
pt[0] = *distance_ptr; // Replace x with the x value saved in distance
|
||||
pt_out = transform * pt;
|
||||
max_range_point = true;
|
||||
//std::cout << pt[0]<<","<<pt[1]<<","<<pt[2]<<" => "<<pt_out[0]<<","<<pt_out[1]<<","<<pt_out[2]<<"\n";
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
pt_out = transform * pt;
|
||||
}
|
||||
|
||||
if (max_range_point)
|
||||
{
|
||||
// Save x value in distance again
|
||||
*(float*)(&out.data[distance_ptr_offset]) = pt_out[0];
|
||||
pt_out[0] = std::numeric_limits<float>::quiet_NaN();
|
||||
}
|
||||
|
||||
memcpy (&out.data[xyz_offset[0]], &pt_out[0], sizeof (float));
|
||||
memcpy (&out.data[xyz_offset[1]], &pt_out[1], sizeof (float));
|
||||
memcpy (&out.data[xyz_offset[2]], &pt_out[2], sizeof (float));
|
||||
|
||||
|
||||
xyz_offset += in.point_step;
|
||||
}
|
||||
|
||||
// Check if the viewpoint information is present
|
||||
int vp_idx = pcl::getFieldIndex (in, "vp_x");
|
||||
if (vp_idx != -1)
|
||||
{
|
||||
// Transform the viewpoint info too
|
||||
for (size_t i = 0; i < out.width * out.height; ++i)
|
||||
{
|
||||
float *pstep = (float*)&out.data[i * out.point_step + out.fields[vp_idx].offset];
|
||||
// Assume vp_x, vp_y, vp_z are consecutive
|
||||
Eigen::Vector4f vp_in (pstep[0], pstep[1], pstep[2], 1);
|
||||
Eigen::Vector4f vp_out = transform * vp_in;
|
||||
|
||||
pstep[0] = vp_out[0];
|
||||
pstep[1] = vp_out[1];
|
||||
pstep[2] = vp_out[2];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
namespace rtabmap_ros
|
||||
{
|
||||
|
||||
@@ -331,7 +220,7 @@ void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::Poi
|
||||
{
|
||||
return;
|
||||
}
|
||||
pcl_ros::transformPointCloud(t.toEigen4f(), *cloudMsgs[0], tmp);
|
||||
rtabmap_ros::transformPointCloud(t.toEigen4f(), *cloudMsgs[0], tmp);
|
||||
pcl_conversions::toPCL(tmp, output);
|
||||
}
|
||||
else
|
||||
@@ -343,7 +232,6 @@ void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::Poi
|
||||
for(unsigned int i=1; i<cloudMsgs.size(); ++i)
|
||||
{
|
||||
rtabmap::Transform cloudDisplacement;
|
||||
bool notsync = false;
|
||||
if(!fixedFrameId_.empty() &&
|
||||
cloudMsgs[0]->header.stamp != cloudMsgs[i]->header.stamp)
|
||||
{
|
||||
@@ -355,7 +243,6 @@ void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::Poi
|
||||
cloudMsgs[0]->header.stamp, //stampTarget
|
||||
*tfBuffer_,
|
||||
0.1);
|
||||
notsync = true;
|
||||
}
|
||||
|
||||
pcl::PCLPointCloud2 cloud2;
|
||||
@@ -363,11 +250,11 @@ void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::Poi
|
||||
{
|
||||
sensor_msgs::msg::PointCloud2 tmp;
|
||||
rtabmap::Transform t = rtabmap_ros::getTransform(frameId, cloudMsgs[i]->header.frame_id, cloudMsgs[i]->header.stamp, *tfBuffer_, 0.1);
|
||||
pcl_ros::transformPointCloud(t.toEigen4f(), *cloudMsgs[i], tmp);
|
||||
rtabmap_ros::transformPointCloud(t.toEigen4f(), *cloudMsgs[i], tmp);
|
||||
if(!cloudDisplacement.isNull())
|
||||
{
|
||||
sensor_msgs::msg::PointCloud2 tmp2;
|
||||
pcl_ros::transformPointCloud(cloudDisplacement.toEigen4f(), tmp, tmp2);
|
||||
rtabmap_ros::transformPointCloud(cloudDisplacement.toEigen4f(), tmp, tmp2);
|
||||
pcl_conversions::toPCL(tmp2, cloud2);
|
||||
}
|
||||
else
|
||||
@@ -381,7 +268,7 @@ void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::Poi
|
||||
if(!cloudDisplacement.isNull())
|
||||
{
|
||||
sensor_msgs::msg::PointCloud2 tmp;
|
||||
pcl_ros::transformPointCloud(cloudDisplacement.toEigen4f(), *cloudMsgs[i], tmp);
|
||||
rtabmap_ros::transformPointCloud(cloudDisplacement.toEigen4f(), *cloudMsgs[i], tmp);
|
||||
pcl_conversions::toPCL(tmp, cloud2);
|
||||
}
|
||||
else
|
||||
|
||||
@@ -25,9 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include <nodelet/nodelet.h>
|
||||
#include <rtabmap_ros/point_cloud_assembler.hpp>
|
||||
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
@@ -35,16 +33,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <pcl/io/pcd_io.h>
|
||||
#include <pcl/filters/voxel_grid.h>
|
||||
|
||||
#include <pcl_ros/transforms.h>
|
||||
|
||||
#include <tf/transform_listener.h>
|
||||
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/sync_policies/exact_time.h>
|
||||
|
||||
#include <rtabmap_ros/MsgConversion.h>
|
||||
#include <rtabmap/core/util3d.h>
|
||||
#include <rtabmap/core/util3d_filtering.h>
|
||||
@@ -52,239 +40,207 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
namespace rtabmap_ros
|
||||
{
|
||||
|
||||
/**
|
||||
* This nodelet can assemble a number of clouds (max_clouds) coming
|
||||
* from the same sensor, taking into account the displacement of the robot based on
|
||||
* fixed_frame_id, then publish the resulting cloud.
|
||||
* If fixed_frame_id is set to "" (empty), the nodelet will subscribe to
|
||||
* an odom topic that should have the exact same stamp than to input cloud.
|
||||
* The output cloud has the same stamp and frame than the last assembled cloud.
|
||||
*/
|
||||
class PointCloudAssembler : public nodelet::Nodelet
|
||||
PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
|
||||
Node("pointcloud_to_depthimage", options),
|
||||
warningThread_(0),
|
||||
callbackCalled_(false),
|
||||
exactSync_(0),
|
||||
maxClouds_(0),
|
||||
skipClouds_(0),
|
||||
cloudsSkipped_(0),
|
||||
assemblingTime_(0),
|
||||
waitForTransformDuration_(0.1),
|
||||
rangeMin_(0),
|
||||
rangeMax_(0),
|
||||
voxelSize_(0),
|
||||
fixedFrameId_("odom")
|
||||
{
|
||||
public:
|
||||
PointCloudAssembler() :
|
||||
warningThread_(0),
|
||||
callbackCalled_(false),
|
||||
exactSync_(0),
|
||||
maxClouds_(0),
|
||||
assemblingTime_(0),
|
||||
skipClouds_(0),
|
||||
cloudsSkipped_(0),
|
||||
waitForTransformDuration_(0.1),
|
||||
rangeMin_(0),
|
||||
rangeMax_(0),
|
||||
voxelSize_(0),
|
||||
fixedFrameId_("odom")
|
||||
{}
|
||||
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_);
|
||||
|
||||
virtual ~PointCloudAssembler()
|
||||
int queueSize = 5;
|
||||
|
||||
queueSize = this->declare_parameter("queue_size", queueSize);
|
||||
fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
|
||||
maxClouds_ = this->declare_parameter("max_clouds", maxClouds_);
|
||||
assemblingTime_ = this->declare_parameter("assembling_time", assemblingTime_);
|
||||
skipClouds_ = this->declare_parameter("skip_clouds", skipClouds_);
|
||||
waitForTransformDuration_ = this->declare_parameter("wait_for_transform", waitForTransformDuration_);
|
||||
rangeMin_ = this->declare_parameter("range_min", rangeMin_);
|
||||
rangeMax_ = this->declare_parameter("range_max", rangeMax_);
|
||||
voxelSize_ = this->declare_parameter("voxel_size", voxelSize_);
|
||||
UASSERT(maxClouds_>0 || assemblingTime_ >0.0);
|
||||
|
||||
cloudsSkipped_ = skipClouds_;
|
||||
|
||||
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("assembled_cloud", 1);
|
||||
|
||||
std::string subscribedTopicsMsg;
|
||||
if(!fixedFrameId_.empty())
|
||||
{
|
||||
delete exactSync_;
|
||||
|
||||
if(warningThread_)
|
||||
{
|
||||
callbackCalled_=true;
|
||||
warningThread_->join();
|
||||
delete warningThread_;
|
||||
}
|
||||
cloudSub_ = create_subscription<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::SensorDataQoS(), std::bind(&PointCloudAssembler::callbackCloud, this, std::placeholders::_1));
|
||||
subscribedTopicsMsg = uFormat("\n%s subscribed to %s",
|
||||
get_name(),
|
||||
cloudSub_->get_topic_name());
|
||||
}
|
||||
|
||||
private:
|
||||
virtual void onInit()
|
||||
else
|
||||
{
|
||||
ros::NodeHandle & nh = getNodeHandle();
|
||||
ros::NodeHandle & pnh = getPrivateNodeHandle();
|
||||
syncCloudSub_.subscribe(this, "cloud", rmw_qos_profile_sensor_data);
|
||||
syncOdomSub_.subscribe(this, "odom", rmw_qos_profile_sensor_data);
|
||||
exactSync_ = new message_filters::Synchronizer<syncPolicy>(syncPolicy(queueSize), syncCloudSub_, syncOdomSub_);
|
||||
exactSync_->registerCallback(std::bind(&rtabmap_ros::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());
|
||||
|
||||
int queueSize = 5;
|
||||
|
||||
pnh.param("queue_size", queueSize, queueSize);
|
||||
pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
|
||||
pnh.param("max_clouds", maxClouds_, maxClouds_);
|
||||
pnh.param("assembling_time", assemblingTime_, assemblingTime_);
|
||||
pnh.param("skip_clouds", skipClouds_, skipClouds_);
|
||||
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
|
||||
pnh.param("range_min", rangeMin_, rangeMin_);
|
||||
pnh.param("range_max", rangeMax_, rangeMax_);
|
||||
pnh.param("voxel_size", voxelSize_, voxelSize_);
|
||||
ROS_ASSERT(maxClouds_>0 || assemblingTime_ >0.0);
|
||||
|
||||
cloudsSkipped_ = skipClouds_;
|
||||
|
||||
std::string subscribedTopicsMsg;
|
||||
if(!fixedFrameId_.empty())
|
||||
{
|
||||
cloudSub_ = nh.subscribe("cloud", 1, &PointCloudAssembler::callbackCloud, this);
|
||||
subscribedTopicsMsg = uFormat("\n%s subscribed to %s",
|
||||
getName().c_str(),
|
||||
cloudSub_.getTopic().c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
syncCloudSub_.subscribe(nh, "cloud", 1);
|
||||
syncOdomSub_.subscribe(nh, "odom", 1);
|
||||
exactSync_ = new message_filters::Synchronizer<syncPolicy>(syncPolicy(queueSize), syncCloudSub_, syncOdomSub_);
|
||||
exactSync_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAssembler::callbackCloudOdom, this, _1, _2));
|
||||
subscribedTopicsMsg = uFormat("\n%s subscribed to (exact sync):\n %s,\n %s",
|
||||
getName().c_str(),
|
||||
syncCloudSub_.getTopic().c_str(),
|
||||
syncOdomSub_.getTopic().c_str());
|
||||
|
||||
warningThread_ = new boost::thread(boost::bind(&PointCloudAssembler::warningLoop, this, subscribedTopicsMsg));
|
||||
}
|
||||
|
||||
cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("assembled_cloud", 1);
|
||||
|
||||
NODELET_INFO("%s", subscribedTopicsMsg.c_str());
|
||||
}
|
||||
|
||||
void callbackCloudOdom(
|
||||
const sensor_msgs::PointCloud2ConstPtr & cloudMsg,
|
||||
const nav_msgs::OdometryConstPtr & odomMsg)
|
||||
{
|
||||
callbackCalled_ = true;
|
||||
rtabmap::Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose);
|
||||
if(!odom.isNull())
|
||||
{
|
||||
fixedFrameId_ = odomMsg->header.frame_id;
|
||||
callbackCloud(cloudMsg);
|
||||
}
|
||||
else
|
||||
{
|
||||
NODELET_WARN("Reseting point cloud assembler as null odometry has been received.");
|
||||
clouds_.clear();
|
||||
}
|
||||
}
|
||||
|
||||
void callbackCloud(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
|
||||
{
|
||||
if(cloudPub_.getNumSubscribers())
|
||||
{
|
||||
if(skipClouds_<=0 || cloudsSkipped_ >= skipClouds_)
|
||||
{
|
||||
cloudsSkipped_ = 0;
|
||||
|
||||
sensor_msgs::PointCloud2Ptr cpy(new sensor_msgs::PointCloud2);
|
||||
*cpy = *cloudMsg;
|
||||
clouds_.push_back(cpy);
|
||||
|
||||
if( (int)clouds_.size() >= maxClouds_ && maxClouds_ > 0
|
||||
||
|
||||
(double)(*cpy).header.stamp.toSec() >= (double)clouds_[0]->header.stamp.toSec() + assemblingTime_ && assemblingTime_ > 0.0 )
|
||||
warningThread_ = new std::thread([&](){
|
||||
rclcpp::Rate r(1.0/5.0);
|
||||
while(!callbackCalled_)
|
||||
{
|
||||
pcl::PCLPointCloud2Ptr assembled(new pcl::PCLPointCloud2);
|
||||
pcl_conversions::toPCL(*clouds_.back(), *assembled);
|
||||
|
||||
for(size_t i=0; i<clouds_.size()-1; ++i)
|
||||
r.sleep();
|
||||
if(!callbackCalled_)
|
||||
{
|
||||
rtabmap::Transform t = rtabmap_ros::getTransform(
|
||||
clouds_[i]->header.frame_id, //sourceTargetFrame
|
||||
fixedFrameId_, //fixedFrame
|
||||
clouds_[i]->header.stamp, //stampSource
|
||||
clouds_.back()->header.stamp, //stampTarget
|
||||
tfListener_,
|
||||
waitForTransformDuration_);
|
||||
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());
|
||||
}
|
||||
}
|
||||
});
|
||||
|
||||
if(t.isNull())
|
||||
{
|
||||
ROS_ERROR("Cloud not transform all clouds! Resetting...");
|
||||
clouds_.clear();
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
pcl::PCLPointCloud2Ptr assembledTmp(new pcl::PCLPointCloud2);
|
||||
if(rangeMin_ > 0.0 || rangeMax_ > 0.0)
|
||||
{
|
||||
pcl::PCLPointCloud2 output2;
|
||||
pcl_conversions::toPCL(*clouds_[i], output2);
|
||||
rtabmap::LaserScan scan = rtabmap::util3d::laserScanFromPointCloud(output2);
|
||||
if(rangeMin_ > 0.0 || rangeMax_ > 0.0)
|
||||
{
|
||||
scan = rtabmap::util3d::rangeFiltering(scan, rangeMin_, rangeMax_);
|
||||
}
|
||||
pcl::concatenatePointCloud(*assembled, *rtabmap::util3d::laserScanToPointCloud2(scan, t), *assembledTmp);
|
||||
}
|
||||
else
|
||||
{
|
||||
sensor_msgs::PointCloud2 output;
|
||||
pcl_ros::transformPointCloud(t.toEigen4f(), *clouds_[i], output);
|
||||
pcl::PCLPointCloud2 output2;
|
||||
pcl_conversions::toPCL(output, output2);
|
||||
pcl::concatenatePointCloud(*assembled, output2, *assembledTmp);
|
||||
}
|
||||
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
|
||||
}
|
||||
|
||||
assembled = assembledTmp;
|
||||
PointCloudAssembler::~PointCloudAssembler()
|
||||
{
|
||||
delete exactSync_;
|
||||
|
||||
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_ros::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();
|
||||
}
|
||||
}
|
||||
|
||||
void PointCloudAssembler::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg)
|
||||
{
|
||||
if(cloudPub_->get_subscription_count())
|
||||
{
|
||||
if(skipClouds_<=0 || cloudsSkipped_ >= skipClouds_)
|
||||
{
|
||||
cloudsSkipped_ = 0;
|
||||
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr cpy(new sensor_msgs::msg::PointCloud2);
|
||||
*cpy = *cloudMsg;
|
||||
clouds_.push_back(cpy);
|
||||
|
||||
if( ((int)clouds_.size() >= maxClouds_ && maxClouds_ > 0)
|
||||
||
|
||||
(timestampFromROS((*cpy).header.stamp) >= timestampFromROS(clouds_[0]->header.stamp) + assemblingTime_ && assemblingTime_ > 0.0))
|
||||
{
|
||||
pcl::PCLPointCloud2Ptr assembled(new pcl::PCLPointCloud2);
|
||||
pcl_conversions::toPCL(*clouds_.back(), *assembled);
|
||||
|
||||
for(size_t i=0; i<clouds_.size()-1; ++i)
|
||||
{
|
||||
rtabmap::Transform t = rtabmap_ros::getTransform(
|
||||
clouds_[i]->header.frame_id, //sourceTargetFrame
|
||||
fixedFrameId_, //fixedFrame
|
||||
clouds_[i]->header.stamp, //stampSource
|
||||
clouds_.back()->header.stamp, //stampTarget
|
||||
*tfBuffer_,
|
||||
waitForTransformDuration_);
|
||||
|
||||
if(t.isNull())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Cloud not transform all clouds! Resetting...");
|
||||
clouds_.clear();
|
||||
return;
|
||||
}
|
||||
|
||||
sensor_msgs::PointCloud2 rosCloud;
|
||||
if(voxelSize_>0.0)
|
||||
pcl::PCLPointCloud2Ptr assembledTmp(new pcl::PCLPointCloud2);
|
||||
if(rangeMin_ > 0.0 || rangeMax_ > 0.0)
|
||||
{
|
||||
pcl::VoxelGrid<pcl::PCLPointCloud2> filter;
|
||||
filter.setLeafSize(voxelSize_, voxelSize_, voxelSize_);
|
||||
filter.setInputCloud(assembled);
|
||||
pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2);
|
||||
filter.filter(*output);
|
||||
pcl_conversions::moveFromPCL(*output, rosCloud);
|
||||
pcl::PCLPointCloud2 output2;
|
||||
pcl_conversions::toPCL(*clouds_[i], output2);
|
||||
rtabmap::LaserScan scan = rtabmap::util3d::laserScanFromPointCloud(output2);
|
||||
if(rangeMin_ > 0.0 || rangeMax_ > 0.0)
|
||||
{
|
||||
scan = rtabmap::util3d::rangeFiltering(scan, rangeMin_, rangeMax_);
|
||||
}
|
||||
pcl::concatenatePointCloud(*assembled, *rtabmap::util3d::laserScanToPointCloud2(scan, t), *assembledTmp);
|
||||
}
|
||||
else
|
||||
{
|
||||
pcl_conversions::moveFromPCL(*assembled, rosCloud);
|
||||
sensor_msgs::msg::PointCloud2 output;
|
||||
rtabmap_ros::transformPointCloud(t.toEigen4f(), *clouds_[i], output);
|
||||
pcl::PCLPointCloud2 output2;
|
||||
pcl_conversions::toPCL(output, output2);
|
||||
pcl::concatenatePointCloud(*assembled, output2, *assembledTmp);
|
||||
}
|
||||
rosCloud.header = cloudMsg->header;
|
||||
cloudPub_.publish(rosCloud);
|
||||
clouds_.clear();
|
||||
|
||||
assembled = assembledTmp;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
++cloudsSkipped_;
|
||||
|
||||
sensor_msgs::msg::PointCloud2 rosCloud;
|
||||
if(voxelSize_>0.0)
|
||||
{
|
||||
pcl::VoxelGrid<pcl::PCLPointCloud2> filter;
|
||||
filter.setLeafSize(voxelSize_, voxelSize_, voxelSize_);
|
||||
filter.setInputCloud(assembled);
|
||||
pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2);
|
||||
filter.filter(*output);
|
||||
pcl_conversions::moveFromPCL(*output, rosCloud);
|
||||
}
|
||||
else
|
||||
{
|
||||
pcl_conversions::moveFromPCL(*assembled, rosCloud);
|
||||
}
|
||||
rosCloud.header = cloudMsg->header;
|
||||
cloudPub_->publish(rosCloud);
|
||||
clouds_.clear();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void warningLoop(const std::string & subscribedTopicsMsg)
|
||||
{
|
||||
ros::Duration r(5.0);
|
||||
while(!callbackCalled_)
|
||||
else
|
||||
{
|
||||
r.sleep();
|
||||
if(!callbackCalled_)
|
||||
{
|
||||
ROS_WARN("%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",
|
||||
getName().c_str(),
|
||||
subscribedTopicsMsg.c_str());
|
||||
}
|
||||
++cloudsSkipped_;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
boost::thread * warningThread_;
|
||||
bool callbackCalled_;
|
||||
|
||||
ros::Subscriber cloudSub_;
|
||||
ros::Publisher cloudPub_;
|
||||
|
||||
typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, nav_msgs::Odometry> syncPolicy;
|
||||
message_filters::Synchronizer<syncPolicy>* exactSync_;
|
||||
message_filters::Subscriber<sensor_msgs::PointCloud2> syncCloudSub_;
|
||||
message_filters::Subscriber<nav_msgs::Odometry> syncOdomSub_;
|
||||
|
||||
int maxClouds_;
|
||||
int skipClouds_;
|
||||
int cloudsSkipped_;
|
||||
double assemblingTime_;
|
||||
double waitForTransformDuration_;
|
||||
double rangeMin_;
|
||||
double rangeMax_;
|
||||
double voxelSize_;
|
||||
std::string fixedFrameId_;
|
||||
tf::TransformListener tfListener_;
|
||||
|
||||
std::vector<sensor_msgs::PointCloud2Ptr> clouds_;
|
||||
};
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudAssembler, nodelet::Nodelet);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#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_ros::PointCloudAssembler)
|
||||
|
||||
Reference in New Issue
Block a user