-
Notifications
You must be signed in to change notification settings - Fork 451
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Feature/filtered elevation layer #84
base: master
Are you sure you want to change the base?
Changes from 26 commits
5af50a9
b3d8ac0
55ea64e
fd906f5
2655dfd
addee5c
105f9a5
4ed82cf
d505d15
afbfd7d
0a5a674
c8abffb
12d5302
c05ebbe
68c83bb
88dec09
b2cfdea
a0387bc
f721df4
cdc2d1a
3c8ee6a
a15f192
570622c
d927d28
eb2d3fd
0e0a86a
437f3f6
9c7860b
927731a
34c43c8
cb4e2d6
3b3dc76
a5b78aa
1833bfc
463fa92
ae5817c
0c350f1
c8d01e9
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 | ||
${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_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 ${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" | ||
) |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,3 @@ | ||
# Elevation layer | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Don't add README files for each package, but please document the new software in the main README. |
||
|
||
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. |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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")) |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,12 @@ | ||
elevation_filters: | ||
# Edge detection on elevation layer with convolution filter as alternative to filter above. | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. This comment makes no sense because there's no 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. | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Delete unused expressions. |
||
compute_empty_cells: false | ||
edge_handling: mean # options: inside, crop, empty, mean | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. The comments here are instructions for the grid map filter example, not fitting here. |
||
window_size: 3 # Make sure to make this compatible with the kernel matrix. |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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> |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,188 @@ | ||
/* | ||
* elevation_layer.h | ||
* | ||
* Created on: Nov 5, 2018 | ||
* Author: Eugenio Chisari | ||
* Institute: ANYbotics | ||
*/ | ||
|
||
#pragma once | ||
|
||
#include <atomic> | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Please group and order the includes correctly, see coding guidelines. |
||
#include <mutex> | ||
#include <boost/algorithm/string/predicate.hpp> | ||
|
||
#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" | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Use correct < and " |
||
|
||
#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; | ||
|
||
/** | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Everywhere: Use correct doxygen formatting! |
||
* @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; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Missing comment. |
||
|
||
/** | ||
* 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); | ||
|
||
ros::NodeHandle nh_; | ||
|
||
///< @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 |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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> | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Are we open sourcing this plugin? There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. I dont know There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. It's already open source, if we keep it in this repo 😉 |
||
|
||
<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> |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
you can have a single include_directories and list the elements