Nav2에서 기본적으로 제공되는 plugin 말고도, 특정 application/platform을 위한 plugin을 만들어 사용할 수 있다.
plugin
plugin은 runtime library에서 동적 loading이 가능한 class들을 말한다.
pluginlib라는 C++ library에서 plugin을 load, unload할 수 있다.
보통 .cpp파일의 하단에 PLUGINLIB_EXPORT_CLASS(plugin명)와 같이 명시하여 exported class로 loading한다.
plugin에 대한 정보를 컴퓨터가 읽을 수 있는 형식으로 저장한 plugin description xml file을 둔다.
Writing a New Costmap2D Plugin
1-Write a new CostMap2D plugin
이 예제는 반복적인 costs gradient를 부여하는 costmap plugin을 만드는 것이다.
nav2_costmap_2d::Layer class를 기반으로 만든다.
이 basic class는 plugin의 costmap layer에서 사용하기 위한 여러 virtual method API를 제공하는데,
해당 method는 onInitialize(), updateBounds(),updateCosts(),matchSize(),onFootprintChanged(),reset() 이 있다.
https://github.com/ros-planning/navigation2_tutorials.git
gradient_layer.hpp
#ifndef GRADIENT_LAYER_HPP_
#define GRADIENT_LAYER_HPP_
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/layer.hpp"
#include "nav2_costmap_2d/layered_costmap.hpp"
namespace nav2_gradient_costmap_plugin
{
class GradientLayer : public nav2_costmap_2d::Layer
{
public:
GradientLayer();
/*LayeredCostmap: plugin에서 costmap layer를 작동시키기 위한 virtual methods API*/
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(
nav2_costmap_2d::Costmap2D & master_grid,
int min_i, int min_j, int max_i, int max_j);
virtual void reset()
{
return;
}
virtual void onFootprintChanged();
virtual bool isClearable() {return false;}
private:
double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
// Indicates that the entire gradient should be recalculated next time.
bool need_recalculation_;
// Size of gradient in cells
int GRADIENT_SIZE = 20;
// Step of increasing cost per one cell in gradient
int GRADIENT_FACTOR = 10;
};
} // namespace nav2_gradient_costmap_plugin
#endif // GRADIENT_LAYER_HPP_gradient_layer.cpp
...
// This method is called at the end of plugin initialization.
// It contains ROS parameter(s) declaration and initialization of need_recalculation_ variable.
void
GradientLayer::onInitialize()
{
auto node = node_.lock();
declareParameter("enabled", rclcpp::ParameterValue(true));
node->get_parameter(name_ + "." + "enabled", enabled_);
need_recalculation_ = false; //recalculation bound
current_ = true;
}
// The method is called to ask the plugin: which area of costmap it needs to update.
// Inside this method window bounds are re-calculated if need_recalculation_ is true
// updated independently on its value.
void
GradientLayer::updateBounds(
double /*robot_x*/, double /*robot_y*/, double /*robot_yaw*/, double * min_x,
double * min_y, double * max_x, double * max_y)
{
if (need_recalculation_) { //true일 경우 recalculate
last_min_x_ = *min_x;
last_min_y_ = *min_y;
last_max_x_ = *max_x;
last_max_y_ = *max_y;
//<double>::max(): does not work with Costmap2D::worldToMapEnforceBounds()
*min_x = -std::numeric_limits<float>::max();
*min_y = -std::numeric_limits<float>::max();
*max_x = std::numeric_limits<float>::max();
*max_y = std::numeric_limits<float>::max();
need_recalculation_ = false;
} else {
double tmp_min_x = last_min_x_;
double tmp_min_y = last_min_y_;
double tmp_max_x = last_max_x_;
double tmp_max_y = last_max_y_;
last_min_x_ = *min_x;
last_min_y_ = *min_y;
last_max_x_ = *max_x;
last_max_y_ = *max_y;
*min_x = std::min(tmp_min_x, *min_x);
*min_y = std::min(tmp_min_y, *min_y);
*max_x = std::max(tmp_max_x, *max_x);
*max_y = std::max(tmp_max_y, *max_y);
}
}
// The method is called when footprint was changed.
// Here it just resets need_recalculation_ variable.
void
GradientLayer::onFootprintChanged()
{
need_recalculation_ = true;
RCLCPP_DEBUG(rclcpp::get_logger(
"nav2_costmap_2d"), "GradientLayer::onFootprintChanged(): num footprint points: %lu",
layered_costmap_->getFootprint().size());
}
// The method is called when costmap recalculation is required.
// It updates the costmap within its window bounds.
// Inside this method the costmap gradient is generated and is writing directly to the resulting costmap master_grid without any merging with previous layers.
void
GradientLayer::updateCosts(
nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j,
int max_i,
int max_j)
{
if (!enabled_) {
return;
}
// master_array - is a direct pointer to the resulting master_grid.
// master_grid - is a resulting costmap combined from all layers.
// By using this pointer all layers will be overwritten!
// To work with costmap layer and merge it with other costmap layers,
// please use costmap_ pointer instead (this is pointer to current
// costmap layer grid) and then call one of updates methods:
// - updateWithAddition()
// - updateWithMax()
// - updateWithOverwrite()
// - updateWithTrueOverwrite()
// In this case using master_array pointer is equal to modifying local costmap_
// pointer and then calling updateWithTrueOverwrite():
unsigned char * master_array = master_grid.getCharMap();
unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
// {min_i, min_j} - {max_i, max_j} - are update-window coordinates.
// These variables are used to update the costmap only within this window
// avoiding the updates of whole area.
//
// Fixing window coordinates with map size if necessary.
min_i = std::max(0, min_i);
min_j = std::max(0, min_j);
max_i = std::min(static_cast<int>(size_x), max_i);
max_j = std::min(static_cast<int>(size_y), max_j);
// Simply computing one-by-one cost per each cell
int gradient_index;
for (int j = min_j; j < max_j; j++) {
// Reset gradient_index each time when reaching the end of re-calculated window
// by OY axis.
gradient_index = 0;
for (int i = min_i; i < max_i; i++) {
int index = master_grid.getIndex(i, j);
// setting the gradient cost
unsigned char cost = (LETHAL_OBSTACLE - gradient_index*GRADIENT_FACTOR)%255;
if (gradient_index <= GRADIENT_SIZE) {
gradient_index++;
} else {
gradient_index = 0;
}
master_array[index] = cost;
}
...2-Export and make GradientLayer plugin
PLUGINLIB_EXPORT_CLASS macro를 통해 plugin class 등록
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(nav2_gradient_costmap_plugin::GradientLayer, nav2_costmap_2d::Layer)plugin description file(.xml)에 plugin 정보 저장
<library path="nav2_gradient_costmap_plugin_core">
<class name="nav2_gradient_costmap_plugin/GradientLayer" type="nav2_gradient_costmap_plugin::GradientLayer" base_class_type="nav2_costmap_2d::Layer">
<description>This is an example plugin which puts repeating costs gradients to costmap</description>
</class>
</library>plugin package를 src directory에 넣고 plugin package를 build
$ colcon build --packages-select nav2_gradient_costmap_plugin --symlink-install3-Enable the plugin in Costmap2D
Costmap2D에 새 plugin에 대해 알리는 작업이 필요하다.
nav2_params.yaml에 plugin_names과 plugin_types을 추가한다.
--- a/nav2_bringup/bringup/params/nav2_params.yaml
+++ b/nav2_bringup/bringup/params/nav2_params.yaml
@@ -124,8 +124,8 @@ local_costmap:
width: 3
height: 3
resolution: 0.05
- plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
+ plugins: ["obstacle_layer", "voxel_layer", "gradient_layer"]
robot_radius: 0.22
inflation_layer:
cost_scaling_factor: 3.0
@@ -171,8 +171,8 @@ global_costmap:
robot_base_frame: base_link
global_frame: map
use_sim_time: True
- plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"]
+ plugins: ["static_layer", "obstacle_layer", "voxel_layer", "gradient_layer"]
robot_radius: 0.22
resolution: 0.05
obstacle_layer:4-Run GradientLayer Plugin
$ ros2 launch nav2_bringup tb3_simulation_launch.pyWriting a New Planner Plugin
straight line planner를 만드는 예제.
nav2_core::GlobalPlanner class를 기반으로 만든다
Writing a New Controller Plugin
pure pursuit controller를 이용한 controller를 만드는 예제.
nav2_core::controller class를 기반으로 만든다
Writing a New Behavior Tree Plugin
다른 서버에서 action을 수행하는 간단한 BT plugin node를 만드는 예제.
nav2_behavior_tree::BtActionNode class를 기반으로 만든다.
Writing a New Behavior Plugin
간단한 sms 전송 behavior를 만드는 예제.
nav2_core::Brhavior plugin class를 기반으로 만든다.
할수이따 ~