⤵️ 📲 👒 Troli truk ROS. Bagian 6. Odometri dengan enkoder roda, peta ruang, lidar 💔 🥘 👩🏼‍🏫

Posting dalam seri:
8. Kami mengontrol dari ponsel-ROS Control, GPS-node
7. Lokalisasi robot: gmapping, AMCL, titik referensi pada peta kamar
6. Odometri dengan enkoder roda, peta ruang, lidar
5. Kami bekerja di rviz dan gazebo: xacro, sensor baru.
4. Buat simulasi robot menggunakan editor rviz dan gazebo.
3. Mempercepat, mengganti kamera, memperbaiki gaya berjalan
2. Perangkat Lunak
1. Besi

Terakhir kali, kami merancang proyek sebagai modul xacro terpisah, menambahkan kamera video virtual dan imu (gyroscope).

Dalam posting ini kami akan bekerja dengan odometry dari encoders optik yang dipasang pada poros roda, memuat peta ruang dan mengendarainya dengan troli robot asli.

Odometri dan tf

Apa itu odometry dan tf dan bagaimana mereka biasanya diimplementasikan dalam ROS sudah dijelaskan dengan baik pada sumber daya, jadi kami merujuk pada artikel yang relevan di bagian teori, misalnya, di sini .
Setelah memulai dari landasan teori, kami akan bekerja dengan latihan.

Mari kita mulai dengan mengerjakan robot troli dengan menghubungkannya melalui VNC.

Buka folder rosbots_driver dan buat simpul file. File ini akan menghasilkan odometry, menerimanya dari pengkode optik, yang pada gilirannya mengirimkannya ke arduino uno dan kemudian ke raspberry pi.

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

Kami memasukkan kode ke dalam file:

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

Simpan file dan buat itu dapat dieksekusi:

CTRL+X
chmod +x diff-tf.py

Sekarang pada robot, jalankan 2 node - driver dan diff-tf:
Terminal 1:

 python diff_tf.py

 rosrun rosbots_driver part2_cmr.py

Di terminal ke-3, kami akan memverifikasi bahwa ada topik odom dan tf baru:

Mari kita lihat dengan perintah oost rostopic odom apa yang diterbitkan dalam topik (dan apakah itu diterbitkan sama sekali).
Outputnya kira-kira sebagai berikut:

Sekarang, tanpa menutup node yang berjalan pada robot, kami akan meluncurkan komputer kontrol dengan lingkungan grafis rviz dan gazebo.

* Gambar (mesin virtual VMWare dengan Ubuntu 16.04 + ROS Kinetic), yang sebelumnya ditawarkan untuk diunduh, berisi semua yang Anda butuhkan.

Pada komputer kontrol (selanjutnya disebut "Komputer"), jalankan model di rviz:

 roslaunch rosbots_description rviz.launch

Model robot yang dimuat dengan yang bekerja di posting sebelumnya akan memuat:

Tambahkan dua tampilan ke rviz dengan mengklik Tambah. Layar dengan odometri dan layar dengan tf, centang kotak untuk memvisualisasikannya.

Di jendela tempat model robot digambarkan, grafik karakteristik akan muncul:

* Untuk membuatnya lebih terlihat, Anda dapat mematikan tampilan Robotmodel.

Kami mengontrol robot dari keyboard Komputer dan melihat bagaimana visualisasi perubahan dan odometri.

Tanpa menutup rviz di terminal ke-2, kami akan memulai kontrol dari keyboard:

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

Saat mengendalikan robot, jendela dengan visualisasi akan menunjukkan: panah merah (visualisasi topik odom), garis vektor (topik tf).

Jika panah merah dari topik odom menunjukkan arah pergerakan robot, garis vektor jika menunjukkan bagaimana elemen individu robot berada di ruang:

videonya

Sekarang, untuk melanjutkan, Anda perlu "menyetel" odometri.
Untuk melakukan ini, tutup editor rviz dan mulai lagi, hanya tanpa memvisualisasikan model dengan perintah:

 rosrun rviz rviz

Ini diperlukan agar hanya base_link dan odom tetap dari vektor topik tf:

Di rviz, satu sel adalah 1 meter. Karena itu, pada kenyataannya, robot juga harus melewati 1 meter agar data dapat dibandingkan.

Kami akan melewati 1 meter pada robot, mengendalikannya dari keyboard. Di rviz, robot juga harus menggerakkan 1 meter - satu sel.

Jika robot melakukan perjalanan lebih lama dari yang seharusnya di rviz, atau sebaliknya, jarak yang lebih pendek dari kenyataannya, maka Anda perlu mengedit file diff_tf.py yang sebelumnya dibuat, yaitu blok ini:

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

Peta

Untuk pergi ke suatu tempat, Anda memerlukan peta. Untuk keperluan robot kami - kami membutuhkan peta kamar.
Mari kita bekerja dengannya.

Untuk mengunggah peta ke rviz, Anda perlu membuat folder peta di proyek (rosbots_description) di Komputer (bukan di robot) dan memasukkan dua file ke dalamnya yang membentuk peta: map.pgm dan map.yaml.
* Faktanya, mungkin ada beberapa file peta dalam folder, tetapi Anda hanya dapat mengunggah satu ke wizard.

Peta di ROS terdiri dari dua file, salah satunya adalah gambar PGM, di mana setiap pikselnya adalah:

ruang putih gratis;
ruang hitam ditempati oleh penghalang;
abu - abu - ruang belum dieksplorasi.

File .yaml kedua adalah file dengan pengaturan peta, di mana dimensinya, hunian piksel dengan berbagai jenis (tercantum di atas), parameter lainnya ditunjukkan.

Jalankan node di komputer yang akan menerbitkan kartu:

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

Di terminal tetangga, jalankan model di rviz:

 roslaunch rosbots_description rviz.launch

Di rviz, tambahkan tampilan Peta.

Di rviz, robot tersebut ternyata berukuran sangat besar, dan terletak di luar peta:

Untuk memperbaiki ini, Anda perlu menjalankan peta di mana ukuran sel akan 1 meter. Mulai ulang kartu dengan parameter 1 di akhir:

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

Sekarang Anda bisa naik peta di rviz, mengendalikan robot dari keyboard:

videonya

Jadi, apa yang dicapai :

menerima data odometry dari enkoder roda optik robot dan mengirimkannya ke topik untuk ditampilkan di rviz;
mengkonfigurasi robot odometri agar sesuai dengan jarak tempuh langsung dan hampir;
memuat dan menampilkan peta ruang.

Namun, terlepas dari kenyataan bahwa peta ditampilkan dan robot dapat naik di atasnya dengan odometry "disetel", pada kenyataannya robot itu buta. Dia tidak melihat rintangan dan akan menemukan mereka. Kekurangan kedua adalah peta ruang virtual yang dimuat di rviz memungkinkan Anda untuk berkendara sendiri ke segala arah, bahkan di tempat-tempat di mana hambatan ditampilkan dengan jelas.

Bagaimana cara membuat robot "melihat" hambatan dalam kenyataan dan secara virtual?

Dengan lingkungan virtual lebih sederhana. Semuanya di sini didasarkan pada emulator-editor gazebo. Dan dalam posting sebelumnya ini disebutkan.

Ini lebih rumit dengan kenyataan. Kami membutuhkan elemen (sensor) yang akan menunjukkan hambatan dan melaporkannya ke sistem.

Salah satu opsi adalah dengan meletakkan lidar pada robot.

Lidar RPlidar A1

Kami akan menggunakan solusi anggaran yang terjangkau dan menempatkan Lidar pada robot. Mungkin solusi ini akan lebih mahal daripada menggunakan Kinect yang sama, tetapi, seperti yang telah ditunjukkan oleh praktik, ini lebih efektif dalam hal kecepatan, ketepatan dan kemudahan pemasangan (kurang rumit). Selain itu, lebih mudah untuk mulai bekerja dengan LIDAR, seperti Tidak diperlukan refleksi tentang cara menyalakannya dan menghubungkannya ke proyek (https://habr.com/en/company/tod/blog/210252/).

Kami membutuhkan paket ros untuk bekerja dengan lidar - wiki.ros.org/rplidar .
Dengan bantuan lidar, kami akan membangun peta ruangan, dan juga menggunakannya dalam navigasi.

Cara menginstal rplidar di ROS memiliki banyak artikel, misalnya di sini .

Kami akan menggunakan pengetahuan pria tua berambut abu-abu dan menginstal paket dengan LIDAR dalam sistem pada robot :

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

Di komputer, instal paket untuk bekerja dengan kartu:

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

Jalankan paket pada robot dan periksa apakah lidar berfungsi:

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

* Perintah pertama memberikan akses ke port usb di mana LIDAR terhubung.

Jika semuanya berjalan lancar, maka akan menampilkan baris ke konsol:

 [ 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

Di sini kita segera mengkonfigurasi Lidar kecil, karena situs web resmi mengatakan bahwa itu (LIDAR) dapat bekerja lebih baik.

Kita perlu mencapai output saat pemindaian bukan 4.0K poin, yang dikeluarkan secara default, tetapi 8.0K. Opsi ini akan sedikit meningkatkan kualitas pemindaian.

Untuk ini, kita akan menetapkan satu parameter lagi dalam paket rplidar - mode pindai:

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

Dan sesudahnya

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

tambahkan baris:

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

Baris kedua yang perlu diperbaiki di sini:

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

Ganti nilai laser dengan base_link.

* Sekarang, jika Anda me-restart node dengan perintah roslaunch rplidar_ros rplidar.launch, hasilnya akan berbeda:

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

Lihatlah. yang menampilkan LIDAR di rviz.

Untuk melakukan ini, jalankan di robot:

 roslaunch rplidar_ros rplidar.launch

Di Komputer:

 roslaunch rosbots_description rviz.launch

Di rviz, tambahkan tampilan LaserScan dan pilih topik pemindaian. Lebih lanjut akan terlihat bahwa pesan masuk ke dalam topik:

Di jendela dengan visualisasi robot, robot itu ternyata raksasa. Dengan ukurannya, kita akan mencari tahu nanti. Sekarang mari kita membangun peta ruang.

Untuk melakukan ini, buat paket dengan simpul:

 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

Mari kita jalankan.

Pada robot:

Terminal 1: roslaunch rplidar_ros rplidar.launch
2: rosrun rosbots_driver part2_cmr.py

Di Komputer:

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

Di layar Anda perlu menambahkan peta, dan Tetap bingkai pilih base_link. Kemudian Anda dapat mengamati secara real time bagaimana lidar "menyinari" ruang di sekitarnya:

Pada langkah saat ini, untuk membangun peta, Anda harus berkeliling ruangan, "berhenti" pada sudut yang berbeda sehingga Lidar menandainya di peta.

Jadi rekomendasikan buku pelajaran. Tetapi saran kami adalah untuk mengambil robot dan berjalan dengannya, memegangnya di depan Anda. Jadi kecepatan membangun peta akan lebih besar dalam arti bahwa Anda tidak perlu terganggu dan melihat di mana robot melaju di kamar sebelah tanpa adanya kontak visual.

Selain itu, ketika memutar robot di sekitar porosnya selama perjalanan, Lidar meninggalkan karakteristik artefak hitam di tempat-tempat di mana sebenarnya tidak ada hambatan:

Setelah membangun peta, simpan dengan perintah:

 rosrun map_server map_saver -f map-1

Membangun peta yang sempurna dengan anggaran lidar adalah mitos. Oleh karena itu, kami akan membantu Lidar di Photoshop. Kami akan menghapus artefak hitam dari peta, di mana sebenarnya tidak ada hambatan, dan sejajarkan dinding dengan garis hitam:

Jangan lupa untuk menyimpan peta dalam format .pgm.

Sekarang kita ulangi di komputer perintah yang ada di awal posting, tetapi dengan peta baru:
Terminal 1: rosrun map_server maserver /home/pi/catkin_ws/src/rosbots_description/maps/map-1.pgm 0.05
2: roslaunch rosbots_description rviz.launch

Hasil dalam rviz:

Peta baru dimuat, seperti model robot di atasnya, tetapi robot di luar peta.

Kami akan membicarakan ini nanti, tetapi untuk sekarang, mari kita simpulkan:

menguasai lidar RP-lidar A1
membangun peta kamar menggunakan lidar, menyesuaikannya dan memuatnya ke editor visual rviz.

File untuk diunduh: peta kamar .

Troli truk ROS. Bagian 6. Odometri dengan enkoder roda, peta ruang, lidar

Odometri dan tf

Peta

Lidar RPlidar A1

More articles: