MAVROS 오프보드 제어 예제(C++)
저작권: 쿼드(QUAD) 드론연구소 https://smartstore.naver.com/maponarooo
이 튜토리얼 에서는 Gazebo Classic/SITL에서 시뮬레이션된 Iris 쿼드콥터를 사용하여 MAVROS를 사용한 오프보드 제어 의 기본 사항을 보여줍니다 . 튜토리얼이 끝나면 아래 비디오와 동일한 동작, 즉 고도 2미터까지 천천히 이륙하는 모습을 볼 수 있습니다.
경고
오프보드 제어는 위험합니다. 실제 차량을 운전하는 경우 문제가 발생할 경우를 대비해 수동 제어를 다시 얻을 수 있는 방법이 있는지 확인하십시오.
이 예제에서는 C++를 사용합니다. 매우 유사한 Python 예제는 ROS/MAVROS Offboard 예제(Python) 에서 찾을 수 있습니다. ( integrationtests/python_src/px4_it/mavros 의 예제도 참조하세요.) (새 창을 엽니다)).
Offboard 제어
ROS 패키지에 offb_node.cpp파일을 생성하고 안에 다음을 붙여넣습니다.
/**
* @file offb_node.cpp
* @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
* Stack and tested in Gazebo Classic SITL
*/
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
current_state = *msg;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "offb_node");
ros::NodeHandle nh;
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
("mavros/state", 10, state_cb);
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
("mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
("mavros/set_mode");
//the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);
// wait for FCU connection
while(ros::ok() && !current_state.connected){
ros::spinOnce();
rate.sleep();
}
geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 2;
//send a few setpoints before starting
for(int i = 100; ros::ok() && i > 0; --i){
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
}
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
ros::Time last_request = ros::Time::now();
while(ros::ok()){
if( current_state.mode != "OFFBOARD" &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( set_mode_client.call(offb_set_mode) &&
offb_set_mode.response.mode_sent){
ROS_INFO("Offboard enabled");
}
last_request = ros::Time::now();
} else {
if( !current_state.armed &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( arming_client.call(arm_cmd) &&
arm_cmd.response.success){
ROS_INFO("Vehicle armed");
}
last_request = ros::Time::now();
}
}
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
}
return 0;
}
코드 설명
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>패키지 mavros_msgs에는 MAVROS 패키지에서 제공하는 서비스 및 토픽을 작동하는 데 필요한 모든 사용자 정의 메시지가 포함되어 있습니다. 모든 서비스와 주제 및 해당 메시지 유형은 mavros wiki 에 문서화되어 있습니다.
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
current_state = *msg;
}자동 조종 장치의 현재 상태를 저장하는 간단한 콜백을 만듭니다. 이를 통해 연결, 무장 및 오프보드 플래그를 확인할 수 있습니다 .
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");명령된 로컬 위치를 게시하기 위해 게시자를 인스턴스화하고 무장 및 모드 변경을 요청하는 적절한 클라이언트를 인스턴스화합니다. 자신의 시스템에서는 "mavros" 접두사가 실행 파일의 노드에 지정된 이름에 따라 다를 수 있다는 점에 유의하세요.
//the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);PX4에는 두 개의 오프보드 명령 사이에 500ms의 시간 초과가 있습니다 . 이 시간 초과가 초과되면 commander는 오프보드 모드로 들어가기 전 마지막 모드로 돌아갑니다 . 이것이 가능한 지연 시간을 고려하기 위해 게시 속도가 2Hz보다 빨라야 하는 이유 입니다. 위치(Position) 모드로 들어가는 것이 권장 됩니다 . 이렇게 하면 차량이 오프보드 모드 에서 벗어나면 트랙에서 멈추고 호버링하게 됩니다.
// wait for FCU connection
while(ros::ok() && !current_state.connected){
ros::spinOnce();
rate.sleep();
}무엇이든 게시하기 전에 MAVROS와 자동 조종 장치 간에 연결이 설정될 때까지 기다립니다. 이 루프는 하트비트 메시지가 수신되는 즉시 종료되어야 합니다.
geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 2;PX4 Pro Flight Stack이 NED 좌표 프레임에서 작동하더라도 MAVROS는 이 좌표를 표준 ENU 프레임으로 변환하거나 그 반대로 변환합니다. 이것이 우리가 z양수 2로 설정한 이유입니다.
//send a few setpoints before starting
for(int i = 100; ros::ok() && i > 0; --i){
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
}오프보드 모드 로 들어가기 전에 이미 설정점(EKF_Origin 초기화) 스트리밍을 해야 합니다. 그렇지 않으면 모드 스위치가 거부됩니다. 여기서는 임의의 수 100으로 선택되었습니다.
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";사용자 정의 모드를 로 설정했습니다 OFFBOARD. 지원되는 모드 목록 (새 창을 엽니다)참고하실 수 있습니다.
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
ros::Time last_request = ros::Time::now();
while(ros::ok()){
if( current_state.mode != "OFFBOARD" &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent) {
ROS_INFO("Offboard enabled");
}
last_request = ros::Time::now();
} else {
if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0))) {
if( arming_client.call(arm_cmd) && arm_cmd.response.success) {
ROS_INFO("Vehicle armed");
}
last_request = ros::Time::now();
}
}
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
}나머지 코드는 설명이 필요하지 않습니다. 오프보드 모드 로 전환을 시도한 후 비행할 수 있도록 준비합니다. 요청으로 인해 자동 조종 장치가 넘치지 않도록 서비스 호출 간격을 5초로 늘립니다. 동일한 루프에서 요청된 포즈를 적절한 속도로 계속 전송합니다.
이 코드는 설명을 위해 최소한으로 단순화되었습니다. 대규모 시스템에서는 주기적으로 설정점 게시를 담당할 새 스레드를 생성하는 것이 유용한 경우가 많습니다.
Last updated