웨이포인트 따라가기 + GUI
import math
import tkinter as tk
from tkinter import ttk
import requests
import threading
import time
# ================= 설정 및 파라미터 =================
URL = "http://192.168.0.243:5555/control"
INTERVAL = 100
WP_TOL = 5.0
ANGLE_TOL = 0.08
SCALE = 3.0
class RobotController(ttk.Frame):
def __init__(self, master):
super().__init__(master)
# 데이터 초기화
self.mode = "IDLE"
self.rx, self.ry, self.ra = 0.0, 0.0, 0.0 # 현재 실제 좌표 (원본)
self.start_pose = None # 기준점 (Home)
self.waypoints = []
self.trail = []
self.lidar_data = []
self.setup_ui()
# 제어 쓰레드 및 루프 시작
threading.Thread(target=self.main_control_logic, daemon=True).start()
self.update_loop()
def setup_ui(self):
"""버튼 및 좌표 표시 UI 구성"""
self.side_panel = tk.Frame(self, width=250, bg="#f8f9fa", padx=15, pady=15)
self.side_panel.pack(side='left', fill='y')
# 1. 좌표 표시 영역 (실제 좌표 vs 로봇 기준 좌표)
tk.Label(self.side_panel, text="[ 좌표 ]", font=("맑은 고딕", 10, "bold")).pack(anchor='w')
self.lbl_real_pos = tk.Label(self.side_panel, text="/control X: 0.0, Y: 0.0", fg="#666666")
self.lbl_real_pos.pack(anchor='w', pady=(5, 0))
self.lbl_robot_pos = tk.Label(self.side_panel, text="Robot X: 0.0, Y: 0.0", font=("맑은 고딕", 10, "bold"),
fg="#007bff")
self.lbl_robot_pos.pack(anchor='w', pady=(0, 10))
self.lbl_status = tk.Label(self.side_panel, text="상태: IDLE", font=("맑은 고딕", 10))
self.lbl_status.pack(anchor='w', pady=5)
tk.Frame(self.side_panel, height=2, bd=1, relief="sunken", bg="#dee2e6").pack(fill="x", pady=15)
# 2. 제어 버튼들
btn_opts = {'width': 22, 'pady': 6, 'font': ("맑은 고딕", 9)}
tk.Button(self.side_panel, text="포인트 따라가기", command=self.start_waypoints, **btn_opts).pack(
pady=3)
tk.Button(self.side_panel, text="최단 경로 설정", command=self.optimize_path, **btn_opts).pack(
pady=3)
# 복귀 설정
tk.Label(self.side_panel, text="주차 방향 선택", bg="#f8f9fa", font=("맑은 고딕", 8)).pack(pady=(10, 0))
self.park_var = tk.StringVar(value="FRONT")
p_frame = tk.Frame(self.side_panel, bg="#f8f9fa")
p_frame.pack()
tk.Radiobutton(p_frame, text="전면", variable=self.park_var, value="FRONT", bg="#f8f9fa").pack(side='left')
tk.Radiobutton(p_frame, text="후면", variable=self.park_var, value="BACK", bg="#f8f9fa").pack(side='left')
tk.Button(self.side_panel, text="복귀하기", command=self.start_return, **btn_opts).pack(pady=3)
tk.Frame(self.side_panel, height=1, bg="#dee2e6").pack(fill="x", pady=10)
# 3. 초기화 버튼 그룹
tk.Button(self.side_panel, text="경로 초기화", command=self.clear_canvas_only, **btn_opts).pack(
pady=3)
tk.Button(self.side_panel, text="🛑정지", command=self.stop_robot, fg='#b71c1c',
font=("맑은 고딕", 9, "bold"), width=22, pady=10).pack(pady=15)
# 캔버스 영역
self.canvas = tk.Canvas(self, width=600, height=600, bg='#121212', highlightthickness=0)
self.canvas.pack(side='right', expand=True, fill='both')
self.canvas.bind('<Button-1>', self.add_waypoint_click)
# ================= 주행 로직 (보존) =================
def main_control_logic(self):
while True:
if self.mode == "WAYPOINT":
self.step_waypoint()
elif self.mode == "RETURNING":
self.step_return()
time.sleep(0.05)
def step_waypoint(self):
if not self.waypoints:
self.mode = "IDLE"
self.send_cmd(0, 0)
return
tx, ty = self.waypoints[0]
dist = math.hypot(tx - self.rx, ty - self.ry)
if dist < WP_TOL:
self.waypoints.pop(0)
return
self.move_to_target(tx, ty)
def step_return(self):
if not self.start_pose: return
sx, sy, sa = self.start_pose
dist = math.hypot(sx - self.rx, sy - self.ry)
if dist < WP_TOL:
target_a = sa if self.park_var.get() == "FRONT" else (sa + math.pi + math.pi) % (2 * math.pi) - math.pi
error_a = (target_a - self.ra + math.pi) % (2 * math.pi) - math.pi
if abs(error_a) < ANGLE_TOL:
self.send_cmd(0, 0)
self.mode = "IDLE"
else:
self.send_cmd(0, 0.3 if error_a > 0 else -0.3)
else:
self.move_to_target(sx, sy)
def move_to_target(self, tx, ty):
t_a = math.atan2(ty - self.ry, tx - self.rx)
e_a = (t_a - self.ra + math.pi) % (2 * math.pi) - math.pi
if abs(e_a) > 0.5:
self.send_cmd(0, 0.4 if e_a > 0 else -0.4)
else:
self.send_cmd(0.15, e_a * 0.8)
# ================= 버튼 기능 =================
def clear_canvas_only(self):
"""로봇의 위치나 Home은 유지하고, 캔버스의 점과 선만 삭제"""
self.waypoints.clear()
self.trail.clear()
print("캔버스 경로가 초기화되었습니다.")
def start_waypoints(self):
self.mode = "WAYPOINT" if self.waypoints else "IDLE"
def optimize_path(self):
if len(self.waypoints) < 2: return
opt = []
cx, cy = self.rx, self.ry
rem = list(self.waypoints)
while rem:
nxt = min(rem, key=lambda p: math.hypot(p[0] - cx, p[1] - cy))
opt.append(nxt)
rem.remove(nxt)
cx, cy = nxt
self.waypoints = opt
def start_return(self):
self.mode = "RETURNING"
def stop_robot(self):
self.mode = "IDLE"
self.send_cmd(0, 0)
# ================= 업데이트 및 그리기 =================
def update_loop(self):
try:
res = requests.get(URL, timeout=0.1).json()
p = res.get("p", {"x": 0, "y": 0, "a": 0})
# 1. 실제 좌표 저장 (Real)
self.rx, self.ry, self.ra = p['x'] * 100, p['y'] * 100, float(p['a'])
# 2. 시스템 초기화 시 Home 설정
if self.start_pose is None:
self.start_pose = (self.rx, self.ry, self.ra)
# 3. 로봇이 생각하는 좌표 계산 (Robot 기준)
robot_x = self.rx - self.start_pose[0]
robot_y = self.ry - self.start_pose[1]
# 4. UI 텍스트 갱신
self.lbl_real_pos.config(text=f"Real X: {self.rx:.1f}, Y: {self.ry:.1f}")
self.lbl_robot_pos.config(text=f"Robot X: {robot_x:.1f}, Y: {robot_y:.1f}")
self.lbl_status.config(text=f"상태: {self.mode}")
# 5. Trail 및 Lidar
if not self.trail or math.hypot(self.trail[-1][0] - self.rx, self.trail[-1][1] - self.ry) > 2.0:
self.trail.append((self.rx, self.ry))
self.lidar_data = res.get("scan", [])
self.draw_canvas()
except:
pass
self.after(INTERVAL, self.update_loop)
def draw_canvas(self):
self.canvas.delete("all")
cx, cy = 300, 300
def to_s(wx, wy):
return cx + (wx - self.rx) * SCALE, cy - (wy - self.ry) * SCALE
# 경로 그리기
if len(self.trail) > 1:
pts = []
for tx, ty in self.trail: pts.extend(to_s(tx, ty))
self.canvas.create_line(pts, fill="#333333", width=1)
# 웨이포인트 연결선
if self.waypoints:
prev_s = to_s(self.rx, self.ry)
for i, (wx, wy) in enumerate(self.waypoints):
curr_s = to_s(wx, wy)
self.canvas.create_line(prev_s[0], prev_s[1], curr_s[0], curr_s[1], fill="#007bff", dash=(3, 3))
self.canvas.create_oval(curr_s[0] - 4, curr_s[1] - 4, curr_s[0] + 4, curr_s[1] + 4, fill="#007bff",
outline="white")
self.canvas.create_text(curr_s[0], curr_s[1] - 15, text=str(i + 1), fill="white")
prev_s = curr_s
# Lidar 센서 표시
if self.lidar_data:
angle_inc = (2 * math.pi) / len(self.lidar_data)
for i, d in enumerate(self.lidar_data):
if 0 < d < 10:
ox = self.rx + (d * 100) * math.cos(self.ra + i * angle_inc)
oy = self.ry + (d * 100) * math.sin(self.ra + i * angle_inc)
sx, sy = to_s(ox, oy)
self.canvas.create_rectangle(sx, sy, sx + 1.5, sy + 1.5, fill="#ff4d4d", outline="")
# 로봇 본체
self.canvas.create_oval(cx - 12, cy - 12, cx + 12, cy + 12, fill="#28a745", outline="white", width=2)
self.canvas.create_line(cx, cy, cx + 22 * math.cos(self.ra), cy - 22 * math.sin(self.ra), fill="white",
arrow=tk.LAST, width=2)
def add_waypoint_click(self, event):
tx = self.rx + (event.x - 300) / SCALE
ty = self.ry + (300 - event.y) / SCALE
self.waypoints.append((tx, ty))
def send_cmd(self, lin, ang):
try:
requests.get(URL, params={"lin": f"{lin:.3f}", "ang": f"{ang:.3f}"}, timeout=0.1)
except:
pass
if __name__ == '__main__':
root = tk.Tk()
root.title("Integrated Robot Mission Controller")
RobotController(root).pack(expand=True, fill='both')
root.mainloop()선준님이 작성해주신 코드
import math
import tkinter as tk
from tkinter import ttk
import requests
import threading
import time
# ================= 설정 및 파라미터 =================
URL = "http://192.168.0.243:5555/control"
INTERVAL = 100
WP_TOL = 5.0
ANGLE_TOL = 0.08
SCALE = 1.5 # ★ 화면 축소 → 더 넓게 보기
CANVAS_SIZE = 600
# ================= MR 파라미터 =================
DIST_DIV = 100.0
MIN_DIST = 0.10
MAX_DIST = 3.5
LIDAR_SKIP = 3
# ================= 화면 표시 =================
GRID_CM = 50
GRID_COLOR = "#dddddd"
WALL_COLOR = "#7a00cc"
class RobotController(ttk.Frame):
def __init__(self, master):
super().__init__(master)
# ===== 원본 상태 =====
self.mode = "IDLE"
self.rx = self.ry = self.ra = 0.0
self.start_pose = None
self.waypoints = []
self.trail = []
self.lidar_data = []
# ===== 표시 전용 =====
self.ra_view = 0.0
self.ra_init = None
self.setup_ui()
threading.Thread(target=self.main_control_logic, daemon=True).start()
self.update_loop()
# ================= UI =================
def setup_ui(self):
self.side_panel = tk.Frame(self, width=250, bg="#f8f9fa", padx=15, pady=15)
self.side_panel.pack(side='left', fill='y')
tk.Label(self.side_panel, text="[ 좌표 ]", font=("맑은 고딕", 10, "bold")).pack(anchor='w')
self.lbl_real_pos = tk.Label(self.side_panel, text="Real X: 0.0, Y: 0.0")
self.lbl_real_pos.pack(anchor='w')
self.lbl_robot_pos = tk.Label(self.side_panel, text="Robot X: 0.0, Y: 0.0", fg="#007bff")
self.lbl_robot_pos.pack(anchor='w')
self.lbl_status = tk.Label(self.side_panel, text="상태: IDLE")
self.lbl_status.pack(anchor='w', pady=5)
btn = {'width': 22, 'pady': 6}
tk.Button(self.side_panel, text="포인트 따라가기", command=self.start_waypoints, **btn).pack(pady=3)
tk.Button(self.side_panel, text="최단 경로 설정", command=self.optimize_path, **btn).pack(pady=3)
tk.Label(self.side_panel, text="주차 방향 선택", font=("맑은 고딕", 8)).pack()
self.park_var = tk.StringVar(value="FRONT")
pf = tk.Frame(self.side_panel)
pf.pack()
tk.Radiobutton(pf, text="전면", variable=self.park_var, value="FRONT").pack(side='left')
tk.Radiobutton(pf, text="후면", variable=self.park_var, value="BACK").pack(side='left')
tk.Button(self.side_panel, text="복귀하기", command=self.start_return, **btn).pack(pady=3)
tk.Button(self.side_panel, text="경로 초기화", command=self.clear_canvas_only, **btn).pack(pady=3)
tk.Button(self.side_panel, text="🛑정지", command=self.stop_robot,
fg='#b71c1c', width=22, pady=10).pack(pady=15)
self.canvas = tk.Canvas(self, width=CANVAS_SIZE, height=CANVAS_SIZE,
bg="white", highlightthickness=0)
self.canvas.pack(side='right', expand=True, fill='both')
self.canvas.bind('<Button-1>', self.add_waypoint_click)
# ================= FSM (원본 유지) =================
def main_control_logic(self):
while True:
if self.mode == "WAYPOINT":
self.step_waypoint()
elif self.mode == "RETURNING":
self.step_return()
time.sleep(0.05)
def step_waypoint(self):
if not self.waypoints:
self.mode = "IDLE"
self.send_cmd(0, 0)
return
tx, ty = self.waypoints[0]
if math.hypot(tx - self.rx, ty - self.ry) < WP_TOL:
self.waypoints.pop(0)
return
self.move_to_target(tx, ty)
def step_return(self):
if not self.start_pose:
return
sx, sy, sa = self.start_pose
if math.hypot(sx - self.rx, sy - self.ry) < WP_TOL:
target = sa if self.park_var.get() == "FRONT" else sa + math.pi
err = (target - self.ra + math.pi) % (2*math.pi) - math.pi
if abs(err) < ANGLE_TOL:
self.send_cmd(0, 0)
self.mode = "IDLE"
else:
self.send_cmd(0, 0.3 if err > 0 else -0.3)
else:
self.move_to_target(sx, sy)
def move_to_target(self, tx, ty):
ta = math.atan2(ty - self.ry, tx - self.rx)
ea = (ta - self.ra + math.pi) % (2*math.pi) - math.pi
if abs(ea) > 0.5:
self.send_cmd(0, 0.4 if ea > 0 else -0.4)
else:
self.send_cmd(0.15, ea * 0.8)
# ================= 업데이트 =================
def update_loop(self):
try:
res = requests.get(URL, timeout=0.1).json()
p = res["p"]
self.rx = p["x"] * 100
self.ry = p["y"] * 100
self.ra = float(p["a"])
if self.start_pose is None:
self.start_pose = (self.rx, self.ry, self.ra)
if self.ra_init is None:
self.ra_init = self.ra
self.ra_view = (self.ra - self.ra_init) + math.pi/2
self.lidar_data = res.get("s", res.get("scan", []))
self.lbl_real_pos.config(text=f"Real X: {self.rx:.1f}, Y: {self.ry:.1f}")
self.lbl_robot_pos.config(
text=f"Robot X: {self.rx - self.start_pose[0]:.1f}, "
f"Y: {self.ry - self.start_pose[1]:.1f}"
)
self.lbl_status.config(text=f"상태: {self.mode}")
self.draw_canvas()
except:
pass
self.after(INTERVAL, self.update_loop)
# ================= 그리기 =================
def draw_canvas(self):
self.canvas.delete("all")
cx = CANVAS_SIZE // 2
cy = int(CANVAS_SIZE * 0.75)
def to_s(xw, yw):
return cx + (xw - self.rx) * SCALE, cy - (yw - self.ry) * SCALE
# ----- 격자 -----
step = GRID_CM * SCALE
for i in range(0, CANVAS_SIZE, int(step)):
self.canvas.create_line(i, 0, i, CANVAS_SIZE, fill=GRID_COLOR)
self.canvas.create_line(0, i, CANVAS_SIZE, i, fill=GRID_COLOR)
# ----- MR 벽 -----
for i in range(0, len(self.lidar_data), LIDAR_SKIP):
d = self.lidar_data[i] / DIST_DIV
if d < MIN_DIST or d > MAX_DIST:
continue
th = math.radians(i)
xr = d * math.cos(th)
yr = d * math.sin(th)
xw = self.rx + xr*100*math.cos(self.ra_view) - yr*100*math.sin(self.ra_view)
yw = self.ry + xr*100*math.sin(self.ra_view) + yr*100*math.cos(self.ra_view)
sx, sy = to_s(xw, yw)
self.canvas.create_rectangle(sx, sy, sx+2, sy+2,
fill=WALL_COLOR, outline="")
# ----- 웨이포인트 (5개 제한 유지) -----
if self.waypoints:
prev = to_s(self.rx, self.ry)
for i, (wx, wy) in enumerate(self.waypoints):
cur = to_s(wx, wy)
self.canvas.create_line(prev[0], prev[1], cur[0], cur[1],
fill="#007bff", dash=(3, 3))
self.canvas.create_oval(cur[0]-4, cur[1]-4,
cur[0]+4, cur[1]+4,
fill="#007bff")
self.canvas.create_text(cur[0], cur[1]-15, text=str(i+1))
prev = cur
# ----- 로봇 -----
self.canvas.create_oval(cx-12, cy-12, cx+12, cy+12,
fill="#28a745", outline="black", width=2)
self.canvas.create_line(cx, cy,
cx + 22 * math.cos(self.ra_view),
cy - 22 * math.sin(self.ra_view),
fill="black", arrow=tk.LAST, width=2)
# ================= 기타 =================
def add_waypoint_click(self, event):
if len(self.waypoints) >= 5:
return
cx = CANVAS_SIZE // 2
cy = int(CANVAS_SIZE * 0.75)
tx = self.rx + (event.x - cx) / SCALE
ty = self.ry + (cy - event.y) / SCALE
self.waypoints.append((tx, ty))
def clear_canvas_only(self):
self.waypoints.clear()
self.trail.clear()
def start_waypoints(self):
self.mode = "WAYPOINT" if self.waypoints else "IDLE"
def optimize_path(self):
if len(self.waypoints) < 2:
return
opt, cx, cy = [], self.rx, self.ry
rem = list(self.waypoints)
while rem:
nxt = min(rem, key=lambda p: math.hypot(p[0]-cx, p[1]-cy))
opt.append(nxt)
rem.remove(nxt)
cx, cy = nxt
self.waypoints = opt
def start_return(self):
self.mode = "RETURNING"
def stop_robot(self):
self.mode = "IDLE"
self.send_cmd(0, 0)
def send_cmd(self, lin, ang):
try:
requests.get(URL,
params={"lin": f"{lin:.3f}", "ang": f"{ang:.3f}"},
timeout=0.1)
except:
pass
if __name__ == "__main__":
root = tk.Tk()
root.title("Integrated Robot Mission Controller + MR")
RobotController(root).pack(expand=True, fill="both")
root.mainloop()map reading 포함 코드로 수정
- ROI 구역으로 이동 및 탐색 로그 저장
- 출발점 복귀
- 웨이포인트 최대 5포인트 주행
- 장애물 회피 주행
- 1~4에 대한 주행 속도 및 정확도
-> 개인별 기여도, 역할
import math
import tkinter as tk
from tkinter import ttk
import requests
import threading
import time
# ================= 설정 =================
URL = "http://192.168.0.243:4444/control"
INTERVAL = 100
CANVAS_SIZE = 600
SCALE = 1.5 # cm → px
WP_TOL = 8.0
ANGLE_TOL = 0.08
# LIDAR
DIST_DIV = 100.0
MIN_DIST = 0.10
MAX_DIST = 3.5
LIDAR_SKIP = 3
GRID_CM = 50
GRID_COLOR = "#dddddd"
WALL_COLOR = "#7a00cc"
VISUAL_OFFSET = math.pi / 2 # 화면 위 = 로봇 전방
class RobotController(ttk.Frame):
def __init__(self, master):
super().__init__(master)
# ===== 로봇 상태 (월드 좌표, cm) =====
self.rx = self.ry = self.ra = 0.0
self.start_pose = None
self.mode = "IDLE"
self.lidar_data = []
self.waypoints = []
self.setup_ui()
threading.Thread(target=self.control_loop, daemon=True).start()
self.update_loop()
# ================= UI =================
def setup_ui(self):
panel = tk.Frame(self, width=260, padx=10, pady=10)
panel.pack(side="left", fill="y")
tk.Label(panel, text="[ 좌표 ]", font=("맑은 고딕", 10, "bold")).pack(anchor="w")
self.lbl_real = tk.Label(panel, text="Real X: 0, Y: 0")
self.lbl_real.pack(anchor="w")
self.lbl_robot = tk.Label(panel, text="Robot X: 0, Y: 0", fg="#007bff")
self.lbl_robot.pack(anchor="w")
self.lbl_state = tk.Label(panel, text="상태: IDLE")
self.lbl_state.pack(anchor="w", pady=5)
ttk.Separator(panel, orient="horizontal").pack(fill="x", pady=8)
tk.Button(panel, text="포인트 따라가기", command=self.start_waypoints).pack(fill="x")
tk.Button(panel, text="최단 경로 설정", command=self.optimize_path).pack(fill="x", pady=3)
tk.Label(panel, text="주차 방향 선택").pack(anchor="w", pady=(10, 0))
self.park_var = tk.StringVar(value="FRONT")
pf = tk.Frame(panel)
pf.pack(anchor="w")
tk.Radiobutton(pf, text="전면", variable=self.park_var, value="FRONT").pack(side="left")
tk.Radiobutton(pf, text="후면", variable=self.park_var, value="BACK").pack(side="left")
tk.Button(panel, text="복귀하기", command=self.start_return).pack(fill="x", pady=5)
tk.Button(panel, text="경로 초기화", command=self.clear_waypoints).pack(fill="x")
tk.Button(panel, text="정지", command=self.stop_robot,
fg="red").pack(fill="x", pady=15)
self.canvas = tk.Canvas(self, width=CANVAS_SIZE, height=CANVAS_SIZE, bg="white")
self.canvas.pack(side="right", expand=True, fill="both")
self.canvas.bind("<Button-1>", self.add_waypoint_click)
# ================= FSM =================
def control_loop(self):
while True:
if self.mode == "WAYPOINT":
self.step_waypoint()
elif self.mode == "RETURN":
self.step_return()
time.sleep(0.05)
def step_waypoint(self):
if not self.waypoints:
self.stop_robot()
return
tx, ty = self.waypoints[0]
dist = math.hypot(tx - self.rx, ty - self.ry)
if dist < WP_TOL:
self.waypoints.pop(0)
return
ta = math.atan2(ty - self.ry, tx - self.rx)
err = (ta - self.ra + math.pi) % (2 * math.pi) - math.pi
if abs(err) > 0.5:
self.send_cmd(0, 0.4 if err > 0 else -0.4)
else:
self.send_cmd(0.15, err * 0.8)
def step_return(self):
if not self.start_pose:
return
sx, sy, sa = self.start_pose
self.waypoints = [(sx, sy)]
if self.park_var.get() == "BACK":
self.waypoints.append(
(sx - 20 * math.cos(sa), sy - 20 * math.sin(sa))
)
self.mode = "WAYPOINT"
# ================= 네트워크 =================
def update_loop(self):
try:
res = requests.get(URL, timeout=0.1).json()
p = res["p"]
self.rx = p["x"] * 100
self.ry = p["y"] * 100
self.ra = float(p["a"])
if self.start_pose is None:
self.start_pose = (self.rx, self.ry, self.ra)
self.lidar_data = res.get("s", [])
self.lbl_real.config(text=f"Real X: {int(self.rx)}, Y: {int(self.ry)}")
self.lbl_robot.config(
text=f"Robot X: {int(self.rx - self.start_pose[0])}, "
f"Y: {int(self.ry - self.start_pose[1])}"
)
self.lbl_state.config(text=f"상태: {self.mode}")
self.draw_canvas()
except:
pass
self.after(INTERVAL, self.update_loop)
# ================= 그리기 =================
def draw_canvas(self):
self.canvas.delete("all")
cx = cy = CANVAS_SIZE // 2
# 격자
step = GRID_CM * SCALE
for i in range(0, CANVAS_SIZE, int(step)):
self.canvas.create_line(i, 0, i, CANVAS_SIZE, fill=GRID_COLOR)
self.canvas.create_line(0, i, CANVAS_SIZE, i, fill=GRID_COLOR)
# 센서
for i in range(0, len(self.lidar_data), LIDAR_SKIP):
d = self.lidar_data[i] / DIST_DIV
if d < MIN_DIST or d > MAX_DIST:
continue
ang = math.radians(i) + VISUAL_OFFSET
x = d * 100 * math.cos(ang)
y = d * 100 * math.sin(ang)
sx = cx + x * SCALE
sy = cy - y * SCALE
self.canvas.create_rectangle(sx, sy, sx+2, sy+2,
fill=WALL_COLOR, outline="")
# 웨이포인트
prev = (cx, cy)
for wx, wy in self.waypoints:
dx = wx - self.rx
dy = wy - self.ry
dist = math.hypot(dx, dy)
ang = math.atan2(dy, dx) - self.ra + VISUAL_OFFSET
sx = cx + dist * math.cos(ang) * SCALE
sy = cy - dist * math.sin(ang) * SCALE
self.canvas.create_line(prev[0], prev[1], sx, sy,
fill="#007bff", dash=(3, 3))
self.canvas.create_oval(sx-4, sy-4, sx+4, sy+4, fill="#007bff")
self.canvas.create_text(
sx + 6, sy + 6,
text=f"({int(wx)}, {int(wy)})",
font=("맑은 고딕", 8)
)
prev = (sx, sy)
# 로봇
self.canvas.create_oval(cx-12, cy-12, cx+12, cy+12,
fill="#28a745", outline="black", width=2)
self.canvas.create_line(cx, cy, cx, cy-22,
fill="black", arrow=tk.LAST, width=2)
# ================= 클릭 =================
def add_waypoint_click(self, event):
if len(self.waypoints) >= 5:
return
cx = cy = CANVAS_SIZE // 2
dx = (event.x - cx) / SCALE
dy = (cy - event.y) / SCALE
dist = math.hypot(dx, dy)
ang = math.atan2(dy, dx) + self.ra - VISUAL_OFFSET
wx = self.rx + dist * math.cos(ang)
wy = self.ry + dist * math.sin(ang)
self.waypoints.append((wx, wy))
# ================= 유틸 =================
def start_waypoints(self):
self.mode = "WAYPOINT"
def start_return(self):
self.mode = "RETURN"
def clear_waypoints(self):
self.waypoints.clear()
def optimize_path(self):
if len(self.waypoints) < 2:
return
opt, cx, cy = [], self.rx, self.ry
rem = list(self.waypoints)
while rem:
nxt = min(rem, key=lambda p: math.hypot(p[0]-cx, p[1]-cy))
opt.append(nxt)
rem.remove(nxt)
cx, cy = nxt
self.waypoints = opt
def stop_robot(self):
self.mode = "IDLE"
self.send_cmd(0, 0)
def send_cmd(self, lin, ang):
try:
requests.get(URL,
params={"lin": f"{lin:.3f}", "ang": f"{ang:.3f}"},
timeout=0.1)
except:
pass
if __name__ == "__main__":
root = tk.Tk()
root.title("Integrated Robot Mission Controller (Aligned)")
RobotController(root).pack(expand=True, fill="both")
root.mainloop()웨이포인트 + 센서 표시 + 이동은 전부 되는데, 정면 후면 주차가 안됨
import math
import tkinter as tk
from tkinter import ttk
import requests
import threading
import time
# ================= 설정 =================
URL = "http://192.168.0.243:3331/control"
INTERVAL = 100
CANVAS_SIZE = 600
SCALE = 1.5 # cm → px
WP_TOL = 5.0
ANGLE_TOL = 0.08
# LIDAR
DIST_DIV = 100.0
MIN_DIST = 0.10
MAX_DIST = 3.5
LIDAR_SKIP = 3
GRID_CM = 50
GRID_COLOR = "#dddddd"
WALL_COLOR = "#7a00cc"
VISUAL_OFFSET = math.pi / 2 # 화면 위 = 로봇 전방
class RobotController(ttk.Frame):
def __init__(self, master):
super().__init__(master)
self.rx = self.ry = self.ra = 0.0
self.start_pose = None
self.mode = "IDLE"
self.return_phase = None
self.lidar_data = []
self.waypoints = []
self.setup_ui()
threading.Thread(target=self.control_loop, daemon=True).start()
self.update_loop()
# ================= UI =================
def setup_ui(self):
panel = tk.Frame(self, width=260, padx=10, pady=10)
panel.pack(side="left", fill="y")
tk.Label(panel, text="[ 좌표 ]", font=("맑은 고딕", 10, "bold")).pack(anchor="w")
self.lbl_real = tk.Label(panel, text="Real X: 0, Y: 0")
self.lbl_real.pack(anchor="w")
self.lbl_robot = tk.Label(panel, text="Robot X: 0, Y: 0", fg="#007bff")
self.lbl_robot.pack(anchor="w")
self.lbl_state = tk.Label(panel, text="상태: IDLE")
self.lbl_state.pack(anchor="w", pady=5)
ttk.Separator(panel, orient="horizontal").pack(fill="x", pady=8)
tk.Button(panel, text="포인트 따라가기", command=self.start_waypoints).pack(fill="x")
tk.Button(panel, text="최단 경로 설정", command=self.optimize_path).pack(fill="x", pady=3)
tk.Label(panel, text="주차 방향 선택").pack(anchor="w", pady=(10, 0))
self.park_var = tk.StringVar(value="BACK")
pf = tk.Frame(panel)
pf.pack(anchor="w")
tk.Radiobutton(pf, text="전면", variable=self.park_var, value="FRONT").pack(side="left")
tk.Radiobutton(pf, text="후면", variable=self.park_var, value="BACK").pack(side="left")
tk.Button(panel, text="복귀하기", command=self.start_return).pack(fill="x", pady=5)
tk.Button(panel, text="경로 초기화", command=self.clear_waypoints).pack(fill="x")
tk.Button(panel, text="정지", command=self.stop_robot,
fg="red").pack(fill="x", pady=15)
self.canvas = tk.Canvas(self, width=CANVAS_SIZE, height=CANVAS_SIZE, bg="white")
self.canvas.pack(side="right", expand=True, fill="both")
self.canvas.bind("<Button-1>", self.add_waypoint_click)
# ================= FSM =================
def control_loop(self):
while True:
if self.mode == "WAYPOINT":
self.step_waypoint()
elif self.mode == "RETURN":
self.step_return()
time.sleep(0.05)
def step_waypoint(self):
if not self.waypoints:
self.stop_robot()
return
tx, ty = self.waypoints[0]
dist = math.hypot(tx - self.rx, ty - self.ry)
if dist < WP_TOL:
self.waypoints.pop(0)
return
ta = math.atan2(ty - self.ry, tx - self.rx)
err = (ta - self.ra + math.pi) % (2 * math.pi) - math.pi
if abs(err) > 0.5:
self.send_cmd(0, 0.4 if err > 0 else -0.4)
else:
self.send_cmd(0.15, err * 0.8)
# ===== 주차 FSM =====
def start_return(self):
self.return_phase = "MOVE"
self.mode = "RETURN"
def step_return(self):
if not self.start_pose:
return
sx, sy, sa = self.start_pose
if self.park_var.get() == "FRONT":
target_yaw = (sa + math.pi) % (2 * math.pi)
else:
target_yaw = sa
# 1단계: 위치 복귀
if self.return_phase == "MOVE":
dist = math.hypot(sx - self.rx, sy - self.ry)
if dist > WP_TOL:
ta = math.atan2(sy - self.ry, sx - self.rx)
err = (ta - self.ra + math.pi) % (2 * math.pi) - math.pi
if abs(err) > 0.4:
self.send_cmd(0, 0.4 if err > 0 else -0.4)
else:
self.send_cmd(0.15, err * 0.8)
return
self.send_cmd(0, 0)
self.return_phase = "ALIGN"
return
# 2단계: 각도 정렬
err_yaw = (target_yaw - self.ra + math.pi) % (2 * math.pi) - math.pi
if abs(err_yaw) < ANGLE_TOL:
self.send_cmd(0, 0)
self.mode = "IDLE"
return
self.send_cmd(0, 0.3 if err_yaw > 0 else -0.3)
# ================= 네트워크 =================
def update_loop(self):
try:
res = requests.get(URL, timeout=0.1).json()
p = res["p"]
self.rx = p["x"] * 100
self.ry = p["y"] * 100
self.ra = float(p["a"])
if self.start_pose is None:
self.start_pose = (self.rx, self.ry, self.ra)
self.lidar_data = res.get("s", [])
self.lbl_real.config(text=f"Real X: {int(self.rx)}, Y: {int(self.ry)}")
self.lbl_robot.config(
text=f"Robot X: {int(self.rx - self.start_pose[0])}, "
f"Y: {int(self.ry - self.start_pose[1])}"
)
self.lbl_state.config(text=f"상태: {self.mode}")
self.draw_canvas()
except:
pass
self.after(INTERVAL, self.update_loop)
# ================= 그리기 =================
def draw_canvas(self):
self.canvas.delete("all")
cx = cy = CANVAS_SIZE // 2
step = GRID_CM * SCALE
for i in range(0, CANVAS_SIZE, int(step)):
self.canvas.create_line(i, 0, i, CANVAS_SIZE, fill=GRID_COLOR)
self.canvas.create_line(0, i, CANVAS_SIZE, i, fill=GRID_COLOR)
for i in range(0, len(self.lidar_data), LIDAR_SKIP):
d = self.lidar_data[i] / DIST_DIV
if d < MIN_DIST or d > MAX_DIST:
continue
ang = math.radians(i) + VISUAL_OFFSET
x = d * 100 * math.cos(ang)
y = d * 100 * math.sin(ang)
sx = cx + x * SCALE
sy = cy - y * SCALE
self.canvas.create_rectangle(sx, sy, sx+2, sy+2,
fill=WALL_COLOR, outline="")
prev = (cx, cy)
for wx, wy in self.waypoints:
dx = wx - self.rx
dy = wy - self.ry
dist = math.hypot(dx, dy)
ang = math.atan2(dy, dx) - self.ra + VISUAL_OFFSET
sx = cx + dist * math.cos(ang) * SCALE
sy = cy - dist * math.sin(ang) * SCALE
self.canvas.create_line(prev[0], prev[1], sx, sy,
fill="#007bff", dash=(3, 3))
self.canvas.create_oval(sx-4, sy-4, sx+4, sy+4, fill="#007bff")
prev = (sx, sy)
self.canvas.create_oval(cx-12, cy-12, cx+12, cy+12,
fill="#28a745", outline="black", width=2)
self.canvas.create_line(cx, cy, cx, cy-22,
fill="black", arrow=tk.LAST, width=2)
# ================= 유틸 =================
def add_waypoint_click(self, event):
if len(self.waypoints) >= 5:
return
cx = cy = CANVAS_SIZE // 2
dx = (event.x - cx) / SCALE
dy = (cy - event.y) / SCALE
dist = math.hypot(dx, dy)
ang = math.atan2(dy, dx) + self.ra - VISUAL_OFFSET
wx = self.rx + dist * math.cos(ang)
wy = self.ry + dist * math.sin(ang)
self.waypoints.append((wx, wy))
def start_waypoints(self):
self.mode = "WAYPOINT"
def clear_waypoints(self):
self.waypoints.clear()
def optimize_path(self):
if len(self.waypoints) < 2:
return
opt, cx, cy = [], self.rx, self.ry
rem = list(self.waypoints)
while rem:
nxt = min(rem, key=lambda p: math.hypot(p[0]-cx, p[1]-cy))
opt.append(nxt)
rem.remove(nxt)
cx, cy = nxt
self.waypoints = opt
def stop_robot(self):
self.mode = "IDLE"
self.send_cmd(0, 0)
def send_cmd(self, lin, ang):
try:
requests.get(URL,
params={"lin": f"{lin:.3f}", "ang": f"{ang:.3f}"},
timeout=0.1)
except:
pass
if __name__ == "__main__":
root = tk.Tk()
root.title("Integrated Robot Mission Controller (Parking Fixed)")
RobotController(root).pack(expand=True, fill="both")
root.mainloop()주차 개선 완료
내일부터 장애물 피하기 코드 추가해야 함.
'로보테크AI' 카테고리의 다른 글
| 융합_로보테크 AI 자율주행 로봇 개발자 과정-26/02/05 (0) | 2026.02.05 |
|---|---|
| 융합_로보테크 AI 자율주행 로봇 개발자 과정-26/02/04 (0) | 2026.02.04 |
| 융합_로보테크 AI 자율주행 로봇 개발자 과정-26/02/02 (0) | 2026.02.02 |
| 융합_로보테크 AI 자율주행 로봇 개발자 과정-26/01/30 (0) | 2026.01.30 |
| 융합_로보테크 AI 자율주행 로봇 개발자 과정-26/01/29 (0) | 2026.01.29 |