Настройте использование costmap_2d_node

алгоритм

​ Предыстория применения: В задаче планирования навигации мобильного робота необходимо знать информацию о расстоянии между препятствием и роботом. Исходная сгенерированная (или нарисованная) 2D-карта сетки не может напрямую получить информацию о расстоянии робота до препятствий, нам необходимо смоделировать большое количество лучей радара и получить эту информацию путем множества вычислений. Даже в статической среде такой процесс расчета требуется для каждого рабочего цикла робота. В качестве важного подключаемого модуля ROS NAVIGATION STACK, costmap_2d_node напрямую выполняет гауссово рассеяние препятствий на карте 2D-сетки, так что каждая сетка содержит информацию о расстоянии до препятствий (занятое значение).

Система: ubuntu 14.04+ros индиго

1. Принцип инфляции

  costmapspec.pngИнфляция - это процесс, посредством которого занятая сетка (препятствие) распространяется на окружающие ее сетки на расстояние. По степени воздействия сетки на робота ее можно разделить на 5 категорий:

  1. Стоимость «Смертельный исход» указывает на то, что в сетке есть препятствие, и если центр робота находится в таком растре, робот столкнулся.

  2. Стоимость «Вписанная» указывает на то, что расстояние от сетки до робота меньше вписанной окружности тела робота.Если робот находится в такой сетке, робот должен столкнуться.

  3. Стоимость «Возможно описанная» аналогична стоимости «Вписанная», но основана на радиусе описанной окружности тела робота. Когда робот находится в такой сетке, высока вероятность столкновения.

  4. Стоимость «Свободного пространства» равна нулю, и в такой сетке робот никогда не столкнется.

  5. «Неизвестная» стоимость.

Все остальные затраты находятся между стоимостью «Свободное пространство» и стоимостью «Возможно ограниченное пространство».

 

Функция расширения Гаусса:

exp(-1.0 * cost_scaling_factor * (distance_from_obstacle - inscribed_radius)) * (costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)

При этом costmap_2d :: INSCRIBED_INFLATED_OBSTACLE равно 254. ПРИМЕЧАНИЕ: Поскольку cost_scaling_factor умножается, он умножается на отрицательный знак, так что коэффициент увеличивается, уменьшая воздействие на препятствие вокруг сетки.

 

2. установка, настройка и применение costmap_2d_node

2.1 Установка

sudo apt-get install ros-indigo-costmap-2d

2.2 Конфигурация

Создайте новый файл minimal.yaml в указанной папке и заполните следующее содержимое для сохранения. (где важны cost_scaling_factor и Inflation_radius)

##################################
#        minimal.yaml            #         
##################################
plugins: []
publish_frequency: 1.0
footprint: [[-1.3, -0.9], [-1.3, 0.9], [1.3, 0.9], [1.3, -0.9]]
rolling_window: false
plugins: 
   - {name: static_layer,            type: "costmap_2d::StaticLayer"}
   - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}
#cost_scaling_factor and inflation_radius were now moved to the inflation_layer ns
inflation_layer:
  enabled:              true
  cost_scaling_factor:  0.5  # exponential rate at which the obstacle cost drops off (default: 10)
  inflation_radius:     30.0  # max. distance from an obstacle at which costs are incurred for planning paths.

static_layer:
  enabled:              true

Поместите изображение карты realmap.pgm для обработки в указанную папку.

20190102123512564.png

И создайте новый файл realmap.yaml, заполните следующий контент для сохранения.

##########################
#     realmap.pgm        #
##########################
image: realmap.pgm
resolution: 0.350000
origin: [0.00000, 0.0000, 0.0]
negate: 0
occupied_thresh: 0.98
free_thresh: 0.0
mode: "scale"

Создайте новый файл запуска costmap_test.launch в указанной папке и заполните следующее содержимое для сохранения.

<!-- -->

<launch>
 <node name="static_tf0" pkg="tf" type="static_transform_publisher" args="2 0 0 0 0 0 /map /base_link 100" />
 <node name="map_server" pkg="map_server" type="map_server" args="$(find main_launch)/params/realmap.yaml" />
 <rosparam file="$(find main_launch)/params/minimal.yaml" command="load" ns="/costmap_node/costmap" /> 
</launch>

Среди них realmap.yaml — глобальная статическая карта, опубликованная map_server. Это сообщение будет использовано costmap_2d_node в качестве информации об инициализации, и будет получена окончательная карта увеличенной стоимости по Гауссу.

 

2.3 Применение

roslaunch main_launch costmap_test.launch

rosrun costmap_2d costmap_2d_node

Получить карту после расширения Гаусса

20190102124042619.png

Категории