Оффлайн
- Регистрация
- 12.04.17
- Сообщения
- 19.095
- Реакции
- 107
- Репутация
- 0
В прошлой
Начнем с практики.
Задача роботу: доехать самостоятельно из точки А в точку 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
Добавим
параметры
Теперь при старте, робот будет стартовать уже с определенной позиции, и ее также не нужно будет выставлять вручную в 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
git clone
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 для запуска:
node>
Запускается он как обычно (все остальные ноды в это время также должны работать):
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
Запустим:
roslaunch my_rviz_markers basic_marker.launch
Добавим в rviz:
Добавим графики.
Которые покажут расстояние и угол между двумя фреймами:
В папке my_rviz_markers/launch создадим launch для запуска:
overlay_meny_my.launch overlay_menu_my.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 = %d.
""" % (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.
Продолжение следует.
You must be registered for see links
, посвященной автономной домашней тележке 2.0, удалось поработать над тем, как улучшить одометрию бюджетного робота, добиться построение приемлемой 2d карты помещения, используя slam алгоритмы и доступный лидар, внести ясность в иные вопросы при сборке проекта. В этот раз посмотрим как работает автономная навигация в rviz, что за что отвечает, внедрим программы, которые позволять уйти из rviz. Рассмотрим также некоторые «элементы красоты» rviz, которые облегчают жизнь робототехника ROS.
Начнем с практики.
Задача роботу: доехать самостоятельно из точки А в точку B, а лучше сразу в несколько точек B.
Предполагается, что скрипты(ноды) проекта linorobot, вокруг которого строится проект, будут запускаться на роботе, а визуализация того, что происходит и управление будут вестись с внешнего ПК, виртуальной машины (далее по тексту «ПК»).
В моем случае из программного обеспечения на тележке, сердцем которой выступает одноплатник raspberry pi 3b, установлено: Ubuntu 16.04, ROS Kinetic, на ПК: Ubuntu 18.04, ROS Melodic.
Перед тем как куда-нибудь поехать, необходимо задать габариты робота — длину и ширину.
Для этого, как указано на
You must be registered for see links
проекта, необходимо внести соответствующие значения:roscd linorobot/param/navigation
nano costmap_common_params.yaml
Поправить параметры footprint:
footprint: [[-x, -y], [-x, y], [x, y], [x, -y]]
где x = длина робота/ 2 и y = ширина / 2
*В идеале, для робота можно нарисовать urdf-модель, как это ранее делалось в предыдущем
You must be registered for see links
.Однако, если этого не сделать, навигация не сильно пострадает, но вместо модели робота на карте будут ездить «оси 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
Добавим
параметры
Теперь при старте, робот будет стартовать уже с определенной позиции, и ее также не нужно будет выставлять вручную в 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.
Репозиторий с плагинами находится
You must be registered for see links
. Установим на ПК в наше окружение плагины:
roscd; cd ../src;
catkin_create_pkg my_rviz_markers rospy
cd my_rviz_markers/
cd src
git clone
You must be registered for see links
git clone
You must be registered for see links
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 для запуска:
node>
Запускается он как обычно (все остальные ноды в это время также должны работать):
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
Запустим:
roslaunch my_rviz_markers basic_marker.launch
Добавим в rviz:
Добавим графики.
Которые покажут расстояние и угол между двумя фреймами:
В папке my_rviz_markers/launch создадим launch для запуска:
overlay_meny_my.launch overlay_menu_my.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 = %d.
""" % (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.
Продолжение следует.