Продолжение цикла статей.

Предыдущие посты серии:

Часть 5 Часть 4 Часть 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.launch
2-й: rosrun rosbots_driver part2_cmr.py

На Компьютере:

1-й терминал: roslaunch my_hector_mapping hector.launch
2-й: roslaunch rosbots_description rviz.launch
3-й: 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.05
2-й: roslaunch rosbots_description rviz.launch

Результат в rviz:



Новая карта загрузилась, как и модель робота на ней, но робот вне карты.

Что с этим делать поговорим позднее, а пока подведем итоги:

  • освоение лидара RP-lidar A1
  • построение карты помещения с помощью лидара, ее корректировка и загрузка в визуальный редактор rviz.

Файлы для загрузки: карта помещения.

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


  1. joger
    14.10.2019 14:48

    >>Построение идеальной карты с помощью бюджетного лидара — миф.
    :(
    А можно вот здесь подробнее? В чём проблема? в недостаточной точности лидара или алгоритмах? Просто у меня есть «мечта»: собрать похожего робота на rplidar a1/a2, чтобы автоматом сделать план (пустой) квартиры с хорошей точностью. Может подскажете куда смотреть?


    1. zoldaten Автор
      14.10.2019 16:24

      Несколько факторов:
      — точность лидара. Для бюджетного а1 она не сильно велика. Мелкие предметы типа ножек от табурета может не увидеть. Заявлено расстояние до 12 м. Но по факту, если близко не подъехать к предмету, он может не попасть на карту.
      — «слепые» зоны (не видит на слишком коротких дистанциях (до 10 см)). Зеркала и стеклянные двери не видит как препятствия (луч отражается). То же самое с ярко освещенными поверхностями (солнцем или лампами) -их может не увидеть.
      — специфика лидара. Если препятствие выше или ниже горизонта луча, препятствия опять же для лидара нет. Если повращать лидар вокруг своей оси, он собирает шумы, в статье об этом сказано.

      Все это решается в определенной степени: установка лидара под углом к поверхности, «ночной режим» при построении карты, фотошоп.


  1. Nenashev_1999
    14.10.2019 22:06

    Здравствуйте, благодарен за ваши статьи, сейчас готовимся к робототехническим соревнованиям, изучая ROS, раньше все сводилось просто к считыванию данных энкодеров и дальномеров, а сейчас одометрия, карты и тд, вопрос в следующем, не подскажете как описать одометрию 3х колесной под 120 градусов омни платформы, тоесть в ардуино написаны формулы чтобы разкидывать правильно движки, данные энкодеров имею


    1. zoldaten Автор
      14.10.2019 23:02

      Спасибо.
      Если вас интересует математическая составляющая, то в двух словах тут не передать. Погуглите «mecanum drive equations». Вот, например, очень близко к вашей теме — youtu.be/NcOT9hOsceE

      --