เริ่มต้นการเขียน Subscriber node

หลังจากที่เราเรียนรู้สร้าง ROS publisher node ไปแล้ว ต่อไปเราจะมาเรียนรู้การสร้าง ROS subscriber node รอบนี้ก็เหมือนเดิมครับเราเปิดโค้ดตัวอย่างใน Arduino IDE มา โดยกดเข้าไปที่
File -> Examples -> ros_lib -> Blink

เราก็จะเห็นโค้ดหน้าตาแบบด้านล่างนี้


/* 
 * rosserial Subscriber Example
 * Blinks an LED on callback
 */

#include <ros.h>
#include <std_msgs/Empty.h>

ros::NodeHandle  nh;

void messageCbconst std_msgs::Empty& toggle_msg){
  digitalWrite(LED_BUILTIN, HIGH-digitalRead(LED_BUILTIN));   // blink the led
}

ros::Subscriber<std_msgs::Empty> sub("toggle_led", &messageCb );

void setup()
  pinMode(LED_BUILTIN, OUTPUT);
  nh.initNode();
  nh.subscribe(sub);
}

void loop()
{  
  nh.spinOnce();
  delay(1);
}


เรามาอธิบายการทำงานทีละส่วนกัน


#include <ros.h>
#include <std_msgs/Empty.h>


ในส่วนนี้ก็เหมือนเดิม เป็นการ Include header file ชื่อ ros.h เข้ามา ซึ่งเราจะต้องทำทุกครั้ง และในบรรทัดต่อมาเป็นการ Include ros message ที่เป็นแบบ Empty ซึ่ง message นั้นมีเยอะมาก ก็แล้วแต่เราจะเลือกใช้


ros::NodeHandle  nh;


ตัวนี้ก็เหมือนเดิม เป็นการสร้าง ROS Node handle ขึ้นมาเพื่อให้ใช้ในการควบคุม จัดการ Serial port ของ Arduino


void messageCbconst std_msgs::Empty& toggle_msg){
  digitalWrite(LED_BUILTIN, HIGH-digitalRead(LED_BUILTIN));   // blink the led
}


ในส่วนนี้เป็นการสร้าง callback ฟังก์ชั่น สำหรับตัว Subscriber โดย argument ที่รับเข้ามานั้นจะต้องเป็นแบบ constant reference ชนิดของ message จะเป็น standard message Empty

ภายในฟังก์ชั่นจะทำหน้าที่ให้การ toggle หลอดไฟ led ซึ่งผมก็ไม่เข้าใจเหมือนกันว่าทำไมเค้าต้องเขียนเป็น HIGH - digitalRead ถ้าหากเป็นผมก็คงจะใช้เป็น

digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));

แต่ไม่เป็นไรมันสามารถใช้งานได้เหมือนกัน


ros::Subscriber<std_msgs::Empty> sub("toggle_led"&messageCb )


จากนั้นเราก็ประกาศ Subscriber โดยในที่นี้มีชื่อว่า sub ซึ่งจะทำการ subscribe topic ที่มีชื่อว่า "toggle_led" ชนิดของ message เป็น std_msgs::Empty และเมื่อได้รับแล้วให้ทำฟังก์ชั่นที่มีชื่อว่า messageCb ที่ได้ประกาศไปก่อนหน้านี้แล้ว


void setup()
  pinMode(LED_BUILTIN, OUTPUT);
  nh.initNode();
  nh.subscribe(sub);
}


ส่วนนี้ก็เช่นเดิมกับ Publisher node
ฟังก์ชั่น setup เราจะต้องทำการ initialize ตัว Node handle จากนั้นก็บอกว่าจะมีการ publisher ตัวไหนบ้าง หรือ subscriber ตัวไหนบ้าง ซึ่งในที่นี้บอกว่าจะมี Subscriber ที่ชื่อว่า sub


void loop()
{  
  nh.spinOnce();
  delay(1);
}


ส่วนสุดท้ายคือฟังก์ชั่น Loop ในนี้จะทำการเรียก ros::SpinOnce() เพื่อทำให้ ROS ทำงานฟังก์ชั่น callback ทุกครั้งที่มีการส่ง message เข้ามาที่ topic ที่เรากำหนดไว้  

เมื่อเราทำความเข้าใจกับโค้ดทั้งหมดแล้ว เราก็จัดการอัพโหลดโปรแกรมเข้าไปยังบอร์ด Arduino ของเราได้เลย

เมื่ออัพโหลดเสร็จสิ้นแล้วให้เรารัน roscore

$ roscore

จากนั้นเปิดอีกหน้าต่างแล้วใช้คำสั่งรัน rosserial

$ rosrun rosserial_python serial_node.py /dev/ttyACM0

ตอนนี้บอร์ด Arduino ก็เชื่อมต่อกับ ROS เรียบร้อยแล้ว ต่อไปเราก็มาทดสอบกัน โดยการ publish message เข้าไปที่ topic /toggle_led ซึ่งผลลัพธ์ก็คือหลอดไฟ LED โดยใช้คำสั่ง

$ rostopic pub /toggle_led std_msgs/Empty "{}"

เมื่อทำได้แบบนี้แล้ว เราก็น่าจะพอเข้าใจวิธีการเขียน Subscriber node แบบง่ายๆแล้ว

สอนการเขียน subscriber ด้วย arduino โดยใช้ rosserial
Views
78 Total Views
1 Members Views
77 Public Views
Actions
0 Likes
0 Dislikes
0 Comments
Share on Social Networks
Share Link
Use permanent link to share in social media
Share by mail

Please login to share this webpage by email.