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.
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
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()
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
Ajouterparamè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
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()
* 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
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()
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
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
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
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()
Le deuxième script esttf_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()
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.