From aaa5fc1770655beac69a351438e67a9c652adbb7 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Fri, 12 Jun 2026 11:58:41 +0900 Subject: [PATCH 1/2] =?UTF-8?q?2=EC=B0=A8=20=ED=85=8C=EC=8A=A4=ED=8A=B8=20?= =?UTF-8?q?=EC=84=B1=EA=B3=B5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/auto_cup_flow_router.launch.py | 8 +++- .../azas_cup_uprighting/_base_node.py | 5 +- .../launch/yolo_cup_uprighting.launch.py | 5 +- .../azas_task_manager/auto_cup_flow_router.py | 46 +++++++++++++------ tools/run/run_color_recipe_sequence.py | 3 ++ 5 files changed, 50 insertions(+), 17 deletions(-) diff --git a/src/azas_bringup/launch/auto_cup_flow_router.launch.py b/src/azas_bringup/launch/auto_cup_flow_router.launch.py index e06d1d4..ef94bba 100644 --- a/src/azas_bringup/launch/auto_cup_flow_router.launch.py +++ b/src/azas_bringup/launch/auto_cup_flow_router.launch.py @@ -27,7 +27,10 @@ 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("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 +58,10 @@ 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"), + "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), }], ), diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py b/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py index fe23838..44a8cd9 100644 --- a/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py +++ b/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() diff --git a/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py b/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py index 2e26847..a5d73d1 100644 --- a/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py +++ b/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] diff --git a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py index 025dffb..00ba5b2 100644 --- a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py +++ b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py @@ -98,8 +98,12 @@ 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) + # 컵홀더에 놓을 때 보정값과 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 +489,16 @@ def _run_recipe_sequence(self) -> bool: if colors: command += f" --colors {shlex.quote(colors)}" self.get_logger().info(f"recipe colors given directly: {colors}") + 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 +644,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: diff --git a/tools/run/run_color_recipe_sequence.py b/tools/run/run_color_recipe_sequence.py index 04f0f75..060ef4d 100644 --- a/tools/run/run_color_recipe_sequence.py +++ b/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), From c9be6bfacf9eeb85c206d8f624815e6d92d73b85 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Fri, 12 Jun 2026 13:39:36 +0900 Subject: [PATCH 2/2] =?UTF-8?q?=EC=89=90=EC=9D=B4=ED=82=B9=EC=A0=84=20?= =?UTF-8?q?=EB=B0=8F=20stt=EB=A1=9C=EC=A7=81=20=EC=A0=9C=EC=99=B8=20?= =?UTF-8?q?=EC=9E=91=EB=8F=99=20=EA=B0=80=EB=8A=A5=20=ED=99=95=EC=9D=B8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/azas_bringup/launch/auto_cup_flow_router.launch.py | 2 ++ .../azas_task_manager/auto_cup_flow_router.py | 4 ++++ 2 files changed, 6 insertions(+) diff --git a/src/azas_bringup/launch/auto_cup_flow_router.launch.py b/src/azas_bringup/launch/auto_cup_flow_router.launch.py index ef94bba..f058c0e 100644 --- a/src/azas_bringup/launch/auto_cup_flow_router.launch.py +++ b/src/azas_bringup/launch/auto_cup_flow_router.launch.py @@ -27,6 +27,7 @@ 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_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"), @@ -58,6 +59,7 @@ 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), diff --git a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py index 00ba5b2..ccd3067 100644 --- a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py +++ b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py @@ -100,6 +100,8 @@ def __init__(self) -> None: self.declare_parameter("recipe_colors", "") # 디스펜서 누르기 종료 후 디스펜서 앞의 컵을 마지막으로 재파지할 때 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) @@ -489,6 +491,8 @@ 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) place_y = float(self.get_parameter("cup_holder_place_y_offset_m").value)