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

API change: updated obstacles_detection nodelet with new segmentObstaclesFromGround()

Browse Source
This commit is contained in:
matlabbe
2016-04-12 18:58:11 -04:00
parent 30ad691027
commit fded3685af
1 changed files with 23 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/obstacles_detection.cpp
+23 -6
View File
@@ -67,8 +67,9 @@ class ObstaclesDetection : public nodelet::Nodelet
public: public:
ObstaclesDetection() : ObstaclesDetection() :
frameId_("base_link"), frameId_("base_link"),
normalEstimationRadius_(0.05), normalKSearch_(20),
groundNormalAngle_(M_PI_4), groundNormalAngle_(M_PI_4),
clusterRadius_(0.05),
minClusterSize_(20), minClusterSize_(20),
maxObstaclesHeight_(0.0), // if<=0.0 -> disabled maxObstaclesHeight_(0.0), // if<=0.0 -> disabled
waitForTransform_(false), waitForTransform_(false),
@@ -87,8 +88,20 @@ private:
int queueSize = 10; int queueSize = 10;
pnh.param("queue_size", queueSize, queueSize); pnh.param("queue_size", queueSize, queueSize);
pnh.param("frame_id", frameId_, frameId_); pnh.param("frame_id", frameId_, frameId_);
pnh.param("normal_estimation_radius", normalEstimationRadius_, normalEstimationRadius_); pnh.param("normal_k", normalKSearch_, normalKSearch_);
pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_); pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_);
if(pnh.hasParam("normal_estimation_radius") && !pnh.hasParam("cluster_radius"))
{
NODELET_WARN("Parameter \"normal_estimation_radius\" has been renamed "
"to \"cluster_radius\"! Your value is still copied to "
"corresponding parameter. Instead of normal radius, nearest neighbors count "
"\"normal_k\" is used instead (default 20).");
pnh.param("normal_estimation_radius", clusterRadius_, clusterRadius_);
}
else
{
pnh.param("cluster_radius", clusterRadius_, clusterRadius_);
}
pnh.param("min_cluster_size", minClusterSize_, minClusterSize_); pnh.param("min_cluster_size", minClusterSize_, minClusterSize_);
pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_); pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
@@ -158,8 +171,9 @@ private:
originalCloud, originalCloud,
ground, ground,
obstacles, obstacles,
normalEstimationRadius_, normalKSearch_,
groundNormalAngle_, groundNormalAngle_,
clusterRadius_,
minClusterSize_); minClusterSize_);
if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
@@ -190,8 +204,9 @@ private:
originalCloud_near, originalCloud_near,
ground, ground,
obstacles, obstacles,
normalEstimationRadius_, normalKSearch_,
groundNormalAngle_, groundNormalAngle_,
clusterRadius_,
minClusterSize_); minClusterSize_);
if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
@@ -211,8 +226,9 @@ private:
originalCloud_far, originalCloud_far,
ground, ground,
obstacles, obstacles,
3.*normalEstimationRadius_, normalKSearch_,
2.*groundNormalAngle_, 2.*groundNormalAngle_,
3.*clusterRadius_,
minClusterSize_); minClusterSize_);
if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
@@ -260,8 +276,9 @@ private:
private: private:
std::string frameId_; std::string frameId_;
double normalEstimationRadius_; int normalKSearch_;
double groundNormalAngle_; double groundNormalAngle_;
double clusterRadius_;
int minClusterSize_; int minClusterSize_;
double maxObstaclesHeight_; double maxObstaclesHeight_;
bool waitForTransform_; bool waitForTransform_;