-
Notifications
You must be signed in to change notification settings - Fork 443
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 14 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,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) | ||
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. You never actually use boost in the cmake. |
||
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 | ||
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. ${PROJECT_NAME} |
||
CATKIN_DEPENDS | ||
roscpp | ||
tf | ||
costmap_2d | ||
dynamic_reconfigure | ||
grid_map_ros | ||
filters | ||
) | ||
|
||
|
||
|
||
## build ## | ||
|
||
include_directories(include) | ||
include_directories( | ||
${catkin_INCLUDE_DIRS} | ||
) | ||
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. you can have a single include_directories and list the elements |
||
|
||
## declare a cpp library | ||
add_library(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. FYI you can use ${PROJECT_NAME} whenever you have 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 | ||
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. ${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/ | ||
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 think this should be: |
||
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,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,82 @@ | ||
// | ||
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. Follow this packages file naming:
|
||
// Created by eugenio on 15.10.18. | ||
// | ||
|
||
#ifndef ELEVATION_LAYER_H | ||
#define ELEVATION_LAYER_H | ||
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. You can use |
||
|
||
|
||
#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, | ||
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 |
||
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 | ||
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. We normally use camelCase_ for members. 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. Since this is a plugin for costmap_2d, I am following its syntax. Is it bad? |
||
std::vector<boost::shared_ptr<message_filters::SubscriberBase> > elevation_subscribers_; ///< @brief Used for the observation message filters | ||
dynamic_reconfigure::Server<elevation_layer::ElevationPluginConfig> *dsrv_; | ||
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 a std::unique_ptr for this, you can get rid of deletion in the destructor. 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. Like this ? 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. If you use a smart pointer here, you do not need to care about memory handling (calling |
||
virtual void setupDynamicReconfigure(ros::NodeHandle& nh); | ||
|
||
|
||
int combination_method_; | ||
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. You could use an 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 don't think it is possible to get an enum type from a yaml file, is it? 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 don't think so, but you could use a string that you transform to an enum. This makes it easier for the user to tweak the parameters of your layer (0, 1 or 2 is not very descriptive) |
||
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 |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,32 @@ | ||
<?xml version="1.0"?> | ||
<package> | ||
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 would use package version 2 here, so you only have 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 package format 2 |
||
<name>elevation_layer</name> | ||
|
||
<version>0.0.2</version> | ||
|
||
<description>adds elevation mapping informations to costmap_2d</description> | ||
|
||
<!-- <author email="[email protected]">Eugenio Chisari</author> --> | ||
<maintainer email="[email protected]">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> | ||
|
||
<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> | ||
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. |
||
<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.
Put this in the build section of the cmake and use
add_definitions(-std=c++11)