6. เขียน publisher กับ subscriber

หัวข้อนี้เขียน node แรกที่เป็นของเราเอง — ตัวส่งกับตัวรับ

ตัวส่ง (publisher)

สร้างไฟล์ ~/ros2_ws/src/my_robot/my_robot/speed_sender.py

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist


class SpeedSender(Node):
    def __init__(self):
        super().__init__('speed_sender')
        # ตัวเลข 10 คือ queue — เก็บได้ 10 ข้อความถ้าตัวรับตามไม่ทัน
        self.pub = self.create_publisher(Twist, '/cmd_vel', 10)
        # ส่งทุก 0.1 วินาที = 10 Hz
        self.timer = self.create_timer(0.1, self.tick)

    def tick(self):
        msg = Twist()
        msg.linear.x = 0.2      # เดินหน้า 0.2 เมตร/วินาที
        msg.angular.z = 0.0     # ไม่หมุน
        self.pub.publish(msg)


def main():
    rclpy.init()
    node = SpeedSender()
    try:
        rclpy.spin(node)        # วนรอจนกว่าจะกด Ctrl+C
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

Important

อย่าเขียน while True: แล้ว sleep() เอง — ให้ใช้ create_timer

rclpy.spin() เป็นตัวจัดคิวงานทั้งหมดของ node ทั้งการส่ง การรับ และ timer ถ้าเขียนลูปเองจะไปบล็อก spin ทำให้ข้อความที่เข้ามาไม่ถูกประมวลผล อาการคือ subscriber เงียบสนิททั้งที่โค้ดถูก

ตัวรับ (subscriber)

สร้างไฟล์ ~/ros2_ws/src/my_robot/my_robot/speed_watcher.py

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist


class SpeedWatcher(Node):
    def __init__(self):
        super().__init__('speed_watcher')
        self.sub = self.create_subscription(
            Twist, '/cmd_vel', self.on_msg, 10)

    def on_msg(self, msg):
        self.get_logger().info(
            'เดินหน้า %.2f m/s · หมุน %.2f rad/s'
            % (msg.linear.x, msg.angular.z))


def main():
    rclpy.init()
    node = SpeedWatcher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

Tip

ใช้ self.get_logger().info() แทน print()

ข้อความจะมีเวลาและชื่อ node กำกับ ไปโผล่ใน ros2 topic echo /rosout ด้วย และถูกบันทึกลงไฟล์ log อัตโนมัติ — ตามหาย้อนหลังได้

บอกให้ระบบรู้จัก — แก้ setup.py

เขียนไฟล์แล้วยังเรียกไม่ได้ ต้องลงทะเบียนก่อน

เปิด ~/ros2_ws/src/my_robot/setup.py แล้วแก้ส่วน entry_points

    entry_points={
        'console_scripts': [
            'speed_sender = my_robot.speed_sender:main',
            'speed_watcher = my_robot.speed_watcher:main',
        ],
    },

อ่านว่า: ชื่อคำสั่ง = โมดูล:ฟังก์ชัน

Warning

🔴 ขั้นนี้ลืมกันบ่อยที่สุดในบท เขียนโค้ดเสร็จ build ผ่าน แต่รันแล้วได้

No executable found

สาเหตุคือไม่ได้เพิ่มบรรทัดใน entry_points — และเพราะ build ผ่าน จึงไม่มีอะไรฟ้องว่าลืม

build แล้วลอง

cd ~/ros2_ws
colcon build --symlink-install --packages-select my_robot
source install/setup.bash

เปิด 2 terminal (source ทั้งคู่)

ros2 run my_robot speed_sender     # terminal 1
ros2 run my_robot speed_watcher    # terminal 2

terminal 2 จะพิมพ์ทุก 0.1 วินาที

[INFO] [speed_watcher]: เดินหน้า 0.20 m/s · หมุน 0.00 rad/s

ตรวจด้วยเครื่องมือจากหัวข้อ 4

โดยไม่ต้องหยุดโปรแกรม เปิด terminal ที่ 3

ros2 topic info -v /cmd_vel     # ควรเห็น Publisher 1 · Subscription 1
ros2 topic hz /cmd_vel          # ควรได้ราว 10 Hz
ros2 topic echo /cmd_vel        # เห็นข้อมูลเหมือนที่ speed_watcher เห็น

Note

ลองปิด speed_sender แล้วดู ros2 topic info /cmd_vel อีกครั้ง — Publisher count จะกลายเป็น 0 ส่วน speed_watcher จะเงียบไปเฉย ๆ โดยไม่มี error

จำอาการนี้ไว้ให้ดี เพราะกับหุ่นจริงมันหน้าตาเหมือนกันเป๊ะ

เอาไปใช้กับหุ่นจริง

โค้ด speed_sender ข้างบนใช้กับหุ่น AGV จริงได้ทันที เพราะ /cmd_vel กับ Twist คือมาตรฐานเดียวกัน

Danger

ก่อนรันกับหุ่นจริง — โค้ดนี้สั่งเดินหน้าทันทีที่เปิดและไม่มีตัวหยุด

• ยกล้อลอยพื้นก่อน

• รู้ว่าปุ่มหยุดฉุกเฉินอยู่ตรงไหน

• เริ่มที่ความเร็วต่ำกว่านี้ เช่น 0.05

และในระบบจริง /cmd_vel ควรผ่านตัวจำกัดความเร็วกับตัวหยุดเมื่อเจอสิ่งกีดขวางก่อน ไม่ใช่ต่อตรงเข้าล้อ

---

ก่อนหน้า: 5. workspace · ต่อไป: 7. launch file กับ parameter