Trolley Robot 2.0. Partie 2. Gestion dans rviz et sans éléments de beauté dans rviz

Dans le dernier article sur le chariot domestique autonome 2.0, nous avons réussi à travailler sur la façon d'améliorer l'odomètre d'un robot à petit budget, de réaliser la construction d'une carte de pièce 2D acceptable en utilisant des algorithmes de slam et un lidar abordable, de clarifier d'autres problèmes lors de la construction du projet. Cette fois, voyons comment fonctionne la navigation autonome dans rviz, de quoi est-il responsable, nous allons implémenter des programmes qui vous permettront de quitter rviz. Considérez également certains des «éléments de beauté» de rviz qui facilitent la vie de la robotique ROS.

image

Commençons par la pratique.


La tâche du robot : voyager indépendamment du point A au point B, ou mieux, immédiatement à plusieurs points B.

On suppose que les scripts (nœuds) du projet linorobot autour desquels le projet est construit seront lancés sur le robot, et la visualisation de ce qui se passe et le contrôle sera effectué à partir d'un PC externe, machine virtuelle (ci-après dénommée "PC").

Dans mon cas, à partir du logiciel sur le chariot, dont le cœur est la carte unique Raspberry Pi 3b, il est installé: Ubuntu 16.04, ROS Kinetic, sur un PC: Ubuntu 18.04, ROS Melodic.

Avant d'aller quelque part, vous devez définir les dimensions du robot - longueur et largeur.
Pour ce faire, comme indiqué sur la page du projet, vous devez saisir les valeurs appropriées:

roscd linorobot/param/navigation
nano costmap_common_params.yaml

Paramètres d'empreinte corrects:

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

où x = longueur du robot / 2 et y = largeur / 2
* Idéalement, pour le robot, vous pouvez dessiner un modèle urdf, comme précédemment dans le projet précédent du chariot robot .

Cependant, si cela n'est pas fait, la navigation ne souffrira pas beaucoup, mais au lieu du modèle du robot, des «axes TF» se déplaceront sur la carte. Comment améliorer un peu la situation avec la représentation visuelle du robot sur la carte et en même temps ne pas perdre de temps à construire le modèle urdf, nous parlerons plus loin.

Nous commençons un environnement


Sur le robot, exécutez dans deux terminaux différents:

roslaunch linorobot bringup.launch
roslaunch linorobot navigate.launch

Sur le PC, exécutez le shell visuel rviz:
roscd lino_visualize/rviz
rviz -d navigate.rviz

Si tout s'est bien passé, sur le PC, vous pouvez observer l'emplacement du robot dans rviz: les



axes TF sont visibles dans rviz, ainsi que l'option Poligon, qui peut être ajoutée dans le panneau Affichages. Polygone - ce ne sont que les dimensions de l'empreinte du robot qui ont été introduites au début, ce qui vous permet de voir les dimensions du robot. * Les axes TF sont visibles depuis les axes de l'image: TF-map - map et TF-base-link - le robot lui-même, les axes restants sont coupés en rviz afin de ne pas fusionner les uns avec les autres.

Les données du lidar ne coïncident pas légèrement avec les murs sur la carte, le robot n'est pas localisé.

Si vous quittez cette position, l'algorithme essaiera de s'ajuster, mais vous pouvez également l'aider en indiquant la position de départ directement dans rviz en utilisant le bouton «Estimation de pose 2d»:



Ensuite, dans la fenêtre de visualisation rviz sur la carte, indiquez l'emplacement initial et la direction du robot avec une flèche verte lourde - pose initiale (vous pouvez le faire plusieurs fois):



Après avoir clarifié la position, base_link se déplacera légèrement par rapport à la carte:



la précision absolue de la coïncidence des bordures du lidar avec la carte lors du réglage manuel de la position initiale ne correspond pas nécessaire, l'algorithme corrigera alors tout seul.

Avant de partir quand même


A chaque fois, mettez en rviz la position de départ, si au sol le robot repart du même endroit occupation so-so. Par conséquent, nous utiliserons le service (rappelez-vous qu'en plus des sujets dans ROS il y a des services), lors de l'accès, il lira "l'emplacement du robot sur la carte", puis nous utiliserons ces données.

Créez un script sur le PC (peu importe à quel endroit)

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.


Après le démarrage, le script dans le terminal n'affiche rien et attend tranquillement son accès. Étant donné que dans rviz en utilisant l'interface graphique, nous avons récemment défini la position initiale du robot, vous pouvez vous référer à notre nouveau script et découvrir cette position. Sans fermer le terminal avec le script (comme les terminaux avec nœuds précédemment lancés), dans un autre terminal, nous exécuterons:

rosservice call /get_pose_service

Après avoir exécuté la requête dans le terminal avec le script, get_pose.py affichera la position du robot:



Enregistrez ces indicateurs et ajoutez-les aux paramètres du nœud amcl sur le robot, afin que le robot démarre à chaque fois à partir de la position spécifiée:

nano linorobot/launch/include/amcl.launch

Ajouter
paramètres
<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"/>


Maintenant au démarrage, le robot démarrera déjà à partir d'une certaine position, et il n'aura pas non plus besoin d'être réglé manuellement dans rviz:



Enfin aller


Redémarrez les commandes pour le robot et le PC données au début et regardez le robot dans rviz.

Nous utiliserons rviz «Objectif NAV 2D» pour envoyer le robot là où l’âme le souhaite:



comme vous pouvez le voir, le robot peut non seulement indiquer la distance du voyage, mais aussi la direction du mouvement.
Dans l'exemple ci-dessus, nous avons roulé un peu en avant, nous nous sommes retournés et sommes revenus.

Dans rviz, les essaims de particules Amcl «flèches vertes» familières sont également visibles, qui indiquent où, selon le programme, le robot est situé. Après le voyage, leur concentration s'approche de l'emplacement réel du robot, le nombre total diminue. Le robot se déplace assez rapidement - 0,3 m / s, donc le programme nécessite plus de temps pour localiser le robot.

Naviguer dans rviz en utilisant le «Objectif NAV 2D» pour comprendre jusqu'où et dans quelle direction le robot se déplacera est pratique. Mais encore plus pratique est un voyage sans la participation de l'opérateur à rviz.

Par conséquent, nous mettons en évidence sur la carte les points d'intérêt où nous irons. Pour ce faire, déplacez le robot aux points souhaités sur la carte à l'aide de "Objectif NAV 2D". À ces points, il accède au service get_pose.py. Ensuite, les coordonnées de tous les points seront transférées au programme de voyage.

Pourquoi ne pas simplement transférer le robot dans vos bras vers des points d'intérêt? Le truc est "Essaim de particules Amcl", l'algorithme qui les construit. Même si le robot est au point d'intérêt et que sa position est spécifiée manuellement, une variante de son emplacement sera «pulvérisée» sur la zone autour du point d'intérêt lui-même. Par conséquent, vous devez encore y aller.

Commençons par le point nommé «couloir»:



Nous enregistrerons la pose lorsque le robot est au point souhaité en utilisant un appel à un service précédemment créé:

rosservice call /get_pose_service

Ensuite - «l'entrée»:



Et le dernier point - «la cuisine»:



Il est maintenant temps de créer un programme de voyage -

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


* Dans GoalPoints, nous ajouterons les coordonnées précédemment enregistrées des points de trajet. Il y a une explication dans le code dans quel ordre vont les paramètres: pose x, y, x; orientation: x, y, z, w - [(5.31514576482, 1.36578380326, 0.0), (0.0, 0.0, -0.498454937648, 0.866915610157)]

Exécutez le script MoveTBtoGoalPoints.py en parallèle avec les nœuds précédemment exécutés sur le robot et le PC, voyez le résultat:



Comme vous pouvez le voir , le robot a réussi à se rendre dans le couloir et sur le chemin de la cuisine s'est vu dans le miroir et s'est coincé. Eh bien avec des lidars lors de la rencontre avec des miroirs, malheureusement.

Ce qui peut être fait?

Il est possible d'ajouter des points intermédiaires avant et après les miroirs, de réduire la vitesse du robot, de coller un ruban opaque sur le miroir le long du parcours.

Mais ce sont les prochaines étapes, mais pour l'instant, nous considérerons la tâche de voyager d'un point A à un point B avec et sans l'aide de rviz terminée. Maintenant, en principe, nous n'avons pas besoin d'un PC externe, car le script de voyage nous emmène aux points souhaités sur la carte.

Mais ne nous précipitons pas pour laisser tomber le PC avec rviz, car le projet linorobot ne fonctionne pas correctement "prêt à l'emploi", et vous devez "travailler avec un fichier" sur la pile de navigation.

Voici le temps de s'opposer - permettez-moi de dire, où est la navigation autonome, le robot a conduit n pas en avant, latéralement, en arrière, et même conduit dans le miroir?

Eh bien, c’est tout, à l’exception de certains détails qui distinguent ROS.

Regardons ces détails.

Supposons qu'un citoyen irresponsable ait laissé un objet inanimé le long du chemin du robot. Cet élément ne figure pas sur la carte qui a été remise au robot pour la navigation:



que va-t-il se passer?

Voyons voir:



Comme vous pouvez le voir, le lidar a vu un obstacle, l'algorithme l'a marqué sur la carte et a ordonné un détour.

Enfin, ajoutez de la beauté à rviz


L'éditeur visuel rviz possède des plugins intéressants qui ne sont pas installés par défaut. Leur utilisation peut ajouter beaucoup de points intéressants lorsque vous travaillez avec rviz.

Le référentiel avec les plugins est ici .

Installez des plugins sur notre PC dans notre environnement:

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

Après l'installation dans rviz, des plugins supplémentaires devraient apparaître.

Maintenant, ajoutons, par exemple, les icônes:



Dans le dossier my_rviz_markers / samples, créez 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()


Rendons-le exécutable:

sudo chmod +x pictogram_array_objects_demo.py

Dans le dossier my_rviz_markers / launch, créez un lancement à lancer:

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

Cela commence comme d'habitude (tous les autres nœuds à ce moment devraient également fonctionner):

roslaunch my_rviz_markers pictogram_array_objects_demo.launch

Placer le marqueur sur la carte


Les marqueurs peuvent être utiles pour comprendre où se trouve le point sur la carte.

Comme dans l'exemple précédent, créez un script dans le dossier 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


Dans le dossier my_rviz_markers / launch, créez un lancement à lancer:

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


Courir:

roslaunch my_rviz_markers basic_marker.launch

Ajouter à rviz:



Ajouter des graphiques


Qui montrera la distance et l'angle entre les deux cadres:



Dans le dossier my_rviz_markers / launch, créez un lancement à lancer:

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


Dans le dossier my_rviz_markers / samples, créez les 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()


Le deuxième script est

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


Cela commence comme ceci (on pense que tous les autres nœuds principaux et rviz sont déjà en cours d'exécution):

roslaunch linorobot overlay_meny_my.launch
python tf_haro_to_object_listener.py base_link map

Nous verrons l'angle et la distance du cadre base_link par rapport à la carte.

À suivre.

All Articles