Updated the documentation.

README.md

-# iri_nav_lidar_obstacles
+## Description
 
-IRI package created to detect obstacles around the robot using a lidar with 16 rings. It also creates a ring of fictional obstacles at maximum range to clearing operations.
+ROS package to detect traversable and non traversable obstacles in a 3D environment using 3D LIDAR data. At the moment only the Velodyne PUCK16 and the RoboSense RS16 sensors have been tested with this node.
 
-## Getting Started
+This package requires the information form the LIDAR sensor to be ordered (like an image). For the real sensors this is not a problem, but in simulation we recommend using a modified version of the [velodyne sensor plugin](https://gitlab.iri.upc.edu/labrobotica/ros/navigation/3d_navigation/velodyne_simulator) which publishes ordered simulated data.
 
-It uses a lidar sensor to detect several types of obstacles. It detects positive and negative obstacles (holes) and ramps with a too high slope which are also obstacles since they are not passables.
+The algorithm computes the slope between all adjacent points in the point cloud. The inclination information is stored as an image, and image processing techniques are used to find out the traversable and non traversable regions. The next image shows the operations performed to the input pointcloud to detect the traversable space.
 
-These instructions will get you a copy of the project up and running on your local machine for development and testing purposes.
+<img src="docs/images/lidar_obstacle_detection.png" alt="Operations performed to the input pointcloud to detect the traversable space">
 
-### Prerequisites
+The inclination computation stage computes the inclination of each point with its neighbors in polar coordinates. That is, for each point, two inclinations are computed:
 
-This package is part of the *3d_navigation* project, but you do not need to install all the packages in the project to see it running.
-More specifically, with the *iri_nav_lidar_filter* package, is possible to see the capacities of this sensor discarting wrong points due to the robot's structure.
+* **radial inclination**: the inclination with the points in the next and previous rings, in the same angle
+* **angular inclination**: the inclination with the next and previous points in the same ring.
 
-In addition, for compilation, several packages are necessary: *iri_base_algorithm*, *sensor_msgs*, *std_msgs* and *velodyne_pointcloud*.
+The maximum of both inclinations is chosen, and placed in a two dimensional array. Then standard image processing techniques are used:
 
-### Installing
+* **Threshold**: a binary image is generated where the white pixel represent points with inclination higher than the allowed one.
+* **Morphological closure**: A closure filter is used to remove isolated points and small regions that can be safely ignored.
+* **Clustering**: The external boundaries of the remaining clusters are used to generate the output obstacle point cloud with the non traversable regions.
 
-Create a new folder for the package and clone the repository using
+The obstacle pointcloud can be used as a source in a costmap layer to both mark and clear occupied cells in the costmap (both local and global).
+
+Additionally, an artificial pointcloud is generated with points at the maximum range in the regions where no obstacles are detected, within the sensor range, with a given angular resolution. This pointcloud can be used to further clear any previous costmap cells in a costmap layer (both local and global).
+
+This package also two additional features:
+
+* **Farthest point detection**: This feature marks the farthest detected points as non traversable space. This feature can be enabled and disabled.
+
+* **Virtual obstacle detection**: This feature marks traversable regions below the ground level (with negative slope) as non traversable. Eventhough the detected inclination between two consecutive points in the same angle may be travesable, the lack of information between these two points makes it impossible two know the actual inclination, so they are marked as non traversable to be safe. This feature can be enabled and disabled. The next image shows an example of such cases.
+
+<img src="docs/images/virtual_obs.png" alt="Example where a a negative obstacle is marked as non traversable for lack of information">
+
+This packages provides a regular node and a [nodelet](http://wiki.ros.org/nodelet) that can be used with a nodelet_manager to group together several packages handling point cloud data.
+
+## ROS interfaces
+
+### Topic publishers
+
+* **obstacles**<sensor_msgs::PointCloud2> - A point cloud with point only on those regions not traversable by the robot.
+* **free_space**<sensor_msgs::PointCloud2> - A point cloud with points at the maximum range on the direction where there is traversable space.
+
+### Topic subscribers
+
+* **pointcloud_in**<sensor_msgs::PointCloud2> - input point cloud. This point must be ordered in order to work properly (that is both with and height must be different from 1).
+
+### Parameters
+
+* **base_frame** (default=base_footprint): Name of the reference frame of the robot.
+* **min_range** (default=0.1): Minimum distance range to take into account in meters.
+* **max_range** (default=50.0): Maximum distance range to take into account in meters.
+* **max_inc** (default=0.3): Maximum traversable slope inclination in radians.
+* **min_inc** (default=0.1): Minimum inclination value to take into account in radians.
+* **inc_area_filter** (default=10): Minimum size of the inclination clusters in pixels to be taken into account.
+* **max_ring** (default=10): The maximum ring index to use in the computation of the traversable and non traversable spaces to reduce the computational cost.
+* **angle_resolution** (default=0.001): The angular resolution for the free space point cloud. This point cloud will have points at the max_range distance separated by this angular resolution.
+* **debug** (default=True): This enables or disables the publication of the image used to generate the occupied space for testing purposes.
+
+For the farthest point detection:
+* **farthest_enable** (default=False): This enables or disables the furthest point detection to generate obstacles.
+* **farthest_num_rings** (default=9): The maximum ring index to use in the computation of the farthest point.
+
+For the virtual obstacle detection:
+* **virtual_enable** (default=True): This enables or disables the virtual obstacle detection to generate obstacles.
+* **virtual_floor_threshold** (default=0.03): Point below this height will be considered to be under the floor.
+* **sensor_height** (default=0.3): Height of the LIDAR sensor with respect to the base_frame parameter in meters.
+
+## Dependencies
+This package requires the following packages:
+
+* [iri_base_algorithm](https://gitlab.iri.upc.edu/labrobotica/ros/iri_core/iri_base_algorithm): 
+
+## Installation
+
+Move to the active workspace:
+```bash
+roscd && cd ../src
 ```
-mkdir iri_nav_lidar_obstacles
-cd iri_nav_lidar_obstacles
-git clone https://gitlab.iri.upc.edu/mobile_robotics/colrobtransp/3d_navigation/iri_hole_detector2.git
+
+Clone the repository: 
+```bash
+git clone ssh://git@gitlab.iri.upc.edu:2202/labrobotica/ros/navigation/3d_navigation/iri_lidar_obstacle_detection.git
 ```
-Compile the package with
+
+Install ROS dependencies:
 ```
 roscd
 cd ..
+rosdep install -i -r --from-paths src
+```
+
+Compile the workspace:
+```
 catkin_make
 ```
 
-## Node inputs, outputs and params
+## Launch
 
-### Subscribers
+This package is normally launched as part of a more complete launch file, either in simulation or with the real robot.
 
-* **sensor_cloud_in**<sensor_msgs::PointCloud2> - It receives a pointcloud (typically the output of the the *iri_nav_lidar_filter*) and it makes all the calculations in order to detect all the obstacles arround the robot.
+The main node launch file (*node.launch*) has the following arguments:
 
-### Publishers
+* **ns** (default=robot): namespace in which to include the node.
+* **node_name** (default=model_car_odometry): name of the ROS node.
+* **output** (default=screen): Desired log output. Possible values are screen and log.
+* **launch_prefix** (default=): Set of parameters to be pre-apended to the node launch command.
+* **config_file** (default=$(find iri_lidar_obstacle_detector)/config/default_config.yaml): file with the configuration of the node parameters
+* **ranges_file** (default=$(find iri_lidar_obstacle_detector)/config/default_ranges.yaml): file with the definition of the angular sections to eliminate.
+* **cloud_in** (default=/encoders): Name of the input PointCloud topic.
 
-* **hole_scan_out**<sensor_msgs::LaserScan> - It generates a pointcloud with virtual obstacles at every position the algorithm has detected an obstacle or at the maximum valid range of the sensor for clearing purposes. This pointcloud is sent to the navigation stack to be aware of the presence of an obstacle at the position of each red point.
-* **test_cloud_out**<sensor_msgs::PointCloud2> - It outputs the same pointcloud as the input but ...
+The main nodelet launch file (*nodelet.launch*) has the following arguments:
 
-### Params
+* **ns** (default=robot): namespace in which to include the node.
+* **node_name** (default=model_car_odometry): name of the ROS node.
+* **output** (default=screen): Desired log output. Possible values are screen and log.
+* **lidar_nodelet_manager** (default=lidar_nodelet_manager): Name of the nodelet manager in which this nodelet will be included.
+* **config_file** (default=$(find iri_lidar_obstacle_detector)/config/default_config.yaml): file with the configuration of the node parameters
+* **ranges_file** (default=$(find iri_lidar_obstacle_detector)/config/default_ranges.yaml): file with the definition of the angular sections to eliminate.
+* **cloud_in** (default=/encoders): Name of the input PointCloud topic.
+
+To include the iri_lidar_obstacle_detection as a node into another launch file, include the following lines:
+
+```
+  <include file="$(find iri_lidar_obstacle_detector)/launch/node.launch">
+    <arg name="ns" value="$(arg ns)"/>
+    <arg name="node_name" value="$(arg lidar_detector_node_name)"/>
+    <arg name="config_file" value="$(arg lidar_detector_config_file)"/>
+    <arg name="ranges_file" value="$(arg lidar_filter_config_file)"/>
+    <arg name="output" value="$(arg output)"/>
+    <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+    <arg name="cloud_in" value="/$(arg ns)/$(arg lidar_filter_node_name)/cloud_out"/>
+  </include>
+```
+
+To include the iri_lidar_obstacle_detection as a nodelet into another launch file, include the following lines:
+
+```
+  <group ns="$(arg ns)">
+    <node name="$(arg lidar_nodelet_manager)"
+          pkg="nodelet"
+          type="nodelet"
+          args="manager"
+          output="screen"/>
+  </group>
+
+  <include file="$(find iri_lidar_obstacle_detector)/launch/nodelet.launch">
+    <arg name="ns" value="$(arg ns)"/>
+    <arg name="node_name" value="$(arg lidar_detector_node_name)"/>
+    <arg name="lidar_nodelet_manager" value="$(arg lidar_nodelet_manager)"/>
+    <arg name="config_file" value="$(arg lidar_detector_config_file)"/>
+    <arg name="ranges_file" value="$(arg lidar_filter_config_file)"/>
+    <arg name="output" value="$(arg output)"/>
+    <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+    <arg name="cloud_in" value="/$(arg ns)/$(arg lidar_filter_node_name)/cloud_out"/>
+  </include>
+```
+A single nodelet_manager is required for all nodelets handling the same input data.
 
-* **x_min**, **x_max**, **y_min**, **y_max** - Filter limit.
-* **n_rings_lidar** - Number of laser rings (typically 16).
-* **subdivisions** - Downsampling param. The bigger, more space between rows.
-* **range_filter** - Downsampling param. It reduces the number of rows.
-* **hole_gap_size** - Minimum gap for considering a hole.
-* **max_incline** - Maximum slope inclination.
-* **angle_min** - LaserScan minimum angle.
-* **angle_max** - LaserScan maximum angle.
-* **range_min** - Laser scan's minimum range.
-* **range_max** - Laser scan's maximum range.
-* **num_readings** - Number of LaserScan points.
-* **base_frame** - Name of the robot's base frame.
-* **debug_mode** - Allows extra topics publication for testing.
+## Disclaimer
 
-## Authors
+Copyright (C) Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+Mantainer IRI labrobotics (labrobotica@iri.upc.edu)
 
-* **Joan Grimalt Oliver** - *Initial work Velodyne sensor*
+This package is distributed in the hope that it will be useful, but without any warranty. It is provided "as is" without warranty of any kind, either expressed or implied, including, but not limited to, the implied warranties of merchantability and fitness for a particular purpose. The entire risk as to the quality and performance of the program is with you. should the program prove defective, the GMR group does not assume the cost of any necessary servicing, repair  or correction.
 
-## Acknowledgments
+In no event unless required by applicable law the author will be liable to you for damages, including any general, special, incidental or consequential damages arising out of the use or inability to use the program (including but not limited to loss of data or data being rendered inaccurate or losses sustained by you or third parties or a failure of the program to operate with any other programs), even if the author has been advised of the possibility of such damages.
 
-* Hat tip to anyone whose code was used.
+You should have received a copy of the GNU Lesser General Public License along with this program. If not, see <http://www.gnu.org/licenses/>

src/lidar_obstacle_detector_alg_node.cpp

@@ -312,12 +312,14 @@ void LidarObstacleDetectorAlgNode::virtual_obstacles(std::vector<pcl::PointXYZ>
       }
     }
   }
+/*  
   if(this->config_.debug && this->obstacles_img_publisher_.getNumSubscribers()>0)
   {
     this->cv_image_=cv_bridge::CvImage(this->input_header, sensor_msgs::image_encodings::MONO8,traversable_space);
     this->cv_image_.toImageMsg(this->obstacles_img_Image_msg_); // from cv_bridge to sensor_msgs::Image
     this->obstacles_img_publisher_.publish(this->obstacles_img_Image_msg_);
   }
+  */
 }
 
 void LidarObstacleDetectorAlgNode::farthest_obstacles(std::vector<pcl::PointXYZ> &points,std::vector<bool> &free_space)
@@ -413,14 +415,12 @@ void LidarObstacleDetectorAlgNode::mainNodeThread(void)
     cv::morphologyEx(threshold,threshold,cv::MORPH_CLOSE,cv::getStructuringElement(cv::MORPH_RECT,cv::Size(3,3)));
     cv::connectedComponentsWithStats(threshold,this->label_image,stats_image,centroid_image,8);
 
-    /*
     if(this->config_.debug && this->obstacles_img_publisher_.getNumSubscribers()>0)
     {
       this->cv_image_=cv_bridge::CvImage(this->input_header, sensor_msgs::image_encodings::MONO8,threshold);
       this->cv_image_.toImageMsg(this->obstacles_img_Image_msg_); // from cv_bridge to sensor_msgs::Image
       this->obstacles_img_publisher_.publish(this->obstacles_img_Image_msg_);
     }
-    */
     std::vector<pcl::PointXYZ> points;
     for(int i=1;i<stats_image.rows;i++)
     {