Geekly articles weekly

Carro para camiones ROS. Parte 6. Odometría con codificadores de rueda, mapa de habitación, lidar

Publicaciones en la serie:
8. Controlamos desde el control del teléfono ROS, nodo GPS
7. Localización de robots: gmapping, AMCL, puntos de referencia en el mapa de la sala
6. Odometría con codificadores de rueda, mapa de habitación, lidar
5. Trabajamos en rviz y gazebo: xacro, nuevos sensores.
4. Cree una simulación de robot utilizando los editores rviz y gazebo.
3. Acelera, cambia la cámara, arregla la marcha
2. Software
1. hierro

La última vez, diseñamos el proyecto como módulos xacro separados, agregamos una cámara de video virtual e imu (giroscopio).

En esta publicación, trabajaremos con odometría a partir de codificadores ópticos montados en ejes de ruedas, cargaremos un mapa de la habitación y lo montaremos en un carro robot real.

Odometria y tf


Lo que es odometría y tf y cómo se implementan generalmente en ROS ya está bien descrito en el recurso, por lo que nos referimos a los artículos relevantes en la parte de teoría, por ejemplo aquí .
Habiendo comenzado desde la base teórica, trabajaremos con práctica.

Comencemos trabajando en un robot carro conectándolo a través de VNC.

Vaya a la carpeta rosbots_driver y cree un nodo de archivo. Este archivo generará odometría, recibiéndolo desde codificadores ópticos, que a su vez lo envían a arduino uno y luego a raspberry pi.

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

Ponemos el código en el archivo:

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()


Guarde el archivo y hágalo ejecutable:
CTRL+X
chmod +x diff-tf.py

Ahora en el robot, ejecute los segundos nodos - driver y diff-tf:
1er terminal:

 python diff_tf.py

2do:

 rosrun rosbots_driver part2_cmr.py

En la tercera terminal, verificaremos que hay nuevos temas odom y tf:



Veamos con el comando rostopic echo odom qué se publica en el tema (y si se publica).
La salida será aproximadamente como sigue:



Ahora, sin cerrar los nodos en ejecución en el robot, iniciaremos la computadora de control con los entornos gráficos rviz y gazebo.

* Una imagen (máquina virtual VMWare con Ubuntu 16.04 + ROS Kinetic), que se ofreció previamente para descargar, contiene todo lo que necesita.

En la computadora de control (en adelante denominada "Computadora"), ejecute el modelo en rviz:

 roslaunch rosbots_description rviz.launch

Se cargará el modelo de robot cargado con el que trabajó en publicaciones anteriores:



Agregue dos pantallas a rviz haciendo clic en Agregar. La pantalla con odometría y la pantalla con tf, marque las casillas para visualizarlas.

En la ventana donde se representa el modelo de robot, aparecerán gráficos característicos:


* Para que sea más visible, puede desactivar la pantalla Robotmodel.

Controlamos el robot desde el teclado de la computadora y vemos cómo cambia la visualización de tf y la odometría.

Sin cerrar rviz en la segunda terminal, comenzaremos a controlar desde el teclado:

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

Al controlar el robot, la ventana con visualización mostrará: flecha roja (visualización del tema principal), líneas vectoriales (tema tf).

Si la flecha roja del tema principal muestra la dirección del movimiento del robot, entonces las líneas vectoriales tf muestran cómo se ubican los elementos individuales del robot en el espacio:

el video


Ahora, para continuar, necesita "ajustar" la odometría.
Para hacer esto, cierre el editor rviz e inícielo nuevamente, solo sin visualizar el modelo con el comando:

 rosrun rviz rviz

Esto es necesario para que solo base_link y odom permanezcan de los vectores del tema tf:



En rviz, una celda es de 1 metro. Por lo tanto, en realidad, el robot también debe pasar 1 metro para que los datos sean comparables.

Pasaremos 1 metro en el robot, controlándolo desde el teclado. En rviz, el robot también tiene que conducir 1 metro, una celda.

Si el robot viaja más de lo que debería en rviz, o viceversa, una distancia más corta que en la realidad, entonces necesita editar el archivo diff_tf.py que se creó anteriormente, a saber, este bloque:

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 algún lado, necesitas un mapa. Para los propósitos de nuestro robot, necesitamos un mapa de la habitación.
Trabajemos con ella.

Para cargar un mapa en rviz, debe crear una carpeta de mapa en el proyecto (rosbots_description) en la computadora (no en el robot) y colocar dos archivos que componen el mapa: map.pgm y map.yaml.
* De hecho, puede haber varios archivos de mapa en una carpeta, pero solo puede cargar uno en el asistente.

Un mapa en ROS consta de dos archivos, uno de los cuales es una imagen PGM, donde cada píxel es:

  • blanco: el espacio es libre;
  • negro: el espacio está ocupado por un obstáculo;
  • gris: el espacio aún no se ha explorado.

El segundo archivo .yaml es un archivo con la configuración del mapa, donde se indican sus dimensiones, la ocupación de píxeles con diferentes tipos (enumerados anteriormente) y otros parámetros.

Ejecute el nodo en la computadora que publicará la tarjeta:

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

En la terminal vecina, ejecute el modelo en rviz:

 roslaunch rosbots_description rviz.launch

En rviz, agregue una visualización de Mapa.

En rviz, el robot resultó ser desproporcionadamente grande y se encuentra fuera del mapa:



Para solucionar esto, debe ejecutar un mapa donde el tamaño de la celda será de 1 metro. Reinicie la tarjeta con el parámetro 1 al final:

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

Ahora puedes montar el mapa en rviz, controlando el robot desde el teclado:

el video


Entonces, lo que se logró :

  • recibir datos de odometría de los codificadores ópticos de rueda del robot y enviarlos a temas para su visualización en rviz;
  • configurar la odometría del robot para que coincida con la distancia recorrida en vivo y virtualmente;
  • cargar y mostrar un mapa de habitación.

Sin embargo, a pesar del hecho de que se muestra el mapa y el robot puede viajar sobre él con una odometría "ajustada", en realidad el robot es ciego. No ve obstáculos y tropezará con ellos. La segunda desventaja es que el mapa de la sala virtual cargado en rviz le permite viajar solo en todas las direcciones, incluso en aquellas donde los obstáculos se muestran claramente.

¿Cómo hacer que el robot "vea" obstáculos en la realidad y virtualmente?

Con un entorno virtual es más sencillo. Todo aquí se basa en el emulador-editor de gazebo. Y en publicaciones anteriores esto fue mencionado.

Es más complicado con la realidad. Necesitamos un elemento (sensor) que indique obstáculos e informe esto al sistema.

Una opción es poner lidar en el robot.

Lidar RPlidar A1


Utilizaremos la solución de presupuesto asequible y pondremos el LIDAR en el robot. Quizás esta solución sea más costosa que usar el mismo Kinect, pero, como lo ha demostrado la práctica, es más efectiva en términos de velocidad, precisión y facilidad de instalación (menos engorrosa). Además, es más fácil comenzar a trabajar con LIDAR, ya que No se requiere reflexión sobre cómo alimentarlo y conectarlo al proyecto (https://habr.com/en/company/tod/blog/210252/).

Necesitaremos el paquete ros para trabajar con lidar - wiki.ros.org/rplidar .
Con la ayuda del LIDAR, construiremos un mapa de la habitación y también lo usaremos en la navegación.

Cómo instalar rplidar en ROS tiene muchos artículos, por ejemplo aquí .

Usaremos el conocimiento de los ancianos canosos e instalaremos paquetes con lidar en el sistema en el robot :

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

En la computadora, instale el paquete para trabajar con la tarjeta:

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

Ejecute el paquete en el robot y compruebe si el LIDAR funciona:

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

* El primer comando da acceso al puerto usb donde está conectado el lidar.

Si todo salió bien, generará líneas en la consola:

 [ 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

Aquí configuramos inmediatamente un pequeño LIDAR, porque el sitio web oficial dice que (lidar) puede funcionar mejor.

Necesitamos lograr el resultado cuando el escaneo no es de 4.0K puntos, que se emiten por defecto, sino 8.0K. Esta opción mejorará ligeramente la calidad del escaneo.

Para esto, estableceremos un parámetro más en el paquete rplidar - modo de exploración:

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

Y despues

 <param name="angle_compensate" type="bool" value="true"/>
agrega la línea:

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

La segunda línea que debe corregirse aquí:

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

Reemplace el valor del láser con base_link.

* Ahora, si reinicia el nodo con el comando roslaunch rplidar_ros rplidar.launch, el resultado será diferente:

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

Echa un vistazo que muestra lidar en rviz.

Para hacer esto, ejecuta el robot:

 roslaunch rplidar_ros rplidar.launch

En una computadora:

 roslaunch rosbots_description rviz.launch

En rviz, agregue la pantalla LaserScan y seleccione el tema de escaneo. Además, se verá que los mensajes están cayendo en el tema:



En la ventana con la visualización del robot, el robot resultó ser un gigante. Con su tamaño, lo resolveremos más tarde. Ahora construyamos un mapa de habitación.

Para hacer esto, cree un paquete con un nodo:

 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 a ejecutarlo.

En el robot:

1er terminal: roslaunch rplidar_ros rplidar.launch
2 °: rosrun rosbots_driver part2_cmr.py

En una computadora:

1er terminal: roslaunch my_hector_mapping hector.launch
2do: roslaunch rosbots_description rviz.launch
3 °: rosrun teleop_twist_keyboard teleop_twist_keyboard.py /cmd_vel:=/part2_cmr/cmd_vel

En las pantallas necesita agregar un mapa, y Marco fijo seleccione base_link. Luego puede observar en tiempo real cómo el lidar "ilumina" el espacio a su alrededor:



En el paso actual, para construir un mapa, debe dar la vuelta a la habitación, "detenerse" en diferentes ángulos para que el LIDAR los marque en el mapa.

Por eso recomiendo libros de texto. Pero nuestro consejo es levantar el robot y caminar con él, sosteniéndolo frente a ti. Por lo tanto, la velocidad de construcción de un mapa será mayor en el sentido de que no tendrá que distraerse y mirar hacia dónde conducía el robot en la habitación contigua en ausencia de contacto visual.

Además, al girar el robot alrededor de su eje durante un viaje, el lidar deja artefactos negros característicos en aquellos lugares donde en realidad no hay obstáculos:



Después de construir el mapa, guárdelo con el comando:

 rosrun map_server map_saver -f map-1

Construir el mapa perfecto con un presupuesto lidar es un mito. Por lo tanto, ayudaremos al lidar en Photoshop. Eliminaremos los artefactos negros del mapa, donde realmente no hay obstáculos, y alinearemos las paredes con líneas negras:



No olvides guardar el mapa en formato .pgm.

Ahora repetimos en la computadora los comandos que estaban al principio de la publicación, pero con un nuevo mapa:
1er terminal: rosrun map_server maserver /home/pi/catkin_ws/src/rosbots_description/maps/map-1.pgm 0.05
2do: roslaunch rosbots_description rviz.launch

Resultado en rviz:



Se cargó el nuevo mapa, como el modelo de robot, pero el robot está fuera del mapa.

Hablaremos de esto más tarde, pero por ahora, resumamos:

  • dominar el lidar RP-lidar A1
  • construyendo un mapa de habitación usando un lidar, ajustándolo y cargándolo en el editor visual rviz.

Archivos para descargar: mapa de la habitación .

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

More articles:


All Articles