C++로 로봇팔 제어용 노드 코드 작성

지금까지는 RViz의 Motion Planning 패널에서 임의의 위치로 목적지를 설정한 후 Plan/Execute를 실행하여 로봇이 움직이는지 확인하였다. 이번엔 RViz 없이, 내가 직접 C++로 코드를 작성하여 MoveIt에게 목표를 주고 로봇이 움직이도록 하는 노드를 만들고자 한다.

1. 패키지 생성

cd ~/robot-arm-study/src
ros2 pkg create arm_control_app --build-type ament_cmake --dependencies rclcpp moveit_ros_planning_interface

arm_description/arm_bringup과 달리 이 패키지는 실행 노드가 목적이므로 --build-type ament_cmake에 add_executable로 실행 파일을 직접 빌드하도록 구성한다.

• rclcpp: ROS 2 C++ 노드 작성을 위한 기본 클라이언트 라이브러리
• moveit_ros_planning_interface: MoveGroupInterface 등 MoveIt을 코드에서 쓰기 위한 ROS2 패키지.

2. MoveGroupInterface로 목표 지정

src/arm_control_app/src/main.cpp

#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.hpp>
#include <geometry_msgs/msg/pose.hpp>

#include <memory>
#include <thread>

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  // NodeOptions에 automatically_declare_parameters_from_overrides(true) 설정 안 하면
  // MoveIt 파라미터를 못 읽어서 초기화 실패
  auto const node = std::make_shared<rclcpp::Node>(
    "arm_control_app",
    rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
  );

  auto const logger = rclcpp::get_logger("arm_control_app");

  // MoveGroupInterface 생성자가 내부적으로 서비스 응답을 기다리는데,
  // 그 응답을 처리해줄 스핀이 안 돌고 있으면 여기서 영원히 멈춘다.
  // 그래서 spin을 별도 스레드로 먼저 띄워 둔다.
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node);
  auto spinner = std::thread([&executor]() { executor.spin(); });

  // 두 번째 인자 "ur_manipulator"는 SRDF에서 정의한 planning group 이름과
  // 정확히 같아야 한다.
  moveit::planning_interface::MoveGroupInterface move_group_interface(node, "ur_manipulator");

  // SRDF의 group_state 중 "home"으로 이동
  move_group_interface.setNamedTarget("home");
  auto ok = static_cast<bool>(move_group_interface.move());
  if (!ok) {
    RCLCPP_ERROR(logger, "Move to 'home' failed");
  }

  if (true == ok) {
    // 이번엔 좌표를 직접 지정
    geometry_msgs::msg::Pose target_pose;
    target_pose.position.x = 0.4;
    target_pose.position.y = 0.1;
    target_pose.position.z = 0.4;
    target_pose.orientation.w = 1.0;
    move_group_interface.setPoseTarget(target_pose);
    ok = static_cast<bool>(move_group_interface.move());
  }

  if (!ok) {
    RCLCPP_ERROR(logger, "Move to position failed");
  }

  rclcpp::shutdown();
  spinner.join();
  return 0;
}

코드 작성 시 주의해야 할 핵심 포인트는 다음과 같다.

• automatically_declare_parameters_from_overrides(true) — 이 부분을 설정하지 않으면 MoveIt이 필요로 하는 파라미터(로봇 모델, 플래닝 파이프라인 등)를 노드가 못 읽어서 MoveGroupInterface 생성이 실패한다.
• spin을 별도 스레드로 먼저 시작 — MoveGroupInterface의 생성 과정에서 발생하는 내부 서비스 요청/응답을 처리하기 위해 비동기 스레드를 만들어서 spin()을 호출하도록 한다. Asio의 run() 또는 libuv의 uv_run()을 생각하면 된다.
• "ur_manipulator" — SRDF에서 정의한 planning group 이름이다. 정확하게 입력하지 않으면 MoveGroupInterface 생성 단계에서 바로 실패한다.
• setNamedTarget("home") vs setPoseTarget(...) — SRDF에 미리 정의해 둔 named target(관절 각도 조합)으로 갈 수도 있고, 작업 공간 좌표(position + orientation)로 직접 갈 수도 있다. move()는 계획과 실행을 한 번에 한다 — 계획만 하고 싶으면 plan()을 쓴다.

3. CMakeLists.txt

find_package(rclcpp REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)

add_executable(arm_control_app src/main.cpp)
ament_target_dependencies(arm_control_app
  rclcpp
  moveit_ros_planning_interface
  geometry_msgs
)

install(TARGETS
  arm_control_app
  DESTINATION lib/${PROJECT_NAME}/
)

4. 빌드 및 실행

colcon build --packages-select arm_control_app --symlink-install

source ~/robot-arm-study/install/setup.bash

Gazebo + ros2_control, MoveIt + RViz가 이미 떠 있는 상태에서, 세 번째 터미널로 이 노드를 실행한다.

터미널 1: Gazebo + ros2_control

vglrun ros2 launch arm_bringup arm_study_bringup.launch.py

터미널 2: MoveIt + RViz

vglrun ros2 launch arm_bringup move_group.launch.py

터미널 3: arm_control_app

vglrun ros2 run arm_control_app arm_control_app

RViz를 보지 않아도, Gazebo 속 팔이 코드에서 지정한 대로 home 자세로 갔다가 지정한 좌표로 움직이면 성공이다.