No último artigo sobre o transporte doméstico autônomo 2.0, conseguimos trabalhar em como melhorar o odômetro de um robô de orçamento, conseguir a construção de um mapa de sala 2D aceitável usando algoritmos slam e um lidar acessível, esclarecer outras questões ao construir o projeto. Desta vez, vamos ver como a navegação autônoma funciona no rviz, o que é responsável, implementaremos programas que permitem que você saia do rviz. Considere também alguns dos "elementos de beleza" do rviz que facilitam a vida da robótica ROS.
Vamos começar com a prática.
A tarefa do robô : viajar independentemente do ponto A ao ponto B e, de preferência, a vários pontos B.Supõe-se que os scripts (nós) do projeto linorobot em torno do qual o projeto é construído serão lançados no robô, e a visualização do que está acontecendo e do controle será realizada. de um PC externo, máquina virtual (doravante denominado "PC").No meu caso, a partir do software no carrinho, cujo coração é a placa única raspberry pi 3b, ele está instalado: Ubuntu 16.04, ROS Kinetic, em um PC: Ubuntu 18.04, ROS Melodic.Antes de ir a algum lugar, você deve definir as dimensões do robô - comprimento e largura.Para fazer isso, conforme indicado na página do projeto, você deve inserir os valores apropriados:roscd linorobot/param/navigation
nano costmap_common_params.yaml
Parâmetros de pegada corretos:footprint: [[-x, -y], [-x, y], [x, y], [x, -y]]
onde x = comprimento do robô / 2 e y = largura / 2* Idealmente, para o robô, você pode desenhar um modelo de urdf, como foi feito anteriormente no projeto anterior do carrinho do robô .No entanto, se isso não for feito, a navegação não sofrerá muito, mas, em vez do modelo do robô, os “eixos TF” trafegam no mapa. Como melhorar a situação com a representação visual do robô no mapa e, ao mesmo tempo, não perder tempo construindo o modelo urdf, falaremos mais adiante.Começamos um ambiente
No robô, execute em dois terminais diferentes:roslaunch linorobot bringup.launch
roslaunch linorobot navigate.launch
No PC, execute o shell visual rviz:roscd lino_visualize/rviz
rviz -d navigate.rviz
Se tudo der certo, no PC você pode observar a localização do robô no rviz: os
eixos TF são visíveis no rviz, bem como a opção Poligon, que pode ser adicionada no painel Monitores. Polígono - estes são apenas os tamanhos de pegada do robô que foram introduzidos no início, o que permite ver as dimensões do robô. * Os eixos TF são visíveis a partir dos eixos na imagem: TF-map - map e TF-base-link - o próprio robô, os demais eixos são silenciados no rviz para não se fundirem.Os dados do lidar ligeiramente não coincidem com as paredes no mapa, o robô não está localizado.Se você sair dessa posição, o algoritmo tentará ajustar, mas você também poderá ajudá-lo indicando a posição inicial diretamente no rviz usando o botão “estimativa de pose em 2d”:
Em seguida, na janela de visualização do rviz no mapa, indique a localização e a direção iniciais do robô com uma pose de seta verde pesada (você pode fazer isso várias vezes):
Depois de esclarecer a posição, o base_link mudará ligeiramente em relação ao mapa:
a precisão absoluta da coincidência das bordas do lidar com o mapa ao ajustar manualmente a posição inicial não necessário, o algoritmo corrigirá tudo sozinho.Antes de você ir de qualquer maneira
Cada vez, coloque em posição a posição inicial, se no chão o robô começar do mesmo local, mais ou menos ocupação. Portanto, usaremos o serviço (lembre-se de que, além dos tópicos no ROS, existem serviços), quando acessado, ele lerá a "localização do robô no mapa", depois usaremos esses dados.Crie um script no PC (não importa em que lugar)get_pose.py
import rospy
from std_srvs.srv import Empty, EmptyResponse
from geometry_msgs.msg import PoseWithCovarianceStamped, Pose
robot_pose = Pose()
def service_callback(request):
print ("Robot Pose:")
print (robot_pose)
return 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)
sub_pose = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, sub_callback)
rospy.spin()
Após o início, o script no terminal não exibe nada e espera silenciosamente que seja acessado. Como no rviz, usando a interface gráfica, definimos recentemente a posição inicial do robô, você pode consultar nosso novo script e descobrir essa posição. Sem fechar o terminal com o script (como os terminais lançados anteriormente com nós), em outro terminal executaremos:rosservice call /get_pose_service
Após executar a solicitação no terminal com o script, get_pose.py exibirá a posição do robô:
Salve estes indicadores e adicione-os às configurações do nó amcl no robô, para que o robô inicie da posição especificada a cada vez:nano linorobot/launch/include/amcl.launch
Adicionarparâmetros<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"/>
Agora, na inicialização, o robô já começará a partir de uma determinada posição e também não precisará ser definido manualmente no rviz:
Finalmente indo
Reinicie os comandos para o robô e o PC fornecidos no início e observe o robô em rviz.Usaremos o rviz "2D NAV Goal" para enviar o robô para onde a alma desejar:
como você pode ver, o robô pode não apenas indicar a distância da viagem, mas também a direção do movimento.No exemplo acima, avançamos um pouco para a frente, nos viramos e voltamos.No rviz, também são visíveis as conhecidas “setas verdes” enxame de partículas Amcl, que mostram onde, de acordo com o programa, o robô está localizado. Após a viagem, sua concentração se aproxima da localização real do robô, o número total diminui. O robô viaja rápido o suficiente - 0,3 m / s, portanto, o programa requer mais tempo para localizar o robô.Navegar no rviz usando o "Objetivo 2D NAV" para entender até que ponto e em qual direção o robô se moverá é conveniente. Ainda mais conveniente é uma viagem sem a participação do operador no rviz.Portanto, destacamos no mapa os pontos de interesse para onde iremos. Para fazer isso, mova o robô para os pontos desejados no mapa usando "Objetivo 2D NAV". Nesses pontos, ele acessará o serviço get_pose.py. Em seguida, as coordenadas de todos os pontos serão transferidas para o programa de viagem.Por que não transferir o robô em seus braços para pontos de interesse? O problema é "enxame de partículas Amcl", o algoritmo que as constrói. Mesmo que o robô esteja no ponto de interesse e sua posição seja especificada manualmente, uma variante de sua localização será "pulverizada" sobre a área em torno do próprio ponto de interesse. Portanto, você ainda precisa ir.Vamos começar a partir do ponto codinome "corredor":
Salvaremos a pose quando o robô estiver no ponto desejado usando uma chamada para um serviço criado anteriormente:rosservice call /get_pose_service
A seguir - a “entrada”:
e o ponto final - “a cozinha”:
agora é hora de criar um programa de viagem -MoveTBtoGoalPoints.py
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
"""
pose x,y,x; orientation: x,y,z,w
"""
GoalPoints = [ [(5.31514576482, 1.36578380326, 0.0), (0.0, 0.0, -0.498454937648, 0.866915610157)],
[(5.77521810772, -1.2464049907, 0.0), (0.0, 0.0, -0.959487358308, 0.281751680114)],
[(2.14926455346, -2.7055208156, 0.0), (0.0, 0.0, -0.99147032254, 0.130332649487)]]
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')
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
client.wait_for_server()
for TBpose in GoalPoints:
TBgoal = assign_goal(TBpose)
client.send_goal(TBgoal)
client.wait_for_result()
* Nos GoalPoints, adicionaremos as coordenadas salvas anteriormente dos pontos de viagem. Há uma explicação no código em que ordem os parâmetros são: pose x, y, x; orientação: x, y, z, w - [(5.31514576482, 1.36578380326, 0.0), (0.0, 0.0, -0.498454937648, 0.866915610157)]Execute o script MoveTBtoGoalPoints.py em paralelo com os nós em execução no robô e no PC, veja o resultado:
Como você pode ver , o robô dirigiu com sucesso para o corredor e, a caminho da cozinha, viu-se no espelho e ficou preso. Bem, com lidares ao encontrar espelhos, infelizmente.O que pode ser feito?É possível adicionar pontos intermediários antes e depois dos espelhos, reduzir a velocidade do robô, colar uma fita opaca no espelho ao longo do percurso.Mas estes são os próximos passos, mas, por enquanto, consideraremos a tarefa de viajar do ponto A ao ponto B com e sem a ajuda do rviz concluída. Agora, em princípio, não precisamos de um PC externo, porque o roteiro da viagem nos leva aos pontos desejados no mapa.Mas não vamos nos apressar em soltar o PC junto com o rviz, pois o projeto linorobot não funciona corretamente "fora da caixa" e você precisa "trabalhar com um arquivo" na pilha de navegação.Aqui está a hora de contestar - digamos, onde está a navegação autônoma, o robô deu n passos para frente, para os lados, para trás e até mesmo para o espelho?Bem, isso é tudo, exceto alguns dos detalhes que fazem o ROS se destacar.Vamos olhar para esses detalhes.Suponha que algum cidadão irresponsável tenha deixado um objeto inanimado ao longo do caminho do robô. Este item não está no mapa que foi entregue ao robô para navegação:
O que acontecerá?Vamos ver:
como você pode ver, o lidar viu um obstáculo, o algoritmo o marcou no mapa e ordenou um desvio.Por fim, adicione um pouco de beleza ao rviz
O editor visual rviz possui plugins interessantes que não são instalados por padrão. Seu uso pode adicionar muitos pontos interessantes ao trabalhar com o rviz.O repositório com plugins está aqui .Instale plugins em nosso PC em nosso ambiente: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
Após a instalação no rviz, plugins adicionais devem aparecer.Agora, vamos adicionar, por exemplo, os ícones:
Na pasta my_rviz_markers / samples, crie um script:pictogram_array_objects_demo.py
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
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()
Vamos torná-lo executável:sudo chmod +x pictogram_array_objects_demo.py
Na pasta my_rviz_markers / launch, crie um lançamento para iniciar:<launch>
<node pkg="my_rviz_markers" type="pictogram_array_objects_demo.py" name="pictogram_array_objects_demo" </node>
</launch>
Inicia como de costume (todos os outros nós neste momento também devem funcionar):roslaunch my_rviz_markers pictogram_array_objects_demo.launch
Coloque o marcador no mapa
Os marcadores podem ser úteis para entender onde o ponto está no mapa.Como no exemplo anterior, crie um script na pasta my_rviz_markers / samples:basic_marker.py
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.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
self.marker_object.color.a = 1.0
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
Na pasta my_rviz_markers / launch, crie um lançamento para iniciar:basic_marker.launch<launch>
<node pkg="my_rviz_markers" type="basic_marker.py" name="basic_marker" />
</launch>
Corre:roslaunch my_rviz_markers basic_marker.launch
Adicionar ao rviz:
Adicionar gráficos
O que mostrará a distância e o ângulo entre os dois quadros:
Na pasta my_rviz_markers / launch, crie um lançamento para iniciar:overlay_meny_my.launch overlay_menu_my.launch<launch>
<node pkg="my_rviz_markers" type="haro_overlay_complete_demo.py" name="overlay_menu" />
</launch>
Na pasta my_rviz_markers / samples, crie os scripts:haro_overlay_complete_demo.py
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)
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_harodistance_demo()
except rospy.ROSInterruptException:
pass
if __name__ == '__main__':
dummy_overlay_demo()
O segundo script étf_haro_to_object_listener.py
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"
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():
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()
Começa assim (acredita-se que todos os outros nós principais e rviz já estejam em execução):roslaunch linorobot overlay_meny_my.launch
python tf_haro_to_object_listener.py base_link map
Veremos o ângulo e a distância do quadro base_link em relação ao mapa.Continua.