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
This commit is contained in:
+1
-1
@@ -1 +1 @@
|
||||
/home/yoo/FAST_LIO
|
||||
/home/yoo/duru_lio_ws
|
||||
@@ -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
|
||||
+114
-6
@@ -1,20 +1,31 @@
|
||||
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/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')
|
||||
|
||||
# 패키지 경로 획득
|
||||
@@ -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,
|
||||
])
|
||||
|
||||
@@ -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
|
||||
|
||||
BIN
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
@@ -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()
|
||||
@@ -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 = []
|
||||
@@ -58,6 +56,13 @@ class ParkingControllerNode(Node):
|
||||
'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
|
||||
@@ -137,6 +142,18 @@ class ParkingControllerNode(Node):
|
||||
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)
|
||||
self.create_subscription(OccupancyGrid, '/map', self.map_callback, map_qos)
|
||||
@@ -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
|
||||
|
||||
@@ -221,59 +221,88 @@ class ForiSerialBridge(Node):
|
||||
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
|
||||
self.get_logger().warn(f'[BATTERY] Modbus read failed: {e}', throttle_duration_sec=5.0)
|
||||
|
||||
# 2. Sliding Window Median + Heavy EMA Filter (eliminates IR drop jumps & Modbus spikes)
|
||||
# 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))
|
||||
|
||||
@@ -282,6 +311,7 @@ class ForiSerialBridge(Node):
|
||||
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).
|
||||
|
||||
@@ -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()
|
||||
@@ -25,6 +25,8 @@ 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/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
|
||||
# Automatically open default web browser after 1 second delay
|
||||
def auto_open_browser():
|
||||
url = f"http://localhost:{self.port}"
|
||||
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}')
|
||||
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:
|
||||
|
||||
@@ -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,6 +199,8 @@ def generate_launch_description():
|
||||
)
|
||||
|
||||
# 4. Robot State Publisher (URDF) to define joints/frames
|
||||
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_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('/home/yoo/fori_ws/src/fori_nav2.launch.py')
|
||||
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(odom_relay_node)
|
||||
|
||||
return ld
|
||||
|
||||
@@ -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',
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
@@ -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');
|
||||
const valDisplay = document.getElementById('battery-value-display');
|
||||
const ringFill = document.getElementById('battery-ring-fill');
|
||||
|
||||
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)';
|
||||
}
|
||||
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));
|
||||
});
|
||||
}
|
||||
|
||||
@@ -2,41 +2,167 @@
|
||||
<html lang="ko">
|
||||
<head>
|
||||
<meta charset="UTF-8">
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
||||
<title>FORI AGV Control Dashboard</title>
|
||||
<!-- Outfit Google Font -->
|
||||
<link href="https://fonts.googleapis.com/css2?family=Outfit:wght@300;400;600;800&display=swap" rel="stylesheet">
|
||||
<link rel="stylesheet" href="style.css">
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0, user-scalable=no">
|
||||
<title>AZMO - FORI AGV 자율주행 관제 대시보드</title>
|
||||
|
||||
<!-- PWA Meta Tags & Web App Manifest -->
|
||||
<link rel="manifest" href="manifest.json">
|
||||
<meta name="theme-color" content="#22A774">
|
||||
<meta name="mobile-web-app-capable" content="yes">
|
||||
<meta name="apple-mobile-web-app-capable" content="yes">
|
||||
<meta name="apple-mobile-web-app-status-bar-style" content="black-translucent">
|
||||
<meta name="apple-mobile-web-app-title" content="FORI AGV">
|
||||
<link rel="apple-touch-icon" href="data:image/svg+xml,<svg xmlns='http://www.w3.org/2000/svg' viewBox='0 0 100 100'><rect width='100' height='100' rx='22' fill='%2322A774'/><text x='50' y='68' font-size='55' text-anchor='middle' fill='white'>⚡</text></svg>">
|
||||
|
||||
<!-- Outfit & Pretendard Google Font -->
|
||||
<link href="https://fonts.googleapis.com/css2?family=Outfit:wght@400;500;600;700;800&display=swap" rel="stylesheet">
|
||||
<link rel="stylesheet" href="style.css?v=8">
|
||||
<!-- ROSlibJS -->
|
||||
<script src="https://cdn.jsdelivr.net/npm/roslib@1/build/roslib.min.js"></script>
|
||||
</head>
|
||||
<body>
|
||||
|
||||
<!-- Mobile Drawer Overlay -->
|
||||
<div class="sidebar-overlay" id="sidebarOverlay" onclick="toggleMobileSidebar()"></div>
|
||||
|
||||
<div class="app-container">
|
||||
<!-- Header -->
|
||||
<header class="dashboard-header">
|
||||
<div class="logo-area">
|
||||
<h1>FORI <span>AGV</span></h1>
|
||||
<span class="system-status">BLDC & ArUco Parking System</span>
|
||||
|
||||
<!-- Emerald Green Sidebar (Matching image_1) -->
|
||||
<aside class="sidebar" id="appSidebar">
|
||||
<div class="sidebar-header">
|
||||
<div class="logo-icon">⚡</div>
|
||||
<div class="logo-text">FORI <span>AGV</span></div>
|
||||
<button class="mobile-close-btn" onclick="toggleMobileSidebar()">×</button>
|
||||
</div>
|
||||
|
||||
<nav class="sidebar-nav">
|
||||
<a href="#" class="nav-item active" onclick="toggleMobileSidebar()">
|
||||
<span>🗺️</span>
|
||||
<span>2D 관제 대시보드</span>
|
||||
</a>
|
||||
</nav>
|
||||
|
||||
<div class="sidebar-footer">
|
||||
© 2026 GARDENTECH Co., Ltd.
|
||||
</div>
|
||||
</aside>
|
||||
|
||||
<!-- Main Content Wrapper -->
|
||||
<div class="main-wrapper">
|
||||
|
||||
<!-- Top Navbar Header -->
|
||||
<header class="top-header">
|
||||
<div class="header-left">
|
||||
<!-- Mobile Hamburger Menu Button -->
|
||||
<button class="mobile-hamburger-btn" onclick="toggleMobileSidebar()">
|
||||
<span>≡</span>
|
||||
</button>
|
||||
|
||||
<span class="mobile-header-title">FORI AGV</span>
|
||||
|
||||
<div class="page-breadcrumb">
|
||||
<span>AZMO 관제 Center</span>
|
||||
<span style="color: var(--text-subtle);">></span>
|
||||
<span style="font-weight: 700; color: var(--text-main);">FORI AGV 자율주행 대시보드</span>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="header-right">
|
||||
<!-- Connection Status -->
|
||||
<div class="connection-status" id="conn-status">
|
||||
<span id="env-badge" class="env-badge sim">Simulation Mode</span>
|
||||
<span id="env-badge" class="env-badge sim">Simulation</span>
|
||||
<span class="pulse-indicator red" id="status-indicator"></span>
|
||||
<span id="status-text">Disconnected</span>
|
||||
</div>
|
||||
</div>
|
||||
</header>
|
||||
|
||||
<!-- Main Dashboard Grid -->
|
||||
<!-- Dashboard Content Container -->
|
||||
<main class="dashboard-content">
|
||||
|
||||
<div class="section-title-bar">
|
||||
<div class="section-title">
|
||||
AGV 실시간 관제 및 주행 제어
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- Metric Gauge Summary Cards ("Total Condition" derived from image_1) -->
|
||||
<section class="metrics-grid">
|
||||
|
||||
<!-- Metric 1: Battery Ring Gauge -->
|
||||
<div class="metric-card">
|
||||
<div class="metric-info">
|
||||
<span class="metric-label">FORI 배터리 잔량</span>
|
||||
<span class="metric-value" id="battery-value-display">85%</span>
|
||||
<span class="metric-subtext">24V 60Ah LiFePO4</span>
|
||||
</div>
|
||||
<div class="ring-gauge-container">
|
||||
<svg class="ring-gauge-svg" viewBox="0 0 72 72">
|
||||
<circle class="ring-bg" cx="36" cy="36" r="30"></circle>
|
||||
<circle class="ring-fill green" id="battery-ring-fill" cx="36" cy="36" r="30" stroke-dasharray="188" stroke-dashoffset="28"></circle>
|
||||
</svg>
|
||||
<div class="ring-center-text" id="battery-badge">85%</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- Metric 2: Connection Status -->
|
||||
<div class="metric-card">
|
||||
<div class="metric-info">
|
||||
<span class="metric-label">통신 & ROS2 연결</span>
|
||||
<span class="metric-value" style="color: var(--primary-green-dark);">정상</span>
|
||||
<span class="metric-subtext">rosbridge ws://9090</span>
|
||||
</div>
|
||||
<div class="ring-gauge-container">
|
||||
<svg class="ring-gauge-svg" viewBox="0 0 72 72">
|
||||
<circle class="ring-bg" cx="36" cy="36" r="30"></circle>
|
||||
<circle class="ring-fill blue" cx="36" cy="36" r="30" stroke-dasharray="188" stroke-dashoffset="0"></circle>
|
||||
</svg>
|
||||
<div class="ring-center-text" style="color: var(--accent-blue);">OK</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- Metric 3: Robot Operating State -->
|
||||
<div class="metric-card">
|
||||
<div class="metric-info">
|
||||
<span class="metric-label">현재 주행 모드</span>
|
||||
<span class="metric-value" id="val-state-top" style="color: var(--primary-green-dark);">Nav2</span>
|
||||
<span class="metric-subtext">BLDC & ArUco Parking System</span>
|
||||
</div>
|
||||
<div class="ring-gauge-container">
|
||||
<svg class="ring-gauge-svg" viewBox="0 0 72 72">
|
||||
<circle class="ring-bg" cx="36" cy="36" r="30"></circle>
|
||||
<circle class="ring-fill green" cx="36" cy="36" r="30" stroke-dasharray="188" stroke-dashoffset="0"></circle>
|
||||
</svg>
|
||||
<div class="ring-center-text" style="color: var(--primary-green);">RUN</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
</section>
|
||||
|
||||
<!-- Main Dashboard 2-Column Grid -->
|
||||
<main class="dashboard-grid">
|
||||
|
||||
<!-- Left Side: Map & Configuration -->
|
||||
<!-- Left Column: Map & Configuration Panel -->
|
||||
<section class="grid-card map-card">
|
||||
<div class="card-header">
|
||||
<h2>🗺️ 실시간 2D 맵 시각화</h2>
|
||||
<span class="help-text">지도를 클릭하여 주차 진입점(Waypoint)을 등록하세요.</span>
|
||||
<h2>🗺️ 실시간 2D 맵 시각화 및 Waypoint 설정</h2>
|
||||
<span class="help-text">지도를 클릭하여 주차 진입점(Waypoint)을 등록하거나 드래그하여 목표 방향을 지정하세요.</span>
|
||||
</div>
|
||||
|
||||
<!-- Canvas Container -->
|
||||
<div class="canvas-container">
|
||||
<canvas id="map-canvas"></canvas>
|
||||
</div>
|
||||
|
||||
<!-- Pose Panel -->
|
||||
<div class="pose-panel">
|
||||
<h3>📍 로봇 실시간 현재 위치 (Pose)</h3>
|
||||
<div class="pose-value-display" id="val-pose">
|
||||
X: 0.00m | Y: 0.00m | Yaw: 0°
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- Waypoint Config Panel -->
|
||||
<div class="config-panel">
|
||||
<h3>📍 주차 진입점 (Waypoint) 설정</h3>
|
||||
<div class="coord-inputs">
|
||||
@@ -53,29 +179,17 @@
|
||||
<input type="number" id="wp-yaw" step="1" value="0">
|
||||
</div>
|
||||
</div>
|
||||
<button class="btn btn-secondary" id="btn-set-wp">진입점 좌표 전송</button>
|
||||
</div>
|
||||
<div class="pose-panel">
|
||||
<h3>📍 로봇 실시간 현재 위치 (Pose)</h3>
|
||||
<div class="pose-value-display" id="val-pose">
|
||||
X: 0.00m | Y: 0.00m | Yaw: 0°
|
||||
</div>
|
||||
</div>
|
||||
<div class="battery-panel" style="margin-top: 15px; background: rgba(255,255,255,0.03); border: 1px solid rgba(255,255,255,0.08); padding: 15px; border-radius: 12px;">
|
||||
<div style="display: flex; justify-content: space-between; align-items: center;">
|
||||
<h3 style="margin: 0; font-size: 1rem; color: var(--text-heading, #fff);">🔋 FORI 배터리 잔량 (24V 60Ah LiFePO4)</h3>
|
||||
<span id="battery-badge" class="badge badge-normal" style="background: rgba(34, 197, 94, 0.2); color: #4ade80; border: 1px solid rgba(74, 222, 128, 0.4); padding: 4px 10px; border-radius: 20px; font-size: 0.85rem; font-weight: 600;">--% (--.-V)</span>
|
||||
</div>
|
||||
<div style="width: 100%; background: rgba(255,255,255,0.08); height: 12px; border-radius: 6px; margin-top: 10px; overflow: hidden; border: 1px solid rgba(255,255,255,0.1);">
|
||||
<div id="battery-bar" style="width: 0%; height: 100%; background: linear-gradient(90deg, #22c55e, #4ade80); transition: width 0.5s ease, background 0.5s ease; border-radius: 6px;"></div>
|
||||
</div>
|
||||
<button class="btn btn-secondary" id="btn-set-wp" style="width: 100%;">진입점 좌표 전송</button>
|
||||
</div>
|
||||
|
||||
<!-- Hidden battery bar element for app.js compatibility -->
|
||||
<div id="battery-bar" style="display: none;"></div>
|
||||
</section>
|
||||
|
||||
<!-- Right Side: Camera View & Telemetry -->
|
||||
<!-- Right Column: Camera View & Telemetry Control -->
|
||||
<div class="right-column">
|
||||
|
||||
<!-- Camera & ArUco view -->
|
||||
<!-- Camera & ArUco View -->
|
||||
<section class="grid-card camera-card">
|
||||
<div class="card-header">
|
||||
<h2>📷 실시간 아루코 인식 카메라</h2>
|
||||
@@ -104,11 +218,11 @@
|
||||
<span class="detail-value" id="aruco-y">-</span>
|
||||
</div>
|
||||
<div class="detail-row">
|
||||
<span class="detail-label">Yaw (좌우 회전)</span>
|
||||
<span class="detail-label">Yaw (회전)</span>
|
||||
<span class="detail-value" id="aruco-yaw">-</span>
|
||||
</div>
|
||||
<div class="detail-row">
|
||||
<span class="detail-label">Roll / Pitch</span>
|
||||
<span class="detail-label">Roll/Pitch</span>
|
||||
<span class="detail-value" id="aruco-rp">-</span>
|
||||
</div>
|
||||
</div>
|
||||
@@ -116,24 +230,27 @@
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<!-- Mode Controls & Telemetry -->
|
||||
<!-- Mode Controls & Telemetry Panel -->
|
||||
<section class="grid-card controls-card">
|
||||
<div class="card-header">
|
||||
<h2>⚙️ 제어 모드 및 텔레메트리</h2>
|
||||
</div>
|
||||
|
||||
<!-- Mode Switch Buttons -->
|
||||
<!-- Mode Selectors -->
|
||||
<div class="mode-selector">
|
||||
<button class="btn btn-primary active" id="btn-mode-nav2">자율 주행 (Nav2)</button>
|
||||
<button class="btn btn-primary" id="btn-mode-patrol">자동 순찰 (Patrol)</button>
|
||||
<button class="btn btn-primary" id="btn-mode-parking">자동 주차 (Parking)</button>
|
||||
</div>
|
||||
<button class="btn btn-secondary" id="btn-pose-estimate" style="margin-bottom: 12px; font-size: 13px; font-weight: 700; width: 100%;">
|
||||
|
||||
<div style="display: flex; gap: 10px; flex-direction: column;">
|
||||
<button class="btn btn-secondary" id="btn-pose-estimate">
|
||||
📍 로봇 위치 초기화 (Pose Estimate)
|
||||
</button>
|
||||
<button class="btn btn-danger" id="btn-emergency-stop" style="margin-bottom: 20px; font-size: 14px; font-weight: 800; width: 100%;">
|
||||
<button class="btn btn-danger" id="btn-emergency-stop">
|
||||
🛑 긴급 정지 (Emergency Stop)
|
||||
</button>
|
||||
</div>
|
||||
|
||||
<!-- Telemetry Metrics Grid -->
|
||||
<div class="telemetry-grid">
|
||||
@@ -142,11 +259,11 @@
|
||||
<span class="metric-value" id="val-state">IDLE</span>
|
||||
</div>
|
||||
<div class="metric-box">
|
||||
<span class="metric-label">로봇 선속도 (Linear)</span>
|
||||
<span class="metric-label">선속도 (Linear)</span>
|
||||
<span class="metric-value" id="val-linear">0.00 m/s</span>
|
||||
</div>
|
||||
<div class="metric-box">
|
||||
<span class="metric-label">로봇 각속도 (Angular)</span>
|
||||
<span class="metric-label">각속도 (Angular)</span>
|
||||
<span class="metric-value" id="val-angular">0.00 rad/s</span>
|
||||
</div>
|
||||
</div>
|
||||
@@ -186,9 +303,16 @@
|
||||
</div>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
</div>
|
||||
|
||||
</main>
|
||||
|
||||
</main>
|
||||
|
||||
</div>
|
||||
<script src="app.js"></script>
|
||||
</div>
|
||||
|
||||
<script src="app.js?v=8"></script>
|
||||
</body>
|
||||
</html>
|
||||
|
||||
@@ -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,<svg xmlns='http://www.w3.org/2000/svg' viewBox='0 0 100 100'><rect width='100' height='100' rx='22' fill='%2322A774'/><text x='50' y='68' font-size='55' text-anchor='middle' fill='white'>⚡</text></svg>",
|
||||
"sizes": "192x192 512x512",
|
||||
"type": "image/svg+xml",
|
||||
"purpose": "any maskable"
|
||||
}
|
||||
]
|
||||
}
|
||||
+924
-343
File diff suppressed because it is too large
Load Diff
@@ -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))
|
||||
);
|
||||
});
|
||||
@@ -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
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
)
|
||||
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()
|
||||
@@ -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 <cstdint>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
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<uint16_t> &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<uint16_t> &out_vals);
|
||||
|
||||
private:
|
||||
int fd_ = -1;
|
||||
};
|
||||
|
||||
} // namespace fori_serial_bridge_cpp
|
||||
@@ -0,0 +1,22 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>fori_serial_bridge_cpp</name>
|
||||
<version>0.0.1</version>
|
||||
<description>Low-latency C++ port of the ZLAC8015D RS485/Arduino motor serial bridge (replaces the Python serial_bridge_node hot loop)</description>
|
||||
<maintainer email="user@todo.todo">yoo</maintainer>
|
||||
<license>Apache License 2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,230 @@
|
||||
#include "fori_serial_bridge_cpp/modbus_serial.hpp"
|
||||
|
||||
#include <chrono>
|
||||
#include <cstring>
|
||||
#include <fcntl.h>
|
||||
#include <termios.h>
|
||||
#include <thread>
|
||||
#include <unistd.h>
|
||||
|
||||
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<std::chrono::milliseconds>(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<uint16_t>(rx_buf[6]) | (static_cast<uint16_t>(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<uint16_t> &vals) {
|
||||
size_t count = vals.size();
|
||||
size_t pkt_len = 7 + count * 2 + 2;
|
||||
std::vector<uint8_t> 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<uint8_t>(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<ssize_t>(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<std::chrono::milliseconds>(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<uint16_t>(rx_buf[6]) | (static_cast<uint16_t>(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<uint16_t> &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<uint8_t> 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<std::chrono::milliseconds>(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<uint16_t>(rx_buf[expected_bytes - 2]) |
|
||||
(static_cast<uint16_t>(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<uint16_t>(rx_buf[3 + i * 2]) << 8) | rx_buf[4 + i * 2];
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
} // namespace fori_serial_bridge_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 <algorithm>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
#include <deque>
|
||||
#include <memory>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <termios.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#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<geometry_msgs::msg::Twist>(
|
||||
"/cmd_vel", qos_depth1,
|
||||
[this](const geometry_msgs::msg::Twist::SharedPtr msg) { cmdVelCallback(msg); });
|
||||
imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
|
||||
"/livox/imu", qos_depth1,
|
||||
[this](const sensor_msgs::msg::Imu::SharedPtr msg) { imuCallback(msg); });
|
||||
initial_pose_sub_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
|
||||
"/initialpose", 10,
|
||||
[this](const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {
|
||||
initialPoseCallback(msg);
|
||||
});
|
||||
mode_status_sub_ = create_subscription<std_msgs::msg::String>(
|
||||
"/robot_mode_status", 10,
|
||||
[this](const std_msgs::msg::String::SharedPtr msg) { modeStatusCallback(msg); });
|
||||
|
||||
odom_pub_ = create_publisher<nav_msgs::msg::Odometry>("/odom_wheels", qos_depth1);
|
||||
joint_pub_ = create_publisher<sensor_msgs::msg::JointState>("/joint_states", qos_depth1);
|
||||
battery_pub_ = create_publisher<sensor_msgs::msg::BatteryState>("/battery_state", qos_depth1);
|
||||
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*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<double> voltage;
|
||||
|
||||
if (!is_mock_mode_ && control_method_ == "direct_pc") {
|
||||
std::vector<uint16_t> v_resp;
|
||||
if (modbus_port_.readRegs(1, 0x20A0, 2, v_resp) && !v_resp.empty()) {
|
||||
double raw_a0 = std::abs(static_cast<double>(static_cast<int16_t>(v_resp[0])));
|
||||
double raw_a1 = v_resp.size() > 1
|
||||
? std::abs(static_cast<double>(static_cast<int16_t>(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<double> 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<float>(final_voltage);
|
||||
msg.percentage = static_cast<float>(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<int>(std::lround(v_left / (2.0 * kPi * wheel_radius_) * 60.0));
|
||||
int rpm_right = static_cast<int>(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<uint16_t>(static_cast<int16_t>(rpm_left)),
|
||||
static_cast<uint16_t>(static_cast<int16_t>(-rpm_right))});
|
||||
modbus_port_.writeRegs(2, 0x2088,
|
||||
{static_cast<uint16_t>(static_cast<int16_t>(rpm_left)),
|
||||
static_cast<uint16_t>(static_cast<int16_t>(-rpm_right))});
|
||||
}
|
||||
|
||||
std::vector<uint16_t> front_resp, rear_resp;
|
||||
if (modbus_port_.readRegs(1, 0x20AD, 2, front_resp)) {
|
||||
feedback_speeds[0] = static_cast<int16_t>(front_resp[0]);
|
||||
feedback_speeds[1] = -static_cast<int16_t>(front_resp[1]);
|
||||
}
|
||||
if (modbus_port_.readRegs(2, 0x20AD, 2, rear_resp)) {
|
||||
feedback_speeds[2] = static_cast<int16_t>(rear_resp[0]);
|
||||
feedback_speeds[3] = -static_cast<int16_t>(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<uint8_t>((rpm_left >> 8) & 0xFF);
|
||||
packet[2] = static_cast<uint8_t>(rpm_left & 0xFF);
|
||||
packet[3] = static_cast<uint8_t>((rpm_right >> 8) & 0xFF);
|
||||
packet[4] = static_cast<uint8_t>(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<ssize_t>(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<int16_t>((data[0] << 8) | data[1]);
|
||||
feedback_speeds[1] = static_cast<int16_t>((data[2] << 8) | data[3]);
|
||||
feedback_speeds[2] = static_cast<int16_t>((data[4] << 8) | data[5]);
|
||||
feedback_speeds[3] = static_cast<int16_t>((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<double> last_valid_voltage_;
|
||||
std::optional<double> filtered_battery_voltage_;
|
||||
std::deque<double> voltage_history_;
|
||||
|
||||
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
|
||||
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_sub_;
|
||||
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr mode_status_sub_;
|
||||
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_pub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr battery_pub_;
|
||||
std::unique_ptr<tf2_ros::TransformBroadcaster> 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<fori_serial_bridge_cpp::ForiSerialBridge>();
|
||||
rclcpp::on_shutdown([node]() { node->safeStop(); });
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
Executable
+37
@@ -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
|
||||
Reference in New Issue
Block a user