import fcntl import json import math import os import socket import struct import subprocess import threading import time import numpy as np import zmq from datetime import datetime from ftplib import FTP from cereal import log import cereal.messaging as messaging from openpilot.common.realtime import Ratekeeper from openpilot.common.params import Params from openpilot.common.filter_simple import MyMovingAverage from openpilot.system.hardware import PC, TICI from openpilot.selfdrive.navd.helpers import Coordinate from opendbc.car.common.conversions import Conversions as CV from openpilot.selfdrive.carrot.carrot_serv import CarrotServ from openpilot.selfdrive.carrot.carrot_speed import CarrotSpeed from openpilot.common.gps import get_gps_location_service try: from shapely.geometry import LineString SHAPELY_AVAILABLE = True except ImportError: SHAPELY_AVAILABLE = False NetworkType = log.DeviceState.NetworkType ################ CarrotNavi ## 국가법령정보센터: 도로설계기준 #V_CURVE_LOOKUP_BP = [0., 1./800., 1./670., 1./560., 1./440., 1./360., 1./265., 1./190., 1./135., 1./85., 1./55., 1./30., 1./15.] #V_CRUVE_LOOKUP_VALS = [300, 150, 120, 110, 100, 90, 80, 70, 60, 50, 45, 35, 30] V_CURVE_LOOKUP_BP = [0., 1./800., 1./670., 1./560., 1./440., 1./360., 1./265., 1./190., 1./135., 1./85., 1./55., 1./30., 1./25.] V_CRUVE_LOOKUP_VALS = [300, 150, 120, 110, 100, 90, 80, 70, 60, 50, 40, 15, 5] # Haversine formula to calculate distance between two GPS coordinates #haversine_cache = {} def haversine(lon1, lat1, lon2, lat2): #key = (lon1, lat1, lon2, lat2) #if key in haversine_cache: # return haversine_cache[key] R = 6371000 # Radius of Earth in meters phi1, phi2 = math.radians(lat1), math.radians(lat2) dphi = math.radians(lat2 - lat1) dlambda = math.radians(lon2 - lon1) a = math.sin(dphi / 2) ** 2 + math.cos(phi1) * math.cos(phi2) * math.sin(dlambda / 2) ** 2 distance = 2 * R * math.atan2(math.sqrt(a), math.sqrt(1 - a)) #haversine_cache[key] = distance return distance # Get the closest point on a segment between two coordinates def closest_point_on_segment(p1, p2, current_position): x1, y1 = p1 x2, y2 = p2 px, py = current_position dx = x2 - x1 dy = y2 - y1 if dx == 0 and dy == 0: return p1 # p1 and p2 are the same point # Parameter t is the projection factor onto the line segment t = ((px - x1) * dx + (py - y1) * dy) / (dx * dx + dy * dy) t = max(0, min(1, t)) # Clamp t to the segment closest_x = x1 + t * dx closest_y = y1 + t * dy return (closest_x, closest_y) # Get path after a certain distance from the current position def get_path_after_distance(start_index, coordinates, current_position, distance_m): total_distance = 0 path_after_distance = [] closest_index = -1 closest_point = None min_distance = float('inf') start_index = max(0, start_index - 2) # 가까운 점만 탐색하도록 수정 for i in range(start_index, len(coordinates) - 1): p1 = coordinates[i] p2 = coordinates[i + 1] candidate_point = closest_point_on_segment(p1, p2, current_position) distance = haversine(current_position[0], current_position[1], candidate_point[0], candidate_point[1]) if distance < min_distance: min_distance = distance closest_point = candidate_point closest_index = i elif distance > min_distance and min_distance < 10: break start_index = closest_index # Start from the closest point and calculate the path after the specified distance if closest_index != -1: path_after_distance.append(closest_point) path_after_distance.append(coordinates[closest_index + 1]) total_distance = haversine(closest_point[0], closest_point[1], coordinates[closest_index + 1][0], coordinates[closest_index + 1][1]) # Traverse the path forward from the next point for i in range(closest_index + 1, len(coordinates) - 1): coord1 = coordinates[i] coord2 = coordinates[i + 1] segment_distance = haversine(coord1[0], coord1[1], coord2[0], coord2[1]) if total_distance + segment_distance >= distance_m and segment_distance > 0: remaining_distance = distance_m - total_distance ratio = remaining_distance / segment_distance interpolated_lon = coord1[0] + ratio * (coord2[0] - coord1[0]) interpolated_lat = coord1[1] + ratio * (coord2[1] - coord1[1]) path_after_distance.append((interpolated_lon, interpolated_lat)) break total_distance += segment_distance path_after_distance.append(coord2) return path_after_distance, start_index, closest_point def calculate_angle(point1, point2): delta_lon = point2[0] - point1[0] delta_lat = point2[1] - point1[1] return math.degrees(math.atan2(delta_lat, delta_lon)) # Convert GPS coordinates to relative x, y coordinates based on a reference point and heading def gps_to_relative_xy(gps_path, reference_point, heading_deg): ref_lon, ref_lat = reference_point relative_coordinates = [] # Convert heading from degrees to radians heading_rad = math.radians(heading_deg) for lon, lat in gps_path: # Convert lat/lon differences to meters (assuming small distances for simple approximation) x = (lon - ref_lon) * 40008000 * math.cos(math.radians(ref_lat)) / 360 y = (lat - ref_lat) * 40008000 / 360 # Rotate coordinates based on the heading angle to align with the car's direction x_rot = x * math.cos(heading_rad) - y * math.sin(heading_rad) y_rot = x * math.sin(heading_rad) + y * math.cos(heading_rad) relative_coordinates.append((y_rot, x_rot)) return relative_coordinates # Calculate curvature given three points using a faster vector-based method #curvature_cache = {} def calculate_curvature(p1, p2, p3): #key = (p1, p2, p3) #if key in curvature_cache: # return curvature_cache[key] v1 = (p2[0] - p1[0], p2[1] - p1[1]) v2 = (p3[0] - p2[0], p3[1] - p2[1]) cross_product = v1[0] * v2[1] - v1[1] * v2[0] len_v1 = math.sqrt(v1[0] ** 2 + v1[1] ** 2) len_v2 = math.sqrt(v2[0] ** 2 + v2[1] ** 2) if len_v1 * len_v2 == 0: curvature = 0 else: curvature = cross_product / (len_v1 * len_v2 * len_v1) #curvature_cache[key] = curvature return curvature class CarrotMan: def __init__(self): print("************************************************CarrotMan init************************************************") self.params = Params() self.params_memory = Params("/dev/shm/params") self.gps_location_service = get_gps_location_service(self.params) self.sm = messaging.SubMaster(['deviceState', 'carState', 'controlsState', 'radarState', 'longitudinalPlan', 'modelV2', 'selfdriveState', 'carControl', 'navRouteNavd', self.gps_location_service, 'navInstruction']) self.pm = messaging.PubMaster(['carrotMan', "navRoute", "navInstructionCarrot"]) self.carrot_serv = CarrotServ() self.show_panda_debug = False self.broadcast_ip = self.get_broadcast_address() self.broadcast_port = 7705 self.carrot_man_port = 7706 self.connection = None self.ip_address = "0.0.0.0" self.remote_addr = None self.turn_speed_last = 250 self.curvatureFilter = MyMovingAverage(20) self.carrot_curve_speed_params() self.carrot_zmq_thread = threading.Thread(target=self.carrot_cmd_zmq, args=[]) self.carrot_zmq_thread.daemon = True self.carrot_zmq_thread.start() self.carrot_panda_debug_thread = threading.Thread(target=self.carrot_panda_debug, args=[]) self.carrot_panda_debug_thread.daemon = True self.carrot_panda_debug_thread.start() self.carrot_route_thread = threading.Thread(target=self.carrot_route, args=[]) self.carrot_route_thread.daemon = True self.carrot_route_thread.start() self.is_running = True threading.Thread(target=self.broadcast_version_info).start() self.navi_points = [] self.navi_points_start_index = 0 self.navi_points_active = False self.navd_active = False self.active_carrot_last = False self.is_metric = self.params.get_bool("IsMetric") def get_broadcast_address(self): if PC: iface = b'br0' else: iface = b'wlan0' try: with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s: ip = fcntl.ioctl( s.fileno(), 0x8919, struct.pack('256s', iface) )[20:24] return socket.inet_ntoa(ip) except (OSError, Exception): return None def get_local_ip(self): try: # 외부 서버와의 연결을 통해 로컬 IP 확인 with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s: s.connect(("8.8.8.8", 80)) # Google DNS로 연결 시도 return s.getsockname()[0] except Exception as e: return f"Error: {e}" # 브로드캐스트 메시지 전송 def broadcast_version_info(self): sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) frame = 0 self.save_toggle_values() carrot_speed = CarrotSpeed(neighbor_ring=2) self.params_memory.put_int_nonblocking("CarrotSpeed", 0) rk = Ratekeeper(20, print_delay_threshold=None) carrotIndex_last = self.carrot_serv.carrotIndex phone_gps_frame = self.carrot_serv.phone_gps_frame carrot_speed_active_count = 0 self.v_cruise_last = 0 self.long_active = False self.v_cruise_change = 0 self._last_vt = 0.0 self.gas_pressed_count = 0 self._last_viz_t = 0.0 while self.is_running: try: self.sm.update(0) if self.sm.updated['navRouteNavd']: self.send_routes(self.sm['navRouteNavd'].coordinates, True) remote_addr = self.remote_addr remote_ip = remote_addr[0] if remote_addr is not None else "" vturn_speed = self.carrot_curve_speed(self.sm) coords, distances, route_speed = self.carrot_navi_route() #print("coords=", coords) #print("curvatures=", curvatures) self.carrot_serv.update_navi(remote_ip, self.sm, self.pm, vturn_speed, coords, distances, route_speed, self.gps_location_service) if phone_gps_frame != self.carrot_serv.phone_gps_frame: phone_gps_frame = self.carrot_serv.phone_gps_frame carrot_speed_active_count = 10 else: carrot_speed_active_count -= 1 if carrot_speed_active_count > 0: self.carrot_speed_serv(carrot_speed, frame) if frame % 20 == 0 or remote_addr is not None: try: self.broadcast_ip = self.get_broadcast_address() if remote_addr is None else remote_addr[0] if not PC: ip_address = socket.gethostbyname(socket.gethostname()) else: ip_address = self.get_local_ip() if ip_address != self.ip_address: self.ip_address = ip_address self.remote_addr = None self.params_memory.put_nonblocking("NetworkAddress", self.ip_address) msg = self.make_send_message() if self.broadcast_ip is not None: dat = msg.encode('utf-8') sock.sendto(dat, (self.broadcast_ip, self.broadcast_port)) #for i in range(1, 255): # ip_tuple = socket.inet_aton(self.broadcast_ip) # new_ip = ip_tuple[:-1] + bytes([i]) # address = (socket.inet_ntoa(new_ip), self.broadcast_port) # sock.sendto(dat, address) if remote_addr is None: print(f"Broadcasting: {self.broadcast_ip}:{msg}") if not self.navd_active: #print("clear path_points: navd_active: ", self.navd_active) self.navi_points = [] self.navi_points_active = False except Exception as e: if self.connection: self.connection.close() self.connection = None print(f"##### broadcast_error...: {e}") traceback.print_exc() rk.keep_time() frame += 1 except Exception as e: print(f"broadcast_version_info error...: {e}") traceback.print_exc() time.sleep(1) def carrot_speed_serv(self, carrot_speed, frame): v_ego = a_ego = 0.0 gas_pressed = False if self.sm.alive['carState'] and self.sm.alive['carControl']: CS = self.sm['carState'] CC = self.sm['carControl'] v_ego = CS.vEgo a_ego = CS.aEgo gas_pressed = CS.gasPressed v_ego_kph = v_ego * 3.6 if gas_pressed: self.gas_pressed_count = 200 self.v_cruise_change = 0 elif self._last_vt == CS.vCruise: self.v_cruise_last = CS.vCruise elif self.long_active and CC.longActive: # and self.gas_pressed_count == 0: if self.v_cruise_last < CS.vCruise: # 속도가 증가하면 if v_ego_kph < CS.vCruise: self.v_cruise_change = 100 else: self.v_cruise_change = 0 elif self.v_cruise_last > CS.vCruise: # 속도가 감소하면 if v_ego_kph < CS.vCruise: # 주행속도가 느리면 self.v_cruise_change = -100 #100 else: # 주행속도가 빠르면 self.v_cruise_change = -100 if self.v_cruise_change != 0: self.gas_pressed_count = 0 else: self.v_cruise_change = 0 self.long_active = CC.longActive self.v_cruise_last = CS.vCruise else: self.v_cruise_change = 0 v_cruise_apply = max(min(CS.vCruise, v_ego_kph), 20) now = time.monotonic() heading = self.carrot_serv.bearing #nPosAnglePhone lat, lon = self.carrot_serv.vpPosPointLat, self.carrot_serv.vpPosPointLon #self.carrot_serv.estimate_position(self.carrot_serv.phone_latitude, self.carrot_serv.phone_longitude, heading, v_ego, now - self.carrot_serv.last_update_gps_time_phone) vt = carrot_speed.query_target_dist(lat, lon, heading, 0.0) if self.v_cruise_change != 0: carrot_speed.add_sample(lat, lon, heading, v_cruise_apply if self.v_cruise_change > 0 else (- v_cruise_apply)) if self.v_cruise_change > 0: self.v_cruise_change -= 1 if self.v_cruise_change < 0: self.v_cruise_change += 1 else: if self.gas_pressed_count > 0: vt = max(vt, v_cruise_apply) carrot_speed.add_sample(lat, lon, heading, vt) self.params_memory.put_int_nonblocking("CarrotSpeed", int(vt)) self._last_vt = vt if gas_pressed and a_ego < -0.5: #self._last_vt < 0.0: carrot_speed.invalidate_last_hit(window_s=2.0, action="clear") self.gas_pressed_count = max(0, self.gas_pressed_count - 1) if now - self._last_viz_t > 0.5: # 2Hz self._last_viz_t = now viz_json = carrot_speed.export_cells_around(lat, lon, heading, ring=2, max_points=64) # 메모리 Params에 쓰는 게 좋음 (디스크 말고) self.params_memory.put_nonblocking("CarrotSpeedViz", viz_json) carrot_speed.maybe_save() def carrot_navi_route(self): if self.carrot_serv.active_carrot > 1: if False and self.navd_active: # mabox always active self.navd_active = False self.params.remove("NavDestination") if not self.navi_points_active or not SHAPELY_AVAILABLE or (self.carrot_serv.active_carrot <= 1 and not self.navd_active): #print(f"navi_points_active: {self.navi_points_active}, active_carrot: {self.carrot_serv.active_carrot}") if self.navi_points_active: print("navi_points_active: ", self.navi_points_active, "active_carrot: ", self.carrot_serv.active_carrot, "navd_active: ", self.navd_active) #haversine_cache.clear() #curvature_cache.clear() self.navi_points = [] self.navi_points_active = False if self.active_carrot_last > 1: #self.params.remove("NavDestination") pass self.active_carrot_last = self.carrot_serv.active_carrot return [],[],300 current_position = (self.carrot_serv.vpPosPointLon, self.carrot_serv.vpPosPointLat) heading_deg = self.carrot_serv.bearing distance_interval = 10.0 out_speed = 300 path, self.navi_points_start_index, start_point = get_path_after_distance(self.navi_points_start_index, self.navi_points, current_position, 300) relative_coords = [] if path: #relative_coords = gps_to_relative_xy(path, current_position, heading_deg) relative_coords = gps_to_relative_xy(path, start_point, heading_deg) # Resample relative_coords at 5m intervals using LineString line = LineString(relative_coords) resampled_points = [] resampled_distances = [] current_distance = 0 while current_distance <= line.length: point = line.interpolate(current_distance) resampled_points.append((point.x, point.y)) resampled_distances.append(current_distance) current_distance += distance_interval curvatures = [] distances = [] distance = 10.0 sample = 4 if len(resampled_points) >= sample * 2 + 1: # Calculate curvatures and speeds based on curvature speeds = [] for i in range(len(resampled_points) - sample * 2): distance += distance_interval p1, p2, p3 = resampled_points[i], resampled_points[i + sample], resampled_points[i + sample * 2] curvature = calculate_curvature(p1, p2, p3) curvatures.append(curvature) speed = np.interp(abs(curvature), V_CURVE_LOOKUP_BP, V_CRUVE_LOOKUP_VALS) if abs(curvature) < 0.02: speed = max(speed, self.carrot_serv.nRoadLimitSpeed) speeds.append(speed) distances.append(distance) #print(f"curvatures= {[round(s, 4) for s in curvatures]}") #print(f"speeds= {[round(s, 1) for s in speeds]}") # Apply acceleration limits in reverse to adjust speeds accel_limit = self.carrot_serv.autoNaviSpeedDecelRate # m/s^2 accel_limit_kmh = accel_limit * 3.6 # Convert to km/h per second out_speeds = [0] * len(speeds) out_speeds[-1] = speeds[-1] # Set the last speed as the initial value v_ego_kph = self.sm['carState'].vEgo * 3.6 time_delay = self.carrot_serv.autoNaviSpeedCtrlEnd time_wait = 0 for i in range(len(speeds) - 2, -1, -1): target_speed = speeds[i] next_out_speed = out_speeds[i + 1] if target_speed < next_out_speed: time_delay = max(0, ((v_ego_kph - target_speed) / accel_limit_kmh)) time_wait = - time_delay # Calculate time interval for the current segment based on speed time_interval = distance_interval / (next_out_speed / 3.6) if next_out_speed > 0 else 0 time_apply = min(time_interval, max(0, time_interval + time_wait)) # Calculate maximum allowed speed with acceleration limit max_allowed_speed = next_out_speed + (accel_limit_kmh * time_apply) adjusted_speed = min(target_speed, max_allowed_speed) #time_wait += time_interval time_wait += min(2.0, time_interval) out_speeds[i] = adjusted_speed #distance_advance = self.sm['carState'].vEgo * 3.0 # Advance distance by 3.0 seconds #out_speed = interp(distance_advance, distances, out_speeds) out_speed = out_speeds[0] #print(f"out_speeds= {[round(s, 1) for s in out_speeds]}") else: resampled_points = [] resampled_distances = [] curvatures = [] speeds = [] distances = [] #self.params.remove("NavDestination") return resampled_points, resampled_distances, out_speed #speeds, distances def make_send_message(self): msg = {} msg['Carrot2'] = self.params.get("Version").decode('utf-8') isOnroad = self.params.get_bool("IsOnroad") msg['IsOnroad'] = isOnroad msg['CarrotRouteActive'] = self.navi_points_active msg['ip'] = self.ip_address msg['port'] = self.carrot_man_port self.controls_active = False self.xState = 0 self.trafficState = 0 v_ego_kph = 0 log_carrot = "" v_cruise_kph = 0 carcruiseSpeed = 0 if not isOnroad: self.xState = 0 self.trafficState = 0 else: if self.sm.alive['carState']: carState = self.sm['carState'] v_ego_kph = int(carState.vEgoCluster * 3.6 + 0.5) log_carrot = carState.logCarrot v_cruise_kph = carState.vCruise carcruiseSpeed = carState.cruiseState.speed * 3.6 if self.sm.alive['selfdriveState']: selfdrive = self.sm['selfdriveState'] self.controls_active = selfdrive.active if self.sm.alive['longitudinalPlan']: lp = self.sm['longitudinalPlan'] self.xState = lp.xState self.trafficState = lp.trafficState msg['log_carrot'] = log_carrot msg['v_cruise_kph'] = v_cruise_kph msg['carcruiseSpeed'] = carcruiseSpeed msg['v_ego_kph'] = v_ego_kph msg['tbt_dist'] = self.carrot_serv.xDistToTurn msg['sdi_dist'] = self.carrot_serv.xSpdDist msg['active'] = self.controls_active msg['xState'] = self.xState msg['trafficState'] = self.trafficState return json.dumps(msg) def receive_fixed_length_data(self, sock, length): buffer = b"" while len(buffer) < length: data = sock.recv(length - len(buffer)) if not data: raise ConnectionError("Connection closed before receiving all data") buffer += data return buffer def carrot_man_thread(self): while True: try: with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: sock.settimeout(10) # 소켓 타임아웃 설정 (10초) sock.bind(('0.0.0.0', self.carrot_man_port)) # UDP 포트 바인딩 print("#########carrot_man_thread: UDP thread started...") while True: try: #self.remote_addr = None # 데이터 수신 (UDP는 recvfrom 사용) try: data, remote_addr = sock.recvfrom(4096) # 최대 4096 바이트 수신 #print(f"Received data from {self.remote_addr}") if not data: raise ConnectionError("No data received") if self.remote_addr is None: print("Connected to: ", remote_addr) self.remote_addr = remote_addr try: json_obj = json.loads(data.decode()) self.carrot_serv.update(json_obj) except Exception as e: print(f"carrot_man_thread: json error...: {e}") print(data) # 응답 메시지 생성 및 송신 (UDP는 sendto 사용) #try: # msg = self.make_send_message() # sock.sendto(msg.encode('utf-8'), self.remote_addr) #except Exception as e: # print(f"carrot_man_thread: send error...: {e}") except TimeoutError: print("Waiting for data (timeout)...") self.remote_addr = None time.sleep(1) except Exception as e: print(f"carrot_man_thread: error...: {e}") self.remote_addr = None break except Exception as e: print(f"carrot_man_thread: recv error...: {e}") self.remote_addr = None break time.sleep(1) except Exception as e: self.remote_addr = None print(f"Network error, retrying...: {e}") time.sleep(2) def parse_kisa_data(self, data: bytes): result = {} try: decoded = data.decode('utf-8') except UnicodeDecodeError: print("Decoding error:", data) return result parts = decoded.split('/') for part in parts: if ':' in part: key, value = part.split(':', 1) try: result[key] = int(value) except ValueError: result[key] = value return result def kisa_app_thread(self): while True: try: with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) sock.settimeout(10) # 소켓 타임아웃 설정 (10초) sock.bind(('', 12345)) # UDP 포트 바인딩 print("#########kisa_app_thread: UDP thread started...") while True: try: #self.remote_addr = None # 데이터 수신 (UDP는 recvfrom 사용) try: data, remote_addr = sock.recvfrom(4096) # 최대 4096 바이트 수신 #print(f"Received data from {self.remote_addr}") if not data: raise ConnectionError("No data received") #if self.remote_addr is None: # print("Connected to: ", remote_addr) #self.remote_addr = remote_addr try: print(data) kisa_data = self.parse_kisa_data(data) self.carrot_serv.update_kisa(kisa_data) #json_obj = json.loads(data.decode()) #print(json_obj) except Exception as e: traceback.print_exc() print(f"kisa_app_thread: json error...: {e}") print(data) except TimeoutError: print("Waiting for data (timeout)...") #self.remote_addr = None time.sleep(1) except Exception as e: print(f"kisa_app_thread: error...: {e}") #self.remote_addr = None break except Exception as e: print(f"kisa_app_thread: recv error...: {e}") #self.remote_addr = None break time.sleep(1) except Exception as e: #self.remote_addr = None print(f"Network error, retrying...: {e}") time.sleep(2) def make_tmux_data(self): try: subprocess.run("rm /data/media/tmux.log; tmux capture-pane -pq -S-1000 > /data/media/tmux.log", shell=True, capture_output=True, text=False) subprocess.run("/data/openpilot/selfdrive/apilot.py", shell=True, capture_output=True, text=False) except Exception as e: print(f"TMUX creation error: {e}") return def send_tmux(self, ftp_password, tmux_why, send_settings=False): ftp_server = "shind0.synology.me" ftp_port = 8021 ftp_username = "carrotpilot" ftp = FTP() ftp.connect(ftp_server, ftp_port) ftp.login(ftp_username, ftp_password) car_selected = Params().get("CarName") if car_selected is None: car_selected = "none" else: car_selected = car_selected.decode('utf-8') git_branch = Params().get("GitBranch").decode('utf-8') try: ftp.mkd(git_branch) except Exception as e: print(f"Directory creation failed: {e}") ftp.cwd(git_branch) directory = car_selected + " " + Params().get("DongleId").decode('utf-8') current_time = datetime.now().strftime("%Y%m%d-%H%M%S") filename = tmux_why + "-" + current_time + "-" + git_branch + ".txt" try: ftp.mkd(directory) except Exception as e: print(f"Directory creation failed: {e}") ftp.cwd(directory) try: with open("/data/media/tmux.log", "rb") as file: ftp.storbinary(f'STOR {filename}', file) except Exception as e: print(f"ftp sending error...: {e}") if send_settings: self.save_toggle_values() try: #with open("/data/backup_params.json", "rb") as file: with open("/data/toggle_values.json", "rb") as file: ftp.storbinary(f'STOR toggles-{current_time}.json', file) except Exception as e: print(f"ftp params sending error...: {e}") ftp.quit() def carrot_panda_debug(self): #time.sleep(2) while True: if self.show_panda_debug: self.show_panda_debug = False try: subprocess.run("/data/openpilot/selfdrive/debug/debug_console_carrot.py", shell=True) except Exception as e: print(f"debug_console error: {e}") time.sleep(2) else: time.sleep(1) def save_toggle_values(self): try: import openpilot.selfdrive.frogpilot.fleetmanager.helpers as fleet toggle_values = fleet.get_all_toggle_values() file_path = os.path.join('/data', 'toggle_values.json') with open(file_path, 'w') as file: json.dump(toggle_values, file, indent=2) except Exception as e: print(f"save_toggle_values error: {e}") def carrot_cmd_zmq(self): context = zmq.Context() def setup_socket(): socket = context.socket(zmq.REP) socket.bind("tcp://*:7710") poller = zmq.Poller() poller.register(socket, zmq.POLLIN) return socket, poller socket, poller = setup_socket() isOnroadCount = 0 is_tmux_sent = False print("#########carrot_cmd_zmq: thread started...") while True: try: socks = dict(poller.poll(100)) if socket in socks and socks[socket] == zmq.POLLIN: message = socket.recv(zmq.NOBLOCK) print(f"Received:7710 request: {message}") json_obj = json.loads(message.decode()) else: json_obj = None if json_obj is None: isOnroadCount = isOnroadCount + 1 if self.params.get_bool("IsOnroad") else 0 if isOnroadCount == 0: is_tmux_sent = False if isOnroadCount == 1: self.show_panda_debug = True network_type = self.sm['deviceState'].networkType # if not force_wifi else NetworkType.wifi networkConnected = False if network_type == NetworkType.none else True if isOnroadCount == 500: self.make_tmux_data() if isOnroadCount > 500 and not is_tmux_sent and networkConnected: self.send_tmux("Ekdrmsvkdlffjt7710", "onroad", send_settings = True) is_tmux_sent = True if self.params.get_bool("CarrotException") and networkConnected: self.params.put_bool("CarrotException", False) self.make_tmux_data() self.send_tmux("Ekdrmsvkdlffjt7710", "exception") elif 'echo_cmd' in json_obj: try: result = subprocess.run(json_obj['echo_cmd'], shell=True, capture_output=True, text=False) exitStatus = result.returncode try: stdout = result.stdout.decode('utf-8') stderr = result.stderr.decode('utf-8') except UnicodeDecodeError: stdout = result.stdout.decode('euc-kr', 'ignore') stderr = result.stderr.decode('euc-kr', 'ignore') echo = json.dumps({"echo_cmd": json_obj['echo_cmd'], "exitStatus": exitStatus, "result": stdout, "error": stderr}) except Exception as e: echo = json.dumps({"echo_cmd": json_obj['echo_cmd'], "exitStatus": exitStatus, "result": "", "error": f"exception error: {str(e)}"}) #print(echo) socket.send(echo.encode()) elif 'tmux_send' in json_obj: self.make_tmux_data() self.send_tmux(json_obj['tmux_send'], "tmux_send") echo = json.dumps({"tmux_send": json_obj['tmux_send'], "result": "success"}) socket.send(echo.encode()) except Exception as e: print(f"carrot_cmd_zmq error: {e}") socket.close() time.sleep(1) socket, poller = setup_socket() def recvall(self, sock, n): """n바이트를 수신할 때까지 반복적으로 데이터를 받는 함수""" data = bytearray() while len(data) < n: packet = sock.recv(n - len(data)) if not packet: return None data.extend(packet) return data def receive_double(self, sock): double_data = self.recvall(sock, 8) # Double은 8바이트 return struct.unpack('!d', double_data)[0] def receive_float(self, sock): float_data = self.recvall(sock, 4) # Float은 4바이트 return struct.unpack('!f', float_data)[0] def send_routes(self, coords, from_navd=False): if from_navd: if len(coords) > 0: self.navi_points = [(c.longitude, c.latitude) for c in coords] self.navi_points_start_index = 0 self.navi_points_active = True print("Received points from navd:", len(self.navi_points)) self.navd_active = True # 경로수신 -> carrotman active되고 약간의 시간지연이 발생함.. if not from_navd: self.carrot_serv.active_count = 80 self.carrot_serv.active_sdi_count = self.carrot_serv.active_sdi_count_max self.carrot_serv.active_carrot = 2 coords = [{"latitude": c.latitude, "longitude": c.longitude} for c in coords] #print("navdNaviPoints=", self.navi_points) else: print("Received points from navd: 0") self.navd_active = False msg = messaging.new_message('navRoute', valid=True) msg.navRoute.coordinates = coords self.pm.send('navRoute', msg) def carrot_route(self): host = '0.0.0.0' # 혹은 다른 호스트 주소 port = 7709 # 포트 번호 try: with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: s.bind((host, port)) s.listen() while True: print("################# waiting connection from CarrotMan route #####################") conn, addr = s.accept() with conn: print(f"Connected by {addr}") #self.clear_route() # 전체 데이터 크기 수신 total_size_bytes = self.recvall(conn, 4) if not total_size_bytes: print("Connection closed or error occurred") continue try: total_size = struct.unpack('!I', total_size_bytes)[0] # 전체 데이터를 한 번에 수신 all_data = self.recvall(conn, total_size) if all_data is None: print("Connection closed or incomplete data received") continue self.navi_points = [] points = [] for i in range(0, len(all_data), 8): x, y = struct.unpack('!ff', all_data[i:i+8]) self.navi_points.append((x, y)) coord = Coordinate.from_mapbox_tuple((x, y)) points.append(coord) coords = [c.as_dict() for c in points] self.navi_points_start_index = 0 self.navi_points_active = True print("Received points:", len(self.navi_points)) #print("Received points:", self.navi_points) self.send_routes(coords) """ try: module_name = "route_engine" class_name = "RouteEngine" moduel = importlib.import_module(module_name) cls = getattr(moduel, class_name) route_engine_instance = cls(name="Loaded at Runtime") route_engine_instance.send_route_coords(coords, True) except Exception as e: print(f"route_engine error: {e}") #msg = messaging.new_message('navRoute', valid=True) #msg.navRoute.coordinates = coords #self.pm.send('navRoute', msg) """ if len(coords): dest = coords[-1] dest['place_name'] = "External Navi" self.params.put("NavDestination", json.dumps(dest)) except Exception as e: print(e) except Exception as e: print("################# CarrotMan route server error #####################") print(e) def carrot_curve_speed_params(self): self.autoCurveSpeedFactor = self.params.get_int("AutoCurveSpeedFactor")*0.01 self.autoCurveSpeedAggressiveness = self.params.get_int("AutoCurveSpeedAggressiveness")*0.01 def carrot_curve_speed(self, sm): self.carrot_curve_speed_params() if not sm.alive['carState'] and not sm.alive['modelV2']: return 250 #print(len(sm['modelV2'].orientationRate.z)) if len(sm['modelV2'].orientationRate.z) == 0: return 250 return self.vturn_speed(sm['carState'], sm) def vturn_speed(self, CS, sm): TARGET_LAT_A = 1.9 # m/s^2 modelData = sm['modelV2'] v_ego = max(CS.vEgo, 0.1) # Set the curve sensitivity orientation_rate = np.array(modelData.orientationRate.z) * self.autoCurveSpeedFactor velocity = np.array(modelData.velocity.x) # Get the maximum lat accel from the model max_index = np.argmax(np.abs(orientation_rate)) curv_direction = np.sign(orientation_rate[max_index]) max_pred_lat_acc = np.amax(np.abs(orientation_rate) * velocity) # Get the maximum curve based on the current velocity max_curve = max_pred_lat_acc / (v_ego**2) # Set the target lateral acceleration adjusted_target_lat_a = TARGET_LAT_A * self.autoCurveSpeedAggressiveness # Get the target velocity for the maximum curve #turnSpeed = max(abs(adjusted_target_lat_a / max_curve)**0.5 * 3.6, self.autoCurveSpeedLowerLimit) turnSpeed = max(abs(adjusted_target_lat_a / max_curve)**0.5 * 3.6, 5) turnSpeed = min(turnSpeed, 250) return turnSpeed * curv_direction import traceback def main(): print("CarrotManager Started") #print("Carrot GitBranch = {}, {}".format(Params().get("GitBranch"), Params().get("GitCommitDate"))) carrot_man = CarrotMan() print(f"CarrotMan {carrot_man}") threading.Thread(target=carrot_man.kisa_app_thread).start() while True: try: carrot_man.carrot_man_thread() except Exception as e: print(f"carrot_man error...: {e}") traceback.print_exc() time.sleep(10) if __name__ == "__main__": main()