Работа с Kinect под ROS - часть 2 - преобразование облака точек в данные лазерного дальномера



Как я уже писал, возможность преобразования одного типа данных в другой, является очень мощным механизмом Операционной Системы для Роботов — ROS.
Данная возможность позволяет использовать разные варианты и типы сенсоров, при этом, обрабатывая их данные одним и тем же алгоритмом.

Для преобразования облака точек, получаемых от сенсора Microsoft Kinect, в ROS уже есть готовый пакет — pointcloud_to_laserscan из стека Turtlebot-а.

Чтобы установить этот стек достаточно выполнить команду:
sudo apt-get install ros-electric-turtlebot-robot


Теперь, необходимо создать свой пакет, в котором создать файл запуска
kinect_to_laser.launch
следующего содержания:

<launch>
  <!-- kinect and frame ids -->
  <include file="$(find openni_camera)/launch/openni_node.launch"/>
 
  <!-- openni_manager -->
  <node pkg="nodelet" type="nodelet" name="openni_manager" output="screen" respawn="true" args="manager"/>
 
  <!-- throttling -->
  <node pkg="nodelet" type="nodelet" name="pointcloud_throttle" args="load pointcloud_to_laserscan/CloudThrottle openni_manager">
    <param name="max_rate" value="2"></param>
    <remap from="cloud_in" to="/camera/depth/points"/>
    <remap from="cloud_out" to="cloud_throttled"/>
  </node>
 
  <!-- fake laser -->
  <node pkg="nodelet" type="nodelet" name="kinect_laser" args="load pointcloud_to_laserscan/CloudToScan openni_manager">
    <param name="output_frame_id" value="/openni_depth_frame"></param>
    <remap from="cloud" to="cloud_throttled"/>
  </node>
</launch>


Рассмотрим содержимое файла:

<include file="$(find openni_camera)/launch/openni_node.launch"/>
— запуск openni_node.launch, который начинает работать с Kinect


<node pkg="nodelet" type="nodelet" name="openni_manager" output="screen" respawn="true" args="manager"/>
— запуск узла openni_manager (типа nodelet), который является контейнером для всех остальных nodelet-ов

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


Затем идёт запуск двух nodelet-ов из Стека turtlebot-а: pointcloud_throttle и kinect_laser.

nodelet pointcloud_throttle — регулирует частоту выдачи облака точек (pcl::PointCloud<pcl::PointXYZ>) — он подписывается на тему задаваемую параметром cloud_in и в тему задаваемую параметром cloud_out передаёт принимаемое облако точек с частотой, определяемой параметром max_rate.
Т.о. pointcloud_throttle передаёт облако точек в cloud_throttled из /camera/depth/points с частотой 2 Гц.

подробности можно посмотреть в исходнике pointcloud_to_laserscan/src/cloud_throttle.cpp:
https://kforge.ros.org/turtlebot/turtlebot/file/tip/pointcloud_to_laserscan/src/cloud_throttle.cpp

nodelet kinect_laser — вырезает горизонтальный отрезок из каждого облака точек cloud_throttled, в столбцах которого выбирается ближайшее расстояние (глубина) и публикует в топик scan.
Этот nodelet поддерживает несколько параметров, которые определяют размер и расположение среза, который используется для получения данных лазерного сканирования, но используются их значения установленные по-умолчанию.

Т.о. узел pointcloud_to_laserscan (типа nodelet), считывает данные типа pcl::PointCloud<pcl::PointXYZ> и преобразует их в данные типа sensor_msgs::LaserScan, которые публикуются в топике scan.

Детальнее процедуру преобразования можно посмотреть в исходнике pointcloud_to_laserscan/src/cloud_to_scan.cpp:
https://kforge.ros.org/turtlebot/turtlebot/file/tip/pointcloud_to_laserscan/src/cloud_to_scan.cpp

nodelet — это пакет, разработанный для запуска разных алгоритмов в одном процессе, что позволяет не тратить ресурсы на копирование данных между ними.
ROS — это распределённая система, в которой каждая программа (Узел — Node) — это отдельный процесс, который может выполняться на разных машинах и обмениваться данными через Сообщения (Message) поверх TCP- или UDP-протокола (TCPROS, UDPROS).
Т.о. обмен данными между узлами сопряжён с накладными расходами сериализации и передачи данных, а если данные объёмные (как, например, изображение с камеры или облако точек от сенсора Kinect), то затраты на передачу данных между узлами становятся очень существенными.
Поэтому и были разработаны nodelet-ы — чтобы не тратить ресурсы на копирование данных между процессами, работающими на одной машине, а запускать алгоритмы в одном процессе, что позволяет им получать доступ к данным без затрат на копирование.


Для удобства, я уже создал пакет (robocraft_kinect) содержащий файл запуска kinect_to_laser.launch, в репозитории
http://code.google.com/p/robocraft-ros-pkg/

Можете скачать этот пакет к себе, например, в директорию ros_tutorials, которую вы создавали при прохождении уроков по ROS.
Или скачать все пакеты в директорию robocraft-ros-pkg командой
svn checkout https://robocraft-ros-pkg.googlecode.com/svn/trunk/ robocraft-ros-pkg

затем прописать эту директорию в свой setup.sh в переменную окружения ROS_PACKAGE_PATH

1. Откроем новый терминал и запустим roscore
2. Используем файл запуска:
roslaunch robocraft_kinect kinect_to_laser.launch


3. посмотрим, какие топики появились в системе:
rostopic list


получим следующий список:

/camera/depth/camera_info
/camera/depth/disparity
/camera/depth/image
/camera/depth/image/compressed
/camera/depth/image/compressed/parameter_descriptions
/camera/depth/image/compressed/parameter_updates
/camera/depth/image/theora
/camera/depth/image/theora/parameter_descriptions
/camera/depth/image/theora/parameter_updates
/camera/depth/image_raw
/camera/depth/image_raw/compressed
/camera/depth/image_raw/compressed/parameter_descriptions
/camera/depth/image_raw/compressed/parameter_updates
/camera/depth/image_raw/theora
/camera/depth/image_raw/theora/parameter_descriptions
/camera/depth/image_raw/theora/parameter_updates
/camera/depth/points
/camera/rgb/camera_info
/camera/rgb/image_color
/camera/rgb/image_color/compressed
/camera/rgb/image_color/compressed/parameter_descriptions
/camera/rgb/image_color/compressed/parameter_updates
/camera/rgb/image_color/theora
/camera/rgb/image_color/theora/parameter_descriptions
/camera/rgb/image_color/theora/parameter_updates
/camera/rgb/image_mono
/camera/rgb/image_mono/compressed
/camera/rgb/image_mono/compressed/parameter_descriptions
/camera/rgb/image_mono/compressed/parameter_updates
/camera/rgb/image_mono/theora
/camera/rgb/image_mono/theora/parameter_descriptions
/camera/rgb/image_mono/theora/parameter_updates
/camera/rgb/image_raw
/camera/rgb/image_raw/compressed
/camera/rgb/image_raw/compressed/parameter_descriptions
/camera/rgb/image_raw/compressed/parameter_updates
/camera/rgb/image_raw/theora
/camera/rgb/image_raw/theora/parameter_descriptions
/camera/rgb/image_raw/theora/parameter_updates
/camera/rgb/points
/cloud_throttled
/kinect_laser/parameter_descriptions
/kinect_laser/parameter_updates
/openni_manager/bond
/openni_node1/parameter_descriptions
/openni_node1/parameter_updates
/rosout
/rosout_agg
/scan
/tf


4. Полюбуемся результатом работы при помощи мощной программы визуализации — rviz
rosrun rviz rviz


Первым делом, укажем программе rviz базовую систему отсчёта.
Для этого, в области глобальных настроек:
.Global Options
щёлкаем напротив Fixed Frame и в появившемся списке доступных систем отсчёта выбираем
/openni_camera


5. Добавляем визуализатор лазерного дальномера: щёлкаем внизу на кнопке Add и из списка объектов визуализации выбираем лазерный сканер:
Laser Scan
, выбираем для визуализатора источник данных — Топик /scan
орты системы координат(Axis) и сетку(Grid) — можно добавить таким же образом


— цвет и вид отображения данных лазерного сканера регулируется в области настроек.

6. Дополнительно выведем текущее облако точек, на котором хорошо видны красные точки лазерного сканера

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


Отключим облако точек, так его визуализация несколько подтормаживает :)

Красота!

продолжение следует...

Ссылки:
http://www.ros.org/wiki/turtlebot
http://www.ros.org/wiki/pointcloud_to_laserscan
формат roslaunch .launch/XML
http://www.ros.org/wiki/rviz
2d SLAM with ROS and Kinect

По теме:
Что же такое ROS?
Юбилей Kinect-а
Работа с Kinect под ROS
  • +1
  • 12 февраля 2012, 11:27
  • noonv

Комментарии (1)

RSS свернуть / развернуть
+
0
поздравляю с успехом!
avatar

aivanov

  • 12 февраля 2012, 13:02

Только зарегистрированные и авторизованные пользователи могут оставлять комментарии.