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

Updated demo_hector_mapping.launch with max_range, pm and p2n options. Odometry: added postProcessData() function (used by icp_odometry to republish filtered scan after odometry processing)

Browse Source
This commit is contained in:
matlabbe
2021-03-20 19:59:38 -04:00
parent d586a8f087
commit 6e34cde8e6
7 changed files with 91 additions and 126 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/icp_odometry.cpp
+19 -79
View File
@@ -414,21 +414,6 @@ private:
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
}
if(filtered_scan_pub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 msg;
if(hasIntensity)
{
pcl::toROSMsg(*pclScanINormal, msg);
}
else
{
pcl::toROSMsg(*pclScanNormal, msg);
}
msg.header = scanMsg->header;
filtered_scan_pub_.publish(msg);
}
}
else
{
@@ -440,28 +425,18 @@ private:
{
scan = util3d::laserScan2dFromPointCloud(*pclScan);
}
if(filtered_scan_pub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 msg;
if(hasIntensity)
{
pcl::toROSMsg(*pclScanI, msg);
}
else
{
pcl::toROSMsg(*pclScan, msg);
}
msg.header = scanMsg->header;
filtered_scan_pub_.publish(msg);
}
}
}
if(scanRangeMin_ > 0 || scanRangeMax_ > 0)
{
scan = util3d::rangeFiltering(scan, scanRangeMin_, scanRangeMax_);
}
rtabmap::SensorData data(
LaserScan(scan,
maxLaserScans,
scanMsg->range_max,
scanRangeMax_>0&&scanRangeMax_<scanMsg->range_max?scanRangeMax_:scanMsg->range_max,
localScanTransform),
cv::Mat(),
cv::Mat(),
@@ -469,7 +444,7 @@ private:
0,
rtabmap_ros::timestampFromROS(scanMsg->header.stamp));
this->processData(data, scanMsg->header.stamp, "");
this->processData(data, scanMsg->header);
}
void callbackCloud(const sensor_msgs::PointCloud2ConstPtr& pointCloudMsg)
@@ -583,13 +558,6 @@ private:
}
}
scan = util3d::laserScanFromPointCloud(*pclScan);
if(filtered_scan_pub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 msg;
pcl::toROSMsg(*pclScan, msg);
msg.header = cloudMsg.header;
filtered_scan_pub_.publish(msg);
}
}
else if(hasNormals)
{
@@ -608,13 +576,6 @@ private:
}
}
scan = util3d::laserScanFromPointCloud(*pclScan);
if(filtered_scan_pub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 msg;
pcl::toROSMsg(*pclScan, msg);
msg.header = cloudMsg.header;
filtered_scan_pub_.publish(msg);
}
}
else if(hasIntensity)
{
@@ -653,26 +614,10 @@ private:
pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointXYZINormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = util3d::laserScanFromPointCloud(*pclScanNormal);
if(filtered_scan_pub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 msg;
pcl::toROSMsg(*pclScanNormal, msg);
msg.header = cloudMsg.header;
filtered_scan_pub_.publish(msg);
}
}
else
{
scan = util3d::laserScanFromPointCloud(*pclScan);
if(filtered_scan_pub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 msg;
pcl::toROSMsg(*pclScan, msg);
msg.header = cloudMsg.header;
filtered_scan_pub_.publish(msg);
}
}
}
}
@@ -713,26 +658,10 @@ private:
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = util3d::laserScanFromPointCloud(*pclScanNormal);
if(filtered_scan_pub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 msg;
pcl::toROSMsg(*pclScanNormal, msg);
msg.header = cloudMsg.header;
filtered_scan_pub_.publish(msg);
}
}
else
{
scan = util3d::laserScanFromPointCloud(*pclScan);
if(filtered_scan_pub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 msg;
pcl::toROSMsg(*pclScan, msg);
msg.header = cloudMsg.header;
filtered_scan_pub_.publish(msg);
}
}
}
}
@@ -758,7 +687,7 @@ private:
0,
rtabmap_ros::timestampFromROS(cloudMsg.header.stamp));
this->processData(data, cloudMsg.header.stamp, cloudMsg.header.frame_id);
this->processData(data, cloudMsg.header);
}
protected:
@@ -767,6 +696,17 @@ protected:
// flush callbacks
}
void postProcessData(const SensorData & data, const std_msgs::Header & header) const
{
if(filtered_scan_pub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 msg;
pcl_conversions::fromPCL(*rtabmap::util3d::laserScanToPointCloud2(data.laserScanRaw()), msg);
msg.header = header;
filtered_scan_pub_.publish(msg);
}
}
private:
ros::Subscriber scan_sub_;
ros::Subscriber cloud_sub_;
src/nodelets/rgbd_odometry.cpp
+4 -1
View File
@@ -447,7 +447,10 @@ private:
0,
rtabmap_ros::timestampFromROS(higherStamp));
this->processData(data, higherStamp, rgbImages.size()==1?rgbImages[0]->header.frame_id:"");
std_msgs::Header header;
header.stamp = higherStamp;
header.frame_id = rgbImages.size()==1?rgbImages[0]->header.frame_id:"";
this->processData(data, header);
}
void callback(
src/nodelets/rgbdicp_odometry.cpp
+4 -1
View File
@@ -418,7 +418,10 @@ private:
0,
rtabmap_ros::timestampFromROS(stamp));
this->processData(data, stamp, image->header.frame_id);
std_msgs::Header header;
header.stamp = stamp;
header.frame_id = image->header.frame_id;
this->processData(data, header);
}
}
}
src/nodelets/stereo_odometry.cpp
+8 -2
View File
@@ -280,7 +280,10 @@ private:
0,
rtabmap_ros::timestampFromROS(stamp));
this->processData(data, stamp, imageRectLeft->header.frame_id);
std_msgs::Header header;
header.stamp = stamp;
header.frame_id = imageRectLeft->header.frame_id;
this->processData(data, header);
}
else
{
@@ -422,7 +425,10 @@ private:
0,
rtabmap_ros::timestampFromROS(stamp));
this->processData(data, stamp, image->header.frame_id);
std_msgs::Header header;
header.stamp = stamp;
header.frame_id = image->header.frame_id;
this->processData(data, header);
}
else
{