아래 코드는 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 |
안녕하세요 ros와 px4로 자율비행 드론을 만들기위해 공부하다가 방문하게 되었습니다.
혹시 offboard control 소스코드를 공유 해주실수 있으신가요?
공유해주신다면 공부하는데 큰 도움이 될것같습니다!
감사합니다.
제가 컨트롤 부분은 예시에 적어 놓은게 전부 입니다. 혹시 어떤게 더 필요하신가요?
전체소스코드를 받아서 구조를 파악할수있으면 도움이 될것 같습니다!
제가 적은건 마브로스만 사용하는 코드가 아니라서 공개하기에는 민감한 사항도 있어서 전체 공개는 힘든것 같습니다. 이해 부탁 드립니다 추가적으로 문의사항이 있으면 알려주세요
아 넵 공부하다가 모르는거 있으면 질문 드리겠습니다 ㅎㅎ
비밀댓글입니다
failsafety 가 뜬다면 센서나 다른 설정이 안되어 있는 경우가 대부분 입니다. 에러 메시지를 참고 하시기 바랍니다.
혹시 어떤 환경에서 진행하시는건가요?