Buenas tardes lectores! Una vez tocamos el tema de localización y SLAM en un artículo sobre Hector SLAM . Continuamos conociendo los algoritmos para construir mapas de terreno y localización en ROS. Hoy intentaremos construir un mapa del área sin una fuente de odometría, usando solo el lidar Hokuyo URG-04LX-UG01 y el algoritmo gmapping y localizar el robot en el mapa construido usando el algoritmo amcl . Laser_scan_matcher nos ayudará con esto . A quién le importa, por favor, debajo del gato.Instalación del paquete laser_scan_matcher
¡Entonces comencemos! Para los experimentos usaremos ROS Indigo, pero puede usar una versión diferente de ROS (Jade, Kinetic, Hydro). La instalación de paquetes debe realizarse de la misma manera (quizás solo en ROS Kinetic algunos paquetes no estarán disponibles a través de apt-get).El paquete laser_scan_matcher es una herramienta para la grabación incremental de datos láser, que se implementa sobre la base del método Canonical Scan Matcher, que se puede leer aquí . Puedes leer sobre el paquete aquí . El paquete se puede usar sin datos de odometría; realiza la evaluación de odometría en sí.Instala el paquete:sudo apt-get install ros-indigo-laser-scan-matcher
Probemos la demo:roscore
roslaunch laser_scan_matcher demo.launch
Veremos algo similar en rviz:Aquí se muestra laser_scan_matcher en acción sobre datos LIDAR grabados en un archivo de bolsa. Ahora prueba el paquete en vivo. Para no perder tiempo, lo intentaremos de inmediato con el algoritmo gmapping. Cree un paquete con el archivo my_gmapping_launch.launch para iniciar gmapping:cd ~/catkin_ws/src
catkin_create_pkg my_laser_matcher
cd src/my_laser_matcher
mkdir launch
vim launch/my_gmapping_launch.launch
Copie el siguiente código en el archivo my_gmapping_launch.launch:Código 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>
Aquí ejecutamos static_transform_publisher desde el paquete tf para publicar la transformación entre los sistemas de coordenadas base_link → laser, los nodos laser_scan_matcher y slam_gmapping. El código fuente del archivo se puede descargar desde aquí . Para usar el Lidar de Hokuyo, necesitamos instalar el paquete hokuyo_node ROS :sudo apt-get install ros-indigo-hokuyo-node
Ejecute el nodo getID desde el paquete hokuyo_node para obtener información lidar:rosrun hokuyo_node getID /dev/ttyACM0
Se puede producir un error: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
En este caso, necesitamos agregar permisos para el puerto / dev / ttyACM0:sudo chmod a+rw /dev/ttyACM0
Ejecute getID desde el paquete hokuyo_node nuevamente y obtenga una salida similar:Device at /dev/ttyACM0 has ID H0906078
Ahora ejecute el nodo hokuyo_node:rosrun hokuyo_node hokuyo_node
Finalmente, ejecute nuestro archivo de inicio my_gmapping_launch.launch:roslaunch my_laser_matcher my_gmapping_launch.launch
rosrun rviz rviz
Hagamos una lista de los temas:rostopic list
Entre los temas veremos los siguientes:/initialpose
/move_base_simple/goal
/odom
/pose2D
...
/imu/data
De esta forma obtenemos odometría y posición del robot gracias a laser_scan_matcher.Agregue a rviz una pantalla de tipo LaserScan con el tema / scan como se describe en el artículo . Agregue también una pantalla para el Mapa Mapa y para la transformación de TF. Expanda la sección TF y los marcos dentro de ella, luego observe los puntos: odom, map, base_link. Permítame recordarle que estos son los sistemas de coordenadas del odómetro, el mapa y el robot, respectivamente. Recuerde establecer el valor / map en el campo Marco fijo en el panel de pantallas izquierdo en la sección Opciones globales.En rviz veremos una imagen similar: a
continuación, simplemente mueva el robot en el espacio para construir un mapa completo del área. Utilizamos la utilidad map_saver del paquete map_server para guardar el mapa:rosrun map_server map_saver
Localización con amcl
Ahora intentemos localizar el robot usando el algoritmo amcl. Cree el archivo my_localize.launch dentro de nuestro paquete con los siguientes contenidos:Código 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>
Aquí, de manera similar al lanzador para gmapping, publicamos la transformación / laser → / base_link usando el nodo static_transform_publisher, los nodos hokuyo_node y laser_scan_matcher. Luego ejecutamos map_server para publicar nuestro mapa construido, donde en args pasamos la ruta al mapa en el archivo yaml. Finalmente, inicie el nodo amcl con los parámetros. Puede leer sobre los parámetros de amcl en la página oficial de algoritmos .El código del archivo de inicio también se puede descargar desde el repositorio de github . Ejecute nuestro archivo de inicio:roslaunch my_laser_matcher my_localize.launch
Ahora pasemos a rviz. Establezca el valor del mapa para Marco fijo en la sección Opciones globales. Hagamos una lista de los temas:rostopic list
Nuevos temas aparecerán en la lista:...
/amcl/parameter_descriptions
/amcl/parameter_updates
/amcl_pose
...
/map
/map_metadata
/map_updates
...
/particlecloud
El tema amcl_pose corresponde a la posición del robot publicado por amcl.Veamos las publicaciones en el tema:rostopic echo /amcl_pose
Obtenga los datos sobre la posición del 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]
---
En rviz obtenemos la siguiente imagen:
Como puede ver, los puntos de escaneo del lidar coinciden parcialmente con las paredes en el mapa. Así es como se veía realmente mi configuración:
intentemos mover el robot. La posición del robot y el punto de vista del mapa deberían cambiar simultáneamente en la ventana rviz. Después de mover el robot, la posición del robot puede no ser determinada con precisión por el algoritmo amcl. Necesitamos ajustar la posición del robot usando la herramienta 2D Pose Estimate. Para hacer esto, haga clic en el botón 2D Pose Estimate en la barra de herramientas superior en rviz, haga clic en el punto aproximado del centro del sistema de coordenadas del robot en el mapa en rviz (sistema de coordenadas base_link) y mantenga presionado el botón del mouse. Aparece una flecha verde, que emana del centro del robot:
Tire de la flecha cambiando su dirección y tratando de combinar los puntos del escaneo desde el LIDAR con bordes negros (paredes) en el mapa. Habiendo recibido la mejor combinación, suelte el botón del mouse.
Recibiremos dichos mensajes en el terminal donde se ejecuta my_localize.launch:[ INFO] [1482431993.717411186]: Setting pose (1482431993.717383): -0.413 -0.071 0.057
En un video corto puedes ver todo en acción:El tema / nube de partículas presenta datos sobre la incertidumbre de la posición del robot en forma de posiciones orientadas (Pose) o la llamada nube de partículas. El tipo de mensaje es geometry_msgs / PoseArray.Agregue una pantalla llamada tema / partícula de partículas :
en rviz, aparecerá una nube de partículas en forma de un grueso grupo de flechas rojas: cuanto
más grueso sea el grupo de partículas, mayor será la probabilidad de la posición del robot en esta posición. Puede leer sobre la herramienta de estimación de Pose 2D, la nube de partículas y otros conceptos en amcl en el tutorial en ros.org.Eso es todo! Todos los algoritmos considerados (gmapping y amcl) son parte de la gran pila de navegación en ROS, puede encontrar mucha información al respecto en Internet. Hoy probamos la herramienta laser_scan_matcher, gmapping y los algoritmos de localización amcl en acción. Ahora puede comenzar a trabajar fácilmente en la localización y navegación de un robot móvil y crear un robot totalmente autónomo que pueda navegar en el espacio sin la necesidad de un control manual.Suscríbase a nuestro grupo en ROS Vkontakte y manténgase actualizado con la aparición de nuevos artículos y noticias sobre la plataforma ROS. ¡Les deseo a todos mucha suerte en los experimentos y hasta pronto!PD: ¡Todo con el próximo 2017!