본문 바로가기

pixhawk

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) 로 바꾸기

- 미션 실행

'pixhawk' 카테고리의 다른 글

MAVROS OFFBOARD 사용 예시 코드  (7) 2020.07.07
PIXHAWK SIMULATION  (39) 2020.05.20
pixhawk 사용법  (0) 2020.03.20
  • 김민우 2021.06.10 14:27

    안녕하세요 ros와 px4로 자율비행 드론을 만들기위해 공부하다가 방문하게 되었습니다.
    혹시 offboard control 소스코드를 공유 해주실수 있으신가요?
    공유해주신다면 공부하는데 큰 도움이 될것같습니다!
    감사합니다.

  • BlogIcon rladntjd 2021.06.11 18:50 신고

    제가 컨트롤 부분은 예시에 적어 놓은게 전부 입니다. 혹시 어떤게 더 필요하신가요?

    • BlogIcon 김민우 2021.06.12 19:23

      전체소스코드를 받아서 구조를 파악할수있으면 도움이 될것 같습니다!

    • BlogIcon rladntjd 2021.06.12 19:25 신고

      제가 적은건 마브로스만 사용하는 코드가 아니라서 공개하기에는 민감한 사항도 있어서 전체 공개는 힘든것 같습니다. 이해 부탁 드립니다 추가적으로 문의사항이 있으면 알려주세요

    • BlogIcon 김민우 2021.06.12 19:48

      아 넵 공부하다가 모르는거 있으면 질문 드리겠습니다 ㅎㅎ

  • 익명 2021.11.21 16:52

    비밀댓글입니다

    • BlogIcon rladntjd 2021.11.21 16:56 신고

      failsafety 가 뜬다면 센서나 다른 설정이 안되어 있는 경우가 대부분 입니다. 에러 메시지를 참고 하시기 바랍니다.
      혹시 어떤 환경에서 진행하시는건가요?