Geekly articles weekly

Carrinho de caminhão ROS. Parte 6. Odometria com codificadores de roda, mapa da sala, lidar

Posts da série:
8. Nós controlamos a partir do telefone-ROS Control, GPS-node
7. Localização do robô: gmapping, AMCL, pontos de referência no mapa da sala
6. Odometria com codificadores de roda, mapa da sala, lidar
5. Trabalhamos em rviz e gazebo: xacro, novos sensores.
4. Crie uma simulação de robô usando os editores rviz e gazebo.
3. Acelere, troque a câmera, conserte a marcha
2. Software
1. Ferro

Na última vez, projetamos o projeto na forma de módulos xacro separados, adicionamos uma câmera de vídeo virtual e imu (giroscópio).

Neste post, trabalharemos com a odometria dos codificadores ópticos montados nos eixos das rodas, carregaremos um mapa da sala e o montaremos em um carrinho de robô real.

Odometria e tf


O que é odometria e tf e como eles geralmente são implementados no ROS já está bem descrito no recurso, portanto, nos referimos aos artigos relevantes na parte da teoria, por exemplo, aqui .
Partindo da base teórica, trabalharemos com a prática.

Vamos começar trabalhando em um robô de carrinho conectando-o via VNC.

Vá para a pasta rosbots_driver e crie um nó de arquivo. Este arquivo gera odometria, recebendo-o de codificadores ópticos, que por sua vez o enviam para o arduino uno e, em seguida, para o raspberry pi.

cd /home/pi/rosbots_catkin_ws/src/rosbots_driver/scripts/rosbots_driver touch diff-tf.py

Colocamos o código no arquivo:

diff_tf.py
 #!/usr/bin/env python """ diff_tf.py - follows the output of a wheel encoder and creates tf and odometry messages. some code borrowed from the arbotix diff_controller script A good reference: http://rossum.sourceforge.net/papers/DiffSteer/ Copyright (C) 2012 Jon Stephan. """ import rospy #import roslib #roslib.load_manifest('differential_drive') from math import sin, cos, pi from geometry_msgs.msg import Quaternion from geometry_msgs.msg import Twist from geometry_msgs.msg import Vector3 from nav_msgs.msg import Odometry import tf from tf.broadcaster import TransformBroadcaster from std_msgs.msg import Int16, Int32, Int64, UInt32 ############################################################################# class DiffTf: ############################################################################# ############################################################################# def __init__(self): ############################################################################# rospy.init_node("diff_tf") self.nodename = rospy.get_name() rospy.loginfo("-I- %s started" % self.nodename) #### parameters ####### #Wheel radius : 0.0325 # wheel circum = 2* 3.14 * 0.0325 = 0.2041 # One rotation encoder ticks : 8 ticks # For 1 meter: 8 * ( 1 / 0.2041) = 39 ticks self.rate = rospy.get_param('~rate',10.0) # the rate at which to publish the transform self.ticks_meter = float(rospy.get_param('ticks_meter', 190)) # The number of wheel encoder ticks per meter of travel self.base_width = float(rospy.get_param('~base_width', 0.11)) # The wheel base width in meters self.base_frame_id = rospy.get_param('~base_frame_id','base_link') # basefootprint /the name of the base frame of the robot self.odom_frame_id = rospy.get_param('~odom_frame_id', 'odom') # the name of the odometry reference frame self.encoder_min = rospy.get_param('encoder_min', -2147483648) self.encoder_max = rospy.get_param('encoder_max', 2147483648) self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min ) self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min ) self.t_delta = rospy.Duration(1.0/self.rate) self.t_next = rospy.Time.now() + self.t_delta # internal data self.enc_left = None # wheel encoder readings self.enc_right = None self.left = 0 # actual values coming back from robot self.right = 0 self.lmult = 0 self.rmult = 0 self.prev_lencoder = 0 self.prev_rencoder = 0 self.x = 0 # position in xy plane self.y = 0 self.th = 0 self.dx = 0 # speeds in x/rotation self.dr = 0 self.yaw = 0.01 self.pitch = 0.01 self.roll = 0.01 self.then = rospy.Time.now() self.quaternion_1 = Quaternion() # subscriptions rospy.Subscriber("wheel_ticks_left", UInt32, self.lwheelCallback) rospy.Subscriber("wheel_ticks_right", UInt32, self.rwheelCallback) #rospy.Subscriber("imu_data", Vector3, self.imu_value_update) self.odomPub = rospy.Publisher("odom", Odometry,queue_size=10) self.odomBroadcaster = TransformBroadcaster() ############################################################################# def spin(self): ############################################################################# r = rospy.Rate(self.rate) while not rospy.is_shutdown(): self.update() r.sleep() ############################################################################# def update(self): ############################################################################# now = rospy.Time.now() if now > self.t_next: elapsed = now - self.then self.then = now elapsed = elapsed.to_sec() # calculate odometry if self.enc_left == None: d_left = 0 d_right = 0 else: d_left = (self.left - self.enc_left) / self.ticks_meter d_right = (self.right - self.enc_right) / self.ticks_meter self.enc_left = self.left self.enc_right = self.right # distance traveled is the average of the two wheels d = ( d_left + d_right ) / 2 # this approximation works (in radians) for small angles th = ( d_right - d_left ) / self.base_width # calculate velocities self.dx = d / elapsed self.dr = th / elapsed if (d != 0): # calculate distance traveled in x and y x = cos( th ) * d y = -sin( th ) * d # calculate the final position of the robot self.x = self.x + ( cos( self.th ) * x - sin( self.th ) * y ) self.y = self.y + ( sin( self.th ) * x + cos( self.th ) * y ) if( th != 0): self.th = self.th + th # publish the odom information quaternion = Quaternion() quaternion.x = 0.0 quaternion.y = 0.0 quaternion.z = sin( self.th / 2 ) quaternion.w = cos( self.th / 2 ) ''' try: quaternion.z = self.quaternion_1[2] quaternion.w = self.quaternion_1[3] except: quaternion.z = sin( self.th / 2 ) quaternion.w = cos( self.th / 2 ) pass ''' self.odomBroadcaster.sendTransform( (self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), rospy.Time.now(), self.base_frame_id, self.odom_frame_id ) odom = Odometry() odom.header.stamp = now odom.header.frame_id = self.odom_frame_id odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y odom.pose.pose.position.z = 0 odom.pose.pose.orientation = quaternion odom.child_frame_id = self.base_frame_id odom.twist.twist.linear.x = self.dx odom.twist.twist.linear.y = 0 odom.twist.twist.angular.z = self.dr self.odomPub.publish(odom) def imu_value_update(self, imu_data): orient = Vector3() orient = imu_data self.yaw = orient.x self.pitch = orient.y self.roll = orient.z try: self.quaternion_1 = tf.transformations.quaternion_from_euler(self.yaw, self.pitch, self.roll) #print self.quaternion_1[0] #print self.quaternion_1[1] #print self.quaternion_1[2] #print self.quaternion_1[3] except: rospy.logwarn("Unable to get quaternion values") pass ############################################################################# def lwheelCallback(self, msg): ############################################################################# enc = msg.data if (enc < self.encoder_low_wrap and self.prev_lencoder > self.encoder_high_wrap): self.lmult = self.lmult + 1 if (enc > self.encoder_high_wrap and self.prev_lencoder < self.encoder_low_wrap): self.lmult = self.lmult - 1 self.left = 1.0 * (enc + self.lmult * (self.encoder_max - self.encoder_min)) self.prev_lencoder = enc ############################################################################# def rwheelCallback(self, msg): ############################################################################# enc = msg.data if(enc < self.encoder_low_wrap and self.prev_rencoder > self.encoder_high_wrap): self.rmult = self.rmult + 1 if(enc > self.encoder_high_wrap and self.prev_rencoder < self.encoder_low_wrap): self.rmult = self.rmult - 1 self.right = 1.0 * (enc + self.rmult * (self.encoder_max - self.encoder_min)) self.prev_rencoder = enc ############################################################################# ############################################################################# if __name__ == '__main__': """ main """ diffTf = DiffTf() diffTf.spin()


Salve o arquivo e torne-o executável:
CTRL+X
chmod +x diff-tf.py

Agora, no robô, execute os 2º nós - driver e diff-tf:
1º terminal:

 python diff_tf.py

2º:

 rosrun rosbots_driver part2_cmr.py

No terceiro terminal, verificaremos se há novos tópicos odom e tf:



Vamos ver com o comando rostopic echo odom o que é publicado no tópico (e se ele é publicado).
A saída será aproximadamente da seguinte maneira:



Agora, sem fechar os nós em execução no robô, lançaremos o computador de controle com os ambientes gráficos rviz e gazebo.

* Uma imagem (máquina virtual VMWare com Ubuntu 16.04 + ROS Kinetic), oferecida anteriormente para download, contém tudo o que você precisa.

No computador de controle (doravante denominado "Computador"), execute o modelo em rviz:

 roslaunch rosbots_description rviz.launch

O modelo do robô carregado com o qual funcionou nas postagens anteriores carregará:



Adicione duas telas ao rviz clicando em Adicionar. A tela com odometria e a tela com tf, marque as caixas para visualizá-las.

Na janela em que o modelo do robô é representado, gráficos característicos aparecerão:


* Para torná-lo mais visível, você pode desativar a tela Robotmodel.

Controlamos o robô a partir do teclado do computador e vemos como a visualização de tf e odometria muda.

Sem fechar o rviz no 2º terminal, iniciaremos o controle a partir do teclado:

 rosrun teleop_twist_keyboard teleop_twist_keyboard.py /cmd_vel:=/part2_cmr/cmd_vel

Ao controlar o robô, a janela com visualização mostrará: seta vermelha (visualização do tópico odom), linhas vetoriais (tópico tf).

Se a seta vermelha do tópico odom mostra a direção do movimento do robô, as linhas vetoriais tf mostram como os elementos individuais do robô estão localizados no espaço:

o video


Agora, para seguir em frente, você precisa "ajustar" a odometria.
Para fazer isso, feche o editor rviz e inicie-o novamente, apenas sem visualizar o modelo com o comando:

 rosrun rviz rviz

Isso é necessário para que apenas base_link e odom permaneçam nos vetores do tópico tf:



No rviz, uma célula tem 1 metro. Portanto, na realidade, o robô também deve passar 1 metro para que os dados sejam comparáveis.

Passaremos 1 metro no robô, controlando-o pelo teclado. No rviz, o robô também precisa dirigir 1 metro - uma célula.

Se o robô viaja mais tempo do que deveria no rviz, ou vice-versa, a uma distância menor do que na realidade, é necessário editar o arquivo diff_tf.py que foi criado anteriormente, ou seja, este bloco:

diff_tf.py
  #### parameters ####### #Wheel radius : 0.0325 # wheel circum = 2* 3.14 * 0.0325 = 0.2041 # One rotation encoder ticks : 8 ticks # For 1 meter: 8 * ( 1 / 0.2041) = 39 ticks self.rate = rospy.get_param('~rate',10.0) # the rate at which to publish the transform self.ticks_meter = float(rospy.get_param('ticks_meter', 190)) # The number of wheel encoder ticks per meter of travel self.base_width = float(rospy.get_param('~base_width', 0.11)) # The wheel base width in meters


Mapa


Para ir a algum lugar, você precisa de um mapa. Para os propósitos do nosso robô - precisamos de um mapa da sala.
Vamos trabalhar com ela.

Para fazer upload de um mapa para o rviz, você precisa criar uma pasta de mapa no projeto (rosbots_description) no Computador (não no robô) e colocar dois arquivos nele que compõem o mapa: map.pgm e map.yaml.
* De fato, pode haver vários arquivos de mapa em uma pasta, mas você só pode enviar um para o assistente.

Um mapa no ROS consiste em dois arquivos, um dos quais é uma imagem PGM, em que cada pixel é:

  • espaço em branco é livre;
  • espaço negro é ocupado por um obstáculo;
  • espaço cinza ainda não foi explorado.

O segundo arquivo .yaml é um arquivo com as configurações do mapa, onde são indicadas suas dimensões, ocupação de pixels com diferentes tipos (listados acima) e outros parâmetros.

Execute o nó no computador que publicará o cartão:

 rosrun map_server map_server /home/pi/catkin_ws/src/rosbots_description/maps/rail_lab.pgm 0.05

No terminal vizinho, execute o modelo em rviz:

 roslaunch rosbots_description rviz.launch

No rviz, adicione uma exibição de mapa.

No rviz, o robô mostrou-se desproporcionalmente grande e está localizado fora do mapa:



Para corrigir isso, você precisa executar um mapa em que o tamanho da célula seja de 1 metro. Reinicie o cartão com o parâmetro 1 no final:

 rosrun map_server map_server /home/pi/catkin_ws/src/rosbots_description/maps/rail_lab.pgm 1

Agora você pode montar o mapa no rviz, controlando o robô a partir do teclado:

o video


Então, o que foi alcançado :

  • receber dados de odometria dos codificadores ópticos das rodas do robô e enviá-los aos tópicos para exibição no rviz;
  • configurar a odometria do robô para corresponder à distância percorrida ao vivo e virtualmente;
  • carregar e exibir um mapa da sala.

No entanto, apesar do mapa ser exibido e o robô poder andar com uma odometria “sintonizada”, na realidade o robô é cego. Ele não vê obstáculos e tropeçará neles. O segundo ponto negativo é o mapa da sala virtual carregado no rviz, que permite que você viaje sozinho em todas as direções, mesmo naquelas em que os obstáculos são claramente mostrados.

Como fazer o robô "ver" obstáculos na realidade e virtualmente?

Com um ambiente virtual é mais simples. Tudo aqui é baseado no emulador-editor de gazebo. E nos posts anteriores isso foi mencionado.

É mais complicado com a realidade. Precisamos de um elemento (sensor) que indique obstáculos e relate isso ao sistema.

Uma opção é colocar o lidar no robô.

Lidar RPlidar A1


Usaremos a solução de orçamento acessível e colocaremos o lidar no robô. Talvez essa solução seja mais cara do que usar o mesmo Kinect, mas, como a prática demonstrou, é mais eficaz em termos de velocidade, precisão e facilidade de instalação (menos complicada). Além disso, é mais fácil começar a trabalhar com o lidar, como Nenhuma reflexão é necessária sobre como alimentá-lo e conectá-lo ao projeto (https://habr.com/en/company/tod/blog/210252/).

Nós precisaremos do pacote ros para trabalhar com o lidar - wiki.ros.org/rplidar .
Com a ajuda do lidar, construiremos um mapa da sala e também o usaremos na navegação.

Como instalar o rplidar no ROS tem muitos artigos, por exemplo aqui .

Usaremos o conhecimento de idosos de cabelos grisalhos e instalaremos pacotes com o lidar no sistema no robô :

 cd /home/pi/rosbots_catkin_ws/src git clone https://github.com/robopeak/rplidar_ros.git cd .. catkin_make

No computador, instale o pacote para trabalhar com a placa:

 cd /home/pi/rosbots_catkin_ws/src git clone https://github.com/tu-darmstadt-ros-pkg/hector_slam </code> cd .. catkin_make

Execute o pacote no robô e verifique se o lidar está funcionando:

 sudo chmod a+rw /dev/ttyUSB0 roslaunch rplidar_ros rplidar.launch

* O primeiro comando dá acesso à porta usb onde o lidar está conectado.

Se tudo correu bem, ele produzirá linhas para o console:

 [ INFO] [1570900184.874891236]: RPLIDAR running on ROS package rplidar_ros. SDK Version:1.9.0 RPLIDAR S/N: ---------------- [ INFO] [1570900187.397858270]: Firmware Ver: 1.24 [ INFO] [1570900187.398081809]: Hardware Rev: 5 [ INFO] [1570900187.401749476]: RPLidar health status : 0 [ INFO] [1570900188.014285166]: current scan mode: Express, max_distance: 12.0 m, Point number: 4.0K , angle_compensate: 1

Aqui configuramos imediatamente um pouco de lidar, porque o site oficial diz que (lidar) pode funcionar melhor.

Precisamos obter a saída quando a digitalização não é de 4,0K pontos, que são emitidos por padrão, mas de 8,0K. Esta opção melhorará um pouco a qualidade da digitalização.

Para isso, definiremos mais um parâmetro no pacote rplidar - modo de verificação:

 cd /rosbots_catkin_ws/src/rplidar_ros/launch nano nano rplidar.launch

E depois

 <param name="angle_compensate" type="bool" value="true"/>
adicione a linha:

 <param name="scan_mode" type="string" value="Boost"/>

A segunda linha que precisa ser corrigida aqui:

 <param name="frame_id" type="string" value="laser"/>

Substitua o valor do laser por base_link.

* Agora, se você reiniciar o nó com o comando roslaunch rplidar_ros rplidar.launch, a saída será diferente:

 [ INFO] [1570900188.014285166]: current scan mode: Boost, max_distance: 12.0 m, Point number: 8.0K , angle_compensate: 1

Dê uma olhada. que exibe lidar em rviz.

Para fazer isso, execute o robô:

 roslaunch rplidar_ros rplidar.launch

Em um computador:

 roslaunch rosbots_description rviz.launch

No rviz, adicione a tela LaserScan e selecione o tópico de digitalização. Além disso, será visto que as mensagens estão caindo no tópico:



Na janela com a visualização do robô, o robô acabou sendo um gigante. Com seu tamanho, descobriremos mais tarde. Agora vamos construir um mapa da sala.

Para fazer isso, crie um pacote com um nó:

 catkin_create_pkg my_hector_mapping rospy cd my_hector_mapping mkdir launch cd launch nano hector.launch

hector.launch
 <?xml version="1.0"?> <launch> <node pkg="tf" type="static_transform_publisher" name="laser_link" args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /laser 50" /> <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen"> <!-- Frame names --> <param name="map_frame" value="map" /> <param name="odom_frame" value="base_link" /> <!-- Map size / start point --> <param name="map_resolution" value="0.050"/> <param name="map_size" value="1024"/> <param name="map_start_x" value="0.5"/> //  <param name="map_start_y" value="0.5" /> <param name="map_multi_res_levels" value="2" /> <!-- Map update parameters --> <param name="update_factor_free" value="0.4"/> <param name="update_factor_occupied" value="0.9" /> <param name="map_update_distance_thresh" value="0.4"/> <param name="map_update_angle_thresh" value="0.06" /> <param name="laser_z_min_value" value="-1.0" /> <param name="laser_z_max_value" value="1.0" /> <!-- Advertising config --> <param name="advertise_map_service" value="true"/> <param name="scan_subscriber_queue_size" value="5"/> <param name="scan_topic" value="scan"/> </node> </launch> 


 cd ~/rosbots_catkin_ws catkin_make

Vamos correr.

No robô:

1º terminal: roslaunch rplidar_ros rplidar.launch
2nd: rosrun rosbots_driver part2_cmr.py

Em um computador:

1º terminal: roslaunch my_hector_mapping hector.launch
2nd: roslaunch rosbots_description rviz.launch
3º: rosrun teleop_twist_keyboard teleop_twist_keyboard.py /cmd_vel:=/part2_cmr/cmd_vel

Nos monitores, você precisa adicionar o mapa, e Quadro fixo, selecione base_link. Então você pode observar em tempo real como o lidar "ilumina" o espaço ao seu redor:



Na etapa atual, para construir um mapa, você precisa andar pela sala, “parando” em diferentes ângulos para que o lidar os marque no mapa.

Portanto, recomende livros didáticos. Mas nosso conselho é pegar o robô e andar com ele, segurando-o na sua frente. Portanto, a velocidade de construção de um mapa será maior no sentido de que você não precisará se distrair e olhar para onde o robô dirigiu na próxima sala na ausência de contato visual.

Além disso, ao girar o robô em torno de seu eixo durante uma viagem, o lidar deixa artefatos pretos característicos nos locais onde não há realmente obstáculos:



Após criar o mapa, salve-o com o comando:

 rosrun map_server map_saver -f map-1

Construir o mapa perfeito com um orçamento lidar é um mito. Portanto, ajudaremos o lidar no Photoshop. Removeremos os artefatos pretos do mapa, onde realmente não há obstáculos, e alinharemos as paredes com linhas pretas:



Não esqueça de salvar o mapa no formato .pgm.

Agora repetimos no computador os comandos que estavam no início do post, mas com um novo mapa:
1º terminal: rosrun map_server maserver /home/pi/catkin_ws/src/rosbots_description/maps/map-1.pgm 0.05
2nd: roslaunch rosbots_description rviz.launch

Resultado em rviz:



O novo mapa foi carregado, como o modelo do robô, mas o robô está fora do mapa.

Falaremos sobre isso mais tarde, mas, por enquanto, vamos resumir:

  • dominando o lidar RP-lidar A1
  • construindo um mapa de sala usando um lidar, ajustando-o e carregando-o no editor visual rviz.

Arquivos para download: mapa da sala .

Source: https://habr.com/ru/post/pt471028/

More articles:


All Articles