마커 위치를 받아 집는 기능을 추가하는 중에 arm_control_app의 코드를 정리했다. main.cpp 한 파일에 MoveIt 설정, 이동, 그리퍼 조작, 물체를 집고 놓는 작업이 모두 들어 있었고, 오류가 발생하였을 때의 정리 코드가 곳곳에 흩어져 있었다. 또 팔이 움직이는 중에 Ctrl+C를 누르면 프로그램이 종료하지 못하고 멈추는 현상이 반복해서 발생하였다.
코드 정리와 예외 처리는 커밋 c8b8f25, Ctrl+C 처리는 커밋 61a317e에 있다.
코드 정리
팔을 움직이는 기능과 관련된 부분을 ArmController 클래스로 옮겼다. rclcpp::Node를 상속하고 InitMoveIt, MoveToNamedTarget, MoveToPose, OperateGripper, Stop을 가진다. 물체를 집고 옮겨 놓는 과정을 main.cpp의 ExecutePickAndPlaceCycle 함수에 모아서 코드가 간결해 질 수 있도록 하였다. 팔을 어떻게 움직이는지는 클래스가, 어떤 순서로 무엇을 하는지는 main.cpp로 나눠지면서 코드를 유지 보수하기 훨씬 좋아졌다.
예외와 실패 처리
정리 전의 main은 노드를 스핀하는 스레드를 직접 만들고, 오류가 날 때마다 finish()라는 함수를 불러 빠져나갔다.
static inline int finish(int retcode, std::thread & spinner)
{
rclcpp::shutdown();
spinner.join();
return retcode;
}
...
return finish(-1, spinner);return 하는 곳마다 finish()를 잊지 않고 불러야 했다. 또 MoveGroupInterface 생성처럼 예외를 던질 수 있는 곳에서 예외가 나면 finish()를 거치지 않고 main을 빠져나간다. 이때 join하지 않은 std::thread가 소멸되는 상황이 발생하는데, C++ 표준상 join할 수 있는 상태의 std::thread가 소멸되면 std::terminate가 불려 프로그램이 비정상 종료된다(cppreference: std::thread::~thread).
그래서 정리 작업을 객체의 소멸자로 옮겼다. 스핀 스레드는 ExecutorThread 클래스로 감싸서, 소멸자가 executor_를 멈추고(cancel) 스레드를 기다리게(join) 하였다. main이 정상으로 끝나든, 중간에 return하든, 예외로 빠져나가든 지역 객체의 소멸자는 항상 호출된다.
~ExecutorThread()
{
try {
executor_.cancel();
if (thread_.joinable()) {
thread_.join();
}
} catch (...) {
// 소멸자에서 예외가 빠져나가면 std::terminate 가 불린다
}
}rclcpp::shutdown()도 같은 방식으로 처리했다. 빈 포인터를 가진 shared_ptr를 만들어서 해당 인스턴스가 소멸될 때 원래 가리키던 메모리를 해제하는 작업 대신 shutdown()을 호출하도록 람다 함수를 삭제자(deleter)로 지정하였다. 이렇게 함으로써 main이 어떤 경로로 끝나든 이 객체가 소멸될 때 shutdown()이 호출되도록 하였다.
std::shared_ptr<void> rclcpp_guard(nullptr, [](void*) { rclcpp::shutdown(); });main의 나머지는 try-catch로 감싸서 예외를 받아 로그를 남기도록 하였다. InitMoveIt 함수에서 MoveGroupInterface 생성, 엔드 이펙터 링크 설정, 그리퍼 액션 클라이언트 생성, 바닥 충돌 객체 추가를 차례로 하면서, 한 단계라도 실패하면 로그를 남기고 false를 반환한다.
Ctrl+C를 입력하였을 때 프로그램의 멈춤
팔이 움직이는 도중에 Ctrl+C를 누르면 프로그램이 끝나지 않고 멈추는 현상이 반복되었다. 이동 명령인 move()가 실행 중일 때 Ctrl+C를 입력하면, 함수가 결과를 반환하지 않고 계속 실행 상태로 남아 있었다.
Ctrl+C는 SIGINT를 발생시키고 rclcpp는 기본적으로 이 signal을 처리하는 함수를 rclcpp::init()에서 등록한다. 시스템에 설치된 헤더 rclcpp/init_options.hpp의 설명이 다음과 같다.
/// If true, the context will be shutdown on SIGINT by the signal handler (if it was installed).
bool shutdown_on_signal = true;rclcpp의 기본 signal 핸들러는 SIGINT를 받으면 컨텍스트, 즉 rclcpp::init()이 생성한 ROS 통신 환경을 종료한다. 스핀은 이 환경이 살아 있는 동안(rclcpp::ok()가 참인 동안)만 콜백을 처리하므로 같이 멈춘다. arm_control_app 노드의 스핀 스레드도, MoveGroupInterface가 내부에 따로 두는 스레드도 모두 같이 멈춘다. 그리퍼 액션의 결과는 노드의 스핀 스레드가, move()의 결과는 MoveGroupInterface의 스레드가 처리해야 하는데 둘 다 멈췄다. 특히 move()는 결과가 올 때까지 아래 코드와 같이 기다린다.(MoveIt 2.12.4 move_group_interface.cpp).
// wait until send_goal_opts.result_callback is called
while (!done)
{
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}이 루프는 result_callback이 done 변수값을 true로 바꿔 줄 때만 끝난다. 그런데 result_callback을 호출해 줄 스핀이 멈췄으므로 done 변수의 값은 바뀌지 않고, move()는 반환되지 않는다.
MoveIt의 API 문서에서 asyncMove() 함수를 찾았지만, 이 함수는 동작이 끝날 때까지 기다리지 않고 바로 반환할 뿐 동작이 끝났는지 알려 주는 방법이 없어서 실행 결과를 확인하고 순서대로 진행하는 현재 방식에는 쓸 수 없었다.
Signal을 직접 처리
rclcpp가 signal을 받아도 컨텍스트를 중지시키지 않도록 옵션을 변경하고(shutdown_on_signal = false), signal을 내가 만든 코드에서 직접 처리하도록 하였다. 일반적인 signal 처리 규칙대로 signal 처리 함수에서는 플래그만 설정하도록 하였다.
rclcpp::InitOptions init_options;
init_options.shutdown_on_signal = false;
rclcpp::init(argc, argv, init_options);
std::signal(SIGINT, signal_handler);
std::signal(SIGTERM, signal_handler);std::atomic<bool> g_quit{false};
void signal_handler(int signum) {
if(SIGINT == signum || SIGTERM == signum)
{
g_quit = true;
}
}100ms 주기 타이머가 signal handler에서 설정하는 플래그(g_quit)를 검사하고 ArmController::Stop()을 호출한다. Stop()은 클래스 안의 취소 플래그를 설정하고 move_group_->stop()을 호출하여 진행 중인 이동을 멈춘다. 컨텍스트가 살아 있어 스핀이 계속 돌고 있으므로 move()는 중단 결과를 받고 반환된다.
그리퍼는 결과를 get()으로 한 번에 기다리지 않고, wait_for(100ms)를 반복하면서 취소 플래그를 모니터링한다. 플래그가 true로 설정되면 async_cancel_goal로 액션을 취소하고, 서버가 취소를 끝낼 때까지 한 번 더 기다린 뒤 실패를 돌려준다.
while (rclcpp::ok() && !quit_flag_) {
if (std::future_status::ready ==
result_future.wait_for(std::chrono::milliseconds(100))) {
break;
}
}ExecutePickAndPlaceCycle은 단계 사이마다 g_quit를 확인해서 signal을 수신한 경우라면 다음 동작으로 진행하지 않고 함수를 빠져나오도록 하였다. 작업이 중단되면 main은 팔을 시작 자세(test_configuration)로 돌려놓고 끝난다. 그런 다음 ExecutorThread와 rclcpp_guard의 소멸자가 스레드와 컨텍스트를 정리한다.
