diff --git a/.gitignore b/.gitignore index 0a21979a..6fbd6f94 100644 --- a/.gitignore +++ b/.gitignore @@ -27,3 +27,7 @@ # Sublime *.sublime* + +# VSCode +.vscode +*/build/* diff --git a/README.md b/README.md index 32c3d13a..52365013 100644 --- a/README.md +++ b/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 diff --git a/elevation_mapping_costmap_2d_plugin/CMakeLists.txt b/elevation_mapping_costmap_2d_plugin/CMakeLists.txt new file mode 100644 index 00000000..92b9df3d --- /dev/null +++ b/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" +) diff --git a/elevation_mapping_costmap_2d_plugin/cfg/ElevationPlugin.cfg b/elevation_mapping_costmap_2d_plugin/cfg/ElevationPlugin.cfg new file mode 100755 index 00000000..e8817f93 --- /dev/null +++ b/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")) diff --git a/elevation_mapping_costmap_2d_plugin/config/elevation_filters.yaml b/elevation_mapping_costmap_2d_plugin/config/elevation_filters.yaml new file mode 100644 index 00000000..2662181e --- /dev/null +++ b/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. \ No newline at end of file diff --git a/elevation_mapping_costmap_2d_plugin/costmap_plugins.xml b/elevation_mapping_costmap_2d_plugin/costmap_plugins.xml new file mode 100644 index 00000000..95553082 --- /dev/null +++ b/elevation_mapping_costmap_2d_plugin/costmap_plugins.xml @@ -0,0 +1,7 @@ + + + + Adds elevation map info to costmap_2d. + + + diff --git a/elevation_mapping_costmap_2d_plugin/include/elevation_mapping_costmap_2d_plugin/elevation_layer.hpp b/elevation_mapping_costmap_2d_plugin/include/elevation_mapping_costmap_2d_plugin/elevation_layer.hpp new file mode 100644 index 00000000..4d5d2061 --- /dev/null +++ b/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 +#include +#include + +// Ros headers. +#include +#include +#include + +// Costmap2d headers. +#include +#include +#include +#include + +// Package related headers. +#include +#include +#include +#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 > 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 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 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 \ No newline at end of file diff --git a/elevation_mapping_costmap_2d_plugin/package.xml b/elevation_mapping_costmap_2d_plugin/package.xml new file mode 100644 index 00000000..797f28d2 --- /dev/null +++ b/elevation_mapping_costmap_2d_plugin/package.xml @@ -0,0 +1,23 @@ + + + elevation_mapping_costmap_2d_plugin + 0.0.2 + adds elevation mapping informations to costmap_2d + Eugenio Chisari + Eugenio Chisari + BSD + + catkin + + roscpp + boost + tf + costmap_2d + dynamic_reconfigure + grid_map_ros + filters + + + + + diff --git a/elevation_mapping_costmap_2d_plugin/src/elevation_layer.cpp b/elevation_mapping_costmap_2d_plugin/src/elevation_layer.cpp new file mode 100644 index 00000000..a04ff4d7 --- /dev/null +++ b/elevation_mapping_costmap_2d_plugin/src/elevation_layer.cpp @@ -0,0 +1,240 @@ +/* + * elevation_layer.cpp + * + * Created on: Nov 5, 2018 + * Author: Eugenio Chisari + * Institute: ANYbotics + */ + +#include "elevation_mapping_costmap_2d_plugin/elevation_layer.hpp" +#include + +PLUGINLIB_EXPORT_CLASS(elevation_mapping_costmap_2d_plugin::ElevationLayer, costmap_2d::Layer) + +using costmap_2d::FREE_SPACE; +using costmap_2d::LETHAL_OBSTACLE; +using costmap_2d::NO_INFORMATION; + +using costmap_2d::Observation; +using costmap_2d::ObservationBuffer; + +namespace elevation_mapping_costmap_2d_plugin { + +ElevationLayer::ElevationLayer() : filterChain_("grid_map::GridMap"), elevationMapReceived_(false), filtersConfigurationLoaded_(false) { + costmap_ = nullptr; // this is the unsigned char* member of parent class Costmap2D. +} + +void ElevationLayer::onInitialize() { + nodeHandle_ = ros::NodeHandle("~/" + name_); + rollingWindow_ = layered_costmap_->isRolling(); + + ElevationLayer::matchSize(); + current_ = true; + elevationMapReceived_ = false; + filtersConfigurationLoaded_ = false; + globalFrame_ = layered_costmap_->getGlobalFrameID(); + + // get parameters from config file + if (!nodeHandle_.param("elevation_topic", elevationTopic_, std::string(""))) { + ROS_WARN("did not find elevation_topic, using default"); + } + if (!nodeHandle_.param("height_threshold", heightThreshold_, 0.0)) { + ROS_WARN("did not find height_treshold, using default"); + } + if (!nodeHandle_.param("filter_chain_parameters_name", filterChainParametersName_, std::string(""))) { + ROS_WARN("did not find filter_chain_param_name, using default"); + } + if (!nodeHandle_.param("elevation_layer_name", elevationLayerName_, std::string(""))) { + ROS_WARN("did not find elevation_layer_name, using default"); + } + if (!nodeHandle_.param("edges_layer_name", edgesLayerName_, std::string(""))) { + ROS_WARN("did not find edges_layer_name, using default"); + } + if (!nodeHandle_.param("footprint_clearing_enabled", footprintClearingEnabled_, false)) { + ROS_WARN("did not find footprint_clearing_enabled, using default"); + } + if (!nodeHandle_.param("edges_sharpness_threshold", edgesSharpnessThreshold_, 0.0)) { + ROS_WARN("did not find edges_sharpness_treshold, using default"); + } + if (!nodeHandle_.param("max_allowed_blind_time", maxAllowedBlindTime_, 0.0)) { + ROS_WARN("did not find max_allowed_blind_time, using default"); + } + bool trackUnknownSpace = layered_costmap_->isTrackingUnknown(); + if (!nodeHandle_.param("track_unknown_space", trackUnknownSpace, false)) { + ROS_WARN("did not find trackUnknownSpace, using default"); + } + default_value_ = trackUnknownSpace ? NO_INFORMATION : FREE_SPACE; + std::string combinationMethod; + if (!nodeHandle_.param("combination_method", combinationMethod, std::string(""))) { + ROS_WARN("did not find combination_method, using default"); + } + combinationMethod_ = convertCombinationMethod(combinationMethod); + + // Subscribe to topic. + elevationSubscriber_ = nodeHandle_.subscribe(elevationTopic_, 1, &ElevationLayer::elevationMapCallback, this); + dsrv_ = nullptr; + setupDynamicReconfigure(nodeHandle_); + + // Setup filter chain. + if (!filterChain_.configure(filterChainParametersName_, nodeHandle_)) { + ROS_WARN("Could not configure the filter chain!"); + } else { + filtersConfigurationLoaded_ = true; + } +} + +void ElevationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, + double* max_y) { + std::lock_guard lock(elevationMapMutex_); + if (rollingWindow_) { + updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); + } + if (!(enabled_ && elevationMapReceived_)) { + return; + } + useExtraBounds(min_x, min_y, max_x, max_y); + + for (grid_map::GridMapIterator iterator(elevationMap_); !iterator.isPastEnd(); ++iterator) { + const grid_map::Index gridmapIndex(*iterator); + grid_map::Position vertexPositionXY; + elevationMap_.getPosition(gridmapIndex, vertexPositionXY); + double px = vertexPositionXY.x(); + double py = vertexPositionXY.y(); + + touch(px, py, min_x, min_y, max_x, max_y); + } + updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); +} + +void ElevationLayer::updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, + double* max_y) { + if (!footprintClearingEnabled_) { + return; + } + costmap_2d::transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformedFootprint_); + + for (auto& i : transformedFootprint_) { + touch(i.x, i.y, min_x, min_y, max_x, max_y); + } +} + +void ElevationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { + std::lock_guard lock(elevationMapMutex_); + if (!enabled_ || !elevationMapReceived_) { + return; + } + const bool hasEdgesLayer = elevationMap_.exists(edgesLayerName_); + if (!hasEdgesLayer) { + ROS_WARN_STREAM_THROTTLE(0.5, edgesLayerName_ << " layer not found !!"); + } + ros::Duration timeSinceElevationMapReceived = ros::Time::now() - lastElevationMapUpdate_; + if (timeSinceElevationMapReceived > ros::Duration(maxAllowedBlindTime_)) { + current_ = false; + } + const grid_map::Matrix& elevationData = elevationMap_[elevationLayerName_]; + for (grid_map::GridMapIterator iterator(elevationMap_); !iterator.isPastEnd(); ++iterator) { + const grid_map::Index gridmapIndex(*iterator); + grid_map::Position vertexPositionXY; + elevationMap_.getPosition(gridmapIndex, vertexPositionXY); + double px = vertexPositionXY.x(); + double py = vertexPositionXY.y(); + // Now we need to compute the map coordinates for the observation. + unsigned int mx, my; + if (!worldToMap(px, py, mx, my)) // If point outside of local costmap, ignore. + { + continue; + } + if (elevationData(gridmapIndex(0), gridmapIndex(1)) > heightThreshold_) // If point too high, it could be an obstacle. + { + if (hasEdgesLayer) { + const grid_map::Matrix& edgesData = elevationMap_[edgesLayerName_]; + if (edgesData(gridmapIndex(0), gridmapIndex(1)) < edgesSharpnessThreshold_) // If area not sharp, dont label as obstacle. + { + setCost(mx, my, FREE_SPACE); + continue; + } + } + setCost(mx, my, LETHAL_OBSTACLE); + } else { + setCost(mx, my, FREE_SPACE); + } + } + + if (footprintClearingEnabled_) { + setConvexPolygonCost(transformedFootprint_, costmap_2d::FREE_SPACE); + } + + switch (combinationMethod_) { + case Overwrite: + updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); + break; + case Maximum: + updateWithMax(master_grid, min_i, min_j, max_i, max_j); + break; + default: // Do Nothing. + break; + } +} + +CombinationMethod convertCombinationMethod(const std::string& str) { + if (boost::iequals(str, "Overwrite")) { // Case insensitive comparison. + return Overwrite; + } else if (boost::iequals(str, "Maximum")) { // Case insensitive comparison. + return Maximum; + } else { + ROS_WARN_THROTTLE(0.5, "Unknow combination method !"); + return Unknown; + } +} + +void ElevationLayer::elevationMapCallback(const grid_map_msgs::GridMapConstPtr& elevation) { + grid_map::GridMap incomingMap; + grid_map::GridMap filteredMap; + if (!grid_map::GridMapRosConverter::fromMessage(*elevation, incomingMap)) { + ROS_WARN_THROTTLE(0.5, "Grid Map msg Conversion failed !"); + return; + } + lastElevationMapUpdate_ = ros::Time::now(); + incomingMap.convertToDefaultStartIndex(); + if (!(globalFrame_ == incomingMap.getFrameId())) { + ROS_WARN_THROTTLE(0.5, "Incoming elevation_map frame different than expected! "); + } + // Apply filter chain. + if (filtersConfigurationLoaded_ && filterChain_.update(incomingMap, filteredMap)) { + std::lock_guard lock(elevationMapMutex_); + elevationMap_ = filteredMap; + heightThreshold_ /= 2.0; // Half the threshold since the highest sharpness is at midheigth of the obstacles. + } else { + std::lock_guard lock(elevationMapMutex_); + ROS_WARN_THROTTLE(0.5, "Could not use the filter chain!"); + elevationMap_ = incomingMap; + } + if (!elevationMapReceived_) { + elevationMapReceived_ = true; + } +} + +void ElevationLayer::setupDynamicReconfigure(ros::NodeHandle& nodeHandle_) { + dsrv_.reset(new dynamic_reconfigure::Server(nodeHandle_)); + dynamic_reconfigure::Server::CallbackType cb = + boost::bind(&ElevationLayer::reconfigureCB, this, _1, _2); + dsrv_->setCallback(cb); +} + +void ElevationLayer::reconfigureCB(elevation_mapping_costmap_2d_plugin::ElevationPluginConfig& config, uint32_t level) { + enabled_ = config.enabled; +} + +void ElevationLayer::reset() { + deactivate(); + resetMaps(); + current_ = true; + activate(); +} + +void ElevationLayer::activate() { + // if we're stopped we need to re-subscribe to topics + elevationSubscriber_ = nodeHandle_.subscribe(elevationTopic_, 1, &ElevationLayer::elevationMapCallback, this); +} +void ElevationLayer::deactivate() { elevationSubscriber_.shutdown(); } +} // namespace elevation_mapping_costmap_2d_plugin \ No newline at end of file