**** 이 글에는 잘못된 정보가 존재할 수도 있습니다. 꼭 읽어보시고 추가로 조사하시면서 오류가 있다면 수정해주세요! ****
ros는 패키지 단위로 작동함. 그 패키지 안에 launch 파일 등등을 비롯한 파일들이 존재함.
ros의 가장 작은 단위로 node 를 사용함. 아래는 waypoint_controller.py(이하 waypoint)를 가져온 것이며 이 파일안에는 하나의 노드만 존재(WaypointNode)
-아래 계속됨
#!/usr/bin/env python
#이 파일을 적기 위해 필요한 선행코드들
import os
import rclpy
import csv
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy
from mavros_msgs.srv import CommandBool
from geometry_msgs.msg import PoseStamped, Vector3, Quaternion
from std_msgs.msg import Bool,Float32
from tf_transformations import quaternion_from_euler
import time
import math
import numpy as np
NAV_PERIOD_SEC = 0.1
#노드 정의 - WaypointNode 라고 정의
class WaypointNode(Node):
def __init__(self):
super().__init__('WaypointNode')
qos_profile = QoSProfile(
reliability=QoSReliabilityPolicy.BEST_EFFORT,
durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
history=QoSHistoryPolicy.KEEP_LAST,
depth=10
)
qos_mavros = QoSProfile(
reliability=QoSReliabilityPolicy.BEST_EFFORT,
durability=QoSDurabilityPolicy.VOLATILE,
history=QoSHistoryPolicy.KEEP_LAST,
depth=10
)
# Subscriptions
self.position_subscriber = self.create_subscription(
PoseStamped,
'/mavros/local_position/pose',
self.position_callback,
qos_mavros
)
# Publishers
self.arm_publisher = self.create_publisher(
Bool,
'/arm_message',
qos_profile
)
self.disarm_publisher = self.create_publisher(
Bool,
'/disarm_message',
qos_profile
)
self.offboard_position_publisher = self.create_publisher(
PoseStamped,
'/mavros/setpoint_position/local',
qos_profile
)
self.current_position = {'x': 0.0, 'y': 0.0, 'z': 0.0} # Current position of the drone
self.target_waypoints = [
{'x': 0.0, 'y': 0.0, 'z': 10.0},
{'x': 30.0, 'y': 0.0, 'z': 10.0},
{'x': 30.0, 'y': 30.0, 'z': 10.0},
{'x': 0.0, 'y': 30.0, 'z': 10.0},
{'x': 0.0, 'y': 0.0, 'z': 10.0},
{'x': 0.0, 'y': 0.0, 'z': 0.0},
]
self.curr_way_index = 0 # Current index in waypoint list
self.max_waypoint = len(self.target_waypoints) # Total number of waypoints
self.target_waypoint = self.target_waypoints[0] # Current target waypoint
time.sleep(5)
self.get_logger().info("Main launched")
self.arm_drone(True)
self.navigate_waypoints()
def navigate_waypoints(self):
self.wp_timer = self.create_timer(NAV_PERIOD_SEC, self.navigate_waypoint_callback)
def position_callback(self, msg):
self.current_position['x'] = msg.pose.position.x
self.current_position['y'] = msg.pose.position.y
self.current_position['z'] = msg.pose.position.z
self.current_orientation = msg.pose.orientation
def navigate_waypoint_callback(self):
self.offboard_position_publisher.publish(self.calculate_position_command(self.target_waypoint))
if self.waypoint_reached(self.target_waypoint):
self.get_logger().info(f"Waypoint {self.curr_way_index + 1}/{self.max_waypoint} reached")
self.curr_way_index += 1
if self.curr_way_index >= self.max_waypoint:
self.get_logger().info("✅ All waypoints reached. Landing and disarming drone.")
self.disarm_drone()
self.destroy_timer(self.wp_timer)
else:
self.target_waypoint = self.target_waypoints[self.curr_way_index]
self.get_logger().info(f"Moving to next waypoint: "
f"({int(self.target_waypoint['x'])}, {int(self.target_waypoint['y'])}, {int(self.target_waypoint['z'])})")
def calculate_position_command(self, target):
pose = PoseStamped()
pose.header.stamp = self.get_clock().now().to_msg()
pose.pose.position.x = target['x']
pose.pose.position.y = target['y']
pose.pose.position.z = target['z']
return pose
def euler_to_quaternion(self, roll, pitch, yaw):
"""
Convert an Euler angle to a quaternion.
roll, pitch, yaw: in radians
"""
qx, qy, qz, qw = quaternion_from_euler(roll, pitch, yaw)
quaternion = Quaternion()
quaternion.x = qx
quaternion.y = qy
quaternion.z = qz
quaternion.w = qw
return quaternion
def waypoint_reached(self, target, tolerance = 1.0):
x_dist = abs(target['x'] - self.current_position['x'])
y_dist = abs(target['y'] - self.current_position['y'])
z_dist = abs(target['z'] - self.current_position['z'])
return (x_dist < tolerance and
y_dist < tolerance and
z_dist < tolerance)
def arm_drone(self, arm):
arm_msg = Bool()
arm_msg.data = arm
self.arm_publisher.publish(arm_msg)
self.get_logger().info("Drone armed")
def disarm_drone(self):
msg = Bool()
msg.data = True
self.disarm_publisher.publish(msg)
self.get_logger().info("Drone disarmed")
def main(args=None):
rclpy.init(args=args)
waypoint_node = WaypointNode()
rclpy.spin(waypoint_node)
print("node destroyed")
waypoint_node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
ros는 노드와 노드끼리 topic을 주고받음. 한 노드에서 topic을 받을수도(subscribe), 줄수도(publish) 있음
waypoint_controller.py는 /mavros/local_position/pose 이라는 토픽 하나를 받으며(subscribe하며)
/arm_message, /disarm_message, /mavros/setpoint_position/local 이라는 토픽 3개를 줌(publish함)
이 토픽을 누가 받느냐 하면, flight_state_controller.py(이하 flightstate)가 subscribe함. 아래는 flight_state_controller의 코드중 일부
# Subscriptions
self.status_sub = self.create_subscription(
State,
'/mavros/state',
self.vehicle_status_callback,
cmd_vel_qos_profile
) ## current state
self.arm_sub = self.create_subscription(
Bool,
'/arm_message',
self.arm_message_callback,
qos_profile
) ## arming message from waypoint_controller.py
self.disarm_sub = self.create_subscription(
Bool,
'/disarm_message',
self.disarm_message_callback,
qos_profile
) # disarm message from waypoint_controller.py
self.vtol_state = self.create_subscription(
ExtendedState,
'/mavros/extended_state',
self.vtol_msg_callback,
qos_profile
) ## vtol state
self.vtol_publisher_fw = self.create_subscription(
Bool,
'/vtol_message_fw',
self.vtol_msg_callback_fw,
qos_profile
) ## vtol transition(fw)
self.vtol_publisher_mc = self.create_subscription(
Bool,
'/vtol_message_mc',
self.vtol_msg_callback_mc,
qos_profile
) ## vtol transition(mc)
잘 읽어보면 waypoint가 publish한 토픽중 flightstate가 subscribe한 것은 2개밖에 없음(/arm_message랑 /disarm_message)
그럼 나머지 4개의 토픽은 누가 publish하고 waypoint의 남은 토픽 하나는 누가 subscribe하는 것일까?
답은 기체(드론 또는 vtol 등)가 보내는 것. 기체와 우리 컴퓨터는 mavros로 통신을 하는데, 이때 mavros가 자체적으로 publish하고 subscribe하는 토픽이 존재한다. 이런 경우 토픽 앞에 /mavros가 붙는다(아래 참조). waypoint가 보내는 /mavros/setpoint_position/local 도 mavros 자체 토픽에 존재하며, flightstate가 받는 /mavros/state와 /mavros/extended_state도 mavros의 토픽인 것이다.
(이렇게 기체와 직접적으로 통신하는 프로토콜이 바뀌면 토픽도 바뀐다. mavros가 아닌 micreXRCE로 통신하면 토픽이 /mavros/state 가 /fmu/out/vehicle_status로 바뀐다.)
이제 flightstate가 받아오는 6개의 토픽중 4개(/mavros/state, /mavros/extended_state, /arm_message, /disarm_message)의 정체를 알아냈다. 그렇다면 마지막으로 남은 2개(/vtol_message_fw와 /vtol_message_mc)는 어디서 받아오는 것일까?