MAVROS OFFBOARD 사용 예시 코드

아래 코드는 PX4 Firmware 기준

publish & Subscribe & service 해야할것

        # Publisher
        self.local_pose_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10)
        self.velocity_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=10)
        # Subscriber
        rospy.Subscriber('/mavros/state', State, self.stateCb)
        rospy.Subscriber('/mavros/home_position/home', HomePosition, self.homeCb)
        rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.LocalPositionCb)

        # Service_client
        self.arming_client = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
        self.set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode)

publish 하여 명령 줄것들

/mavros/setpoint_position/local	point - point 명령 줄때, 모드 바꿀때
/mavros/setpoint_velocity/cmd_vel_unstamped	velocity input 으로 명령 줄때

 

Subscribe 하여 받을 것들 

/mavros/state	현재 모드 상태 (ex offboard, position)
/mavros/home_position/home	시작 위치(전원 켠 상태) 위치 값 받기
/mavros/local_position/pose	IMU 센서 EKF 를 통한 Attitude 추정 값

 

Service 하여 명령 줄것들

/mavros/cmd/arming	시동 걸때 사용
/mavros/set_mode	모드 바꿀때 사용

Cliant 사용 예시

 

      - Mode 바꿀때

    def setMode(self, mode):
        rate = rospy.Rate(0.5)
        while True:
            self.PubLocalPosition(self.local_home_position)
            if self.current_state.mode != mode:
                self.set_mode_client(base_mode=0, custom_mode=mode)          
            else:
                break
            rate.sleep()

      - Arming 할때

    def setArm(self):
        rate = rospy.Rate(0.5)
        while True:
            if self.current_state.armed is not True:
                self.arming_client(True)
            else:
                break
            rate.sleep()

 

처음 시작하기 전

- Connection 확인

    def check_FCU_connection(self):
        while not self.current_state.connected:
            rospy.loginfo_throttle(1, "Wait FCU connection")
        rospy.loginfo("FCU connected")

 

 

Callback 예시

 

State Call Back 함수

    def stateCb(self, msg):
        prev_state = self.current_state

        self.current_state = msg
        
        if self.current_state.mode != prev_state.mode:
            rospy.loginfo("Current mode: %s" % self.current_state.mode)

        if self.current_state.armed != prev_state.armed:
            rospy.loginfo("Vehicle armed: %r" % self.current_state.armed)

 

Position Call Back 함수 (내 현재 위치)

    def LocalPositionCb(self, msg):
        self.current_local_position.x = msg.pose.position.x
        self.current_local_position.y = msg.pose.position.y
        self.current_local_position.z = msg.pose.position.z

 

Home Position Call Back 함수

  def homeCb(self, msg):
        self.home_set = True
        self.global_home_position.latitude = msg.geo.latitude
        self.global_home_position.longitude = msg.geo.longitude

 

Point to point publish 함수

    def PubLocalPosition(self, point):
        pose = PoseStamped()
        pose.header.stamp = rospy.Time.now()
        pose.pose.position.x = point.x
        pose.pose.position.y = point.y
        pose.pose.position.z = point.z
        self.local_pose_pub.publish(pose)

 

publisher 예시

    def PubLocalPosition(self, point):
        pose = PoseStamped()

        pose.header.stamp = rospy.Time.now()
        pose.pose.position.x = point.x
        pose.pose.position.y = point.y
        pose.pose.position.z = point.z

        self.local_pose_pub.publish(pose)

 

모드 변환 및 실행 예시

    flight = Mission()
    rate = rospy.Rate(200.0)
    try:
        flight.check_FCU_connection()
        rospy.sleep(1)
        flight.check_home_setting()
        rospy.sleep(1) 
        flight.setArm()
        flight.setMode("OFFBOARD")
        while not rospy.is_shutdown():
            flight.process()
            rate.sleep()

- connection 확인

- home setting (OFFBOARD 모드하려면 필수!!)

- Arming

- 모드 (OFFBOARD) 로 바꾸기

- 미션 실행