دليل البدء السريع لـ ROS (نظام تشغيل الروبوت)

ROS Introduction (captioned) from Open Robotics on Vimeo.

ما هو ROS؟

يُعد Robot Operating System (ROS) إطار عمل مفتوح المصدر يُستخدم على نطاق واسع في أبحاث الروبوتات والصناعة. يوفر ROS مجموعة من المكتبات والأدوات لمساعدة المطورين على إنشاء تطبيقات الروبوتات. تم تصميم ROS للعمل مع منصات روبوتية متنوعة، مما يجعله أداة مرنة وقوية لخبراء الروبوتات.

الميزات الرئيسية لـ ROS

  1. بنية معيارية (Modular Architecture): يتمتع ROS ببنية معيارية، مما يسمح للمطورين ببناء أنظمة معقدة من خلال دمج مكونات أصغر قابلة لإعادة الاستخدام تُسمى nodes. عادةً ما تؤدي كل عقدة (node) وظيفة محددة، وتتواصل العقد مع بعضها البعض باستخدام رسائل عبر topics أو services.

  2. برمجيات وسيطة للاتصالات (Communication Middleware): يوفر ROS بنية تحتية قوية للاتصالات تدعم الاتصال بين العمليات والحوسبة الموزعة. يتم تحقيق ذلك من خلال نموذج "الناشر-المشترك" (publish-subscribe) لتدفقات البيانات (topics) ونموذج "الطلب-الرد" (request-reply) لاستدعاءات الخدمات.

  3. تجريد الأجهزة (Hardware Abstraction): يوفر ROS طبقة تجريد فوق الأجهزة، مما يمكن المطورين من كتابة كود مستقل عن نوع الجهاز. يسمح هذا باستخدام نفس الكود مع إعدادات أجهزة مختلفة، مما يسهل التكامل والتجربة.

  4. الأدوات والمرافق: يأتي ROS مع مجموعة غنية من الأدوات والمرافق للتصور، والتصحيح، والمحاكاة. على سبيل المثال، يُستخدم RViz لتصور بيانات المستشعرات ومعلومات حالة الروبوت، بينما يوفر Gazebo بيئة محاكاة قوية لاختبار الخوارزميات وتصميمات الروبوتات.

  5. نظام بيئي واسع: النظام البيئي لـ ROS واسع وينمو باستمرار، مع توفر العديد من الحزم لمختلف التطبيقات الروبوتية، بما في ذلك الملاحة، والمعالجة، والإدراك، والمزيد. يساهم المجتمع بفعالية في تطوير وصيانة هذه الحزم.

تطور إصدارات ROS

منذ تطويره في عام 2007، تطور ROS عبر إصدارات متعددة، حيث يقدم كل منها ميزات وتحسينات جديدة لتلبية الاحتياجات المتزايدة لمجتمع الروبوتات. يمكن تصنيف تطوير ROS إلى سلسلتين رئيسيتين: ROS 1 و ROS 2. يركز هذا الدليل على إصدار الدعم طويل المدى (LTS) من ROS 1، والمعروف باسم ROS Noetic Ninjemys، ويجب أن يعمل الكود أيضاً مع الإصدارات السابقة.

ROS 1 مقابل ROS 2

بينما وفر ROS 1 أساساً قوياً لتطوير الروبوتات، يعالج ROS 2 أوجه قصوره من خلال توفير:

  • الأداء في الوقت الفعلي: دعم محسن للأنظمة ذات الوقت الفعلي والسلوك الحتمي.
  • الأمان: ميزات أمان معززة للتشغيل الآمن والموثوق في بيئات مختلفة.
  • القابلية للتوسع: دعم أفضل لأنظمة الروبوتات المتعددة وعمليات النشر واسعة النطاق.
  • دعم عبر المنصات: توسيع التوافق مع أنظمة تشغيل مختلفة بخلاف Linux، بما في ذلك Windows و macOS.
  • اتصال مرن: استخدام DDS لاتصال أكثر مرونة وكفاءة بين العمليات.

رسائل ومواضيع ROS

في ROS، يتم تسهيل التواصل بين العقد من خلال messages و topics. الرسالة هي هيكل بيانات يحدد المعلومات المتبادلة بين العقد، بينما الموضوع (topic) هو قناة مسماة يتم إرسال واستقبال الرسائل عبرها. يمكن للعقد نشر رسائل إلى موضوع أو الاشتراك في رسائل من موضوع، مما يمكنها من التواصل مع بعضها البعض. يسمح نموذج "الناشر-المشترك" هذا باتصال غير متزامن وفك الارتباط بين العقد. عادة ما ينشر كل مستشعر أو مشغل في نظام روبوتي بيانات إلى موضوع، والتي يمكن بعد ذلك استهلاكها بواسطة عقد أخرى للمعالجة أو التحكم. لأغراض هذا الدليل، سنركز على رسائل Image و Depth و PointCloud ومواضيع الكاميرا.

إعداد Ultralytics YOLO مع ROS

تم اختبار هذا الدليل باستخدام بيئة ROS هذه، وهي نسخة مشتقة من مستودع ROSbot ROS. تتضمن هذه البيئة حزمة Ultralytics YOLO، وحاوية Docker لسهولة الإعداد، وحزم ROS شاملة، وعوالم Gazebo للاختبار السريع. وهي مصممة للعمل مع Husarion ROSbot 2 PRO. ستعمل أمثلة الكود المقدمة في أي بيئة ROS Noetic/Melodic، بما في ذلك المحاكاة والواقع الحقيقي.

Husarion ROSbot 2 PRO autonomous robot platform

تثبيت التبعيات

بصرف النظر عن بيئة ROS، ستحتاج إلى تثبيت التبعيات التالية:

  • حزمة ROS NumPy: مطلوبة للتحويل السريع بين رسائل ROS Image ومصفوفات NumPy.

    pip install ros_numpy
  • حزمة Ultralytics:

    pip install ultralytics

استخدم Ultralytics مع sensor_msgs/Image في ROS

The sensor_msgs/Image message type is commonly used in ROS for representing image data. It contains fields for encoding, height, width, and pixel data, making it suitable for transmitting images captured by cameras or other sensors. Image messages are widely used in robotic applications for tasks such as visual perception, object detection, and navigation.

Detection and Segmentation in ROS Gazebo

خطوات استخدام الصور

يوضح مقتطف الكود التالي كيفية استخدام حزمة Ultralytics YOLO مع ROS. في هذا المثال، نشترك في موضوع كاميرا، ونعالج الصورة الواردة باستخدام YOLO، وننشر الكائنات المكتشفة في مواضيع جديدة لـ detection و segmentation.

أولاً، قم باستيراد المكتبات الضرورية وقم بإنشاء نموذجين: واحد لـ segmentation وواحد لـ detection. قم بتهيئة عقدة ROS (باسم ultralytics) لتمكين التواصل مع ROS master. لضمان اتصال مستقر، نقوم بتضمين توقف مؤقت قصير، مما يمنح العقدة وقتاً كافياً لإنشاء الاتصال قبل المتابعة.

import time

import rospy

from ultralytics import YOLO

detection_model = YOLO("yolo26m.pt")
segmentation_model = YOLO("yolo26m-seg.pt")
rospy.init_node("ultralytics")
time.sleep(1)

قم بتهيئة موضوعين في ROS: واحد لـ detection وواحد لـ segmentation. سيتم استخدام هذه المواضيع لنشر الصور المشروحة، مما يجعلها متاحة لمزيد من المعالجة. يتم تسهيل التواصل بين العقد باستخدام رسائل sensor_msgs/Image.

from sensor_msgs.msg import Image

det_image_pub = rospy.Publisher("/ultralytics/detection/image", Image, queue_size=5)
seg_image_pub = rospy.Publisher("/ultralytics/segmentation/image", Image, queue_size=5)

أخيراً، قم بإنشاء مشترك (subscriber) يستمع إلى الرسائل على الموضوع /camera/color/image_raw ويستدعي دالة رد اتصال (callback function) لكل رسالة جديدة. تتلقى دالة رد الاتصال هذه رسائل من نوع sensor_msgs/Image، وتقوم بتحويلها إلى مصفوفة NumPy باستخدام ros_numpy، وتعالج الصور باستخدام نماذج YOLO التي تم إنشاؤها مسبقاً، وتضيف تعليقات توضيحية على الصور، ثم تنشرها مرة أخرى إلى المواضيع المعنية: /ultralytics/detection/image للاكتشاف و /ultralytics/segmentation/image للتجزئة.

import ros_numpy

def callback(data):
    """Callback function to process image and publish annotated images."""
    array = ros_numpy.numpify(data)
    if det_image_pub.get_num_connections():
        det_result = detection_model(array)
        det_annotated = det_result[0].plot(show=False)
        det_image_pub.publish(ros_numpy.msgify(Image, det_annotated, encoding="rgb8"))

    if seg_image_pub.get_num_connections():
        seg_result = segmentation_model(array)
        seg_annotated = seg_result[0].plot(show=False)
        seg_image_pub.publish(ros_numpy.msgify(Image, seg_annotated, encoding="rgb8"))

rospy.Subscriber("/camera/color/image_raw", Image, callback)

while True:
    rospy.spin()
الكود الكامل
import time

import ros_numpy
import rospy
from sensor_msgs.msg import Image

from ultralytics import YOLO

detection_model = YOLO("yolo26m.pt")
segmentation_model = YOLO("yolo26m-seg.pt")
rospy.init_node("ultralytics")
time.sleep(1)

det_image_pub = rospy.Publisher("/ultralytics/detection/image", Image, queue_size=5)
seg_image_pub = rospy.Publisher("/ultralytics/segmentation/image", Image, queue_size=5)

def callback(data):
    """Callback function to process image and publish annotated images."""
    array = ros_numpy.numpify(data)
    if det_image_pub.get_num_connections():
        det_result = detection_model(array)
        det_annotated = det_result[0].plot(show=False)
        det_image_pub.publish(ros_numpy.msgify(Image, det_annotated, encoding="rgb8"))

    if seg_image_pub.get_num_connections():
        seg_result = segmentation_model(array)
        seg_annotated = seg_result[0].plot(show=False)
        seg_image_pub.publish(ros_numpy.msgify(Image, seg_annotated, encoding="rgb8"))

rospy.Subscriber("/camera/color/image_raw", Image, callback)

while True:
    rospy.spin()
تصحيح الأخطاء (Debugging)

يمكن أن يكون تصحيح أخطاء عقد ROS (نظام تشغيل الروبوت) أمراً صعباً بسبب الطبيعة الموزعة للنظام. يمكن للعديد من الأدوات المساعدة في هذه العملية:

  1. rostopic echo <TOPIC-NAME> : يسمح لك هذا الأمر بعرض الرسائل المنشورة على موضوع معين، مما يساعدك على فحص تدفق البيانات.
  2. rostopic list: استخدم هذا الأمر لسرد جميع المواضيع المتاحة في نظام ROS، مما يمنحك نظرة عامة على تدفقات البيانات النشطة.
  3. rqt_graph: تعرض أداة التصور هذه رسم الاتصالات بين العقد، مما يوفر رؤى حول كيفية ترابط العقد وكيفية تفاعلها.
  4. للحصول على تصورات أكثر تعقيداً، مثل التمثيلات ثلاثية الأبعاد، يمكنك استخدام RViz. RViz (تصور ROS) هي أداة تصور ثلاثية الأبعاد قوية لـ ROS. تسمح لك بتصور حالة الروبوت الخاص بك وبيئته في الوقت الفعلي. باستخدام RViz، يمكنك عرض بيانات المستشعرات (مثل sensor_msgs/Image)، وحالات نماذج الروبوت، وأنواع أخرى متنوعة من المعلومات، مما يسهل تصحيح أخطاء وفهم سلوك نظامك الروبوتي.

نشر الفئات المكتشفة باستخدام std_msgs/String

Standard ROS messages also include std_msgs/String messages. In many applications, it is not necessary to republish the entire annotated image; instead, only the classes present in the robot's view are needed. The following example demonstrates how to use std_msgs/String messages to republish the detected classes on the /ultralytics/detection/classes topic. These messages are more lightweight and provide essential information, making them valuable for various applications.

مثال على حالة الاستخدام

تخيل روبوت مستودع مجهز بكاميرا ونموذج detection للكائنات. بدلاً من إرسال صور كبيرة مشروحة عبر الشبكة، يمكن للروبوت نشر قائمة بالفئات المكتشفة كرسائل std_msgs/String. على سبيل المثال، عندما يكتشف الروبوت كائنات مثل "صندوق"، "منصة نقالة" و"رافعة شوكية"، فإنه ينشر هذه الفئات إلى موضوع /ultralytics/detection/classes. يمكن بعد ذلك استخدام هذه المعلومات من قبل نظام مراقبة مركزي لتتبع المخزون في الوقت الفعلي، أو تحسين تخطيط مسار الروبوت لتجنب العوائق، أو تشغيل إجراءات محددة مثل التقاط صندوق مكتشف. يقلل هذا النهج من النطاق الترددي المطلوب للاتصال ويركز على نقل البيانات المهمة.

خطوات استخدام السلاسل النصية

يوضح هذا المثال كيفية استخدام حزمة Ultralytics YOLO مع ROS. في هذا المثال، نشترك في موضوع كاميرا، ونعالج الصورة الواردة باستخدام YOLO، وننشر الكائنات المكتشفة إلى موضوع جديد /ultralytics/detection/classes باستخدام رسائل std_msgs/String. تُستخدم حزمة ros_numpy لتحويل رسالة ROS Image إلى مصفوفة NumPy للمعالجة باستخدام YOLO.

import time

import ros_numpy
import rospy
from sensor_msgs.msg import Image
from std_msgs.msg import String

from ultralytics import YOLO

detection_model = YOLO("yolo26m.pt")
rospy.init_node("ultralytics")
time.sleep(1)
classes_pub = rospy.Publisher("/ultralytics/detection/classes", String, queue_size=5)

def callback(data):
    """Callback function to process image and publish detected classes."""
    array = ros_numpy.numpify(data)
    if classes_pub.get_num_connections():
        det_result = detection_model(array)
        classes = det_result[0].boxes.cls.cpu().numpy().astype(int)
        names = [det_result[0].names[i] for i in classes]
        classes_pub.publish(String(data=str(names)))

rospy.Subscriber("/camera/color/image_raw", Image, callback)
while True:
    rospy.spin()

استخدم Ultralytics مع صور العمق (Depth Images) في ROS

بالإضافة إلى صور RGB، يدعم ROS صور العمق، والتي توفر معلومات حول مسافة الكائنات عن الكاميرا. تُعد صور العمق ضرورية للتطبيقات الروبوتية مثل تجنب العوائق، والخرائط ثلاثية الأبعاد، وتحديد الموقع.

صورة العمق هي صورة يمثل فيها كل بكسل المسافة من الكاميرا إلى كائن ما. على عكس صور RGB التي تلتقط اللون، تلتقط صور العمق معلومات مكانية، مما يمكن الروبوتات من إدراك الهيكل ثلاثي الأبعاد لبيئتها.

الحصول على صور العمق

يمكن الحصول على صور العمق باستخدام مستشعرات متنوعة:

  1. Stereo Cameras: استخدم كاميرتين لحساب العمق بناءً على اختلاف الصور.
  2. Time-of-Flight (ToF) Cameras: قياس الوقت الذي يستغرقه الضوء للعودة من كائن ما.
  3. Structured Light Sensors: عرض نمط ما وقياس تشوهه على الأسطح.

استخدام YOLO مع صور العمق

في ROS، يتم تمثيل صور العمق بواسطة نوع الرسالة sensor_msgs/Image، والذي يتضمن حقولاً للترميز، والارتفاع، والعرض، وبيانات البكسل. غالباً ما يستخدم حقل الترميز لصور العمق تنسيقاً مثل "16UC1"، مما يشير إلى عدد صحيح غير موقّع بـ 16 بت لكل بكسل، حيث تمثل كل قيمة المسافة إلى الكائن. تُستخدم صور العمق بشكل شائع جنباً إلى جنب مع صور RGB لتوفير رؤية أكثر شمولاً للبيئة.

باستخدام YOLO، من الممكن استخراج ودمج المعلومات من كل من صور RGB وصور العمق. على سبيل المثال، يمكن لـ YOLO اكتشاف الكائنات داخل صورة RGB، ويمكن استخدام هذا الاكتشاف لتحديد المناطق المقابلة في صورة العمق. يسمح هذا باستخراج معلومات دقيقة عن العمق للكائنات المكتشفة، مما يعزز قدرة الروبوت على فهم بيئته بأبعاد ثلاثية.

كاميرات RGB-D

عند العمل مع صور العمق، من الضروري التأكد من محاذاة صور RGB والعمق بشكل صحيح. توفر كاميرات RGB-D، مثل سلسلة Intel RealSense، صور RGB وصور عمق متزامنة، مما يسهل دمج المعلومات من كلا المصدرين. في حالة استخدام كاميرات RGB وعمق منفصلة، من الضروري معايرتها لضمان محاذاة دقيقة.

خطوات استخدام العمق

في هذا المثال، نستخدم YOLO لتجزئة صورة وتطبيق القناع المستخرج لتجزئة الكائن في صورة العمق. يسمح لنا هذا بتحديد مسافة كل بكسل من الكائن المهتم من المركز البؤري للكاميرا. من خلال الحصول على معلومات المسافة هذه، يمكننا حساب المسافة بين الكاميرا والكائن المحدد في المشهد. ابدأ باستيراد المكتبات الضرورية، وإنشاء عقدة ROS، وتهيئة نموذج تجزئة وموضوع ROS.

import time

import rospy
from std_msgs.msg import String

from ultralytics import YOLO

rospy.init_node("ultralytics")
time.sleep(1)

segmentation_model = YOLO("yolo26m-seg.pt")

classes_pub = rospy.Publisher("/ultralytics/detection/distance", String, queue_size=5)

بعد ذلك، حدد دالة رد اتصال تعالج رسالة صورة العمق الواردة. تنتظر الدالة رسائل صورة العمق وصورة RGB، وتحولها إلى مصفوفات NumPy، وتطبق نموذج التجزئة على صورة RGB. ثم تستخرج قناع التجزئة لكل كائن مكتشف وتحسب متوسط مسافة الكائن عن الكاميرا باستخدام صورة العمق. تحتوي معظم المستشعرات على مسافة قصوى، تُعرف بمسافة القص (clip distance)، والتي يتم تمثيل القيم بعدها كـ inf (np.inf). قبل المعالجة، من المهم تصفية هذه القيم الفارغة وتعيين قيمة 0 لها. أخيراً، تنشر الكائنات المكتشفة مع متوسط مسافاتها إلى الموضوع /ultralytics/detection/distance.

import numpy as np
import ros_numpy
from sensor_msgs.msg import Image

def callback(data):
    """Callback function to process depth image and RGB image."""
    image = rospy.wait_for_message("/camera/color/image_raw", Image)
    image = ros_numpy.numpify(image)
    depth = ros_numpy.numpify(data)
    result = segmentation_model(image)

    all_objects = []
    for index, cls in enumerate(result[0].boxes.cls):
        class_index = int(cls.cpu().numpy())
        name = result[0].names[class_index]
        mask = result[0].masks.data.cpu().numpy()[index, :, :].astype(int)
        obj = depth[mask == 1]
        obj = obj[~np.isnan(obj)]
        avg_distance = np.mean(obj) if len(obj) else np.inf
        all_objects.append(f"{name}: {avg_distance:.2f}m")

    classes_pub.publish(String(data=str(all_objects)))

rospy.Subscriber("/camera/depth/image_raw", Image, callback)

while True:
    rospy.spin()
الكود الكامل
import time

import numpy as np
import ros_numpy
import rospy
from sensor_msgs.msg import Image
from std_msgs.msg import String

from ultralytics import YOLO

rospy.init_node("ultralytics")
time.sleep(1)

segmentation_model = YOLO("yolo26m-seg.pt")

classes_pub = rospy.Publisher("/ultralytics/detection/distance", String, queue_size=5)

def callback(data):
    """Callback function to process depth image and RGB image."""
    image = rospy.wait_for_message("/camera/color/image_raw", Image)
    image = ros_numpy.numpify(image)
    depth = ros_numpy.numpify(data)
    result = segmentation_model(image)

    all_objects = []
    for index, cls in enumerate(result[0].boxes.cls):
        class_index = int(cls.cpu().numpy())
        name = result[0].names[class_index]
        mask = result[0].masks.data.cpu().numpy()[index, :, :].astype(int)
        obj = depth[mask == 1]
        obj = obj[~np.isnan(obj)]
        avg_distance = np.mean(obj) if len(obj) else np.inf
        all_objects.append(f"{name}: {avg_distance:.2f}m")

    classes_pub.publish(String(data=str(all_objects)))

rospy.Subscriber("/camera/depth/image_raw", Image, callback)

while True:
    rospy.spin()

استخدم Ultralytics مع sensor_msgs/PointCloud2 في ROS

Detection and Segmentation in ROS Gazebo

The sensor_msgs/PointCloud2 message type is a data structure used in ROS to represent 3D point cloud data. This message type is integral to robotic applications, enabling tasks such as 3D mapping, object recognition, and localization.

سحابة النقاط هي مجموعة من نقاط البيانات المحددة داخل نظام إحداثيات ثلاثي الأبعاد. تمثل نقاط البيانات هذه السطح الخارجي لكائن أو مشهد، يتم التقاطه عبر تقنيات المسح ثلاثي الأبعاد. كل نقطة في السحابة لها إحداثيات X و Y و Z، والتي تتوافق مع موقعها في الفضاء، وقد تتضمن أيضاً معلومات إضافية مثل اللون والشدة.

إطار المرجع

عند العمل مع sensor_msgs/PointCloud2، من الضروري مراعاة إطار مرجع المستشعر الذي تم الحصول على بيانات سحابة النقاط منه. يتم التقاط سحابة النقاط في البداية في إطار مرجع المستشعر. يمكنك تحديد إطار المرجع هذا عن طريق الاستماع إلى الموضوع /tf_static. ومع ذلك، اعتماداً على متطلبات تطبيقك الخاصة، قد تحتاج إلى تحويل سحابة النقاط إلى إطار مرجع آخر. يمكن تحقيق هذا التحويل باستخدام حزمة tf2_ros، التي توفر أدوات لإدارة إطارات الإحداثيات وتحويل البيانات بينها.

الحصول على سحابات النقاط

يمكن الحصول على سحابات النقاط باستخدام مستشعرات متنوعة:

  1. LIDAR (كشف الضوء وتحديد المدى): يستخدم نبضات الليزر لقياس المسافات إلى الكائنات وإنشاء خرائط ثلاثية الأبعاد عالية الـprecision.
  2. كاميرات العمق: تلتقط معلومات العمق لكل بكسل، مما يسمح بإعادة بناء المشهد ثلاثي الأبعاد.
  3. كاميرات ستيريو: تستخدم كاميرتين أو أكثر للحصول على معلومات العمق من خلال التثليث.
  4. ماسحات الضوء المهيكل: تعرض نمطاً معروفاً على سطح ما وتقيس التشوه لحساب العمق.

استخدام YOLO مع سحابات النقاط

لدمج YOLO مع رسائل من نوع sensor_msgs/PointCloud2، يمكننا استخدام طريقة مشابهة لتلك المستخدمة لخرائط العمق. من خلال الاستفادة من معلومات اللون المضمنة في سحابة النقاط، يمكننا استخراج صورة ثنائية الأبعاد، وإجراء تجزئة على هذه الصورة باستخدام YOLO، ثم تطبيق القناع الناتج على النقاط ثلاثية الأبعاد لعزل الكائن ثلاثي الأبعاد المهتم.

للتعامل مع سحابات النقاط، نوصي باستخدام Open3D (pip install open3d)، وهي مكتبة Python سهلة الاستخدام. توفر Open3D أدوات قوية لإدارة هياكل بيانات سحابة النقاط، وتصورها، وتنفيذ عمليات معقدة بسلاسة. يمكن لهذه المكتبة تبسيط العملية بشكل كبير وتعزيز قدرتنا على معالجة وتحليل سحابات النقاط بالتزامن مع التجزئة القائمة على YOLO.

خطوات استخدام سحابة النقاط

استيراد المكتبات الضرورية وتهيئة نموذج YOLO للتجزئة.

import time

import rospy

from ultralytics import YOLO

rospy.init_node("ultralytics")
time.sleep(1)
segmentation_model = YOLO("yolo26m-seg.pt")

أنشئ دالة pointcloud2_to_array، التي تحول رسالة sensor_msgs/PointCloud2 إلى مصفوفيتي NumPy. تحتوي رسائل sensor_msgs/PointCloud2 على n من النقاط بناءً على width و height الصورة المكتسبة. على سبيل المثال، ستكون للصورة 480 x 640 307,200 نقطة. تتضمن كل نقطة ثلاث إحداثيات مكانية (xyz) واللون المقابل بتنسيق RGB. يمكن اعتبار هذه قناتين منفصلتين من المعلومات.

تعيد الدالة إحداثيات xyz وقيم RGB بتنسيق دقة الكاميرا الأصلية (width x height). تحتوي معظم المستشعرات على مسافة قصوى، تُعرف بمسافة القص، والتي يتم تمثيل القيم بعدها كـ inf (np.inf). قبل المعالجة، من المهم تصفية هذه القيم الفارغة وتعيين قيمة 0 لها.

import numpy as np
import ros_numpy

def pointcloud2_to_array(pointcloud2: PointCloud2) -> tuple:
    """Convert a ROS PointCloud2 message to a numpy array.

    Args:
        pointcloud2 (PointCloud2): the PointCloud2 message

    Returns:
        (tuple): tuple containing (xyz, rgb)
    """
    pc_array = ros_numpy.point_cloud2.pointcloud2_to_array(pointcloud2)
    split = ros_numpy.point_cloud2.split_rgb_field(pc_array)
    rgb = np.stack([split["b"], split["g"], split["r"]], axis=2)
    xyz = ros_numpy.point_cloud2.get_xyz_points(pc_array, remove_nans=False)
    xyz = np.array(xyz).reshape((pointcloud2.height, pointcloud2.width, 3))
    nan_rows = np.isnan(xyz).all(axis=2)
    xyz[nan_rows] = [0, 0, 0]
    rgb[nan_rows] = [0, 0, 0]
    return xyz, rgb

بعد ذلك، اشترك في الموضوع /camera/depth/points لاستلام رسالة سحابة النقاط وتحويل رسالة sensor_msgs/PointCloud2 إلى مصفوفات NumPy تحتوي على إحداثيات XYZ وقيم RGB (باستخدام دالة pointcloud2_to_array). قم بمعالجة صورة RGB باستخدام نموذج YOLO لاستخراج الكائنات المجزأة. لكل كائن مكتشف، قم باستخراج قناع التجزئة وتطبيقه على كل من صورة RGB وإحداثيات XYZ لعزل الكائن في الفضاء ثلاثي الأبعاد.

معالجة القناع مباشرة لأنها تتكون من قيم ثنائية، حيث تشير 1 إلى وجود الكائن و 0 إلى غيابه. لتطبيق القناع، ببساطة اضرب القنوات الأصلية في القناع. تؤدي هذه العملية بفعالية إلى عزل الكائن المهتم داخل الصورة. أخيراً، قم بإنشاء كائن سحابة نقاط Open3D وقم بتصور الكائن المجزأ في الفضاء ثلاثي الأبعاد مع الألوان المرتبطة به.

import sys

import open3d as o3d

ros_cloud = rospy.wait_for_message("/camera/depth/points", PointCloud2)
xyz, rgb = pointcloud2_to_array(ros_cloud)
result = segmentation_model(rgb)

if not len(result[0].boxes.cls):
    print("No objects detected")
    sys.exit()

classes = result[0].boxes.cls.cpu().numpy().astype(int)
for index, class_id in enumerate(classes):
    mask = result[0].masks.data.cpu().numpy()[index, :, :].astype(int)
    mask_expanded = np.stack([mask, mask, mask], axis=2)

    obj_rgb = rgb * mask_expanded
    obj_xyz = xyz * mask_expanded

    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(obj_xyz.reshape((ros_cloud.height * ros_cloud.width, 3)))
    pcd.colors = o3d.utility.Vector3dVector(obj_rgb.reshape((ros_cloud.height * ros_cloud.width, 3)) / 255)
    o3d.visualization.draw_geometries([pcd])
الكود الكامل
import sys
import time

import numpy as np
import open3d as o3d
import ros_numpy
import rospy
from sensor_msgs.msg import PointCloud2

from ultralytics import YOLO

rospy.init_node("ultralytics")
time.sleep(1)
segmentation_model = YOLO("yolo26m-seg.pt")

def pointcloud2_to_array(pointcloud2: PointCloud2) -> tuple:
    """Convert a ROS PointCloud2 message to a numpy array.

    Args:
        pointcloud2 (PointCloud2): the PointCloud2 message

    Returns:
        (tuple): tuple containing (xyz, rgb)
    """
    pc_array = ros_numpy.point_cloud2.pointcloud2_to_array(pointcloud2)
    split = ros_numpy.point_cloud2.split_rgb_field(pc_array)
    rgb = np.stack([split["b"], split["g"], split["r"]], axis=2)
    xyz = ros_numpy.point_cloud2.get_xyz_points(pc_array, remove_nans=False)
    xyz = np.array(xyz).reshape((pointcloud2.height, pointcloud2.width, 3))
    nan_rows = np.isnan(xyz).all(axis=2)
    xyz[nan_rows] = [0, 0, 0]
    rgb[nan_rows] = [0, 0, 0]
    return xyz, rgb

ros_cloud = rospy.wait_for_message("/camera/depth/points", PointCloud2)
xyz, rgb = pointcloud2_to_array(ros_cloud)
result = segmentation_model(rgb)

if not len(result[0].boxes.cls):
    print("No objects detected")
    sys.exit()

classes = result[0].boxes.cls.cpu().numpy().astype(int)
for index, class_id in enumerate(classes):
    mask = result[0].masks.data.cpu().numpy()[index, :, :].astype(int)
    mask_expanded = np.stack([mask, mask, mask], axis=2)

    obj_rgb = rgb * mask_expanded
    obj_xyz = xyz * mask_expanded

    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(obj_xyz.reshape((ros_cloud.height * ros_cloud.width, 3)))
    pcd.colors = o3d.utility.Vector3dVector(obj_rgb.reshape((ros_cloud.height * ros_cloud.width, 3)) / 255)
    o3d.visualization.draw_geometries([pcd])

Point Cloud Segmentation with Ultralytics

الأسئلة الشائعة

ما هو نظام تشغيل الروبوت (ROS)؟

يُعد Robot Operating System (ROS) إطار عمل مفتوح المصدر يُستخدم عادةً في الروبوتات لمساعدة المطورين على إنشاء تطبيقات روبوت قوية. يوفر مجموعة من المكتبات والأدوات لبناء والتفاعل مع الأنظمة الروبوتية، مما يتيح تطوير تطبيقات معقدة بشكل أسهل. يدعم ROS التواصل بين العقد باستخدام رسائل عبر topics أو services.

كيف أقوم بدمج Ultralytics YOLO مع ROS لاكتشاف الكائنات في الوقت الفعلي؟

يتضمن دمج Ultralytics YOLO مع ROS إعداد بيئة ROS واستخدام YOLO لمعالجة بيانات المستشعرات. ابدأ بتثبيت التبعيات المطلوبة مثل ros_numpy و Ultralytics YOLO:

pip install ros_numpy ultralytics

بعد ذلك، قم بإنشاء عقدة ROS واشترك في موضوع صورة لمعالجة البيانات الواردة. فيما يلي مثال مبسط:

import ros_numpy
import rospy
from sensor_msgs.msg import Image

from ultralytics import YOLO

detection_model = YOLO("yolo26m.pt")
rospy.init_node("ultralytics")
det_image_pub = rospy.Publisher("/ultralytics/detection/image", Image, queue_size=5)

def callback(data):
    array = ros_numpy.numpify(data)
    det_result = detection_model(array)
    det_annotated = det_result[0].plot(show=False)
    det_image_pub.publish(ros_numpy.msgify(Image, det_annotated, encoding="rgb8"))

rospy.Subscriber("/camera/color/image_raw", Image, callback)
rospy.spin()

ما هي مواضيع ROS وكيف تُستخدم في Ultralytics YOLO؟

تسهل مواضيع ROS التواصل بين العقد في شبكة ROS باستخدام نموذج "الناشر-المشترك". الموضوع هو قناة مسماة تستخدمها العقد لإرسال واستقبال الرسائل بشكل غير متزامن. في سياق Ultralytics YOLO، يمكنك جعل عقدة تشترك في موضوع صورة، ومعالجة الصور باستخدام YOLO لمهام مثل detection أو segmentation، ونشر النتائج إلى مواضيع جديدة.

على سبيل المثال، اشترك في موضوع كاميرا وقم بمعالجة الصورة الواردة للاكتشاف:

rospy.Subscriber("/camera/color/image_raw", Image, callback)

لماذا نستخدم صور العمق مع Ultralytics YOLO في ROS؟

توفر صور العمق في ROS، والتي يمثلها sensor_msgs/Image، مسافة الكائنات عن الكاميرا، وهو أمر بالغ الأهمية لمهام مثل تجنب العوائق، والخرائط ثلاثية الأبعاد، وتحديد الموقع. من خلال استخدام معلومات العمق جنباً إلى جنب مع صور RGB، يمكن للروبوتات فهم بيئتها ثلاثية الأبعاد بشكل أفضل.

باستخدام YOLO، يمكنك استخراج أقنعة التجزئة من صور RGB وتطبيق هذه الأقنعة على صور العمق للحصول على معلومات دقيقة عن الكائن ثلاثي الأبعاد، مما يحسن قدرة الروبوت على التنقل والتفاعل مع محيطه.

كيف يمكنني تصور سحابات النقاط ثلاثية الأبعاد مع YOLO في ROS؟

لتصور سحابات النقاط ثلاثية الأبعاد في ROS مع YOLO:

  1. قم بتحويل رسائل sensor_msgs/PointCloud2 إلى مصفوفات NumPy.
  2. استخدم YOLO لتجزئة صور RGB.
  3. طبق قناع التجزئة على سحابة النقاط.

فيما يلي مثال باستخدام Open3D للتصور:

import sys

import open3d as o3d
import ros_numpy
import rospy
from sensor_msgs.msg import PointCloud2

from ultralytics import YOLO

rospy.init_node("ultralytics")
segmentation_model = YOLO("yolo26m-seg.pt")

def pointcloud2_to_array(pointcloud2):
    pc_array = ros_numpy.point_cloud2.pointcloud2_to_array(pointcloud2)
    split = ros_numpy.point_cloud2.split_rgb_field(pc_array)
    rgb = np.stack([split["b"], split["g"], split["r"]], axis=2)
    xyz = ros_numpy.point_cloud2.get_xyz_points(pc_array, remove_nans=False)
    xyz = np.array(xyz).reshape((pointcloud2.height, pointcloud2.width, 3))
    return xyz, rgb

ros_cloud = rospy.wait_for_message("/camera/depth/points", PointCloud2)
xyz, rgb = pointcloud2_to_array(ros_cloud)
result = segmentation_model(rgb)

if not len(result[0].boxes.cls):
    print("No objects detected")
    sys.exit()

classes = result[0].boxes.cls.cpu().numpy().astype(int)
for index, class_id in enumerate(classes):
    mask = result[0].masks.data.cpu().numpy()[index, :, :].astype(int)
    mask_expanded = np.stack([mask, mask, mask], axis=2)

    obj_rgb = rgb * mask_expanded
    obj_xyz = xyz * mask_expanded

    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(obj_xyz.reshape((-1, 3)))
    pcd.colors = o3d.utility.Vector3dVector(obj_rgb.reshape((-1, 3)) / 255)
    o3d.visualization.draw_geometries([pcd])

يوفر هذا النهج تصوراً ثلاثي الأبعاد للكائنات المجزأة، مفيداً لمهام مثل الملاحة والمعالجة في التطبيقات الروبوتية.

التعليقات