Boa tarde leitores! Certa vez, tocamos no tópico de localização e SLAM em um artigo sobre Hector SLAM . Continuamos familiarizados com os algoritmos para a construção de mapas e localização de terrenos no ROS. Hoje tentaremos construir um mapa da área sem uma fonte de odometria, usando apenas o lidar com o Hokuyo URG-04LX-UG01 e o algoritmo de gmapping e localizar o robô no mapa construído usando o algoritmo amcl . Laser_scan_matcher nos ajudará com isso . Quem se importa, por favor, debaixo do gato.Instalando o pacote laser_scan_matcher
Então, vamos começar! Para experimentos, usaremos o ROS Indigo, mas você pode usar uma versão diferente do ROS (Jade, Kinetic, Hydro). A instalação de pacotes deve ser feita da mesma maneira (talvez apenas no ROS Kinetic, alguns pacotes não estejam disponíveis no apt-get).O pacote laser_scan_matcher é uma ferramenta para gravação incremental de dados a laser, implementada com base no método Canonical Scan Matcher, que pode ser lido aqui . Você pode ler sobre o pacote aqui . A embalagem pode ser usada sem dados de odometria; ela realiza a própria avaliação de odometria.Instale o pacote:sudo apt-get install ros-indigo-laser-scan-matcher
Vamos tentar a demonstração:roscore
roslaunch laser_scan_matcher demo.launch
Veremos algo semelhante em rviz:É mostrado aqui laser_scan_matcher em ação nos dados do lidar gravados em um arquivo bag. Agora experimente o pacote ao vivo. Para não perder tempo, tentaremos imediatamente com o algoritmo gmapping. Crie um pacote com o arquivo my_gmapping_launch.launch para iniciar o 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 o seguinte código no arquivo 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>
Aqui, rodamos static_transform_publisher a partir do pacote tf para publicar a transformação entre os sistemas de coordenadas a laser base_link →, os nós laser_scan_matcher e slam_gmapping. O código fonte do arquivo pode ser baixado aqui . Para usar o Hokuyo lidar, precisamos instalar o pacote ROS hokuyo_node :sudo apt-get install ros-indigo-hokuyo-node
Execute o nó getID do pacote hokuyo_node para obter informações do lidar:rosrun hokuyo_node getID /dev/ttyACM0
Pode ocorrer um erro: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
Nesse caso, precisamos adicionar permissões para a porta / dev / ttyACM0:sudo chmod a+rw /dev/ttyACM0
Execute o getID do pacote hokuyo_node novamente e obtenha uma saída semelhante:Device at /dev/ttyACM0 has ID H0906078
Agora execute o nó hokuyo_node:rosrun hokuyo_node hokuyo_node
Por fim, execute nosso arquivo iniciador my_gmapping_launch.launch:roslaunch my_laser_matcher my_gmapping_launch.launch
rosrun rviz rviz
Vamos listar os tópicos:rostopic list
Entre os tópicos, veremos o seguinte:/initialpose
/move_base_simple/goal
/odom
/pose2D
...
/imu/data
Dessa forma, obtemos a odometria e a posição do robô, graças ao laser_scan_matcher.Adicione ao rviz uma tela do tipo LaserScan com o tópico / scan, conforme descrito no artigo . Adicione também uma exibição para o Map Map e para a transformação do TF. Expanda a seção TF e os quadros dentro dela e observe os pontos: odom, mapa, base_link. Deixe-me lembrá-lo de que esses são os sistemas de coordenadas odômetro, mapa e robô, respectivamente. Lembre-se de definir o valor / map para o campo Quadro fixo no painel esquerdo Monitores na seção Opções globais.Em rviz, veremos uma figura semelhante:
Em seguida, basta mover o robô no espaço para construir um mapa completo da área. Usamos o utilitário map_saver do pacote map_server para salvar o mapa:rosrun map_server map_saver
Localização com amcl
Agora vamos tentar localizar o robô usando o algoritmo amcl. Crie o arquivo my_localize.launch dentro do nosso pacote com o seguinte conteúdo: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>
Aqui, da mesma forma que o iniciador do gmapping, publicamos a transformação / laser → / base_link usando o nó static_transform_publisher, os nós hokuyo_node e laser_scan_matcher. Em seguida, rodamos o map_server para publicar nosso mapa construído, onde nos argumentos passamos o caminho para o mapa no arquivo yaml. Por fim, inicie o nó amcl com os parâmetros Você pode ler sobre os parâmetros amcl na página oficial do algoritmo .O código do arquivo do iniciador também pode ser baixado do repositório do github . Execute nosso arquivo do iniciador:roslaunch my_laser_matcher my_localize.launch
Agora vamos para o rviz. Defina o valor do mapa para Quadro fixo na seção Opções globais. Vamos listar os tópicos:rostopic list
Novos tópicos aparecerão na lista:...
/amcl/parameter_descriptions
/amcl/parameter_updates
/amcl_pose
...
/map
/map_metadata
/map_updates
...
/particlecloud
O tópico amcl_pose corresponde à posição do robô publicada pela amcl.Vamos ver as postagens no tópico:rostopic echo /amcl_pose
Obtenha os dados sobre a posição do robô: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]
---
Em rviz, temos a seguinte imagem:
Como você pode ver, os pontos de varredura do lidar coincidem parcialmente com as paredes no mapa. Aqui está a aparência da minha configuração:
vamos tentar mover o robô. A posição do robô e o ponto de vista do mapa devem mudar simultaneamente na janela do rviz. Após mover o robô, a posição do robô pode não ser determinada com precisão pelo algoritmo amcl. Precisamos ajustar a posição do robô usando a ferramenta 2D Pose Estimate. Para fazer isso, clique no botão 2D Pose Estimate na barra de ferramentas superior no rviz, clique no ponto aproximado do centro do sistema de coordenadas do robô no mapa em rviz (sistema de coordenadas base_link) e mantenha pressionado o botão do mouse. Uma seta verde aparece, emanando do centro do robô:
Puxe a seta mudando de direção e tentando combinar os pontos da digitalização do lidar com bordas pretas (paredes) no mapa. Tendo recebido a melhor combinação, solte o botão do mouse.
Nós receberemos essas mensagens no terminal em que my_localize.launch está sendo executado:[ INFO] [1482431993.717411186]: Setting pose (1482431993.717383): -0.413 -0.071 0.057
Em um pequeno vídeo, você pode assistir a tudo em ação:O tópico / nuvem de partículas apresenta dados sobre a incerteza da posição do robô na forma de posições orientadas (Pose) ou na chamada nuvem de partículas. O tipo de mensagem é geometry_msgs / PoseArray.Adicione uma tela denominada topic / particlecloud :
no rviz, uma nuvem de partículas aparecerá na forma de um grosso conjunto de setas vermelhas:
Quanto mais espesso o cluster de partículas, maior a probabilidade da posição do robô nessa posição. Você pode ler sobre a ferramenta de estimativa de pose 2D, nuvem de partículas e outros conceitos em amcl no tutorial em ros.org.Isso é tudo! Todos os algoritmos considerados (gmapping e amcl) fazem parte da grande pilha de Navegação no ROS, você pode encontrar muitas informações sobre ele na Internet. Hoje tentamos a ferramenta laser_scan_matcher, os algoritmos de gmapping e localização de amcl em ação. Agora você pode facilmente começar a trabalhar na localização e navegação de um robô móvel e criar um robô totalmente autônomo que possa navegar no espaço sem a necessidade de controle manual.Assine o nosso grupo no ROS Vkontakte e mantenha-se atualizado com o surgimento de novos artigos e notícias sobre a plataforma ROS. Desejo a todos boa sorte nos experimentos e até breve!PS: Tudo com o próximo 2017!