add option for low obstacle detection by grid map

DCO 1.1 Signed-off-by: Tatsuya Ishihara <[email protected]>

cabot_navigation2/CMakeLists.txt

@@ -18,6 +18,8 @@ endif()
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(grid_map_msgs REQUIRED)
+find_package(grid_map_ros REQUIRED)
 find_package(nav2_lifecycle_manager REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
 find_package(nav2_core REQUIRED)
@@ -49,23 +51,25 @@ find_package(rosbag2_cpp REQUIRED)
 find_package(rosbag2_transport REQUIRED)
 find_package(rosbag2_storage REQUIRED)
 find_package(rosbag2_compression REQUIRED)
-#find_package(OpenMP REQUIRED)
+find_package(OpenMP REQUIRED)
 
 
 include_directories(
   include
-#  ${OPENMP_INCLUDES}
+  ${OPENMP_INCLUDES}
   ${OpenCV_INCLUDE_DIRS}
   ${PCL_INCLUDE_DIRS}
+  ${grid_map_core_INCLUDE_DIRS}
 )
 
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
-#set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
 
 if(NOT DEBUG)
 
 set(dependencies
   geometry_msgs
+  grid_map_msgs
+  grid_map_ros
   rclcpp
   nav2_lifecycle_manager
   nav2_costmap_2d
@@ -83,7 +87,7 @@ set(dependencies
   diagnostic_updater
   visualization_msgs
   yaml_cpp_vendor
-#  OpenMP
+  OpenMP
 )
 
 ### dwb critics
@@ -178,6 +182,7 @@ ament_target_dependencies(clip_ground_filter_node
   pcl_conversions
   pcl_ros
   tf2_ros
+  visualization_msgs
 )
 
 target_link_libraries(clip_ground_filter_node
@@ -203,6 +208,28 @@ target_link_libraries(ransac_ground_filter_node
   ${PCL_LIBRARIES}
 )
 
+### grid map ground filter
+add_executable(grid_map_ground_filter_node
+  src/abstract_ground_filter_node.cpp
+  src/grid_map_ground_filter_node.cpp
+)
+
+ament_target_dependencies(grid_map_ground_filter_node
+  rclcpp
+  sensor_msgs
+  pcl_conversions
+  pcl_ros
+  tf2_ros
+  visualization_msgs
+  grid_map_msgs
+  grid_map_ros
+)
+
+target_link_libraries(grid_map_ground_filter_node
+  ${PCL_LIBRARIES}
+  ${OPENMP_LIBRARIES}
+)
+
 ### cabot lifecycle manager
 add_executable(cabot_lifecycle_manager
   src/cabot_lifecycle_manager.cpp
@@ -234,6 +261,7 @@ install(TARGETS
   cabot_lifecycle_manager
   clip_ground_filter_node
   ransac_ground_filter_node
+  grid_map_ground_filter_node
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}

cabot_navigation2/include/cabot_navigation2/abstract_ground_filter_node.hpp

@@ -64,9 +64,17 @@ class AbstractGroundFilterNode : public rclcpp::Node
   ~AbstractGroundFilterNode();
 
 protected:
-  virtual void filterGround(const pcl::PointCloud<pcl::PointXYZ>::Ptr input, pcl::PointCloud<pcl::PointXYZ>::Ptr ground, pcl::PointCloud<pcl::PointXYZ>::Ptr filtered) = 0;
+  virtual void filterGround(const rclcpp::Time & time, const pcl::PointCloud<pcl::PointXYZ>::Ptr input, pcl::PointCloud<pcl::PointXYZ>::Ptr ground, pcl::PointCloud<pcl::PointXYZ>::Ptr filtered) = 0;
 
   std::string target_frame_;
+  float min_range_;
+  float max_range_;
+  bool publish_debug_ground_;
+  std::string output_debug_ground_topic_;
+  float ground_distance_threshold_;
+
+  tf2_ros::TransformListener * tf_listener_;
+  tf2_ros::Buffer * tf_buffer_;
 
 private:
   void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr input);
@@ -76,11 +84,6 @@ class AbstractGroundFilterNode : public rclcpp::Node
   std::string input_topic_;
   std::string output_ground_topic_;
   std::string output_filtered_topic_;
-  float min_range_;
-  float max_range_;
-
-  tf2_ros::TransformListener * tf_listener_;
-  tf2_ros::Buffer * tf_buffer_;
 
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_sub_;
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr ground_pointcloud_pub_;

cabot_navigation2/include/cabot_navigation2/clip_ground_filter_node.hpp

@@ -21,6 +21,8 @@
 #ifndef CABOT_NAVIGATION2__CLIP_GROUND_FILTER_NODE_HPP_
 #define CABOT_NAVIGATION2__CLIP_GROUND_FILTER_NODE_HPP_
 
+#include <visualization_msgs/msg/marker.hpp>
+
 #include "cabot_navigation2/abstract_ground_filter_node.hpp"
 
 namespace cabot_navigation2
@@ -32,10 +34,10 @@ class ClipGroundFilterNode : public AbstractGroundFilterNode
   explicit ClipGroundFilterNode(const rclcpp::NodeOptions & options);
 
 protected:
-  void filterGround(const pcl::PointCloud<pcl::PointXYZ>::Ptr input, pcl::PointCloud<pcl::PointXYZ>::Ptr ground, pcl::PointCloud<pcl::PointXYZ>::Ptr filtered) override;
+  void filterGround(const rclcpp::Time & time, const pcl::PointCloud<pcl::PointXYZ>::Ptr input, pcl::PointCloud<pcl::PointXYZ>::Ptr ground, pcl::PointCloud<pcl::PointXYZ>::Ptr filtered) override;
 
 private:
-  float clip_height_;
+  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr debug_plane_pub_;
 };  // class ClipGroundFilterNode
 
 }  // namespace cabot_navigation2

cabot_navigation2/include/cabot_navigation2/grid_map_ground_filter_node.hpp

@@ -0,0 +1,92 @@
+// Copyright (c) 2024  Carnegie Mellon University, IBM Corporation, and others
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#ifndef CABOT_NAVIGATION2__GRID_MAP_GROUND_FILTER_NODE_HPP_
+#define CABOT_NAVIGATION2__GRID_MAP_GROUND_FILTER_NODE_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <grid_map_core/grid_map_core.hpp>
+#include <grid_map_msgs/msg/grid_map.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+
+#include "cabot_navigation2/abstract_ground_filter_node.hpp"
+
+namespace cabot_navigation2
+{
+
+class GridMapGroundFilterNode : public AbstractGroundFilterNode
+{
+public:
+  explicit GridMapGroundFilterNode(const rclcpp::NodeOptions & options);
+
+protected:
+  void filterGround(const rclcpp::Time & time, const pcl::PointCloud<pcl::PointXYZ>::Ptr input, pcl::PointCloud<pcl::PointXYZ>::Ptr ground, pcl::PointCloud<pcl::PointXYZ>::Ptr filtered) override;
+
+private:
+  void odomCallback(const nav_msgs::msg::Odometry::SharedPtr input);
+  int calcLivoxGridEstimatedNumPoints(float distance, float resolution);
+  bool isVisibleAngle(const grid_map::Position & check_position, const grid_map::Position & sensor_position, double sensor_yaw);
+
+  int num_threads_;
+  std::string odom_topic_;
+  float grid_resolution_;
+  float grid_length_;
+  std::vector<int64_t> grid_patch_sizes_;
+  std::vector<double> grid_patch_change_distances_;
+  int grid_occupied_inflate_size_;
+  int grid_num_points_min_threshold_;
+  float grid_num_points_raio_threshold_;
+  float grid_var_threshold_;
+  float grid_prob_prior_;
+  float grid_prob_free_;
+  float grid_prob_occupied_;
+  float grid_prob_forget_rate_;
+  float grid_prob_free_threshold_;
+  float grid_prob_occupied_threshold_;
+  float outlier_old_ground_threshold_;
+  float outlier_los_ground_threshold_;
+  float ground_estimate_angle_min_;
+  float ground_estimate_angle_max_;
+  float ground_slope_threshold_;
+  float ground_confidence_interpolate_decay_;
+
+  static const int livox_num_points_;
+  static const float livox_tan_fov_angle_;
+
+  std::vector<int64_t> grid_half_patch_sizes_;
+  float log_odds_prior_;
+  float log_odds_free_;
+  float log_odds_occupied_;
+  float ground_estimate_radius_;
+
+  std::recursive_mutex grid_map_mutex_;
+  std::shared_ptr<grid_map::GridMap> grid_map_ptr_;
+
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr debug_outlier_pointcloud_pub_;
+  rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr debug_grid_map_pub_;
+};  // class GridMapGroundFilterNode
+
+}  // namespace cabot_navigation2
+#endif  // CABOT_NAVIGATION2__GRID_MAP_GROUND_FILTER_NODE_HPP_

cabot_navigation2/include/cabot_navigation2/ransac_ground_filter_node.hpp

@@ -36,7 +36,7 @@ class RansacGroundFilterNode : public AbstractGroundFilterNode
   explicit RansacGroundFilterNode(const rclcpp::NodeOptions & options);
 
 protected:
-  void filterGround(const pcl::PointCloud<pcl::PointXYZ>::Ptr input, pcl::PointCloud<pcl::PointXYZ>::Ptr ground, pcl::PointCloud<pcl::PointXYZ>::Ptr filtered) override;
+  void filterGround(const rclcpp::Time & time, const pcl::PointCloud<pcl::PointXYZ>::Ptr input, pcl::PointCloud<pcl::PointXYZ>::Ptr ground, pcl::PointCloud<pcl::PointXYZ>::Ptr filtered) override;
 
 private:
   int ransac_max_iteration_;
@@ -45,9 +45,6 @@ class RansacGroundFilterNode : public AbstractGroundFilterNode
   float ransac_input_min_height_;
   float ransac_input_max_height_;
   float ransac_inlier_threshold_;
-  float ground_distance_threshold_;
-  bool debug_;
-  std::string debug_output_plane_topic_;
 
   rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr debug_plane_pub_;
 };  // class RansacGroundFilterNode

cabot_navigation2/launch/bringup_launch.py

@@ -55,7 +55,9 @@ def generate_launch_description():
     offset = LaunchConfiguration('offset')
     cabot_side = LaunchConfiguration('cabot_side')
     low_obstacle_detect_version = LaunchConfiguration('low_obstacle_detect_version')
-    use_low_obstacle_detect = PythonExpression([low_obstacle_detect_version, ' > 0'])
+    publish_low_obstacle_ground = LaunchConfiguration('publish_low_obstacle_ground')
+
+    use_low_obstacle_detect = PythonExpression([low_obstacle_detect_version, " > 0"])
 
     remappings = [('/tf', 'tf'),
                   ('/tf_static', 'tf_static')]
@@ -162,7 +164,11 @@ def generate_launch_description():
 
         DeclareLaunchArgument(
             'low_obstacle_detect_version', default_value='0',
-            description='0: do not detect, 1: remove ground by fixed height, 2: remove groud by RANSAC'),
+            description='0: do not detect, 1: remove ground by fixed height, 2: remove groud by RANSAC, 3: remove groud by grid map'),
+
+        DeclareLaunchArgument(
+            'publish_low_obstacle_ground', default_value='false',
+            description='publish ground to detect low obstacles only for debug purpose'),
 
         # default navigator
         Node(
@@ -364,16 +370,18 @@ def generate_launch_description():
             parameters=[{
                 'use_sim_time': use_sim_time,
                 'target_frame': 'livox_footprint',
+                'min_range': 0.05,
+                'max_range': 5.0,
+                'publish_debug_ground': publish_low_obstacle_ground,
+                'output_debug_ground_topic': '/ground_filter_ground',
+                'ground_distance_threshold': 0.05,
                 'xfer_format': PythonExpression(["2 if '", use_sim_time, "'=='true' else 0"]),
                 'ignore_noise': True,
                 'input_topic': '/livox/points',
                 'output_ground_topic': '/livox/points_ground',
-                'output_filtered_topic': '/livox/points_filtered',
-                'min_range': 0.05,
-                'max_range': 5.0,
-                'clip_height': 0.05
+                'output_filtered_topic': '/livox/points_filtered'
             }],
-            condition=IfCondition(PythonExpression([low_obstacle_detect_version, ' == 1']))
+            condition=IfCondition(PythonExpression([low_obstacle_detect_version, " == 1"]))
         ),
 
         Node(
@@ -384,24 +392,77 @@ def generate_launch_description():
             parameters=[{
                 'use_sim_time': use_sim_time,
                 'target_frame': 'livox_footprint',
+                'min_range': 0.05,
+                'max_range': 5.0,
+                'publish_debug_ground': publish_low_obstacle_ground,
+                'output_debug_ground_topic': '/ground_filter_ground',
+                'ground_distance_threshold': 0.05,
                 'xfer_format': PythonExpression(["2 if '", use_sim_time, "'=='true' else 0"]),
                 'ignore_noise': True,
                 'input_topic': '/livox/points',
                 'output_ground_topic': '/livox/points_ground',
                 'output_filtered_topic': '/livox/points_filtered',
-                'min_range': 0.05,
-                'max_range': 5.0,
                 'ransac_max_iteration': 10000,
                 'ransac_probability': 0.999,
                 'ransac_eps_angle': 5.0,
                 'ransac_input_min_height': -0.50,
                 'ransac_input_max_height': 0.50,
-                'ransac_inlier_threshold': 0.01,
+                'ransac_inlier_threshold': 0.01
+            }],
+            condition=IfCondition(PythonExpression([low_obstacle_detect_version, " == 2"]))
+        ),
+
+        Node(
+            package='cabot_navigation2',
+            executable='grid_map_ground_filter_node',
+            namespace='',
+            name='grid_map_ground_filter_node',
+            parameters=[{
+                'use_sim_time': use_sim_time,
+                'target_frame': 'livox_footprint',
+                'min_range': 0.05,
+                'max_range': 5.0,
+                'publish_debug_ground': publish_low_obstacle_ground,
+                'output_debug_ground_topic': '/ground_filter_ground',
                 'ground_distance_threshold': 0.05,
-                'debug': False,
-                'debug_output_plane_topic': '/ground_filter/ransac_plane'
+                'xfer_format': PythonExpression(["2 if '", use_sim_time, "'=='true' else 0"]),
+                'ignore_noise': True,
+                'input_topic': '/livox/points',
+                'output_ground_topic': '/livox/points_ground',
+                'output_filtered_topic': '/livox/points_filtered',
+                'num_threads': 2,
+                'odom_topic': '/odom',
+                'grid_resolution': 0.05,
+                'grid_length': 10.0,
+                'grid_patch_sizes': [3, 5],
+                'grid_patch_change_distances': [1.0],
+                'grid_occupied_inflate_size': 5,
+                'grid_num_points_min_threshold': 5,
+                'grid_num_points_raio_threshold': 0.1,
+                'grid_var_threshold': 0.001,
+                'grid_prob_prior': 0.5,
+                'grid_prob_free': 0.1,
+                'grid_prob_occupied': 0.9,
+                'grid_prob_forget_rate': 0.2,
+                'grid_prob_free_threshold': 0.15,
+                'grid_prob_occupied_threshold': 0.55,
+                'outlier_old_ground_threshold': 0.05,
+                'outlier_los_ground_threshold': 0.05,
+                'ground_estimate_angle_min': -0.614,  # -35.2*M_PI/180
+                'ground_estimate_angle_max': 0.614,  # 35.2*M_PI/180
+                'ground_slope_threshold': 0.262,  # 15.0*M_PI/180
+                'ground_confidence_interpolate_decay': 0.5
             }],
-            condition=IfCondition(PythonExpression([low_obstacle_detect_version, ' == 2']))
+            condition=IfCondition(PythonExpression([low_obstacle_detect_version, " == 3"]))
+        ),
+
+        Node(
+            package='grid_map_visualization',
+            executable='grid_map_visualization',
+            namespace='',
+            name='grid_map_visualization',
+            parameters=[configured_params],
+            condition=IfCondition(PythonExpression([low_obstacle_detect_version, " == 3 and '", publish_low_obstacle_ground, "' == 'true'"]))
         ),
 
         # others

cabot_navigation2/package.xml

@@ -31,6 +31,8 @@
   <license>MIT</license>
 
   <depend>geometry_msgs</depend>
+  <depend>grid_map_msgs</depend>
+  <depend>grid_map_ros</depend>
   <depend>nav2_costmap_2d</depend>
   <depend>nav2_core</depend>
   <depend>angles</depend>