🐭 👂🏾 ✴️ Trolley Robot 2.0. Parte 2. Gestión en rviz y sin ella Elementos de belleza en rviz 🤗 🖇️ 📼

En el último artículo sobre el carro de hogar autónomo 2.0, logramos trabajar en cómo mejorar el odómetro de un robot económico, lograr la construcción de un mapa de habitación 2D aceptable utilizando algoritmos de golpe y un lidar asequible, aclarar otros problemas al construir el proyecto. Esta vez, veamos cómo funciona la navegación autónoma en rviz, de qué es responsable, implementaremos programas que le permitan salir de rviz. Considere también algunos de los "elementos de belleza" de rviz que hacen la vida más fácil para la robótica ROS.

imagen

Comencemos con la práctica.

La tarea del robot : viajar independientemente del punto A al punto B, o mejor, inmediatamente a varios puntos B.

Se supone que los scripts (nodos) del proyecto linorobot alrededor del cual se construye el proyecto se lanzarán en el robot, y se visualizará lo que está sucediendo y el control se llevará a cabo. desde una PC externa, máquina virtual (en adelante denominada "PC").

En mi caso, desde el software en el carrito, cuyo corazón es la placa única raspberry pi 3b, está instalado: Ubuntu 16.04, ROS Kinetic, en una PC: Ubuntu 18.04, ROS Melodic.

Antes de ir a algún lugar, debe establecer las dimensiones del robot: longitud y ancho.
Para hacer esto, como se indica en la página del proyecto, debe ingresar los valores apropiados:

roscd linorobot/param/navigation
nano costmap_common_params.yaml

Parámetros correctos de la huella:

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

donde x = longitud del robot / 2 e y = ancho / 2
* Idealmente, para el robot, puede dibujar un modelo urdf, como se hizo anteriormente en el proyecto anterior del carro del robot .

Sin embargo, si esto no se hace, la navegación no sufrirá mucho, pero en lugar del modelo del robot, los "ejes TF" viajarán en el mapa. Cómo mejorar un poco la situación con la representación visual del robot en el mapa y, al mismo tiempo, no perder el tiempo construyendo el modelo urdf, hablaremos más.

Comenzamos un ambiente

En el robot, ejecute en dos terminales diferentes:

roslaunch linorobot bringup.launch
roslaunch linorobot navigate.launch

En la PC, ejecute el shell visual rviz:

roscd lino_visualize/rviz
rviz -d navigate.rviz

Si todo salió bien, en la PC puede observar la ubicación del robot en rviz: los

ejes TF son visibles en rviz, así como la opción Poligon, que se puede agregar en el panel de pantallas. Polígono: estos son solo los tamaños de huella del robot que se introdujeron al principio, lo que le permite ver las dimensiones del robot. * Los ejes TF son visibles desde los ejes en la imagen: TF-map - mapa y TF-base-link - el robot en sí, los ejes restantes se silencian en rviz para no fusionarse entre sí.

Los datos del LIDAR no coinciden ligeramente con las paredes en el mapa, el robot no está localizado.

Si sale de esta posición, el algoritmo intentará ajustarse, pero también puede ayudarlo indicando la posición inicial directamente en rviz usando el botón "Estimación de pose 2d":

A continuación, en la ventana de visualización de rviz en el mapa, indique la ubicación inicial y la dirección del robot con una fuerte postura inicial de flecha verde (puede hacer esto muchas veces):

después de aclarar la posición, base_link cambiará ligeramente en relación con el mapa:

la precisión absoluta de la coincidencia de los bordes del lidar con el mapa cuando se ajusta manualmente la posición inicial no necesario, el algoritmo corregirá todo por sí mismo.

Antes de que te vayas de todos modos

Cada vez, coloque en posición de inicio, si en el suelo el robot comienza desde el mismo lugar, así que la ocupación. Por lo tanto, utilizaremos el servicio (recuerde que además de los temas en ROS hay servicios), cuando se acceda, leerá la "ubicación del robot en el mapa", luego usaremos estos datos.

Cree un script en la PC (no importa en qué lugar)

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.

Después de comenzar, el script en la terminal no muestra nada y espera silenciosamente a que se acceda a él. Dado que en rviz usando la interfaz gráfica recientemente configuramos la posición inicial del robot, puede consultar nuestro nuevo script y descubrir esta posición. Sin cerrar la terminal con el script (como las terminales lanzadas previamente con nodos), en otra terminal ejecutaremos:

rosservice call /get_pose_service

Después de ejecutar la solicitud en el terminal con el script, get_pose.py mostrará la posición del robot:

guarde estos indicadores y agréguelos a la configuración del nodo amcl en el robot, de modo que el robot comience desde la posición especificada cada vez:

nano linorobot/launch/include/amcl.launch

Añadir

pará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"/>

Ahora, al inicio, el robot comenzará desde una determinada posición y tampoco será necesario configurarlo manualmente en rviz:

Finalmente yendo

Reinicie los comandos para el robot y la PC dados al principio y mire el robot en rviz.

Usaremos el rviz "2D NAV Goal" para enviar el robot a donde el alma lo desee:

como puede ver, el robot no solo puede indicar la distancia del viaje, sino también la dirección del movimiento.
En el ejemplo anterior, avanzamos un poco, giramos y volvimos.

En rviz, también son visibles las conocidas “flechas verdes” enjambre de partículas Amcl, que muestran dónde, según el programa, se encuentra el robot. Después del viaje, su concentración se aproxima a la ubicación real del robot, el número total disminuye. El robot viaja lo suficientemente rápido: 0.3 m / s, por lo que el programa requiere más tiempo para localizar el robot.

Navegar en rviz usando el "Objetivo 2D NAV" para comprender qué tan lejos y en qué dirección viajará el robot es conveniente. Pero aún más conveniente es un viaje sin participación del operador en rviz.

Por lo tanto, destacamos en el mapa los puntos de interés a donde iremos. Para hacer esto, mueva el robot a los puntos deseados en el mapa usando "Objetivo NAV 2D". En estos puntos accederá al servicio get_pose.py. A continuación, las coordenadas de todos los puntos se transferirán al programa de viaje.

¿Por qué no solo transfieres el robot en tus brazos a puntos de interés? La cosa es "enjambre de partículas Amcl", el algoritmo que los construye. Incluso si el robot está en el punto de interés, y su posición se especifica manualmente, una variante de su ubicación se "rociará" sobre el área alrededor del punto de interés. Por lo tanto, todavía tienes que ir.

Comencemos desde el punto con nombre en código "corredor":

Guardaremos la pose cuando el robot esté en el punto deseado utilizando una llamada a un servicio creado previamente:

rosservice call /get_pose_service

A continuación, la “entrada”:

y el punto final: “la cocina”:

ahora es el momento de crear un programa de viaje.

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")

* En GoalPoints, agregaremos las coordenadas guardadas previamente de los puntos de viaje. Hay una explicación en el código en qué orden van los parámetros: pose x, y, x; orientación: x, y, z, w - [(5.31514576482, 1.36578380326, 0.0), (0.0, 0.0, -0.498454937648, 0.866915610157)]

Ejecute el script MoveTBtoGoalPoints.py en paralelo con los nodos que se ejecutan previamente en el robot y la PC, vea el resultado:

Como puede ver , el robot condujo con éxito al pasillo y de camino a la cocina se vio en el espejo y se quedó atascado. Bueno, con lidares cuando se encuentran con espejos, desafortunadamente.

¿Qué se puede hacer?

Es posible agregar puntos intermedios antes y después de los espejos, reducir la velocidad del robot, pegar una cinta opaca en el espejo a lo largo de la ruta.

Pero estos son los próximos pasos, pero por ahora consideraremos la tarea de viajar del punto A al punto B con y sin la ayuda de rviz completada. Ahora, en principio, no necesitamos una PC externa, porque El guión de viaje nos lleva a los puntos deseados en el mapa.

Pero no nos apresuremos a soltar la PC junto con rviz, ya que el proyecto linorobot no funciona correctamente "fuera de la caja", y debe "trabajar con un archivo" en la pila de navegación.

Este es el momento de objetar: déjenme decir, ¿dónde está la navegación autónoma, el robot avanzó y dio pasos hacia adelante, de lado, hacia atrás e incluso se dirigió al espejo?

Bueno, eso es todo, excepto algunos de los detalles que hacen que ROS se destaque.

Veamos estos detalles.

Supongamos que un ciudadano irresponsable deja un objeto inanimado a lo largo del camino del robot. Este elemento no está en el mapa que se le entregó al robot para la navegación:

¿qué sucederá?

Veamos:

como puede ver, el LIDAR vio un obstáculo, el algoritmo lo marcó en el mapa y ordenó un desvío.

Finalmente, agrega un poco de belleza a rviz

El editor visual rviz tiene complementos interesantes que no están instalados por defecto. Su uso puede agregar muchos puntos interesantes al trabajar con rviz.

El repositorio con complementos está aquí .

Instale complementos en nuestra PC en nuestro entorno:

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

Después de la instalación en rviz, deben aparecer complementos adicionales.

Ahora, agreguemos, por ejemplo, los íconos:

En la carpeta my_rviz_markers / samples, cree un script:

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()

Hagámoslo ejecutable:

sudo chmod +x pictogram_array_objects_demo.py

En la carpeta my_rviz_markers / launch, cree un inicio para iniciar:

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

Comienza como de costumbre (todos los demás nodos en este momento también deberían funcionar):

roslaunch my_rviz_markers pictogram_array_objects_demo.launch

Pon el marcador en el mapa

Los marcadores pueden ser útiles para comprender dónde está el punto en el mapa.

Como en el ejemplo anterior, cree un script en la carpeta 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

En la carpeta my_rviz_markers / launch, cree un inicio para iniciar:

basic_marker.launch

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

Correr:

roslaunch my_rviz_markers basic_marker.launch

Añadir a rviz:

Agregar gráficos

Que mostrará la distancia y el ángulo entre los dos cuadros:

en la carpeta my_rviz_markers / launch, cree un lanzamiento para lanzar:

overlay_meny_my.launch overlay_menu_my.launch

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

En la carpeta my_rviz_markers / samples, cree los scripts:

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()

El segundo guión es

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()

Comienza así (se cree que todos los demás nodos principales y rviz ya se están ejecutando):

roslaunch linorobot overlay_meny_my.launch
python tf_haro_to_object_listener.py base_link map

Veremos el ángulo y la distancia del marco base_link en relación con el mapa.

Continuará.

Trolley Robot 2.0. Parte 2. Gestión en rviz y sin ella Elementos de belleza en rviz