diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp
index 79ee6bb2..d262d54d 100644
--- a/src/nodelets/obstacles_detection.cpp
+++ b/src/nodelets/obstacles_detection.cpp
@@ -157,55 +157,46 @@ private:
 
 		ros::Time lasttime = ros::Time::now();
 
+		hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits<int>::min(), maxFloorHeight_);
+
 		if (!simpleSegmentation_){
 
-			ROS_ERROR("1-1");
-			hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits<int>::min(), maxFloorHeight_);
-			ROS_ERROR("1-2");
 
 			rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(hypotheticalGroundCloud,
 					ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_);
 
-			ROS_ERROR("1-3");
-
 			if(ground.get() && ground->size())
 			{
 				pcl::copyPointCloud(*hypotheticalGroundCloud, *ground, *groundCloud);
 			}
 
-			ROS_ERROR("1-4");
-
 			obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_);
 
-			ROS_ERROR("1-5");
-
 			if(obstacles.get() && obstacles->size())
 			{
 				pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesNearFloorCloud(new pcl::PointCloud<pcl::PointXYZ>);
 				pcl::copyPointCloud(*hypotheticalGroundCloud, *obstacles, *obstaclesNearFloorCloud);
 				*obstaclesCloud += *obstaclesNearFloorCloud;
 			}
-			ROS_ERROR("R 44444444444444444444444444444");
 
 		}
 		else{
-			ROS_ERROR("2-1");
 			obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_);
-			ROS_ERROR("2-2");
 		}
 
 
-		/*
+
 		if(groundPub_.getNumSubscribers())
 		{
 			sensor_msgs::PointCloud2 rosCloud;
-			pcl::toROSMsg(*groundCloud, rosCloud);
+			if (simpleSegmentation_) {pcl::toROSMsg(*hypotheticalGroundCloud, rosCloud);;}
+			else {pcl::toROSMsg(*groundCloud, rosCloud);}
 			rosCloud.header.stamp = cloudMsg->header.stamp;
 			rosCloud.header.frame_id = frameId_;
 
 			//publish the message
 			groundPub_.publish(rosCloud);
-		}*/
+		}
 
 		if(obstaclesPub_.getNumSubscribers())
 		{
@@ -223,9 +214,9 @@ private:
 		ros::Duration process_duration = curtime - lasttime;
 		ros::Duration between_frames = curtime - this->_lastFrameTime;
 		this->_lastFrameTime = curtime;
-		std::stringstream buffer;
-		buffer << "cloud=" << originalCloud->size() << " ground=" << hypotheticalGroundCloud->size() << " floor=" << ground->size() << " obst=" << obstacles->size();
-		buffer << " t=" << process_duration.toSec() << "s; " << (1./between_frames.toSec()) << "Hz";
+		//std::stringstream buffer;
+		//buffer << "cloud=" << originalCloud->size() << " ground=" << hypotheticalGroundCloud->size() << " floor=" << ground->size() << " obst=" << obstacles->size();
+		//buffer << " t=" << process_duration.toSec() << "s; " << (1./between_frames.toSec()) << "Hz";
 		//ROS_ERROR("3%s: %s", this->getName().c_str(), buffer.str().c_str());
 
 	}
diff --git a/src/nodelets/point_cloud_xyz.cpp b/src/nodelets/point_cloud_xyz.cpp
index e944b9cf..3207371f 100644
--- a/src/nodelets/point_cloud_xyz.cpp
+++ b/src/nodelets/point_cloud_xyz.cpp
@@ -69,7 +69,8 @@ public:
 		approxSyncDepth_(0),
 		approxSyncDisparity_(0),
 		exactSyncDepth_(0),
-		exactSyncDisparity_(0)
+		exactSyncDisparity_(0),
+		cut_(0)
 	{}
 
 	virtual ~PointCloudXYZ()
@@ -99,6 +100,7 @@ private:
 		pnh.param("decimation", decimation_, decimation_);
 		pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
 		pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
+		pnh.param("cut", cut_, cut_);
 		ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
 
 		if(approxSync)
@@ -147,6 +149,18 @@ private:
 		if(cloudPub_.getNumSubscribers())
 		{
 			cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
+			cv::Mat image=imageDepthPtr->image;
+			int rows = image.rows;
+			int cols = image.cols;
+
+			if (cut_>0){
+				cv::Mat pRoi = image(cv::Rect(0, 0, cut_, rows));
+				pRoi.setTo(cv::Scalar(0.));
+			}
+			else if (cut_<0){
+				cv::Mat pRoi = image(cv::Rect(cols+cut_, 0, -cut_, rows));
+				pRoi.setTo(cv::Scalar(0.));
+			}
 
 			image_geometry::PinholeCameraModel model;
 			model.fromCameraInfo(*cameraInfo);
@@ -157,13 +171,12 @@ private:
 
 			pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
 			pclCloud = rtabmap::util3d::cloudFromDepth(
-					imageDepthPtr->image,
+					image,
 					cx,
 					cy,
 					fx,
 					fy,
 					decimation_);
-
 			processAndPublish(pclCloud, depth->header);
 		}
 	}
@@ -245,6 +258,7 @@ private:
 	int decimation_;
 	double noiseFilterRadius_;
 	int noiseFilterMinNeighbors_;
+	int cut_;
 
 	ros::Publisher cloudPub_;