Начнем с практики.
Задача роботу: доехать самостоятельно из точки А в точку 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 есть сервисы), при обращении к которому будет считывать «расположение робота на карте», далее, эти данные мы будем использовать.
Создадим скрипт на ПК (не имеет значение в каком месте)
#! /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 Далее — «прихожая»:
И финальная точка — «кухня»:
Теперь пришло время создать программу поездок —
#!/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 создадим скрипт:
#!/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 создадим скрипт:
#!/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 для запуска:
<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 для запуска:
<launch> <node pkg="my_rviz_markers" type="haro_overlay_complete_demo.py" name="overlay_menu" /> </launch>
В папке my_rviz_markers/samples создадим скрипты:
#!/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()
Второй скрипт-
#!/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.
Продолжение следует.
ссылка на оригинал статьи https://habr.com/ru/post/502508/
Добавить комментарий