From 9deb8a2a81f7a12c41d2ec61868d0ab24b5e5510 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Thu, 5 Sep 2024 22:04:20 -0700
Subject: [PATCH] Fixed #1198

---
 .../rtabmap_util/obstacles_detection.hpp      |  2 +
 .../src/nodelets/obstacles_detection.cpp      | 47 ++++++++++++++++++-
 2 files changed, 48 insertions(+), 1 deletion(-)

diff --git a/rtabmap_util/include/rtabmap_util/obstacles_detection.hpp b/rtabmap_util/include/rtabmap_util/obstacles_detection.hpp
index 6cadfc1a..9610886a 100644
--- a/rtabmap_util/include/rtabmap_util/obstacles_detection.hpp
+++ b/rtabmap_util/include/rtabmap_util/obstacles_detection.hpp
@@ -56,6 +56,8 @@ private:
 	rtabmap::LocalGridMaker localMapMaker_;
 	bool mapFrameProjection_;
 	bool warned_;
+	float rangeMin_;
+	float rangeMax_;
 
 	std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
 	std::shared_ptr<tf2_ros::TransformListener> tfListener_;
diff --git a/rtabmap_util/src/nodelets/obstacles_detection.cpp b/rtabmap_util/src/nodelets/obstacles_detection.cpp
index ac535b7b..c9a99db0 100644
--- a/rtabmap_util/src/nodelets/obstacles_detection.cpp
+++ b/rtabmap_util/src/nodelets/obstacles_detection.cpp
@@ -45,7 +45,9 @@ ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) :
 	frameId_("base_link"),
 	waitForTransform_(0.2),
 	mapFrameProjection_(rtabmap::Parameters::defaultGridMapFrameProjection()),
-	warned_(false)
+	warned_(false),
+	rangeMin_(0),
+	rangeMax_(0)
 {
 	ULogger::setType(ULogger::kTypeConsole);
 	ULogger::setLevel(ULogger::kWarning);
@@ -75,6 +77,8 @@ ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) :
 	}
 
 	localMapMaker_.parseParameters(gridParameters);
+	rtabmap::Parameters::parse(gridParameters, rtabmap::Parameters::kGridRangeMin(), rangeMin_);
+	rtabmap::Parameters::parse(gridParameters, rtabmap::Parameters::kGridRangeMax(), rangeMax_);
 
 	tfBuffer_ = std::make_shared< tf2_ros::Buffer >(this->get_clock());
 	tfListener_ = std::make_shared< tf2_ros::TransformListener >(*tfBuffer_);
@@ -86,6 +90,42 @@ ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) :
 	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));
 }
 
+pcl::PointCloud<pcl::PointXYZ> rangeFiltering(
+		const pcl::PointCloud<pcl::PointXYZ> & cloud,
+		float rangeMin,
+		float rangeMax)
+{
+	if(!cloud.empty() && (rangeMin > 0.0f || rangeMax > 0.0f))
+	{
+		pcl::PointCloud<pcl::PointXYZ> output;
+		output.reserve(cloud.size());
+		int oi = 0;
+		float rangeMinSqrd = rangeMin * rangeMin;
+		float rangeMaxSqrd = rangeMax * rangeMax;
+		for(size_t i=0; i<cloud.size(); ++i)
+		{
+			const pcl::PointXYZ & pt = cloud.at(i);
+			float r = pt.x*pt.x + pt.y*pt.y + pt.z*pt.z;
+
+			if(rangeMin > 0.0f && r < rangeMinSqrd)
+			{
+				continue;
+			}
+			if(rangeMax > 0.0f && r > rangeMaxSqrd)
+			{
+				continue;
+			}
+			
+			output.push_back(pt);
+			++oi;
+		}
+		output.resize(oi);
+		return output;
+	}
+
+	return cloud;
+}
+
 void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg)
 {
 	rclcpp::Time time = now();
@@ -137,6 +177,11 @@ void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstShar
 		inputCloud->is_dense = true;
 	}
 
+	if(rangeMin_ > 0.0f || rangeMax_ > 0.0f)
+	{
+		*inputCloud = rangeFiltering(*inputCloud, rangeMin_, rangeMax_);
+	}
+	
 	//Common variables for all strategies
 	pcl::IndicesPtr ground, obstacles;
 	pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);