Dalam artikel terakhir tentang kereta otonom rumah 2.0, kami berhasil bekerja pada bagaimana meningkatkan odometer robot anggaran, mencapai pembangunan peta ruang 2d yang dapat diterima menggunakan algoritma slam dan lidar yang terjangkau, memperjelas masalah lain ketika membangun proyek. Kali ini, mari kita lihat bagaimana navigasi otonom bekerja di rviz, apa yang bertanggung jawab, kami akan mengimplementasikan program yang memungkinkan Anda meninggalkan rviz. Pertimbangkan juga beberapa "elemen kecantikan" rviz yang membuat hidup lebih mudah untuk robot ROS.
Mari kita mulai dengan latihan.
Tugas robot : untuk melakukan perjalanan secara independen dari titik A ke titik B, atau lebih baik, langsung ke beberapa titik B.Diasumsikan bahwa skrip (node) proyek linorobot di sekitar mana proyek dibangun akan diluncurkan pada robot, dan visualisasi dari apa yang terjadi dan kontrol akan dilakukan. dari PC eksternal, mesin virtual (selanjutnya disebut "PC").Dalam kasus saya, dari perangkat lunak pada kereta, yang jantungnya adalah raspberry pi 3b single-board, diinstal: Ubuntu 16.04, ROS Kinetic, pada PC: Ubuntu 18.04, ROS Melodic.Sebelum Anda pergi ke suatu tempat, Anda harus mengatur dimensi robot - panjang dan lebar.Untuk melakukan ini, seperti yang ditunjukkan pada halaman proyek, Anda harus memasukkan nilai yang sesuai:roscd linorobot/param/navigation
nano costmap_common_params.yaml
Parameter jejak kaki yang benar:footprint: [[-x, -y], [-x, y], [x, y], [x, -y]]
di mana x = panjang robot / 2 dan y = lebar / 2* Idealnya, untuk robot, Anda dapat menggambar model urdf, seperti yang sebelumnya dilakukan dalam proyek kereta robot sebelumnya .Namun, jika ini tidak dilakukan, navigasi tidak akan banyak menderita, tetapi alih-alih model robot, "sumbu TF" akan melakukan perjalanan di peta. Cara memperbaiki situasi dengan representasi visual robot pada peta sedikit dan pada saat yang sama tidak membuang waktu membangun model urdf, kita akan berbicara lebih jauh.Kami memulai sebuah lingkungan
Pada robot, jalankan di dua terminal yang berbeda:roslaunch linorobot bringup.launch
roslaunch linorobot navigate.launch
Di PC, jalankan rviz visual shell:roscd lino_visualize/rviz
rviz -d navigate.rviz
Jika semuanya berjalan dengan baik, pada PC Anda dapat mengamati lokasi robot di rviz:
Sumbu TF terlihat di rviz, serta opsi Poligon, yang dapat ditambahkan pada panel Display. Poligon - ini hanya ukuran tapak robot yang diperkenalkan di awal, yang memungkinkan Anda untuk melihat dimensi robot. * Sumbu TF terlihat dari sumbu pada gambar: TF-map - map dan TF-base-link - robot itu sendiri, sisa sumbu diredam dalam rviz agar tidak bergabung satu sama lain.Data dari LIDAR sedikit tidak bertepatan dengan dinding pada peta, robot tidak terlokalisasi.Jika Anda beralih dari posisi ini, algoritme akan mencoba menyesuaikan, tetapi Anda juga dapat membantu dengan menunjukkan posisi awal secara langsung di rviz menggunakan tombol “perkiraan pose 2d”:
Selanjutnya, di jendela visualisasi rviz pada peta, tentukan lokasi awal dan arah robot dengan panah hijau besar - pose awal (Anda dapat melakukan ini berkali-kali):
Setelah menentukan posisi, base_link akan bergerak sedikit relatif ke peta:
Keakuratan absolut dari coincidence dari perbatasan dari lidar dengan peta ketika secara manual menyesuaikan posisi awal tidak dengan posisi awal. diperlukan, algoritma kemudian akan memperbaiki semuanya sendiri.Sebelum kamu pergi
Setiap kali, masukkan rviz posisi awal, jika di tanah robot mulai dari tempat yang sama pekerjaan begitu-begitu. Oleh karena itu, kami akan menggunakan layanan (ingat bahwa selain topik di ROS ada layanan), saat diakses, ia akan membaca "lokasi robot di peta", maka kami akan menggunakan data ini.Buat skrip pada PC (tidak masalah di tempat mana)get_pose.py
import rospy
from std_srvs.srv import Empty, EmptyResponse
from geometry_msgs.msg import PoseWithCovarianceStamped, Pose
robot_pose = Pose()
def service_callback(request):
print ("Robot Pose:")
print (robot_pose)
return EmptyResponse()
def sub_callback(msg):
global robot_pose
robot_pose = msg.pose.pose
rospy.init_node('service_server')
my_service = rospy.Service('/get_pose_service', Empty , service_callback)
sub_pose = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, sub_callback)
rospy.spin()
Setelah memulai, skrip di terminal tidak menampilkan apa pun dan diam-diam menunggu untuk diakses. Karena di rviz menggunakan antarmuka grafis, kami baru-baru ini mengatur posisi awal robot, Anda dapat merujuk ke skrip baru kami dan mengetahui posisi ini. Tanpa menutup terminal dengan skrip (seperti terminal yang diluncurkan sebelumnya dengan node), di terminal lain kami akan menjalankan:rosservice call /get_pose_service
Setelah menjalankan permintaan di terminal dengan skrip, get_pose.py akan menampilkan posisi robot:
Simpan indikator ini dan tambahkan ke pengaturan simpul amcl pada robot, sehingga robot mulai dari posisi yang ditentukan setiap kali:nano linorobot/launch/include/amcl.launch
Menambahkanparameter<param name="initial_pose_x" value="0.548767569629"/>
<param name="initial_pose_y" value="0.218281839179"/>
<param name="initial_pose_z" value="0.0"/>
<param name="initial_orientation_z" value="0.128591756735"/>
<param name="initial_orientation_w" value="0.991697615254"/>
Sekarang saat startup, robot akan mulai dari posisi tertentu, dan itu juga tidak perlu diatur secara manual di rviz:
Akhirnya pergi
Mulai ulang perintah untuk robot dan PC yang diberikan di awal dan lihat robot di rviz.Kami akan menggunakan rviz "Tujuan 2D NAV" untuk mengirim robot ke mana pun yang diinginkan jiwa:
Seperti yang Anda lihat, robot tidak hanya dapat menunjukkan jarak perjalanan, tetapi juga arah gerakan.Dalam contoh di atas, kami melaju sedikit ke depan, berbalik dan kembali.Di rviz, kawanan "panah hijau" Amcl yang berkerumun partikel juga terlihat, yang menunjukkan di mana, menurut program, robot berada. Setelah perjalanan, konsentrasi mereka mendekati lokasi sebenarnya dari robot, jumlah total berkurang. Robot berjalan cukup cepat - 0,3 m / s, sehingga program ini membutuhkan lebih banyak waktu untuk melokalisasi robot.Menavigasi di rviz menggunakan "2D NAV Goal" untuk memahami seberapa jauh dan ke arah mana robot akan melakukan perjalanan nyaman. Tetapi yang lebih nyaman adalah perjalanan tanpa partisipasi operator di rviz.Karena itu, kami menyoroti di peta tempat-tempat menarik di mana kami akan pergi. Untuk melakukan ini, pindahkan robot ke titik yang diinginkan di peta menggunakan "2D NAV Goal". Pada titik-titik ini ia akan mengakses layanan get_pose.py. Selanjutnya, koordinat semua titik akan ditransfer ke program perjalanan.Mengapa tidak memindahkan robot di tangan Anda ke tempat menarik? Masalahnya adalah "Amcl particle swarm", algoritma yang membangunnya. Sekalipun robot berada di tempat tujuan, dan posisinya ditentukan secara manual, varian lokasinya akan "disemprotkan" ke area di sekitar tempat tujuan itu sendiri. Karena itu, Anda masih harus pergi.Mari kita mulai dari titik dengan nama sandi "koridor":
Kami akan menyimpan pose ketika robot berada di titik yang diinginkan menggunakan panggilan ke layanan yang dibuat sebelumnya:rosservice call /get_pose_service
Berikutnya - "pintu masuk":
Dan titik terakhir - "dapur":
Sekarang saatnya untuk membuat program perjalanan -MoveTBtoGoalPoints.py
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
"""
pose x,y,x; orientation: x,y,z,w
"""
GoalPoints = [ [(5.31514576482, 1.36578380326, 0.0), (0.0, 0.0, -0.498454937648, 0.866915610157)],
[(5.77521810772, -1.2464049907, 0.0), (0.0, 0.0, -0.959487358308, 0.281751680114)],
[(2.14926455346, -2.7055208156, 0.0), (0.0, 0.0, -0.99147032254, 0.130332649487)]]
def assign_goal(pose):
goal_pose = MoveBaseGoal()
goal_pose.target_pose.header.frame_id = 'map'
goal_pose.target_pose.pose.position.x = pose[0][0]
goal_pose.target_pose.pose.position.y = pose[0][1]
goal_pose.target_pose.pose.position.z = pose[0][2]
goal_pose.target_pose.pose.orientation.x = pose[1][0]
goal_pose.target_pose.pose.orientation.y = pose[1][1]
goal_pose.target_pose.pose.orientation.z = pose[1][2]
goal_pose.target_pose.pose.orientation.w = pose[1][3]
return goal_pose
if __name__ == '__main__':
rospy.init_node('MoveTBtoGoalPoints')
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
client.wait_for_server()
for TBpose in GoalPoints:
TBgoal = assign_goal(TBpose)
client.send_goal(TBgoal)
client.wait_for_result()
* Di GoalPoints, kami akan menambahkan koordinat sebelumnya dari titik perjalanan. Ada penjelasan dalam kode di mana urutan parameter: pose x, y, x; orientasi: x, y, z, w - [(5.31514576482, 1.36578380326, 0.0), (0.0, 0.0, -0.498454937648, 0.866915610157)]Jalankan skrip MoveTBtoGoalPoints.py bersamaan dengan node yang sedang berjalan di robot dan PC, lihat hasilnya:
Seperti yang Anda lihat , robot berhasil melaju ke lorong dan dalam perjalanan ke dapur melihat dirinya di cermin dan macet. Baik dengan larsar saat bertemu dengan cermin, sayangnya begitu.Apa yang bisa dilakukan?Dimungkinkan untuk menambahkan titik perantara sebelum dan sesudah cermin, mengurangi kecepatan robot, menempelkan pita buram pada cermin di sepanjang rute.Tetapi ini adalah langkah selanjutnya, tetapi untuk sekarang kita akan mempertimbangkan tugas bepergian dari titik A ke titik B dengan dan tanpa bantuan rviz selesai. Sekarang, pada prinsipnya, kita tidak perlu PC eksternal, karena skrip perjalanan membawa kita ke titik yang diinginkan di peta.Tapi jangan terburu-buru untuk menjatuhkan PC bersama dengan rviz, karena proyek linorobot tidak berfungsi dengan benar "di luar kotak", dan Anda perlu "bekerja dengan file" pada tumpukan navigasi.Inilah saatnya untuk menolak - izinkan saya mengatakan, di mana navigasi otonom, robot melaju dan melangkah maju, menyamping, mundur, dan bahkan melaju ke cermin?Yah, itu saja, kecuali untuk beberapa detail yang membuat ROS menonjol.Mari kita lihat detail-detail ini.Misalkan beberapa warga negara yang tidak bertanggung jawab meninggalkan benda mati di sepanjang jalur robot. Item ini tidak ada di peta yang diberikan kepada robot untuk navigasi:
Apa yang akan terjadi?Mari kita lihat:
Seperti yang Anda lihat, Lidar melihat hambatan, algoritma menandainya di peta dan memesan jalan memutar.Akhirnya, tambahkan beberapa keindahan ke rviz
Editor visual rviz memiliki plugin menarik yang tidak diinstal secara default. Penggunaannya dapat menambah banyak poin menarik saat bekerja dengan rviz.Repositori dengan plugins ada di sini .Pasang pengaya pada PC kami di lingkungan kami:roscd; cd ../src;
catkin_create_pkg my_rviz_markers rospy
cd my_rviz_markers/
cd src
git clone https://github.com/jsk-ros-pkg/jsk_visualization
git clone https://github.com/ClementLeBihan/rviz_layer
cd ..
rosdep install --from-paths -y -r src
cd ~/linorobot_ws/
catkin_make
Setelah instalasi di rviz, plugin tambahan akan muncul.Sekarang, mari kita tambahkan, misalnya, ikon:
Di folder my_rviz_markers / samples, buat skrip:pictogram_array_objects_demo.py
import rospy
import math
from jsk_rviz_plugins.msg import Pictogram, PictogramArray
from random import random, choice
rospy.init_node("pictogram_object_demo_node")
p = rospy.Publisher("/pictogram_array", PictogramArray, queue_size=1)
r = rospy.Rate(1)
actions = [Pictogram.JUMP, Pictogram.JUMP_ONCE, Pictogram.ADD,
Pictogram.ROTATE_X, Pictogram.ROTATE_Y, Pictogram.ROTATE_Z]
pictograms = ["home","fa-robot"]
object_frames = ["map","base_link"]
while not rospy.is_shutdown():
arr = PictogramArray()
arr.header.frame_id = "/map"
arr.header.stamp = rospy.Time.now()
for index, character in enumerate(pictograms):
msg = Pictogram()
msg.header.frame_id = object_frames[index]
msg.action = actions[2]
msg.header.stamp = rospy.Time.now()
msg.pose.position.x = 0
msg.pose.position.y = 0
msg.pose.position.z = 0.5
msg.pose.orientation.w = 0.7
msg.pose.orientation.x = 0
msg.pose.orientation.y = -0.7
msg.pose.orientation.z = 0
msg.size = 0.5
msg.color.r = 25 / 255.0
msg.color.g = 255 / 255.0
msg.color.b = 240 / 255.0
msg.color.a = 1.0
msg.character = character
arr.pictograms.append(msg)
p.publish(arr)
r.sleep()
Mari kita buat itu dapat dieksekusi:sudo chmod +x pictogram_array_objects_demo.py
Di folder my_rviz_markers / launch, buat peluncuran untuk memulai:<launch>
<node pkg="my_rviz_markers" type="pictogram_array_objects_demo.py" name="pictogram_array_objects_demo" </node>
</launch>
Itu dimulai seperti biasa (semua node lain saat ini juga harus bekerja):roslaunch my_rviz_markers pictogram_array_objects_demo.launch
Letakkan penanda di peta
Marker dapat membantu dalam memahami di mana titik tersebut ada di peta.Seperti pada contoh sebelumnya, buat skrip di folder my_rviz_markers / samples:basic_marker.py
import rospy
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point
class MarkerBasics(object):
def __init__(self):
self.marker_objectlisher = rospy.Publisher('/marker_basic', Marker, queue_size=1)
self.rate = rospy.Rate(1)
self.init_marker(index=0,z_val=0)
def init_marker(self,index=0, z_val=0):
self.marker_object = Marker()
self.marker_object.header.frame_id = "/map"
self.marker_object.header.stamp = rospy.get_rostime()
self.marker_object.ns = "start"
self.marker_object.id = index
self.marker_object.type = Marker.SPHERE
self.marker_object.action = Marker.ADD
my_point = Point()
my_point.z = z_val
self.marker_object.pose.position.x = 5.31514576482
self.marker_object.pose.position.y = 1.36578380326
self.marker_object.pose.orientation.x = 0
self.marker_object.pose.orientation.y = 0
self.marker_object.pose.orientation.z = -0.498454937648
self.marker_object.pose.orientation.w = 0.866915610157
self.marker_object.scale.x = 0.2
self.marker_object.scale.y = 0.2
self.marker_object.scale.z = 0.2
self.marker_object.color.r = 0.0
self.marker_object.color.g = 0.0
self.marker_object.color.b = 1.0
self.marker_object.color.a = 1.0
self.marker_object.lifetime = rospy.Duration(0)
def start(self):
while not rospy.is_shutdown():
self.marker_objectlisher.publish(self.marker_object)
self.rate.sleep()
if __name__ == '__main__':
rospy.init_node('marker_basic_node', anonymous=True)
markerbasics_object = MarkerBasics()
try:
markerbasics_object.start()
except rospy.ROSInterruptException:
pass
Di folder my_rviz_markers / launch, buat peluncuran untuk memulai:basic_marker.launch<launch>
<node pkg="my_rviz_markers" type="basic_marker.py" name="basic_marker" />
</launch>
Lari:roslaunch my_rviz_markers basic_marker.launch
Tambahkan ke rviz:
Tambahkan gambar
Yang akan menunjukkan jarak dan sudut antara dua frame:
Di folder my_rviz_markers / launch, buat peluncuran untuk memulai:overlay_meny_my.launch overlay_menu_my.launch<launch>
<node pkg="my_rviz_markers" type="haro_overlay_complete_demo.py" name="overlay_menu" />
</launch>
Di folder my_rviz_markers / samples, buat skrip:haro_overlay_complete_demo.py
from jsk_rviz_plugins.msg import OverlayText, OverlayMenu
from std_msgs.msg import ColorRGBA, Float32
import rospy
import math
import random
from geometry_msgs.msg import Twist
class HaroOverlay(object):
def __init__(self):
self.text_pub = rospy.Publisher("/text_sample", OverlayText, queue_size=1)
self.plot_value_pub = rospy.Publisher("/plot_value_sample", Float32, queue_size=1)
self.piechart_value_pub = rospy.Publisher("/piechart_value_sample", Float32, queue_size=1)
self.menu_publisher = rospy.Publisher("/test_menu", OverlayMenu, queue_size=1)
self.plot_value = 0
self.piechart_value = 0
self.max_distance_from_object = 10.0
self.subs = rospy.Subscriber("/haro_base_link_to_person_standing_tf_translation", Twist, self.twist_callback)
self.counter = 0
self.rate = rospy.Rate(100)
self.overlaytext = self.update_overlaytext()
self.menu = self.update_overlay_menu()
def twist_callback(self, msg):
self.plot_value = msg.linear.x
self.piechart_value = msg.angular.z
def update_overlaytext(self, number=1.23, number2=20):
text = OverlayText()
text.width = 200
text.height = 400
text.left = 10
text.top = 10
text.text_size = 12
text.line_width = 2
text.font = "DejaVu Sans Mono"
text.text = """Distance= %s.
Angle=%s.
Counter = <span style="color: green;">%d.</span>
""" % (str(number), str(number2) ,self.counter)
text.fg_color = ColorRGBA(25 / 255.0, 1.0, 240.0 / 255.0, 1.0)
text.bg_color = ColorRGBA(0.0, 0.0, 0.0, 0.2)
return text
def update_overlay_textonly(self, new_text):
self.overlaytext.text = new_text
def update_overlay_menu(self):
menu = OverlayMenu()
menu.title = "HaroSystemMode"
menu.menus = ["Sleep", "Searching", "MovingInCircles","Waiting"]
menu.current_index = self.counter % len(menu.menus)
return menu
def update_overlay_menu_haro_tf(self):
menu = OverlayMenu()
menu.title = "HaroDistanceFromPerson"
menu.menus = ["FarAway", "CloseBy", "Target", "OtherWayRound"]
fraction = 10.0
if self.piechart_value < (math.pi/fraction):
if self.plot_value >= self.max_distance_from_object:
index = 0
elif self.plot_value >= (self.max_distance_from_object/ fraction) and self.plot_value < self.max_distance_from_object:
index = 1
elif self.plot_value < (self.max_distance_from_object/fraction):
index = 2
else:
index = 3
menu.current_index = index
return menu
def start_dummy_demo(self):
while not rospy.is_shutdown():
self.overlaytext = self.update_overlaytext()
self.menu = self.update_overlay_menu()
if self.counter % 100 == 0:
self.menu.action = OverlayMenu.ACTION_CLOSE
self.text_pub.publish(self.overlaytext)
self.value_pub.publish(math.cos(self.counter * math.pi * 2 / 1000))
self.menu_publisher.publish(self.menu)
self.rate.sleep()
self.counter += 1
def start_harodistance_demo(self):
while not rospy.is_shutdown():
self.overlaytext = self.update_overlaytext(number=self.plot_value, number2=self.piechart_value)
self.menu = self.update_overlay_menu_haro_tf()
self.text_pub.publish(self.overlaytext)
self.plot_value_pub.publish(self.plot_value)
self.piechart_value_pub.publish(self.piechart_value)
self.menu_publisher.publish(self.menu)
self.rate.sleep()
self.counter += 1
def dummy_overlay_demo():
rospy.init_node('distance_overlay_demo_node', anonymous=True)
haro_overlay_object = HaroOverlay()
try:
haro_overlay_object.start_harodistance_demo()
except rospy.ROSInterruptException:
pass
if __name__ == '__main__':
dummy_overlay_demo()
Script kedua adalahtf_haro_to_object_listener.py
import sys
import rospy
import math
import tf
from geometry_msgs.msg import Twist, Vector3
if __name__ == '__main__':
rospy.init_node('tf_listener_haro_to_person')
listener = tf.TransformListener()
if len(sys.argv) < 3:
print("usage: tf_haro_to_object_listener.py parent child")
else:
follower_model_name = sys.argv[1]
model_to_be_followed_name = sys.argv[2]
topic_to_publish_name = "/"+str(follower_model_name)+"_to_"+str(model_to_be_followed_name)+"_tf_translation"
tf_translation = rospy.Publisher(topic_to_publish_name, Twist ,queue_size=1)
rate = rospy.Rate(10.0)
ctrl_c = False
follower_model_frame = "/"+follower_model_name
model_to_be_followed_frame = "/"+model_to_be_followed_name
def shutdownhook():
global ctrl_c
ctrl_c = True
rospy.on_shutdown(shutdownhook)
while not ctrl_c:
try:
(trans,rot) = listener.lookupTransform(follower_model_frame, model_to_be_followed_frame, rospy.Time(0))
translation_object = Vector3()
translation_object.x = trans[0]
translation_object.y = trans[1]
translation_object.z = trans[2]
angular = math.atan2(translation_object.y, translation_object.x)
linear = math.sqrt(translation_object.x ** 2 + translation_object.y ** 2)
cmd = Twist()
cmd.linear.x = linear
cmd.angular.z = angular
tf_translation.publish(cmd)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
rate.sleep()
Dimulai seperti ini (diyakini bahwa semua node utama dan rviz sudah berjalan):roslaunch linorobot overlay_meny_my.launch
python tf_haro_to_object_listener.py base_link map
Kita akan melihat sudut dan jarak frame base_link relatif terhadap peta.Bersambung.