์ฝ˜ํ…์ธ ๋กœ ๊ฑด๋„ˆ๋›ฐ๊ธฐ

ROS(Robot Operating System) ํ€ต์Šคํƒ€ํŠธ ๊ฐ€์ด๋“œ

์˜คํ”ˆ ๋กœ๋ณดํ‹ฑ์Šค์˜ ROS ์†Œ๊ฐœ(์บก์…˜) Vimeo์˜ ๋™์˜์ƒ์ž…๋‹ˆ๋‹ค.

ROS๋ž€?

ROS(๋กœ๋ด‡ ์šด์˜ ์ฒด์ œ) ๋Š” ๋กœ๋ด‡ ์—ฐ๊ตฌ ๋ฐ ์‚ฐ์—…์—์„œ ๋„๋ฆฌ ์‚ฌ์šฉ๋˜๋Š” ์˜คํ”ˆ ์†Œ์Šค ํ”„๋ ˆ์ž„์›Œํฌ์ž…๋‹ˆ๋‹ค. ROS๋Š” ๊ฐœ๋ฐœ์ž๊ฐ€ ๋กœ๋ด‡ ์• ํ”Œ๋ฆฌ์ผ€์ด์…˜์„ ๋งŒ๋“œ๋Š” ๋ฐ ๋„์›€์ด ๋˜๋Š” ๋ผ์ด๋ธŒ๋Ÿฌ๋ฆฌ ๋ฐ ๋„๊ตฌ ๋ชจ์Œ์„ ์ œ๊ณตํ•ฉ๋‹ˆ๋‹ค. ROS๋Š” ๋‹ค์–‘ํ•œ ๋กœ๋ด‡ ํ”Œ๋žซํผ๊ณผ ํ•จ๊ป˜ ์ž‘๋™ํ•˜๋„๋ก ์„ค๊ณ„๋˜์–ด ๋กœ๋ด‡ ์ „๋ฌธ๊ฐ€๋ฅผ ์œ„ํ•œ ์œ ์—ฐํ•˜๊ณ  ๊ฐ•๋ ฅํ•œ ๋„๊ตฌ์ž…๋‹ˆ๋‹ค.

ROS์˜ ์ฃผ์š” ๊ธฐ๋Šฅ

  1. ๋ชจ๋“ˆ์‹ ์•„ํ‚คํ…์ฒ˜: ROS๋Š” ๋ชจ๋“ˆ์‹ ์•„ํ‚คํ…์ฒ˜๋ฅผ ๊ฐ–์ถ”๊ณ  ์žˆ์–ด ๊ฐœ๋ฐœ์ž๊ฐ€ ๋…ธ๋“œ๋ผ๊ณ  ํ•˜๋Š” ์žฌ์‚ฌ์šฉ ๊ฐ€๋Šฅํ•œ ์ž‘์€ ๊ตฌ์„ฑ ์š”์†Œ๋ฅผ ๊ฒฐํ•ฉํ•˜์—ฌ ๋ณต์žกํ•œ ์‹œ์Šคํ…œ์„ ๊ตฌ์ถ•ํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค. ๊ฐ ๋…ธ๋“œ๋Š” ์ผ๋ฐ˜์ ์œผ๋กœ ํŠน์ • ๊ธฐ๋Šฅ์„ ์ˆ˜ํ–‰ํ•˜๋ฉฐ, ๋…ธ๋“œ๋Š” ์ฃผ์ œ ๋˜๋Š” ์„œ๋น„์Šค๋ฅผ ํ†ตํ•ด ๋ฉ”์‹œ์ง€๋ฅผ ์‚ฌ์šฉํ•˜์—ฌ ์„œ๋กœ ํ†ต์‹ ํ•ฉ๋‹ˆ๋‹ค.

  2. ํ†ต์‹  ๋ฏธ๋“ค์›จ์–ด: ROS๋Š” ํ”„๋กœ์„ธ์Šค ๊ฐ„ ํ†ต์‹ ๊ณผ ๋ถ„์‚ฐ ์ปดํ“จํŒ…์„ ์ง€์›ํ•˜๋Š” ๊ฐ•๋ ฅํ•œ ํ†ต์‹  ์ธํ”„๋ผ๋ฅผ ์ œ๊ณตํ•ฉ๋‹ˆ๋‹ค. ์ด๋Š” ๋ฐ์ดํ„ฐ ์ŠคํŠธ๋ฆผ(ํ† ํ”ฝ)์— ๋Œ€ํ•œ ๊ฒŒ์‹œ-๊ตฌ๋… ๋ชจ๋ธ๊ณผ ์„œ๋น„์Šค ํ˜ธ์ถœ์— ๋Œ€ํ•œ ์š”์ฒญ-์‘๋‹ต ๋ชจ๋ธ์„ ํ†ตํ•ด ์ด๋ฃจ์–ด์ง‘๋‹ˆ๋‹ค.

  3. ํ•˜๋“œ์›จ์–ด ์ถ”์ƒํ™”: ROS๋Š” ํ•˜๋“œ์›จ์–ด์— ๋Œ€ํ•œ ์ถ”์ƒํ™” ๊ณ„์ธต์„ ์ œ๊ณตํ•˜์—ฌ ๊ฐœ๋ฐœ์ž๊ฐ€ ๋””๋ฐ”์ด์Šค์— ๊ตฌ์• ๋ฐ›์ง€ ์•Š๋Š” ์ฝ”๋“œ๋ฅผ ์ž‘์„ฑํ•  ์ˆ˜ ์žˆ๋„๋ก ํ•ฉ๋‹ˆ๋‹ค. ์ด๋ฅผ ํ†ตํ•ด ๋‹ค์–‘ํ•œ ํ•˜๋“œ์›จ์–ด ์„ค์ •์—์„œ ๋™์ผํ•œ ์ฝ”๋“œ๋ฅผ ์‚ฌ์šฉํ•  ์ˆ˜ ์žˆ์œผ๋ฏ€๋กœ ํ†ตํ•ฉ๊ณผ ์‹คํ—˜์ด ๋” ์‰ฌ์›Œ์ง‘๋‹ˆ๋‹ค.

  4. ๋„๊ตฌ ๋ฐ ์œ ํ‹ธ๋ฆฌํ‹ฐ: ROS์—๋Š” ์‹œ๊ฐํ™”, ๋””๋ฒ„๊น… ๋ฐ ์‹œ๋ฎฌ๋ ˆ์ด์…˜์„ ์œ„ํ•œ ๋‹ค์–‘ํ•œ ๋„๊ตฌ์™€ ์œ ํ‹ธ๋ฆฌํ‹ฐ๊ฐ€ ํ•จ๊ป˜ ์ œ๊ณต๋ฉ๋‹ˆ๋‹ค. ์˜ˆ๋ฅผ ๋“ค์–ด RViz๋Š” ์„ผ์„œ ๋ฐ์ดํ„ฐ์™€ ๋กœ๋ด‡ ์ƒํƒœ ์ •๋ณด๋ฅผ ์‹œ๊ฐํ™”ํ•˜๋Š” ๋ฐ ์‚ฌ์šฉ๋˜๋ฉฐ, Gazebo๋Š” ์•Œ๊ณ ๋ฆฌ์ฆ˜๊ณผ ๋กœ๋ด‡ ์„ค๊ณ„๋ฅผ ํ…Œ์ŠคํŠธํ•  ์ˆ˜ ์žˆ๋Š” ๊ฐ•๋ ฅํ•œ ์‹œ๋ฎฌ๋ ˆ์ด์…˜ ํ™˜๊ฒฝ์„ ์ œ๊ณตํ•ฉ๋‹ˆ๋‹ค.

  5. ๊ด‘๋ฒ”์œ„ํ•œ ์—์ฝ”์‹œ์Šคํ…œ: ROS ์ƒํƒœ๊ณ„๋Š” ๋ฐฉ๋Œ€ํ•˜๊ณ  ์ง€์†์ ์œผ๋กœ ์„ฑ์žฅํ•˜๊ณ  ์žˆ์œผ๋ฉฐ, ๋‚ด๋น„๊ฒŒ์ด์…˜, ์กฐ์ž‘, ์ธ์‹ ๋“ฑ ๋‹ค์–‘ํ•œ ๋กœ๋ด‡ ์• ํ”Œ๋ฆฌ์ผ€์ด์…˜์— ์‚ฌ์šฉํ•  ์ˆ˜ ์žˆ๋Š” ์ˆ˜๋งŽ์€ ํŒจํ‚ค์ง€๊ฐ€ ์žˆ์Šต๋‹ˆ๋‹ค. ์ปค๋ฎค๋‹ˆํ‹ฐ๋Š” ์ด๋Ÿฌํ•œ ํŒจํ‚ค์ง€์˜ ๊ฐœ๋ฐœ๊ณผ ์œ ์ง€ ๊ด€๋ฆฌ์— ์ ๊ทน์ ์œผ๋กœ ๊ธฐ์—ฌํ•˜๊ณ  ์žˆ์Šต๋‹ˆ๋‹ค.

ROS ๋ฒ„์ „์˜ ์ง„ํ™”

2007๋…„ ๊ฐœ๋ฐœ ์ดํ›„ ROS๋Š” ์—ฌ๋Ÿฌ ๋ฒ„์ „์„ ํ†ตํ•ด ๋ฐœ์ „ํ•ด ์™”์œผ๋ฉฐ, ๊ฐ ๋ฒ„์ „์€ ๋กœ๋ด‡ ์ปค๋ฎค๋‹ˆํ‹ฐ์˜ ์ฆ๊ฐ€ํ•˜๋Š” ์š”๊ตฌ ์‚ฌํ•ญ์„ ์ถฉ์กฑํ•˜๊ธฐ ์œ„ํ•ด ์ƒˆ๋กœ์šด ๊ธฐ๋Šฅ๊ณผ ๊ฐœ์„  ์‚ฌํ•ญ์„ ๋„์ž…ํ–ˆ์Šต๋‹ˆ๋‹ค. ROS์˜ ๊ฐœ๋ฐœ์€ ํฌ๊ฒŒ ๋‘ ๊ฐ€์ง€ ์‹œ๋ฆฌ์ฆˆ๋กœ ๋ถ„๋ฅ˜ํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค: ROS 1๊ณผ ROS 2. ์ด ๊ฐ€์ด๋“œ๋Š” ROS ๋…ธ์—ํ‹ฑ ๋‹Œ์ œ๋ฏธ๋กœ ์•Œ๋ ค์ง„ ROS 1์˜ ์žฅ๊ธฐ ์ง€์›(LTS) ๋ฒ„์ „์— ์ดˆ์ ์„ ๋งž์ถ”๊ณ  ์žˆ์œผ๋ฉฐ, ์ด์ „ ๋ฒ„์ „์—์„œ๋„ ์ฝ”๋“œ๊ฐ€ ์ž‘๋™ํ•ฉ๋‹ˆ๋‹ค.

ROS 1 ๋Œ€ ROS 2

ROS 1์€ ๋กœ๋ด‡ ๊ฐœ๋ฐœ์„ ์œ„ํ•œ ๊ฒฌ๊ณ ํ•œ ๊ธฐ๋ฐ˜์„ ์ œ๊ณตํ–ˆ์ง€๋งŒ ROS 2๋Š” ๋‹ค์Œ๊ณผ ๊ฐ™์€ ๊ธฐ๋Šฅ์„ ์ œ๊ณตํ•˜์—ฌ ๋‹จ์ ์„ ํ•ด๊ฒฐํ•ฉ๋‹ˆ๋‹ค.

  • ์‹ค์‹œ๊ฐ„ ํผํฌ๋จผ์Šค: ์‹ค์‹œ๊ฐ„ ์‹œ์Šคํ…œ ๋ฐ ๊ฒฐ์ •๋ก ์  ๋™์ž‘์— ๋Œ€ํ•œ ์ง€์›์ด ๊ฐœ์„ ๋˜์—ˆ์Šต๋‹ˆ๋‹ค.
  • ๋ณด์•ˆ: ๋‹ค์–‘ํ•œ ํ™˜๊ฒฝ์—์„œ ์•ˆ์ „ํ•˜๊ณ  ์•ˆ์ •์ ์œผ๋กœ ์šด์˜ํ•  ์ˆ˜ ์žˆ๋„๋ก ๋ณด์•ˆ ๊ธฐ๋Šฅ์ด ๊ฐ•ํ™”๋˜์—ˆ์Šต๋‹ˆ๋‹ค.
  • ํ™•์žฅ์„ฑ: ๋ฉ€ํ‹ฐ ๋กœ๋ด‡ ์‹œ์Šคํ…œ ๋ฐ ๋Œ€๊ทœ๋ชจ ๋ฐฐํฌ์— ๋Œ€ํ•œ ์ง€์› ๊ฐ•ํ™”.
  • ํฌ๋กœ์Šค ํ”Œ๋žซํผ ์ง€์›: Linux๋ฅผ ๋„˜์–ด Windows, macOS ๋“ฑ ๋‹ค์–‘ํ•œ ์šด์˜ ์ฒด์ œ์™€์˜ ํ˜ธํ™˜์„ฑ์ด ํ™•๋Œ€๋˜์—ˆ์Šต๋‹ˆ๋‹ค.
  • ์œ ์—ฐํ•œ ์ปค๋ฎค๋‹ˆ์ผ€์ด์…˜: ๋ณด๋‹ค ์œ ์—ฐํ•˜๊ณ  ํšจ์œจ์ ์ธ ํ”„๋กœ์„ธ์Šค ๊ฐ„ ์ปค๋ฎค๋‹ˆ์ผ€์ด์…˜์„ ์œ„ํ•ด DDS๋ฅผ ์‚ฌ์šฉํ•ฉ๋‹ˆ๋‹ค.

ROS ๋ฉ”์‹œ์ง€ ๋ฐ ์ฃผ์ œ

ROS์—์„œ ๋…ธ๋“œ ๊ฐ„์˜ ํ†ต์‹ ์€ ๋ฉ”์‹œ์ง€์™€ ํ† ํ”ฝ์„ ํ†ตํ•ด ์ด๋ฃจ์–ด์ง‘๋‹ˆ๋‹ค. ๋ฉ”์‹œ์ง€๋Š” ๋…ธ๋“œ ๊ฐ„์— ๊ตํ™˜๋˜๋Š” ์ •๋ณด๋ฅผ ์ •์˜ํ•˜๋Š” ๋ฐ์ดํ„ฐ ๊ตฌ์กฐ์ด๋ฉฐ, ํ† ํ”ฝ์€ ๋ฉ”์‹œ์ง€๋ฅผ ์ฃผ๊ณ ๋ฐ›๋Š” ๋ช…๋ช…๋œ ์ฑ„๋„์ž…๋‹ˆ๋‹ค. ๋…ธ๋“œ๋Š” ํ† ํ”ฝ์— ๋ฉ”์‹œ์ง€๋ฅผ ๊ฒŒ์‹œํ•˜๊ฑฐ๋‚˜ ํ† ํ”ฝ์˜ ๋ฉ”์‹œ์ง€๋ฅผ ๊ตฌ๋…ํ•˜์—ฌ ์„œ๋กœ ์†Œํ†ตํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค. ์ด ๊ฒŒ์‹œ-๊ตฌ๋… ๋ชจ๋ธ์„ ์‚ฌ์šฉํ•˜๋ฉด ๋…ธ๋“œ ๊ฐ„์— ๋น„๋™๊ธฐ ํ†ต์‹  ๋ฐ ๋ถ„๋ฆฌ๊ฐ€ ๊ฐ€๋Šฅํ•ฉ๋‹ˆ๋‹ค. ๋กœ๋ด‡ ์‹œ์Šคํ…œ์˜ ๊ฐ ์„ผ์„œ ๋˜๋Š” ์•ก์ถ”์—์ดํ„ฐ๋Š” ์ผ๋ฐ˜์ ์œผ๋กœ ๋ฐ์ดํ„ฐ๋ฅผ ํ† ํ”ฝ์— ๊ฒŒ์‹œํ•˜๊ณ , ๋‹ค๋ฅธ ๋…ธ๋“œ์—์„œ ์ฒ˜๋ฆฌ ๋˜๋Š” ์ œ์–ด๋ฅผ ์œ„ํ•ด ๋ฐ์ดํ„ฐ๋ฅผ ์‚ฌ์šฉํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค. ์ด ๊ฐ€์ด๋“œ์—์„œ๋Š” ์ด๋ฏธ์ง€, ๊นŠ์ด, ํฌ์ธํŠธ ํด๋ผ์šฐ๋“œ ๋ฉ”์‹œ์ง€์™€ ์นด๋ฉ”๋ผ ํ† ํ”ฝ์— ์ดˆ์ ์„ ๋งž์ถ”๊ฒ ์Šต๋‹ˆ๋‹ค.

์„ค์ • Ultralytics YOLO ROS์™€ ํ•จ๊ป˜

์ด ๊ฐ€์ด๋“œ๋Š” ROSbot ROS ๋ฆฌํฌ์ง€ํ† ๋ฆฌ์˜ ํฌํฌ์ธ ์ด ROS ํ™˜๊ฒฝ์„ ์‚ฌ์šฉํ•˜์—ฌ ํ…Œ์ŠคํŠธ๋˜์—ˆ์Šต๋‹ˆ๋‹ค. ์ด ํ™˜๊ฒฝ์—๋Š” Ultralytics YOLO ํŒจํ‚ค์ง€, ์†์‰ฌ์šด ์„ค์ •์„ ์œ„ํ•œ ๋„์ปค ์ปจํ…Œ์ด๋„ˆ, ํฌ๊ด„์ ์ธ ROS ํŒจํ‚ค์ง€, ๋น ๋ฅธ ํ…Œ์ŠคํŠธ๋ฅผ ์œ„ํ•œ ๊ฐ€์ œ๋ณด ์›”๋“œ๊ฐ€ ํฌํ•จ๋˜์–ด ์žˆ์Šต๋‹ˆ๋‹ค. ์ด ํ™˜๊ฒฝ์€ ํ›„์‚ฌ๋ฆฌ์˜จ ROSbot 2 PRO์™€ ํ•จ๊ป˜ ์ž‘๋™ํ•˜๋„๋ก ์„ค๊ณ„๋˜์—ˆ์Šต๋‹ˆ๋‹ค. ์ œ๊ณต๋˜๋Š” ์ฝ”๋“œ ์˜ˆ์ œ๋Š” ์‹œ๋ฎฌ๋ ˆ์ด์…˜๊ณผ ์‹ค์ œ ํ™˜๊ฒฝ์„ ํฌํ•จํ•œ ๋ชจ๋“  ROS Noetic/Melodic ํ™˜๊ฒฝ์—์„œ ์ž‘๋™ํ•ฉ๋‹ˆ๋‹ค.

ํ›„์‚ฌ๋ฆฌ์˜จ ROS๋ด‡ 2 PRO

์ข…์†์„ฑ ์„ค์น˜

ROS ํ™˜๊ฒฝ๊ณผ๋Š” ๋ณ„๋„๋กœ ๋‹ค์Œ ์ข…์†์„ฑ์„ ์„ค์น˜ํ•ด์•ผ ํ•ฉ๋‹ˆ๋‹ค.

  • ROS ๋„˜ํ”ผ ํŒจํ‚ค์ง€: ROS ์ด๋ฏธ์ง€ ๋ฉ”์‹œ์ง€์™€ ์ˆซ์ž ๋ฐฐ์—ด ๊ฐ„์˜ ๋น ๋ฅธ ๋ณ€ํ™˜์„ ์œ„ํ•ด ํ•„์š”ํ•ฉ๋‹ˆ๋‹ค.

    pip install ros_numpy
    
  • Ultralytics ํŒจํ‚ค์ง€์ž…๋‹ˆ๋‹ค:

    pip install ultralytics
    

์“ฐ๋‹ค Ultralytics ROS์™€ ํ•จ๊ป˜ sensor_msgs/Image

๊ทธ๋ฆฌ๊ณ  sensor_msgs/Image ๋ฉ”์‹œ์ง€ ์œ ํ˜• ๋Š” ์ด๋ฏธ์ง€ ๋ฐ์ดํ„ฐ๋ฅผ ํ‘œํ˜„ํ•˜๊ธฐ ์œ„ํ•ด ROS์—์„œ ์ผ๋ฐ˜์ ์œผ๋กœ ์‚ฌ์šฉ๋ฉ๋‹ˆ๋‹ค. ์—ฌ๊ธฐ์—๋Š” ์ธ์ฝ”๋”ฉ, ๋†’์ด, ๋„ˆ๋น„, ํ”ฝ์…€ ๋ฐ์ดํ„ฐ ํ•„๋“œ๊ฐ€ ํฌํ•จ๋˜์–ด ์žˆ์–ด ์นด๋ฉ”๋ผ๋‚˜ ๊ธฐํƒ€ ์„ผ์„œ๋กœ ์บก์ฒ˜ํ•œ ์ด๋ฏธ์ง€๋ฅผ ์ „์†กํ•˜๋Š” ๋ฐ ์ ํ•ฉํ•ฉ๋‹ˆ๋‹ค. ์ด๋ฏธ์ง€ ๋ฉ”์‹œ์ง€๋Š” ์‹œ๊ฐ ์ธ์‹, ๋ฌผ์ฒด ๊ฐ์ง€, ๋‚ด๋น„๊ฒŒ์ด์…˜๊ณผ ๊ฐ™์€ ์ž‘์—…์„ ์œ„ํ•ด ๋กœ๋ด‡ ์• ํ”Œ๋ฆฌ์ผ€์ด์…˜์—์„œ ๋„๋ฆฌ ์‚ฌ์šฉ๋ฉ๋‹ˆ๋‹ค.

ROS ๊ฐ€์ œ๋ณด์˜ ํƒ์ง€ ๋ฐ ์„ธ๋ถ„ํ™”

์ด๋ฏธ์ง€ ๋‹จ๊ณ„๋ณ„ ์‚ฌ์šฉ๋ฒ•

๋‹ค์Œ ์ฝ”๋“œ ์Šค๋‹ˆํŽซ์€ Ultralytics YOLO ํŒจํ‚ค์ง€๋ฅผ ROS์™€ ํ•จ๊ป˜ ์‚ฌ์šฉํ•˜๋Š” ๋ฐฉ๋ฒ•์„ ๋ณด์—ฌ์ค๋‹ˆ๋‹ค. ์ด ์˜ˆ์—์„œ๋Š” ์นด๋ฉ”๋ผ ํ† ํ”ฝ์„ ๊ตฌ๋…ํ•˜๊ณ  YOLO ์„ ์‚ฌ์šฉํ•˜์—ฌ ๋“ค์–ด์˜ค๋Š” ์ด๋ฏธ์ง€๋ฅผ ์ฒ˜๋ฆฌํ•œ ๋‹ค์Œ ๊ฐ์ง€๋œ ๊ฐ์ฒด๋ฅผ ์ƒˆ ํ† ํ”ฝ์— ๊ฒŒ์‹œํ•˜์—ฌ ๊ฐ์ง€ ๋ฐ ๋ถ„ํ• ์„ ์ˆ˜ํ–‰ํ•ฉ๋‹ˆ๋‹ค.

๋จผ์ € ํ•„์š”ํ•œ ๋ผ์ด๋ธŒ๋Ÿฌ๋ฆฌ๋ฅผ ๊ฐ€์ ธ์™€์„œ ๋‹ค์Œ ๋‘ ๊ฐ€์ง€ ๋ชจ๋ธ์„ ์ธ์Šคํ„ด์Šคํ™”ํ•ฉ๋‹ˆ๋‹ค. ์„ธ๋ถ„ํ™” ๊ทธ๋ฆฌ๊ณ  ํ•˜๋‚˜๋Š” ํƒ์ง€. ROS ๋…ธ๋“œ๋ฅผ ์ดˆ๊ธฐํ™”ํ•ฉ๋‹ˆ๋‹ค( ultralytics)๋ฅผ ์‚ฌ์šฉํ•˜์—ฌ ROS ๋งˆ์Šคํ„ฐ์™€ ํ†ต์‹ ํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค. ์•ˆ์ •์ ์ธ ์—ฐ๊ฒฐ์„ ๋ณด์žฅํ•˜๊ธฐ ์œ„ํ•ด ์ž ๊น์˜ ์ผ์‹œ ์ค‘์ง€๋ฅผ ํฌํ•จํ•˜์—ฌ ์ง„ํ–‰ํ•˜๊ธฐ ์ „์— ๋…ธ๋“œ๊ฐ€ ์—ฐ๊ฒฐ์„ ์„ค์ •ํ•  ์ˆ˜ ์žˆ๋Š” ์ถฉ๋ถ„ํ•œ ์‹œ๊ฐ„์„ ์ œ๊ณตํ•ฉ๋‹ˆ๋‹ค.

import time

import rospy

from ultralytics import YOLO

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

๋‘ ๊ฐœ์˜ ROS ์ฃผ์ œ๋ฅผ ์ดˆ๊ธฐํ™”ํ•ฉ๋‹ˆ๋‹ค. ํƒ์ง€ ๊ทธ๋ฆฌ๊ณ  ํ•˜๋‚˜๋Š” ์„ธ๋ถ„ํ™”. ์ด ์ฃผ์ œ๋Š” ์ฃผ์„์ด ๋‹ฌ๋ฆฐ ์ด๋ฏธ์ง€๋ฅผ ๊ฒŒ์‹œํ•˜๋Š” ๋ฐ ์‚ฌ์šฉ๋˜์–ด ์ถ”๊ฐ€ ์ฒ˜๋ฆฌ๋ฅผ ์œ„ํ•ด ์•ก์„ธ์Šคํ•  ์ˆ˜ ์žˆ๋„๋ก ํ•ฉ๋‹ˆ๋‹ค. ๋…ธ๋“œ ๊ฐ„์˜ ํ†ต์‹ ์€ ๋‹ค์Œ์„ ์‚ฌ์šฉํ•˜์—ฌ ์ด‰์ง„๋ฉ๋‹ˆ๋‹ค. 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)

๋งˆ์ง€๋ง‰์œผ๋กœ, ๋ฉ”์‹œ์ง€๋ฅผ ์ˆ˜์‹ ํ•˜๋Š” ๊ตฌ๋…์ž๋ฅผ ๋งŒ๋“ญ๋‹ˆ๋‹ค. /camera/color/image_raw topic์„ ํ˜ธ์ถœํ•˜๊ณ  ๊ฐ๊ฐ์˜ ์ƒˆ ๋ฉ”์‹œ์ง€์— ๋Œ€ํ•œ ์ฝœ๋ฐฑ ํ•จ์ˆ˜๋ฅผ ํ˜ธ์ถœํ•ฉ๋‹ˆ๋‹ค. ์ด ์ฝœ๋ฐฑ ํ•จ์ˆ˜๋Š” ๋‹ค์Œ๊ณผ ๊ฐ™์€ ์œ ํ˜•์˜ ๋ฉ”์‹œ์ง€๋ฅผ ์ˆ˜์‹ ํ•ฉ๋‹ˆ๋‹ค. sensor_msgs/Image๋ฅผ ์‚ฌ์šฉํ•˜์—ฌ numpy ๋ฐฐ์—ด๋กœ ๋ณ€ํ™˜ํ•ฉ๋‹ˆ๋‹ค. ros_numpy๋Š” ์ด์ „์— ์ธ์Šคํ„ด์Šคํ™”๋œ ์ด๋ฏธ์ง€๋ฅผ ์ฒ˜๋ฆฌํ•ฉ๋‹ˆ๋‹ค. YOLO models, ์ด๋ฏธ์ง€์— ์ฃผ์„์„ ์ถ”๊ฐ€ํ•œ ๋‹ค์Œ, ํ•ด๋‹น ์ฃผ์ œ์— ๋‹ค์‹œ ๊ฒŒ์‹œํ•ฉ๋‹ˆ๋‹ค. /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("yolov8m.pt")
segmentation_model = YOLO("yolov8m-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()
๋””๋ฒ„๊น…

ROS(Robot Operating System) ๋…ธ๋“œ๋ฅผ ๋””๋ฒ„๊น…ํ•˜๋Š” ๊ฒƒ์€ ์‹œ์Šคํ…œ์˜ ๋ถ„์‚ฐ ํŠน์„ฑ์œผ๋กœ ์ธํ•ด ์–ด๋ ค์šธ ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค. ์ด ํ”„๋กœ์„ธ์Šค๋ฅผ ์ง€์›ํ•˜๋Š” ๋ช‡ ๊ฐ€์ง€ ๋„๊ตฌ๊ฐ€ ์žˆ์Šต๋‹ˆ๋‹ค.

  1. rostopic echo <TOPIC-NAME> : ์ด ๋ช…๋ น์„ ์‚ฌ์šฉํ•˜๋ฉด ํŠน์ • ์ฃผ์ œ์— ๊ฒŒ์‹œ๋œ ๋ฉ”์‹œ์ง€๋ฅผ ๋ณผ ์ˆ˜ ์žˆ์œผ๋ฏ€๋กœ ๋ฐ์ดํ„ฐ ํ๋ฆ„์„ ๊ฒ€์‚ฌํ•˜๋Š” ๋ฐ ๋„์›€์ด ๋ฉ๋‹ˆ๋‹ค.
  2. rostopic list: ์ด ๋ช…๋ น์„ ์‚ฌ์šฉํ•˜์—ฌ ROS ์‹œ์Šคํ…œ์—์„œ ์‚ฌ์šฉ ๊ฐ€๋Šฅํ•œ ๋ชจ๋“  ํ† ํ”ฝ์„ ๋‚˜์—ดํ•˜๊ณ  ํ™œ์„ฑ ๋ฐ์ดํ„ฐ ์ŠคํŠธ๋ฆผ์— ๋Œ€ํ•œ ๊ฐœ์š”๋ฅผ ์ œ๊ณตํ•ฉ๋‹ˆ๋‹ค.
  3. rqt_graph: ์ด ์‹œ๊ฐํ™” ๋„๊ตฌ๋Š” ๋…ธ๋“œ ๊ฐ„์˜ ํ†ต์‹  ๊ทธ๋ž˜ํ”„๋ฅผ ํ‘œ์‹œํ•˜์—ฌ ๋…ธ๋“œ๊ฐ€ ์ƒํ˜ธ ์—ฐ๊ฒฐ๋˜๋Š” ๋ฐฉ์‹๊ณผ ์ƒํ˜ธ ์ž‘์šฉํ•˜๋Š” ๋ฐฉ์‹์— ๋Œ€ํ•œ ํ†ต์ฐฐ๋ ฅ์„ ์ œ๊ณตํ•ฉ๋‹ˆ๋‹ค.
  4. 3D ํ‘œํ˜„๊ณผ ๊ฐ™์ด ๋” ๋ณต์žกํ•œ ์‹œ๊ฐํ™”์˜ ๊ฒฝ์šฐ ๋‹ค์Œ์„ ์‚ฌ์šฉํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค. RViz. RViz(ROS ์‹œ๊ฐํ™”)๋Š” ROS๋ฅผ ์œ„ํ•œ ๊ฐ•๋ ฅํ•œ 3D ์‹œ๊ฐํ™” ๋„๊ตฌ์ž…๋‹ˆ๋‹ค. ๋กœ๋ด‡์˜ ์ƒํƒœ์™€ ์ฃผ๋ณ€ ํ™˜๊ฒฝ์„ ์‹ค์‹œ๊ฐ„์œผ๋กœ ์‹œ๊ฐํ™”ํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค. RViz๋ฅผ ์‚ฌ์šฉํ•˜๋ฉด ์„ผ์„œ ๋ฐ์ดํ„ฐ๋ฅผ ๋ณผ ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค(์˜ˆ sensors_msgs/Image), ๋กœ๋ด‡ ๋ชจ๋ธ ์ƒํƒœ ๋ฐ ๊ธฐํƒ€ ๋‹ค์–‘ํ•œ ์œ ํ˜•์˜ ์ •๋ณด๋ฅผ ํ†ตํ•ด ๋กœ๋ด‡ ์‹œ์Šคํ…œ์˜ ๋™์ž‘์„ ๋” ์‰ฝ๊ฒŒ ๋””๋ฒ„๊น…ํ•˜๊ณ  ์ดํ•ดํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค.

Publish Detected Classes with std_msgs/String

ํ‘œ์ค€ ROS ๋ฉ”์‹œ์ง€์—๋Š” ๋‹ค์Œ๋„ ํฌํ•จ๋ฉ๋‹ˆ๋‹ค std_msgs/String ๋ฉ”์‹œ์ง€. ๋งŽ์€ ์‘์šฉ ํ”„๋กœ๊ทธ๋žจ์—์„œ๋Š” ์ฃผ์„์ด ๋‹ฌ๋ฆฐ ์ „์ฒด ์ด๋ฏธ์ง€๋ฅผ ๋‹ค์‹œ ๊ฒŒ์‹œํ•  ํ•„์š”๊ฐ€ ์—†์Šต๋‹ˆ๋‹ค. ๋Œ€์‹  ๋กœ๋ด‡์˜ ๋ทฐ์— ์žˆ๋Š” ํด๋ž˜์Šค๋งŒ ํ•„์š”ํ•ฉ๋‹ˆ๋‹ค. ๋‹ค์Œ ์˜ˆ์ œ์—์„œ๋Š” ๋‹ค์Œ์„ ์‚ฌ์šฉํ•˜๋Š” ๋ฐฉ๋ฒ•์„ ๋ณด์—ฌ ์ค๋‹ˆ๋‹ค. std_msgs/String ๋ฉ”์‹œ์ง€ ๋ฅผ ์‚ฌ์šฉํ•˜์—ฌ ๊ฐ์ง€๋œ ํด๋ž˜์Šค๋ฅผ ๋‹ค์‹œ ๊ฒŒ์‹œํ•ฉ๋‹ˆ๋‹ค. /ultralytics/detection/classes ์ฃผ์ œ. ์ด๋Ÿฌํ•œ ๋ฉ”์‹œ์ง€๋Š” ๋” ๊ฐ€๋ณ๊ณ  ํ•„์ˆ˜ ์ •๋ณด๋ฅผ ์ œ๊ณตํ•˜๋ฏ€๋กœ ๋‹ค์–‘ํ•œ ์‘์šฉ ํ”„๋กœ๊ทธ๋žจ์— ์œ ์šฉํ•ฉ๋‹ˆ๋‹ค.

์‚ฌ์šฉ ์‚ฌ๋ก€ ์˜ˆ์‹œ

์นด๋ฉ”๋ผ์™€ ๋ฌผ์ฒด๊ฐ€ ์žฅ์ฐฉ๋œ ์ฐฝ๊ณ  ๋กœ๋ด‡์„ ์˜ˆ๋กœ ๋“ค์–ด ๋ณด๊ฒ ์Šต๋‹ˆ๋‹ค. ํƒ์ง€ ๋ชจ๋ธ. ๋กœ๋ด‡์€ ๋„คํŠธ์›Œํฌ๋ฅผ ํ†ตํ•ด ์ฃผ์„์ด ๋‹ฌ๋ฆฐ ๋Œ€์šฉ๋Ÿ‰ ์ด๋ฏธ์ง€๋ฅผ ์ „์†กํ•˜๋Š” ๋Œ€์‹  ๊ฐ์ง€๋œ ํด๋ž˜์Šค ๋ชฉ๋ก์„ ๋‹ค์Œ๊ณผ ๊ฐ™์ด ๊ฒŒ์‹œํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค. std_msgs/String ๋ฉ”์‹œ์ง€. ์˜ˆ๋ฅผ ๋“ค์–ด, ๋กœ๋ด‡์ด "์ƒ์ž", "ํŒ”๋ ˆํŠธ" ๋ฐ "์ง€๊ฒŒ์ฐจ"์™€ ๊ฐ™์€ ๋ฌผ์ฒด๋ฅผ ๊ฐ์ง€ํ•˜๋ฉด ์ด๋Ÿฌํ•œ ํด๋ž˜์Šค๋ฅผ /ultralytics/detection/classes ์ฃผ์ œ. ๊ทธ๋Ÿฐ ๋‹ค์Œ ์ค‘์•™ ๋ชจ๋‹ˆํ„ฐ๋ง ์‹œ์Šคํ…œ์—์„œ ์ด ์ •๋ณด๋ฅผ ์‚ฌ์šฉํ•˜์—ฌ ์‹ค์‹œ๊ฐ„์œผ๋กœ ์žฌ๊ณ ๋ฅผ ์ถ”์ ํ•˜๊ฑฐ๋‚˜, ๋กœ๋ด‡์˜ ๊ฒฝ๋กœ ๊ณ„ํš์„ ์ตœ์ ํ™”ํ•˜์—ฌ ์žฅ์• ๋ฌผ์„ ํ”ผํ•˜๊ฑฐ๋‚˜, ๊ฐ์ง€๋œ ์ƒ์ž๋ฅผ ์ง‘์–ด ๋“œ๋Š” ๊ฒƒ๊ณผ ๊ฐ™์€ ํŠน์ • ์ž‘์—…์„ ํŠธ๋ฆฌ๊ฑฐํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค. ์ด ์ ‘๊ทผ ๋ฐฉ์‹์€ ํ†ต์‹ ์— ํ•„์š”ํ•œ ๋Œ€์—ญํญ์„ ์ค„์ด๊ณ  ์ค‘์š”ํ•œ ๋ฐ์ดํ„ฐ ์ „์†ก์— ์ค‘์ ์„ ๋‘ก๋‹ˆ๋‹ค.

๋ฌธ์ž์—ด ๋‹จ๊ณ„๋ณ„ ์‚ฌ์šฉ๋ฒ•

์ด ์˜ˆ์ œ์—์„œ๋Š” Ultralytics YOLO ROS์™€ ํ•จ๊ป˜ ํŒจํ‚ค์ง€. ์ด ์˜ˆ์—์„œ๋Š” ์นด๋ฉ”๋ผ ์ฃผ์ œ๋ฅผ ๊ตฌ๋…ํ•˜๊ณ  ๋‹ค์Œ์„ ์‚ฌ์šฉํ•˜์—ฌ ๋“ค์–ด์˜ค๋Š” ์ด๋ฏธ์ง€๋ฅผ ์ฒ˜๋ฆฌํ•ฉ๋‹ˆ๋‹ค. YOLO๋ฅผ ํด๋ฆญํ•˜๊ณ  ๊ฐ์ง€๋œ ๊ฐ์ฒด๋ฅผ ์ƒˆ ํ•ญ๋ชฉ์— ๊ฒŒ์‹œํ•ฉ๋‹ˆ๋‹ค /ultralytics/detection/classes ์‚ฌ์šฉ std_msgs/String ๋ฉ”์‹œ์ง€. ์ด ros_numpy package๋Š” 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("yolov8m.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 ROS ๊นŠ์ด ์ด๋ฏธ์ง€ ์‚ฌ์šฉ

ROS๋Š” RGB ์ด๋ฏธ์ง€ ์™ธ์—๋„ ์นด๋ฉ”๋ผ์—์„œ ๋ฌผ์ฒด๊นŒ์ง€์˜ ๊ฑฐ๋ฆฌ์— ๋Œ€ํ•œ ์ •๋ณด๋ฅผ ์ œ๊ณตํ•˜๋Š” ๊นŠ์ด ์ด๋ฏธ์ง€๋ฅผ ์ง€์›ํ•ฉ๋‹ˆ๋‹ค. ์‹ฌ๋„ ์ด๋ฏธ์ง€๋Š” ์žฅ์• ๋ฌผ ํšŒํ”ผ, 3D ๋งคํ•‘, ๋กœ์ปฌ๋ผ์ด์ œ์ด์…˜๊ณผ ๊ฐ™์€ ๋กœ๋ด‡ ์• ํ”Œ๋ฆฌ์ผ€์ด์…˜์— ๋งค์šฐ ์ค‘์š”ํ•œ ์š”์†Œ์ž…๋‹ˆ๋‹ค.

๊นŠ์ด ์ด๋ฏธ์ง€๋Š” ๊ฐ ํ”ฝ์…€์ด ์นด๋ฉ”๋ผ์—์„œ ๋ฌผ์ฒด๊นŒ์ง€์˜ ๊ฑฐ๋ฆฌ๋ฅผ ๋‚˜ํƒ€๋‚ด๋Š” ์ด๋ฏธ์ง€์ž…๋‹ˆ๋‹ค. ์ƒ‰์ƒ์„ ์บก์ฒ˜ํ•˜๋Š” RGB ์ด๋ฏธ์ง€์™€ ๋‹ฌ๋ฆฌ ๊นŠ์ด ์ด๋ฏธ์ง€๋Š” ๊ณต๊ฐ„ ์ •๋ณด๋ฅผ ์บก์ฒ˜ํ•˜์—ฌ ๋กœ๋ด‡์ด ํ™˜๊ฒฝ์˜ 3D ๊ตฌ์กฐ๋ฅผ ์ธ์‹ํ•  ์ˆ˜ ์žˆ๋„๋ก ํ•ฉ๋‹ˆ๋‹ค.

๊นŠ์ด ์ด๋ฏธ์ง€ ์–ป๊ธฐ

๊นŠ์ด ์ด๋ฏธ์ง€๋Š” ๋‹ค์–‘ํ•œ ์„ผ์„œ๋ฅผ ์‚ฌ์šฉํ•˜์—ฌ ์–ป์„ ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค.

  1. ์Šคํ…Œ๋ ˆ์˜ค ์นด๋ฉ”๋ผ: ๋‘ ๋Œ€์˜ ์นด๋ฉ”๋ผ๋ฅผ ์‚ฌ์šฉํ•˜์—ฌ ์ด๋ฏธ์ง€ ๋ถˆ๊ท ํ˜•์— ๋”ฐ๋ผ ๊นŠ์ด๋ฅผ ๊ณ„์‚ฐํ•ฉ๋‹ˆ๋‹ค.
  2. ToF(๋น„ํ–‰ ์‹œ๊ฐ„) ์นด๋ฉ”๋ผ: ๋น›์ด ๋ฌผ์ฒด์—์„œ ๋Œ์•„์˜ค๋Š” ๋ฐ ๊ฑธ๋ฆฌ๋Š” ์‹œ๊ฐ„์„ ์ธก์ •ํ•ฉ๋‹ˆ๋‹ค.
  3. ๊ตฌ์กฐํ˜• ๊ด‘ ์„ผ์„œ: ํŒจํ„ด์„ ํˆฌ์‚ฌํ•˜๊ณ  ํ‘œ๋ฉด์˜ ๋ณ€ํ˜•์„ ์ธก์ •ํ•ฉ๋‹ˆ๋‹ค.

์‚ฌ์šฉ YOLO ๊นŠ์ด ์ด๋ฏธ์ง€ ํฌํ•จ

ROS์—์„œ ๊นŠ์ด ์˜์ƒ์€ sensor_msgs/Image ๋ฉ”์‹œ์ง€ ์œ ํ˜• - ์ธ์ฝ”๋”ฉ, ๋†’์ด, ๋„ˆ๋น„ ๋ฐ ํ”ฝ์…€ ๋ฐ์ดํ„ฐ์— ๋Œ€ํ•œ ํ•„๋“œ๋ฅผ ํฌํ•จํ•ฉ๋‹ˆ๋‹ค. ๊นŠ์ด ์ด๋ฏธ์ง€์˜ ์ธ์ฝ”๋”ฉ ํ•„๋“œ๋Š” ์ข…์ข… "16UC1"๊ณผ ๊ฐ™์€ ํ˜•์‹์„ ์‚ฌ์šฉํ•˜์—ฌ ํ”ฝ์…€๋‹น 16๋น„ํŠธ ๋ถ€ํ˜ธ ์—†๋Š” ์ •์ˆ˜๋ฅผ ๋‚˜ํƒ€๋‚ด๋ฉฐ, ์—ฌ๊ธฐ์„œ ๊ฐ ๊ฐ’์€ ๊ฐœ์ฒด๊นŒ์ง€์˜ ๊ฑฐ๋ฆฌ๋ฅผ ๋‚˜ํƒ€๋ƒ…๋‹ˆ๋‹ค. ๊นŠ์ด ์ด๋ฏธ์ง€๋Š” ์ผ๋ฐ˜์ ์œผ๋กœ RGB ์ด๋ฏธ์ง€์™€ ํ•จ๊ป˜ ์‚ฌ์šฉ๋˜์–ด ํ™˜๊ฒฝ์— ๋Œ€ํ•œ ๋ณด๋‹ค ํฌ๊ด„์ ์ธ ๋ณด๊ธฐ๋ฅผ ์ œ๊ณตํ•ฉ๋‹ˆ๋‹ค.

์‚ฌ์šฉ YOLO๋ฅผ ์‚ฌ์šฉํ•˜๋ฉด RGB ๋ฐ ๊นŠ์ด ์ด๋ฏธ์ง€ ๋ชจ๋‘์—์„œ ์ •๋ณด๋ฅผ ์ถ”์ถœํ•˜๊ณ  ๊ฒฐํ•ฉํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค. ์˜ˆ์ปจ๋Œ€ YOLO RGB ์ด๋ฏธ์ง€ ๋‚ด์—์„œ ๋ฌผ์ฒด๋ฅผ ๊ฐ์ง€ํ•  ์ˆ˜ ์žˆ์œผ๋ฉฐ ์ด ๊ฐ์ง€๋Š” ๊นŠ์ด ์ด๋ฏธ์ง€์—์„œ ํ•ด๋‹น ์˜์—ญ์„ ์ •ํ™•ํžˆ ์ฐพ์•„๋‚ด๋Š” ๋ฐ ์‚ฌ์šฉํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค. ์ด๋ฅผ ํ†ตํ•ด ๊ฐ์ง€๋œ ๋ฌผ์ฒด์— ๋Œ€ํ•œ ์ •ํ™•ํ•œ ๊นŠ์ด ์ •๋ณด๋ฅผ ์ถ”์ถœํ•  ์ˆ˜ ์žˆ์–ด ๋กœ๋ด‡์ด ํ™˜๊ฒฝ์„ 3์ฐจ์›์œผ๋กœ ์ดํ•ดํ•˜๋Š” ๋Šฅ๋ ฅ์ด ํ–ฅ์ƒ๋ฉ๋‹ˆ๋‹ค.

RGB-D ์นด๋ฉ”๋ผ

์‹ฌ๋„ ์ด๋ฏธ์ง€๋กœ ์ž‘์—…ํ•  ๋•Œ๋Š” RGB์™€ ์‹ฌ๋„ ์ด๋ฏธ์ง€๊ฐ€ ์˜ฌ๋ฐ”๋ฅด๊ฒŒ ์ •๋ ฌ๋˜์–ด ์žˆ๋Š”์ง€ ํ™•์ธํ•˜๋Š” ๊ฒƒ์ด ์ค‘์š”ํ•ฉ๋‹ˆ๋‹ค. RealSense ์‹œ๋ฆฌ์ฆˆ( Intel )์™€ ๊ฐ™์€ RGB-D ์นด๋ฉ”๋ผ๋Š” ๋™๊ธฐํ™”๋œ 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("yolov8m-seg.pt")

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

๋‹ค์Œ์œผ๋กœ, ๋“ค์–ด์˜ค๋Š” ๊นŠ์ด ์ด๋ฏธ์ง€ ๋ฉ”์‹œ์ง€๋ฅผ ์ฒ˜๋ฆฌํ•˜๋Š” ์ฝœ๋ฐฑ ํ•จ์ˆ˜๋ฅผ ์ •์˜ํ•ฉ๋‹ˆ๋‹ค. ์ด ํ•จ์ˆ˜๋Š” ๊นŠ์ด ์˜์ƒ๊ณผ RGB ์˜์ƒ ๋ฉ”์‹œ์ง€๋ฅผ ๊ธฐ๋‹ค๋ ธ๋‹ค๊ฐ€ numpy ๋ฐฐ์—ด๋กœ ๋ณ€ํ™˜ํ•˜๊ณ  ๋ถ„ํ•  ๋ชจ๋ธ์„ RGB ์˜์ƒ์— ์ ์šฉํ•ฉ๋‹ˆ๋‹ค. ๊ทธ๋Ÿฐ ๋‹ค์Œ ๊ฐ์ง€๋œ ๊ฐ ๋ฌผ์ฒด์— ๋Œ€ํ•œ ๋ถ„ํ•  ๋งˆ์Šคํฌ๋ฅผ ์ถ”์ถœํ•˜๊ณ  ๊นŠ์ด ์ด๋ฏธ์ง€๋ฅผ ์‚ฌ์šฉํ•˜์—ฌ ์นด๋ฉ”๋ผ์—์„œ ๋ฌผ์ฒด๊นŒ์ง€์˜ ํ‰๊ท  ๊ฑฐ๋ฆฌ๋ฅผ ๊ณ„์‚ฐํ•ฉ๋‹ˆ๋‹ค. ๋Œ€๋ถ€๋ถ„์˜ ์„ผ์„œ์—๋Š” ํด๋ฆฝ ๊ฑฐ๋ฆฌ๋ผ๊ณ  ํ•˜๋Š” ์ตœ๋Œ€ ๊ฑฐ๋ฆฌ๊ฐ€ ์žˆ์œผ๋ฉฐ, ์ด ๊ฑฐ๋ฆฌ๋ฅผ ์ดˆ๊ณผํ•˜๋ฉด ๊ฐ’์ด inf(np.inf). ์ฒ˜๋ฆฌํ•˜๊ธฐ ์ „์— ์ด๋Ÿฌํ•œ null ๊ฐ’์„ ํ•„ํ„ฐ๋งํ•˜๊ณ  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)

    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

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

    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

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


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

while True:
    rospy.spin()

์“ฐ๋‹ค Ultralytics ROS์™€ ํ•จ๊ป˜ sensor_msgs/PointCloud2

ROS ๊ฐ€์ œ๋ณด์˜ ํƒ์ง€ ๋ฐ ์„ธ๋ถ„ํ™”

๊ทธ๋ฆฌ๊ณ  sensor_msgs/PointCloud2 ๋ฉ”์‹œ์ง€ ์œ ํ˜• ๋Š” ROS์—์„œ 3D ํฌ์ธํŠธ ํด๋ผ์šฐ๋“œ ๋ฐ์ดํ„ฐ๋ฅผ ํ‘œํ˜„ํ•˜๋Š” ๋ฐ ์‚ฌ์šฉ๋˜๋Š” ๋ฐ์ดํ„ฐ ๊ตฌ์กฐ์ž…๋‹ˆ๋‹ค. ์ด ๋ฉ”์‹œ์ง€ ์œ ํ˜•์€ ๋กœ๋ด‡ ์• ํ”Œ๋ฆฌ์ผ€์ด์…˜์— ํ•„์ˆ˜์ ์ธ ์š”์†Œ๋กœ 3D ๋งคํ•‘, ๋ฌผ์ฒด ์ธ์‹, ๋กœ์ปฌ๋ผ์ด์ œ์ด์…˜ ๋“ฑ์˜ ์ž‘์—…์„ ๊ฐ€๋Šฅํ•˜๊ฒŒ ํ•ฉ๋‹ˆ๋‹ค.

ํฌ์ธํŠธ ํด๋ผ์šฐ๋“œ๋Š” 3์ฐจ์› ์ขŒํ‘œ๊ณ„ ๋‚ด์— ์ •์˜๋œ ๋ฐ์ดํ„ฐ ํฌ์ธํŠธ์˜ ๋ชจ์Œ์ž…๋‹ˆ๋‹ค. ์ด๋Ÿฌํ•œ ๋ฐ์ดํ„ฐ ํฌ์ธํŠธ๋Š” 3D ์Šค์บ๋‹ ๊ธฐ์ˆ ์„ ํ†ตํ•ด ์บก์ฒ˜๋œ ๋ฌผ์ฒด ๋˜๋Š” ์žฅ๋ฉด์˜ ์™ธ๋ถ€ ํ‘œ๋ฉด์„ ๋‚˜ํƒ€๋ƒ…๋‹ˆ๋‹ค. ํด๋ผ์šฐ๋“œ์˜ ๊ฐ ์ง€์ ์—๋Š” X, Y๋ฐ Z ์ขŒํ‘œ๋Š” ๊ณต๊ฐ„์—์„œ์˜ ์œ„์น˜์— ํ•ด๋‹นํ•˜๋ฉฐ ์ƒ‰์ƒ ๋ฐ ๊ฐ•๋„์™€ ๊ฐ™์€ ์ถ”๊ฐ€ ์ •๋ณด๋„ ํฌํ•จํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค.

์ฐธ์กฐ ํ”„๋ ˆ์ž„

๋กœ ์ž‘์—…ํ•  ๋•Œ sensor_msgs/PointCloud2, ํฌ์ธํŠธ ํด๋ผ์šฐ๋“œ ๋ฐ์ดํ„ฐ๊ฐ€ ์ˆ˜์ง‘๋œ ์„ผ์„œ์˜ ๊ธฐ์ค€ ํ”„๋ ˆ์ž„์„ ๊ณ ๋ คํ•˜๋Š” ๊ฒƒ์ด ์ค‘์š”ํ•ฉ๋‹ˆ๋‹ค. ํฌ์ธํŠธ ํด๋ผ์šฐ๋“œ๋Š” ์ฒ˜์Œ์— ์„ผ์„œ์˜ ์ฐธ์กฐ ํ”„๋ ˆ์ž„์—์„œ ์บก์ฒ˜๋ฉ๋‹ˆ๋‹ค. ์ด ์ฐธ์กฐ ํ”„๋ ˆ์ž„์€ ๋‹ค์Œ์„ ๋“ฃ๊ณ  ํ™•์ธํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค. /tf_static ์ฃผ์ œ. ๊ทธ๋Ÿฌ๋‚˜ ํŠน์ • ์‘์šฉ ํ”„๋กœ๊ทธ๋žจ ์š”๊ตฌ ์‚ฌํ•ญ์— ๋”ฐ๋ผ ํฌ์ธํŠธ ํด๋ผ์šฐ๋“œ๋ฅผ ๋‹ค๋ฅธ ์ฐธ์กฐ ํ”„๋ ˆ์ž„์œผ๋กœ ๋ณ€ํ™˜ํ•ด์•ผ ํ•  ์ˆ˜๋„ ์žˆ์Šต๋‹ˆ๋‹ค. ์ด ๋ณ€ํ™˜์€ ๋‹ค์Œ์„ ์‚ฌ์šฉํ•˜์—ฌ ์ˆ˜ํ–‰ํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค. tf2_ros package๋Š” ์ขŒํ‘œ ํ”„๋ ˆ์ž„์„ ๊ด€๋ฆฌํ•˜๊ณ  ์ขŒํ‘œ ํ”„๋ ˆ์ž„ ๊ฐ„์— ๋ฐ์ดํ„ฐ๋ฅผ ๋ณ€ํ™˜ํ•˜๊ธฐ ์œ„ํ•œ ๋„๊ตฌ๋ฅผ ์ œ๊ณตํ•ฉ๋‹ˆ๋‹ค.

ํฌ์ธํŠธ ํด๋ผ์šฐ๋“œ ์–ป๊ธฐ

ํฌ์ธํŠธ ํด๋ผ์šฐ๋“œ๋Š” ๋‹ค์–‘ํ•œ ์„ผ์„œ๋ฅผ ์‚ฌ์šฉํ•˜์—ฌ ์–ป์„ ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค.

  1. LIDAR(๋น› ๊ฐ์ง€ ๋ฐ ๊ฑฐ๋ฆฌ ์ธก์ •): ๋ ˆ์ด์ € ํŽ„์Šค๋ฅผ ์‚ฌ์šฉํ•˜์—ฌ ๋ฌผ์ฒด๊นŒ์ง€์˜ ๊ฑฐ๋ฆฌ๋ฅผ ์ธก์ •ํ•˜๊ณ  ๊ณ ์ •๋ฐ€ 3D ์ง€๋„๋ฅผ ์ƒ์„ฑํ•ฉ๋‹ˆ๋‹ค.
  2. ๋Ž์Šค ์นด๋ฉ”๋ผ: ๊ฐ ํ”ฝ์…€์— ๋Œ€ํ•œ ๊นŠ์ด ์ •๋ณด๋ฅผ ์บก์ฒ˜ํ•˜์—ฌ ์žฅ๋ฉด์„ 3D๋กœ ์žฌ๊ตฌ์„ฑํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค.
  3. ์Šคํ…Œ๋ ˆ์˜ค ์นด๋ฉ”๋ผ: ๋‘ ๋Œ€ ์ด์ƒ์˜ ์นด๋ฉ”๋ผ๋ฅผ ํ™œ์šฉํ•˜์—ฌ ์‚ผ๊ฐ ์ธก๋Ÿ‰์„ ํ†ตํ•ด ๊นŠ์ด ์ •๋ณด๋ฅผ ์–ป์Šต๋‹ˆ๋‹ค.
  4. ๊ตฌ์กฐ๊ด‘ ์Šค์บ๋„ˆ: ์•Œ๋ ค์ง„ ํŒจํ„ด์„ ํ‘œ๋ฉด์— ํˆฌ์‚ฌํ•˜๊ณ  ๋ณ€ํ˜•์„ ์ธก์ •ํ•˜์—ฌ ๊นŠ์ด๋ฅผ ๊ณ„์‚ฐํ•ฉ๋‹ˆ๋‹ค.

์‚ฌ์šฉ YOLO ํฌ์ธํŠธ ํด๋ผ์šฐ๋“œ ์‚ฌ์šฉ

ํ†ตํ•ฉํ•˜๋ ค๋ฉด YOLO ์™€ sensor_msgs/PointCloud2 ์œ ํ˜• ๋ฉ”์‹œ์ง€, ๊นŠ์ด ๋งต์— ์‚ฌ์šฉ๋˜๋Š” ๊ฒƒ๊ณผ ์œ ์‚ฌํ•œ ๋ฐฉ๋ฒ•์„ ์‚ฌ์šฉํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค. ํฌ์ธํŠธ ํด๋ผ์šฐ๋“œ์— ํฌํ•จ๋œ ์ƒ‰์ƒ ์ •๋ณด๋ฅผ ํ™œ์šฉํ•˜์—ฌ 2D ์ด๋ฏธ์ง€๋ฅผ ์ถ”์ถœํ•˜๊ณ  ๋‹ค์Œ์„ ์‚ฌ์šฉํ•˜์—ฌ ์ด ์ด๋ฏธ์ง€์— ๋Œ€ํ•œ ๋ถ„ํ• ์„ ์ˆ˜ํ–‰ํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค. YOLO๋ฅผ ํด๋ฆญํ•œ ๋‹ค์Œ ๊ฒฐ๊ณผ ๋งˆ์Šคํฌ๋ฅผ 3์ฐจ์› ์ ์— ์ ์šฉํ•˜์—ฌ ๊ด€์‹ฌ ์žˆ๋Š” 3D ๊ฐœ์ฒด๋ฅผ ๋ถ„๋ฆฌํ•ฉ๋‹ˆ๋‹ค.

ํฌ์ธํŠธ ํด๋ผ์šฐ๋“œ๋ฅผ ์ฒ˜๋ฆฌํ•˜๋ ค๋ฉด 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("yolov8m-seg.pt")

ํ•จ์ˆ˜ ๋งŒ๋“ค๊ธฐ pointcloud2_to_array, sensor_msgs/PointCloud2 ๋ฉ”์‹œ์ง€๋ฅผ ๋‘ ๊ฐœ์˜ numpy ๋ฐฐ์—ด๋กœ ๋‚˜๋ˆ•๋‹ˆ๋‹ค. ์ด sensor_msgs/PointCloud2 ๋ฉ”์‹œ์ง€์—๋Š” ๋‹ค์Œ์ด ํฌํ•จ๋ฉ๋‹ˆ๋‹ค. n ๋ฅผ ๊ธฐ๋ฐ˜์œผ๋กœ ํ•œ ํฌ์ธํŠธ width ๊ทธ๋ฆฌ๊ณ  height ํš๋“ํ•œ ์ด๋ฏธ์ง€์˜. ์˜ˆ๋ฅผ ๋“ค์–ด, 480 x 640 ์ด๋ฏธ์ง€๋Š” 307,200 ํฌ์ธํŠธ. ๊ฐ ์ ์—๋Š” ์„ธ ๊ฐœ์˜ ๊ณต๊ฐ„ ์ขŒํ‘œ(xyz) ๋ฐ ํ•ด๋‹น ์ƒ‰์ƒ RGB ํŒ. ์ด๋Š” ๋‘ ๊ฐœ์˜ ๊ฐœ๋ณ„ ์ •๋ณด ์ฑ„๋„๋กœ ๊ฐ„์ฃผ๋  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค.

์ด ํ•จ์ˆ˜๋Š” xyz coordinates ๋ฐ RGB ์›๋ž˜ ์นด๋ฉ”๋ผ ํ•ด์ƒ๋„ ํ˜•์‹์˜ ๊ฐ’(width x height). ๋Œ€๋ถ€๋ถ„์˜ ์„ผ์„œ์—๋Š” ํด๋ฆฝ ๊ฑฐ๋ฆฌ๋ผ๊ณ  ํ•˜๋Š” ์ตœ๋Œ€ ๊ฑฐ๋ฆฌ๊ฐ€ ์žˆ์œผ๋ฉฐ, ์ด ๊ฑฐ๋ฆฌ๋ฅผ ์ดˆ๊ณผํ•˜๋ฉด ๊ฐ’์ด inf(np.inf). ์ฒ˜๋ฆฌํ•˜๊ธฐ ์ „์— ์ด๋Ÿฌํ•œ null ๊ฐ’์„ ํ•„ํ„ฐ๋งํ•˜๊ณ  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 ๋ฉ”์‹œ์ง€๋ฅผ XYZ ์ขŒํ‘œ์™€ RGB ๊ฐ’์„ ํฌํ•จํ•˜๋Š” numpy ๋ฐฐ์—ด๋กœ ๋ณ€ํ™˜ํ•ฉ๋‹ˆ๋‹ค( pointcloud2_to_array ๊ธฐ๋Šฅ). ๋ฅผ ์‚ฌ์šฉํ•˜์—ฌ RGB ์ด๋ฏธ์ง€๋ฅผ ์ฒ˜๋ฆฌํ•ฉ๋‹ˆ๋‹ค. YOLO model์„ ์‚ฌ์šฉํ•˜์—ฌ ๋ถ„ํ• ๋œ ๊ฐ์ฒด๋ฅผ ์ถ”์ถœํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค. ๊ฐ์ง€๋œ ๊ฐ ๊ฐ์ฒด์— ๋Œ€ํ•ด ๋ถ„ํ•  ๋งˆ์Šคํฌ๋ฅผ ์ถ”์ถœํ•˜๊ณ  RGB ์˜์ƒ๊ณผ XYZ ์ขŒํ‘œ ๋ชจ๋‘์— ์ ์šฉํ•˜์—ฌ 3D ๊ณต๊ฐ„์—์„œ ๊ฐ์ฒด๋ฅผ ๋ถ„๋ฆฌํ•ฉ๋‹ˆ๋‹ค.

๋งˆ์Šคํฌ ์ฒ˜๋ฆฌ๋Š” ์ด์ง„ ๊ฐ’์œผ๋กœ ๊ตฌ์„ฑ๋˜๊ธฐ ๋•Œ๋ฌธ์— ๊ฐ„๋‹จํ•ฉ๋‹ˆ๋‹ค. 1 ๋ฌผ์ฒด์˜ ์กด์žฌ๋ฅผ ๋‚˜ํƒ€๋‚ด๊ณ  0 ๋ถ€์žฌ๋ฅผ ๋‚˜ํƒ€๋ƒ…๋‹ˆ๋‹ค. ๋งˆ์Šคํฌ๋ฅผ ์ ์šฉํ•˜๋ ค๋ฉด ์›๋ณธ ์ฑ„๋„์— ๋งˆ์Šคํฌ๋ฅผ ๊ณฑํ•˜๋ฉด ๋ฉ๋‹ˆ๋‹ค. ์ด ์ž‘์—…์€ ์ด๋ฏธ์ง€ ๋‚ด์—์„œ ๊ด€์‹ฌ ์žˆ๋Š” ๊ฐœ์ฒด๋ฅผ ํšจ๊ณผ์ ์œผ๋กœ ๊ฒฉ๋ฆฌํ•ฉ๋‹ˆ๋‹ค. ๋งˆ์ง€๋ง‰์œผ๋กœ Open3D ํฌ์ธํŠธ ํด๋ผ์šฐ๋“œ ๊ฐ์ฒด๋ฅผ ์ƒ์„ฑํ•˜๊ณ  ์„ธ๊ทธ๋จผํŠธํ™”๋œ ๊ฐ์ฒด๋ฅผ ์—ฐ๊ด€๋œ ์ƒ‰์ƒ๊ณผ ํ•จ๊ป˜ 3D ๊ณต๊ฐ„์—์„œ ์‹œ๊ฐํ™”ํ•ฉ๋‹ˆ๋‹ค.

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 ultralytics import YOLO

rospy.init_node("ultralytics")
time.sleep(1)
segmentation_model = YOLO("yolov8m-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])

ํฌ์ธํŠธ ํด๋ผ์šฐ๋“œ ์„ธ๋ถ„ํ™” Ultralytics

์ž์ฃผ ๋ฌป๋Š” ์งˆ๋ฌธ

๋กœ๋ด‡ ์šด์˜ ์ฒด์ œ(ROS)๋ž€ ๋ฌด์—‡์ธ๊ฐ€์š”?

ROS(๋กœ๋ด‡ ์šด์˜ ์ฒด์ œ) ๋Š” ๊ฐœ๋ฐœ์ž๊ฐ€ ๊ฐ•๋ ฅํ•œ ๋กœ๋ด‡ ์• ํ”Œ๋ฆฌ์ผ€์ด์…˜์„ ๋งŒ๋“œ๋Š” ๋ฐ ๋„์›€์ด ๋˜๋Š” ๋กœ๋ด‡ ๊ณตํ•™์—์„œ ์ผ๋ฐ˜์ ์œผ๋กœ ์‚ฌ์šฉ๋˜๋Š” ์˜คํ”ˆ ์†Œ์Šค ํ”„๋ ˆ์ž„์›Œํฌ์ž…๋‹ˆ๋‹ค. ๋กœ๋ด‡ ์‹œ์Šคํ…œ ๊ตฌ์ถ• ๋ฐ ์ธํ„ฐํŽ˜์ด์Šค๋ฅผ ์œ„ํ•œ ๋ผ์ด๋ธŒ๋Ÿฌ๋ฆฌ์™€ ๋„๊ตฌ ๋ชจ์Œ์„ ์ œ๊ณตํ•˜์—ฌ ๋ณต์žกํ•œ ์• ํ”Œ๋ฆฌ์ผ€์ด์…˜์„ ๋ณด๋‹ค ์‰ฝ๊ฒŒ ๊ฐœ๋ฐœํ•  ์ˆ˜ ์žˆ๋„๋ก ํ•ด์ค๋‹ˆ๋‹ค. ROS๋Š” ์ฃผ์ œ ๋˜๋Š” ์„œ๋น„์Šค์— ๋Œ€ํ•œ ๋ฉ”์‹œ์ง€๋ฅผ ์‚ฌ์šฉํ•˜์—ฌ ๋…ธ๋“œ ๊ฐ„์˜ ํ†ต์‹ ์„ ์ง€์›ํ•ฉ๋‹ˆ๋‹ค.

์‹ค์‹œ๊ฐ„ ์˜ค๋ธŒ์ ํŠธ ๊ฐ์ง€๋ฅผ ์œ„ํ•ด 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("yolov8m.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 ์„ ์‚ฌ์šฉํ•˜์—ฌ ์ด๋ฏธ์ง€๋ฅผ ์ฒ˜๋ฆฌํ•˜์—ฌ ๊ฐ์ง€ ๋˜๋Š” ๋ถ„ํ• ๊ณผ ๊ฐ™์€ ์ž‘์—…์„ ์ˆ˜ํ–‰ํ•˜๊ณ , ๊ฒฐ๊ณผ๋ฅผ ์ƒˆ ํ† ํ”ฝ์— ๊ฒŒ์‹œํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค.

์˜ˆ๋ฅผ ๋“ค์–ด, ์นด๋ฉ”๋ผ ํ† ํ”ฝ์„ ๊ตฌ๋…ํ•˜๊ณ  ๋“ค์–ด์˜ค๋Š” ์ด๋ฏธ์ง€๋ฅผ ์ฒ˜๋ฆฌํ•˜์—ฌ ๊ฐ์ง€ํ•ฉ๋‹ˆ๋‹ค:

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

ROS์—์„œ ๋Ž์Šค ์ด๋ฏธ์ง€( Ultralytics YOLO )๋ฅผ ์‚ฌ์šฉํ•˜๋Š” ์ด์œ ๋Š” ๋ฌด์—‡์ธ๊ฐ€์š”?

ROS์˜ ๊นŠ์ด ์ด๋ฏธ์ง€๋Š” ๋‹ค์Œ๊ณผ ๊ฐ™์ด ํ‘œํ˜„๋ฉ๋‹ˆ๋‹ค. sensor_msgs/Image๋Š” ์žฅ์• ๋ฌผ ํšŒํ”ผ, 3D ๋งคํ•‘ ๋ฐ ๋กœ์ปฌ๋ผ์ด์ œ์ด์…˜๊ณผ ๊ฐ™์€ ์ž‘์—…์— ์ค‘์š”ํ•œ ์นด๋ฉ”๋ผ๋กœ๋ถ€ํ„ฐ ๋ฌผ์ฒด๊นŒ์ง€์˜ ๊ฑฐ๋ฆฌ๋ฅผ ์ œ๊ณตํ•ฉ๋‹ˆ๋‹ค. ๊ธฐ์ค€ ๊นŠ์ด ์ •๋ณด ์‚ฌ์šฉ RGB ์ด๋ฏธ์ง€์™€ ํ•จ๊ป˜ ๋กœ๋ด‡์ด 3D ํ™˜๊ฒฝ์„ ๋” ์ž˜ ์ดํ•ดํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค.

YOLO ์„ ์‚ฌ์šฉํ•˜๋ฉด RGB ์ด๋ฏธ์ง€์—์„œ ๋ถ„ํ•  ๋งˆ์Šคํฌ๋ฅผ ์ถ”์ถœํ•˜๊ณ  ์ด ๋งˆ์Šคํฌ๋ฅผ ๊นŠ์ด ์ด๋ฏธ์ง€์— ์ ์šฉํ•˜์—ฌ ์ •ํ™•ํ•œ 3D ๋ฌผ์ฒด ์ •๋ณด๋ฅผ ์–ป์„ ์ˆ˜ ์žˆ์œผ๋ฏ€๋กœ ๋กœ๋ด‡์ด ์ฃผ๋ณ€ ํ™˜๊ฒฝ์„ ํƒ์ƒ‰ํ•˜๊ณ  ์ƒํ˜ธ ์ž‘์šฉํ•˜๋Š” ๋Šฅ๋ ฅ์ด ํ–ฅ์ƒ๋ฉ๋‹ˆ๋‹ค.

ROS์—์„œ YOLO ์„ ์‚ฌ์šฉํ•˜์—ฌ 3D ํฌ์ธํŠธ ํด๋ผ์šฐ๋“œ๋ฅผ ์‹œ๊ฐํ™”ํ•˜๋ ค๋ฉด ์–ด๋–ป๊ฒŒ ํ•ด์•ผ ํ•˜๋‚˜์š”?

YOLO ์„ ์‚ฌ์šฉํ•˜์—ฌ ROS์—์„œ 3D ํฌ์ธํŠธ ํด๋ผ์šฐ๋“œ๋ฅผ ์‹œ๊ฐํ™”ํ•˜๋ ค๋ฉด:

  1. ๋ณ€ํ™˜ sensor_msgs/PointCloud2 ๋ฉ”์‹œ์ง€๋ฅผ ๋ฐฐ์—ด์— ์ „๋‹ฌํ•ฉ๋‹ˆ๋‹ค.
  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("yolov8m-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])

์ด ์ ‘๊ทผ ๋ฐฉ์‹์€ ์„ธ๊ทธ๋จผํŠธํ™”๋œ ๊ฐ์ฒด๋ฅผ 3D ์‹œ๊ฐํ™”ํ•˜์—ฌ ํƒ์ƒ‰ ๋ฐ ์กฐ์ž‘๊ณผ ๊ฐ™์€ ์ž‘์—…์— ์œ ์šฉํ•ฉ๋‹ˆ๋‹ค.



์ƒ์„ฑ 2024-06-19, ์—…๋ฐ์ดํŠธ 2024-07-05
์ž‘์„ฑ์ž: glenn-jocher (2), RizwanMunawar (1), ambitious-octopus (3)

๋Œ“๊ธ€