Fix/traversability ghost obstacles

igvc_msgs/CMakeLists.txt

@@ -17,7 +17,6 @@ add_message_files(
     velocity_quad.msg
     velocity_pair.msg
     lights.msg
-    action_path.msg
     system_stats.msg
     map.msg
     igvc_path.msg

igvc_navigation/src/mapper/traversability_layer.cpp

@@ -82,7 +82,7 @@ void TraversabilityLayer::slopeMapCallback(const grid_map_msgs::GridMap &slope_m
   {
     grid_map::Position pos;
     slope_map.getPosition((*it), pos);
-    if (map_.isInside(pos))
+    if (map_.isInside(pos) && slope_map.get("mask_dilated")((*it)[0], (*it)[1]) > 0)
     {
       float slope = slope_map.get("slope")((*it)[0], (*it)[1]);
       grid_map::Index map_index;

igvc_perception/CMakeLists.txt

@@ -25,6 +25,7 @@ find_package(catkin REQUIRED COMPONENTS
     grid_map_core
     grid_map_ros
     grid_map_msgs
+    grid_map_cv
     )
 
 set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)

igvc_perception/config/elevation_mapping.yaml

@@ -19,7 +19,7 @@ min_variance: 0.000009
 max_variance: 0.01
 mahalanobis_distance_threshold: 2.5
 multi_height_noise: 0.0000009
-fused_map_publishing_rate: 0
+fused_map_publishing_rate: 1.0
 enable_visibility_cleanup: false
 
 # SENSOR PARAMETERS

igvc_perception/config/slope_filter.yaml

@@ -4,11 +4,11 @@ slope_map_filters:
       type: gridMapFilters/BufferNormalizerFilter
 
     # Delete unneeded layers.
-    - name: delete_original_layers
-      type: gridMapFilters/DeletionFilter
-      params:
-          layers: [horizontal_variance_x, horizontal_variance_y, horizontal_variance_xy,
-                   color, time, standard_deviation, horizontal_standard_deviation, two_sigma_bound] # List of layers.
+    # - name: delete_original_layers
+    #   type: gridMapFilters/DeletionFilter
+    #   params:
+    #       layers: [horizontal_variance_x, horizontal_variance_y, horizontal_variance_xy,
+    #                color, time, standard_deviation, horizontal_standard_deviation, two_sigma_bound] # List of layers.
 
     # Boxblur as an alternative to the inpaint and radial blurring filter above.
     - name: boxblur
@@ -36,3 +36,6 @@ slope_map_filters:
       params:
           output_layer: slope
           expression: acos(normal_vectors_z)
+
+erode_radius: 7
+dilate_radius: 6

igvc_perception/package.xml

@@ -46,6 +46,7 @@
   <depend>grid_map_core</depend>
   <depend>grid_map_ros</depend>
   <depend>grid_map_msgs</depend>
+  <depend>grid_map_cv</depend>
 
   <build_depend>image_transport</build_depend>
   <build_depend>cv_bridge</build_depend>

igvc_perception/src/slope_filter/slope_filter.cpp

@@ -1,13 +1,19 @@
 #include "slope_filter.h"
 #include <grid_map_ros/GridMapRosConverter.hpp>
+#include <parameter_assertions/assertions.h>
+#include <grid_map_cv/grid_map_cv.hpp>
 
 TraversabilityFilter::TraversabilityFilter() : filter_chain_("grid_map::GridMap")
 {
+  ROS_INFO_STREAM("RAW INFO RECEIVED");
   private_nh_ = ros::NodeHandle("~");
-  elevation_map_sub_ = private_nh_.subscribe("/elevation_mapping/elevation_map_raw", 1,
-                                             &TraversabilityFilter::elevationMapCallback, this);
+  elevation_map_sub_ =
+      private_nh_.subscribe("/elevation_mapping/elevation_map", 1, &TraversabilityFilter::elevationMapCallback, this);
   traversability_map_pub_ = private_nh_.advertise<grid_map_msgs::GridMap>("/slope/gridmap", 1);
 
+  assertions::getParam(private_nh_, "erode_radius", erode_radius);
+  assertions::getParam(private_nh_, "dilate_radius", dilate_radius);
+
   // setup filter chain
   if (!filter_chain_.configure("/slope_filter/slope_map_filters", private_nh_))
   {
@@ -21,15 +27,32 @@ void TraversabilityFilter::elevationMapCallback(const grid_map_msgs::GridMap& me
   grid_map::GridMap input_map;
   grid_map::GridMapRosConverter::fromMessage(message, input_map);
 
-  grid_map::GridMap output_map;
-  if (!filter_chain_.update(input_map, output_map))
+  grid_map::GridMap slope_map;
+  if (!filter_chain_.update(input_map, slope_map))
   {
     ROS_ERROR_STREAM("Could not update the grid map filter chain!");
     return;
   }
 
+  cv::Mat elevation_data_mask;
+  grid_map::GridMapCvConverter::toImage<cv::uint8_t, 1>(input_map, "elevation", CV_8U, -1.0, 0.0, elevation_data_mask);
+  grid_map::GridMapCvConverter::addLayerFromImage<cv::uint8_t, 1>(elevation_data_mask, "mask_original", slope_map, 0,
+                                                                  255, 0);
+
+  cv::erode(elevation_data_mask, elevation_data_mask,
+            cv::getStructuringElement(cv::MORPH_RECT, cv::Size(2 * erode_radius + 1, 2 * erode_radius + 1),
+                                      cv::Point(erode_radius, erode_radius)));
+  grid_map::GridMapCvConverter::addLayerFromImage<cv::uint8_t, 1>(elevation_data_mask, "mask_eroded", slope_map, 0, 255,
+                                                                  0);
+
+  cv::dilate(elevation_data_mask, elevation_data_mask,
+             cv::getStructuringElement(cv::MORPH_RECT, cv::Size(2 * dilate_radius + 1, 2 * dilate_radius + 1),
+                                       cv::Point(dilate_radius, dilate_radius)));
+  grid_map::GridMapCvConverter::addLayerFromImage<cv::uint8_t, 1>(elevation_data_mask, "mask_dilated", slope_map, 0,
+                                                                  255, 0);
+
   grid_map_msgs::GridMap output_message;
-  grid_map::GridMapRosConverter::toMessage(output_map, output_message);
+  grid_map::GridMapRosConverter::toMessage(slope_map, output_message);
   traversability_map_pub_.publish(output_message);
 }
 

igvc_perception/src/slope_filter/slope_filter.h

@@ -2,7 +2,7 @@
 #define SRC_SLOPE_FILTER_H
 
 #include <ros/ros.h>
-#include <filters/filter_chain.h>
+#include <filters/filter_chain.hpp>
 #include <grid_map_core/GridMap.hpp>
 #include <grid_map_msgs/GridMap.h>
 
@@ -17,6 +17,9 @@ class TraversabilityFilter
   ros::Publisher traversability_map_pub_;
   filters::FilterChain<grid_map::GridMap> filter_chain_;
 
+  int erode_radius;
+  int dilate_radius;
+
   void elevationMapCallback(const grid_map_msgs::GridMap& message);
 };