왕다방

인공지능 & 로봇 사용자들의 자유게시판입니다. 인공지능, 가정용로봇, 서비스로봇, 인공지능스피커 등 자유롭게 올려주세요. 질문과답변도 이곳에 올려주세요.

제목micro-ROS : 토픽 및 서비스 만들기2022-02-02 03:46
작성자user icon Level 4

88x31.png


지난번 글에 이어서 이번에는 실제로 토픽과 서비스를 만들어서 esp32-cam 펌웨어를 빌드해보겠습니다.

발행자(publisher)를 통해 핀 상태를 발행(publish)하고, 서비스(service)를 통해 led 상태를 변경하도록 작성합시다.


FreeRTOS의 프로젝트앱은 작업공간의 firmware/freertos_apps/apps 폴더 아래에 생성합니다.

프로젝트 이름은 'led'로 하겠습니다.

 $ cd ~/ros_ws

 $ mkdir firmware/freertos_apps/apps/led


esp32 확장 모듈은 프로젝트의 app 모듈에서 appMain 함수를 실행시킵니다.

헤더에 선언된 형태는 다음과 같습니다.

void appMain(void *argument);


app 모듈을 다음과 같이 작성해주세요.

 $ xed firmware/freertos_apps/apps/led/app.c


#include <stdio.h>

#include <unistd.h>


#include <driver/gpio.h>


#include <rcl/rcl.h>

#include <rcl/error_handling.h>


#include <rclc/rclc.h>

#include <rclc/executor.h>


#include <std_msgs/msg/bool.h>

#include <std_srvs/srv/set_bool.h>


#ifdef ESP_PLATFORM

#include "freertos/FreeRTOS.h"

#include "freertos/task.h"

#endif


#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc);vTaskDelete(NULL);}}

#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}


#define ledPin 4


rcl_publisher_t publisher;

std_msgs__msg__Bool msg;


void timer_callback(rcl_timer_t * timer, int64_t last_call_time)

{

RCLC_UNUSED(last_call_time);

msg.data = gpio_get_level(ledPin);


if (timer != NULL) {

RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));

}

}


void service_callback(const void * req, void * res){

std_srvs__srv__SetBool_Request * req_in = (std_srvs__srv__SetBool_Request *) req;

std_srvs__srv__SetBool_Response * res_in = (std_srvs__srv__SetBool_Response *) res;

gpio_set_level(ledPin, req_in->data);


res_in->success = true;

}


void appMain(void * arg)

{

std_srvs__srv__SetBool_Request req;

std_srvs__srv__SetBool_Response res;


std_msgs__msg__Bool__init(&msg);

std_srvs__srv__SetBool_Request__init(&req);

std_srvs__srv__SetBool_Response__init(&res);


gpio_pad_select_gpio(ledPin);

gpio_set_direction(ledPin, GPIO_MODE_INPUT_OUTPUT);

gpio_set_level(ledPin, 0);


rcl_allocator_t allocator = rcl_get_default_allocator();


// create init_options

rclc_support_t support;

RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));


// create node

rcl_node_t node;

RCCHECK(rclc_node_init_default(&node, "led", "esp32", &support));


// create publisher

RCCHECK(rclc_publisher_init_default(

&publisher,

&node,

ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),

"/led_state"));


// create service

    rcl_service_t service;

    RCCHECK(rclc_service_init_default(&service, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(std_srvs, srv, SetBool), "/set_state"));


// create timer,

rcl_timer_t timer;

const unsigned int timer_timeout = 1000;

RCCHECK(rclc_timer_init_default(

&timer,

&support,

RCL_MS_TO_NS(timer_timeout),

timer_callback));


// create executor

rclc_executor_t executor;


RCCHECK(rclc_executor_init(&executor, &support.context, 2, &allocator));

RCCHECK(rclc_executor_add_timer(&executor, &timer));

RCCHECK(rclc_executor_add_service(&executor, &service, &req, &res, service_callback));


rclc_executor_spin(&executor);


// free resources

RCCHECK(rclc_executor_fini(&executor));

RCCHECK(rcl_publisher_fini(&publisher, &node));

RCCHECK(rcl_timer_fini(&timer));

RCCHECK(rcl_service_fini(&service, &node));

RCCHECK(rcl_node_fini(&node));

RCCHECK(rclc_support_fini(&support));


std_msgs__msg__Bool__fini(&msg);

std_srvs__srv__SetBool_Request__fini(&req);

std_srvs__srv__SetBool_Response__fini(&res);


  vTaskDelete(NULL);

}


의존성 모듈을 include 합니다. 

gpio 모듈은 esp32-cam 보드의 gpio를 통제하는 모듈입니다. rcl 모듈은 ros client library이고, rclc 모듈은 ros client library for c 입니다. 메시지 인터페이스는 std_msgs 모듈의 Bool 타입을 사용하고, 서비스 인터페이스는 std_srvs 모듈의 SetBool 타입을 사용하겠습니다.

#include <stdio.h>

#include <unistd.h>


#include <driver/gpio.h>


#include <rcl/rcl.h>

#include <rcl/error_handling.h>


#include <rclc/rclc.h>

#include <rclc/executor.h>


#include <std_msgs/msg/bool.h>

#include <std_srvs/srv/set_bool.h>


#ifdef ESP_PLATFORM

#include "freertos/FreeRTOS.h"

#include "freertos/task.h"

#endif


RCCHECK 함수와 RCSOFTCHECK 함수를 정의합니다. 둘다 ROS 클라이언트가 잘 생성되었는지 확인하기 위한 함수입니다. ledPin 번호도 설정합니다. ESP32-CAM 보드의 내장 led는 GPIO4 이므로 4를 적습니다. 발행자(publisher)와 메시지 인터페이스는 전역함수로 선언해줍니다.

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc);vTaskDelete(NULL);}}

#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}


#define ledPin 4


rcl_publisher_t publisher;

std_msgs__msg__Bool msg;


타이머의 콜백함수를 정의합니다. 메시지 인터페이스의 data 필드에 led 상태를 설정하여 토픽을 발행하도록 작성합니다. gpio 상태는 gpio_get_level 함수를 통해 확인가능합니다.  토픽은 rcl_publish 함수를 통해 발행합니다. 발행자 객체메시지 인터페이스 객체를 매개변수로 넘겨줍니다.

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)

{

RCLC_UNUSED(last_call_time);

msg.data = gpio_get_level(ledPin);


if (timer != NULL) {

RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));

}

}



서비스 콜백함수를 정의합니다. 요청받은 data 필드에 따라 led 상태를 설정합니다. gpio 상태는 gpio_set_level 함수를 통해 설정 가능합니다.

void service_callback(const void * req, void * res){

std_srvs__srv__SetBool_Request * req_in = (std_srvs__srv__SetBool_Request *) req;

std_srvs__srv__SetBool_Response * res_in = (std_srvs__srv__SetBool_Response *) res;

gpio_set_level(ledPin, req_in->data);


res_in->success = true;

}


appMain 함수를 정의합니다. 요청(request) 객체응답(response) 객체를 선언하고 각각 초기화해주세요. 전역변수로 선언한 메시지 인터페이스 객체도 초기화해주세요.

void appMain(void * arg)

{

std_srvs__srv__SetBool_Request req;

std_srvs__srv__SetBool_Response res;


std_msgs__msg__Bool__init(&msg);

std_srvs__srv__SetBool_Request__init(&req);

std_srvs__srv__SetBool_Response__init(&res);


gpio 포트 모드를 입출력 모드(GPIO_MODE_INPUT_OUTPUT)로 설정합니다.

gpio_pad_select_gpio(ledPin);

gpio_set_direction(ledPin, GPIO_MODE_INPUT_OUTPUT);

gpio_set_level(ledPin, 0);


할당자(alocator) 객체서포트(serport) 객체를 선언하고 초기화 해주세요.

할당자 객체는 동적 메모리를 관리하는 역할을 하며 rcl_get_default_allocator 함수를 통해 초기화합니다. 서포트 객체는 ROS 클라이언트를 서포트하는 객체입니다. 서포트 객체, 환경변수 개수(argc), 환경변수 배열(argv), 할당자 객체를 매개변수로 넘겨주며, rclc_support_init 함수를 통해 초기화합니다.

rcl_allocator_t allocator = rcl_get_default_allocator();


// create init_options

rclc_support_t support;

RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));


노드(node) 객체를 선언하고 rclc_node_init_default 함수를 통해 초기화합니다. 노드 객체, 노드 이름, 네임스페이스, 서포트 객체를 매개변수로 넘겨줍니다. 네임스페이스는 같은 이름의 노드를 각각 구분해주는 역할을 합니다. 기본값으로는 빈 문자열 ""을 선언합니다. 아래 코드처럼 네임스페이스를 작성했다면 노드이름은 "/esp32/led"가 됩니다.

// create node

rcl_node_t node;

RCCHECK(rclc_node_init_default(&node, "led", "esp32", &support));


발행자(publisher) 객체를 rclc_publisher_init_default 함수를 통해 생성합니다. 발행자 객체, 노드 객체, 메시지 타입 객체, 토픽 이름을 매개변수로 넘겨줍니다. 메시지타입 객체는 ROSIDL_GET_MSG_TYPE_SUPPORT 함수를 통해 생성가능하며, 패키지이름, 하위폴더, 메시지 타입을 매개변수로 넘겨줍니다.

// create publisher

RCCHECK(rclc_publisher_init_default(

&publisher,

&node,

ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),

"/led_state"));


서비스 객체를 선언하고 rclc_service_init_default 함수를 통해 초기화합니다. 서비스 객체, 노드 객체, 서비스 타입 객체, 서비스 이름을 매개변수로 넘겨주며, 서비스 타입 객체는 ROSIDL_GET_SRV_TYPE_SUPPORT 함수를 통해 생성가능합니다. 패키지이름하위폴더메시지 타입을 매개변수로 넘겨줍니다.

// create service

    rcl_service_t service;

    RCCHECK(rclc_service_init_default(&service, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(std_srvs, srv, SetBool), "/set_state"));


발행자가 지속적으로 토픽을 발행하기 위해 타이머 객체를 선언하고  rclc_timer_init_default 함수를 통해 초기화합니다. 타이머 객체, 서포트 객체, 시간 간격, 타이머 콜백함수를 매개변수로 넘겨줍니다.

// create timer,

rcl_timer_t timer;

const unsigned int timer_timeout = 1000;

RCCHECK(rclc_timer_init_default(

&timer,

&support,

RCL_MS_TO_NS(timer_timeout),

timer_callback));


실행자(executor) 객체를 선언하고 rclc_executor_init 함수를 통해 초기화합니다. 실행자 객체, 서포트 컨텍스트, 실행자 개수, 할당자 객체를 매개변수로 넘겨줍니다. 실행자 객체를 초기화한 후 실행자에 타이머와 서비스를 추가합니다rclc_executor_spin 함수를 통해 실행자가 지속적으로 실행되도록 설정합니다.

// create executor

rclc_executor_t executor;


RCCHECK(rclc_executor_init(&executor, &support.context, 2, &allocator));

RCCHECK(rclc_executor_add_timer(&executor, &timer));

RCCHECK(rclc_executor_add_service(&executor, &service, &req, &res, service_callback));


rclc_executor_spin(&executor);


실행자가 스핀을 벗어나면 각각의 객체를 종료하고 쓰레드를 삭제하도록 작성합니다.

// free resources

RCCHECK(rclc_executor_fini(&executor));

RCCHECK(rcl_publisher_fini(&publisher, &node));

RCCHECK(rcl_timer_fini(&timer));

RCCHECK(rcl_service_fini(&service, &node));

RCCHECK(rcl_node_fini(&node));

RCCHECK(rclc_support_fini(&support));


std_msgs__msg__Bool__fini(&msg);

std_srvs__srv__SetBool_Request__fini(&req);

std_srvs__srv__SetBool_Response__fini(&res);


   vTaskDelete(NULL);



저장하고 빌드툴에 매개변수를 전달하기 위해 app-colcon.meta 파일을 작성합니다. 노드가 1개, 발행자가 1개, 서비스가 1개이므로 다음과 같이 작성합니다.

 $ xed firmware/freertos_apps/apps/led/app-colcon.meta

{

    "names": {

        "rmw_microxrcedds": {

            "cmake-args": [

                "-DRMW_UXRCE_MAX_NODES=1",

                "-DRMW_UXRCE_MAX_PUBLISHERS=1",

                "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=0",

                "-DRMW_UXRCE_MAX_SERVICES=1",

                "-DRMW_UXRCE_MAX_CLIENTS=0",

                "-DRMW_UXRCE_MAX_HISTORY=4",

            ]

        }

    }

}



오버레이 작업공간 설정환경을 소싱하세요.

 $ . install/setup.bash


펌웨어를 설정합니다.

 $ ros2 run micro_ros_setup configure_firmware.sh led -t udp -i [ip주소] -p 8888


와이파이 정보를 입력합니다.

 $ ros2 run micro_ros_setup build_firmware.sh menuconfig


펌웨어를 빌드합니다.

 $ ros2 run micro_ros_setup build_firmware.sh


ESP32-CAM 보드를 프로그래밍 모드로 배선합니다.

 ESP-CAM

TTL Converter 

5V

 5V

GND

GND 

UOR

TXD

UOT

RXD 

GND - GPIO0

 

mb-file.php?path=2021%2F09%2F27%2FF3774_esp32-cam-ftdi.jpg


펌웨어를 플래싱합니다.

 $ ros2 run micro_ros_setup flash_firmware.sh


에이전트 서버를 실행하세요.

 $ ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888


GPIO0-GND 선을 제거후 보드를 재부팅하세요. 그 후 다음 명령어를 통해 서비스를 요청해보세요. led가 켜집니다.

 $ ros2 run micro_ros_agent mros2 service call /set_state std_srvs/srv/SetBool "{data: 1}"


다음 명령어로 led를 꺼보세요.

 $ ros2 run micro_ros_agent mros2 service call /set_state std_srvs/srv/SetBool "{data: 0}"


mb-file.php?path=2022%2F02%2F02%2FF4580_ezgif.com-gif-maker%20%283%29.gif
 

#ROS2# micro-ROS# XRCE-DDS# 아두이노# esp32-cam# FreeRTOS# ESP-IDF
댓글
자동등록방지
(자동등록방지 숫자를 입력해 주세요)