연결기는 이 실습실에서 아래 5곳을 씁니다. 내 PC의 ROS 2 쪽에서는 퍼블리셔 · 구독자 · 서비스 · 액션 · 파라미터 클라이언트 노릇을, 사이트 쪽에서는 HTTP로 묻고 올리는 노릇을 합니다.
-
1 허용 목록 52번째 줄부터
드론 토픽 /drone/pose · /drone/battery · /drone/command가 이 표에 있어야 연결기가 다룹니다.
# ── 판 4 허용 목록 (바로잡기 명세 1-2절) ── 서버(app/link/state.py)의 목록과 같아야 한다. 목록 밖은 연결기가 거절한다.
INTERFACES = "robom_interfaces"
SERVICES = {"/shuttle/arrival": "robom_interfaces/srv/ShuttleEta"} # 셔틀 정류장
ACTIONS = {"/cafe/brew": "robom_interfaces/action/Brew"} # 바리스타 로봇
PUBLISH_TOPICS = { # 사이트가 보내 달라고 할 수 있는 토픽
"/crossroad/signal": "std_msgs/msg/String",
"/farm/moisture": "std_msgs/msg/Float64",
}
REPEAT_TOPICS = ("/crossroad/signal",) # 다음 값이 올 때까지 같은 값을 다시 보낸다(교차로가 신호를 계속 방송하는 장면)
REPEAT_EVERY = 0.5
WATCH_TOPICS = { # 켜질 때 모두 구독해 두고, 받은 것을 1초에 한 번 모아 올린다
"/shuttle/position": "std_msgs/msg/Float64",
"/crossroad/cmd_vel": "geometry_msgs/msg/Twist",
"/energy/robot_status": "std_msgs/msg/Header",
"/farm/water": "std_msgs/msg/Bool",
}
PARAM_NODES = ("/robom_farm",) # 파라미터를 읽고 바꿀 수 있는 노드
DRONE_TOPICS = { # 드론(3차부터 그대로)
"/drone/pose": "geometry_msgs/msg/PoseStamped",
"/drone/battery": "sensor_msgs/msg/BatteryState",
"/drone/command": "std_msgs/msg/String",
}
-
2 올리기 줄기 247번째 줄부터
넣어 둔 마지막 위치 · 배터리를 1초에 5번 HTTP로 서버에 올리고, 처음 닿으면 「연결됨」을 찍습니다.
def send_loop(self) -> None:
connected, tries = False, 0
while not self.stop.is_set():
payload = self._take()
if payload is not None:
try:
self.server.send(payload)
if not connected:
say(f"연결됨 · {self.server.base}")
connected = True
tries = 0
except KeyExpired:
self._expired()
return
except ServerSaid as error:
say(error.args[0])
self.stop.wait(1.0 if error.status == 429 else 5.0)
continue
except (urllib.error.URLError, OSError):
connected = False
tries = self._retry(tries, "보내기")
continue
self.stop.wait(1.0 / SEND_PER_SECOND)
-
3 명령 받기 273번째 줄부터
HTTP로 기다렸다가 서버가 보낸 「출발 · 멈춤」을 받아 드론 쪽으로 넘깁니다.
def command_loop(self) -> None:
tries = 0
while not self.stop.is_set():
try:
commands = self.server.wait_commands()
tries = 0
except KeyExpired:
self._expired()
return
except ServerSaid as error:
say(error.args[0])
self.stop.wait(1.0)
continue
except (urllib.error.URLError, OSError):
tries = self._retry(tries, "받기")
continue
for line in commands:
if isinstance(line, str): # 판 1 모양(서버가 판 머리를 못 본 때)
line = {"kind": "drone", "command": line}
if not isinstance(line, dict):
continue
kind = line.get("kind")
if kind == "drone" and line.get("command") in ("start", "stop"):
say(f"명령 받음: {line['command']}")
self.on_command(line["command"])
elif kind in REQUEST_KINDS and isinstance(line.get("id"), str):
what = line.get("name") or line.get("topic") or ""
say(f"요청 받음: {kind} {what}".rstrip())
self.on_request(line)
-
4 드론 토픽 677번째 줄부터
내 노드의 /drone/pose · /drone/battery를 구독하고, 받은 명령을 /drone/command로 내보낼 퍼블리셔를 만듭니다.
# 드론(3차부터 그대로). 받은 frame_id는 비어 있어도 그대로 올린다.
string = self._need("std_msgs/msg/String")
node.create_subscription(self._need(DRONE_TOPICS["/drone/pose"]), self.prefix + "/drone/pose", self.on_pose, 10)
node.create_subscription(self._need(DRONE_TOPICS["/drone/battery"]), self.prefix + "/drone/battery", self.on_battery, 10)
command = node.create_publisher(string, self.prefix + "/drone/command", 10)
link.on_command = lambda text: command.publish(string(data=text))
-
5 위치 올리기 731번째 줄부터
받은 위치(frame_id는 고치지 않고 64자까지)와 배터리를 마지막 값 하나로 넣어 둡니다.
def on_pose(self, message) -> None:
p, q = message.pose.position, message.pose.orientation
self.link.update(
pose={
"frame_id": str(message.header.frame_id)[:FRAME_MAX],
"position": {"x": p.x, "y": p.y, "z": p.z},
"orientation": {"x": q.x, "y": q.y, "z": q.z, "w": q.w},
}
)
def on_battery(self, message) -> None:
if math.isfinite(message.percentage):
battery = {"percentage": min(1.0, max(0.0, message.percentage))}
if math.isfinite(message.voltage) and message.voltage >= 0:
battery["voltage"] = message.voltage
self.link.update(battery=battery)
파일 전체입니다(표지 주석 줄만 뺐습니다). 그대로 두는 뼈대는 어두운 바탕, 내가 채우거나 고칠 줄(「# 할 일」 주석과 그 자리의 코드)은 노란 바탕이고, 「# 할 일」 옆에 그 과제 이름이 있습니다.
1#!/usr/bin/env python32"""로봄 연습 드론 노드 — 하늘 배달 드론. 뼈대는 되어 있고, 「# 할 일」을 채워 완성하는 노드다.34쓰는 법 (Ubuntu 24.04 + ROS 2 Jazzy):5 source /opt/ros/jazzy/setup.bash6 python3 robom_drone_sim.py7 # 다른 터미널에서 연결기를 켠다: python3 robom_link.py --key <연결 열쇠>89이 드론 노드가 하는 일 (퍼블리셔 둘 · 구독자 하나):10- /drone/pose(geometry_msgs/msg/PoseStamped)와 /drone/battery(sensor_msgs/msg/BatteryState)를 초당 5번 보낸다. (완성됨)11- 위치의 header.frame_id는 지도 좌표의 이름이어야 한다. 지금은 odom이다. (「# 할 일」)12- /drone/command(std_msgs/msg/String)를 받는다. start면 창고(12, 48)와 도착지(88, 14) 사이 배달 길을 오가며 날고(끝에 닿으면13 돌아 나온다), stop이면 그 자리 그 높이에서 떠 있는다. 지금은 받은 글자를 찍기만 한다. (「# 할 일」)14- --ns <이름>: 토픽 앞에 이름 공간을 붙인다(/<이름>/drone/pose). 연결기의 --ns와 같은 이름을 쓴다.1516지도 좌표는 ROS 2 map 좌표처럼 x는 오른쪽, y는 위로 커진다(가로 100 · 세로 60). 사이트 화면이 뒤집어 그린다.17그대로 켜면 과제 1(연결하기)은 되지만, 과제 2 · 3은 「무엇이 안 됐는지」 한 줄로 떨어진다.18파이썬 표준 라이브러리와 ROS 2에 들어 있는 rclpy만 쓴다(따로 설치할 것이 없다).19"""2021import argparse22import re23import sys2425VERSION = "2"26RATE = 5 # 초당 보내기 횟수27DEPOT, DROPOFF = (12.0, 48.0), (88.0, 14.0) # 창고 · 도착지(map 좌표)28SPEED = 4.0 # 지도 단위/초29ALTITUDE = 10.0
지도 좌표 이름 내가 채움 과제 2 · 지도에 나타나기
위치에 붙일 좌표계 이름으로, 지도 좌표로 보이려면 무엇이어야 할지 정하는 자리입니다.
31FRAME_ID = "odom" # 할 일: 하늘 배달 지도의 좌표 이름으로 바꾸세요(과제 2)과제 2 · 지도에 나타나기과제 2 · 지도에 나타나기
33DRAIN_FLYING = 0.001 # 날 때 초당 배터리 감소(0~1): 다 차 있으면 16분 남짓 난다34DRAIN_HOVER = 0.0 # 서 있거나 떠 있을 때는 닳지 않는다(수업 내내 켜 두어도 날 수 있게)35FULL_VOLTAGE = 12.6363738def say(text: str) -> None:39 print(text, flush=True)404142class SimDrone:43 """ROS 2와 상관없는 드론 상태. 처음에는 창고 바닥에 서 있다."""4445 def __init__(self):46 self.x, self.y, self.z = DEPOT[0], DEPOT[1], 0.047 self.flying = False # 배달 길을 날고 있는가48 self.goal = DROPOFF # 지금 날아가는 끝(닿으면 다른 끝으로 바꿔 돌아 나온다)49 self.battery = 1.050
명령 알아듣기 내가 채움 과제 3 · 명령이 닿기
/drone/command로 받은 글자를 보고 드론 상태를 바꾸는 자리입니다.
52 def command(self, command: str) -> bool:53 """/drone/command로 받은 글자. 알아들은 명령이면 True."""54 if command not in ("start", "stop"):55 return False56 # 할 일: start면 날기 시작하고(self.flying), stop이면 그 자리 그 높이에서 떠 있게 하세요과제 3 · 명령이 닿기과제 3 · 명령이 닿기57 return True
나는 걸음 내가 채움 과제 3 · 명령이 닿기
타이머마다 불려, 날고 있을 때 드론을 어디로 얼마나 옮길지 정하는 자리입니다.
61 def step(self, dt: float) -> None:62 """타이머마다 dt초만큼 상태를 고친다."""63 if self.flying and self.battery > 0:64 pass # 할 일: 날고 있으면 높이를 맞추고, 지금 자리에서 가는 끝(self.goal) 쪽으로 이번 주기에 갈 만큼 옮기세요. 끝에 닿으면 다른 끝으로 바꿔 돌아 나옵니다과제 3 · 명령이 닿기과제 3 · 명령이 닿기65 drain = DRAIN_FLYING if self.flying else DRAIN_HOVER66 self.battery = max(0.0, self.battery - drain * dt)
6869 def position(self) -> tuple[float, float, float]:70 return self.x, self.y, self.z717273def run(namespace: str) -> int:74 # rclpy는 여기서만 불러온다.75 import rclpy76 from geometry_msgs.msg import PoseStamped77 from rclpy.node import Node78 from sensor_msgs.msg import BatteryState79 from std_msgs.msg import String8081 prefix = f"/{namespace}/drone" if namespace else "/drone"82 rclpy.init()83 node = Node("robom_drone_sim")84 drone = SimDrone()
퍼블리셔 · 구독자 만들기 뼈대
위치 · 배터리를 보낼 퍼블리셔와 명령을 받을 구독자를 만듭니다.
86 pose_pub = node.create_publisher(PoseStamped, prefix + "/pose", 10)87 battery_pub = node.create_publisher(BatteryState, prefix + "/battery", 10)8889 def on_command(message) -> None: # 구독자 콜백: 연결기가 사이트의 「출발 · 멈춤」을 이 토픽으로 넘긴다90 if drone.command(message.data):91 say(f"명령 받음: {message.data}")9293 node.create_subscription(String, prefix + "/command", on_command, 10)
위치 · 배터리 보내기 뼈대
1초에 5번 위치(PoseStamped)와 배터리(BatteryState)를 채워 보냅니다.
97 def tick() -> None:98 drone.step(1.0 / RATE)99 x, y, z = drone.position()100 pose = PoseStamped()101 pose.header.stamp = node.get_clock().now().to_msg()102 pose.header.frame_id = FRAME_ID103 pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = float(x), float(y), float(z)104 pose.pose.orientation.w = 1.0105 pose_pub.publish(pose)106 battery = BatteryState()107 battery.header.stamp = pose.header.stamp108 battery.percentage = float(drone.battery)109 battery.voltage = float(FULL_VOLTAGE * (0.8 + 0.2 * drone.battery))110 battery.present = True111 battery_pub.publish(battery)112113 node.create_timer(1.0 / RATE, tick)
115 say(f"로봄 연습 드론 노드 판 {VERSION} · 토픽 {prefix}/pose · {prefix}/battery 보냄 · {prefix}/command 받음")116 say(f"지금은 창고({DEPOT[0]:.0f}, {DEPOT[1]:.0f})에 서 있습니다. 「# 할 일」을 채우면 「출발」에 배달 길을 오가며 납니다")
노드 돌리기 뼈대
노드를 돌려 타이머와 구독 콜백이 불리게 하고, 끌 때 정리합니다.
118 try:119 rclpy.spin(node)120 except KeyboardInterrupt:121 say("연습 드론 노드를 끕니다")122 finally:123 node.destroy_node()124 if rclpy.ok():125 rclpy.shutdown()
127 return 0128129130def main(argv=None) -> int:131 parser = argparse.ArgumentParser(prog="robom_drone_sim.py", description="로봄 연습 드론 노드 판 " + VERSION)132 parser.add_argument("--ns", default="", help="토픽 이름 공간(예: drone1 → /drone1/drone/pose)")133 args = parser.parse_args(argv)134 if args.ns and not re.fullmatch(r"[A-Za-z][A-Za-z0-9_]{0,31}", args.ns):135 say("--ns는 영문자로 시작하는 32자 안의 영문 · 숫자 · _만 됩니다")136 return 2137 try:138 return run(args.ns)139 except ImportError:140 say("ROS 2(rclpy)를 찾지 못했습니다. source /opt/ros/jazzy/setup.bash 를 먼저 하세요")141 return 2142143144if __name__ == "__main__":145 sys.exit(main())