Exponenta Banner

Преобразуйте лазерные данные сканирования от сети ROS

Открыть скрипт

Преобразуйте лазерные данные сканирования с помощью дерева преобразования ROS. При работе с лазерными данными сканирования датчик не может быть смонтирован в центре робота. Много алгоритмов делают это предположение, так, чтобы вы могли должны быть преобразовать свои данные, таким образом, это относительно центра робота. Этот пример использует дерево преобразования ROS, чтобы получить преобразования между различными координатными кадрами. Чтобы преобразовать данные о датчике, вы должны быть соединены с сетью ROS и иметь преобразования в наличии.

Setup и подключение к сети ROS. Задайте IP-адрес устройства ROS.

rosinit('192.168.203.129')

Initializing global node /matlab_global_node_43056 with NodeURI http://192.168.203.1:57424/

Создайте дерево преобразования ROS использование rostf. Получите преобразование между '/camera_link' и кадрами координаты '/base_link'. Эти координатные имена кадра зависят от вашей настройки робота.

tftree = rostf;
pause(1);
tf = getTransform(tftree,'/camera_link','/base_link',rostime('now'));

Извлеките матрицы вращения и перевода от преобразования.

quat = [tf.Transform.Rotation.W,...
        tf.Transform.Rotation.X,...
        tf.Transform.Rotation.Y,...
        tf.Transform.Rotation.Z];
rotm = quat2rotm(quat);
trvec = [tf.Transform.Translation.X,...
         tf.Transform.Translation.Y ...
         tf.Transform.Translation.Z];

Создайте гомогенное преобразование путем объединения матриц перевода и вращения.

tform = trvec2tform(trvec);
tform(1:3,1:3) = rotm(1:3,1:3);

Настройте подписчика, чтобы получить лазерные данные сканирования. Получите лазерные данные сканирования как Декартовы точки. Заполните точки с нулями для оси z и преобразуйте их в однородные координаты.

scansub = rossubscriber('/scan');
scan = receive(scansub)
cartScanData = scan.readCartesian;
cartScanData(:,3) = 0;
homScanData = cart2hom(cartScanData);

scan = 

  ROS LaserScan message with properties:

       MessageType: 'sensor_msgs/LaserScan'
            Header: [1×1 Header]
          AngleMin: -0.5216
          AngleMax: 0.5243
    AngleIncrement: 0.0016
     TimeIncrement: 0
          ScanTime: 0.0330
          RangeMin: 0.4500
          RangeMax: 10
            Ranges: [640×1 single]
       Intensities: [0×1 single]

  Use showdetails to show the contents of the message

Применяйте гомогенное преобразование и преобразуйте данные сканирования назад в Декартовы точки.

trPts = tform*homScanData';
cartScanDataTransformed = hom2cart(trPts');

Получите углы в полярных координатах и диапазоны от Декартовых Точек.

[angles,ranges] = cart2pol(cartScanDataTransformed(:,1),...
                           cartScanDataTransformed(:,2));

Сеть Shutdown ROS.

rosshutdown

Shutting down global node /matlab_global_node_43056 with NodeURI http://192.168.203.1:57424/

Смотрите также

| | |

Документация Robotics System Toolbox
Поддержка