Посты серии:
8.Управляем с телефона-ROS Control, GPS-нода
7. Локализация робота: gmapping, AMCL, реперные точки на карте помещения
6. Одометрия с энкодеров колес, карта помещения, лидар
5. Работаем в rviz и gazebo: xacro, новые сенсоры.
4. Создаем симуляцию робота, используя редакторы rviz и gazebo.
3. Ускоряемся, меняем камеру, исправляем походку
2. Софт
1. Железо
В прошлый раз мы оформили проект в виде отдельных модулей xacro, добавили виртуальные видеокамеру и imu(гироскоп).
В этом посте поработаем с одометрией с оптических энкодеров, которые установлены на валы колес, загрузим карту помещения и поездим по ней на реальном роботе-тележке.
Что такое одометрия и tf и как они обычно реализуется в ROS уже отлично описано на ресурсе, поэтому отсылаем в части теории к соответствующим статьям, например здесь.
Оттолкнувшись от теоретической базы, поработаем с практикой.
Начнем с работы на роботе-тележке, подключившись к нему по VNC.
Перейдем в папку rosbots_driver и создадим файл-ноду. Данный файл будет формировать одометрию, получая ее от оптических энкодеров, которые в свою очередь отправляют ее в arduino uno и далее в raspberry pi.
В файл поместим код:
Сохраним файл и сделаем его исполняемым:
Теперь на роботе запустим 2-е ноды — driver и diff-tf:
1-й терминал:
2-й:
В 3-м терминале проверим, что появились новые топики odom и tf:
Посмотрим командой rostopic echo odom, что публикуется в топик (и публикуется ли вообще).
Вывод будет примерно следующим:
Теперь, не закрывая запущенные ноды на роботе, запустим управляющий компьютер с графическими средами rviz и gazebo.
*Образ (виртуальная машина VMWare с Ubuntu 16.04+ROS Kinetic), который ранее предлагался для скачивания содержит все необходимое.
На управляющем компьютере (далее по тексту «Компьютер») запустим модель в rviz:
Загрузится модель робота, с которой работали в предыдущих постах:
Добавим два дисплея в rviz, нажав Add. Дисплей с odometry и дисплей с tf, поставим галочки, чтобы их визуализировать.
В окне, где изображена модель робота появится характерная графика:
*чтобы она была наглядней можно отключить дисплей Robotmodel.
Поуправляем роботом с клавиатуры Компьютера и посмотрим, как изменяется визуализация tf и одометрии.
Не закрывая rviz во 2-м терминале запустим управление с клавиатуры:
При управлении роботом в окне с визуализацией будут видны: красная стрелка (визуализация топика odom), векторные линии (топик tf).
Если красная стрелка топика odom демонстрирует направление движения робота, то векторные линии tf показывают, как располагаются отдельные элементы робота в пространстве:
Теперь, чтобы продвинуться дальше, необходимо «настроить» одометрию.
Для этого закроем редактор rviz и запустим его заново, только без визуализации модели командой:
Этого необходимо для того, чтобы из векторов топика tf остались только base_link и odom:
В rviz одна клетка равна 1 метру. Поэтому в реальности робот также должен проходить 1 метр, чтобы данные были сопоставимы.
Проедем 1 метр на роботе, управляя им с клавиатуры. В rviz робот также должен проехать 1 метр — одну клетку.
Если робот проезжает больше положенного в rviz или наоборот более короткое расстояние, чем в реальности, то необходимо править файл diff_tf.py, который ранее создавался, а именно вот этот блок:
Чтобы куда-то поехать, необходима карта. Для целей нашего робота — нужна карта помещения.
Поработаем с ней.
Для того, чтобы загрузить карту в rviz, необходимо в проекте (rosbots_description) на Компьютере (не на роботе) создать папку map и поместить в нее два файла, из которых «состоит» карта: map.pgm и map.yaml.
*На самом деле в папке может быть несколько файлов-карт, но загрузить в мастер можно только одну.
Карта в ROS представляет собой два файла, один из которых изображения в формате PGM, где каждый пиксель либо:
Второй файл .yaml — файл с настройками карты, где указаны ее размеры, заполненность пикселями разных видов (перечисленных выше), иные параметры.
Запустим на Компьютере ноду, которая опубликует карту:
В соседнем терминале запустим модель в rviz:
В rviz добавим дисплей Map.
В rviz робот получился непропорционально большим, и расположен вне карты:
Чтобы это исправить, надо запустить карту, где размер клетки будет 1 метр. Перезапустим карту с параметром 1 на окончании:
Теперь можно поездить по карте в rviz, управляя роботом с клавиатуры:
Итак, что удалось добиться:
Однако, несмотря на то, что карта отображается и робот может по ней ездить с «настроенной» одометрией, в реальности робот слеп. Он не видит препятствий и будет натыкаться на них. Второй минус — виртуальная карта помещения, загруженная в rviz позволяет ездить по себе во всех направлениях, даже в тех, где явно изображены препятствия.
Как заставить робота «видеть» препятствия в реальности и виртуально?
С виртуальной средой попроще. Здесь все строится на базе эмулятора-редактора gazebo. И в предыдущих постах об этом упоминалось.
С реальностью посложнее. Нужен элемент (датчик), который будет обозначать препятствия и сообщать об этом системе.
Один из вариантов — поставить на робота лидар.
Воспользуемся доступным бюджетным решением и поставим на робота лидар. Возможно, это решение будет дороже использования того же Kinectа, но оно, как показала практика, более эффективно в плане скорости работы, точности и простоты монтажа (менее громоздко). Кроме того, с lidarом проще начать работать, т.к. не требуются размышления, как его запитать и подключить к проекту (https://habr.com/ru/company/tod/blog/210252/).
Нам понадобится пакет ros для работы с лидаром — wiki.ros.org/rplidar.
С помощью лидара мы построим карту помещения, а также используем его в навигации.
Как установить rplidar в ROS есть масса статей, например здесь.
Воспользуемся знаниями седых старцев и установим пакеты с лидаром в систему на роботе:
На Компьютере установим пакет для работы с картой:
Запустим на роботе пакет и проверим работает ли лидар:
*первая команда дает доступ к порту usb, где подключен лидар.
Если все пошло гладко, то в консоль выдаст строки:
Здесь сразу немного настроим лидар, т.к. официальный сайт говорит, что он (лидар) может и получше работать.
Нам необходимо добиться выдачи при сканировании не 4.0K точек, котрые выдаются по умолчанию, а 8.0К. Данная опция немного улучшит качество сканирования.
Для это в пакете rplidar зададим еще один параметр — scan mode:
И после
Вторую строку, которую надо поправить здесь же:
Значение laser заменить на base_link.
*Теперь, если перезапустить ноду командой roslaunch rplidar_ros rplidar.launch, вывод будет другим:
Посмотри. что выводит лидар в rviz.
Для этого запустим на роботе:
На Компьютере:
В rviz добавим дисплей LaserScan и выберем топик scan. Далее будет видно, что в топик падают сообщения:
В окне с визуализацией робота, робот получился великаном. С его размерами мы разберемся позднее. А сейчас построим карту помещения.
Для этого создадим пакет с нодой:
Запустим.
На роботе:
1-й терминал:
2-й:
На Компьютере:
1-й терминал:
2-й:
3-й:
В дисплеи надо добавить map, а Fixed frame выбрать base_link. Далее можно наблюдать в режиме реального времени, как лидар «освещает» пространство вокруг себя:
На текущем шаге, чтобы построить карту, надо поездить по помещению, «заезжая» в разные углы, чтобы лидар обозначил их на карте.
Так рекомендуют учебники. Но наш совет — взять робота в руки и пройтись с ним, держа его перед собой. Так скорость построения карты будет больше в том плане, что вам не надо будет отвлекаться и смотреть, куда заехал робот в соседнем помещении при отсутствии визуального контакта.
Кроме того, при поворотах роботом вокруг своей оси при поездке лидар оставляет характерные артефакты черного цвета в тех местах, где на самом деле нет препятствий:
После построения карты, сохраним ее командой:
Построение идеальной карты с помощью бюджетного лидара — миф. Поэтому поможем лидару в фотошопе. Удалим артефакты черного цвета с карты, где их на самом деле нет препятствий, а стены выровняем линиями черного цвета:
Не забываем сохранить карту в формате .pgm.
Теперь повторим на Компьютере команды, которые были в начале поста, но уже с новой картой:
1-й терминал:
2-й:
Результат в rviz:
Новая карта загрузилась, как и модель робота на ней, но робот вне карты.
Что с этим делать поговорим позднее, а пока подведем итоги:
Файлы для загрузки: карта помещения.
8.Управляем с телефона-ROS Control, GPS-нода
7. Локализация робота: gmapping, AMCL, реперные точки на карте помещения
6. Одометрия с энкодеров колес, карта помещения, лидар
5. Работаем в rviz и gazebo: xacro, новые сенсоры.
4. Создаем симуляцию робота, используя редакторы rviz и gazebo.
3. Ускоряемся, меняем камеру, исправляем походку
2. Софт
1. Железо
В прошлый раз мы оформили проект в виде отдельных модулей xacro, добавили виртуальные видеокамеру и imu(гироскоп).
В этом посте поработаем с одометрией с оптических энкодеров, которые установлены на валы колес, загрузим карту помещения и поездим по ней на реальном роботе-тележке.
Одометрия и tf
Что такое одометрия и tf и как они обычно реализуется в ROS уже отлично описано на ресурсе, поэтому отсылаем в части теории к соответствующим статьям, например здесь.
Оттолкнувшись от теоретической базы, поработаем с практикой.
Начнем с работы на роботе-тележке, подключившись к нему по VNC.
Перейдем в папку rosbots_driver и создадим файл-ноду. Данный файл будет формировать одометрию, получая ее от оптических энкодеров, которые в свою очередь отправляют ее в arduino uno и далее в raspberry pi.
cd /home/pi/rosbots_catkin_ws/src/rosbots_driver/scripts/rosbots_driver touch diff-tf.py
В файл поместим код:
diff_tf.py
#!/usr/bin/env python """ diff_tf.py - follows the output of a wheel encoder and creates tf and odometry messages. some code borrowed from the arbotix diff_controller script A good reference: http://rossum.sourceforge.net/papers/DiffSteer/ Copyright (C) 2012 Jon Stephan. """ import rospy #import roslib #roslib.load_manifest('differential_drive') from math import sin, cos, pi from geometry_msgs.msg import Quaternion from geometry_msgs.msg import Twist from geometry_msgs.msg import Vector3 from nav_msgs.msg import Odometry import tf from tf.broadcaster import TransformBroadcaster from std_msgs.msg import Int16, Int32, Int64, UInt32 ############################################################################# class DiffTf: ############################################################################# ############################################################################# def __init__(self): ############################################################################# rospy.init_node("diff_tf") self.nodename = rospy.get_name() rospy.loginfo("-I- %s started" % self.nodename) #### parameters ####### #Wheel radius : 0.0325 # wheel circum = 2* 3.14 * 0.0325 = 0.2041 # One rotation encoder ticks : 8 ticks # For 1 meter: 8 * ( 1 / 0.2041) = 39 ticks self.rate = rospy.get_param('~rate',10.0) # the rate at which to publish the transform self.ticks_meter = float(rospy.get_param('ticks_meter', 190)) # The number of wheel encoder ticks per meter of travel self.base_width = float(rospy.get_param('~base_width', 0.11)) # The wheel base width in meters self.base_frame_id = rospy.get_param('~base_frame_id','base_link') # basefootprint /the name of the base frame of the robot self.odom_frame_id = rospy.get_param('~odom_frame_id', 'odom') # the name of the odometry reference frame self.encoder_min = rospy.get_param('encoder_min', -2147483648) self.encoder_max = rospy.get_param('encoder_max', 2147483648) self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min ) self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min ) self.t_delta = rospy.Duration(1.0/self.rate) self.t_next = rospy.Time.now() + self.t_delta # internal data self.enc_left = None # wheel encoder readings self.enc_right = None self.left = 0 # actual values coming back from robot self.right = 0 self.lmult = 0 self.rmult = 0 self.prev_lencoder = 0 self.prev_rencoder = 0 self.x = 0 # position in xy plane self.y = 0 self.th = 0 self.dx = 0 # speeds in x/rotation self.dr = 0 self.yaw = 0.01 self.pitch = 0.01 self.roll = 0.01 self.then = rospy.Time.now() self.quaternion_1 = Quaternion() # subscriptions rospy.Subscriber("wheel_ticks_left", UInt32, self.lwheelCallback) rospy.Subscriber("wheel_ticks_right", UInt32, self.rwheelCallback) #rospy.Subscriber("imu_data", Vector3, self.imu_value_update) self.odomPub = rospy.Publisher("odom", Odometry,queue_size=10) self.odomBroadcaster = TransformBroadcaster() ############################################################################# def spin(self): ############################################################################# r = rospy.Rate(self.rate) while not rospy.is_shutdown(): self.update() r.sleep() ############################################################################# def update(self): ############################################################################# now = rospy.Time.now() if now > self.t_next: elapsed = now - self.then self.then = now elapsed = elapsed.to_sec() # calculate odometry if self.enc_left == None: d_left = 0 d_right = 0 else: d_left = (self.left - self.enc_left) / self.ticks_meter d_right = (self.right - self.enc_right) / self.ticks_meter self.enc_left = self.left self.enc_right = self.right # distance traveled is the average of the two wheels d = ( d_left + d_right ) / 2 # this approximation works (in radians) for small angles th = ( d_right - d_left ) / self.base_width # calculate velocities self.dx = d / elapsed self.dr = th / elapsed if (d != 0): # calculate distance traveled in x and y x = cos( th ) * d y = -sin( th ) * d # calculate the final position of the robot self.x = self.x + ( cos( self.th ) * x - sin( self.th ) * y ) self.y = self.y + ( sin( self.th ) * x + cos( self.th ) * y ) if( th != 0): self.th = self.th + th # publish the odom information quaternion = Quaternion() quaternion.x = 0.0 quaternion.y = 0.0 quaternion.z = sin( self.th / 2 ) quaternion.w = cos( self.th / 2 ) ''' try: quaternion.z = self.quaternion_1[2] quaternion.w = self.quaternion_1[3] except: quaternion.z = sin( self.th / 2 ) quaternion.w = cos( self.th / 2 ) pass ''' self.odomBroadcaster.sendTransform( (self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), rospy.Time.now(), self.base_frame_id, self.odom_frame_id ) odom = Odometry() odom.header.stamp = now odom.header.frame_id = self.odom_frame_id odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y odom.pose.pose.position.z = 0 odom.pose.pose.orientation = quaternion odom.child_frame_id = self.base_frame_id odom.twist.twist.linear.x = self.dx odom.twist.twist.linear.y = 0 odom.twist.twist.angular.z = self.dr self.odomPub.publish(odom) def imu_value_update(self, imu_data): orient = Vector3() orient = imu_data self.yaw = orient.x self.pitch = orient.y self.roll = orient.z try: self.quaternion_1 = tf.transformations.quaternion_from_euler(self.yaw, self.pitch, self.roll) #print self.quaternion_1[0] #print self.quaternion_1[1] #print self.quaternion_1[2] #print self.quaternion_1[3] except: rospy.logwarn("Unable to get quaternion values") pass ############################################################################# def lwheelCallback(self, msg): ############################################################################# enc = msg.data if (enc < self.encoder_low_wrap and self.prev_lencoder > self.encoder_high_wrap): self.lmult = self.lmult + 1 if (enc > self.encoder_high_wrap and self.prev_lencoder < self.encoder_low_wrap): self.lmult = self.lmult - 1 self.left = 1.0 * (enc + self.lmult * (self.encoder_max - self.encoder_min)) self.prev_lencoder = enc ############################################################################# def rwheelCallback(self, msg): ############################################################################# enc = msg.data if(enc < self.encoder_low_wrap and self.prev_rencoder > self.encoder_high_wrap): self.rmult = self.rmult + 1 if(enc > self.encoder_high_wrap and self.prev_rencoder < self.encoder_low_wrap): self.rmult = self.rmult - 1 self.right = 1.0 * (enc + self.rmult * (self.encoder_max - self.encoder_min)) self.prev_rencoder = enc ############################################################################# ############################################################################# if __name__ == '__main__': """ main """ diffTf = DiffTf() diffTf.spin()
Сохраним файл и сделаем его исполняемым:
CTRL+X
chmod +x diff-tf.pyТеперь на роботе запустим 2-е ноды — driver и diff-tf:
1-й терминал:
python diff_tf.py
2-й:
rosrun rosbots_driver part2_cmr.py
В 3-м терминале проверим, что появились новые топики odom и tf:
Посмотрим командой rostopic echo odom, что публикуется в топик (и публикуется ли вообще).
Вывод будет примерно следующим:
Теперь, не закрывая запущенные ноды на роботе, запустим управляющий компьютер с графическими средами rviz и gazebo.
*Образ (виртуальная машина VMWare с Ubuntu 16.04+ROS Kinetic), который ранее предлагался для скачивания содержит все необходимое.
На управляющем компьютере (далее по тексту «Компьютер») запустим модель в rviz:
roslaunch rosbots_description rviz.launch
Загрузится модель робота, с которой работали в предыдущих постах:
Добавим два дисплея в rviz, нажав Add. Дисплей с odometry и дисплей с tf, поставим галочки, чтобы их визуализировать.
В окне, где изображена модель робота появится характерная графика:
*чтобы она была наглядней можно отключить дисплей Robotmodel.
Поуправляем роботом с клавиатуры Компьютера и посмотрим, как изменяется визуализация tf и одометрии.
Не закрывая rviz во 2-м терминале запустим управление с клавиатуры:
rosrun teleop_twist_keyboard teleop_twist_keyboard.py /cmd_vel:=/part2_cmr/cmd_vel
При управлении роботом в окне с визуализацией будут видны: красная стрелка (визуализация топика odom), векторные линии (топик tf).
Если красная стрелка топика odom демонстрирует направление движения робота, то векторные линии tf показывают, как располагаются отдельные элементы робота в пространстве:
видео
Теперь, чтобы продвинуться дальше, необходимо «настроить» одометрию.
Для этого закроем редактор rviz и запустим его заново, только без визуализации модели командой:
rosrun rviz rviz
Этого необходимо для того, чтобы из векторов топика tf остались только base_link и odom:
В rviz одна клетка равна 1 метру. Поэтому в реальности робот также должен проходить 1 метр, чтобы данные были сопоставимы.
Проедем 1 метр на роботе, управляя им с клавиатуры. В rviz робот также должен проехать 1 метр — одну клетку.
Если робот проезжает больше положенного в rviz или наоборот более короткое расстояние, чем в реальности, то необходимо править файл diff_tf.py, который ранее создавался, а именно вот этот блок:
diff_tf.py
#### parameters ####### #Wheel radius : 0.0325 # wheel circum = 2* 3.14 * 0.0325 = 0.2041 # One rotation encoder ticks : 8 ticks # For 1 meter: 8 * ( 1 / 0.2041) = 39 ticks self.rate = rospy.get_param('~rate',10.0) # the rate at which to publish the transform self.ticks_meter = float(rospy.get_param('ticks_meter', 190)) # The number of wheel encoder ticks per meter of travel self.base_width = float(rospy.get_param('~base_width', 0.11)) # The wheel base width in meters
Map (карта)
Чтобы куда-то поехать, необходима карта. Для целей нашего робота — нужна карта помещения.
Поработаем с ней.
Для того, чтобы загрузить карту в rviz, необходимо в проекте (rosbots_description) на Компьютере (не на роботе) создать папку map и поместить в нее два файла, из которых «состоит» карта: map.pgm и map.yaml.
*На самом деле в папке может быть несколько файлов-карт, но загрузить в мастер можно только одну.
Карта в ROS представляет собой два файла, один из которых изображения в формате PGM, где каждый пиксель либо:
- белый — пространство свободно;
- черный — пространство занято препятствием;
- серый — пространство еще не исследовано.
Второй файл .yaml — файл с настройками карты, где указаны ее размеры, заполненность пикселями разных видов (перечисленных выше), иные параметры.
Запустим на Компьютере ноду, которая опубликует карту:
rosrun map_server map_server /home/pi/catkin_ws/src/rosbots_description/maps/rail_lab.pgm 0.05
В соседнем терминале запустим модель в rviz:
roslaunch rosbots_description rviz.launch
В rviz добавим дисплей Map.
В rviz робот получился непропорционально большим, и расположен вне карты:
Чтобы это исправить, надо запустить карту, где размер клетки будет 1 метр. Перезапустим карту с параметром 1 на окончании:
rosrun map_server map_server /home/pi/catkin_ws/src/rosbots_description/maps/rail_lab.pgm 1
Теперь можно поездить по карте в rviz, управляя роботом с клавиатуры:
видео
Итак, что удалось добиться:
- получить данные одометрии с оптических энкодеров колес робота и отправить их в топики для отображения в rviz;
- настроить одометрию робота для соответствия пройденного расстояния вживую и виртуально;
- загрузить и отобразить карту помещения.
Однако, несмотря на то, что карта отображается и робот может по ней ездить с «настроенной» одометрией, в реальности робот слеп. Он не видит препятствий и будет натыкаться на них. Второй минус — виртуальная карта помещения, загруженная в rviz позволяет ездить по себе во всех направлениях, даже в тех, где явно изображены препятствия.
Как заставить робота «видеть» препятствия в реальности и виртуально?
С виртуальной средой попроще. Здесь все строится на базе эмулятора-редактора gazebo. И в предыдущих постах об этом упоминалось.
С реальностью посложнее. Нужен элемент (датчик), который будет обозначать препятствия и сообщать об этом системе.
Один из вариантов — поставить на робота лидар.
Лидар RPlidar A1
Воспользуемся доступным бюджетным решением и поставим на робота лидар. Возможно, это решение будет дороже использования того же Kinectа, но оно, как показала практика, более эффективно в плане скорости работы, точности и простоты монтажа (менее громоздко). Кроме того, с lidarом проще начать работать, т.к. не требуются размышления, как его запитать и подключить к проекту (https://habr.com/ru/company/tod/blog/210252/).
Нам понадобится пакет ros для работы с лидаром — wiki.ros.org/rplidar.
С помощью лидара мы построим карту помещения, а также используем его в навигации.
Как установить rplidar в ROS есть масса статей, например здесь.
Воспользуемся знаниями седых старцев и установим пакеты с лидаром в систему на роботе:
cd /home/pi/rosbots_catkin_ws/src git clone https://github.com/robopeak/rplidar_ros.git cd .. catkin_make
На Компьютере установим пакет для работы с картой:
cd /home/pi/rosbots_catkin_ws/src git clone https://github.com/tu-darmstadt-ros-pkg/hector_slam </code> cd .. catkin_make
Запустим на роботе пакет и проверим работает ли лидар:
sudo chmod a+rw /dev/ttyUSB0 roslaunch rplidar_ros rplidar.launch
*первая команда дает доступ к порту usb, где подключен лидар.
Если все пошло гладко, то в консоль выдаст строки:
[ INFO] [1570900184.874891236]: RPLIDAR running on ROS package rplidar_ros. SDK Version:1.9.0 RPLIDAR S/N: ---------------- [ INFO] [1570900187.397858270]: Firmware Ver: 1.24 [ INFO] [1570900187.398081809]: Hardware Rev: 5 [ INFO] [1570900187.401749476]: RPLidar health status : 0 [ INFO] [1570900188.014285166]: current scan mode: Express, max_distance: 12.0 m, Point number: 4.0K , angle_compensate: 1
Здесь сразу немного настроим лидар, т.к. официальный сайт говорит, что он (лидар) может и получше работать.
Нам необходимо добиться выдачи при сканировании не 4.0K точек, котрые выдаются по умолчанию, а 8.0К. Данная опция немного улучшит качество сканирования.
Для это в пакете rplidar зададим еще один параметр — scan mode:
cd /rosbots_catkin_ws/src/rplidar_ros/launch nano nano rplidar.launch
И после
добавим строку:<param name="angle_compensate" type="bool" value="true"/>
<param name="scan_mode" type="string" value="Boost"/>
Вторую строку, которую надо поправить здесь же:
<param name="frame_id" type="string" value="laser"/>
Значение laser заменить на base_link.
*Теперь, если перезапустить ноду командой roslaunch rplidar_ros rplidar.launch, вывод будет другим:
[ INFO] [1570900188.014285166]: current scan mode: Boost, max_distance: 12.0 m, Point number: 8.0K , angle_compensate: 1
Посмотри. что выводит лидар в rviz.
Для этого запустим на роботе:
roslaunch rplidar_ros rplidar.launch
На Компьютере:
roslaunch rosbots_description rviz.launch
В rviz добавим дисплей LaserScan и выберем топик scan. Далее будет видно, что в топик падают сообщения:
В окне с визуализацией робота, робот получился великаном. С его размерами мы разберемся позднее. А сейчас построим карту помещения.
Для этого создадим пакет с нодой:
catkin_create_pkg my_hector_mapping rospy cd my_hector_mapping mkdir launch cd launch nano hector.launch
hector.launch
<?xml version="1.0"?> <launch> <node pkg="tf" type="static_transform_publisher" name="laser_link" args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /laser 50" /> <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen"> <!-- Frame names --> <param name="map_frame" value="map" /> <param name="odom_frame" value="base_link" /> <!-- Map size / start point --> <param name="map_resolution" value="0.050"/> <param name="map_size" value="1024"/> <param name="map_start_x" value="0.5"/> //середина карты <param name="map_start_y" value="0.5" /> <param name="map_multi_res_levels" value="2" /> <!-- Map update parameters --> <param name="update_factor_free" value="0.4"/> <param name="update_factor_occupied" value="0.9" /> <param name="map_update_distance_thresh" value="0.4"/> <param name="map_update_angle_thresh" value="0.06" /> <param name="laser_z_min_value" value="-1.0" /> <param name="laser_z_max_value" value="1.0" /> <!-- Advertising config --> <param name="advertise_map_service" value="true"/> <param name="scan_subscriber_queue_size" value="5"/> <param name="scan_topic" value="scan"/> </node> </launch>
cd ~/rosbots_catkin_ws catkin_make
Запустим.
На роботе:
1-й терминал:
roslaunch rplidar_ros rplidar.launch2-й:
rosrun rosbots_driver part2_cmr.pyНа Компьютере:
1-й терминал:
roslaunch my_hector_mapping hector.launch2-й:
roslaunch rosbots_description rviz.launch3-й:
rosrun teleop_twist_keyboard teleop_twist_keyboard.py /cmd_vel:=/part2_cmr/cmd_velВ дисплеи надо добавить map, а Fixed frame выбрать base_link. Далее можно наблюдать в режиме реального времени, как лидар «освещает» пространство вокруг себя:
На текущем шаге, чтобы построить карту, надо поездить по помещению, «заезжая» в разные углы, чтобы лидар обозначил их на карте.
Так рекомендуют учебники. Но наш совет — взять робота в руки и пройтись с ним, держа его перед собой. Так скорость построения карты будет больше в том плане, что вам не надо будет отвлекаться и смотреть, куда заехал робот в соседнем помещении при отсутствии визуального контакта.
Кроме того, при поворотах роботом вокруг своей оси при поездке лидар оставляет характерные артефакты черного цвета в тех местах, где на самом деле нет препятствий:
После построения карты, сохраним ее командой:
rosrun map_server map_saver -f map-1
Построение идеальной карты с помощью бюджетного лидара — миф. Поэтому поможем лидару в фотошопе. Удалим артефакты черного цвета с карты, где их на самом деле нет препятствий, а стены выровняем линиями черного цвета:
Не забываем сохранить карту в формате .pgm.
Теперь повторим на Компьютере команды, которые были в начале поста, но уже с новой картой:
1-й терминал:
rosrun map_server maserver /home/pi/catkin_ws/src/rosbots_description/maps/map-1.pgm 0.052-й:
roslaunch rosbots_description rviz.launchРезультат в rviz:
Новая карта загрузилась, как и модель робота на ней, но робот вне карты.
Что с этим делать поговорим позднее, а пока подведем итоги:
- освоение лидара RP-lidar A1
- построение карты помещения с помощью лидара, ее корректировка и загрузка в визуальный редактор rviz.
Файлы для загрузки: карта помещения.
