코드 정리, 예외 처리와 Ctrl+C 처리

About this offer

마커 위치를 받아 집는 기능을 추가하는 중에 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 의 소멸자가 스레드와 컨텍스트를 정리한다.