STM32 ARM에서 ROS와 연동하기(2, subscriber)

ROS(Robot OS System)

STM32 ARM에서 ROS와 연동하기(1, publisher) 에 이어서, Master PC로부터 메시지를 받아 동작하는 코드를 구현해본다.

 

Master PC에서 servo motor의 position 값을 메세지로 날려주면, microcontroller에서 메세지를 읽고 servo motor의 위치를 조절한다.

 

먼저, 지난 번과 동일하게 ros_lib_kinetic를 사용하며 추가로 servo 라이브러리를 받았다.

그리고 메세지의 데이터 타입은 float형을 사용한다.

ros_lib_kinetic과 servo 라이브러리 추가

 

 

publisher와 달리 subscriber는 callback 함수를 만들어서 사용한다는 차이가 있다. 메세지를 받아서 그에 맞게 적절한 행동을 취할 수 있게 된다.

subscriber 생성

전체 코드는 아래와 같다.

 

[ Code ]

#include "mbed.h"
#include "Servo.h"
#include <ros.h>
#include <std_msgs/Float32.h>
 
Servo myservo(PC_8);
 
ros::NodeHandle nh;
/////////////////////////////////////////////////////
 
DigitalOut led1(LED1);
//////////////////////////////////////////////////////
void servo_cb(const std_msgs::Float32& cmd_msg)
{
   myservo.position(cmd_msg.data);
}
 
//////////////////////////////////////////////////////
ros::Subscriber<std_msgs::Float32> sub("servo", servo_cb);
 
//////////////////////////////////////////////////////
int main()
{
    nh.initNode();
    nh.subscribe(sub);
    nh.getHardware()->setBaud(57600);
  
    while(1) {
    
        led1 = !led1;
        nh.spinOnce();       
    }
}