ROS에서 아두이노로 만든 로봇 제어하기 - 일단은 LED 깜빡거리기

ROS에서도 아두이노와 작업을 할 수 있습니다. 이와 관련된 내용을 한 번 다룬 적이 있습니다. rosserial을 사용해서 OpenCR 보드의 IMU 값을 받아오는 내용을 다루었던 적이 있네요^^ 이번에도 그렇게 접근하려고 합니다. 그래서 아두이노에 연결된 LED를 끄고 키는걸 해볼려구요~ (물론 언제나 그렇듯 초보적 내용입니다. ㅎㅎ^^)

아두이노 설치및 세팅

아두이노를 다운 받아서 HOME/에 tools 폴더에 복사해두고, 아두이노를 한 번 실행하고 나면 위 그림처럼 HOME에 Arduino폴더가 또 하나 생깁니다. 그 안에 libraries 폴더도 생기구요... 거기로 가서 작업하는게 좀 편하죠.^^

거기서, 

rosrun rosserial_arduino make_libraries.py .

위의 명령을 수행합니다. 그러면

아두이노를 실행해서 라이브러리 예제를 보면 ros_lib이 생성된 것을 알 수 있습니다. 이제 아두이노 세팅은 되었네요.

그리로 시리얼포트에 읽고쓰기 권한도 함께 주었습니다. 이제 실습 대상이 있어야죠... 아두이노 보드의 Hello World를 담당하는 13번 LED를 사용하고 싶었지만...

젤리비 보드와 로봇 젤리비 소개....

이번에는 젤리비 보드를 사용할려구요.

이 아이가 젤리비보드입니다. 아두이노 나노 호환이구요. 버튼과 LED, 부저, 조명센서 등등을 가지고 있지요. 조만간 이 젤리비보드를 소개해 드릴께요. 지금은 그냥 제 책상에 있는 아두이노 보드라서 사용합니다.^^

그 젤리비 보드가 장착된 이 아이가, 로봇 젤리비입니다. 괜찮게 생겼죠?^^

그걸 저렇게 PC와 연결하면 되죠^^

ROS와 연결하는 아두이노 코드

아두이노쪽 코드를 작성해야 합니다. ROS의 아두이노 관련 투토리얼을 보고 작성했습니다. 너무 쉽게 되어 있어서 오히려 실수를 할 수가 없더라구요^^ 아 그전에 rosserial 페이지에서 데이터형의 선언과 필요한 그래서 설치해야할 패키지등을 확인해야죠.

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

String LED;

ros::NodeHandle  nh;

먼저 필요한 ros.h와 std_msgs/String.h를 include합니다.

std_msgs::String led_pos;
ros::Publisher LED_status("LED_status", &led_pos);

그리고 아두이노에서 Publisher를 설정합니다. LED_status로 이름을 잡아야죠. 꺼졌는지 켜졌는지를 알려주는 거라고 생각하죠. 다음 학습은 나만의 메세지를 만드는 것을 해야겠어요.

void action_LED( const std_msgs::String& msg){
  LED = msg.data;
  LED_status.publish(&msg);

  if (LED=="left") {
    digitalWrite(6, HIGH-digitalRead(6));
  }

  if (LED=="right") {
    digitalWrite(5, HIGH-digitalRead(5));
  }
}

ros::Subscriber<std_msgs::String> sub("toggle_led", &action_LED );

그 다음은  PC에서 명령을 주면 받아서 처리할 Subscriber를 준비합니다. 콜백함수를 만들어 주고, left, right에 따라 깜박거리도록 합니다.

void setup()
{ 
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  
  nh.initNode();
  nh.advertise(LED_status);
  nh.subscribe(sub);
}

아두이노 setup 함수에서 중요한 세 줄이 있네요. init하는 코드입니다. publish에는 advertise, subscribe에는 subscribe을 사용합니다.

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

그리고 loop()에서는 그냥 spin 실행하기... 전체 코드는 아래입니다.

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

String LED;

ros::NodeHandle  nh;

std_msgs::String led_pos;
ros::Publisher LED_status("LED_status", &led_pos);

void action_LED( const std_msgs::String& msg){
  LED = msg.data;
  LED_status.publish(&msg);

  if (LED=="left") {
    digitalWrite(6, HIGH-digitalRead(6));
  }

  if (LED=="right") {
    digitalWrite(5, HIGH-digitalRead(5));
  }
}

ros::Subscriber<std_msgs::String> sub("toggle_led", &action_LED );

void setup()
{ 
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  
  nh.initNode();
  nh.advertise(LED_status);
  nh.subscribe(sub);
}

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

ROS에서 실행해보기

이제 실행해보죠~~~ 달콤한 열매 먹기~~~

일단 아두이노 컴파일 후에 업로드하고~ 이후 절차는 동영상으로 대신합니다. 사용한 명령은 아래와 같습니다.

• roscore

• rosrun rosserial_python serial_node.py /dev/ttyUSB0

• rosrun pub /toggle_led std_msgs/String "data: "left""

그러면 아두이노로 된 로봇(로봇젤리비)에 명령을 인가할 수 있습니다.

rqt_graph로 보면 확실히 알 수 있습니다. serial_node는 아두이노로 된 로봇 젤리비입니다. PC에서 LED를 제어하는 명령이 toggle_led로 들어가고, 로봇 젤리비에서는 LED의 상태를 LED_status를 통해 PC로 알려주는 것이죠.