Get best of the week

Trolley Robot 2.0. Teil 2. Management in rviz und ohne. Elemente der Schönheit in rviz

Im letzten Artikel über den autonomen Heimwagen 2.0 haben wir es geschafft, den Kilometerzähler eines Budgetroboters zu verbessern, eine akzeptable 2D-Raumkarte mit Slam-Algorithmen und einem erschwinglichen Lidar zu erstellen und andere Probleme beim Erstellen des Projekts zu klären. Dieses Mal werden wir sehen, wie die autonome Navigation in rviz funktioniert, wofür wir verantwortlich sind. Wir werden Programme implementieren, mit denen Sie rviz verlassen können. Betrachten Sie auch einige der „Schönheitselemente“ von rviz, die der ROS-Robotik das Leben erleichtern.

Bild

Beginnen wir mit dem Üben.


Die Aufgabe des Roboters : unabhängig von Punkt A nach Punkt B oder besser sofort zu mehreren Punkten B zu fahren.

Es wird davon ausgegangen, dass die Skripte (Knoten) des Linorobot-Projekts, um das das Projekt erstellt wird, auf dem Roboter gestartet werden und die Visualisierung des Geschehens und die Steuerung durchgeführt werden von einem externen PC, einer virtuellen Maschine (im Folgenden als "PC" bezeichnet).

In meinem Fall wird von der Software auf dem Wagen, deren Herzstück das Himbeer-Pi-3b-Single-Board ist, Folgendes installiert: Ubuntu 16.04, ROS Kinetic, auf einem PC: Ubuntu 18.04, ROS Melodic.

Bevor Sie irgendwohin gehen, müssen Sie die Abmessungen des Roboters einstellen - Länge und Breite.
Dazu müssen Sie, wie auf der Projektseite angegeben, die entsprechenden Werte eingeben:

roscd linorobot/param/navigation
nano costmap_common_params.yaml

Richtige Footprint-Parameter:

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

Dabei ist x = Länge des Roboters / 2 und y = Breite / 2
* Idealerweise können Sie für den Roboter ein Urdf-Modell zeichnen, wie dies zuvor im vorherigen Projekt des Roboterwagens getan wurde .

Wenn dies nicht getan wird, leidet die Navigation nicht viel, aber anstelle des Modells des Roboters bewegen sich „TF-Achsen“ auf der Karte. Wir werden weiter darauf eingehen, wie Sie die Situation mit der visuellen Darstellung des Roboters auf der Karte ein wenig verbessern und gleichzeitig keine Zeit mit der Erstellung des Urdf-Modells verschwenden können.

Wir schaffen eine Umgebung


Führen Sie auf dem Roboter zwei verschiedene Terminals aus:

roslaunch linorobot bringup.launch
roslaunch linorobot navigate.launch

Führen Sie auf dem PC die rviz Visual Shell aus:
roscd lino_visualize/rviz
rviz -d navigate.rviz

Wenn alles gut gegangen ist, können Sie auf dem PC die Position des Roboters in rviz beobachten:



TF-Achsen sind in rviz sichtbar, sowie die Option Poligon, die im Anzeigefeld hinzugefügt werden kann. Polygon - Dies sind nur die Footprint-Größen des Roboters, die zu Beginn eingeführt wurden, sodass Sie die Abmessungen des Roboters sehen können. * TF-Achsen sind von den Achsen im Bild aus sichtbar: TF-Map - Map und TF-Base-Link - der Roboter selbst, die verbleibenden Achsen werden in Rviz stummgeschaltet, um nicht miteinander zu verschmelzen.

Die Daten vom Lidar stimmen geringfügig nicht mit den Wänden auf der Karte überein, der Roboter ist nicht lokalisiert.

Wenn Sie von dieser Position aus gehen, versucht der Algorithmus, sich anzupassen. Sie können ihm jedoch auch helfen, indem Sie die Startposition direkt in rviz mit der Schaltfläche „2D-Posenschätzung“ angeben:



Geben Sie als Nächstes im rviz-Visualisierungsfenster auf der Karte die anfängliche Position und Richtung des Roboters mit einer kräftigen grünen Pfeil-Anfangspose an (Sie können dies viele Male tun):



Nach Klärung der Position verschiebt sich base_link relativ zur Karte geringfügig: Die



absolute Genauigkeit der Übereinstimmung der Ränder vom Lidar mit der Karte beim manuellen Anpassen der Anfangsposition nicht benötigt, korrigiert der Algorithmus dann alles selbst.

Bevor du trotzdem gehst


Setzen Sie jedes Mal die Startposition ein, wenn der Roboter am Boden von derselben Stelle aus startet. Daher werden wir den Dienst verwenden (denken Sie daran, dass es zusätzlich zu den Themen in ROS Dienste gibt). Beim Zugriff wird der "Standort des Roboters auf der Karte" gelesen und diese Daten werden dann verwendet.

Erstellen Sie ein Skript auf dem PC (es spielt keine Rolle, an welcher Stelle)

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.


Nach dem Start zeigt das Skript im Terminal nichts an und wartet leise auf den Zugriff. Da wir in rviz über die grafische Oberfläche kürzlich die Anfangsposition des Roboters festgelegt haben, können Sie sich auf unser neues Skript beziehen und diese Position herausfinden. Ohne das Terminal mit dem Skript zu schließen (wie zuvor gestartete Terminals mit Knoten), werden wir in einem anderen Terminal Folgendes ausführen:

rosservice call /get_pose_service

Nachdem die Anforderung im Terminal mit dem Skript ausgeführt wurde, zeigt get_pose.py die Position des Roboters an:



Speichern Sie diese Indikatoren und fügen Sie sie zu den Einstellungen des amcl-Knotens auf dem Roboter hinzu, sodass der Roboter jedes Mal an der angegebenen Position startet:

nano linorobot/launch/include/amcl.launch

Hinzufügen
Parameter
<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"/>


Jetzt startet der Roboter beim Start bereits an einer bestimmten Position und muss auch nicht manuell in rviz eingestellt werden:



Endlich geht es los


Starten Sie die zu Beginn angegebenen Befehle für den Roboter und den PC neu und sehen Sie sich den Roboter in rviz an.

Wir werden das "2D NAV Goal" -Rviz verwenden, um den Roboter zu senden, wohin die Seele es wünscht:



Wie Sie sehen können, kann der Roboter nicht nur die Entfernung der Reise, sondern auch die Bewegungsrichtung anzeigen.
Im obigen Beispiel fuhren wir ein wenig vorwärts, drehten uns um und gingen zurück.

In rviz sind auch die bekannten Amcl-Partikelschwärme „grüne Pfeile“ sichtbar, die zeigen, wo sich der Roboter laut Programm befindet. Nach der Fahrt nähert sich ihre Konzentration dem realen Standort des Roboters, die Gesamtzahl nimmt ab. Der Roboter fährt schnell genug - 0,3 m / s, sodass das Programm mehr Zeit benötigt, um den Roboter zu lokalisieren.

Das Navigieren in rviz mithilfe des „2D-NAV-Ziels“, um zu verstehen, wie weit und in welche Richtung sich der Roboter bewegt, ist praktisch. Noch bequemer ist jedoch eine Reise ohne Beteiligung des Betreibers an rviz.

Daher markieren wir auf der Karte die Punkte von Interesse, an die wir gehen werden. Bewegen Sie dazu den Roboter mit "2D NAV Goal" zu den gewünschten Punkten auf der Karte. An diesen Stellen wird auf den Dienst get_pose.py zugegriffen. Als nächstes werden die Koordinaten aller Punkte in das Reiseprogramm übertragen.

Warum nicht einfach den Roboter in Ihren Armen zu Sehenswürdigkeiten bringen? Die Sache ist "Amcl Partikelschwarm", der Algorithmus, der sie erstellt. Selbst wenn sich der Roboter am interessierenden Punkt befindet und seine Position manuell festgelegt wird, wird eine Variante seiner Position über den Bereich um den interessierenden Punkt selbst "gesprüht". Deshalb musst du noch gehen.

Beginnen wir mit dem Codenamen „Korridor“:



Wir speichern die Pose, wenn sich der Roboter am gewünschten Punkt befindet, indem wir einen zuvor erstellten Dienst anrufen:

rosservice call /get_pose_service

Weiter - der "Eingang":



Und der letzte Punkt - "die Küche":



Jetzt ist es Zeit, ein Reiseprogramm zu erstellen -

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


* In GoalPoints fügen wir die zuvor gespeicherten Koordinaten der Trip Points hinzu. Der Code enthält eine Erklärung, in welcher Reihenfolge die Parameter ablaufen: Pose x, y, x; Ausrichtung: x, y, z, w - [(5.31514576482, 1.36578380326, 0.0), (0.0, 0.0, -0.498454937648, 0.866915610157)]

Führen Sie das Skript MoveTBtoGoalPoints.py parallel zu den zuvor gestarteten Knoten auf dem Roboter und dem PC aus. Siehe das Ergebnis:



Wie Sie sehen können Der Roboter fuhr erfolgreich zum Flur und sah sich auf dem Weg in die Küche im Spiegel und blieb stecken. Gut mit Lidars beim Treffen mit Spiegeln, leider so.

Was kann getan werden?

Es ist möglich, Zwischenpunkte vor und nach den Spiegeln hinzuzufügen, die Geschwindigkeit des Roboters zu verringern und entlang der Route ein undurchsichtiges Klebeband auf den Spiegel zu kleben.

Dies sind jedoch die nächsten Schritte, aber wir werden zunächst die Aufgabe in Betracht ziehen, mit und ohne die Hilfe von rviz von Punkt A nach Punkt B zu reisen. Im Prinzip brauchen wir jetzt keinen externen PC, weil Das Reiseskript führt uns zu den gewünschten Punkten auf der Karte.

Aber lassen Sie uns nicht eilen, um den PC zusammen mit rviz fallen zu lassen, da das Linorobot-Projekt "out of the box" nicht richtig funktioniert und Sie "mit einer Datei" auf dem Navigationsstapel arbeiten müssen.

Hier ist die Zeit zum Einspruch - lassen Sie mich sagen, wo ist die autonome Navigation, der Roboter fuhr n Schritte vorwärts, seitwärts, rückwärts und sogar in den Spiegel?

Nun, das ist alles, bis auf einige Details, die ROS auszeichnen.

Schauen wir uns diese Details an.

Angenommen, ein verantwortungsloser Bürger hat ein lebloses Objekt auf dem Weg des Roboters zurückgelassen. Dieser Gegenstand befindet sich nicht auf der Karte, die dem Roboter zur Navigation übergeben wurde:



Was wird passieren?

Mal sehen:



Wie Sie sehen, hat der Lidar ein Hindernis gesehen, der Algorithmus hat es auf der Karte markiert und einen Umweg angeordnet.

Fügen Sie schließlich etwas Schönheit zu rviz hinzu


Der visuelle Editor von rviz verfügt über interessante Plugins, die nicht standardmäßig installiert sind. Ihre Verwendung kann bei der Arbeit mit rviz viele interessante Punkte hinzufügen.

Das Repository mit Plugins ist hier .

Installieren Sie Plugins auf unserem PC in unserer Umgebung:

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

Nach der Installation in rviz sollten zusätzliche Plugins angezeigt werden. Fügen

wir nun beispielsweise die Symbole hinzu:



Erstellen Sie im Ordner my_rviz_markers / samples ein Skript:

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


Machen wir es ausführbar:

sudo chmod +x pictogram_array_objects_demo.py

Erstellen Sie im Ordner my_rviz_markers / launch einen Start zum Starten:

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

Es beginnt wie gewohnt (alle anderen Knoten zu diesem Zeitpunkt sollten ebenfalls funktionieren):

roslaunch my_rviz_markers pictogram_array_objects_demo.launch

Setzen Sie den Marker auf die Karte


Markierungen können hilfreich sein, um zu verstehen, wo sich der Punkt auf der Karte befindet.

Erstellen Sie wie im vorherigen Beispiel ein Skript im Ordner 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


Erstellen Sie im Ordner my_rviz_markers / launch einen Start zum Starten:

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


Lauf:

roslaunch my_rviz_markers basic_marker.launch

Zu rviz hinzufügen:



Fügen Sie Grafiken hinzu


Hier werden der Abstand und der Winkel zwischen den beiden Frames



angezeigt : Erstellen Sie im Ordner my_rviz_markers / launch einen Start zum Starten:

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


Erstellen Sie im Ordner my_rviz_markers / samples die folgenden Skripte:

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


Das zweite Skript ist

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


Es beginnt so (es wird angenommen, dass alle anderen Hauptknoten und rviz bereits ausgeführt werden):

roslaunch linorobot overlay_meny_my.launch
python tf_haro_to_object_listener.py base_link map

Wir werden den Winkel und die Entfernung des base_link-Frames relativ zur Karte sehen.

Fortsetzung folgt.

More articles:


All Articles