!REDIRECT “https://docs.px4.io/master/ko/ros/mavros_offboard.html“

MAVROS 보드 외부 제어 예제

이 자습서에서는 가제보/SITL에서 모의시험을 진행할 쿼드콥터로 MAVROS의 보드 외부 제어 기본을 설명하도록 하겠습니다. 자습서의 마지막 부분에서는 아래 동영상에서 나타난 2미터 고도에서의 저속 이륙과 같은 동일한 동작이 나타나야 합니다.

Caution 보드 외부 제어는 위험합니다. 실제 기체를 운용할 때는 어떤 동작에 문제가 있을 경우를 감안하여 수동 조작으로 전환하는 방안을 강구하십시오.

Tip 이 예제는 C++ 언어를 사용합니다. Similar examples in Python can be found here: integrationtests/python_src/px4_it/mavros.

코드

ROS 패키지에 offb_node.cpp 파일을 만들고(컴파일 할 수 있게끔 CMakeList.txt에 파일을 추가), 다음 코드를 파일에 붙여넣으십시오:

  1. /**
  2.  * @file offb_node.cpp
  3.  * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
  4.  * Stack and tested in Gazebo SITL
  5.  */
  7. #include <ros/ros.h>
  8. #include <geometry_msgs/PoseStamped.h>
  9. #include <mavros_msgs/CommandBool.h>
  10. #include <mavros_msgs/SetMode.h>
  11. #include <mavros_msgs/State.h>
  13. mavros_msgs::State current_state;
  14. void state_cb(const mavros_msgs::State::ConstPtr& msg){
  15.     current_state = *msg;
  16. }
  18. int main(int argc, char **argv)
  19. {
  20.     ros::init(argc, argv, "offb_node");
  21.     ros::NodeHandle nh;
  23.     ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
  24.             ("mavros/state", 10, state_cb);
  25.     ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
  26.             ("mavros/setpoint_position/local", 10);
  27.     ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
  28.             ("mavros/cmd/arming");
  29.     ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
  30.             ("mavros/set_mode");
  32.     //the setpoint publishing rate MUST be faster than 2Hz
  33.     ros::Rate rate(20.0);
  35.     // wait for FCU connection
  36.     while(ros::ok() && !current_state.connected){
  37.         ros::spinOnce();
  38.         rate.sleep();
  39.     }
  41.     geometry_msgs::PoseStamped pose;
  42.     pose.pose.position.x = 0;
  43.     pose.pose.position.y = 0;
  44.     pose.pose.position.z = 2;
  46.     //send a few setpoints before starting
  47.     for(int i = 100; ros::ok() && i > 0; --i){
  48.         local_pos_pub.publish(pose);
  49.         ros::spinOnce();
  50.         rate.sleep();
  51.     }
  53.     mavros_msgs::SetMode offb_set_mode;
  54.     offb_set_mode.request.custom_mode = "OFFBOARD";
  56.     mavros_msgs::CommandBool arm_cmd;
  57.     arm_cmd.request.value = true;
  59.     ros::Time last_request = ros::Time::now();
  61.     while(ros::ok()){
  62.         if( current_state.mode != "OFFBOARD" &&
  63.             (ros::Time::now() - last_request > ros::Duration(5.0))){
  64.             if( set_mode_client.call(offb_set_mode) &&
  65.                 offb_set_mode.response.mode_sent){
  66.                 ROS_INFO("Offboard enabled");
  67.             }
  68.             last_request = ros::Time::now();
  69.         } else {
  70.             if( !current_state.armed &&
  71.                 (ros::Time::now() - last_request > ros::Duration(5.0))){
  72.                 if( arming_client.call(arm_cmd) &&
  73.                     arm_cmd.response.success){
  74.                     ROS_INFO("Vehicle armed");
  75.                 }
  76.                 last_request = ros::Time::now();
  77.             }
  78.         }
  80.         local_pos_pub.publish(pose);
  82.         ros::spinOnce();
  83.         rate.sleep();
  84.     }
  86.     return 0;
  87. }

코드 설명

  1. #include <ros/ros.h>
  2. #include <geometry_msgs/PoseStamped.h>
  3. #include <mavros_msgs/CommandBool.h>
  4. #include <mavros_msgs/SetMode.h>
  5. #include <mavros_msgs/State.h>

mavros_msgs 패키지에는 서비스와 MAVROS 패키지에서 제공하는 토픽 운용에 필요한 모든 개별 메세지가 들어있습니다. 모든 서비스와 토픽에 해당하는 메세지 형식은 mavros 위키에 있습니다.

  1. mavros_msgs::State current_state;
  2. void state_cb(const mavros_msgs::State::ConstPtr& msg){
  3.     current_state = *msg;
  4. }

오토파일럿의 현재 상태를 저장하는 간단한 콜백 메서드를 만들었습니다. 이 콜백 메서드로 연결, 이륙 준비 상태, OFFBOARD 플래그를 확인합니다.

  1. ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);
  2. ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);
  3. ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
  4. ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");

명령에 따른 로컬 위치를 내보내는 전송자와, 이륙 준비와 모드 변경을 요청하는 적당한 클라이언트를 초기화합니다. 우리 시스템에서 “mavros” 접두부는 실행 파일에서 노드에 주어진 이름에 따라 달라질 수 있습니다.

  1. //the setpoint publishing rate MUST be faster than 2Hz
  2. ros::Rate rate(20.0);

PX4는 보드 외부에 인가하는 명령 두개 사이에 500ms의 제한 시간을 둡니다. 제한 시간이 지나면, 통제 주체는 보드 외부 통제 모드로 들어가기 전 기체의 최근 상태로 복귀합니다. 이게 바로 가능한 지연 시간을 고려하여 메세지와 명령을 내보내는 속도가 2Hz보다 빨라야 하는 이유입니다. 또한 위치 통제 모드에서 보드 외부 통제 모드로의 진입을 추천하는 동일한 이유이기도 하며, 보드 외부 통제 모드 진입을 기체에서 중단하면 추적을 멈추고 그 자리에서 떠 있습니다.

  1. // wait for FCU connection
  2. while(ros::ok() && !current_state.connected){
  3.     ros::spinOnce();
  4.     rate.sleep();
  5. }

무언가를 전송하기 전 MAVROS와 오토파일럿의 연결을 기다립니다. 이 루프는 하트비트 메세지를 수신한 즉시 빠져나갑니다.

  1. geometry_msgs::PoseStamped pose;
  2. pose.pose.position.x = 0;
  3. pose.pose.position.y = 0;
  4. pose.pose.position.z = 2;

PX4 프로 플라이트 스택이 항공 NED 좌표 영역에서 동작하긴 하지만, MAVROS는 이 좌표를 표준 ENU 프레임 또는 그 반대로 (자유 자재로) 변환합니다. 따라서 z 값을 양의 정수 2로 설정했습니다.

  1. //send a few setpoints before starting
  2. for(int i = 100; ros::ok() && i > 0; --i){
  3.     local_pos_pub.publish(pose);
  4.     ros::spinOnce();
  5.     rate.sleep();
  6. }

보드 외부 제어 모드로 들어가기 전에 실시간 데이터 전송 설정값을 둔 상태로 시작해야합니다. 그렇지 않으면 모드 전환이 불가능합니다. 여기서는 100을 임의의 값으로 선택했습니다.

  1. mavros_msgs::SetMode offb_set_mode;
  2. offb_set_mode.request.custom_mode = "OFFBOARD";

OFFBOARD를 custom_mode 값으로 설정했습니다. 지원 모드 목록을 참고 목적으로 두었습니다.

  1. mavros_msgs::CommandBool arm_cmd;
  2. arm_cmd.request.value = true;
  4. ros::Time last_request = ros::Time::now();
  6. while(ros::ok()){
  7.         if( current_state.mode != "OFFBOARD" &&
  8.                 (ros::Time::now() - last_request > ros::Duration(5.0))){
  9.                 if( set_mode_client.call(offb_set_mode) &&
  10.                         offb_set_mode.response.mode_sent){
  11.                         ROS_INFO("Offboard enabled");
  12.                 }
  13.                 last_request = ros::Time::now();
  14.         } else {
  15.                 if( !current_state.armed &&
  16.                         (ros::Time::now() - last_request > ros::Duration(5.0))){
  17.                         if( arming_client.call(arm_cmd) &&
  18.                                 arm_cmd.response.success){
  19.                                 ROS_INFO("Vehicle armed");
  20.                         }
  21.                         last_request = ros::Time::now();
  22.                 }
  23.         }
  25.         local_pos_pub.publish(pose);
  27.         ros::spinOnce();
  28.         rate.sleep();
  29. }

나머지 코드는 굳이 설명하지 않아도 있는 그대로 동작합니다. 쿼드콥터 비행을 할 수 있도록 이륙 준비가 끝나고 나면, OFFBOARD 모드로 전환을 시도합니다. 5초간의 서비스 호출 여유 시간을 두어 오토 파일럿에 요청이 과도하게 들어가지 않게 합니다. 동일한 루프에서 적당한 속도로 요청한 자세를 계속 보냅니다.

Tip 이 코드는 최소한의 묘사를 목적으로 단순화했습니다. 좀 더 규모가 큰 시스템에서는 설정값을 주기적으로 내보내도록 새 스레드를 만드는게 좋습니다.