Bonjour lecteurs! Nous avons déjà abordé le sujet de la localisation et du SLAM dans un article sur Hector SLAM . Nous continuons notre connaissance des algorithmes de construction de cartes de terrain et de localisation dans ROS. Aujourd'hui, nous allons essayer de construire une carte de la zone sans source d'odométrie, en utilisant uniquement le lidar Hokuyo URG-04LX-UG01 et l'algorithme gmapping et localiser le robot sur la carte construite en utilisant l'algorithme amcl . Laser_scan_matcher nous aidera avec cela . Peu importe, s'il vous plaît, sous le chat.Installation du package laser_scan_matcher
Commençons donc! Pour les expériences, nous utiliserons ROS Indigo, mais vous pouvez utiliser une version différente de ROS (Jade, Kinetic, Hydro). L'installation des packages doit se faire de la même manière (peut-être seulement dans ROS Kinetic certains packages ne seront pas disponibles via apt-get).Le package laser_scan_matcher est un outil d'enregistrement incrémentiel des données laser, qui est implémenté sur la base de la méthode Canonical Scan Matcher, qui peut être lue ici . Vous pouvez en savoir plus sur le package ici . Le package peut être utilisé sans données d'odométrie; il effectue lui-même l'évaluation de l'odométrie.Installez le package:sudo apt-get install ros-indigo-laser-scan-matcher
Essayons la démo:roscore
roslaunch laser_scan_matcher demo.launch
Nous verrons quelque chose de similaire dans rviz:Ici, laser_scan_matcher en action sur les données lidar enregistrées dans un fichier bag. Essayez maintenant le package en direct. Afin de ne pas perdre de temps, nous allons immédiatement l'essayer avec l'algorithme gmapping. Créez un package avec le fichier my_gmapping_launch.launch pour démarrer gmapping:cd ~/catkin_ws/src
catkin_create_pkg my_laser_matcher
cd src/my_laser_matcher
mkdir launch
vim launch/my_gmapping_launch.launch
Copiez le code suivant dans le fichier my_gmapping_launch.launch:Code my_gmapping_launch.launch<?xml version="1.0"?>
<launch>
<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser"
args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /laser 40" />
<node pkg="laser_scan_matcher" type="laser_scan_matcher_node" name="laser_scan_matcher_node" output="screen">
<param name="fixed_frame" value = "odom"/>
<param name="use_odom" value="true"/>
<param name="publish_odom" value = "true"/>
<param name="use_alpha_beta" value="true"/>
<param name="max_iterations" value="10"/>
</node>
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
<param name="map_udpate_interval" value="1.0"/>
<param name="delta" value="0.02"/>
</node>
</launch>
Ici, nous exécutons static_transform_publisher à partir du package tf pour publier la transformation entre les systèmes de coordonnées base_link → laser, les nœuds laser_scan_matcher et slam_gmapping. Le code source du fichier peut être téléchargé ici . Pour utiliser le lidar Hokuyo, nous devons installer le package hokuyo_node ROS :sudo apt-get install ros-indigo-hokuyo-node
Exécutez le nœud getID à partir du package hokuyo_node pour obtenir des informations lidar:rosrun hokuyo_node getID /dev/ttyACM0
Une erreur peut se produire:Error: Failed to open port. Permission denied.
[ERROR] 1263843357.793873000: Exception thrown while opening Hokuyo.
Failed to open port: /dev/ttyACM0. Permission denied (errno = 13). You probably don't have premission to open the port for reading and writing. (in hokuyo::laser::open) You may find further details at http://www.ros.org/wiki/hokuyo_node/Troubleshooting
Dans ce cas, nous devons ajouter des autorisations pour le port / dev / ttyACM0:sudo chmod a+rw /dev/ttyACM0
Exécutez à nouveau getID à partir du package hokuyo_node et obtenez une sortie similaire:Device at /dev/ttyACM0 has ID H0906078
Exécutez maintenant le nœud hokuyo_node:rosrun hokuyo_node hokuyo_node
Enfin, exécutez notre fichier de lancement my_gmapping_launch.launch:roslaunch my_laser_matcher my_gmapping_launch.launch
rosrun rviz rviz
Énumérons les sujets:rostopic list
Parmi les sujets, nous verrons les suivants:/initialpose
/move_base_simple/goal
/odom
/pose2D
...
/imu/data
De cette façon, nous obtenons l'odométrie et la position du robot grâce à laser_scan_matcher.Ajoutez à rviz un affichage de type LaserScan avec la rubrique / scan comme décrit dans l' article . Ajoutez également un affichage pour la Map Map et pour la transformation de TF. Développez la section TF et les cadres à l'intérieur, puis notez les points: odom, map, base_link. Permettez-moi de vous rappeler que ce sont les systèmes de coordonnées du compteur kilométrique, de la carte et du robot, respectivement. N'oubliez pas de définir la valeur / map sur le champ Cadre fixe dans le panneau d'affichage gauche dans la section Options globales.Dans rviz, nous verrons une image similaire:
Ensuite, déplacez simplement le robot dans l'espace pour construire une carte complète de la zone. Nous utilisons l'utilitaire map_saver du package map_server pour enregistrer la carte:rosrun map_server map_saver
Localisation avec amcl
Essayons maintenant de localiser le robot en utilisant l'algorithme amcl. Créez le fichier my_localize.launch dans notre package avec le contenu suivant:Code my_localize.launch<launch>
<param name="/use_sim_time" value="false"/>
<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser"
args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /laser 40" />
<node name="hokuyo" pkg="hokuyo_node" type="hokuyo_node" respawn="false" output="screen">
<param name="calibrate_time" type="bool" value="false"/>
<param name="port" type="string" value="/dev/ttyACM0"/>
<param name="intensity" type="bool" value="false"/>
</node>
<node pkg="laser_scan_matcher" type="laser_scan_matcher_node"
name="laser_scan_matcher_node" output="screen">
<param name="fixed_frame" value = "odom"/>
<param name="use_alpha_beta" value="true"/>
<param name="max_iterations" value="10"/>
</node>
<node name="map_server" pkg="map_server" type="map_server" args="/home/vladimir/catkin_ws/map.yaml"/>
<node pkg="amcl" type="amcl" name="amcl" output="screen" >
<param name="odom_model_type" value="diff"/>
<param name="odom_alpha5" value="0.1"/>
<param name="transform_tolerance" value="0.2" />
<param name="gui_publish_rate" value="10.0"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom"/>
<param name="base_frame_id" type="str" value="base_link" />
<param name="global_frame_id" type="str" value="map" />
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<param name="use_map_topic" value="true" />
<param name="first_map_only" value="true" />
</node>
</launch>
Ici, nous, de la même manière que le lanceur pour gmapping, publions la transformation / laser → / base_link en utilisant le nœud static_transform_publisher, les nœuds hokuyo_node et laser_scan_matcher. Ensuite, nous exécutons map_server pour publier notre carte construite, où en args nous passons le chemin vers la carte dans le fichier yaml. Enfin, démarrez le nœud amcl avec les paramètres. Vous pouvez lire sur les paramètres amcl sur la page d'algorithme officielle .Le code du fichier du lanceur peut également être téléchargé à partir du référentiel github . Exécutez notre fichier de lancement:roslaunch my_laser_matcher my_localize.launch
Passons maintenant à rviz. Définissez la valeur de la carte pour Cadre fixe dans la section Options globales. Énumérons les sujets:rostopic list
De nouveaux sujets apparaîtront dans la liste:...
/amcl/parameter_descriptions
/amcl/parameter_updates
/amcl_pose
...
/map
/map_metadata
/map_updates
...
/particlecloud
La rubrique amcl_pose correspond à la position du robot publiée par amcl.Voyons les messages dans le sujet:rostopic echo /amcl_pose
Obtenez les données sur la position du robot:header:
seq: 15
stamp:
secs: 1482430591
nsecs: 39625000
frame_id: map
pose:
pose:
position:
x: 0.781399671581
y: 0.273353260585
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.636073020536
w: 0.771628869694
covariance: [0.2187289446708912, -0.010178711317316846, 0.0, 0.0, 0.0, 0.0, -0.010178711317316819, 0.23720047371620548, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.07106236846890918]
---
Dans rviz, nous obtenons l'image suivante:
Comme vous pouvez le voir, les points de numérisation du lidar coïncident partiellement avec les murs de la carte. Voici à quoi ressemblait ma configuration:
Essayons de déplacer le robot. La position du robot et le point de vue de la carte doivent changer simultanément dans la fenêtre rviz. Après avoir déplacé le robot, la position du robot peut ne pas être déterminée avec précision par l'algorithme amcl. Nous devons ajuster la position du robot à l'aide de l'outil d'estimation de pose 2D. Pour ce faire, cliquez sur le bouton Estimation de pose 2D dans la barre d'outils supérieure de rviz, cliquez sur le point approximatif du centre du système de coordonnées du robot sur la carte dans rviz (système de coordonnées base_link) et maintenez enfoncé le bouton de la souris. Une flèche verte apparaît, émanant du centre du robot:
Tirez sur la flèche en changeant de direction et essayez de combiner les points du scan du lidar avec des bords noirs (murs) sur la carte. Après avoir reçu la meilleure combinaison, lâchez le bouton de la souris.
Nous recevrons ces messages dans le terminal où s'exécute my_localize.launch:[ INFO] [1482431993.717411186]: Setting pose (1482431993.717383): -0.413 -0.071 0.057
Sur une courte vidéo, vous pouvez tout regarder en action:Topic / particlecloud présente des données sur l'incertitude de la position du robot sous la forme de positions orientées (Pose) ou du soi-disant nuage de particules. Le type de message est geometry_msgs / PoseArray.Ajoutez un affichage nommé topic / particlecloud :
Dans rviz, un nuage de particules apparaîtra sous la forme d'un épais groupe de flèches rouges:
plus le groupe de particules est épais, plus la probabilité de la position du robot dans cette position est élevée. Vous pouvez en savoir plus sur l'outil d'estimation de pose 2D, le nuage de particules et d'autres concepts dans amcl dans le didacticiel sur ros.org.C’est tout! Tous les algorithmes considérés (gmapping et amcl) font partie de la grande pile de navigation dans ROS, vous pouvez trouver beaucoup d'informations à ce sujet sur Internet. Aujourd'hui, nous avons essayé l'outil laser_scan_matcher, les algorithmes de localisation gmapping et amcl en action. Maintenant, vous pouvez facilement commencer à travailler sur la localisation et la navigation d'un robot mobile et créer un robot entièrement autonome qui peut naviguer dans l'espace sans avoir besoin d'un contrôle manuel.Abonnez-vous à notre groupe sur ROS Vkontakte et restez au courant de l'apparition de nouveaux articles et nouvelles sur la plate-forme ROS. Je vous souhaite bonne chance dans les expériences et à bientôt!PS: Tout avec le prochain 2017!