👘 🍡 👨🏽‍💼 Chariot de camion ROS. Partie 6. Odométrie avec encodeurs de roue, plan de salle, lidar 💖 ⛹🏾 🏧

Messages dans la série:
8. Nous contrôlons à partir du téléphone-ROS Control, GPS-node
7. Localisation du robot: gmapping, AMCL, points de référence sur le plan de la salle
6. Odométrie avec encodeurs de roue, plan de salle, lidar
5. Nous travaillons en rviz et gazebo: xacro, nouveaux capteurs.
4. Créez une simulation de robot à l'aide des éditeurs rviz et gazebo.
3. Accélérez, changez la caméra, corrigez la démarche
2. Logiciel
1. Fer

La dernière fois, nous avons conçu le projet en modules xacro séparés, ajouté une caméra vidéo virtuelle et un imu (gyroscope).

Dans cet article, nous travaillerons avec l'odométrie à partir d'encodeurs optiques montés sur des axes de roue, chargerons un plan de salle et le monterons sur un vrai chariot robot.

Odométrie et tf

Qu'est-ce que l'odométrie et le tf et comment ils sont généralement mis en œuvre dans ROS est déjà bien décrit dans la ressource, nous nous référons donc aux articles pertinents dans la partie théorique, par exemple, ici .
Partant de la base théorique, nous travaillerons avec la pratique.

Commençons par travailler sur un robot trolley en nous y connectant via VNC.

Accédez au dossier rosbots_driver et créez un nœud de fichier. Ce fichier va générer l'odométrie, le recevoir des encodeurs optiques, qui à son tour l'envoyer à Arduino Uno puis à Raspberry Pi.

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

Nous mettons le code dans le fichier:

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

Enregistrez le fichier et rendez-le exécutable:

CTRL+X
chmod +x diff-tf.py

Maintenant sur le robot, exécutez les 2e noeuds - driver et diff-tf:
1er terminal:

 python diff_tf.py

2e:

 rosrun rosbots_driver part2_cmr.py

Dans le 3ème terminal, nous vérifierons qu'il y a de nouveaux sujets odom et tf:

Voyons avec la commande rostopic echo odom ce qui est publié dans le sujet (et s'il est publié du tout).
La sortie sera approximativement la suivante:

Maintenant, sans fermer les nœuds en cours d'exécution sur le robot, nous allons lancer l'ordinateur de contrôle avec les environnements graphiques rviz et gazebo.

* Une image (machine virtuelle VMWare avec Ubuntu 16.04 + ROS Kinetic), précédemment proposée en téléchargement, contient tout ce dont vous avez besoin.

Sur l'ordinateur de contrôle (ci-après dénommé «ordinateur»), exécutez le modèle dans rviz:

 roslaunch rosbots_description rviz.launch

Le modèle de robot chargé qui a fonctionné dans les articles précédents chargera:

Ajoutez deux écrans à rviz en cliquant sur Ajouter. L'affichage avec odométrie et l'affichage avec tf, cochez les cases pour les visualiser.

Dans la fenêtre où le modèle de robot est représenté, des graphiques caractéristiques apparaîtront:

* Pour le rendre plus visible, vous pouvez désactiver l'affichage Robotmodel.

Nous contrôlons le robot depuis le clavier de l'ordinateur et voyons comment la visualisation du tf et de l'odométrie change.

Sans fermer rviz dans le 2ème terminal, nous allons commencer le contrôle depuis le clavier:

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

Lors du contrôle du robot, la fenêtre de visualisation affichera: flèche rouge (visualisation du sujet odom), lignes vectorielles (sujet tf).

Si la flèche rouge du sujet odom indique la direction du mouvement du robot, alors les lignes vectorielles tf montrent comment les éléments individuels du robot sont situés dans l'espace:

la vidéo

Maintenant, pour continuer, vous devez «régler» l'odométrie.
Pour ce faire, fermez l'éditeur rviz et redémarrez-le, uniquement sans visualiser le modèle avec la commande:

 rosrun rviz rviz

Ceci est nécessaire pour que seuls base_link et odom restent des vecteurs du sujet tf:

En rviz, une cellule mesure 1 mètre. Par conséquent, en réalité, le robot doit également passer 1 mètre pour que les données soient comparables.

Nous passerons 1 mètre sur le robot, en le contrôlant depuis le clavier. En rviz, le robot doit également conduire 1 mètre - une cellule.

Si le robot voyage plus longtemps qu'il ne devrait l'être dans rviz, ou vice versa, une distance plus courte qu'en réalité, alors vous devez éditer le fichier diff_tf.py qui a été créé précédemment, à savoir ce bloc:

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

La carte

Pour aller quelque part, vous avez besoin d'une carte. Pour les besoins de notre robot - nous avons besoin d'un plan de salle.
Travaillons avec elle.

Pour télécharger une carte sur rviz, vous devez créer un dossier de carte dans le projet (rosbots_description) sur l'ordinateur (pas sur le robot) et y mettre deux fichiers qui composent la carte: map.pgm et map.yaml.
* En fait, il peut y avoir plusieurs fichiers de carte dans un dossier, mais vous ne pouvez en télécharger qu'un dans l'assistant.

Une carte dans ROS se compose de deux fichiers, dont l'un est une image PGM, où chaque pixel est soit:

blanc - l'espace est libre;
noir - l'espace est occupé par un obstacle;
l'espace gris n'a pas encore été exploré.

Le deuxième fichier .yaml est un fichier avec les paramètres de la carte, où ses dimensions, l'occupation des pixels avec différents types (répertoriés ci-dessus), d'autres paramètres sont indiqués.

Exécutez le nœud sur l'ordinateur qui publiera la carte:

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

Dans le terminal voisin, exécutez le modèle dans rviz:

 roslaunch rosbots_description rviz.launch

Dans rviz, ajoutez un affichage de carte.

Dans rviz, le robot s'est avéré être de taille disproportionnée et est situé hors de la carte:

Pour résoudre ce problème, vous devez exécuter une carte où la taille des cellules sera de 1 mètre. Redémarrez la carte avec le paramètre 1 à la fin:

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

Vous pouvez maintenant parcourir la carte dans rviz, en contrôlant le robot depuis le clavier:

la vidéo

Alors, ce qui a été réalisé :

recevoir les données d'odométrie des encodeurs de roue optique du robot et les envoyer aux sujets pour les afficher dans rviz;
configurer l'odométrie du robot pour correspondre à la distance parcourue en direct et virtuellement;
charger et afficher un plan de salle.

Cependant, malgré le fait que la carte soit affichée et que le robot puisse y rouler avec une odométrie «réglée», en réalité le robot est aveugle. Il ne voit aucun obstacle et tombera sur eux. Le deuxième inconvénient est le plan de salle virtuel chargé dans rviz vous permet de rouler seul dans toutes les directions, même dans celles où les obstacles sont clairement indiqués.

Comment faire «voir» les obstacles au robot dans la réalité et virtuellement?

Avec un environnement virtuel, c'est plus simple. Tout ici est basé sur l'émulateur-éditeur de gazebo. Et dans les messages précédents, cela a été mentionné.

C'est plus compliqué avec la réalité. Nous avons besoin d'un élément (capteur) qui indiquera les obstacles et le signalera au système.

Une option consiste à mettre du lidar sur le robot.

Lidar RPlidar A1

Nous utiliserons la solution économique et mettrons le lidar sur le robot. Peut-être que cette solution sera plus chère que d'utiliser le même Kinect, mais, comme la pratique l'a montré, elle est plus efficace en termes de vitesse, de précision et de facilité d'installation (moins lourde). De plus, il est plus facile de commencer à travailler avec le lidar, car Aucune réflexion n'est nécessaire sur la façon de l'alimenter et de le connecter au projet (https://habr.com/en/company/tod/blog/210252/).

Nous aurons besoin du paquet ros pour travailler avec lidar - wiki.ros.org/rplidar .
Avec l'aide du lidar, nous allons construire une carte de la pièce et l'utiliser également dans la navigation.

Comment installer rplidar dans ROS a beaucoup d'articles, par exemple ici .

Nous utiliserons les connaissances des vieillards aux cheveux gris et installerons des packages avec lidar dans le système du robot :

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

Sur l' ordinateur, installez le package pour travailler avec la carte:

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

Exécutez le package sur le robot et vérifiez si le lidar fonctionne:

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

* La première commande donne accès au port USB où le lidar est connecté.

Si tout s'est bien passé, il affichera des lignes sur la 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

Ici, nous configurons immédiatement un petit lidar, car le site officiel dit qu'il (lidar) peut mieux fonctionner.

Nous devons obtenir la sortie lorsque la numérisation n'est pas de 4,0K points, qui sont émis par défaut, mais de 8,0K. Cette option améliorera légèrement la qualité de la numérisation.

Pour cela, nous allons définir un paramètre supplémentaire dans le package rplidar - mode scan:

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

Et après

 <param name="angle_compensate" type="bool" value="true"/>

ajoutez la ligne:

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

La deuxième ligne qui doit être corrigée ici:

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

Remplacez la valeur laser par base_link.

* Maintenant, si vous redémarrez le nœud avec la commande roslaunch rplidar_ros rplidar.launch, la sortie sera différente:

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

Jetez un oeil. qui affiche lidar dans rviz.

Pour ce faire, exécutez sur le robot:

 roslaunch rplidar_ros rplidar.launch

Sur un ordinateur:

 roslaunch rosbots_description rviz.launch

Dans rviz, ajoutez l'affichage LaserScan et sélectionnez la rubrique de numérisation. De plus, on verra que les messages tombent dans le sujet:

Dans la fenêtre avec la visualisation du robot, le robot s'est avéré être un géant. Avec sa taille, nous le découvrirons plus tard. Construisons maintenant un plan de salle.

Pour ce faire, créez un package avec un nœud:

 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

Lançons-le.

Sur le robot:

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

Sur un ordinateur:

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

Dans les écrans, vous devez ajouter une carte et un cadre fixe sélectionnez base_link. Ensuite, vous pouvez observer en temps réel comment le lidar "illumine" l'espace autour de lui:

À l'étape actuelle, pour construire une carte, vous devez faire le tour de la pièce, en vous «arrêtant» à différents angles pour que le lidar les marque sur la carte.

Je recommande donc les manuels. Mais notre conseil est de prendre le robot et de marcher avec lui, en le tenant devant vous. Ainsi, la vitesse de construction d'une carte sera plus grande dans le sens où vous n'aurez pas à être distrait et à regarder où le robot a conduit dans la pièce voisine en l'absence de contact visuel.

De plus, lors de la rotation du robot autour de son axe pendant un voyage, le lidar laisse des artefacts noirs caractéristiques dans les endroits où il n'y a en fait aucun obstacle:

Après avoir construit la carte, enregistrez-la avec la commande:

 rosrun map_server map_saver -f map-1

Construire la carte parfaite avec un lidar économique est un mythe. Par conséquent, nous allons aider le lidar dans Photoshop. Nous allons supprimer les artefacts noirs de la carte, où il n'y a vraiment aucun obstacle, et aligner les murs avec des lignes noires:

N'oubliez pas de sauvegarder la carte au format .pgm.

Maintenant, nous répétons sur l'ordinateur les commandes qui étaient au début du post, mais avec une nouvelle carte:
1er terminal: rosrun map_server maserver /home/pi/catkin_ws/src/rosbots_description/maps/map-1.pgm 0.05
2e: roslaunch rosbots_description rviz.launch

Résultat dans rviz:

La nouvelle carte est chargée, comme le modèle de robot dessus, mais le robot est en dehors de la carte.

Nous en reparlerons plus tard, mais pour l'instant, résumons:

maîtriser le lidar RP-lidar A1
construire un plan de pièce à l'aide d'un lidar, l'ajuster et le charger dans l'éditeur visuel rviz.

Fichiers à télécharger: plan de salle .

Chariot de camion ROS. Partie 6. Odométrie avec encodeurs de roue, plan de salle, lidar

Odométrie et tf

La carte

Lidar RPlidar A1

More articles: