Преобразуйте лазерные данные сканирования с помощью дерева преобразования 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/
apply | getTransform | robotics.VectorFieldHistogram | rostf