diff --git a/README.md b/README.md index 9d121bd62d4271f3ec7fbaee6f3ea53c9b296016..0c2141507b8bb39a7d16e0f7a06c5fe47742e13e 100644 --- a/README.md +++ b/README.md @@ -1,61 +1,137 @@ -# iri_nav_obstacle_detection_normals +## Description -IRI package created to calculate the normal vector of each pointcloud's point in navigation using a depth camera (usually a Realsense D435 or D435i) as input sensor. +ROS package that computes the the normal vector of each pointcloud's point using information from its neighbors, and fitting the best plane to each group of point-neighbors. The normal vectors are then used to compute its estimated inclination, and teo different pointclouds are generated: -## Getting Started +* **Occupied space**: Those points that have an inclination bigger that the desired threshold. This pointcloud can be used as a source in a costmap layer to mark occupied cells in the costmap (both local and global). +* **Free space**: Those points that have an inclination below the desired threshold. This pointcloud can be used as a source in a costmap layer clear any previous costmap cells in a costmap (both local and global). -The main idea is to calculate the normal vector of each point using it and its closest neighbours and fitting the best plane to each group of point-neighbours. After that, if that vector is out of some thresholds (relative to the inclination with respect to the plane of each point), that point is classified as not passable and, therefore, as an obstacle. On the other hand, if the vector fulfills all the thresholds, it corresponds to a passable terrain. +This ROS node is intended to be used with pointclouds generated by depth cameras because they have a higher point density that the ones generated by 3D LIDAR sensors. -These instructions will get you a copy of the project up and running on your local machine for development and testing purposes. +The next image shows the output pointclouds generated by this ROs node in the case of two ramps, one with a traversable slope (shown in green), and the other one with a non traversable slop (shown in red): -### Prerequisites +<img src="docs/images/pointcloud_normals.png" alt="Traversable (green) and non traversable (red) spaces detected by the pointcloud normal detector"> -This package is part of the *3d_navigation* project, so you need to install all the necessary packages in the project to see it running. If you do not care about detecting holes, you can avoid installing the hole detection part of the project. -More specifically, this hole detector works after using a filtering chain aplied to the original pointcloud generated by the camera. This means that you need the package *iri_3dnav* for launchers and the packages *average_point_cloud* and *pcl* for voxel dodwnsampling and several outliers filters. +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. -In addition, for compilation, several packages are necessary: *iri_base_algorithm*, *sensor_msgs*, *pcl_ros*, and *iriutils*. +The implementation of this node is base on the following papers: + +* Àngel Santamaria-Navarro, Ernesto H. Teniente, Martà Morta and Juan Andrade-Cetto - [Initial work for PMD camera]() + +* Àngel Santamaria-Navarro, Ernesto H. Teniente, Martà Morta and Juan Andrade-Cetto - [Terrain classification in complex 3D outdoor environments](https://upcommons.upc.edu/bitstream/handle/2117/27991/1496-Terrain-classification-in-complex-3D-outdoor-environments.pdf?sequence=1&isAllowed=y) + +## ROS Interfaces + +### Topic subscribers + +* **input**<sensor_msgs::PointCloud2> - Input pointcloud. + +### Topic publishers + +* **obstacles**<sensor_msgs::PointCloud2> - Output pointcloud including those points that have an inclination bigger than the desired threshold (obstacles), and therefore it describes the non traversable space. These points are colored in red. +* **free_space**<sensor_msgs::PointCloud2> - Output pointcloud including those points that have an inclination below the desired threshold (free space), and therefore it describes the traversable space. These points are colored in green. + +### Parameters + +* **filter_min_dist** (default=0.3) - Minimum distance threshold for considering a point as valid in meters. +* **normal_KSearch** (default=100) - Number of neighbours to estimate the surface. +* **max_inc** (default=0.25) - Maximum traversable inclination in radians. + +## Dependencies +This package requires the following packages: + +* [iri_base_algorithm](https://gitlab.iri.upc.edu/labrobotica/ros/iri_core/iri_base_algorithm): ### Installing -Create a new folder for the package and clone the repository using +Move to the active workspace: +```bash +roscd && cd ../src ``` -mkdir iri_nav_obstacle_detection_normals -cd iri_nav_obstacle_detection_normals -git clone https://gitlab.iri.upc.edu/mobile_robotics/colrobtransp/3d_navigation/iri_obstacle_detection_normals.git + +Clone the repository: +```bash +git clone ssh://git@gitlab.iri.upc.edu:2202/labrobotica/ros/navigation/3d_navigation/iri_obstacle_detection_normals.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 + +This package is normally launched as part of a more complete launch file, either in simulation or with the real robot. -### Subscribers +The main node launch file (*node.launch*) has the following arguments: -* **input**<sensor_msgs::PointCloud2> - It receives a pointcloud (typically the output of an outliers filter or any other filtering stage) and it calculates the normal vector to each point. +* **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_obstacle_detection_normals)/config/ana_params.yaml): file with the definition of the angular sections to eliminate. +* **cloud_in** (default=/encoders): Name of the input PointCloud topic. -### Publishers +The main nodelet launch file (*nodelet.launch*) has the following arguments: -* **all_rg**<sensor_msgs::PointCloud2> - It outputs the same pointcloud as the input but marking each point using colors: blue for a point at a distance lower than the minimum and, therefore, a non-valid point; green for a point with a valid normal vector (passable terrain) and red for a point with a non-valid normal vector (obstacle). -* **obstacles**<sensor_msgs::PointCloud> - It generates a pointcloud with only the points marked as red points. This pointcloud is sent to the navigation stack to be aware of the presence of an obstacle at the position of each red point. +* **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. +* **camera_nodelet_manager** (default=camera_nodelet_manager): Name of the nodelet manager in which this nodelet will be included. +* **config_file** (default=$(find iri_obstacle_detection_normals)/config/ana_params.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_obstacle_detection_normals as a node into another launch file, include the following lines: + +``` + <include file="$(find iri_obstacle_detection_normals)/launch/node.launch"> + <arg name="ns" value="$(arg ns)"/> + <arg name="node_name" value="$(arg normals_node_name)"/> + <arg name="config_file" value="$(arg normals_config_file)"/> + <arg name="output" value="$(arg output)"/> + <arg name="launch_prefix" default="$(arg launch_prefix)"/> + <arg name="cloud_in" default="$(arg normals_cloud_in)"/> + </include> + +``` + +To include the iri_obstacle_detection_normals as a nodelet into another launch file, include the following lines: + +``` + <group ns="$(arg ns)"> + <node name="$(arg camera_nodelet_manager)" + pkg="nodelet" + type="nodelet" + args="manager" + output="screen"/> + </group> + + <include file="$(find iri_obstacle_detection_normals)/launch/nodelet.launch"> + <arg name="ns" value="$(arg ns)"/> + <arg name="node_name" value="$(arg normals_node_name)"/> + <arg name="camera_nodelet_manager" value="$(arg obstacle_nodelet_manager)"/> + <arg name="config_file" value="$(arg normals_config_file)"/> + <arg name="output" value="$(arg output)"/> + <arg name="launch_prefix" value="$(arg launch_prefix)"/> + <arg name="cloud_in" value="radius_outlier_removal/output"/> + </include> +``` -### Params +A single nodelet_manager is required for all nodelets handling the same input data. -* **filter_min_dist** - Minimum distance threshold for considering a point as valid. -* **normal_KSearch** - Number of neighbours to estimate the surface. -* **normal_x** - Normal X threshold. -* **normal_y** - Normal Y threshold. -* **normal_z** - Normal Z threshold. +## Disclaimer -## Authors +Copyright (C) Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +Mantainer IRI labrobotics (labrobotica@iri.upc.edu) -* **Àngel Santamaria-Navarro, Ernesto H. Teniente, Martà Morta and Juan Andrade-Cetto** - *Initial work for PMD camera* -* **Joan Grimalt Oliver** - *Adaptation to Realsense D435 and D435i* -* **José Enrique DomÃnguez Vidal** - *Adaptation to Realsense D435 and D435i* +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. -* *Terrain classification in complex 3D outdoor environments* paper written by Àngel Santamaria-Navarro, Ernesto H. Teniente, Martà Morta and Juan Andrade-Cetto. -* Hat tip to Git. +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/> diff --git a/docs/images/pointcloud_normals.png b/docs/images/pointcloud_normals.png new file mode 100644 index 0000000000000000000000000000000000000000..756d95635bc942de92ec20fa9f8a7182ff29caad Binary files /dev/null and b/docs/images/pointcloud_normals.png differ