From 99ae42f5b9cf4ffdd95a36006517e6bae3258a36 Mon Sep 17 00:00:00 2001 From: robin Date: Wed, 26 Aug 2026 15:23:06 +0900 Subject: [PATCH] Switch to Gunsan 2.5D map, add terrain-aware speed control and collision monitor, port motor control to C++ - Port serial_bridge_node's low-level Modbus RTU transport to C++ (fori_serial_bridge_cpp) to remove Python control-loop delay, keeping the existing control algorithm/parameters unchanged - Switch Nav2 and mock/patrol map to the Gunsan 2.5D site (pcd_gunsan_output), with terrain_speed_node scaling drive speed up on uphill grades and down on downhill grades for both Nav2 and patrol - Unify Nav2 cruise speed and motor safety-clamp speed at 0.3 m/s - Add nav2_collision_monitor as a final safety gate against sudden dynamic obstacles, routed so it can't be bypassed by velocity_smoother's internal remap - Fix web dashboard map rendering (stray map_server process, oversized GetMap payload, rosbridge fragment size vs. roslib.js incompatibility) and PWA reachability via mDNS - Fix TF tree: wheel-odometry broadcasts odom->base_link, and camera_init is statically bridged to both odom and map so AMCL's own map->odom broadcast no longer conflicts with the SLAM anchor frame --- src/FAST_LIO | 2 +- src/fori_collision_monitor_params.yaml | 59 + src/fori_nav2.launch.py | 122 +- src/fori_nav2_params.yaml | 24 +- .../parking_controller_node.cpython-310.pyc | Bin 23156 -> 24126 bytes .../serial_bridge_node.cpython-310.pyc | Bin 13933 -> 14536 bytes .../ui_server_node.cpython-310.pyc | Bin 2846 -> 3715 bytes .../fori_serial_bridge/odom_relay_node.py | 42 + .../parking_controller_node.py | 50 +- .../fori_serial_bridge/serial_bridge_node.py | 86 +- .../fori_serial_bridge/terrain_speed_node.py | 228 +++ .../fori_serial_bridge/ui_server_node.py | 35 +- .../launch/fori_full.launch.py | 81 +- src/fori_serial_bridge/setup.py | 6 +- src/fori_serial_bridge/ui/app.js | 379 ++++- src/fori_serial_bridge/ui/index.html | 464 ++++-- src/fori_serial_bridge/ui/manifest.json | 18 + src/fori_serial_bridge/ui/style.css | 1379 ++++++++++++----- src/fori_serial_bridge/ui/sw.js | 36 + src/fori_serial_bridge_cpp/CMakeLists.txt | 42 + .../fori_serial_bridge_cpp/modbus_serial.hpp | 43 + src/fori_serial_bridge_cpp/package.xml | 22 + .../src/modbus_serial.cpp | 230 +++ .../src/serial_bridge_node.cpp | 567 +++++++ start_fori.sh | 37 + 25 files changed, 3237 insertions(+), 715 deletions(-) create mode 100644 src/fori_collision_monitor_params.yaml create mode 100644 src/fori_serial_bridge/fori_serial_bridge/odom_relay_node.py create mode 100644 src/fori_serial_bridge/fori_serial_bridge/terrain_speed_node.py create mode 100644 src/fori_serial_bridge/ui/manifest.json create mode 100644 src/fori_serial_bridge/ui/sw.js create mode 100644 src/fori_serial_bridge_cpp/CMakeLists.txt create mode 100644 src/fori_serial_bridge_cpp/include/fori_serial_bridge_cpp/modbus_serial.hpp create mode 100644 src/fori_serial_bridge_cpp/package.xml create mode 100644 src/fori_serial_bridge_cpp/src/modbus_serial.cpp create mode 100644 src/fori_serial_bridge_cpp/src/serial_bridge_node.cpp create mode 100755 start_fori.sh diff --git a/src/FAST_LIO b/src/FAST_LIO index 35d558a..5547e51 120000 --- a/src/FAST_LIO +++ b/src/FAST_LIO @@ -1 +1 @@ -/home/yoo/FAST_LIO \ No newline at end of file +/home/yoo/duru_lio_ws \ No newline at end of file diff --git a/src/fori_collision_monitor_params.yaml b/src/fori_collision_monitor_params.yaml new file mode 100644 index 0000000..0ee34fe --- /dev/null +++ b/src/fori_collision_monitor_params.yaml @@ -0,0 +1,59 @@ +# Nav2 Collision Monitor: 마지막 안전 게이트. +# +# terrain_speed_node가 낸 /cmd_vel_terrain(Nav2 순찰/자율주행 + parking_controller의 +# 도킹/순찰 명령 모두 여기로 합쳐짐)을 받아서, /scan 라이다 데이터를 기준으로 갑자기 +# 튀어나온 사람/장애물이 있으면 감속하거나(SlowZone) 완전 정지시킨(StopZone) 뒤 최종 +# /cmd_vel로 내보낸다. FootprintApproach는 현재 속도 명령대로 주행했을 때 +# time_before_collision초 안에 로봇 발자국(costmap footprint)이 장애물과 닿는지 +# 시뮬레이션해서 미리 감속하는 예측형 정지 로직이다. +# +# 로봇 사양(fori_nav2_params.yaml의 robot_radius: 0.38m, 순항속도 0.3m/s)에 맞춰 +# 반경/시간을 잡았다. 실제 부지에서 저속 시운전하며 StopZone/SlowZone 반경과 +# slowdown_ratio를 조정할 것을 권장. +collision_monitor: + ros__parameters: + use_sim_time: False + base_frame_id: "base_link" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_terrain" + cmd_vel_out_topic: "cmd_vel" + transform_tolerance: 0.5 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + polygons: ["StopZone", "SlowZone", "FootprintApproach"] + # 로봇 중심에서 0.45m(로봇 반경 0.38m + 여유 0.07m) 이내에 장애물이 잡히면 완전 정지. + StopZone: + type: "circle" + radius: 0.45 + action_type: "stop" + max_points: 3 + visualize: True + polygon_pub_topic: "collision_monitor/stop_zone" + enabled: True + # 0.90m 이내로 접근하면 지령 속도를 40%로 줄여 미리 서행. + SlowZone: + type: "circle" + radius: 0.90 + action_type: "slowdown" + slowdown_ratio: 0.4 + max_points: 3 + visualize: True + polygon_pub_topic: "collision_monitor/slow_zone" + enabled: True + # 현재 속도 명령으로 1.5초 앞을 시뮬레이션해서 로봇 발자국이 장애물과 겹치면 + # 미리 감속 (local_costmap이 robot_radius 기준으로 자동 발행하는 발자국 사용). + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 1.5 + simulation_time_step: 0.1 + max_points: 3 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "/scan" + enabled: True diff --git a/src/fori_nav2.launch.py b/src/fori_nav2.launch.py index 5162bd8..7247755 100644 --- a/src/fori_nav2.launch.py +++ b/src/fori_nav2.launch.py @@ -1,21 +1,32 @@ import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import GroupAction, IncludeLaunchDescription, SetEnvironmentVariable from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node +from launch_ros.actions import Node, SetRemap def generate_launch_description(): # --- [경로 설정 - 사용자 환경에 맞춰 수정] --- - # 1. 2D 지도 파일 경로 (데스크탑에 있는 파일 기준) - map_yaml_file = '/home/yoo/fori_map.yaml' + # 1. 2D 지도 파일 경로 (군산 부지 PCD -> 2.5D 변환 결과, pcd_gridmap_converter 산출물) + map_yaml_file = '/home/yoo/pcd_gunsan_output/map.yaml' # 2. Nav2 파라미터 파일 경로 - nav2_params_file = '/home/yoo/fori_ws/src/fori_nav2_params.yaml' + nav2_params_file = os.path.expanduser('~/fori_ws/fori_ws/src/fori_nav2_params.yaml') + if not os.path.exists(nav2_params_file): + nav2_params_file = os.path.expanduser('~/fori_ws/src/fori_nav2_params.yaml') + + # 2b. Collision Monitor 파라미터 파일 경로 (급출현 장애물 감속/정지 안전계층) + collision_monitor_params_file = os.path.expanduser( + '~/fori_ws/fori_ws/src/fori_collision_monitor_params.yaml') + if not os.path.exists(collision_monitor_params_file): + collision_monitor_params_file = os.path.expanduser( + '~/fori_ws/src/fori_collision_monitor_params.yaml') # 3. URDF 파일 경로 (분석된 full workspace 내 경로) - urdf_file_path = os.path.expanduser('~/fori_ws/src/FAST-LIVO2/urdf/fori_robot.urdf') + urdf_file_path = os.path.expanduser('~/fori_ws/fori_ws/src/FAST-LIVO2/urdf/fori_robot.urdf') + if not os.path.exists(urdf_file_path): + urdf_file_path = os.path.expanduser('~/fori_ws/src/FAST-LIVO2/urdf/fori_robot.urdf') # 패키지 경로 획득 nav2_bringup_dir = get_package_share_directory('nav2_bringup') @@ -34,6 +45,46 @@ def generate_launch_description(): output='screen' ) + # 2단계: Static TF for camera_init -> odom (FAST-LIO2/FAST_LIO uses camera_init, Nav2 uses odom) + static_tf_camera_init_node = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='static_tf_camera_init_to_odom', + arguments=['0', '0', '0', '0', '0', '0', 'camera_init', 'odom'], + output='screen' + ) + + # 2b단계: Static TF for camera_init -> map. AMCL이 표준대로 map->odom을 발행하면 + # odom의 부모를 camera_init(위 정적 브릿지)과 map(AMCL)이 동시에 주장하게 돼 tf2 + # 트리가 깨진다(실측으로 base_link<->odom "unconnected trees" 확인함). 그래서 + # AMCL은 tf_broadcast:false로 TF 발행을 끄고(fori_nav2_params.yaml), 대신 map도 + # odom과 마찬가지로 camera_init의 자식으로 정적 연결한다 - map과 odom은 형제라 충돌이 + # 없다. AMCL은 여전히 /amcl_pose(스캔 매칭 기반 정밀 위치)는 정상 발행하므로 + # terrain_speed_node/parking_controller_node의 위치 소스로는 계속 쓸 수 있다. + static_tf_camera_init_to_map_node = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='static_tf_camera_init_to_map', + arguments=['0', '0', '0', '0', '0', '0', 'camera_init', 'map'], + output='screen' + ) + + # 3단계-B: FAST_LIO (카메라 불필요, LiDAR+IMU만 사용하는 LIO). FAST-LIVO2와 달리 + # 카메라 연결 없이도 /cloud_registered + camera_init->aft_mapped TF를 발행해서 + # AMCL이 map->odom을 낼 수 있게 해준다(child_frame_id를 URDF 루트 이름인 + # "aft_mapped"에 맞춰 FAST_LIO/src/laserMapping.cpp를 수정해서 씀 - 원본 FAST_LIO는 + # "body"를 써서 그대로 두면 TF 트리가 끊긴다). 라이다 드라이버 + # (livox_ros_driver2 mid360s_fastlivo_launch.py)는 별도 터미널에서 먼저 띄워야 한다. + fast_lio_dir = get_package_share_directory('fast_lio') + fast_lio_config = os.path.join(fast_lio_dir, 'config', 'mid360s.yaml') + fast_lio_node = Node( + package='fast_lio', + executable='fastlio_mapping', + name='fastlio_mapping', + parameters=[fast_lio_config, {'use_sim_time': False}], + output='screen' + ) + # 3단계-A: 3D PointCloud -> 2D Scan 변환 pc_to_ls_node = Node( package='pointcloud_to_laserscan', @@ -62,8 +113,65 @@ def generate_launch_description(): }.items() ) + # Nav2 humble bringup은 controller_server가 낸 cmd_vel을 velocity_smoother가 받아 + # 감속 램프를 적용한 뒤 'cmd_vel_smoothed'라는 내부 이름으로 최종 'cmd_vel'에 발행한다 + # (navigation_launch.py에 하드코딩된 리맵: controller_server의 ('cmd_vel','cmd_vel_nav'), + # velocity_smoother의 ('cmd_vel','cmd_vel_nav')+('cmd_vel_smoothed','cmd_vel')). 즉 + # 실제로 모터까지 가는 "진짜 최종" 순찰/자율주행 명령은 velocity_smoother의 출력이다. + # 바깥 이름 'cmd_vel'을 통째로 가로채면(처음 시도했던 방식) velocity_smoother 내부의 + # 'cmd_vel'->'cmd_vel_nav' 입력 리맵까지 덮어써서 오히려 스무딩 단계를 건너뛰게 되고, + # 정작 진짜 최종 출력('cmd_vel_smoothed'->'cmd_vel')은 그대로 남아 terrain_speed_node/ + # collision_monitor를 완전히 우회해버린다(실측으로 확인함). 그래서 velocity_smoother의 + # 내부 입력 리맵은 건드리지 않고, 그 최종 출력 이름('cmd_vel_smoothed')만 정확히 + # /cmd_vel_raw로 리다이렉트한다. + # + # [알려진 한계] behavior_server(spin/backup/drive_on_heading 복구 동작)는 + # navigation_launch.py에서 cmd_vel 리맵이 없어 velocity_smoother를 거치지 않고 bare + # 'cmd_vel'에 직접 낸다 - 리맵 스코프가 GroupAction 단위라 controller_server의 + # cmd_vel->cmd_vel_nav 페어링을 건드리지 않으면서 behavior_server만 선택적으로 + # 가로챌 방법이 없다(nav2_bringup 내부 launch 파일을 포크하지 않는 한). 즉 복구 동작 + # 중에는 terrain_speed_node의 경사 보정과 collision_monitor의 급출현 장애물 감속/정지가 + # 적용되지 않는다. 다만 (1) 복구는 costmap이 이미 경로 막힘을 감지한 뒤에만 트리거되고 + # (2) 각 복구 동작 자체가 저속·단거리로 제한돼 있고 (3) serial_bridge_node의 + # target_linear_speed(0.3m/s) 하드 클램프는 경로와 무관하게 항상 적용되므로, 남은 + # 위험은 제한적이다. + nav2_launch_group = GroupAction([ + SetRemap('cmd_vel_smoothed', 'cmd_vel_raw'), + nav2_launch, + ]) + + # 5단계: Collision Monitor (최종 안전 게이트) - /cmd_vel_terrain(=terrain_speed_node + # 출력, Nav2 순찰/자율주행 + parking_controller 도킹/순찰 명령이 모두 여기로 합쳐짐)을 + # 받아 /scan 기준으로 급출현 장애물을 감속/정지시킨 뒤 최종 /cmd_vel로 발행한다. + # Nav2 자체 lifecycle_manager가 관리하는 노드 목록에는 없으므로 별도 lifecycle_manager로 + # 직접 configure/activate 시킨다. + collision_monitor_node = Node( + package='nav2_collision_monitor', + executable='collision_monitor', + name='collision_monitor', + parameters=[collision_monitor_params_file], + output='screen' + ) + + collision_monitor_lifecycle_manager = Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_collision_monitor', + parameters=[{ + 'use_sim_time': False, + 'autostart': True, + 'node_names': ['collision_monitor'] + }], + output='screen' + ) + return LaunchDescription([ rsp_node, + static_tf_camera_init_node, + static_tf_camera_init_to_map_node, + fast_lio_node, pc_to_ls_node, - nav2_launch + nav2_launch_group, + collision_monitor_node, + collision_monitor_lifecycle_manager, ]) diff --git a/src/fori_nav2_params.yaml b/src/fori_nav2_params.yaml index bb2bd7e..9bb0bb2 100644 --- a/src/fori_nav2_params.yaml +++ b/src/fori_nav2_params.yaml @@ -1,6 +1,6 @@ map_server: ros__parameters: - yaml_filename: "/home/yoo/fori_map.yaml" + yaml_filename: "/home/yoo/pcd_gunsan_output/map.yaml" use_sim_time: False amcl: ros__parameters: @@ -12,16 +12,23 @@ amcl: alpha4: 0.2 base_frame_id: "base_link" global_frame_id: "map" - odom_frame_id: "camera_init" # FAST-LIO2의 오도메트리 프레임 + odom_frame_id: "odom" scan_topic: "scan" map_topic: "map" set_initial_pose: true + # fast_lio(LIDAR SLAM)가 camera_init->aft_mapped->base_link TF를 이미 발행하고, + # camera_init은 fori_nav2.launch.py의 static_tf_camera_init_node가 odom과 동일시해서 + # 브릿지한다. AMCL이 표준대로 map->odom을 또 발행하면 odom의 부모를 두 군데(camera_init + # 정적 브릿지 vs AMCL의 map->odom)에서 동시에 주장하게 돼 tf2가 base_link<->odom + # 연결을 못 찾는 "unconnected trees" 오류로 이어진다. tf_broadcast를 꺼서 AMCL은 + # /amcl_pose(위치 추정값)만 내고 TF는 발행하지 않게 한다. + tf_broadcast: false behavior_server: ros__parameters: local_frame: base_link global_frame: map - odom_frame: camera_init # <--- 이 부분이 odom 에러를 해결합니다. + odom_frame: odom use_sim_time: False device_id: "robot" simulate_ahead_time: 2.0 @@ -31,7 +38,12 @@ bt_navigator: use_sim_time: False global_frame: map robot_base_frame: base_link - odom_topic: /aft_mapped_to_init # FAST-LIO2의 위치 토픽 + odom_topic: /odom + default_nav_to_pose_bt_xml: "" # Use Nav2 built-in default BehaviorTree + default_nav_through_poses_bt_xml: "" # Use Nav2 built-in default BehaviorTree + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 controller_server: ros__parameters: @@ -50,7 +62,7 @@ controller_server: yaw_goal_tolerance: 0.35 FollowPath: plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" - desired_linear_vel: 0.25 + desired_linear_vel: 0.3 lookahead_dist: 0.6 min_lookahead_dist: 0.3 max_lookahead_dist: 0.9 @@ -65,7 +77,7 @@ local_costmap: ros__parameters: update_frequency: 5.0 publish_frequency: 2.0 - global_frame: camera_init # 지역 지도는 오도메트리 기준 + global_frame: odom robot_base_frame: base_link use_sim_time: False transform_tolerance: 1.0 diff --git a/src/fori_serial_bridge/fori_serial_bridge/__pycache__/parking_controller_node.cpython-310.pyc b/src/fori_serial_bridge/fori_serial_bridge/__pycache__/parking_controller_node.cpython-310.pyc index 3be71fdafb9f8640a6181eab0e9aed94ca098209..059a49cff650ce2c5a60f029696ee1bc6f9b9742 100644 GIT binary patch delta 6255 zcmb7|3wTt=b;s}B7poOo39)(t;tHY1iiaeGL81mBKwv=PAz?6JyuR#Ri506|dG1Oe zMe0&pPI={X@EAXcj3rELlQ=e1&Sz+C*Tr?H^BKo!Qm4I68vAox8*6Fo)O|UP-SeL- zcEvW$_ht3n-_D#lb7ty+jm(j3 z=nYs3s3*R)oIEK}CjA0Q%N@3k%CdCNap>!@OEl#4QE9}Z3mO*tOK7f+V+1l9Xy*lB~Z;Qc02xlO!9pc`*_-M_UqxHfi%ovU!rER9irj zEt9%!)fSSZ)s$%2T2-j6YulhzmZG<7)dSYf?a>{bcW5==unyXU3wDZ0Z`Br2uU!K+ zj76F`q_ta%)qcY=Xcr21YKuu>&wyPh+%+IYecCPeVe#%3W>nJJt1TfN|3qXUx-@Mm zqKn$hTHQ^x`kQJ(QdG6$lq{W<~C<-tY?-iNxQc+Zfo7#yhrI)qbw5g z9Z>taBf)S~SFRsFKK}SiY;^WOPzD%?Xuz78fj|{go5hcU?>>v+Y$){{e9Q$wD)+ehFuGENB0@&EU93) zv>rC0-0!MVyZj+vcSH|(Eo?D$VoM0TEA1&cz&)-PDix|4cBY0q0){md2pbNyyE_op z3|B`W>I+3WIs(kF9Z*@AzvpU~E63KQyDg4tSgPT>Gk$fuk;eR??!JZhM0B&85+`hTFaohUlCc_<4^{7uwEE?Wf6G+9NFJ zJD?}N_x1!snpx8WEU1Qjds$HH2nZ2hkXh+kM)S<1M}LQ_ndOSjIaPMc4mnrOCOYM* z^ymDY<&eYSlr6IDD?XAL%AIPn$@p`8>9C4#+4iN|G3L%HlO1!&7AxV)rrn!WisA~O z3~1nQPMa&QuAH1lR&>=Ed2$@=;!wJ;!qb0)Yon>LYxQhqLd9!ME`E zYI^iD1WH*+lX_&9M`tIJwK1(pB1VU=J9?;mgA$kgU8c6Z=*GTp%J-UbBvgA`F z*eEXCvP+&R^H--YizWPjFKLk1CGt9Xo?h2NKD5#wd6;UBL~RPyoD;RQL5n62SUO$w zq?3Owq-*J%Vmgp~Z?;Y`WzfNsaIY;oH8zqlV5O}d{&j0s+Hr0DOo+L_uFn6EUptZr695lX>ezZwlj!rq0+HQD6N zOsRTIZH^FVW;bMi-ufKx7y*STd?q&SbntZ-7D}!4- z&)mqd3!cR>4x0}FCkbBn&5`m)PB58^zi+C8G{Dd3F{f(A!joD~BIP@C=cP_3i&bK3 z39p$>+YOlr-aAqHe}i`qT6Y0|cwz&j@*X|9hkNpds_sG$v7`8?bk35D{#54=QG0ys zN?y4v*YRKHmw0N>71?80x`S*pYSp}~pmZLR$*}6duwnH_bi=Cd)fpq1*EPxddFWr@ zy#*^$x2}F_`E=~>3W65*^RVRD(%8DOX$|x8hMBt@52N2B{E3+Xc`mols+T!$m{n9z zL1J$q`yM*jLy-dkc4Ld|oi!(x^vim57w;=}1r=4XE9^yS70?W{0N4=NCW2uL1@$OX zP~QNo12zI{2}YJq2jaxJE}W6wd9k`@O@*2LG${=fl$vDf(m<;0!vlXt2aQetg(qU|j}L|7L} zVITr@1L9CghR^RC>mccE7%MEEVJS|@rXq8!WcT~|_7Wu^4lgm{2!TSw-hy(cFsEU^+ueyT^9Tb;$B% zu9nu!{yGWhO5pd?y@+;pFrmYkUZ&1G@V=q=*l5a%_ZDX&)&=o*r%cCegL->dud{#1<$Vvzu7mMy6-+J+M z1FPfDR`$ij7Sw@O5ey=6#JdC_YX=gmXCAB~dqk#u2d!d5u<7@(D7%Du(!u*s4Fa9O zejr5frir&Q?EWr|KRd6WQ3T|7sl~8p2kGgf8!M$BrB5%d*v&6*Wc&98{QG@+v`0{l z_UI3A*ZkLG&%(5mIbp{U3WftJ6FaIgg|4zaA*zX>LVB5L@fkYU%(=+D;)Y<@qX&@q zDPwvjvXN#)gVMBpy@Ajr@~Q438@LHN<*`i*9+G2l@fUJczipXvaJjjmC(}kmb~(nM z4ipf)#peDZwie;KBWPL+h^1M9lE{mdDBTbI7;poB4>SU1#)&l?MLoG_KSA}UKr+$9 zOj2@eQhGu8AAooGg{sn9j#7~_SL|u*A0huIzyRI_Zgv-YlVq=srB~k~*Z&;NUk82x zB!a_mt6DF@ePRqcQ-3?$Mm z6McRKB>gGe`vqj{fe(Qcv%e+T$9zle%VgEPxNXOepo4XyQ%rOhYq9}Syx%0_B3=BD zwE-G{%u5{s<9|MOadEaaR)=O7zcv*mM2$$%Yv|l@>M?xYZ(tuoJ{zcl{OX(deBR*A zos@~lKlTam1aKGNBN#3g&?BK9+}jKX{pZsW4BvK4eu{O!70`DJdaKa)PmrTjvDk^JS@+x7M= z$KOEvRsQb!rSjkM=^NI`6@2%G>bz?ZKSTJaR(w8aVCSgDpV}~(^E%qD0Kzw=D3$TL zjq~XE*Eaq@_VRhni_(+3-6MQobNkBYp*DlywNBW4t%1!?2<>Kh0R+OU$Us&>d2H^F zUvhU#ab_jeJIooq_Sb9yU(s?<5CtcL1^WA(TY;q(ngg4Jpx|Oe2v8yP%SYI#!M9SAYb+ zo}&N9CXRIanMh*vQ>et~w<<|o5Lr1&yP(hJM+k*#_X<=lDvn$F}IwR=#%K3Mg5R33RN@A2(cuQ-O{8yZ}R?SX_ z52gYcfE(}tIl#;O{nlFfzj;pEoEhR}ze`4pG;t8&KRI*tALp%Y%Tuo`he`9cu_xQE zS(ZLaMY^Jjn9(r{gc|++a};GU!4oJ6Jc!Z{3F$We+V+L=O#YkgrLkp@_5oi5?ga*c z0bmH|2YP`6z(D}lG4?aym%tmqXMk8%5kT0_*>8a_01*LvakB&(?CGus=Iomninc;Me{a{C~T)c%mY1qR5_cm_Wy3}Ae)5d>y zM_Jw-&~o~G5Yb4&XEWJWzHDc?o&719=AoS61VNGTQr@sG`E53fo82(KUkKp@ zS4~?FzIC*UdNje7Q%>!%xK^c#NU4efzKWjuJHC2+Ak`MJVyk`7`@Oql!`AknZqDzM zJNI|z&g`9gXJ(&!R($=0aE6BuPq)zL(CGHfRnP2mzAeNt^@(^X^G1u^vUbc0i_2~1 zsj^9VMXdsM@sFMUJLs{|qo1&;GuBUAv=O_-4$*7rw`wDM#eh{(gS^=>@-EJ?4jA4BtvxqrXE2Ou@S`o2C z8%r$JFvBvMb-7WY6>B8}@HAMm&Eqp7+iEk{^D)?CIiR&Hk9i@w_Y`DX#wZeAj;* zlw(PK@|)`QxBuqsX9wffA0CuBq?Tm_GrQAkNcBP5#Aw~Ha*=B_Q#4N$nP_4L0`yB^>%jpLYk_{Y!kK7yE2{D)XDJOp!PUF zPM0Z=Qwi#WtY9SF?=w8Xu;1^Kracg93%l&5UH1jsOsD2+^L7Od4`ve%nj&DPb$B~H zTf+KYVGi?nT7zC)_jvYNzFX&R4|n+7J>jssEi8RKo^IW}sVflF;!WKr1Kyx#lMHBn z-vv7LW_CsT44O<9J^HWJx$LUZ4tr*rQ>2PfVkFTaGU?CpPwRE+(h)(c(-u84V!TKl zhi_4-z8-l)b```~Ks7L1{U~SL!X^^U)OMd&qc7%Ylim)WC!on@Sj+*g2W|is16L4C zr%rM6guGiPd;DQ{=!SYXUypDG*;-_5 zf~0q4$~)vWuSfgG2CU9U;3;+KoaKw=&yzJOzjSTtZRmHq>Mr$(@#?+O z8RBM@UshZ&fz+-sasVAtg5hqTjHk0|DJ$PF7#Gvt?UkX}7v}8S2zQ))9!PV5%Ye%P zPP|1Vnf72nHzc+gxd6BnSP0Bl$I3?64u;LlFuW3bNk^C}E-8jObvR{F+2ya!szG0# zwLMXIvXiuCT4&cL8m?XLhE0+SgLIXuu;Qas1V`>y_HlQvM^LD|IDf8ydL?ie;F8-6 z=^7vibO2oKiCp=7fIks*fsa zD=pQK0H_1*d=XY}xnEe-4<`=kK=;&OAmsB(K4Z)@x({>(sl{1^`ij-XXXs$(wJ^n2 zK6cC$qZ?H|bc8dL3dF8xM*VIPLCC%n?(O!AO1I659bh-Z-7Nqot_xBQP(W~%#kPut zP$V*8KAIeAKB_tGImLJ2#fyLwcnN3*Vg-dTCsmX@0^Q3%qK*Rp2$?bzn#^&^$!wkG|b7Rph;aHs|P@kYbChN~h*#pnT--fTOBu zYFWWs5R)98Cf$K(&s4h*mFn7Q%NMaH_Q_R|g+A{9iO_K*&p^k?WI{@E`xWVqsKUkr zY)dMpd#6I=f%-tD7VxW3w6^r+y56nu*PW=_EpHO=iH;Sj# z`->Nd32J=Hq{8>1evfdnfj=KLq1x%=XjUC9JMt2H6o;&e-pFcouBCFR zlF1o^`xhPe?xk(bkHD;u;IhU29&VC#gUXbc?1qAajT)3CfgxxEz2q+*&mzL z;e*nxQkHEK_3GMX)9F(4)Ux`#Kg0Ad0B6n%kV@4*m)()^B*ck8ow{%NmtvgSxng_d zDN?xB#7>RHc=;TV25^lJK-vNv23mlPz!Ja%B$obqG`V;jkWK*{oIgTJY+y*Q*j5|~ zLi7O$aco27{q+MhdHn;BIIEH}fw%RfqDWNNA#_^>3?`$TjOI7MUxB+U5h7Ah{tfsR z_znmI#T09~8WLryMgE;YiAVX%chwEh{R!{_R|9-c>41bRl_4N$m%I|XWx#U6<#GkY zVZagKKHz0w7jP$_0RCOE64G2?3{V6V6I}Il{Je5iQQcq-n8W$VF{F2^FG#1(7ck{u ze-4SgJ+5+B&KHlVwJY5v^i$g+Z2DY{|@;$0x)vV-;#dTy^9o@X@l(n&+M3;@vIKC)yO1a`!cAnr)vWV?hH$g%! zer{7Yt*sMy2V z;Z?s5O$=`ikN-Yy^#y~IJf0!D>5Z`Ad1jO3q(`4la9Aj05r@-}<8VapUN=gVEuavZ z_RxRcr%ii@HxN2)Q%Bd2&tC#7hsOgIjnhxATt#xBa%`x|Fo((3U}sMq{bW~DHuN`g r$zfE9Uh)zk5j&PJG1D=VZt-0~-)u6l=yfV%qsx)r?#M_N_H+LR{rmID diff --git a/src/fori_serial_bridge/fori_serial_bridge/__pycache__/serial_bridge_node.cpython-310.pyc b/src/fori_serial_bridge/fori_serial_bridge/__pycache__/serial_bridge_node.cpython-310.pyc index 20154498fc8931b0d2b4dc7863856fe21c4f11a2..3506907b3b1eea09b3017fe405d61214c1606690 100644 GIT binary patch delta 2154 zcmZuyYitx%6ux(MW_NaWyDaU}?zY?R^i3&!Q9zz%5w&2X%0p^H$U5zvmTjlIWoDMb zde=2+Voab3_5RREYD!}ChoJ@NGdc`90piyiRQUW-d5irU~)JsAURFE@0WCc)`7&#IK23=;X zbQl{!#OE^Ja5Sb|hR@Y#1dWG`w;hf5mKmT?gftQABc)K~2QY#w6M(S{=5sJYAsLTd zMt!g&nMA1UCc8{lK{`vo{_xCuE7 z_Ev85JK^juN*YO%TSSBk)Kh^-bCq*v-1XI(@wG(|4U+ZQ${*7dKdmt@bo5!hn#0)a=6=hwkH=#4b8Gs$222N zQpd7}tqtpDFObbj0QOgWM-LB0Xnk^c$w(&i<%gcyap1r+`}z;7Pn+pux~ZDl#1_?j z7Z`c(K~<%y>azQjc~5)S(7(8movf}a<~KYF-KR~cZbmhmv(@a7I&Nq=D?MQ7Aeb4} zK~kGYX`S2i{p2!Ne4GdfZA zvGJmP0_N6uHe>1S`I`R^7}C;)PPVA|CmyZ8WTi91DsPqQv}(C_Al1^u zLN&$Eg71iCX|`>eAA*xj)pTy12E%-d!>b%x02Ts+S;No=?KEt5K_dFmoNk`yt4Y4< zwX!C3(7eFz)f}!0nPq(C9ljzROF;?FpdB|qXG4iPpV`irFSD6MgVIJyDP7L?~Fr-bF!>@5|mO z7O?!FR1#kg;|46hE-Wfy5Lgi$^u*znaR>(mMTm!SRFHAR;}v)cMNlAh80VXvZ_FO` zd0e#>Lv=x@E-Wdq#0d({H#me(YIkA$J$q2sf_vEN`oz))X?{;VRQv;km_Ksp=Wvy! z8ukMhHZ6Szjz0EnLqb~0^LK++lFT{qvc;Ay;h(@rpHm^|Ww6ZaZ1w-XP}$GSJ;#C7hu z!2ueC%xxU_pWUqEaFZQfzq`^2{guyu<50?>iNh^+V|^PgWx|Gfp-N!Q8=k~5Hniaa PV02@zZ-*RJu(vv=?QblWcN?zY|D-O@r63TP<>(%?#6Xhn#WhT34$Y@MZUyIjKT zN`Eq!Mfb@dkvcH}l%!462csfsLexkC#zdnCB7HE6!9-0oM&5nU;5T;*aW?nNnRCya z`DXTJzCM0GA3PQamzFI>H{3J2}ZWYRq78TN4HCRXyB1$k_N$IKeE)gmaPz?t%l6HA*EG^ zwi?n784n$=x*2C5IjR|9uF((_c@BC|Y#;QsFkUPHfl6$pSj#ookxuIz4^_C%JrP9f z!>AECgrmi}B{Wq}!(69p-co4;CvM+(QKJzWJp+3hL3W*ris89C7&!(;^ZE!HMHKi+ z!`7<(44y&7H5}1cnG}Ab21WT&gX87isYcpFo6lg5#v^e2k;Nz{OBkbKOpc%|lbdkV zCj14S7>Xjt=f<>@$yv-4@HA2M5(4hwKFDaYO;?MD5%H08YeG)2*i5~}7MkS2GAR=n*K$(E9FNC=1zpx!L{k7tTYAZcmAp_};_aZtF`;?)&@QllO-IdSc!>yz9x~;q=g` zHJpAXy)QjHG&*KI#d0UI%o@tf?6BAc$eE8HvbuI!{h54jf@O+XY7J#Gd23guSj@7y z9aaS&5|`phL9Bsjb;a{NsY$m4)R$r-z&h8816E1gvo;4hVOa6Jm?^+?CK8D?Egyiw zQc~6n#~9?ZkEF67;SDjE7z@;}dO0{J;d604k?gDZS!QNj?ie2yVnSvJ&#!l{frD9=%j9>l z-12A1e%#!Gp#VP=5YkL(2CIqZ+p8BzWbFXP--=9oC+-xd+gn#JmE}wAP~Hug#J-a- zD&eNM-fCJ-#{~%s}d;#ATiSCr|l5}p9@Uh5u z$9>l%^-8!b-s~R1zl!_aQ;B~-sE`-tDx@&S8su9S8YTM$;0rOk<-o(+^yZeK&@K7K zt>U!n5u1DZaeBGfvmLkImdYIoaDIsONr*{EN>~v$whqSJsXxi_JqdLZIwbroHuZMl qS~1q!M*hX(mEN5=BChvd_C+9!?U9xSIP`jPcH3i~HX~wU^}&BMl5Z~GR}<83gqQ*yqDzMvoG4a zG|Ay$q(n-2fCo;zP+Hj%0#!n)5TAf|9*}rD-jLv#ZxCkgYD3jjVy&Iqotd4T`OVDw z!SLS6yqn8q7>?*sP!DoH<`-dlsl4tstn!ZQmRDVAn_FK0yRvCJBH4MCw5yJ}B5ko| z#jNlvcmzL$*LkJ(x}T~WovhHi&|+pL-VL-^J<|oRCrc%=7VG4?tdkc8$?viIybHkm z=jwyF9};Pi`JAJT0`3Z(V_gPJumx5c2qS*-gF@`!6Rt#j(+ z|0?&CembMP0>dMSOBS+99t>p=@*IDVhp?eewG|Y3e;~Qb>g+Rkj~&1&2oQYV5Zs44 zuPb}XE(CA+s!&izTjed-<9C^fv!i3cDWOuJ{hjv2PNy^Xm`L4Mp5Vu44%j?iW{C8@ zvCDmK&(!!Qj z&3ENaiOV6>mjxsxMULSoQmb;!i}c%D^hE^vMMNX0t7EFXcYp26sH9h=OTWr^m*&pORMRyFR_Yo4zCUyt4^ezjAedu5# zcVQP=;Eno9M-{lo8ds7UPs+7lwAxM?5MO)>p3?jrT13={UuytK*l7;|Nj~or^MC*2;=>x3Fd-m9*tBv2}09 z^Q}gdzP??vHhkM{mUyIWN+;4u$r2ACt~)}oos?HJRNeGllf?KQ#ujJLlt}5hPH70{bn%sZq;@yQS{wn__2Np^5J*-wYEHk(nwph8fP7qlpD*VNc4-u^bn!EU8HZlu|iOe6W&&;E5pap15&}D5Zw-$h@wMaGomQ8<@v< z^7t|irU&s&oB$*IH1##igfAK=kX|t+N9C({K*p8TuXy(Z(UySethy*<~Il%*(hV{!53Li}(^YGXX+sI<_4*vm^Ny*Ou delta 931 zcmY*WQBTuQ6u$Si+q$kTW3UAc=!ha21V$i1MH2;4gNZyS$)Yi7y4}LslyEHbt7;)BBzKo$q}2+{2L%In7g58NpXdF4>>t zZ`vB3ds3`=n`Uv>^NQ;pGk5j6Uo3A~4(&nTWR~IRWoFS0vzz1Z@K^knKO(CSr-Fnn zwG9;BVmMXL z4;C7u4-CsOX+H4s##Z1>c{S6`*UfUi%)C0h#X_FPOh^W6S*+n9SVYJ{%wy>#IZX@vME-~;`RmjLh@Vr_!v)-F z#>SxbKh2u}7x=H#%0RRWRb8(*hVNHQ$I*3uM;RBggE?*~@8h9punnKf>Z}@3*>wm7 z!fx>u^%7p>RdqbO6iGEm diff --git a/src/fori_serial_bridge/fori_serial_bridge/odom_relay_node.py b/src/fori_serial_bridge/fori_serial_bridge/odom_relay_node.py new file mode 100644 index 0000000..457ed1a --- /dev/null +++ b/src/fori_serial_bridge/fori_serial_bridge/odom_relay_node.py @@ -0,0 +1,42 @@ +#!/usr/bin/env python3 +""" +Simple relay node: /odom_wheels -> /odom +Provides standard /odom topic for Nav2 from wheel odometry source. +No external topic_tools package dependency required. +""" +import rclpy +from rclpy.node import Node +from nav_msgs.msg import Odometry +from rclpy.qos import qos_profile_sensor_data + + +class OdomRelayNode(Node): + def __init__(self): + super().__init__('odom_relay') + self.pub = self.create_publisher(Odometry, '/odom', 10) + self.sub = self.create_subscription( + Odometry, '/odom_wheels', self.callback, qos_profile_sensor_data + ) + self.get_logger().info('OdomRelay: /odom_wheels -> /odom') + + def callback(self, msg): + # Ensure frame_id is 'odom' for Nav2 compatibility + msg.header.frame_id = 'odom' + msg.child_frame_id = 'base_link' + self.pub.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = OdomRelayNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/fori_serial_bridge/fori_serial_bridge/parking_controller_node.py b/src/fori_serial_bridge/fori_serial_bridge/parking_controller_node.py index 89a5e59..64a2ddc 100755 --- a/src/fori_serial_bridge/fori_serial_bridge/parking_controller_node.py +++ b/src/fori_serial_bridge/fori_serial_bridge/parking_controller_node.py @@ -39,14 +39,12 @@ class ParkingControllerNode(Node): self.kp_yaw = self.get_parameter('kp_yaw').value # Declare patrol waypoints as parameter (flat list of x, y, yaw) - # Default patrol coordinates following the 100% obstacle-free map circuit - default_patrol = [ - 1.10, -1.35, 2.35, - -2.20, 2.00, 3.14, - -3.00, 0.50, -1.57, - -1.80, 0.80, -0.78, - -0.50, 0.00, -0.78 - ] + # No default coordinates: these were tuned for the old indoor map and are + # meaningless (possibly unsafe) on the new Gunsan site map. Patrol falls back + # to "go home and scan" until the user supplies real waypoints for the new + # map via this parameter (see send_patrol_goal/advance_patrol below, which + # already append the home position regardless of this list's length). + default_patrol = [] self.declare_parameter('patrol_waypoints', default_patrol) self.patrol_waypoints_raw = self.get_parameter('patrol_waypoints').value self.patrol_waypoints = [] @@ -57,7 +55,14 @@ class ParkingControllerNode(Node): 'y': self.patrol_waypoints_raw[i+1], 'yaw': self.patrol_waypoints_raw[i+2] }) - + + if not self.patrol_waypoints: + self.get_logger().warn( + '[PATROL] patrol_waypoints 파라미터가 비어 있습니다 (신규 군산 지도용 좌표 미설정). ' + '순찰 시작 시 홈 위치에서 360도 스캔만 수행합니다. rviz2로 새 지도를 확인한 뒤 ' + 'patrol_waypoints 파라미터로 실제 순찰 좌표를 설정하세요.' + ) + self.patrol_targets = [] self.patrol_index = 0 self.patrol_start_x = 0.0 @@ -136,6 +141,18 @@ class ParkingControllerNode(Node): self.create_subscription(Odometry, '/odom_wheels', self.odom_callback, qos_profile_sensor_data) self.create_subscription(PoseStamped, '/goal_pose', self.mock_nav2_goal_callback, 10) self.create_subscription(PoseWithCovarianceStamped, '/initialpose', self.initial_pose_callback, 10) + + # AMCL pose subscriber (Real AGV mode: map-frame accurate position) + # Used to update sim_x/sim_y/sim_yaw with real localization data + from rclpy.qos import QoSReliabilityPolicy + amcl_qos = QoSProfile(depth=5, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE) + self.last_amcl_time = None + self.create_subscription( + PoseWithCovarianceStamped, '/amcl_pose', + self.amcl_pose_callback, amcl_qos + ) # ROS2 OccupancyGrid map server uses TRANSIENT_LOCAL durability. map_qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) @@ -173,6 +190,21 @@ class ParkingControllerNode(Node): self.send_nav2_goal() def odom_callback(self, msg): + # Only use wheel odom if AMCL hasn't updated in the last 3 seconds + if self.last_amcl_time is not None: + elapsed = (self.get_clock().now() - self.last_amcl_time).nanoseconds / 1e9 + if elapsed < 3.0: + return # AMCL data is fresher, skip wheel odom update + self.sim_x = msg.pose.pose.position.x + self.sim_y = msg.pose.pose.position.y + q = msg.pose.pose.orientation + siny_cosp = 2 * (q.w * q.z + q.x * q.y) + cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z) + self.sim_yaw = math.atan2(siny_cosp, cosy_cosp) + + def amcl_pose_callback(self, msg): + """Primary pose update for Real AGV mode (map frame, AMCL localization).""" + self.last_amcl_time = self.get_clock().now() self.sim_x = msg.pose.pose.position.x self.sim_y = msg.pose.pose.position.y q = msg.pose.pose.orientation diff --git a/src/fori_serial_bridge/fori_serial_bridge/serial_bridge_node.py b/src/fori_serial_bridge/fori_serial_bridge/serial_bridge_node.py index f25bcdb..1116698 100755 --- a/src/fori_serial_bridge/fori_serial_bridge/serial_bridge_node.py +++ b/src/fori_serial_bridge/fori_serial_bridge/serial_bridge_node.py @@ -220,68 +220,98 @@ class ForiSerialBridge(Node): msg.design_capacity = 60.0 # 60Ah LiFePO4 msg.capacity = 60.0 # 60Ah LiFePO4 msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LIFE # LiFePO4 - - voltage = 25.1 # Default nominal voltage for mock simulation mode + + # === LiFePO4 8S Battery Voltage Bounds === + # Full charge: 8 × 3.65V = 29.2V + # Cutoff: 8 × 2.80V = 22.4V + v_min = 22.4 + v_max = 29.2 + + # Initialize last-valid-voltage storage + if not hasattr(self, 'last_valid_voltage'): + self.last_valid_voltage = None # None = no valid reading yet + + voltage = None # Will be set from Modbus or fallback + if not self.is_mock_mode and self.control_method == 'direct_pc': try: - # Read both 0x20A0 (External Voltage) and 0x20A1 (Bus Voltage) + # Read registers 0x20A0 (External Voltage) and 0x20A1 (Bus Voltage) v_resp = self.modbus.read_registers(1, 0x20A0, 2) if v_resp and len(v_resp) > 0: raw_a0 = abs(v_resp[0]) raw_a1 = abs(v_resp[1]) if len(v_resp) > 1 else raw_a0 - - # Uncalibrated raw voltage from driver sensor + + # ZLAC8015D reports voltage in units of 0.1V (e.g. 283 = 28.3V) + # or sometimes 0.01V (e.g. 2830 = 28.3V). Detect scale by range. raw_v = 0.0 - if 200 <= raw_a0 <= 320: + if 200 <= raw_a0 <= 320: # 20.0V ~ 32.0V in 0.1V steps raw_v = raw_a0 * 0.1 - elif 2000 <= raw_a0 <= 3200: + elif 2000 <= raw_a0 <= 3200: # 20.0V ~ 32.0V in 0.01V steps raw_v = raw_a0 * 0.01 elif 200 <= raw_a1 <= 320: raw_v = raw_a1 * 0.1 elif 2000 <= raw_a1 <= 3200: raw_v = raw_a1 * 0.01 else: + # Fallback guess raw_v = raw_a1 * 0.01 if raw_a1 > 1000 else (raw_a1 * 0.1 if raw_a1 > 100 else float(raw_a1)) - - # Offset-Linear Calibration for 25.1V Multimeter Baseline - # Midpoint of driver's raw reading band for 25.1V is 28.2V - raw_baseline = 28.2 - voltage = 25.1 + (raw_v - raw_baseline) * 0.88536 - - self.get_logger().info(f'[BATTERY TELEMETRY] Driver Raw: {raw_v:.2f}V -> Calibrated Real Battery: {voltage:.2f}V', throttle_duration_sec=3.0) + + # Sanity check: plausible battery voltage range (20V ~ 32V) + if 20.0 <= raw_v <= 32.0: + voltage = raw_v + self.last_valid_voltage = voltage + self.get_logger().info( + f'[BATTERY] Driver raw: {raw_v:.2f}V | ' + f'{max(0.0, min(100.0, (raw_v - v_min) / (v_max - v_min) * 100)):.1f}%', + throttle_duration_sec=3.0 + ) + else: + self.get_logger().warn( + f'[BATTERY] Raw voltage out of plausible range: raw_a0={raw_a0}, raw_a1={raw_a1}, computed={raw_v:.2f}V', + throttle_duration_sec=5.0 + ) + else: + self.get_logger().warn('[BATTERY] Modbus read returned empty response.', throttle_duration_sec=5.0) except Exception as e: - pass - - # 2. Sliding Window Median + Heavy EMA Filter (eliminates IR drop jumps & Modbus spikes) + self.get_logger().warn(f'[BATTERY] Modbus read failed: {e}', throttle_duration_sec=5.0) + + # Fallback priority: last valid reading > mock nominal + if voltage is None: + if self.last_valid_voltage is not None: + voltage = self.last_valid_voltage + self.get_logger().warn('[BATTERY] Using last valid voltage reading as fallback.', throttle_duration_sec=10.0) + else: + # Absolute fallback: use midpoint of nominal LiFePO4 8S range + voltage = (v_min + v_max) / 2.0 # ~25.8V nominal + self.get_logger().warn(f'[BATTERY] No valid reading yet - using nominal fallback: {voltage:.1f}V', throttle_duration_sec=10.0) + + # Sliding Window Median + Heavy EMA Filter (eliminates Modbus spikes) if not hasattr(self, 'voltage_history'): import collections self.voltage_history = collections.deque(maxlen=10) - + self.voltage_history.append(voltage) - - # Median filtering to reject any sudden Modbus voltage spike + sorted_v = sorted(self.voltage_history) median_v = sorted_v[len(sorted_v) // 2] - + if not hasattr(self, 'filtered_battery_voltage') or self.filtered_battery_voltage is None: self.filtered_battery_voltage = median_v else: - # Heavy EMA smoothing (Alpha = 0.05 for 10-second smooth transition) + # EMA smoothing (Alpha=0.05 = ~10s transition) self.filtered_battery_voltage = self.filtered_battery_voltage * 0.95 + median_v * 0.05 - + voltage = self.filtered_battery_voltage - - # 3. 24V LiFePO4 8S battery voltage bounds: Full = 28.0V, Cutoff = 21.6V - v_min = 21.6 - v_max = 28.0 + percentage = (voltage - v_min) / (v_max - v_min) percentage = max(0.0, min(1.0, percentage)) - + msg.voltage = float(voltage) msg.percentage = float(percentage) msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING self.battery_pub.publish(msg) + def init_direct_drivers(self): """ Initializes ZLAC8015D parameters (Operating mode to Velocity Control). diff --git a/src/fori_serial_bridge/fori_serial_bridge/terrain_speed_node.py b/src/fori_serial_bridge/fori_serial_bridge/terrain_speed_node.py new file mode 100644 index 0000000..27c0b5b --- /dev/null +++ b/src/fori_serial_bridge/fori_serial_bridge/terrain_speed_node.py @@ -0,0 +1,228 @@ +#!/usr/bin/env python3 +""" +Terrain-aware cmd_vel speed shaper. + +Sits between raw velocity command producers (Nav2's controller_server, remapped in +fori_nav2.launch.py, and parking_controller_node's own patrol/docking/mock-drive +commands, remapped in fori_full.launch.py) and the real /cmd_vel that +serial_bridge_node drives the motors from. Looks up the 2.5D elevation map +(pcd_gridmap_converter output) at the robot's current position and a short +lookahead point in the direction of travel, computes the local grade, and scales +the commanded forward speed: faster uphill (more torque margin needed), slower +downhill (safer descent). serial_bridge_node's own target_linear_speed clamp is +still the final hard safety ceiling regardless of this node's output. +""" +import math + +import numpy as np +import rclpy +import yaml +from geometry_msgs.msg import PoseWithCovarianceStamped, Twist +from nav_msgs.msg import Odometry +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, QoSProfile, QoSReliabilityPolicy, qos_profile_sensor_data + + +def _read_pgm_token(f): + token = b'' + while True: + c = f.read(1) + if not c: + raise EOFError('Unexpected EOF while reading PGM header') + if c in b' \t\n\r': + if token: + return token + continue + if c == b'#': + while c not in (b'\n', b''): + c = f.read(1) + continue + token += c + + +class ElevationMap: + """Loads a pcd_gridmap_converter elevation_map.pgm + elevation_map.yaml pair. + + Pixel 0 means unknown. Pixel 1..255 linearly encodes height between + min_elevation and max_elevation (see pcd_to_gridmap_node.cpp's + publishAndSaveMaps()). Image rows are stored top-to-bottom for the highest-y + grid row first, mirroring how nav2_map_server decodes map.pgm. + """ + + def __init__(self, pgm_path, yaml_path, logger=None): + self.logger = logger + self.valid = False + try: + with open(yaml_path, 'r') as f: + meta = yaml.safe_load(f) + self.resolution = float(meta['resolution']) + origin = meta['origin'] + self.origin_x = float(origin[0]) + self.origin_y = float(origin[1]) + self.width = int(meta['width']) + self.height = int(meta['height']) + self.min_elevation = float(meta['min_elevation']) + self.max_elevation = float(meta['max_elevation']) + self.unknown_value = int(meta.get('unknown_value', 0)) + + with open(pgm_path, 'rb') as f: + magic = _read_pgm_token(f) + if magic != b'P5': + raise ValueError(f'Unsupported PGM magic: {magic!r}') + w = int(_read_pgm_token(f)) + h = int(_read_pgm_token(f)) + _read_pgm_token(f) # maxval, assumed 255 + data = f.read(w * h) + + if len(data) != w * h: + raise ValueError('Truncated PGM pixel data') + if (w, h) != (self.width, self.height): + raise ValueError(f'PGM size {w}x{h} does not match yaml size {self.width}x{self.height}') + + self.pixels = np.frombuffer(data, dtype=np.uint8).reshape((h, w)) + self.valid = True + except Exception as e: # noqa: BLE001 - any load failure -> safe passthrough mode + if self.logger: + self.logger.error(f'[ELEVATION] 고도 지도 로드 실패 ({pgm_path}): {e}') + + def lookup(self, x, y): + if not self.valid: + return None + col = int(math.floor((x - self.origin_x) / self.resolution)) + grid_y = int(math.floor((y - self.origin_y) / self.resolution)) + if col < 0 or col >= self.width or grid_y < 0 or grid_y >= self.height: + return None + image_row = (self.height - 1) - grid_y + pixel = int(self.pixels[image_row, col]) + if pixel == self.unknown_value: + return None + return self.min_elevation + (pixel - 1) / 254.0 * (self.max_elevation - self.min_elevation) + + +class TerrainSpeedNode(Node): + def __init__(self): + super().__init__('terrain_speed_node') + + self.declare_parameter('elevation_pgm', '/home/yoo/pcd_gunsan_output/elevation_map.pgm') + self.declare_parameter('elevation_yaml', '/home/yoo/pcd_gunsan_output/elevation_map.yaml') + # 기본값은 /cmd_vel (목 모드 등 collision_monitor가 없는 구성에서 안전한 기본). + # 실기체 모드에서는 fori_full.launch.py가 이 값을 /cmd_vel_terrain으로 넘겨서 + # collision_monitor(급출현 장애물 감속/정지)가 마지막 안전 게이트로 끼어들게 한다. + self.declare_parameter('output_topic', '/cmd_vel') + self.declare_parameter('lookahead_dist', 0.5) + self.declare_parameter('uphill_gain', 3.0) + self.declare_parameter('downhill_gain', 4.0) + self.declare_parameter('max_boost', 1.3) + self.declare_parameter('min_scale', 0.5) + self.declare_parameter('flat_deadband', 0.02) + self.declare_parameter('smoothing_alpha', 0.25) + + self.lookahead_dist = self.get_parameter('lookahead_dist').value + self.uphill_gain = self.get_parameter('uphill_gain').value + self.downhill_gain = self.get_parameter('downhill_gain').value + self.max_boost = self.get_parameter('max_boost').value + self.min_scale = self.get_parameter('min_scale').value + self.flat_deadband = self.get_parameter('flat_deadband').value + self.smoothing_alpha = self.get_parameter('smoothing_alpha').value + + pgm_path = self.get_parameter('elevation_pgm').value + yaml_path = self.get_parameter('elevation_yaml').value + self.elevation = ElevationMap(pgm_path, yaml_path, logger=self.get_logger()) + if self.elevation.valid: + self.get_logger().info( + f'[TERRAIN] 고도 지도 로드 완료: {pgm_path} ' + f'({self.elevation.width}x{self.elevation.height}px, ' + f'{self.elevation.min_elevation:.2f}m ~ {self.elevation.max_elevation:.2f}m)' + ) + else: + self.get_logger().warn( + '[TERRAIN] 고도 지도를 사용할 수 없어 경사 보정 없이 /cmd_vel_raw를 그대로 통과시킵니다.' + ) + + self.pose_x = 0.0 + self.pose_y = 0.0 + self.pose_yaw = 0.0 + self.last_amcl_time = None + self.scale = 1.0 + + # AMCL(맵 기준 정밀 위치)을 우선 사용하고, 3초 이상 갱신이 없으면 휠 오도메트리로 + # 대체한다 - parking_controller_node.py의 amcl_pose_callback/odom_callback과 동일한 + # freshness 기준. + amcl_qos = QoSProfile(depth=5, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE) + self.create_subscription(PoseWithCovarianceStamped, '/amcl_pose', self.amcl_pose_callback, amcl_qos) + self.create_subscription(Odometry, '/odom_wheels', self.odom_callback, qos_profile_sensor_data) + self.create_subscription(Twist, '/cmd_vel_raw', self.cmd_vel_callback, 10) + output_topic = self.get_parameter('output_topic').value + self.cmd_vel_pub = self.create_publisher(Twist, output_topic, 10) + self.get_logger().info(f'[TERRAIN] 출력 토픽: {output_topic}') + + def _update_pose(self, x, y, q): + self.pose_x = x + self.pose_y = y + siny_cosp = 2 * (q.w * q.z + q.x * q.y) + cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z) + self.pose_yaw = math.atan2(siny_cosp, cosy_cosp) + + def amcl_pose_callback(self, msg): + self.last_amcl_time = self.get_clock().now() + self._update_pose(msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.orientation) + + def odom_callback(self, msg): + if self.last_amcl_time is not None: + elapsed = (self.get_clock().now() - self.last_amcl_time).nanoseconds / 1e9 + if elapsed < 3.0: + return + self._update_pose(msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.orientation) + + def cmd_vel_callback(self, msg): + out = Twist() + out.angular.z = msg.angular.z + + # 제자리 회전이나 지도가 없는 경우엔 경사 보정 없이 그대로 통과 (안전 기본값) + if not self.elevation.valid or abs(msg.linear.x) < 0.02: + out.linear.x = msg.linear.x + self.scale = 1.0 + self.cmd_vel_pub.publish(out) + return + + direction = 1.0 if msg.linear.x >= 0.0 else -1.0 + to_x = self.pose_x + direction * math.cos(self.pose_yaw) * self.lookahead_dist + to_y = self.pose_y + direction * math.sin(self.pose_yaw) * self.lookahead_dist + + z_from = self.elevation.lookup(self.pose_x, self.pose_y) + z_to = self.elevation.lookup(to_x, to_y) + + if z_from is None or z_to is None: + raw_scale = 1.0 + else: + grade = (z_to - z_from) / self.lookahead_dist + if abs(grade) < self.flat_deadband: + raw_scale = 1.0 + elif grade > 0.0: + raw_scale = 1.0 + self.uphill_gain * grade # 오르막: 속도 부스트 + else: + raw_scale = 1.0 + self.downhill_gain * grade # 내리막: 속도 감쇠 (grade<0) + raw_scale = max(self.min_scale, min(self.max_boost, raw_scale)) + + # 셀 경계를 지날 때 속도가 튀지 않도록 EMA로 스무딩 + self.scale = self.scale * (1.0 - self.smoothing_alpha) + raw_scale * self.smoothing_alpha + out.linear.x = msg.linear.x * self.scale + self.cmd_vel_pub.publish(out) + + +def main(args=None): + rclpy.init(args=args) + node = TerrainSpeedNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/fori_serial_bridge/fori_serial_bridge/ui_server_node.py b/src/fori_serial_bridge/fori_serial_bridge/ui_server_node.py index 83ba729..68b6494 100755 --- a/src/fori_serial_bridge/fori_serial_bridge/ui_server_node.py +++ b/src/fori_serial_bridge/fori_serial_bridge/ui_server_node.py @@ -25,7 +25,9 @@ class UIServerNode(Node): if not os.path.exists(self.ui_dir): # Fallback to source directory for development / symlink install - self.ui_dir = os.path.expanduser('~/fori_ws/src/fori_serial_bridge/ui') + self.ui_dir = os.path.expanduser('~/fori_ws/fori_ws/src/fori_serial_bridge/ui') + if not os.path.exists(self.ui_dir): + self.ui_dir = os.path.expanduser('~/fori_ws/src/fori_serial_bridge/ui') self.get_logger().info(f'Serving UI files from directory: {self.ui_dir}') @@ -34,22 +36,35 @@ class UIServerNode(Node): self.server_thread.start() def start_server(self): - # Change working directory of the handler to UI directory - os.chdir(self.ui_dir) - handler = http.server.SimpleHTTPRequestHandler + # Use a custom handler that serves from the UI directory without changing + # the global working directory (which would break other ROS node operations) + ui_dir = self.ui_dir + + class UIHandler(http.server.SimpleHTTPRequestHandler): + def __init__(self, *args, **kwargs): + super().__init__(*args, directory=ui_dir, **kwargs) + + def log_message(self, format, *args): + pass # Suppress HTTP access logs in ROS terminal output try: - self.server = ThreadingHTTPServer(("", self.port), handler) + self.server = ThreadingHTTPServer(("", self.port), UIHandler) self.get_logger().info('==========================================') self.get_logger().info(f' Web UI Dashboard server launched! ') self.get_logger().info(f' Connect via: http://localhost:{self.port} ') self.get_logger().info('==========================================') - # Automatically open default web browser - try: - webbrowser.open(f"http://localhost:{self.port}") - except Exception as browser_err: - self.get_logger().warn(f'Failed to auto-open web browser: {browser_err}') + # Automatically open default web browser after 1 second delay + def auto_open_browser(): + url = f"http://localhost:{self.port}" + try: + opened = webbrowser.open(url) + if not opened: + os.system(f"xdg-open {url} >/dev/null 2>&1 &") + except Exception: + os.system(f"xdg-open {url} >/dev/null 2>&1 &") + + threading.Timer(1.0, auto_open_browser).start() self.server.serve_forever() except Exception as e: diff --git a/src/fori_serial_bridge/launch/fori_full.launch.py b/src/fori_serial_bridge/launch/fori_full.launch.py index 5c90881..19ca0cb 100644 --- a/src/fori_serial_bridge/launch/fori_full.launch.py +++ b/src/fori_serial_bridge/launch/fori_full.launch.py @@ -14,8 +14,11 @@ def generate_launch_description(): serial_bridge_dir = get_package_share_directory('fori_serial_bridge') hik_camera_dir = get_package_share_directory('hik_camera_ros2_driver') - # Map configuration file path - map_yaml_file = '/home/yoo/fori_map.yaml' + # Map configuration file path (군산 부지 PCD -> 2.5D 변환 결과, pcd_gridmap_converter 산출물) + map_output_dir = '/home/yoo/pcd_gunsan_output' + map_yaml_file = os.path.join(map_output_dir, 'map.yaml') + elevation_pgm_file = os.path.join(map_output_dir, 'elevation_map.pgm') + elevation_yaml_file = os.path.join(map_output_dir, 'elevation_map.yaml') # 2. Include Hikrobot Camera launch description (pass camera config to isolate it from Nav2 parameters) camera_launch = IncludeLaunchDescription( @@ -36,18 +39,26 @@ def generate_launch_description(): ) # 4. Launch Motor Bridge Node (ZLAC8015D direct PC RS485 Mode by default) + # C++ port (fori_serial_bridge_cpp) replaces the Python node here: same + # topics/params, but a CRC-validated low-latency serial transport removes + # the Python control-loop jitter/delay. motor_bridge_node = Node( - package='fori_serial_bridge', + package='fori_serial_bridge_cpp', executable='serial_bridge_node', name='serial_bridge_node', parameters=[{ 'control_method': 'direct_pc', 'port': '/dev/ttyUSB0', 'baud': 115200, - 'target_linear_speed': 0.5, + 'target_linear_speed': 0.3, 'accel_limit': 0.5, 'wheel_base': 0.374, - 'wheel_radius': 0.127 + 'wheel_radius': 0.127, + # 실기체 모드는 fori_nav2.launch.py가 fast_lio(LIDAR SLAM)를 같이 띄워서 + # camera_init->aft_mapped->base_link TF를 이미 발행하므로, 여기서 휠 + # 오도메트리로 odom->base_link를 또 쏘면 base_link 부모가 충돌한다. 모의 + # 모드(SLAM 없음)에서만 휠 오도메트리 TF를 켠다. + 'publish_odom_tf': mock_mode_param }], output='screen' ) @@ -69,18 +80,47 @@ def generate_launch_description(): 'mock_mode': mock_mode_param }], remappings=[ - ('/cmd_vel', '/cmd_vel') # Keep topic clean + # 최종 속도 명령이 아니라 원시(raw) 명령으로 발행한다. terrain_speed_node가 + # 이 값을 경사도에 맞게 보정한 뒤 실제 /cmd_vel로 발행한다. + ('/cmd_vel', '/cmd_vel_raw') ], output='screen' ) - + + # 5b. Terrain-aware speed shaper: /cmd_vel_raw(parking_controller의 모의주행/도킹/순찰 + # 명령 + Nav2 controller_server가 fori_nav2.launch.py에서 리맵되어 들어오는 실주행 + # 명령)를 받아 진행방향 지면 경사(오르막/내리막)에 맞춰 선속도를 보정한 뒤 발행한다. + # 이 노드가 없으면 /cmd_vel_raw -> /cmd_vel 연결이 끊겨 로봇이 전혀 움직이지 않으므로 + # 항상 실행되어야 한다. + # 실기체 모드에서는 fori_nav2.launch.py에 있는 collision_monitor(급출현 장애물 + # 감속/정지 안전계층)가 마지막 게이트로 끼어들 수 있게 /cmd_vel_terrain으로 낸다. + # 목 모드는 collision_monitor가 없으므로 기본값인 /cmd_vel로 그대로 발행. + terrain_output_topic = '/cmd_vel_terrain' if not mock_mode_param else '/cmd_vel' + terrain_speed_node = Node( + package='fori_serial_bridge', + executable='terrain_speed_node', + name='terrain_speed_node', + parameters=[{ + 'elevation_pgm': elevation_pgm_file, + 'elevation_yaml': elevation_yaml_file, + 'output_topic': terrain_output_topic, + }], + output='screen' + ) + # 6. Launch ROSbridge WebSocket Server (run node directly to bypass XML syntax conflict) + # max_message_size 기본값(1MB)보다 큰 메시지(/map GetMap 응답 등, 약 2.5MB)는 rosbridge가 + # 'fragment' 프로토콜 메시지로 쪼개서 보내는데, 대시보드가 쓰는 roslib.js(CDN, + # index.html)는 fragment 재조립을 구현하지 않아 이 경우 메시지가 그냥 유실된다(맵만 + # 계속 빈 화면으로 보이는 원인이었음). 맵을 쪼개지지 않는 단일 메시지로 보내도록 상한을 + # 넉넉히(20MB) 올려서 아예 fragmentation 경로를 타지 않게 한다. rosbridge_node = Node( package='rosbridge_server', executable='rosbridge_websocket', name='rosbridge_websocket', parameters=[{ - 'port': 9090 + 'port': 9090, + 'max_message_size': 20000000 }], output='screen' ) @@ -117,6 +157,7 @@ def generate_launch_description(): ld.add_action(aruco_detector_node) ld.add_action(motor_bridge_node) ld.add_action(parking_controller_node) + ld.add_action(terrain_speed_node) ld.add_action(rosbridge_node) ld.add_action(ui_server_node) ld.add_action(web_video_server_node) @@ -158,7 +199,9 @@ def generate_launch_description(): ) # 4. Robot State Publisher (URDF) to define joints/frames - urdf_file_path = os.path.expanduser('~/fori_ws/src/FAST-LIVO2/urdf/fori_robot.urdf') + urdf_file_path = os.path.expanduser('~/fori_ws/fori_ws/src/FAST-LIVO2/urdf/fori_robot.urdf') + if not os.path.exists(urdf_file_path): + urdf_file_path = os.path.expanduser('~/fori_ws/src/FAST-LIVO2/urdf/fori_robot.urdf') with open(urdf_file_path, 'r') as infp: robot_desc = infp.read() @@ -176,9 +219,23 @@ def generate_launch_description(): ld.add_action(rsp_node) else: # In real mode, include the full Nav2/AMCL/LIDAR localization launch - nav2_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource('/home/yoo/fori_ws/src/fori_nav2.launch.py') + nav2_launch_path = os.path.expanduser('~/fori_ws/fori_ws/src/fori_nav2.launch.py') + if not os.path.exists(nav2_launch_path): + nav2_launch_path = os.path.expanduser('~/fori_ws/src/fori_nav2.launch.py') + if os.path.exists(nav2_launch_path): + nav2_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(nav2_launch_path) + ) + ld.add_action(nav2_launch) + + # Relay /odom_wheels -> /odom so Nav2 receives standard odometry + # (Uses internal odom_relay_node - no topic_tools dependency needed) + odom_relay_node = Node( + package='fori_serial_bridge', + executable='odom_relay_node', + name='odom_relay', + output='screen' ) - ld.add_action(nav2_launch) + ld.add_action(odom_relay_node) return ld diff --git a/src/fori_serial_bridge/setup.py b/src/fori_serial_bridge/setup.py index c3998ce..28075ff 100644 --- a/src/fori_serial_bridge/setup.py +++ b/src/fori_serial_bridge/setup.py @@ -11,7 +11,7 @@ setup( ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), ('share/' + package_name + '/launch', ['launch/fori_full.launch.py']), - ('share/' + package_name + '/ui', ['ui/index.html', 'ui/style.css', 'ui/app.js']), + ('share/' + package_name + '/ui', ['ui/index.html', 'ui/style.css', 'ui/app.js', 'ui/manifest.json', 'ui/sw.js']), ], install_requires=['setuptools'], zip_safe=True, @@ -24,7 +24,9 @@ setup( 'console_scripts': [ 'serial_bridge_node = fori_serial_bridge.serial_bridge_node:main', 'parking_controller_node = fori_serial_bridge.parking_controller_node:main', - 'ui_server_node = fori_serial_bridge.ui_server_node:main' + 'ui_server_node = fori_serial_bridge.ui_server_node:main', + 'odom_relay_node = fori_serial_bridge.odom_relay_node:main', + 'terrain_speed_node = fori_serial_bridge.terrain_speed_node:main', ], }, ) diff --git a/src/fori_serial_bridge/ui/app.js b/src/fori_serial_bridge/ui/app.js index 34c09ba..18edb0d 100644 --- a/src/fori_serial_bridge/ui/app.js +++ b/src/fori_serial_bridge/ui/app.js @@ -18,6 +18,8 @@ ros.on('connection', () => { statusIndicator.className = 'pulse-indicator green'; statusText.innerText = 'Connected'; console.log('Connected to rosbridge WebSocket server.'); + // Request map immediately via service (handles transient_local QoS on reconnect) + setTimeout(() => requestMapFromService(), 1500); }); ros.on('error', (error) => { @@ -83,20 +85,78 @@ const arucoPoseSub = new ROSLIB.Topic({ throttle_rate: 50 }); -// Map subscriber (throttled to 1Hz since maps update infrequently) +// Map subscriber const mapSub = new ROSLIB.Topic({ ros: ros, name: '/map', messageType: 'nav_msgs/msg/OccupancyGrid', - throttle_rate: 1000 + throttle_rate: 2000 }); -// Odometry subscriber (throttled to 10Hz for smoother rendering without CPU spikes) +// Map service client - to request map from map_server after connection +const mapServiceClient = new ROSLIB.Service({ + ros: ros, + name: '/map_server/map', + serviceType: 'nav_msgs/srv/GetMap' +}); + +// Also try the GetMap service name used in Nav2 Humble +const getMapServiceClient = new ROSLIB.Service({ + ros: ros, + name: '/map_server/get_map', + serviceType: 'nav_msgs/srv/GetMap' +}); + +function requestMapFromService() { + // Try /map_server/get_map first (Nav2 Humble standard) + getMapServiceClient.callService(new ROSLIB.ServiceRequest({}), (result) => { + if (result && result.map) { + console.log('Map received via /map_server/get_map service.'); + processMapMessage(result.map); + } + }, (err) => { + // Fallback: try /map_server/map + mapServiceClient.callService(new ROSLIB.ServiceRequest({}), (result2) => { + if (result2 && result2.map) { + console.log('Map received via /map_server/map service.'); + processMapMessage(result2.map); + } + }, (err2) => { + console.warn('Both map services unavailable, waiting for /map topic publish:', err2); + }); + }); +} + +// Primary AMCL pose subscriber (Nav2 Map frame pose) +const amclPoseSub = new ROSLIB.Topic({ + ros: ros, + name: '/amcl_pose', + messageType: 'geometry_msgs/msg/PoseWithCovarianceStamped', + throttle_rate: 50 +}); + +// FAST-LIO2 LiDAR Odometry subscriber +const fastLioOdomSub = new ROSLIB.Topic({ + ros: ros, + name: '/aft_mapped_to_init', + messageType: 'nav_msgs/msg/Odometry', + throttle_rate: 50 +}); + +// Standard Odometry subscriber const odomSub = new ROSLIB.Topic({ + ros: ros, + name: '/odom', + messageType: 'nav_msgs/msg/Odometry', + throttle_rate: 50 +}); + +// Wheel Odometry subscriber +const wheelOdomSub = new ROSLIB.Topic({ ros: ros, name: '/odom_wheels', messageType: 'nav_msgs/msg/Odometry', - throttle_rate: 100 + throttle_rate: 50 }); // Joint State subscriber (throttled to 5Hz to update RPM gauges efficiently) @@ -116,39 +176,31 @@ const batterySub = new ROSLIB.Topic({ }); batterySub.subscribe(function(msg) { - const voltage = msg.voltage ? msg.voltage.toFixed(1) : '26.4'; - const pct = Math.round((msg.percentage !== undefined ? msg.percentage : 0.85) * 100); - + // percentage is 0.0-1.0 range from sensor_msgs/BatteryState + let pct; + if (msg.percentage !== undefined && msg.percentage !== null && !isNaN(msg.percentage)) { + // Normalize: if value > 1.0, it's already in percent form + pct = msg.percentage > 1.0 ? Math.round(msg.percentage) : Math.round(msg.percentage * 100); + } else if (msg.voltage && msg.voltage > 0) { + // Fallback: estimate from voltage (24V LiFePO4 8S: 21.6V=0%, 28.0V=100%) + const minV = 21.6, maxV = 28.0; + pct = Math.round(Math.max(0, Math.min(100, (msg.voltage - minV) / (maxV - minV) * 100))); + } else { + return; // No valid data + } + const badge = document.getElementById('battery-badge'); const bar = document.getElementById('battery-bar'); - - if (badge) { - badge.innerText = `${pct}% (${voltage}V)`; - if (pct >= 50) { - badge.style.background = 'rgba(34, 197, 94, 0.2)'; - badge.style.color = '#4ade80'; - badge.style.borderColor = 'rgba(74, 222, 128, 0.4)'; - } else if (pct >= 20) { - badge.style.background = 'rgba(234, 179, 8, 0.2)'; - badge.style.color = '#facc15'; - badge.style.borderColor = 'rgba(250, 204, 21, 0.4)'; - } else { - badge.style.background = 'rgba(239, 68, 68, 0.2)'; - badge.style.color = '#f87171'; - badge.style.borderColor = 'rgba(248, 113, 113, 0.4)'; - } - } - - if (bar) { - bar.style.width = `${pct}%`; - if (pct >= 50) { - bar.style.background = 'linear-gradient(90deg, #22c55e, #4ade80)'; - } else if (pct >= 20) { - bar.style.background = 'linear-gradient(90deg, #eab308, #facc15)'; - } else { - bar.style.background = 'linear-gradient(90deg, #ef4444, #f87171)'; - } + const valDisplay = document.getElementById('battery-value-display'); + const ringFill = document.getElementById('battery-ring-fill'); + + if (badge) badge.innerText = `${pct}%`; + if (valDisplay) valDisplay.innerText = `${pct}%`; + if (ringFill) { + const strokeDash = 188 - Math.round((pct / 100) * 188); + ringFill.setAttribute('stroke-dashoffset', strokeDash); } + if (bar) bar.style.width = `${pct}%`; }); @@ -177,7 +229,16 @@ let panStart = { x: 0, y: 0 }; const canvas = document.getElementById('map-canvas'); const ctx = canvas.getContext('2d'); +// requestAnimationFrame throttle to avoid redundant redraws +let drawMapPending = false; function drawMap() { + if (drawMapPending) return; // Already queued + drawMapPending = true; + requestAnimationFrame(_doDrawMap); +} + +function _doDrawMap() { + drawMapPending = false; if (!mapData || !mapInfo) return; const w = mapInfo.width; @@ -260,16 +321,19 @@ function drawRobot(rx, ry, ryaw, color) { ctx.rotate(-ryaw); // Draw triangle - ctx.fillStyle = color; - ctx.shadowBlur = 10; - ctx.shadowColor = color; + ctx.fillStyle = '#22A774'; + ctx.strokeStyle = '#047857'; + ctx.lineWidth = 2; + ctx.shadowBlur = 6; + ctx.shadowColor = 'rgba(34, 167, 116, 0.4)'; ctx.beginPath(); - ctx.moveTo(10, 0); - ctx.lineTo(-8, -6); - ctx.lineTo(-4, 0); - ctx.lineTo(-8, 6); + ctx.moveTo(12, 0); + ctx.lineTo(-9, -7); + ctx.lineTo(-5, 0); + ctx.lineTo(-9, 7); ctx.closePath(); ctx.fill(); + ctx.stroke(); ctx.restore(); } @@ -300,8 +364,8 @@ function drawTarget(rx, ry, ryaw, color) { } // --- [Subscribe Listeners] --- -// Map listener -mapSub.subscribe((message) => { +// processMapMessage: shared handler for both /map topic and /map_server/map service +function processMapMessage(message) { mapData = message.data; mapInfo = message.info; @@ -320,11 +384,14 @@ mapSub.subscribe((message) => { const val = mapData[i]; let r, g, b, a; if (val === 0) { - r = 15; g = 18; b = 32; a = 255; + // Free space: Clean crisp white + r = 255; g = 255; b = 255; a = 255; } else if (val === 100) { - r = 157; g = 78; b = 221; a = 255; + // Occupied wall: Dark slate + r = 30; g = 41; b = 59; a = 255; } else { - r = 6; g = 6; b = 10; a = 255; + // Unknown area: Soft light gray + r = 226; g = 232; b = 240; a = 255; } const col = i % w; @@ -339,33 +406,82 @@ mapSub.subscribe((message) => { offscreenCtx.putImageData(imgData, 0, 0); drawMap(); +} + +// Map listener - handles live updates +mapSub.subscribe((message) => { + processMapMessage(message); }); -// Odom listener -odomSub.subscribe((message) => { - const pose = message.pose.pose; - robotPose.x = pose.position.x; - robotPose.y = pose.position.y; +// --- [Pose Tracking] --- +// Strategy: Use AMCL pose (map frame) as primary. Fall back to wheel odom ONLY if AMCL +// has not been received for 3+ seconds. NEVER mix frame origins. +let amclActive = false; +let lastAmclTime = 0; - // Quaternion to Euler yaw - const q = pose.orientation; +function applySmoothedPose(newX, newY, newYaw) { + // Strong smoothing to prevent UI jitter (alpha=0.4 = 40% new, 60% old) + const alpha = 0.4; + robotPose.x = alpha * newX + (1 - alpha) * robotPose.x; + robotPose.y = alpha * newY + (1 - alpha) * robotPose.y; + + // Yaw wrap-around aware interpolation + let diffYaw = newYaw - robotPose.yaw; + while (diffYaw > Math.PI) diffYaw -= 2 * Math.PI; + while (diffYaw < -Math.PI) diffYaw += 2 * Math.PI; + robotPose.yaw += alpha * diffYaw; + + const poseVal = document.getElementById('val-pose'); + if (poseVal) { + let yawDeg = Math.round(robotPose.yaw * 180 / Math.PI); + if (yawDeg < 0) yawDeg += 360; + poseVal.innerHTML = `X: ${robotPose.x.toFixed(2)}m  |  Y: ${robotPose.y.toFixed(2)}m  |  Yaw: ${yawDeg}°`; + } + drawMap(); +} + +function quatToYaw(q) { const siny_cosp = 2 * (q.w * q.z + q.x * q.y); const cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z); - robotPose.yaw = Math.atan2(siny_cosp, cosy_cosp); + return Math.atan2(siny_cosp, cosy_cosp); +} + +// 1. AMCL Pose Listener - PRIMARY source for Real AGV mode (map frame, most accurate) +amclPoseSub.subscribe((message) => { + amclActive = true; + lastAmclTime = Date.now(); + const pose = message.pose.pose; + applySmoothedPose(pose.position.x, pose.position.y, quatToYaw(pose.orientation)); +}); + +// 2. Wheel Odometry - FALLBACK only when AMCL is not active (simulation/odom frame) +wheelOdomSub.subscribe((message) => { + // Only use wheel odom if AMCL hasn't been received in last 3 seconds + if (Date.now() - lastAmclTime < 3000) return; + const pose = message.pose.pose; + applySmoothedPose(pose.position.x, pose.position.y, quatToYaw(pose.orientation)); - // Update speeds const twist = message.twist.twist; document.getElementById('val-linear').innerText = `${twist.linear.x.toFixed(2)} m/s`; document.getElementById('val-angular').innerText = `${twist.angular.z.toFixed(2)} rad/s`; +}); - // Update real-time pose metrics - const poseVal = document.getElementById('val-pose'); - if (poseVal) { - const yawDeg = Math.round(robotPose.yaw * 180 / Math.PI); - poseVal.innerHTML = `X: ${robotPose.x.toFixed(2)}m  |  Y: ${robotPose.y.toFixed(2)}m  |  Yaw: ${yawDeg}°`; +// 3. Standard /odom - Only update velocity display, never pose (to avoid frame mixing) +odomSub.subscribe((message) => { + const twist = message.twist.twist; + document.getElementById('val-linear').innerText = `${twist.linear.x.toFixed(2)} m/s`; + document.getElementById('val-angular').innerText = `${twist.angular.z.toFixed(2)} rad/s`; +}); + +// 4. FAST-LIO2 - Only velocity display, AMCL gives more accurate map-frame pose +fastLioOdomSub.subscribe((message) => { + if (!amclActive) { + // If no AMCL available (e.g. before localization converges), use FAST-LIO pose + if (Date.now() - lastAmclTime > 3000) { + const pose = message.pose.pose; + applySmoothedPose(pose.position.x, pose.position.y, quatToYaw(pose.orientation)); + } } - - drawMap(); }); // Camera stream setup with automatic fallback @@ -473,6 +589,14 @@ arucoPoseSub.subscribe((message) => { }, 1000); }); +// Helper to update top mode summary card +function updateTopModeText(modeStr) { + const topEl = document.getElementById('val-state-top'); + if (topEl) { + topEl.innerText = modeStr; + } +} + // Robot state listener robotModeStatusSub.subscribe((message) => { document.getElementById('val-state').innerText = message.data; @@ -490,24 +614,28 @@ robotModeStatusSub.subscribe((message) => { if (status.includes('mode: nav2')) { currentMode = 'nav2'; + updateTopModeText('Nav2'); document.getElementById('btn-mode-nav2').className = 'btn btn-primary active'; document.getElementById('btn-mode-patrol').className = 'btn btn-primary'; document.getElementById('btn-mode-parking').className = 'btn btn-primary'; document.getElementById('btn-emergency-stop').className = 'btn btn-danger'; } else if (status.includes('mode: patrol')) { currentMode = 'patrol'; + updateTopModeText('Patrol'); document.getElementById('btn-mode-patrol').className = 'btn btn-primary active'; document.getElementById('btn-mode-nav2').className = 'btn btn-primary'; document.getElementById('btn-mode-parking').className = 'btn btn-primary'; document.getElementById('btn-emergency-stop').className = 'btn btn-danger'; } else if (status.includes('mode: parking')) { currentMode = 'parking'; + updateTopModeText('Parking'); document.getElementById('btn-mode-parking').className = 'btn btn-primary active'; document.getElementById('btn-mode-nav2').className = 'btn btn-primary'; document.getElementById('btn-mode-patrol').className = 'btn btn-primary'; document.getElementById('btn-emergency-stop').className = 'btn btn-danger'; } else if (status.includes('mode: stop') || status.includes('state: estop')) { currentMode = 'stop'; + updateTopModeText('STOP'); document.getElementById('btn-mode-nav2').className = 'btn btn-primary'; document.getElementById('btn-mode-patrol').className = 'btn btn-primary'; document.getElementById('btn-mode-parking').className = 'btn btn-primary'; @@ -549,6 +677,7 @@ document.getElementById('btn-mode-nav2').addEventListener('click', () => { currentMode = 'nav2'; nav2GoalActive = false; parkingWaypoint = null; // Clear waypoint on UI + updateTopModeText('Nav2'); document.getElementById('btn-mode-nav2').className = 'btn btn-primary active'; document.getElementById('btn-mode-patrol').className = 'btn btn-primary'; document.getElementById('btn-mode-parking').className = 'btn btn-primary'; @@ -562,6 +691,7 @@ document.getElementById('btn-mode-patrol').addEventListener('click', () => { currentMode = 'patrol'; parkingWaypoint = null; nav2GoalActive = false; + updateTopModeText('Patrol'); document.getElementById('btn-mode-patrol').className = 'btn btn-primary active'; document.getElementById('btn-mode-nav2').className = 'btn btn-primary'; document.getElementById('btn-mode-parking').className = 'btn btn-primary'; @@ -574,6 +704,7 @@ document.getElementById('btn-mode-patrol').addEventListener('click', () => { document.getElementById('btn-mode-parking').addEventListener('click', () => { currentMode = 'parking'; parkingWaypoint = null; // Clear waypoint on UI until set by user + updateTopModeText('Parking'); document.getElementById('btn-mode-parking').className = 'btn btn-primary active'; document.getElementById('btn-mode-nav2').className = 'btn btn-primary'; document.getElementById('btn-mode-patrol').className = 'btn btn-primary'; @@ -597,6 +728,7 @@ document.getElementById('btn-emergency-stop').addEventListener('click', () => { currentMode = 'stop'; nav2GoalActive = false; parkingWaypoint = null; // Clear waypoint on UI + updateTopModeText('STOP'); document.getElementById('btn-mode-nav2').className = 'btn btn-primary'; document.getElementById('btn-mode-patrol').className = 'btn btn-primary'; document.getElementById('btn-mode-parking').className = 'btn btn-primary'; @@ -787,6 +919,93 @@ canvas.addEventListener('wheel', (event) => { drawMap(); }, { passive: false }); +// Mobile Touch Interactions (touchstart, touchmove, touchend) +let touchStartCoords = { x: 0, y: 0 }; +let isTouchDragging = false; + +canvas.addEventListener('touchstart', (e) => { + if (e.touches.length === 1) { + e.preventDefault(); + const touch = e.touches[0]; + const rect = canvas.getBoundingClientRect(); + const scale = Math.min(rect.width / canvas.width, rect.height / canvas.height); + const dx_padding = (rect.width - canvas.width * scale) / 2; + const dy_padding = (rect.height - canvas.height * scale) / 2; + + const clickU = (touch.clientX - rect.left - dx_padding) / scale; + const clickV = (touch.clientY - rect.top - dy_padding) / scale; + + touchStartCoords.x = touch.clientX; + touchStartCoords.y = touch.clientY; + dragStartCoords.x = clickU; + dragStartCoords.y = clickV; + + const coords = defCanvasToRos(clickU, clickV); + dragStartRos.x = coords.rx; + dragStartRos.y = coords.ry; + + document.getElementById('wp-x').value = coords.rx.toFixed(2); + document.getElementById('wp-y').value = coords.ry.toFixed(2); + + isTouchDragging = true; + + if (poseEstimateMode) { + robotPose.x = coords.rx; + robotPose.y = coords.ry; + } else if (currentMode === 'nav2') { + nav2Goal.x = coords.rx; + nav2Goal.y = coords.ry; + nav2GoalActive = true; + } else { + parkingWaypoint = { x: coords.rx, y: coords.ry, yaw: 0.0 }; + } + drawMap(); + } +}, { passive: false }); + +canvas.addEventListener('touchmove', (e) => { + if (!isTouchDragging || e.touches.length !== 1) return; + e.preventDefault(); + const touch = e.touches[0]; + const rect = canvas.getBoundingClientRect(); + const scale = Math.min(rect.width / canvas.width, rect.height / canvas.height); + + const dx = touch.clientX - touchStartCoords.x; + const dy = touch.clientY - touchStartCoords.y; + + if (Math.abs(dx) > 5 || Math.abs(dy) > 5) { + currentDragYaw = Math.atan2(-dy, dx); + let yawDeg = Math.round(currentDragYaw * 180.0 / Math.PI); + if (yawDeg < 0) yawDeg += 360; + document.getElementById('wp-yaw').value = yawDeg; + + if (poseEstimateMode) { + robotPose.yaw = currentDragYaw; + } else if (currentMode === 'nav2') { + nav2Goal.yaw = currentDragYaw; + } else { + if (parkingWaypoint) parkingWaypoint.yaw = currentDragYaw; + } + drawMap(); + } +}, { passive: false }); + +canvas.addEventListener('touchend', (e) => { + if (isTouchDragging) { + isTouchDragging = false; + if (poseEstimateMode) { + publishInitialPose(dragStartRos.x, dragStartRos.y, currentDragYaw); + poseEstimateMode = false; + document.getElementById('btn-pose-estimate').className = 'btn btn-secondary'; + document.getElementById('btn-pose-estimate').innerText = '📍 로봇 위치 초기화 (Pose Estimate)'; + } else if (currentMode === 'nav2') { + publishNav2Goal(); + } else { + publishWaypoint(); + } + } +}); + // Double-click to reset zoom & pan translation canvas.addEventListener('dblclick', () => { zoom = 1.0; @@ -844,6 +1063,12 @@ function publishNav2Goal() { } function publishInitialPose(x, y, yaw) { + // Covariance is 6x6 row-major, indices [0][0]=x, [1][1]=y, [5][5]=yaw + const cov = new Array(36).fill(0.0); + cov[0] = 0.25; // xx uncertainty (0.5m std dev) + cov[7] = 0.25; // yy uncertainty (0.5m std dev) + cov[35] = 0.06853891945200942; // yaw*yaw uncertainty (~15deg std dev) + const msg = new ROSLIB.Message({ header: { frame_id: 'map', @@ -859,18 +1084,30 @@ function publishInitialPose(x, y, yaw) { w: Math.cos(yaw * 0.5) } }, - covariance: [ - 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.06853891945200942, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942 - ] + covariance: cov } }); initialPosePub.publish(msg); console.log(`Published Initial Pose Reset: x=${x.toFixed(2)}, y=${y.toFixed(2)}, yaw=${(yaw * 180 / Math.PI).toFixed(0)}deg`); - drawMap(); +} + +// --- [Mobile UI Functions] --- + +// Toggle Mobile Drawer Sidebar +function toggleMobileSidebar() { + const sidebar = document.getElementById('appSidebar'); + const overlay = document.getElementById('sidebarOverlay'); + if (sidebar && overlay) { + sidebar.classList.toggle('mobile-open'); + overlay.classList.toggle('active'); + } +} + +if ('serviceWorker' in navigator) { + window.addEventListener('load', () => { + navigator.serviceWorker.register('./sw.js') + .then(reg => console.log('PWA ServiceWorker registered:', reg.scope)) + .catch(err => console.warn('PWA ServiceWorker registration failed:', err)); + }); } diff --git a/src/fori_serial_bridge/ui/index.html b/src/fori_serial_bridge/ui/index.html index d027adc..814febf 100644 --- a/src/fori_serial_bridge/ui/index.html +++ b/src/fori_serial_bridge/ui/index.html @@ -1,194 +1,318 @@ - - - FORI AGV Control Dashboard - - - - - + + + AZMO - FORI AGV 자율주행 관제 대시보드 + + + + + + + + + + + + + + + -
- -
-
-

FORI AGV

- BLDC & ArUco Parking System -
-
- Simulation Mode - - Disconnected -
-
- + + + +
+ + + + + +
+ + +
+
+ + + + FORI AGV + +
+ AZMO 관제 Center + > + FORI AGV 자율주행 대시보드 +
+
+ +
+ +
+ Simulation + + Disconnected +
+
+
+ + +
+ +
+
+ AGV 실시간 관제 및 주행 제어 +
+
+ + +
+ + +
+
+ FORI 배터리 잔량 + 85% + 24V 60Ah LiFePO4 +
+
+ + + + +
85%
+
+
+ + +
+
+ 통신 & ROS2 연결 + 정상 + rosbridge ws://9090 +
+
+ + + + +
OK
+
+
+ + +
+
+ 현재 주행 모드 + Nav2 + BLDC & ArUco Parking System +
+
+ + + + +
RUN
+
+
+ +
+ +
+ + +
+
+

🗺️ 실시간 2D 맵 시각화 및 Waypoint 설정

+ 지도를 클릭하여 주차 진입점(Waypoint)을 등록하거나 드래그하여 목표 방향을 지정하세요. +
+ + +
+ +
+ + +
+

📍 로봇 실시간 현재 위치 (Pose)

+
+ X: 0.00m  |  Y: 0.00m  |  Yaw: 0° +
+
+ + +
+

📍 주차 진입점 (Waypoint) 설정

+
+
+ + +
+
+ + +
+
+ + +
+
+ +
+ + + +
+ + +
- -
-
-

🗺️ 실시간 2D 맵 시각화

- 지도를 클릭하여 주차 진입점(Waypoint)을 등록하세요. + +
+
+

📷 실시간 아루코 인식 카메라

+
+
+
+ 카메라 스트림 대기 중...
-
- -
-
-

📍 주차 진입점 (Waypoint) 설정

-
-
- - -
-
- - -
-
- - -
+
+
미인식 (No Marker)
+
+
+ 마커 ID + -
- -
-
-

📍 로봇 실시간 현재 위치 (Pose)

-
- X: 0.00m  |  Y: 0.00m  |  Yaw: 0° +
+ Z (직선 거리) + -
-
-
-
-

🔋 FORI 배터리 잔량 (24V 60Ah LiFePO4)

- --% (--.-V) +
+ X (가로 오프셋) + -
-
-
+
+ Y (높이 오프셋) + -
+
+ Yaw (회전) + - +
+
+ Roll/Pitch + - +
+
+
- -
- - -
-
-

📷 실시간 아루코 인식 카메라

-
-
-
- 카메라 스트림 대기 중... -
-
-
미인식 (No Marker)
-
-
- 마커 ID - - -
-
- Z (직선 거리) - - -
-
- X (가로 오프셋) - - -
-
- Y (높이 오프셋) - - -
-
- Yaw (좌우 회전) - - -
-
- Roll / Pitch - - -
-
-
-
-
+ +
+
+

⚙️ 제어 모드 및 텔레메트리

+
+ + +
+ + + +
- -
-
-

⚙️ 제어 모드 및 텔레메트리

-
- - -
- - - -
- - +
+ + +
- -
-
- 현재 시스템 상태 - IDLE -
-
- 로봇 선속도 (Linear) - 0.00 m/s -
-
- 로봇 각속도 (Angular) - 0.00 rad/s -
-
+ +
+
+ 현재 시스템 상태 + IDLE +
+
+ 선속도 (Linear) + 0.00 m/s +
+
+ 각속도 (Angular) + 0.00 rad/s +
+
- -
-

🔄 실시간 바퀴 회전수 (Feedback RPM)

-
-
- Front Left -
-
-
- 0 RPM -
-
- Front Right -
-
-
- 0 RPM -
-
- Rear Left -
-
-
- 0 RPM -
-
- Rear Right -
-
-
- 0 RPM -
-
+ +
+

🔄 실시간 바퀴 회전수 (Feedback RPM)

+
+
+ Front Left +
+
-
-
+ 0 RPM +
+
+ Front Right +
+
+
+ 0 RPM +
+
+ Rear Left +
+
+
+ 0 RPM +
+
+ Rear Right +
+
+
+ 0 RPM +
+
+
+ + +
+ + + +
- + + + diff --git a/src/fori_serial_bridge/ui/manifest.json b/src/fori_serial_bridge/ui/manifest.json new file mode 100644 index 0000000..eacd18e --- /dev/null +++ b/src/fori_serial_bridge/ui/manifest.json @@ -0,0 +1,18 @@ +{ + "name": "FORI AGV 자율주행 관제", + "short_name": "FORI AGV", + "description": "AZMO - FORI AGV 모바일 자율주행 관제 대시보드", + "start_url": "./index.html", + "display": "standalone", + "orientation": "portrait", + "background_color": "#F4F6F5", + "theme_color": "#22A774", + "icons": [ + { + "src": "data:image/svg+xml,⚡", + "sizes": "192x192 512x512", + "type": "image/svg+xml", + "purpose": "any maskable" + } + ] +} diff --git a/src/fori_serial_bridge/ui/style.css b/src/fori_serial_bridge/ui/style.css index 24d440c..d627ec6 100644 --- a/src/fori_serial_bridge/ui/style.css +++ b/src/fori_serial_bridge/ui/style.css @@ -1,516 +1,1097 @@ +/* FORI AGV Control Dashboard - AZMO Modern Design System (image_1 style) */ +@import url('https://cdn.jsdelivr.net/gh/orioncactus/pretendard@v1.3.9/dist/web/static/pretendard.min.css'); +@import url('https://fonts.googleapis.com/css2?family=Outfit:wght@400;500;600;700;800&display=swap'); + :root { - --bg-dark: #0a0b10; - --card-bg: rgba(20, 22, 37, 0.55); - --border-color: rgba(255, 255, 255, 0.08); - --accent-blue: #00d2ff; - --accent-purple: #9d4edd; - --text-main: #f3f4f6; - --text-muted: #9ca3af; - --green-glow: #10b981; - --red-glow: #ef4444; + /* AZMO Theme Colors from image_1 */ + --sidebar-gradient: linear-gradient(180deg, #28AE7B 0%, #1E9165 60%, #15734F 100%); + --sidebar-bg: #22A774; + --sidebar-hover: rgba(255, 255, 255, 0.1); + --sidebar-active: #FFFFFF; + + --bg-main: #F4F6F5; + --bg-card: #FFFFFF; + --bg-card-subtle: #F8FAFC; + + --text-main: #1E293B; + --text-muted: #64748B; + --text-subtle: #94A3B8; + + --primary-green: #22A774; + --primary-green-dark: #18865C; + --primary-green-light: #ECFDF5; + --primary-green-border: #A7F3D0; + + --accent-blue: #3B82F6; + --accent-blue-light: #EFF6FF; + --accent-blue-border: #BFDBFE; + + --status-normal: #10B981; + --status-warning: #F59E0B; + --status-error: #EF4444; + + --border-color: #E2E8F0; + --border-color-light: #F1F5F9; + + --shadow-sm: 0 2px 4px rgba(0, 0, 0, 0.02); + --shadow-md: 0 4px 16px -2px rgba(0, 0, 0, 0.04), 0 2px 6px -1px rgba(0, 0, 0, 0.02); + --shadow-lg: 0 10px 30px -5px rgba(0, 0, 0, 0.06), 0 4px 12px -2px rgba(0, 0, 0, 0.03); + --shadow-glow: 0 0 0 3px rgba(34, 167, 116, 0.15); + + --radius-sm: 8px; + --radius-md: 12px; + --radius-lg: 16px; + --radius-full: 9999px; + + --font-sans: 'Pretendard', 'Outfit', -apple-system, system-ui, sans-serif; } * { - margin: 0; - padding: 0; - box-sizing: border-box; - font-family: 'Outfit', sans-serif; - -webkit-font-smoothing: antialiased; + box-sizing: border-box; + margin: 0; + padding: 0; + font-family: var(--font-sans); + -webkit-font-smoothing: antialiased; } body { - background-color: var(--bg-dark); - color: var(--text-main); - min-height: 100vh; - overflow-x: hidden; - background-image: - radial-gradient(at 10% 20%, rgba(157, 78, 221, 0.1) 0px, transparent 50%), - radial-gradient(at 90% 80%, rgba(0, 210, 255, 0.1) 0px, transparent 50%); + background-color: var(--bg-main); + color: var(--text-main); + min-height: 100vh; + display: flex; + overflow-x: hidden; } +/* App Container with Green Sidebar */ .app-container { - max-width: 1400px; - margin: 0 auto; - padding: 20px; + display: flex; + width: 100vw; + min-height: 100vh; } -/* Header */ -.dashboard-header { - display: flex; - justify-content: space-between; - align-items: center; - padding: 15px 25px; - background: var(--card-bg); - border: 1px solid var(--border-color); - border-radius: 16px; - backdrop-filter: blur(12px); - margin-bottom: 25px; - box-shadow: 0 8px 32px 0 rgba(0, 0, 0, 0.37); +/* Emerald Green Sidebar */ +.sidebar { + width: 240px; + background: var(--sidebar-gradient); + color: #FFFFFF; + display: flex; + flex-direction: column; + flex-shrink: 0; + box-shadow: 4px 0 20px rgba(0, 0, 0, 0.04); + z-index: 10; } -.logo-area h1 { - font-size: 24px; - font-weight: 800; - letter-spacing: 1px; +.sidebar-header { + padding: 24px 20px 28px 24px; + display: flex; + align-items: center; + gap: 12px; + border-bottom: 1px solid rgba(255, 255, 255, 0.12); } -.logo-area h1 span { - color: var(--accent-blue); - text-shadow: 0 0 10px rgba(0, 210, 255, 0.4); +.logo-icon { + width: 36px; + height: 36px; + background: rgba(255, 255, 255, 0.2); + border-radius: 10px; + display: flex; + align-items: center; + justify-content: center; + font-size: 18px; + font-weight: 800; + color: #FFFFFF; } -.system-status { - font-size: 12px; - color: var(--text-muted); - font-weight: 300; - margin-left: 10px; +.logo-text { + font-size: 22px; + font-weight: 800; + letter-spacing: -0.5px; + color: #FFFFFF; +} + +.logo-text span { + color: #A7F3D0; + font-size: 14px; + font-weight: 600; + margin-left: 4px; +} + +.sidebar-nav { + padding: 20px 12px; + display: flex; + flex-direction: column; + gap: 6px; + flex: 1; +} + +.nav-item { + display: flex; + align-items: center; + gap: 12px; + padding: 12px 16px; + color: rgba(255, 255, 255, 0.85); + text-decoration: none; + font-size: 14px; + font-weight: 500; + border-radius: var(--radius-md); + transition: all 0.2s ease; + cursor: pointer; +} + +.nav-item:hover { + background: var(--sidebar-hover); + color: #FFFFFF; + transform: translateX(3px); +} + +.nav-item.active { + background: #FFFFFF; + color: var(--primary-green-dark); + font-weight: 700; + box-shadow: 0 4px 12px rgba(0, 0, 0, 0.08); +} + +.sidebar-footer { + padding: 20px 16px; + border-top: 1px solid rgba(255, 255, 255, 0.12); + font-size: 12px; + color: rgba(255, 255, 255, 0.6); + text-align: center; +} + +/* Main Content Wrapper */ +.main-wrapper { + flex: 1; + display: flex; + flex-direction: column; + min-width: 0; + overflow-y: auto; +} + +/* Header Navbar */ +.top-header { + height: 64px; + background: #FFFFFF; + border-bottom: 1px solid var(--border-color); + display: flex; + align-items: center; + justify-content: space-between; + padding: 0 32px; + position: sticky; + top: 0; + z-index: 5; +} + +.header-left { + display: flex; + align-items: center; + gap: 16px; +} + +.page-breadcrumb { + display: flex; + align-items: center; + gap: 8px; + font-size: 13px; + color: var(--text-muted); +} + +.header-right { + display: flex; + align-items: center; + gap: 20px; } .connection-status { - display: flex; - align-items: center; - gap: 10px; - font-size: 14px; - font-weight: 600; + display: flex; + align-items: center; + gap: 10px; + font-size: 13px; + font-weight: 600; + background: var(--bg-card-subtle); + padding: 6px 14px; + border-radius: var(--radius-full); + border: 1px solid var(--border-color); } .pulse-indicator { - width: 10px; - height: 10px; - border-radius: 50%; - display: inline-block; + width: 8px; + height: 8px; + border-radius: 50%; + display: inline-block; } .pulse-indicator.green { - background-color: var(--green-glow); - box-shadow: 0 0 10px var(--green-glow); - animation: pulse 1.8s infinite; + background-color: var(--status-normal); + box-shadow: 0 0 0 3px rgba(16, 185, 129, 0.2); } .pulse-indicator.red { - background-color: var(--red-glow); - box-shadow: 0 0 10px var(--red-glow); - animation: pulse-red 1.8s infinite; + background-color: var(--status-error); + box-shadow: 0 0 0 3px rgba(239, 68, 68, 0.2); } -@keyframes pulse { - 0% { transform: scale(0.95); box-shadow: 0 0 0 0 rgba(16, 185, 129, 0.7); } - 70% { transform: scale(1); box-shadow: 0 0 0 8px rgba(16, 185, 129, 0); } - 100% { transform: scale(0.95); box-shadow: 0 0 0 0 rgba(16, 185, 129, 0); } +.env-badge { + padding: 4px 10px; + border-radius: var(--radius-full); + font-size: 11px; + font-weight: 700; + text-transform: uppercase; } -@keyframes pulse-red { - 0% { transform: scale(0.95); box-shadow: 0 0 0 0 rgba(239, 68, 68, 0.7); } - 70% { transform: scale(1); box-shadow: 0 0 0 8px rgba(239, 68, 68, 0); } - 100% { transform: scale(0.95); box-shadow: 0 0 0 0 rgba(239, 68, 68, 0); } +.env-badge.sim { + background: #FFFBEB; + color: #B45309; + border: 1px solid #FDE68A; } -/* Grid Layout */ +.env-badge.real { + background: var(--primary-green-light); + color: var(--primary-green-dark); + border: 1px solid var(--primary-green-border); +} + +.user-profile { + display: flex; + align-items: center; + gap: 10px; +} + +.avatar { + width: 34px; + height: 34px; + background: var(--primary-green-light); + color: var(--primary-green-dark); + border-radius: 50%; + display: flex; + align-items: center; + justify-content: center; + font-weight: 700; + font-size: 13px; + border: 2px solid var(--primary-green-border); +} + +/* Main Dashboard Layout */ +.dashboard-content { + padding: 28px 32px 40px 32px; + display: flex; + flex-direction: column; + gap: 24px; + max-width: 1700px; + margin: 0 auto; + width: 100%; +} + +.section-title-bar { + display: flex; + align-items: center; + justify-content: space-between; +} + +.section-title { + font-size: 22px; + font-weight: 800; + color: var(--text-main); + letter-spacing: -0.4px; +} + +/* Metric Gauge Summary Section ("Total Condition" derived from image_1) */ +.metrics-grid { + display: grid; + grid-template-columns: repeat(3, 1fr); + gap: 20px; +} + +.metric-card { + background: var(--bg-card); + border-radius: var(--radius-lg); + padding: 20px 24px; + border: 1px solid var(--border-color); + box-shadow: var(--shadow-md); + display: flex; + align-items: center; + justify-content: space-between; + transition: transform 0.2s ease, box-shadow 0.2s ease; +} + +.metric-card:hover { + transform: translateY(-2px); + box-shadow: var(--shadow-lg); +} + +.metric-info { + display: flex; + flex-direction: column; + gap: 6px; +} + +.metric-label { + font-size: 13px; + font-weight: 600; + color: var(--text-muted); + text-transform: uppercase; +} + +.metric-value { + font-size: 28px; + font-weight: 800; + color: var(--text-main); + line-height: 1.1; +} + +.metric-subtext { + font-size: 12px; + color: var(--text-subtle); + margin-top: 2px; +} + +/* SVG Donut Ring Gauge Component */ +.ring-gauge-container { + position: relative; + width: 72px; + height: 72px; + display: flex; + align-items: center; + justify-content: center; +} + +.ring-gauge-svg { + transform: rotate(-90deg); + width: 72px; + height: 72px; +} + +.ring-bg { + fill: none; + stroke: #F1F5F9; + stroke-width: 6; +} + +.ring-fill { + fill: none; + stroke-width: 6; + stroke-linecap: round; + transition: stroke-dashoffset 0.8s ease; +} + +.ring-fill.green { stroke: var(--primary-green); } +.ring-fill.blue { stroke: var(--accent-blue); } +.ring-fill.amber { stroke: var(--status-warning); } + +.ring-center-text { + position: absolute; + font-size: 13px; + font-weight: 800; + color: var(--text-main); +} + +/* Dashboard Main Grid (2 Columns) */ .dashboard-grid { - display: grid; - grid-template-columns: 1.2fr 1fr; - gap: 25px; + display: grid; + grid-template-columns: 1.25fr 1fr; + gap: 24px; } -@media (max-width: 1024px) { - .dashboard-grid { - grid-template-columns: 1fr; - } +@media (max-width: 1200px) { + .dashboard-grid { + grid-template-columns: 1fr; + } } -/* Glassmorphism Card Style */ +/* Cards (Soft Shadows & Rounded Corners) */ .grid-card { - background: var(--card-bg); - border: 1px solid var(--border-color); - border-radius: 20px; - backdrop-filter: blur(12px); - padding: 20px; - box-shadow: 0 8px 32px 0 rgba(0, 0, 0, 0.3); - display: flex; - flex-direction: column; + background: var(--bg-card); + border: 1px solid var(--border-color); + border-radius: var(--radius-lg); + padding: 24px; + box-shadow: var(--shadow-md); + display: flex; + flex-direction: column; + gap: 16px; } .card-header { - margin-bottom: 15px; + display: flex; + flex-direction: column; + gap: 4px; } .card-header h2 { - font-size: 18px; - font-weight: 600; - margin-bottom: 4px; + font-size: 17px; + font-weight: 700; + color: var(--text-main); } .help-text { - font-size: 12px; - color: var(--text-muted); + font-size: 12px; + color: var(--text-muted); } -/* Map Card Details */ +/* Map Card */ .map-card { - height: 720px; + height: auto; } .canvas-container { - flex-grow: 1; - background: rgba(10, 10, 15, 0.7); - border-radius: 12px; - border: 1px solid var(--border-color); - position: relative; - overflow: hidden; - display: flex; - justify-content: center; - align-items: center; - height: 0; + width: 100%; + height: 380px; + background: #F1F5F9; + border-radius: var(--radius-md); + border: 1px solid var(--border-color); + position: relative; + overflow: hidden; + display: flex; + align-items: center; + justify-content: center; } #map-canvas { - width: 100%; - height: 100%; - object-fit: contain; - image-rendering: pixelated; - image-rendering: crisp-edges; + width: 100%; + height: 100%; + object-fit: contain; } +/* Config & Pose Panel */ .config-panel { - margin-top: 15px; - padding: 15px; - background: rgba(255, 255, 255, 0.02); - border-radius: 12px; - border: 1px solid var(--border-color); + background: var(--bg-card-subtle); + border-radius: var(--radius-md); + padding: 16px; + border: 1px solid var(--border-color); + display: flex; + flex-direction: column; + gap: 12px; } .config-panel h3 { - font-size: 14px; - font-weight: 600; - margin-bottom: 10px; + font-size: 13px; + font-weight: 700; + color: var(--text-muted); } .coord-inputs { - display: flex; - gap: 15px; - margin-bottom: 12px; + display: grid; + grid-template-columns: repeat(3, 1fr); + gap: 12px; } .input-group { - display: flex; - flex-direction: column; - flex: 1; + display: flex; + flex-direction: column; + gap: 4px; } .input-group label { - font-size: 11px; - color: var(--text-muted); - margin-bottom: 4px; + font-size: 11px; + font-weight: 600; + color: var(--text-muted); } .input-group input { - background: rgba(10, 11, 16, 0.6); - border: 1px solid var(--border-color); - border-radius: 6px; - padding: 6px 10px; - color: var(--text-main); - font-size: 14px; - outline: none; - text-align: center; + height: 38px; + padding: 0 10px; + background: #FFFFFF; + border: 1px solid var(--border-color); + border-radius: var(--radius-sm); + font-size: 13px; + font-weight: 600; + text-align: center; + color: var(--text-main); + outline: none; + transition: all 0.2s ease; } .input-group input:focus { - border-color: var(--accent-blue); + border-color: var(--primary-green); + box-shadow: var(--shadow-glow); +} + +.pose-panel { + background: var(--primary-green-light); + border: 1px solid var(--primary-green-border); + border-radius: var(--radius-md); + padding: 14px 16px; +} + +.pose-panel h3 { + font-size: 12px; + font-weight: 700; + color: var(--primary-green-dark); + margin-bottom: 4px; +} + +.pose-value-display { + font-size: 15px; + font-weight: 800; + color: var(--primary-green-dark); + text-align: center; +} + +/* Battery Panel */ +.battery-panel { + background: var(--bg-card-subtle); + border: 1px solid var(--border-color); + border-radius: var(--radius-md); + padding: 16px; } /* Right Column Panels */ .right-column { - display: flex; - flex-direction: column; - gap: 25px; + display: flex; + flex-direction: column; + gap: 24px; } -/* Camera Card */ -.camera-card { - overflow: hidden; -} - -.camera-stream-container { - background: #000; - border-radius: 12px; - border: 1px solid var(--border-color); - aspect-ratio: 4/3; - display: flex; - justify-content: center; - align-items: center; - overflow: hidden; -} - -.camera-stream-container img { - width: 100%; - height: 100%; - object-fit: cover; -} - -/* Mode selectors and buttons */ -.mode-selector { - display: flex; - gap: 15px; - margin-bottom: 20px; -} - -.btn { - flex: 1; - border: 1px solid var(--border-color); - background: rgba(255, 255, 255, 0.04); - color: var(--text-main); - padding: 12px; - border-radius: 12px; - font-size: 15px; - font-weight: 600; - cursor: pointer; - transition: all 0.3s ease; - outline: none; -} - -.btn:hover { - background: rgba(255, 255, 255, 0.1); -} - -.btn-primary.active { - background: linear-gradient(135deg, var(--accent-blue), var(--accent-purple)); - border: none; - box-shadow: 0 0 15px rgba(157, 78, 221, 0.4); -} - -.btn-secondary { - background: rgba(0, 210, 255, 0.1); - color: var(--accent-blue); - border: 1px solid rgba(0, 210, 255, 0.2); - width: 100%; -} - -.btn-secondary:hover { - background: rgba(0, 210, 255, 0.2); -} - -.btn-danger { - background: rgba(239, 68, 68, 0.12); - color: var(--red-glow); - border: 1px solid rgba(239, 68, 68, 0.25); - width: 100%; -} - -.btn-danger:hover { - background: rgba(239, 68, 68, 0.25); -} - -.btn-danger.active { - background: #ef4444 !important; - color: #ffffff !important; - border: none !important; - box-shadow: 0 0 20px rgba(239, 68, 68, 0.6) !important; -} - -.pose-panel { - margin-top: 15px; - padding-top: 15px; - border-top: 1px solid var(--border-color); -} - -.pose-panel h3 { - font-size: 14px; - font-weight: 600; - margin-bottom: 10px; - color: var(--text-muted); -} - -.pose-value-display { - background: rgba(14, 165, 233, 0.12); - border: 1px solid rgba(14, 165, 233, 0.35); - border-radius: 10px; - padding: 12px; - font-size: 16px; - font-weight: 800; - color: #38bdf8; - text-align: center; - text-shadow: 0 0 10px rgba(56, 189, 248, 0.4); - letter-spacing: 0.5px; -} - -/* Telemetry display */ -.telemetry-grid { - display: grid; - grid-template-columns: repeat(3, 1fr); - gap: 15px; - margin-bottom: 20px; -} - -.metric-box { - background: rgba(10, 11, 16, 0.5); - border: 1px solid var(--border-color); - border-radius: 12px; - padding: 12px; - text-align: center; -} - -.metric-label { - display: block; - font-size: 11px; - color: var(--text-muted); - margin-bottom: 6px; -} - -.metric-value { - font-size: 16px; - font-weight: 800; - color: var(--text-main); -} - -#val-state { - color: var(--accent-blue); - text-shadow: 0 0 10px rgba(0, 210, 255, 0.3); -} - -/* RPM Panels */ -.rpm-panel h3 { - font-size: 14px; - font-weight: 600; - margin-bottom: 12px; -} - -.rpm-bars { - display: flex; - flex-direction: column; - gap: 12px; -} - -.rpm-bar-group { - display: flex; - align-items: center; - gap: 15px; -} - -.wheel-name { - width: 80px; - font-size: 12px; - color: var(--text-muted); -} - -.bar-container { - flex-grow: 1; - height: 8px; - background: rgba(255, 255, 255, 0.05); - border-radius: 4px; - overflow: hidden; -} - -.bar { - height: 100%; - background: linear-gradient(90deg, var(--accent-blue), var(--accent-purple)); - border-radius: 4px; - transition: width 0.15s ease-out; -} - -.rpm-val { - width: 60px; - text-align: right; - font-size: 13px; - font-weight: 600; -} - -/* Environment mode badge */ -.env-badge { - padding: 5px 12px; - border-radius: 20px; - font-size: 11px; - font-weight: 700; - text-transform: uppercase; - letter-spacing: 0.5px; - border: 1px solid transparent; - margin-right: 15px; - transition: all 0.3s ease; -} - -.env-badge.sim { - background: rgba(234, 179, 8, 0.1); - color: #eab308; - border-color: rgba(234, 179, 8, 0.25); - box-shadow: 0 0 10px rgba(234, 179, 8, 0.15); -} - -.env-badge.real { - background: rgba(16, 185, 129, 0.1); - color: #10b981; - border-color: rgba(16, 185, 129, 0.25); - box-shadow: 0 0 10px rgba(16, 185, 129, 0.15); -} - -/* Camera & ArUco Layout */ +/* Camera Stream Section */ .camera-container-layout { - display: grid; - grid-template-columns: 1.3fr 1fr; - gap: 20px; - align-items: center; + display: grid; + grid-template-columns: 1.3fr 1fr; + gap: 16px; } @media (max-width: 768px) { - .camera-container-layout { - grid-template-columns: 1fr; - } + .camera-container-layout { + grid-template-columns: 1fr; + } } -/* ArUco Info Card Styles */ +.camera-stream-container { + background: #0F172A; + border-radius: var(--radius-md); + border: 1px solid var(--border-color); + aspect-ratio: 4/3; + overflow: hidden; + display: flex; + align-items: center; + justify-content: center; +} + +.camera-stream-container img { + width: 100%; + height: 100%; + object-fit: cover; +} + +/* ArUco Panel */ .aruco-info-panel { - display: flex; - flex-direction: column; - gap: 12px; + display: flex; + flex-direction: column; + gap: 10px; } .aruco-status-badge { - padding: 6px 8px; - border-radius: 8px; - text-align: center; - font-size: 12px; - font-weight: 700; - text-transform: uppercase; - letter-spacing: 0.5px; - border: 1px solid transparent; - transition: all 0.3s ease; + padding: 6px 10px; + border-radius: var(--radius-sm); + text-align: center; + font-size: 12px; + font-weight: 700; } .aruco-status-badge.disconnected { - background: rgba(239, 68, 68, 0.1); - color: var(--red-glow); - border-color: rgba(239, 68, 68, 0.2); + background: #FEF2F2; + color: var(--status-error); + border: 1px solid #FCA5A5; } .aruco-status-badge.connected { - background: rgba(16, 185, 129, 0.1); - color: var(--green-glow); - border-color: rgba(16, 185, 129, 0.2); - box-shadow: 0 0 10px rgba(16, 185, 129, 0.15); + background: var(--primary-green-light); + color: var(--primary-green-dark); + border: 1px solid var(--primary-green-border); } .aruco-details { - display: grid; - grid-template-columns: 1fr; - gap: 6px; + display: flex; + flex-direction: column; + gap: 6px; } .detail-row { - background: rgba(10, 11, 16, 0.4); - border: 1px solid var(--border-color); - border-radius: 8px; - padding: 6px 12px; - display: flex; - justify-content: space-between; - align-items: center; + background: var(--bg-card-subtle); + border: 1px solid var(--border-color); + border-radius: 6px; + padding: 6px 10px; + display: flex; + justify-content: space-between; + align-items: center; } .detail-label { - font-size: 11px; - color: var(--text-muted); + font-size: 11px; + color: var(--text-muted); } .detail-value { - font-size: 13px; - font-weight: 700; - color: var(--text-main); + font-size: 12px; + font-weight: 700; + color: var(--text-main); } +/* Controls & Buttons */ +.mode-selector { + display: grid; + grid-template-columns: repeat(3, 1fr); + gap: 10px; +} + +.btn { + height: 40px; + padding: 0 16px; + font-size: 13px; + font-weight: 600; + border-radius: var(--radius-md); + border: 1px solid var(--border-color); + background: var(--bg-card-subtle); + color: var(--text-main); + cursor: pointer; + transition: all 0.2s ease; + display: inline-flex; + align-items: center; + justify-content: center; +} + +.btn:hover { + background: #F1F5F9; + border-color: #CBD5E1; +} + +.btn-primary.active { + background: var(--primary-green); + color: #FFFFFF; + border-color: var(--primary-green-dark); + font-weight: 700; + box-shadow: 0 2px 8px rgba(34, 167, 116, 0.3); +} + +.btn-secondary { + background: var(--accent-blue-light); + color: var(--accent-blue); + border: 1px solid var(--accent-blue-border); +} + +.btn-secondary:hover { + background: #DBEAFE; +} + +.btn-danger { + background: #FEF2F2; + color: var(--status-error); + border: 1px solid #FCA5A5; +} + +.btn-danger:hover { + background: #FEE2E2; +} + +.btn-danger.active { + background: var(--status-error) !important; + color: #FFFFFF !important; + border-color: #DC2626 !important; + box-shadow: 0 0 12px rgba(239, 68, 68, 0.4) !important; +} + +/* Telemetry Grid */ +.telemetry-grid { + display: grid; + grid-template-columns: repeat(3, 1fr); + gap: 10px; +} + +.metric-box { + background: var(--bg-card-subtle); + border: 1px solid var(--border-color); + border-radius: var(--radius-md); + padding: 10px; + text-align: center; +} + +.metric-box .metric-label { + font-size: 11px; + color: var(--text-muted); + margin-bottom: 4px; +} + +.metric-box .metric-value { + font-size: 14px; + font-weight: 700; + color: var(--text-main); +} + +#val-state { + color: var(--primary-green-dark); +} + +/* RPM Bars */ +.rpm-panel h3 { + font-size: 13px; + font-weight: 700; + color: var(--text-muted); + margin-bottom: 10px; +} + +.rpm-bars { + display: flex; + flex-direction: column; + gap: 10px; +} + +.rpm-bar-group { + display: flex; + align-items: center; + gap: 12px; +} + +.wheel-name { + width: 75px; + font-size: 12px; + color: var(--text-muted); + font-weight: 500; +} + +.bar-container { + flex: 1; + height: 8px; + background: #F1F5F9; + border-radius: 4px; + overflow: hidden; + border: 1px solid var(--border-color); +} + +.bar { + height: 100%; + background: linear-gradient(90deg, var(--primary-green), #3B82F6); + border-radius: 4px; + transition: width 0.15s ease-out; +} + +.rpm-val { + width: 55px; + text-align: right; + font-size: 12px; + font-weight: 700; +} + +/* Mobile & PWA Responsive Styling */ +.mobile-hamburger-btn { + display: none; + align-items: center; + justify-content: center; + width: 38px; + height: 38px; + border-radius: var(--radius-sm); + background: var(--bg-card-subtle); + border: 1px solid var(--border-color); + font-size: 24px; + font-weight: 700; + color: var(--primary-green-dark); + cursor: pointer; +} + +.mobile-close-btn { + display: none; + background: transparent; + border: none; + color: #FFFFFF; + font-size: 24px; + cursor: pointer; + margin-left: auto; +} + +.pwa-install-btn { + background: var(--primary-green); + color: #FFFFFF; + border: none; + padding: 6px 12px; + border-radius: var(--radius-full); + font-size: 12px; + font-weight: 700; + cursor: pointer; + box-shadow: 0 2px 8px rgba(34, 167, 116, 0.3); + transition: all 0.2s ease; +} + +.mobile-header-title { + display: none; + font-size: 16px; + font-weight: 800; + color: var(--primary-green-dark); +} + +.pwa-install-btn:hover { + background: var(--primary-green-dark); + transform: translateY(-1px); +} + +.sidebar-overlay { + position: fixed; + top: 0; + left: 0; + width: 100vw; + height: 100vh; + background: rgba(15, 23, 42, 0.5); + backdrop-filter: blur(4px); + z-index: 90; + opacity: 0; + pointer-events: none; + transition: opacity 0.25s ease; +} + +.sidebar-overlay.active { + opacity: 1; + pointer-events: auto; +} + +/* Mobile Media Queries (max-width: 768px) */ +@media (max-width: 768px) { + body { + font-size: 13px; + } + + .sidebar { + position: fixed; + top: 0; + left: -280px; + height: 100vh; + width: 260px; + z-index: 100; + transition: left 0.3s cubic-bezier(0.4, 0, 0.2, 1); + box-shadow: 10px 0 30px rgba(0, 0, 0, 0.2); + } + + .sidebar.mobile-open { + left: 0; + } + + .mobile-hamburger-btn { + display: flex; + flex-shrink: 0; + } + + .mobile-header-title { + display: inline-block; + white-space: nowrap; + margin-left: 6px; + font-size: 15px; + font-weight: 800; + color: var(--primary-green-dark); + } + + .mobile-close-btn { + display: block; + } + + .top-header { + padding: 0 10px; + height: 54px; + } + + .header-left { + gap: 4px; + min-width: 0; + } + + .header-right { + gap: 4px; + flex-shrink: 0; + } + + .page-breadcrumb { + display: none !important; + } + + .connection-status { + padding: 3px 6px; + font-size: 10px; + gap: 4px; + border-radius: var(--radius-full); + } + + #status-text { + display: none; + } + + .pwa-install-btn { + padding: 4px 8px; + font-size: 10px; + white-space: nowrap; + } + + .dashboard-content { + padding: 12px; + gap: 14px; + max-width: 100%; + overflow-x: hidden; + } + + .section-title { + font-size: 16px; + } + + /* 1. Top 3 Metrics: Single Horizontal Row (1 Row x 3 Columns) */ + .metrics-grid { + grid-template-columns: repeat(3, 1fr); + gap: 4px; + width: 100%; + box-sizing: border-box; + } + + .metric-card { + padding: 8px 4px; + border-radius: var(--radius-md); + flex-direction: column; + align-items: center; + justify-content: center; + text-align: center; + gap: 2px; + width: 100%; + min-width: 0; + box-sizing: border-box; + overflow: hidden; + } + + .metric-info { + align-items: center; + text-align: center; + gap: 1px; + width: 100%; + min-width: 0; + overflow: hidden; + } + + .metric-label { + font-size: 9px; + font-weight: 700; + letter-spacing: -0.3px; + white-space: nowrap; + overflow: hidden; + text-overflow: ellipsis; + max-width: 100%; + } + + .metric-value { + font-size: 14px; + font-weight: 800; + margin: 0; + line-height: 1.1; + } + + .metric-subtext { + display: none; + } + + .ring-gauge-container { + width: 32px; + height: 32px; + flex-shrink: 0; + } + + .ring-gauge-svg { + width: 32px; + height: 32px; + } + + .ring-bg, .ring-fill { + stroke-width: 4; + } + + .ring-center-text { + font-size: 9px; + } + + /* 2. Vertical Card Stack Order on Mobile */ + .dashboard-grid { + display: flex; + flex-direction: column; + gap: 14px; + width: 100%; + box-sizing: border-box; + } + + .grid-card { + padding: 14px; + border-radius: var(--radius-md); + width: 100%; + max-width: 100%; + box-sizing: border-box; + overflow: hidden; + } + + .canvas-container { + height: 280px; + width: 100%; + box-sizing: border-box; + } + + .config-panel { + width: 100%; + box-sizing: border-box; + overflow: hidden; + padding: 10px; + } + + .coord-inputs { + display: grid; + grid-template-columns: repeat(3, 1fr); + gap: 4px; + width: 100%; + box-sizing: border-box; + overflow: hidden; + } + + .input-group { + width: 100%; + min-width: 0; + box-sizing: border-box; + } + + .input-group input { + width: 100%; + min-width: 0; + height: 34px; + font-size: 12px; + padding: 0 2px; + text-align: center; + box-sizing: border-box; + border-radius: 6px; + } + + .camera-container-layout { + grid-template-columns: 1fr; + gap: 12px; + width: 100%; + box-sizing: border-box; + } + + .mode-selector { + grid-template-columns: repeat(3, 1fr); + gap: 6px; + width: 100%; + box-sizing: border-box; + } + + .btn { + height: 36px; + padding: 0 6px; + font-size: 11px; + box-sizing: border-radius; + } + + .telemetry-grid { + grid-template-columns: repeat(3, 1fr); + gap: 4px; + width: 100%; + box-sizing: border-box; + } + + .metric-box { + padding: 6px 2px; + min-width: 0; + box-sizing: border-box; + } + + .metric-box .metric-label { + font-size: 9px; + } + + .metric-box .metric-value { + font-size: 11px; + } +} diff --git a/src/fori_serial_bridge/ui/sw.js b/src/fori_serial_bridge/ui/sw.js new file mode 100644 index 0000000..fdaa353 --- /dev/null +++ b/src/fori_serial_bridge/ui/sw.js @@ -0,0 +1,36 @@ +// FORI AGV PWA Service Worker +const CACHE_NAME = 'fori-agv-pwa-v1'; +const ASSETS = [ + './', + './index.html', + './style.css', + './app.js', + './manifest.json' +]; + +self.addEventListener('install', (event) => { + event.waitUntil( + caches.open(CACHE_NAME).then((cache) => cache.addAll(ASSETS)) + ); + self.skipWaiting(); +}); + +self.addEventListener('activate', (event) => { + event.waitUntil( + caches.keys().then((keys) => { + return Promise.all( + keys.map((key) => { + if (key !== CACHE_NAME) return caches.delete(key); + }) + ); + }) + ); + self.clients.claim(); +}); + +self.addEventListener('fetch', (event) => { + // Network first strategy with fallback to cache for real-time WebSocket dashboard + event.respondWith( + fetch(event.request).catch(() => caches.match(event.request)) + ); +}); diff --git a/src/fori_serial_bridge_cpp/CMakeLists.txt b/src/fori_serial_bridge_cpp/CMakeLists.txt new file mode 100644 index 0000000..bc562e6 --- /dev/null +++ b/src/fori_serial_bridge_cpp/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required(VERSION 3.8) +project(fori_serial_bridge_cpp) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() +add_compile_options(-O2 -Wall -Wextra) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2_ros REQUIRED) + +add_executable(serial_bridge_node + src/modbus_serial.cpp + src/serial_bridge_node.cpp +) +target_include_directories(serial_bridge_node PRIVATE + $ +) +ament_target_dependencies(serial_bridge_node + rclcpp + geometry_msgs + sensor_msgs + nav_msgs + std_msgs + tf2_ros +) + +install(TARGETS + serial_bridge_node + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/src/fori_serial_bridge_cpp/include/fori_serial_bridge_cpp/modbus_serial.hpp b/src/fori_serial_bridge_cpp/include/fori_serial_bridge_cpp/modbus_serial.hpp new file mode 100644 index 0000000..e626d9b --- /dev/null +++ b/src/fori_serial_bridge_cpp/include/fori_serial_bridge_cpp/modbus_serial.hpp @@ -0,0 +1,43 @@ +#pragma once +// Low-level RS485 Modbus RTU transport for the ZLAC8015D BLDC driver. +// +// Ported from the field-verified xbox_motor_control.cpp (fori_zltech_motor_test): +// unlike the previous Python pyserial implementation (single fixed-timeout read, +// no CRC check on write acks), every response here is accumulated in a short +// polling loop and CRC16-validated before being trusted. That is what actually +// removes the serial-jitter delay in the control loop. + +#include +#include +#include + +namespace fori_serial_bridge_cpp { + +uint16_t calcCRC16(const uint8_t *data, size_t len); + +class SerialPort { +public: + SerialPort() = default; + ~SerialPort(); + + SerialPort(const SerialPort &) = delete; + SerialPort &operator=(const SerialPort &) = delete; + + bool openPort(const std::string &port_name, int baudrate = 115200); + void closePort(); + bool isOpen() const { return fd_ >= 0; } + int fd() const { return fd_; } + + // Modbus function 0x06: write single holding register. + bool writeReg(uint8_t slave, uint16_t reg, uint16_t val); + // Modbus function 0x10: write multiple holding registers. + bool writeRegs(uint8_t slave, uint16_t reg, const std::vector &vals); + // Modbus function 0x03: read holding registers (returned as raw uint16, caller + // reinterprets signed fields as needed, matching ZLAC8015D register semantics). + bool readRegs(uint8_t slave, uint16_t reg, uint16_t count, std::vector &out_vals); + +private: + int fd_ = -1; +}; + +} // namespace fori_serial_bridge_cpp diff --git a/src/fori_serial_bridge_cpp/package.xml b/src/fori_serial_bridge_cpp/package.xml new file mode 100644 index 0000000..bf61ea1 --- /dev/null +++ b/src/fori_serial_bridge_cpp/package.xml @@ -0,0 +1,22 @@ + + + + fori_serial_bridge_cpp + 0.0.1 + Low-latency C++ port of the ZLAC8015D RS485/Arduino motor serial bridge (replaces the Python serial_bridge_node hot loop) + yoo + Apache License 2.0 + + ament_cmake + + rclcpp + geometry_msgs + sensor_msgs + nav_msgs + std_msgs + tf2_ros + + + ament_cmake + + diff --git a/src/fori_serial_bridge_cpp/src/modbus_serial.cpp b/src/fori_serial_bridge_cpp/src/modbus_serial.cpp new file mode 100644 index 0000000..5bde0b6 --- /dev/null +++ b/src/fori_serial_bridge_cpp/src/modbus_serial.cpp @@ -0,0 +1,230 @@ +#include "fori_serial_bridge_cpp/modbus_serial.hpp" + +#include +#include +#include +#include +#include +#include + +namespace fori_serial_bridge_cpp { + +uint16_t calcCRC16(const uint8_t *data, size_t len) { + uint16_t crc = 0xFFFF; + for (size_t i = 0; i < len; ++i) { + crc ^= data[i]; + for (int j = 0; j < 8; ++j) { + if (crc & 0x0001) { + crc >>= 1; + crc ^= 0xA001; + } else { + crc >>= 1; + } + } + } + return crc; +} + +SerialPort::~SerialPort() { closePort(); } + +bool SerialPort::openPort(const std::string &port_name, int baudrate) { + fd_ = open(port_name.c_str(), O_RDWR | O_NOCTTY | O_NDELAY); + if (fd_ < 0) { + return false; + } + + fcntl(fd_, F_SETFL, 0); + + struct termios options; + tcgetattr(fd_, &options); + + speed_t speed = B115200; + switch (baudrate) { + case 9600: + speed = B9600; + break; + case 19200: + speed = B19200; + break; + case 38400: + speed = B38400; + break; + case 57600: + speed = B57600; + break; + default: + speed = B115200; + break; + } + + cfsetispeed(&options, speed); + cfsetospeed(&options, speed); + + options.c_cflag &= ~PARENB; // No parity + options.c_cflag &= ~CSTOPB; // 1 stop bit + options.c_cflag &= ~CSIZE; + options.c_cflag |= CS8; // 8 data bits + options.c_cflag &= ~CRTSCTS; // No hardware flow control + options.c_cflag |= CREAD | CLOCAL; + + options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + options.c_iflag &= ~(IXON | IXOFF | IXANY | IGNBRK | BRKINT | PARMRK | + ISTRIP | INLCR | IGNCR | ICRNL); + options.c_oflag &= ~OPOST; + + options.c_cc[VMIN] = 0; + options.c_cc[VTIME] = 1; // 0.1s timeout + + tcsetattr(fd_, TCSANOW, &options); + tcflush(fd_, TCIOFLUSH); + return true; +} + +void SerialPort::closePort() { + if (fd_ >= 0) { + close(fd_); + fd_ = -1; + } +} + +bool SerialPort::writeReg(uint8_t slave, uint16_t reg, uint16_t val) { + uint8_t pkt[8]; + pkt[0] = slave; + pkt[1] = 0x06; + pkt[2] = (reg >> 8) & 0xFF; + pkt[3] = reg & 0xFF; + pkt[4] = (val >> 8) & 0xFF; + pkt[5] = val & 0xFF; + + uint16_t crc = calcCRC16(pkt, 6); + pkt[6] = crc & 0xFF; + pkt[7] = (crc >> 8) & 0xFF; + + tcflush(fd_, TCIOFLUSH); + ssize_t written = write(fd_, pkt, 8); + if (written != 8) + return false; + + if (slave == 0) + return true; // Broadcast: driver sends no response. + + uint8_t rx_buf[8]; + ssize_t rx_bytes = 0; + auto start = std::chrono::steady_clock::now(); + while (rx_bytes < 8) { + ssize_t res = read(fd_, rx_buf + rx_bytes, 8 - rx_bytes); + if (res > 0) + rx_bytes += res; + auto now = std::chrono::steady_clock::now(); + if (std::chrono::duration_cast(now - start).count() > 50) + break; + std::this_thread::sleep_for(std::chrono::microseconds(500)); + } + if (rx_bytes != 8) + return false; + + uint16_t resp_crc = calcCRC16(rx_buf, 6); + uint16_t recv_crc = static_cast(rx_buf[6]) | (static_cast(rx_buf[7]) << 8); + return resp_crc == recv_crc && rx_buf[0] == slave; +} + +bool SerialPort::writeRegs(uint8_t slave, uint16_t reg, const std::vector &vals) { + size_t count = vals.size(); + size_t pkt_len = 7 + count * 2 + 2; + std::vector pkt(pkt_len); + + pkt[0] = slave; + pkt[1] = 0x10; + pkt[2] = (reg >> 8) & 0xFF; + pkt[3] = reg & 0xFF; + pkt[4] = (count >> 8) & 0xFF; + pkt[5] = count & 0xFF; + pkt[6] = static_cast(count * 2); + + for (size_t i = 0; i < count; ++i) { + pkt[7 + i * 2] = (vals[i] >> 8) & 0xFF; + pkt[8 + i * 2] = vals[i] & 0xFF; + } + + uint16_t crc = calcCRC16(pkt.data(), pkt_len - 2); + pkt[pkt_len - 2] = crc & 0xFF; + pkt[pkt_len - 1] = (crc >> 8) & 0xFF; + + tcflush(fd_, TCIOFLUSH); + ssize_t written = write(fd_, pkt.data(), pkt_len); + if (written != static_cast(pkt_len)) + return false; + + if (slave == 0) + return true; + + uint8_t rx_buf[8]; + ssize_t rx_bytes = 0; + auto start = std::chrono::steady_clock::now(); + while (rx_bytes < 8) { + ssize_t res = read(fd_, rx_buf + rx_bytes, 8 - rx_bytes); + if (res > 0) + rx_bytes += res; + auto now = std::chrono::steady_clock::now(); + if (std::chrono::duration_cast(now - start).count() > 50) + break; + std::this_thread::sleep_for(std::chrono::microseconds(500)); + } + if (rx_bytes != 8) + return false; + + uint16_t resp_crc = calcCRC16(rx_buf, 6); + uint16_t recv_crc = static_cast(rx_buf[6]) | (static_cast(rx_buf[7]) << 8); + return resp_crc == recv_crc && rx_buf[0] == slave; +} + +bool SerialPort::readRegs(uint8_t slave, uint16_t reg, uint16_t count, std::vector &out_vals) { + uint8_t pkt[8]; + pkt[0] = slave; + pkt[1] = 0x03; + pkt[2] = (reg >> 8) & 0xFF; + pkt[3] = reg & 0xFF; + pkt[4] = (count >> 8) & 0xFF; + pkt[5] = count & 0xFF; + + uint16_t crc = calcCRC16(pkt, 6); + pkt[6] = crc & 0xFF; + pkt[7] = (crc >> 8) & 0xFF; + + tcflush(fd_, TCIOFLUSH); + if (write(fd_, pkt, 8) != 8) + return false; + + size_t expected_bytes = 5 + count * 2; + std::vector rx_buf(expected_bytes); + size_t rx_bytes = 0; + + auto start = std::chrono::steady_clock::now(); + while (rx_bytes < expected_bytes) { + ssize_t res = read(fd_, rx_buf.data() + rx_bytes, expected_bytes - rx_bytes); + if (res > 0) + rx_bytes += res; + auto now = std::chrono::steady_clock::now(); + // 100Hz(10ms) 루프 안에서도 응답을 안정적으로 받도록 25ms까지 허용. + if (std::chrono::duration_cast(now - start).count() > 25) + break; + std::this_thread::sleep_for(std::chrono::microseconds(100)); + } + + if (rx_bytes == expected_bytes && rx_buf[0] == slave && rx_buf[1] == 0x03) { + uint16_t resp_crc = calcCRC16(rx_buf.data(), expected_bytes - 2); + uint16_t recv_crc = static_cast(rx_buf[expected_bytes - 2]) | + (static_cast(rx_buf[expected_bytes - 1]) << 8); + if (resp_crc != recv_crc) + return false; + + out_vals.resize(count); + for (uint16_t i = 0; i < count; ++i) { + out_vals[i] = (static_cast(rx_buf[3 + i * 2]) << 8) | rx_buf[4 + i * 2]; + } + return true; + } + return false; +} + +} // namespace fori_serial_bridge_cpp diff --git a/src/fori_serial_bridge_cpp/src/serial_bridge_node.cpp b/src/fori_serial_bridge_cpp/src/serial_bridge_node.cpp new file mode 100644 index 0000000..8d53068 --- /dev/null +++ b/src/fori_serial_bridge_cpp/src/serial_bridge_node.cpp @@ -0,0 +1,567 @@ +// C++ port of fori_serial_bridge/fori_serial_bridge/serial_bridge_node.py. +// +// This node owns the only hardware-realtime loop in the FORI stack (20Hz RS485 +// Modbus RTU to the ZLAC8015D BLDC drivers, or the Arduino serial fallback). +// The control algorithm (accel ramp + IMU yaw-rate PI correction, differential +// kinematics, odometry integration, battery filtering) is kept identical to the +// Python original; only the low-level serial transport is replaced with the +// CRC-validated, short-poll implementation verified on the real robot in +// zlac8015d_motor_test/fori_zltech_motor_test/xbox_motor_control.cpp. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "sensor_msgs/msg/battery_state.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "std_msgs/msg/string.hpp" +#include "tf2_ros/transform_broadcaster.h" + +#include "fori_serial_bridge_cpp/modbus_serial.hpp" + +using namespace std::chrono_literals; + +namespace fori_serial_bridge_cpp { + +namespace { +constexpr double kPi = 3.14159265358979323846; + +double quatToYaw(double w, double x, double y, double z) { + double siny_cosp = 2.0 * (w * z + x * y); + double cosy_cosp = 1.0 - 2.0 * (y * y + z * z); + return std::atan2(siny_cosp, cosy_cosp); +} +} // namespace + +class ForiSerialBridge : public rclcpp::Node { +public: + ForiSerialBridge() : rclcpp::Node("fori_serial_bridge") { + declare_parameter("control_method", std::string("direct_pc")); + declare_parameter("port", std::string("/dev/ttyUSB0")); + declare_parameter("baud", 115200); + declare_parameter("target_linear_speed", 0.5); + declare_parameter("accel_limit", 0.5); + declare_parameter("wheel_base", 0.374); + declare_parameter("wheel_radius", 0.127); + // FAST_LIO/FAST-LIVO2가 함께 도는 실주행 모드에서는 그쪽이 이미 + // camera_init->aft_mapped->base_link TF 체인을 소유하고 있어서, 여기서 동시에 + // odom->base_link를 쏘면 base_link에 부모가 둘(aft_mapped, odom) 생겨 TF 트리가 + // 충돌한다. LIDAR SLAM 없이 순수 휠 오도메트리로만 굴릴 때(모의주행/벤치 테스트)만 + // true로 켠다. + declare_parameter("publish_odom_tf", true); + + control_method_ = get_parameter("control_method").as_string(); + port_ = get_parameter("port").as_string(); + baud_ = get_parameter("baud").as_int(); + limit_v_ = get_parameter("target_linear_speed").as_double(); + accel_limit_ = get_parameter("accel_limit").as_double(); + publish_odom_tf_ = get_parameter("publish_odom_tf").as_bool(); + wheel_base_ = get_parameter("wheel_base").as_double(); + wheel_radius_ = get_parameter("wheel_radius").as_double(); + + last_time_ = now(); + + RCLCPP_INFO(get_logger(), "=========================================="); + RCLCPP_INFO(get_logger(), " FORI BLDC ZLAC8015D CONTROL BRIDGE (C++) "); + RCLCPP_INFO(get_logger(), "=========================================="); + RCLCPP_INFO(get_logger(), " Control Method: %s", control_method_.c_str()); + RCLCPP_INFO(get_logger(), " Serial Port: %s at %d baud", port_.c_str(), baud_); + RCLCPP_INFO(get_logger(), "=========================================="); + + if (control_method_ == "direct_pc") { + if (modbus_port_.openPort(port_, baud_)) { + RCLCPP_INFO(get_logger(), "BLDC 드라이버 초기화 대기 중..."); + initDirectDrivers(); + RCLCPP_INFO(get_logger(), "BLDC 드라이버 초기화 완료!"); + } else { + RCLCPP_WARN(get_logger(), "!!! 드라이버 연결 실패: %s !!!", port_.c_str()); + RCLCPP_WARN(get_logger(), "실제 모터 드라이버 연결을 찾을 수 없어 [가상 모의 주행(Mock Mode)]으로 동작합니다."); + is_mock_mode_ = true; + } + } else if (control_method_ == "arduino") { + if (arduino_port_.openPort(port_, baud_)) { + RCLCPP_INFO(get_logger(), "아두이노 초기화 대기 중 (3초)..."); + std::this_thread::sleep_for(3s); + RCLCPP_INFO(get_logger(), "아두이노 메가 통신 준비 완료!"); + } else { + RCLCPP_WARN(get_logger(), "!!! 아두이노 시리얼 연결 실패: %s !!!", port_.c_str()); + RCLCPP_WARN(get_logger(), "아두이노 연결을 찾을 수 없어 [가상 모의 주행(Mock Mode)]으로 동작합니다."); + is_mock_mode_ = true; + } + } + + rclcpp::QoS qos_depth1(1); + + cmd_vel_sub_ = create_subscription( + "/cmd_vel", qos_depth1, + [this](const geometry_msgs::msg::Twist::SharedPtr msg) { cmdVelCallback(msg); }); + imu_sub_ = create_subscription( + "/livox/imu", qos_depth1, + [this](const sensor_msgs::msg::Imu::SharedPtr msg) { imuCallback(msg); }); + initial_pose_sub_ = create_subscription( + "/initialpose", 10, + [this](const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { + initialPoseCallback(msg); + }); + mode_status_sub_ = create_subscription( + "/robot_mode_status", 10, + [this](const std_msgs::msg::String::SharedPtr msg) { modeStatusCallback(msg); }); + + odom_pub_ = create_publisher("/odom_wheels", qos_depth1); + joint_pub_ = create_publisher("/joint_states", qos_depth1); + battery_pub_ = create_publisher("/battery_state", qos_depth1); + tf_broadcaster_ = std::make_unique(*this); + + control_timer_ = create_wall_timer(50ms, [this]() { controlLoop(); }); + battery_timer_ = create_wall_timer(1000ms, [this]() { publishBatteryStatus(); }); + } + + // Safe stop invoked from the shutdown hook (Ctrl+C), mirrors the Python + // KeyboardInterrupt handler in main(). + void safeStop() { + if (control_method_ == "direct_pc") { + disableDirectDrivers(); + } else if (control_method_ == "arduino") { + uint8_t packet[7] = {0}; + packet[0] = 0xFE; + packet[5] = 0; + uint8_t checksum = 0; + for (int i = 1; i < 6; ++i) + checksum += packet[i]; + packet[6] = checksum; + if (arduino_port_.isOpen()) { + ssize_t written = write(arduino_port_.fd(), packet, sizeof(packet)); + (void)written; + } + } + RCLCPP_INFO(get_logger(), "ROS2 BRIDGE STOPPED"); + } + +private: + // ------------------------------------------------------------------- + // Subscription callbacks + // ------------------------------------------------------------------- + void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { + target_v_ = std::clamp(msg->linear.x, -limit_v_, limit_v_); + target_w_ = msg->angular.z; + } + + void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) { + current_imu_w_ = msg->angular_velocity.z; + } + + void initialPoseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { + odom_x_ = msg->pose.pose.position.x; + odom_y_ = msg->pose.pose.position.y; + const auto &q = msg->pose.pose.orientation; + odom_th_ = quatToYaw(q.w, q.x, q.y, q.z); + RCLCPP_INFO(get_logger(), "[MOCK] Reset robot pose to: x=%.2f, y=%.2f, yaw=%.2f", odom_x_, odom_y_, odom_th_); + } + + void modeStatusCallback(const std_msgs::msg::String::SharedPtr msg) { + is_parked_ = msg->data.find("State: PARKED") != std::string::npos; + } + + // ------------------------------------------------------------------- + // Direct-PC (RS485 Modbus) driver control, ported 1:1 from the Python + // ZLAC8015D register sequence but using the CRC-validated SerialPort. + // ------------------------------------------------------------------- + void initDirectDrivers() { + modbus_port_.writeReg(1, 0x200D, 3); // Velocity mode + std::this_thread::sleep_for(10ms); + modbus_port_.writeReg(2, 0x200D, 3); + std::this_thread::sleep_for(10ms); + disableDirectDrivers(); + drivers_initialized_ = true; + } + + void enableDirectDrivers() { + if (drivers_enabled_) + return; + modbus_port_.writeReg(1, 0x201A, 0); + modbus_port_.writeReg(1, 0x201B, 0); + modbus_port_.writeReg(2, 0x201A, 0); + modbus_port_.writeReg(2, 0x201B, 0); + std::this_thread::sleep_for(10ms); + modbus_port_.writeReg(1, 0x200E, 8); + modbus_port_.writeReg(2, 0x200E, 8); + drivers_enabled_ = true; + RCLCPP_INFO(get_logger(), "BLDC Drivers and Brakes Released (ENABLED)"); + } + + void disableDirectDrivers() { + modbus_port_.writeRegs(1, 0x2088, {0, 0}); + modbus_port_.writeRegs(2, 0x2088, {0, 0}); + modbus_port_.writeReg(1, 0x201A, 1); + modbus_port_.writeReg(1, 0x201B, 1); + modbus_port_.writeReg(2, 0x201A, 1); + modbus_port_.writeReg(2, 0x201B, 1); + std::this_thread::sleep_for(10ms); + modbus_port_.writeReg(1, 0x200E, 7); + modbus_port_.writeReg(2, 0x200E, 7); + drivers_enabled_ = false; + RCLCPP_INFO(get_logger(), "BLDC Brakes Locked (DISABLED)"); + } + + // ------------------------------------------------------------------- + // Battery telemetry (1Hz) + // ------------------------------------------------------------------- + void publishBatteryStatus() { + sensor_msgs::msg::BatteryState msg; + msg.header.stamp = now(); + msg.header.frame_id = "base_link"; + msg.design_capacity = 60.0f; + msg.capacity = 60.0f; + msg.power_supply_technology = sensor_msgs::msg::BatteryState::POWER_SUPPLY_TECHNOLOGY_LIFE; + + constexpr double v_min = 22.4; + constexpr double v_max = 29.2; + + std::optional voltage; + + if (!is_mock_mode_ && control_method_ == "direct_pc") { + std::vector v_resp; + if (modbus_port_.readRegs(1, 0x20A0, 2, v_resp) && !v_resp.empty()) { + double raw_a0 = std::abs(static_cast(static_cast(v_resp[0]))); + double raw_a1 = v_resp.size() > 1 + ? std::abs(static_cast(static_cast(v_resp[1]))) + : raw_a0; + + double raw_v = 0.0; + if (raw_a0 >= 200 && raw_a0 <= 320) { + raw_v = raw_a0 * 0.1; + } else if (raw_a0 >= 2000 && raw_a0 <= 3200) { + raw_v = raw_a0 * 0.01; + } else if (raw_a1 >= 200 && raw_a1 <= 320) { + raw_v = raw_a1 * 0.1; + } else if (raw_a1 >= 2000 && raw_a1 <= 3200) { + raw_v = raw_a1 * 0.01; + } else { + raw_v = raw_a1 > 1000 ? raw_a1 * 0.01 : (raw_a1 > 100 ? raw_a1 * 0.1 : raw_a1); + } + + if (raw_v >= 20.0 && raw_v <= 32.0) { + voltage = raw_v; + last_valid_voltage_ = voltage; + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 3000, + "[BATTERY] Driver raw: %.2fV | %.1f%%", raw_v, + std::clamp((raw_v - v_min) / (v_max - v_min) * 100.0, 0.0, 100.0)); + } else { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, + "[BATTERY] Raw voltage out of plausible range: raw_a0=%.0f, raw_a1=%.0f, computed=%.2fV", + raw_a0, raw_a1, raw_v); + } + } else { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "[BATTERY] Modbus read returned empty response."); + } + } + + if (!voltage.has_value()) { + if (last_valid_voltage_.has_value()) { + voltage = last_valid_voltage_; + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 10000, + "[BATTERY] Using last valid voltage reading as fallback."); + } else { + voltage = (v_min + v_max) / 2.0; + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 10000, + "[BATTERY] No valid reading yet - using nominal fallback: %.1fV", *voltage); + } + } + + voltage_history_.push_back(*voltage); + if (voltage_history_.size() > 10) + voltage_history_.pop_front(); + + std::vector sorted_v(voltage_history_.begin(), voltage_history_.end()); + std::sort(sorted_v.begin(), sorted_v.end()); + double median_v = sorted_v[sorted_v.size() / 2]; + + if (!filtered_battery_voltage_.has_value()) { + filtered_battery_voltage_ = median_v; + } else { + filtered_battery_voltage_ = *filtered_battery_voltage_ * 0.95 + median_v * 0.05; + } + + double final_voltage = *filtered_battery_voltage_; + double percentage = std::clamp((final_voltage - v_min) / (v_max - v_min), 0.0, 1.0); + + msg.voltage = static_cast(final_voltage); + msg.percentage = static_cast(percentage); + msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING; + battery_pub_->publish(msg); + } + + // ------------------------------------------------------------------- + // 20Hz realtime control loop + // ------------------------------------------------------------------- + void controlLoop() { + rclcpp::Time current_now = now(); + double dt = (current_now - last_time_).seconds(); + last_time_ = current_now; + if (dt <= 0.0) + return; + + // 1. Acceleration ramp + double dv = target_v_ - current_v_; + double max_dv = accel_limit_ * dt; + current_v_ += std::clamp(dv, -max_dv, max_dv); + + double dw = target_w_ - current_w_; + double max_dw = 2.0 * dt; + current_w_ += std::clamp(dw, -max_dw, max_dw); + + // 2. Yaw rate PI control + double yaw_error = current_w_ - current_imu_w_; + bool is_idle = std::abs(target_v_) < 0.01 && std::abs(target_w_) < 0.01; + double correction = 0.0; + if (is_idle) { + yaw_error_integral_ = 0.0; + current_v_ = 0.0; + current_w_ = 0.0; + } else { + yaw_error_integral_ += yaw_error * dt; + yaw_error_integral_ = std::clamp(yaw_error_integral_, -max_integral_, max_integral_); + correction = kp_yaw_ * yaw_error + ki_yaw_ * yaw_error_integral_; + } + + // 3. Inverse kinematics -> wheel RPM + double v_left = current_v_ - (current_w_ + correction) * wheel_base_ / 2.0; + double v_right = current_v_ + (current_w_ + correction) * wheel_base_ / 2.0; + + int rpm_left = static_cast(std::lround(v_left / (2.0 * kPi * wheel_radius_) * 60.0)); + int rpm_right = static_cast(std::lround(v_right / (2.0 * kPi * wheel_radius_) * 60.0)); + rpm_left = std::clamp(rpm_left, -kMaxRpm, kMaxRpm); + rpm_right = std::clamp(rpm_right, -kMaxRpm, kMaxRpm); + + // 4. Command the motors + int feedback_speeds[4] = {0, 0, 0, 0}; // FL, FR, RL, RR + + if (is_mock_mode_) { + feedback_speeds[0] = rpm_left; + feedback_speeds[1] = rpm_right; + feedback_speeds[2] = rpm_left; + feedback_speeds[3] = rpm_right; + } else if (control_method_ == "direct_pc") { + if (is_idle && is_parked_) { + if (drivers_enabled_) + disableDirectDrivers(); + } else { + if (!drivers_enabled_) + enableDirectDrivers(); + modbus_port_.writeRegs(1, 0x2088, + {static_cast(static_cast(rpm_left)), + static_cast(static_cast(-rpm_right))}); + modbus_port_.writeRegs(2, 0x2088, + {static_cast(static_cast(rpm_left)), + static_cast(static_cast(-rpm_right))}); + } + + std::vector front_resp, rear_resp; + if (modbus_port_.readRegs(1, 0x20AD, 2, front_resp)) { + feedback_speeds[0] = static_cast(front_resp[0]); + feedback_speeds[1] = -static_cast(front_resp[1]); + } + if (modbus_port_.readRegs(2, 0x20AD, 2, rear_resp)) { + feedback_speeds[2] = static_cast(rear_resp[0]); + feedback_speeds[3] = -static_cast(rear_resp[1]); + } + } else if (control_method_ == "arduino") { + runArduinoIo(rpm_left, rpm_right, is_idle && is_parked_, feedback_speeds); + } + + // 5. Odometry & telemetry + processFeedback(feedback_speeds, dt); + + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 2000, + "[BLDC FEEDBACK] FL:%d FR:%d | RL:%d RR:%d RPM | Target_W:%.2f IMU_W:%.2f", + feedback_speeds[0], feedback_speeds[1], feedback_speeds[2], feedback_speeds[3], + current_w_, current_imu_w_); + } + + void runArduinoIo(int rpm_left, int rpm_right, bool disable, int feedback_speeds[4]) { + if (!arduino_port_.isOpen()) + return; + + uint8_t packet[7]; + packet[0] = 0xFE; + packet[1] = static_cast((rpm_left >> 8) & 0xFF); + packet[2] = static_cast(rpm_left & 0xFF); + packet[3] = static_cast((rpm_right >> 8) & 0xFF); + packet[4] = static_cast(rpm_right & 0xFF); + packet[5] = disable ? 0 : 1; + uint8_t checksum = 0; + for (int i = 1; i < 6; ++i) + checksum += packet[i]; + packet[6] = checksum; + + if (write(arduino_port_.fd(), packet, sizeof(packet)) != static_cast(sizeof(packet))) { + RCLCPP_ERROR(get_logger(), "아두이노 송신 오류"); + } + + int bytes_avail = 0; + if (ioctl(arduino_port_.fd(), FIONREAD, &bytes_avail) == 0 && bytes_avail >= 10) { + uint8_t head = 0; + if (read(arduino_port_.fd(), &head, 1) == 1 && head == 0xFD) { + uint8_t data[9]; + ssize_t got = read(arduino_port_.fd(), data, 9); + if (got == 9) { + uint8_t sum = 0; + for (int i = 0; i < 8; ++i) + sum += data[i]; + if (sum == data[8]) { + feedback_speeds[0] = static_cast((data[0] << 8) | data[1]); + feedback_speeds[1] = static_cast((data[2] << 8) | data[3]); + feedback_speeds[2] = static_cast((data[4] << 8) | data[5]); + feedback_speeds[3] = static_cast((data[6] << 8) | data[7]); + } + } + } + } + } + + void processFeedback(const int feedback_speeds[4], double dt) { + double actual_rpm_l = (feedback_speeds[0] + feedback_speeds[2]) / 2.0; + double actual_rpm_r = (feedback_speeds[1] + feedback_speeds[3]) / 2.0; + + double v_l = actual_rpm_l * 2.0 * kPi * wheel_radius_ / 60.0; + double v_r = actual_rpm_r * 2.0 * kPi * wheel_radius_ / 60.0; + + double linear_vel = (v_r + v_l) / 2.0; + double angular_vel = (v_r - v_l) / wheel_base_; + + double delta_th = angular_vel * dt; + odom_th_ += delta_th; + odom_x_ += linear_vel * std::cos(odom_th_) * dt; + odom_y_ += linear_vel * std::sin(odom_th_) * dt; + + double rads_l = v_l / wheel_radius_; + double rads_r = v_r / wheel_radius_; + left_wheel_joint_pos_ += rads_l * dt; + right_wheel_joint_pos_ += rads_r * dt; + + auto stamp = now(); + + sensor_msgs::msg::JointState joint_state; + joint_state.header.stamp = stamp; + joint_state.name = {"front_left_wheel_joint", "front_right_wheel_joint", "rear_left_wheel_joint", + "rear_right_wheel_joint"}; + joint_state.position = {left_wheel_joint_pos_, right_wheel_joint_pos_, left_wheel_joint_pos_, + right_wheel_joint_pos_}; + joint_state.velocity = {rads_l, rads_r, rads_l, rads_r}; + joint_pub_->publish(joint_state); + + nav_msgs::msg::Odometry odom; + odom.header.stamp = stamp; + odom.header.frame_id = "odom"; + odom.child_frame_id = "base_link"; + odom.pose.pose.position.x = odom_x_; + odom.pose.pose.position.y = odom_y_; + odom.pose.pose.position.z = 0.0; + + double cy = std::cos(odom_th_ * 0.5); + double sy = std::sin(odom_th_ * 0.5); + odom.pose.pose.orientation.w = cy; + odom.pose.pose.orientation.z = sy; + + odom.twist.twist.linear.x = linear_vel; + odom.twist.twist.angular.z = angular_vel; + + odom_pub_->publish(odom); + + // Nav2's local_costmap needs a live odom->base_link TF (not just the /odom_wheels + // topic) to transform sensor data into the robot frame. Only broadcast it when no + // LIDAR SLAM (FAST_LIO/FAST-LIVO2) is providing base_link's TF via aft_mapped — + // see publish_odom_tf_ declaration above for why both can't be active at once. + if (publish_odom_tf_) { + geometry_msgs::msg::TransformStamped tf_msg; + tf_msg.header.stamp = stamp; + tf_msg.header.frame_id = "odom"; + tf_msg.child_frame_id = "base_link"; + tf_msg.transform.translation.x = odom.pose.pose.position.x; + tf_msg.transform.translation.y = odom.pose.pose.position.y; + tf_msg.transform.translation.z = 0.0; + tf_msg.transform.rotation = odom.pose.pose.orientation; + tf_broadcaster_->sendTransform(tf_msg); + } + } + + static constexpr int kMaxRpm = 250; + + // Parameters + std::string control_method_; + std::string port_; + int baud_ = 115200; + double limit_v_ = 0.5; + double accel_limit_ = 0.5; + double wheel_base_ = 0.374; + double wheel_radius_ = 0.127; + bool publish_odom_tf_ = true; + + // Yaw PI gains + const double kp_yaw_ = 0.50; + const double ki_yaw_ = 0.20; + double yaw_error_integral_ = 0.0; + const double max_integral_ = 1.0; + + // Robot state + double target_v_ = 0.0, target_w_ = 0.0; + double current_v_ = 0.0, current_w_ = 0.0; + double current_imu_w_ = 0.0; + rclcpp::Time last_time_; + + double odom_x_ = 0.0, odom_y_ = 0.0, odom_th_ = 0.0; + double left_wheel_joint_pos_ = 0.0, right_wheel_joint_pos_ = 0.0; + + bool drivers_initialized_ = false; + bool drivers_enabled_ = false; + bool is_parked_ = false; + bool is_mock_mode_ = false; + + SerialPort modbus_port_; + SerialPort arduino_port_; + + std::optional last_valid_voltage_; + std::optional filtered_battery_voltage_; + std::deque voltage_history_; + + rclcpp::Subscription::SharedPtr cmd_vel_sub_; + rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Subscription::SharedPtr initial_pose_sub_; + rclcpp::Subscription::SharedPtr mode_status_sub_; + + rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Publisher::SharedPtr joint_pub_; + rclcpp::Publisher::SharedPtr battery_pub_; + std::unique_ptr tf_broadcaster_; + + rclcpp::TimerBase::SharedPtr control_timer_; + rclcpp::TimerBase::SharedPtr battery_timer_; +}; + +} // namespace fori_serial_bridge_cpp + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::on_shutdown([node]() { node->safeStop(); }); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/start_fori.sh b/start_fori.sh new file mode 100755 index 0000000..3838f53 --- /dev/null +++ b/start_fori.sh @@ -0,0 +1,37 @@ +#!/bin/bash +# FORI AGV 안전 시작 스크립트 +# 이전 프로세스 잔류 문제를 자동으로 처리합니다 + +echo "==========================================" +echo " FORI AGV - 안전 시작 스크립트" +echo "==========================================" + +# 1. 이전 실행 잔류 프로세스 모두 종료 +echo "[1/3] 이전 프로세스 정리 중..." +pkill -9 -f "serial_bridge_node" 2>/dev/null +pkill -9 -f "parking_controller_node" 2>/dev/null +pkill -9 -f "ui_server_node" 2>/dev/null +pkill -9 -f "rosbridge_websocket" 2>/dev/null +pkill -9 -f "web_video_server" 2>/dev/null +pkill -9 -f "aruco_detector_node.py" 2>/dev/null +pkill -9 -f "fori_full.launch" 2>/dev/null +pkill -9 -f "fori_nav2.launch" 2>/dev/null +fuser -k -9 8080/tcp 9090/tcp 8085/tcp 2>/dev/null +sleep 2 + +echo "[2/3] ROS2 환경 로드..." +source /opt/ros/humble/setup.bash +source /home/yoo/fori_ws/fori_ws/install/setup.bash + +# Hikrobot 카메라 SDK(/opt/MVS/lib/64)가 LD_LIBRARY_PATH 맨 앞에 있어서 시스템 +# libusb보다 먼저 잡히는데, 그 안의 libusb가 구버전이라 PCL(fast_lio, pcd_gridmap_converter +# 등)이 필요로 하는 libusb_set_option 심볼이 없어 "symbol lookup error"로 죽는다. +# 시스템 라이브러리 경로를 앞에 둬서 심볼 충돌만 피하고, MVS 경로는 카메라 전용 라이브러리 +# 탐색을 위해 그대로 뒤에 남겨둔다. +export LD_LIBRARY_PATH="/usr/lib/x86_64-linux-gnu:/lib/x86_64-linux-gnu:${LD_LIBRARY_PATH}" + +echo "[3/3] FORI AGV 시작!" +echo " ✅ 웹 UI: http://localhost:8080" +echo " ✅ 외부(모바일): http://$(hostname -I | awk '{print $1}'):8080" +echo "==========================================" +ros2 launch fori_serial_bridge fori_full.launch.py