지난 포스트

Gazebo, Ros, Yolov5를 활용한 군집 드론추적 - 2(실패) | Notion

→ mavsdk 비동기 제어로 추적 알고리즘을 구현하려다가 실패하였다.

그래서 mavros오프보드 제어로 추적알고리즘을 재설계하였고 결국 성공했다.

Gazebo, Ros, Yolov5를 활용한 군집드론추적 - 1

지난 포스트에서 Yolo와 goturn 모델로 상대 드론을 탐지하는 단계까지 구현했다.

이번 포스트에서는 드론을 px4 autopilot 오프보드 제어 예제코드 를 활용하여 추적알고리즘을 구현 할 수 있도록 파이썬으로 추적 알고리즘을 설계할 것이다.

PX4 autopilot mavros example(python)

https://docs.px4.io/v1.12/ko/flight_modes/mission.html

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()

해당 코드를 간략히 설명하자면

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)

roslaunch mavros px4.launch와 같은 mavros로 px4를 런치 후

ROS노드를 활용하여 오프보드 제어 명령을 발행하면, px4에서 해당 노드를 구독하여 드론을 파이썬으로

제어하도록 할 수 있다.

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