Посты серии:
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.py2-й:
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 metersMap (карта)
Чтобы куда-то поехать, необходима карта. Для целей нашего робота — нужна карта помещения.
Поработаем с ней.
Для того, чтобы загрузить карту в 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.
Файлы для загрузки: карта помещения.
