Свой стек навигации. Лучше чем у ROS?

Это вторая статья команды setUP про наш опыт создания автономных роботов для соревнований Eurobot Open и использования для этого ROS.

Первая статья про механику и общую архитектуру роботов.

Роботы ездят по ровному полю и большинство препятствий заранее известно, однако, коварные противники могут попытаться украсть наши ресурсы (да и мы не прочь иногда полакомиться парой десяткой дополнительных баллов), при этом мы хотим проехать к желаемой точке как можно быстрее и не задеть препятствия. С внешней камеры на поле мы получаем данные о положении противника и знаем, где он сейчас находится. Однако, мало знать его положение — нужно уметь использовать эту информацию.

Сегодня мы попробуем проехать из точки А в точку Б, не проехав при этом по хвосту котика, заснувшего на поле. В частности объясним, как мы строим маршрут и управляем скоростью робота, а также расскажем как запустить всё на своем компьютере.

Попытка обойтись малой кровью

При решении такой задачи можно взять уже готовый материал, написанный профессионалами, тогда мучений и изобретения очередного «велосипеда» не будет. Мы использовали омниколесную платформу, поэтому хоть и была попытка использовать готовый local planer от ROS, но по ряду причин сочли это неперспективным. Ниже видно сколько всякой всячины нужно конструктору стандартного планера:

/**
   * @class TrajectoryPlanner
   * @brief Computes control velocities for a robot given a costmap, a plan, and the robot's position in the world. 
   */
  class TrajectoryPlanner{
    friend class TrajectoryPlannerTest; //Need this for gtest to work
    public:
      /**
       * @brief  Constructs a trajectory controller
       * @param world_model The WorldModel the trajectory controller uses to check for collisions 
       * @param costmap A reference to the Costmap the controller should use
       * @param footprint_spec A polygon representing the footprint of the robot. (Must be convex)
       * @param inscribed_radius The radius of the inscribed circle of the robot
       * @param circumscribed_radius The radius of the circumscribed circle of the robot
       * @param acc_lim_x The acceleration limit of the robot in the x direction
       * @param acc_lim_y The acceleration limit of the robot in the y direction
       * @param acc_lim_theta The acceleration limit of the robot in the theta direction
       * @param sim_time The number of seconds to "roll-out" each trajectory
       * @param sim_granularity The distance between simulation points should be small enough that the robot doesn't hit things
       * @param vx_samples The number of trajectories to sample in the x dimension
       * @param vtheta_samples The number of trajectories to sample in the theta dimension
       * @param pdist_scale A scaling factor for how close the robot should stay to the path
       * @param gdist_scale A scaling factor for how aggresively the robot should pursue a local goal
       * @param occdist_scale A scaling factor for how much the robot should prefer to stay away from obstacles
       * @param heading_lookahead How far the robot should look ahead of itself when differentiating between different rotational velocities
       * @param oscillation_reset_dist The distance the robot must travel before it can explore rotational velocities that were unsuccessful in the past
       * @param escape_reset_dist The distance the robot must travel before it can exit escape mode
       * @param escape_reset_theta The distance the robot must rotate before it can exit escape mode
       * @param holonomic_robot Set this to true if the robot being controlled can take y velocities and false otherwise
       * @param max_vel_x The maximum x velocity the controller will explore
       * @param min_vel_x The minimum x velocity the controller will explore
       * @param max_vel_th The maximum rotational velocity the controller will explore
       * @param min_vel_th The minimum rotational velocity the controller will explore
       * @param min_in_place_vel_th The absolute value of the minimum in-place rotational velocity the controller will explore
       * @param backup_vel The velocity to use while backing up
       * @param dwa Set this to true to use the Dynamic Window Approach, false to use acceleration limits
       * @param heading_scoring Set this to true to score trajectories based on the robot's heading after 1 timestep
       * @param heading_scoring_timestep How far to look ahead in time when we score heading based trajectories
       * @param meter_scoring adapt parameters to costmap resolution
       * @param simple_attractor Set this to true to allow simple attraction to a goal point instead of intelligent cost propagation
       * @param y_vels A vector of the y velocities the controller will explore
       * @param angular_sim_granularity The distance between simulation points for angular velocity should be small enough that the robot doesn't hit things
       */
      TrajectoryPlanner(WorldModel& world_model, 
          const costmap_2d::Costmap2D& costmap, 
          std::vector<geometry_msgs::Point> footprint_spec,
          double acc_lim_x = 1.0, double acc_lim_y = 1.0, double acc_lim_theta = 1.0,
          double sim_time = 1.0, double sim_granularity = 0.025, 
          int vx_samples = 20, int vtheta_samples = 20,
          double pdist_scale = 0.6, double gdist_scale = 0.8, double occdist_scale = 0.2,
          double heading_lookahead = 0.325, double oscillation_reset_dist = 0.05, 
          double escape_reset_dist = 0.10, double escape_reset_theta = M_PI_2,
          bool holonomic_robot = true,
          double max_vel_x = 0.5, double min_vel_x = 0.1, 
          double max_vel_th = 1.0, double min_vel_th = -1.0, double min_in_place_vel_th = 0.4,
          double backup_vel = -0.1,
          bool dwa = false, bool heading_scoring = false, double heading_scoring_timestep = 0.1,
          bool meter_scoring = true,
          bool simple_attractor = false,
          std::vector<double> y_vels = std::vector<double>(0),
          double stop_time_buffer = 0.2,
          double sim_period = 0.1, double angular_sim_granularity = 0.025);

Это пример инициализации параметров для регуляции скоростей и траектории в целом.

Важные параметры для полного расчета:

1. Параметр world_model.
2. Параметр Cost map: ссылка на карту, содержащую препятствия, а также их «виртуальное расширение», учитывающее потенциальную возможность столкновения.

Из плюсов стандартного стека можно выделить наличие документации и возможность найти информацию на форумах. Подробнее можете почитать на официальном сайте с документацией

Важно упоминуть что ROS пакеты были написаны для двухколесных платформ и под омни они оптимизировались засчет увелечения доступного угла поворота при движении до 360 градусов, что непременно является костылем.

Проведя анализ проекта, мы осознали, что возникнет сложность в изучении и дополнении, а так же имеется много тяжелых фишек, которые нам не нужны. Казалось бы, пусть будут, но мы использовали Odroid xu4 (процессор от которого стоял еще на Samsung s5), и результаты производительности были удручающими, а места под что-то более мощное (а raspberry 4 и процессор jetson nano нервно курят в сторонке по сравнению с ним) нет. Пришлось отказаться от стандартного стека и попытаться создать самим global planner, local planner и trajectory regulator

Global planer, local planner, trajectory regulator и все все все

Global и local планеры нужны для прокладывания маршрута до точки назначения. Зачем нужно разделение? Почему нельзя просто взять A* и катать по нему? Как правило, global planner при построении маршрута может использовать всю карту в своей работе, поэтому алгоритм должен быть максимально быстрым, возможно даже с некоторыми упрощениями. Чтобы сгладить эти упрощения и используют local planner, который на основе результата работы global planner (или просто некоторую, ограниченную, область вокруг робота) и пытается учесть все нюансы.

После того как мы построили маршрут, мы знаем, где должен проехать робот, но как ему об этом сказать? Для этого существует траекторный регулятор(trajectory regulator). Он вычисляет, с какой скоростью и в каком направлении в данный момент должен двигаться робот, чтобы не отклоняться от траектории. Во многом именно этот пакет ответственен за то, насколько быстро и красиво будет ехать робот.

Помимо этих трёх сущностей, существует четвертая — сервер карты, который позволяет удобно обрабатывать состояние мира. Он задает то, как мы описываем карту, какие у нас возможности при работе с картой и, во многом, определяет скорость работы планеров.

Прежде чем приступать к описанию стека навигации, неплохо бы обрисовать причины по которым был выбран cost_map в качестве сервера карты. Вообще, мы опробовали разные варианты обработчика карт: Occupancy_grid, Grid_map, Cost_map, но остановились на последнем.

Причины:

1. Удобно взаимодействовать с картой.
2. Есть нужные нам итераторы различных форм (круговые, линейные, прямоугольные и т.п.).
3. Можно хранить нескольких слоев карты с разными параметрами.
4. Хороший менеджмент памяти.
5. И самое важное — скорость работы. Grid map работает с типом double и из-за этого в разы медленнее чем те серверы карт, которые используют int8 для работы.

Несмотря на то, что Occupancy grid так же работает с int8, он не может похвастаться таким же удобством использования, поэтому от него пришлось отказаться.

От карты нам нужно знать где свободные, опасные и непреодолимые зоны. У каждого объекта, который есть на поле, мы можем настроить поле инфляции — значение, которое в зависимости от расстояния до объекта характеризует проходимость клетки. Инфляция — это тот самый кошкин хвост, его легко не заметить, но потом будешь об этом очень долго жалеть. Мы наносим на карту роботов противника и добавляем опасную зону, которую учитывает только local planner. Global planner же игнорирует все точки, если они не являются препятствием.

Global planner

Первое, что написали в навигации — это global planner. Он основан на алгоритме theta*. Если говорить кратко, то это модифицированный A*, где упор идет на поиск родительской node, к которой можно доехать напрямую, т.е. на пути к ней нет препятствий. Это позволяет нам строить удобные и сглаженные пути, которые используются в local planner.


Сравнение A* и theta*

Для global planner у нас есть файл с параметрами (params/path_planner.yaml), описывающий топики карты и топики с местоположением всех роботов (для всех четырех роботов на поле, где “null” — это топик с данными о текущем роботе).

# small robot debug param list
robot_id: "small"

###########################
# cost_map_server params ##
###########################
cost_map_server/big_robot_size: 0.45
cost_map_server/little_robot_size: 0.45
cost_map_server/cube_size: 0.11
cost_map_server/inscribed_radius: 0.3
cost_map_server/inflation_radius: 0.3
cost_map_server/inflation_exponential_rate: 0.6
cost_map_server/big_robot1: "/aruco/big_robot"
cost_map_server/big_robot2: "/aruco/enemy_robot1"
cost_map_server/small_robot1: "null" 
cost_map_server/small_robot2: "/aruco/enemy_robot2"
cost_map_server/collision: "collision"
cost_map_server/image_resource_name: "$(find cost_map_server)/param/image_resource.yaml"
cost_map_server/min_diff_points: 0.01

###########################
### path_planner params ###
###########################
global_planner/path_layer: "inflation_layer"
global_planner/path_force_layer: "obstacle_inflation_layer"
global_planner/frame_id: "map"
global_planner/current_position: "real_corr"
global_planner/path_filter_epsilon: 0

Так же указывается:

1. один из алгоритмов, который можно выбрать для построения маршрута,
2. названия слоев, на которых мы будем строить сам маршрут,
3. топик о нашем положении, где выдаются фильтрованные данные (у нас это комплексирование данных о местоположении с камеры и одометрии).

Сам алгоритм поиска пути — Theta Star — выделен в отдельный файл (src/global_planner.cpp) для удобства добавления новых алгоритмов:

float cost_so_far[300][200];
PriorityPoint neighbors[8];
PriorityPoint current;
int come_from[300][200][2];

double makePath_ThetaStar(std::vector<cost_map::Index> &path, PriorityPoint start, PriorityPoint goal, std::string layer, cost_map::CostMap &cost_map, double grid_cost, bool only_cost)
{
  std::priority_queue<PriorityPoint, std::vector<PriorityPoint>, myCompare> openSet;
  size_t max_x = cost_map.getSize()[0];
  size_t max_y = cost_map.getSize()[1];
  std::fill_n(*cost_so_far, max_x * max_y, std::numeric_limits<float>::max());
  cost_so_far[start.x][start.y] = 0;
  come_from[start.x][start.y][0] = start.x;
  come_from[start.x][start.y][1] = start.y;
  openSet.push(start);
  grid_cost=5;
  while (!openSet.empty())
  {
    current = openSet.top();
    openSet.pop();
    if (current == goal)
    {
      break;
    }
    current.GetNeighbors(neighbors);
    float current_cost = cost_so_far[current.x][current.y];
    int parent_carent[2] ={come_from[current.x][current.y][0], come_from[current.x][current.y][1]};
    for (int i = 0; i < 8; i++)
    {
      if (!neighbors[i].OnMap(max_x, max_y))
      {
        continue;
      }
      bool onLine = lineOfSight(parent_carent[0], parent_carent[1], neighbors[i].x, neighbors[i].y, cost_map, layer, grid_cost + 1);
      if (onLine)
      {
        float new_cost = cost_so_far[parent_carent[0]][parent_carent[1]] + HeuristicEvclid(parent_carent, neighbors[i], grid_cost*10);
        if (new_cost < cost_so_far[neighbors[i].x][neighbors[i].y])
        {
          cost_so_far[neighbors[i].x][neighbors[i].y] = new_cost;
          neighbors[i].priority = HeuristicEvclid(neighbors[i], goal, grid_cost*10) + new_cost;
          openSet.push(neighbors[i]);
          come_from[neighbors[i].x][neighbors[i].y][0] = parent_carent[0];
          come_from[neighbors[i].x][neighbors[i].y][1] = parent_carent[1];
        }
      }
      else
      {
        float neighbor_price = cost_map.at(layer, cost_map::Index({neighbors[i].x, neighbors[i].y})) + neighbors[i].priority;
        float new_cost = current_cost + neighbor_price;
        if (new_cost < cost_so_far[neighbors[i].x][neighbors[i].y])
        {
          cost_so_far[neighbors[i].x][neighbors[i].y] = new_cost;
          neighbors[i].priority =HeuristicEvclid(neighbors[i], goal, grid_cost*10) + new_cost;
          openSet.push(neighbors[i]);
          come_from[neighbors[i].x][neighbors[i].y][0] = current.x;
          come_from[neighbors[i].x][neighbors[i].y][1] = current.y;
        }
      }
    }
  }

  if (only_cost)
  {
    return cost_so_far[current.x][current.y];
  }
  path.clear();
  PriorityPoint temp_point;
  
  while (current != start)
  {
    path.push_back({current.x, current.y});
    temp_point.x = come_from[current.x][current.y][0];
    temp_point.y = come_from[current.x][current.y][1];
    current = temp_point;
  }
  path.push_back({current.x, current.y});
  return 0;
}

В отдельный файл так же выделен алгоритм удаления лишних точек пути RamerDouglasPeucker.
Он удаляет точки из пути, если она находится дальше заданного расстояния от линии, которая соединяет две соседние точки.

Local planner

У нас он работает на основе градиентного спуска в потенциальном поле. В качестве входной информации — путь из global planner. Однако, это не всё, на что он способен. В local_planner существуют внутренние сервисы для выбора режима построения пути. Всего есть два режима работы: режим смещения точек по градиенту, использующий многократный проход по карте, а так же режим смещения, при котором мы сразу вычисляем приращение по двум координатам и передвигаем точку на край безопасной зоны. Если точка попала в несколько таких зон, то смещаем на места их пересечения, ибо там безопаснее всего.

Режим работы таков: если препятствия нет на пути с прошлой итерации, то мы разбиваем путь через каждые 2 см, а после смещаем по градиенту, в ином случае мы используем второй режим работы local planner.

Второй вариант работы довольно легковесен и, так же как и global planner, не сильно нагружает процессор. Мы использовали несколько версий данного алгоритма и разные манипуляции с картой. Например, пробовали написать граф, в котором вершины находятся через каждые 10 см и смещаются максимум на 4 см, после чего на полученном графе использовали алгоритм Дейкстры для поиска наименьшего расстояния. Конечной точкой в таком случае используется ближайшая смещенная точка. Но такой алгоритм больше подходил для global planner и мы решили отказаться от данной реализации.

Также пробовали использовать построение пути с нуля, используя метод градиентного спуска. Этот метод был первым, который мы решили написать. Он оказался не эффективным по памяти (занимал больше 400 мб чистой оперативной памяти, так как использовал карту cost map при каждом проходе) и медленным. Регулирование частоты отключили ввиду плохой оптимизации и скорость работы оказалась меньше 30 раз в секунду, что нас не устраивало.

В итоге мы приняли решение использовать градиентный спуск в потенциальном поле на основе пути global planer'а. Получился легковесный и относительно простой алгоритм, который полностью устраивает нас по качеству пути, времени работы и количеству потребляемой оперативной памяти в районе 100-150 мб, что в несколько раз меньше, чем было на первых итерациях разработки.


Пример смещения пути local planer'ом

В отличии от global_planner, параметров в local_planner крайне мало, что обусловливается его простотой, потому что все самые важные задачи лежат на global_planner:

grid_map_server/big_robot1 : 0.4
grid_map_server/big_robot2 : 0.4
grid_map_server/small_robot1 : 0.4
grid_map_server/small_robot2 : 0.4
local_planner/radius : 0.15
global_planner/frame_id : "map"

В данном случае мы настраиваем:

• Радиусы безопасных зон для каждого из роботов.
• Максимальное смещение пути local planer'ом.
• Название слоя карты, с которой работаем.

В отдельный класс были выделены все самые важные функции. В данном случае это рекурсивная разбивка пути, создание планера и виртуальные конструкторы и деструкторы.

class Planner
{
  public:
    Planner(double inflation_radius_, std::string frame_id_)
    {    
      inflation_radius = inflation_radius_;
      for (int i = 0; i < 300; i++)
      {
        for(int j = 0; j < 200; j++)
          our_map[i][j] = 3000.0;
      }
      OurPath->poses.resize(50);
      
      geometry_msgs::Pose pose;
      
      pose.position.x = 0.0;
      pose.position.y = 0.0;
    
      for( int i = 0; i < 50; i++)
      {
        OurPath->poses[i].pose = pose;
      }
      
      frame_id = frame_id_;
    }
    virtual ~Planner() {  } 
    virtual void UpdatePath() = 0;

    ros::Publisher move_pub;  
    ros::Publisher BigEnemyPub;
    ros::Publisher LittleEnemyPub;
    ros::Publisher local_path_publisher;
    
  protected:

    // virtual double Calculate_Path_Len();

    nav_msgs::Path* recursive_path(nav_msgs::Path *path, std::pair<double, double> start, std::pair<double, double> end,
              double epsilon, int &index)
    {
      if (CalcDistance(start, end) < (epsilon) || path->poses.size() > 200)
        return path;

      double start_x = (start.first + end.first) / 2.0;
      double start_y = (start.second + end.second) / 2.0;

      index = find_out(path, start);

      geometry_msgs::PoseStamped pose;
      pose.pose.position.x = start_x;
      pose.pose.position.y = start_y;
      path->poses.insert(path->poses.begin() + index, pose);

      recursive_path(path, start, std::pair<double, double>{start_x, start_y}, epsilon, index);
      recursive_path(path, std::pair<double, double>{start_x, start_y}, end, epsilon, index);
    }
    int find_out(nav_msgs::Path *path, pair_double point)
    {
      int index = 0;
      for (int i = 0; i < path->poses.size(); i++)
        if (path->poses[i].pose.position.x == point.first &&
          path->poses[i].pose.position.y == point.second)
          return ++i;
      return index;
    }
    void add_in_path(nav_msgs::Path* Path, geometry_msgs::PoseStamped pose, int& Index)
    {
      Path->poses[Index % max_size] = pose;
      Index++;
    }


    double inflation_radius;
    double our_map[300][200];

    int max_size = 49;
    
    std::string frame_id;

    nav_msgs::Path *path = new nav_msgs::Path;
    nav_msgs::Path *OurPath = new nav_msgs::Path; 
};

От него наследовался класс LocalPlanning, где и находится всё ядро планера, то есть сдвиг точек на край безопасной зоны и принятие решения, что конкретно делать с путем.

Все остальные функции выделены в отдельный файл fichi.hpp, а градиентный спуск в potential_field.hpp. Ниже снапшоте из этого файла, где представлены функции по созданию потенциального поля на карте cost_map:

double CalcAttractivePotential(grid_map::Index index, double goalx, double goaly)
{
    return sqrt(pow(index[0] - goalx, 2) + pow(index[1] - goaly, 2));
}


void CalcPotential(double startx, double starty, double goalx, double goaly, cost_map::CostMap &cost_map, double (&our_map)[300][200])
{
    // Use some magic for normalisation of Field
    double max_distance = (CalcAttractivePotential(grid_map::Index(startx, starty), goalx, goaly) + 15);
    if (max_distance == 0.0)
    {
        max_distance = 0.01;
    }
    for (cost_map::CircleIterator iterator(cost_map, grid_map::Position(startx / 100.0, starty / 100.0), max_distance / 100.);
            !iterator.isPastEnd(); ++iterator)
    {
        try
        {
            grid_map::Index index(*iterator);
            double uf;
            uf = cost_map.at("obstacle_inflation_layer", *iterator);

            // if we on free podouble - field is more less then if it not free podouble
            if ( uf >= 10)
            {
                uf = 3000.0; // CP - is const variable
            }
            else
                uf += CalcAttractivePotential(index, goalx, goaly)/max_distance * 256;

            our_map[299-index(0)][199-index(1)] = uf;
        }
        catch(std::exception& e)
        {
            ROS_INFO("Exception! %s", e.what() );
        }
    }
}

Trajectory regulator

Последнее, но не менее важное — это trajectory regulator. Он отвечает за преобразование пути local planner в траекторию и выдает скорости для текущего шага.

Первая его версия, которую мы использовали на финале Eurobot 2018, это смесь пид регулятора разгона и торможения, где идет нормирование вектора к следующей точке в пути, относительно расстояния до конечной точки.

Пид регулятор, если кратко, это сумма трех состояний системы, помогающих исправить системные и рандомные ошибки, которые иногда случаются.

Эти функции подбирались эмпирическим путем и зависят от расстояния до конечной точки в пути (может быть квадратичной, кубической, обратной, но мы остановились тогда на квадратичной). Это работало, но единственное, что нас не устраивало, так это то, что робот не может затормозить вовремя при скоростях выше 0.7 метров в секунду. Поэтому, когда появилось время, мы решили перестроить весь алгоритм.

Первой итерацией на пути к новому траекторнику была замена вектора, к которому мы едем. Теперь это была сумма векторов к трем последующим с разными коэффициентами. Второй итерацией было написание Minimum Jerk. Если кратко, это построение полинома 5-ой степени, где координаты x и y зависят от времени доезда до каждой из точек.


На рисунке представлен график зависимости одной из координат от времени, а также скорость по данной координате

Такой вид траекторного регулятора устраивал нас больше, так как требовал меньше манипуляций с подбором разных коэффициентов, потому что все коэффициенты — это значения в полиноме, которые рассчитывались исходя из времени для доезда, текущей скорости и ускорения, выходной скорости и ускорения.

Результатом переписывания траекторника стало то, что удалось в два раза поднять среднюю скорость движения робота.

Как и в двух предыдущих случаях, все основные функции выделены в отдельный файл для удобства взаимодействия. В этот раз класс PlannerTrajectory отвечает за построение траектории на основе MinimumJerk

typedef struct State {
    double velocity;
    double acceleration;

    State(double v_, double a_) : velocity(v_), acceleration(a_) {}

    State() {
        velocity = 0;
        acceleration = 0;
    }
};

class PlannerTrajectory {
private:
    nav_msgs::Path global_path;
    cost_map::CostMap *costmap_ptr;
    geometry_msgs::PoseStamped Goal_pred;

    std::vector<double> trajectory_x;
    std::vector<double> trajectory_y;

    double average_velocity = 80; // Defualt or get from param //
    double max_velocity = 100;
    double coef_x[6];
    double coef_y[6];
    int frequency = 30; // Defualt or get from param //
    int index;

public:

    PlannerTrajectory(cost_map::CostMap *costmap_, const int &frequency_, const double &max_velocity_,
                      const double &average_velocity_) {
        average_velocity = average_velocity;;
        max_velocity = max_velocity_;
        costmap_ptr = costmap_;
        frequency = frequency_;
        Goal_pred.pose.position.x = 0.0;
        Goal_pred.pose.position.y = 0.0;
    }

На фото представлены все объявленные переменные, которые мы используем

В другой же файл ( include/trajectory_regulator.h) выделено все остальное: прием точек из топиков, решение ехать ли в следующую точку (в случае, если она находится в препятствии, то не едем) и многое другое.

Переход на ROS Melodic

Вплоть до прошлого года мы использовали lte релиз ROS — ROS Kinetic. Он нас в целом устраивал, но уже в следующем году заканчивается его поддержка, да и многие нужные нам пакеты стали выходить исключительно под ROS Melodic. И тут оказалось, что используемого нами costmap_server нет под Melodic.

Появилась проблема с обработкой данных с карт.

Мы выбрали Grid map, так как стек похож, но начало карты находится в другом месте, а значения самой карты варьируются от 0 до 1. Это стало большой проблемой во всем стеке навигации. Если раньше global planner запускался 50 раз в секунду (стояли ограничения по частоте, и поэтому не использовался процессор слишком сильно, даже в половину одного потока), то теперь он прокладывал путь раз в две секунды и считал плохо: полностью грузил одно ядро. За 2 секунды робот мог пересечь всю карту. Это нас не устраивало, а попытки распараллелить данный процесс закончились неудачей, так как тогда не оставалось производительности для других проектов (учитывая затраты на распараллеливание).

Решили снова поменять стек, отказаться от grid map в пользу occupancy grid. Появилась новая проблема — отсутствие возможности хранить одновременно несколько версий карт (например, полную, со всеми препятствиями, и статическую, карту только с динамическими препятствиями). Пришлось бы менять половину кода, что было не особо надежно. Поэтому мы решили поискать альтернативные варианты решения данной проблемы.

Cost Map сервер

После долгих поисков, мы нашли fork карты costmap_serverr: https://github.com/lelongg/cost_map.git — крайне полезный для нашей системы fork.



И теперь вместо обслуживания только grid map, на сдачу мы успеваем считать вероятное местоположение противника основываясь на предсказании фильтра Калмана.



Одна из наиболее важных вещей для сервера карт — это файл карты, использующийся для первичного создания всех слоев, которые в последствии только обновляются. Он представляет собой бинарную png картинку, где черное — это препятствие, а белое – свободная зона.



Для конфигурации cost_map_server так же есть файл настроек. В нем присутствуют топики с точками противников, зона инфляции и размер квадрата, который так же может быть использован для нанесения опасной зоны противника на карту.

###########################
# cost_map_server params ##
###########################
cost_map_server/big_robot_size: 0.4
cost_map_server/little_robot_size: 0.4
cost_map_server/cube_size: 0.11
cost_map_server/inscribed_radius: 0.25
cost_map_server/inflation_radius: 0.25
cost_map_server/inflation_exponential_rate: 0.6
cost_map_server/big_robot1: "/aruco/robot1"
cost_map_server/big_robot2: "/aruco/robot2"
cost_map_server/small_robot1: "/aruco/robot3" 
cost_map_server/small_robot2: "/aruco/robot4"
cost_map_server/collision: "collision"
cost_map_server/image_resource_name: "$(find cost_map_server)/param/image_resource.yaml"
cost_map_server/min_diff_points: 0.01

Публикация всех слоев идет только в случае, если на них кто-то подпишется:

void PublishChanges()
    {
        nav_msgs::OccupancyGrid msg;
        cost_map_msgs::CostMap cost_map_msg;
        if (obstacle_inflation_publisher.getNumSubscribers() > 0)
        {
            cost_map::toOccupancyGrid(costmap, "obstacle_layer", msg);
            obstacle_inflation_publisher.publish(msg);
        }
        if (inflation_publisher.getNumSubscribers() > 0)
        {
            cost_map::toOccupancyGrid(costmap, "inflation_layer", msg);
            inflation_publisher.publish(msg);
        }
        if (cost_map_publisher.getNumSubscribers() > 0)
        {
            cost_map::toMessage(costmap, cost_map_msg);
            cost_map_publisher.publish(cost_map_msg);
        }
    }

Запуск на своем компьютере

Для запуска всего стека необходимо:

• Поставить ROS
• roslaunch cost_map_server cost_map_server_alone.launch — для запуска карты
• roslaunch global_planner global_planner.launch — запуск global planer с параметрами
• rosparam load $(find local_planner)/param/param.yaml
• rosrun local_planner local_planning
• rosrun trajectory_regulator trajectory_regulator
• rosrun global_planner mover
• rosrun rviz
• добавляем inflation_layer
• теперь послав сообщение в топик /gp/goal мы отправляем робота в желаемую точку

По итогу запуска всех пунктов, у вас будет готова симуляция для запуска нашего стека на вашем компьютере. Необходимый форк


Изначально нам нужна была навигация, которая поможет нашему роботу красиво, быстро и аккуратно ездить на омни колесах. За время подготовки к соревнованиям ни один кот не пострадал, а робот — красавчик. В итоге у нас есть легковесный стек навигации для конкурсов, похожих на eurobot, которым мы вполне удовлетворены.

Для нас это стек лучше, чем стандартный, но…

Наш Телеграм: t.me/SetUpSber
Сам репозиторий всего нашего творчества