Integration/panel tmux lid flow 20260609

src/azas_bringup/launch/auto_cup_flow_router.launch.py

@@ -27,7 +27,11 @@ def generate_launch_description():
         DeclareLaunchArgument("color_scan_at_start", default_value="true"),
         DeclareLaunchArgument("recipe_after_success", default_value="true"),
         DeclareLaunchArgument("recipe_colors", default_value=""),
-        DeclareLaunchArgument("cup_holder_place_z_offset_m", default_value="-0.04"),
+        DeclareLaunchArgument("cup_pre_from_place_x_offset_m", default_value="-0.12"),
+        DeclareLaunchArgument("final_regrasp_z_offset_m", default_value="-0.02"),
+        DeclareLaunchArgument("cup_holder_place_z_offset_m", default_value="-0.03"),
+        DeclareLaunchArgument("cup_holder_place_y_offset_m", default_value="0.0"),
+        DeclareLaunchArgument("cup_holder_z_min_m", default_value="0.08"),
         DeclareLaunchArgument("lid_shake_after_recipe", default_value="true"),
         Node(
             package="azas_task_manager",
@@ -55,7 +59,11 @@ def generate_launch_description():
                 "color_scan_at_start": ParameterValue(LaunchConfiguration("color_scan_at_start"), value_type=bool),
                 "recipe_after_success": ParameterValue(LaunchConfiguration("recipe_after_success"), value_type=bool),
                 "recipe_colors": LaunchConfiguration("recipe_colors"),
+                "cup_pre_from_place_x_offset_m": ParameterValue(LaunchConfiguration("cup_pre_from_place_x_offset_m"), value_type=float),
+                "final_regrasp_z_offset_m": ParameterValue(LaunchConfiguration("final_regrasp_z_offset_m"), value_type=float),
                 "cup_holder_place_z_offset_m": ParameterValue(LaunchConfiguration("cup_holder_place_z_offset_m"), value_type=float),
+                "cup_holder_place_y_offset_m": ParameterValue(LaunchConfiguration("cup_holder_place_y_offset_m"), value_type=float),
+                "cup_holder_z_min_m": ParameterValue(LaunchConfiguration("cup_holder_z_min_m"), value_type=float),
                 "lid_shake_after_recipe": ParameterValue(LaunchConfiguration("lid_shake_after_recipe"), value_type=bool),
             }],
         ),

src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py

@@ -505,4 +505,7 @@ def run_node(node_cls):
         node.run()
     finally:
         node.destroy_node()
-        rclpy.shutdown()
+        # exit_after_pick 경로 등에서 컨텍스트가 이미 닫혀 있으면 재호출 시
+        # RuntimeError로 exit code 1이 되어 라우터가 실패로 오인한다.
+        if rclpy.ok():
+            rclpy.shutdown()

src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py

@@ -1,7 +1,7 @@
 from copy import deepcopy
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction, Shutdown
 from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
@@ -54,6 +54,9 @@ def _runtime_nodes(context, moveit_params, moveit_py_params):
                 "controller_action_wait_sec": 60.0,
             },
         ],
+        # exit_after_pick으로 메인 노드가 끝나면 collision/tf 보조 노드들도 함께
+        # 정리해 launch가 종료되도록 한다. 안 그러면 라우터가 영원히 대기한다.
+        on_exit=Shutdown(),
     )
     return [yolo_cup_uprighting_node]
 

src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py

@@ -98,8 +98,14 @@ def __init__(self) -> None:
         )
         # 키오스크/음성 주문(latest_recipe.json) 없이 색을 직접 내릴 때: 예) "red:2,blue:1"
         self.declare_parameter("recipe_colors", "")
-        # 디스펜서 누르기 종료 후 컵홀더에 놓을 때 z를 더 낮추는 실측 보정값
-        self.declare_parameter("cup_holder_place_z_offset_m", -0.04)
+        # 디스펜서 누르기 종료 후 디스펜서 앞의 컵을 마지막으로 재파지할 때 z 실측 보정값
+        self.declare_parameter("final_regrasp_z_offset_m", -0.02)
+        # 잡기 직전 pre 위치(cup_place 기준 X offset) 보정값. 스크립트 기본 -0.09에 -30mm 추가
+        self.declare_parameter("cup_pre_from_place_x_offset_m", -0.12)
+        # 컵홀더에 놓을 때 보정값과 place 목표 z 안전 하한 (필요 시 조정)
+        self.declare_parameter("cup_holder_place_z_offset_m", -0.03)
+        self.declare_parameter("cup_holder_place_y_offset_m", 0.0)
+        self.declare_parameter("cup_holder_z_min_m", 0.08)
         self.declare_parameter("lid_shake_after_recipe", True)
         self.declare_parameter(
             "lid_shake_command",
@@ -485,8 +491,18 @@ def _run_recipe_sequence(self) -> bool:
         if colors:
             command += f" --colors {shlex.quote(colors)}"
             self.get_logger().info(f"recipe colors given directly: {colors}")
+        cup_pre_x = float(self.get_parameter("cup_pre_from_place_x_offset_m").value)
+        command += f" --cup-pre-from-place-x-offset-m {cup_pre_x}"
+        regrasp_z = float(self.get_parameter("final_regrasp_z_offset_m").value)
         place_z = float(self.get_parameter("cup_holder_place_z_offset_m").value)
-        command += f" --cup-holder-place-final-z-offset-m {place_z}"
+        place_y = float(self.get_parameter("cup_holder_place_y_offset_m").value)
+        z_min = float(self.get_parameter("cup_holder_z_min_m").value)
+        command += (
+            f" --final-regrasp-extra-z-offset-m {regrasp_z}"
+            f" --cup-holder-place-final-z-offset-m {place_z}"
+            f" --cup-holder-place-final-y-offset-m {place_y}"
+            f" --cup-holder-z-min-m {z_min}"
+        )
         self.get_logger().info("pick flow succeeded; starting integrated dispenser recipe sequence")
         return self._run_process(["bash", "-c", command], "recipe")
 
@@ -632,18 +648,24 @@ def _run_process(self, cmd: list[str], label: str) -> bool:
     def _forward_output(self, proc: subprocess.Popen[str], label: str) -> None:
         if proc.stdout is None:
             return
+        # 단계별 출력을 파일로도 남겨 실패 시 터미널 스크롤백 없이 진단할 수 있게 한다.
+        log_dir = "/tmp/azas_router_logs"
+        os.makedirs(log_dir, exist_ok=True)
+        log_path = os.path.join(log_dir, f"{label}_{time.strftime('%Y%m%d_%H%M%S')}_{proc.pid}.log")
         shutting_down = False
-        for line in proc.stdout:
-            text = line.rstrip()
-            self.get_logger().info(f"{label}> {text}")
-            if not shutting_down and self._SHUTDOWN_PATTERN.search(text):
-                shutting_down = True
-            match = self._NODE_DIED_PATTERN.search(text)
-            # launch 종료 신호 이후의 죽음(SIGINT 받은 KeyboardInterrupt 등)과
-            # 음수 exit code(시그널 종료)는 정상 정리 과정이므로 제외
-            if match and int(match.group("code")) > 0 and not shutting_down:
-                self._child_node_failures.setdefault(label, []).append(
-                    f"{match.group('node')} exit code {match.group('code')}")
+        with open(log_path, "w", encoding="utf-8", errors="replace") as log_file:
+            for line in proc.stdout:
+                text = line.rstrip()
+                self.get_logger().info(f"{label}> {text}")
+                log_file.write(text + "\n")
+                if not shutting_down and self._SHUTDOWN_PATTERN.search(text):
+                    shutting_down = True
+                match = self._NODE_DIED_PATTERN.search(text)
+                # launch 종료 신호 이후의 죽음(SIGINT 받은 KeyboardInterrupt 등)과
+                # 음수 exit code(시그널 종료)는 정상 정리 과정이므로 제외
+                if match and int(match.group("code")) > 0 and not shutting_down:
+                    self._child_node_failures.setdefault(label, []).append(
+                        f"{match.group('node')} exit code {match.group('code')}")
 
     def _stop_process(self, proc: Optional[subprocess.Popen[str]], label: str) -> None:
         if proc is None or proc.poll() is not None:

tools/run/run_color_recipe_sequence.py

@@ -369,6 +369,8 @@ def main() -> int:
     )
     parser.add_argument("--cup-holder-place-final-z-offset-m", default="-0.030")
     parser.add_argument("--cup-holder-place-final-y-offset-m", default="-0.010")
+    parser.add_argument("--cup-holder-z-min-m", default="0.08",
+                        help="컵홀더 place 목표 z 안전 하한. place z offset을 크게 낮출 때 함께 내려야 함")
     parser.add_argument("--cup-holder-approach-velocity", default="80.0")
     parser.add_argument("--cup-holder-approach-acceleration", default="20.0")
     parser.add_argument("--cup-holder-place-velocity", default="80.0")
@@ -450,6 +452,7 @@ def main() -> int:
         "--gripper-settle-seconds", str(args.gripper_settle_seconds),
         "--cup-holder-place-final-z-offset-m", str(args.cup_holder_place_final_z_offset_m),
         "--cup-holder-place-final-y-offset-m", str(args.cup_holder_place_final_y_offset_m),
+        "--cup-holder-z-min-m", str(args.cup_holder_z_min_m),
         "--cup-holder-approach-velocity", str(args.cup_holder_approach_velocity),
         "--cup-holder-approach-acceleration", str(args.cup_holder_approach_acceleration),
         "--cup-holder-place-velocity", str(args.cup_holder_place_velocity),