지난번 글에 이어서 이번에는 실제로 토픽과 서비스를 만들어서 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", ] } } } |
오버레이 작업공간 설정환경을 소싱하세요.
펌웨어를 설정합니다.
$ 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 | |
펌웨어를 플래싱합니다. $ 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}" |
|
스팟 
쎄라토 
