mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-05 17:27:46 +08:00
reverted fa53c7c000
This commit is contained in:
matlabbe
@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/point_cloud2_iterator.h>
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/sync_policies/exact_time.h>
|
||||
@@ -50,9 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <rtabmap_ros/OdomInfo.h>
|
||||
#include <rtabmap/core/util3d.h>
|
||||
#include <rtabmap/core/util3d_filtering.h>
|
||||
|
||||
//ADC
|
||||
#include <sensor_msgs/Range.h>
|
||||
#include <rtabmap/core/Version.h>
|
||||
|
||||
namespace rtabmap_ros
|
||||
{
|
||||
@@ -78,16 +77,17 @@ public:
|
||||
skipClouds_(0),
|
||||
cloudsSkipped_(0),
|
||||
circularBuffer_(false),
|
||||
linearUpdate_(0),
|
||||
angularUpdate_(0),
|
||||
waitForTransformDuration_(0.1),
|
||||
rangeMin_(0),
|
||||
rangeMax_(0),
|
||||
voxelSize_(0),
|
||||
noiseRadius_(0),
|
||||
noiseMinNeighbors_(5),
|
||||
removeZ_(false),
|
||||
fixedFrameId_("odom"),
|
||||
frameId_(""),
|
||||
//ADC
|
||||
use_lidar_topics_(false)
|
||||
frameId_("")
|
||||
{}
|
||||
|
||||
virtual ~PointCloudAssembler()
|
||||
@@ -119,20 +119,17 @@ private:
|
||||
pnh.param("assembling_time", assemblingTime_, assemblingTime_);
|
||||
pnh.param("skip_clouds", skipClouds_, skipClouds_);
|
||||
pnh.param("circular_buffer", circularBuffer_, circularBuffer_);
|
||||
pnh.param("linear_update", linearUpdate_, linearUpdate_);
|
||||
pnh.param("angular_update", angularUpdate_, angularUpdate_);
|
||||
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_);
|
||||
pnh.param("noise_radius", noiseRadius_, noiseRadius_);
|
||||
pnh.param("noise_min_neighbors", noiseMinNeighbors_, noiseMinNeighbors_);
|
||||
pnh.param("remove_z", removeZ_, removeZ_);
|
||||
pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
|
||||
ROS_ASSERT(maxClouds_>0 || assemblingTime_ >0.0);
|
||||
//ADC
|
||||
pnh.param("use_lidar_topics", use_lidar_topics_, use_lidar_topics_);
|
||||
std::vector<std::string> lidar_topics;
|
||||
pnh.param("lidar_topics", lidar_topics, lidar_topics);
|
||||
lidarSub_.resize(lidar_topics.size());
|
||||
|
||||
|
||||
ROS_INFO("%s: queue_size=%d", getName().c_str(), queueSize);
|
||||
ROS_INFO("%s: fixed_frame_id=%s", getName().c_str(), fixedFrameId_.c_str());
|
||||
@@ -141,40 +138,31 @@ private:
|
||||
ROS_INFO("%s: assembling_time=%fs", getName().c_str(), assemblingTime_);
|
||||
ROS_INFO("%s: skip_clouds=%d", getName().c_str(), skipClouds_);
|
||||
ROS_INFO("%s: circular_buffer=%s", getName().c_str(), circularBuffer_?"true":"false");
|
||||
ROS_INFO("%s: linear_update=%f m", getName().c_str(), linearUpdate_);
|
||||
ROS_INFO("%s: angular_update=%f rad", getName().c_str(), angularUpdate_);
|
||||
ROS_INFO("%s: wait_for_transform_duration=%f", getName().c_str(), waitForTransformDuration_);
|
||||
ROS_INFO("%s: range_min=%f", getName().c_str(), rangeMin_);
|
||||
ROS_INFO("%s: range_max=%f", getName().c_str(), rangeMax_);
|
||||
ROS_INFO("%s: voxel_size=%fm", getName().c_str(), voxelSize_);
|
||||
ROS_INFO("%s: noise_radius=%fm", getName().c_str(), noiseRadius_);
|
||||
ROS_INFO("%s: noise_min_neighbors=%d", getName().c_str(), noiseMinNeighbors_);
|
||||
//ADC
|
||||
ROS_INFO("%s: use_lidar_topics=%d", getName().c_str(), use_lidar_topics_);
|
||||
ROS_INFO("%s: remove_z=%s", getName().c_str(), removeZ_?"true":"false");
|
||||
|
||||
if(maxClouds_==0 && assemblingTime_ ==0.0)
|
||||
{
|
||||
ROS_ERROR("point_cloud_assembler: max_cloud or assembling_time parameters should be set!");
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
cloudsSkipped_ = skipClouds_;
|
||||
|
||||
std::string subscribedTopicsMsg;
|
||||
if(!fixedFrameId_.empty())
|
||||
{
|
||||
if(!use_lidar_topics_)
|
||||
{
|
||||
ROS_INFO("ADC: fixedFrameId != empty");
|
||||
cloudSub_ = nh.subscribe("cloud", queueSize, &PointCloudAssembler::callbackCloud, this);
|
||||
subscribedTopicsMsg = uFormat("\n%s subscribed to %s",
|
||||
getName().c_str(),
|
||||
cloudSub_.getTopic().c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
if (lidar_topics.size() == 0)
|
||||
{
|
||||
ROS_WARN("Missing lidar topics!");
|
||||
}
|
||||
for (int i = 0; i < lidar_topics.size(); ++i)
|
||||
{
|
||||
ROS_INFO("Subscribing to lidar_topics `%s`", lidar_topics[i].c_str());
|
||||
lidarSub_[i] = nh.subscribe(lidar_topics[i], queueSize, &PointCloudAssembler::callbackLidar, this);
|
||||
}
|
||||
}
|
||||
cloudSub_ = nh.subscribe("cloud", queueSize, &PointCloudAssembler::callbackCloud, this);
|
||||
subscribedTopicsMsg = uFormat("\n%s subscribed to %s",
|
||||
getName().c_str(),
|
||||
cloudSub_.getTopic().c_str());
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
@@ -254,29 +242,50 @@ private:
|
||||
}
|
||||
}
|
||||
|
||||
void callbackLidar(const sensor_msgs::RangeConstPtr & lidarMsg)
|
||||
sensor_msgs::PointCloud2 removeField(const sensor_msgs::PointCloud2 & input, const std::string & field)
|
||||
{
|
||||
if(lidarMsg->range > 0.1)
|
||||
sensor_msgs::PointCloud2 output;
|
||||
int offset = 0;
|
||||
std::vector<int> inputFieldIndex;
|
||||
for(size_t i=0; i<input.fields.size(); ++i)
|
||||
{
|
||||
callbackCalled_ = true;
|
||||
|
||||
//transform lidar to PC
|
||||
pcl::PointCloud<pcl::PointXYZ>* pcl_cloud = new pcl::PointCloud<pcl::PointXYZ>;
|
||||
pcl_cloud->push_back (pcl::PointXYZ (lidarMsg->range, 0, 0));
|
||||
|
||||
//const sensor_msgs::PointCloudConstPtr pcl_ptr(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
//pcl::PointCloud<pcl::PointXYZ>::Ptr assembledGround_(pcl);
|
||||
sensor_msgs::PointCloud2::Ptr pc2(new sensor_msgs::PointCloud2);
|
||||
|
||||
pcl::toROSMsg(*pcl_cloud, *pc2);
|
||||
pc2->header = lidarMsg->header;
|
||||
callbackCloud(pc2);
|
||||
if(input.fields[i].name.compare(field) == 0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
else
|
||||
{
|
||||
sensor_msgs::PointField outputField = input.fields[i];
|
||||
outputField.offset = offset;
|
||||
offset += outputField.count * 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 * sizeOfPointField(output.fields[j].datatype));
|
||||
}
|
||||
}
|
||||
return output;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void callbackCloud(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
|
||||
{
|
||||
if(cloudPub_.getNumSubscribers())
|
||||
@@ -287,20 +296,36 @@ private:
|
||||
if(skipClouds_<=0 || cloudsSkipped_ >= skipClouds_)
|
||||
{
|
||||
cloudsSkipped_ = 0;
|
||||
rtabmap::Transform t = rtabmap_ros::getTransform(
|
||||
|
||||
rtabmap::Transform pose = rtabmap_ros::getTransform(
|
||||
fixedFrameId_, //fromFrame
|
||||
cloudMsg->header.frame_id, //toFrame
|
||||
cloudMsg->header.stamp,
|
||||
*tfListener_,
|
||||
tfListener_,
|
||||
waitForTransformDuration_);
|
||||
|
||||
if(t.isNull())
|
||||
if(pose.isNull())
|
||||
{
|
||||
ROS_WARN("Cloud not use transform! Ignoring...");
|
||||
//clouds_.clear(); //comment out to prevent PC reset
|
||||
ROS_ERROR("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)
|
||||
{
|
||||
@@ -312,13 +337,13 @@ private:
|
||||
#else
|
||||
pcl::uint64_t stamp = newCloud->header.stamp;
|
||||
#endif
|
||||
newCloud = rtabmap::util3d::laserScanToPointCloud2(scan, t);
|
||||
newCloud = rtabmap::util3d::laserScanToPointCloud2(scan, pose);
|
||||
newCloud->header.stamp = stamp;
|
||||
}
|
||||
else
|
||||
{
|
||||
sensor_msgs::PointCloud2 output;
|
||||
pcl_ros::transformPointCloud(t.toEigen4f(), *cloudMsg, output);
|
||||
pcl_ros::transformPointCloud(pose.toEigen4f(), *cloudMsg, output);
|
||||
pcl_conversions::toPCL(output, *newCloud);
|
||||
}
|
||||
|
||||
@@ -368,12 +393,52 @@ private:
|
||||
sensor_msgs::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);
|
||||
assembled = output;
|
||||
// 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)
|
||||
{
|
||||
@@ -387,6 +452,7 @@ private:
|
||||
}
|
||||
|
||||
pcl_conversions::moveFromPCL(*assembled, rosCloud);
|
||||
rtabmap::Transform t = pose;
|
||||
if(!frameId_.empty())
|
||||
{
|
||||
// transform in target frame_id instead of sensor frame
|
||||
@@ -394,41 +460,57 @@ private:
|
||||
fixedFrameId_, //fromFrame
|
||||
frameId_, //toFrame
|
||||
cloudMsg->header.stamp,
|
||||
*tfListener_,
|
||||
tfListener_,
|
||||
waitForTransformDuration_);
|
||||
if(t.isNull())
|
||||
{
|
||||
ROS_ERROR("Cloud not transform back assembled clouds in target frame \"%s\"! Ignoring...", frameId_.c_str());
|
||||
//clouds_.clear(); // don;t clear the cloud
|
||||
ROS_ERROR("Cloud not transform back assembled clouds in target frame \"%s\"! Resetting...", frameId_.c_str());
|
||||
clouds_.clear();
|
||||
return;
|
||||
}
|
||||
}
|
||||
pcl_ros::transformPointCloud(t.toEigen4f().inverse(), rosCloud, rosCloud);
|
||||
|
||||
if(removeZ_)
|
||||
{
|
||||
rosCloud = removeField(rosCloud, "z");
|
||||
}
|
||||
|
||||
rosCloud.header = cloudMsg->header;
|
||||
if(!frameId_.empty())
|
||||
{
|
||||
rosCloud.header.frame_id = frameId_;
|
||||
}
|
||||
ros::Duration rate_sleep = ros::Duration(0.1);
|
||||
if((rosCloud.header.stamp - cloudPub_timestamp) > rate_sleep )
|
||||
{
|
||||
cloudPub_timestamp = rosCloud.header.stamp;
|
||||
cloudPub_.publish(rosCloud);
|
||||
}
|
||||
cloudPub_.publish(rosCloud);
|
||||
if(circularBuffer_)
|
||||
{
|
||||
if(reachedMaxSize)
|
||||
if(!isMoving)
|
||||
{
|
||||
clouds_.pop_front();
|
||||
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
|
||||
{
|
||||
@@ -473,6 +555,8 @@ private:
|
||||
int skipClouds_;
|
||||
int cloudsSkipped_;
|
||||
bool circularBuffer_;
|
||||
double linearUpdate_;
|
||||
double angularUpdate_;
|
||||
double assemblingTime_;
|
||||
double waitForTransformDuration_;
|
||||
double rangeMin_;
|
||||
@@ -480,19 +564,13 @@ private:
|
||||
double voxelSize_;
|
||||
double noiseRadius_;
|
||||
int noiseMinNeighbors_;
|
||||
bool removeZ_;
|
||||
std::string fixedFrameId_;
|
||||
std::string frameId_;
|
||||
//tf::TransformListener tfListener_;
|
||||
ros::Duration not_time = ros::Duration(1000);
|
||||
tf::TransformListener* tfListener_ = new tf::TransformListener(not_time, true);
|
||||
tf::TransformListener tfListener_;
|
||||
rtabmap::Transform previousPose_;
|
||||
|
||||
std::list<pcl::PCLPointCloud2::Ptr> clouds_;
|
||||
|
||||
//ADC
|
||||
bool use_lidar_topics_;
|
||||
std::vector<ros::Subscriber> lidarSub_;
|
||||
sensor_msgs::PointCloud2 LidarCloudMsg;
|
||||
ros::Time cloudPub_timestamp = ros::Time(0.0);
|
||||
};
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudAssembler, nodelet::Nodelet);
|
||||
|
||||
Reference in New Issue
Block a user