융합_로보테크 AI 자율주행 로봇 개발자 과정-26/05/07

1. 전체 구조 파악

  프로젝트: TurtleBot3 기반 자율주행 공기청정 로봇 (졸업 프로젝트로 추정)

  3개 계층이 MQTT로 묶여있음:

  [웹/Spring Boot]                [ROS2 (group4_ws)]
  robot-web                        ┌─ Nav2 AMCL (localization_launch)
   ├─ MqttCommandPublisher    ◄────┤
   ├─ MqttRobotPoseSubscriber      │
   ├─ MqttRobotBatterySubscriber   ├─ air_clean_pure_controller  ← 주행
  (A*+Pure Pursuit)
   ├─ MqttRobotDustSubscriber      ├─ mqtt_cmd_bridge            ← MQTT↔ROS
  양방향
   └─ MqttRobotPathSubscriber      ├─ amcl_pose_mqtt_publisher
          ▲                        ├─ battery_state_mqtt_publisher
          │   192.168.0.55:1883    ├─ dust_cells_mqtt_publisher
     [MQTT 브로커]                  ├─ air_clean_gui (Tkinter)
                                   └─ auto_initial_pose

  데이터 흐름 (주행 시):
  1. 웹에서 명령 발행 → MQTT robot/1/cmd
  2. mqtt_cmd_bridge.handle_command() 가 명령 파싱 → ROS /goal_pose
  (PoseStamped) 발행
  3. air_clean_pure_controller.goal_callback() 이 받아서 goal_pose 저장 +
  need_replan=True
  4. plan_path() 가 자체 A* 실행 (정적 inflation + 동적 LiDAR 장애물 +
  safety cost) → path_world 채움
  5. 10Hz control_loop() 가 Pure Pursuit으로 /cmd_vel 발행
  6. /astar_path, /amcl_pose 는 별도 브리지 노드들이 MQTT로 다시 웹에 전달

  메인 launch: air_clean_astar.launch.py (= pure_controller + GUI + MQTT
  브리지들). 별도로 air_clean_localization.launch.py 를 띄워 AMCL/RViz를
  실행.

  핵심 파일:
  - 컨트롤러:
  src/air_clean_robot/air_clean_robot/air_clean_pure_controller.py (자체 A*
  + 클리어셀 + 안전코스트맵 + Pure Pursuit이 한 노드에)
  - 파라미터: src/air_clean_robot/config/air_clean_params.yaml
  - air_clean_astar_controller.py 와 astar_follower.py 는 구버전 - 현재
  launch에서 사용 안 함.

  ---
  2. 문제가 있던 부분

   신규 기능 상태
  - pose_timeout_sec (AMCL pose 가 stale이면 cmd_vel 정지)
  - path_index_max_advance_cells (advance_path_index 점프 한도, 기존
  하드코딩 +40 대체)

  그런데 pose_timeout_sec 의 구현이 stationary AMCL 환경에서 영구 정지
  deadlock 을 일으킴

  # Code.docx의 control_loop
  if self.last_pose_time > 0.0 and self.pose_timeout_sec > 0.0:
      now = self.get_clock().now().nanoseconds / 1_000_000_000.0
      if (now - self.last_pose_time) > self.pose_timeout_sec:
          self.publish_stop()        # cmd_vel 정지
          self.need_replan = True
          return                      # control_loop 종료

  deadlock 시나리오:
  1. AMCL은 update_min_d/update_min_a 임계 이상 움직여야만 /amcl_pose를 다시
   publish함 (정지 상태에서는 publish 멈춤).
  2. 사용자가 MQTT로 명령을 보내는 시점에는 로봇이 아직 정지 상태라
  last_pose_time이 이미 오래됨.
  3. control_loop의 timeout이 즉시 fire → publish_stop() + need_replan=True
  + return.
  4. cmd_vel을 0으로 깔았으니 로봇은 안 움직이고, AMCL도 publish를 안 하니
  last_pose_time이 갱신될 일이 없음.
  5. 다음 100ms에 control_loop가 다시 돌아도 똑같이 timeout fire → 영구
  정지.

  이 패턴이 증상 — "MQTT/위치는 정상인데 주행만 안 됨" —
  과 일치 (/cmd_vel은 0인 Twist만 계속 publish 중).

  use_scan_obstacles: false, inflation_radius_cells: 5,
  path_smoothing_enabled: false 등 다른 변경은 주행 가능성에 영향이 없음을
  코드 추적으로 확인

  ---
  3. 해결한 방법과 수정한 코드

  원칙: Code.docx와 데스크톱 yaml의 의도(stale pose 방어 + path_index 점프
  제한)는 그대로 유지하면서, deadlock만 끊어냄. 다른 기능은 건들지 않음.

  3가지 수정:

  (a) pose_timeout 가드를 "path 추종 중일 때만" 으로 한정

  def control_loop(self):
      if self.shoe_detected:
          self.publish_stop()
          return
      if self.goal_pose is None:
          return
      if self.current_pose is None:
          return

      # AMCL pose가 일정 시간 이상 stale이면 cmd_vel 정지.
      # 단, 아직 path가 없으면(목표 직후/계획 전) 체크하지 않는다.
      # AMCL이 update_min_d/a 때문에 stationary 동안 publish를 멈추는
  환경에서는
      # path가 생기기 전 이 체크가 즉시 fire되어 영구 정지 deadlock이
  발생하므로,
      # 실제 path를 추종 중일 때에만 동작시킨다.
      if (
          self.path_world
          and self.last_pose_time > 0.0
          and self.pose_timeout_sec > 0.0
      ):
          now = self.get_clock().now().nanoseconds / 1_000_000_000.0
          if (now - self.last_pose_time) > self.pose_timeout_sec:
              self.publish_stop()
              self.need_replan = True
              return

      if self.need_replan:
          self.plan_path()
          return
      ...

  (b) goal 수신 시 last_pose_time 리셋

  목표가 들어온 순간을 "pose가 fresh한 시점"으로 처리해 stationary
  잔여시간이 timeout을 잡아먹는 일을 방지.

  # set_goal_from_target / goal_callback 에 동일하게 추가
  self.last_pose_time = self.get_clock().now().nanoseconds / 1_000_000_000.0

  (c) pose_callback에서 last_pose_time 갱신, advance_path_index의 +40
  하드코딩 → 파라미터화

  # pose_callback 안
  self.last_pose_time = self.get_clock().now().nanoseconds / 1_000_000_000.0

  # advance_path_index
  max_step = max(1, self.path_index_max_advance_cells)
  search_end = min(last_index, self.path_index + max_step)

  이로써:
  - 새 명령 직후엔 timeout이 안 깨물어서 일단 출발 가능
  - 출발 후엔 AMCL이 motion을 감지하므로 last_pose_time이 정상 갱신되어
  timeout이 본래 의도(통신 단절 방어)로 동작
  - 이후 정말 stale이 길어지면 그때 비로소 정지

  ---
  4. 수정된 파일 경로와 수정 방법

  파일:
  group4_ws\src\air_clean_robot\air_clean_robot\air_clean_pure_controller.py
  수정 방법: pose_timeout_sec (default 3.0)·path_index_max_advance_cells
    (default 5) 파라미터 declare/get, self.last_pose_time = 0.0 상태 추가,
    pose_callback·set_goal_from_target·goal_callback에서 last_pose_time
  갱신,
     control_loop에 path 추종 중에만 동작하는 pose_timeout 가드 추가,
    advance_path_index의 하드코딩 +40 을 path_index_max_advance_cells
    파라미터로 교체
  ────────────────────────────────────────
  파일: group4_ws\src\air_clean_robot\config\air_clean_params.yaml
  수정 방법: air_clean_pure_controller: 섹션에 pose_timeout_sec: 3.0,
    path_index_max_advance_cells: 5 추가. 데스크톱 yaml과 같은
    튜닝값(max_angular_speed: 0.50, heading_gain: 1.2,
    rotate_min_angular_speed: 0.12, waypoint_tolerance: 0.08,
    rotate_in_place_threshold: 0.55, lookahead_distance: 0.22)으로 갱신
  ────────────────────────────────────────
  파일: group4_ws\install\air_clean_robot\lib\python3.10\site-packages\air_c
  lean_robot\air_clean_pure_controller.py
  수정 방법: src에서 동기화 (재빌드 안 해도 즉시 반영되도록)
  ────────────────────────────────────────
  파일: group4_ws\install\air_clean_robot\share\air_clean_robot\config\air_c
  lean_params.yaml
  수정 방법: src에서 동기화
  ────────────────────────────────────────
  파일: group4_ws\install\air_clean_robot\lib\python3.10\site-packages\air_c
  lean_robot\__pycache__\air_clean_pure_controller.cpython-310.pyc
  수정 방법: 삭제 (stale .pyc 제거)
 

---

카메라 2대. 전체 구조 파악
 
  프로젝트: TurtleBot3 기반 자율주행 공기청정 로봇 (졸업 프로젝트로 추정)
 
  3개 계층이 MQTT로 묶여있음:
 
  [웹/Spring Boot] [ROS2 (group4_ws)]
  robot-web ┌─ Nav2 AMCL (localization_launch)
   ├─ MqttCommandPublisher ◄────┤
   ├─ MqttRobotPoseSubscriber │
   ├─ MqttRobotBatterySubscriber ├─ air_clean_pure_controller ← 주행
  (A*+Pure Pursuit)
   ├─ MqttRobotDustSubscriber ├─ mqtt_cmd_bridge ← MQTT↔ROS
  양방향
   └─ MqttRobotPathSubscriber ├─ amcl_pose_mqtt_publisher
          ▲ ├─ battery_state_mqtt_publisher
          │ 192.168.0.55:1883 ├─ dust_cells_mqtt_publisher
     [MQTT 브로커] ├─ air_clean_gui (Tkinter)
                                   └─ auto_initial_pose
 
  데이터 흐름 (주행 시):
  1. 웹에서 명령 발행 → MQTT robot/1/cmd
  2. mqtt_cmd_bridge.handle_command() 가 명령 파싱 → ROS /goal_pose
  (PoseStamped) 발행
  3. air_clean_pure_controller.goal_callback() 이 받아서 goal_pose 저장 +
  need_replan=True
  4. plan_path() 가 자체 A* 실행 (정적 inflation + 동적 LiDAR 장애물 +
  safety cost) → path_world 채움
  5. 10Hz control_loop() 가 Pure Pursuit으로 /cmd_vel 발행
  6. /astar_path, /amcl_pose 는 별도 브리지 노드들이 MQTT로 다시 웹에 전달
 
  메인 launch: air_clean_astar.launch.py (= pure_controller + GUI + MQTT
  브리지들). 별도로 air_clean_localization.launch.py 를 띄워 AMCL/RViz를
  실행.
 
  핵심 파일:
  - 컨트롤러:
  src/air_clean_robot/air_clean_robot/air_clean_pure_controller.py (자체 A*
  + 클리어셀 + 안전코스트맵 + Pure Pursuit이 한 노드에)
  - 파라미터: src/air_clean_robot/config/air_clean_params.yaml
  - air_clean_astar_controller.py 와 astar_follower.py 는 구버전 - 현재
  launch에서 사용 안 함.
 
  ---
  2. 문제가 있던 부분
 
   신규 기능 상태
  - pose_timeout_sec (AMCL pose 가 stale이면 cmd_vel 정지)
  - path_index_max_advance_cells (advance_path_index 점프 한도, 기존
  하드코딩 +40 대체)
 
  그런데 pose_timeout_sec 의 구현이 stationary AMCL 환경에서 영구 정지
  deadlock 을 일으킴
 
  # Code.docx의 control_loop
  if self.last_pose_time > 0.0 and self.pose_timeout_sec > 0.0:
      now = self.get_clock().now().nanoseconds / 1_000_000_000.0
      if (now - self.last_pose_time) > self.pose_timeout_sec:
          self.publish_stop() # cmd_vel 정지
          self.need_replan = True
          return # control_loop 종료
 
  deadlock 시나리오:
  1. AMCL은 update_min_d/update_min_a 임계 이상 움직여야만 /amcl_pose를 다시
   publish함 (정지 상태에서는 publish 멈춤).
  2. 사용자가 MQTT로 명령을 보내는 시점에는 로봇이 아직 정지 상태라
  last_pose_time이 이미 오래됨.
  3. control_loop의 timeout이 즉시 fire → publish_stop() + need_replan=True
  + return.
  4. cmd_vel을 0으로 깔았으니 로봇은 안 움직이고, AMCL도 publish를 안 하니
  last_pose_time이 갱신될 일이 없음.
  5. 다음 100ms에 control_loop가 다시 돌아도 똑같이 timeout fire → 영구
  정지.
 
  이 패턴이 증상 — "MQTT/위치는 정상인데 주행만 안 됨" —
  과 일치 (/cmd_vel은 0인 Twist만 계속 publish 중).
 
  use_scan_obstacles: false, inflation_radius_cells: 5,
  path_smoothing_enabled: false 등 다른 변경은 주행 가능성에 영향이 없음을
  코드 추적으로 확인
 
  ---
  3. 해결한 방법과 수정한 코드
 
  원칙: Code.docx와 데스크톱 yaml의 의도(stale pose 방어 + path_index 점프
  제한)는 그대로 유지하면서, deadlock만 끊어냄. 다른 기능은 건들지 않음.
 
  3가지 수정:
 
  (a) pose_timeout 가드를 "path 추종 중일 때만" 으로 한정
 
  def control_loop(self):
      if self.shoe_detected:
          self.publish_stop()
          return
      if self.goal_pose is None:
          return
      if self.current_pose is None:
          return
 
      # AMCL pose가 일정 시간 이상 stale이면 cmd_vel 정지.
      # 단, 아직 path가 없으면(목표 직후/계획 전) 체크하지 않는다.
      # AMCL이 update_min_d/a 때문에 stationary 동안 publish를 멈추는
  환경에서는
      # path가 생기기 전 이 체크가 즉시 fire되어 영구 정지 deadlock이
  발생하므로,
      # 실제 path를 추종 중일 때에만 동작시킨다.
      if (
          self.path_world
          and self.last_pose_time > 0.0
          and self.pose_timeout_sec > 0.0
      ):
          now = self.get_clock().now().nanoseconds / 1_000_000_000.0
          if (now - self.last_pose_time) > self.pose_timeout_sec:
              self.publish_stop()
              self.need_replan = True
              return
 
      if self.need_replan:
          self.plan_path()
          return
      ...
 
  (b) goal 수신 시 last_pose_time 리셋
 
  목표가 들어온 순간을 "pose가 fresh한 시점"으로 처리해 stationary
  잔여시간이 timeout을 잡아먹는 일을 방지.
 
  # set_goal_from_target / goal_callback 에 동일하게 추가
  self.last_pose_time = self.get_clock().now().nanoseconds / 1_000_000_000.0
 
  (c) pose_callback에서 last_pose_time 갱신, advance_path_index의 +40
  하드코딩 → 파라미터화
 
  # pose_callback 안
  self.last_pose_time = self.get_clock().now().nanoseconds / 1_000_000_000.0
 
  # advance_path_index
  max_step = max(1, self.path_index_max_advance_cells)
  search_end = min(last_index, self.path_index + max_step)
 
  이로써:
  - 새 명령 직후엔 timeout이 안 깨물어서 일단 출발 가능
  - 출발 후엔 AMCL이 motion을 감지하므로 last_pose_time이 정상 갱신되어
  timeout이 본래 의도(통신 단절 방어)로 동작
  - 이후 정말 stale이 길어지면 그때 비로소 정지
 
  ---
  4. 수정된 파일 경로와 수정 방법
 
  파일:
  group4_ws\src\air_clean_robot\air_clean_robot\air_clean_pure_controller.py
  수정 방법: pose_timeout_sec (default 3.0)·path_index_max_advance_cells
    (default 5) 파라미터 declare/get, self.last_pose_time = 0.0 상태 추가,
    pose_callback·set_goal_from_target·goal_callback에서 last_pose_time
  갱신,
     control_loop에 path 추종 중에만 동작하는 pose_timeout 가드 추가,
    advance_path_index의 하드코딩 +40 을 path_index_max_advance_cells
    파라미터로 교체
  ────────────────────────────────────────
  파일: group4_ws\src\air_clean_robot\config\air_clean_params.yaml
  수정 방법: air_clean_pure_controller: 섹션에 pose_timeout_sec: 3.0,
    path_index_max_advance_cells: 5 추가. 데스크톱 yaml과 같은
    튜닝값(max_angular_speed: 0.50, heading_gain: 1.2,
    rotate_min_angular_speed: 0.12, waypoint_tolerance: 0.08,
    rotate_in_place_threshold: 0.55, lookahead_distance: 0.22)으로 갱신
  ────────────────────────────────────────
  파일: group4_ws\install\air_clean_robot\lib\python3.10\site-packages\air_c
  lean_robot\air_clean_pure_controller.py
  수정 방법: src에서 동기화 (재빌드 안 해도 즉시 반영되도록)
  ────────────────────────────────────────
  파일: group4_ws\install\air_clean_robot\share\air_clean_robot\config\air_c
  lean_params.yaml
  수정 방법: src에서 동기화
  ────────────────────────────────────────
  파일: group4_ws\install\air_clean_robot\lib\python3.10\site-packages\air_c
  lean_robot\__pycache__\air_clean_pure_controller.cpython-310.pyc
  수정 방법: 삭제 (stale .pyc 제거)
 
 

---

웹캠 2대 캘리브레이션