Робот-тележка 2.0. Часть 2. Управление в rviz и без.Элементы красоты в rviz +7


В прошлой статье, посвященной автономной домашней тележке 2.0, удалось поработать над тем, как улучшить одометрию бюджетного робота, добиться построение приемлемой 2d карты помещения, используя slam алгоритмы и доступный лидар, внести ясность в иные вопросы при сборке проекта. В этот раз посмотрим как работает автономная навигация в rviz, что за что отвечает, внедрим программы, которые позволять уйти из rviz. Рассмотрим также некоторые «элементы красоты» rviz, которые облегчают жизнь робототехника ROS.

image

Начнем с практики


Задача роботу: доехать самостоятельно из точки А в точку B, а лучше сразу в несколько точек B.

Предполагается, что скрипты(ноды) проекта linorobot, вокруг которого строится проект, будут запускаться на роботе, а визуализация того, что происходит и управление будут вестись с внешнего ПК, виртуальной машины (далее по тексту «ПК»).

В моем случае из программного обеспечения на тележке, сердцем которой выступает одноплатник raspberry pi 3b, установлено: Ubuntu 16.04, ROS Kinetic, на ПК: Ubuntu 18.04, ROS Melodic.

Перед тем как куда-нибудь поехать, необходимо задать габариты робота — длину и ширину.
Для этого, как указано на странице проекта, необходимо внести соответствующие значения:

roscd linorobot/param/navigation
nano costmap_common_params.yaml

Поправить параметры footprint:

footprint: [[-x, -y], [-x, y], [x, y], [x, -y]]

где x = длина робота/ 2 и y = ширина / 2
*В идеале, для робота можно нарисовать urdf-модель, как это ранее делалось в предыдущем проекте робота-тележки.

Однако, если этого не сделать, навигация не сильно пострадает, но вместо модели робота на карте будут ездить «оси TF». Как немного улучшить ситуацию с визуальным представлением робота на карте и при этом не тратить время на построение urdf-модели, поговорим далее.

Запускаем окружение


На роботе выполним в двух разных терминалах:

roslaunch linorobot bringup.launch
roslaunch linorobot navigate.launch

На ПК запустим визуальную оболочку rviz:
roscd lino_visualize/rviz
rviz -d navigate.rviz

Если все пошло удачно, на ПК можно наблюдать в rviz расположение робота:



В rviz видны оси TF, а также опция Poligon, которую можно добавить на панели Displays. Polygon — это как раз те footprint-размеры робота, которые вносили в начале, позволяет увидеть габариты робота. *Из осей на картинке видны оси TF: TF-map — карта и TF-base-link — сам робот, остальные оси, чтобы не сливаться между собой, приглушены в rviz.

Данные с лидара немного не совпадают со стенами на карте, робот не локализован.

Если поехать с данной позиции, алгоритм попытается подстроиться, но можно и помочь ему, указав начальную позицию прямо в rviz c помощью кнопки «2d pose estimate»:



Далее в окне визуализации rviz на карте обозначить начальное расположение и направление робота здоровенной зеленой стрелкой- initial pose (сделать это можно многократно):



После уточнения позиции, base_link немного сдвинется относительно map:



Абсолютная точность совпадения границ с лидара с картой при ручной корректировке начальной позиции не нужна, алгоритм далее подкорректирует все самостоятельно.

Перед тем как все-таки поехать


Каждый раз выставлять в rviz начальную позицию, если на местности робот стартует с одного и того же места так себе занятие. Поэтому воспользуемся сервисом (помним, что кроме topicов в ROS есть сервисы), при обращении к которому будет считывать «расположение робота на карте», далее, эти данные мы будем использовать.

Создадим скрипт на ПК (не имеет значение в каком месте)

get_pose.py
#! /usr/bin/env python
import rospy
from std_srvs.srv import Empty, EmptyResponse # Import the service message python classes generated from Empty.srv.
from geometry_msgs.msg import PoseWithCovarianceStamped, Pose
robot_pose = Pose()
def service_callback(request):
    print ("Robot Pose:")
    print (robot_pose)
    return EmptyResponse() # the service Response class, in this case EmptyResponse    
def sub_callback(msg):
    global robot_pose
    robot_pose = msg.pose.pose   
rospy.init_node('service_server') 
my_service = rospy.Service('/get_pose_service', Empty , service_callback) # create the Service called get_pose_service with the defined callback
sub_pose = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, sub_callback)
rospy.spin() # mantain the service open.


После запуска скрипт в терминале ничего не выводит и тихо ждет, когда к нему обратятся. Так как в rviz с помощью графического интерфейса мы недавно уже выставили начальную позицию робота, можно обратиться к нашему новому скрипту и узнать эту позицию. Не закрывая терминал со скриптом (как и ранее запущенные терминалы с нодами), в другом терминале выполним:

rosservice call /get_pose_service

После выполнения запроса в терминале со скриптом get_pose.py выдаст позицию робота:



Сохраним эти показатели и внесем их в настройки amcl-ноды на роботе, чтобы робот каждый раз стартовал с указанной позиции:

nano linorobot/launch/include/amcl.launch

Добавим
параметры
<param name="initial_pose_x" value="0.548767569629"/>
<param name="initial_pose_y" value="0.218281839179"/>
<param name="initial_pose_z" value="0.0"/>
<param name="initial_orientation_z" value="0.128591756735"/>
<param name="initial_orientation_w" value="0.991697615254"/>


Теперь при старте, робот будет стартовать уже с определенной позиции, и ее также не нужно будет выставлять вручную в rviz:



Едем наконец


Перезапустим команды для робота и ПК, приведенные в начале и посмотрим на робота в rviz.

Воспользуемся в rviz «2D NAV Goal», чтобы отправить робота туда, куда душа пожелает:



Как видно, роботу можно не только указывать расстояние поездки, но и направление движения.
В приведенном примере мы проехали немного вперед, развернулись и вернулись назад.

В rviz видны также знакомые нам «зеленые стрелочки» Amcl particle swarm, которые показывают, где, по мнению программы, расположен робот. После поездки их концентрация приближается к реальному расположению робота, общее количество уменьшается. Робот едет достаточно быстро — 0.3 м/c, поэтому программе требуется больше времени, чтобы локализовать робота.

Навигация в rviz с помощью «2D NAV Goal» для понимания как далеко и в какую сторону поедет робот удобна. Но еще удобнее поездка без участия оператора в rviz.

Поэтому выделим на карте точки интереса, куда будем ездить. Для этого переместим робота в нужные точки на карте, используя «2D NAV Goal». В этих точках будет обращаться к сервису get_pose.py. Далее координаты всех точек перенесем в программу поездки.

Почему просто не перенести робота на руках в точки интереса? Дело в «Amcl particle swarm», в алгоритме, который их строит. Даже если робот будет в точке интереса, и его позиция будет указана вручную, вариант его местонахождения будет «распылен» по области вокруг самой точки интереса. Поэтому, придется все-таки ехать.

Начнем с точки под кодовым названием «коридор»:



Сохраним позу, когда робот будет в нужной точке, используя вызов ранее созданного сервиса:

rosservice call /get_pose_service

Далее — «прихожая»:



И финальная точка — «кухня»:



Теперь пришло время создать программу поездок —

MoveTBtoGoalPoints.py
#!/usr/bin/env python
import rospy
import actionlib
# Use the actionlib package for client and server
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
# Define Goal Points and orientations for TurtleBot in a list
"""
pose x,y,x; orientation: x,y,z,w
"""
GoalPoints = [ [(5.31514576482, 1.36578380326, 0.0), (0.0, 0.0, -0.498454937648, 0.866915610157)], #corridor
		[(5.77521810772, -1.2464049907, 0.0), (0.0, 0.0,  -0.959487358308, 0.281751680114)], #prixozaya
		[(2.14926455346, -2.7055208156, 0.0), (0.0, 0.0,  -0.99147032254, 0.130332649487)]] #kitchen
# The function assign_goal initializes goal_pose variable as a MoveBaseGoal action type.
def assign_goal(pose):
    goal_pose = MoveBaseGoal()
    goal_pose.target_pose.header.frame_id = 'map'
    goal_pose.target_pose.pose.position.x = pose[0][0]
    goal_pose.target_pose.pose.position.y = pose[0][1]
    goal_pose.target_pose.pose.position.z = pose[0][2]
    goal_pose.target_pose.pose.orientation.x = pose[1][0]
    goal_pose.target_pose.pose.orientation.y = pose[1][1]
    goal_pose.target_pose.pose.orientation.z = pose[1][2]
    goal_pose.target_pose.pose.orientation.w = pose[1][3]
 
    return goal_pose
if __name__ == '__main__':
    rospy.init_node('MoveTBtoGoalPoints')
    # Create a SimpleActionClient of a move_base action type and wait for server.
    client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
    client.wait_for_server()
    # for each goal point in the list, call the action server and move to goal
    for TBpose in GoalPoints:
        TBgoal = assign_goal(TBpose)
        # For each goal point assign pose
        client.send_goal(TBgoal)
        client.wait_for_result()
        # print the results to the screen
        #if(client.get_state() == GoalStatus.SUCCEEDED):
        #    rospy.loginfo("success")
        #else:        
        #   rospy.loginfo("failed")


*В GoalPoints внесем ранее сохраненные координаты точек поездки. В коде есть пояснение в каком порядке идут параметры: pose x,y,x; orientation: x,y,z,w — [(5.31514576482, 1.36578380326, 0.0), (0.0, 0.0, -0.498454937648, 0.866915610157)]

Запускаем скрипт MoveTBtoGoalPoints.py параллельно с ранее запущенными нодами на роботе и ПК, смотрим результат:



Как видно, робот успешно доехал до прихожей и по пути на кухню увидел себя в зеркало и застрял. Что ж с лидарами при встрече с зеркалами, к сожалению так.

Что можно сделать?

Возможно, добавить промежуточные точки до и после зеркал, снизить скорость передвижения робота, наклеить непрозрачную ленту на зеркало по пути следования.

Но это уже следующие шаги, а пока будем считать задачу поездки из точки А в точку B с помощью и без помощи rviz выполненной. Теперь нам, в принципе, не нужен внешний ПК, т.к. скрипт поездки довезет нас до нужных точек на карте.

Но не будем торопиться отбрасывать ПК вместе с rviz, так как проект linorobot не работает корректно «из коробки», и нужно «поработать напильником» над навигационным стеком.

Тут самое время возразить — позвольте, где тут автономная навигация, робот проехал n шагов вперед, в сторону, назад, да еще и в зеркало заехал?

Что ж, все так, за исключением некоторых деталей, которые и выделяют ROS.

Посмотрим на эти детали.

Допустим, какой нибудь несознательный гражданин оставил неодушевленный предмет на пути следования робота. Этого предмета нет на карте, которая выдана роботу для навигации:



Что произойдет?

Посмотрим:



Как видно, лидар увидел препятствие, алгоритм его обозначил на карте и скомандовал на объезд.

Напоследок, добавим немного красоты в rviz


В визуальном редакторе rviz есть интересные плагины, которые не установлены по умолчанию. Их использование может добавить достаточно много интересных моментов при работе с rviz.

Репозиторий с плагинами находится здесь.

Установим на ПК в наше окружение плагины:

roscd; cd ../src;
catkin_create_pkg my_rviz_markers rospy
cd my_rviz_markers/
cd src
git clone https://github.com/jsk-ros-pkg/jsk_visualization
git clone https://github.com/ClementLeBihan/rviz_layer
cd ..
rosdep install --from-paths -y -r src
cd ~/linorobot_ws/
catkin_make

После установки в rviz должны появиться дополнительные плагины.

Теперь, добавим, например, пиктограммы:



В папке my_rviz_markers/samples создадим скрипт:

pictogram_array_objects_demo.py
#!/usr/bin/env python
import rospy
import math
from jsk_rviz_plugins.msg import Pictogram, PictogramArray
from random import random, choice
rospy.init_node("pictogram_object_demo_node")
p = rospy.Publisher("/pictogram_array", PictogramArray,  queue_size=1)

r = rospy.Rate(1)
actions = [Pictogram.JUMP, Pictogram.JUMP_ONCE, Pictogram.ADD, 
           Pictogram.ROTATE_X, Pictogram.ROTATE_Y, Pictogram.ROTATE_Z]
pictograms = ["home","fa-robot"]
object_frames = ["map","base_link"]

while not rospy.is_shutdown():
    
    arr = PictogramArray()
    arr.header.frame_id = "/map"
    arr.header.stamp = rospy.Time.now()
    for index, character in enumerate(pictograms):
        msg = Pictogram()
        msg.header.frame_id = object_frames[index]
        msg.action = actions[2]
        msg.header.stamp = rospy.Time.now()
        msg.pose.position.x = 0
        msg.pose.position.y = 0
        msg.pose.position.z = 0.5
        # It has to be like this to have them vertically orient the icons.
        msg.pose.orientation.w = 0.7
        msg.pose.orientation.x = 0
        msg.pose.orientation.y = -0.7
        msg.pose.orientation.z = 0
        msg.size = 0.5
        msg.color.r = 25 / 255.0
        msg.color.g = 255 / 255.0
        msg.color.b = 240 / 255.0
        msg.color.a = 1.0
        msg.character = character
        arr.pictograms.append(msg)
    p.publish(arr)
    r.sleep()


Сделаем его исполняемым:

sudo chmod +x pictogram_array_objects_demo.py

В папке my_rviz_markers/launch создадим launch для запуска:

<launch>
  <node pkg="my_rviz_markers" type="pictogram_array_objects_demo.py" name="pictogram_array_objects_demo" </node>
</launch>

Запускается он как обычно (все остальные ноды в это время также должны работать):

roslaunch my_rviz_markers pictogram_array_objects_demo.launch

Поставим маркер на карту


Маркеры могут быть полезны для понимания где на карте находится точка.

Как и в предыдущем примере в папке my_rviz_markers/samples создадим скрипт:

basic_marker.py
#!/usr/bin/env python

import rospy
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point


class MarkerBasics(object):
    def __init__(self):
        self.marker_objectlisher = rospy.Publisher('/marker_basic', Marker, queue_size=1)
        self.rate = rospy.Rate(1)
        self.init_marker(index=0,z_val=0)
    
    def init_marker(self,index=0, z_val=0):
        self.marker_object = Marker()
        self.marker_object.header.frame_id = "/map"
        self.marker_object.header.stamp    = rospy.get_rostime()
        self.marker_object.ns = "start"
        self.marker_object.id = index
        self.marker_object.type = Marker.SPHERE
        self.marker_object.action = Marker.ADD

        my_point = Point()
        my_point.z = z_val
        #self.marker_object.pose.position = my_point
        self.marker_object.pose.position.x = 5.31514576482
        self.marker_object.pose.position.y = 1.36578380326 
        self.marker_object.pose.orientation.x = 0
        self.marker_object.pose.orientation.y = 0
        self.marker_object.pose.orientation.z = -0.498454937648
        self.marker_object.pose.orientation.w = 0.866915610157
        self.marker_object.scale.x = 0.2
        self.marker_object.scale.y = 0.2
        self.marker_object.scale.z = 0.2
    
        self.marker_object.color.r = 0.0
        self.marker_object.color.g = 0.0
        self.marker_object.color.b = 1.0
        # This has to be, otherwise it will be transparent
        self.marker_object.color.a = 1.0

        # If we want it for ever, 0, otherwise seconds before desapearing
        self.marker_object.lifetime = rospy.Duration(0)
    
    def start(self):
        while not rospy.is_shutdown():
            self.marker_objectlisher.publish(self.marker_object)
            self.rate.sleep()
   
if __name__ == '__main__':
    rospy.init_node('marker_basic_node', anonymous=True)
    markerbasics_object = MarkerBasics()
    try:
        markerbasics_object.start()
    except rospy.ROSInterruptException:
        pass


В папке my_rviz_markers/launch создадим launch для запуска:

basic_marker.launch
<launch>
  <node pkg="my_rviz_markers" type="basic_marker.py" name="basic_marker" />
</launch>


Запустим:

roslaunch my_rviz_markers basic_marker.launch

Добавим в rviz:



Добавим графики


Которые покажут расстояние и угол между двумя фреймами:



В папке my_rviz_markers/launch создадим launch для запуска:

overlay_meny_my.launch overlay_menu_my.launch
<launch>
  <node pkg="my_rviz_markers" type="haro_overlay_complete_demo.py" name="overlay_menu" />
</launch>


В папке my_rviz_markers/samples создадим скрипты:

haro_overlay_complete_demo.py
#!/usr/bin/env python

from jsk_rviz_plugins.msg import OverlayText, OverlayMenu
from std_msgs.msg import ColorRGBA, Float32
import rospy
import math
import random
from geometry_msgs.msg import Twist

class HaroOverlay(object):
    def __init__(self):
        
        self.text_pub = rospy.Publisher("/text_sample", OverlayText, queue_size=1)
        
        self.plot_value_pub = rospy.Publisher("/plot_value_sample", Float32, queue_size=1)
        self.piechart_value_pub = rospy.Publisher("/piechart_value_sample", Float32, queue_size=1)
        self.menu_publisher = rospy.Publisher("/test_menu", OverlayMenu, queue_size=1)
        self.plot_value = 0
        self.piechart_value = 0
        self.max_distance_from_object = 10.0
        
        self.subs = rospy.Subscriber("/haro_base_link_to_person_standing_tf_translation", Twist, self.twist_callback)
        
        self.counter = 0
        self.rate = rospy.Rate(100)
        self.overlaytext = self.update_overlaytext()
        self.menu = self.update_overlay_menu()

    def twist_callback(self, msg):
        self.plot_value = msg.linear.x
        self.piechart_value = msg.angular.z

    def update_overlaytext(self, number=1.23, number2=20):  
        
        text = OverlayText()
        text.width = 200
        text.height = 400
        text.left = 10
        text.top = 10
        text.text_size = 12
        text.line_width = 2
        text.font = "DejaVu Sans Mono"
        text.text = """Distance=  %s.
        Angle=%s.
        Counter = <span style="color: green;">%d.</span>
        """ % (str(number), str(number2) ,self.counter)
        text.fg_color = ColorRGBA(25 / 255.0, 1.0, 240.0 / 255.0, 1.0)
        text.bg_color = ColorRGBA(0.0, 0.0, 0.0, 0.2)
        
        return text
    
    def update_overlay_textonly(self, new_text):
        self.overlaytext.text = new_text


    def update_overlay_menu(self):
        menu = OverlayMenu()
        menu.title = "HaroSystemMode"
        menu.menus = ["Sleep", "Searching", "MovingInCircles","Waiting"]
        
        menu.current_index = self.counter % len(menu.menus)
        return menu
        
    def update_overlay_menu_haro_tf(self):
        menu = OverlayMenu()
        menu.title = "HaroDistanceFromPerson"
        menu.menus = ["FarAway", "CloseBy", "Target", "OtherWayRound"]
        
        fraction = 10.0
        
        if self.piechart_value < (math.pi/fraction):
            if self.plot_value >= self.max_distance_from_object:
                index = 0
            elif self.plot_value >= (self.max_distance_from_object/ fraction) and self.plot_value < self.max_distance_from_object:
                index = 1
            elif self.plot_value < (self.max_distance_from_object/fraction):
                index = 2
            
        else:
            index = 3
            
        menu.current_index = index
            
        return menu

    def start_dummy_demo(self):
        
        while not rospy.is_shutdown():
            self.overlaytext = self.update_overlaytext()
            self.menu = self.update_overlay_menu()
            
            if self.counter % 100 == 0:
                self.menu.action = OverlayMenu.ACTION_CLOSE
            
            self.text_pub.publish(self.overlaytext)
            # If values are very high it autoadjusts so be careful
            self.value_pub.publish(math.cos(self.counter * math.pi * 2 / 1000))
            self.menu_publisher.publish(self.menu)
            
            self.rate.sleep()
            self.counter += 1
    
    def start_harodistance_demo(self):
        
        while not rospy.is_shutdown():
            self.overlaytext = self.update_overlaytext(number=self.plot_value, number2=self.piechart_value)
            self.menu = self.update_overlay_menu_haro_tf()
            
            self.text_pub.publish(self.overlaytext)
            
            self.plot_value_pub.publish(self.plot_value)
            self.piechart_value_pub.publish(self.piechart_value)
            
            self.menu_publisher.publish(self.menu)
            
            self.rate.sleep()
            self.counter += 1
  
  
def dummy_overlay_demo():
    rospy.init_node('distance_overlay_demo_node', anonymous=True)
    haro_overlay_object = HaroOverlay()
    try:
        #haro_overlay_object.start_dummy_demo()
        haro_overlay_object.start_harodistance_demo()
    except rospy.ROSInterruptException:
        pass

if __name__ == '__main__':
    dummy_overlay_demo()


Второй скрипт —

tf_haro_to_object_listener.py
#!/usr/bin/env python
import sys
import rospy
import math
import tf
from geometry_msgs.msg import Twist, Vector3

if __name__ == '__main__':
    rospy.init_node('tf_listener_haro_to_person')

    listener = tf.TransformListener()

    if len(sys.argv) < 3:
        print("usage: tf_haro_to_object_listener.py parent child")
    else:
        follower_model_name = sys.argv[1]
        model_to_be_followed_name = sys.argv[2]
        
        topic_to_publish_name = "/"+str(follower_model_name)+"_to_"+str(model_to_be_followed_name)+"_tf_translation"
        # We will publish a Twist message but it's positional data not speed, just reusing an existing structure.
        tf_translation = rospy.Publisher(topic_to_publish_name, Twist ,queue_size=1)
        
        rate = rospy.Rate(10.0)
        ctrl_c = False
        
        follower_model_frame = "/"+follower_model_name
        model_to_be_followed_frame = "/"+model_to_be_followed_name
        
        def shutdownhook():
            # works better than the rospy.is_shut_down()
            global ctrl_c
            ctrl_c = True

        rospy.on_shutdown(shutdownhook)
        
        while not ctrl_c:
            try:
                (trans,rot) = listener.lookupTransform(follower_model_frame, model_to_be_followed_frame, rospy.Time(0))
                translation_object = Vector3()
                translation_object.x = trans[0]
                translation_object.y = trans[1]
                translation_object.z = trans[2]
                
                angular = math.atan2(translation_object.y, translation_object.x)
                linear = math.sqrt(translation_object.x ** 2 + translation_object.y ** 2)
                cmd = Twist()
                cmd.linear.x = linear
                cmd.angular.z = angular
                tf_translation.publish(cmd)
                
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                continue
            rate.sleep()


Запускается так (считается, что все остальные главные ноды и rviz уже запущены):

roslaunch linorobot overlay_meny_my.launch
python tf_haro_to_object_listener.py base_link map

Увидим угол и расстояние фрейма base_link относительно map.

Продолжение следует.




К сожалению, не доступен сервер mySQL