Merge branch 'anymal_research/rsl/fix/grid_map_pcl' into 'master'

Rsl/fix/grid map pcl

GitOrigin-RevId: a98450ede0d459d12f7b80ef694a1a5717d1abee

.gitignore

@@ -36,4 +36,5 @@
 # OS X
 .DS_Store
 
-grid_map_pcl/data/
+grid_map_pcl/data/*
+!grid_map_pcl/data/input_cloud.pcd

grid_map_pcl/CMakeLists.txt

@@ -99,7 +99,7 @@ target_link_libraries(${PROJECT_NAME}
 )
 
 
-# Node.
+# Nodes.
 add_executable(grid_map_pcl_loader_node
   src/grid_map_pcl_loader_node.cpp
 )
@@ -118,13 +118,32 @@ target_link_libraries(grid_map_pcl_loader_node
   ${catkin_LIBRARIES}
 )
 
+add_executable(pointcloud_publisher_node
+  src/pointcloud_publisher_node.cpp
+)
+add_dependencies(pointcloud_publisher_node
+  ${PROJECT_NAME}
+)
+target_include_directories(pointcloud_publisher_node PRIVATE
+  include
+)
+target_include_directories(pointcloud_publisher_node SYSTEM PUBLIC
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIR}
+)
+target_link_libraries(pointcloud_publisher_node
+  ${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)
+
 #############
 ## Install ##
 #############
 install(
   TARGETS
     ${PROJECT_NAME}
     grid_map_pcl_loader_node
+    pointcloud_publisher_node
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -162,6 +181,14 @@ install(
 if(CATKIN_ENABLE_TESTING)
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
 
+  find_package(catkin REQUIRED
+    COMPONENTS
+      ${CATKIN_PACKAGE_DEPENDENCIES}
+      roslaunch
+  )
+
+  roslaunch_add_file_check(launch)
+
   ## Add gtest based cpp test target and link libraries
   catkin_add_gtest(${PROJECT_NAME}-test
     test/test_grid_map_pcl.cpp

grid_map_pcl/README.md

@@ -14,6 +14,19 @@ The elevation is computed by slicing the point cloud in the x-y plane into colum
 **Authors: Edo Jelavic, Dominic Jud<br />
 Affiliation: [ETH Zurich, Robotics Systems Lab](https://rsl.ethz.ch/)<br />**
 
+## Publications
+The code for point cloud to grid map conversion has been developed as a part of research on autonomous precision harvesting. If you are using the point cloud to grid map conversion, please add the following citation to your publication:
+
+Jelavic, E., Jud, D., Egli, P. and Hutter, M., 2021. Towards Autonomous Robotic Precision Harvesting: Mapping, Localization, Planning and Control for a Legged Tree Harvester.
+
+    @article{jelavic2021towards,
+        title={Towards Autonomous Robotic Precision Harvesting: Mapping, Localization, Planning and Control for a Legged Tree Harvester},
+        author={Jelavic, Edo and Jud, Dominic and Egli, Pascal and Hutter, Marco},
+        journal={Field Robotics},
+        year={2021},
+        publisher={Wiley}
+    }
+
 ## Examples
 
 Examples of elevation maps computed from point clouds using this package:
@@ -30,15 +43,15 @@ Examples of elevation maps computed from point clouds using this package:
 
 ## Usage
 
-The algorithm will open the .pcd file, convert the point cloud to a grid map and save the grid map as a rosbag into the folder specified by the user.
+The algorithm will open the `.pcd` file, convert the point cloud to a grid map and save the grid map as a rosbag into the folder specified by the user.
 
-1.  Place .pcd files in the package folder or anywhere on the system (e.g. grid_map_pcl/data/example.pcd).
-2.  Modify the *folder_path* inside the launch file such that the folder file points to the folder containing .pcd files (e.g. /*"path to the grid_map_pcl folder"*/data).
-3.  Change the *pcd_filename* to the point cloud file that you want to process
-4.  You can run the algorithm with: *roslaunch grid_map_pcl grid_map_pcl_loader_node.launch* 
-5.  Once the algorithm is done you will see the output in the console, then you can run rviz in a separate terminal (make sure that you have sourced your workspace, **DO NOT CLOSE** the terminal where *grid_map_pcl_loader_node* is running ) and visualize the resulting grid map. Instructions on how to visualize a grid map are in the grid map [README](../README.md).
+1.  Place your `.pcd` file in the package folder or anywhere on the system (e.g. `/.../grid_map_pcl/data/example.pcd`).
+2.  Modify the `pcd_filename` inside the launch file such that it points to the `.pcd` file you would like to process (e.g. `/.../grid_map_pcl/data/example.pcd`). Set the `output_grid_map` variable to point to the location where you wish to save the resulting grid map (e.g. `/.../grid_map_pcl/data/example_grid_map.bag`)
+3.  Change the `config_file_path` variable in the launch file to point to the `.yaml` file with configuration parameters (e.g. `/.../grid_map_pcl/config/parameters.yaml`)
+4.  You can run the algorithm with: `roslaunch grid_map_pcl grid_map_pcl_loader_node.launch` 
+5.  Once the algorithm is done you will see the output in the console, and it should visualize the point cloud and the grid map in rviz. If that does not happen, then you can run rviz in a separate terminal (make sure that you have sourced your workspace, **DO NOT CLOSE** the terminal where `grid_map_pcl_loader_node` is running ) and visualize the resulting grid map. Instructions on how to visualize a grid map are in the grid map [README](../README.md).
 
-The resulting grid map will be saved in the folder given by *folder_path* variable in the launch file and will be named with a string contained inside the *output_grid_map* variable. For large point clouds (100M-140M points) the algorithm takes about 30-60 min to finish (with 6 threads). For sizes that are in the range of 40M to 60M points, the runtime varies between 5 and 15 min, depending on the number of points. Point cloud with around 10M points or less can be processed in a minute or two.
+The resulting grid map will be saved in to the location pointed by `output_grid_map` variable in the launch file. For large point clouds (100M-140M points) the algorithm takes about 30-60 min to finish (with 6 threads). For sizes that are in the range of 40M to 60M points, the runtime varies between 5 and 15 min, depending on the number of points. Point cloud with around 10M points or less can be processed in a minute or two.
 
 ## Parameters
 
@@ -50,6 +63,7 @@ The resulting grid map will be saved in the folder given by *folder_path* variab
 ##### Grid map parameters
 Resulting grid map parameters.
 * **pcl_grid_map_extraction/grid_map/min_num_points_per_cell** Minimum number of points in the point cloud that have to fall within any of the grid map cells. Otherwise the cell elevation will be set to NaN.
+* **pcl_grid_map_extraction/grid_map/max_num_points_per_cell** Maximum number of points in the point cloud that are allowed to fall within any of the grid map cells. If there is more points, the value will be set to NaN. This number can be used to ignore cells with a lot of points which speeds up the processing (but results with some holes in the map).
 * **pcl_grid_map_extraction/grid_map/resolution** Resolution of the grid map. Width and lengths are computed automatically.
 
 ### Point Cloud Pre-processing Parameters

grid_map_pcl/config/parameters.yaml

@@ -15,17 +15,18 @@ pcl_grid_map_extraction:
     max_num_points: 1000000
     use_max_height_as_cell_elevation: false
   outlier_removal:
-    is_remove_outliers: true
+    is_remove_outliers: false
     mean_K: 10
     stddev_threshold: 1.0
   downsampling:
-    is_downsample_cloud: true
+    is_downsample_cloud: false
     voxel_size:
       x: 0.02
       y: 0.02
       z: 0.02 
   grid_map:
     min_num_points_per_cell: 4
+    max_num_points_per_cell: 100000
     resolution: 0.1
 
 

grid_map_pcl/include/grid_map_pcl/GridMapPclLoader.hpp

@@ -92,6 +92,12 @@ class GridMapPclLoader {
    */
   void loadParameters(const std::string& filename);
 
+  /*!
+   * Set algorithm's parameters.
+   * @param[in] parameters of the algorithm
+   */
+  void setParameters(const grid_map_pcl::PclLoaderParameters::Parameters parameters);
+
   //! @return the parameters.
   const grid_map_pcl::PclLoaderParameters::Parameters& getParameters() const { return params_.get(); }
 

grid_map_pcl/include/grid_map_pcl/PclLoaderParameters.hpp

@@ -46,6 +46,7 @@ class PclLoaderParameters {
   struct GridMapParameters {
     double resolution_ = 0.1;
     unsigned int minCloudPointsPerCell_ = 2;
+    unsigned int maxCloudPointsPerCell_ = 100000;
   };
 
   struct Parameters {
@@ -64,7 +65,6 @@ class PclLoaderParameters {
   PclLoaderParameters() = default;
   ~PclLoaderParameters() = default;
 
- private:
   /*!
    * Load parameters for the GridMapPclLoader class.
    * @param[in] full path to the config file with parameters
@@ -76,7 +76,7 @@ class PclLoaderParameters {
    * Invoke operator[] on the yaml node. Finds
    * the parameters in the yaml tree.
    */
-  void handleYamlNode(const YAML::Node& yamlNode);
+  void loadParameters(const YAML::Node& yamlNode);
 
   /*!
    * Saves typing parameters_ in the owner class. The owner of this
@@ -85,6 +85,7 @@ class PclLoaderParameters {
    */
   const Parameters& get() const;
 
+ private:
   // Parameters for the GridMapPclLoader class.
   Parameters parameters_;
 };

grid_map_pcl/include/grid_map_pcl/helpers.hpp

@@ -22,7 +22,7 @@ namespace grid_map_pcl {
 
 void setVerbosityLevelToDebugIfFlagSet(const ros::NodeHandle& nh);
 
-std::string getParameterPath();
+std::string getParameterPath(const ros::NodeHandle& nh);
 
 std::string getOutputBagPath(const ros::NodeHandle& nh);
 

grid_map_pcl/launch/grid_map_pcl_loader_node.launch

@@ -1,24 +1,44 @@
 <?xml version="1.0" encoding="UTF-8"?>
 
 <launch>
-  <arg name="folder_path"            default="$(find grid_map_pcl)/config"/>
-  <arg name="pcd_filename"           default="plane_noisy.pcd" />
+  <arg name="folder_path"            default="$(find grid_map_pcl)/data"/>
+  <arg name="pcd_filename"           default="input_cloud.pcd" />
+  <arg name="config_file_path"       default="$(find grid_map_pcl)/config/parameters.yaml" />
   <arg name="map_rosbag_topic"       default="grid_map" />
-  <arg name="output_grid_map"        default="elevation_map.bag" />
+  <arg name="output_grid_map"        default="$(find grid_map_pcl)/data/input_cloud.bag" />
   <arg name="map_frame"              default="map" />
   <arg name="map_layer_name"         default="elevation" />
   <arg name="prefix"                 default=""/>
   <arg name="set_verbosity_to_debug" default="false"/>
+  <arg name="publish_point_cloud"    default="true"/>
 
-  <node name="grid_map_pcl_loader_node" pkg="grid_map_pcl"
-      type="grid_map_pcl_loader_node" output="screen" launch-prefix="$(arg prefix)">
-    <rosparam file="$(find grid_map_pcl)/config/parameters.yaml" />
+  <node name="grid_map_pcl_loader_node" 
+        pkg="grid_map_pcl"
+        type="grid_map_pcl_loader_node" 
+        output="screen" 
+        launch-prefix="$(arg prefix)">
+    <rosparam file="$(arg config_file_path)" />
+    <param name="config_file_path"        type="string" value="$(arg config_file_path)" />
     <param name="folder_path"             type="string" value="$(arg folder_path)" />
     <param name="pcd_filename"            type="string" value="$(arg pcd_filename)" />
     <param name="map_rosbag_topic"        type="string" value="$(arg map_rosbag_topic)" />
     <param name="output_grid_map"         type="string" value="$(arg output_grid_map)" />
     <param name="map_frame"               type="string" value="$(arg map_frame)" />
     <param name="map_layer_name"          type="string" value="$(arg map_layer_name)" />
-    <param name="set_verbosity_to_debug"  type="bool" value="$(arg set_verbosity_to_debug)" />
+    <param name="set_verbosity_to_debug"  type="bool"   value="$(arg set_verbosity_to_debug)" />
   </node>
-</launch>
+
+  <node name="rviz_grid_map_pcl" pkg="rviz" type="rviz" args="-d $(find grid_map_pcl)/rviz/grid_map_vis.rviz" />
+
+  <node name="point_cloud_publisher_node" 
+        pkg="grid_map_pcl"
+        type="pointcloud_publisher_node"
+        output="screen"
+        launch-prefix="$(arg prefix)"
+        if="$(arg publish_point_cloud)">
+    <param name="folder_path"             type="string" value="$(arg folder_path)" />
+    <param name="pcd_filename"            type="string" value="$(arg pcd_filename)" />
+    <param name="cloud_frame"             type="string" value="$(arg map_frame)" />
+  </node>
+
+</launch>

grid_map_pcl/package.xml

@@ -12,14 +12,19 @@
   <author email="[email protected]">Edo Jelavic</author>
 
   <buildtool_depend>catkin</buildtool_depend>
+
 <!--   <build_depend>cmake_clang_tools</build_depend> -->
+
   <depend>grid_map_core</depend>
   <depend>grid_map_msgs</depend>
   <depend>grid_map_ros</depend>
   <depend>libpcl-all-dev</depend>
   <depend>pcl_ros</depend>
   <depend>roscpp</depend>
   <depend>yaml-cpp</depend>
+
+  <exec_depend>rviz</exec_depend>
+
 <!--   <test_depend>cmake_code_coverage</test_depend> -->
   <test_depend>gtest</test_depend>
 </package>