Robot/ROS

[MicroROS] Publisher와 Subscriber 생성하기

interactics 2022. 8. 6. 22:00

MicroROS + Arduino DUE 글에서 이어집니다.

 

[MicroROS] MicroRos + Arduino DUE 튜토리얼

레퍼런스 https://github.com/micro-ROS/micro_ros_arduino GitHub - micro-ROS/micro_ros_arduino: micro-ROS library for Arduino micro-ROS library for Arduino. Contribute to micro-ROS/micro_ros_arduino d..

huroint.tistory.com

 

마이크로로스의 ROS 클라이언트 라이브러리는 C언어로 생성된 rclc를 사용합니다.

 

본 예제의 개발 환경은

보드 - 아두이노 DUE

PC의 OS - Mac M1 + 패러렐즈 Ubuntu 20.04

ROS2 - Foxy

입니다.

Publisher

다음은 마이크로로스의 Publisher 공식예제입니다.

#include <micro_ros_arduino.h>

#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>

#include <std_msgs/msg/int32.h>

rcl_publisher_t publisher;
std_msgs__msg__Int32 msg;
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;

#define LED_PIN 13

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}


void error_loop(){
  while(1){
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{  
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
    msg.data++;
  }
}

void setup() {
  set_microros_transports();
  
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);  
  
  delay(2000);

  allocator = rcl_get_default_allocator();

  //create init_options
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  // create node
  RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_node", "", &support));

  // create publisher
  RCCHECK(rclc_publisher_init_default(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "micro_ros_arduino_node_publisher"));

  // create timer,
  const unsigned int timer_timeout = 1000;
  RCCHECK(rclc_timer_init_default(
    &timer,
    &support,
    RCL_MS_TO_NS(timer_timeout),
    timer_callback));

  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_timer(&executor, &timer));

  msg.data = 0;
}

void loop() {
  delay(100);
  RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}

 

도입

rcl_publisher_t publisher;
std_msgs__msg__Int32 msg;
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;

마이크로로스의 타입은 ROS2의 타입 이름과 약간의 차이가 있습니다.

cpp 환경에서 ROS메시지 선언시 std_msgs::msg::Int32라 작성하지만, 
c환경에서는 std_msgs__msg__Int32와 같이 2개의 언더바로 메시지의 소속을 정의합니다.

 

rclcpp와 달리 rclc에서는 executor, support, allocator를 선언해야합니다.

 

allocator 객체는 임베디드 환경에서 동적 메모리의 할당과 해제를 관리합니다.

support 객체는 ROS 클라이언트를 생성을 도와주는 역할을 합니다. rcl-node, rcl-subscription, rcl-timer, rclc-executor 생성을 단순화시켜줍니다.

executor 객체는 rclcpp의 executor와 같은 역할을 수행합니다. Subscription과 timer callback을 관리합니다.

 

#define LED_PIN 13

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}

매크로 부분에는 RCCHECK(fn)과 RCSOFTCHECK(fn)이 있습니다.

두 함수 모두 ROS 클라이언트가 제대로 생성이 되어있는지 검사합니다.
RCCHECK(fn)는 클라이언트 생성이 실패하면 error_loop()로 보내 해당 프로그래밍 진행을 중단합니다.

void error_loop(){
  while(1){
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{  
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
    msg.data++;
  }
}

error_loop는 ROS클라이언트 생성 실패시 호출 목적으로 선언되었습니다.

 

timer_callback은 타이머에 의해 일정 주기로 호출됩니다. 

여기서 RCLC_UNUSED함수는 매크로 함수로 린터 경고(linter warning)를 방지하기 위해 작성되었습니다.

- 린터 경고는 빌드엔 문제가 없으나 미사용된 변수과 같은 부분을 경고해주는 역할을 합니다. last_call_time는 여기서 사용하지 않는 변수라 린터 경고가 발생하는데, 이를 막기위해 작성되었습니다.

 

setup()

void setup() {
  set_microros_transports();
  
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);  
  
  delay(2000);

  allocator = rcl_get_default_allocator();

  //create init_options
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  // create node
  RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_node", "", &support));

  // create publisher
  RCCHECK(rclc_publisher_init_default(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "micro_ros_arduino_node_publisher"));

  // create timer,
  const unsigned int timer_timeout = 1000;
  RCCHECK(rclc_timer_init_default(
    &timer,
    &support,
    RCL_MS_TO_NS(timer_timeout),
    timer_callback));

  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_timer(&executor, &timer));

  msg.data = 0;
}

 

set_microros_transports()는 아두이노에 맞추어진 transports를 생성해줍니다. 

transport 부분은 네트워크 이론 영역이라 넘어가겠습니다.

 

allocator = rcl_get_default_allocator(); 

디폴트 메모리 할당 정책을 allocator 객체에 전달합니다.

 

임베디드 시스템은 보유한 메모리량이 매우 적습니다. 더불어 메모리 할당 순서에 따라 메모리가 비어있음에도 메모리가 부족해지는 단편화 현상이 발생하기도 합니다.
이에 임베디드에서는 동적 메모리 할당 정책을 일반적인 소프트웨어와 달리 해주게 되는데, 그 정책을 사용자가 커스터마이징할 수 있도록 allocator 객체를 만들어 놓은것입니다.

 

RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_node", "", &support));

노드 생성하는 부분입니다. 노드의 이름은 "micro_ros_arduino_node"이며 네임스페이스는 없이 생성됩니다.

rclc_node_init_default(&node, node_name, namespace, &support);

 

 

RCCHECK(rclc_publisher_init_default(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "micro_ros_arduino_node_publisher"));

 

퍼블리셔를 생성하는 부분입니다.

rclc_publisher_init_default(&publisher, &node, type_support, topic_name);

위 규칙에 맞게 작성하면 됩니다.

특이사항으로 ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32)부분은 type_support입니다.

ROSIDL_GET_MSG_TYPE_SUPPORT(패키지이름, 하위폴더-msg,service,action-, 메시지타입)으로 작성해 매개변수로 넘겨줍니다.

 

 

const unsigned int timer_timeout = 1000;
RCCHECK(rclc_timer_init_default(
   &timer,
   &support,
   RCL_MS_TO_NS(timer_timeout),
   timer_callback));

 

타이머는 사용자가 정한 시간마다 콜백함수를 실행시킵니다. 

이 경우 매 1000ms마다 정수 값을 증가한 후 토픽을 퍼블리시하는 timer_callback()함수가 실행됩니다.

 

RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
RCCHECK(rclc_executor_add_timer(&executor, &timer));

 

rclc_executor_init는 executor를 생성합니다.

  rclc_executor_init(&executor, &support.context, num_handles, &allocator);

위와 같은 규칙으로 생성하는데, 여기서 num_handles는 (서브스크라이버 수 +  타이머 수)입니다. 
퍼블리시 예제에는 서브스크라이버는 없고 타이머 하나만 executor로 동작시킬 것이니 '1'로 작성합니다.

rclc_executor_add_timer()를 사용해 timer를 executor에 추가합니다.

 

loop문

void loop() {
  delay(100);
  RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}

rclc_executor_spin_some(&executor, timeout)함수를 이용해 executor를 실행합니다.
이 함수는 한번만 spin하기 때문에 delay(100)로 인해 100ms마다 작업을 수행하게 됩니다.

 

마이크로로스 에이전트 실행 

이를 아두이노에 업로드한 다음 터미널에 아래 명령어를 작성해 마이크로로스 에이전트를 실행합니다.

ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0

 

토픽을 echo 하면 다음과 같이 1초마다 1씩 증가된 데이터가 나타나게 됩니다.

 

Subscriber

이 예제는 아두이노의 LED가 토픽 메시지가 0이면 OFF, 그 외에는 ON되는 코드입니다.

#include <micro_ros_arduino.h>

#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>

#include <std_msgs/msg/int32.h>

rcl_subscription_t subscriber;
std_msgs__msg__Int32 msg;
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;

#define LED_PIN 13

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}


void error_loop(){
  while(1){
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}

void subscription_callback(const void * msgin)
{  
  const std_msgs__msg__Int32 * msg = (const std_msgs__msg__Int32 *)msgin;
  digitalWrite(LED_PIN, (msg->data == 0) ? LOW : HIGH);  
}

void setup() {
  set_microros_transports();
  
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);  
  
  delay(2000);

  allocator = rcl_get_default_allocator();

  //create init_options
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  // create node
  RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_node", "", &support));

  // create subscriber
  RCCHECK(rclc_subscription_init_default(
    &subscriber,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "micro_ros_arduino_subscriber"));

  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));
}

void loop() {
  delay(100);
  RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}

크게 publisher과 다르지 않으므로 차이점이 있는 부분만 다루겠습니다.

 

 

서브스크립션 콜백 함수

void subscription_callback(const void * msgin)
{  
  const std_msgs__msg__Int32 * msg = (const std_msgs__msg__Int32 *)msgin;
  digitalWrite(LED_PIN, (msg->data == 0) ? LOW : HIGH);  
}

rclcpp와 다르게 callback함수의 인자 타입이 const void입니다.
msgin을 가져와 캐스팅하여 사용하니 유의해주세요.

 

Setup부

RCCHECK(rclc_subscription_init_default(
    &subscriber,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "micro_ros_arduino_subscriber"));

  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));

 

퍼블리셔 초기화와 마찬가지로 서브스크라이버도 초기화해주는 함수를 이용해 생성합니다.

rclc_subscription_init_default(&subscriber, &node, type_support, topic_name);

 

rclc_executor_init(&executor, &support.context, 1, &allocator);

여기서는 executor에 subscription 1개만 돌리므로 hanble_num에 '1'로 작성합니다.

 

rclc_executor_add_subscription(&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA);

executor에 서브스크라이버를 추가하는 함수입니다. 여기서 ON_NEW_DATA라는 부분이 퍼블리셔와 차이를 보이는데,

저 부분은 executor의 데이터 핸들링 옵션입니다. ON_NEW_DATA는 새 데이터가 가능할 때만 핸들러가 Callback을 호출할 수 있도록하며, ALWAYS일때는 데이터가 없더라도, 즉 NULL값이여도 Callback이 호출될 수 있게 합니다.

 

 

마이크로로스 에이전트 실행 

마찬가지로 마이크로로스 에이전트를 실행합니다.

ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0

 

RQT를 이용하여 토픽을 퍼블리시 해봅니다.

0이면 LED Off, 그 외 값이면 LED On인 상태가 됩니다.

좌) 13번 LED OFF, 우) 13번 LED ON