아두이노 + ROS 2 튜토리얼 [2]

[지난글]

지난 시간에 아두이노 듀에 보드를 이용하여 topic을 pub 하고 echo로 직접 확인하는 과정을 수행했다.

https://stupidly-honest.tistory.com/39

아두이노 + ROS 2 튜토리얼

---

Subscriber [Arduino Due]

이번시간에는 아두이노 듀에 보드로 값을 pub 하여 모터가 동작하도록 프로그래밍을 해보자

먼저 듀에보드를 subscriber로 만든 후, pub 했을 때 반응을 보도록 하자

지난 글과 유사하게 이번에는 예제에서 micro-ros_subscriber를 실행시킨후, 터미널을 열어서 ros2 run을 실행한다.

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

토픽의 타입을 알아보자

std_msgs/msg/Int32 이다.

이렇게 해서 Arduino due 보드는 subscriber가 되었다.

이제 보드에 topic을 publish 한 다음 결과를 보자

1번 터미널 : ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0

2번 터미널 : ros2 topic list

3버 터미널 : ros2 topic echo /micro_ros_arduino_subscriber

4번 터미널 : ros2 topic pub /micro_ros_arduino_subscriber --once std_msgs/msg/Int32 data:\ 0\

최종 결과 : 3번 터미널에서 data 값이 들어오는 것을 확인할 수 있다.

 

 

 

---

Subscriber [Motor]

다음 듀에 보드로 pub 하는 값에 따라 모터를 동작시켜보자

 

준비물 및 참고 자료는 아래와 같다.

 

Cytron motor driver : MDD20A

Board : Arduino Due SAM 32bit -M3

Arduino Sketch : 1.8.19

ROS Version : ROS 2 humble

Motor

 

Cytron 깃 허브

https://github.com/CytronTechnologies/CytronMotorDriver

GitHub - CytronTechnologies/CytronMotorDriver: Arduino library for Cytron motor drivers.

 

Data sheet

위 데이터 시트를 참고해서  회로를 구성한다.

코드는 아래와 같다.

#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>
#include "CytronMotorDriver.h"

// ROS 2 토픽을 구독하기 위한 구조체와 변수 선언
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;


#define LED_PIN 13

// 모터 드라이버 설정
CytronMD motor1(PWM_DIR, 3, 4);  // PWM 1 = Pin 3, DIR 1 = Pin 4.
CytronMD motor2(PWM_DIR, 9, 10); // PWM 2 = Pin 9, DIR 2 = Pin 10.

// 오류 체크 매크로 정의
#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)){}}

// 오류 발생 시 LED를 깜빡이게 하는 함수
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;

  // 토픽 값을 확인하고 모터를 제어
  int motorSpeed = msg->data;

  if (motorSpeed > 0) {
    // 모터를 전진으로 설정
    motor1.setSpeed(motorSpeed);
    motor2.setSpeed(motorSpeed);
  } else if (motorSpeed < 0) {
    // 모터를 후진으로 설정
    motor1.setSpeed(-motorSpeed);
    motor2.setSpeed(-motorSpeed);
  } else {
    // 모터를 정지
    motor1.setSpeed(0);
    motor2.setSpeed(0);
  }
}

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

  allocator = rcl_get_default_allocator();

  // 초기화 옵션 생성
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  // 노드 생성
  RCCHECK(rclc_node_init_default(&node, "go_back_node", "", &support));

  // 토픽 구독자 생성
  RCCHECK(rclc_subscription_init_default(
    &subscriber,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "go_back_subscriber"));

  // 실행자 생성
  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)));
}

go_back_test.ino

0.00MB

먼저 아두이노 스케치에서 코드를 컴파일 한다.

실행방법은 이전과 유사하다.

다른 점은 노드 이름을 micro_ros_arduino_subscriber에서 go_bact_subscriber로 수정했다는 점과

코드에서 확인할 수 있는 값에 따라 모터를 제어하는 부분이다.

cytron 깃허브에 있는 예제 pwm_dir _dual 과 micro_ros 예제의 micro_ros_subscriber 코드를 이용했다. 

메세지 포맷에 맞게 publish 하는 것을 주의하자

 

Demo Video

https://youtube.com/shorts/fPd9ORIVrfM?feature=share 

 

감사합니다.