i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

ported point_cloud_assembler node

Browse Source
This commit is contained in:
matlabbe
2020-02-12 14:33:19 -05:00
parent 68c62d05bb
commit 2c5cf1a6b3
9 changed files with 440 additions and 343 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+8 -1
View File
@@ -221,7 +221,7 @@ SET(rtabmap_plugins_lib_src
src/nodelets/obstacles_detection.cpp
# src/nodelets/obstacles_detection_old.cpp
src/nodelets/point_cloud_aggregator.cpp
# src/nodelets/point_cloud_assembler.cpp
src/nodelets/point_cloud_assembler.cpp
# src/nodelets/undistort_depth.cpp
# src/nodelets/imu_to_tf.cpp
src/nodelets/rgbd_sync.cpp
@@ -285,6 +285,7 @@ rclcpp_components_register_nodes(rtabmap_plugins "rtabmap_ros::PointCloudXYZRGB"
rclcpp_components_register_nodes(rtabmap_plugins "rtabmap_ros::PointCloudToDepthImage")
rclcpp_components_register_nodes(rtabmap_plugins "rtabmap_ros::ObstaclesDetection")
rclcpp_components_register_nodes(rtabmap_plugins "rtabmap_ros::PointCloudAggregator")
rclcpp_components_register_nodes(rtabmap_plugins "rtabmap_ros::PointCloudAssembler")
rclcpp_components_register_nodes(rtabmap_sync "rtabmap_ros::CoreWrapper")
@@ -346,6 +347,11 @@ ament_target_dependencies(rtabmap_point_cloud_aggregator ${Libraries})
target_link_libraries(rtabmap_point_cloud_aggregator rtabmap_plugins ${RTABMap_LIBRARIES})
set_target_properties(rtabmap_point_cloud_aggregator PROPERTIES OUTPUT_NAME "point_cloud_aggregator")
add_executable(rtabmap_point_cloud_assembler src/PointCloudAssemblerNode.cpp)
ament_target_dependencies(rtabmap_point_cloud_assembler ${Libraries})
target_link_libraries(rtabmap_point_cloud_assembler rtabmap_plugins ${RTABMap_LIBRARIES})
set_target_properties(rtabmap_point_cloud_assembler PROPERTIES OUTPUT_NAME "point_cloud_assembler")
#add_executable(rtabmap_map_optimizer src/MapOptimizerNode.cpp)
#ament_target_dependencies(rtabmap_map_optimizer rtabmap_ros)
#set_target_properties(rtabmap_map_optimizer PROPERTIES OUTPUT_NAME "map_optimizer")
@@ -579,6 +585,7 @@ install(TARGETS
rtabmap_point_cloud_xyzrgb
rtabmap_obstacles_detection
rtabmap_point_cloud_aggregator
rtabmap_point_cloud_assembler
# rtabmap_camera
rtabmap_rgbd_sync
rtabmap_stereo_sync
include/rtabmap_ros/MsgConversion.h
+5
View File
@@ -238,6 +238,11 @@ bool convertScan3dMsg(
int maxPoints = 0,
float maxRange = 0.0f);
// Missing function in ros2 (from old pcl_ros)
void transformPointCloud (
const Eigen::Matrix4f &transform,
const sensor_msgs::msg::PointCloud2 &in,
sensor_msgs::msg::PointCloud2 &out);
}
#endif /* MSGCONVERSION_H_ */
include/rtabmap_ros/point_cloud_aggregator.hpp
-1
View File
@@ -93,7 +93,6 @@ private:
std::string frameId_;
std::string fixedFrameId_;
std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
std::shared_ptr<tf2_ros::TransformListener> tfListener_;
};
include/rtabmap_ros/point_cloud_assembler.hpp
+98
View File
@@ -0,0 +1,98 @@
/*
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_ros/visibility.h>
#include "rclcpp/rclcpp.hpp"
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#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>
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 rclcpp::Node
{
public:
RTABMAP_ROS_PUBLIC
explicit PointCloudAssembler(const rclcpp::NodeOptions & options);
virtual ~PointCloudAssembler();
private:
void callbackCloudOdom(
const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg,
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg);
void callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg);
private:
std::thread * warningThread_;
bool callbackCalled_;
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloudSub_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloudPub_;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::PointCloud2, nav_msgs::msg::Odometry> syncPolicy;
message_filters::Synchronizer<syncPolicy>* exactSync_;
message_filters::Subscriber<sensor_msgs::msg::PointCloud2> syncCloudSub_;
message_filters::Subscriber<nav_msgs::msg::Odometry> syncOdomSub_;
int maxClouds_;
int skipClouds_;
int cloudsSkipped_;
double assemblingTime_;
double waitForTransformDuration_;
double rangeMin_;
double rangeMax_;
double voxelSize_;
std::string fixedFrameId_;
std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
std::shared_ptr<tf2_ros::TransformListener> tfListener_;
std::vector<sensor_msgs::msg::PointCloud2::SharedPtr> clouds_;
};
}
src/MsgConversion.cpp
+108
View File
@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <image_geometry/pinhole_camera_model.h>
#include <image_geometry/stereo_camera_model.h>
#include <sensor_msgs/image_encodings.hpp>
#include <sensor_msgs/PointField.h>
#include <geometry_msgs/msg/transform.hpp>
#include <laser_geometry/laser_geometry.hpp>
#include <rtabmap/core/util3d_surface.h>
@@ -1921,4 +1922,111 @@ bool convertScan3dMsg(
return true;
}
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];
}
}
}
}
src/PointCloudAssemblerNode.cpp
+38
View File
@@ -0,0 +1,38 @@
/*
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_ros/point_cloud_assembler.hpp"
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<rtabmap_ros::PointCloudAssembler>(rclcpp::NodeOptions()));
rclcpp::shutdown();
return 0;
}
src/nodelets/point_cloud_aggregator.cpp
+4 -117
View File
@@ -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
src/nodelets/point_cloud_assembler.cpp
+179 -223
View File
@@ -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)
src/rviz/MapCloudDisplay.h
-1
View File
@@ -40,7 +40,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_ros/msg/map_data.hpp>
#include <rtabmap/core/Transform.h>
#include <pluginlib/class_loader.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include "rviz_common/message_filter_display.hpp"