Feature/filtered elevation layer

elevation_layer/CMakeLists.txt

@@ -0,0 +1,97 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(elevation_layer)
+
+## 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
+    elevation_layer
+  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_layer_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 elevation_layer
+  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_layer/README.md

@@ -0,0 +1,3 @@
+# Elevation layer
+
+A plugin for [costmap_2d]](http://wiki.ros.org/costmap_2d). It takes the elevation map as input and generates a layer of obstacles in the local costmap_2d.

elevation_layer/cfg/ElevationPlugin.cfg

@@ -0,0 +1,9 @@
+#!/usr/bin/env python
+PACKAGE = "elevation_layer"
+
+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_layer", "elevation_layer", "ElevationPlugin"))

elevation_layer/config/elevation_filters.yaml

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

elevation_layer/costmap_plugins.xml

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

elevation_layer/include/elevation_layer/elevation_layer.h

@@ -0,0 +1,184 @@
+/*
+ * elevation_layer.h
+ *
+ *  Created on: Nov 5, 2018
+ *      Author: Eugenio Chisari
+ *	 Institute: ANYbotics
+ */
+
+#pragma once
+
+#include <atomic>
+#include <mutex>
+
+#include <costmap_2d/costmap_layer.h>
+#include <costmap_2d/layered_costmap.h>
+#include <costmap_2d/observation_buffer.h>
+#include <filters/filter_chain.h>
+#include <message_filters/subscriber.h>
+#include <ros/ros.h>
+#include "grid_map_ros/GridMapRosConverter.hpp"
+
+#include <costmap_2d/footprint.h>
+#include <dynamic_reconfigure/server.h>
+#include <elevation_layer/ElevationPluginConfig.h>
+#include <nav_msgs/OccupancyGrid.h>
+
+namespace elevation_layer {
+
+/*!
+ * 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;
+
+  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 footprint_clearing_enabled_ 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);
+
+  ///< @brief The global frame for the costmap
+  std::string global_frame_;
+
+  ///< @brief Used for the observation message filters
+  std::vector<boost::shared_ptr<message_filters::SubscriberBase> > elevation_subscribers_;
+
+  ///< @brief dynamic reconfigure server
+  std::unique_ptr<dynamic_reconfigure::Server<elevation_layer::ElevationPluginConfig> > dsrv_;
+
+  ///< @brief combination method to use to update the cost of a portion of map
+  CombinationMethod combination_method_;
+
+  ///< @brief polygon describing the form of the footprint of the robot
+  std::vector<geometry_msgs::Point> transformed_footprint_;
+
+  ///< @brief whether the local costmap should move together with the robot
+  bool rolling_window_;
+
+  ///< @brief whether to clean the obstacles inside the robot footprint
+  bool footprint_clearing_enabled_;
+
+  ///< @brief whether the elevation_map msg was received
+  std::atomic_bool elevation_map_received_;
+
+  ///< @brief after this time [seconds] without receiving any elevation_map, the robot will have to stop
+  double max_allowed_blind_time_;
+
+ private:
+
+  /**
+   * dynamic reconfiguration of the parameters
+   * @param config
+   * @param level
+   */
+  void reconfigureCB(elevation_layer::ElevationPluginConfig& config, uint32_t level);
+
+  ///< @brief the elevation_map from which to take the information abut the environment (filtered or not)
+  grid_map::GridMap elevation_map_;
+
+  ///< @brief lock_guard mutex to make elevation_map setting thread safe
+  std::mutex elevation_map_mutex_;
+
+  ///< @brief Ros subscriber to grid_map msgs
+  ros::Subscriber elevation_subscriber_;
+
+  ///< @brief last time an elevation_map was received
+  ros::Time last_elevation_map_update_;
+
+  ///< @brief height threshold below which nothing is considered obstacle
+  double height_threshold_;
+
+  ///< @brief sharpness threshold above which an object is considered an obstacle
+  double edges_sharpness_threshold_;
+
+  ///< @brief topic_name of the elevation_map incoming msg
+  std::string elevation_topic_;
+
+  ///< @brief Filter chain used to filter the incoming elevation_map
+  filters::FilterChain<grid_map::GridMap> filterChain_;
+
+  ///< @brief Filter chain parameters name to use
+  std::string filter_chain_parameters_name_;
+
+  ///< @brief whether filters configuration parameters was found
+  bool filters_configuration_loaded_;
+
+  ///< @brief name of the layer of the incoming map to use
+  std::string elevation_layer_name_;
+
+  ///< @brief name to give to the filtered layer
+  std::string edges_layer_name_;
+};
+
+}  // namespace elevation_layer

elevation_layer/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>elevation_layer</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>