Feature/filtered elevation layer

elevation_layer/CMakeLists.txt

@@ -0,0 +1,77 @@
+# use c++11
+set(CMAKE_CXX_COMPILER_ARG1 -std=c++11)
+
+cmake_minimum_required(VERSION 2.8.3)
+project(elevation_layer)
+
+find_package(Boost REQUIRED COMPONENTS thread)
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  tf
+  costmap_2d
+  dynamic_reconfigure
+  grid_map_ros
+  filters
+)
+
+# add dynamic reconfigure configs
+generate_dynamic_reconfigure_options(
+  cfg/ElevationPlugin.cfg
+)
+
+## declare catkin package
+catkin_package(
+  INCLUDE_DIRS
+    include
+  LIBRARIES
+    elevation_layer
+  CATKIN_DEPENDS
+    roscpp
+    tf
+    costmap_2d
+    dynamic_reconfigure
+    grid_map_ros
+    filters
+)
+
+
+
+## build ##
+
+include_directories(include)
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+## declare a cpp library
+add_library(elevation_layer
+  src/elevation_layer.cpp
+)
+
+
+## cmake target dependencies of the executable/library
+
+# build config headers
+add_dependencies(elevation_layer elevation_layer_gencfg)
+
+## libraries to link a library or executable target against
+target_link_libraries(elevation_layer
+  ${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/
+  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/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.h

@@ -0,0 +1,82 @@
+//
+// Created by eugenio on 15.10.18.
+//
+
+#ifndef ELEVATION_LAYER_H
+#define ELEVATION_LAYER_H
+
+
+#include <atomic>
+#include <mutex>
+
+#include <ros/ros.h>
+#include <costmap_2d/costmap_layer.h>
+#include <costmap_2d/layered_costmap.h>
+#include <costmap_2d/observation_buffer.h>
+#include <message_filters/subscriber.h>
+#include "grid_map_ros/GridMapRosConverter.hpp"
+#include <filters/filter_chain.h>
+
+#include <nav_msgs/OccupancyGrid.h>
+#include <dynamic_reconfigure/server.h>
+#include <costmap_2d/footprint.h>
+#include <elevation_layer/ElevationPluginConfig.h>
+
+
+namespace elevation_layer
+{
+class ElevationLayer : public costmap_2d::CostmapLayer
+    {
+    public:
+        ElevationLayer();
+        virtual ~ElevationLayer();
+        virtual void onInitialize();
+        virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
+                                  double* max_x, double* max_y);
+        virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
+
+        virtual void activate();
+        virtual void deactivate();
+        virtual void reset();
+
+        void elevationMapCallback(const grid_map_msgs::GridMapConstPtr& occupancy_grid);
+
+    protected:
+        std::string global_frame_;  ///< @brief The global frame for the costmap
+        std::vector<boost::shared_ptr<message_filters::SubscriberBase> > elevation_subscribers_;  ///< @brief Used for the observation message filters
+        dynamic_reconfigure::Server<elevation_layer::ElevationPluginConfig> *dsrv_;
+        virtual void setupDynamicReconfigure(ros::NodeHandle& nh);
+
+
+        int combination_method_;
+        std::vector<geometry_msgs::Point> transformed_footprint_;
+        bool rolling_window_;
+        bool footprint_clearing_enabled_;
+        std::atomic_bool elevation_map_received_;
+        void updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
+                             double* max_x, double* max_y);
+
+    private:
+        void reconfigureCB(elevation_layer::ElevationPluginConfig &config, uint32_t level);
+        grid_map::GridMap elevation_map_;
+        std::mutex elevation_map_mutex_;
+        ros::Subscriber elevation_subscriber_;
+        double height_treshold_;
+        double edges_sharpness_treshold_;
+        std::string elevation_topic_;
+
+        //! Filter chain.
+        filters::FilterChain<grid_map::GridMap> filterChain_;
+
+        //! Filter chain parameters name.
+        std::string filter_chain_parameters_name_;
+
+        bool filters_configuration_loaded_;
+        std::string elevation_layer_name_;
+        std::string edges_layer_name_;
+};
+
+}   // namespace elevation_layer
+
+
+#endif //ELEVATION_LAYER_H

elevation_layer/package.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<package>
+  <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>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>costmap_2d</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>grid_map_ros</build_depend>
+  <build_depend>filters</build_depend>
+
+  <run_depend>roscpp</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>costmap_2d</run_depend>
+  <run_depend>dynamic_reconfigure</run_depend>
+  <run_depend>grid_map_ros</run_depend>
+  <run_depend>filters</run_depend>
+  <export>
+    <costmap_2d plugin="${prefix}/costmap_plugins.xml" />
+  </export>
+</package>