이 튜토리얼에서는 Gazebo Classic 에서 시뮬레이션된 Iris 쿼드콥터를 사용하여 MAVROS Python을 사용한 OFFBOARD 제어 의 기본 사항을 보여줍니다 . 차량 제어 프로그램 개발을 시작하고 시뮬레이션에서 코드를 실행하는 방법을 보여주는 단계별 지침을 제공합니다.
튜토리얼이 끝나면 아래 비디오와 동일한 동작, 즉 고도 2미터까지 천천히 이륙하는 모습을 볼 수 있습니다.
경고
오프보드 제어는 위험합니다. 실제 차량을 운전하는 경우 문제가 발생할 경우를 대비해 수동 제어를 다시 얻을 수 있는 방법이 있는지 확인하십시오.
ROS 패키지 생성
터미널을 열고 ~/catkin_ws/src디렉토리 로 이동
roscd # Should cd into ~/catkin_ws/devel
cd ..
cd src
~/catkin_ws/src디렉터리에서 종속성이 있는 (이 경우)offboard_pyrospy라는 이름의 새 패키지를 만듭니다 .
catkin_create_pkg offboard_py rospy
~/catkin_ws/디렉터리에 새 패키지를 빌드합니다.
cd .. # Assuming previous directory to be ~/catkin_ws/src
catkin build
source devel/setup.bash
이제 다음을 사용하여 패키지로 이동 할 수 있습니다.
roscd offboard_py
Python 파일을 저장하려면 패키지에 /scripts라는 새 폴더를 만듭니다 .
mkdir scripts
cd scripts
Offboard 제어
ROS 패키지와 스크립트 폴더를 생성하면 Python 스크립트를 시작할 준비가 된 것입니다. scripts 폴더 내에서 offb_node.py파일을 생성하고 실행 권한을 부여합니다.
touch offb_node.py
chmod +x offb_node.py
그런 다음 offb_node.py파일을 열고 다음 코드를 붙여넣습니다.
#저작권: 쿼드(QUAD) 드론연구소 https://smartstore.naver.com/maponarooo
#! /usr/bin/env python3
import rospy
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import State
from mavros_msgs.srv import CommandBool, CommandBoolRequest, SetMode, SetModeRequest
current_state = State()
def state_cb(msg):
global current_state
current_state = msg
if __name__ == "__main__":
rospy.init_node("offb_node_py")
state_sub = rospy.Subscriber("mavros/state", State, callback = state_cb)
local_pos_pub = rospy.Publisher("mavros/setpoint_position/local", PoseStamped, queue_size=10)
rospy.wait_for_service("/mavros/cmd/arming")
arming_client = rospy.ServiceProxy("mavros/cmd/arming", CommandBool)
rospy.wait_for_service("/mavros/set_mode")
set_mode_client = rospy.ServiceProxy("mavros/set_mode", SetMode)
# Setpoint publishing MUST be faster than 2Hz
rate = rospy.Rate(20)
# Wait for Flight Controller connection
while(not rospy.is_shutdown() and not current_state.connected):
rate.sleep()
pose = PoseStamped()
pose.pose.position.x = 0
pose.pose.position.y = 0
pose.pose.position.z = 2
# Send a few setpoints before starting
for i in range(100):
if(rospy.is_shutdown()):
break
local_pos_pub.publish(pose)
rate.sleep()
offb_set_mode = SetModeRequest()
offb_set_mode.custom_mode = 'OFFBOARD'
arm_cmd = CommandBoolRequest()
arm_cmd.value = True
last_req = rospy.Time.now()
while(not rospy.is_shutdown()):
if(current_state.mode != "OFFBOARD" and (rospy.Time.now() - last_req) > rospy.Duration(5.0)):
if(set_mode_client.call(offb_set_mode).mode_sent == True):
rospy.loginfo("OFFBOARD enabled")
last_req = rospy.Time.now()
else:
if(not current_state.armed and (rospy.Time.now() - last_req) > rospy.Duration(5.0)):
if(arming_client.call(arm_cmd).success == True):
rospy.loginfo("Vehicle armed")
last_req = rospy.Time.now()
local_pos_pub.publish(pose)
rate.sleep()
코드 설명
import rospy
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import State
from mavros_msgs.srv import CommandBool, CommandBoolRequest, SetMode, SetModeRequest
자동 조종 장치의 현재 상태를 저장하는 간단한 콜백을 만듭니다. 이를 통해 연결, 무장 및 OFFBOARD 플래그를 확인할 수 있습니다 .:
current_state = State()
def state_cb(msg):
global current_state
current_state = msg
명령된 로컬 위치를 게시하기 위해 게시자를 인스턴스화하고 무장 및 모드 변경을 요청하는 적절한 클라이언트를 인스턴스화합니다. 자신의 시스템에서는 "mavros" 접두사가 실행 파일의 노드에 지정된 이름에 따라 다를 수 있다는 점에 유의하세요.
PX4는 두 개의 OFFBOARD 명령 사이에 500ms의 시간 제한이 있습니다. 이 시간이 초과되면 commander는 OFFBOARD 모드 로 들어가기 전 차량이 있었던 마지막 모드로 돌아갑니다. 이것이 가능한 지연 시간을 고려하기 위해 게시 속도가 2Hz보다 빨라야 하는 이유 입니다. 이는 POSITION 모드 에서 OFFBOARD 모드로 들어가는 것이 권장되는 이유와도 같습니다. 차량이 OFFBOARD 모드에서 벗어나면 트랙에서 멈추고 호버링하게 됩니다.
여기서는 게시 속도를 적절하게 설정합니다.
# Setpoint publishing MUST be faster than 2Hz
rate = rospy.Rate(20)
무엇이든 게시하기 전에 MAVROS와 자동 조종 장치 간에 연결이 설정될 때까지 기다립니다. 이 루프는 하트비트 메시지가 수신되는 즉시 종료되어야 합니다.
# Wait for Flight Controller connection
while(not rospy.is_shutdown() and not current_state.connected):
rate.sleep()
PX4가 항공우주 NED 좌표 프레임에서 작동하더라도 MAVROS는 이러한 좌표를 표준 ENU 프레임으로 변환하거나 그 반대로 변환합니다. 이것이 우리가 z양수 2로 설정한 이유입니다.
코드의 나머지 부분은 대체로 자명합니다. 오프보드 모드 로 전환을 시도한 후 쿼드가 비행할 수 있도록 준비합니다. 요청으로 인해 자동 조종 장치 버퍼가 넘치지 않도록 서비스 호출 간격을 5초로 늘립니다. 동일한 루프에서 이전에 정의된 속도로 요청된 포즈를 계속 전송합니다.
이 코드는 설명을 위해 최소한으로 단순화되었습니다. 대규모 시스템에서는 주기적으로 설정점 게시를 담당할 새 스레드를 생성하는 것이 유용한 경우가 많습니다.
ROS launch 파일 생성
offboard_py패키지에서 이름이 지정된python ~/catkin_ws/src/offboard_py디렉터리 내에 launch 폴더를 만듭니다. 여기에 패키지의 시작 파일이 저장됩니다. 그런 다음 첫 번째 실행 파일 start_offb.launch을 만듭니다.
roscd offboard_py
mkdir launch
cd launch
touch start_offb.launch
start_offb.launch에 다음 코드를 사용하세요.
<?xml version="1.0"?>
<launch>
<!-- Include the MAVROS node with SITL and Gazebo -->
<include file="$(find px4)/launch/mavros_posix_sitl.launch">
</include>
<!-- Our node to control the drone -->
<node pkg="offboard_py" type="offb_node.py" name="offb_node_py" required="true" output="screen" />
</launch>
보시다시피 mavros_posix_sitl.launch파일이 포함되어 있습니다. 이 파일은 MAVROS, PX4 SITL, Gazebo Classic 환경을 시작하고 주어진 세계에서 차량을 생성하는 역할을 담당합니다.
팁
파일 mavros_posix_sitl.launch은 생성할 차량이나 Gazebo Classic 세계와 같은 기본 설정에 따라 설정할 수 있는 여러 인수를 사용합니다.
태그 내에서 선언하여 에 정의된 이러한 인수의 기본값을 재정의할 수 있습니다 . 예를 들어, 세계를 창고warehouse.world로 변경하려면 아래와 같이 바꿉니다.
<!-- Include the MAVROS node with SITL and Gazebo -->
<include file="$(find px4)/launch/mavros_posix_sitl.launch">
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/warehouse.world"/>
</include>