diff --git a/README.md b/README.md index fa5a68df1d90b382366a00346cf7bc829087eff0..329299a480e749286031a06a590fd136e818a81f 100644 --- a/README.md +++ b/README.md @@ -1,52 +1,137 @@ -# iri_nav_average_point_cloud +## Description -IRI package created to calculate the moving average of several frames (NOT of several pixels of the same frame) generated by a depth camera (usually a Realsense D435 or D435i) as input sensor. +This ROS package includes two different nodes: -## Getting Started +* **Pointcloud throttle**: A ROS nodelet that reduces the rate of the incoming pointclouds. This node only exports an existing nodelet provide by the [nodelet_topic_tools](http://wiki.ros.org/nodelet_topic_tools#Throttle) package. -This filter is desinged to filter noise due to variations in the amount of infrared radiation received by the camera at each moment working outdoors. That's why it filters inter-frame and not intra-frame (for that there are outliers filters in the filtering chain). -These instructions will get you a copy of the project up and running on your local machine for development and testing purposes. +* **Pointcloud average**: A ROS node and nodelet that averages several pointcloud using a moving window to reduce the noise. The implementation of this node is base on the following papers: -### Prerequisites + * Àngel Santamaria-Navarro, Ernesto H. Teniente, Martà Morta and Juan Andrade-Cetto - [Initial work for PMD camera]() -This package is part of the *3d_navigation* project, but it can be used for other projects so it is not necessary to download the whole filtering chain. -Anyway, for compilation, several packages are necessary: *iri_base_algorithm*, *sensor_msgs*, *pcl_ros*, *iriutils* and *Eigen3*. + * À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) -### Installing +## Average ROS interfaces -Create a new folder for the package and clone the repository using +### Topic subscribers + +* **input**<sensor_msgs::PointCloud2> - Input pointcloud. + +### Topic publishers + +* **output**<sensor_msgs::PointCloud2> - Averaged pointcloud using the previous *iBuffer* received pointclouds. + +### Parameters + +* **iBuffer** ((default=5): The number of frames to average. Increasing this number, it smooths the output but it also introduces a delay in the detection. + +## 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_average_point_cloud -cd iri_nav_average_point_cloud -git clone https://gitlab.iri.upc.edu/mobile_robotics/colrobtransp/3d_navigation/average_point_cloud.git + +Clone the repository: +```bash +git clone ssh://git@gitlab.iri.upc.edu:2202/labrobotica/ros/navigation/3d_navigation/iri_average_point_cloud.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 +## Throttle launch -### Subscribers +To include the pointcloud throttle nodelet into another launch file, include the following lines: -* **input**<sensor_msgs::PointCloud2> - It receives a pointcloud (from the original sensor or from a previous filter) that it uses together to the previous frames to calculate the moving average. +``` + <node name="pc_throttler" + type="throttle" + pkg="topic_tools" + args="messages $(arg cloud_in) <number of frames> $(arg cloud_in)_throttle" /> +``` +where *number of frames* is the desired number of frames per second + +## Average launch + +This package is normally launched as part of a more complete launch file, either in simulation or with the real robot. + +The main node launch file (*node.launch*) has the following arguments: + +* **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_average_point_cloud)/config/default_config.yaml): file with the configuration of the node parameters +* **cloud_in** (default=/encoders): Name of the input PointCloud topic. + +The main nodelet launch file (*nodelet.launch*) has the following arguments: + +* **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_average_point_cloud)/config/default_config.yaml): file with the configuration of the node parameters +* **cloud_in** (default=/encoders): Name of the input PointCloud topic. -### Publishers +To include the iri_average_point_cloud as a node into another launch file, include the following lines: + +``` + <include file="$(find iri_average_point_cloud)/launch/node.launch"> + <arg name="ns" value="$(arg ns)"/> + <arg name="node_name" value="$(arg average_node_name)"/> + <arg name="config_file" value="$(arg average_config_file)"/> + <arg name="output" value="$(arg output)"/> + <arg name="launch_prefix" default="$(arg launch_prefix)"/> + <arg name="cloud_in" default="$(arg average_cloud_in)"/> + </include> +``` + +To include the iri_average_point_cloud as a nodelet into another launch file, include the following lines: + +``` + <group ns="$(arg ns)"> + <node name="$(arg obstacle_nodelet_manager)" + pkg="nodelet" + type="nodelet" + args="manager" + output="$(arg output)"/> + </group> + + <include file="$(find iri_average_point_cloud)/launch/nodelet.launch"> + <arg name="ns" value="$(arg ns)"/> + <arg name="node_name" value="$(arg average_node_name)"/> + <arg name="camera_nodelet_manager" value="$(arg obstacle_nodelet_manager)"/> + <arg name="config_file" value="$(arg average_config_file)"/> + <arg name="output" value="$(arg output)"/> + <arg name="launch_prefix" value="$(arg launch_prefix)"/> + <arg name="cloud_in" value="$(arg camera_cloud_in)_throttle"/> + </include> +``` +A single nodelet_manager is required for all nodelets handling the same input data. -* **output**<sensor_msgs::PointCloud2> - Its output is a pointcloud with exactly the same number of points as the input with average of the last inputs for each point. +## Disclaimer -### Params +Copyright (C) Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +Mantainer IRI labrobotics (labrobotica@iri.upc.edu) -* **iBuffer** - Number of frames for averaging. Increasing this number, it smooths the output but it also introduce a delay in the detection of aproximately iBuffer/2. +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. -## Authors +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. -* **À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* +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/> -## Acknowledgments -* *Terrain classification in complex 3D outdoor environments* paper written by Àngel Santamaria-Navarro, Ernesto H. Teniente, Martà Morta and Juan Andrade-Cetto. \ No newline at end of file diff --git a/launch/node.launch b/launch/node.launch index bc5f12f5fa876288f45d35a8a4aceda8966c323e..ee4e2c0bcb6fe73f0e231f3f0fa155394c3dc5c6 100644 --- a/launch/node.launch +++ b/launch/node.launch @@ -1,7 +1,7 @@ <?xml version="1.0" encoding="UTF-8"?> <launch> - <arg name="ns" default="ana"/> + <arg name="ns" default="robot"/> <arg name="node_name" default="average_point_cloud"/> <arg name="config_file" default="$(find iri_average_point_cloud)/config/default_config.yaml"/> <arg name="output" default="screen"/> diff --git a/launch/nodelet.launch b/launch/nodelet.launch index d64e8c194b7a651d0f19aa55f645529be8147420..ce5e906288c9ac561b3d7ac3af1c83906bdbd3c1 100644 --- a/launch/nodelet.launch +++ b/launch/nodelet.launch @@ -1,7 +1,7 @@ <?xml version="1.0" encoding="UTF-8"?> <launch> - <arg name="ns" default="ana"/> + <arg name="ns" default="robot"/> <arg name="node_name" default="average_point_cloud"/> <arg name="camera_nodelet_manager" default="camera_nodelet_manager"/> <arg name="config_file" default="$(find iri_average_point_cloud)/config/default_config.yaml"/>