앞서 마커 인식에서 얻은 좌표는 카메라의 입장에서 바라본 위치이다. 이를 다시 로봇 팔의 좌표계로 변환해 주어야 로봇이 보드를 파지할 수 있다.
코드가 계속 수정되기 때문에 글 내용과 코드를 맞출 수 있도록 커밋을 정리하였다.
자세 추정 , 좌표 변환, TF 발행 , 회전 반영 ,로그 간격 , 공용 헤더 패키지
자세 추정
마커의 꼭짓점 픽셀 좌표로 카메라 기준 3차원 위치와 회전을 구하는 함수가 cv::aruco::estimatePoseSingleMarkers다.
cv::aruco::estimatePoseSingleMarkers(corners, MARKER_LENGTH, camera_matrix_,
dist_coeffs_, rvecs, tvecs);corners:detectMarkers가 찾은 꼭짓점이다.MARKER_LENGTH: 마커 한 변의 실제 길이다.camera_matrix_,dist_coeffs_:camera_info의 K와 D다. 영상과camera_info를 함께 받도록 구독을image_transport::create_camera_subscription으로 바꾸고, 첫 프레임에서 한 번만 꺼내 둔다.tvecs,rvecs: 좌표 변환의 결과이다. tvec는 마커 중심의 위치(X, Y, Z)이고, rvec는 마커의 회전이다. 둘 다 광학 좌표계 기준이다.
좌표계 변환
로봇팔은 base_link 기준 좌표로 움직이므로, 광학 좌표계에서 얻은 위치를 base_link 기준으로 다시 표현해야 한다.
먼저 위치를 PoseStamped 메시지에 넣는다. PoseStamped는 위치와 회전을 함께 포함하고, header.frame_id에 그 값이 어느 프레임 기준인지를 적는다. header에는 영상 메시지의 헤더를 그대로 넣어, 이 값이 overhead_camera_link_optical 기준으로 영상이 찍힌 시각의 값임을 알려준다. pose.position에는 tvec를 넣는다.
일단 회전을 고려하지 않고 위치만 고려하여 진행 한 뒤에 회전 추가하는 것이 쉬울 것 같아서 회전 부분에는 단위 쿼터니언을 넣고 시작하였다.
다음으로 두 프레임 사이의 변환을 TF에서 얻는다.
cam_to_base = tf_buffer_->lookupTransform(
"base_link", "overhead_camera_link_optical",
image_msg->header.stamp, rclcpp::Duration::from_seconds(0.1));lookupTransform은 TransformListener가 /tf, /tf_static을 구독해 버퍼에 쌓아 둔 변환들에서 두 프레임 사이의 변환을 계산해 돌려준다. 두 프레임이 직접 이어져 있지 않으면 TF 트리를 따라 변환들을 이어 붙인다. 지금 진행하는 시뮬레이션에서는 base_link → overhead_camera_link → overhead_camera_link_optical이다.
첫번째 인자는 데이터를 옮겨 갈 프레임(base_link), 두번째 인자는 데이터의 원본 프레임(overhead_camera_link_optical), 세번째는 원하는 시각, 네번째는 변환이 버퍼에 없을 때 기다릴 시간이다. 지정된 시간 내에 프레임을 찾지 못하면 예외를 던진다.
조회 시각은 영상에 포함된 타임 스탬프로 설정하였다.
lookupTransform이 돌려주는 cam_to_base는 TransformStamped 메시지로, 광학 좌표계를 base_link로 옮기는 이동(카메라 위치)과 회전(쿼터니언)을 담고 있다. 이 변환은 카메라와 base_link 사이의 것이라 영상 속 마커가 몇 개든 같다. 그래서 영상 한 장마다 한 번 조회해 두고, 그 영상에서 찾은 모든 마커에 같은 변환을 쓴다.
마지막으로 tf2::doTransform(입력, 출력, 변환)으로 좌표를 변환한다. 입력 자세에 변환을 곱해, 위치는 회전시킨 뒤 이동하고 회전은 변환의 회전과 합친다. 계산 결과의 frame_id는 base_link가 된다. 위치와 회전까지 함께 변환하기 위해 PoseStamped를 사용했다.
TF 프레임으로 발행
base_link 기준으로 옮긴 마커 위치는 arm_control_app이 잡을 대상체의 위치로 사용한다. 이 위치를 marker_<marker id> 형식으로 TF 프레임에 발행하면 arm_control_app은 lookupTransform("base_link", "marker_0", …)으로 base_link 기준 위치으로 마커의 위치를 얻을 수 있어서 목표 지점을 설정할 수 있다.
TF 프레임은 TransformBroadcaster의 sendTransform 함수로 발행하는데, 이 함수의 인자는 TransformStamped 메시지 형식이다. 그런데 doTransform의 결과는 PoseStamped라서 그대로 인자로 넘길 수 없고, TransformStamped 형식에 맞춰서 데이터를 옮겨 주어야 한다.
두 메시지의 구조를 살펴보면 아래와 같다.
PoseStamped
header.frame_id
pose.position
pose.orientation
TransformStamped
header.frame_id 부모 프레임, base_link
child_frame_id 대상 프레임, marker_0
transform.translation
transform.rotationPoseStamped는 base_link 기준으로 위치와 자세만 알려준다. TF는 이름 붙은 프레임의 트리라서 마커 위치를 프레임에 붙이려면 새 프레임의 이름이 필요하다. 그 자리가 child_frame_id이며 마커 id를 붙인 이름인 marker_0으로 설정했다. 검출된 마커마다 프레임을 만들어 position은 translation에, orientation은 rotation에 옮기고 모든 마커에 대한 처리가 끝난 뒤 한 번에 발행하도록 구성하였다.
회전에 대한 처리 추가
카메라 영상으로 마커의 위치를 로봇에 전달하는 것은 확인하였다. 이제 앞서 미뤄두었던 보드의 회전에 대한 처리를 추가한다.
estimatePoseSingleMarkers는 위치(tvec)와 함께 회전(rvec)도 계산해 준다. rvec는 벡터의 방향이 회전축이고 길이가 회전각(라디안)인 3차원 벡터다. 그런데 회전을 담을 PoseStamped의 pose.orientation은 쿼터니언(x, y, z, w)이라 rvec를 그대로 넣을 수 없다. 그래서 OpenCV의 cv::Quatd::createFromRvec(rvec)로 rvec를 쿼터니언으로 변환하였다.
rvec가 나타내는 회전은 마커 좌표계 기준이다. 마커 좌표계의 축 방향은 estimatePoseSingleMarkers가 계산에 쓰는 마커 꼭짓점 좌표로 결정된다. OpenCV 4.6의 기본값(CCW_center)에서 네 꼭짓점의 좌표는 (−L/2, L/2), (L/2, L/2), (L/2, −L/2), (−L/2, −L/2)이다(aruco.cpp). 그래서 마커를 정면에서 바라볼 때 x축은 마커 그림의 오른쪽, y축은 위쪽을 향한다. 오른손 좌표계이므로 z축은 x축에서 y축으로 감아 도는 방향의 오른손 엄지 방향, 즉 마커 면에서 바라보는 사람 쪽으로 나오는 방향이다. 작업대 위에 놓인 마커에서는 위쪽 즉, 카메라를 향하는 방향이다.
createFromRvec가 돌려주는 값은 OpenCV의 쿼터니언 타입인 cv::Quatd다. cv::Quat의 멤버는 w, x, y, z 순이고 geometry_msgs::msg::Quaternion은 x, y, z, w
순이다. 그래서 memcpy 같은 방식으로 복사하면 문제가 발생한다. 필드 이름으로 하나씩 대입하는 것으로 진행하였다.
테스트를 위해서 보드를 z 축 기준으로 회전시켜 보드의 방향이 변화하는지 확인해 볼 수 있다. 아래는 45도 회전시키는 가제보 명령이다.
gz service -s /world/default/set_pose --reqtype gz.msgs.Pose --reptype gz.msgs.Boolean \
--timeout 300 --req 'name: "board", position: {x: 0.4, y: 0.6, z: 0.0025}, orientation: {x: 0, y: 0, z: 0.3827, w: 0.9239}'순환문에서의 로그 출력
검출 좌표를 매 프레임마다 출력하면 너무 많은 메시지가 출력되어 로그를 살펴보기 힘들다. RCLCPP_INFO_THROTTLE로 출력을 해보려고 했지만 이것도 마지막 출력 시간을 기준으로 시간 차이를 사용하는 형태라서 반복문에서 동일한 매크로를 연속적으로 사용하면 시간 간격 설정에 따라 첫 출력 이후의 메시지를 출력하지 못할 수 있다. 매크로 내용을 일부 발췌하면 아래와 같다.
static rcutils_time_point_value_t __rcutils_logging_last_logged = 0;
...
if (RCUTILS_LIKELY(__rcutils_logging_condition)) {
__rcutils_logging_last_logged = __rcutils_logging_now;
rcutils_log_internal(&__rcutils_logging_location, severity, name, __VA_ARGS__);
}마지막 로그 시각을 저장하는 변수가 static 이기 때문에 매크로를 연이어 호출하면 처음 호출한 로그 이후에는 한동안 로그가 출력되지 않는다. 로그 목적별로 출력 시간을 개별적으로 관리할 수 있도록 하기 위해서 LogThrottle 클래스를 만들어 인스턴스를 여러 개 생성하도록 하였다.
LogThrottle disp_marker_throttle{LOG_SHOW_INTERVAL};
LogThrottle disp_transform_throttle{LOG_SHOW_INTERVAL};이 클래스를 다른 패키지에서도 쓰기 위해 헤더 전용 패키지인 arm_common을 생성하여 거기에 코드를 작성하였다.
빌드할 소스 파일 없이 헤더만 있는 패키지라서, arm_common은 CMake의 INTERFACE 라이브러리로 선언해 헤더 경로만 다른 타겟에 알려 주도록 했다.
헤더 경로는 $<BUILD_INTERFACE:…>와 $<INSTALL_INTERFACE:…>로 나눠 적는데 이는 패키지를 빌드할 때에는 헤더가 현재 작업 중인 디렉토리(src/arm_common/include)에 있고, 설치한 뒤에는 install 디렉토리(install/arm_common/include)로 복사되기 때문이다.
log_throttle.hpp가 rclcpp를 쓰므로 target_link_libraries로 헤더를 쓸 때 rclcpp가 함께 링크되도록 했다.
add_library(arm_common INTERFACE)
target_include_directories(arm_common INTERFACE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_link_libraries(arm_common INTERFACE rclcpp::rclcpp)다른 패키지가 이 헤더를 쓰려면 두 가지가 필요하다. 하나는 빌드할 때 헤더 파일을 install 디렉토리에 복사해 두는 것이다.
install(DIRECTORY include/ DESTINATION include)다른 하나는 다른 패키지가 이 패키지를 find_package(arm_common)으로 찾았을 때 헤더 경로를 알 수 있도록 정보를 내보내는 것이다.
install(TARGETS arm_common EXPORT arm_commonTargets)
ament_export_targets(arm_commonTargets HAS_LIBRARY_TARGET)
ament_export_include_directories(include)
ament_export_dependencies(rclcpp)그러면 이 헤더 패키지를 사용하려는 패키지는 find_package(arm_common)과 ament_target_dependencies에 arm_common을 넣는 것만으로 헤더를 쓸 수 있다.
위 내용은 ROS 2 jazzy 문서에서 확인할 수 있다. (https://docs.ros.org/en/jazzy/How-To-Guides/Ament-CMake-Documentation.html#installing)
CMake 설정 전체는 커밋 0ba54dc에 있다.
결과
$ ros2 run tf2_ros tf2_echo base_link marker_0
- Translation: [0.398, 0.603, -0.017]시뮬레이션에서 보드의 위치가 (0.4, 0.6)이니 x와 y가 3mm의 오차 범위 내에 있다. z는 「ArUco 마커 인식」 글에서처럼 오차가 너무 커서 사용하지 않고 파지할 때에는 이미 알고 있는 고정값을 사용한다.
