Feature/filtered elevation layer

.gitignore

@@ -27,3 +27,7 @@
 
 # Sublime
 *.sublime*
+
+# VSCode
+.vscode
+*/build/*

README.md

@@ -91,9 +91,9 @@ Run the unit tests with
 In order to get the Robot-Centric Elevation Mapping to run with your robot, you will need to adapt a few parameters. It is the easiest if duplicate and adapt all the parameter files that you need to change from the `elevation_mapping_demos` package (e.g. the `simple_demo` example). These are specifically the parameter files in `config` and the launch file from the `launch` folder.
 
 
-## Nodes
+## Packages
 
-### Node: elevation_mapping
+### elevation_mapping
 
 This is the main Robot-Centric Elevation Mapping node. It uses the distance sensor measurements and the pose and covariance of the robot to generate an elevation map with variance estimates.
 
@@ -235,6 +235,11 @@ This is the main Robot-Centric Elevation Mapping node. It uses the distance sens
 
     The data for the sensor noise model.
 
+### elevation_mapping_costmap_2d_plugin
+
+This is a plugin for [costmap_2d] and provides an implementation of a 2d costmap. With the `elevation_mapping_costmap_2d_plugin` package, the generated elevation map can be used to build a costmap_2d to work with the [ROS] [navigation] stack. It takes the elevation map as input and generates a layer of obstacles in the local costmap_2d.
+
+The layer applies a height and a sharpness filter to the incoming data. The behaviour can be adjusted tuning the parameters `heightThreshold_` and `edgesSharpnessThreshold_`. The former labels as obstacles all data above a certain height, while the latter labels as obstacles data above a certain edge sharpness, where a sharp edge is defined as a sudden change in height; this is used to avoid labeling slopes as obstacle. 
 
 ## Bugs & Feature Requests
 
@@ -249,3 +254,5 @@ Please report bugs and request features using the [Issue Tracker](https://github
 [tf/tfMessage]: http://docs.ros.org/kinetic/api/tf/html/msg/tfMessage.html
 [std_srvs/Empty]: http://docs.ros.org/api/std_srvs/html/srv/Empty.html
 [grid_map_msg/GetGridMap]: https://github.com/anybotics/grid_map/blob/master/grid_map_msg/srv/GetGridMap.srv
+[costmap_2d]: http://wiki.ros.org/costmap_2d
+[navigation]: http://wiki.ros.org/navigation

elevation_mapping_costmap_2d_plugin/CMakeLists.txt

@@ -0,0 +1,97 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(elevation_mapping_costmap_2d_plugin)
+
+## Use C++11
+add_definitions(-std=c++11)
+## By adding -Wall and -Werror, the compiler does not ignore warnings anymore,
+## enforcing cleaner code.
+#add_definitions(-std=c++11 -Wall -Werror)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  tf
+  costmap_2d
+  dynamic_reconfigure
+  grid_map_ros
+  filters
+)
+
+# Find system libraries
+find_package(Boost REQUIRED)
+
+# add dynamic reconfigure configs
+generate_dynamic_reconfigure_options(
+  cfg/ElevationPlugin.cfg
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS
+    include
+  LIBRARIES
+    ${PROJECT_NAME}
+  CATKIN_DEPENDS
+    roscpp
+    tf
+    costmap_2d
+    dynamic_reconfigure
+    grid_map_ros
+    filters
+  DEPENDS
+    Boost
+)
+
+## build ##
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+        include
+        ${catkin_INCLUDE_DIRS}
+        ${Boost_INCLUDE_DIRS}
+)
+
+## declare a cpp library
+add_library(${PROJECT_NAME}
+  src/elevation_layer.cpp
+)
+
+
+## cmake target dependencies of the executable/library
+
+# build config headers
+add_dependencies(${PROJECT_NAME} elevation_mapping_costmap_2d_plugin_gencfg)
+
+## libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)
+
+
+## install ##
+
+## executables and/or libraries for installation
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## cpp-header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+)

elevation_mapping_costmap_2d_plugin/cfg/ElevationPlugin.cfg

@@ -0,0 +1,9 @@
+#!/usr/bin/env python
+PACKAGE = "elevation_mapping_costmap_2d_plugin"
+
+from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t
+
+gen = ParameterGenerator()
+gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True)
+
+exit(gen.generate("elevation_mapping_costmap_2d_plugin", "elevation_mapping_costmap_2d_plugin", "ElevationPlugin"))

elevation_mapping_costmap_2d_plugin/config/elevation_filters.yaml

@@ -0,0 +1,10 @@
+elevation_filters:
+ - name: edge_detection     # Edge detection on elevation layer with convolution filter.
+   type: gridMapFilters/SlidingWindowMathExpressionFilter
+   params:
+     input_layer: elevation
+     output_layer: edges
+     expression: 'sumOfFinites([0,1,0;1,-4,1;0,1,0].*elevation)' # Edge detection.
+     compute_empty_cells: false
+     edge_handling: mean
+     window_size: 3 # Make sure to make this compatible with the kernel matrix.

elevation_mapping_costmap_2d_plugin/costmap_plugins.xml

@@ -0,0 +1,7 @@
+<class_libraries>
+  <library path="lib/libelevation_mapping_costmap_2d_plugin">
+    <class type="elevation_mapping_costmap_2d_plugin::ElevationLayer" base_class_type="costmap_2d::Layer">
+      <description>Adds elevation map info to costmap_2d.</description>
+    </class>
+  </library>
+</class_libraries>

elevation_mapping_costmap_2d_plugin/include/elevation_mapping_costmap_2d_plugin/elevation_layer.hpp

@@ -0,0 +1,194 @@
+/*
+ * elevation_layer.h
+ *
+ *  Created on: Nov 5, 2018
+ *      Author: Eugenio Chisari
+ *	 Institute: ANYbotics
+ */
+
+#pragma once
+
+// C++ Libraries.
+#include <atomic>
+#include <boost/algorithm/string/predicate.hpp>
+#include <mutex>
+
+// Ros headers.
+#include <dynamic_reconfigure/server.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <ros/ros.h>
+
+// Costmap2d headers.
+#include <costmap_2d/costmap_layer.h>
+#include <costmap_2d/footprint.h>
+#include <costmap_2d/layered_costmap.h>
+#include <costmap_2d/observation_buffer.h>
+
+// Package related headers.
+#include <filters/filter_chain.h>
+#include <message_filters/subscriber.h>
+#include <grid_map_ros/GridMapRosConverter.hpp>
+#include "elevation_mapping_costmap_2d_plugin/ElevationPluginConfig.h"
+
+namespace elevation_mapping_costmap_2d_plugin {
+
+/*!
+ * Method to update the cost of a portion of map.
+ */
+enum CombinationMethod { Overwrite, Maximum, Unknown };
+
+/*!
+ * Converts the string from the yaml file to the proper CombinationMethod enum.
+ * @param str Input string from yaml file.
+ * @return CombinationMethod Enum equivalent.
+ */
+CombinationMethod convertCombinationMethod(const std::string& str);
+
+/*!
+ * Plug-in layer of a costmap_2d derived from elevation_map information.
+ */
+class ElevationLayer : public costmap_2d::CostmapLayer {
+ public:
+  /*!
+   * Constructor.
+   */
+  ElevationLayer();
+
+  /*!
+   * Destructor.
+   */
+  ~ElevationLayer() override = default;
+
+  /*!
+   * Function called from parent class at initialization.
+   */
+  void onInitialize() override;
+
+  /*!
+   * @brief This is called by the LayeredCostmap to poll this plugin as to how
+   *        much of the costmap it needs to update. Each layer can increase
+   *        the size of this bounds.
+   *
+   * For more details, see "Layered Costmaps for Context-Sensitive Navigation",
+   * by Lu et. Al, IROS 2014.
+   */
+  void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y) override;
+
+  /*!
+   * @brief Actually update the underlying costmap, only within the bounds
+   *        calculated during UpdateBounds().
+   */
+  void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) override;
+
+  /*! 
+   * @brief Restart publishers if they've been stopped. 
+   */
+  void activate() override;
+
+  /*!
+   * @brief Stop publishers. 
+   */
+  void deactivate() override;
+
+  /*! 
+   * @brief Deactivate, reset the map and then reactivate
+   */
+  void reset() override;
+
+  /*!
+   * Callback to receive the grid_map msg from elevation_map.
+   * @param occupancy_grid GridMap msg from elevation_map.
+   */
+  void elevationMapCallback(const grid_map_msgs::GridMapConstPtr& occupancy_grid);
+
+ protected:
+  /*!
+   * Set up the dynamic reconfigure.
+   * @param nh
+   */
+  virtual void setupDynamicReconfigure(ros::NodeHandle& nh);
+
+  /*!
+   * Clear obstacles inside the footprint of the robort if the flag footprintClearingEnabled_ is true.
+   * @param robot_x
+   * @param robot_y
+   * @param robot_yaw
+   * @param min_x
+   * @param min_y
+   * @param max_x
+   * @param max_y
+   */
+  void updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y);
+
+  //! The global frame for the costmap.
+  std::string globalFrame_;
+
+  //! Dynamic reconfigure server.
+  std::unique_ptr<dynamic_reconfigure::Server<elevation_mapping_costmap_2d_plugin::ElevationPluginConfig> > dsrv_;
+
+  //! Combination method to use to update the cost of a portion of map.
+  CombinationMethod combinationMethod_;
+
+  //! Polygon describing the form of the footprint of the robot.
+  std::vector<geometry_msgs::Point> transformedFootprint_;
+
+  //! Whether the local costmap should move together with the robot.
+  bool rollingWindow_;
+
+  //! Whether to clean the obstacles inside the robot footprint.
+  bool footprintClearingEnabled_;
+
+  //! Whether the elevation_map msg was received.
+  std::atomic_bool elevationMapReceived_;
+
+  //! After this time [seconds] without receiving any elevation_map, the robot will have to stop.
+  double maxAllowedBlindTime_;
+
+ private:
+  /*!
+   * Dynamic reconfiguration of the parameters.
+   * @param config
+   * @param level
+   */
+  void reconfigureCB(elevation_mapping_costmap_2d_plugin::ElevationPluginConfig& config, uint32_t level);
+
+  ros::NodeHandle nodeHandle_;
+
+  //! The elevation_map from which to take the information abut the environment (filtered or not).
+  grid_map::GridMap elevationMap_;
+
+  //! lock_guard mutex to make elevation_map setting thread safe.
+  std::mutex elevationMapMutex_;
+
+  //! Ros subscriber to grid_map msgs.
+  ros::Subscriber elevationSubscriber_;
+
+  //! Last time an elevation_map was received.
+  ros::Time lastElevationMapUpdate_;
+
+  //! Height threshold below which nothing is considered obstacle.
+  double heightThreshold_;
+
+  //! Sharpness threshold above which an object is considered an obstacle.
+  double edgesSharpnessThreshold_;
+
+  //! Topic_name of the elevation_map incoming msg.
+  std::string elevationTopic_;
+
+  //! Filter chain used to filter the incoming elevation_map.
+  filters::FilterChain<grid_map::GridMap> filterChain_;
+
+  //! Filter chain parameters name to use.
+  std::string filterChainParametersName_;
+
+  //! Whether filters configuration parameters was found.
+  bool filtersConfigurationLoaded_;
+
+  //! Name of the layer of the incoming map to use.
+  std::string elevationLayerName_;
+
+  //! Name to give to the filtered layer.
+  std::string edgesLayerName_;
+};
+
+}  // namespace elevation_mapping_costmap_2d_plugin

elevation_mapping_costmap_2d_plugin/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>elevation_mapping_costmap_2d_plugin</name>
+  <version>0.0.2</version>
+  <description>adds elevation mapping informations to costmap_2d</description>
+  <author email="echisari@anybotics.com">Eugenio Chisari</author>
+  <maintainer email="echisari@anybotics.com">Eugenio Chisari</maintainer>
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>roscpp</depend>
+  <depend>boost</depend>
+  <depend>tf</depend>
+  <depend>costmap_2d</depend>
+  <depend>dynamic_reconfigure</depend>
+  <depend>grid_map_ros</depend>
+  <depend>filters</depend>
+
+  <export>
+    <costmap_2d plugin="${prefix}/costmap_plugins.xml" />
+  </export>
+</package>