Построение карты октодерева ROS - краткое изложение процесса отображения с использованием octomap_server!

При построении семантической карты первоначальное использованиеoctomap_server, заменяетсяsemantic_slam: octomap_generator, но все же организуйте предыдущие учебные заметки.

1. Шаги для постепенного построения карты октодерева

Чтобы пакет построения octomap_server мог реализовать построение инкрементной карты, необходимо выполнить следующие 2 шага:

1.1 Настройка параметров запуска запуска

Эти 3 параметра необходимы для отображения:

• разрешение картыresolution: используется для инициализации объекта карты
• глобальная система координатframe_id: Система координат построенной глобальной карты
• Введите тему облака точек/cloud_in: В качестве входных данных для картографирования пакет картографирования должен накладывать облако точек кадр за кадром на глобальную систему координат для реализации картографирования.

<launch>
  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
    <!-- resolution in meters per pixel -->
    <param name="resolution" value="0.10" />

    <!-- 增量式构建地图时，需要提供输入的点云帧和静态全局帧之间的 TF 变换 -->
    <param name="frame_id" type="string" value="map" />

    <!-- 要订阅的点云主题名称 /fusion_cloud -->
    <remap from="/cloud_in" to="/fusion_cloud" />
  </node>
</launch>

Ниже приведены все настраиваемые параметры:

• frame_id (string, default: /map)
  • Static global frame in which the map will be published. A transform from sensor data to this frame needs to be available when dynamically building maps.
• resolution (float, default: 0.05)
  • Resolution in meter for the map when starting with an empty map. Otherwise the loaded file's resolution is used
• height_map (bool, default: true)
  • Whether visualization should encode height with different colors
• color/[r/g/b/a] (float)
  • Color for visualizing occupied cells when ~heigh_map=False, in range [0:1]
• sensor_model/max_range (float, default: -1 (unlimited))
  • Максимальная протяженность (в метрах), используемая для интерполяции данных облака точек при динамическом построении карты, ограничение протяженности полезным диапазоном (например, 5 м) предотвращает появление ложных ложных точек.
• sensor_model/[hit|miss] (float, default: 0.7 / 0.4)
  • Частота попаданий и промахов сенсорных моделей при динамическом построении карт
• sensor_model/[min|max] (float, default: 0.12 / 0.97)
  • Минимальная и максимальная вероятности зажима при динамическом построении карты
• latch (bool, default: True for a static map, false if no initial map is given)
  • Для лучшей производительности при построении карт (частые обновления), независимо от того, заблокирована ли тема для публикации или публикуется только один раз для каждого изменения, установите для этого параметра значение false, если установлено значение true, при каждом изменении карты будут создаваться все темы и визуализации.
• base_frame_id(string, default: base_footprint)
  • The robot's base frame in which ground plane detection is performed (if enabled)
• filter_ground(bool, default: false)
  • Должна ли плоскость земли обнаруживаться и игнорироваться из данных сканирования при динамическом построении карты, это очистит все на земле, но не вставит землю на карту в качестве препятствия. Если эта функция включена, ее можно дополнительно настроить с помощью Ground_filter.
• ground_filter/distance (float, default: 0.04)
  • Порог расстояния до точек разделения (в направлении z) на плоскость земли, ниже которого порог считается плоскостью
• ground_filter/angle (float, default: 0.15)
  • Угловой порог обнаруженной плоскости относительно горизонтальной плоскости, которая определяется как земля
• ground_filter/plane_distance (float, default: 0.07)
  • Для самолетов, которые должны быть обнаружены как самолеты, от z = 0 до порога расстояния (4-й коэффициент из уравнения плоскости PCL)
• pointcloud_[min|max]_z (float, default: -/+ infinity)
  • Минимальная и максимальная высоты точек для интерполяции в обратном вызове. Любые точки за пределами этого интервала будут отброшены перед запуском любой интерполяции или фильтрации нулевой плоскости. Вы можете использовать это в качестве основы для грубой фильтрации по высоте, но если включена функция Ground_filter, этот интервал должен включать плоскость земли.
• occupancy_[min|max]_z (float, default: -/+ infinity)
  • Минимальная и максимальная высота занятых ячеек, которые следует учитывать в окончательной карте, при отправке визуализаций и карт коллизий игнорирует все занятые воксели за пределами интервала, но не влияет на фактическое представление октокарты.
• filter_speckles(bool)
  • Отфильтровывать ли пятна

1.2 Требовать преобразования TF

С глобальной системой координат и облаком точек каждого кадра, как пакет картографирования узнает, куда вставить облако точек каждого кадра?

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

В ROS очень удобно использовать TF для представления этого преобразования.Нам нужно только обеспечить преобразование «облачный кадр -> мировой кадр» в дереве TF системы при запуске пакета сопоставления:

cloud frame -> world frame (static world frame, changeable with parameter frame_id)

Уведомление:

• cloud frame: тема входного облака точекhead.frame_id, такие как радар Robosenseframe_id = rslidar
• world frame: Это frame_id глобальной системы координат, который нужно давать вручную при запуске запуска.Например то, что я дал этоmap

если вы дадитеworld frame idУказывает входное облако точекframe_id,Напримерfusion_cloud.head.frame_id == wolrd_frame_id == rslidar, будет отображаться только октодерево текущего кадра, а карта не будет инкрементно строиться, на это стоит обратить внимание, и вы можете проверить это самостоятельно.

Тогда для построения карты пошагово необходимо также предусмотреть преобразование "rslidar -> world" в дереве ТФ системы.Данное преобразование можно получить через другие SLAM.Например дерево ТФ в моем тесте как следует:

Я нашел исходный кодOctomapServer.cppЧасть, которая ищет TF в:

	tf::StampedTransform sensorToWorldTf;
  try {
    m_tfListener.lookupTransform(m_worldFrameId, cloud->header.frame_id, cloud->header.stamp, sensorToWorldTf);
  } catch(tf::TransformException& ex){
    ROS_ERROR_STREAM( "Transform error of sensor data: " << ex.what() << ", quitting callback");
    return;
  }

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

Советы: Что делать, если нет ТФ?

Когда я впервые начал картографировать, в дереве TF, записанном моими одноклассниками, не было преобразования «мир -> rslidar», только «мир -> базовая_ссылка», поэтому для того, чтобы иметь возможность протестировать инкрементное сопоставление, потому что мой кадр облака точек frame_id равен rslidar, поэтому я вручную публикую статическое преобразование base_link -> rslidar:

rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 base_link rslidar

Таким образом, в системе происходит преобразование "rslidar -> world", но позы, которые я отправляю, все 0, поэтому на тест маппинга это не влияет. Для облегчения запуска он помещен в запуск:

<node pkg = "tf2_ros" type = "static_transform_publisher" name = "dlonng_static_test_broadcaster" args = "0 0 0 0 0 0 base_link rslidar" />

Если вы тоже столкнулись с этой проблемой, вы можете попробовать отправить статический ТФ на тестирование, подробное описание технологии статического ТФ вы найдете в предыдущей статье:ROS Robotics - Статические TF кадры

2. Метод включения ColorOctomap

Чтобы отобразить цвет RGB, я проанализировал исходный код, первым делом нужно изменить заголовочный файл, открыть аннотацию для переключения типа карты:OctomapServer.h

// switch color here - easier maintenance, only maintain OctomapServer. 
// Two targets are defined in the cmake, octomap_server_color and octomap_server. One has this defined, and the other doesn't
// 打开这个注释
#define COLOR_OCTOMAP_SERVER

#ifdef COLOR_OCTOMAP_SERVER
  typedef pcl::PointXYZRGB PCLPoint;
  typedef pcl::PointCloud<pcl::PointXYZRGB> PCLPointCloud;
  typedef octomap::ColorOcTree OcTreeT;
#else
  typedef pcl::PointXYZ PCLPoint;
  typedef pcl::PointCloud<pcl::PointXYZ> PCLPointCloud;
  typedef octomap::OcTree OcTreeT;
#endif

CMakeList.txtфайл имеетCOLOR_OCTOMAP_SERVERварианты компиляции:

target_compile_definitions(${PROJECT_NAME}_color PUBLIC COLOR_OCTOMAP_SERVER)

OctomapServer.cppЕсть параметры для colors_map:

m_useHeightMap = true;
m_useColoredMap = false;
  
m_nh_private.param("height_map", m_useHeightMap, m_useHeightMap);
m_nh_private.param("colored_map", m_useColoredMap, m_useColoredMap);

Карта по умолчанию устанавливает цвет в соответствии с высотой.Если вы хотите установить ее как карту с цветом, вы должны отключить HeightMap и включить ColoredMap:

if (m_useHeightMap && m_useColoredMap) {
    ROS_WARN_STREAM("You enabled both height map and RGB color registration. This is contradictory. Defaulting to height map.");
    m_useColoredMap = false;
  }

Второй шаг, вам нужно отключить height_map в файле запуска octomap_server и включить егоcolored_map:

<param name="height_map" value="false" />
<param name="colored_map" value="true" />

2 основные функции генерации октодереваinsertCloudCallbackиinsertScanЕсть операции над цветами:

#ifdef COLOR_OCTOMAP_SERVER
  unsigned char* colors = new unsigned char[3];
#endif

// NB: Only read and interpret color if it's an occupied node
#ifdef COLOR_OCTOMAP_SERVER 
        m_octree->averageNodeColor(it->x, it->y, it->z, /*r=*/it->r, /*g=*/it->g, /*b=*/it->b);
#endif

3. Сохраните и отобразите карту

После запуска узла octomap_server вы можете использовать сервис сохранения карт, который он предоставляет, для сохранения карт в сжатом двоичном формате:

octomap_saver mapfile.bt

Сохраните полную вероятностную карту октодерева:

octomap_saver -f mapfile.ot

Установите визуализатор octree octovis для просмотра карты:

sudo apt-get install octovis

Перезапустите терминал после установки и используйте следующую команду для отображения карты октодерева:

octovis xxx.ot[bt]

Четыре, заметки о чтении исходного кода

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

Первый шаг — подписаться на обратный вызов облака точек:

Второй шаг — вставка одного кадра облака точек для построения октодерева.Этот процесс подробно описываться здесь не будет, так как он включает в себя принцип обновления библиотеки октомап:

Поместите карту пути позади нашего колледжа с разрешением 15см:

Ниже приведен запуск моего процесса сопоставления:

<launch>
  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
    <!-- resolution in meters per pixel -->
    <param name = "resolution" value = "0.15" />

    <!-- name of the fixed frame, needs to be "/map" for SLAM -->
    <!-- 静态全局地图的 frame_id，但在增量式构建地图时，需要提供输入的点云帧和静态全局帧之间的 TF 变换 -->
    <param name = "frame_id" type = "string" value = "camera_init" />

    <!-- set min to speed up! -->
    <param name = "sensor_model/max_range" value = "15.0" />

    <!-- 机器人坐标系 base_link，滤除地面需要该 frame -->
    <!-- <param name = "base_frame_id" type = "string" value = "base_link" /> -->

    <!-- filter ground plane, distance value should be big! 项目并不需要滤除地面 -->
	<!-- <param name = "filter_ground" type = "bool" value = "true" /> -->
    <!-- <param name = "ground_filter/distance" type = "double" value = "1.0" /> -->
    <!-- 分割地面的 Z 轴阈值 value 值 -->
	<!-- <param name = "ground_filter/plane_distance" type = "double" value = "0.3" /> -->

    <!-- 直通滤波的 Z 轴范围，保留 [-1.0, 10.0] 范围内的点 -->
    <!-- <param name = "pointcloud_max_z" type = "double" value = "100.0" /> -->
    <!-- <param name = "pointcloud_min_z" type = "double" value = "-1.0" /> -->

    <!-- <param name = "filter_speckles" type = "bool" value = "true" /> -->

    <param name = "height_map" value = "false" />
    <param name = "colored_map" value = "true" />
	
    <!-- 增加了半径滤波器 -->
    <param name = "outrem_radius" type = "double" value = "1.0" />
    <param name = "outrem_neighbors" type = "int" value = "10" />

    <!-- when building map, set to false to speed up!!! -->
    <param name = "latch" value = "false" /> 

    <!-- topic from where pointcloud2 messages are subscribed -->
    <!-- 要订阅的点云主题名称 /pointcloud/output -->
    <!-- 这句话的意思是把当前节点订阅的主题名称从 cloud_in 变为 /pointcloud/output -->
    <remap from = "/cloud_in" to = "/fusion_cloud" />
 
  </node>
</launch>

Код проекта, который я сделал, находится здесь:AI - Notes: semantic_map