From 2840281641f1400d368482f478577d2372a4b9f8 Mon Sep 17 00:00:00 2001 From: shining-b-02 Date: Fri, 15 May 2026 15:03:28 +0900 Subject: [PATCH 1/7] Add dsr_practice package --- src/dsr_practice/config/T_gripper2camera.npy | Bin 0 -> 256 bytes src/dsr_practice/config/moveit_py.yaml | 54 + .../dsr_practice/.claude/settings.local.json | 20 + .../Calibration_Tutorial/data_recording.py | 72 ++ .../eye2hand_calibration.py | 283 +++++ .../handeye_calibration.py | 227 ++++ .../Calibration_Tutorial/onrobot.py | 184 +++ .../Calibration_Tutorial/realsense.py | 41 + .../dsr_practice/Calibration_Tutorial/test.py | 258 ++++ src/dsr_practice/dsr_practice/__init__.py | 0 .../dsr_practice/bar_detect_test.py | 116 ++ .../dsr_practice/bar_sort_node.py | 566 +++++++++ .../dsr_practice/click_pick_node.py | 408 ++++++ .../dsr_practice/collision_obstacle.py | 292 +++++ .../dsr_practice/config/T_gripper2camera.npy | Bin 0 -> 256 bytes .../dsr_practice/config/moveit_py.yaml | 54 + .../Calibration_Tutorial/data_recording.py | 72 ++ .../eye2hand_calibration.py | 283 +++++ .../handeye_calibration.py | 227 ++++ .../Calibration_Tutorial/onrobot.py | 184 +++ .../Calibration_Tutorial/realsense.py | 41 + .../dsr_practice/Calibration_Tutorial/test.py | 258 ++++ .../dsr_practice/dsr_practice/__init__.py | 0 .../dsr_practice/bar_detect_test.py | 116 ++ .../dsr_practice/bar_sort_node.py | 566 +++++++++ .../dsr_practice/click_pick_node.py | 408 ++++++ .../dsr_practice/collision_obstacle.py | 292 +++++ .../dsr_practice/gear_assembly.py | 423 ++++++ .../dsr_practice/dsr_practice/gripper.py | 98 ++ .../dsr_practice/joint_state_relay.py | 41 + .../dsr_practice/dsr_practice/mp_basic.py | 174 +++ .../dsr_practice/dsr_practice/mp_waypoint.py | 189 +++ .../dsr_practice/mp_waypoint_pilz.py | 239 ++++ .../dsr_practice/mp_waypoint_pilz_lin.py | 228 ++++ .../dsr_practice/dsr_practice/onrobot.py | 184 +++ .../dsr_practice/pick_and_place.py | 305 +++++ .../dsr_practice/realsense_data_collector.py | 213 ++++ .../dsr_practice/dsr_practice/stt_node.py | 108 ++ .../dsr_practice/stt_pick_and_place.py | 387 ++++++ .../dsr_practice/stt_robot_control.py | 376 ++++++ .../dsr_practice/syrup_pump_press.py | 227 ++++ .../dsr_practice/dsr_practice/test.py | 20 + .../dsr_practice/dsr_practice/test_2.py | 15 + .../dsr_practice/yolo_cup_pick_node.py | 1129 +++++++++++++++++ .../dsr_practice/gear_assembly.py | 423 ++++++ src/dsr_practice/dsr_practice/gripper.py | 98 ++ .../dsr_practice/joint_state_relay.py | 41 + .../launch/bar_sort_node.launch.py | 39 + .../launch/click_pick_node.launch.py | 42 + .../launch/collision_obstacle.launch.py | 43 + .../launch/gear_assembly.launch.py | 42 + .../dsr_practice/launch/mp_basic.launch.py | 43 + .../dsr_practice/launch/mp_waypoint.launch.py | 43 + .../launch/mp_waypoint_pilz.launch.py | 43 + .../launch/mp_waypoint_pilz_lin.launch.py | 43 + .../launch/pick_and_place.launch.py | 42 + .../launch/realsense_data_collector.launch.py | 52 + .../launch/stt_pick_and_place.launch.py | 54 + .../launch/stt_robot_control.launch.py | 54 + .../launch/syrup_pump_press.launch.py | 86 ++ .../launch/yolo_cup_pick_node.launch.py | 274 ++++ src/dsr_practice/dsr_practice/mp_basic.py | 174 +++ src/dsr_practice/dsr_practice/mp_waypoint.py | 189 +++ .../dsr_practice/mp_waypoint_pilz.py | 239 ++++ .../dsr_practice/mp_waypoint_pilz_lin.py | 228 ++++ src/dsr_practice/dsr_practice/onrobot.py | 184 +++ src/dsr_practice/dsr_practice/package.xml | 27 + .../dsr_practice/pick_and_place.py | 305 +++++ .../dsr_practice/realsense_data_collector.py | 213 ++++ .../dsr_practice/resource/dsr_practice | 0 src/dsr_practice/dsr_practice/setup.cfg | 4 + src/dsr_practice/dsr_practice/setup.py | 52 + src/dsr_practice/dsr_practice/stt_node.py | 108 ++ .../dsr_practice/stt_pick_and_place.py | 387 ++++++ .../dsr_practice/stt_robot_control.py | 376 ++++++ .../dsr_practice/syrup_pump_press.py | 227 ++++ src/dsr_practice/dsr_practice/test.py | 20 + .../dsr_practice/test/test_copyright.py | 25 + .../dsr_practice/test/test_flake8.py | 25 + .../dsr_practice/test/test_pep257.py | 23 + src/dsr_practice/dsr_practice/test_2.py | 15 + .../dsr_practice/yolo_cup_pick_node.py | 1129 +++++++++++++++++ .../launch/bar_sort_node.launch.py | 39 + .../launch/click_pick_node.launch.py | 42 + .../launch/collision_obstacle.launch.py | 43 + .../launch/gear_assembly.launch.py | 42 + src/dsr_practice/launch/mp_basic.launch.py | 43 + src/dsr_practice/launch/mp_waypoint.launch.py | 43 + .../launch/mp_waypoint_pilz.launch.py | 43 + .../launch/mp_waypoint_pilz_lin.launch.py | 43 + .../launch/pick_and_place.launch.py | 42 + .../launch/realsense_data_collector.launch.py | 52 + .../launch/stt_pick_and_place.launch.py | 54 + .../launch/stt_robot_control.launch.py | 54 + .../launch/syrup_pump_press.launch.py | 86 ++ .../launch/yolo_cup_pick_node.launch.py | 274 ++++ src/dsr_practice/package.xml | 27 + src/dsr_practice/resource/dsr_practice | 0 src/dsr_practice/setup.cfg | 4 + src/dsr_practice/setup.py | 52 + src/dsr_practice/test/test_copyright.py | 25 + src/dsr_practice/test/test_flake8.py | 25 + src/dsr_practice/test/test_pep257.py | 23 + 103 files changed, 15846 insertions(+) create mode 100644 src/dsr_practice/config/T_gripper2camera.npy create mode 100644 src/dsr_practice/config/moveit_py.yaml create mode 100644 src/dsr_practice/dsr_practice/.claude/settings.local.json create mode 100644 src/dsr_practice/dsr_practice/Calibration_Tutorial/data_recording.py create mode 100644 src/dsr_practice/dsr_practice/Calibration_Tutorial/eye2hand_calibration.py create mode 100644 src/dsr_practice/dsr_practice/Calibration_Tutorial/handeye_calibration.py create mode 100644 src/dsr_practice/dsr_practice/Calibration_Tutorial/onrobot.py create mode 100644 src/dsr_practice/dsr_practice/Calibration_Tutorial/realsense.py create mode 100644 src/dsr_practice/dsr_practice/Calibration_Tutorial/test.py create mode 100644 src/dsr_practice/dsr_practice/__init__.py create mode 100644 src/dsr_practice/dsr_practice/bar_detect_test.py create mode 100644 src/dsr_practice/dsr_practice/bar_sort_node.py create mode 100644 src/dsr_practice/dsr_practice/click_pick_node.py create mode 100644 src/dsr_practice/dsr_practice/collision_obstacle.py create mode 100644 src/dsr_practice/dsr_practice/config/T_gripper2camera.npy create mode 100644 src/dsr_practice/dsr_practice/config/moveit_py.yaml create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/Calibration_Tutorial/data_recording.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/Calibration_Tutorial/eye2hand_calibration.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/Calibration_Tutorial/handeye_calibration.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/Calibration_Tutorial/onrobot.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/Calibration_Tutorial/realsense.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/Calibration_Tutorial/test.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/__init__.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/bar_detect_test.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/bar_sort_node.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/click_pick_node.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/collision_obstacle.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/gear_assembly.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/gripper.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/joint_state_relay.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/mp_basic.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/mp_waypoint.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/mp_waypoint_pilz.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/mp_waypoint_pilz_lin.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/onrobot.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/pick_and_place.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/realsense_data_collector.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/stt_node.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/stt_pick_and_place.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/stt_robot_control.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/syrup_pump_press.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/test.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/test_2.py create mode 100644 src/dsr_practice/dsr_practice/dsr_practice/yolo_cup_pick_node.py create mode 100644 src/dsr_practice/dsr_practice/gear_assembly.py create mode 100644 src/dsr_practice/dsr_practice/gripper.py create mode 100644 src/dsr_practice/dsr_practice/joint_state_relay.py create mode 100644 src/dsr_practice/dsr_practice/launch/bar_sort_node.launch.py create mode 100644 src/dsr_practice/dsr_practice/launch/click_pick_node.launch.py create mode 100644 src/dsr_practice/dsr_practice/launch/collision_obstacle.launch.py create mode 100644 src/dsr_practice/dsr_practice/launch/gear_assembly.launch.py create mode 100644 src/dsr_practice/dsr_practice/launch/mp_basic.launch.py create mode 100644 src/dsr_practice/dsr_practice/launch/mp_waypoint.launch.py create mode 100644 src/dsr_practice/dsr_practice/launch/mp_waypoint_pilz.launch.py create mode 100644 src/dsr_practice/dsr_practice/launch/mp_waypoint_pilz_lin.launch.py create mode 100644 src/dsr_practice/dsr_practice/launch/pick_and_place.launch.py create mode 100644 src/dsr_practice/dsr_practice/launch/realsense_data_collector.launch.py create mode 100644 src/dsr_practice/dsr_practice/launch/stt_pick_and_place.launch.py create mode 100644 src/dsr_practice/dsr_practice/launch/stt_robot_control.launch.py create mode 100644 src/dsr_practice/dsr_practice/launch/syrup_pump_press.launch.py create mode 100644 src/dsr_practice/dsr_practice/launch/yolo_cup_pick_node.launch.py create mode 100644 src/dsr_practice/dsr_practice/mp_basic.py create mode 100644 src/dsr_practice/dsr_practice/mp_waypoint.py create mode 100644 src/dsr_practice/dsr_practice/mp_waypoint_pilz.py create mode 100644 src/dsr_practice/dsr_practice/mp_waypoint_pilz_lin.py create mode 100644 src/dsr_practice/dsr_practice/onrobot.py create mode 100644 src/dsr_practice/dsr_practice/package.xml create mode 100644 src/dsr_practice/dsr_practice/pick_and_place.py create mode 100644 src/dsr_practice/dsr_practice/realsense_data_collector.py create mode 100644 src/dsr_practice/dsr_practice/resource/dsr_practice create mode 100644 src/dsr_practice/dsr_practice/setup.cfg create mode 100644 src/dsr_practice/dsr_practice/setup.py create mode 100644 src/dsr_practice/dsr_practice/stt_node.py create mode 100644 src/dsr_practice/dsr_practice/stt_pick_and_place.py create mode 100644 src/dsr_practice/dsr_practice/stt_robot_control.py create mode 100644 src/dsr_practice/dsr_practice/syrup_pump_press.py create mode 100644 src/dsr_practice/dsr_practice/test.py create mode 100644 src/dsr_practice/dsr_practice/test/test_copyright.py create mode 100644 src/dsr_practice/dsr_practice/test/test_flake8.py create mode 100644 src/dsr_practice/dsr_practice/test/test_pep257.py create mode 100644 src/dsr_practice/dsr_practice/test_2.py create mode 100644 src/dsr_practice/dsr_practice/yolo_cup_pick_node.py create mode 100644 src/dsr_practice/launch/bar_sort_node.launch.py create mode 100644 src/dsr_practice/launch/click_pick_node.launch.py create mode 100644 src/dsr_practice/launch/collision_obstacle.launch.py create mode 100644 src/dsr_practice/launch/gear_assembly.launch.py create mode 100644 src/dsr_practice/launch/mp_basic.launch.py create mode 100644 src/dsr_practice/launch/mp_waypoint.launch.py create mode 100644 src/dsr_practice/launch/mp_waypoint_pilz.launch.py create mode 100644 src/dsr_practice/launch/mp_waypoint_pilz_lin.launch.py create mode 100644 src/dsr_practice/launch/pick_and_place.launch.py create mode 100644 src/dsr_practice/launch/realsense_data_collector.launch.py create mode 100644 src/dsr_practice/launch/stt_pick_and_place.launch.py create mode 100644 src/dsr_practice/launch/stt_robot_control.launch.py create mode 100644 src/dsr_practice/launch/syrup_pump_press.launch.py create mode 100644 src/dsr_practice/launch/yolo_cup_pick_node.launch.py create mode 100644 src/dsr_practice/package.xml create mode 100644 src/dsr_practice/resource/dsr_practice create mode 100644 src/dsr_practice/setup.cfg create mode 100644 src/dsr_practice/setup.py create mode 100644 src/dsr_practice/test/test_copyright.py create mode 100644 src/dsr_practice/test/test_flake8.py create mode 100644 src/dsr_practice/test/test_pep257.py diff --git a/src/dsr_practice/config/T_gripper2camera.npy b/src/dsr_practice/config/T_gripper2camera.npy new file mode 100644 index 0000000000000000000000000000000000000000..2a9474fc4635b9de2f2ce7ae3b2a2963d298f551 GIT binary patch literal 256 zcmbR27wQ`j$;eQ~P_3SlTAW;@Zl$1ZlV+i=qoAIaUsO_*m=~X4l#&V(cT3DEP6dh= zXCxM+0{I#yItnJ5ItsN4WCN~sdPnQ^UbxHmB!8Oyj>D6tnz~N1Uvitx;B2d% z!|&!N-Bxo__KU`TTsHCl`~5kcoR4|qTI|JEX!mov1UYOM+x-5w_aysMkGHK@?b>3$ aSnP3u*T47nR}_95_xyBpV89DL*aHBfCQbnW literal 0 HcmV?d00001 diff --git a/src/dsr_practice/config/moveit_py.yaml b/src/dsr_practice/config/moveit_py.yaml new file mode 100644 index 0000000..0e89b41 --- /dev/null +++ b/src/dsr_practice/config/moveit_py.yaml @@ -0,0 +1,54 @@ +/**: + ros__parameters: + planning_scene_monitor_options: + name: "planning_scene_monitor" + robot_description: "robot_description" + joint_state_topic: "/joint_states" + attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor" + publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene" + monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene" + wait_for_initial_state_timeout: 10.0 + + planning_pipelines: + pipeline_names: ["ompl", "pilz_industrial_motion_planner", "chomp", "ompl_rrt_star"] + + plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + max_velocity_scaling_factor: 0.1 + max_acceleration_scaling_factor: 0.1 + + ompl_rrtc: + plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + planner_id: "RRTConnectkConfigDefault" + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + planning_time: 1.0 + + ompl_rrt_star: + plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl_rrt_star + planner_id: "RRTstarkConfigDefault" + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + planning_time: 1.5 + + pilz_lin: + plan_request_params: + planning_attempts: 1 + planning_pipeline: pilz_industrial_motion_planner + planner_id: "PTP" + max_velocity_scaling_factor: 0.1 + max_acceleration_scaling_factor: 0.1 + planning_time: 0.8 + + chomp: + plan_request_params: + planning_attempts: 1 + planning_pipeline: chomp + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + planning_time: 1.5 \ No newline at end of file diff --git a/src/dsr_practice/dsr_practice/.claude/settings.local.json b/src/dsr_practice/dsr_practice/.claude/settings.local.json new file mode 100644 index 0000000..f48e03b --- /dev/null +++ b/src/dsr_practice/dsr_practice/.claude/settings.local.json @@ -0,0 +1,20 @@ +{ + "permissions": { + "allow": [ + "Bash(ros2 node:*)", + "Bash(python3:*)", + "Bash(source /home/deeptree/ros2_ws/install/setup.bash)", + "Bash(source /home/deeptree/ws_moveit/install/setup.bash)", + "Bash(colcon build:*)", + "Bash(python3 -c \":*)", + "Bash(find /home/deeptree/ws_moveit/build/moveit_py -name *.cpp)", + "Bash(find /home/deeptree/ws_moveit/src -name *.cpp -path */moveit_py/*)", + "Bash(grep:*)", + "Bash(for f:*)", + "Bash(do echo:*)", + "Read(//home/deeptree/ros2_ws/src/doosan-robot2/dsr_practice/**)", + "Bash(done)", + "Bash(sudo apt-get:*)" + ] + } +} diff --git a/src/dsr_practice/dsr_practice/Calibration_Tutorial/data_recording.py b/src/dsr_practice/dsr_practice/Calibration_Tutorial/data_recording.py new file mode 100644 index 0000000..ccbfcc3 --- /dev/null +++ b/src/dsr_practice/dsr_practice/Calibration_Tutorial/data_recording.py @@ -0,0 +1,72 @@ +import os +import cv2 +import json +import rclpy +import DR_init + +# 로봇 설정 +ROBOT_ID = "dsr01" +ROBOT_MODEL = "m0609" +VELOCITY, ACC = 60, 60 +DEVICE_NUMBER = 4 + +DR_init.dsr__id = ROBOT_ID +DR_init.__dsr__model = ROBOT_MODEL + + +def main(args=None): + rclpy.init(args=args) + node = rclpy.create_node("dsr_example_demo_py", namespace=ROBOT_ID) + DR_init.__dsr__node = node + # 로봇 제어 모듈 가져오기 + try: + from DSR_ROBOT2 import ( + get_current_posx, + set_tool, + set_tcp, + ) + except ImportError as e: + print(f"Error importing DSR_ROBOT2 : {e}") + return + # 공구 및 TCP 설정 + set_tool("Tool Weight_2FG") + set_tcp("2FG_TCP") + + # 데이터 저장 경로 설정 + source_path = "./data" + os.makedirs(source_path, exist_ok=True) + # 카메라 연결 + print(f"현재 선택된 device number는 {DEVICE_NUMBER}입니다.") + cap = cv2.VideoCapture(DEVICE_NUMBER) # 4 is camera number, set your camera number + + write_data = {} + write_data["poses"] = [] + write_data["file_name"] = [] + + while True: + ret, frame = cap.read() + + if not ret: + print("카메라를 찾을 수 없습니다. DEVICE_NUMBER를 변경해주세요.") + exit(True) + cv2.imshow("camera", frame) + + if cv2.waitKey(1) & 0xFF == ord("q"): + pos = get_current_posx()[0] + file_name = f"{pos[0]}_{pos[1]}_{pos[2]}.jpg" + # 현재 위치 기반 이미지 저장 + cv2.imwrite(f"{source_path}/{file_name}", frame) + print("current position1 : ", pos) + write_data["file_name"].append(file_name) + write_data["poses"].append(pos) + print(f"save img to {source_path}/{file_name}") + with open(f"{source_path}/calibrate_data.json", "w") as json_file: + json.dump(write_data, json_file, indent=4) + + cap.release() + cv2.destroyAllWindows() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/Calibration_Tutorial/eye2hand_calibration.py b/src/dsr_practice/dsr_practice/Calibration_Tutorial/eye2hand_calibration.py new file mode 100644 index 0000000..132faa5 --- /dev/null +++ b/src/dsr_practice/dsr_practice/Calibration_Tutorial/eye2hand_calibration.py @@ -0,0 +1,283 @@ +import json +from scipy.spatial.transform import Rotation +import numpy as np +import cv2 + +# 1) 로봇 그리퍼의 절대 좌표 (x, y, z, rx, ry, rz)를 행렬로 변환하는 함수 +def get_robot_pose_matrix(x, y, z, rx, ry, rz): + """ + 베이스->그리퍼 변환행렬 (4x4)을 반환. + """ + R = Rotation.from_euler('ZYZ', [rx, ry, rz], degrees=True).as_matrix() + T = np.eye(4) + T[:3, :3] = R + T[:3, 3] = [x, y, z] + return T + +# 2) 체커보드 코너 검출 (카메라→체커보드 변환 구하기) +def find_checkerboard_pose( + image, board_size, square_size, camera_matrix, dist_coeffs +): + """ + checkerboard_size = (7, 5) # 내부 코너 개수 + square_size = 25.0 # mm 단위 + 이미지에서 체커보드를 찾고, solvePnP로 카메라→체커보드 변환(R, t)을 구함. + 반환값: (R_camera2checker, t_camera2checker) + """ + objp = np.zeros((board_size[0] * board_size[1], 3), np.float32) + # 예: x 방향으로 square_size씩 증가, y 방향으로 square_size씩 증가 + objp[:, :2] = ( + np.mgrid[0 : board_size[0], 0 : board_size[1]].T.reshape(-1, 2) * 25 + ) + + gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) + found, corners = cv2.findChessboardCorners( + gray, + board_size, + flags=cv2.CALIB_CB_ADAPTIVE_THRESH + + cv2.CALIB_CB_FAST_CHECK + + cv2.CALIB_CB_NORMALIZE_IMAGE, + ) + if not found: + return None, None + + # 코너 좌표를 더 정확히 + corners_sub = cv2.cornerSubPix( + gray, + corners, + (11, 11), + (-1, -1), + criteria=(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001), + ) + + # solvePnP + retval, rvec, tvec = cv2.solvePnP(objp, corners_sub, camera_matrix, dist_coeffs) + if not retval: + return None, None + + # 회전벡터 -> 회전행렬 + R, _ = cv2.Rodrigues(rvec) + + return R, tvec + +# 체커보드 이미지를 이용한 카메라 보정 +def calibrate_camera_from_chessboard( + image_folder_path, + board_size, # (7, 5)처럼 내부 코너 개수 + square_size, # mm 단위 +): + """ + 지정된 폴더 안의 체커보드 이미지를 읽고, 카메라 행렬(camera_matrix)와 왜곡 계수(dist_coeffs)를 추정한다. + board_size: 체커보드 내부 코너 수 (cols, rows) + square_size: 체커보드 한 칸 크기 (mm) + """ + # 3D 세계 좌표계에 대한 좌표 생성 (z=0 평면 상에 체커보드) + objp = np.zeros((board_size[0] * board_size[1], 3), np.float32) + # 예: x 방향으로 square_size씩 증가, y 방향으로 square_size씩 증가 + objp[:, :2] = ( + np.mgrid[0 : board_size[0], 0 : board_size[1]].T.reshape(-1, 2) * square_size + ) + + # 모든 이미지에 대해 3D / 2D 포인트 누적 + obj_points = [] # 3D world points + img_points = [] # 2D image points + image_shape = None + + # 폴더 내에 있는 이미지 파일 읽기 + image_paths = image_folder_path # JPG, PNG 등 확장자 맞춰서 + # 필요하면 jpg 등 다른 확장자도 처리 가능 + + for fname in image_paths: + img = cv2.imread(fname) + if img is None: + continue + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + if image_shape is None: + image_shape = gray.shape[::-1] # (width, height) + + # 체커보드 코너 찾기 + ret, corners = cv2.findChessboardCorners(gray, board_size, None) + if ret: + # 코너를 더 정밀하게 + corners_sub = cv2.cornerSubPix( + gray, + corners, + (11, 11), + (-1, -1), + (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001), + ) + # 누적 + obj_points.append(objp) + img_points.append(corners_sub) + + # 내부 파라미터, 왜곡 계수, 외부 파라미터 구하기 + if len(obj_points) < 1: + print("체커보드 코너를 충분히 찾지 못하였습니다.") + return None, None, None, None + + # flags = cv2.CALIB_ZERO_TANGENT_DIST + cv2.CALIB_FIX_K3 등 필요에 따라 추가 + ret, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera( + obj_points, # 3D 실세계 점 + img_points, # 2D 이미지 점 + image_shape, # (width, height) + None, # 초기 camera_matrix + None, # 초기 dist_coeffs + ) + + if not ret: + print("캘리브레이션이 제대로 수렴하지 않았습니다.") + return None, None, None, None + + return camera_matrix, dist_coeffs, rvecs, tvecs + +from scipy.linalg import sqrtm +from numpy.linalg import inv + +# 4) 여러 개의 변환 행렬 조합 함수 +def compose_transformation_matrices(R_list, t_list): + T_list = [] + for R, t in zip(R_list, t_list): + T = np.eye(4) + T[:3, :3] = R + T[:3, 3] = np.ravel(t) # t가 벡터 형태여야 합니다. + T_list.append(T) + return T_list + +# 회전행렬을 로그변환하는 함수 +def logR(T): + R = T[0:3, 0:3] + theta = np.arccos((np.trace(R) - 1) / 2) + logr = np.array([ + R[2, 1] - R[1, 2], + R[0, 2] - R[2, 0], + R[1, 0] - R[0, 1] + ]) * theta / (2 * np.sin(theta)) + return logr + +# A와 B의 변환을 이용하여 보정 행렬 계산 +def Calibrate(A, B): + n_data = len(A) + M = np.zeros((3, 3)) + + for i in range(n_data - 1): + alpha = logR(A[i]) + beta = logR(B[i]) + alpha2 = logR(A[i + 1]) + beta2 = logR(B[i + 1]) + + alpha3 = np.cross(alpha, alpha2) + beta3 = np.cross(beta, beta2) + + M1 = np.dot(beta.reshape(3, 1), alpha.reshape(1, 3)) + M2 = np.dot(beta2.reshape(3, 1), alpha2.reshape(1, 3)) + M3 = np.dot(beta3.reshape(3, 1), alpha3.reshape(1, 3)) + + M += M1 + M2 + M3 + + theta = np.dot(sqrtm(inv(np.dot(M.T, M))), M.T) + + C = np.zeros((3 * n_data, 3)) + d = np.zeros((3 * n_data, 1)) + for i in range(n_data): + rot_a = A[i][:3, :3] + trans_a = A[i][:3, 3] + trans_b = B[i][:3, 3] + C[3 * i:3 * i + 3, :] = np.eye(3) - rot_a + d[3 * i:3 * i + 3, 0] = trans_a - np.dot(theta, trans_b) + + b_x = np.dot(inv(np.dot(C.T, C)), np.dot(C.T, d)) + return theta, b_x + +# Main Function +if __name__ == "__main__": + data = json.load(open("data/calibrate_data.json")) + robot_poses = np.array(data["poses"]) + + robot_poses[:, :3] = robot_poses[:, :3] + image_paths = ["data/" + d for d in data["file_name"]] + + valid_indices = [] + for i, pose in enumerate(robot_poses): + T_base2gripper = get_robot_pose_matrix(*pose) + det_T = np.linalg.det(T_base2gripper) + print(f"Index {i}: det(T_base2gripper) = {det_T}") + + if np.abs(det_T) > 1e-6: + valid_indices.append(i) + else: + print(f"⚠️ Warning: Singular T_base2gripper at index {i}!") + + robot_poses = robot_poses[valid_indices] + image_paths = [image_paths[i] for i in valid_indices] + + checkerboard_size = (8, 6) # 내부 코너 개수 + square_size = 25 + + camera_matrix, dist_coeffs, rvecs, tvecs = calibrate_camera_from_chessboard( + image_paths, checkerboard_size, square_size + ) + + R_gripper2base_list = [] + t_gripper2base_list = [] + R_camera2checker_list = [] + t_camera2checker_list = [] + R_checker2camera_list = [] + t_checker2camera_list = [] + + for img_path, pose in zip(image_paths, robot_poses): + # 1) 베이스->그리퍼 변환행렬 + T_base2gripper = get_robot_pose_matrix(*pose) + + # 2) 이미지 로딩 + image = cv2.imread(img_path) + if image is None: + continue + + # 3) 카메라->체커보드 변환 구하기 + R_cam2checker, t_cam2checker = find_checkerboard_pose( + image, checkerboard_size, square_size, camera_matrix, dist_coeffs + ) + if R_cam2checker is None: + continue + + T_gripper2base= np.linalg.inv(T_base2gripper) + + R_gripper2base = T_gripper2base[:3, :3] + t_gripper2base = T_gripper2base[:3, 3] + + R_gripper2base_list.append(R_gripper2base.copy()) + t_gripper2base_list.append(t_gripper2base.reshape(-1, 1).copy()) + + T_cam2checker = np.eye(4) + T_cam2checker[:3, :3] = R_cam2checker + T_cam2checker[:3, 3] = t_cam2checker.flatten() + T_checker2cam = np.linalg.inv(T_cam2checker) + + R_checker2camera_list.append(T_checker2cam[:3, :3].copy()) + t_checker2camera_list.append(T_checker2cam[:3, 3].copy()) + + T_gripper2base_list = compose_transformation_matrices(R_gripper2base_list, t_gripper2base_list) + T_checker2cam_list = compose_transformation_matrices(R_checker2camera_list, t_checker2camera_list) + A_list = [] + B_list = [] + num_pairs = min(len(T_gripper2base_list), len(T_checker2cam_list)) + + for i, T in enumerate(T_gripper2base_list): + det = np.linalg.det(T) + if np.abs(det) < 1e-6: + print(f"⚠️ Warning: T_gripper2base_list[{i}] is singular or nearly singular!") + + for i in range(num_pairs - 1): + A_i = np.dot(inv(T_gripper2base_list[i]), T_gripper2base_list[i + 1]) + B_i = np.dot(inv(T_checker2cam_list[i]), T_checker2cam_list[i + 1]) + A_list.append(A_i) + B_list.append(B_i) + + theta, b_x = Calibrate(A_list, B_list) + X = np.eye(4) + X[:3, :3] = theta + X[:3, 3] = b_x.flatten() + T_cam2base = X + print(T_cam2base) + print(T_cam2base[:3, 3]) + np.save("T_cam2base.npy", T_cam2base) diff --git a/src/dsr_practice/dsr_practice/Calibration_Tutorial/handeye_calibration.py b/src/dsr_practice/dsr_practice/Calibration_Tutorial/handeye_calibration.py new file mode 100644 index 0000000..75bbba1 --- /dev/null +++ b/src/dsr_practice/dsr_practice/Calibration_Tutorial/handeye_calibration.py @@ -0,0 +1,227 @@ +import cv2 +import numpy as np +import json +from scipy.spatial.transform import Rotation + +# 1) 로봇 그리퍼의 절대 좌표 (x, y, z, rx, ry, rz)를 행렬로 변환하는 함수 +def get_robot_pose_matrix(x, y, z, rx, ry, rz): + """ + 베이스->그리퍼 변환행렬 (4x4)을 반환. + """ + R = Rotation.from_euler('ZYZ', [rx, ry, rz], degrees=True).as_matrix() + T = np.eye(4) + T[:3, :3] = R + T[:3, 3] = [x, y, z] + return T + + +# 2) 체커보드 코너 검출 (카메라→체커보드 변환 구하기) +def find_checkerboard_pose( + image, board_size, square_size, camera_matrix, dist_coeffs +): + """ + checkerboard_size = (7, 5) # 내부 코너 개수 + square_size = 25.0 # mm 단위 + 이미지에서 체커보드를 찾고, solvePnP로 카메라→체커보드 변환(R, t)을 구함. + 반환값: (R_camera2checker, t_camera2checker) + """ + objp = np.zeros((board_size[0] * board_size[1], 3), np.float32) + # 예: x 방향으로 square_size씩 증가, y 방향으로 square_size씩 증가 + objp[:, :2] = ( + np.mgrid[0 : board_size[0], 0 : board_size[1]].T.reshape(-1, 2) * 25 + ) + + gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) + found, corners = cv2.findChessboardCorners( + gray, + board_size, + flags=cv2.CALIB_CB_ADAPTIVE_THRESH + + cv2.CALIB_CB_FAST_CHECK + + cv2.CALIB_CB_NORMALIZE_IMAGE, + ) + if not found: + return None, None + + # 코너 좌표를 더 정확히 + corners_sub = cv2.cornerSubPix( + gray, + corners, + (11, 11), + (-1, -1), + criteria=(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001), + ) + + # solvePnP + retval, rvec, tvec = cv2.solvePnP(objp, corners_sub, camera_matrix, dist_coeffs) + if not retval: + return None, None + + # 회전벡터 -> 회전행렬 + R, _ = cv2.Rodrigues(rvec) + + return R, tvec + + +def calibrate_camera_from_chessboard( + image_folder_path, + board_size, # (7, 5)처럼 내부 코너 개수 + square_size, # mm 단위 +): + """ + 지정된 폴더 안의 체커보드 이미지를 읽고, 카메라 행렬(camera_matrix)와 왜곡 계수(dist_coeffs)를 추정한다. + board_size: 체커보드 내부 코너 수 (cols, rows) + square_size: 체커보드 한 칸 크기 (mm) + """ + # 3D 세계 좌표계에 대한 좌표 생성 (z=0 평면 상에 체커보드) + objp = np.zeros((board_size[0] * board_size[1], 3), np.float32) + # 예: x 방향으로 square_size씩 증가, y 방향으로 square_size씩 증가 + objp[:, :2] = ( + np.mgrid[0 : board_size[0], 0 : board_size[1]].T.reshape(-1, 2) * square_size + ) + + # 모든 이미지에 대해 3D / 2D 포인트 누적 + obj_points = [] # 3D world points + img_points = [] # 2D image points + image_shape = None + + # 폴더 내에 있는 이미지 파일 읽기 + image_paths = image_folder_path # JPG, PNG 등 확장자 맞춰서 + # 필요하면 jpg 등 다른 확장자도 처리 가능 + + for fname in image_paths: + img = cv2.imread(fname) + if img is None: + continue + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + if image_shape is None: + image_shape = gray.shape[::-1] # (width, height) + + # 체커보드 코너 찾기 + ret, corners = cv2.findChessboardCorners(gray, board_size, None) + if ret: + # 코너를 더 정밀하게 + corners_sub = cv2.cornerSubPix( + gray, + corners, + (11, 11), + (-1, -1), + (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001), + ) + # 누적 + obj_points.append(objp) + img_points.append(corners_sub) + + # 내부 파라미터, 왜곡 계수, 외부 파라미터 구하기 + if len(obj_points) < 1: + print("체커보드 코너를 충분히 찾지 못하였습니다.") + return None, None, None, None + + # flags = cv2.CALIB_ZERO_TANGENT_DIST + cv2.CALIB_FIX_K3 등 필요에 따라 추가 + ret, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera( + obj_points, # 3D 실세계 점 + img_points, # 2D 이미지 점 + image_shape, # (width, height) + None, # 초기 camera_matrix + None, # 초기 dist_coeffs + ) + + if not ret: + print("캘리브레이션이 제대로 수렴하지 않았습니다.") + return None, None, None, None + + return camera_matrix, dist_coeffs, rvecs, tvecs + + + +# Main Function +if __name__ == "__main__": + # 캘리브레이션 데이터 로드 + data = json.load(open("data/calibrate_data.json")) + robot_poses = np.array(data["poses"]) + + robot_poses[:, :3] = robot_poses[:, :3] + image_paths = ["data/" + d for d in data["file_name"]] + + checkerboard_size = (10, 7) # 내부 코너 개수 + square_size = 25 + # 카메라 캘리브레이션 수행(내부 파라미터 왜곡 보정) + camera_matrix, dist_coeffs, rvecs, tvecs = calibrate_camera_from_chessboard( + image_paths, checkerboard_size, square_size + ) + + R_gripper2base_list = [] + t_gripper2base_list = [] + R_camera2checker_list = [] + t_camera2checker_list = [] + R_checker2camera_list = [] + t_checker2camera_list = [] + + for img_path, pose in zip(image_paths, robot_poses): + # 1) 베이스->그리퍼 변환행렬 + T_base2gripper = get_robot_pose_matrix(*pose) + + # 2) 이미지 로딩 + image = cv2.imread(img_path) + if image is None: + continue + + # 3) 카메라->체커보드 변환 구하기 + R_cam2checker, t_cam2checker = find_checkerboard_pose( + image, checkerboard_size, square_size, camera_matrix, dist_coeffs + ) + if R_cam2checker is None: + continue + + # T_gripper2base= np.linalg.inv(T_base2gripper) + T_gripper2base= T_base2gripper + + R_gripper2base = T_gripper2base[:3, :3] + t_gripper2base = T_gripper2base[:3, 3] + + R_gripper2base_list.append(R_gripper2base.copy()) + t_gripper2base_list.append(t_gripper2base.reshape(-1, 1).copy()) + + T_cam2checker = np.eye(4) + T_cam2checker[:3, :3] = R_cam2checker + T_cam2checker[:3, 3] = t_cam2checker.flatten() + + T_checker2cam = T_cam2checker + + R_checker2camera_list.append(T_checker2cam[:3, :3].copy()) + t_checker2camera_list.append(T_checker2cam[:3, 3].copy()) + + + # Hand-Eye 캘리브레이션 수행 + R_cam2gripper, t_cam2gripper = cv2.calibrateHandEye( + R_gripper2base_list, + t_gripper2base_list, + R_checker2camera_list, + t_checker2camera_list, + method=cv2.CALIB_HAND_EYE_PARK, + ) + + + T_base2gripper_example = get_robot_pose_matrix(*robot_poses[2]) + R_base2gripper_example = T_base2gripper_example[:3, :3] + t_base2gripper_example = T_base2gripper_example[:3, 3] + + # 그리퍼->카메라 변환행렬 + T_gripper2cam = np.eye(4) + T_gripper2cam[:3, :3] = R_cam2gripper + T_gripper2cam[:3, 3] = t_cam2gripper.flatten() + + # 최종 베이스->카메라 + T_base2cam = T_base2gripper_example @ T_gripper2cam + + print("===== Hand-Eye Calibration Results =====") + print("R_base2gripper:\n", T_base2gripper_example[:3, :3]) + print("T_base2gripper:\n", T_base2gripper_example[:3, 3]) + print("\n") + print("R_base2camera:\n", T_base2cam[:3, :3]) + print("T_base2camera:\n", T_base2cam[:3, 3]) + print("\n") + print("R_gripper2camera:\n", T_gripper2cam[:3, :3]) + print("T_gripper2camera:\n", T_gripper2cam[:3, 3].tolist()) + + # save T_grigper2camera + np.save("T_gripper2camera.npy", T_gripper2cam) diff --git a/src/dsr_practice/dsr_practice/Calibration_Tutorial/onrobot.py b/src/dsr_practice/dsr_practice/Calibration_Tutorial/onrobot.py new file mode 100644 index 0000000..73931dc --- /dev/null +++ b/src/dsr_practice/dsr_practice/Calibration_Tutorial/onrobot.py @@ -0,0 +1,184 @@ +#!/usr/bin/env python3 + +from pymodbus.client.sync import ModbusTcpClient as ModbusClient + + +class RG(): + + def __init__(self, gripper, ip, port): + self.client = ModbusClient( + ip, + port=port, + stopbits=1, + bytesize=8, + parity='E', + baudrate=115200, + timeout=1) + if gripper not in ['rg2', 'rg6']: + print("Please specify either rg2 or rg6.") + return + self.gripper = gripper # RG2/6 + if self.gripper == 'rg2': + self.max_width = 1100 + self.max_force = 400 + elif self.gripper == 'rg6': + self.max_width = 1600 + self.max_force = 1200 + self.open_connection() + + def open_connection(self): + """Opens the connection with a gripper.""" + self.client.connect() + + def close_connection(self): + """Closes the connection with the gripper.""" + self.client.close() + + def get_fingertip_offset(self): + """Reads the current fingertip offset in 1/10 millimeters. + Please note that the value is a signed two's complement number. + """ + result = self.client.read_holding_registers( + address=258, count=1, unit=65) + offset_mm = result.registers[0] / 10.0 + return offset_mm + + def get_width(self): + """Reads current width between gripper fingers in 1/10 millimeters. + Please note that the width is provided without any fingertip offset, + as it is measured between the insides of the aluminum fingers. + """ + result = self.client.read_holding_registers( + address=267, count=1, unit=65) + width_mm = result.registers[0] / 10.0 + return width_mm + + def get_status(self): + """Reads current device status. + This status field indicates the status of the gripper and its motion. + It is composed of 7 flags, described in the table below. + + Bit Name Description + 0 (LSB): busy High (1) when a motion is ongoing, + low (0) when not. + The gripper will only accept new commands + when this flag is low. + 1: grip detected High (1) when an internal- or + external grip is detected. + 2: S1 pushed High (1) when safety switch 1 is pushed. + 3: S1 trigged High (1) when safety circuit 1 is activated. + The gripper will not move + while this flag is high; + can only be reset by power cycling. + 4: S2 pushed High (1) when safety switch 2 is pushed. + 5: S2 trigged High (1) when safety circuit 2 is activated. + The gripper will not move + while this flag is high; + can only be reset by power cycling. + 6: safety error High (1) when on power on any of + the safety switch is pushed. + 10-16: reserved Not used. + """ + # address : register number + # count : number of registers to be read + # unit : slave device address + result = self.client.read_holding_registers( + address=268, count=1, unit=65) + status = format(result.registers[0], '016b') + status_list = [0] * 7 + if int(status[-1]): + print("A motion is ongoing so new commands are not accepted.") + status_list[0] = 1 + if int(status[-2]): + print("An internal- or external grip is detected.") + status_list[1] = 1 + if int(status[-3]): + print("Safety switch 1 is pushed.") + status_list[2] = 1 + if int(status[-4]): + print("Safety circuit 1 is activated so it will not move.") + status_list[3] = 1 + if int(status[-5]): + print("Safety switch 2 is pushed.") + status_list[4] = 1 + if int(status[-6]): + print("Safety circuit 2 is activated so it will not move.") + status_list[5] = 1 + if int(status[-7]): + print("Any of the safety switch is pushed.") + status_list[6] = 1 + + return status_list + + def get_width_with_offset(self): + """Reads current width between gripper fingers in 1/10 millimeters. + The set fingertip offset is considered. + """ + result = self.client.read_holding_registers( + address=275, count=1, unit=65) + width_mm = result.registers[0] / 10.0 + return width_mm + + def set_control_mode(self, command): + """The control field is used to start and stop gripper motion. + Only one option should be set at a time. + Please note that the gripper will not start a new motion + before the one currently being executed is done + (see busy flag in the Status field). + The valid flags are: + + 1 (0x0001): grip + Start the motion, with the target force and width. + Width is calculated without the fingertip offset. + Please note that the gripper will ignore this command + if the busy flag is set in the status field. + 8 (0x0008): stop + Stop the current motion. + 16 (0x0010): grip_w_offset + Same as grip, but width is calculated + with the set fingertip offset. + """ + result = self.client.write_register( + address=2, value=command, unit=65) + + def set_target_force(self, force_val): + """Writes the target force to be reached + when gripping and holding a workpiece. + It must be provided in 1/10th Newtons. + The valid range is 0 to 400 for the RG2 and 0 to 1200 for the RG6. + """ + result = self.client.write_register( + address=0, value=force_val, unit=65) + + def set_target_width(self, width_val): + """Writes the target width between + the finger to be moved to and maintained. + It must be provided in 1/10th millimeters. + The valid range is 0 to 1100 for the RG2 and 0 to 1600 for the RG6. + Please note that the target width should be provided + corrected for any fingertip offset, + as it is measured between the insides of the aluminum fingers. + """ + result = self.client.write_register( + address=1, value=width_val, unit=65) + + def close_gripper(self, force_val=400): + """Closes gripper.""" + params = [force_val, 0, 16] + print("Start closing gripper.") + result = self.client.write_registers( + address=0, values=params, unit=65) + + def open_gripper(self, force_val=400): + """Opens gripper.""" + params = [force_val, self.max_width, 16] + print("Start opening gripper.") + result = self.client.write_registers( + address=0, values=params, unit=65) + + def move_gripper(self, width_val, force_val=400): + """Moves gripper to the specified width.""" + params = [force_val, width_val, 16] + print("Start moving gripper.") + result = self.client.write_registers( + address=0, values=params, unit=65) diff --git a/src/dsr_practice/dsr_practice/Calibration_Tutorial/realsense.py b/src/dsr_practice/dsr_practice/Calibration_Tutorial/realsense.py new file mode 100644 index 0000000..45a6813 --- /dev/null +++ b/src/dsr_practice/dsr_practice/Calibration_Tutorial/realsense.py @@ -0,0 +1,41 @@ +from rclpy.node import Node +from sensor_msgs.msg import Image, CameraInfo +from cv_bridge import CvBridge + + +class ImgNode(Node): + def __init__(self): + super().__init__('img_node') + self.bridge = CvBridge() + self.color_frame = None + self.color_frame_stamp = None + self.depth_frame = None + self.intrinsics = None + self.color_subscription = self.create_subscription( + Image, '/camera/camera/color/image_raw', self.color_callback, 10) + self.depth_subscription = self.create_subscription( + Image, '/camera/camera/aligned_depth_to_color/image_raw', self.depth_callback, 10) + self.camera_info_subscription = self.create_subscription( + CameraInfo, '/camera/camera/color/camera_info', self.camera_info_callback, 10) + + def camera_info_callback(self, msg): + self.intrinsics = {"fx": msg.k[0], "fy": msg.k[4], "ppx": msg.k[2], "ppy": msg.k[5]} + + def color_callback(self, msg): + self.color_frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8') + self.color_frame_stamp = str(msg.header.stamp.sec) + str(msg.header.stamp.nanosec) + + def depth_callback(self, msg): + self.depth_frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough') + + def get_color_frame(self): + return self.color_frame + + def get_color_frame_stamp(self): + return self.color_frame_stamp + + def get_depth_frame(self): + return self.depth_frame + + def get_camera_intrinsic(self): + return self.intrinsics \ No newline at end of file diff --git a/src/dsr_practice/dsr_practice/Calibration_Tutorial/test.py b/src/dsr_practice/dsr_practice/Calibration_Tutorial/test.py new file mode 100644 index 0000000..0f974f5 --- /dev/null +++ b/src/dsr_practice/dsr_practice/Calibration_Tutorial/test.py @@ -0,0 +1,258 @@ +import cv2 +import rclpy +import time +import numpy as np +import threading +from scipy.spatial.transform import Rotation + +from realsense import ImgNode +from onrobot import RG +import DR_init + +# ====================== +# 로봇 / 그리퍼 설정 +# ====================== +ROBOT_ID = "dsr01" +ROBOT_MODEL = "m0609" +VELOCITY, ACC = 60, 60 + +DR_init.__dsr__id = ROBOT_ID +DR_init.__dsr__model = ROBOT_MODEL + +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 # 정수 + +# ====================== +# Z 관련 파라미터 +# ====================== +Z_OFFSET = 200.0 # 클릭한 지점 z에 더해 줄 오프셋 (mm) +SAFE_Z = 400.0 # 집은 뒤 올라갈 안전 높이 (mm) – 환경 보고 조정 + + +class TestNode: + def __init__(self): + # RealSense 노드 + self.img_node = ImgNode() + + # Intrinsic 수신될 때까지 대기 + while rclpy.ok() and self.img_node.get_camera_intrinsic() is None: + rclpy.spin_once(self.img_node, timeout_sec=0.1) + + self.intrinsics = self.img_node.get_camera_intrinsic() + + # Hand-eye 결과 (그리퍼 → 카메라) + self.gripper2cam = np.load("T_gripper2camera.npy") + + # 그리퍼 + self.gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + + # 준비자세(Joint) + self.JReady = posj([0, 0, 90, 0, 90, 90]) + + # 홈 XY (초기 자세에서 저장) + self.home_pose = None # [x, y, z, rx, ry, rz] + + # ========================= + # 카메라 → 베이스 좌표 변환 + # ========================= + def transform_to_base(self, camera_coords): + """ + camera_coords: (Xc, Yc, Zc) in mm (카메라 기준) + 반환: (Xb, Yb, Zb) in mm (base_link 기준) + """ + coord = np.append(np.array(camera_coords, dtype=float), 1.0) # [x,y,z,1] + + # 현재 베이스→그리퍼 + base2gripper = self.get_robot_pose_matrix(*get_current_posx()[0]) + + # 베이스→카메라 = 베이스→그리퍼 · 그리퍼→카메라 + base2cam = base2gripper @ self.gripper2cam + + td_coord = base2cam @ coord + + return td_coord[:3] + + def get_robot_pose_matrix(self, x, y, z, rx, ry, rz): + R = Rotation.from_euler("ZYZ", [rx, ry, rz], degrees=True).as_matrix() + T = np.eye(4) + T[:3, :3] = R + T[:3, 3] = [x, y, z] + return T + + # ========================= + # Pick 시퀀스 + # ========================= + def pick_and_place(self, x, y, z): + + print("\n========== PICK SEQUENCE ==========") + print(f"Base coord raw : x={x:.2f}, y={y:.2f}, z={z:.2f}") + print(f"Z_OFFSET : {Z_OFFSET}") + print(f"SAFE_Z : {SAFE_Z}") + print("===================================\n") + + # 현재 포즈 + cur = get_current_posx()[0] + cur_x, cur_y, cur_z, rx, ry, rz = cur + + # home pose (run()에서 설정) + if self.home_pose is None: + self.home_pose = cur + home_x, home_y, home_z, hrx, hry, hrz = self.home_pose + + # 0) 안전용으로 한 번 더 open (혹시 이전에 안 열려 있으면) + self.gripper.open_gripper() + time.sleep(0.5) + + # 1) 클릭한 위치 x,y로 이동 (z는 현재 값 유지) + target_xy = posx([x, y, cur_z, rx, ry, rz]) + print("[1] Move to XY only:", target_xy) + movel(target_xy, VELOCITY, ACC) + wait(0.5) + + # 2) z_correct 계산 및 이동 + z_correct = z + Z_OFFSET + target_xyz = posx([x, y, z_correct, rx, ry, rz]) + print(f"[2] Move down to z_correct={z_correct:.2f}:", target_xyz) + movel(target_xyz, VELOCITY, ACC) + wait(0.3) + + # 3) gripper close + print("[3] Gripper Close") + self.gripper.close_gripper() + time.sleep(1.0) + + # 4) z 상승: SAFE_Z까지 (x,y는 그대로) + up_pose = posx([x, y, SAFE_Z, rx, ry, rz]) + print(f"[4] Move up to SAFE_Z={SAFE_Z:.2f}:", up_pose) + movel(up_pose, VELOCITY, ACC) + wait(0.5) + + # 5) home xy 이동 (z는 SAFE_Z 유지) + home_xy_pose = posx([home_x, home_y, SAFE_Z, hrx, hry, hrz]) + print("[5] Move to home XY:", home_xy_pose) + movel(home_xy_pose, VELOCITY, ACC) + wait(0.5) + + # 6) place 높이 = pick 높이 이상으로 보장 + z_final = max(250.0, z_correct) + home_xy_z280 = posx([home_x, home_y, z_final, hrx, hry, hrz]) + print(f"[6] Move to home XY, z={z_final}:", home_xy_z280) + movel(home_xy_z280, VELOCITY, ACC) + wait(0.3) + + # 7) gripper open + print("[7] Gripper Open") + self.gripper.open_gripper() + time.sleep(1.0) + + # 8) 다시 SAFE_Z로 올려두고 끝낼지 여부 (원하면 유지) + back_up = posx([home_x, home_y, SAFE_Z, hrx, hry, hrz]) + print("[8] Back up to SAFE_Z:", back_up) + movel(back_up, VELOCITY, ACC) + wait(0.5) + + print("========== PICK END ==========\n") + + # ========================= + # 마우스 콜백 + # ========================= + def mouse_callback(self, event, x, y, flags, param): + if event == cv2.EVENT_LBUTTONDOWN and not hasattr(self, '_pick_thread_running'): + depth_frame = self.img_node.get_depth_frame() + if depth_frame is None: + print("No depth frame") + return + + # 픽셀 범위 체크 + h, w = depth_frame.shape + if not (0 <= x < w and 0 <= y < h): + print("Click out of range") + return + + z = depth_frame[y, x] + if z == 0: + print("Depth invalid at clicked point") + return + + # 카메라 좌표 (mm) 계산 + fx = self.intrinsics["fx"] + fy = self.intrinsics["fy"] + ppx = self.intrinsics["ppx"] + ppy = self.intrinsics["ppy"] + + X = (x - ppx) * z / fx + Y = (y - ppy) * z / fy + Z = z + + cam_coord = (X, Y, Z) + base_coord = self.transform_to_base(cam_coord) + + print("Camera:", cam_coord) + print("Base :", base_coord) + + def run_pick(): + self._pick_thread_running = True + self.pick_and_place(*base_coord) + del self._pick_thread_running + + threading.Thread(target=run_pick, daemon=True).start() + + # ========================= + # 메인 루프 + # ========================= + def run(self): + cv2.namedWindow("Webcam") + cv2.setMouseCallback("Webcam", self.mouse_callback) + + # rclpy spin을 별도 스레드로 분리 (pick 스레드와 충돌 방지) + executor = rclpy.executors.MultiThreadedExecutor() + executor.add_node(self.img_node) + spin_thread = threading.Thread(target=executor.spin, daemon=True) + spin_thread.start() + + # 초기 자세로 이동 + print("[Init] movej JReady") + movej(self.JReady, VELOCITY, ACC) + wait(1.0) + + # 현재 자세를 home_pose로 저장 + self.home_pose = get_current_posx()[0] + + # 초기 gripper open + print("[Init] Gripper Open") + self.gripper.open_gripper() + time.sleep(1.0) + + while True: + img = self.img_node.get_color_frame() + if img is None: + time.sleep(0.01) + continue + + cv2.imshow("Webcam", img) + + if cv2.waitKey(1) & 0xFF == 27: # ESC + break + + executor.shutdown() + cv2.destroyAllWindows() + + +if __name__ == "__main__": + rclpy.init() + node = rclpy.create_node("dsr_example_demo_py", namespace=ROBOT_ID) + DR_init.__dsr__node = node + + try: + from DSR_ROBOT2 import get_current_posx, movej, movel, wait + from DR_common2 import posx, posj + except ImportError as e: + print(f"Error importing DSR_ROBOT2 : {e}") + rclpy.shutdown() + raise SystemExit(1) + + test = TestNode() + test.run() + + rclpy.shutdown() \ No newline at end of file diff --git a/src/dsr_practice/dsr_practice/__init__.py b/src/dsr_practice/dsr_practice/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/dsr_practice/dsr_practice/bar_detect_test.py b/src/dsr_practice/dsr_practice/bar_detect_test.py new file mode 100644 index 0000000..dacfa6c --- /dev/null +++ b/src/dsr_practice/dsr_practice/bar_detect_test.py @@ -0,0 +1,116 @@ +#!/usr/bin/env python3 +""" +bar_detect_test.py – 마우스 호버 depth 표시 (시각화 전용) + +RealSense 영상 위에서 마우스 커서 위치의 depth(mm)를 실시간으로 표시. +ESC 로 종료. +""" + +import cv2 +import numpy as np +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import Image +from cv_bridge import CvBridge + + +class BarDetectTest(Node): + def __init__(self): + super().__init__("bar_detect_test") + self.bridge = CvBridge() + self.color_image = None + self.depth_image = None + self.mouse_xy = None # (x, y) 커서 좌표 + + self.create_subscription( + Image, "/camera/camera/color/image_raw", + self._color_cb, 10) + self.create_subscription( + Image, "/camera/camera/aligned_depth_to_color/image_raw", + self._depth_cb, 10) + + def _color_cb(self, msg): + self.color_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") + + def _depth_cb(self, msg): + self.depth_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough") + + # ── 마우스 콜백 ── + def _mouse_cb(self, event, x, y, flags, param): + if event == cv2.EVENT_MOUSEMOVE: + self.mouse_xy = (x, y) + + # ── depth 샘플링 (5×5 median, mm) ── + def sample_depth_mm(self, px, py): + if self.depth_image is None: + return None + h, w = self.depth_image.shape[:2] + if not (0 <= px < w and 0 <= py < h): + return None + x0, x1 = max(0, px - 2), min(w, px + 3) + y0, y1 = max(0, py - 2), min(h, py + 3) + patch = self.depth_image[y0:y1, x0:x1] + valid = patch[patch > 0] + if valid.size == 0: + return None + z = float(np.median(valid)) + if self.depth_image.dtype != np.uint16: + z *= 1000.0 # m → mm + return z + + # ── 메인 ── + def run(self): + window = "DepthHover" + cv2.namedWindow(window) + cv2.setMouseCallback(window, self._mouse_cb) + + while rclpy.ok(): + rclpy.spin_once(self, timeout_sec=0.01) + if self.color_image is None: + continue + + vis = self.color_image.copy() + + if self.mouse_xy is not None: + mx, my = self.mouse_xy + # 십자선 + cv2.drawMarker(vis, (mx, my), (0, 255, 255), + markerType=cv2.MARKER_CROSS, + markerSize=20, thickness=1) + # depth + d = self.sample_depth_mm(mx, my) + if d is None: + text = f"({mx},{my}) d=?" + else: + text = f"({mx},{my}) d={d:.0f}mm" + # 좌상단 상태바 + cv2.putText(vis, text, (10, 25), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, + (255, 255, 255), 2) + # 커서 옆 + cv2.putText(vis, + "?" if d is None else f"{d:.0f}", + (mx + 10, my - 10), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, + (0, 255, 255), 2) + + cv2.imshow(window, vis) + if (cv2.waitKey(1) & 0xFF) == 27: + break + + cv2.destroyAllWindows() + + +def main(args=None): + rclpy.init(args=args) + node = BarDetectTest() + try: + node.run() + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/bar_sort_node.py b/src/dsr_practice/dsr_practice/bar_sort_node.py new file mode 100644 index 0000000..096c5e1 --- /dev/null +++ b/src/dsr_practice/dsr_practice/bar_sort_node.py @@ -0,0 +1,566 @@ +#!/usr/bin/env python3 +""" +bar_sort_node.py – 3 bars 크기순 자동 Pick & Place + +click_pick_node.py 기반: +- 카메라로 bar 3개 자동 검출 (OpenCV contour) +- 길이(minAreaRect 장변) 순으로 정렬 +- 미리 정의된 3개 위치에 작은→큰 순서로 배치 +""" + +import math +import time +from pathlib import Path + +import cv2 +import numpy as np +import rclpy +from rclpy.node import Node + +from scipy.spatial.transform import Rotation +from ament_index_python.packages import get_package_share_directory + +from geometry_msgs.msg import PoseStamped +from sensor_msgs.msg import Image, CameraInfo +from cv_bridge import CvBridge + +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters + +from .onrobot import RG + + +# ═══════════════════════════════════════════ +# 설정 +# ═══════════════════════════════════════════ +GROUP_NAME = "manipulator" +BASE_FRAME = "base_link" +EE_LINK = "link_6" + +HOME_JOINTS = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(90.0), +} + +# 안전 작업 영역 (m, base_link) +SAFE_X_MIN = 0.0 +SAFE_Y_MIN = -0.40 +SAFE_Y_MAX = 0.40 +SAFE_Z_MIN = 0.25 + +# Pick/Place 파라미터 (m) +Z_OFFSET = 0.20 # click_pick_node와 동일 +SAFE_Z = 0.40 + +# Place 위치 : x=0 에 1열 (작은 → 큰, base_link) +PLACE_POSITIONS = [ + (0.0, -0.20, 0.20), # 0 : 가장 작은 bar + (0.0, 0.00, 0.20), # 1 : 중간 + (0.0, 0.20, 0.20), # 2 : 가장 큰 bar +] + +# 시작 후 카메라 안정화를 위한 대기 시간 (초) +CAMERA_WARMUP_SEC = 3.0 + +# Bar 검출 파라미터 +MIN_CONTOUR_AREA_PX = 100 # 픽셀 노이즈 컷 + +# Depth(mm) 기반 bar 분류 +# (d_min, d_max, 이름, rank) rank 0=가장 작음, 2=가장 큼 +# LONG : 308~311 → 305~315 +# MEDIUM : 318~321 → 315~325 +# SHORT : 328~331 → 325~335 +BAR_CLASSES = [ + (325, 335, "SHORT", 0), + (315, 325, "MEDIUM", 1), + (305, 315, "LONG", 2), +] + + +def classify_depth_mm(d_mm): + """depth(mm) → (이름, rank) / 해당 없으면 (None, -1)""" + for d_min, d_max, name, rank in BAR_CLASSES: + if d_min <= d_mm < d_max: + return name, rank + return None, -1 + +# 그리퍼 +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 + +DOWN_ORI = {"x": 0.0, "y": 1.0, "z": 0.0, "w": 0.0} + + +# ═══════════════════════════════════════════ +# 유틸 +# ═══════════════════════════════════════════ +def clamp_to_safe_workspace(x, y, z, logger): + if x < SAFE_X_MIN: + logger.warning(f"x={x:.3f} → {SAFE_X_MIN}") + x = SAFE_X_MIN + if y < SAFE_Y_MIN: + logger.warning(f"y={y:.3f} → {SAFE_Y_MIN}") + y = SAFE_Y_MIN + elif y > SAFE_Y_MAX: + logger.warning(f"y={y:.3f} → {SAFE_Y_MAX}") + y = SAFE_Y_MAX + if z < SAFE_Z_MIN: + logger.warning(f"z={z:.3f} → {SAFE_Z_MIN}") + z = SAFE_Z_MIN + return x, y, z + + +def plan_and_execute(robot, arm, logger, pose_goal=None, + state_goal=None, params=None): + arm.set_start_state_to_current_state() + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link=EE_LINK) + elif state_goal is not None: + arm.set_goal_state(robot_state=state_goal) + else: + logger.error("pose/state 없음") + return False + + plan_result = (arm.plan(parameters=params) + if params is not None else arm.plan()) + if not plan_result: + logger.error("Planning 실패") + return False + + robot.execute(group_name=GROUP_NAME, + robot_trajectory=plan_result.trajectory, + blocking=True) + return True + + +def make_pose(x, y, z, ori=None): + if ori is None: + ori = DOWN_ORI + p = PoseStamped() + p.header.frame_id = BASE_FRAME + p.pose.position.x = float(x) + p.pose.position.y = float(y) + p.pose.position.z = float(z) + p.pose.orientation.x = ori["x"] + p.pose.orientation.y = ori["y"] + p.pose.orientation.z = ori["z"] + p.pose.orientation.w = ori["w"] + return p + + +def get_ee_matrix(moveit_robot): + psm = moveit_robot.get_planning_scene_monitor() + with psm.read_only() as scene: + T = scene.current_state.get_global_link_transform(EE_LINK) + return np.asarray(T, dtype=float) + + +def detect_bars(color_img, logger): + """ + bar 후보 검출: OTSU contour → 픽셀 장변(length). + 반환: [{'pixel':(cx,cy), 'length_px':..., 'rect':rect}, ...] + """ + gray = cv2.cvtColor(color_img, cv2.COLOR_BGR2GRAY) + blurred = cv2.GaussianBlur(gray, (5, 5), 0) + _, thresh = cv2.threshold( + blurred, 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU + ) + + kernel = np.ones((3, 3), np.uint8) + thresh = cv2.morphologyEx(thresh, cv2.MORPH_OPEN, kernel, iterations=1) + thresh = cv2.morphologyEx(thresh, cv2.MORPH_CLOSE, kernel, iterations=2) + + contours, _ = cv2.findContours( + thresh, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE + ) + + bars = [] + for c in contours: + if cv2.contourArea(c) < MIN_CONTOUR_AREA_PX: + continue + rect = cv2.minAreaRect(c) + (cx, cy), (rw, rh), _ = rect + bars.append({ + "pixel": (int(cx), int(cy)), + "length_px": float(max(rw, rh)), + "rect": rect, + }) + + logger.info(f"검출된 contour: {len(bars)} 개") + return bars, thresh + + +# ═══════════════════════════════════════════ +# BarSortNode +# ═══════════════════════════════════════════ +class BarSortNode(Node): + def __init__(self): + super().__init__("bar_sort_node") + self.bridge = CvBridge() + self.color_image = None + self.depth_image = None + self.intrinsics = None + + # Hand-Eye + calib_file = ( + Path(get_package_share_directory("dsr_practice")) + / "config" / "T_gripper2camera.npy" + ) + self.gripper2cam = np.load(str(calib_file)).astype(float) + self.gripper2cam[:3, 3] /= 1000.0 # mm → m + self.get_logger().info(f"Hand-Eye 로드: {calib_file}") + + # 그리퍼 + self.gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + + # MoveIt + self.get_logger().info("MoveItPy 초기화 중…") + self.robot = MoveItPy(node_name="bar_sort_moveit_py") + self.arm = self.robot.get_planning_component(GROUP_NAME) + self.robot_model = self.robot.get_robot_model() + self.get_logger().info("MoveItPy 초기화 완료") + + # Plan 파라미터 + self.ompl_params = PlanRequestParameters(self.robot) + self.ompl_params.planning_pipeline = "ompl" + self.ompl_params.planner_id = "RRTConnect" + self.ompl_params.max_velocity_scaling_factor = 0.2 + self.ompl_params.max_acceleration_scaling_factor = 0.1 + self.ompl_params.planning_time = 2.0 + + self.pilz_params = PlanRequestParameters(self.robot) + self.pilz_params.planning_pipeline = "pilz_industrial_motion_planner" + self.pilz_params.planner_id = "PTP" + self.pilz_params.max_velocity_scaling_factor = 0.15 + self.pilz_params.max_acceleration_scaling_factor = 0.1 + self.pilz_params.planning_time = 2.0 + + self.home_xyz = None + self.home_ori = None + + # 구독 + self.create_subscription( + CameraInfo, "/camera/camera/color/camera_info", + self._cam_info_cb, 10) + self.create_subscription( + Image, "/camera/camera/color/image_raw", + self._color_cb, 10) + self.create_subscription( + Image, "/camera/camera/aligned_depth_to_color/image_raw", + self._depth_cb, 10) + + # ── 콜백 ── + def _cam_info_cb(self, msg): + self.intrinsics = { + "fx": msg.k[0], "fy": msg.k[4], + "ppx": msg.k[2], "ppy": msg.k[5], + } + + def _color_cb(self, msg): + self.color_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") + + def _depth_cb(self, msg): + self.depth_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough") + + # ── depth 샘플링 (5×5 median, mm) ── + def sample_depth_mm(self, px, py): + if self.depth_image is None: + return None + h, w = self.depth_image.shape[:2] + if not (0 <= px < w and 0 <= py < h): + return None + x0, x1 = max(0, px - 2), min(w, px + 3) + y0, y1 = max(0, py - 2), min(h, py + 3) + patch = self.depth_image[y0:y1, x0:x1] + valid = patch[patch > 0] + if valid.size == 0: + return None + z = float(np.median(valid)) + if self.depth_image.dtype != np.uint16: + z *= 1000.0 + return z + + # ── 좌표 변환 ── + def transform_to_base(self, cam_xyz_m): + coord = np.append(np.array(cam_xyz_m, dtype=float), 1.0) + base2ee = get_ee_matrix(self.robot) + base2cam = base2ee @ self.gripper2cam + return (base2cam @ coord)[:3] + + def pixel_to_base(self, px, py): + """(px,py) → base 좌표 (m). 주변 5x5 median으로 depth 안정화.""" + if self.depth_image is None or self.intrinsics is None: + return None + h, w = self.depth_image.shape[:2] + if not (0 <= px < w and 0 <= py < h): + return None + + x0, x1 = max(0, px - 2), min(w, px + 3) + y0, y1 = max(0, py - 2), min(h, py + 3) + patch = self.depth_image[y0:y1, x0:x1] + valid = patch[patch > 0] + if valid.size == 0: + return None + z_raw = float(np.median(valid)) + z_m = (z_raw / 1000.0 + if self.depth_image.dtype == np.uint16 else z_raw) + + fx, fy = self.intrinsics["fx"], self.intrinsics["fy"] + ppx, ppy = self.intrinsics["ppx"], self.intrinsics["ppy"] + cam_x = (px - ppx) * z_m / fx + cam_y = (py - ppy) * z_m / fy + return self.transform_to_base((cam_x, cam_y, z_m)) + + # ── Pick & Place ── + def pick_and_place(self, bx, by, bz, place_xyz): + """ + 1) 현재 z로 pick XY 이동 + 2) pick_z (= bz + Z_OFFSET) 하강 + 3) gripper close + 4) SAFE_Z 상승 + 5) place XY 이동 (SAFE_Z) + 6) place_z 하강 + 7) gripper open + 8) SAFE_Z 상승 + """ + log = self.get_logger() + ori = self.home_ori or DOWN_ORI + + pick_z = bz + Z_OFFSET + px, py, pz = place_xyz + place_z = max(pz, pick_z) # pick 높이 이상 보장 + + log.info( + f"Pick ({bx:.3f},{by:.3f},pick_z={pick_z:.3f}) → " + f"Place ({px:.3f},{py:.3f},{place_z:.3f})" + ) + + cur_ee = get_ee_matrix(self.robot) + cur_z = cur_ee[2, 3] + + # 0) gripper open + self.gripper.open_gripper() + time.sleep(0.5) + + # 1) pick XY + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(bx, by, cur_z, ori), + params=self.pilz_params): + log.error("[1] plan 실패"); return False + + # 2) pick_z 하강 + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(bx, by, pick_z, ori), + params=self.pilz_params): + log.error("[2] plan 실패"); return False + + # 3) close + log.info("[3] Gripper CLOSE") + self.gripper.close_gripper() + time.sleep(1.0) + + # 4) SAFE_Z 상승 + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(bx, by, SAFE_Z, ori), + params=self.pilz_params): + log.error("[4] plan 실패"); return False + + # 5) place XY (SAFE_Z) + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(px, py, SAFE_Z, ori), + params=self.pilz_params): + log.error("[5] plan 실패"); return False + + # 6) place_z 하강 + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(px, py, place_z, ori), + params=self.pilz_params): + log.error("[6] plan 실패"); return False + + # 7) open + log.info("[7] Gripper OPEN") + self.gripper.open_gripper() + time.sleep(1.0) + + # 8) SAFE_Z 상승 + plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(px, py, SAFE_Z, ori), + params=self.pilz_params) + return True + + # ── 정렬 시퀀스 ── + def sort_bars(self): + log = self.get_logger() + if (self.color_image is None or self.depth_image is None + or self.intrinsics is None): + log.error("이미지/내참 준비 안 됨"); return + + bars, thresh = detect_bars(self.color_image, log) + if not bars: + log.error("contour 없음 — 중단") + cv2.imshow("BarSort_Thresh", thresh) + cv2.waitKey(500) + return + + # 각 contour 중심의 depth 로 SHORT / MEDIUM / LONG 분류 + # rank당 후보가 여럿이면 contour 면적(length_px) 큰 것 선택 + classified = {} # rank -> target dict + for b in bars: + cx, cy = b["pixel"] + d_mm = self.sample_depth_mm(cx, cy) + if d_mm is None: + continue + name, rank = classify_depth_mm(d_mm) + if rank < 0: + log.info(f"skip: d={d_mm:.0f}mm 범위 밖 @ ({cx},{cy})") + continue + cand = { + "pixel": (cx, cy), + "length_px": b["length_px"], + "depth_mm": d_mm, + "name": name, + "rank": rank, + } + if rank not in classified \ + or cand["length_px"] > classified[rank]["length_px"]: + classified[rank] = cand + + missing = [r for r in (0, 1, 2) if r not in classified] + if missing: + names = ["SHORT", "MEDIUM", "LONG"] + log.error(f"분류 부족: 누락 {[names[r] for r in missing]}") + return + + # 시각화 + vis = self.color_image.copy() + colors = {0: (0, 255, 0), 1: (0, 200, 255), 2: (0, 0, 255)} + for rank in (0, 1, 2): + t = classified[rank] + cx, cy = t["pixel"] + col = colors[rank] + cv2.circle(vis, (cx, cy), 10, col, 2) + cv2.putText(vis, + f"{t['name']} d={t['depth_mm']:.0f}", + (cx + 12, cy), cv2.FONT_HERSHEY_SIMPLEX, + 0.5, col, 2) + cv2.imshow("BarSort", vis) + cv2.waitKey(500) + + log.info("─── 분류 결과 ───") + for rank in (0, 1, 2): + t = classified[rank] + log.info( + f"[{rank}] {t['name']:6s} " + f"d={t['depth_mm']:.0f}mm pixel={t['pixel']}" + ) + + # Pick & Place (SHORT(0) → MEDIUM(1) → LONG(2)) + for rank in (0, 1, 2): + t = classified[rank] + cx, cy = t["pixel"] + base = self.pixel_to_base(cx, cy) + if base is None: + log.error(f"{t['name']} 좌표 변환 실패 — 중단") + return + bx, by, bz = float(base[0]), float(base[1]), float(base[2]) + log.info( + f"═══ {t['name']} (d={t['depth_mm']:.0f}mm, " + f"base z={bz:.3f}) ═══" + ) + ok = self.pick_and_place(bx, by, bz, PLACE_POSITIONS[rank]) + if not ok: + log.error(f"{t['name']} 실패 — 시퀀스 중단") + return + time.sleep(0.5) + + log.info("========== 정렬 완료 ==========") + + # ── 메인 ── + def run(self): + log = self.get_logger() + window = "BarSort" + cv2.namedWindow(window) + + # Home 이동 + log.info("[Init] Home 이동") + home_state = RobotState(self.robot_model) + home_state.joint_positions = HOME_JOINTS + home_state.update() + if not plan_and_execute(self.robot, self.arm, log, + state_goal=home_state, + params=self.ompl_params): + log.error("Home 이동 실패 — 종료") + return + time.sleep(0.5) + + # Home pose 저장 + T = get_ee_matrix(self.robot) + self.home_xyz = (T[0, 3], T[1, 3], T[2, 3]) + qx, qy, qz, qw = Rotation.from_matrix(T[:3, :3]).as_quat() + self.home_ori = {"x": float(qx), "y": float(qy), + "z": float(qz), "w": float(qw)} + log.info(f"[Init] Home = ({T[0,3]:.3f},{T[1,3]:.3f},{T[2,3]:.3f})") + + self.gripper.open_gripper() + time.sleep(1.0) + + # 프레임 수신 대기 + log.info("프레임 수신 대기…") + t0 = time.time() + while rclpy.ok() and (self.color_image is None + or self.depth_image is None + or self.intrinsics is None): + rclpy.spin_once(self, timeout_sec=0.1) + if time.time() - t0 > 10.0: + log.error("타임아웃"); return + + # 카메라 안정화 대기 (spin 계속 돌려 최신 프레임 수신) + log.info(f"카메라 안정화 {CAMERA_WARMUP_SEC:.1f}초 대기…") + t0 = time.time() + while rclpy.ok() and (time.time() - t0) < CAMERA_WARMUP_SEC: + rclpy.spin_once(self, timeout_sec=0.05) + if self.color_image is not None: + cv2.imshow(window, self.color_image) + cv2.waitKey(1) + + # 자동 실행 + log.info("정렬 시퀀스 시작") + self.sort_bars() + + # 완료 후 이미지만 띄워두고 ESC 대기 + log.info("완료. ESC 로 종료") + while rclpy.ok(): + rclpy.spin_once(self, timeout_sec=0.01) + if self.color_image is None: + continue + cv2.imshow(window, self.color_image) + if (cv2.waitKey(1) & 0xFF) == 27: + break + + cv2.destroyAllWindows() + + +def main(args=None): + rclpy.init(args=args) + node = BarSortNode() + try: + node.run() + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/click_pick_node.py b/src/dsr_practice/dsr_practice/click_pick_node.py new file mode 100644 index 0000000..9925314 --- /dev/null +++ b/src/dsr_practice/dsr_practice/click_pick_node.py @@ -0,0 +1,408 @@ +#!/usr/bin/env python3 +""" +click_pick_node.py – 카메라 클릭 → Pick & Place (MoveIt 기반) + +gear_assembly.py 구조 + test.py 시퀀스를 MoveIt/m 단위로 통합. +""" + +import math +import time +from pathlib import Path + +import cv2 +import numpy as np +import rclpy +from rclpy.node import Node +from rclpy.logging import get_logger + +from scipy.spatial.transform import Rotation +from ament_index_python.packages import get_package_share_directory + +from geometry_msgs.msg import PoseStamped +from sensor_msgs.msg import Image, CameraInfo +from cv_bridge import CvBridge + +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters + +from .onrobot import RG + + +# ═══════════════════════════════════════════ +# 설정 +# ═══════════════════════════════════════════ +GROUP_NAME = "manipulator" +BASE_FRAME = "base_link" +EE_LINK = "link_6" + +HOME_JOINTS = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(90.0), +} + +# 안전 작업 영역 (m, base_link 기준) +SAFE_X_MIN = 0.0 +SAFE_Y_MIN = -0.30 +SAFE_Y_MAX = 0.30 +SAFE_Z_MIN = 0.25 + +# Pick/Place 파라미터 (m) +Z_OFFSET = 0.20 # base_z에 더할 오프셋 (test.py의 200mm와 동일) +SAFE_Z = 0.40 # 안전 이동 높이 +APPROACH_OFFSET = 0.05 # pick/place 위에서 접근 거리 + +# 그리퍼 +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 + +# TCP 아래로 향한 자세 +DOWN_ORI = {"x": 0.0, "y": 1.0, "z": 0.0, "w": 0.0} + + +# ═══════════════════════════════════════════ +# 유틸 함수 (gear_assembly 스타일) +# ═══════════════════════════════════════════ +def clamp_to_safe_workspace(x, y, z, logger): + if x < SAFE_X_MIN: + logger.warning(f"x={x:.3f} → {SAFE_X_MIN}") + x = SAFE_X_MIN + if y < SAFE_Y_MIN: + logger.warning(f"y={y:.3f} → {SAFE_Y_MIN}") + y = SAFE_Y_MIN + elif y > SAFE_Y_MAX: + logger.warning(f"y={y:.3f} → {SAFE_Y_MAX}") + y = SAFE_Y_MAX + if z < SAFE_Z_MIN: + logger.warning(f"z={z:.3f} → {SAFE_Z_MIN}") + z = SAFE_Z_MIN + return x, y, z + + +def plan_and_execute(robot, arm, logger, pose_goal=None, + state_goal=None, params=None): + """plan 후 execute. 실패 시 False 반환.""" + arm.set_start_state_to_current_state() + + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link=EE_LINK) + elif state_goal is not None: + arm.set_goal_state(robot_state=state_goal) + else: + logger.error("pose/state 없음") + return False + + plan_result = (arm.plan(parameters=params) + if params is not None else arm.plan()) + if not plan_result: + logger.error("Planning 실패") + return False + + robot.execute(group_name=GROUP_NAME, + robot_trajectory=plan_result.trajectory, + blocking=True) + return True + + +def make_pose(x, y, z, ori=None): + if ori is None: + ori = DOWN_ORI + p = PoseStamped() + p.header.frame_id = BASE_FRAME + p.pose.position.x = float(x) + p.pose.position.y = float(y) + p.pose.position.z = float(z) + p.pose.orientation.x = ori["x"] + p.pose.orientation.y = ori["y"] + p.pose.orientation.z = ori["z"] + p.pose.orientation.w = ori["w"] + return p + + +def get_ee_matrix(moveit_robot): + """MoveIt FK로 base_link → EE_LINK 4×4 행렬 (m).""" + psm = moveit_robot.get_planning_scene_monitor() + with psm.read_only() as scene: + T = scene.current_state.get_global_link_transform(EE_LINK) + return np.asarray(T, dtype=float) + + +# ═══════════════════════════════════════════ +# ClickPickNode +# ═══════════════════════════════════════════ +class ClickPickNode(Node): + def __init__(self): + super().__init__("click_pick_moveit_node") + self.bridge = CvBridge() + self.color_image = None + self.depth_image = None + self.intrinsics = None + self.picking = False # pick 중복 방지 + + # Hand-Eye 변환행렬 로드 + calib_file = ( + Path(get_package_share_directory("dsr_practice")) + / "config" / "T_gripper2camera.npy" + ) + self.gripper2cam = np.load(str(calib_file)).astype(float) + self.gripper2cam[:3, 3] /= 1000.0 # mm → m + self.get_logger().info(f"Hand-Eye 로드: {calib_file}") + + # 그리퍼 + self.gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + + # MoveIt + self.get_logger().info("MoveItPy 초기화 중…") + self.robot = MoveItPy(node_name="click_pick_moveit_py") + self.arm = self.robot.get_planning_component(GROUP_NAME) + self.robot_model = self.robot.get_robot_model() + self.get_logger().info("MoveItPy 초기화 완료") + + # Plan 파라미터 + self.ompl_params = PlanRequestParameters(self.robot) + self.ompl_params.planning_pipeline = "ompl" + self.ompl_params.planner_id = "RRTConnect" + self.ompl_params.max_velocity_scaling_factor = 0.2 + self.ompl_params.max_acceleration_scaling_factor = 0.1 + self.ompl_params.planning_time = 2.0 + + self.pilz_params = PlanRequestParameters(self.robot) + self.pilz_params.planning_pipeline = "pilz_industrial_motion_planner" + self.pilz_params.planner_id = "PTP" + self.pilz_params.max_velocity_scaling_factor = 0.15 + self.pilz_params.max_acceleration_scaling_factor = 0.1 + self.pilz_params.planning_time = 2.0 + + # Home pose (run에서 설정) + self.home_xyz = None # (x, y, z) in m + self.home_ori = None # dict {x, y, z, w} + + # 구독 + self.create_subscription( + CameraInfo, "/camera/camera/color/camera_info", + self._cam_info_cb, 10) + self.create_subscription( + Image, "/camera/camera/color/image_raw", + self._color_cb, 10) + self.create_subscription( + Image, "/camera/camera/aligned_depth_to_color/image_raw", + self._depth_cb, 10) + + # ── 콜백 ── + def _cam_info_cb(self, msg): + self.intrinsics = { + "fx": msg.k[0], "fy": msg.k[4], + "ppx": msg.k[2], "ppy": msg.k[5], + } + + def _color_cb(self, msg): + self.color_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") + + def _depth_cb(self, msg): + self.depth_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough") + + # ── 좌표 변환 ── + def transform_to_base(self, cam_xyz_m): + """카메라 좌표 (m) → 베이스 좌표 (m).""" + coord = np.append(np.array(cam_xyz_m, dtype=float), 1.0) + base2ee = get_ee_matrix(self.robot) + base2cam = base2ee @ self.gripper2cam + return (base2cam @ coord)[:3] + + # ── Pick & Place (test.py 시퀀스를 MoveIt으로) ── + def pick_and_place(self, bx, by, bz): + """ + 1) 현재 z 유지하며 XY 이동 + 2) pick_z (= bz + Z_OFFSET) 로 하강 + 3) gripper close + 4) SAFE_Z로 상승 + 5) home XY로 이동 (SAFE_Z 유지) + 6) place_z로 하강 (pick_z 이상 보장) + 7) gripper open + 8) SAFE_Z로 상승 + """ + log = self.get_logger() + ori = self.home_ori or DOWN_ORI + + pick_z = bz + Z_OFFSET + place_z = max(pick_z, 0.25) # pick 높이 이상 보장 + + log.info(f"Base raw: ({bx:.3f}, {by:.3f}, {bz:.3f}) m") + log.info(f"Z_OFFSET={Z_OFFSET}, pick_z={pick_z:.3f}, place_z={place_z:.3f}") + + # 현재 EE 위치 + cur_ee = get_ee_matrix(self.robot) + cur_z = cur_ee[2, 3] + + hx, hy, hz = self.home_xyz + + # 0) gripper open + self.gripper.open_gripper() + time.sleep(0.5) + + # 1) 현재 z 유지하며 클릭 XY로 이동 + log.info(f"[1] XY → ({bx:.3f}, {by:.3f}) @ cur_z={cur_z:.3f}") + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(bx, by, cur_z, ori), + params=self.pilz_params): + log.error("pick 중단: [1] plan 실패"); return + + # 2) pick_z로 하강 + log.info(f"[2] down to pick_z={pick_z:.3f}") + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(bx, by, pick_z, ori), + params=self.pilz_params): + log.error("pick 중단: [2] plan 실패"); return + + # 3) gripper close + log.info("[3] Gripper CLOSE") + self.gripper.close_gripper() + time.sleep(1.0) + + # 4) SAFE_Z로 상승 + log.info(f"[4] up to SAFE_Z={SAFE_Z:.3f}") + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(bx, by, SAFE_Z, ori), + params=self.pilz_params): + log.error("pick 중단: [4] plan 실패"); return + + # 5) home XY로 이동 (SAFE_Z 유지) + log.info(f"[5] home XY → ({hx:.3f}, {hy:.3f}) @ SAFE_Z") + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(hx, hy, SAFE_Z, ori), + params=self.pilz_params): + log.error("pick 중단: [5] plan 실패"); return + + # 6) place_z로 하강 + log.info(f"[6] down to place_z={place_z:.3f}") + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(hx, hy, place_z, ori), + params=self.pilz_params): + log.error("pick 중단: [6] plan 실패"); return + + # 7) gripper open + log.info("[7] Gripper OPEN") + self.gripper.open_gripper() + time.sleep(1.0) + + # 8) SAFE_Z로 상승 + log.info(f"[8] up to SAFE_Z={SAFE_Z:.3f}") + plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(hx, hy, SAFE_Z, ori), + params=self.pilz_params) + + log.info("========== PICK END ==========") + + # ── 마우스 콜백 ── + def mouse_callback(self, event, x, y, flags, param): + if event != cv2.EVENT_LBUTTONDOWN: + return + if self.picking: + self.get_logger().warn("pick 동작 중 — 무시") + return + if self.color_image is None or self.depth_image is None or self.intrinsics is None: + self.get_logger().warn("프레임/내참 아직 준비 안 됨") + return + + h, w = self.depth_image.shape[:2] + if not (0 <= x < w and 0 <= y < h): + self.get_logger().warn("클릭 범위 초과") + return + + z_raw = self.depth_image[y, x] + if z_raw == 0: + self.get_logger().warn("해당 픽셀 depth=0") + return + + # depth → m + z_m = float(z_raw) / 1000.0 if self.depth_image.dtype == np.uint16 else float(z_raw) + + fx, fy = self.intrinsics["fx"], self.intrinsics["fy"] + ppx, ppy = self.intrinsics["ppx"], self.intrinsics["ppy"] + + cam_x = (x - ppx) * z_m / fx + cam_y = (y - ppy) * z_m / fy + cam_z = z_m + + base = self.transform_to_base((cam_x, cam_y, cam_z)) + if base is None: + self.get_logger().error("좌표 변환 실패") + return + + bx, by, bz = float(base[0]), float(base[1]), float(base[2]) + self.get_logger().info( + f"Camera: ({cam_x:.3f}, {cam_y:.3f}, {cam_z:.3f}) m → " + f"Base: ({bx:.3f}, {by:.3f}, {bz:.3f}) m" + ) + + self.picking = True + try: + self.pick_and_place(bx, by, bz) + finally: + self.picking = False + + # ── 메인 루프 ── + def run(self): + log = self.get_logger() + window = "ClickToPick (MoveIt)" + cv2.namedWindow(window) + cv2.setMouseCallback(window, self.mouse_callback) + + # Home 이동 + log.info("[Init] Home 이동") + home_state = RobotState(self.robot_model) + home_state.joint_positions = HOME_JOINTS + home_state.update() + if not plan_and_execute(self.robot, self.arm, log, + state_goal=home_state, + params=self.ompl_params): + log.error("Home 이동 실패 — 종료") + return + + time.sleep(0.5) + + # Home pose 저장 + T = get_ee_matrix(self.robot) + self.home_xyz = (T[0, 3], T[1, 3], T[2, 3]) + qx, qy, qz, qw = Rotation.from_matrix(T[:3, :3]).as_quat() + self.home_ori = {"x": float(qx), "y": float(qy), + "z": float(qz), "w": float(qw)} + log.info(f"[Init] Home = ({T[0,3]:.3f}, {T[1,3]:.3f}, {T[2,3]:.3f}) m") + + self.gripper.open_gripper() + time.sleep(1.0) + + while rclpy.ok(): + rclpy.spin_once(self, timeout_sec=0.01) + if self.color_image is None: + continue + cv2.imshow(window, self.color_image) + if (cv2.waitKey(1) & 0xFF) == 27: + break + + cv2.destroyAllWindows() + + +def main(args=None): + rclpy.init(args=args) + node = ClickPickNode() + try: + node.run() + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/collision_obstacle.py b/src/dsr_practice/dsr_practice/collision_obstacle.py new file mode 100644 index 0000000..3211162 --- /dev/null +++ b/src/dsr_practice/dsr_practice/collision_obstacle.py @@ -0,0 +1,292 @@ +#!/usr/bin/env python3 +import math + +import rclpy +from rclpy.logging import get_logger +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy + +from geometry_msgs.msg import Pose +from geometry_msgs.msg import PoseStamped +from shape_msgs.msg import SolidPrimitive +from moveit_msgs.msg import CollisionObject +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters + +# ================================ +# 기본 설정 +# ================================ +GROUP_NAME = "manipulator" # SRDF에 정의된 planning group 이름 +BASE_FRAME = "base_link" # 로봇 베이스 프레임 +EE_LINK = "link_6" # 엔드이펙터 링크 이름 (SRDF/URDF 기준) + +HOME_JOINTS_DEG = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0] +HOME_JOINTS_RAD = [math.radians(d) for d in HOME_JOINTS_DEG] + +# ====== 안전 작업 영역 정의 (base_link 기준) ====== +SAFE_X_MIN = 0.0 # x는 0 이상 +SAFE_Y_MIN = -0.3 # y 하한 +SAFE_Y_MAX = 0.3 # y 상한 +SAFE_Z_MIN = 0.27 # z는 이 값보다 낮아지면 안 됨 +# ================================================== + + +def clamp_to_safe_workspace(x: float, y: float, z: float, logger): + """안전 작업 영역으로 (x, y, z) 클램핑""" + safe_x = x + safe_y = y + safe_z = z + + if safe_x < SAFE_X_MIN: + logger.warning( + f"Requested x ({safe_x:.3f} m) is below safety limit " + f"({SAFE_X_MIN:.3f} m). Clamping to SAFE_X_MIN." + ) + safe_x = SAFE_X_MIN + + if safe_y < SAFE_Y_MIN: + logger.warning( + f"Requested y ({safe_y:.3f} m) is below safety limit " + f"({SAFE_Y_MIN:.3f} m). Clamping to SAFE_Y_MIN." + ) + safe_y = SAFE_Y_MIN + elif safe_y > SAFE_Y_MAX: + logger.warning( + f"Requested y ({safe_y:.3f} m) is above safety limit " + f"({SAFE_Y_MAX:.3f} m). Clamping to SAFE_Y_MAX." + ) + safe_y = SAFE_Y_MAX + + if safe_z < SAFE_Z_MIN: + logger.warning( + f"Requested z ({safe_z:.3f} m) is below safety limit " + f"({SAFE_Z_MIN:.3f} m). Clamping to SAFE_Z_MIN." + ) + safe_z = SAFE_Z_MIN + + return safe_x, safe_y, safe_z + + +def plan_and_execute( + robot: MoveItPy, + planning_component, + logger, + pose_goal: PoseStamped = None, + plan_parameters=None, +): + """ + 공식 문서 스타일 helper: 계획 후 곧바로 실행 + + - pose_goal이 주어지면: + · 안전 영역 클램핑 + · start_state = current + · pose 기반 goal 설정 (EE_LINK) + - 그 다음 plan_parameters 유무에 따라 plan() 호출 후 execute + """ + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + + planning_component.set_start_state_to_current_state() + planning_component.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link=EE_LINK, + ) + + logger.info("Planning trajectory") + + if plan_parameters is not None: + plan_result = planning_component.plan( + parameters=plan_parameters + ) + else: + plan_result = planning_component.plan() + + if not plan_result: + logger.error("Planning failed") + return False + + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + robot.execute( + group_name=GROUP_NAME, + robot_trajectory=robot_trajectory, + blocking=True, + ) + logger.info("Execution finished") + return True + + +def add_box_obstacle(node: Node, + object_id: str, + frame_id: str, + size_xyz=(0.2, 0.2, 0.2), + pos_xyz=(0.5, 0.0, 0.2)): + """ + collision_object 토픽으로 박스 장애물 추가(ADD). + - QoS를 TRANSIENT_LOCAL로 해서 구독자가 나중에 붙어도 유지되게 함(퍼블리셔가 살아있는 동안). + """ + qos = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) + pub = node.create_publisher(CollisionObject, "/collision_object", qos) + + box = CollisionObject() + box.id = object_id + box.header.frame_id = frame_id + + primitive = SolidPrimitive() + primitive.type = SolidPrimitive.BOX + primitive.dimensions = list(size_xyz) # [x, y, z] + + pose = Pose() + pose.position.x, pose.position.y, pose.position.z = pos_xyz + pose.orientation.w = 1.0 # 회전 없음 + + box.primitives.append(primitive) + box.primitive_poses.append(pose) + box.operation = CollisionObject.ADD + + pub.publish(box) + node.get_logger().info( + f"[scene] ADD box id={object_id} frame={frame_id} size={size_xyz} pos={pos_xyz}" + ) + + +def main(args=None): + # ================================ + # MoveItPy 인스턴스 생성 + # ================================ + rclpy.init(args=args) + logger = get_logger("m0609.moveit_py.waypoint_pilz") + + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + logger.info("MoveItPy instance created") + robot_model = robot.get_robot_model() + + # ================================ + # 플래닝 파라미터 + # ================================ + home_params = PlanRequestParameters(robot) + waypoint_params = PlanRequestParameters(robot) + + # MoveIt2 config의 planning_pipelines.pipeline_names와 일치해야 함 + home_params.planning_pipeline = "ompl" + home_params.planner_id = "RRTConnect" + + waypoint_params.planning_pipeline = "pilz_industrial_motion_planner" + waypoint_params.planner_id = "PTP" + + home_params.max_velocity_scaling_factor = 0.2 + home_params.max_acceleration_scaling_factor = 0.1 + home_params.planning_time = 2.0 + + waypoint_params.max_velocity_scaling_factor = 0.15 + waypoint_params.max_acceleration_scaling_factor = 0.1 + waypoint_params.planning_time = 5.0 + + home_state = RobotState(robot_model) + home_state.set_joint_group_positions(GROUP_NAME, HOME_JOINTS_RAD) + home_state.update() + + # HOME 이동 (조인트 목표 → pose_goal 없이, plan_parameters만 사용) + arm.set_start_state_to_current_state() + arm.set_goal_state(robot_state=home_state) + plan_and_execute( + robot, + arm, + logger, + plan_parameters=home_params, + ) + + # ================================ + # 기본 3개 waypoint + 수동 우회 2개 = 총 5개 경로점 + # ================================ + logger.info("=== Waypoints from HOME, orientation fixed (Pilz PTP) ===") + + WAYPOINTS = [ + { # waypoint_1 + "pos": {"x": 0.45, "y": 0.22, "z": 0.52}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + { # detour_1 + "pos": {"x": 0.45, "y": 0.22, "z": 0.28}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + { # waypoint_2 + "pos": {"x": 0.62, "y": -0.18, "z": 0.28}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + { # detour_2 + "pos": {"x": 0.36, "y": -0.26, "z": 0.28}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + { # waypoint_3 + "pos": {"x": 0.36, "y": -0.26, "z": 0.55}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + ] + + # ====== 여기(중간)에서 장애물 추가 (경로 사이 2개) ====== + scene_node = rclpy.create_node("scene_obstacle_publisher") + mid_12 = (0.535, 0.020, 0.600) + mid_23 = (0.590, -0.220, 0.615) + obstacles = [ + {"id": "mid_box_1", "size_xyz": (0.10, 0.10, 0.10), "pos_xyz": mid_12}, + {"id": "mid_box_2", "size_xyz": (0.10, 0.10, 0.10), "pos_xyz": mid_23}, + ] + for obstacle in obstacles: + add_box_obstacle( + node=scene_node, + object_id=obstacle["id"], + frame_id=BASE_FRAME, + size_xyz=obstacle["size_xyz"], + pos_xyz=obstacle["pos_xyz"], + ) + + pose_goal = PoseStamped() + pose_goal.header.frame_id = BASE_FRAME + + for i, wp in enumerate(WAYPOINTS, start=1): + pos = wp["pos"] + ori = wp["ori"] + + logger.info( + f"--- Waypoint {i}: " + f"x={pos['x']:.3f}, y={pos['y']:.3f}, z={pos['z']:.3f} ---" + ) + + # pose_goal에 의도 좌표/자세 설정 + pose_goal.pose.position.x = pos["x"] + pose_goal.pose.position.y = pos["y"] + pose_goal.pose.position.z = pos["z"] + + pose_goal.pose.orientation.x = ori["x"] + pose_goal.pose.orientation.y = ori["y"] + pose_goal.pose.orientation.z = ori["z"] + pose_goal.pose.orientation.w = ori["w"] + + # plan_and_execute 안에서 안전영역 + goal 설정 + Pilz PTP 플래너 호출 + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=waypoint_params, + ) + + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/config/T_gripper2camera.npy b/src/dsr_practice/dsr_practice/config/T_gripper2camera.npy new file mode 100644 index 0000000000000000000000000000000000000000..2a9474fc4635b9de2f2ce7ae3b2a2963d298f551 GIT binary patch literal 256 zcmbR27wQ`j$;eQ~P_3SlTAW;@Zl$1ZlV+i=qoAIaUsO_*m=~X4l#&V(cT3DEP6dh= zXCxM+0{I#yItnJ5ItsN4WCN~sdPnQ^UbxHmB!8Oyj>D6tnz~N1Uvitx;B2d% z!|&!N-Bxo__KU`TTsHCl`~5kcoR4|qTI|JEX!mov1UYOM+x-5w_aysMkGHK@?b>3$ aSnP3u*T47nR}_95_xyBpV89DL*aHBfCQbnW literal 0 HcmV?d00001 diff --git a/src/dsr_practice/dsr_practice/config/moveit_py.yaml b/src/dsr_practice/dsr_practice/config/moveit_py.yaml new file mode 100644 index 0000000..0e89b41 --- /dev/null +++ b/src/dsr_practice/dsr_practice/config/moveit_py.yaml @@ -0,0 +1,54 @@ +/**: + ros__parameters: + planning_scene_monitor_options: + name: "planning_scene_monitor" + robot_description: "robot_description" + joint_state_topic: "/joint_states" + attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor" + publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene" + monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene" + wait_for_initial_state_timeout: 10.0 + + planning_pipelines: + pipeline_names: ["ompl", "pilz_industrial_motion_planner", "chomp", "ompl_rrt_star"] + + plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + max_velocity_scaling_factor: 0.1 + max_acceleration_scaling_factor: 0.1 + + ompl_rrtc: + plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + planner_id: "RRTConnectkConfigDefault" + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + planning_time: 1.0 + + ompl_rrt_star: + plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl_rrt_star + planner_id: "RRTstarkConfigDefault" + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + planning_time: 1.5 + + pilz_lin: + plan_request_params: + planning_attempts: 1 + planning_pipeline: pilz_industrial_motion_planner + planner_id: "PTP" + max_velocity_scaling_factor: 0.1 + max_acceleration_scaling_factor: 0.1 + planning_time: 0.8 + + chomp: + plan_request_params: + planning_attempts: 1 + planning_pipeline: chomp + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + planning_time: 1.5 \ No newline at end of file diff --git a/src/dsr_practice/dsr_practice/dsr_practice/Calibration_Tutorial/data_recording.py b/src/dsr_practice/dsr_practice/dsr_practice/Calibration_Tutorial/data_recording.py new file mode 100644 index 0000000..ccbfcc3 --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/Calibration_Tutorial/data_recording.py @@ -0,0 +1,72 @@ +import os +import cv2 +import json +import rclpy +import DR_init + +# 로봇 설정 +ROBOT_ID = "dsr01" +ROBOT_MODEL = "m0609" +VELOCITY, ACC = 60, 60 +DEVICE_NUMBER = 4 + +DR_init.dsr__id = ROBOT_ID +DR_init.__dsr__model = ROBOT_MODEL + + +def main(args=None): + rclpy.init(args=args) + node = rclpy.create_node("dsr_example_demo_py", namespace=ROBOT_ID) + DR_init.__dsr__node = node + # 로봇 제어 모듈 가져오기 + try: + from DSR_ROBOT2 import ( + get_current_posx, + set_tool, + set_tcp, + ) + except ImportError as e: + print(f"Error importing DSR_ROBOT2 : {e}") + return + # 공구 및 TCP 설정 + set_tool("Tool Weight_2FG") + set_tcp("2FG_TCP") + + # 데이터 저장 경로 설정 + source_path = "./data" + os.makedirs(source_path, exist_ok=True) + # 카메라 연결 + print(f"현재 선택된 device number는 {DEVICE_NUMBER}입니다.") + cap = cv2.VideoCapture(DEVICE_NUMBER) # 4 is camera number, set your camera number + + write_data = {} + write_data["poses"] = [] + write_data["file_name"] = [] + + while True: + ret, frame = cap.read() + + if not ret: + print("카메라를 찾을 수 없습니다. DEVICE_NUMBER를 변경해주세요.") + exit(True) + cv2.imshow("camera", frame) + + if cv2.waitKey(1) & 0xFF == ord("q"): + pos = get_current_posx()[0] + file_name = f"{pos[0]}_{pos[1]}_{pos[2]}.jpg" + # 현재 위치 기반 이미지 저장 + cv2.imwrite(f"{source_path}/{file_name}", frame) + print("current position1 : ", pos) + write_data["file_name"].append(file_name) + write_data["poses"].append(pos) + print(f"save img to {source_path}/{file_name}") + with open(f"{source_path}/calibrate_data.json", "w") as json_file: + json.dump(write_data, json_file, indent=4) + + cap.release() + cv2.destroyAllWindows() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/dsr_practice/Calibration_Tutorial/eye2hand_calibration.py b/src/dsr_practice/dsr_practice/dsr_practice/Calibration_Tutorial/eye2hand_calibration.py new file mode 100644 index 0000000..132faa5 --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/Calibration_Tutorial/eye2hand_calibration.py @@ -0,0 +1,283 @@ +import json +from scipy.spatial.transform import Rotation +import numpy as np +import cv2 + +# 1) 로봇 그리퍼의 절대 좌표 (x, y, z, rx, ry, rz)를 행렬로 변환하는 함수 +def get_robot_pose_matrix(x, y, z, rx, ry, rz): + """ + 베이스->그리퍼 변환행렬 (4x4)을 반환. + """ + R = Rotation.from_euler('ZYZ', [rx, ry, rz], degrees=True).as_matrix() + T = np.eye(4) + T[:3, :3] = R + T[:3, 3] = [x, y, z] + return T + +# 2) 체커보드 코너 검출 (카메라→체커보드 변환 구하기) +def find_checkerboard_pose( + image, board_size, square_size, camera_matrix, dist_coeffs +): + """ + checkerboard_size = (7, 5) # 내부 코너 개수 + square_size = 25.0 # mm 단위 + 이미지에서 체커보드를 찾고, solvePnP로 카메라→체커보드 변환(R, t)을 구함. + 반환값: (R_camera2checker, t_camera2checker) + """ + objp = np.zeros((board_size[0] * board_size[1], 3), np.float32) + # 예: x 방향으로 square_size씩 증가, y 방향으로 square_size씩 증가 + objp[:, :2] = ( + np.mgrid[0 : board_size[0], 0 : board_size[1]].T.reshape(-1, 2) * 25 + ) + + gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) + found, corners = cv2.findChessboardCorners( + gray, + board_size, + flags=cv2.CALIB_CB_ADAPTIVE_THRESH + + cv2.CALIB_CB_FAST_CHECK + + cv2.CALIB_CB_NORMALIZE_IMAGE, + ) + if not found: + return None, None + + # 코너 좌표를 더 정확히 + corners_sub = cv2.cornerSubPix( + gray, + corners, + (11, 11), + (-1, -1), + criteria=(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001), + ) + + # solvePnP + retval, rvec, tvec = cv2.solvePnP(objp, corners_sub, camera_matrix, dist_coeffs) + if not retval: + return None, None + + # 회전벡터 -> 회전행렬 + R, _ = cv2.Rodrigues(rvec) + + return R, tvec + +# 체커보드 이미지를 이용한 카메라 보정 +def calibrate_camera_from_chessboard( + image_folder_path, + board_size, # (7, 5)처럼 내부 코너 개수 + square_size, # mm 단위 +): + """ + 지정된 폴더 안의 체커보드 이미지를 읽고, 카메라 행렬(camera_matrix)와 왜곡 계수(dist_coeffs)를 추정한다. + board_size: 체커보드 내부 코너 수 (cols, rows) + square_size: 체커보드 한 칸 크기 (mm) + """ + # 3D 세계 좌표계에 대한 좌표 생성 (z=0 평면 상에 체커보드) + objp = np.zeros((board_size[0] * board_size[1], 3), np.float32) + # 예: x 방향으로 square_size씩 증가, y 방향으로 square_size씩 증가 + objp[:, :2] = ( + np.mgrid[0 : board_size[0], 0 : board_size[1]].T.reshape(-1, 2) * square_size + ) + + # 모든 이미지에 대해 3D / 2D 포인트 누적 + obj_points = [] # 3D world points + img_points = [] # 2D image points + image_shape = None + + # 폴더 내에 있는 이미지 파일 읽기 + image_paths = image_folder_path # JPG, PNG 등 확장자 맞춰서 + # 필요하면 jpg 등 다른 확장자도 처리 가능 + + for fname in image_paths: + img = cv2.imread(fname) + if img is None: + continue + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + if image_shape is None: + image_shape = gray.shape[::-1] # (width, height) + + # 체커보드 코너 찾기 + ret, corners = cv2.findChessboardCorners(gray, board_size, None) + if ret: + # 코너를 더 정밀하게 + corners_sub = cv2.cornerSubPix( + gray, + corners, + (11, 11), + (-1, -1), + (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001), + ) + # 누적 + obj_points.append(objp) + img_points.append(corners_sub) + + # 내부 파라미터, 왜곡 계수, 외부 파라미터 구하기 + if len(obj_points) < 1: + print("체커보드 코너를 충분히 찾지 못하였습니다.") + return None, None, None, None + + # flags = cv2.CALIB_ZERO_TANGENT_DIST + cv2.CALIB_FIX_K3 등 필요에 따라 추가 + ret, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera( + obj_points, # 3D 실세계 점 + img_points, # 2D 이미지 점 + image_shape, # (width, height) + None, # 초기 camera_matrix + None, # 초기 dist_coeffs + ) + + if not ret: + print("캘리브레이션이 제대로 수렴하지 않았습니다.") + return None, None, None, None + + return camera_matrix, dist_coeffs, rvecs, tvecs + +from scipy.linalg import sqrtm +from numpy.linalg import inv + +# 4) 여러 개의 변환 행렬 조합 함수 +def compose_transformation_matrices(R_list, t_list): + T_list = [] + for R, t in zip(R_list, t_list): + T = np.eye(4) + T[:3, :3] = R + T[:3, 3] = np.ravel(t) # t가 벡터 형태여야 합니다. + T_list.append(T) + return T_list + +# 회전행렬을 로그변환하는 함수 +def logR(T): + R = T[0:3, 0:3] + theta = np.arccos((np.trace(R) - 1) / 2) + logr = np.array([ + R[2, 1] - R[1, 2], + R[0, 2] - R[2, 0], + R[1, 0] - R[0, 1] + ]) * theta / (2 * np.sin(theta)) + return logr + +# A와 B의 변환을 이용하여 보정 행렬 계산 +def Calibrate(A, B): + n_data = len(A) + M = np.zeros((3, 3)) + + for i in range(n_data - 1): + alpha = logR(A[i]) + beta = logR(B[i]) + alpha2 = logR(A[i + 1]) + beta2 = logR(B[i + 1]) + + alpha3 = np.cross(alpha, alpha2) + beta3 = np.cross(beta, beta2) + + M1 = np.dot(beta.reshape(3, 1), alpha.reshape(1, 3)) + M2 = np.dot(beta2.reshape(3, 1), alpha2.reshape(1, 3)) + M3 = np.dot(beta3.reshape(3, 1), alpha3.reshape(1, 3)) + + M += M1 + M2 + M3 + + theta = np.dot(sqrtm(inv(np.dot(M.T, M))), M.T) + + C = np.zeros((3 * n_data, 3)) + d = np.zeros((3 * n_data, 1)) + for i in range(n_data): + rot_a = A[i][:3, :3] + trans_a = A[i][:3, 3] + trans_b = B[i][:3, 3] + C[3 * i:3 * i + 3, :] = np.eye(3) - rot_a + d[3 * i:3 * i + 3, 0] = trans_a - np.dot(theta, trans_b) + + b_x = np.dot(inv(np.dot(C.T, C)), np.dot(C.T, d)) + return theta, b_x + +# Main Function +if __name__ == "__main__": + data = json.load(open("data/calibrate_data.json")) + robot_poses = np.array(data["poses"]) + + robot_poses[:, :3] = robot_poses[:, :3] + image_paths = ["data/" + d for d in data["file_name"]] + + valid_indices = [] + for i, pose in enumerate(robot_poses): + T_base2gripper = get_robot_pose_matrix(*pose) + det_T = np.linalg.det(T_base2gripper) + print(f"Index {i}: det(T_base2gripper) = {det_T}") + + if np.abs(det_T) > 1e-6: + valid_indices.append(i) + else: + print(f"⚠️ Warning: Singular T_base2gripper at index {i}!") + + robot_poses = robot_poses[valid_indices] + image_paths = [image_paths[i] for i in valid_indices] + + checkerboard_size = (8, 6) # 내부 코너 개수 + square_size = 25 + + camera_matrix, dist_coeffs, rvecs, tvecs = calibrate_camera_from_chessboard( + image_paths, checkerboard_size, square_size + ) + + R_gripper2base_list = [] + t_gripper2base_list = [] + R_camera2checker_list = [] + t_camera2checker_list = [] + R_checker2camera_list = [] + t_checker2camera_list = [] + + for img_path, pose in zip(image_paths, robot_poses): + # 1) 베이스->그리퍼 변환행렬 + T_base2gripper = get_robot_pose_matrix(*pose) + + # 2) 이미지 로딩 + image = cv2.imread(img_path) + if image is None: + continue + + # 3) 카메라->체커보드 변환 구하기 + R_cam2checker, t_cam2checker = find_checkerboard_pose( + image, checkerboard_size, square_size, camera_matrix, dist_coeffs + ) + if R_cam2checker is None: + continue + + T_gripper2base= np.linalg.inv(T_base2gripper) + + R_gripper2base = T_gripper2base[:3, :3] + t_gripper2base = T_gripper2base[:3, 3] + + R_gripper2base_list.append(R_gripper2base.copy()) + t_gripper2base_list.append(t_gripper2base.reshape(-1, 1).copy()) + + T_cam2checker = np.eye(4) + T_cam2checker[:3, :3] = R_cam2checker + T_cam2checker[:3, 3] = t_cam2checker.flatten() + T_checker2cam = np.linalg.inv(T_cam2checker) + + R_checker2camera_list.append(T_checker2cam[:3, :3].copy()) + t_checker2camera_list.append(T_checker2cam[:3, 3].copy()) + + T_gripper2base_list = compose_transformation_matrices(R_gripper2base_list, t_gripper2base_list) + T_checker2cam_list = compose_transformation_matrices(R_checker2camera_list, t_checker2camera_list) + A_list = [] + B_list = [] + num_pairs = min(len(T_gripper2base_list), len(T_checker2cam_list)) + + for i, T in enumerate(T_gripper2base_list): + det = np.linalg.det(T) + if np.abs(det) < 1e-6: + print(f"⚠️ Warning: T_gripper2base_list[{i}] is singular or nearly singular!") + + for i in range(num_pairs - 1): + A_i = np.dot(inv(T_gripper2base_list[i]), T_gripper2base_list[i + 1]) + B_i = np.dot(inv(T_checker2cam_list[i]), T_checker2cam_list[i + 1]) + A_list.append(A_i) + B_list.append(B_i) + + theta, b_x = Calibrate(A_list, B_list) + X = np.eye(4) + X[:3, :3] = theta + X[:3, 3] = b_x.flatten() + T_cam2base = X + print(T_cam2base) + print(T_cam2base[:3, 3]) + np.save("T_cam2base.npy", T_cam2base) diff --git a/src/dsr_practice/dsr_practice/dsr_practice/Calibration_Tutorial/handeye_calibration.py b/src/dsr_practice/dsr_practice/dsr_practice/Calibration_Tutorial/handeye_calibration.py new file mode 100644 index 0000000..75bbba1 --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/Calibration_Tutorial/handeye_calibration.py @@ -0,0 +1,227 @@ +import cv2 +import numpy as np +import json +from scipy.spatial.transform import Rotation + +# 1) 로봇 그리퍼의 절대 좌표 (x, y, z, rx, ry, rz)를 행렬로 변환하는 함수 +def get_robot_pose_matrix(x, y, z, rx, ry, rz): + """ + 베이스->그리퍼 변환행렬 (4x4)을 반환. + """ + R = Rotation.from_euler('ZYZ', [rx, ry, rz], degrees=True).as_matrix() + T = np.eye(4) + T[:3, :3] = R + T[:3, 3] = [x, y, z] + return T + + +# 2) 체커보드 코너 검출 (카메라→체커보드 변환 구하기) +def find_checkerboard_pose( + image, board_size, square_size, camera_matrix, dist_coeffs +): + """ + checkerboard_size = (7, 5) # 내부 코너 개수 + square_size = 25.0 # mm 단위 + 이미지에서 체커보드를 찾고, solvePnP로 카메라→체커보드 변환(R, t)을 구함. + 반환값: (R_camera2checker, t_camera2checker) + """ + objp = np.zeros((board_size[0] * board_size[1], 3), np.float32) + # 예: x 방향으로 square_size씩 증가, y 방향으로 square_size씩 증가 + objp[:, :2] = ( + np.mgrid[0 : board_size[0], 0 : board_size[1]].T.reshape(-1, 2) * 25 + ) + + gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) + found, corners = cv2.findChessboardCorners( + gray, + board_size, + flags=cv2.CALIB_CB_ADAPTIVE_THRESH + + cv2.CALIB_CB_FAST_CHECK + + cv2.CALIB_CB_NORMALIZE_IMAGE, + ) + if not found: + return None, None + + # 코너 좌표를 더 정확히 + corners_sub = cv2.cornerSubPix( + gray, + corners, + (11, 11), + (-1, -1), + criteria=(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001), + ) + + # solvePnP + retval, rvec, tvec = cv2.solvePnP(objp, corners_sub, camera_matrix, dist_coeffs) + if not retval: + return None, None + + # 회전벡터 -> 회전행렬 + R, _ = cv2.Rodrigues(rvec) + + return R, tvec + + +def calibrate_camera_from_chessboard( + image_folder_path, + board_size, # (7, 5)처럼 내부 코너 개수 + square_size, # mm 단위 +): + """ + 지정된 폴더 안의 체커보드 이미지를 읽고, 카메라 행렬(camera_matrix)와 왜곡 계수(dist_coeffs)를 추정한다. + board_size: 체커보드 내부 코너 수 (cols, rows) + square_size: 체커보드 한 칸 크기 (mm) + """ + # 3D 세계 좌표계에 대한 좌표 생성 (z=0 평면 상에 체커보드) + objp = np.zeros((board_size[0] * board_size[1], 3), np.float32) + # 예: x 방향으로 square_size씩 증가, y 방향으로 square_size씩 증가 + objp[:, :2] = ( + np.mgrid[0 : board_size[0], 0 : board_size[1]].T.reshape(-1, 2) * square_size + ) + + # 모든 이미지에 대해 3D / 2D 포인트 누적 + obj_points = [] # 3D world points + img_points = [] # 2D image points + image_shape = None + + # 폴더 내에 있는 이미지 파일 읽기 + image_paths = image_folder_path # JPG, PNG 등 확장자 맞춰서 + # 필요하면 jpg 등 다른 확장자도 처리 가능 + + for fname in image_paths: + img = cv2.imread(fname) + if img is None: + continue + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + if image_shape is None: + image_shape = gray.shape[::-1] # (width, height) + + # 체커보드 코너 찾기 + ret, corners = cv2.findChessboardCorners(gray, board_size, None) + if ret: + # 코너를 더 정밀하게 + corners_sub = cv2.cornerSubPix( + gray, + corners, + (11, 11), + (-1, -1), + (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001), + ) + # 누적 + obj_points.append(objp) + img_points.append(corners_sub) + + # 내부 파라미터, 왜곡 계수, 외부 파라미터 구하기 + if len(obj_points) < 1: + print("체커보드 코너를 충분히 찾지 못하였습니다.") + return None, None, None, None + + # flags = cv2.CALIB_ZERO_TANGENT_DIST + cv2.CALIB_FIX_K3 등 필요에 따라 추가 + ret, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera( + obj_points, # 3D 실세계 점 + img_points, # 2D 이미지 점 + image_shape, # (width, height) + None, # 초기 camera_matrix + None, # 초기 dist_coeffs + ) + + if not ret: + print("캘리브레이션이 제대로 수렴하지 않았습니다.") + return None, None, None, None + + return camera_matrix, dist_coeffs, rvecs, tvecs + + + +# Main Function +if __name__ == "__main__": + # 캘리브레이션 데이터 로드 + data = json.load(open("data/calibrate_data.json")) + robot_poses = np.array(data["poses"]) + + robot_poses[:, :3] = robot_poses[:, :3] + image_paths = ["data/" + d for d in data["file_name"]] + + checkerboard_size = (10, 7) # 내부 코너 개수 + square_size = 25 + # 카메라 캘리브레이션 수행(내부 파라미터 왜곡 보정) + camera_matrix, dist_coeffs, rvecs, tvecs = calibrate_camera_from_chessboard( + image_paths, checkerboard_size, square_size + ) + + R_gripper2base_list = [] + t_gripper2base_list = [] + R_camera2checker_list = [] + t_camera2checker_list = [] + R_checker2camera_list = [] + t_checker2camera_list = [] + + for img_path, pose in zip(image_paths, robot_poses): + # 1) 베이스->그리퍼 변환행렬 + T_base2gripper = get_robot_pose_matrix(*pose) + + # 2) 이미지 로딩 + image = cv2.imread(img_path) + if image is None: + continue + + # 3) 카메라->체커보드 변환 구하기 + R_cam2checker, t_cam2checker = find_checkerboard_pose( + image, checkerboard_size, square_size, camera_matrix, dist_coeffs + ) + if R_cam2checker is None: + continue + + # T_gripper2base= np.linalg.inv(T_base2gripper) + T_gripper2base= T_base2gripper + + R_gripper2base = T_gripper2base[:3, :3] + t_gripper2base = T_gripper2base[:3, 3] + + R_gripper2base_list.append(R_gripper2base.copy()) + t_gripper2base_list.append(t_gripper2base.reshape(-1, 1).copy()) + + T_cam2checker = np.eye(4) + T_cam2checker[:3, :3] = R_cam2checker + T_cam2checker[:3, 3] = t_cam2checker.flatten() + + T_checker2cam = T_cam2checker + + R_checker2camera_list.append(T_checker2cam[:3, :3].copy()) + t_checker2camera_list.append(T_checker2cam[:3, 3].copy()) + + + # Hand-Eye 캘리브레이션 수행 + R_cam2gripper, t_cam2gripper = cv2.calibrateHandEye( + R_gripper2base_list, + t_gripper2base_list, + R_checker2camera_list, + t_checker2camera_list, + method=cv2.CALIB_HAND_EYE_PARK, + ) + + + T_base2gripper_example = get_robot_pose_matrix(*robot_poses[2]) + R_base2gripper_example = T_base2gripper_example[:3, :3] + t_base2gripper_example = T_base2gripper_example[:3, 3] + + # 그리퍼->카메라 변환행렬 + T_gripper2cam = np.eye(4) + T_gripper2cam[:3, :3] = R_cam2gripper + T_gripper2cam[:3, 3] = t_cam2gripper.flatten() + + # 최종 베이스->카메라 + T_base2cam = T_base2gripper_example @ T_gripper2cam + + print("===== Hand-Eye Calibration Results =====") + print("R_base2gripper:\n", T_base2gripper_example[:3, :3]) + print("T_base2gripper:\n", T_base2gripper_example[:3, 3]) + print("\n") + print("R_base2camera:\n", T_base2cam[:3, :3]) + print("T_base2camera:\n", T_base2cam[:3, 3]) + print("\n") + print("R_gripper2camera:\n", T_gripper2cam[:3, :3]) + print("T_gripper2camera:\n", T_gripper2cam[:3, 3].tolist()) + + # save T_grigper2camera + np.save("T_gripper2camera.npy", T_gripper2cam) diff --git a/src/dsr_practice/dsr_practice/dsr_practice/Calibration_Tutorial/onrobot.py b/src/dsr_practice/dsr_practice/dsr_practice/Calibration_Tutorial/onrobot.py new file mode 100644 index 0000000..73931dc --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/Calibration_Tutorial/onrobot.py @@ -0,0 +1,184 @@ +#!/usr/bin/env python3 + +from pymodbus.client.sync import ModbusTcpClient as ModbusClient + + +class RG(): + + def __init__(self, gripper, ip, port): + self.client = ModbusClient( + ip, + port=port, + stopbits=1, + bytesize=8, + parity='E', + baudrate=115200, + timeout=1) + if gripper not in ['rg2', 'rg6']: + print("Please specify either rg2 or rg6.") + return + self.gripper = gripper # RG2/6 + if self.gripper == 'rg2': + self.max_width = 1100 + self.max_force = 400 + elif self.gripper == 'rg6': + self.max_width = 1600 + self.max_force = 1200 + self.open_connection() + + def open_connection(self): + """Opens the connection with a gripper.""" + self.client.connect() + + def close_connection(self): + """Closes the connection with the gripper.""" + self.client.close() + + def get_fingertip_offset(self): + """Reads the current fingertip offset in 1/10 millimeters. + Please note that the value is a signed two's complement number. + """ + result = self.client.read_holding_registers( + address=258, count=1, unit=65) + offset_mm = result.registers[0] / 10.0 + return offset_mm + + def get_width(self): + """Reads current width between gripper fingers in 1/10 millimeters. + Please note that the width is provided without any fingertip offset, + as it is measured between the insides of the aluminum fingers. + """ + result = self.client.read_holding_registers( + address=267, count=1, unit=65) + width_mm = result.registers[0] / 10.0 + return width_mm + + def get_status(self): + """Reads current device status. + This status field indicates the status of the gripper and its motion. + It is composed of 7 flags, described in the table below. + + Bit Name Description + 0 (LSB): busy High (1) when a motion is ongoing, + low (0) when not. + The gripper will only accept new commands + when this flag is low. + 1: grip detected High (1) when an internal- or + external grip is detected. + 2: S1 pushed High (1) when safety switch 1 is pushed. + 3: S1 trigged High (1) when safety circuit 1 is activated. + The gripper will not move + while this flag is high; + can only be reset by power cycling. + 4: S2 pushed High (1) when safety switch 2 is pushed. + 5: S2 trigged High (1) when safety circuit 2 is activated. + The gripper will not move + while this flag is high; + can only be reset by power cycling. + 6: safety error High (1) when on power on any of + the safety switch is pushed. + 10-16: reserved Not used. + """ + # address : register number + # count : number of registers to be read + # unit : slave device address + result = self.client.read_holding_registers( + address=268, count=1, unit=65) + status = format(result.registers[0], '016b') + status_list = [0] * 7 + if int(status[-1]): + print("A motion is ongoing so new commands are not accepted.") + status_list[0] = 1 + if int(status[-2]): + print("An internal- or external grip is detected.") + status_list[1] = 1 + if int(status[-3]): + print("Safety switch 1 is pushed.") + status_list[2] = 1 + if int(status[-4]): + print("Safety circuit 1 is activated so it will not move.") + status_list[3] = 1 + if int(status[-5]): + print("Safety switch 2 is pushed.") + status_list[4] = 1 + if int(status[-6]): + print("Safety circuit 2 is activated so it will not move.") + status_list[5] = 1 + if int(status[-7]): + print("Any of the safety switch is pushed.") + status_list[6] = 1 + + return status_list + + def get_width_with_offset(self): + """Reads current width between gripper fingers in 1/10 millimeters. + The set fingertip offset is considered. + """ + result = self.client.read_holding_registers( + address=275, count=1, unit=65) + width_mm = result.registers[0] / 10.0 + return width_mm + + def set_control_mode(self, command): + """The control field is used to start and stop gripper motion. + Only one option should be set at a time. + Please note that the gripper will not start a new motion + before the one currently being executed is done + (see busy flag in the Status field). + The valid flags are: + + 1 (0x0001): grip + Start the motion, with the target force and width. + Width is calculated without the fingertip offset. + Please note that the gripper will ignore this command + if the busy flag is set in the status field. + 8 (0x0008): stop + Stop the current motion. + 16 (0x0010): grip_w_offset + Same as grip, but width is calculated + with the set fingertip offset. + """ + result = self.client.write_register( + address=2, value=command, unit=65) + + def set_target_force(self, force_val): + """Writes the target force to be reached + when gripping and holding a workpiece. + It must be provided in 1/10th Newtons. + The valid range is 0 to 400 for the RG2 and 0 to 1200 for the RG6. + """ + result = self.client.write_register( + address=0, value=force_val, unit=65) + + def set_target_width(self, width_val): + """Writes the target width between + the finger to be moved to and maintained. + It must be provided in 1/10th millimeters. + The valid range is 0 to 1100 for the RG2 and 0 to 1600 for the RG6. + Please note that the target width should be provided + corrected for any fingertip offset, + as it is measured between the insides of the aluminum fingers. + """ + result = self.client.write_register( + address=1, value=width_val, unit=65) + + def close_gripper(self, force_val=400): + """Closes gripper.""" + params = [force_val, 0, 16] + print("Start closing gripper.") + result = self.client.write_registers( + address=0, values=params, unit=65) + + def open_gripper(self, force_val=400): + """Opens gripper.""" + params = [force_val, self.max_width, 16] + print("Start opening gripper.") + result = self.client.write_registers( + address=0, values=params, unit=65) + + def move_gripper(self, width_val, force_val=400): + """Moves gripper to the specified width.""" + params = [force_val, width_val, 16] + print("Start moving gripper.") + result = self.client.write_registers( + address=0, values=params, unit=65) diff --git a/src/dsr_practice/dsr_practice/dsr_practice/Calibration_Tutorial/realsense.py b/src/dsr_practice/dsr_practice/dsr_practice/Calibration_Tutorial/realsense.py new file mode 100644 index 0000000..45a6813 --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/Calibration_Tutorial/realsense.py @@ -0,0 +1,41 @@ +from rclpy.node import Node +from sensor_msgs.msg import Image, CameraInfo +from cv_bridge import CvBridge + + +class ImgNode(Node): + def __init__(self): + super().__init__('img_node') + self.bridge = CvBridge() + self.color_frame = None + self.color_frame_stamp = None + self.depth_frame = None + self.intrinsics = None + self.color_subscription = self.create_subscription( + Image, '/camera/camera/color/image_raw', self.color_callback, 10) + self.depth_subscription = self.create_subscription( + Image, '/camera/camera/aligned_depth_to_color/image_raw', self.depth_callback, 10) + self.camera_info_subscription = self.create_subscription( + CameraInfo, '/camera/camera/color/camera_info', self.camera_info_callback, 10) + + def camera_info_callback(self, msg): + self.intrinsics = {"fx": msg.k[0], "fy": msg.k[4], "ppx": msg.k[2], "ppy": msg.k[5]} + + def color_callback(self, msg): + self.color_frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8') + self.color_frame_stamp = str(msg.header.stamp.sec) + str(msg.header.stamp.nanosec) + + def depth_callback(self, msg): + self.depth_frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough') + + def get_color_frame(self): + return self.color_frame + + def get_color_frame_stamp(self): + return self.color_frame_stamp + + def get_depth_frame(self): + return self.depth_frame + + def get_camera_intrinsic(self): + return self.intrinsics \ No newline at end of file diff --git a/src/dsr_practice/dsr_practice/dsr_practice/Calibration_Tutorial/test.py b/src/dsr_practice/dsr_practice/dsr_practice/Calibration_Tutorial/test.py new file mode 100644 index 0000000..0f974f5 --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/Calibration_Tutorial/test.py @@ -0,0 +1,258 @@ +import cv2 +import rclpy +import time +import numpy as np +import threading +from scipy.spatial.transform import Rotation + +from realsense import ImgNode +from onrobot import RG +import DR_init + +# ====================== +# 로봇 / 그리퍼 설정 +# ====================== +ROBOT_ID = "dsr01" +ROBOT_MODEL = "m0609" +VELOCITY, ACC = 60, 60 + +DR_init.__dsr__id = ROBOT_ID +DR_init.__dsr__model = ROBOT_MODEL + +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 # 정수 + +# ====================== +# Z 관련 파라미터 +# ====================== +Z_OFFSET = 200.0 # 클릭한 지점 z에 더해 줄 오프셋 (mm) +SAFE_Z = 400.0 # 집은 뒤 올라갈 안전 높이 (mm) – 환경 보고 조정 + + +class TestNode: + def __init__(self): + # RealSense 노드 + self.img_node = ImgNode() + + # Intrinsic 수신될 때까지 대기 + while rclpy.ok() and self.img_node.get_camera_intrinsic() is None: + rclpy.spin_once(self.img_node, timeout_sec=0.1) + + self.intrinsics = self.img_node.get_camera_intrinsic() + + # Hand-eye 결과 (그리퍼 → 카메라) + self.gripper2cam = np.load("T_gripper2camera.npy") + + # 그리퍼 + self.gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + + # 준비자세(Joint) + self.JReady = posj([0, 0, 90, 0, 90, 90]) + + # 홈 XY (초기 자세에서 저장) + self.home_pose = None # [x, y, z, rx, ry, rz] + + # ========================= + # 카메라 → 베이스 좌표 변환 + # ========================= + def transform_to_base(self, camera_coords): + """ + camera_coords: (Xc, Yc, Zc) in mm (카메라 기준) + 반환: (Xb, Yb, Zb) in mm (base_link 기준) + """ + coord = np.append(np.array(camera_coords, dtype=float), 1.0) # [x,y,z,1] + + # 현재 베이스→그리퍼 + base2gripper = self.get_robot_pose_matrix(*get_current_posx()[0]) + + # 베이스→카메라 = 베이스→그리퍼 · 그리퍼→카메라 + base2cam = base2gripper @ self.gripper2cam + + td_coord = base2cam @ coord + + return td_coord[:3] + + def get_robot_pose_matrix(self, x, y, z, rx, ry, rz): + R = Rotation.from_euler("ZYZ", [rx, ry, rz], degrees=True).as_matrix() + T = np.eye(4) + T[:3, :3] = R + T[:3, 3] = [x, y, z] + return T + + # ========================= + # Pick 시퀀스 + # ========================= + def pick_and_place(self, x, y, z): + + print("\n========== PICK SEQUENCE ==========") + print(f"Base coord raw : x={x:.2f}, y={y:.2f}, z={z:.2f}") + print(f"Z_OFFSET : {Z_OFFSET}") + print(f"SAFE_Z : {SAFE_Z}") + print("===================================\n") + + # 현재 포즈 + cur = get_current_posx()[0] + cur_x, cur_y, cur_z, rx, ry, rz = cur + + # home pose (run()에서 설정) + if self.home_pose is None: + self.home_pose = cur + home_x, home_y, home_z, hrx, hry, hrz = self.home_pose + + # 0) 안전용으로 한 번 더 open (혹시 이전에 안 열려 있으면) + self.gripper.open_gripper() + time.sleep(0.5) + + # 1) 클릭한 위치 x,y로 이동 (z는 현재 값 유지) + target_xy = posx([x, y, cur_z, rx, ry, rz]) + print("[1] Move to XY only:", target_xy) + movel(target_xy, VELOCITY, ACC) + wait(0.5) + + # 2) z_correct 계산 및 이동 + z_correct = z + Z_OFFSET + target_xyz = posx([x, y, z_correct, rx, ry, rz]) + print(f"[2] Move down to z_correct={z_correct:.2f}:", target_xyz) + movel(target_xyz, VELOCITY, ACC) + wait(0.3) + + # 3) gripper close + print("[3] Gripper Close") + self.gripper.close_gripper() + time.sleep(1.0) + + # 4) z 상승: SAFE_Z까지 (x,y는 그대로) + up_pose = posx([x, y, SAFE_Z, rx, ry, rz]) + print(f"[4] Move up to SAFE_Z={SAFE_Z:.2f}:", up_pose) + movel(up_pose, VELOCITY, ACC) + wait(0.5) + + # 5) home xy 이동 (z는 SAFE_Z 유지) + home_xy_pose = posx([home_x, home_y, SAFE_Z, hrx, hry, hrz]) + print("[5] Move to home XY:", home_xy_pose) + movel(home_xy_pose, VELOCITY, ACC) + wait(0.5) + + # 6) place 높이 = pick 높이 이상으로 보장 + z_final = max(250.0, z_correct) + home_xy_z280 = posx([home_x, home_y, z_final, hrx, hry, hrz]) + print(f"[6] Move to home XY, z={z_final}:", home_xy_z280) + movel(home_xy_z280, VELOCITY, ACC) + wait(0.3) + + # 7) gripper open + print("[7] Gripper Open") + self.gripper.open_gripper() + time.sleep(1.0) + + # 8) 다시 SAFE_Z로 올려두고 끝낼지 여부 (원하면 유지) + back_up = posx([home_x, home_y, SAFE_Z, hrx, hry, hrz]) + print("[8] Back up to SAFE_Z:", back_up) + movel(back_up, VELOCITY, ACC) + wait(0.5) + + print("========== PICK END ==========\n") + + # ========================= + # 마우스 콜백 + # ========================= + def mouse_callback(self, event, x, y, flags, param): + if event == cv2.EVENT_LBUTTONDOWN and not hasattr(self, '_pick_thread_running'): + depth_frame = self.img_node.get_depth_frame() + if depth_frame is None: + print("No depth frame") + return + + # 픽셀 범위 체크 + h, w = depth_frame.shape + if not (0 <= x < w and 0 <= y < h): + print("Click out of range") + return + + z = depth_frame[y, x] + if z == 0: + print("Depth invalid at clicked point") + return + + # 카메라 좌표 (mm) 계산 + fx = self.intrinsics["fx"] + fy = self.intrinsics["fy"] + ppx = self.intrinsics["ppx"] + ppy = self.intrinsics["ppy"] + + X = (x - ppx) * z / fx + Y = (y - ppy) * z / fy + Z = z + + cam_coord = (X, Y, Z) + base_coord = self.transform_to_base(cam_coord) + + print("Camera:", cam_coord) + print("Base :", base_coord) + + def run_pick(): + self._pick_thread_running = True + self.pick_and_place(*base_coord) + del self._pick_thread_running + + threading.Thread(target=run_pick, daemon=True).start() + + # ========================= + # 메인 루프 + # ========================= + def run(self): + cv2.namedWindow("Webcam") + cv2.setMouseCallback("Webcam", self.mouse_callback) + + # rclpy spin을 별도 스레드로 분리 (pick 스레드와 충돌 방지) + executor = rclpy.executors.MultiThreadedExecutor() + executor.add_node(self.img_node) + spin_thread = threading.Thread(target=executor.spin, daemon=True) + spin_thread.start() + + # 초기 자세로 이동 + print("[Init] movej JReady") + movej(self.JReady, VELOCITY, ACC) + wait(1.0) + + # 현재 자세를 home_pose로 저장 + self.home_pose = get_current_posx()[0] + + # 초기 gripper open + print("[Init] Gripper Open") + self.gripper.open_gripper() + time.sleep(1.0) + + while True: + img = self.img_node.get_color_frame() + if img is None: + time.sleep(0.01) + continue + + cv2.imshow("Webcam", img) + + if cv2.waitKey(1) & 0xFF == 27: # ESC + break + + executor.shutdown() + cv2.destroyAllWindows() + + +if __name__ == "__main__": + rclpy.init() + node = rclpy.create_node("dsr_example_demo_py", namespace=ROBOT_ID) + DR_init.__dsr__node = node + + try: + from DSR_ROBOT2 import get_current_posx, movej, movel, wait + from DR_common2 import posx, posj + except ImportError as e: + print(f"Error importing DSR_ROBOT2 : {e}") + rclpy.shutdown() + raise SystemExit(1) + + test = TestNode() + test.run() + + rclpy.shutdown() \ No newline at end of file diff --git a/src/dsr_practice/dsr_practice/dsr_practice/__init__.py b/src/dsr_practice/dsr_practice/dsr_practice/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/dsr_practice/dsr_practice/dsr_practice/bar_detect_test.py b/src/dsr_practice/dsr_practice/dsr_practice/bar_detect_test.py new file mode 100644 index 0000000..dacfa6c --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/bar_detect_test.py @@ -0,0 +1,116 @@ +#!/usr/bin/env python3 +""" +bar_detect_test.py – 마우스 호버 depth 표시 (시각화 전용) + +RealSense 영상 위에서 마우스 커서 위치의 depth(mm)를 실시간으로 표시. +ESC 로 종료. +""" + +import cv2 +import numpy as np +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import Image +from cv_bridge import CvBridge + + +class BarDetectTest(Node): + def __init__(self): + super().__init__("bar_detect_test") + self.bridge = CvBridge() + self.color_image = None + self.depth_image = None + self.mouse_xy = None # (x, y) 커서 좌표 + + self.create_subscription( + Image, "/camera/camera/color/image_raw", + self._color_cb, 10) + self.create_subscription( + Image, "/camera/camera/aligned_depth_to_color/image_raw", + self._depth_cb, 10) + + def _color_cb(self, msg): + self.color_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") + + def _depth_cb(self, msg): + self.depth_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough") + + # ── 마우스 콜백 ── + def _mouse_cb(self, event, x, y, flags, param): + if event == cv2.EVENT_MOUSEMOVE: + self.mouse_xy = (x, y) + + # ── depth 샘플링 (5×5 median, mm) ── + def sample_depth_mm(self, px, py): + if self.depth_image is None: + return None + h, w = self.depth_image.shape[:2] + if not (0 <= px < w and 0 <= py < h): + return None + x0, x1 = max(0, px - 2), min(w, px + 3) + y0, y1 = max(0, py - 2), min(h, py + 3) + patch = self.depth_image[y0:y1, x0:x1] + valid = patch[patch > 0] + if valid.size == 0: + return None + z = float(np.median(valid)) + if self.depth_image.dtype != np.uint16: + z *= 1000.0 # m → mm + return z + + # ── 메인 ── + def run(self): + window = "DepthHover" + cv2.namedWindow(window) + cv2.setMouseCallback(window, self._mouse_cb) + + while rclpy.ok(): + rclpy.spin_once(self, timeout_sec=0.01) + if self.color_image is None: + continue + + vis = self.color_image.copy() + + if self.mouse_xy is not None: + mx, my = self.mouse_xy + # 십자선 + cv2.drawMarker(vis, (mx, my), (0, 255, 255), + markerType=cv2.MARKER_CROSS, + markerSize=20, thickness=1) + # depth + d = self.sample_depth_mm(mx, my) + if d is None: + text = f"({mx},{my}) d=?" + else: + text = f"({mx},{my}) d={d:.0f}mm" + # 좌상단 상태바 + cv2.putText(vis, text, (10, 25), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, + (255, 255, 255), 2) + # 커서 옆 + cv2.putText(vis, + "?" if d is None else f"{d:.0f}", + (mx + 10, my - 10), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, + (0, 255, 255), 2) + + cv2.imshow(window, vis) + if (cv2.waitKey(1) & 0xFF) == 27: + break + + cv2.destroyAllWindows() + + +def main(args=None): + rclpy.init(args=args) + node = BarDetectTest() + try: + node.run() + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/dsr_practice/bar_sort_node.py b/src/dsr_practice/dsr_practice/dsr_practice/bar_sort_node.py new file mode 100644 index 0000000..096c5e1 --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/bar_sort_node.py @@ -0,0 +1,566 @@ +#!/usr/bin/env python3 +""" +bar_sort_node.py – 3 bars 크기순 자동 Pick & Place + +click_pick_node.py 기반: +- 카메라로 bar 3개 자동 검출 (OpenCV contour) +- 길이(minAreaRect 장변) 순으로 정렬 +- 미리 정의된 3개 위치에 작은→큰 순서로 배치 +""" + +import math +import time +from pathlib import Path + +import cv2 +import numpy as np +import rclpy +from rclpy.node import Node + +from scipy.spatial.transform import Rotation +from ament_index_python.packages import get_package_share_directory + +from geometry_msgs.msg import PoseStamped +from sensor_msgs.msg import Image, CameraInfo +from cv_bridge import CvBridge + +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters + +from .onrobot import RG + + +# ═══════════════════════════════════════════ +# 설정 +# ═══════════════════════════════════════════ +GROUP_NAME = "manipulator" +BASE_FRAME = "base_link" +EE_LINK = "link_6" + +HOME_JOINTS = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(90.0), +} + +# 안전 작업 영역 (m, base_link) +SAFE_X_MIN = 0.0 +SAFE_Y_MIN = -0.40 +SAFE_Y_MAX = 0.40 +SAFE_Z_MIN = 0.25 + +# Pick/Place 파라미터 (m) +Z_OFFSET = 0.20 # click_pick_node와 동일 +SAFE_Z = 0.40 + +# Place 위치 : x=0 에 1열 (작은 → 큰, base_link) +PLACE_POSITIONS = [ + (0.0, -0.20, 0.20), # 0 : 가장 작은 bar + (0.0, 0.00, 0.20), # 1 : 중간 + (0.0, 0.20, 0.20), # 2 : 가장 큰 bar +] + +# 시작 후 카메라 안정화를 위한 대기 시간 (초) +CAMERA_WARMUP_SEC = 3.0 + +# Bar 검출 파라미터 +MIN_CONTOUR_AREA_PX = 100 # 픽셀 노이즈 컷 + +# Depth(mm) 기반 bar 분류 +# (d_min, d_max, 이름, rank) rank 0=가장 작음, 2=가장 큼 +# LONG : 308~311 → 305~315 +# MEDIUM : 318~321 → 315~325 +# SHORT : 328~331 → 325~335 +BAR_CLASSES = [ + (325, 335, "SHORT", 0), + (315, 325, "MEDIUM", 1), + (305, 315, "LONG", 2), +] + + +def classify_depth_mm(d_mm): + """depth(mm) → (이름, rank) / 해당 없으면 (None, -1)""" + for d_min, d_max, name, rank in BAR_CLASSES: + if d_min <= d_mm < d_max: + return name, rank + return None, -1 + +# 그리퍼 +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 + +DOWN_ORI = {"x": 0.0, "y": 1.0, "z": 0.0, "w": 0.0} + + +# ═══════════════════════════════════════════ +# 유틸 +# ═══════════════════════════════════════════ +def clamp_to_safe_workspace(x, y, z, logger): + if x < SAFE_X_MIN: + logger.warning(f"x={x:.3f} → {SAFE_X_MIN}") + x = SAFE_X_MIN + if y < SAFE_Y_MIN: + logger.warning(f"y={y:.3f} → {SAFE_Y_MIN}") + y = SAFE_Y_MIN + elif y > SAFE_Y_MAX: + logger.warning(f"y={y:.3f} → {SAFE_Y_MAX}") + y = SAFE_Y_MAX + if z < SAFE_Z_MIN: + logger.warning(f"z={z:.3f} → {SAFE_Z_MIN}") + z = SAFE_Z_MIN + return x, y, z + + +def plan_and_execute(robot, arm, logger, pose_goal=None, + state_goal=None, params=None): + arm.set_start_state_to_current_state() + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link=EE_LINK) + elif state_goal is not None: + arm.set_goal_state(robot_state=state_goal) + else: + logger.error("pose/state 없음") + return False + + plan_result = (arm.plan(parameters=params) + if params is not None else arm.plan()) + if not plan_result: + logger.error("Planning 실패") + return False + + robot.execute(group_name=GROUP_NAME, + robot_trajectory=plan_result.trajectory, + blocking=True) + return True + + +def make_pose(x, y, z, ori=None): + if ori is None: + ori = DOWN_ORI + p = PoseStamped() + p.header.frame_id = BASE_FRAME + p.pose.position.x = float(x) + p.pose.position.y = float(y) + p.pose.position.z = float(z) + p.pose.orientation.x = ori["x"] + p.pose.orientation.y = ori["y"] + p.pose.orientation.z = ori["z"] + p.pose.orientation.w = ori["w"] + return p + + +def get_ee_matrix(moveit_robot): + psm = moveit_robot.get_planning_scene_monitor() + with psm.read_only() as scene: + T = scene.current_state.get_global_link_transform(EE_LINK) + return np.asarray(T, dtype=float) + + +def detect_bars(color_img, logger): + """ + bar 후보 검출: OTSU contour → 픽셀 장변(length). + 반환: [{'pixel':(cx,cy), 'length_px':..., 'rect':rect}, ...] + """ + gray = cv2.cvtColor(color_img, cv2.COLOR_BGR2GRAY) + blurred = cv2.GaussianBlur(gray, (5, 5), 0) + _, thresh = cv2.threshold( + blurred, 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU + ) + + kernel = np.ones((3, 3), np.uint8) + thresh = cv2.morphologyEx(thresh, cv2.MORPH_OPEN, kernel, iterations=1) + thresh = cv2.morphologyEx(thresh, cv2.MORPH_CLOSE, kernel, iterations=2) + + contours, _ = cv2.findContours( + thresh, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE + ) + + bars = [] + for c in contours: + if cv2.contourArea(c) < MIN_CONTOUR_AREA_PX: + continue + rect = cv2.minAreaRect(c) + (cx, cy), (rw, rh), _ = rect + bars.append({ + "pixel": (int(cx), int(cy)), + "length_px": float(max(rw, rh)), + "rect": rect, + }) + + logger.info(f"검출된 contour: {len(bars)} 개") + return bars, thresh + + +# ═══════════════════════════════════════════ +# BarSortNode +# ═══════════════════════════════════════════ +class BarSortNode(Node): + def __init__(self): + super().__init__("bar_sort_node") + self.bridge = CvBridge() + self.color_image = None + self.depth_image = None + self.intrinsics = None + + # Hand-Eye + calib_file = ( + Path(get_package_share_directory("dsr_practice")) + / "config" / "T_gripper2camera.npy" + ) + self.gripper2cam = np.load(str(calib_file)).astype(float) + self.gripper2cam[:3, 3] /= 1000.0 # mm → m + self.get_logger().info(f"Hand-Eye 로드: {calib_file}") + + # 그리퍼 + self.gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + + # MoveIt + self.get_logger().info("MoveItPy 초기화 중…") + self.robot = MoveItPy(node_name="bar_sort_moveit_py") + self.arm = self.robot.get_planning_component(GROUP_NAME) + self.robot_model = self.robot.get_robot_model() + self.get_logger().info("MoveItPy 초기화 완료") + + # Plan 파라미터 + self.ompl_params = PlanRequestParameters(self.robot) + self.ompl_params.planning_pipeline = "ompl" + self.ompl_params.planner_id = "RRTConnect" + self.ompl_params.max_velocity_scaling_factor = 0.2 + self.ompl_params.max_acceleration_scaling_factor = 0.1 + self.ompl_params.planning_time = 2.0 + + self.pilz_params = PlanRequestParameters(self.robot) + self.pilz_params.planning_pipeline = "pilz_industrial_motion_planner" + self.pilz_params.planner_id = "PTP" + self.pilz_params.max_velocity_scaling_factor = 0.15 + self.pilz_params.max_acceleration_scaling_factor = 0.1 + self.pilz_params.planning_time = 2.0 + + self.home_xyz = None + self.home_ori = None + + # 구독 + self.create_subscription( + CameraInfo, "/camera/camera/color/camera_info", + self._cam_info_cb, 10) + self.create_subscription( + Image, "/camera/camera/color/image_raw", + self._color_cb, 10) + self.create_subscription( + Image, "/camera/camera/aligned_depth_to_color/image_raw", + self._depth_cb, 10) + + # ── 콜백 ── + def _cam_info_cb(self, msg): + self.intrinsics = { + "fx": msg.k[0], "fy": msg.k[4], + "ppx": msg.k[2], "ppy": msg.k[5], + } + + def _color_cb(self, msg): + self.color_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") + + def _depth_cb(self, msg): + self.depth_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough") + + # ── depth 샘플링 (5×5 median, mm) ── + def sample_depth_mm(self, px, py): + if self.depth_image is None: + return None + h, w = self.depth_image.shape[:2] + if not (0 <= px < w and 0 <= py < h): + return None + x0, x1 = max(0, px - 2), min(w, px + 3) + y0, y1 = max(0, py - 2), min(h, py + 3) + patch = self.depth_image[y0:y1, x0:x1] + valid = patch[patch > 0] + if valid.size == 0: + return None + z = float(np.median(valid)) + if self.depth_image.dtype != np.uint16: + z *= 1000.0 + return z + + # ── 좌표 변환 ── + def transform_to_base(self, cam_xyz_m): + coord = np.append(np.array(cam_xyz_m, dtype=float), 1.0) + base2ee = get_ee_matrix(self.robot) + base2cam = base2ee @ self.gripper2cam + return (base2cam @ coord)[:3] + + def pixel_to_base(self, px, py): + """(px,py) → base 좌표 (m). 주변 5x5 median으로 depth 안정화.""" + if self.depth_image is None or self.intrinsics is None: + return None + h, w = self.depth_image.shape[:2] + if not (0 <= px < w and 0 <= py < h): + return None + + x0, x1 = max(0, px - 2), min(w, px + 3) + y0, y1 = max(0, py - 2), min(h, py + 3) + patch = self.depth_image[y0:y1, x0:x1] + valid = patch[patch > 0] + if valid.size == 0: + return None + z_raw = float(np.median(valid)) + z_m = (z_raw / 1000.0 + if self.depth_image.dtype == np.uint16 else z_raw) + + fx, fy = self.intrinsics["fx"], self.intrinsics["fy"] + ppx, ppy = self.intrinsics["ppx"], self.intrinsics["ppy"] + cam_x = (px - ppx) * z_m / fx + cam_y = (py - ppy) * z_m / fy + return self.transform_to_base((cam_x, cam_y, z_m)) + + # ── Pick & Place ── + def pick_and_place(self, bx, by, bz, place_xyz): + """ + 1) 현재 z로 pick XY 이동 + 2) pick_z (= bz + Z_OFFSET) 하강 + 3) gripper close + 4) SAFE_Z 상승 + 5) place XY 이동 (SAFE_Z) + 6) place_z 하강 + 7) gripper open + 8) SAFE_Z 상승 + """ + log = self.get_logger() + ori = self.home_ori or DOWN_ORI + + pick_z = bz + Z_OFFSET + px, py, pz = place_xyz + place_z = max(pz, pick_z) # pick 높이 이상 보장 + + log.info( + f"Pick ({bx:.3f},{by:.3f},pick_z={pick_z:.3f}) → " + f"Place ({px:.3f},{py:.3f},{place_z:.3f})" + ) + + cur_ee = get_ee_matrix(self.robot) + cur_z = cur_ee[2, 3] + + # 0) gripper open + self.gripper.open_gripper() + time.sleep(0.5) + + # 1) pick XY + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(bx, by, cur_z, ori), + params=self.pilz_params): + log.error("[1] plan 실패"); return False + + # 2) pick_z 하강 + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(bx, by, pick_z, ori), + params=self.pilz_params): + log.error("[2] plan 실패"); return False + + # 3) close + log.info("[3] Gripper CLOSE") + self.gripper.close_gripper() + time.sleep(1.0) + + # 4) SAFE_Z 상승 + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(bx, by, SAFE_Z, ori), + params=self.pilz_params): + log.error("[4] plan 실패"); return False + + # 5) place XY (SAFE_Z) + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(px, py, SAFE_Z, ori), + params=self.pilz_params): + log.error("[5] plan 실패"); return False + + # 6) place_z 하강 + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(px, py, place_z, ori), + params=self.pilz_params): + log.error("[6] plan 실패"); return False + + # 7) open + log.info("[7] Gripper OPEN") + self.gripper.open_gripper() + time.sleep(1.0) + + # 8) SAFE_Z 상승 + plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(px, py, SAFE_Z, ori), + params=self.pilz_params) + return True + + # ── 정렬 시퀀스 ── + def sort_bars(self): + log = self.get_logger() + if (self.color_image is None or self.depth_image is None + or self.intrinsics is None): + log.error("이미지/내참 준비 안 됨"); return + + bars, thresh = detect_bars(self.color_image, log) + if not bars: + log.error("contour 없음 — 중단") + cv2.imshow("BarSort_Thresh", thresh) + cv2.waitKey(500) + return + + # 각 contour 중심의 depth 로 SHORT / MEDIUM / LONG 분류 + # rank당 후보가 여럿이면 contour 면적(length_px) 큰 것 선택 + classified = {} # rank -> target dict + for b in bars: + cx, cy = b["pixel"] + d_mm = self.sample_depth_mm(cx, cy) + if d_mm is None: + continue + name, rank = classify_depth_mm(d_mm) + if rank < 0: + log.info(f"skip: d={d_mm:.0f}mm 범위 밖 @ ({cx},{cy})") + continue + cand = { + "pixel": (cx, cy), + "length_px": b["length_px"], + "depth_mm": d_mm, + "name": name, + "rank": rank, + } + if rank not in classified \ + or cand["length_px"] > classified[rank]["length_px"]: + classified[rank] = cand + + missing = [r for r in (0, 1, 2) if r not in classified] + if missing: + names = ["SHORT", "MEDIUM", "LONG"] + log.error(f"분류 부족: 누락 {[names[r] for r in missing]}") + return + + # 시각화 + vis = self.color_image.copy() + colors = {0: (0, 255, 0), 1: (0, 200, 255), 2: (0, 0, 255)} + for rank in (0, 1, 2): + t = classified[rank] + cx, cy = t["pixel"] + col = colors[rank] + cv2.circle(vis, (cx, cy), 10, col, 2) + cv2.putText(vis, + f"{t['name']} d={t['depth_mm']:.0f}", + (cx + 12, cy), cv2.FONT_HERSHEY_SIMPLEX, + 0.5, col, 2) + cv2.imshow("BarSort", vis) + cv2.waitKey(500) + + log.info("─── 분류 결과 ───") + for rank in (0, 1, 2): + t = classified[rank] + log.info( + f"[{rank}] {t['name']:6s} " + f"d={t['depth_mm']:.0f}mm pixel={t['pixel']}" + ) + + # Pick & Place (SHORT(0) → MEDIUM(1) → LONG(2)) + for rank in (0, 1, 2): + t = classified[rank] + cx, cy = t["pixel"] + base = self.pixel_to_base(cx, cy) + if base is None: + log.error(f"{t['name']} 좌표 변환 실패 — 중단") + return + bx, by, bz = float(base[0]), float(base[1]), float(base[2]) + log.info( + f"═══ {t['name']} (d={t['depth_mm']:.0f}mm, " + f"base z={bz:.3f}) ═══" + ) + ok = self.pick_and_place(bx, by, bz, PLACE_POSITIONS[rank]) + if not ok: + log.error(f"{t['name']} 실패 — 시퀀스 중단") + return + time.sleep(0.5) + + log.info("========== 정렬 완료 ==========") + + # ── 메인 ── + def run(self): + log = self.get_logger() + window = "BarSort" + cv2.namedWindow(window) + + # Home 이동 + log.info("[Init] Home 이동") + home_state = RobotState(self.robot_model) + home_state.joint_positions = HOME_JOINTS + home_state.update() + if not plan_and_execute(self.robot, self.arm, log, + state_goal=home_state, + params=self.ompl_params): + log.error("Home 이동 실패 — 종료") + return + time.sleep(0.5) + + # Home pose 저장 + T = get_ee_matrix(self.robot) + self.home_xyz = (T[0, 3], T[1, 3], T[2, 3]) + qx, qy, qz, qw = Rotation.from_matrix(T[:3, :3]).as_quat() + self.home_ori = {"x": float(qx), "y": float(qy), + "z": float(qz), "w": float(qw)} + log.info(f"[Init] Home = ({T[0,3]:.3f},{T[1,3]:.3f},{T[2,3]:.3f})") + + self.gripper.open_gripper() + time.sleep(1.0) + + # 프레임 수신 대기 + log.info("프레임 수신 대기…") + t0 = time.time() + while rclpy.ok() and (self.color_image is None + or self.depth_image is None + or self.intrinsics is None): + rclpy.spin_once(self, timeout_sec=0.1) + if time.time() - t0 > 10.0: + log.error("타임아웃"); return + + # 카메라 안정화 대기 (spin 계속 돌려 최신 프레임 수신) + log.info(f"카메라 안정화 {CAMERA_WARMUP_SEC:.1f}초 대기…") + t0 = time.time() + while rclpy.ok() and (time.time() - t0) < CAMERA_WARMUP_SEC: + rclpy.spin_once(self, timeout_sec=0.05) + if self.color_image is not None: + cv2.imshow(window, self.color_image) + cv2.waitKey(1) + + # 자동 실행 + log.info("정렬 시퀀스 시작") + self.sort_bars() + + # 완료 후 이미지만 띄워두고 ESC 대기 + log.info("완료. ESC 로 종료") + while rclpy.ok(): + rclpy.spin_once(self, timeout_sec=0.01) + if self.color_image is None: + continue + cv2.imshow(window, self.color_image) + if (cv2.waitKey(1) & 0xFF) == 27: + break + + cv2.destroyAllWindows() + + +def main(args=None): + rclpy.init(args=args) + node = BarSortNode() + try: + node.run() + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/dsr_practice/click_pick_node.py b/src/dsr_practice/dsr_practice/dsr_practice/click_pick_node.py new file mode 100644 index 0000000..9925314 --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/click_pick_node.py @@ -0,0 +1,408 @@ +#!/usr/bin/env python3 +""" +click_pick_node.py – 카메라 클릭 → Pick & Place (MoveIt 기반) + +gear_assembly.py 구조 + test.py 시퀀스를 MoveIt/m 단위로 통합. +""" + +import math +import time +from pathlib import Path + +import cv2 +import numpy as np +import rclpy +from rclpy.node import Node +from rclpy.logging import get_logger + +from scipy.spatial.transform import Rotation +from ament_index_python.packages import get_package_share_directory + +from geometry_msgs.msg import PoseStamped +from sensor_msgs.msg import Image, CameraInfo +from cv_bridge import CvBridge + +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters + +from .onrobot import RG + + +# ═══════════════════════════════════════════ +# 설정 +# ═══════════════════════════════════════════ +GROUP_NAME = "manipulator" +BASE_FRAME = "base_link" +EE_LINK = "link_6" + +HOME_JOINTS = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(90.0), +} + +# 안전 작업 영역 (m, base_link 기준) +SAFE_X_MIN = 0.0 +SAFE_Y_MIN = -0.30 +SAFE_Y_MAX = 0.30 +SAFE_Z_MIN = 0.25 + +# Pick/Place 파라미터 (m) +Z_OFFSET = 0.20 # base_z에 더할 오프셋 (test.py의 200mm와 동일) +SAFE_Z = 0.40 # 안전 이동 높이 +APPROACH_OFFSET = 0.05 # pick/place 위에서 접근 거리 + +# 그리퍼 +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 + +# TCP 아래로 향한 자세 +DOWN_ORI = {"x": 0.0, "y": 1.0, "z": 0.0, "w": 0.0} + + +# ═══════════════════════════════════════════ +# 유틸 함수 (gear_assembly 스타일) +# ═══════════════════════════════════════════ +def clamp_to_safe_workspace(x, y, z, logger): + if x < SAFE_X_MIN: + logger.warning(f"x={x:.3f} → {SAFE_X_MIN}") + x = SAFE_X_MIN + if y < SAFE_Y_MIN: + logger.warning(f"y={y:.3f} → {SAFE_Y_MIN}") + y = SAFE_Y_MIN + elif y > SAFE_Y_MAX: + logger.warning(f"y={y:.3f} → {SAFE_Y_MAX}") + y = SAFE_Y_MAX + if z < SAFE_Z_MIN: + logger.warning(f"z={z:.3f} → {SAFE_Z_MIN}") + z = SAFE_Z_MIN + return x, y, z + + +def plan_and_execute(robot, arm, logger, pose_goal=None, + state_goal=None, params=None): + """plan 후 execute. 실패 시 False 반환.""" + arm.set_start_state_to_current_state() + + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link=EE_LINK) + elif state_goal is not None: + arm.set_goal_state(robot_state=state_goal) + else: + logger.error("pose/state 없음") + return False + + plan_result = (arm.plan(parameters=params) + if params is not None else arm.plan()) + if not plan_result: + logger.error("Planning 실패") + return False + + robot.execute(group_name=GROUP_NAME, + robot_trajectory=plan_result.trajectory, + blocking=True) + return True + + +def make_pose(x, y, z, ori=None): + if ori is None: + ori = DOWN_ORI + p = PoseStamped() + p.header.frame_id = BASE_FRAME + p.pose.position.x = float(x) + p.pose.position.y = float(y) + p.pose.position.z = float(z) + p.pose.orientation.x = ori["x"] + p.pose.orientation.y = ori["y"] + p.pose.orientation.z = ori["z"] + p.pose.orientation.w = ori["w"] + return p + + +def get_ee_matrix(moveit_robot): + """MoveIt FK로 base_link → EE_LINK 4×4 행렬 (m).""" + psm = moveit_robot.get_planning_scene_monitor() + with psm.read_only() as scene: + T = scene.current_state.get_global_link_transform(EE_LINK) + return np.asarray(T, dtype=float) + + +# ═══════════════════════════════════════════ +# ClickPickNode +# ═══════════════════════════════════════════ +class ClickPickNode(Node): + def __init__(self): + super().__init__("click_pick_moveit_node") + self.bridge = CvBridge() + self.color_image = None + self.depth_image = None + self.intrinsics = None + self.picking = False # pick 중복 방지 + + # Hand-Eye 변환행렬 로드 + calib_file = ( + Path(get_package_share_directory("dsr_practice")) + / "config" / "T_gripper2camera.npy" + ) + self.gripper2cam = np.load(str(calib_file)).astype(float) + self.gripper2cam[:3, 3] /= 1000.0 # mm → m + self.get_logger().info(f"Hand-Eye 로드: {calib_file}") + + # 그리퍼 + self.gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + + # MoveIt + self.get_logger().info("MoveItPy 초기화 중…") + self.robot = MoveItPy(node_name="click_pick_moveit_py") + self.arm = self.robot.get_planning_component(GROUP_NAME) + self.robot_model = self.robot.get_robot_model() + self.get_logger().info("MoveItPy 초기화 완료") + + # Plan 파라미터 + self.ompl_params = PlanRequestParameters(self.robot) + self.ompl_params.planning_pipeline = "ompl" + self.ompl_params.planner_id = "RRTConnect" + self.ompl_params.max_velocity_scaling_factor = 0.2 + self.ompl_params.max_acceleration_scaling_factor = 0.1 + self.ompl_params.planning_time = 2.0 + + self.pilz_params = PlanRequestParameters(self.robot) + self.pilz_params.planning_pipeline = "pilz_industrial_motion_planner" + self.pilz_params.planner_id = "PTP" + self.pilz_params.max_velocity_scaling_factor = 0.15 + self.pilz_params.max_acceleration_scaling_factor = 0.1 + self.pilz_params.planning_time = 2.0 + + # Home pose (run에서 설정) + self.home_xyz = None # (x, y, z) in m + self.home_ori = None # dict {x, y, z, w} + + # 구독 + self.create_subscription( + CameraInfo, "/camera/camera/color/camera_info", + self._cam_info_cb, 10) + self.create_subscription( + Image, "/camera/camera/color/image_raw", + self._color_cb, 10) + self.create_subscription( + Image, "/camera/camera/aligned_depth_to_color/image_raw", + self._depth_cb, 10) + + # ── 콜백 ── + def _cam_info_cb(self, msg): + self.intrinsics = { + "fx": msg.k[0], "fy": msg.k[4], + "ppx": msg.k[2], "ppy": msg.k[5], + } + + def _color_cb(self, msg): + self.color_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") + + def _depth_cb(self, msg): + self.depth_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough") + + # ── 좌표 변환 ── + def transform_to_base(self, cam_xyz_m): + """카메라 좌표 (m) → 베이스 좌표 (m).""" + coord = np.append(np.array(cam_xyz_m, dtype=float), 1.0) + base2ee = get_ee_matrix(self.robot) + base2cam = base2ee @ self.gripper2cam + return (base2cam @ coord)[:3] + + # ── Pick & Place (test.py 시퀀스를 MoveIt으로) ── + def pick_and_place(self, bx, by, bz): + """ + 1) 현재 z 유지하며 XY 이동 + 2) pick_z (= bz + Z_OFFSET) 로 하강 + 3) gripper close + 4) SAFE_Z로 상승 + 5) home XY로 이동 (SAFE_Z 유지) + 6) place_z로 하강 (pick_z 이상 보장) + 7) gripper open + 8) SAFE_Z로 상승 + """ + log = self.get_logger() + ori = self.home_ori or DOWN_ORI + + pick_z = bz + Z_OFFSET + place_z = max(pick_z, 0.25) # pick 높이 이상 보장 + + log.info(f"Base raw: ({bx:.3f}, {by:.3f}, {bz:.3f}) m") + log.info(f"Z_OFFSET={Z_OFFSET}, pick_z={pick_z:.3f}, place_z={place_z:.3f}") + + # 현재 EE 위치 + cur_ee = get_ee_matrix(self.robot) + cur_z = cur_ee[2, 3] + + hx, hy, hz = self.home_xyz + + # 0) gripper open + self.gripper.open_gripper() + time.sleep(0.5) + + # 1) 현재 z 유지하며 클릭 XY로 이동 + log.info(f"[1] XY → ({bx:.3f}, {by:.3f}) @ cur_z={cur_z:.3f}") + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(bx, by, cur_z, ori), + params=self.pilz_params): + log.error("pick 중단: [1] plan 실패"); return + + # 2) pick_z로 하강 + log.info(f"[2] down to pick_z={pick_z:.3f}") + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(bx, by, pick_z, ori), + params=self.pilz_params): + log.error("pick 중단: [2] plan 실패"); return + + # 3) gripper close + log.info("[3] Gripper CLOSE") + self.gripper.close_gripper() + time.sleep(1.0) + + # 4) SAFE_Z로 상승 + log.info(f"[4] up to SAFE_Z={SAFE_Z:.3f}") + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(bx, by, SAFE_Z, ori), + params=self.pilz_params): + log.error("pick 중단: [4] plan 실패"); return + + # 5) home XY로 이동 (SAFE_Z 유지) + log.info(f"[5] home XY → ({hx:.3f}, {hy:.3f}) @ SAFE_Z") + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(hx, hy, SAFE_Z, ori), + params=self.pilz_params): + log.error("pick 중단: [5] plan 실패"); return + + # 6) place_z로 하강 + log.info(f"[6] down to place_z={place_z:.3f}") + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(hx, hy, place_z, ori), + params=self.pilz_params): + log.error("pick 중단: [6] plan 실패"); return + + # 7) gripper open + log.info("[7] Gripper OPEN") + self.gripper.open_gripper() + time.sleep(1.0) + + # 8) SAFE_Z로 상승 + log.info(f"[8] up to SAFE_Z={SAFE_Z:.3f}") + plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(hx, hy, SAFE_Z, ori), + params=self.pilz_params) + + log.info("========== PICK END ==========") + + # ── 마우스 콜백 ── + def mouse_callback(self, event, x, y, flags, param): + if event != cv2.EVENT_LBUTTONDOWN: + return + if self.picking: + self.get_logger().warn("pick 동작 중 — 무시") + return + if self.color_image is None or self.depth_image is None or self.intrinsics is None: + self.get_logger().warn("프레임/내참 아직 준비 안 됨") + return + + h, w = self.depth_image.shape[:2] + if not (0 <= x < w and 0 <= y < h): + self.get_logger().warn("클릭 범위 초과") + return + + z_raw = self.depth_image[y, x] + if z_raw == 0: + self.get_logger().warn("해당 픽셀 depth=0") + return + + # depth → m + z_m = float(z_raw) / 1000.0 if self.depth_image.dtype == np.uint16 else float(z_raw) + + fx, fy = self.intrinsics["fx"], self.intrinsics["fy"] + ppx, ppy = self.intrinsics["ppx"], self.intrinsics["ppy"] + + cam_x = (x - ppx) * z_m / fx + cam_y = (y - ppy) * z_m / fy + cam_z = z_m + + base = self.transform_to_base((cam_x, cam_y, cam_z)) + if base is None: + self.get_logger().error("좌표 변환 실패") + return + + bx, by, bz = float(base[0]), float(base[1]), float(base[2]) + self.get_logger().info( + f"Camera: ({cam_x:.3f}, {cam_y:.3f}, {cam_z:.3f}) m → " + f"Base: ({bx:.3f}, {by:.3f}, {bz:.3f}) m" + ) + + self.picking = True + try: + self.pick_and_place(bx, by, bz) + finally: + self.picking = False + + # ── 메인 루프 ── + def run(self): + log = self.get_logger() + window = "ClickToPick (MoveIt)" + cv2.namedWindow(window) + cv2.setMouseCallback(window, self.mouse_callback) + + # Home 이동 + log.info("[Init] Home 이동") + home_state = RobotState(self.robot_model) + home_state.joint_positions = HOME_JOINTS + home_state.update() + if not plan_and_execute(self.robot, self.arm, log, + state_goal=home_state, + params=self.ompl_params): + log.error("Home 이동 실패 — 종료") + return + + time.sleep(0.5) + + # Home pose 저장 + T = get_ee_matrix(self.robot) + self.home_xyz = (T[0, 3], T[1, 3], T[2, 3]) + qx, qy, qz, qw = Rotation.from_matrix(T[:3, :3]).as_quat() + self.home_ori = {"x": float(qx), "y": float(qy), + "z": float(qz), "w": float(qw)} + log.info(f"[Init] Home = ({T[0,3]:.3f}, {T[1,3]:.3f}, {T[2,3]:.3f}) m") + + self.gripper.open_gripper() + time.sleep(1.0) + + while rclpy.ok(): + rclpy.spin_once(self, timeout_sec=0.01) + if self.color_image is None: + continue + cv2.imshow(window, self.color_image) + if (cv2.waitKey(1) & 0xFF) == 27: + break + + cv2.destroyAllWindows() + + +def main(args=None): + rclpy.init(args=args) + node = ClickPickNode() + try: + node.run() + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/dsr_practice/collision_obstacle.py b/src/dsr_practice/dsr_practice/dsr_practice/collision_obstacle.py new file mode 100644 index 0000000..3211162 --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/collision_obstacle.py @@ -0,0 +1,292 @@ +#!/usr/bin/env python3 +import math + +import rclpy +from rclpy.logging import get_logger +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy + +from geometry_msgs.msg import Pose +from geometry_msgs.msg import PoseStamped +from shape_msgs.msg import SolidPrimitive +from moveit_msgs.msg import CollisionObject +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters + +# ================================ +# 기본 설정 +# ================================ +GROUP_NAME = "manipulator" # SRDF에 정의된 planning group 이름 +BASE_FRAME = "base_link" # 로봇 베이스 프레임 +EE_LINK = "link_6" # 엔드이펙터 링크 이름 (SRDF/URDF 기준) + +HOME_JOINTS_DEG = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0] +HOME_JOINTS_RAD = [math.radians(d) for d in HOME_JOINTS_DEG] + +# ====== 안전 작업 영역 정의 (base_link 기준) ====== +SAFE_X_MIN = 0.0 # x는 0 이상 +SAFE_Y_MIN = -0.3 # y 하한 +SAFE_Y_MAX = 0.3 # y 상한 +SAFE_Z_MIN = 0.27 # z는 이 값보다 낮아지면 안 됨 +# ================================================== + + +def clamp_to_safe_workspace(x: float, y: float, z: float, logger): + """안전 작업 영역으로 (x, y, z) 클램핑""" + safe_x = x + safe_y = y + safe_z = z + + if safe_x < SAFE_X_MIN: + logger.warning( + f"Requested x ({safe_x:.3f} m) is below safety limit " + f"({SAFE_X_MIN:.3f} m). Clamping to SAFE_X_MIN." + ) + safe_x = SAFE_X_MIN + + if safe_y < SAFE_Y_MIN: + logger.warning( + f"Requested y ({safe_y:.3f} m) is below safety limit " + f"({SAFE_Y_MIN:.3f} m). Clamping to SAFE_Y_MIN." + ) + safe_y = SAFE_Y_MIN + elif safe_y > SAFE_Y_MAX: + logger.warning( + f"Requested y ({safe_y:.3f} m) is above safety limit " + f"({SAFE_Y_MAX:.3f} m). Clamping to SAFE_Y_MAX." + ) + safe_y = SAFE_Y_MAX + + if safe_z < SAFE_Z_MIN: + logger.warning( + f"Requested z ({safe_z:.3f} m) is below safety limit " + f"({SAFE_Z_MIN:.3f} m). Clamping to SAFE_Z_MIN." + ) + safe_z = SAFE_Z_MIN + + return safe_x, safe_y, safe_z + + +def plan_and_execute( + robot: MoveItPy, + planning_component, + logger, + pose_goal: PoseStamped = None, + plan_parameters=None, +): + """ + 공식 문서 스타일 helper: 계획 후 곧바로 실행 + + - pose_goal이 주어지면: + · 안전 영역 클램핑 + · start_state = current + · pose 기반 goal 설정 (EE_LINK) + - 그 다음 plan_parameters 유무에 따라 plan() 호출 후 execute + """ + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + + planning_component.set_start_state_to_current_state() + planning_component.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link=EE_LINK, + ) + + logger.info("Planning trajectory") + + if plan_parameters is not None: + plan_result = planning_component.plan( + parameters=plan_parameters + ) + else: + plan_result = planning_component.plan() + + if not plan_result: + logger.error("Planning failed") + return False + + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + robot.execute( + group_name=GROUP_NAME, + robot_trajectory=robot_trajectory, + blocking=True, + ) + logger.info("Execution finished") + return True + + +def add_box_obstacle(node: Node, + object_id: str, + frame_id: str, + size_xyz=(0.2, 0.2, 0.2), + pos_xyz=(0.5, 0.0, 0.2)): + """ + collision_object 토픽으로 박스 장애물 추가(ADD). + - QoS를 TRANSIENT_LOCAL로 해서 구독자가 나중에 붙어도 유지되게 함(퍼블리셔가 살아있는 동안). + """ + qos = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) + pub = node.create_publisher(CollisionObject, "/collision_object", qos) + + box = CollisionObject() + box.id = object_id + box.header.frame_id = frame_id + + primitive = SolidPrimitive() + primitive.type = SolidPrimitive.BOX + primitive.dimensions = list(size_xyz) # [x, y, z] + + pose = Pose() + pose.position.x, pose.position.y, pose.position.z = pos_xyz + pose.orientation.w = 1.0 # 회전 없음 + + box.primitives.append(primitive) + box.primitive_poses.append(pose) + box.operation = CollisionObject.ADD + + pub.publish(box) + node.get_logger().info( + f"[scene] ADD box id={object_id} frame={frame_id} size={size_xyz} pos={pos_xyz}" + ) + + +def main(args=None): + # ================================ + # MoveItPy 인스턴스 생성 + # ================================ + rclpy.init(args=args) + logger = get_logger("m0609.moveit_py.waypoint_pilz") + + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + logger.info("MoveItPy instance created") + robot_model = robot.get_robot_model() + + # ================================ + # 플래닝 파라미터 + # ================================ + home_params = PlanRequestParameters(robot) + waypoint_params = PlanRequestParameters(robot) + + # MoveIt2 config의 planning_pipelines.pipeline_names와 일치해야 함 + home_params.planning_pipeline = "ompl" + home_params.planner_id = "RRTConnect" + + waypoint_params.planning_pipeline = "pilz_industrial_motion_planner" + waypoint_params.planner_id = "PTP" + + home_params.max_velocity_scaling_factor = 0.2 + home_params.max_acceleration_scaling_factor = 0.1 + home_params.planning_time = 2.0 + + waypoint_params.max_velocity_scaling_factor = 0.15 + waypoint_params.max_acceleration_scaling_factor = 0.1 + waypoint_params.planning_time = 5.0 + + home_state = RobotState(robot_model) + home_state.set_joint_group_positions(GROUP_NAME, HOME_JOINTS_RAD) + home_state.update() + + # HOME 이동 (조인트 목표 → pose_goal 없이, plan_parameters만 사용) + arm.set_start_state_to_current_state() + arm.set_goal_state(robot_state=home_state) + plan_and_execute( + robot, + arm, + logger, + plan_parameters=home_params, + ) + + # ================================ + # 기본 3개 waypoint + 수동 우회 2개 = 총 5개 경로점 + # ================================ + logger.info("=== Waypoints from HOME, orientation fixed (Pilz PTP) ===") + + WAYPOINTS = [ + { # waypoint_1 + "pos": {"x": 0.45, "y": 0.22, "z": 0.52}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + { # detour_1 + "pos": {"x": 0.45, "y": 0.22, "z": 0.28}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + { # waypoint_2 + "pos": {"x": 0.62, "y": -0.18, "z": 0.28}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + { # detour_2 + "pos": {"x": 0.36, "y": -0.26, "z": 0.28}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + { # waypoint_3 + "pos": {"x": 0.36, "y": -0.26, "z": 0.55}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + ] + + # ====== 여기(중간)에서 장애물 추가 (경로 사이 2개) ====== + scene_node = rclpy.create_node("scene_obstacle_publisher") + mid_12 = (0.535, 0.020, 0.600) + mid_23 = (0.590, -0.220, 0.615) + obstacles = [ + {"id": "mid_box_1", "size_xyz": (0.10, 0.10, 0.10), "pos_xyz": mid_12}, + {"id": "mid_box_2", "size_xyz": (0.10, 0.10, 0.10), "pos_xyz": mid_23}, + ] + for obstacle in obstacles: + add_box_obstacle( + node=scene_node, + object_id=obstacle["id"], + frame_id=BASE_FRAME, + size_xyz=obstacle["size_xyz"], + pos_xyz=obstacle["pos_xyz"], + ) + + pose_goal = PoseStamped() + pose_goal.header.frame_id = BASE_FRAME + + for i, wp in enumerate(WAYPOINTS, start=1): + pos = wp["pos"] + ori = wp["ori"] + + logger.info( + f"--- Waypoint {i}: " + f"x={pos['x']:.3f}, y={pos['y']:.3f}, z={pos['z']:.3f} ---" + ) + + # pose_goal에 의도 좌표/자세 설정 + pose_goal.pose.position.x = pos["x"] + pose_goal.pose.position.y = pos["y"] + pose_goal.pose.position.z = pos["z"] + + pose_goal.pose.orientation.x = ori["x"] + pose_goal.pose.orientation.y = ori["y"] + pose_goal.pose.orientation.z = ori["z"] + pose_goal.pose.orientation.w = ori["w"] + + # plan_and_execute 안에서 안전영역 + goal 설정 + Pilz PTP 플래너 호출 + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=waypoint_params, + ) + + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/dsr_practice/gear_assembly.py b/src/dsr_practice/dsr_practice/dsr_practice/gear_assembly.py new file mode 100644 index 0000000..69a7923 --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/gear_assembly.py @@ -0,0 +1,423 @@ +#!/usr/bin/env python3 + +import math +import time + +import rclpy +from rclpy.logging import get_logger + +from geometry_msgs.msg import PoseStamped +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters + +# ====== OnRobot RG2 설정 ====== +from .onrobot import RG # 같은 패키지 내부의 onrobot.py 사용 + +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 + +# 그리퍼 폭 (raw 단위: 1/10 mm) +GRIPPER_OPEN_WIDTH = 500 # 50.0 mm +GRIPPER_CLOSE_WIDTH = 150 # 20.0 mm +GRIPPER_FORCE = 300 # 약 20 N + +# ================================ +# 기본 설정 +# ================================ +GROUP_NAME = "manipulator" # SRDF에 정의된 planning group 이름 +BASE_FRAME = "base_link" # 로봇 베이스 프레임 +EE_LINK = "link_6" # 엔드이펙터 링크 이름 (SRDF/URDF 기준) + +HOME_JOINTS_RAD = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(0.0), +} + +# ====== 안전 작업 영역 정의 (base_link 기준) ====== +SAFE_X_MIN = 0.0 # x는 0 이상 +SAFE_Y_MIN = -0.3 # y 하한 +SAFE_Y_MAX = 0.3 # y 상한 +SAFE_Z_MIN = 0.27 # z는 이 값보다 낮아지면 안 됨 +# ================================================== + +# ====== 기어 픽업/조립 포즈 (base_link 기준) ====== +GEAR_TASKS = [ + { # Gear 1 + "pick": { + "pos": {"x": 0.393, "y": 0.094, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + "place": { + "pos": {"x": 0.393, "y": -0.206, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + }, + { # Gear 2 + "pick": { + "pos": {"x": 0.392, "y": 0.200, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + "place": { + "pos": {"x": 0.392, "y": -0.101, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + }, + { # Gear 3 + "pick": { + "pos": {"x": 0.486, "y": 0.153, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + "place": { + "pos": {"x": 0.486, "y": -0.149, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + }, + { # Gear 4 + "pick": { + "pos": {"x": 0.427, "y": 0.148, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + "place": { + "pos": {"x": 0.426, "y": -0.153, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + }, +] + +APPROACH_OFFSET = 0.05 # 위에서 접근/후퇴할 거리 [m] + +# ----- 마지막 기어용 wiggle 파라미터 ----- +WIGGLE_Z = 0.295 # z축 회전할 높이 (place_z보다 약간 위) +WIGGLE_YAW_DEG = 5.0 # 좌우로 회전 각도 (deg) +WIGGLE_COUNT = 3 # 좌우 반복 횟수 + + +def quat_mul(q1, q2): + """쿼터니언 곱: q = q1 * q2""" + x1, y1, z1, w1 = q1 + x2, y2, z2, w2 = q2 + x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2 + y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2 + z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2 + w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2 + return x, y, z, w + + +def make_yaw_quat(yaw_rad): + """z축(yaw) 회전에 해당하는 쿼터니언 생성""" + half = yaw_rad / 2.0 + return (0.0, 0.0, math.sin(half), math.cos(half)) + + +def clamp_to_safe_workspace(x: float, y: float, z: float, logger): + """안전 작업 영역으로 (x, y, z) 클램핑""" + safe_x = x + safe_y = y + safe_z = z + + if safe_x < SAFE_X_MIN: + logger.warning( + f"Requested x ({safe_x:.3f} m) is below safety limit " + f"({SAFE_X_MIN:.3f} m). Clamping to SAFE_X_MIN." + ) + safe_x = SAFE_X_MIN + + if safe_y < SAFE_Y_MIN: + logger.warning( + f"Requested y ({safe_y:.3f} m) is below safety limit " + f"({SAFE_Y_MIN:.3f} m). Clamping to SAFE_Y_MIN." + ) + safe_y = SAFE_Y_MIN + elif safe_y > SAFE_Y_MAX: + logger.warning( + f"Requested y ({safe_y:.3f} m) is above safety limit " + f"({SAFE_Y_MAX:.3f} m). Clamping to SAFE_Y_MAX." + ) + safe_y = SAFE_Y_MAX + + if safe_z < SAFE_Z_MIN: + logger.warning( + f"Requested z ({safe_z:.3f} m) is below safety limit " + f"({SAFE_Z_MIN:.3f} m). Clamping to SAFE_Z_MIN." + ) + safe_z = SAFE_Z_MIN + + return safe_x, safe_y, safe_z + + +def plan_and_execute( + robot: MoveItPy, + planning_component, + logger, + pose_goal: PoseStamped = None, + plan_parameters=None, +): + """ + 공식 문서 스타일 helper: 계획 후 곧바로 실행 + + - pose_goal이 주어지면: + · 안전 영역 클램핑 + · start_state = current + · pose 기반 goal 설정 (EE_LINK) + - 그 다음 plan_parameters 유무에 따라 plan() 호출 후 execute + """ + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + + planning_component.set_start_state_to_current_state() + planning_component.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link=EE_LINK, + ) + + logger.info("Planning trajectory") + + if plan_parameters is not None: + plan_result = planning_component.plan( + parameters=plan_parameters + ) + else: + plan_result = planning_component.plan() + + if not plan_result: + logger.error("Planning failed") + return False + + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + robot.execute( + group_name=GROUP_NAME, + robot_trajectory=robot_trajectory, + blocking=True, + ) + logger.info("Execution finished") + return True + + +def main(args=None): + rclpy.init(args=args) + + logger = get_logger("m0609_gear_assembly") + logger.info("=== M0609 Gear Assembly 시작 ===") + + # ---- Gripper ---- + gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + time.sleep(0.5) + gripper.move_gripper(GRIPPER_OPEN_WIDTH, GRIPPER_FORCE) + + # ---- MoveIt ---- + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + robot_model = robot.get_robot_model() + + # ---- PlanRequestParameters (HOME / Pilz) ---- + home_params = PlanRequestParameters(robot) + home_params.planning_pipeline = "ompl" + home_params.planner_id = "RRTConnect" + home_params.max_velocity_scaling_factor = 0.2 + home_params.max_acceleration_scaling_factor = 0.1 + home_params.planning_time = 2.0 + + pilz_params = PlanRequestParameters(robot) + pilz_params.planning_pipeline = "pilz_industrial_motion_planner" + pilz_params.planner_id = "PTP" + pilz_params.max_velocity_scaling_factor = 0.15 + pilz_params.max_acceleration_scaling_factor = 0.1 + pilz_params.planning_time = 2.0 + + # ---- HOME 자세로 이동 (joint goal) ---- + logger.info("=== HOME 자세로 이동 ===") + home_state = RobotState(robot_model) + home_state.joint_positions = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(0.0), + } + home_state.update() + + arm.set_start_state_to_current_state() + arm.set_goal_state(robot_state=home_state) + plan_and_execute(robot, arm, logger, plan_parameters=home_params) + + # ---- PoseStamped 공용 객체 준비 ---- + pose_goal = PoseStamped() + pose_goal.header.frame_id = BASE_FRAME + + logger.info("=== Gear Pick & Place 시작 ===") + + total_gears = len(GEAR_TASKS) + + # ------------------------------- + # 각 기어에 대해 Pick & Place + # ------------------------------- + for gear_idx, task in enumerate(GEAR_TASKS, start=1): + pick = task["pick"] + place = task["place"] + + logger.info(f"--- Gear {gear_idx} 작업 시작 ---") + + # 1) Pick 위에서 접근 (z + offset) + pose_goal.pose.position.x = pick["pos"]["x"] + pose_goal.pose.position.y = pick["pos"]["y"] + pose_goal.pose.position.z = pick["pos"]["z"] + APPROACH_OFFSET + + pose_goal.pose.orientation.x = pick["ori"]["x"] + pose_goal.pose.orientation.y = pick["ori"]["y"] + pose_goal.pose.orientation.z = pick["ori"]["z"] + pose_goal.pose.orientation.w = pick["ori"]["w"] + + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # 2) Pick 위치로 내려가기 + pose_goal.pose.position.z = pick["pos"]["z"] + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # 3) Gripper 닫기 (집기) + logger.info("Gripper CLOSE (20mm) – 기어 집기") + gripper.move_gripper(width_val=GRIPPER_CLOSE_WIDTH, + force_val=GRIPPER_FORCE) + time.sleep(1.0) + + # 4) 다시 위로 올라가기 (Pick z + offset) + pose_goal.pose.position.z = pick["pos"]["z"] + APPROACH_OFFSET + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # 5) Place 위에서 접근 + pose_goal.pose.position.x = place["pos"]["x"] + pose_goal.pose.position.y = place["pos"]["y"] + pose_goal.pose.position.z = place["pos"]["z"] + APPROACH_OFFSET + + pose_goal.pose.orientation.x = place["ori"]["x"] + pose_goal.pose.orientation.y = place["ori"]["y"] + pose_goal.pose.orientation.z = place["ori"]["z"] + pose_goal.pose.orientation.w = place["ori"]["w"] + + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # ------------------------------- + # Place 동작 + # - 1~3번 기어: 바로 place_z로 하강 + # - 4번 기어(마지막): z=WIGGLE_Z에서 z축 회전 wiggle 후 place_z로 하강 + # ------------------------------- + if gear_idx < total_gears: + # 6) Place 위치로 내려가기 (일반) + pose_goal.pose.position.z = place["pos"]["z"] + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + else: + # ---- 마지막 기어: z축 wiggle ---- + logger.info( + f"마지막 기어: z={WIGGLE_Z:.3f}에서 " + f"±{WIGGLE_YAW_DEG}deg 좌우 회전 {WIGGLE_COUNT}회씩 수행" + ) + + # 우선 WIGGLE_Z까지 z만 이동 + pose_goal.pose.position.z = WIGGLE_Z + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # 기준 쿼터니언 (place 자세) + q_base = ( + place["ori"]["x"], + place["ori"]["y"], + place["ori"]["z"], + place["ori"]["w"], + ) + + yaw_rad = math.radians(WIGGLE_YAW_DEG) + + # 좌우로 WIGGLE_COUNT 번 반복 + for i in range(1, WIGGLE_COUNT + 1): + # +yaw + q_plus = quat_mul(make_yaw_quat(+yaw_rad), q_base) + pose_goal.pose.orientation.x = q_plus[0] + pose_goal.pose.orientation.y = q_plus[1] + pose_goal.pose.orientation.z = q_plus[2] + pose_goal.pose.orientation.w = q_plus[3] + + logger.info(f"Wiggle {i}/{WIGGLE_COUNT}: +{WIGGLE_YAW_DEG:.1f} deg") + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # -yaw + q_minus = quat_mul(make_yaw_quat(-yaw_rad), q_base) + pose_goal.pose.orientation.x = q_minus[0] + pose_goal.pose.orientation.y = q_minus[1] + pose_goal.pose.orientation.z = q_minus[2] + pose_goal.pose.orientation.w = q_minus[3] + + logger.info(f"Wiggle {i}/{WIGGLE_COUNT}: -{WIGGLE_YAW_DEG:.1f} deg") + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # 마지막에 기준 자세로 복귀 + pose_goal.pose.orientation.x = q_base[0] + pose_goal.pose.orientation.y = q_base[1] + pose_goal.pose.orientation.z = q_base[2] + pose_goal.pose.orientation.w = q_base[3] + + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # 그리고 place_z로 직선 하강 + pose_goal.pose.position.z = place["pos"]["z"] + logger.info(f"마지막 기어: wiggle 후 place_z={place['pos']['z']:.3f}로 하강") + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # 7) Gripper 열기 (놓기) + logger.info("Gripper OPEN (50mm) – 기어 놓기") + gripper.move_gripper(width_val=GRIPPER_OPEN_WIDTH, + force_val=GRIPPER_FORCE) + time.sleep(1.0) + + # 8) 다시 위로 올라가기 (Place z + offset) + pose_goal.pose.position.z = place["pos"]["z"] + APPROACH_OFFSET + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + logger.info(f"--- Gear {gear_idx} 작업 완료 ---") + + logger.info("=== 모든 기어 조립 완료. HOME으로 복귀 ===") + + # 마지막으로 HOME으로 복귀 + arm.set_start_state_to_current_state() + arm.set_goal_state(robot_state=home_state) + plan_and_execute(robot, arm, logger, plan_parameters=home_params) + + logger.info("=== Gear Assembly 노드 종료 ===") + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/dsr_practice/gripper.py b/src/dsr_practice/dsr_practice/dsr_practice/gripper.py new file mode 100644 index 0000000..5202e69 --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/gripper.py @@ -0,0 +1,98 @@ +#!/usr/bin/env python3 +# rg2_gripper_node.py + +import time + +import rclpy + +from .onrobot import RG # 같은 패키지 내부의 onrobot.py 사용 + + +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 + +# 그리퍼 폭 (raw 단위: 1/10 mm) +GRIPPER_OPEN_WIDTH = 1100 # 110.0 mm, RG2 최대 벌림 +GRIPPER_CLOSE_WIDTH = 0 # 완전히 닫는 방향으로 이동하다가 grip_detected 시 정지 +GRIPPER_FORCE = 200 + +STATUS_CHECK_INTERVAL = 0.05 +GRIP_TIMEOUT_SEC = 8.0 + + +def wait_until_not_busy(gripper, logger, timeout_sec=5.0): + start_time = time.time() + while time.time() - start_time < timeout_sec: + status = gripper.get_status() + busy = bool(status[0]) + if not busy: + return True + time.sleep(STATUS_CHECK_INTERVAL) + + logger.warning("그리퍼 동작 완료 대기 시간이 초과되었습니다.") + return False + + +def close_until_grip_detected(gripper, logger): + logger.info( + f"그리퍼 CLOSE 시작 (목표 폭={GRIPPER_CLOSE_WIDTH}, 힘={GRIPPER_FORCE})" + ) + gripper.move_gripper(width_val=GRIPPER_CLOSE_WIDTH, force_val=GRIPPER_FORCE) + + start_time = time.time() + while time.time() - start_time < GRIP_TIMEOUT_SEC: + status = gripper.get_status() + busy = bool(status[0]) + grip_detected = bool(status[1]) + + if grip_detected: + logger.info("Grip detected - 컵을 잡았으므로 그리퍼를 정지합니다.") + gripper.set_control_mode(8) + return True + + if not busy: + logger.warning("그리퍼가 끝까지 닫혔지만 grip_detected가 감지되지 않았습니다.") + return False + + time.sleep(STATUS_CHECK_INTERVAL) + + logger.error("Grip 감지 대기 시간이 초과되었습니다. 그리퍼를 정지합니다.") + gripper.set_control_mode(8) + return False + + +def main(args=None): + rclpy.init(args=args) + node = rclpy.create_node("rg2_gripper_node") + + logger = node.get_logger() + logger.info("=== RG2 Gripper Control Node 시작 ===") + + # ---- 그리퍼 연결 ---- + gripper = RG( + gripper=GRIPPER_NAME, + ip=TOOLCHARGER_IP, + port=TOOLCHARGER_PORT, + ) + time.sleep(0.5) # 연결 안정화 대기 + logger.info("그리퍼와 연결됨") + + logger.info(f"그리퍼 최대 OPEN (폭={GRIPPER_OPEN_WIDTH})") + gripper.move_gripper(width_val=GRIPPER_OPEN_WIDTH, force_val=GRIPPER_FORCE) + wait_until_not_busy(gripper, logger) + + grip_detected = close_until_grip_detected(gripper, logger) + if grip_detected: + logger.info("Grip 성공 - 컵을 잡은 위치에서 정지했습니다.") + else: + logger.error("Grip 실패 - 컵을 잡지 못했거나 감지하지 못했습니다.") + + logger.info("=== RG2 Gripper Control Node 종료 ===") + gripper.close_connection() + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/dsr_practice/joint_state_relay.py b/src/dsr_practice/dsr_practice/dsr_practice/joint_state_relay.py new file mode 100644 index 0000000..c727f8c --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/joint_state_relay.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import JointState + + +class JointStateRelay(Node): + def __init__(self): + super().__init__("joint_state_relay") + self.declare_parameter("input_topic", "/dsr01/joint_states") + self.declare_parameter("output_topic", "/joint_states") + + input_topic = self.get_parameter("input_topic").value + output_topic = self.get_parameter("output_topic").value + + self.publisher = self.create_publisher(JointState, output_topic, 10) + self.subscription = self.create_subscription( + JointState, + input_topic, + self.callback, + 10, + ) + self.get_logger().info(f"Relaying {input_topic} -> {output_topic}") + + def callback(self, msg): + self.publisher.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = JointStateRelay() + try: + rclpy.spin(node) + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/dsr_practice/mp_basic.py b/src/dsr_practice/dsr_practice/dsr_practice/mp_basic.py new file mode 100644 index 0000000..d1296b4 --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/mp_basic.py @@ -0,0 +1,174 @@ +#!/usr/bin/env python3 +import math + +import rclpy +from rclpy.logging import get_logger + +from geometry_msgs.msg import PoseStamped +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy + + + +GROUP_NAME = "manipulator" # SRDF에 정의된 planning group 이름 +BASE_FRAME = "base_link" # 로봇 베이스 프레임 +EE_LINK = "link_6" # 엔드이펙터 링크 이름 (SRDF/URDF 기준) + +# HOME_JOINTS_DEG = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0] +# HOME_JOINTS_RAD = [math.radians(d) for d in HOME_JOINTS_DEG] + +# 이걸로 교체 +HOME_JOINTS_RAD = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(0.0), +} + +# ====== 안전 작업 영역 정의 (base_link 기준) ====== +SAFE_X_MIN = 0.0 # x는 0 이상 +SAFE_Y_MIN = -0.3 # y 하한 +SAFE_Y_MAX = 0.3 # y 상한 +SAFE_Z_MIN = 0.27 # z는 이 값보다 낮아지면 안 됨 +# ================================================== + + +def clamp_to_safe_workspace(x: float, y: float, z: float, logger): + """안전 작업 영역으로 (x, y, z) 클램핑""" + safe_x = x + safe_y = y + safe_z = z + + if safe_x < SAFE_X_MIN: + logger.warning( + f"Requested x ({safe_x:.3f} m) is below safety limit " + f"({SAFE_X_MIN:.3f} m). Clamping to SAFE_X_MIN." + ) + safe_x = SAFE_X_MIN + + if safe_y < SAFE_Y_MIN: + logger.warning( + f"Requested y ({safe_y:.3f} m) is below safety limit " + f"({SAFE_Y_MIN:.3f} m). Clamping to SAFE_Y_MIN." + ) + safe_y = SAFE_Y_MIN + elif safe_y > SAFE_Y_MAX: + logger.warning( + f"Requested y ({safe_y:.3f} m) is above safety limit " + f"({SAFE_Y_MAX:.3f} m). Clamping to SAFE_Y_MAX." + ) + safe_y = SAFE_Y_MAX + + if safe_z < SAFE_Z_MIN: + logger.warning( + f"Requested z ({safe_z:.3f} m) is below safety limit " + f"({SAFE_Z_MIN:.3f} m). Clamping to SAFE_Z_MIN." + ) + safe_z = SAFE_Z_MIN + + return safe_x, safe_y, safe_z + + +def plan_and_execute( + robot: MoveItPy, + planning_component, + logger, + pose_goal: PoseStamped = None, + plan_parameters=None, +): + """ + 공식 문서 스타일 helper: 계획 후 곧바로 실행 + + - pose_goal이 주어지면: + · 안전 영역 클램핑 + · start_state = current + · pose 기반 goal 설정 (EE_LINK) + - 그 다음 plan_parameters 유무에 따라 plan() 호출 후 execute + """ + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + + planning_component.set_start_state_to_current_state() + planning_component.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link=EE_LINK, + ) + + logger.info("Planning trajectory") + + if plan_parameters is not None: + plan_result = planning_component.plan( + parameters=plan_parameters + ) + else: + plan_result = planning_component.plan() + + if not plan_result: + logger.error("Planning failed") + return False + + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + robot.execute( + group_name=GROUP_NAME, + robot_trajectory=robot_trajectory, + blocking=True, + ) + logger.info("Execution finished") + return True + +def main(): + rclpy.init() + logger = get_logger("m0609.moveit_py.basic") + + robot = MoveItPy(node_name="moveit_py") + import time + time.sleep(2.0) + + arm = robot.get_planning_component(GROUP_NAME) + logger.info("MoveItPy instance created") + + # ── 1) Home position으로 이동 ────────────────────────────── + # RobotState 방식 대신 joint value map으로 goal 설정 + arm.set_start_state_to_current_state() + + robot_model = robot.get_robot_model() + home_state = RobotState(robot_model) + + # set_joint_group_positions 대신 이걸로 + home_state.joint_positions = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(0.0), + } + home_state.update() + + arm.set_goal_state(robot_state=home_state) + plan_and_execute(robot, arm, logger) + + # ── 2) Pose goal로 이동 ──────────────────────────────────── + pose_goal = PoseStamped() + pose_goal.header.frame_id = BASE_FRAME + pose_goal.pose.position.x = 0.5 + pose_goal.pose.position.y = 0.0 + pose_goal.pose.position.z = 0.5 + pose_goal.pose.orientation.x = 0.0 + pose_goal.pose.orientation.y = 1.0 + pose_goal.pose.orientation.z = 0.0 + pose_goal.pose.orientation.w = 0.0 + + plan_and_execute(robot, arm, logger, pose_goal=pose_goal) + + rclpy.shutdown() \ No newline at end of file diff --git a/src/dsr_practice/dsr_practice/dsr_practice/mp_waypoint.py b/src/dsr_practice/dsr_practice/dsr_practice/mp_waypoint.py new file mode 100644 index 0000000..4b76f0e --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/mp_waypoint.py @@ -0,0 +1,189 @@ +#!/usr/bin/env python3 +import math + +import rclpy +from rclpy.logging import get_logger + +from geometry_msgs.msg import PoseStamped +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy + +# ================================ +# 기본 설정 +# ================================ +GROUP_NAME = "manipulator" # SRDF에 정의된 planning group 이름 +BASE_FRAME = "base_link" # 로봇 베이스 프레임 +EE_LINK = "link_6" # 엔드이펙터 링크 이름 (SRDF/URDF 기준) + +HOME_JOINTS_DEG = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0] +HOME_JOINTS_RAD = [math.radians(d) for d in HOME_JOINTS_DEG] + +# ====== 안전 작업 영역 정의 (base_link 기준) ====== +SAFE_X_MIN = 0.0 # x는 0 이상 +SAFE_Y_MIN = -0.3 # y 하한 +SAFE_Y_MAX = 0.3 # y 상한 +SAFE_Z_MIN = 0.27 # z는 이 값보다 낮아지면 안 됨 +# ================================================== + + +def clamp_to_safe_workspace(x: float, y: float, z: float, logger): + """안전 작업 영역으로 (x, y, z) 클램핑""" + safe_x = x + safe_y = y + safe_z = z + + if safe_x < SAFE_X_MIN: + logger.warning( + f"Requested x ({safe_x:.3f} m) is below safety limit " + f"({SAFE_X_MIN:.3f} m). Clamping to SAFE_X_MIN." + ) + safe_x = SAFE_X_MIN + + if safe_y < SAFE_Y_MIN: + logger.warning( + f"Requested y ({safe_y:.3f} m) is below safety limit " + f"({SAFE_Y_MIN:.3f} m). Clamping to SAFE_Y_MIN." + ) + safe_y = SAFE_Y_MIN + elif safe_y > SAFE_Y_MAX: + logger.warning( + f"Requested y ({safe_y:.3f} m) is above safety limit " + f"({SAFE_Y_MAX:.3f} m). Clamping to SAFE_Y_MAX." + ) + safe_y = SAFE_Y_MAX + + if safe_z < SAFE_Z_MIN: + logger.warning( + f"Requested z ({safe_z:.3f} m) is below safety limit " + f"({SAFE_Z_MIN:.3f} m). Clamping to SAFE_Z_MIN." + ) + safe_z = SAFE_Z_MIN + + return safe_x, safe_y, safe_z + + +def plan_and_execute( + robot: MoveItPy, + planning_component, + logger, + pose_goal: PoseStamped = None, + plan_parameters=None, +): + """ + 공식 문서 스타일 helper: 계획 후 곧바로 실행 + + - pose_goal이 주어지면: + · 안전 영역 클램핑 + · start_state = current + · pose 기반 goal 설정 (EE_LINK) + - 그 다음 plan_parameters 유무에 따라 plan() 호출 후 execute + """ + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + + planning_component.set_start_state_to_current_state() + planning_component.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link=EE_LINK, + ) + + logger.info("Planning trajectory") + + if plan_parameters is not None: + plan_result = planning_component.plan( + parameters=plan_parameters + ) + else: + plan_result = planning_component.plan() + + if not plan_result: + logger.error("Planning failed") + return False + + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + robot.execute( + group_name=GROUP_NAME, + robot_trajectory=robot_trajectory, + blocking=True, + ) + logger.info("Execution finished") + return True + + +def main(args=None): + # ================================ + # Instantiating moveit_py and planning component + # ================================ + rclpy.init(args=args) + logger = get_logger("m0609.moveit_py.waypoint") + + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + logger.info("MoveItPy instance created") + + # 로봇 모델 / RobotState 준비 + robot_model = robot.get_robot_model() + home_state = RobotState(robot_model) + home_state.set_joint_group_positions(GROUP_NAME, HOME_JOINTS_RAD) + home_state.update() + + # 1) HOME 자세로 이동 (조인트 목표) + arm.set_start_state_to_current_state() + arm.set_goal_state(robot_state=home_state) + plan_and_execute(robot, arm, logger) + + # 2) WAYPOINT 이동 (안전영역 포함) + WAYPOINTS = [ + { # waypoint 1 + "pos": {"x": 0.493, "y": 0.010, "z": 0.417}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + { # waypoint 2 + "pos": {"x": 0.493, "y": -0.218, "z": 0.417}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + { # waypoint 3 + "pos": {"x": 0.371, "y": -0.218, "z": 0.419}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + ] + + pose_goal = PoseStamped() + pose_goal.header.frame_id = BASE_FRAME + + for i, wp in enumerate(WAYPOINTS, start=1): + pos = wp["pos"] + ori = wp["ori"] + + logger.info( + f"--- Waypoint {i}: " + f"x={pos['x']:.3f}, y={pos['y']:.3f}, z={pos['z']:.3f} ---" + ) + + # pose_goal에 원래 의도 좌표/자세 설정 + pose_goal.pose.position.x = pos["x"] + pose_goal.pose.position.y = pos["y"] + pose_goal.pose.position.z = pos["z"] + + pose_goal.pose.orientation.x = ori["x"] + pose_goal.pose.orientation.y = ori["y"] + pose_goal.pose.orientation.z = ori["z"] + pose_goal.pose.orientation.w = ori["w"] + + # plan_and_execute 안에서 안전영역 + goal 설정까지 처리 + plan_and_execute(robot, arm, logger, pose_goal=pose_goal) + + robot.shutdown() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/dsr_practice/mp_waypoint_pilz.py b/src/dsr_practice/dsr_practice/dsr_practice/mp_waypoint_pilz.py new file mode 100644 index 0000000..9eb6ff5 --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/mp_waypoint_pilz.py @@ -0,0 +1,239 @@ +#!/usr/bin/env python3 +import math + +import rclpy +from rclpy.logging import get_logger + +from geometry_msgs.msg import PoseStamped +from moveit.planning import MoveItPy, PlanRequestParameters +from moveit_msgs.msg import Constraints, JointConstraint + +# ================================ +# 기본 설정 +# ================================ +GROUP_NAME = "manipulator" # SRDF에 정의된 planning group 이름 +BASE_FRAME = "base_link" # 로봇 베이스 프레임 +EE_LINK = "link_6" # 엔드이펙터 링크 이름 (SRDF/URDF 기준) + +HOME_JOINTS_DEG = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0] +HOME_JOINTS_RAD = [math.radians(d) for d in HOME_JOINTS_DEG] + +# ====== 안전 작업 영역 정의 (base_link 기준) ====== +SAFE_X_MIN = 0.0 # x는 0 이상 +SAFE_Y_MIN = -0.3 # y 하한 +SAFE_Y_MAX = 0.3 # y 상한 +SAFE_Z_MIN = 0.27 # z는 이 값보다 낮아지면 안 됨 +# ================================================== + + +def clamp_to_safe_workspace(x: float, y: float, z: float, logger): + """안전 작업 영역으로 (x, y, z) 클램핑""" + safe_x = x + safe_y = y + safe_z = z + + if safe_x < SAFE_X_MIN: + logger.warning( + f"Requested x ({safe_x:.3f} m) is below safety limit " + f"({SAFE_X_MIN:.3f} m). Clamping to SAFE_X_MIN." + ) + safe_x = SAFE_X_MIN + + if safe_y < SAFE_Y_MIN: + logger.warning( + f"Requested y ({safe_y:.3f} m) is below safety limit " + f"({SAFE_Y_MIN:.3f} m). Clamping to SAFE_Y_MIN." + ) + safe_y = SAFE_Y_MIN + elif safe_y > SAFE_Y_MAX: + logger.warning( + f"Requested y ({safe_y:.3f} m) is above safety limit " + f"({SAFE_Y_MAX:.3f} m). Clamping to SAFE_Y_MAX." + ) + safe_y = SAFE_Y_MAX + + if safe_z < SAFE_Z_MIN: + logger.warning( + f"Requested z ({safe_z:.3f} m) is below safety limit " + f"({SAFE_Z_MIN:.3f} m). Clamping to SAFE_Z_MIN." + ) + safe_z = SAFE_Z_MIN + + return safe_x, safe_y, safe_z + + +def build_home_joint_constraints() -> list[Constraints]: + constraints = Constraints() + joint_names = [f"joint_{i}" for i in range(1, 7)] + + for joint_name, position in zip(joint_names, HOME_JOINTS_RAD): + joint_constraint = JointConstraint() + joint_constraint.joint_name = joint_name + joint_constraint.position = position + joint_constraint.tolerance_above = 0.001 + joint_constraint.tolerance_below = 0.001 + joint_constraint.weight = 1.0 + constraints.joint_constraints.append(joint_constraint) + + return [constraints] + + +def plan_and_execute( + robot: MoveItPy, + planning_component, + logger, + pose_goal: PoseStamped = None, + plan_parameters=None, +): + """ + 공식 문서 스타일 helper: 계획 후 곧바로 실행 + + - pose_goal이 주어지면: + · 안전 영역 클램핑 + · start_state = current + · pose 기반 goal 설정 (EE_LINK) + - 그 다음 plan_parameters 유무에 따라 plan() 호출 후 execute + """ + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + + planning_component.set_start_state_to_current_state() + planning_component.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link=EE_LINK, + ) + + logger.info("Planning trajectory") + + if plan_parameters is not None: + plan_result = planning_component.plan( + parameters=plan_parameters + ) + else: + plan_result = planning_component.plan() + + if not plan_result: + logger.error("Planning failed") + return False + + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + robot.execute( + group_name=GROUP_NAME, + robot_trajectory=robot_trajectory, + blocking=True, + ) + logger.info("Execution finished") + return True + + +def main(args=None): + # ================================ + # MoveItPy 인스턴스 생성 + # ================================ + rclpy.init(args=args) + logger = get_logger("m0609.moveit_py.waypoint_pilz") + + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + logger.info("MoveItPy instance created") + # ================================ + # 플래닝 파라미터 + # ================================ + home_params = PlanRequestParameters(robot) + pilz_params = PlanRequestParameters(robot) + + # MoveIt2 config의 planning_pipelines.pipeline_names와 일치해야 함 + home_params.planning_pipeline = "ompl" + home_params.planner_id = "RRTConnectkConfigDefault" + + pilz_params.planning_pipeline = "pilz_industrial_motion_planner" + pilz_params.planner_id = "PTP" + + # HOME: OMPL + home_params.max_velocity_scaling_factor = 0.2 + home_params.max_acceleration_scaling_factor = 0.1 + home_params.planning_time = 2.0 + + # Waypoint: Pilz PTP + pilz_params.max_velocity_scaling_factor = 0.15 + pilz_params.max_acceleration_scaling_factor = 0.1 + pilz_params.planning_time = 2.0 + + logger.info("=== Move to HOME joints (OMPL + slow) ===") + + # HOME 이동 (조인트 목표 → pose_goal 없이, plan_parameters만 사용) + arm.set_start_state_to_current_state() + arm.set_goal_state( + motion_plan_constraints=build_home_joint_constraints() + ) + plan_and_execute( + robot, + arm, + logger, + plan_parameters=home_params, + ) + + # ================================ + # Waypoints (Pilz PTP, 안전영역 포함) + # ================================ + logger.info("=== Waypoints from HOME, orientation fixed (Pilz PTP) ===") + + WAYPOINTS = [ + { # waypoint 1 + "pos": {"x": 0.493, "y": 0.010, "z": 0.417}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + { # waypoint 2 + "pos": {"x": 0.493, "y": -0.218, "z": 0.417}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + { # waypoint 3 + "pos": {"x": 0.371, "y": -0.218, "z": 0.419}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + ] + + pose_goal = PoseStamped() + pose_goal.header.frame_id = BASE_FRAME + + for i, wp in enumerate(WAYPOINTS, start=1): + pos = wp["pos"] + ori = wp["ori"] + + logger.info( + f"--- Waypoint {i}: " + f"x={pos['x']:.3f}, y={pos['y']:.3f}, z={pos['z']:.3f} ---" + ) + + # pose_goal에 의도 좌표/자세 설정 + pose_goal.pose.position.x = pos["x"] + pose_goal.pose.position.y = pos["y"] + pose_goal.pose.position.z = pos["z"] + + pose_goal.pose.orientation.x = ori["x"] + pose_goal.pose.orientation.y = ori["y"] + pose_goal.pose.orientation.z = ori["z"] + pose_goal.pose.orientation.w = ori["w"] + + # plan_and_execute 안에서 안전영역 + goal 설정 + Pilz 플래너 호출 + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=pilz_params, + ) + + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/dsr_practice/mp_waypoint_pilz_lin.py b/src/dsr_practice/dsr_practice/dsr_practice/mp_waypoint_pilz_lin.py new file mode 100644 index 0000000..5863d36 --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/mp_waypoint_pilz_lin.py @@ -0,0 +1,228 @@ +#!/usr/bin/env python3 +import math + +import rclpy +from rclpy.logging import get_logger + +from geometry_msgs.msg import PoseStamped +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters + +# ================================ +# 기본 설정 +# ================================ +GROUP_NAME = "manipulator" # SRDF에 정의된 planning group 이름 +BASE_FRAME = "base_link" # 로봇 베이스 프레임 +EE_LINK = "link_6" # 엔드이펙터 링크 이름 (SRDF/URDF 기준) + +HOME_JOINTS_DEG = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0] +HOME_JOINTS_RAD = [math.radians(d) for d in HOME_JOINTS_DEG] + +# ====== 안전 작업 영역 정의 (base_link 기준) ====== +SAFE_X_MIN = 0.0 # x는 0 이상 +SAFE_Y_MIN = -0.3 # y 하한 +SAFE_Y_MAX = 0.3 # y 상한 +SAFE_Z_MIN = 0.27 # z는 이 값보다 낮아지면 안 됨 +# ================================================== + + +def clamp_to_safe_workspace(x: float, y: float, z: float, logger): + """안전 작업 영역으로 (x, y, z) 클램핑""" + safe_x = x + safe_y = y + safe_z = z + + if safe_x < SAFE_X_MIN: + logger.warning( + f"Requested x ({safe_x:.3f} m) is below safety limit " + f"({SAFE_X_MIN:.3f} m). Clamping to SAFE_X_MIN." + ) + safe_x = SAFE_X_MIN + + if safe_y < SAFE_Y_MIN: + logger.warning( + f"Requested y ({safe_y:.3f} m) is below safety limit " + f"({SAFE_Y_MIN:.3f} m). Clamping to SAFE_Y_MIN." + ) + safe_y = SAFE_Y_MIN + elif safe_y > SAFE_Y_MAX: + logger.warning( + f"Requested y ({safe_y:.3f} m) is above safety limit " + f"({SAFE_Y_MAX:.3f} m). Clamping to SAFE_Y_MAX." + ) + safe_y = SAFE_Y_MAX + + if safe_z < SAFE_Z_MIN: + logger.warning( + f"Requested z ({safe_z:.3f} m) is below safety limit " + f"({SAFE_Z_MIN:.3f} m). Clamping to SAFE_Z_MIN." + ) + safe_z = SAFE_Z_MIN + + return safe_x, safe_y, safe_z + + +def plan_and_execute( + robot: MoveItPy, + planning_component, + logger, + pose_goal: PoseStamped = None, + plan_parameters=None, +): + """ + 공식 문서 스타일 helper: 계획 후 곧바로 실행 + + - pose_goal이 주어지면: + · 안전 영역 클램핑 + · start_state = current + · pose 기반 goal 설정 (EE_LINK) + - 그 다음 plan_parameters 유무에 따라 plan() 호출 후 execute + """ + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + + planning_component.set_start_state_to_current_state() + planning_component.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link=EE_LINK, + ) + + logger.info("Planning trajectory") + + if plan_parameters is not None: + plan_result = planning_component.plan( + parameters=plan_parameters + ) + else: + plan_result = planning_component.plan() + + if not plan_result: + logger.error("Planning failed") + return False + + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + robot.execute( + group_name=GROUP_NAME, + robot_trajectory=robot_trajectory, + blocking=True, + ) + logger.info("Execution finished") + return True + + +def main(args=None): + # ================================ + # MoveItPy 인스턴스 생성 + # ================================ + rclpy.init(args=args) + logger = get_logger("m0609.moveit_py.waypoint_pilz") + + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + logger.info("MoveItPy instance created") + robot_model = robot.get_robot_model() + + # ================================ + # 플래닝 파라미터 + # ================================ + home_params = PlanRequestParameters(robot) + pilz_params = PlanRequestParameters(robot) + + # MoveIt2 config의 planning_pipelines.pipeline_names와 일치해야 함 + home_params.planning_pipeline = "ompl" + home_params.planner_id = "RRTConnect" + + pilz_params.planning_pipeline = "pilz_industrial_motion_planner" + pilz_params.planner_id = "LIN" + + # HOME: OMPL + home_params.max_velocity_scaling_factor = 0.2 + home_params.max_acceleration_scaling_factor = 0.1 + home_params.planning_time = 2.0 + + # Waypoint: Pilz LIN + pilz_params.max_velocity_scaling_factor = 0.05 # 기존 0.10 -> 0.05 + pilz_params.max_acceleration_scaling_factor = 0.03 # 기존 0.10 -> 0.03 + pilz_params.planning_time = 2.0 + + logger.info("=== Move to HOME joints (OMPL + slow) ===") + + home_state = RobotState(robot_model) + home_state.set_joint_group_positions(GROUP_NAME, HOME_JOINTS_RAD) + home_state.update() + + # HOME 이동 (조인트 목표 → pose_goal 없이, plan_parameters만 사용) + arm.set_start_state_to_current_state() + arm.set_goal_state(robot_state=home_state) + plan_and_execute( + robot, + arm, + logger, + plan_parameters=home_params, + ) + + # ================================ + # Waypoints (Pilz LIN, 안전영역 포함) + # ================================ + logger.info("=== Waypoints from HOME, orientation fixed (Pilz LIN) ===") + + WAYPOINTS = [ + { # waypoint 1 (높게 시작) + "pos": {"x": 0.45, "y": 0.22, "z": 0.52}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + { # waypoint 2 (대각선으로 내려가며 이동) + "pos": {"x": 0.62, "y": -0.18, "z": 0.33}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + { # waypoint 3 (다시 대각선으로 올라가며 이동) + "pos": {"x": 0.36, "y": -0.26, "z": 0.55}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + ] + + pose_goal = PoseStamped() + pose_goal.header.frame_id = BASE_FRAME + + for i, wp in enumerate(WAYPOINTS, start=1): + pos = wp["pos"] + ori = wp["ori"] + + logger.info( + f"--- Waypoint {i}: " + f"x={pos['x']:.3f}, y={pos['y']:.3f}, z={pos['z']:.3f} ---" + ) + + # pose_goal에 의도 좌표/자세 설정 + pose_goal.pose.position.x = pos["x"] + pose_goal.pose.position.y = pos["y"] + pose_goal.pose.position.z = pos["z"] + + pose_goal.pose.orientation.x = ori["x"] + pose_goal.pose.orientation.y = ori["y"] + pose_goal.pose.orientation.z = ori["z"] + pose_goal.pose.orientation.w = ori["w"] + + # plan_and_execute 안에서 안전영역 + goal 설정 + Pilz 플래너 호출 + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=pilz_params, + ) + + robot.shutdown() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/dsr_practice/onrobot.py b/src/dsr_practice/dsr_practice/dsr_practice/onrobot.py new file mode 100644 index 0000000..22ae003 --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/onrobot.py @@ -0,0 +1,184 @@ +#!/usr/bin/env python3 + +from pymodbus.client.sync import ModbusTcpClient as ModbusClient + + +class RG(): + + def __init__(self, gripper, ip, port): + self.client = ModbusClient( + ip, + port=port, + stopbits=1, + bytesize=8, + parity='E', + baudrate=115200, + timeout=1) + if gripper not in ['rg2', 'rg6']: + print("Please specify either rg2 or rg6.") + return + self.gripper = gripper # RG2/6 + if self.gripper == 'rg2': + self.max_width = 1100 + self.max_force = 400 + elif self.gripper == 'rg6': + self.max_width = 1600 + self.max_force = 1200 + self.open_connection() + + def open_connection(self): + """Opens the connection with a gripper.""" + self.client.connect() + + def close_connection(self): + """Closes the connection with the gripper.""" + self.client.close() + + def get_fingertip_offset(self): + """Reads the current fingertip offset in 1/10 millimeters. + Please note that the value is a signed two's complement number. + """ + result = self.client.read_holding_registers( + address=258, count=1, unit=65) + offset_mm = result.registers[0] / 10.0 + return offset_mm + + def get_width(self): + """Reads current width between gripper fingers in 1/10 millimeters. + Please note that the width is provided without any fingertip offset, + as it is measured between the insides of the aluminum fingers. + """ + result = self.client.read_holding_registers( + address=267, count=1, unit=65) + width_mm = result.registers[0] / 10.0 + return width_mm + + def get_status(self): + """Reads current device status. + This status field indicates the status of the gripper and its motion. + It is composed of 7 flags, described in the table below. + + Bit Name Description + 0 (LSB): busy High (1) when a motion is ongoing, + low (0) when not. + The gripper will only accept new commands + when this flag is low. + 1: grip detected High (1) when an internal- or + external grip is detected. + 2: S1 pushed High (1) when safety switch 1 is pushed. + 3: S1 trigged High (1) when safety circuit 1 is activated. + The gripper will not move + while this flag is high; + can only be reset by power cycling. + 4: S2 pushed High (1) when safety switch 2 is pushed. + 5: S2 trigged High (1) when safety circuit 2 is activated. + The gripper will not move + while this flag is high; + can only be reset by power cycling. + 6: safety error High (1) when on power on any of + the safety switch is pushed. + 10-16: reserved Not used. + """ + # address : register number + # count : number of registers to be read + # unit : slave device address + result = self.client.read_holding_registers( + address=268, count=1, unit=65) + status = format(result.registers[0], '016b') + status_list = [0] * 7 + if int(status[-1]): + print("A motion is ongoing so new commands are not accepted.") + status_list[0] = 1 + if int(status[-2]): + print("An internal- or external grip is detected.") + status_list[1] = 1 + if int(status[-3]): + print("Safety switch 1 is pushed.") + status_list[2] = 1 + if int(status[-4]): + print("Safety circuit 1 is activated so it will not move.") + status_list[3] = 1 + if int(status[-5]): + print("Safety switch 2 is pushed.") + status_list[4] = 1 + if int(status[-6]): + print("Safety circuit 2 is activated so it will not move.") + status_list[5] = 1 + if int(status[-7]): + print("Any of the safety switch is pushed.") + status_list[6] = 1 + + return status_list + + def get_width_with_offset(self): + """Reads current width between gripper fingers in 1/10 millimeters. + The set fingertip offset is considered. + """ + result = self.client.read_holding_registers( + address=275, count=1, unit=65) + width_mm = result.registers[0] / 10.0 + return width_mm + + def set_control_mode(self, command): + """The control field is used to start and stop gripper motion. + Only one option should be set at a time. + Please note that the gripper will not start a new motion + before the one currently being executed is done + (see busy flag in the Status field). + The valid flags are: + + 1 (0x0001): grip + Start the motion, with the target force and width. + Width is calculated without the fingertip offset. + Please note that the gripper will ignore this command + if the busy flag is set in the status field. + 8 (0x0008): stop + Stop the current motion. + 16 (0x0010): grip_w_offset + Same as grip, but width is calculated + with the set fingertip offset. + """ + result = self.client.write_register( + address=2, value=command, unit=65) + + def set_target_force(self, force_val): + """Writes the target force to be reached + when gripping and holding a workpiece. + It must be provided in 1/10th Newtons. + The valid range is 0 to 400 for the RG2 and 0 to 1200 for the RG6. + """ + result = self.client.write_register( + address=0, value=force_val, unit=65) + + def set_target_width(self, width_val): + """Writes the target width between + the finger to be moved to and maintained. + It must be provided in 1/10th millimeters. + The valid range is 0 to 1100 for the RG2 and 0 to 1600 for the RG6. + Please note that the target width should be provided + corrected for any fingertip offset, + as it is measured between the insides of the aluminum fingers. + """ + result = self.client.write_register( + address=1, value=width_val, unit=65) + + def close_gripper(self, force_val=400): + """Closes gripper.""" + params = [force_val, 0, 16] + print("Start closing gripper.") + result = self.client.write_registers( + address=0, values=params, unit=65) + + def open_gripper(self, force_val=400): + """Opens gripper.""" + params = [force_val, self.max_width, 16] + print("Start opening gripper.") + result = self.client.write_registers( + address=0, values=params, unit=65) + + def move_gripper(self, width_val, force_val=400): + """Moves gripper to the specified width.""" + params = [force_val, width_val, 16] + print("Start moving gripper.") + result = self.client.write_registers( + address=0, values=params, unit=65) \ No newline at end of file diff --git a/src/dsr_practice/dsr_practice/dsr_practice/pick_and_place.py b/src/dsr_practice/dsr_practice/dsr_practice/pick_and_place.py new file mode 100644 index 0000000..a32bf07 --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/pick_and_place.py @@ -0,0 +1,305 @@ +#!/usr/bin/env python3 + +import math +import time + +import rclpy +from rclpy.logging import get_logger + +from geometry_msgs.msg import PoseStamped +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters + +# ====== OnRobot RG2 설정 ====== +from .onrobot import RG # 같은 패키지 내부의 onrobot.py 사용 + +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 + +# 그리퍼 폭 (raw 단위: 1/10 mm) +GRIPPER_OPEN_WIDTH = 500 # 50.0 mm +GRIPPER_CLOSE_WIDTH = 150 # 20.0 mm +GRIPPER_FORCE = 300 # 약 20 N + +# ================================ +# 기본 설정 +# ================================ +GROUP_NAME = "manipulator" # SRDF에 정의된 planning group 이름 +BASE_FRAME = "base_link" # 로봇 베이스 프레임 +EE_LINK = "link_6" # 엔드이펙터 링크 이름 (SRDF/URDF 기준) + +HOME_JOINTS_DEG = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0] +HOME_JOINTS_RAD = [math.radians(d) for d in HOME_JOINTS_DEG] + +# ====== 안전 작업 영역 정의 (base_link 기준) ====== +SAFE_X_MIN = 0.0 # x는 0 이상 +SAFE_Y_MIN = -0.3 # y 하한 +SAFE_Y_MAX = 0.3 # y 상한 +SAFE_Z_MIN = 0.27 # z는 이 값보다 낮아지면 안 됨 +# ================================================== + +# ====== 기어 Pick/Place 포즈 (base_link 기준) ====== +GEAR_TASK = { + "pick": { + "pos": {"x": 0.427, "y": 0.148, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + "place": { + "pos": {"x": 0.426, "y": -0.153, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, +} + +APPROACH_OFFSET = 0.05 # 위에서 접근/후퇴할 거리 [m] + + +def clamp_to_safe_workspace(x: float, y: float, z: float, logger): + """안전 작업 영역으로 (x, y, z) 클램핑""" + safe_x = x + safe_y = y + safe_z = z + + if safe_x < SAFE_X_MIN: + logger.warning( + f"Requested x ({safe_x:.3f} m) is below safety limit " + f"({SAFE_X_MIN:.3f} m). Clamping to SAFE_X_MIN." + ) + safe_x = SAFE_X_MIN + + if safe_y < SAFE_Y_MIN: + logger.warning( + f"Requested y ({safe_y:.3f} m) is below safety limit " + f"({SAFE_Y_MIN:.3f} m). Clamping to SAFE_Y_MIN." + ) + safe_y = SAFE_Y_MIN + elif safe_y > SAFE_Y_MAX: + logger.warning( + f"Requested y ({safe_y:.3f} m) is above safety limit " + f"({SAFE_Y_MAX:.3f} m). Clamping to SAFE_Y_MAX." + ) + safe_y = SAFE_Y_MAX + + if safe_z < SAFE_Z_MIN: + logger.warning( + f"Requested z ({safe_z:.3f} m) is below safety limit " + f"({SAFE_Z_MIN:.3f} m). Clamping to SAFE_Z_MIN." + ) + safe_z = SAFE_Z_MIN + + return safe_x, safe_y, safe_z + + +def plan_and_execute( + robot: MoveItPy, + planning_component, + logger, + pose_goal: PoseStamped = None, + plan_parameters=None, +): + """ + 공식 문서 스타일 helper: 계획 후 곧바로 실행 + + - pose_goal이 주어지면: + · 안전 영역 클램핑 + · start_state = current + · pose 기반 goal 설정 (EE_LINK) + - 그 다음 plan_parameters 유무에 따라 plan() 호출 후 execute + """ + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + + planning_component.set_start_state_to_current_state() + planning_component.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link=EE_LINK, + ) + + logger.info("Planning trajectory") + + if plan_parameters is not None: + plan_result = planning_component.plan( + parameters=plan_parameters + ) + else: + plan_result = planning_component.plan() + + if not plan_result: + logger.error("Planning failed") + return False + + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + robot.execute( + group_name=GROUP_NAME, + robot_trajectory=robot_trajectory, + blocking=True, + ) + logger.info("Execution finished") + return True + + +def main(args=None): + rclpy.init(args=args) + logger = get_logger("gear_pick_place_simple") + + # ---- Gripper ---- + gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + time.sleep(0.5) + gripper.move_gripper(GRIPPER_OPEN_WIDTH, GRIPPER_FORCE) + + # ---- MoveIt ---- + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + robot_model = robot.get_robot_model() + + # ---- PlanRequestParameters (HOME / Pilz) ---- + home_params = PlanRequestParameters(robot) + home_params.planning_pipeline = "ompl" + home_params.planner_id = "RRTConnectkConfigDefault" + home_params.max_velocity_scaling_factor = 0.2 + home_params.max_acceleration_scaling_factor = 0.1 + home_params.planning_time = 2.0 + + pilz_params = PlanRequestParameters(robot) + pilz_params.planning_pipeline = "pilz_industrial_motion_planner" + pilz_params.planner_id = "PTP" + pilz_params.max_velocity_scaling_factor = 0.15 + pilz_params.max_acceleration_scaling_factor = 0.1 + pilz_params.planning_time = 2.0 + + # ---- HOME 자세로 이동 (joint goal) ---- + logger.info("=== HOME 자세로 이동 ===") + home_state = RobotState(robot_model) + home_state.set_joint_group_positions(GROUP_NAME, HOME_JOINTS_RAD) + home_state.update() + + arm.set_start_state_to_current_state() + arm.set_goal_state(robot_state=home_state) + plan_and_execute(robot, arm, logger, plan_parameters=home_params) + + logger.info("=== Gear Pick & Place 시작 ===") + + # ---- PoseStamped 공용 객체 준비 ---- + pose_goal = PoseStamped() + pose_goal.header.frame_id = BASE_FRAME + + pick = GEAR_TASK["pick"] + place = GEAR_TASK["place"] + + # ============================ + # PICK + # ============================ + pos = pick["pos"] + ori = pick["ori"] + + # 1) pick 위에서 접근 + pose_goal.pose.position.x = pos["x"] + pose_goal.pose.position.y = pos["y"] + pose_goal.pose.position.z = pos["z"] + APPROACH_OFFSET + + pose_goal.pose.orientation.x = ori["x"] + pose_goal.pose.orientation.y = ori["y"] + pose_goal.pose.orientation.z = ori["z"] + pose_goal.pose.orientation.w = ori["w"] + + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=pilz_params, + ) + + # 2) pick 높이까지 내려가기 + pose_goal.pose.position.z = pos["z"] + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=pilz_params, + ) + + # 3) 그리퍼 닫기 (집기) + logger.info("Gripper CLOSE – gear pick") + gripper.move_gripper(GRIPPER_CLOSE_WIDTH, GRIPPER_FORCE) + time.sleep(1.0) + + # 4) 다시 위로 + pose_goal.pose.position.z = pos["z"] + APPROACH_OFFSET + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=pilz_params, + ) + + # ============================ + # PLACE + # ============================ + pos = place["pos"] + ori = place["ori"] + + # 5) place 위에서 접근 + pose_goal.pose.position.x = pos["x"] + pose_goal.pose.position.y = pos["y"] + pose_goal.pose.position.z = pos["z"] + APPROACH_OFFSET + + pose_goal.pose.orientation.x = ori["x"] + pose_goal.pose.orientation.y = ori["y"] + pose_goal.pose.orientation.z = ori["z"] + pose_goal.pose.orientation.w = ori["w"] + + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=pilz_params, + ) + + # 6) place 높이까지 내려가기 + pose_goal.pose.position.z = pos["z"] + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=pilz_params, + ) + + # 7) 그리퍼 열기 (놓기) + logger.info("Gripper OPEN – gear place") + gripper.move_gripper(GRIPPER_OPEN_WIDTH, GRIPPER_FORCE) + time.sleep(1.0) + + # 8) 다시 위로 + pose_goal.pose.position.z = pos["z"] + APPROACH_OFFSET + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=pilz_params, + ) + + # 마지막으로 HOME으로 복귀 (선택) + arm.set_start_state_to_current_state() + arm.set_goal_state(robot_state=home_state) + plan_and_execute(robot, arm, logger, plan_parameters=home_params) + + logger.info("=== Gear Pick & Place 노드 종료 ===") + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/dsr_practice/realsense_data_collector.py b/src/dsr_practice/dsr_practice/dsr_practice/realsense_data_collector.py new file mode 100644 index 0000000..03e47ba --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/realsense_data_collector.py @@ -0,0 +1,213 @@ +import csv +import json +import os +from pathlib import Path + +import cv2 +import rclpy +from cv_bridge import CvBridge +from rclpy.node import Node +from sensor_msgs.msg import CameraInfo, Image + + +class RealSenseDataCollector(Node): + def __init__(self): + super().__init__("realsense_data_collector") + + self.declare_parameter("color_topic", "/camera/camera/color/image_raw") + self.declare_parameter( + "depth_topic", "/camera/camera/aligned_depth_to_color/image_raw" + ) + self.declare_parameter("camera_info_topic", "/camera/camera/color/camera_info") + self.declare_parameter( + "output_dir", "/home/ssu/ros2_ws/realsense_dataset/raw" + ) + self.declare_parameter("save_interval_sec", 1.0) + self.declare_parameter("max_frames", 0) + self.declare_parameter("save_depth", True) + self.declare_parameter("jpeg_quality", 95) + + self.color_topic = ( + self.get_parameter("color_topic").get_parameter_value().string_value + ) + self.depth_topic = ( + self.get_parameter("depth_topic").get_parameter_value().string_value + ) + self.camera_info_topic = ( + self.get_parameter("camera_info_topic").get_parameter_value().string_value + ) + self.output_dir = Path( + self.get_parameter("output_dir").get_parameter_value().string_value + ).expanduser() + self.save_interval_sec = ( + self.get_parameter("save_interval_sec").get_parameter_value().double_value + ) + self.max_frames = ( + self.get_parameter("max_frames").get_parameter_value().integer_value + ) + self.save_depth = ( + self.get_parameter("save_depth").get_parameter_value().bool_value + ) + self.jpeg_quality = ( + self.get_parameter("jpeg_quality").get_parameter_value().integer_value + ) + + self.bridge = CvBridge() + self.latest_color = None + self.latest_depth = None + self.latest_color_stamp = None + self.latest_depth_stamp = None + self.camera_info_saved = False + self.frame_index = 0 + + self.color_dir = self.output_dir / "color" + self.depth_dir = self.output_dir / "depth" + self.meta_dir = self.output_dir / "meta" + self.color_dir.mkdir(parents=True, exist_ok=True) + if self.save_depth: + self.depth_dir.mkdir(parents=True, exist_ok=True) + self.meta_dir.mkdir(parents=True, exist_ok=True) + + self.metadata_path = self.meta_dir / "frames.csv" + self._init_metadata_file() + + self.create_subscription(Image, self.color_topic, self.color_callback, 10) + if self.save_depth: + self.create_subscription(Image, self.depth_topic, self.depth_callback, 10) + self.create_subscription( + CameraInfo, self.camera_info_topic, self.camera_info_callback, 10 + ) + self.create_timer(self.save_interval_sec, self.save_latest_frames) + + self.get_logger().info(f"Saving RealSense data to {self.output_dir}") + self.get_logger().info(f"Color topic: {self.color_topic}") + if self.save_depth: + self.get_logger().info(f"Depth topic: {self.depth_topic}") + self.get_logger().info("Press Ctrl+C to stop collecting data.") + + def _init_metadata_file(self): + if self.metadata_path.exists(): + return + + with self.metadata_path.open("w", newline="") as csv_file: + writer = csv.writer(csv_file) + writer.writerow( + [ + "frame_index", + "color_file", + "depth_file", + "color_stamp_sec", + "color_stamp_nanosec", + "depth_stamp_sec", + "depth_stamp_nanosec", + ] + ) + + def color_callback(self, msg): + self.latest_color = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") + self.latest_color_stamp = msg.header.stamp + + def depth_callback(self, msg): + self.latest_depth = self.bridge.imgmsg_to_cv2( + msg, desired_encoding="passthrough" + ) + self.latest_depth_stamp = msg.header.stamp + + def camera_info_callback(self, msg): + if self.camera_info_saved: + return + + camera_info = { + "width": msg.width, + "height": msg.height, + "distortion_model": msg.distortion_model, + "d": list(msg.d), + "k": list(msg.k), + "r": list(msg.r), + "p": list(msg.p), + "intrinsics": { + "fx": msg.k[0], + "fy": msg.k[4], + "cx": msg.k[2], + "cy": msg.k[5], + }, + } + camera_info_path = self.meta_dir / "camera_info.json" + with camera_info_path.open("w") as json_file: + json.dump(camera_info, json_file, indent=2) + + self.camera_info_saved = True + self.get_logger().info(f"Saved camera info to {camera_info_path}") + + def save_latest_frames(self): + if self.latest_color is None: + self.get_logger().warn("Waiting for color image...") + return + + if self.save_depth and self.latest_depth is None: + self.get_logger().warn("Waiting for aligned depth image...") + return + + self.frame_index += 1 + base_name = f"frame_{self.frame_index:06d}" + color_file = f"{base_name}.jpg" + depth_file = f"{base_name}.png" if self.save_depth else "" + + color_path = self.color_dir / color_file + cv2.imwrite( + str(color_path), + self.latest_color, + [int(cv2.IMWRITE_JPEG_QUALITY), self.jpeg_quality], + ) + + if self.save_depth: + depth_path = self.depth_dir / depth_file + cv2.imwrite(str(depth_path), self.latest_depth) + + self._append_metadata(color_file, depth_file) + self.get_logger().info(f"Saved {base_name}") + + if self.max_frames > 0 and self.frame_index >= self.max_frames: + self.get_logger().info(f"Reached max_frames={self.max_frames}. Stopping.") + rclpy.shutdown() + + def _append_metadata(self, color_file, depth_file): + color_stamp_sec = self.latest_color_stamp.sec if self.latest_color_stamp else "" + color_stamp_nanosec = ( + self.latest_color_stamp.nanosec if self.latest_color_stamp else "" + ) + depth_stamp_sec = self.latest_depth_stamp.sec if self.latest_depth_stamp else "" + depth_stamp_nanosec = ( + self.latest_depth_stamp.nanosec if self.latest_depth_stamp else "" + ) + + with self.metadata_path.open("a", newline="") as csv_file: + writer = csv.writer(csv_file) + writer.writerow( + [ + self.frame_index, + color_file, + depth_file, + color_stamp_sec, + color_stamp_nanosec, + depth_stamp_sec, + depth_stamp_nanosec, + ] + ) + + +def main(args=None): + rclpy.init(args=args) + node = RealSenseDataCollector() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/dsr_practice/stt_node.py b/src/dsr_practice/dsr_practice/dsr_practice/stt_node.py new file mode 100644 index 0000000..81aefbf --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/stt_node.py @@ -0,0 +1,108 @@ +import rclpy +from rclpy.node import Node +from std_msgs.msg import String + +import speech_recognition as sr + + +class SttNode(Node): + + def __init__(self): + super().__init__("stt_node") + + self.declare_parameter("language", "ko-KR") + self.declare_parameter("device_index", -1) + self.declare_parameter("energy_threshold", 300.0) + self.declare_parameter("pause_threshold", 0.8) + self.declare_parameter("phrase_time_limit", 5.0) + self.declare_parameter("dynamic_energy", True) + self.declare_parameter("ambient_duration", 1.0) + + self._lang = self.get_parameter("language").get_parameter_value().string_value + self._device_idx = self.get_parameter("device_index").get_parameter_value().integer_value + energy_thresh = self.get_parameter("energy_threshold").get_parameter_value().double_value + pause_thresh = self.get_parameter("pause_threshold").get_parameter_value().double_value + self._phrase_lim = self.get_parameter("phrase_time_limit").get_parameter_value().double_value + dynamic_energy = self.get_parameter("dynamic_energy").get_parameter_value().bool_value + ambient_duration = self.get_parameter("ambient_duration").get_parameter_value().double_value + + self._pub = self.create_publisher(String, "/stt_result", 10) + + self._log_devices() + + self._recognizer = sr.Recognizer() + self._recognizer.energy_threshold = energy_thresh + self._recognizer.pause_threshold = pause_thresh + self._recognizer.dynamic_energy_threshold = dynamic_energy + + device = self._device_idx if self._device_idx >= 0 else None + try: + self._mic = sr.Microphone(device_index=device) + except Exception as e: + self.get_logger().error(f"마이크 열기 실패: {e}") + raise + + with self._mic as source: + self.get_logger().info(f"주변 소음 측정 중 ({ambient_duration:.1f}s) ...") + self._recognizer.adjust_for_ambient_noise(source, duration=ambient_duration) + self.get_logger().info( + f"energy_threshold={self._recognizer.energy_threshold:.1f}" + ) + + self._stop_listen = self._recognizer.listen_in_background( + self._mic, self._on_audio, phrase_time_limit=self._phrase_lim, + ) + + self.get_logger().info( + f"STT 준비 완료 언어={self._lang} device_index={self._device_idx} " + f"phrase_time_limit={self._phrase_lim:.1f}s" + ) + + def _log_devices(self): + self.get_logger().info("=== 마이크 장치 목록 ===") + for idx, name in enumerate(sr.Microphone.list_microphone_names()): + mark = " ◀" if idx == self._device_idx else "" + self.get_logger().info(f" [{idx}] {name}{mark}") + + def _on_audio(self, recognizer: sr.Recognizer, audio: sr.AudioData): + try: + text = recognizer.recognize_google(audio, language=self._lang) + except sr.UnknownValueError: + return + except sr.RequestError as e: + self.get_logger().warning(f"Google STT 요청 실패: {e}") + return + + text = (text or "").strip() + if not text: + return + + self.get_logger().info(f"[STT] {text}") + msg = String() + msg.data = text + self._pub.publish(msg) + + def destroy_node(self): + stop = getattr(self, "_stop_listen", None) + if stop is not None: + try: + stop(wait_for_stop=False) + except Exception: + pass + super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + node = SttNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/dsr_practice/stt_pick_and_place.py b/src/dsr_practice/dsr_practice/dsr_practice/stt_pick_and_place.py new file mode 100644 index 0000000..fe107e6 --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/stt_pick_and_place.py @@ -0,0 +1,387 @@ +#!/usr/bin/env python3 +""" +STT 기반 Pick & Place 제어 노드 + +/stt_result (std_msgs/String) 구독 + -> 키워드 매핑 -> 명령 큐 -> 워커 스레드에서 MoveItPy + Gripper 실행 +""" +import math +import os +import queue +import tempfile +import threading +import time + +import rclpy +from rclpy.executors import MultiThreadedExecutor +from rclpy.logging import get_logger +from rclpy.node import Node +from std_msgs.msg import String + +from geometry_msgs.msg import PoseStamped +from moveit.planning import MoveItPy, PlanRequestParameters +from moveit_msgs.msg import Constraints, JointConstraint + +try: + from gtts import gTTS + import pygame + _TTS_OK = True +except ImportError: + _TTS_OK = False + +from .onrobot import RG + +# ----- Gripper ----- +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 +GRIPPER_OPEN_WIDTH = 500 +GRIPPER_CLOSE_WIDTH = 150 +GRIPPER_FORCE = 300 + +# ----- MoveIt / Robot ----- +GROUP_NAME = "manipulator" +BASE_FRAME = "base_link" +EE_LINK = "link_6" + +HOME_JOINTS_RAD = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(0.0), +} + +# ----- Safety bounds ----- +SAFE_X_MIN = 0.0 +SAFE_Y_MIN = -0.3 +SAFE_Y_MAX = 0.3 +SAFE_Z_MIN = 0.27 + +# ----- Task poses ----- +TASK_POSES = { + "pick": { + "pos": {"x": 0.427, "y": 0.148, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + "place": { + "pos": {"x": 0.426, "y": -0.153, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, +} +APPROACH_OFFSET = 0.05 + + +TTS_PHRASES = { + "home": "홈 위치로 이동합니다", + "pick": "물체를 집습니다", + "place": "물체를 내려놓습니다", + "pickplace": "픽 앤 플레이스를 시작합니다", + "stop": "정지합니다", + "done": "완료되었습니다", + "fail": "실행에 실패했습니다", +} + + +class _TtsPlayer: + """gTTS + pygame 기반 비동기 음성 재생 (캐시 사용).""" + + def __init__(self, lang: str = "ko", enabled: bool = True): + self._enabled = enabled and _TTS_OK + self._lang = lang + self._cache: dict[str, str] = {} + if self._enabled: + try: + pygame.mixer.init() + except Exception: + self._enabled = False + + def speak(self, text: str): + if not self._enabled or not text: + return + try: + path = self._cache.get(text) + if not path or not os.path.exists(path): + fd, path = tempfile.mkstemp(suffix=".mp3", prefix="dsr_tts_") + os.close(fd) + gTTS(text=text, lang=self._lang).save(path) + self._cache[text] = path + pygame.mixer.music.load(path) + pygame.mixer.music.play() + except Exception: + pass + + +KEYWORD_MAP: dict[str, str] = { + "홈": "home", + "home": "home", + "홈으로": "home", + "픽": "pick", + "집어": "pick", + "잡아": "pick", + "pick": "pick", + "플레이스": "place", + "놓아": "place", + "내려놔": "place", + "place": "place", + "픽앤플레이스": "pickplace", + "픽플레이스": "pickplace", + "pickandplace": "pickplace", + "pickplace": "pickplace", + "정지": "stop", + "멈춰": "stop", + "스톱": "stop", + "stop": "stop", +} + + +def _text_to_cmd(text: str) -> str | None: + normalized = text.lower().replace(" ", "") + for kw, cmd in KEYWORD_MAP.items(): + if kw in normalized: + return cmd + return None + + +def _clamp_to_safe_workspace(x: float, y: float, z: float, logger): + sx, sy, sz = x, y, z + + if sx < SAFE_X_MIN: + logger.warning(f"x safety clamp: {sx:.3f} -> {SAFE_X_MIN:.3f}") + sx = SAFE_X_MIN + if sy < SAFE_Y_MIN: + logger.warning(f"y safety clamp: {sy:.3f} -> {SAFE_Y_MIN:.3f}") + sy = SAFE_Y_MIN + elif sy > SAFE_Y_MAX: + logger.warning(f"y safety clamp: {sy:.3f} -> {SAFE_Y_MAX:.3f}") + sy = SAFE_Y_MAX + if sz < SAFE_Z_MIN: + logger.warning(f"z safety clamp: {sz:.3f} -> {SAFE_Z_MIN:.3f}") + sz = SAFE_Z_MIN + + return sx, sy, sz + + +def _build_home_joint_constraints() -> list[Constraints]: + constraints = Constraints() + joint_names = [f"joint_{i}" for i in range(1, 7)] + + for joint_name, position in zip(joint_names, HOME_JOINTS_RAD.values()): + joint_constraint = JointConstraint() + joint_constraint.joint_name = joint_name + joint_constraint.position = position + joint_constraint.tolerance_above = 0.001 + joint_constraint.tolerance_below = 0.001 + joint_constraint.weight = 1.0 + constraints.joint_constraints.append(joint_constraint) + + return [constraints] + + +def _plan_and_execute(robot, arm, logger, plan_params=None) -> bool: + result = arm.plan(parameters=plan_params) if plan_params else arm.plan() + if not result: + logger.error("Planning failed") + return False + robot.execute(group_name=GROUP_NAME, robot_trajectory=result.trajectory, blocking=True) + return True + + +def _move_home(robot, arm, logger, home_params) -> bool: + arm.set_start_state_to_current_state() + arm.set_goal_state(motion_plan_constraints=_build_home_joint_constraints()) + return _plan_and_execute(robot, arm, logger, plan_params=home_params) + + +def _move_pose(robot, arm, logger, pose_goal: PoseStamped, pilz_params) -> bool: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + sx, sy, sz = _clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + + arm.set_start_state_to_current_state() + arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link=EE_LINK) + return _plan_and_execute(robot, arm, logger, plan_params=pilz_params) + + +class SttPickAndPlaceNode(Node): + def __init__(self, robot: MoveItPy, arm, home_params, pilz_params, gripper): + super().__init__("stt_pick_and_place") + + self.declare_parameter("use_tts", True) + use_tts = self.get_parameter("use_tts").get_parameter_value().bool_value + + self._robot = robot + self._arm = arm + self._home = home_params + self._pilz = pilz_params + self._cmd_q: queue.Queue[str] = queue.Queue() + self._holding = False + self._tts = _TtsPlayer(enabled=use_tts) + self._gripper = gripper + + self.create_subscription(String, "/stt_result", self._stt_cb, 10) + threading.Thread(target=self._worker, daemon=True).start() + self.get_logger().info( + f"준비 완료 명령어: home / pick / place / pickplace / stop (tts={'on' if self._tts._enabled else 'off'})" + ) + + def _stt_cb(self, msg: String): + cmd = _text_to_cmd(msg.data) + if cmd is None: + self.get_logger().debug(f"매핑 없음: '{msg.data}'") + return + + if cmd == "stop": + n = 0 + while not self._cmd_q.empty(): + try: + self._cmd_q.get_nowait() + n += 1 + except queue.Empty: + break + self.get_logger().info(f"[STOP] 큐 {n}개 취소") + self._tts.speak(TTS_PHRASES["stop"]) + return + + self._cmd_q.put(cmd) + self.get_logger().info(f"[CMD] '{cmd}' 큐 추가 (크기={self._cmd_q.qsize()})") + + def _build_pose_goal(self, task_key: str, z_value: float) -> PoseStamped: + task = TASK_POSES[task_key] + pose_goal = PoseStamped() + pose_goal.header.frame_id = BASE_FRAME + pose_goal.pose.position.x = task["pos"]["x"] + pose_goal.pose.position.y = task["pos"]["y"] + pose_goal.pose.position.z = z_value + pose_goal.pose.orientation.x = task["ori"]["x"] + pose_goal.pose.orientation.y = task["ori"]["y"] + pose_goal.pose.orientation.z = task["ori"]["z"] + pose_goal.pose.orientation.w = task["ori"]["w"] + return pose_goal + + def _set_gripper(self, logger, width: int): + try: + self._gripper.move_gripper(width, GRIPPER_FORCE) + time.sleep(1.0) + except Exception as e: + logger.error(f"Gripper error: {e}") + + def _run_pick(self, logger) -> bool: + pick = TASK_POSES["pick"]["pos"] + approach = self._build_pose_goal("pick", pick["z"] + APPROACH_OFFSET) + target = self._build_pose_goal("pick", pick["z"]) + + if not _move_pose(self._robot, self._arm, logger, approach, self._pilz): + return False + if not _move_pose(self._robot, self._arm, logger, target, self._pilz): + return False + + logger.info("Gripper CLOSE") + self._set_gripper(logger, GRIPPER_CLOSE_WIDTH) + self._holding = True + + if not _move_pose(self._robot, self._arm, logger, approach, self._pilz): + return False + return True + + def _run_place(self, logger) -> bool: + place = TASK_POSES["place"]["pos"] + approach = self._build_pose_goal("place", place["z"] + APPROACH_OFFSET) + target = self._build_pose_goal("place", place["z"]) + + if not _move_pose(self._robot, self._arm, logger, approach, self._pilz): + return False + if not _move_pose(self._robot, self._arm, logger, target, self._pilz): + return False + + logger.info("Gripper OPEN") + self._set_gripper(logger, GRIPPER_OPEN_WIDTH) + self._holding = False + + if not _move_pose(self._robot, self._arm, logger, approach, self._pilz): + return False + return True + + def _worker(self): + logger = get_logger("stt_pick_and_place.worker") + while True: + try: + cmd = self._cmd_q.get(timeout=1.0) + except queue.Empty: + continue + + logger.info(f"===== '{cmd}' 실행 =====") + self._tts.speak(TTS_PHRASES.get(cmd, "")) + ok = True + + if cmd == "home": + ok = _move_home(self._robot, self._arm, logger, self._home) + elif cmd == "pick": + ok = self._run_pick(logger) + elif cmd == "place": + if not self._holding: + logger.warning("현재 물체를 쥐고 있지 않습니다. place를 계속 진행합니다.") + ok = self._run_place(logger) + elif cmd == "pickplace": + ok = self._run_pick(logger) + if ok: + ok = self._run_place(logger) + + if ok: + logger.info(f"===== '{cmd}' 완료 =====") + else: + logger.error(f"===== '{cmd}' 실패 =====") + self._tts.speak(TTS_PHRASES["done" if ok else "fail"]) + + +def main(args=None): + rclpy.init(args=args) + logger = get_logger("stt_pick_and_place.main") + + gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + time.sleep(0.5) + gripper.move_gripper(GRIPPER_OPEN_WIDTH, GRIPPER_FORCE) + + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + logger.info("MoveItPy 초기화 완료") + + home = PlanRequestParameters(robot) + home.planning_pipeline = "ompl" + home.planner_id = "RRTConnect" + home.max_velocity_scaling_factor = 0.2 + home.max_acceleration_scaling_factor = 0.1 + home.planning_time = 3.0 + + pilz = PlanRequestParameters(robot) + pilz.planning_pipeline = "pilz_industrial_motion_planner" + pilz.planner_id = "PTP" + pilz.max_velocity_scaling_factor = 0.15 + pilz.max_acceleration_scaling_factor = 0.1 + pilz.planning_time = 3.0 + + logger.info("시작 자세를 home으로 이동합니다") + _move_home(robot, arm, logger, home) + + node = SttPickAndPlaceNode(robot, arm, home, pilz, gripper) + executor = MultiThreadedExecutor() + executor.add_node(node) + + logger.info("음성 명령 대기 중 ... (Ctrl+C 종료)") + try: + executor.spin() + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + robot.shutdown() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/dsr_practice/stt_robot_control.py b/src/dsr_practice/dsr_practice/dsr_practice/stt_robot_control.py new file mode 100644 index 0000000..2888275 --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/stt_robot_control.py @@ -0,0 +1,376 @@ +#!/usr/bin/env python3 +""" +STT 음성 명령 로봇 제어 노드 (오프셋 이동 방식) + +/stt_result (std_msgs/String) 구독 + → 키워드 매핑 → 명령 큐 → 워커 스레드에서 MoveItPy 실행 + +명령어: + home → HOME 조인트 자세 (OMPL RRTConnect) + 앞/뒤/왼쪽/오른쪽/위/아래 → 현재 EE 위치 기준 5cm 오프셋 이동 (Pilz PTP) + stop → 명령 큐 비우기 +""" +import math +import os +import queue +import tempfile +import threading + +import numpy as np +import rclpy +from rclpy.executors import MultiThreadedExecutor +from rclpy.logging import get_logger +from rclpy.node import Node +from std_msgs.msg import String + +from geometry_msgs.msg import PoseStamped +from moveit.planning import MoveItPy, PlanRequestParameters +from moveit_msgs.msg import Constraints, JointConstraint + +try: + from gtts import gTTS + import pygame + _TTS_OK = True +except ImportError: + _TTS_OK = False + +# ── 로봇 설정 ────────────────────────────────────────────── +GROUP_NAME = "manipulator" +BASE_FRAME = "base_link" +EE_LINK = "link_6" +HOME_JOINTS_RAD = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(0.0), +} + +# ── 안전 작업 영역 (base_link 기준) ─────────────────────── +SAFE_X_MIN = 0.0 +SAFE_Y_MIN = -0.3 +SAFE_Y_MAX = 0.3 +SAFE_Z_MIN = 0.27 + +# ── 방향 → (dx, dy, dz) 오프셋 ─────────────────────────── +JOG_OFFSET = 0.05 # m +DIRECTIONS = { + "forward": ( JOG_OFFSET, 0.0, 0.0), + "backward": (-JOG_OFFSET, 0.0, 0.0), + "left": ( 0.0, JOG_OFFSET, 0.0), + "right": ( 0.0, -JOG_OFFSET, 0.0), + "up": ( 0.0, 0.0, JOG_OFFSET), + "down": ( 0.0, 0.0, -JOG_OFFSET), +} + +# ── 명령별 음성 안내 ────────────────────────────────────── +TTS_PHRASES = { + "home": "홈 위치로 이동합니다", + "forward": "앞으로 이동합니다", + "backward": "뒤로 이동합니다", + "left": "왼쪽으로 이동합니다", + "right": "오른쪽으로 이동합니다", + "up": "위로 이동합니다", + "down": "아래로 이동합니다", + "stop": "정지합니다", + "done": "완료되었습니다", + "fail": "실행에 실패했습니다", +} + +# ── 키워드 → 명령 매핑 ──────────────────────────────────── +KEYWORD_MAP: dict[str, str] = { + "홈": "home", "home": "home", "홈으로": "home", + "왼쪽": "left", "왼": "left", "left": "left", + "오른쪽": "right", "오른": "right", "right": "right", + "앞": "forward", "앞으로": "forward", "전방": "forward", + "front": "forward", "forward": "forward", + "뒤": "backward", "뒤로": "backward", "후방": "backward", + "back": "backward", "backward": "backward", + "위": "up", "위로": "up", "올려": "up", "올라가": "up", "up": "up", + "아래": "down", "아래로": "down", "내려": "down", "내려가": "down", "down": "down", + "정지": "stop", "멈춰": "stop", "스톱": "stop", "stop": "stop", +} + +VALID_CMDS = {"home"} | set(DIRECTIONS.keys()) + + +class _TtsPlayer: + """gTTS + pygame 기반 비동기 음성 재생 (캐시 사용).""" + + def __init__(self, lang: str = "ko", enabled: bool = True): + self._enabled = enabled and _TTS_OK + self._lang = lang + self._cache: dict[str, str] = {} + if self._enabled: + try: + pygame.mixer.init() + except Exception: + self._enabled = False + + def speak(self, text: str): + if not self._enabled or not text: + return + try: + path = self._cache.get(text) + if not path or not os.path.exists(path): + fd, path = tempfile.mkstemp(suffix=".mp3", prefix="dsr_tts_") + os.close(fd) + gTTS(text=text, lang=self._lang).save(path) + self._cache[text] = path + pygame.mixer.music.load(path) + pygame.mixer.music.play() + except Exception: + pass + + +def _text_to_cmd(text: str) -> str | None: + normalized = text.lower().replace(" ", "") + for kw, cmd in KEYWORD_MAP.items(): + if kw in normalized: + return cmd + return None + + +def _clamp_to_safe(x: float, y: float, z: float, logger) -> tuple[float, float, float]: + if x < SAFE_X_MIN: + logger.warning(f"x 클램핑: {x:.3f} → {SAFE_X_MIN:.3f}") + x = SAFE_X_MIN + if y < SAFE_Y_MIN: + logger.warning(f"y 클램핑: {y:.3f} → {SAFE_Y_MIN:.3f}") + y = SAFE_Y_MIN + elif y > SAFE_Y_MAX: + logger.warning(f"y 클램핑: {y:.3f} → {SAFE_Y_MAX:.3f}") + y = SAFE_Y_MAX + if z < SAFE_Z_MIN: + logger.warning(f"z 클램핑: {z:.3f} → {SAFE_Z_MIN:.3f}") + z = SAFE_Z_MIN + return x, y, z + + +def _rot_matrix_to_quat(R: np.ndarray) -> tuple[float, float, float, float]: + """3x3 회전행렬 → (x, y, z, w) 쿼터니언.""" + trace = R[0, 0] + R[1, 1] + R[2, 2] + if trace > 0: + s = 0.5 / math.sqrt(trace + 1.0) + w = 0.25 / s + x = (R[2, 1] - R[1, 2]) * s + y = (R[0, 2] - R[2, 0]) * s + z = (R[1, 0] - R[0, 1]) * s + elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]: + s = 2.0 * math.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) + w = (R[2, 1] - R[1, 2]) / s + x = 0.25 * s + y = (R[0, 1] + R[1, 0]) / s + z = (R[0, 2] + R[2, 0]) / s + elif R[1, 1] > R[2, 2]: + s = 2.0 * math.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) + w = (R[0, 2] - R[2, 0]) / s + x = (R[0, 1] + R[1, 0]) / s + y = 0.25 * s + z = (R[1, 2] + R[2, 1]) / s + else: + s = 2.0 * math.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) + w = (R[1, 0] - R[0, 1]) / s + x = (R[0, 2] + R[2, 0]) / s + y = (R[1, 2] + R[2, 1]) / s + z = 0.25 * s + return x, y, z, w + + +def _plan_and_execute(robot, arm, logger, plan_params=None) -> bool: + logger.info("Planning ...") + result = arm.plan(parameters=plan_params) if plan_params else arm.plan() + if not result: + logger.error("Planning failed") + return False + logger.info("Executing ...") + robot.execute(group_name=GROUP_NAME, robot_trajectory=result.trajectory, blocking=True) + logger.info("Done") + return True + + +def _build_home_joint_constraints() -> list[Constraints]: + constraints = Constraints() + joint_names = [f"joint_{i}" for i in range(1, 7)] + + for joint_name, position in zip(joint_names, HOME_JOINTS_RAD.values()): + joint_constraint = JointConstraint() + joint_constraint.joint_name = joint_name + joint_constraint.position = position + joint_constraint.tolerance_above = 0.001 + joint_constraint.tolerance_below = 0.001 + joint_constraint.weight = 1.0 + constraints.joint_constraints.append(joint_constraint) + + return [constraints] + + +def _move_home(robot, arm, logger, plan_params=None) -> bool: + arm.set_start_state_to_current_state() + arm.set_goal_state(motion_plan_constraints=_build_home_joint_constraints()) + return _plan_and_execute(robot, arm, logger, plan_params=plan_params) + + +# ══════════════════════════════════════════════════════════ +class SttRobotControlNode(Node): + + def __init__( + self, + robot: MoveItPy, + arm, + home_params: PlanRequestParameters, + pilz_params: PlanRequestParameters, + ): + super().__init__("stt_robot_control") + + self.declare_parameter("use_tts", True) + use_tts = self.get_parameter("use_tts").get_parameter_value().bool_value + + self._robot = robot + self._arm = arm + self._home = home_params + self._pilz = pilz_params + self._cmd_q: queue.Queue[str] = queue.Queue() + self._tts = _TtsPlayer(enabled=use_tts) + + threading.Thread(target=self._worker, daemon=True).start() + + self.create_subscription(String, "/stt_result", self._stt_cb, 10) + self.get_logger().info( + f"준비 완료 명령어: home / {' / '.join(DIRECTIONS)} / stop " + f"(jog={JOG_OFFSET*100:.0f}cm, tts={'on' if self._tts._enabled else 'off'})" + ) + + # ── ROS 콜백 ─────────────────────────────────────────── + def _stt_cb(self, msg: String): + cmd = _text_to_cmd(msg.data) + if cmd is None: + self.get_logger().debug(f"매핑 없음: '{msg.data}'") + return + + if cmd == "stop": + n = 0 + while not self._cmd_q.empty(): + try: + self._cmd_q.get_nowait() + n += 1 + except queue.Empty: + break + self.get_logger().info(f"[STOP] 큐 {n}개 취소") + self._tts.speak(TTS_PHRASES["stop"]) + else: + self._cmd_q.put(cmd) + self.get_logger().info(f"[CMD] '{cmd}' 큐 추가 (크기={self._cmd_q.qsize()})") + + # ── 워커 스레드 ──────────────────────────────────────── + def _worker(self): + logger = get_logger("stt_robot_control.worker") + + while True: + try: + cmd = self._cmd_q.get(timeout=1.0) + except queue.Empty: + continue + + logger.info(f"===== '{cmd}' 실행 =====") + self._tts.speak(TTS_PHRASES.get(cmd, "")) + ok = True + + if cmd == "home": + ok = _move_home( + self._robot, + self._arm, + logger, + plan_params=self._home, + ) + + elif cmd in DIRECTIONS: + ok = self._move_offset(logger, cmd) + + logger.info(f"===== '{cmd}' 완료 =====") + self._tts.speak(TTS_PHRASES["done" if ok else "fail"]) + + # ── 현재 EE 위치 기준 오프셋 이동 ────────────────────── + def _move_offset(self, logger, direction: str) -> bool: + try: + with self._robot.get_planning_scene_monitor().read_only() as scene: + state = scene.current_state + state.update() + tf = state.get_frame_transform(EE_LINK) + except Exception as e: + logger.error(f"현재 EE 상태 조회 실패: {e}") + return False + + cx = float(tf[0, 3]) + cy = float(tf[1, 3]) + cz = float(tf[2, 3]) + qx, qy, qz, qw = _rot_matrix_to_quat(tf[:3, :3]) + + dx, dy, dz = DIRECTIONS[direction] + tx, ty, tz = _clamp_to_safe(cx + dx, cy + dy, cz + dz, logger) + + logger.info( + f"JOG dir={direction} offset={JOG_OFFSET*100:.1f}cm " + f"({cx:.3f},{cy:.3f},{cz:.3f}) → ({tx:.3f},{ty:.3f},{tz:.3f})" + ) + + ps = PoseStamped() + ps.header.frame_id = BASE_FRAME + ps.pose.position.x = tx + ps.pose.position.y = ty + ps.pose.position.z = tz + ps.pose.orientation.x = qx + ps.pose.orientation.y = qy + ps.pose.orientation.z = qz + ps.pose.orientation.w = qw + + self._arm.set_start_state_to_current_state() + self._arm.set_goal_state(pose_stamped_msg=ps, pose_link=EE_LINK) + return _plan_and_execute(self._robot, self._arm, logger, plan_params=self._pilz) + + +# ══════════════════════════════════════════════════════════ +def main(args=None): + rclpy.init(args=args) + logger = get_logger("stt_robot_control.main") + + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + logger.info("MoveItPy 초기화 완료") + + home = PlanRequestParameters(robot) + home.planning_pipeline = "ompl" + home.planner_id = "RRTConnect" + home.max_velocity_scaling_factor = 0.2 + home.max_acceleration_scaling_factor = 0.1 + home.planning_time = 3.0 + + pilz = PlanRequestParameters(robot) + pilz.planning_pipeline = "pilz_industrial_motion_planner" + pilz.planner_id = "PTP" + pilz.max_velocity_scaling_factor = 0.2 + pilz.max_acceleration_scaling_factor = 0.1 + pilz.planning_time = 3.0 + + logger.info("시작 자세를 home으로 이동합니다") + _move_home(robot, arm, logger, plan_params=home) + + node = SttRobotControlNode(robot, arm, home, pilz) + + executor = MultiThreadedExecutor() + executor.add_node(node) + + logger.info("음성 명령 대기 중 ... (Ctrl+C 종료)") + try: + executor.spin() + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + robot.shutdown() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/dsr_practice/syrup_pump_press.py b/src/dsr_practice/dsr_practice/dsr_practice/syrup_pump_press.py new file mode 100644 index 0000000..67176c6 --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/syrup_pump_press.py @@ -0,0 +1,227 @@ +#!/usr/bin/env python3 + +import math +import time + +import rclpy +from geometry_msgs.msg import PoseStamped +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters +from rclpy.node import Node + +from .onrobot import RG + + +GROUP_NAME = "manipulator" +BASE_FRAME = "base_link" +EE_LINK = "link_6" + +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 +GRIPPER_CLOSE_WIDTH = 0 +GRIPPER_FORCE = 200 + +HOME_JOINTS_DEG = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0] +HOME_JOINTS_RAD = [math.radians(d) for d in HOME_JOINTS_DEG] + +DOWN_ORI = { + "x": 0.0, + "y": 1.0, + "z": 0.0, + "w": 0.0, +} + + +class SyrupPumpPressNode(Node): + def __init__(self): + super().__init__("syrup_pump_press") + + self.declare_parameter("pump_x", 0.45) + self.declare_parameter("pump_y", 0.0) + self.declare_parameter("start_z", 0.50) + self.declare_parameter("pump_top_z", 0.35) + self.declare_parameter("press_depth", 0.05) + self.declare_parameter("hold_sec", 0.5) + self.declare_parameter("go_home_first", True) + self.declare_parameter("return_home", False) + + self.pump_x = self.get_parameter("pump_x").value + self.pump_y = self.get_parameter("pump_y").value + self.start_z = self.get_parameter("start_z").value + self.pump_top_z = self.get_parameter("pump_top_z").value + self.press_depth = self.get_parameter("press_depth").value + self.hold_sec = self.get_parameter("hold_sec").value + self.go_home_first = self.get_parameter("go_home_first").value + self.return_home = self.get_parameter("return_home").value + + self.press_z = self.pump_top_z - self.press_depth + + self.robot = MoveItPy(node_name="syrup_pump_press_moveit_py") + self.arm = self.robot.get_planning_component(GROUP_NAME) + self.robot_model = self.robot.get_robot_model() + + self.home_params = PlanRequestParameters(self.robot) + self.home_params.planning_pipeline = "ompl" + self.home_params.planner_id = "RRTConnectkConfigDefault" + self.home_params.max_velocity_scaling_factor = 0.2 + self.home_params.max_acceleration_scaling_factor = 0.1 + self.home_params.planning_time = 3.0 + + self.ptp_params = PlanRequestParameters(self.robot) + self.ptp_params.planning_pipeline = "pilz_industrial_motion_planner" + self.ptp_params.planner_id = "PTP" + self.ptp_params.max_velocity_scaling_factor = 0.15 + self.ptp_params.max_acceleration_scaling_factor = 0.1 + self.ptp_params.planning_time = 3.0 + + self.lin_params = PlanRequestParameters(self.robot) + self.lin_params.planning_pipeline = "pilz_industrial_motion_planner" + self.lin_params.planner_id = "LIN" + self.lin_params.max_velocity_scaling_factor = 0.08 + self.lin_params.max_acceleration_scaling_factor = 0.05 + self.lin_params.planning_time = 3.0 + + self.press_params = PlanRequestParameters(self.robot) + self.press_params.planning_pipeline = "pilz_industrial_motion_planner" + self.press_params.planner_id = "LIN" + self.press_params.max_velocity_scaling_factor = 0.02 + self.press_params.max_acceleration_scaling_factor = 0.02 + self.press_params.planning_time = 3.0 + + self.gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + time.sleep(0.5) + + def make_pose(self, x, y, z, ori=None): + if ori is None: + ori = DOWN_ORI + + pose = PoseStamped() + pose.header.frame_id = BASE_FRAME + pose.pose.position.x = float(x) + pose.pose.position.y = float(y) + pose.pose.position.z = float(z) + pose.pose.orientation.x = ori["x"] + pose.pose.orientation.y = ori["y"] + pose.pose.orientation.z = ori["z"] + pose.pose.orientation.w = ori["w"] + return pose + + def plan_and_execute(self, pose_goal=None, state_goal=None, params=None): + log = self.get_logger() + self.arm.set_start_state_to_current_state() + + if pose_goal is not None: + self.arm.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link=EE_LINK, + ) + elif state_goal is not None: + self.arm.set_goal_state(robot_state=state_goal) + else: + log.error("No pose/state goal was provided.") + return False + + plan_result = self.arm.plan(parameters=params) if params else self.arm.plan() + if not plan_result: + log.error("Planning failed.") + return False + + self.robot.execute( + group_name=GROUP_NAME, + robot_trajectory=plan_result.trajectory, + blocking=True, + ) + return True + + def move_home(self): + home_state = RobotState(self.robot_model) + home_state.set_joint_group_positions(GROUP_NAME, HOME_JOINTS_RAD) + home_state.update() + return self.plan_and_execute(state_goal=home_state, params=self.home_params) + + def run_task(self): + log = self.get_logger() + log.info("=== Syrup pump press task start ===") + log.info( + "Target pump pose: " + f"x={self.pump_x:.3f}, y={self.pump_y:.3f}, " + f"start_z={self.start_z:.3f}, pump_top_z={self.pump_top_z:.3f}, " + f"press_z={self.press_z:.3f}" + ) + + if self.press_z <= 0.0: + log.error("Invalid press_z. Check pump_top_z and press_depth.") + return False + + if self.go_home_first: + log.info("[0] Move HOME") + if not self.move_home(): + return False + + log.info("[1] Close gripper fully") + self.gripper.move_gripper( + width_val=GRIPPER_CLOSE_WIDTH, + force_val=GRIPPER_FORCE, + ) + time.sleep(1.0) + + log.info("[2] Move above syrup pump") + if not self.plan_and_execute( + pose_goal=self.make_pose(self.pump_x, self.pump_y, self.start_z), + params=self.ptp_params, + ): + return False + + log.info("[3] Move vertically down to pump top height") + if not self.plan_and_execute( + pose_goal=self.make_pose(self.pump_x, self.pump_y, self.pump_top_z), + params=self.lin_params, + ): + return False + + log.info("[4] Slowly press syrup pump by 5 cm") + if not self.plan_and_execute( + pose_goal=self.make_pose(self.pump_x, self.pump_y, self.press_z), + params=self.press_params, + ): + return False + + if self.hold_sec > 0.0: + log.info(f"[5] Hold for {self.hold_sec:.2f} sec") + time.sleep(self.hold_sec) + + log.info("[6] Retract vertically") + if not self.plan_and_execute( + pose_goal=self.make_pose(self.pump_x, self.pump_y, self.start_z), + params=self.lin_params, + ): + return False + + if self.return_home: + log.info("[7] Return HOME") + if not self.move_home(): + return False + + log.info("=== Syrup pump press task finished ===") + return True + + def destroy_node(self): + try: + self.gripper.close_connection() + finally: + super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + node = SyrupPumpPressNode() + try: + node.run_task() + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/dsr_practice/test.py b/src/dsr_practice/dsr_practice/dsr_practice/test.py new file mode 100644 index 0000000..36ccfac --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/test.py @@ -0,0 +1,20 @@ +import time +from onrobot import RG + +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = "502" + +gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + +#그리퍼 열기 +gripper.open_gripper() + +#그리퍼의 상태를 체크하고 동작 중이면 wait(동작이 끝날 때까지 기다리기) +while gripper.get_status()[0]: + time.sleep(0.5) + + #그리퍼 닫기 + gripper.close_gripper() +while gripper.get_status()[0]: + time.sleep(0.5) diff --git a/src/dsr_practice/dsr_practice/dsr_practice/test_2.py b/src/dsr_practice/dsr_practice/dsr_practice/test_2.py new file mode 100644 index 0000000..55c414f --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/test_2.py @@ -0,0 +1,15 @@ +from onrobot import RG +import time + +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = "502" + +gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) +gripper.move_gripper(width_val=400, force_val=10) +while gripper.get_status()[0]: + time.sleep(0.5) + + #그리퍼의 현재 너비 출력 + print(f'get_width_with_offset: {gripper.get_width_with_offset()}') + \ No newline at end of file diff --git a/src/dsr_practice/dsr_practice/dsr_practice/yolo_cup_pick_node.py b/src/dsr_practice/dsr_practice/dsr_practice/yolo_cup_pick_node.py new file mode 100644 index 0000000..5fa18e4 --- /dev/null +++ b/src/dsr_practice/dsr_practice/dsr_practice/yolo_cup_pick_node.py @@ -0,0 +1,1129 @@ +#!/usr/bin/env python3 + +import math +import time +from collections import Counter +from pathlib import Path + +import cv2 +import numpy as np +import rclpy +from ament_index_python.packages import get_package_share_directory +from cv_bridge import CvBridge +from geometry_msgs.msg import PoseStamped +from rcl_interfaces.msg import ParameterDescriptor +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters +from rclpy.node import Node +from scipy.spatial.transform import Rotation +from sensor_msgs.msg import CameraInfo, Image +from ultralytics import YOLO + +from .onrobot import RG + + +GROUP_NAME = "manipulator" +BASE_FRAME = "base_link" +EE_LINK = "link_6" + +HOME_JOINTS = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(90.0), +} +HOME_JOINTS_RAD = [ + math.radians(0.0), + math.radians(0.0), + math.radians(90.0), + math.radians(0.0), + math.radians(90.0), + math.radians(90.0), +] + +SAFE_X_MIN = 0.0 +SAFE_Y_MIN = -0.35 +SAFE_Y_MAX = 0.35 +SAFE_Z_MIN = 0.20 + +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 +GRIPPER_OPEN_WIDTH = 1100 +GRIPPER_CLOSE_WIDTH = 120 +GRIPPER_FORCE = 250 +GRIPPER_OPEN_TIMEOUT_SEC = 5.0 +GRIPPER_STATUS_POLL_SEC = 0.15 + +DOWN_ORI = {"x": 0.0, "y": 1.0, "z": 0.0, "w": 0.0} + + +def clamp_to_safe_workspace(x, y, z, logger, z_min=SAFE_Z_MIN): + if x < SAFE_X_MIN: + logger.warning(f"x={x:.3f} -> {SAFE_X_MIN:.3f}") + x = SAFE_X_MIN + if y < SAFE_Y_MIN: + logger.warning(f"y={y:.3f} -> {SAFE_Y_MIN:.3f}") + y = SAFE_Y_MIN + elif y > SAFE_Y_MAX: + logger.warning(f"y={y:.3f} -> {SAFE_Y_MAX:.3f}") + y = SAFE_Y_MAX + if z < z_min: + logger.warning(f"z={z:.3f} -> {z_min:.3f}") + z = z_min + return x, y, z + + +def make_pose(x, y, z, ori=None): + if ori is None: + ori = DOWN_ORI + pose = PoseStamped() + pose.header.frame_id = BASE_FRAME + pose.pose.position.x = float(x) + pose.pose.position.y = float(y) + pose.pose.position.z = float(z) + pose.pose.orientation.x = ori["x"] + pose.pose.orientation.y = ori["y"] + pose.pose.orientation.z = ori["z"] + pose.pose.orientation.w = ori["w"] + return pose + + +def parse_bool(value): + if isinstance(value, bool): + return value + if isinstance(value, (int, float)): + return bool(value) + return str(value).strip().lower() in {"1", "true", "yes", "on"} + + +def parse_axis(value): + if isinstance(value, bool): + return "y" if value else "x" + + normalized = str(value).strip().lower() + if normalized in {"y", "y_axis", "axis_y", "true", "yes", "on"}: + return "y" + if normalized in {"x", "x_axis", "axis_x"}: + return "x" + return normalized + + +def quat_dict_from_matrix(matrix): + qx, qy, qz, qw = Rotation.from_matrix(matrix).as_quat() + return { + "x": float(qx), + "y": float(qy), + "z": float(qz), + "w": float(qw), + } + + +def quat_dict_from_euler(roll_deg, pitch_deg, yaw_deg): + qx, qy, qz, qw = Rotation.from_euler( + "xyz", + [roll_deg, pitch_deg, yaw_deg], + degrees=True, + ).as_quat() + return { + "x": float(qx), + "y": float(qy), + "z": float(qz), + "w": float(qw), + } + + +def get_ee_matrix(moveit_robot): + psm = moveit_robot.get_planning_scene_monitor() + with psm.read_only() as scene: + transform = scene.current_state.get_global_link_transform(EE_LINK) + return np.asarray(transform, dtype=float) + + +class YoloCupPickNode(Node): + def __init__(self): + super().__init__("yolo_cup_pick_node") + + self.declare_parameter( + "model_path", + "/home/ssu/ros2_ws/yolo_runs/cup_yolov8n_ft1/weights/best.pt", + ) + self.declare_parameter("conf", 0.35) + self.declare_parameter("imgsz", 640) + self.declare_parameter("device", "cpu") + self.declare_parameter("target_class", "cup") + self.declare_parameter("auto_pick", False) + self.declare_parameter("auto_pick_interval", 3.0) + self.declare_parameter("pick_depth_ratio", 0.55) + self.declare_parameter("depth_patch_radius", 7) + self.declare_parameter("min_depth_valid_ratio", 0.03) + self.declare_parameter("min_depth_m", 0.15) + self.declare_parameter("max_depth_m", 1.20) + self.declare_parameter("redetect_on_approach", True) + self.declare_parameter("redetect_settle_sec", 0.5) + self.declare_parameter("grasp_mode", "side") + dynamic_param = ParameterDescriptor(dynamic_typing=True) + self.declare_parameter("side_grasp_axis", "y_axis", dynamic_param) + self.declare_parameter("side_grasp_direction", -1.0) + self.declare_parameter("side_approach_offset", 0.12) + self.declare_parameter("side_staging_offset", 0.24) + self.declare_parameter("side_grasp_offset", 0.035) + self.declare_parameter("side_grasp_z_offset", 0.05) + self.declare_parameter("side_orientation_mode", "approach") + self.declare_parameter("side_tool_roll_deg", 0.0) + self.declare_parameter("side_roll_deg", 0.0) + self.declare_parameter("side_pitch_deg", 90.0) + self.declare_parameter("side_yaw_deg", 0.0) + self.declare_parameter("pick_z_offset", 0.20) + self.declare_parameter("approach_offset", 0.12) + self.declare_parameter("safe_z", 0.50) + self.declare_parameter("min_motion_z", 0.12) + self.declare_parameter("return_home_after_task", True) + self.declare_parameter("verify_motion", True) + self.declare_parameter("motion_verify_tolerance", 0.01) + self.declare_parameter("move_to_camera_home", True) + self.declare_parameter("camera_home_x", 0.45) + self.declare_parameter("camera_home_y", 0.00) + self.declare_parameter("camera_home_z", 0.62) + self.declare_parameter("place_x", 0.45) + self.declare_parameter("place_y", 0.0) + self.declare_parameter("place_z", 0.30) + + self.model_path = self.get_parameter("model_path").value + self.conf = float(self.get_parameter("conf").value) + self.imgsz = int(self.get_parameter("imgsz").value) + self.device = self.get_parameter("device").value + self.target_class = self.get_parameter("target_class").value + self.auto_pick = parse_bool(self.get_parameter("auto_pick").value) + self.auto_pick_interval = float(self.get_parameter("auto_pick_interval").value) + self.pick_depth_ratio = float(self.get_parameter("pick_depth_ratio").value) + self.depth_patch_radius = int(self.get_parameter("depth_patch_radius").value) + self.min_depth_valid_ratio = float( + self.get_parameter("min_depth_valid_ratio").value + ) + self.min_depth_m = float(self.get_parameter("min_depth_m").value) + self.max_depth_m = float(self.get_parameter("max_depth_m").value) + self.redetect_on_approach = parse_bool( + self.get_parameter("redetect_on_approach").value + ) + self.redetect_settle_sec = float(self.get_parameter("redetect_settle_sec").value) + self.grasp_mode = str(self.get_parameter("grasp_mode").value).strip().lower() + self.side_grasp_axis = parse_axis(self.get_parameter("side_grasp_axis").value) + self.side_grasp_direction = float( + self.get_parameter("side_grasp_direction").value + ) + self.side_approach_offset = float( + self.get_parameter("side_approach_offset").value + ) + self.side_staging_offset = float( + self.get_parameter("side_staging_offset").value + ) + self.side_grasp_offset = float(self.get_parameter("side_grasp_offset").value) + self.side_grasp_z_offset = float( + self.get_parameter("side_grasp_z_offset").value + ) + self.side_orientation_mode = str( + self.get_parameter("side_orientation_mode").value + ).strip().lower() + self.side_tool_roll_deg = float( + self.get_parameter("side_tool_roll_deg").value + ) + self.side_roll_deg = float(self.get_parameter("side_roll_deg").value) + self.side_pitch_deg = float(self.get_parameter("side_pitch_deg").value) + self.side_yaw_deg = float(self.get_parameter("side_yaw_deg").value) + self.pick_z_offset = float(self.get_parameter("pick_z_offset").value) + self.approach_offset = float(self.get_parameter("approach_offset").value) + self.safe_z = float(self.get_parameter("safe_z").value) + self.min_motion_z = float(self.get_parameter("min_motion_z").value) + self.return_home_after_task = parse_bool( + self.get_parameter("return_home_after_task").value + ) + self.verify_motion = parse_bool(self.get_parameter("verify_motion").value) + self.motion_verify_tolerance = float( + self.get_parameter("motion_verify_tolerance").value + ) + self.move_to_camera_home = parse_bool( + self.get_parameter("move_to_camera_home").value + ) + self.camera_home_x = float(self.get_parameter("camera_home_x").value) + self.camera_home_y = float(self.get_parameter("camera_home_y").value) + self.camera_home_z = float(self.get_parameter("camera_home_z").value) + self.place_x = float(self.get_parameter("place_x").value) + self.place_y = float(self.get_parameter("place_y").value) + self.place_z = float(self.get_parameter("place_z").value) + + model_file = Path(self.model_path).expanduser() + if not model_file.exists(): + raise FileNotFoundError(f"YOLO model not found: {model_file}") + + self.get_logger().info(f"Loading YOLO model: {model_file}") + self.model = YOLO(str(model_file)) + self.get_logger().info(f"YOLO classes: {self.model.names}") + if self.target_class not in self.model.names.values(): + raise ValueError( + f"target_class='{self.target_class}' is not in model classes " + f"{self.model.names}" + ) + if self.grasp_mode not in {"side", "top"}: + raise ValueError("grasp_mode must be 'side' or 'top'") + if self.side_grasp_axis not in {"x", "y"}: + raise ValueError("side_grasp_axis must be 'x' or 'y'") + self.side_grasp_direction = 1.0 if self.side_grasp_direction >= 0 else -1.0 + if self.side_orientation_mode not in {"approach", "euler", "home"}: + raise ValueError( + "side_orientation_mode must be 'approach', 'euler', or 'home'" + ) + if self.side_staging_offset < self.side_approach_offset: + self.get_logger().warning( + "side_staging_offset is smaller than side_approach_offset; " + "using side_approach_offset for staging." + ) + self.side_staging_offset = self.side_approach_offset + + self.bridge = CvBridge() + self.color_image = None + self.depth_image = None + self.intrinsics = None + self.last_detection = None + self.picking = False + self.has_picked_once = False + self.last_pick_time = 0.0 + self.last_status = "waiting for command" + + calib_file = ( + Path(get_package_share_directory("dsr_practice")) + / "config" + / "T_gripper2camera.npy" + ) + self.gripper2cam = np.load(str(calib_file)).astype(float) + self.gripper2cam[:3, 3] /= 1000.0 + self.get_logger().info(f"Loaded hand-eye calibration: {calib_file}") + + self.gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + + self.get_logger().info("Initializing MoveItPy...") + self.robot = MoveItPy(node_name="yolo_cup_pick_moveit_py") + self.arm = self.robot.get_planning_component(GROUP_NAME) + self.robot_model = self.robot.get_robot_model() + self.get_logger().info("MoveItPy initialized") + + self.ompl_params = PlanRequestParameters(self.robot) + self.ompl_params.planning_pipeline = "ompl" + self.ompl_params.planner_id = "RRTConnect" + self.ompl_params.max_velocity_scaling_factor = 0.2 + self.ompl_params.max_acceleration_scaling_factor = 0.1 + self.ompl_params.planning_time = 3.0 + + self.pilz_params = PlanRequestParameters(self.robot) + self.pilz_params.planning_pipeline = "pilz_industrial_motion_planner" + self.pilz_params.planner_id = "PTP" + self.pilz_params.max_velocity_scaling_factor = 0.12 + self.pilz_params.max_acceleration_scaling_factor = 0.08 + self.pilz_params.planning_time = 3.0 + + self.home_ori = DOWN_ORI + + self.create_subscription( + CameraInfo, + "/camera/camera/color/camera_info", + self._camera_info_callback, + 10, + ) + self.create_subscription( + Image, + "/camera/camera/color/image_raw", + self._color_callback, + 10, + ) + self.create_subscription( + Image, + "/camera/camera/aligned_depth_to_color/image_raw", + self._depth_callback, + 10, + ) + + def _camera_info_callback(self, msg): + self.intrinsics = { + "fx": msg.k[0], + "fy": msg.k[4], + "cx": msg.k[2], + "cy": msg.k[5], + } + + def _color_callback(self, msg): + self.color_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") + + def _depth_callback(self, msg): + self.depth_image = self.bridge.imgmsg_to_cv2( + msg, desired_encoding="passthrough" + ) + + def plan_and_execute(self, pose_goal=None, state_goal=None, params=None): + log = self.get_logger() + self.arm.set_start_state_to_current_state() + start_matrix = get_ee_matrix(self.robot) + start_xyz = start_matrix[:3, 3].copy() + goal_xyz = None + + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + x, y, z = clamp_to_safe_workspace(x, y, z, log, self.min_motion_z) + pose_goal.pose.position.x = x + pose_goal.pose.position.y = y + pose_goal.pose.position.z = z + goal_xyz = np.array([x, y, z], dtype=float) + log.info( + f"Planning pose goal -> ({x:.3f}, {y:.3f}, {z:.3f}) " + f"from ({start_xyz[0]:.3f}, {start_xyz[1]:.3f}, {start_xyz[2]:.3f})" + ) + self.arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link=EE_LINK) + elif state_goal is not None: + log.info( + f"Planning joint/state goal from EE " + f"({start_xyz[0]:.3f}, {start_xyz[1]:.3f}, {start_xyz[2]:.3f})" + ) + self.arm.set_goal_state(robot_state=state_goal) + else: + log.error("No pose/state goal was provided") + return False + + plan_result = self.arm.plan(parameters=params) if params else self.arm.plan() + if not plan_result: + log.error("Planning failed") + return False + + self.robot.execute( + group_name=GROUP_NAME, + robot_trajectory=plan_result.trajectory, + blocking=True, + ) + self.spin_for_camera_update(0.2) + + end_matrix = get_ee_matrix(self.robot) + end_xyz = end_matrix[:3, 3].copy() + moved = float(np.linalg.norm(end_xyz - start_xyz)) + if goal_xyz is None: + log.info( + f"Execution finished. EE moved {moved:.3f} m -> " + f"({end_xyz[0]:.3f}, {end_xyz[1]:.3f}, {end_xyz[2]:.3f})" + ) + else: + goal_error = float(np.linalg.norm(end_xyz - goal_xyz)) + log.info( + f"Execution finished. EE moved {moved:.3f} m, " + f"goal_error={goal_error:.3f} m -> " + f"({end_xyz[0]:.3f}, {end_xyz[1]:.3f}, {end_xyz[2]:.3f})" + ) + if self.verify_motion and goal_error > self.motion_verify_tolerance: + log.error( + "MoveIt execution did not reach the requested pose. " + "Check that the real robot MoveIt/trajectory controller is running." + ) + return False + return True + + def move_joint_home(self): + home_state = RobotState(self.robot_model) + home_state.set_joint_group_positions(GROUP_NAME, HOME_JOINTS_RAD) + home_state.update() + if not self.plan_and_execute(state_goal=home_state, params=self.ompl_params): + return False + + transform = get_ee_matrix(self.robot) + self.update_home_orientation_from_matrix(transform) + return True + + def move_home(self): + if self.move_to_camera_home: + return self.move_camera_home() + return self.move_joint_home() + + def update_home_orientation_from_matrix(self, transform): + qx, qy, qz, qw = Rotation.from_matrix(transform[:3, :3]).as_quat() + self.home_ori = { + "x": float(qx), + "y": float(qy), + "z": float(qz), + "w": float(qw), + } + + def move_camera_home(self): + log = self.get_logger() + candidate_zs = [] + for z in (self.camera_home_z, 0.62, 0.58, 0.54): + if z > self.camera_home_z + 1e-6: + continue + if all(abs(z - candidate) > 1e-6 for candidate in candidate_zs): + candidate_zs.append(z) + + for idx, z in enumerate(candidate_zs): + if idx > 0: + log.warning( + f"Camera home IK failed at higher z; retrying z={z:.3f}" + ) + + log.info( + f"Move CAMERA HOME -> ({self.camera_home_x:.3f}, " + f"{self.camera_home_y:.3f}, {z:.3f})" + ) + if not self.plan_and_execute( + pose_goal=make_pose( + self.camera_home_x, + self.camera_home_y, + z, + self.home_ori, + ), + params=self.pilz_params, + ): + continue + + self.camera_home_z = z + transform = get_ee_matrix(self.robot) + self.update_home_orientation_from_matrix(transform) + return True + + return False + + def wait_until_gripper_idle(self, timeout_sec=GRIPPER_OPEN_TIMEOUT_SEC): + log = self.get_logger() + start_time = time.time() + while time.time() - start_time < timeout_sec: + status = self.gripper.get_status() + busy = bool(status[0]) + if not busy: + try: + width_mm = self.gripper.get_width_with_offset() + log.info(f"Gripper ready. current width={width_mm:.1f} mm") + except Exception as exc: + log.warning(f"Gripper width read failed: {exc}") + return True + time.sleep(GRIPPER_STATUS_POLL_SEC) + + log.warning("Timed out waiting for gripper to finish opening.") + return False + + def open_gripper_max(self, wait=False): + self.get_logger().info( + f"Open gripper to max width={GRIPPER_OPEN_WIDTH} " + f"({GRIPPER_OPEN_WIDTH / 10.0:.1f} mm)" + ) + self.gripper.move_gripper(GRIPPER_OPEN_WIDTH, GRIPPER_FORCE) + if wait: + return self.wait_until_gripper_idle() + return True + + def detect_objects(self, image): + results = self.model.predict( + source=image, + imgsz=self.imgsz, + conf=self.conf, + device=self.device, + verbose=False, + ) + boxes = results[0].boxes + if boxes is None or len(boxes) == 0: + self.last_detection = None + return [] + + detections = [] + for box in boxes: + cls_id = int(box.cls[0]) + class_name = self.model.names.get(cls_id, str(cls_id)) + x1, y1, x2, y2 = box.xyxy[0].cpu().numpy().tolist() + x1, y1, x2, y2 = int(x1), int(y1), int(x2), int(y2) + detections.append( + { + "bbox": (x1, y1, x2, y2), + "cx": int((x1 + x2) / 2), + "cy": int((y1 + y2) / 2), + "conf": float(box.conf[0]), + "class_name": class_name, + } + ) + + target_detections = [ + det for det in detections if det["class_name"] == self.target_class + ] + if not target_detections: + self.last_detection = None + else: + self.last_detection = max( + target_detections, + key=lambda det: det["conf"], + ) + + return detections + + def depth_candidates_from_bbox(self, bbox): + x1, y1, x2, y2 = bbox + h, w = self.depth_image.shape[:2] + x_ratios = [0.50, 0.35, 0.65, 0.25, 0.75] + y_ratios = [ + self.pick_depth_ratio, + 0.45, + 0.35, + 0.65, + 0.25, + 0.75, + ] + + points = [] + seen = set() + for yr in y_ratios: + for xr in x_ratios: + u = int(x1 + xr * (x2 - x1)) + v = int(y1 + yr * (y2 - y1)) + u = max(0, min(w - 1, u)) + v = max(0, min(h - 1, v)) + if (u, v) not in seen: + points.append((u, v)) + seen.add((u, v)) + return points + + def depth_patch_at(self, u, v): + h, w = self.depth_image.shape[:2] + r = self.depth_patch_radius + patch = self.depth_image[ + max(0, v - r) : min(h, v + r + 1), + max(0, u - r) : min(w, u + r + 1), + ] + valid = patch[patch > 0] + valid_ratio = valid.size / float(patch.size) + if valid.size == 0 or valid_ratio < self.min_depth_valid_ratio: + return None + + z_raw = float(np.median(valid)) + z_m = z_raw / 1000.0 if self.depth_image.dtype == np.uint16 else z_raw + if z_m < self.min_depth_m or z_m > self.max_depth_m: + return None + return u, v, z_m, valid_ratio + + def depth_from_bbox(self, bbox, log_reason=False): + log = self.get_logger() + if self.depth_image is None: + if log_reason: + log.warning("Depth image is not ready") + return None + + valid_samples = [] + for u, v in self.depth_candidates_from_bbox(bbox): + sample = self.depth_patch_at(u, v) + if sample is not None: + valid_samples.append(sample) + + if not valid_samples: + if log_reason: + log.warning( + "No valid depth found inside target bbox. " + "Try a larger depth_patch_radius or lower min_depth_valid_ratio." + ) + return None + + # Prefer the closest valid surface in the bbox. Transparent cups often + # expose background/table depth, so closest valid depth is usually safer. + u, v, z_m, valid_ratio = min(valid_samples, key=lambda sample: sample[2]) + if log_reason: + log.info( + f"Depth sample selected at ({u}, {v}): " + f"{z_m:.3f} m, valid_ratio={valid_ratio:.2f}" + ) + return u, v, z_m + + def pixel_to_camera(self, u, v, z_m): + fx = self.intrinsics["fx"] + fy = self.intrinsics["fy"] + cx = self.intrinsics["cx"] + cy = self.intrinsics["cy"] + + cam_x = (u - cx) * z_m / fx + cam_y = (v - cy) * z_m / fy + cam_z = z_m + return np.array([cam_x, cam_y, cam_z], dtype=float) + + def camera_to_base(self, camera_xyz): + coord = np.append(camera_xyz, 1.0) + base2ee = get_ee_matrix(self.robot) + base2cam = base2ee @ self.gripper2cam + return (base2cam @ coord)[:3] + + def side_unit_vector(self): + if self.side_grasp_axis == "x": + return np.array([self.side_grasp_direction, 0.0], dtype=float) + return np.array([0.0, self.side_grasp_direction], dtype=float) + + def side_grasp_orientation(self, side_vec): + if self.side_orientation_mode == "home": + return self.home_ori + + if self.side_orientation_mode == "euler": + return quat_dict_from_euler( + self.side_roll_deg, + self.side_pitch_deg, + self.side_yaw_deg, + ) + + # Make the tool's local +Z direction point horizontally into the cup. + # The local +Y axis is kept close to world +Z so the wrist is laid over + # the table instead of keeping the top-down grasp posture. + tool_z = np.array([-side_vec[0], -side_vec[1], 0.0], dtype=float) + tool_z_norm = np.linalg.norm(tool_z) + if tool_z_norm < 1e-6: + return self.home_ori + tool_z /= tool_z_norm + + world_up = np.array([0.0, 0.0, 1.0], dtype=float) + tool_x = np.cross(world_up, tool_z) + tool_x_norm = np.linalg.norm(tool_x) + if tool_x_norm < 1e-6: + return self.home_ori + tool_x /= tool_x_norm + tool_y = np.cross(tool_z, tool_x) + tool_y /= np.linalg.norm(tool_y) + + base_from_tool = np.column_stack((tool_x, tool_y, tool_z)) + if abs(self.side_tool_roll_deg) > 1e-6: + base_from_tool = ( + base_from_tool + @ Rotation.from_euler( + "z", + self.side_tool_roll_deg, + degrees=True, + ).as_matrix() + ) + return quat_dict_from_matrix(base_from_tool) + + def spin_for_camera_update(self, duration_sec): + end_time = time.time() + max(0.0, duration_sec) + while rclpy.ok() and time.time() < end_time: + rclpy.spin_once(self, timeout_sec=0.05) + + def select_redetect_target(self, detections): + if self.color_image is None: + return None + + candidates = [ + det for det in detections if det["class_name"] == self.target_class + ] + if not candidates: + return None + + h, w = self.color_image.shape[:2] + image_cx = w / 2.0 + image_cy = h / 2.0 + return min( + candidates, + key=lambda det: (det["cx"] - image_cx) ** 2 + + (det["cy"] - image_cy) ** 2, + ) + + def base_from_detection(self, detection, log_prefix): + depth_info = self.depth_from_bbox(detection["bbox"], log_reason=True) + if depth_info is None: + return None + + u, v, z_m = depth_info + camera_xyz = self.pixel_to_camera(u, v, z_m) + base_xyz = self.camera_to_base(camera_xyz) + self.get_logger().info( + f"{log_prefix} pixel=({u}, {v}), depth={z_m:.3f} m, " + f"camera=({camera_xyz[0]:.3f}, {camera_xyz[1]:.3f}, " + f"{camera_xyz[2]:.3f}) -> base=({base_xyz[0]:.3f}, " + f"{base_xyz[1]:.3f}, {base_xyz[2]:.3f})" + ) + return base_xyz + + def pick_and_place(self, base_xyz): + if self.grasp_mode == "side": + task_ok = self.pick_and_place_side(base_xyz) + else: + task_ok = self.pick_and_place_top(base_xyz) + + if task_ok and self.return_home_after_task: + self.get_logger().info("return home after task") + return self.move_home() + return task_ok + + def refine_target_from_current_view(self, log): + if not self.redetect_on_approach: + return None + + log.info("redetect target after approach") + self.spin_for_camera_update(self.redetect_settle_sec) + if self.color_image is None: + return None + + detections = self.detect_objects(self.color_image.copy()) + target = self.select_redetect_target(detections) + if target is None: + log.warning("redetect target not found; using initial target") + return None + return self.base_from_detection(target, "[redetect]") + + def pick_and_place_side(self, base_xyz): + log = self.get_logger() + bx, by, bz = [float(v) for v in base_xyz] + side_vec = self.side_unit_vector() + side_ori = self.side_grasp_orientation(side_vec) + stage_xy = ( + np.array([bx, by], dtype=float) + side_vec * self.side_staging_offset + ) + pre_xy = np.array([bx, by], dtype=float) + side_vec * self.side_approach_offset + grasp_xy = np.array([bx, by], dtype=float) + side_vec * self.side_grasp_offset + grasp_z = max(bz + self.side_grasp_z_offset, self.min_motion_z) + pre_z = grasp_z + lift_z = max(grasp_z + self.approach_offset, self.safe_z) + place_approach_z = max(self.place_z + self.approach_offset, self.safe_z) + + log.info( + f"Side grasp target base=({bx:.3f}, {by:.3f}, {bz:.3f}), " + f"axis={self.side_grasp_axis}, dir={self.side_grasp_direction:.0f}, " + f"ori_mode={self.side_orientation_mode}, " + f"tool_roll={self.side_tool_roll_deg:.1f}deg, " + f"stage=({stage_xy[0]:.3f}, {stage_xy[1]:.3f}, {pre_z:.3f}), " + f"pre=({pre_xy[0]:.3f}, {pre_xy[1]:.3f}, {pre_z:.3f}), " + f"grasp=({grasp_xy[0]:.3f}, {grasp_xy[1]:.3f}, {grasp_z:.3f})" + ) + + self.open_gripper_max(wait=False) + + steps = [ + ( + "move to outside side-staging pose", + make_pose(stage_xy[0], stage_xy[1], lift_z, side_ori), + ), + ( + "lower at outside side-staging pose", + make_pose(stage_xy[0], stage_xy[1], pre_z, side_ori), + ), + ( + "move horizontally to side pre-grasp", + make_pose(pre_xy[0], pre_xy[1], pre_z, side_ori), + ), + ] + for label, pose in steps: + log.info(label) + if not self.plan_and_execute(pose_goal=pose, params=self.pilz_params): + return False + + if not self.wait_until_gripper_idle(): + return False + + refined_base = self.refine_target_from_current_view(log) + if refined_base is not None: + bx, by, bz = [float(v) for v in refined_base] + pre_xy = np.array([bx, by], dtype=float) + side_vec * self.side_approach_offset + grasp_xy = np.array([bx, by], dtype=float) + side_vec * self.side_grasp_offset + grasp_z = max(bz + self.side_grasp_z_offset, self.min_motion_z) + pre_z = grasp_z + lift_z = max(grasp_z + self.approach_offset, self.safe_z) + log.info( + f"refined side grasp=({grasp_xy[0]:.3f}, {grasp_xy[1]:.3f}, " + f"{grasp_z:.3f})" + ) + if not self.plan_and_execute( + pose_goal=make_pose(pre_xy[0], pre_xy[1], pre_z, side_ori), + params=self.pilz_params, + ): + return False + + log.info("slide horizontally into cup side") + if not self.plan_and_execute( + pose_goal=make_pose(grasp_xy[0], grasp_xy[1], grasp_z, side_ori), + params=self.pilz_params, + ): + return False + + log.info("close gripper for side grasp") + self.gripper.move_gripper(GRIPPER_CLOSE_WIDTH, GRIPPER_FORCE) + time.sleep(1.0) + + move_steps = [ + ("lift cup", make_pose(grasp_xy[0], grasp_xy[1], lift_z, side_ori)), + ( + "move above syrup pump front", + make_pose(self.place_x, self.place_y, place_approach_z, side_ori), + ), + ( + "place cup", + make_pose(self.place_x, self.place_y, self.place_z, side_ori), + ), + ] + for label, pose in move_steps: + log.info(label) + if not self.plan_and_execute(pose_goal=pose, params=self.pilz_params): + return False + + log.info("open gripper") + self.open_gripper_max(wait=True) + + log.info("retract") + return self.plan_and_execute( + pose_goal=make_pose(self.place_x, self.place_y, place_approach_z, + side_ori), + params=self.pilz_params, + ) + + def pick_and_place_top(self, base_xyz): + log = self.get_logger() + bx, by, bz = [float(v) for v in base_xyz] + pick_z = bz + self.pick_z_offset + approach_z = max(pick_z + self.approach_offset, self.safe_z) + place_approach_z = max(self.place_z + self.approach_offset, self.safe_z) + + log.info( + f"Cup base point=({bx:.3f}, {by:.3f}, {bz:.3f}), " + f"pick_z={pick_z:.3f}" + ) + + self.open_gripper_max(wait=False) + + steps = [ + ("move above cup", make_pose(bx, by, approach_z, self.home_ori)), + ] + for label, pose in steps: + log.info(label) + if not self.plan_and_execute(pose_goal=pose, params=self.pilz_params): + return False + + if not self.wait_until_gripper_idle(): + return False + + refined_base = self.refine_target_from_current_view(log) + if refined_base is not None: + bx, by, bz = [float(v) for v in refined_base] + pick_z = bz + self.pick_z_offset + approach_z = max(pick_z + self.approach_offset, self.safe_z) + log.info( + f"refined cup base=({bx:.3f}, {by:.3f}, {bz:.3f}), " + f"pick_z={pick_z:.3f}" + ) + if not self.plan_and_execute( + pose_goal=make_pose(bx, by, approach_z, self.home_ori), + params=self.pilz_params, + ): + return False + + log.info("move down to cup") + if not self.plan_and_execute( + pose_goal=make_pose(bx, by, pick_z, self.home_ori), + params=self.pilz_params, + ): + return False + + log.info("close gripper") + self.gripper.move_gripper(GRIPPER_CLOSE_WIDTH, GRIPPER_FORCE) + time.sleep(1.0) + + move_steps = [ + ("lift cup", make_pose(bx, by, approach_z, self.home_ori)), + ( + "move above syrup pump front", + make_pose(self.place_x, self.place_y, place_approach_z, self.home_ori), + ), + ( + "place cup", + make_pose(self.place_x, self.place_y, self.place_z, self.home_ori), + ), + ] + for label, pose in move_steps: + log.info(label) + if not self.plan_and_execute(pose_goal=pose, params=self.pilz_params): + return False + + log.info("open gripper") + self.open_gripper_max(wait=True) + time.sleep(1.0) + + log.info("retract") + return self.plan_and_execute( + pose_goal=make_pose(self.place_x, self.place_y, place_approach_z, + self.home_ori), + params=self.pilz_params, + ) + + def start_pick_from_detection(self): + log = self.get_logger() + if self.picking: + log.warning("Already picking") + self.last_status = "already picking" + return + if self.color_image is None or self.depth_image is None or self.intrinsics is None: + log.warning("Waiting for color/depth/camera_info") + self.last_status = "waiting for color/depth/camera_info" + return + if self.last_detection is None: + log.warning(f"No {self.target_class} detection available") + self.last_status = f"no {self.target_class} detection" + return + + self.last_status = f"pick requested: {self.target_class}" + base_xyz = self.base_from_detection(self.last_detection, "[initial]") + if base_xyz is None: + log.error(f"No valid depth around {self.target_class} bbox") + self.last_status = f"no valid depth for {self.target_class}" + return + + self.picking = True + self.last_status = "moving robot" + try: + if self.pick_and_place(base_xyz): + self.has_picked_once = True + self.last_pick_time = time.time() + self.last_status = "pick finished" + else: + self.last_status = "pick failed" + finally: + self.picking = False + + def draw_detections(self, image, detections): + for detection in detections: + x1, y1, x2, y2 = detection["bbox"] + conf = detection["conf"] + class_name = detection.get("class_name", "") + + if class_name == self.target_class: + color = (0, 255, 0) + thickness = 2 + elif class_name == "lid": + color = (255, 0, 0) + thickness = 2 + else: + color = (180, 180, 180) + thickness = 1 + + cv2.rectangle(image, (x1, y1), (x2, y2), color, thickness) + + label = f"{class_name} {conf:.2f}" + if class_name == self.target_class: + depth_info = self.depth_from_bbox(detection["bbox"]) + if depth_info is not None: + u, v, z_m = depth_info + label += f" {z_m:.2f}m" + cv2.circle(image, (u, v), 5, (0, 0, 255), -1) + + cv2.putText( + image, + label, + (x1, max(20, y1 - 8)), + cv2.FONT_HERSHEY_SIMPLEX, + 0.6, + color, + 2, + cv2.LINE_AA, + ) + self.draw_hud(image, detections) + return image + + def draw_hud(self, image, detections): + counts = Counter(det["class_name"] for det in detections) + count_text = " ".join( + f"{name}:{counts[name]}" for name in sorted(counts) + ) or "none" + mode = "AUTO" if self.auto_pick else "MANUAL" + target_state = "ready" if self.last_detection is not None else "not found" + picked_state = "picked" if self.has_picked_once else "waiting" + + lines = [ + ( + f"[{mode}] {self.grasp_mode} target={self.target_class} " + f"conf>={self.conf:.2f} " + "p:pick a:auto r:reset ESC:quit" + ), + f"detections: {count_text} | target: {target_state} | {picked_state}", + f"status: {self.last_status}", + ] + color = (0, 255, 255) if self.auto_pick else (230, 230, 230) + for idx, text in enumerate(lines): + y = 26 + idx * 24 + cv2.putText( + image, + text, + (10, y), + cv2.FONT_HERSHEY_SIMPLEX, + 0.58, + color, + 2, + cv2.LINE_AA, + ) + + def run(self): + log = self.get_logger() + log.info("Move JOINT HOME") + if not self.move_joint_home(): + log.error("Joint home move failed") + return + + if self.move_to_camera_home: + log.info("Move HIGH CAMERA HOME") + if not self.move_camera_home(): + log.error("High camera home move failed") + return + + self.open_gripper_max(wait=True) + + window = "YOLO Cup Pick - p pick, a auto, r reset, esc quit" + cv2.namedWindow(window) + + while rclpy.ok(): + rclpy.spin_once(self, timeout_sec=0.01) + if self.color_image is None: + continue + + frame = self.color_image.copy() + detections = self.detect_objects(frame) + frame = self.draw_detections(frame, detections) + cv2.imshow(window, frame) + + now = time.time() + can_auto_pick = ( + self.auto_pick + and self.last_detection is not None + and not self.has_picked_once + and not self.picking + and (now - self.last_pick_time) >= self.auto_pick_interval + ) + if can_auto_pick: + self.start_pick_from_detection() + + key = cv2.waitKey(1) & 0xFF + if key == 27: + break + if key in (ord("p"), ord("P")): + log.info("pick key pressed") + self.start_pick_from_detection() + elif key in (ord("a"), ord("A")): + self.auto_pick = not self.auto_pick + self.last_pick_time = time.time() + self.last_status = f"auto_pick {'ON' if self.auto_pick else 'OFF'}" + log.info(f"auto_pick {'ON' if self.auto_pick else 'OFF'}") + elif key in (ord("r"), ord("R")): + self.has_picked_once = False + self.last_pick_time = 0.0 + self.last_status = "pick state reset" + log.info("pick state reset") + + cv2.destroyAllWindows() + + def destroy_node(self): + try: + self.gripper.close_connection() + finally: + super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + node = YoloCupPickNode() + try: + node.run() + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/gear_assembly.py b/src/dsr_practice/dsr_practice/gear_assembly.py new file mode 100644 index 0000000..69a7923 --- /dev/null +++ b/src/dsr_practice/dsr_practice/gear_assembly.py @@ -0,0 +1,423 @@ +#!/usr/bin/env python3 + +import math +import time + +import rclpy +from rclpy.logging import get_logger + +from geometry_msgs.msg import PoseStamped +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters + +# ====== OnRobot RG2 설정 ====== +from .onrobot import RG # 같은 패키지 내부의 onrobot.py 사용 + +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 + +# 그리퍼 폭 (raw 단위: 1/10 mm) +GRIPPER_OPEN_WIDTH = 500 # 50.0 mm +GRIPPER_CLOSE_WIDTH = 150 # 20.0 mm +GRIPPER_FORCE = 300 # 약 20 N + +# ================================ +# 기본 설정 +# ================================ +GROUP_NAME = "manipulator" # SRDF에 정의된 planning group 이름 +BASE_FRAME = "base_link" # 로봇 베이스 프레임 +EE_LINK = "link_6" # 엔드이펙터 링크 이름 (SRDF/URDF 기준) + +HOME_JOINTS_RAD = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(0.0), +} + +# ====== 안전 작업 영역 정의 (base_link 기준) ====== +SAFE_X_MIN = 0.0 # x는 0 이상 +SAFE_Y_MIN = -0.3 # y 하한 +SAFE_Y_MAX = 0.3 # y 상한 +SAFE_Z_MIN = 0.27 # z는 이 값보다 낮아지면 안 됨 +# ================================================== + +# ====== 기어 픽업/조립 포즈 (base_link 기준) ====== +GEAR_TASKS = [ + { # Gear 1 + "pick": { + "pos": {"x": 0.393, "y": 0.094, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + "place": { + "pos": {"x": 0.393, "y": -0.206, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + }, + { # Gear 2 + "pick": { + "pos": {"x": 0.392, "y": 0.200, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + "place": { + "pos": {"x": 0.392, "y": -0.101, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + }, + { # Gear 3 + "pick": { + "pos": {"x": 0.486, "y": 0.153, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + "place": { + "pos": {"x": 0.486, "y": -0.149, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + }, + { # Gear 4 + "pick": { + "pos": {"x": 0.427, "y": 0.148, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + "place": { + "pos": {"x": 0.426, "y": -0.153, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + }, +] + +APPROACH_OFFSET = 0.05 # 위에서 접근/후퇴할 거리 [m] + +# ----- 마지막 기어용 wiggle 파라미터 ----- +WIGGLE_Z = 0.295 # z축 회전할 높이 (place_z보다 약간 위) +WIGGLE_YAW_DEG = 5.0 # 좌우로 회전 각도 (deg) +WIGGLE_COUNT = 3 # 좌우 반복 횟수 + + +def quat_mul(q1, q2): + """쿼터니언 곱: q = q1 * q2""" + x1, y1, z1, w1 = q1 + x2, y2, z2, w2 = q2 + x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2 + y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2 + z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2 + w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2 + return x, y, z, w + + +def make_yaw_quat(yaw_rad): + """z축(yaw) 회전에 해당하는 쿼터니언 생성""" + half = yaw_rad / 2.0 + return (0.0, 0.0, math.sin(half), math.cos(half)) + + +def clamp_to_safe_workspace(x: float, y: float, z: float, logger): + """안전 작업 영역으로 (x, y, z) 클램핑""" + safe_x = x + safe_y = y + safe_z = z + + if safe_x < SAFE_X_MIN: + logger.warning( + f"Requested x ({safe_x:.3f} m) is below safety limit " + f"({SAFE_X_MIN:.3f} m). Clamping to SAFE_X_MIN." + ) + safe_x = SAFE_X_MIN + + if safe_y < SAFE_Y_MIN: + logger.warning( + f"Requested y ({safe_y:.3f} m) is below safety limit " + f"({SAFE_Y_MIN:.3f} m). Clamping to SAFE_Y_MIN." + ) + safe_y = SAFE_Y_MIN + elif safe_y > SAFE_Y_MAX: + logger.warning( + f"Requested y ({safe_y:.3f} m) is above safety limit " + f"({SAFE_Y_MAX:.3f} m). Clamping to SAFE_Y_MAX." + ) + safe_y = SAFE_Y_MAX + + if safe_z < SAFE_Z_MIN: + logger.warning( + f"Requested z ({safe_z:.3f} m) is below safety limit " + f"({SAFE_Z_MIN:.3f} m). Clamping to SAFE_Z_MIN." + ) + safe_z = SAFE_Z_MIN + + return safe_x, safe_y, safe_z + + +def plan_and_execute( + robot: MoveItPy, + planning_component, + logger, + pose_goal: PoseStamped = None, + plan_parameters=None, +): + """ + 공식 문서 스타일 helper: 계획 후 곧바로 실행 + + - pose_goal이 주어지면: + · 안전 영역 클램핑 + · start_state = current + · pose 기반 goal 설정 (EE_LINK) + - 그 다음 plan_parameters 유무에 따라 plan() 호출 후 execute + """ + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + + planning_component.set_start_state_to_current_state() + planning_component.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link=EE_LINK, + ) + + logger.info("Planning trajectory") + + if plan_parameters is not None: + plan_result = planning_component.plan( + parameters=plan_parameters + ) + else: + plan_result = planning_component.plan() + + if not plan_result: + logger.error("Planning failed") + return False + + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + robot.execute( + group_name=GROUP_NAME, + robot_trajectory=robot_trajectory, + blocking=True, + ) + logger.info("Execution finished") + return True + + +def main(args=None): + rclpy.init(args=args) + + logger = get_logger("m0609_gear_assembly") + logger.info("=== M0609 Gear Assembly 시작 ===") + + # ---- Gripper ---- + gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + time.sleep(0.5) + gripper.move_gripper(GRIPPER_OPEN_WIDTH, GRIPPER_FORCE) + + # ---- MoveIt ---- + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + robot_model = robot.get_robot_model() + + # ---- PlanRequestParameters (HOME / Pilz) ---- + home_params = PlanRequestParameters(robot) + home_params.planning_pipeline = "ompl" + home_params.planner_id = "RRTConnect" + home_params.max_velocity_scaling_factor = 0.2 + home_params.max_acceleration_scaling_factor = 0.1 + home_params.planning_time = 2.0 + + pilz_params = PlanRequestParameters(robot) + pilz_params.planning_pipeline = "pilz_industrial_motion_planner" + pilz_params.planner_id = "PTP" + pilz_params.max_velocity_scaling_factor = 0.15 + pilz_params.max_acceleration_scaling_factor = 0.1 + pilz_params.planning_time = 2.0 + + # ---- HOME 자세로 이동 (joint goal) ---- + logger.info("=== HOME 자세로 이동 ===") + home_state = RobotState(robot_model) + home_state.joint_positions = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(0.0), + } + home_state.update() + + arm.set_start_state_to_current_state() + arm.set_goal_state(robot_state=home_state) + plan_and_execute(robot, arm, logger, plan_parameters=home_params) + + # ---- PoseStamped 공용 객체 준비 ---- + pose_goal = PoseStamped() + pose_goal.header.frame_id = BASE_FRAME + + logger.info("=== Gear Pick & Place 시작 ===") + + total_gears = len(GEAR_TASKS) + + # ------------------------------- + # 각 기어에 대해 Pick & Place + # ------------------------------- + for gear_idx, task in enumerate(GEAR_TASKS, start=1): + pick = task["pick"] + place = task["place"] + + logger.info(f"--- Gear {gear_idx} 작업 시작 ---") + + # 1) Pick 위에서 접근 (z + offset) + pose_goal.pose.position.x = pick["pos"]["x"] + pose_goal.pose.position.y = pick["pos"]["y"] + pose_goal.pose.position.z = pick["pos"]["z"] + APPROACH_OFFSET + + pose_goal.pose.orientation.x = pick["ori"]["x"] + pose_goal.pose.orientation.y = pick["ori"]["y"] + pose_goal.pose.orientation.z = pick["ori"]["z"] + pose_goal.pose.orientation.w = pick["ori"]["w"] + + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # 2) Pick 위치로 내려가기 + pose_goal.pose.position.z = pick["pos"]["z"] + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # 3) Gripper 닫기 (집기) + logger.info("Gripper CLOSE (20mm) – 기어 집기") + gripper.move_gripper(width_val=GRIPPER_CLOSE_WIDTH, + force_val=GRIPPER_FORCE) + time.sleep(1.0) + + # 4) 다시 위로 올라가기 (Pick z + offset) + pose_goal.pose.position.z = pick["pos"]["z"] + APPROACH_OFFSET + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # 5) Place 위에서 접근 + pose_goal.pose.position.x = place["pos"]["x"] + pose_goal.pose.position.y = place["pos"]["y"] + pose_goal.pose.position.z = place["pos"]["z"] + APPROACH_OFFSET + + pose_goal.pose.orientation.x = place["ori"]["x"] + pose_goal.pose.orientation.y = place["ori"]["y"] + pose_goal.pose.orientation.z = place["ori"]["z"] + pose_goal.pose.orientation.w = place["ori"]["w"] + + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # ------------------------------- + # Place 동작 + # - 1~3번 기어: 바로 place_z로 하강 + # - 4번 기어(마지막): z=WIGGLE_Z에서 z축 회전 wiggle 후 place_z로 하강 + # ------------------------------- + if gear_idx < total_gears: + # 6) Place 위치로 내려가기 (일반) + pose_goal.pose.position.z = place["pos"]["z"] + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + else: + # ---- 마지막 기어: z축 wiggle ---- + logger.info( + f"마지막 기어: z={WIGGLE_Z:.3f}에서 " + f"±{WIGGLE_YAW_DEG}deg 좌우 회전 {WIGGLE_COUNT}회씩 수행" + ) + + # 우선 WIGGLE_Z까지 z만 이동 + pose_goal.pose.position.z = WIGGLE_Z + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # 기준 쿼터니언 (place 자세) + q_base = ( + place["ori"]["x"], + place["ori"]["y"], + place["ori"]["z"], + place["ori"]["w"], + ) + + yaw_rad = math.radians(WIGGLE_YAW_DEG) + + # 좌우로 WIGGLE_COUNT 번 반복 + for i in range(1, WIGGLE_COUNT + 1): + # +yaw + q_plus = quat_mul(make_yaw_quat(+yaw_rad), q_base) + pose_goal.pose.orientation.x = q_plus[0] + pose_goal.pose.orientation.y = q_plus[1] + pose_goal.pose.orientation.z = q_plus[2] + pose_goal.pose.orientation.w = q_plus[3] + + logger.info(f"Wiggle {i}/{WIGGLE_COUNT}: +{WIGGLE_YAW_DEG:.1f} deg") + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # -yaw + q_minus = quat_mul(make_yaw_quat(-yaw_rad), q_base) + pose_goal.pose.orientation.x = q_minus[0] + pose_goal.pose.orientation.y = q_minus[1] + pose_goal.pose.orientation.z = q_minus[2] + pose_goal.pose.orientation.w = q_minus[3] + + logger.info(f"Wiggle {i}/{WIGGLE_COUNT}: -{WIGGLE_YAW_DEG:.1f} deg") + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # 마지막에 기준 자세로 복귀 + pose_goal.pose.orientation.x = q_base[0] + pose_goal.pose.orientation.y = q_base[1] + pose_goal.pose.orientation.z = q_base[2] + pose_goal.pose.orientation.w = q_base[3] + + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # 그리고 place_z로 직선 하강 + pose_goal.pose.position.z = place["pos"]["z"] + logger.info(f"마지막 기어: wiggle 후 place_z={place['pos']['z']:.3f}로 하강") + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # 7) Gripper 열기 (놓기) + logger.info("Gripper OPEN (50mm) – 기어 놓기") + gripper.move_gripper(width_val=GRIPPER_OPEN_WIDTH, + force_val=GRIPPER_FORCE) + time.sleep(1.0) + + # 8) 다시 위로 올라가기 (Place z + offset) + pose_goal.pose.position.z = place["pos"]["z"] + APPROACH_OFFSET + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + logger.info(f"--- Gear {gear_idx} 작업 완료 ---") + + logger.info("=== 모든 기어 조립 완료. HOME으로 복귀 ===") + + # 마지막으로 HOME으로 복귀 + arm.set_start_state_to_current_state() + arm.set_goal_state(robot_state=home_state) + plan_and_execute(robot, arm, logger, plan_parameters=home_params) + + logger.info("=== Gear Assembly 노드 종료 ===") + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/gripper.py b/src/dsr_practice/dsr_practice/gripper.py new file mode 100644 index 0000000..5202e69 --- /dev/null +++ b/src/dsr_practice/dsr_practice/gripper.py @@ -0,0 +1,98 @@ +#!/usr/bin/env python3 +# rg2_gripper_node.py + +import time + +import rclpy + +from .onrobot import RG # 같은 패키지 내부의 onrobot.py 사용 + + +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 + +# 그리퍼 폭 (raw 단위: 1/10 mm) +GRIPPER_OPEN_WIDTH = 1100 # 110.0 mm, RG2 최대 벌림 +GRIPPER_CLOSE_WIDTH = 0 # 완전히 닫는 방향으로 이동하다가 grip_detected 시 정지 +GRIPPER_FORCE = 200 + +STATUS_CHECK_INTERVAL = 0.05 +GRIP_TIMEOUT_SEC = 8.0 + + +def wait_until_not_busy(gripper, logger, timeout_sec=5.0): + start_time = time.time() + while time.time() - start_time < timeout_sec: + status = gripper.get_status() + busy = bool(status[0]) + if not busy: + return True + time.sleep(STATUS_CHECK_INTERVAL) + + logger.warning("그리퍼 동작 완료 대기 시간이 초과되었습니다.") + return False + + +def close_until_grip_detected(gripper, logger): + logger.info( + f"그리퍼 CLOSE 시작 (목표 폭={GRIPPER_CLOSE_WIDTH}, 힘={GRIPPER_FORCE})" + ) + gripper.move_gripper(width_val=GRIPPER_CLOSE_WIDTH, force_val=GRIPPER_FORCE) + + start_time = time.time() + while time.time() - start_time < GRIP_TIMEOUT_SEC: + status = gripper.get_status() + busy = bool(status[0]) + grip_detected = bool(status[1]) + + if grip_detected: + logger.info("Grip detected - 컵을 잡았으므로 그리퍼를 정지합니다.") + gripper.set_control_mode(8) + return True + + if not busy: + logger.warning("그리퍼가 끝까지 닫혔지만 grip_detected가 감지되지 않았습니다.") + return False + + time.sleep(STATUS_CHECK_INTERVAL) + + logger.error("Grip 감지 대기 시간이 초과되었습니다. 그리퍼를 정지합니다.") + gripper.set_control_mode(8) + return False + + +def main(args=None): + rclpy.init(args=args) + node = rclpy.create_node("rg2_gripper_node") + + logger = node.get_logger() + logger.info("=== RG2 Gripper Control Node 시작 ===") + + # ---- 그리퍼 연결 ---- + gripper = RG( + gripper=GRIPPER_NAME, + ip=TOOLCHARGER_IP, + port=TOOLCHARGER_PORT, + ) + time.sleep(0.5) # 연결 안정화 대기 + logger.info("그리퍼와 연결됨") + + logger.info(f"그리퍼 최대 OPEN (폭={GRIPPER_OPEN_WIDTH})") + gripper.move_gripper(width_val=GRIPPER_OPEN_WIDTH, force_val=GRIPPER_FORCE) + wait_until_not_busy(gripper, logger) + + grip_detected = close_until_grip_detected(gripper, logger) + if grip_detected: + logger.info("Grip 성공 - 컵을 잡은 위치에서 정지했습니다.") + else: + logger.error("Grip 실패 - 컵을 잡지 못했거나 감지하지 못했습니다.") + + logger.info("=== RG2 Gripper Control Node 종료 ===") + gripper.close_connection() + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/joint_state_relay.py b/src/dsr_practice/dsr_practice/joint_state_relay.py new file mode 100644 index 0000000..c727f8c --- /dev/null +++ b/src/dsr_practice/dsr_practice/joint_state_relay.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import JointState + + +class JointStateRelay(Node): + def __init__(self): + super().__init__("joint_state_relay") + self.declare_parameter("input_topic", "/dsr01/joint_states") + self.declare_parameter("output_topic", "/joint_states") + + input_topic = self.get_parameter("input_topic").value + output_topic = self.get_parameter("output_topic").value + + self.publisher = self.create_publisher(JointState, output_topic, 10) + self.subscription = self.create_subscription( + JointState, + input_topic, + self.callback, + 10, + ) + self.get_logger().info(f"Relaying {input_topic} -> {output_topic}") + + def callback(self, msg): + self.publisher.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = JointStateRelay() + try: + rclpy.spin(node) + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/launch/bar_sort_node.launch.py b/src/dsr_practice/dsr_practice/launch/bar_sort_node.launch.py new file mode 100644 index 0000000..f4e6655 --- /dev/null +++ b/src/dsr_practice/dsr_practice/launch/bar_sort_node.launch.py @@ -0,0 +1,39 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() + .robot_description_semantic(file_path="config/dsr.srdf") + .robot_description_kinematics() + .joint_limits() + .trajectory_execution() + .planning_scene_monitor() + .sensors_3d() + .to_moveit_configs() + ) + + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="dsr_practice", + executable="bar_sort_node", + output="screen", + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) diff --git a/src/dsr_practice/dsr_practice/launch/click_pick_node.launch.py b/src/dsr_practice/dsr_practice/launch/click_pick_node.launch.py new file mode 100644 index 0000000..fc007a3 --- /dev/null +++ b/src/dsr_practice/dsr_practice/launch/click_pick_node.launch.py @@ -0,0 +1,42 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # Doosan M0609 MoveIt 기본 설정 (URDF, SRDF, kinematics, controllers 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() # m0609.urdf.xacro + .robot_description_semantic(file_path="config/dsr.srdf") # dsr.srdf + .robot_description_kinematics() # kinematics.yaml + .joint_limits() # joint_limits.yaml + .trajectory_execution() # moveit_controllers.yaml + .planning_scene_monitor() # sensors_3d.yaml 등과 연동 + .sensors_3d() + .to_moveit_configs() + ) + + # 🔹 MoveItPy 전용 YAML 추가 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="dsr_practice", + executable="click_pick_node", + output="screen", + # MoveIt config + MoveItPy용 설정을 같이 넘김 + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) diff --git a/src/dsr_practice/dsr_practice/launch/collision_obstacle.launch.py b/src/dsr_practice/dsr_practice/launch/collision_obstacle.launch.py new file mode 100644 index 0000000..05843c4 --- /dev/null +++ b/src/dsr_practice/dsr_practice/launch/collision_obstacle.launch.py @@ -0,0 +1,43 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # Doosan M0609 MoveIt 기본 설정 (URDF, SRDF, kinematics, controllers 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() # m0609.urdf.xacro + .robot_description_semantic() # dsr.srdf + .robot_description_kinematics() # kinematics.yaml + .joint_limits() # joint_limits.yaml + .trajectory_execution() # moveit_controllers.yaml + .planning_scene_monitor() # sensors_3d.yaml 등과 연동 + .sensors_3d() + .to_moveit_configs() + ) + + # 🔹 MoveItPy 전용 YAML 추가 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="dsr_practice", + executable="collision_obstacle", + output="screen", + # MoveIt config + MoveItPy용 설정을 같이 넘김 + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) + diff --git a/src/dsr_practice/dsr_practice/launch/gear_assembly.launch.py b/src/dsr_practice/dsr_practice/launch/gear_assembly.launch.py new file mode 100644 index 0000000..c86744d --- /dev/null +++ b/src/dsr_practice/dsr_practice/launch/gear_assembly.launch.py @@ -0,0 +1,42 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # Doosan M0609 MoveIt 기본 설정 (URDF, SRDF, kinematics, controllers 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() # m0609.urdf.xacro + .robot_description_semantic() # dsr.srdf + .robot_description_kinematics() # kinematics.yaml + .joint_limits() # joint_limits.yaml + .trajectory_execution() # moveit_controllers.yaml + .planning_scene_monitor() # sensors_3d.yaml 등과 연동 + .sensors_3d() + .to_moveit_configs() + ) + + # 🔹 MoveItPy 전용 YAML 추가 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="dsr_practice", + executable="gear_assembly", + output="screen", + # MoveIt config + MoveItPy용 설정을 같이 넘김 + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) diff --git a/src/dsr_practice/dsr_practice/launch/mp_basic.launch.py b/src/dsr_practice/dsr_practice/launch/mp_basic.launch.py new file mode 100644 index 0000000..7d32f76 --- /dev/null +++ b/src/dsr_practice/dsr_practice/launch/mp_basic.launch.py @@ -0,0 +1,43 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # Doosan M0609 MoveIt 기본 설정 (URDF, SRDF, kinematics, controllers 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() # m0609.urdf.xacro + .robot_description_semantic(file_path="config/dsr.srdf") # dsr.srdf + .robot_description_kinematics() # kinematics.yaml + .joint_limits() # joint_limits.yaml + .trajectory_execution() # moveit_controllers.yaml + .planning_scene_monitor() # sensors_3d.yaml 등과 연동 + .sensors_3d() + .to_moveit_configs() + ) + + # 🔹 MoveItPy 전용 YAML 추가 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="dsr_practice", + executable="mp_basic", + output="screen", + # MoveIt config + MoveItPy용 설정을 같이 넘김 + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) + diff --git a/src/dsr_practice/dsr_practice/launch/mp_waypoint.launch.py b/src/dsr_practice/dsr_practice/launch/mp_waypoint.launch.py new file mode 100644 index 0000000..55a6d5e --- /dev/null +++ b/src/dsr_practice/dsr_practice/launch/mp_waypoint.launch.py @@ -0,0 +1,43 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # Doosan M0609 MoveIt 기본 설정 (URDF, SRDF, kinematics, controllers 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() # m0609.urdf.xacro + .robot_description_semantic() # dsr.srdf + .robot_description_kinematics() # kinematics.yaml + .joint_limits() # joint_limits.yaml + .trajectory_execution() # moveit_controllers.yaml + .planning_scene_monitor() # sensors_3d.yaml 등과 연동 + .sensors_3d() + .to_moveit_configs() + ) + + # 🔹 MoveItPy 전용 YAML 추가 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="dsr_practice", + executable="mp_waypoint", + output="screen", + # MoveIt config + MoveItPy용 설정을 같이 넘김 + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) + diff --git a/src/dsr_practice/dsr_practice/launch/mp_waypoint_pilz.launch.py b/src/dsr_practice/dsr_practice/launch/mp_waypoint_pilz.launch.py new file mode 100644 index 0000000..c94e276 --- /dev/null +++ b/src/dsr_practice/dsr_practice/launch/mp_waypoint_pilz.launch.py @@ -0,0 +1,43 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # Doosan M0609 MoveIt 기본 설정 (URDF, SRDF, kinematics, controllers 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() # m0609.urdf.xacro + .robot_description_semantic() # dsr.srdf + .robot_description_kinematics() # kinematics.yaml + .joint_limits() # joint_limits.yaml + .trajectory_execution() # moveit_controllers.yaml + .planning_scene_monitor() # sensors_3d.yaml 등과 연동 + .sensors_3d() + .to_moveit_configs() + ) + + # 🔹 MoveItPy 전용 YAML 추가 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="dsr_practice", + executable="mp_waypoint_pilz", + output="screen", + # MoveIt config + MoveItPy용 설정을 같이 넘김 + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) + diff --git a/src/dsr_practice/dsr_practice/launch/mp_waypoint_pilz_lin.launch.py b/src/dsr_practice/dsr_practice/launch/mp_waypoint_pilz_lin.launch.py new file mode 100644 index 0000000..6d2fe09 --- /dev/null +++ b/src/dsr_practice/dsr_practice/launch/mp_waypoint_pilz_lin.launch.py @@ -0,0 +1,43 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # Doosan M0609 MoveIt 기본 설정 (URDF, SRDF, kinematics, controllers 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() # m0609.urdf.xacro + .robot_description_semantic() # dsr.srdf + .robot_description_kinematics() # kinematics.yaml + .joint_limits() # joint_limits.yaml + .trajectory_execution() # moveit_controllers.yaml + .planning_scene_monitor() # sensors_3d.yaml 등과 연동 + .sensors_3d() + .to_moveit_configs() + ) + + # 🔹 MoveItPy 전용 YAML 추가 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="dsr_practice", + executable="mp_waypoint_pilz_lin", + output="screen", + # MoveIt config + MoveItPy용 설정을 같이 넘김 + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) + diff --git a/src/dsr_practice/dsr_practice/launch/pick_and_place.launch.py b/src/dsr_practice/dsr_practice/launch/pick_and_place.launch.py new file mode 100644 index 0000000..eff7963 --- /dev/null +++ b/src/dsr_practice/dsr_practice/launch/pick_and_place.launch.py @@ -0,0 +1,42 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # Doosan M0609 MoveIt 기본 설정 (URDF, SRDF, kinematics, controllers 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() # m0609.urdf.xacro + .robot_description_semantic() # dsr.srdf + .robot_description_kinematics() # kinematics.yaml + .joint_limits() # joint_limits.yaml + .trajectory_execution() # moveit_controllers.yaml + .planning_scene_monitor() # sensors_3d.yaml 등과 연동 + .sensors_3d() + .to_moveit_configs() + ) + + # 🔹 MoveItPy 전용 YAML 추가 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="dsr_practice", + executable="pick_and_place", + output="screen", + # MoveIt config + MoveItPy용 설정을 같이 넘김 + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) diff --git a/src/dsr_practice/dsr_practice/launch/realsense_data_collector.launch.py b/src/dsr_practice/dsr_practice/launch/realsense_data_collector.launch.py new file mode 100644 index 0000000..831386a --- /dev/null +++ b/src/dsr_practice/dsr_practice/launch/realsense_data_collector.launch.py @@ -0,0 +1,52 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + output_dir_arg = DeclareLaunchArgument( + "output_dir", + default_value="/home/ssu/ros2_ws/realsense_dataset/raw", + description="Directory where color, depth, and metadata files are saved.", + ) + save_interval_arg = DeclareLaunchArgument( + "save_interval_sec", + default_value="1.0", + description="Seconds between saved frames.", + ) + max_frames_arg = DeclareLaunchArgument( + "max_frames", + default_value="0", + description="Maximum number of frames to save. 0 means unlimited.", + ) + save_depth_arg = DeclareLaunchArgument( + "save_depth", + default_value="true", + description="Whether to save aligned depth images with RGB images.", + ) + + collector_node = Node( + package="dsr_practice", + executable="realsense_data_collector", + name="realsense_data_collector", + output="screen", + parameters=[ + { + "output_dir": LaunchConfiguration("output_dir"), + "save_interval_sec": LaunchConfiguration("save_interval_sec"), + "max_frames": LaunchConfiguration("max_frames"), + "save_depth": LaunchConfiguration("save_depth"), + } + ], + ) + + return LaunchDescription( + [ + output_dir_arg, + save_interval_arg, + max_frames_arg, + save_depth_arg, + collector_node, + ] + ) diff --git a/src/dsr_practice/dsr_practice/launch/stt_pick_and_place.launch.py b/src/dsr_practice/dsr_practice/launch/stt_pick_and_place.launch.py new file mode 100644 index 0000000..61c9e0d --- /dev/null +++ b/src/dsr_practice/dsr_practice/launch/stt_pick_and_place.launch.py @@ -0,0 +1,54 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() + .robot_description_semantic(file_path="config/dsr.srdf") + .robot_description_kinematics() + .joint_limits() + .trajectory_execution() + .planning_scene_monitor() + .sensors_3d() + .to_moveit_configs() + ) + + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + pick_place_node = Node( + package="dsr_practice", + executable="stt_pick_and_place", + output="screen", + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + {"use_tts": True}, + ], + ) + + stt_node = Node( + package="dsr_practice", + executable="stt_node", + output="screen", + parameters=[ + {"language": "ko-KR"}, + {"device_index": -1}, + {"energy_threshold": 300.0}, + {"pause_threshold": 0.8}, + {"phrase_time_limit": 5.0}, + {"dynamic_energy": True}, + {"ambient_duration": 1.0}, + ], + ) + + return LaunchDescription([pick_place_node, stt_node]) diff --git a/src/dsr_practice/dsr_practice/launch/stt_robot_control.launch.py b/src/dsr_practice/dsr_practice/launch/stt_robot_control.launch.py new file mode 100644 index 0000000..ff8411b --- /dev/null +++ b/src/dsr_practice/dsr_practice/launch/stt_robot_control.launch.py @@ -0,0 +1,54 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() + .robot_description_semantic(file_path="config/dsr.srdf") + .robot_description_kinematics() + .joint_limits() + .trajectory_execution() + .planning_scene_monitor() + .sensors_3d() + .to_moveit_configs() + ) + + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + robot_control_node = Node( + package="dsr_practice", + executable="stt_robot_control", + output="screen", + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + {"use_tts": True}, + ], + ) + + stt_node = Node( + package="dsr_practice", + executable="stt_node", + output="screen", + parameters=[ + {"language": "ko-KR"}, + {"device_index": -1}, + {"energy_threshold": 300.0}, + {"pause_threshold": 0.8}, + {"phrase_time_limit": 5.0}, + {"dynamic_energy": True}, + {"ambient_duration": 1.0}, + ], + ) + + return LaunchDescription([robot_control_node, stt_node]) diff --git a/src/dsr_practice/dsr_practice/launch/syrup_pump_press.launch.py b/src/dsr_practice/dsr_practice/launch/syrup_pump_press.launch.py new file mode 100644 index 0000000..09ea49b --- /dev/null +++ b/src/dsr_practice/dsr_practice/launch/syrup_pump_press.launch.py @@ -0,0 +1,86 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description(file_path="config/m0609.urdf.xacro") + .robot_description_semantic(file_path="config/dsr.srdf") + .robot_description_kinematics() + .joint_limits() + .trajectory_execution() + .planning_scene_monitor() + .sensors_3d() + .to_moveit_configs() + ) + + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + pump_x_arg = DeclareLaunchArgument( + "pump_x", + default_value="0.45", + description="Syrup pump x position in base_link frame [m].", + ) + pump_y_arg = DeclareLaunchArgument( + "pump_y", + default_value="0.0", + description="Syrup pump y position in base_link frame [m].", + ) + start_z_arg = DeclareLaunchArgument( + "start_z", + default_value="0.50", + description="Vertical approach start height [m].", + ) + pump_top_z_arg = DeclareLaunchArgument( + "pump_top_z", + default_value="0.35", + description="Syrup pump top height [m].", + ) + press_depth_arg = DeclareLaunchArgument( + "press_depth", + default_value="0.05", + description="Press depth from pump top [m].", + ) + hold_sec_arg = DeclareLaunchArgument( + "hold_sec", + default_value="0.5", + description="Holding time at pressed position [sec].", + ) + + return LaunchDescription( + [ + pump_x_arg, + pump_y_arg, + start_z_arg, + pump_top_z_arg, + press_depth_arg, + hold_sec_arg, + Node( + package="dsr_practice", + executable="syrup_pump_press", + output="screen", + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + { + "pump_x": LaunchConfiguration("pump_x"), + "pump_y": LaunchConfiguration("pump_y"), + "start_z": LaunchConfiguration("start_z"), + "pump_top_z": LaunchConfiguration("pump_top_z"), + "press_depth": LaunchConfiguration("press_depth"), + "hold_sec": LaunchConfiguration("hold_sec"), + }, + ], + ), + ] + ) diff --git a/src/dsr_practice/dsr_practice/launch/yolo_cup_pick_node.launch.py b/src/dsr_practice/dsr_practice/launch/yolo_cup_pick_node.launch.py new file mode 100644 index 0000000..dc2deae --- /dev/null +++ b/src/dsr_practice/dsr_practice/launch/yolo_cup_pick_node.launch.py @@ -0,0 +1,274 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description(file_path="config/m0609.urdf.xacro") + .robot_description_semantic(file_path="config/dsr.srdf") + .robot_description_kinematics() + .joint_limits() + .trajectory_execution() + .planning_scene_monitor() + .sensors_3d() + .to_moveit_configs() + ) + + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + model_path_arg = DeclareLaunchArgument( + "model_path", + default_value="/home/ssu/ros2_ws/yolo_runs/cup_yolov8n_ft1/weights/best.pt", + description="Path to trained cup YOLO weights.", + ) + conf_arg = DeclareLaunchArgument("conf", default_value="0.35") + imgsz_arg = DeclareLaunchArgument("imgsz", default_value="640") + device_arg = DeclareLaunchArgument("device", default_value="cpu") + target_class_arg = DeclareLaunchArgument("target_class", default_value="cup") + auto_pick_interval_arg = DeclareLaunchArgument( + "auto_pick_interval", default_value="3.0" + ) + pick_depth_ratio_arg = DeclareLaunchArgument( + "pick_depth_ratio", default_value="0.55" + ) + depth_patch_radius_arg = DeclareLaunchArgument( + "depth_patch_radius", default_value="7" + ) + min_depth_valid_ratio_arg = DeclareLaunchArgument( + "min_depth_valid_ratio", default_value="0.03" + ) + min_depth_m_arg = DeclareLaunchArgument("min_depth_m", default_value="0.15") + max_depth_m_arg = DeclareLaunchArgument("max_depth_m", default_value="1.20") + redetect_on_approach_arg = DeclareLaunchArgument( + "redetect_on_approach", default_value="true" + ) + redetect_settle_sec_arg = DeclareLaunchArgument( + "redetect_settle_sec", default_value="0.5" + ) + grasp_mode_arg = DeclareLaunchArgument("grasp_mode", default_value="side") + side_grasp_axis_arg = DeclareLaunchArgument( + "side_grasp_axis", default_value="y_axis" + ) + side_grasp_direction_arg = DeclareLaunchArgument( + "side_grasp_direction", default_value="-1.0" + ) + side_approach_offset_arg = DeclareLaunchArgument( + "side_approach_offset", default_value="0.12" + ) + side_staging_offset_arg = DeclareLaunchArgument( + "side_staging_offset", + default_value="0.24", + description="Far outside offset where the wrist first turns horizontal.", + ) + side_grasp_offset_arg = DeclareLaunchArgument( + "side_grasp_offset", default_value="0.035" + ) + side_grasp_z_offset_arg = DeclareLaunchArgument( + "side_grasp_z_offset", + default_value="0.05", + description="Side grasp height offset from detected base point.", + ) + side_orientation_mode_arg = DeclareLaunchArgument( + "side_orientation_mode", + default_value="approach", + description="Side grasp orientation: approach, euler, or home.", + ) + side_tool_roll_deg_arg = DeclareLaunchArgument( + "side_tool_roll_deg", + default_value="0.0", + description="Twist around the horizontal approach direction for RG2 finger alignment.", + ) + side_roll_deg_arg = DeclareLaunchArgument( + "side_roll_deg", + default_value="0.0", + description="Manual side grasp roll, used when side_orientation_mode:=euler.", + ) + side_pitch_deg_arg = DeclareLaunchArgument( + "side_pitch_deg", + default_value="90.0", + description="Manual side grasp pitch, used when side_orientation_mode:=euler.", + ) + side_yaw_deg_arg = DeclareLaunchArgument( + "side_yaw_deg", + default_value="0.0", + description="Manual side grasp yaw, used when side_orientation_mode:=euler.", + ) + verify_motion_arg = DeclareLaunchArgument("verify_motion", default_value="true") + motion_verify_tolerance_arg = DeclareLaunchArgument( + "motion_verify_tolerance", default_value="0.01" + ) + move_to_camera_home_arg = DeclareLaunchArgument( + "move_to_camera_home", default_value="true" + ) + camera_home_x_arg = DeclareLaunchArgument("camera_home_x", default_value="0.45") + camera_home_y_arg = DeclareLaunchArgument("camera_home_y", default_value="0.0") + camera_home_z_arg = DeclareLaunchArgument("camera_home_z", default_value="0.62") + min_motion_z_arg = DeclareLaunchArgument( + "min_motion_z", + default_value="0.12", + description="Minimum allowed commanded Z in base frame.", + ) + return_home_after_task_arg = DeclareLaunchArgument( + "return_home_after_task", default_value="true" + ) + place_x_arg = DeclareLaunchArgument("place_x", default_value="0.45") + place_y_arg = DeclareLaunchArgument("place_y", default_value="0.0") + place_z_arg = DeclareLaunchArgument("place_z", default_value="0.30") + auto_pick_arg = DeclareLaunchArgument("auto_pick", default_value="false") + + return LaunchDescription( + [ + model_path_arg, + conf_arg, + imgsz_arg, + device_arg, + target_class_arg, + auto_pick_interval_arg, + pick_depth_ratio_arg, + depth_patch_radius_arg, + min_depth_valid_ratio_arg, + min_depth_m_arg, + max_depth_m_arg, + redetect_on_approach_arg, + redetect_settle_sec_arg, + grasp_mode_arg, + side_grasp_axis_arg, + side_grasp_direction_arg, + side_approach_offset_arg, + side_staging_offset_arg, + side_grasp_offset_arg, + side_grasp_z_offset_arg, + side_orientation_mode_arg, + side_tool_roll_deg_arg, + side_roll_deg_arg, + side_pitch_deg_arg, + side_yaw_deg_arg, + verify_motion_arg, + motion_verify_tolerance_arg, + move_to_camera_home_arg, + camera_home_x_arg, + camera_home_y_arg, + camera_home_z_arg, + min_motion_z_arg, + return_home_after_task_arg, + place_x_arg, + place_y_arg, + place_z_arg, + auto_pick_arg, + Node( + package="dsr_practice", + executable="joint_state_relay", + name="joint_state_relay", + output="screen", + parameters=[ + { + "input_topic": "/dsr01/joint_states", + "output_topic": "/joint_states", + } + ], + ), + Node( + package="dsr_practice", + executable="yolo_cup_pick_node", + output="screen", + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + { + "model_path": ParameterValue( + LaunchConfiguration("model_path"), + value_type=str, + ), + "conf": LaunchConfiguration("conf"), + "imgsz": LaunchConfiguration("imgsz"), + "device": ParameterValue( + LaunchConfiguration("device"), + value_type=str, + ), + "target_class": ParameterValue( + LaunchConfiguration("target_class"), + value_type=str, + ), + "auto_pick_interval": LaunchConfiguration( + "auto_pick_interval" + ), + "pick_depth_ratio": LaunchConfiguration("pick_depth_ratio"), + "depth_patch_radius": LaunchConfiguration( + "depth_patch_radius" + ), + "min_depth_valid_ratio": LaunchConfiguration( + "min_depth_valid_ratio" + ), + "min_depth_m": LaunchConfiguration("min_depth_m"), + "max_depth_m": LaunchConfiguration("max_depth_m"), + "redetect_on_approach": LaunchConfiguration( + "redetect_on_approach" + ), + "redetect_settle_sec": LaunchConfiguration( + "redetect_settle_sec" + ), + "grasp_mode": ParameterValue( + LaunchConfiguration("grasp_mode"), + value_type=str, + ), + "side_grasp_axis": ParameterValue( + LaunchConfiguration("side_grasp_axis"), + value_type=str, + ), + "side_grasp_direction": LaunchConfiguration( + "side_grasp_direction" + ), + "side_approach_offset": LaunchConfiguration( + "side_approach_offset" + ), + "side_staging_offset": LaunchConfiguration( + "side_staging_offset" + ), + "side_grasp_offset": LaunchConfiguration("side_grasp_offset"), + "side_grasp_z_offset": LaunchConfiguration( + "side_grasp_z_offset" + ), + "side_orientation_mode": ParameterValue( + LaunchConfiguration("side_orientation_mode"), + value_type=str, + ), + "side_tool_roll_deg": LaunchConfiguration( + "side_tool_roll_deg" + ), + "side_roll_deg": LaunchConfiguration("side_roll_deg"), + "side_pitch_deg": LaunchConfiguration("side_pitch_deg"), + "side_yaw_deg": LaunchConfiguration("side_yaw_deg"), + "verify_motion": LaunchConfiguration("verify_motion"), + "motion_verify_tolerance": LaunchConfiguration( + "motion_verify_tolerance" + ), + "move_to_camera_home": LaunchConfiguration( + "move_to_camera_home" + ), + "camera_home_x": LaunchConfiguration("camera_home_x"), + "camera_home_y": LaunchConfiguration("camera_home_y"), + "camera_home_z": LaunchConfiguration("camera_home_z"), + "min_motion_z": LaunchConfiguration("min_motion_z"), + "return_home_after_task": LaunchConfiguration( + "return_home_after_task" + ), + "place_x": LaunchConfiguration("place_x"), + "place_y": LaunchConfiguration("place_y"), + "place_z": LaunchConfiguration("place_z"), + "auto_pick": LaunchConfiguration("auto_pick"), + }, + ], + ), + ] + ) diff --git a/src/dsr_practice/dsr_practice/mp_basic.py b/src/dsr_practice/dsr_practice/mp_basic.py new file mode 100644 index 0000000..d1296b4 --- /dev/null +++ b/src/dsr_practice/dsr_practice/mp_basic.py @@ -0,0 +1,174 @@ +#!/usr/bin/env python3 +import math + +import rclpy +from rclpy.logging import get_logger + +from geometry_msgs.msg import PoseStamped +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy + + + +GROUP_NAME = "manipulator" # SRDF에 정의된 planning group 이름 +BASE_FRAME = "base_link" # 로봇 베이스 프레임 +EE_LINK = "link_6" # 엔드이펙터 링크 이름 (SRDF/URDF 기준) + +# HOME_JOINTS_DEG = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0] +# HOME_JOINTS_RAD = [math.radians(d) for d in HOME_JOINTS_DEG] + +# 이걸로 교체 +HOME_JOINTS_RAD = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(0.0), +} + +# ====== 안전 작업 영역 정의 (base_link 기준) ====== +SAFE_X_MIN = 0.0 # x는 0 이상 +SAFE_Y_MIN = -0.3 # y 하한 +SAFE_Y_MAX = 0.3 # y 상한 +SAFE_Z_MIN = 0.27 # z는 이 값보다 낮아지면 안 됨 +# ================================================== + + +def clamp_to_safe_workspace(x: float, y: float, z: float, logger): + """안전 작업 영역으로 (x, y, z) 클램핑""" + safe_x = x + safe_y = y + safe_z = z + + if safe_x < SAFE_X_MIN: + logger.warning( + f"Requested x ({safe_x:.3f} m) is below safety limit " + f"({SAFE_X_MIN:.3f} m). Clamping to SAFE_X_MIN." + ) + safe_x = SAFE_X_MIN + + if safe_y < SAFE_Y_MIN: + logger.warning( + f"Requested y ({safe_y:.3f} m) is below safety limit " + f"({SAFE_Y_MIN:.3f} m). Clamping to SAFE_Y_MIN." + ) + safe_y = SAFE_Y_MIN + elif safe_y > SAFE_Y_MAX: + logger.warning( + f"Requested y ({safe_y:.3f} m) is above safety limit " + f"({SAFE_Y_MAX:.3f} m). Clamping to SAFE_Y_MAX." + ) + safe_y = SAFE_Y_MAX + + if safe_z < SAFE_Z_MIN: + logger.warning( + f"Requested z ({safe_z:.3f} m) is below safety limit " + f"({SAFE_Z_MIN:.3f} m). Clamping to SAFE_Z_MIN." + ) + safe_z = SAFE_Z_MIN + + return safe_x, safe_y, safe_z + + +def plan_and_execute( + robot: MoveItPy, + planning_component, + logger, + pose_goal: PoseStamped = None, + plan_parameters=None, +): + """ + 공식 문서 스타일 helper: 계획 후 곧바로 실행 + + - pose_goal이 주어지면: + · 안전 영역 클램핑 + · start_state = current + · pose 기반 goal 설정 (EE_LINK) + - 그 다음 plan_parameters 유무에 따라 plan() 호출 후 execute + """ + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + + planning_component.set_start_state_to_current_state() + planning_component.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link=EE_LINK, + ) + + logger.info("Planning trajectory") + + if plan_parameters is not None: + plan_result = planning_component.plan( + parameters=plan_parameters + ) + else: + plan_result = planning_component.plan() + + if not plan_result: + logger.error("Planning failed") + return False + + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + robot.execute( + group_name=GROUP_NAME, + robot_trajectory=robot_trajectory, + blocking=True, + ) + logger.info("Execution finished") + return True + +def main(): + rclpy.init() + logger = get_logger("m0609.moveit_py.basic") + + robot = MoveItPy(node_name="moveit_py") + import time + time.sleep(2.0) + + arm = robot.get_planning_component(GROUP_NAME) + logger.info("MoveItPy instance created") + + # ── 1) Home position으로 이동 ────────────────────────────── + # RobotState 방식 대신 joint value map으로 goal 설정 + arm.set_start_state_to_current_state() + + robot_model = robot.get_robot_model() + home_state = RobotState(robot_model) + + # set_joint_group_positions 대신 이걸로 + home_state.joint_positions = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(0.0), + } + home_state.update() + + arm.set_goal_state(robot_state=home_state) + plan_and_execute(robot, arm, logger) + + # ── 2) Pose goal로 이동 ──────────────────────────────────── + pose_goal = PoseStamped() + pose_goal.header.frame_id = BASE_FRAME + pose_goal.pose.position.x = 0.5 + pose_goal.pose.position.y = 0.0 + pose_goal.pose.position.z = 0.5 + pose_goal.pose.orientation.x = 0.0 + pose_goal.pose.orientation.y = 1.0 + pose_goal.pose.orientation.z = 0.0 + pose_goal.pose.orientation.w = 0.0 + + plan_and_execute(robot, arm, logger, pose_goal=pose_goal) + + rclpy.shutdown() \ No newline at end of file diff --git a/src/dsr_practice/dsr_practice/mp_waypoint.py b/src/dsr_practice/dsr_practice/mp_waypoint.py new file mode 100644 index 0000000..4b76f0e --- /dev/null +++ b/src/dsr_practice/dsr_practice/mp_waypoint.py @@ -0,0 +1,189 @@ +#!/usr/bin/env python3 +import math + +import rclpy +from rclpy.logging import get_logger + +from geometry_msgs.msg import PoseStamped +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy + +# ================================ +# 기본 설정 +# ================================ +GROUP_NAME = "manipulator" # SRDF에 정의된 planning group 이름 +BASE_FRAME = "base_link" # 로봇 베이스 프레임 +EE_LINK = "link_6" # 엔드이펙터 링크 이름 (SRDF/URDF 기준) + +HOME_JOINTS_DEG = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0] +HOME_JOINTS_RAD = [math.radians(d) for d in HOME_JOINTS_DEG] + +# ====== 안전 작업 영역 정의 (base_link 기준) ====== +SAFE_X_MIN = 0.0 # x는 0 이상 +SAFE_Y_MIN = -0.3 # y 하한 +SAFE_Y_MAX = 0.3 # y 상한 +SAFE_Z_MIN = 0.27 # z는 이 값보다 낮아지면 안 됨 +# ================================================== + + +def clamp_to_safe_workspace(x: float, y: float, z: float, logger): + """안전 작업 영역으로 (x, y, z) 클램핑""" + safe_x = x + safe_y = y + safe_z = z + + if safe_x < SAFE_X_MIN: + logger.warning( + f"Requested x ({safe_x:.3f} m) is below safety limit " + f"({SAFE_X_MIN:.3f} m). Clamping to SAFE_X_MIN." + ) + safe_x = SAFE_X_MIN + + if safe_y < SAFE_Y_MIN: + logger.warning( + f"Requested y ({safe_y:.3f} m) is below safety limit " + f"({SAFE_Y_MIN:.3f} m). Clamping to SAFE_Y_MIN." + ) + safe_y = SAFE_Y_MIN + elif safe_y > SAFE_Y_MAX: + logger.warning( + f"Requested y ({safe_y:.3f} m) is above safety limit " + f"({SAFE_Y_MAX:.3f} m). Clamping to SAFE_Y_MAX." + ) + safe_y = SAFE_Y_MAX + + if safe_z < SAFE_Z_MIN: + logger.warning( + f"Requested z ({safe_z:.3f} m) is below safety limit " + f"({SAFE_Z_MIN:.3f} m). Clamping to SAFE_Z_MIN." + ) + safe_z = SAFE_Z_MIN + + return safe_x, safe_y, safe_z + + +def plan_and_execute( + robot: MoveItPy, + planning_component, + logger, + pose_goal: PoseStamped = None, + plan_parameters=None, +): + """ + 공식 문서 스타일 helper: 계획 후 곧바로 실행 + + - pose_goal이 주어지면: + · 안전 영역 클램핑 + · start_state = current + · pose 기반 goal 설정 (EE_LINK) + - 그 다음 plan_parameters 유무에 따라 plan() 호출 후 execute + """ + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + + planning_component.set_start_state_to_current_state() + planning_component.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link=EE_LINK, + ) + + logger.info("Planning trajectory") + + if plan_parameters is not None: + plan_result = planning_component.plan( + parameters=plan_parameters + ) + else: + plan_result = planning_component.plan() + + if not plan_result: + logger.error("Planning failed") + return False + + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + robot.execute( + group_name=GROUP_NAME, + robot_trajectory=robot_trajectory, + blocking=True, + ) + logger.info("Execution finished") + return True + + +def main(args=None): + # ================================ + # Instantiating moveit_py and planning component + # ================================ + rclpy.init(args=args) + logger = get_logger("m0609.moveit_py.waypoint") + + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + logger.info("MoveItPy instance created") + + # 로봇 모델 / RobotState 준비 + robot_model = robot.get_robot_model() + home_state = RobotState(robot_model) + home_state.set_joint_group_positions(GROUP_NAME, HOME_JOINTS_RAD) + home_state.update() + + # 1) HOME 자세로 이동 (조인트 목표) + arm.set_start_state_to_current_state() + arm.set_goal_state(robot_state=home_state) + plan_and_execute(robot, arm, logger) + + # 2) WAYPOINT 이동 (안전영역 포함) + WAYPOINTS = [ + { # waypoint 1 + "pos": {"x": 0.493, "y": 0.010, "z": 0.417}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + { # waypoint 2 + "pos": {"x": 0.493, "y": -0.218, "z": 0.417}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + { # waypoint 3 + "pos": {"x": 0.371, "y": -0.218, "z": 0.419}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + ] + + pose_goal = PoseStamped() + pose_goal.header.frame_id = BASE_FRAME + + for i, wp in enumerate(WAYPOINTS, start=1): + pos = wp["pos"] + ori = wp["ori"] + + logger.info( + f"--- Waypoint {i}: " + f"x={pos['x']:.3f}, y={pos['y']:.3f}, z={pos['z']:.3f} ---" + ) + + # pose_goal에 원래 의도 좌표/자세 설정 + pose_goal.pose.position.x = pos["x"] + pose_goal.pose.position.y = pos["y"] + pose_goal.pose.position.z = pos["z"] + + pose_goal.pose.orientation.x = ori["x"] + pose_goal.pose.orientation.y = ori["y"] + pose_goal.pose.orientation.z = ori["z"] + pose_goal.pose.orientation.w = ori["w"] + + # plan_and_execute 안에서 안전영역 + goal 설정까지 처리 + plan_and_execute(robot, arm, logger, pose_goal=pose_goal) + + robot.shutdown() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/mp_waypoint_pilz.py b/src/dsr_practice/dsr_practice/mp_waypoint_pilz.py new file mode 100644 index 0000000..9eb6ff5 --- /dev/null +++ b/src/dsr_practice/dsr_practice/mp_waypoint_pilz.py @@ -0,0 +1,239 @@ +#!/usr/bin/env python3 +import math + +import rclpy +from rclpy.logging import get_logger + +from geometry_msgs.msg import PoseStamped +from moveit.planning import MoveItPy, PlanRequestParameters +from moveit_msgs.msg import Constraints, JointConstraint + +# ================================ +# 기본 설정 +# ================================ +GROUP_NAME = "manipulator" # SRDF에 정의된 planning group 이름 +BASE_FRAME = "base_link" # 로봇 베이스 프레임 +EE_LINK = "link_6" # 엔드이펙터 링크 이름 (SRDF/URDF 기준) + +HOME_JOINTS_DEG = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0] +HOME_JOINTS_RAD = [math.radians(d) for d in HOME_JOINTS_DEG] + +# ====== 안전 작업 영역 정의 (base_link 기준) ====== +SAFE_X_MIN = 0.0 # x는 0 이상 +SAFE_Y_MIN = -0.3 # y 하한 +SAFE_Y_MAX = 0.3 # y 상한 +SAFE_Z_MIN = 0.27 # z는 이 값보다 낮아지면 안 됨 +# ================================================== + + +def clamp_to_safe_workspace(x: float, y: float, z: float, logger): + """안전 작업 영역으로 (x, y, z) 클램핑""" + safe_x = x + safe_y = y + safe_z = z + + if safe_x < SAFE_X_MIN: + logger.warning( + f"Requested x ({safe_x:.3f} m) is below safety limit " + f"({SAFE_X_MIN:.3f} m). Clamping to SAFE_X_MIN." + ) + safe_x = SAFE_X_MIN + + if safe_y < SAFE_Y_MIN: + logger.warning( + f"Requested y ({safe_y:.3f} m) is below safety limit " + f"({SAFE_Y_MIN:.3f} m). Clamping to SAFE_Y_MIN." + ) + safe_y = SAFE_Y_MIN + elif safe_y > SAFE_Y_MAX: + logger.warning( + f"Requested y ({safe_y:.3f} m) is above safety limit " + f"({SAFE_Y_MAX:.3f} m). Clamping to SAFE_Y_MAX." + ) + safe_y = SAFE_Y_MAX + + if safe_z < SAFE_Z_MIN: + logger.warning( + f"Requested z ({safe_z:.3f} m) is below safety limit " + f"({SAFE_Z_MIN:.3f} m). Clamping to SAFE_Z_MIN." + ) + safe_z = SAFE_Z_MIN + + return safe_x, safe_y, safe_z + + +def build_home_joint_constraints() -> list[Constraints]: + constraints = Constraints() + joint_names = [f"joint_{i}" for i in range(1, 7)] + + for joint_name, position in zip(joint_names, HOME_JOINTS_RAD): + joint_constraint = JointConstraint() + joint_constraint.joint_name = joint_name + joint_constraint.position = position + joint_constraint.tolerance_above = 0.001 + joint_constraint.tolerance_below = 0.001 + joint_constraint.weight = 1.0 + constraints.joint_constraints.append(joint_constraint) + + return [constraints] + + +def plan_and_execute( + robot: MoveItPy, + planning_component, + logger, + pose_goal: PoseStamped = None, + plan_parameters=None, +): + """ + 공식 문서 스타일 helper: 계획 후 곧바로 실행 + + - pose_goal이 주어지면: + · 안전 영역 클램핑 + · start_state = current + · pose 기반 goal 설정 (EE_LINK) + - 그 다음 plan_parameters 유무에 따라 plan() 호출 후 execute + """ + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + + planning_component.set_start_state_to_current_state() + planning_component.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link=EE_LINK, + ) + + logger.info("Planning trajectory") + + if plan_parameters is not None: + plan_result = planning_component.plan( + parameters=plan_parameters + ) + else: + plan_result = planning_component.plan() + + if not plan_result: + logger.error("Planning failed") + return False + + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + robot.execute( + group_name=GROUP_NAME, + robot_trajectory=robot_trajectory, + blocking=True, + ) + logger.info("Execution finished") + return True + + +def main(args=None): + # ================================ + # MoveItPy 인스턴스 생성 + # ================================ + rclpy.init(args=args) + logger = get_logger("m0609.moveit_py.waypoint_pilz") + + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + logger.info("MoveItPy instance created") + # ================================ + # 플래닝 파라미터 + # ================================ + home_params = PlanRequestParameters(robot) + pilz_params = PlanRequestParameters(robot) + + # MoveIt2 config의 planning_pipelines.pipeline_names와 일치해야 함 + home_params.planning_pipeline = "ompl" + home_params.planner_id = "RRTConnectkConfigDefault" + + pilz_params.planning_pipeline = "pilz_industrial_motion_planner" + pilz_params.planner_id = "PTP" + + # HOME: OMPL + home_params.max_velocity_scaling_factor = 0.2 + home_params.max_acceleration_scaling_factor = 0.1 + home_params.planning_time = 2.0 + + # Waypoint: Pilz PTP + pilz_params.max_velocity_scaling_factor = 0.15 + pilz_params.max_acceleration_scaling_factor = 0.1 + pilz_params.planning_time = 2.0 + + logger.info("=== Move to HOME joints (OMPL + slow) ===") + + # HOME 이동 (조인트 목표 → pose_goal 없이, plan_parameters만 사용) + arm.set_start_state_to_current_state() + arm.set_goal_state( + motion_plan_constraints=build_home_joint_constraints() + ) + plan_and_execute( + robot, + arm, + logger, + plan_parameters=home_params, + ) + + # ================================ + # Waypoints (Pilz PTP, 안전영역 포함) + # ================================ + logger.info("=== Waypoints from HOME, orientation fixed (Pilz PTP) ===") + + WAYPOINTS = [ + { # waypoint 1 + "pos": {"x": 0.493, "y": 0.010, "z": 0.417}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + { # waypoint 2 + "pos": {"x": 0.493, "y": -0.218, "z": 0.417}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + { # waypoint 3 + "pos": {"x": 0.371, "y": -0.218, "z": 0.419}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + ] + + pose_goal = PoseStamped() + pose_goal.header.frame_id = BASE_FRAME + + for i, wp in enumerate(WAYPOINTS, start=1): + pos = wp["pos"] + ori = wp["ori"] + + logger.info( + f"--- Waypoint {i}: " + f"x={pos['x']:.3f}, y={pos['y']:.3f}, z={pos['z']:.3f} ---" + ) + + # pose_goal에 의도 좌표/자세 설정 + pose_goal.pose.position.x = pos["x"] + pose_goal.pose.position.y = pos["y"] + pose_goal.pose.position.z = pos["z"] + + pose_goal.pose.orientation.x = ori["x"] + pose_goal.pose.orientation.y = ori["y"] + pose_goal.pose.orientation.z = ori["z"] + pose_goal.pose.orientation.w = ori["w"] + + # plan_and_execute 안에서 안전영역 + goal 설정 + Pilz 플래너 호출 + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=pilz_params, + ) + + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/mp_waypoint_pilz_lin.py b/src/dsr_practice/dsr_practice/mp_waypoint_pilz_lin.py new file mode 100644 index 0000000..5863d36 --- /dev/null +++ b/src/dsr_practice/dsr_practice/mp_waypoint_pilz_lin.py @@ -0,0 +1,228 @@ +#!/usr/bin/env python3 +import math + +import rclpy +from rclpy.logging import get_logger + +from geometry_msgs.msg import PoseStamped +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters + +# ================================ +# 기본 설정 +# ================================ +GROUP_NAME = "manipulator" # SRDF에 정의된 planning group 이름 +BASE_FRAME = "base_link" # 로봇 베이스 프레임 +EE_LINK = "link_6" # 엔드이펙터 링크 이름 (SRDF/URDF 기준) + +HOME_JOINTS_DEG = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0] +HOME_JOINTS_RAD = [math.radians(d) for d in HOME_JOINTS_DEG] + +# ====== 안전 작업 영역 정의 (base_link 기준) ====== +SAFE_X_MIN = 0.0 # x는 0 이상 +SAFE_Y_MIN = -0.3 # y 하한 +SAFE_Y_MAX = 0.3 # y 상한 +SAFE_Z_MIN = 0.27 # z는 이 값보다 낮아지면 안 됨 +# ================================================== + + +def clamp_to_safe_workspace(x: float, y: float, z: float, logger): + """안전 작업 영역으로 (x, y, z) 클램핑""" + safe_x = x + safe_y = y + safe_z = z + + if safe_x < SAFE_X_MIN: + logger.warning( + f"Requested x ({safe_x:.3f} m) is below safety limit " + f"({SAFE_X_MIN:.3f} m). Clamping to SAFE_X_MIN." + ) + safe_x = SAFE_X_MIN + + if safe_y < SAFE_Y_MIN: + logger.warning( + f"Requested y ({safe_y:.3f} m) is below safety limit " + f"({SAFE_Y_MIN:.3f} m). Clamping to SAFE_Y_MIN." + ) + safe_y = SAFE_Y_MIN + elif safe_y > SAFE_Y_MAX: + logger.warning( + f"Requested y ({safe_y:.3f} m) is above safety limit " + f"({SAFE_Y_MAX:.3f} m). Clamping to SAFE_Y_MAX." + ) + safe_y = SAFE_Y_MAX + + if safe_z < SAFE_Z_MIN: + logger.warning( + f"Requested z ({safe_z:.3f} m) is below safety limit " + f"({SAFE_Z_MIN:.3f} m). Clamping to SAFE_Z_MIN." + ) + safe_z = SAFE_Z_MIN + + return safe_x, safe_y, safe_z + + +def plan_and_execute( + robot: MoveItPy, + planning_component, + logger, + pose_goal: PoseStamped = None, + plan_parameters=None, +): + """ + 공식 문서 스타일 helper: 계획 후 곧바로 실행 + + - pose_goal이 주어지면: + · 안전 영역 클램핑 + · start_state = current + · pose 기반 goal 설정 (EE_LINK) + - 그 다음 plan_parameters 유무에 따라 plan() 호출 후 execute + """ + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + + planning_component.set_start_state_to_current_state() + planning_component.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link=EE_LINK, + ) + + logger.info("Planning trajectory") + + if plan_parameters is not None: + plan_result = planning_component.plan( + parameters=plan_parameters + ) + else: + plan_result = planning_component.plan() + + if not plan_result: + logger.error("Planning failed") + return False + + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + robot.execute( + group_name=GROUP_NAME, + robot_trajectory=robot_trajectory, + blocking=True, + ) + logger.info("Execution finished") + return True + + +def main(args=None): + # ================================ + # MoveItPy 인스턴스 생성 + # ================================ + rclpy.init(args=args) + logger = get_logger("m0609.moveit_py.waypoint_pilz") + + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + logger.info("MoveItPy instance created") + robot_model = robot.get_robot_model() + + # ================================ + # 플래닝 파라미터 + # ================================ + home_params = PlanRequestParameters(robot) + pilz_params = PlanRequestParameters(robot) + + # MoveIt2 config의 planning_pipelines.pipeline_names와 일치해야 함 + home_params.planning_pipeline = "ompl" + home_params.planner_id = "RRTConnect" + + pilz_params.planning_pipeline = "pilz_industrial_motion_planner" + pilz_params.planner_id = "LIN" + + # HOME: OMPL + home_params.max_velocity_scaling_factor = 0.2 + home_params.max_acceleration_scaling_factor = 0.1 + home_params.planning_time = 2.0 + + # Waypoint: Pilz LIN + pilz_params.max_velocity_scaling_factor = 0.05 # 기존 0.10 -> 0.05 + pilz_params.max_acceleration_scaling_factor = 0.03 # 기존 0.10 -> 0.03 + pilz_params.planning_time = 2.0 + + logger.info("=== Move to HOME joints (OMPL + slow) ===") + + home_state = RobotState(robot_model) + home_state.set_joint_group_positions(GROUP_NAME, HOME_JOINTS_RAD) + home_state.update() + + # HOME 이동 (조인트 목표 → pose_goal 없이, plan_parameters만 사용) + arm.set_start_state_to_current_state() + arm.set_goal_state(robot_state=home_state) + plan_and_execute( + robot, + arm, + logger, + plan_parameters=home_params, + ) + + # ================================ + # Waypoints (Pilz LIN, 안전영역 포함) + # ================================ + logger.info("=== Waypoints from HOME, orientation fixed (Pilz LIN) ===") + + WAYPOINTS = [ + { # waypoint 1 (높게 시작) + "pos": {"x": 0.45, "y": 0.22, "z": 0.52}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + { # waypoint 2 (대각선으로 내려가며 이동) + "pos": {"x": 0.62, "y": -0.18, "z": 0.33}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + { # waypoint 3 (다시 대각선으로 올라가며 이동) + "pos": {"x": 0.36, "y": -0.26, "z": 0.55}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + ] + + pose_goal = PoseStamped() + pose_goal.header.frame_id = BASE_FRAME + + for i, wp in enumerate(WAYPOINTS, start=1): + pos = wp["pos"] + ori = wp["ori"] + + logger.info( + f"--- Waypoint {i}: " + f"x={pos['x']:.3f}, y={pos['y']:.3f}, z={pos['z']:.3f} ---" + ) + + # pose_goal에 의도 좌표/자세 설정 + pose_goal.pose.position.x = pos["x"] + pose_goal.pose.position.y = pos["y"] + pose_goal.pose.position.z = pos["z"] + + pose_goal.pose.orientation.x = ori["x"] + pose_goal.pose.orientation.y = ori["y"] + pose_goal.pose.orientation.z = ori["z"] + pose_goal.pose.orientation.w = ori["w"] + + # plan_and_execute 안에서 안전영역 + goal 설정 + Pilz 플래너 호출 + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=pilz_params, + ) + + robot.shutdown() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/onrobot.py b/src/dsr_practice/dsr_practice/onrobot.py new file mode 100644 index 0000000..22ae003 --- /dev/null +++ b/src/dsr_practice/dsr_practice/onrobot.py @@ -0,0 +1,184 @@ +#!/usr/bin/env python3 + +from pymodbus.client.sync import ModbusTcpClient as ModbusClient + + +class RG(): + + def __init__(self, gripper, ip, port): + self.client = ModbusClient( + ip, + port=port, + stopbits=1, + bytesize=8, + parity='E', + baudrate=115200, + timeout=1) + if gripper not in ['rg2', 'rg6']: + print("Please specify either rg2 or rg6.") + return + self.gripper = gripper # RG2/6 + if self.gripper == 'rg2': + self.max_width = 1100 + self.max_force = 400 + elif self.gripper == 'rg6': + self.max_width = 1600 + self.max_force = 1200 + self.open_connection() + + def open_connection(self): + """Opens the connection with a gripper.""" + self.client.connect() + + def close_connection(self): + """Closes the connection with the gripper.""" + self.client.close() + + def get_fingertip_offset(self): + """Reads the current fingertip offset in 1/10 millimeters. + Please note that the value is a signed two's complement number. + """ + result = self.client.read_holding_registers( + address=258, count=1, unit=65) + offset_mm = result.registers[0] / 10.0 + return offset_mm + + def get_width(self): + """Reads current width between gripper fingers in 1/10 millimeters. + Please note that the width is provided without any fingertip offset, + as it is measured between the insides of the aluminum fingers. + """ + result = self.client.read_holding_registers( + address=267, count=1, unit=65) + width_mm = result.registers[0] / 10.0 + return width_mm + + def get_status(self): + """Reads current device status. + This status field indicates the status of the gripper and its motion. + It is composed of 7 flags, described in the table below. + + Bit Name Description + 0 (LSB): busy High (1) when a motion is ongoing, + low (0) when not. + The gripper will only accept new commands + when this flag is low. + 1: grip detected High (1) when an internal- or + external grip is detected. + 2: S1 pushed High (1) when safety switch 1 is pushed. + 3: S1 trigged High (1) when safety circuit 1 is activated. + The gripper will not move + while this flag is high; + can only be reset by power cycling. + 4: S2 pushed High (1) when safety switch 2 is pushed. + 5: S2 trigged High (1) when safety circuit 2 is activated. + The gripper will not move + while this flag is high; + can only be reset by power cycling. + 6: safety error High (1) when on power on any of + the safety switch is pushed. + 10-16: reserved Not used. + """ + # address : register number + # count : number of registers to be read + # unit : slave device address + result = self.client.read_holding_registers( + address=268, count=1, unit=65) + status = format(result.registers[0], '016b') + status_list = [0] * 7 + if int(status[-1]): + print("A motion is ongoing so new commands are not accepted.") + status_list[0] = 1 + if int(status[-2]): + print("An internal- or external grip is detected.") + status_list[1] = 1 + if int(status[-3]): + print("Safety switch 1 is pushed.") + status_list[2] = 1 + if int(status[-4]): + print("Safety circuit 1 is activated so it will not move.") + status_list[3] = 1 + if int(status[-5]): + print("Safety switch 2 is pushed.") + status_list[4] = 1 + if int(status[-6]): + print("Safety circuit 2 is activated so it will not move.") + status_list[5] = 1 + if int(status[-7]): + print("Any of the safety switch is pushed.") + status_list[6] = 1 + + return status_list + + def get_width_with_offset(self): + """Reads current width between gripper fingers in 1/10 millimeters. + The set fingertip offset is considered. + """ + result = self.client.read_holding_registers( + address=275, count=1, unit=65) + width_mm = result.registers[0] / 10.0 + return width_mm + + def set_control_mode(self, command): + """The control field is used to start and stop gripper motion. + Only one option should be set at a time. + Please note that the gripper will not start a new motion + before the one currently being executed is done + (see busy flag in the Status field). + The valid flags are: + + 1 (0x0001): grip + Start the motion, with the target force and width. + Width is calculated without the fingertip offset. + Please note that the gripper will ignore this command + if the busy flag is set in the status field. + 8 (0x0008): stop + Stop the current motion. + 16 (0x0010): grip_w_offset + Same as grip, but width is calculated + with the set fingertip offset. + """ + result = self.client.write_register( + address=2, value=command, unit=65) + + def set_target_force(self, force_val): + """Writes the target force to be reached + when gripping and holding a workpiece. + It must be provided in 1/10th Newtons. + The valid range is 0 to 400 for the RG2 and 0 to 1200 for the RG6. + """ + result = self.client.write_register( + address=0, value=force_val, unit=65) + + def set_target_width(self, width_val): + """Writes the target width between + the finger to be moved to and maintained. + It must be provided in 1/10th millimeters. + The valid range is 0 to 1100 for the RG2 and 0 to 1600 for the RG6. + Please note that the target width should be provided + corrected for any fingertip offset, + as it is measured between the insides of the aluminum fingers. + """ + result = self.client.write_register( + address=1, value=width_val, unit=65) + + def close_gripper(self, force_val=400): + """Closes gripper.""" + params = [force_val, 0, 16] + print("Start closing gripper.") + result = self.client.write_registers( + address=0, values=params, unit=65) + + def open_gripper(self, force_val=400): + """Opens gripper.""" + params = [force_val, self.max_width, 16] + print("Start opening gripper.") + result = self.client.write_registers( + address=0, values=params, unit=65) + + def move_gripper(self, width_val, force_val=400): + """Moves gripper to the specified width.""" + params = [force_val, width_val, 16] + print("Start moving gripper.") + result = self.client.write_registers( + address=0, values=params, unit=65) \ No newline at end of file diff --git a/src/dsr_practice/dsr_practice/package.xml b/src/dsr_practice/dsr_practice/package.xml new file mode 100644 index 0000000..aa3a275 --- /dev/null +++ b/src/dsr_practice/dsr_practice/package.xml @@ -0,0 +1,27 @@ + + + + dsr_practice + 0.0.0 + TODO: Package description + deeptree + TODO: License declaration + + rclpy + geometry_msgs + sensor_msgs + cv_bridge + moveit_py + dsr_msgs2 + + python3-pymodbus + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/dsr_practice/dsr_practice/pick_and_place.py b/src/dsr_practice/dsr_practice/pick_and_place.py new file mode 100644 index 0000000..a32bf07 --- /dev/null +++ b/src/dsr_practice/dsr_practice/pick_and_place.py @@ -0,0 +1,305 @@ +#!/usr/bin/env python3 + +import math +import time + +import rclpy +from rclpy.logging import get_logger + +from geometry_msgs.msg import PoseStamped +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters + +# ====== OnRobot RG2 설정 ====== +from .onrobot import RG # 같은 패키지 내부의 onrobot.py 사용 + +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 + +# 그리퍼 폭 (raw 단위: 1/10 mm) +GRIPPER_OPEN_WIDTH = 500 # 50.0 mm +GRIPPER_CLOSE_WIDTH = 150 # 20.0 mm +GRIPPER_FORCE = 300 # 약 20 N + +# ================================ +# 기본 설정 +# ================================ +GROUP_NAME = "manipulator" # SRDF에 정의된 planning group 이름 +BASE_FRAME = "base_link" # 로봇 베이스 프레임 +EE_LINK = "link_6" # 엔드이펙터 링크 이름 (SRDF/URDF 기준) + +HOME_JOINTS_DEG = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0] +HOME_JOINTS_RAD = [math.radians(d) for d in HOME_JOINTS_DEG] + +# ====== 안전 작업 영역 정의 (base_link 기준) ====== +SAFE_X_MIN = 0.0 # x는 0 이상 +SAFE_Y_MIN = -0.3 # y 하한 +SAFE_Y_MAX = 0.3 # y 상한 +SAFE_Z_MIN = 0.27 # z는 이 값보다 낮아지면 안 됨 +# ================================================== + +# ====== 기어 Pick/Place 포즈 (base_link 기준) ====== +GEAR_TASK = { + "pick": { + "pos": {"x": 0.427, "y": 0.148, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + "place": { + "pos": {"x": 0.426, "y": -0.153, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, +} + +APPROACH_OFFSET = 0.05 # 위에서 접근/후퇴할 거리 [m] + + +def clamp_to_safe_workspace(x: float, y: float, z: float, logger): + """안전 작업 영역으로 (x, y, z) 클램핑""" + safe_x = x + safe_y = y + safe_z = z + + if safe_x < SAFE_X_MIN: + logger.warning( + f"Requested x ({safe_x:.3f} m) is below safety limit " + f"({SAFE_X_MIN:.3f} m). Clamping to SAFE_X_MIN." + ) + safe_x = SAFE_X_MIN + + if safe_y < SAFE_Y_MIN: + logger.warning( + f"Requested y ({safe_y:.3f} m) is below safety limit " + f"({SAFE_Y_MIN:.3f} m). Clamping to SAFE_Y_MIN." + ) + safe_y = SAFE_Y_MIN + elif safe_y > SAFE_Y_MAX: + logger.warning( + f"Requested y ({safe_y:.3f} m) is above safety limit " + f"({SAFE_Y_MAX:.3f} m). Clamping to SAFE_Y_MAX." + ) + safe_y = SAFE_Y_MAX + + if safe_z < SAFE_Z_MIN: + logger.warning( + f"Requested z ({safe_z:.3f} m) is below safety limit " + f"({SAFE_Z_MIN:.3f} m). Clamping to SAFE_Z_MIN." + ) + safe_z = SAFE_Z_MIN + + return safe_x, safe_y, safe_z + + +def plan_and_execute( + robot: MoveItPy, + planning_component, + logger, + pose_goal: PoseStamped = None, + plan_parameters=None, +): + """ + 공식 문서 스타일 helper: 계획 후 곧바로 실행 + + - pose_goal이 주어지면: + · 안전 영역 클램핑 + · start_state = current + · pose 기반 goal 설정 (EE_LINK) + - 그 다음 plan_parameters 유무에 따라 plan() 호출 후 execute + """ + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + + planning_component.set_start_state_to_current_state() + planning_component.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link=EE_LINK, + ) + + logger.info("Planning trajectory") + + if plan_parameters is not None: + plan_result = planning_component.plan( + parameters=plan_parameters + ) + else: + plan_result = planning_component.plan() + + if not plan_result: + logger.error("Planning failed") + return False + + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + robot.execute( + group_name=GROUP_NAME, + robot_trajectory=robot_trajectory, + blocking=True, + ) + logger.info("Execution finished") + return True + + +def main(args=None): + rclpy.init(args=args) + logger = get_logger("gear_pick_place_simple") + + # ---- Gripper ---- + gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + time.sleep(0.5) + gripper.move_gripper(GRIPPER_OPEN_WIDTH, GRIPPER_FORCE) + + # ---- MoveIt ---- + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + robot_model = robot.get_robot_model() + + # ---- PlanRequestParameters (HOME / Pilz) ---- + home_params = PlanRequestParameters(robot) + home_params.planning_pipeline = "ompl" + home_params.planner_id = "RRTConnectkConfigDefault" + home_params.max_velocity_scaling_factor = 0.2 + home_params.max_acceleration_scaling_factor = 0.1 + home_params.planning_time = 2.0 + + pilz_params = PlanRequestParameters(robot) + pilz_params.planning_pipeline = "pilz_industrial_motion_planner" + pilz_params.planner_id = "PTP" + pilz_params.max_velocity_scaling_factor = 0.15 + pilz_params.max_acceleration_scaling_factor = 0.1 + pilz_params.planning_time = 2.0 + + # ---- HOME 자세로 이동 (joint goal) ---- + logger.info("=== HOME 자세로 이동 ===") + home_state = RobotState(robot_model) + home_state.set_joint_group_positions(GROUP_NAME, HOME_JOINTS_RAD) + home_state.update() + + arm.set_start_state_to_current_state() + arm.set_goal_state(robot_state=home_state) + plan_and_execute(robot, arm, logger, plan_parameters=home_params) + + logger.info("=== Gear Pick & Place 시작 ===") + + # ---- PoseStamped 공용 객체 준비 ---- + pose_goal = PoseStamped() + pose_goal.header.frame_id = BASE_FRAME + + pick = GEAR_TASK["pick"] + place = GEAR_TASK["place"] + + # ============================ + # PICK + # ============================ + pos = pick["pos"] + ori = pick["ori"] + + # 1) pick 위에서 접근 + pose_goal.pose.position.x = pos["x"] + pose_goal.pose.position.y = pos["y"] + pose_goal.pose.position.z = pos["z"] + APPROACH_OFFSET + + pose_goal.pose.orientation.x = ori["x"] + pose_goal.pose.orientation.y = ori["y"] + pose_goal.pose.orientation.z = ori["z"] + pose_goal.pose.orientation.w = ori["w"] + + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=pilz_params, + ) + + # 2) pick 높이까지 내려가기 + pose_goal.pose.position.z = pos["z"] + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=pilz_params, + ) + + # 3) 그리퍼 닫기 (집기) + logger.info("Gripper CLOSE – gear pick") + gripper.move_gripper(GRIPPER_CLOSE_WIDTH, GRIPPER_FORCE) + time.sleep(1.0) + + # 4) 다시 위로 + pose_goal.pose.position.z = pos["z"] + APPROACH_OFFSET + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=pilz_params, + ) + + # ============================ + # PLACE + # ============================ + pos = place["pos"] + ori = place["ori"] + + # 5) place 위에서 접근 + pose_goal.pose.position.x = pos["x"] + pose_goal.pose.position.y = pos["y"] + pose_goal.pose.position.z = pos["z"] + APPROACH_OFFSET + + pose_goal.pose.orientation.x = ori["x"] + pose_goal.pose.orientation.y = ori["y"] + pose_goal.pose.orientation.z = ori["z"] + pose_goal.pose.orientation.w = ori["w"] + + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=pilz_params, + ) + + # 6) place 높이까지 내려가기 + pose_goal.pose.position.z = pos["z"] + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=pilz_params, + ) + + # 7) 그리퍼 열기 (놓기) + logger.info("Gripper OPEN – gear place") + gripper.move_gripper(GRIPPER_OPEN_WIDTH, GRIPPER_FORCE) + time.sleep(1.0) + + # 8) 다시 위로 + pose_goal.pose.position.z = pos["z"] + APPROACH_OFFSET + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=pilz_params, + ) + + # 마지막으로 HOME으로 복귀 (선택) + arm.set_start_state_to_current_state() + arm.set_goal_state(robot_state=home_state) + plan_and_execute(robot, arm, logger, plan_parameters=home_params) + + logger.info("=== Gear Pick & Place 노드 종료 ===") + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/realsense_data_collector.py b/src/dsr_practice/dsr_practice/realsense_data_collector.py new file mode 100644 index 0000000..03e47ba --- /dev/null +++ b/src/dsr_practice/dsr_practice/realsense_data_collector.py @@ -0,0 +1,213 @@ +import csv +import json +import os +from pathlib import Path + +import cv2 +import rclpy +from cv_bridge import CvBridge +from rclpy.node import Node +from sensor_msgs.msg import CameraInfo, Image + + +class RealSenseDataCollector(Node): + def __init__(self): + super().__init__("realsense_data_collector") + + self.declare_parameter("color_topic", "/camera/camera/color/image_raw") + self.declare_parameter( + "depth_topic", "/camera/camera/aligned_depth_to_color/image_raw" + ) + self.declare_parameter("camera_info_topic", "/camera/camera/color/camera_info") + self.declare_parameter( + "output_dir", "/home/ssu/ros2_ws/realsense_dataset/raw" + ) + self.declare_parameter("save_interval_sec", 1.0) + self.declare_parameter("max_frames", 0) + self.declare_parameter("save_depth", True) + self.declare_parameter("jpeg_quality", 95) + + self.color_topic = ( + self.get_parameter("color_topic").get_parameter_value().string_value + ) + self.depth_topic = ( + self.get_parameter("depth_topic").get_parameter_value().string_value + ) + self.camera_info_topic = ( + self.get_parameter("camera_info_topic").get_parameter_value().string_value + ) + self.output_dir = Path( + self.get_parameter("output_dir").get_parameter_value().string_value + ).expanduser() + self.save_interval_sec = ( + self.get_parameter("save_interval_sec").get_parameter_value().double_value + ) + self.max_frames = ( + self.get_parameter("max_frames").get_parameter_value().integer_value + ) + self.save_depth = ( + self.get_parameter("save_depth").get_parameter_value().bool_value + ) + self.jpeg_quality = ( + self.get_parameter("jpeg_quality").get_parameter_value().integer_value + ) + + self.bridge = CvBridge() + self.latest_color = None + self.latest_depth = None + self.latest_color_stamp = None + self.latest_depth_stamp = None + self.camera_info_saved = False + self.frame_index = 0 + + self.color_dir = self.output_dir / "color" + self.depth_dir = self.output_dir / "depth" + self.meta_dir = self.output_dir / "meta" + self.color_dir.mkdir(parents=True, exist_ok=True) + if self.save_depth: + self.depth_dir.mkdir(parents=True, exist_ok=True) + self.meta_dir.mkdir(parents=True, exist_ok=True) + + self.metadata_path = self.meta_dir / "frames.csv" + self._init_metadata_file() + + self.create_subscription(Image, self.color_topic, self.color_callback, 10) + if self.save_depth: + self.create_subscription(Image, self.depth_topic, self.depth_callback, 10) + self.create_subscription( + CameraInfo, self.camera_info_topic, self.camera_info_callback, 10 + ) + self.create_timer(self.save_interval_sec, self.save_latest_frames) + + self.get_logger().info(f"Saving RealSense data to {self.output_dir}") + self.get_logger().info(f"Color topic: {self.color_topic}") + if self.save_depth: + self.get_logger().info(f"Depth topic: {self.depth_topic}") + self.get_logger().info("Press Ctrl+C to stop collecting data.") + + def _init_metadata_file(self): + if self.metadata_path.exists(): + return + + with self.metadata_path.open("w", newline="") as csv_file: + writer = csv.writer(csv_file) + writer.writerow( + [ + "frame_index", + "color_file", + "depth_file", + "color_stamp_sec", + "color_stamp_nanosec", + "depth_stamp_sec", + "depth_stamp_nanosec", + ] + ) + + def color_callback(self, msg): + self.latest_color = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") + self.latest_color_stamp = msg.header.stamp + + def depth_callback(self, msg): + self.latest_depth = self.bridge.imgmsg_to_cv2( + msg, desired_encoding="passthrough" + ) + self.latest_depth_stamp = msg.header.stamp + + def camera_info_callback(self, msg): + if self.camera_info_saved: + return + + camera_info = { + "width": msg.width, + "height": msg.height, + "distortion_model": msg.distortion_model, + "d": list(msg.d), + "k": list(msg.k), + "r": list(msg.r), + "p": list(msg.p), + "intrinsics": { + "fx": msg.k[0], + "fy": msg.k[4], + "cx": msg.k[2], + "cy": msg.k[5], + }, + } + camera_info_path = self.meta_dir / "camera_info.json" + with camera_info_path.open("w") as json_file: + json.dump(camera_info, json_file, indent=2) + + self.camera_info_saved = True + self.get_logger().info(f"Saved camera info to {camera_info_path}") + + def save_latest_frames(self): + if self.latest_color is None: + self.get_logger().warn("Waiting for color image...") + return + + if self.save_depth and self.latest_depth is None: + self.get_logger().warn("Waiting for aligned depth image...") + return + + self.frame_index += 1 + base_name = f"frame_{self.frame_index:06d}" + color_file = f"{base_name}.jpg" + depth_file = f"{base_name}.png" if self.save_depth else "" + + color_path = self.color_dir / color_file + cv2.imwrite( + str(color_path), + self.latest_color, + [int(cv2.IMWRITE_JPEG_QUALITY), self.jpeg_quality], + ) + + if self.save_depth: + depth_path = self.depth_dir / depth_file + cv2.imwrite(str(depth_path), self.latest_depth) + + self._append_metadata(color_file, depth_file) + self.get_logger().info(f"Saved {base_name}") + + if self.max_frames > 0 and self.frame_index >= self.max_frames: + self.get_logger().info(f"Reached max_frames={self.max_frames}. Stopping.") + rclpy.shutdown() + + def _append_metadata(self, color_file, depth_file): + color_stamp_sec = self.latest_color_stamp.sec if self.latest_color_stamp else "" + color_stamp_nanosec = ( + self.latest_color_stamp.nanosec if self.latest_color_stamp else "" + ) + depth_stamp_sec = self.latest_depth_stamp.sec if self.latest_depth_stamp else "" + depth_stamp_nanosec = ( + self.latest_depth_stamp.nanosec if self.latest_depth_stamp else "" + ) + + with self.metadata_path.open("a", newline="") as csv_file: + writer = csv.writer(csv_file) + writer.writerow( + [ + self.frame_index, + color_file, + depth_file, + color_stamp_sec, + color_stamp_nanosec, + depth_stamp_sec, + depth_stamp_nanosec, + ] + ) + + +def main(args=None): + rclpy.init(args=args) + node = RealSenseDataCollector() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/resource/dsr_practice b/src/dsr_practice/dsr_practice/resource/dsr_practice new file mode 100644 index 0000000..e69de29 diff --git a/src/dsr_practice/dsr_practice/setup.cfg b/src/dsr_practice/dsr_practice/setup.cfg new file mode 100644 index 0000000..e4a9452 --- /dev/null +++ b/src/dsr_practice/dsr_practice/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/dsr_practice +[install] +install_scripts=$base/lib/dsr_practice diff --git a/src/dsr_practice/dsr_practice/setup.py b/src/dsr_practice/dsr_practice/setup.py new file mode 100644 index 0000000..9e3c281 --- /dev/null +++ b/src/dsr_practice/dsr_practice/setup.py @@ -0,0 +1,52 @@ +from setuptools import find_packages, setup +from glob import glob + +package_name = 'dsr_practice' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name + '/launch', glob('launch/*.launch.py')), + ( + 'share/' + package_name + '/config', + glob('config/*.yaml') + glob('config/*.npy') + ), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='deeptree', + maintainer_email='deeptree@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'mp_basic = dsr_practice.mp_basic:main', + 'mp_waypoint = dsr_practice.mp_waypoint:main', + 'mp_waypoint_pilz = dsr_practice.mp_waypoint_pilz:main', + 'mp_waypoint_pilz_lin = dsr_practice.mp_waypoint_pilz_lin:main', + 'collision_obstacle = dsr_practice.collision_obstacle:main', + 'gripper = dsr_practice.gripper:main', + 'gear_assembly = dsr_practice.gear_assembly:main', + 'click_pick_node = dsr_practice.click_pick_node:main', + 'bar_sort_node = dsr_practice.bar_sort_node:main', + 'bar_detect_test = dsr_practice.bar_detect_test:main', + 'stt_node = dsr_practice.stt_node:main', + 'stt_robot_control = dsr_practice.stt_robot_control:main', + 'stt_pick_and_place = dsr_practice.stt_pick_and_place:main', + 'realsense_data_collector = dsr_practice.realsense_data_collector:main', + 'syrup_pump_press = dsr_practice.syrup_pump_press:main', + 'yolo_cup_pick_node = dsr_practice.yolo_cup_pick_node:main', + 'joint_state_relay = dsr_practice.joint_state_relay:main', + ], + }, +) diff --git a/src/dsr_practice/dsr_practice/stt_node.py b/src/dsr_practice/dsr_practice/stt_node.py new file mode 100644 index 0000000..81aefbf --- /dev/null +++ b/src/dsr_practice/dsr_practice/stt_node.py @@ -0,0 +1,108 @@ +import rclpy +from rclpy.node import Node +from std_msgs.msg import String + +import speech_recognition as sr + + +class SttNode(Node): + + def __init__(self): + super().__init__("stt_node") + + self.declare_parameter("language", "ko-KR") + self.declare_parameter("device_index", -1) + self.declare_parameter("energy_threshold", 300.0) + self.declare_parameter("pause_threshold", 0.8) + self.declare_parameter("phrase_time_limit", 5.0) + self.declare_parameter("dynamic_energy", True) + self.declare_parameter("ambient_duration", 1.0) + + self._lang = self.get_parameter("language").get_parameter_value().string_value + self._device_idx = self.get_parameter("device_index").get_parameter_value().integer_value + energy_thresh = self.get_parameter("energy_threshold").get_parameter_value().double_value + pause_thresh = self.get_parameter("pause_threshold").get_parameter_value().double_value + self._phrase_lim = self.get_parameter("phrase_time_limit").get_parameter_value().double_value + dynamic_energy = self.get_parameter("dynamic_energy").get_parameter_value().bool_value + ambient_duration = self.get_parameter("ambient_duration").get_parameter_value().double_value + + self._pub = self.create_publisher(String, "/stt_result", 10) + + self._log_devices() + + self._recognizer = sr.Recognizer() + self._recognizer.energy_threshold = energy_thresh + self._recognizer.pause_threshold = pause_thresh + self._recognizer.dynamic_energy_threshold = dynamic_energy + + device = self._device_idx if self._device_idx >= 0 else None + try: + self._mic = sr.Microphone(device_index=device) + except Exception as e: + self.get_logger().error(f"마이크 열기 실패: {e}") + raise + + with self._mic as source: + self.get_logger().info(f"주변 소음 측정 중 ({ambient_duration:.1f}s) ...") + self._recognizer.adjust_for_ambient_noise(source, duration=ambient_duration) + self.get_logger().info( + f"energy_threshold={self._recognizer.energy_threshold:.1f}" + ) + + self._stop_listen = self._recognizer.listen_in_background( + self._mic, self._on_audio, phrase_time_limit=self._phrase_lim, + ) + + self.get_logger().info( + f"STT 준비 완료 언어={self._lang} device_index={self._device_idx} " + f"phrase_time_limit={self._phrase_lim:.1f}s" + ) + + def _log_devices(self): + self.get_logger().info("=== 마이크 장치 목록 ===") + for idx, name in enumerate(sr.Microphone.list_microphone_names()): + mark = " ◀" if idx == self._device_idx else "" + self.get_logger().info(f" [{idx}] {name}{mark}") + + def _on_audio(self, recognizer: sr.Recognizer, audio: sr.AudioData): + try: + text = recognizer.recognize_google(audio, language=self._lang) + except sr.UnknownValueError: + return + except sr.RequestError as e: + self.get_logger().warning(f"Google STT 요청 실패: {e}") + return + + text = (text or "").strip() + if not text: + return + + self.get_logger().info(f"[STT] {text}") + msg = String() + msg.data = text + self._pub.publish(msg) + + def destroy_node(self): + stop = getattr(self, "_stop_listen", None) + if stop is not None: + try: + stop(wait_for_stop=False) + except Exception: + pass + super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + node = SttNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/stt_pick_and_place.py b/src/dsr_practice/dsr_practice/stt_pick_and_place.py new file mode 100644 index 0000000..fe107e6 --- /dev/null +++ b/src/dsr_practice/dsr_practice/stt_pick_and_place.py @@ -0,0 +1,387 @@ +#!/usr/bin/env python3 +""" +STT 기반 Pick & Place 제어 노드 + +/stt_result (std_msgs/String) 구독 + -> 키워드 매핑 -> 명령 큐 -> 워커 스레드에서 MoveItPy + Gripper 실행 +""" +import math +import os +import queue +import tempfile +import threading +import time + +import rclpy +from rclpy.executors import MultiThreadedExecutor +from rclpy.logging import get_logger +from rclpy.node import Node +from std_msgs.msg import String + +from geometry_msgs.msg import PoseStamped +from moveit.planning import MoveItPy, PlanRequestParameters +from moveit_msgs.msg import Constraints, JointConstraint + +try: + from gtts import gTTS + import pygame + _TTS_OK = True +except ImportError: + _TTS_OK = False + +from .onrobot import RG + +# ----- Gripper ----- +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 +GRIPPER_OPEN_WIDTH = 500 +GRIPPER_CLOSE_WIDTH = 150 +GRIPPER_FORCE = 300 + +# ----- MoveIt / Robot ----- +GROUP_NAME = "manipulator" +BASE_FRAME = "base_link" +EE_LINK = "link_6" + +HOME_JOINTS_RAD = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(0.0), +} + +# ----- Safety bounds ----- +SAFE_X_MIN = 0.0 +SAFE_Y_MIN = -0.3 +SAFE_Y_MAX = 0.3 +SAFE_Z_MIN = 0.27 + +# ----- Task poses ----- +TASK_POSES = { + "pick": { + "pos": {"x": 0.427, "y": 0.148, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + "place": { + "pos": {"x": 0.426, "y": -0.153, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, +} +APPROACH_OFFSET = 0.05 + + +TTS_PHRASES = { + "home": "홈 위치로 이동합니다", + "pick": "물체를 집습니다", + "place": "물체를 내려놓습니다", + "pickplace": "픽 앤 플레이스를 시작합니다", + "stop": "정지합니다", + "done": "완료되었습니다", + "fail": "실행에 실패했습니다", +} + + +class _TtsPlayer: + """gTTS + pygame 기반 비동기 음성 재생 (캐시 사용).""" + + def __init__(self, lang: str = "ko", enabled: bool = True): + self._enabled = enabled and _TTS_OK + self._lang = lang + self._cache: dict[str, str] = {} + if self._enabled: + try: + pygame.mixer.init() + except Exception: + self._enabled = False + + def speak(self, text: str): + if not self._enabled or not text: + return + try: + path = self._cache.get(text) + if not path or not os.path.exists(path): + fd, path = tempfile.mkstemp(suffix=".mp3", prefix="dsr_tts_") + os.close(fd) + gTTS(text=text, lang=self._lang).save(path) + self._cache[text] = path + pygame.mixer.music.load(path) + pygame.mixer.music.play() + except Exception: + pass + + +KEYWORD_MAP: dict[str, str] = { + "홈": "home", + "home": "home", + "홈으로": "home", + "픽": "pick", + "집어": "pick", + "잡아": "pick", + "pick": "pick", + "플레이스": "place", + "놓아": "place", + "내려놔": "place", + "place": "place", + "픽앤플레이스": "pickplace", + "픽플레이스": "pickplace", + "pickandplace": "pickplace", + "pickplace": "pickplace", + "정지": "stop", + "멈춰": "stop", + "스톱": "stop", + "stop": "stop", +} + + +def _text_to_cmd(text: str) -> str | None: + normalized = text.lower().replace(" ", "") + for kw, cmd in KEYWORD_MAP.items(): + if kw in normalized: + return cmd + return None + + +def _clamp_to_safe_workspace(x: float, y: float, z: float, logger): + sx, sy, sz = x, y, z + + if sx < SAFE_X_MIN: + logger.warning(f"x safety clamp: {sx:.3f} -> {SAFE_X_MIN:.3f}") + sx = SAFE_X_MIN + if sy < SAFE_Y_MIN: + logger.warning(f"y safety clamp: {sy:.3f} -> {SAFE_Y_MIN:.3f}") + sy = SAFE_Y_MIN + elif sy > SAFE_Y_MAX: + logger.warning(f"y safety clamp: {sy:.3f} -> {SAFE_Y_MAX:.3f}") + sy = SAFE_Y_MAX + if sz < SAFE_Z_MIN: + logger.warning(f"z safety clamp: {sz:.3f} -> {SAFE_Z_MIN:.3f}") + sz = SAFE_Z_MIN + + return sx, sy, sz + + +def _build_home_joint_constraints() -> list[Constraints]: + constraints = Constraints() + joint_names = [f"joint_{i}" for i in range(1, 7)] + + for joint_name, position in zip(joint_names, HOME_JOINTS_RAD.values()): + joint_constraint = JointConstraint() + joint_constraint.joint_name = joint_name + joint_constraint.position = position + joint_constraint.tolerance_above = 0.001 + joint_constraint.tolerance_below = 0.001 + joint_constraint.weight = 1.0 + constraints.joint_constraints.append(joint_constraint) + + return [constraints] + + +def _plan_and_execute(robot, arm, logger, plan_params=None) -> bool: + result = arm.plan(parameters=plan_params) if plan_params else arm.plan() + if not result: + logger.error("Planning failed") + return False + robot.execute(group_name=GROUP_NAME, robot_trajectory=result.trajectory, blocking=True) + return True + + +def _move_home(robot, arm, logger, home_params) -> bool: + arm.set_start_state_to_current_state() + arm.set_goal_state(motion_plan_constraints=_build_home_joint_constraints()) + return _plan_and_execute(robot, arm, logger, plan_params=home_params) + + +def _move_pose(robot, arm, logger, pose_goal: PoseStamped, pilz_params) -> bool: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + sx, sy, sz = _clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + + arm.set_start_state_to_current_state() + arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link=EE_LINK) + return _plan_and_execute(robot, arm, logger, plan_params=pilz_params) + + +class SttPickAndPlaceNode(Node): + def __init__(self, robot: MoveItPy, arm, home_params, pilz_params, gripper): + super().__init__("stt_pick_and_place") + + self.declare_parameter("use_tts", True) + use_tts = self.get_parameter("use_tts").get_parameter_value().bool_value + + self._robot = robot + self._arm = arm + self._home = home_params + self._pilz = pilz_params + self._cmd_q: queue.Queue[str] = queue.Queue() + self._holding = False + self._tts = _TtsPlayer(enabled=use_tts) + self._gripper = gripper + + self.create_subscription(String, "/stt_result", self._stt_cb, 10) + threading.Thread(target=self._worker, daemon=True).start() + self.get_logger().info( + f"준비 완료 명령어: home / pick / place / pickplace / stop (tts={'on' if self._tts._enabled else 'off'})" + ) + + def _stt_cb(self, msg: String): + cmd = _text_to_cmd(msg.data) + if cmd is None: + self.get_logger().debug(f"매핑 없음: '{msg.data}'") + return + + if cmd == "stop": + n = 0 + while not self._cmd_q.empty(): + try: + self._cmd_q.get_nowait() + n += 1 + except queue.Empty: + break + self.get_logger().info(f"[STOP] 큐 {n}개 취소") + self._tts.speak(TTS_PHRASES["stop"]) + return + + self._cmd_q.put(cmd) + self.get_logger().info(f"[CMD] '{cmd}' 큐 추가 (크기={self._cmd_q.qsize()})") + + def _build_pose_goal(self, task_key: str, z_value: float) -> PoseStamped: + task = TASK_POSES[task_key] + pose_goal = PoseStamped() + pose_goal.header.frame_id = BASE_FRAME + pose_goal.pose.position.x = task["pos"]["x"] + pose_goal.pose.position.y = task["pos"]["y"] + pose_goal.pose.position.z = z_value + pose_goal.pose.orientation.x = task["ori"]["x"] + pose_goal.pose.orientation.y = task["ori"]["y"] + pose_goal.pose.orientation.z = task["ori"]["z"] + pose_goal.pose.orientation.w = task["ori"]["w"] + return pose_goal + + def _set_gripper(self, logger, width: int): + try: + self._gripper.move_gripper(width, GRIPPER_FORCE) + time.sleep(1.0) + except Exception as e: + logger.error(f"Gripper error: {e}") + + def _run_pick(self, logger) -> bool: + pick = TASK_POSES["pick"]["pos"] + approach = self._build_pose_goal("pick", pick["z"] + APPROACH_OFFSET) + target = self._build_pose_goal("pick", pick["z"]) + + if not _move_pose(self._robot, self._arm, logger, approach, self._pilz): + return False + if not _move_pose(self._robot, self._arm, logger, target, self._pilz): + return False + + logger.info("Gripper CLOSE") + self._set_gripper(logger, GRIPPER_CLOSE_WIDTH) + self._holding = True + + if not _move_pose(self._robot, self._arm, logger, approach, self._pilz): + return False + return True + + def _run_place(self, logger) -> bool: + place = TASK_POSES["place"]["pos"] + approach = self._build_pose_goal("place", place["z"] + APPROACH_OFFSET) + target = self._build_pose_goal("place", place["z"]) + + if not _move_pose(self._robot, self._arm, logger, approach, self._pilz): + return False + if not _move_pose(self._robot, self._arm, logger, target, self._pilz): + return False + + logger.info("Gripper OPEN") + self._set_gripper(logger, GRIPPER_OPEN_WIDTH) + self._holding = False + + if not _move_pose(self._robot, self._arm, logger, approach, self._pilz): + return False + return True + + def _worker(self): + logger = get_logger("stt_pick_and_place.worker") + while True: + try: + cmd = self._cmd_q.get(timeout=1.0) + except queue.Empty: + continue + + logger.info(f"===== '{cmd}' 실행 =====") + self._tts.speak(TTS_PHRASES.get(cmd, "")) + ok = True + + if cmd == "home": + ok = _move_home(self._robot, self._arm, logger, self._home) + elif cmd == "pick": + ok = self._run_pick(logger) + elif cmd == "place": + if not self._holding: + logger.warning("현재 물체를 쥐고 있지 않습니다. place를 계속 진행합니다.") + ok = self._run_place(logger) + elif cmd == "pickplace": + ok = self._run_pick(logger) + if ok: + ok = self._run_place(logger) + + if ok: + logger.info(f"===== '{cmd}' 완료 =====") + else: + logger.error(f"===== '{cmd}' 실패 =====") + self._tts.speak(TTS_PHRASES["done" if ok else "fail"]) + + +def main(args=None): + rclpy.init(args=args) + logger = get_logger("stt_pick_and_place.main") + + gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + time.sleep(0.5) + gripper.move_gripper(GRIPPER_OPEN_WIDTH, GRIPPER_FORCE) + + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + logger.info("MoveItPy 초기화 완료") + + home = PlanRequestParameters(robot) + home.planning_pipeline = "ompl" + home.planner_id = "RRTConnect" + home.max_velocity_scaling_factor = 0.2 + home.max_acceleration_scaling_factor = 0.1 + home.planning_time = 3.0 + + pilz = PlanRequestParameters(robot) + pilz.planning_pipeline = "pilz_industrial_motion_planner" + pilz.planner_id = "PTP" + pilz.max_velocity_scaling_factor = 0.15 + pilz.max_acceleration_scaling_factor = 0.1 + pilz.planning_time = 3.0 + + logger.info("시작 자세를 home으로 이동합니다") + _move_home(robot, arm, logger, home) + + node = SttPickAndPlaceNode(robot, arm, home, pilz, gripper) + executor = MultiThreadedExecutor() + executor.add_node(node) + + logger.info("음성 명령 대기 중 ... (Ctrl+C 종료)") + try: + executor.spin() + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + robot.shutdown() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/stt_robot_control.py b/src/dsr_practice/dsr_practice/stt_robot_control.py new file mode 100644 index 0000000..2888275 --- /dev/null +++ b/src/dsr_practice/dsr_practice/stt_robot_control.py @@ -0,0 +1,376 @@ +#!/usr/bin/env python3 +""" +STT 음성 명령 로봇 제어 노드 (오프셋 이동 방식) + +/stt_result (std_msgs/String) 구독 + → 키워드 매핑 → 명령 큐 → 워커 스레드에서 MoveItPy 실행 + +명령어: + home → HOME 조인트 자세 (OMPL RRTConnect) + 앞/뒤/왼쪽/오른쪽/위/아래 → 현재 EE 위치 기준 5cm 오프셋 이동 (Pilz PTP) + stop → 명령 큐 비우기 +""" +import math +import os +import queue +import tempfile +import threading + +import numpy as np +import rclpy +from rclpy.executors import MultiThreadedExecutor +from rclpy.logging import get_logger +from rclpy.node import Node +from std_msgs.msg import String + +from geometry_msgs.msg import PoseStamped +from moveit.planning import MoveItPy, PlanRequestParameters +from moveit_msgs.msg import Constraints, JointConstraint + +try: + from gtts import gTTS + import pygame + _TTS_OK = True +except ImportError: + _TTS_OK = False + +# ── 로봇 설정 ────────────────────────────────────────────── +GROUP_NAME = "manipulator" +BASE_FRAME = "base_link" +EE_LINK = "link_6" +HOME_JOINTS_RAD = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(0.0), +} + +# ── 안전 작업 영역 (base_link 기준) ─────────────────────── +SAFE_X_MIN = 0.0 +SAFE_Y_MIN = -0.3 +SAFE_Y_MAX = 0.3 +SAFE_Z_MIN = 0.27 + +# ── 방향 → (dx, dy, dz) 오프셋 ─────────────────────────── +JOG_OFFSET = 0.05 # m +DIRECTIONS = { + "forward": ( JOG_OFFSET, 0.0, 0.0), + "backward": (-JOG_OFFSET, 0.0, 0.0), + "left": ( 0.0, JOG_OFFSET, 0.0), + "right": ( 0.0, -JOG_OFFSET, 0.0), + "up": ( 0.0, 0.0, JOG_OFFSET), + "down": ( 0.0, 0.0, -JOG_OFFSET), +} + +# ── 명령별 음성 안내 ────────────────────────────────────── +TTS_PHRASES = { + "home": "홈 위치로 이동합니다", + "forward": "앞으로 이동합니다", + "backward": "뒤로 이동합니다", + "left": "왼쪽으로 이동합니다", + "right": "오른쪽으로 이동합니다", + "up": "위로 이동합니다", + "down": "아래로 이동합니다", + "stop": "정지합니다", + "done": "완료되었습니다", + "fail": "실행에 실패했습니다", +} + +# ── 키워드 → 명령 매핑 ──────────────────────────────────── +KEYWORD_MAP: dict[str, str] = { + "홈": "home", "home": "home", "홈으로": "home", + "왼쪽": "left", "왼": "left", "left": "left", + "오른쪽": "right", "오른": "right", "right": "right", + "앞": "forward", "앞으로": "forward", "전방": "forward", + "front": "forward", "forward": "forward", + "뒤": "backward", "뒤로": "backward", "후방": "backward", + "back": "backward", "backward": "backward", + "위": "up", "위로": "up", "올려": "up", "올라가": "up", "up": "up", + "아래": "down", "아래로": "down", "내려": "down", "내려가": "down", "down": "down", + "정지": "stop", "멈춰": "stop", "스톱": "stop", "stop": "stop", +} + +VALID_CMDS = {"home"} | set(DIRECTIONS.keys()) + + +class _TtsPlayer: + """gTTS + pygame 기반 비동기 음성 재생 (캐시 사용).""" + + def __init__(self, lang: str = "ko", enabled: bool = True): + self._enabled = enabled and _TTS_OK + self._lang = lang + self._cache: dict[str, str] = {} + if self._enabled: + try: + pygame.mixer.init() + except Exception: + self._enabled = False + + def speak(self, text: str): + if not self._enabled or not text: + return + try: + path = self._cache.get(text) + if not path or not os.path.exists(path): + fd, path = tempfile.mkstemp(suffix=".mp3", prefix="dsr_tts_") + os.close(fd) + gTTS(text=text, lang=self._lang).save(path) + self._cache[text] = path + pygame.mixer.music.load(path) + pygame.mixer.music.play() + except Exception: + pass + + +def _text_to_cmd(text: str) -> str | None: + normalized = text.lower().replace(" ", "") + for kw, cmd in KEYWORD_MAP.items(): + if kw in normalized: + return cmd + return None + + +def _clamp_to_safe(x: float, y: float, z: float, logger) -> tuple[float, float, float]: + if x < SAFE_X_MIN: + logger.warning(f"x 클램핑: {x:.3f} → {SAFE_X_MIN:.3f}") + x = SAFE_X_MIN + if y < SAFE_Y_MIN: + logger.warning(f"y 클램핑: {y:.3f} → {SAFE_Y_MIN:.3f}") + y = SAFE_Y_MIN + elif y > SAFE_Y_MAX: + logger.warning(f"y 클램핑: {y:.3f} → {SAFE_Y_MAX:.3f}") + y = SAFE_Y_MAX + if z < SAFE_Z_MIN: + logger.warning(f"z 클램핑: {z:.3f} → {SAFE_Z_MIN:.3f}") + z = SAFE_Z_MIN + return x, y, z + + +def _rot_matrix_to_quat(R: np.ndarray) -> tuple[float, float, float, float]: + """3x3 회전행렬 → (x, y, z, w) 쿼터니언.""" + trace = R[0, 0] + R[1, 1] + R[2, 2] + if trace > 0: + s = 0.5 / math.sqrt(trace + 1.0) + w = 0.25 / s + x = (R[2, 1] - R[1, 2]) * s + y = (R[0, 2] - R[2, 0]) * s + z = (R[1, 0] - R[0, 1]) * s + elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]: + s = 2.0 * math.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) + w = (R[2, 1] - R[1, 2]) / s + x = 0.25 * s + y = (R[0, 1] + R[1, 0]) / s + z = (R[0, 2] + R[2, 0]) / s + elif R[1, 1] > R[2, 2]: + s = 2.0 * math.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) + w = (R[0, 2] - R[2, 0]) / s + x = (R[0, 1] + R[1, 0]) / s + y = 0.25 * s + z = (R[1, 2] + R[2, 1]) / s + else: + s = 2.0 * math.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) + w = (R[1, 0] - R[0, 1]) / s + x = (R[0, 2] + R[2, 0]) / s + y = (R[1, 2] + R[2, 1]) / s + z = 0.25 * s + return x, y, z, w + + +def _plan_and_execute(robot, arm, logger, plan_params=None) -> bool: + logger.info("Planning ...") + result = arm.plan(parameters=plan_params) if plan_params else arm.plan() + if not result: + logger.error("Planning failed") + return False + logger.info("Executing ...") + robot.execute(group_name=GROUP_NAME, robot_trajectory=result.trajectory, blocking=True) + logger.info("Done") + return True + + +def _build_home_joint_constraints() -> list[Constraints]: + constraints = Constraints() + joint_names = [f"joint_{i}" for i in range(1, 7)] + + for joint_name, position in zip(joint_names, HOME_JOINTS_RAD.values()): + joint_constraint = JointConstraint() + joint_constraint.joint_name = joint_name + joint_constraint.position = position + joint_constraint.tolerance_above = 0.001 + joint_constraint.tolerance_below = 0.001 + joint_constraint.weight = 1.0 + constraints.joint_constraints.append(joint_constraint) + + return [constraints] + + +def _move_home(robot, arm, logger, plan_params=None) -> bool: + arm.set_start_state_to_current_state() + arm.set_goal_state(motion_plan_constraints=_build_home_joint_constraints()) + return _plan_and_execute(robot, arm, logger, plan_params=plan_params) + + +# ══════════════════════════════════════════════════════════ +class SttRobotControlNode(Node): + + def __init__( + self, + robot: MoveItPy, + arm, + home_params: PlanRequestParameters, + pilz_params: PlanRequestParameters, + ): + super().__init__("stt_robot_control") + + self.declare_parameter("use_tts", True) + use_tts = self.get_parameter("use_tts").get_parameter_value().bool_value + + self._robot = robot + self._arm = arm + self._home = home_params + self._pilz = pilz_params + self._cmd_q: queue.Queue[str] = queue.Queue() + self._tts = _TtsPlayer(enabled=use_tts) + + threading.Thread(target=self._worker, daemon=True).start() + + self.create_subscription(String, "/stt_result", self._stt_cb, 10) + self.get_logger().info( + f"준비 완료 명령어: home / {' / '.join(DIRECTIONS)} / stop " + f"(jog={JOG_OFFSET*100:.0f}cm, tts={'on' if self._tts._enabled else 'off'})" + ) + + # ── ROS 콜백 ─────────────────────────────────────────── + def _stt_cb(self, msg: String): + cmd = _text_to_cmd(msg.data) + if cmd is None: + self.get_logger().debug(f"매핑 없음: '{msg.data}'") + return + + if cmd == "stop": + n = 0 + while not self._cmd_q.empty(): + try: + self._cmd_q.get_nowait() + n += 1 + except queue.Empty: + break + self.get_logger().info(f"[STOP] 큐 {n}개 취소") + self._tts.speak(TTS_PHRASES["stop"]) + else: + self._cmd_q.put(cmd) + self.get_logger().info(f"[CMD] '{cmd}' 큐 추가 (크기={self._cmd_q.qsize()})") + + # ── 워커 스레드 ──────────────────────────────────────── + def _worker(self): + logger = get_logger("stt_robot_control.worker") + + while True: + try: + cmd = self._cmd_q.get(timeout=1.0) + except queue.Empty: + continue + + logger.info(f"===== '{cmd}' 실행 =====") + self._tts.speak(TTS_PHRASES.get(cmd, "")) + ok = True + + if cmd == "home": + ok = _move_home( + self._robot, + self._arm, + logger, + plan_params=self._home, + ) + + elif cmd in DIRECTIONS: + ok = self._move_offset(logger, cmd) + + logger.info(f"===== '{cmd}' 완료 =====") + self._tts.speak(TTS_PHRASES["done" if ok else "fail"]) + + # ── 현재 EE 위치 기준 오프셋 이동 ────────────────────── + def _move_offset(self, logger, direction: str) -> bool: + try: + with self._robot.get_planning_scene_monitor().read_only() as scene: + state = scene.current_state + state.update() + tf = state.get_frame_transform(EE_LINK) + except Exception as e: + logger.error(f"현재 EE 상태 조회 실패: {e}") + return False + + cx = float(tf[0, 3]) + cy = float(tf[1, 3]) + cz = float(tf[2, 3]) + qx, qy, qz, qw = _rot_matrix_to_quat(tf[:3, :3]) + + dx, dy, dz = DIRECTIONS[direction] + tx, ty, tz = _clamp_to_safe(cx + dx, cy + dy, cz + dz, logger) + + logger.info( + f"JOG dir={direction} offset={JOG_OFFSET*100:.1f}cm " + f"({cx:.3f},{cy:.3f},{cz:.3f}) → ({tx:.3f},{ty:.3f},{tz:.3f})" + ) + + ps = PoseStamped() + ps.header.frame_id = BASE_FRAME + ps.pose.position.x = tx + ps.pose.position.y = ty + ps.pose.position.z = tz + ps.pose.orientation.x = qx + ps.pose.orientation.y = qy + ps.pose.orientation.z = qz + ps.pose.orientation.w = qw + + self._arm.set_start_state_to_current_state() + self._arm.set_goal_state(pose_stamped_msg=ps, pose_link=EE_LINK) + return _plan_and_execute(self._robot, self._arm, logger, plan_params=self._pilz) + + +# ══════════════════════════════════════════════════════════ +def main(args=None): + rclpy.init(args=args) + logger = get_logger("stt_robot_control.main") + + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + logger.info("MoveItPy 초기화 완료") + + home = PlanRequestParameters(robot) + home.planning_pipeline = "ompl" + home.planner_id = "RRTConnect" + home.max_velocity_scaling_factor = 0.2 + home.max_acceleration_scaling_factor = 0.1 + home.planning_time = 3.0 + + pilz = PlanRequestParameters(robot) + pilz.planning_pipeline = "pilz_industrial_motion_planner" + pilz.planner_id = "PTP" + pilz.max_velocity_scaling_factor = 0.2 + pilz.max_acceleration_scaling_factor = 0.1 + pilz.planning_time = 3.0 + + logger.info("시작 자세를 home으로 이동합니다") + _move_home(robot, arm, logger, plan_params=home) + + node = SttRobotControlNode(robot, arm, home, pilz) + + executor = MultiThreadedExecutor() + executor.add_node(node) + + logger.info("음성 명령 대기 중 ... (Ctrl+C 종료)") + try: + executor.spin() + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + robot.shutdown() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/syrup_pump_press.py b/src/dsr_practice/dsr_practice/syrup_pump_press.py new file mode 100644 index 0000000..67176c6 --- /dev/null +++ b/src/dsr_practice/dsr_practice/syrup_pump_press.py @@ -0,0 +1,227 @@ +#!/usr/bin/env python3 + +import math +import time + +import rclpy +from geometry_msgs.msg import PoseStamped +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters +from rclpy.node import Node + +from .onrobot import RG + + +GROUP_NAME = "manipulator" +BASE_FRAME = "base_link" +EE_LINK = "link_6" + +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 +GRIPPER_CLOSE_WIDTH = 0 +GRIPPER_FORCE = 200 + +HOME_JOINTS_DEG = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0] +HOME_JOINTS_RAD = [math.radians(d) for d in HOME_JOINTS_DEG] + +DOWN_ORI = { + "x": 0.0, + "y": 1.0, + "z": 0.0, + "w": 0.0, +} + + +class SyrupPumpPressNode(Node): + def __init__(self): + super().__init__("syrup_pump_press") + + self.declare_parameter("pump_x", 0.45) + self.declare_parameter("pump_y", 0.0) + self.declare_parameter("start_z", 0.50) + self.declare_parameter("pump_top_z", 0.35) + self.declare_parameter("press_depth", 0.05) + self.declare_parameter("hold_sec", 0.5) + self.declare_parameter("go_home_first", True) + self.declare_parameter("return_home", False) + + self.pump_x = self.get_parameter("pump_x").value + self.pump_y = self.get_parameter("pump_y").value + self.start_z = self.get_parameter("start_z").value + self.pump_top_z = self.get_parameter("pump_top_z").value + self.press_depth = self.get_parameter("press_depth").value + self.hold_sec = self.get_parameter("hold_sec").value + self.go_home_first = self.get_parameter("go_home_first").value + self.return_home = self.get_parameter("return_home").value + + self.press_z = self.pump_top_z - self.press_depth + + self.robot = MoveItPy(node_name="syrup_pump_press_moveit_py") + self.arm = self.robot.get_planning_component(GROUP_NAME) + self.robot_model = self.robot.get_robot_model() + + self.home_params = PlanRequestParameters(self.robot) + self.home_params.planning_pipeline = "ompl" + self.home_params.planner_id = "RRTConnectkConfigDefault" + self.home_params.max_velocity_scaling_factor = 0.2 + self.home_params.max_acceleration_scaling_factor = 0.1 + self.home_params.planning_time = 3.0 + + self.ptp_params = PlanRequestParameters(self.robot) + self.ptp_params.planning_pipeline = "pilz_industrial_motion_planner" + self.ptp_params.planner_id = "PTP" + self.ptp_params.max_velocity_scaling_factor = 0.15 + self.ptp_params.max_acceleration_scaling_factor = 0.1 + self.ptp_params.planning_time = 3.0 + + self.lin_params = PlanRequestParameters(self.robot) + self.lin_params.planning_pipeline = "pilz_industrial_motion_planner" + self.lin_params.planner_id = "LIN" + self.lin_params.max_velocity_scaling_factor = 0.08 + self.lin_params.max_acceleration_scaling_factor = 0.05 + self.lin_params.planning_time = 3.0 + + self.press_params = PlanRequestParameters(self.robot) + self.press_params.planning_pipeline = "pilz_industrial_motion_planner" + self.press_params.planner_id = "LIN" + self.press_params.max_velocity_scaling_factor = 0.02 + self.press_params.max_acceleration_scaling_factor = 0.02 + self.press_params.planning_time = 3.0 + + self.gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + time.sleep(0.5) + + def make_pose(self, x, y, z, ori=None): + if ori is None: + ori = DOWN_ORI + + pose = PoseStamped() + pose.header.frame_id = BASE_FRAME + pose.pose.position.x = float(x) + pose.pose.position.y = float(y) + pose.pose.position.z = float(z) + pose.pose.orientation.x = ori["x"] + pose.pose.orientation.y = ori["y"] + pose.pose.orientation.z = ori["z"] + pose.pose.orientation.w = ori["w"] + return pose + + def plan_and_execute(self, pose_goal=None, state_goal=None, params=None): + log = self.get_logger() + self.arm.set_start_state_to_current_state() + + if pose_goal is not None: + self.arm.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link=EE_LINK, + ) + elif state_goal is not None: + self.arm.set_goal_state(robot_state=state_goal) + else: + log.error("No pose/state goal was provided.") + return False + + plan_result = self.arm.plan(parameters=params) if params else self.arm.plan() + if not plan_result: + log.error("Planning failed.") + return False + + self.robot.execute( + group_name=GROUP_NAME, + robot_trajectory=plan_result.trajectory, + blocking=True, + ) + return True + + def move_home(self): + home_state = RobotState(self.robot_model) + home_state.set_joint_group_positions(GROUP_NAME, HOME_JOINTS_RAD) + home_state.update() + return self.plan_and_execute(state_goal=home_state, params=self.home_params) + + def run_task(self): + log = self.get_logger() + log.info("=== Syrup pump press task start ===") + log.info( + "Target pump pose: " + f"x={self.pump_x:.3f}, y={self.pump_y:.3f}, " + f"start_z={self.start_z:.3f}, pump_top_z={self.pump_top_z:.3f}, " + f"press_z={self.press_z:.3f}" + ) + + if self.press_z <= 0.0: + log.error("Invalid press_z. Check pump_top_z and press_depth.") + return False + + if self.go_home_first: + log.info("[0] Move HOME") + if not self.move_home(): + return False + + log.info("[1] Close gripper fully") + self.gripper.move_gripper( + width_val=GRIPPER_CLOSE_WIDTH, + force_val=GRIPPER_FORCE, + ) + time.sleep(1.0) + + log.info("[2] Move above syrup pump") + if not self.plan_and_execute( + pose_goal=self.make_pose(self.pump_x, self.pump_y, self.start_z), + params=self.ptp_params, + ): + return False + + log.info("[3] Move vertically down to pump top height") + if not self.plan_and_execute( + pose_goal=self.make_pose(self.pump_x, self.pump_y, self.pump_top_z), + params=self.lin_params, + ): + return False + + log.info("[4] Slowly press syrup pump by 5 cm") + if not self.plan_and_execute( + pose_goal=self.make_pose(self.pump_x, self.pump_y, self.press_z), + params=self.press_params, + ): + return False + + if self.hold_sec > 0.0: + log.info(f"[5] Hold for {self.hold_sec:.2f} sec") + time.sleep(self.hold_sec) + + log.info("[6] Retract vertically") + if not self.plan_and_execute( + pose_goal=self.make_pose(self.pump_x, self.pump_y, self.start_z), + params=self.lin_params, + ): + return False + + if self.return_home: + log.info("[7] Return HOME") + if not self.move_home(): + return False + + log.info("=== Syrup pump press task finished ===") + return True + + def destroy_node(self): + try: + self.gripper.close_connection() + finally: + super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + node = SyrupPumpPressNode() + try: + node.run_task() + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/dsr_practice/test.py b/src/dsr_practice/dsr_practice/test.py new file mode 100644 index 0000000..36ccfac --- /dev/null +++ b/src/dsr_practice/dsr_practice/test.py @@ -0,0 +1,20 @@ +import time +from onrobot import RG + +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = "502" + +gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + +#그리퍼 열기 +gripper.open_gripper() + +#그리퍼의 상태를 체크하고 동작 중이면 wait(동작이 끝날 때까지 기다리기) +while gripper.get_status()[0]: + time.sleep(0.5) + + #그리퍼 닫기 + gripper.close_gripper() +while gripper.get_status()[0]: + time.sleep(0.5) diff --git a/src/dsr_practice/dsr_practice/test/test_copyright.py b/src/dsr_practice/dsr_practice/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/src/dsr_practice/dsr_practice/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/dsr_practice/dsr_practice/test/test_flake8.py b/src/dsr_practice/dsr_practice/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/dsr_practice/dsr_practice/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/dsr_practice/dsr_practice/test/test_pep257.py b/src/dsr_practice/dsr_practice/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/dsr_practice/dsr_practice/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/dsr_practice/dsr_practice/test_2.py b/src/dsr_practice/dsr_practice/test_2.py new file mode 100644 index 0000000..55c414f --- /dev/null +++ b/src/dsr_practice/dsr_practice/test_2.py @@ -0,0 +1,15 @@ +from onrobot import RG +import time + +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = "502" + +gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) +gripper.move_gripper(width_val=400, force_val=10) +while gripper.get_status()[0]: + time.sleep(0.5) + + #그리퍼의 현재 너비 출력 + print(f'get_width_with_offset: {gripper.get_width_with_offset()}') + \ No newline at end of file diff --git a/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py b/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py new file mode 100644 index 0000000..5fa18e4 --- /dev/null +++ b/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py @@ -0,0 +1,1129 @@ +#!/usr/bin/env python3 + +import math +import time +from collections import Counter +from pathlib import Path + +import cv2 +import numpy as np +import rclpy +from ament_index_python.packages import get_package_share_directory +from cv_bridge import CvBridge +from geometry_msgs.msg import PoseStamped +from rcl_interfaces.msg import ParameterDescriptor +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters +from rclpy.node import Node +from scipy.spatial.transform import Rotation +from sensor_msgs.msg import CameraInfo, Image +from ultralytics import YOLO + +from .onrobot import RG + + +GROUP_NAME = "manipulator" +BASE_FRAME = "base_link" +EE_LINK = "link_6" + +HOME_JOINTS = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(90.0), +} +HOME_JOINTS_RAD = [ + math.radians(0.0), + math.radians(0.0), + math.radians(90.0), + math.radians(0.0), + math.radians(90.0), + math.radians(90.0), +] + +SAFE_X_MIN = 0.0 +SAFE_Y_MIN = -0.35 +SAFE_Y_MAX = 0.35 +SAFE_Z_MIN = 0.20 + +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 +GRIPPER_OPEN_WIDTH = 1100 +GRIPPER_CLOSE_WIDTH = 120 +GRIPPER_FORCE = 250 +GRIPPER_OPEN_TIMEOUT_SEC = 5.0 +GRIPPER_STATUS_POLL_SEC = 0.15 + +DOWN_ORI = {"x": 0.0, "y": 1.0, "z": 0.0, "w": 0.0} + + +def clamp_to_safe_workspace(x, y, z, logger, z_min=SAFE_Z_MIN): + if x < SAFE_X_MIN: + logger.warning(f"x={x:.3f} -> {SAFE_X_MIN:.3f}") + x = SAFE_X_MIN + if y < SAFE_Y_MIN: + logger.warning(f"y={y:.3f} -> {SAFE_Y_MIN:.3f}") + y = SAFE_Y_MIN + elif y > SAFE_Y_MAX: + logger.warning(f"y={y:.3f} -> {SAFE_Y_MAX:.3f}") + y = SAFE_Y_MAX + if z < z_min: + logger.warning(f"z={z:.3f} -> {z_min:.3f}") + z = z_min + return x, y, z + + +def make_pose(x, y, z, ori=None): + if ori is None: + ori = DOWN_ORI + pose = PoseStamped() + pose.header.frame_id = BASE_FRAME + pose.pose.position.x = float(x) + pose.pose.position.y = float(y) + pose.pose.position.z = float(z) + pose.pose.orientation.x = ori["x"] + pose.pose.orientation.y = ori["y"] + pose.pose.orientation.z = ori["z"] + pose.pose.orientation.w = ori["w"] + return pose + + +def parse_bool(value): + if isinstance(value, bool): + return value + if isinstance(value, (int, float)): + return bool(value) + return str(value).strip().lower() in {"1", "true", "yes", "on"} + + +def parse_axis(value): + if isinstance(value, bool): + return "y" if value else "x" + + normalized = str(value).strip().lower() + if normalized in {"y", "y_axis", "axis_y", "true", "yes", "on"}: + return "y" + if normalized in {"x", "x_axis", "axis_x"}: + return "x" + return normalized + + +def quat_dict_from_matrix(matrix): + qx, qy, qz, qw = Rotation.from_matrix(matrix).as_quat() + return { + "x": float(qx), + "y": float(qy), + "z": float(qz), + "w": float(qw), + } + + +def quat_dict_from_euler(roll_deg, pitch_deg, yaw_deg): + qx, qy, qz, qw = Rotation.from_euler( + "xyz", + [roll_deg, pitch_deg, yaw_deg], + degrees=True, + ).as_quat() + return { + "x": float(qx), + "y": float(qy), + "z": float(qz), + "w": float(qw), + } + + +def get_ee_matrix(moveit_robot): + psm = moveit_robot.get_planning_scene_monitor() + with psm.read_only() as scene: + transform = scene.current_state.get_global_link_transform(EE_LINK) + return np.asarray(transform, dtype=float) + + +class YoloCupPickNode(Node): + def __init__(self): + super().__init__("yolo_cup_pick_node") + + self.declare_parameter( + "model_path", + "/home/ssu/ros2_ws/yolo_runs/cup_yolov8n_ft1/weights/best.pt", + ) + self.declare_parameter("conf", 0.35) + self.declare_parameter("imgsz", 640) + self.declare_parameter("device", "cpu") + self.declare_parameter("target_class", "cup") + self.declare_parameter("auto_pick", False) + self.declare_parameter("auto_pick_interval", 3.0) + self.declare_parameter("pick_depth_ratio", 0.55) + self.declare_parameter("depth_patch_radius", 7) + self.declare_parameter("min_depth_valid_ratio", 0.03) + self.declare_parameter("min_depth_m", 0.15) + self.declare_parameter("max_depth_m", 1.20) + self.declare_parameter("redetect_on_approach", True) + self.declare_parameter("redetect_settle_sec", 0.5) + self.declare_parameter("grasp_mode", "side") + dynamic_param = ParameterDescriptor(dynamic_typing=True) + self.declare_parameter("side_grasp_axis", "y_axis", dynamic_param) + self.declare_parameter("side_grasp_direction", -1.0) + self.declare_parameter("side_approach_offset", 0.12) + self.declare_parameter("side_staging_offset", 0.24) + self.declare_parameter("side_grasp_offset", 0.035) + self.declare_parameter("side_grasp_z_offset", 0.05) + self.declare_parameter("side_orientation_mode", "approach") + self.declare_parameter("side_tool_roll_deg", 0.0) + self.declare_parameter("side_roll_deg", 0.0) + self.declare_parameter("side_pitch_deg", 90.0) + self.declare_parameter("side_yaw_deg", 0.0) + self.declare_parameter("pick_z_offset", 0.20) + self.declare_parameter("approach_offset", 0.12) + self.declare_parameter("safe_z", 0.50) + self.declare_parameter("min_motion_z", 0.12) + self.declare_parameter("return_home_after_task", True) + self.declare_parameter("verify_motion", True) + self.declare_parameter("motion_verify_tolerance", 0.01) + self.declare_parameter("move_to_camera_home", True) + self.declare_parameter("camera_home_x", 0.45) + self.declare_parameter("camera_home_y", 0.00) + self.declare_parameter("camera_home_z", 0.62) + self.declare_parameter("place_x", 0.45) + self.declare_parameter("place_y", 0.0) + self.declare_parameter("place_z", 0.30) + + self.model_path = self.get_parameter("model_path").value + self.conf = float(self.get_parameter("conf").value) + self.imgsz = int(self.get_parameter("imgsz").value) + self.device = self.get_parameter("device").value + self.target_class = self.get_parameter("target_class").value + self.auto_pick = parse_bool(self.get_parameter("auto_pick").value) + self.auto_pick_interval = float(self.get_parameter("auto_pick_interval").value) + self.pick_depth_ratio = float(self.get_parameter("pick_depth_ratio").value) + self.depth_patch_radius = int(self.get_parameter("depth_patch_radius").value) + self.min_depth_valid_ratio = float( + self.get_parameter("min_depth_valid_ratio").value + ) + self.min_depth_m = float(self.get_parameter("min_depth_m").value) + self.max_depth_m = float(self.get_parameter("max_depth_m").value) + self.redetect_on_approach = parse_bool( + self.get_parameter("redetect_on_approach").value + ) + self.redetect_settle_sec = float(self.get_parameter("redetect_settle_sec").value) + self.grasp_mode = str(self.get_parameter("grasp_mode").value).strip().lower() + self.side_grasp_axis = parse_axis(self.get_parameter("side_grasp_axis").value) + self.side_grasp_direction = float( + self.get_parameter("side_grasp_direction").value + ) + self.side_approach_offset = float( + self.get_parameter("side_approach_offset").value + ) + self.side_staging_offset = float( + self.get_parameter("side_staging_offset").value + ) + self.side_grasp_offset = float(self.get_parameter("side_grasp_offset").value) + self.side_grasp_z_offset = float( + self.get_parameter("side_grasp_z_offset").value + ) + self.side_orientation_mode = str( + self.get_parameter("side_orientation_mode").value + ).strip().lower() + self.side_tool_roll_deg = float( + self.get_parameter("side_tool_roll_deg").value + ) + self.side_roll_deg = float(self.get_parameter("side_roll_deg").value) + self.side_pitch_deg = float(self.get_parameter("side_pitch_deg").value) + self.side_yaw_deg = float(self.get_parameter("side_yaw_deg").value) + self.pick_z_offset = float(self.get_parameter("pick_z_offset").value) + self.approach_offset = float(self.get_parameter("approach_offset").value) + self.safe_z = float(self.get_parameter("safe_z").value) + self.min_motion_z = float(self.get_parameter("min_motion_z").value) + self.return_home_after_task = parse_bool( + self.get_parameter("return_home_after_task").value + ) + self.verify_motion = parse_bool(self.get_parameter("verify_motion").value) + self.motion_verify_tolerance = float( + self.get_parameter("motion_verify_tolerance").value + ) + self.move_to_camera_home = parse_bool( + self.get_parameter("move_to_camera_home").value + ) + self.camera_home_x = float(self.get_parameter("camera_home_x").value) + self.camera_home_y = float(self.get_parameter("camera_home_y").value) + self.camera_home_z = float(self.get_parameter("camera_home_z").value) + self.place_x = float(self.get_parameter("place_x").value) + self.place_y = float(self.get_parameter("place_y").value) + self.place_z = float(self.get_parameter("place_z").value) + + model_file = Path(self.model_path).expanduser() + if not model_file.exists(): + raise FileNotFoundError(f"YOLO model not found: {model_file}") + + self.get_logger().info(f"Loading YOLO model: {model_file}") + self.model = YOLO(str(model_file)) + self.get_logger().info(f"YOLO classes: {self.model.names}") + if self.target_class not in self.model.names.values(): + raise ValueError( + f"target_class='{self.target_class}' is not in model classes " + f"{self.model.names}" + ) + if self.grasp_mode not in {"side", "top"}: + raise ValueError("grasp_mode must be 'side' or 'top'") + if self.side_grasp_axis not in {"x", "y"}: + raise ValueError("side_grasp_axis must be 'x' or 'y'") + self.side_grasp_direction = 1.0 if self.side_grasp_direction >= 0 else -1.0 + if self.side_orientation_mode not in {"approach", "euler", "home"}: + raise ValueError( + "side_orientation_mode must be 'approach', 'euler', or 'home'" + ) + if self.side_staging_offset < self.side_approach_offset: + self.get_logger().warning( + "side_staging_offset is smaller than side_approach_offset; " + "using side_approach_offset for staging." + ) + self.side_staging_offset = self.side_approach_offset + + self.bridge = CvBridge() + self.color_image = None + self.depth_image = None + self.intrinsics = None + self.last_detection = None + self.picking = False + self.has_picked_once = False + self.last_pick_time = 0.0 + self.last_status = "waiting for command" + + calib_file = ( + Path(get_package_share_directory("dsr_practice")) + / "config" + / "T_gripper2camera.npy" + ) + self.gripper2cam = np.load(str(calib_file)).astype(float) + self.gripper2cam[:3, 3] /= 1000.0 + self.get_logger().info(f"Loaded hand-eye calibration: {calib_file}") + + self.gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + + self.get_logger().info("Initializing MoveItPy...") + self.robot = MoveItPy(node_name="yolo_cup_pick_moveit_py") + self.arm = self.robot.get_planning_component(GROUP_NAME) + self.robot_model = self.robot.get_robot_model() + self.get_logger().info("MoveItPy initialized") + + self.ompl_params = PlanRequestParameters(self.robot) + self.ompl_params.planning_pipeline = "ompl" + self.ompl_params.planner_id = "RRTConnect" + self.ompl_params.max_velocity_scaling_factor = 0.2 + self.ompl_params.max_acceleration_scaling_factor = 0.1 + self.ompl_params.planning_time = 3.0 + + self.pilz_params = PlanRequestParameters(self.robot) + self.pilz_params.planning_pipeline = "pilz_industrial_motion_planner" + self.pilz_params.planner_id = "PTP" + self.pilz_params.max_velocity_scaling_factor = 0.12 + self.pilz_params.max_acceleration_scaling_factor = 0.08 + self.pilz_params.planning_time = 3.0 + + self.home_ori = DOWN_ORI + + self.create_subscription( + CameraInfo, + "/camera/camera/color/camera_info", + self._camera_info_callback, + 10, + ) + self.create_subscription( + Image, + "/camera/camera/color/image_raw", + self._color_callback, + 10, + ) + self.create_subscription( + Image, + "/camera/camera/aligned_depth_to_color/image_raw", + self._depth_callback, + 10, + ) + + def _camera_info_callback(self, msg): + self.intrinsics = { + "fx": msg.k[0], + "fy": msg.k[4], + "cx": msg.k[2], + "cy": msg.k[5], + } + + def _color_callback(self, msg): + self.color_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") + + def _depth_callback(self, msg): + self.depth_image = self.bridge.imgmsg_to_cv2( + msg, desired_encoding="passthrough" + ) + + def plan_and_execute(self, pose_goal=None, state_goal=None, params=None): + log = self.get_logger() + self.arm.set_start_state_to_current_state() + start_matrix = get_ee_matrix(self.robot) + start_xyz = start_matrix[:3, 3].copy() + goal_xyz = None + + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + x, y, z = clamp_to_safe_workspace(x, y, z, log, self.min_motion_z) + pose_goal.pose.position.x = x + pose_goal.pose.position.y = y + pose_goal.pose.position.z = z + goal_xyz = np.array([x, y, z], dtype=float) + log.info( + f"Planning pose goal -> ({x:.3f}, {y:.3f}, {z:.3f}) " + f"from ({start_xyz[0]:.3f}, {start_xyz[1]:.3f}, {start_xyz[2]:.3f})" + ) + self.arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link=EE_LINK) + elif state_goal is not None: + log.info( + f"Planning joint/state goal from EE " + f"({start_xyz[0]:.3f}, {start_xyz[1]:.3f}, {start_xyz[2]:.3f})" + ) + self.arm.set_goal_state(robot_state=state_goal) + else: + log.error("No pose/state goal was provided") + return False + + plan_result = self.arm.plan(parameters=params) if params else self.arm.plan() + if not plan_result: + log.error("Planning failed") + return False + + self.robot.execute( + group_name=GROUP_NAME, + robot_trajectory=plan_result.trajectory, + blocking=True, + ) + self.spin_for_camera_update(0.2) + + end_matrix = get_ee_matrix(self.robot) + end_xyz = end_matrix[:3, 3].copy() + moved = float(np.linalg.norm(end_xyz - start_xyz)) + if goal_xyz is None: + log.info( + f"Execution finished. EE moved {moved:.3f} m -> " + f"({end_xyz[0]:.3f}, {end_xyz[1]:.3f}, {end_xyz[2]:.3f})" + ) + else: + goal_error = float(np.linalg.norm(end_xyz - goal_xyz)) + log.info( + f"Execution finished. EE moved {moved:.3f} m, " + f"goal_error={goal_error:.3f} m -> " + f"({end_xyz[0]:.3f}, {end_xyz[1]:.3f}, {end_xyz[2]:.3f})" + ) + if self.verify_motion and goal_error > self.motion_verify_tolerance: + log.error( + "MoveIt execution did not reach the requested pose. " + "Check that the real robot MoveIt/trajectory controller is running." + ) + return False + return True + + def move_joint_home(self): + home_state = RobotState(self.robot_model) + home_state.set_joint_group_positions(GROUP_NAME, HOME_JOINTS_RAD) + home_state.update() + if not self.plan_and_execute(state_goal=home_state, params=self.ompl_params): + return False + + transform = get_ee_matrix(self.robot) + self.update_home_orientation_from_matrix(transform) + return True + + def move_home(self): + if self.move_to_camera_home: + return self.move_camera_home() + return self.move_joint_home() + + def update_home_orientation_from_matrix(self, transform): + qx, qy, qz, qw = Rotation.from_matrix(transform[:3, :3]).as_quat() + self.home_ori = { + "x": float(qx), + "y": float(qy), + "z": float(qz), + "w": float(qw), + } + + def move_camera_home(self): + log = self.get_logger() + candidate_zs = [] + for z in (self.camera_home_z, 0.62, 0.58, 0.54): + if z > self.camera_home_z + 1e-6: + continue + if all(abs(z - candidate) > 1e-6 for candidate in candidate_zs): + candidate_zs.append(z) + + for idx, z in enumerate(candidate_zs): + if idx > 0: + log.warning( + f"Camera home IK failed at higher z; retrying z={z:.3f}" + ) + + log.info( + f"Move CAMERA HOME -> ({self.camera_home_x:.3f}, " + f"{self.camera_home_y:.3f}, {z:.3f})" + ) + if not self.plan_and_execute( + pose_goal=make_pose( + self.camera_home_x, + self.camera_home_y, + z, + self.home_ori, + ), + params=self.pilz_params, + ): + continue + + self.camera_home_z = z + transform = get_ee_matrix(self.robot) + self.update_home_orientation_from_matrix(transform) + return True + + return False + + def wait_until_gripper_idle(self, timeout_sec=GRIPPER_OPEN_TIMEOUT_SEC): + log = self.get_logger() + start_time = time.time() + while time.time() - start_time < timeout_sec: + status = self.gripper.get_status() + busy = bool(status[0]) + if not busy: + try: + width_mm = self.gripper.get_width_with_offset() + log.info(f"Gripper ready. current width={width_mm:.1f} mm") + except Exception as exc: + log.warning(f"Gripper width read failed: {exc}") + return True + time.sleep(GRIPPER_STATUS_POLL_SEC) + + log.warning("Timed out waiting for gripper to finish opening.") + return False + + def open_gripper_max(self, wait=False): + self.get_logger().info( + f"Open gripper to max width={GRIPPER_OPEN_WIDTH} " + f"({GRIPPER_OPEN_WIDTH / 10.0:.1f} mm)" + ) + self.gripper.move_gripper(GRIPPER_OPEN_WIDTH, GRIPPER_FORCE) + if wait: + return self.wait_until_gripper_idle() + return True + + def detect_objects(self, image): + results = self.model.predict( + source=image, + imgsz=self.imgsz, + conf=self.conf, + device=self.device, + verbose=False, + ) + boxes = results[0].boxes + if boxes is None or len(boxes) == 0: + self.last_detection = None + return [] + + detections = [] + for box in boxes: + cls_id = int(box.cls[0]) + class_name = self.model.names.get(cls_id, str(cls_id)) + x1, y1, x2, y2 = box.xyxy[0].cpu().numpy().tolist() + x1, y1, x2, y2 = int(x1), int(y1), int(x2), int(y2) + detections.append( + { + "bbox": (x1, y1, x2, y2), + "cx": int((x1 + x2) / 2), + "cy": int((y1 + y2) / 2), + "conf": float(box.conf[0]), + "class_name": class_name, + } + ) + + target_detections = [ + det for det in detections if det["class_name"] == self.target_class + ] + if not target_detections: + self.last_detection = None + else: + self.last_detection = max( + target_detections, + key=lambda det: det["conf"], + ) + + return detections + + def depth_candidates_from_bbox(self, bbox): + x1, y1, x2, y2 = bbox + h, w = self.depth_image.shape[:2] + x_ratios = [0.50, 0.35, 0.65, 0.25, 0.75] + y_ratios = [ + self.pick_depth_ratio, + 0.45, + 0.35, + 0.65, + 0.25, + 0.75, + ] + + points = [] + seen = set() + for yr in y_ratios: + for xr in x_ratios: + u = int(x1 + xr * (x2 - x1)) + v = int(y1 + yr * (y2 - y1)) + u = max(0, min(w - 1, u)) + v = max(0, min(h - 1, v)) + if (u, v) not in seen: + points.append((u, v)) + seen.add((u, v)) + return points + + def depth_patch_at(self, u, v): + h, w = self.depth_image.shape[:2] + r = self.depth_patch_radius + patch = self.depth_image[ + max(0, v - r) : min(h, v + r + 1), + max(0, u - r) : min(w, u + r + 1), + ] + valid = patch[patch > 0] + valid_ratio = valid.size / float(patch.size) + if valid.size == 0 or valid_ratio < self.min_depth_valid_ratio: + return None + + z_raw = float(np.median(valid)) + z_m = z_raw / 1000.0 if self.depth_image.dtype == np.uint16 else z_raw + if z_m < self.min_depth_m or z_m > self.max_depth_m: + return None + return u, v, z_m, valid_ratio + + def depth_from_bbox(self, bbox, log_reason=False): + log = self.get_logger() + if self.depth_image is None: + if log_reason: + log.warning("Depth image is not ready") + return None + + valid_samples = [] + for u, v in self.depth_candidates_from_bbox(bbox): + sample = self.depth_patch_at(u, v) + if sample is not None: + valid_samples.append(sample) + + if not valid_samples: + if log_reason: + log.warning( + "No valid depth found inside target bbox. " + "Try a larger depth_patch_radius or lower min_depth_valid_ratio." + ) + return None + + # Prefer the closest valid surface in the bbox. Transparent cups often + # expose background/table depth, so closest valid depth is usually safer. + u, v, z_m, valid_ratio = min(valid_samples, key=lambda sample: sample[2]) + if log_reason: + log.info( + f"Depth sample selected at ({u}, {v}): " + f"{z_m:.3f} m, valid_ratio={valid_ratio:.2f}" + ) + return u, v, z_m + + def pixel_to_camera(self, u, v, z_m): + fx = self.intrinsics["fx"] + fy = self.intrinsics["fy"] + cx = self.intrinsics["cx"] + cy = self.intrinsics["cy"] + + cam_x = (u - cx) * z_m / fx + cam_y = (v - cy) * z_m / fy + cam_z = z_m + return np.array([cam_x, cam_y, cam_z], dtype=float) + + def camera_to_base(self, camera_xyz): + coord = np.append(camera_xyz, 1.0) + base2ee = get_ee_matrix(self.robot) + base2cam = base2ee @ self.gripper2cam + return (base2cam @ coord)[:3] + + def side_unit_vector(self): + if self.side_grasp_axis == "x": + return np.array([self.side_grasp_direction, 0.0], dtype=float) + return np.array([0.0, self.side_grasp_direction], dtype=float) + + def side_grasp_orientation(self, side_vec): + if self.side_orientation_mode == "home": + return self.home_ori + + if self.side_orientation_mode == "euler": + return quat_dict_from_euler( + self.side_roll_deg, + self.side_pitch_deg, + self.side_yaw_deg, + ) + + # Make the tool's local +Z direction point horizontally into the cup. + # The local +Y axis is kept close to world +Z so the wrist is laid over + # the table instead of keeping the top-down grasp posture. + tool_z = np.array([-side_vec[0], -side_vec[1], 0.0], dtype=float) + tool_z_norm = np.linalg.norm(tool_z) + if tool_z_norm < 1e-6: + return self.home_ori + tool_z /= tool_z_norm + + world_up = np.array([0.0, 0.0, 1.0], dtype=float) + tool_x = np.cross(world_up, tool_z) + tool_x_norm = np.linalg.norm(tool_x) + if tool_x_norm < 1e-6: + return self.home_ori + tool_x /= tool_x_norm + tool_y = np.cross(tool_z, tool_x) + tool_y /= np.linalg.norm(tool_y) + + base_from_tool = np.column_stack((tool_x, tool_y, tool_z)) + if abs(self.side_tool_roll_deg) > 1e-6: + base_from_tool = ( + base_from_tool + @ Rotation.from_euler( + "z", + self.side_tool_roll_deg, + degrees=True, + ).as_matrix() + ) + return quat_dict_from_matrix(base_from_tool) + + def spin_for_camera_update(self, duration_sec): + end_time = time.time() + max(0.0, duration_sec) + while rclpy.ok() and time.time() < end_time: + rclpy.spin_once(self, timeout_sec=0.05) + + def select_redetect_target(self, detections): + if self.color_image is None: + return None + + candidates = [ + det for det in detections if det["class_name"] == self.target_class + ] + if not candidates: + return None + + h, w = self.color_image.shape[:2] + image_cx = w / 2.0 + image_cy = h / 2.0 + return min( + candidates, + key=lambda det: (det["cx"] - image_cx) ** 2 + + (det["cy"] - image_cy) ** 2, + ) + + def base_from_detection(self, detection, log_prefix): + depth_info = self.depth_from_bbox(detection["bbox"], log_reason=True) + if depth_info is None: + return None + + u, v, z_m = depth_info + camera_xyz = self.pixel_to_camera(u, v, z_m) + base_xyz = self.camera_to_base(camera_xyz) + self.get_logger().info( + f"{log_prefix} pixel=({u}, {v}), depth={z_m:.3f} m, " + f"camera=({camera_xyz[0]:.3f}, {camera_xyz[1]:.3f}, " + f"{camera_xyz[2]:.3f}) -> base=({base_xyz[0]:.3f}, " + f"{base_xyz[1]:.3f}, {base_xyz[2]:.3f})" + ) + return base_xyz + + def pick_and_place(self, base_xyz): + if self.grasp_mode == "side": + task_ok = self.pick_and_place_side(base_xyz) + else: + task_ok = self.pick_and_place_top(base_xyz) + + if task_ok and self.return_home_after_task: + self.get_logger().info("return home after task") + return self.move_home() + return task_ok + + def refine_target_from_current_view(self, log): + if not self.redetect_on_approach: + return None + + log.info("redetect target after approach") + self.spin_for_camera_update(self.redetect_settle_sec) + if self.color_image is None: + return None + + detections = self.detect_objects(self.color_image.copy()) + target = self.select_redetect_target(detections) + if target is None: + log.warning("redetect target not found; using initial target") + return None + return self.base_from_detection(target, "[redetect]") + + def pick_and_place_side(self, base_xyz): + log = self.get_logger() + bx, by, bz = [float(v) for v in base_xyz] + side_vec = self.side_unit_vector() + side_ori = self.side_grasp_orientation(side_vec) + stage_xy = ( + np.array([bx, by], dtype=float) + side_vec * self.side_staging_offset + ) + pre_xy = np.array([bx, by], dtype=float) + side_vec * self.side_approach_offset + grasp_xy = np.array([bx, by], dtype=float) + side_vec * self.side_grasp_offset + grasp_z = max(bz + self.side_grasp_z_offset, self.min_motion_z) + pre_z = grasp_z + lift_z = max(grasp_z + self.approach_offset, self.safe_z) + place_approach_z = max(self.place_z + self.approach_offset, self.safe_z) + + log.info( + f"Side grasp target base=({bx:.3f}, {by:.3f}, {bz:.3f}), " + f"axis={self.side_grasp_axis}, dir={self.side_grasp_direction:.0f}, " + f"ori_mode={self.side_orientation_mode}, " + f"tool_roll={self.side_tool_roll_deg:.1f}deg, " + f"stage=({stage_xy[0]:.3f}, {stage_xy[1]:.3f}, {pre_z:.3f}), " + f"pre=({pre_xy[0]:.3f}, {pre_xy[1]:.3f}, {pre_z:.3f}), " + f"grasp=({grasp_xy[0]:.3f}, {grasp_xy[1]:.3f}, {grasp_z:.3f})" + ) + + self.open_gripper_max(wait=False) + + steps = [ + ( + "move to outside side-staging pose", + make_pose(stage_xy[0], stage_xy[1], lift_z, side_ori), + ), + ( + "lower at outside side-staging pose", + make_pose(stage_xy[0], stage_xy[1], pre_z, side_ori), + ), + ( + "move horizontally to side pre-grasp", + make_pose(pre_xy[0], pre_xy[1], pre_z, side_ori), + ), + ] + for label, pose in steps: + log.info(label) + if not self.plan_and_execute(pose_goal=pose, params=self.pilz_params): + return False + + if not self.wait_until_gripper_idle(): + return False + + refined_base = self.refine_target_from_current_view(log) + if refined_base is not None: + bx, by, bz = [float(v) for v in refined_base] + pre_xy = np.array([bx, by], dtype=float) + side_vec * self.side_approach_offset + grasp_xy = np.array([bx, by], dtype=float) + side_vec * self.side_grasp_offset + grasp_z = max(bz + self.side_grasp_z_offset, self.min_motion_z) + pre_z = grasp_z + lift_z = max(grasp_z + self.approach_offset, self.safe_z) + log.info( + f"refined side grasp=({grasp_xy[0]:.3f}, {grasp_xy[1]:.3f}, " + f"{grasp_z:.3f})" + ) + if not self.plan_and_execute( + pose_goal=make_pose(pre_xy[0], pre_xy[1], pre_z, side_ori), + params=self.pilz_params, + ): + return False + + log.info("slide horizontally into cup side") + if not self.plan_and_execute( + pose_goal=make_pose(grasp_xy[0], grasp_xy[1], grasp_z, side_ori), + params=self.pilz_params, + ): + return False + + log.info("close gripper for side grasp") + self.gripper.move_gripper(GRIPPER_CLOSE_WIDTH, GRIPPER_FORCE) + time.sleep(1.0) + + move_steps = [ + ("lift cup", make_pose(grasp_xy[0], grasp_xy[1], lift_z, side_ori)), + ( + "move above syrup pump front", + make_pose(self.place_x, self.place_y, place_approach_z, side_ori), + ), + ( + "place cup", + make_pose(self.place_x, self.place_y, self.place_z, side_ori), + ), + ] + for label, pose in move_steps: + log.info(label) + if not self.plan_and_execute(pose_goal=pose, params=self.pilz_params): + return False + + log.info("open gripper") + self.open_gripper_max(wait=True) + + log.info("retract") + return self.plan_and_execute( + pose_goal=make_pose(self.place_x, self.place_y, place_approach_z, + side_ori), + params=self.pilz_params, + ) + + def pick_and_place_top(self, base_xyz): + log = self.get_logger() + bx, by, bz = [float(v) for v in base_xyz] + pick_z = bz + self.pick_z_offset + approach_z = max(pick_z + self.approach_offset, self.safe_z) + place_approach_z = max(self.place_z + self.approach_offset, self.safe_z) + + log.info( + f"Cup base point=({bx:.3f}, {by:.3f}, {bz:.3f}), " + f"pick_z={pick_z:.3f}" + ) + + self.open_gripper_max(wait=False) + + steps = [ + ("move above cup", make_pose(bx, by, approach_z, self.home_ori)), + ] + for label, pose in steps: + log.info(label) + if not self.plan_and_execute(pose_goal=pose, params=self.pilz_params): + return False + + if not self.wait_until_gripper_idle(): + return False + + refined_base = self.refine_target_from_current_view(log) + if refined_base is not None: + bx, by, bz = [float(v) for v in refined_base] + pick_z = bz + self.pick_z_offset + approach_z = max(pick_z + self.approach_offset, self.safe_z) + log.info( + f"refined cup base=({bx:.3f}, {by:.3f}, {bz:.3f}), " + f"pick_z={pick_z:.3f}" + ) + if not self.plan_and_execute( + pose_goal=make_pose(bx, by, approach_z, self.home_ori), + params=self.pilz_params, + ): + return False + + log.info("move down to cup") + if not self.plan_and_execute( + pose_goal=make_pose(bx, by, pick_z, self.home_ori), + params=self.pilz_params, + ): + return False + + log.info("close gripper") + self.gripper.move_gripper(GRIPPER_CLOSE_WIDTH, GRIPPER_FORCE) + time.sleep(1.0) + + move_steps = [ + ("lift cup", make_pose(bx, by, approach_z, self.home_ori)), + ( + "move above syrup pump front", + make_pose(self.place_x, self.place_y, place_approach_z, self.home_ori), + ), + ( + "place cup", + make_pose(self.place_x, self.place_y, self.place_z, self.home_ori), + ), + ] + for label, pose in move_steps: + log.info(label) + if not self.plan_and_execute(pose_goal=pose, params=self.pilz_params): + return False + + log.info("open gripper") + self.open_gripper_max(wait=True) + time.sleep(1.0) + + log.info("retract") + return self.plan_and_execute( + pose_goal=make_pose(self.place_x, self.place_y, place_approach_z, + self.home_ori), + params=self.pilz_params, + ) + + def start_pick_from_detection(self): + log = self.get_logger() + if self.picking: + log.warning("Already picking") + self.last_status = "already picking" + return + if self.color_image is None or self.depth_image is None or self.intrinsics is None: + log.warning("Waiting for color/depth/camera_info") + self.last_status = "waiting for color/depth/camera_info" + return + if self.last_detection is None: + log.warning(f"No {self.target_class} detection available") + self.last_status = f"no {self.target_class} detection" + return + + self.last_status = f"pick requested: {self.target_class}" + base_xyz = self.base_from_detection(self.last_detection, "[initial]") + if base_xyz is None: + log.error(f"No valid depth around {self.target_class} bbox") + self.last_status = f"no valid depth for {self.target_class}" + return + + self.picking = True + self.last_status = "moving robot" + try: + if self.pick_and_place(base_xyz): + self.has_picked_once = True + self.last_pick_time = time.time() + self.last_status = "pick finished" + else: + self.last_status = "pick failed" + finally: + self.picking = False + + def draw_detections(self, image, detections): + for detection in detections: + x1, y1, x2, y2 = detection["bbox"] + conf = detection["conf"] + class_name = detection.get("class_name", "") + + if class_name == self.target_class: + color = (0, 255, 0) + thickness = 2 + elif class_name == "lid": + color = (255, 0, 0) + thickness = 2 + else: + color = (180, 180, 180) + thickness = 1 + + cv2.rectangle(image, (x1, y1), (x2, y2), color, thickness) + + label = f"{class_name} {conf:.2f}" + if class_name == self.target_class: + depth_info = self.depth_from_bbox(detection["bbox"]) + if depth_info is not None: + u, v, z_m = depth_info + label += f" {z_m:.2f}m" + cv2.circle(image, (u, v), 5, (0, 0, 255), -1) + + cv2.putText( + image, + label, + (x1, max(20, y1 - 8)), + cv2.FONT_HERSHEY_SIMPLEX, + 0.6, + color, + 2, + cv2.LINE_AA, + ) + self.draw_hud(image, detections) + return image + + def draw_hud(self, image, detections): + counts = Counter(det["class_name"] for det in detections) + count_text = " ".join( + f"{name}:{counts[name]}" for name in sorted(counts) + ) or "none" + mode = "AUTO" if self.auto_pick else "MANUAL" + target_state = "ready" if self.last_detection is not None else "not found" + picked_state = "picked" if self.has_picked_once else "waiting" + + lines = [ + ( + f"[{mode}] {self.grasp_mode} target={self.target_class} " + f"conf>={self.conf:.2f} " + "p:pick a:auto r:reset ESC:quit" + ), + f"detections: {count_text} | target: {target_state} | {picked_state}", + f"status: {self.last_status}", + ] + color = (0, 255, 255) if self.auto_pick else (230, 230, 230) + for idx, text in enumerate(lines): + y = 26 + idx * 24 + cv2.putText( + image, + text, + (10, y), + cv2.FONT_HERSHEY_SIMPLEX, + 0.58, + color, + 2, + cv2.LINE_AA, + ) + + def run(self): + log = self.get_logger() + log.info("Move JOINT HOME") + if not self.move_joint_home(): + log.error("Joint home move failed") + return + + if self.move_to_camera_home: + log.info("Move HIGH CAMERA HOME") + if not self.move_camera_home(): + log.error("High camera home move failed") + return + + self.open_gripper_max(wait=True) + + window = "YOLO Cup Pick - p pick, a auto, r reset, esc quit" + cv2.namedWindow(window) + + while rclpy.ok(): + rclpy.spin_once(self, timeout_sec=0.01) + if self.color_image is None: + continue + + frame = self.color_image.copy() + detections = self.detect_objects(frame) + frame = self.draw_detections(frame, detections) + cv2.imshow(window, frame) + + now = time.time() + can_auto_pick = ( + self.auto_pick + and self.last_detection is not None + and not self.has_picked_once + and not self.picking + and (now - self.last_pick_time) >= self.auto_pick_interval + ) + if can_auto_pick: + self.start_pick_from_detection() + + key = cv2.waitKey(1) & 0xFF + if key == 27: + break + if key in (ord("p"), ord("P")): + log.info("pick key pressed") + self.start_pick_from_detection() + elif key in (ord("a"), ord("A")): + self.auto_pick = not self.auto_pick + self.last_pick_time = time.time() + self.last_status = f"auto_pick {'ON' if self.auto_pick else 'OFF'}" + log.info(f"auto_pick {'ON' if self.auto_pick else 'OFF'}") + elif key in (ord("r"), ord("R")): + self.has_picked_once = False + self.last_pick_time = 0.0 + self.last_status = "pick state reset" + log.info("pick state reset") + + cv2.destroyAllWindows() + + def destroy_node(self): + try: + self.gripper.close_connection() + finally: + super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + node = YoloCupPickNode() + try: + node.run() + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/dsr_practice/launch/bar_sort_node.launch.py b/src/dsr_practice/launch/bar_sort_node.launch.py new file mode 100644 index 0000000..f4e6655 --- /dev/null +++ b/src/dsr_practice/launch/bar_sort_node.launch.py @@ -0,0 +1,39 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() + .robot_description_semantic(file_path="config/dsr.srdf") + .robot_description_kinematics() + .joint_limits() + .trajectory_execution() + .planning_scene_monitor() + .sensors_3d() + .to_moveit_configs() + ) + + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="dsr_practice", + executable="bar_sort_node", + output="screen", + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) diff --git a/src/dsr_practice/launch/click_pick_node.launch.py b/src/dsr_practice/launch/click_pick_node.launch.py new file mode 100644 index 0000000..fc007a3 --- /dev/null +++ b/src/dsr_practice/launch/click_pick_node.launch.py @@ -0,0 +1,42 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # Doosan M0609 MoveIt 기본 설정 (URDF, SRDF, kinematics, controllers 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() # m0609.urdf.xacro + .robot_description_semantic(file_path="config/dsr.srdf") # dsr.srdf + .robot_description_kinematics() # kinematics.yaml + .joint_limits() # joint_limits.yaml + .trajectory_execution() # moveit_controllers.yaml + .planning_scene_monitor() # sensors_3d.yaml 등과 연동 + .sensors_3d() + .to_moveit_configs() + ) + + # 🔹 MoveItPy 전용 YAML 추가 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="dsr_practice", + executable="click_pick_node", + output="screen", + # MoveIt config + MoveItPy용 설정을 같이 넘김 + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) diff --git a/src/dsr_practice/launch/collision_obstacle.launch.py b/src/dsr_practice/launch/collision_obstacle.launch.py new file mode 100644 index 0000000..05843c4 --- /dev/null +++ b/src/dsr_practice/launch/collision_obstacle.launch.py @@ -0,0 +1,43 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # Doosan M0609 MoveIt 기본 설정 (URDF, SRDF, kinematics, controllers 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() # m0609.urdf.xacro + .robot_description_semantic() # dsr.srdf + .robot_description_kinematics() # kinematics.yaml + .joint_limits() # joint_limits.yaml + .trajectory_execution() # moveit_controllers.yaml + .planning_scene_monitor() # sensors_3d.yaml 등과 연동 + .sensors_3d() + .to_moveit_configs() + ) + + # 🔹 MoveItPy 전용 YAML 추가 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="dsr_practice", + executable="collision_obstacle", + output="screen", + # MoveIt config + MoveItPy용 설정을 같이 넘김 + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) + diff --git a/src/dsr_practice/launch/gear_assembly.launch.py b/src/dsr_practice/launch/gear_assembly.launch.py new file mode 100644 index 0000000..c86744d --- /dev/null +++ b/src/dsr_practice/launch/gear_assembly.launch.py @@ -0,0 +1,42 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # Doosan M0609 MoveIt 기본 설정 (URDF, SRDF, kinematics, controllers 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() # m0609.urdf.xacro + .robot_description_semantic() # dsr.srdf + .robot_description_kinematics() # kinematics.yaml + .joint_limits() # joint_limits.yaml + .trajectory_execution() # moveit_controllers.yaml + .planning_scene_monitor() # sensors_3d.yaml 등과 연동 + .sensors_3d() + .to_moveit_configs() + ) + + # 🔹 MoveItPy 전용 YAML 추가 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="dsr_practice", + executable="gear_assembly", + output="screen", + # MoveIt config + MoveItPy용 설정을 같이 넘김 + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) diff --git a/src/dsr_practice/launch/mp_basic.launch.py b/src/dsr_practice/launch/mp_basic.launch.py new file mode 100644 index 0000000..7d32f76 --- /dev/null +++ b/src/dsr_practice/launch/mp_basic.launch.py @@ -0,0 +1,43 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # Doosan M0609 MoveIt 기본 설정 (URDF, SRDF, kinematics, controllers 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() # m0609.urdf.xacro + .robot_description_semantic(file_path="config/dsr.srdf") # dsr.srdf + .robot_description_kinematics() # kinematics.yaml + .joint_limits() # joint_limits.yaml + .trajectory_execution() # moveit_controllers.yaml + .planning_scene_monitor() # sensors_3d.yaml 등과 연동 + .sensors_3d() + .to_moveit_configs() + ) + + # 🔹 MoveItPy 전용 YAML 추가 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="dsr_practice", + executable="mp_basic", + output="screen", + # MoveIt config + MoveItPy용 설정을 같이 넘김 + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) + diff --git a/src/dsr_practice/launch/mp_waypoint.launch.py b/src/dsr_practice/launch/mp_waypoint.launch.py new file mode 100644 index 0000000..55a6d5e --- /dev/null +++ b/src/dsr_practice/launch/mp_waypoint.launch.py @@ -0,0 +1,43 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # Doosan M0609 MoveIt 기본 설정 (URDF, SRDF, kinematics, controllers 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() # m0609.urdf.xacro + .robot_description_semantic() # dsr.srdf + .robot_description_kinematics() # kinematics.yaml + .joint_limits() # joint_limits.yaml + .trajectory_execution() # moveit_controllers.yaml + .planning_scene_monitor() # sensors_3d.yaml 등과 연동 + .sensors_3d() + .to_moveit_configs() + ) + + # 🔹 MoveItPy 전용 YAML 추가 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="dsr_practice", + executable="mp_waypoint", + output="screen", + # MoveIt config + MoveItPy용 설정을 같이 넘김 + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) + diff --git a/src/dsr_practice/launch/mp_waypoint_pilz.launch.py b/src/dsr_practice/launch/mp_waypoint_pilz.launch.py new file mode 100644 index 0000000..c94e276 --- /dev/null +++ b/src/dsr_practice/launch/mp_waypoint_pilz.launch.py @@ -0,0 +1,43 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # Doosan M0609 MoveIt 기본 설정 (URDF, SRDF, kinematics, controllers 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() # m0609.urdf.xacro + .robot_description_semantic() # dsr.srdf + .robot_description_kinematics() # kinematics.yaml + .joint_limits() # joint_limits.yaml + .trajectory_execution() # moveit_controllers.yaml + .planning_scene_monitor() # sensors_3d.yaml 등과 연동 + .sensors_3d() + .to_moveit_configs() + ) + + # 🔹 MoveItPy 전용 YAML 추가 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="dsr_practice", + executable="mp_waypoint_pilz", + output="screen", + # MoveIt config + MoveItPy용 설정을 같이 넘김 + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) + diff --git a/src/dsr_practice/launch/mp_waypoint_pilz_lin.launch.py b/src/dsr_practice/launch/mp_waypoint_pilz_lin.launch.py new file mode 100644 index 0000000..6d2fe09 --- /dev/null +++ b/src/dsr_practice/launch/mp_waypoint_pilz_lin.launch.py @@ -0,0 +1,43 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # Doosan M0609 MoveIt 기본 설정 (URDF, SRDF, kinematics, controllers 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() # m0609.urdf.xacro + .robot_description_semantic() # dsr.srdf + .robot_description_kinematics() # kinematics.yaml + .joint_limits() # joint_limits.yaml + .trajectory_execution() # moveit_controllers.yaml + .planning_scene_monitor() # sensors_3d.yaml 등과 연동 + .sensors_3d() + .to_moveit_configs() + ) + + # 🔹 MoveItPy 전용 YAML 추가 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="dsr_practice", + executable="mp_waypoint_pilz_lin", + output="screen", + # MoveIt config + MoveItPy용 설정을 같이 넘김 + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) + diff --git a/src/dsr_practice/launch/pick_and_place.launch.py b/src/dsr_practice/launch/pick_and_place.launch.py new file mode 100644 index 0000000..eff7963 --- /dev/null +++ b/src/dsr_practice/launch/pick_and_place.launch.py @@ -0,0 +1,42 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # Doosan M0609 MoveIt 기본 설정 (URDF, SRDF, kinematics, controllers 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() # m0609.urdf.xacro + .robot_description_semantic() # dsr.srdf + .robot_description_kinematics() # kinematics.yaml + .joint_limits() # joint_limits.yaml + .trajectory_execution() # moveit_controllers.yaml + .planning_scene_monitor() # sensors_3d.yaml 등과 연동 + .sensors_3d() + .to_moveit_configs() + ) + + # 🔹 MoveItPy 전용 YAML 추가 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="dsr_practice", + executable="pick_and_place", + output="screen", + # MoveIt config + MoveItPy용 설정을 같이 넘김 + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) diff --git a/src/dsr_practice/launch/realsense_data_collector.launch.py b/src/dsr_practice/launch/realsense_data_collector.launch.py new file mode 100644 index 0000000..831386a --- /dev/null +++ b/src/dsr_practice/launch/realsense_data_collector.launch.py @@ -0,0 +1,52 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + output_dir_arg = DeclareLaunchArgument( + "output_dir", + default_value="/home/ssu/ros2_ws/realsense_dataset/raw", + description="Directory where color, depth, and metadata files are saved.", + ) + save_interval_arg = DeclareLaunchArgument( + "save_interval_sec", + default_value="1.0", + description="Seconds between saved frames.", + ) + max_frames_arg = DeclareLaunchArgument( + "max_frames", + default_value="0", + description="Maximum number of frames to save. 0 means unlimited.", + ) + save_depth_arg = DeclareLaunchArgument( + "save_depth", + default_value="true", + description="Whether to save aligned depth images with RGB images.", + ) + + collector_node = Node( + package="dsr_practice", + executable="realsense_data_collector", + name="realsense_data_collector", + output="screen", + parameters=[ + { + "output_dir": LaunchConfiguration("output_dir"), + "save_interval_sec": LaunchConfiguration("save_interval_sec"), + "max_frames": LaunchConfiguration("max_frames"), + "save_depth": LaunchConfiguration("save_depth"), + } + ], + ) + + return LaunchDescription( + [ + output_dir_arg, + save_interval_arg, + max_frames_arg, + save_depth_arg, + collector_node, + ] + ) diff --git a/src/dsr_practice/launch/stt_pick_and_place.launch.py b/src/dsr_practice/launch/stt_pick_and_place.launch.py new file mode 100644 index 0000000..61c9e0d --- /dev/null +++ b/src/dsr_practice/launch/stt_pick_and_place.launch.py @@ -0,0 +1,54 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() + .robot_description_semantic(file_path="config/dsr.srdf") + .robot_description_kinematics() + .joint_limits() + .trajectory_execution() + .planning_scene_monitor() + .sensors_3d() + .to_moveit_configs() + ) + + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + pick_place_node = Node( + package="dsr_practice", + executable="stt_pick_and_place", + output="screen", + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + {"use_tts": True}, + ], + ) + + stt_node = Node( + package="dsr_practice", + executable="stt_node", + output="screen", + parameters=[ + {"language": "ko-KR"}, + {"device_index": -1}, + {"energy_threshold": 300.0}, + {"pause_threshold": 0.8}, + {"phrase_time_limit": 5.0}, + {"dynamic_energy": True}, + {"ambient_duration": 1.0}, + ], + ) + + return LaunchDescription([pick_place_node, stt_node]) diff --git a/src/dsr_practice/launch/stt_robot_control.launch.py b/src/dsr_practice/launch/stt_robot_control.launch.py new file mode 100644 index 0000000..ff8411b --- /dev/null +++ b/src/dsr_practice/launch/stt_robot_control.launch.py @@ -0,0 +1,54 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() + .robot_description_semantic(file_path="config/dsr.srdf") + .robot_description_kinematics() + .joint_limits() + .trajectory_execution() + .planning_scene_monitor() + .sensors_3d() + .to_moveit_configs() + ) + + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + robot_control_node = Node( + package="dsr_practice", + executable="stt_robot_control", + output="screen", + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + {"use_tts": True}, + ], + ) + + stt_node = Node( + package="dsr_practice", + executable="stt_node", + output="screen", + parameters=[ + {"language": "ko-KR"}, + {"device_index": -1}, + {"energy_threshold": 300.0}, + {"pause_threshold": 0.8}, + {"phrase_time_limit": 5.0}, + {"dynamic_energy": True}, + {"ambient_duration": 1.0}, + ], + ) + + return LaunchDescription([robot_control_node, stt_node]) diff --git a/src/dsr_practice/launch/syrup_pump_press.launch.py b/src/dsr_practice/launch/syrup_pump_press.launch.py new file mode 100644 index 0000000..09ea49b --- /dev/null +++ b/src/dsr_practice/launch/syrup_pump_press.launch.py @@ -0,0 +1,86 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description(file_path="config/m0609.urdf.xacro") + .robot_description_semantic(file_path="config/dsr.srdf") + .robot_description_kinematics() + .joint_limits() + .trajectory_execution() + .planning_scene_monitor() + .sensors_3d() + .to_moveit_configs() + ) + + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + pump_x_arg = DeclareLaunchArgument( + "pump_x", + default_value="0.45", + description="Syrup pump x position in base_link frame [m].", + ) + pump_y_arg = DeclareLaunchArgument( + "pump_y", + default_value="0.0", + description="Syrup pump y position in base_link frame [m].", + ) + start_z_arg = DeclareLaunchArgument( + "start_z", + default_value="0.50", + description="Vertical approach start height [m].", + ) + pump_top_z_arg = DeclareLaunchArgument( + "pump_top_z", + default_value="0.35", + description="Syrup pump top height [m].", + ) + press_depth_arg = DeclareLaunchArgument( + "press_depth", + default_value="0.05", + description="Press depth from pump top [m].", + ) + hold_sec_arg = DeclareLaunchArgument( + "hold_sec", + default_value="0.5", + description="Holding time at pressed position [sec].", + ) + + return LaunchDescription( + [ + pump_x_arg, + pump_y_arg, + start_z_arg, + pump_top_z_arg, + press_depth_arg, + hold_sec_arg, + Node( + package="dsr_practice", + executable="syrup_pump_press", + output="screen", + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + { + "pump_x": LaunchConfiguration("pump_x"), + "pump_y": LaunchConfiguration("pump_y"), + "start_z": LaunchConfiguration("start_z"), + "pump_top_z": LaunchConfiguration("pump_top_z"), + "press_depth": LaunchConfiguration("press_depth"), + "hold_sec": LaunchConfiguration("hold_sec"), + }, + ], + ), + ] + ) diff --git a/src/dsr_practice/launch/yolo_cup_pick_node.launch.py b/src/dsr_practice/launch/yolo_cup_pick_node.launch.py new file mode 100644 index 0000000..dc2deae --- /dev/null +++ b/src/dsr_practice/launch/yolo_cup_pick_node.launch.py @@ -0,0 +1,274 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description(file_path="config/m0609.urdf.xacro") + .robot_description_semantic(file_path="config/dsr.srdf") + .robot_description_kinematics() + .joint_limits() + .trajectory_execution() + .planning_scene_monitor() + .sensors_3d() + .to_moveit_configs() + ) + + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + model_path_arg = DeclareLaunchArgument( + "model_path", + default_value="/home/ssu/ros2_ws/yolo_runs/cup_yolov8n_ft1/weights/best.pt", + description="Path to trained cup YOLO weights.", + ) + conf_arg = DeclareLaunchArgument("conf", default_value="0.35") + imgsz_arg = DeclareLaunchArgument("imgsz", default_value="640") + device_arg = DeclareLaunchArgument("device", default_value="cpu") + target_class_arg = DeclareLaunchArgument("target_class", default_value="cup") + auto_pick_interval_arg = DeclareLaunchArgument( + "auto_pick_interval", default_value="3.0" + ) + pick_depth_ratio_arg = DeclareLaunchArgument( + "pick_depth_ratio", default_value="0.55" + ) + depth_patch_radius_arg = DeclareLaunchArgument( + "depth_patch_radius", default_value="7" + ) + min_depth_valid_ratio_arg = DeclareLaunchArgument( + "min_depth_valid_ratio", default_value="0.03" + ) + min_depth_m_arg = DeclareLaunchArgument("min_depth_m", default_value="0.15") + max_depth_m_arg = DeclareLaunchArgument("max_depth_m", default_value="1.20") + redetect_on_approach_arg = DeclareLaunchArgument( + "redetect_on_approach", default_value="true" + ) + redetect_settle_sec_arg = DeclareLaunchArgument( + "redetect_settle_sec", default_value="0.5" + ) + grasp_mode_arg = DeclareLaunchArgument("grasp_mode", default_value="side") + side_grasp_axis_arg = DeclareLaunchArgument( + "side_grasp_axis", default_value="y_axis" + ) + side_grasp_direction_arg = DeclareLaunchArgument( + "side_grasp_direction", default_value="-1.0" + ) + side_approach_offset_arg = DeclareLaunchArgument( + "side_approach_offset", default_value="0.12" + ) + side_staging_offset_arg = DeclareLaunchArgument( + "side_staging_offset", + default_value="0.24", + description="Far outside offset where the wrist first turns horizontal.", + ) + side_grasp_offset_arg = DeclareLaunchArgument( + "side_grasp_offset", default_value="0.035" + ) + side_grasp_z_offset_arg = DeclareLaunchArgument( + "side_grasp_z_offset", + default_value="0.05", + description="Side grasp height offset from detected base point.", + ) + side_orientation_mode_arg = DeclareLaunchArgument( + "side_orientation_mode", + default_value="approach", + description="Side grasp orientation: approach, euler, or home.", + ) + side_tool_roll_deg_arg = DeclareLaunchArgument( + "side_tool_roll_deg", + default_value="0.0", + description="Twist around the horizontal approach direction for RG2 finger alignment.", + ) + side_roll_deg_arg = DeclareLaunchArgument( + "side_roll_deg", + default_value="0.0", + description="Manual side grasp roll, used when side_orientation_mode:=euler.", + ) + side_pitch_deg_arg = DeclareLaunchArgument( + "side_pitch_deg", + default_value="90.0", + description="Manual side grasp pitch, used when side_orientation_mode:=euler.", + ) + side_yaw_deg_arg = DeclareLaunchArgument( + "side_yaw_deg", + default_value="0.0", + description="Manual side grasp yaw, used when side_orientation_mode:=euler.", + ) + verify_motion_arg = DeclareLaunchArgument("verify_motion", default_value="true") + motion_verify_tolerance_arg = DeclareLaunchArgument( + "motion_verify_tolerance", default_value="0.01" + ) + move_to_camera_home_arg = DeclareLaunchArgument( + "move_to_camera_home", default_value="true" + ) + camera_home_x_arg = DeclareLaunchArgument("camera_home_x", default_value="0.45") + camera_home_y_arg = DeclareLaunchArgument("camera_home_y", default_value="0.0") + camera_home_z_arg = DeclareLaunchArgument("camera_home_z", default_value="0.62") + min_motion_z_arg = DeclareLaunchArgument( + "min_motion_z", + default_value="0.12", + description="Minimum allowed commanded Z in base frame.", + ) + return_home_after_task_arg = DeclareLaunchArgument( + "return_home_after_task", default_value="true" + ) + place_x_arg = DeclareLaunchArgument("place_x", default_value="0.45") + place_y_arg = DeclareLaunchArgument("place_y", default_value="0.0") + place_z_arg = DeclareLaunchArgument("place_z", default_value="0.30") + auto_pick_arg = DeclareLaunchArgument("auto_pick", default_value="false") + + return LaunchDescription( + [ + model_path_arg, + conf_arg, + imgsz_arg, + device_arg, + target_class_arg, + auto_pick_interval_arg, + pick_depth_ratio_arg, + depth_patch_radius_arg, + min_depth_valid_ratio_arg, + min_depth_m_arg, + max_depth_m_arg, + redetect_on_approach_arg, + redetect_settle_sec_arg, + grasp_mode_arg, + side_grasp_axis_arg, + side_grasp_direction_arg, + side_approach_offset_arg, + side_staging_offset_arg, + side_grasp_offset_arg, + side_grasp_z_offset_arg, + side_orientation_mode_arg, + side_tool_roll_deg_arg, + side_roll_deg_arg, + side_pitch_deg_arg, + side_yaw_deg_arg, + verify_motion_arg, + motion_verify_tolerance_arg, + move_to_camera_home_arg, + camera_home_x_arg, + camera_home_y_arg, + camera_home_z_arg, + min_motion_z_arg, + return_home_after_task_arg, + place_x_arg, + place_y_arg, + place_z_arg, + auto_pick_arg, + Node( + package="dsr_practice", + executable="joint_state_relay", + name="joint_state_relay", + output="screen", + parameters=[ + { + "input_topic": "/dsr01/joint_states", + "output_topic": "/joint_states", + } + ], + ), + Node( + package="dsr_practice", + executable="yolo_cup_pick_node", + output="screen", + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + { + "model_path": ParameterValue( + LaunchConfiguration("model_path"), + value_type=str, + ), + "conf": LaunchConfiguration("conf"), + "imgsz": LaunchConfiguration("imgsz"), + "device": ParameterValue( + LaunchConfiguration("device"), + value_type=str, + ), + "target_class": ParameterValue( + LaunchConfiguration("target_class"), + value_type=str, + ), + "auto_pick_interval": LaunchConfiguration( + "auto_pick_interval" + ), + "pick_depth_ratio": LaunchConfiguration("pick_depth_ratio"), + "depth_patch_radius": LaunchConfiguration( + "depth_patch_radius" + ), + "min_depth_valid_ratio": LaunchConfiguration( + "min_depth_valid_ratio" + ), + "min_depth_m": LaunchConfiguration("min_depth_m"), + "max_depth_m": LaunchConfiguration("max_depth_m"), + "redetect_on_approach": LaunchConfiguration( + "redetect_on_approach" + ), + "redetect_settle_sec": LaunchConfiguration( + "redetect_settle_sec" + ), + "grasp_mode": ParameterValue( + LaunchConfiguration("grasp_mode"), + value_type=str, + ), + "side_grasp_axis": ParameterValue( + LaunchConfiguration("side_grasp_axis"), + value_type=str, + ), + "side_grasp_direction": LaunchConfiguration( + "side_grasp_direction" + ), + "side_approach_offset": LaunchConfiguration( + "side_approach_offset" + ), + "side_staging_offset": LaunchConfiguration( + "side_staging_offset" + ), + "side_grasp_offset": LaunchConfiguration("side_grasp_offset"), + "side_grasp_z_offset": LaunchConfiguration( + "side_grasp_z_offset" + ), + "side_orientation_mode": ParameterValue( + LaunchConfiguration("side_orientation_mode"), + value_type=str, + ), + "side_tool_roll_deg": LaunchConfiguration( + "side_tool_roll_deg" + ), + "side_roll_deg": LaunchConfiguration("side_roll_deg"), + "side_pitch_deg": LaunchConfiguration("side_pitch_deg"), + "side_yaw_deg": LaunchConfiguration("side_yaw_deg"), + "verify_motion": LaunchConfiguration("verify_motion"), + "motion_verify_tolerance": LaunchConfiguration( + "motion_verify_tolerance" + ), + "move_to_camera_home": LaunchConfiguration( + "move_to_camera_home" + ), + "camera_home_x": LaunchConfiguration("camera_home_x"), + "camera_home_y": LaunchConfiguration("camera_home_y"), + "camera_home_z": LaunchConfiguration("camera_home_z"), + "min_motion_z": LaunchConfiguration("min_motion_z"), + "return_home_after_task": LaunchConfiguration( + "return_home_after_task" + ), + "place_x": LaunchConfiguration("place_x"), + "place_y": LaunchConfiguration("place_y"), + "place_z": LaunchConfiguration("place_z"), + "auto_pick": LaunchConfiguration("auto_pick"), + }, + ], + ), + ] + ) diff --git a/src/dsr_practice/package.xml b/src/dsr_practice/package.xml new file mode 100644 index 0000000..aa3a275 --- /dev/null +++ b/src/dsr_practice/package.xml @@ -0,0 +1,27 @@ + + + + dsr_practice + 0.0.0 + TODO: Package description + deeptree + TODO: License declaration + + rclpy + geometry_msgs + sensor_msgs + cv_bridge + moveit_py + dsr_msgs2 + + python3-pymodbus + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/dsr_practice/resource/dsr_practice b/src/dsr_practice/resource/dsr_practice new file mode 100644 index 0000000..e69de29 diff --git a/src/dsr_practice/setup.cfg b/src/dsr_practice/setup.cfg new file mode 100644 index 0000000..e4a9452 --- /dev/null +++ b/src/dsr_practice/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/dsr_practice +[install] +install_scripts=$base/lib/dsr_practice diff --git a/src/dsr_practice/setup.py b/src/dsr_practice/setup.py new file mode 100644 index 0000000..9e3c281 --- /dev/null +++ b/src/dsr_practice/setup.py @@ -0,0 +1,52 @@ +from setuptools import find_packages, setup +from glob import glob + +package_name = 'dsr_practice' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name + '/launch', glob('launch/*.launch.py')), + ( + 'share/' + package_name + '/config', + glob('config/*.yaml') + glob('config/*.npy') + ), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='deeptree', + maintainer_email='deeptree@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'mp_basic = dsr_practice.mp_basic:main', + 'mp_waypoint = dsr_practice.mp_waypoint:main', + 'mp_waypoint_pilz = dsr_practice.mp_waypoint_pilz:main', + 'mp_waypoint_pilz_lin = dsr_practice.mp_waypoint_pilz_lin:main', + 'collision_obstacle = dsr_practice.collision_obstacle:main', + 'gripper = dsr_practice.gripper:main', + 'gear_assembly = dsr_practice.gear_assembly:main', + 'click_pick_node = dsr_practice.click_pick_node:main', + 'bar_sort_node = dsr_practice.bar_sort_node:main', + 'bar_detect_test = dsr_practice.bar_detect_test:main', + 'stt_node = dsr_practice.stt_node:main', + 'stt_robot_control = dsr_practice.stt_robot_control:main', + 'stt_pick_and_place = dsr_practice.stt_pick_and_place:main', + 'realsense_data_collector = dsr_practice.realsense_data_collector:main', + 'syrup_pump_press = dsr_practice.syrup_pump_press:main', + 'yolo_cup_pick_node = dsr_practice.yolo_cup_pick_node:main', + 'joint_state_relay = dsr_practice.joint_state_relay:main', + ], + }, +) diff --git a/src/dsr_practice/test/test_copyright.py b/src/dsr_practice/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/src/dsr_practice/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/dsr_practice/test/test_flake8.py b/src/dsr_practice/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/dsr_practice/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/dsr_practice/test/test_pep257.py b/src/dsr_practice/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/dsr_practice/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' From 4f6e88aca6440475f59c4233ba5490eafec1cf89 Mon Sep 17 00:00:00 2001 From: shining-b-02 Date: Fri, 15 May 2026 15:37:03 +0900 Subject: [PATCH 2/7] Integrate RG2 gripper driver --- src/azas_gripper/azas_gripper/onrobot.py | 123 ++++++++++++++++++ .../azas_gripper/rg2_gripper_node.py | 102 +++++++++++++-- src/azas_gripper/package.xml | 2 +- 3 files changed, 214 insertions(+), 13 deletions(-) create mode 100644 src/azas_gripper/azas_gripper/onrobot.py diff --git a/src/azas_gripper/azas_gripper/onrobot.py b/src/azas_gripper/azas_gripper/onrobot.py new file mode 100644 index 0000000..df9062b --- /dev/null +++ b/src/azas_gripper/azas_gripper/onrobot.py @@ -0,0 +1,123 @@ +"""Low-level Modbus TCP driver for OnRobot RG2/RG6 grippers.""" + +try: + from pymodbus.client import ModbusTcpClient as ModbusClient +except ImportError: # pragma: no cover - pymodbus 2.x fallback + from pymodbus.client.sync import ModbusTcpClient as ModbusClient + + +class RG: + """Small wrapper around the OnRobot RG Modbus register interface.""" + + UNIT_ID = 65 + + def __init__(self, gripper, ip, port): + if gripper not in ["rg2", "rg6"]: + raise ValueError("Please specify either rg2 or rg6.") + + self.client = ModbusClient( + ip, + port=int(port), + stopbits=1, + bytesize=8, + parity="E", + baudrate=115200, + timeout=1, + ) + self.gripper = gripper + if self.gripper == "rg2": + self.max_width = 1100 + self.max_force = 400 + else: + self.max_width = 1600 + self.max_force = 1200 + self.open_connection() + + def _read_holding_registers(self, address, count): + try: + return self.client.read_holding_registers( + address=address, count=count, slave=self.UNIT_ID + ) + except TypeError: + return self.client.read_holding_registers( + address=address, count=count, unit=self.UNIT_ID + ) + + def _write_register(self, address, value): + try: + return self.client.write_register( + address=address, value=value, slave=self.UNIT_ID + ) + except TypeError: + return self.client.write_register( + address=address, value=value, unit=self.UNIT_ID + ) + + def _write_registers(self, address, values): + try: + return self.client.write_registers( + address=address, values=values, slave=self.UNIT_ID + ) + except TypeError: + return self.client.write_registers( + address=address, values=values, unit=self.UNIT_ID + ) + + def open_connection(self): + """Open the TCP connection with the gripper.""" + connected = self.client.connect() + if connected is False: + raise ConnectionError("Failed to connect to OnRobot gripper") + + def close_connection(self): + """Close the TCP connection with the gripper.""" + self.client.close() + + def get_fingertip_offset(self): + """Read the current fingertip offset in millimeters.""" + result = self._read_holding_registers(address=258, count=1) + return result.registers[0] / 10.0 + + def get_width(self): + """Read current width between gripper fingers in millimeters.""" + result = self._read_holding_registers(address=267, count=1) + return result.registers[0] / 10.0 + + def get_status(self): + """Read the current device status as seven boolean-like flags.""" + result = self._read_holding_registers(address=268, count=1) + status = format(result.registers[0], "016b") + return [int(status[-idx]) for idx in range(1, 8)] + + def get_width_with_offset(self): + """Read current width with the configured fingertip offset included.""" + result = self._read_holding_registers(address=275, count=1) + return result.registers[0] / 10.0 + + def set_control_mode(self, command): + """Set the gripper control mode register.""" + return self._write_register(address=2, value=command) + + def set_target_force(self, force_val): + """Write target force in 1/10 newton units.""" + force_val = max(0, min(int(force_val), self.max_force)) + return self._write_register(address=0, value=force_val) + + def set_target_width(self, width_val): + """Write target width in 1/10 millimeter units.""" + width_val = max(0, min(int(width_val), self.max_width)) + return self._write_register(address=1, value=width_val) + + def close_gripper(self, force_val=400): + """Close the gripper.""" + return self.move_gripper(0, force_val) + + def open_gripper(self, force_val=400): + """Open the gripper to its maximum width.""" + return self.move_gripper(self.max_width, force_val) + + def move_gripper(self, width_val, force_val=400): + """Move gripper to width and force targets in register units.""" + force_val = max(0, min(int(force_val), self.max_force)) + width_val = max(0, min(int(width_val), self.max_width)) + return self._write_registers(address=0, values=[force_val, width_val, 16]) diff --git a/src/azas_gripper/azas_gripper/rg2_gripper_node.py b/src/azas_gripper/azas_gripper/rg2_gripper_node.py index da208a1..39a25bd 100644 --- a/src/azas_gripper/azas_gripper/rg2_gripper_node.py +++ b/src/azas_gripper/azas_gripper/rg2_gripper_node.py @@ -1,18 +1,56 @@ import rclpy from azas_interfaces.srv import SetGripper +from rclpy.executors import ExternalShutdownException from rclpy.node import Node class RG2GripperNode(Node): - """Azas-internal placeholder boundary; it does not command a real RG2.""" + """ROS service boundary for dry-run or real OnRobot RG2 commands.""" def __init__(self): super().__init__("rg2_gripper_node") - self.create_service(SetGripper, "/azas/gripper/open_close", self.on_set_gripper) - self.get_logger().warn( - "Azas internal SetGripper placeholder ready on /azas/gripper/open_close; " - "does not command real RG2 and does not provide /jarvis/rg2/* services" + self.declare_parameter("use_real_hardware", False) + self.declare_parameter("gripper", "rg2") + self.declare_parameter("host", "192.168.1.1") + self.declare_parameter("port", 502) + self.declare_parameter("default_open_width_m", 0.110) + self.declare_parameter("default_close_width_m", 0.0) + self.declare_parameter("default_force_n", 40.0) + + self.use_real_hardware = ( + self.get_parameter("use_real_hardware").get_parameter_value().bool_value ) + self.gripper = None + if self.use_real_hardware: + self.gripper = self._connect_real_gripper() + + self.create_service(SetGripper, "/azas/gripper/open_close", self.on_set_gripper) + if self.use_real_hardware: + self.get_logger().info( + "RG2 hardware service ready on /azas/gripper/open_close" + ) + else: + self.get_logger().warn( + "Dry-run gripper service ready on /azas/gripper/open_close; " + "set use_real_hardware:=true to command the real RG2" + ) + + def _connect_real_gripper(self): + from azas_gripper.onrobot import RG + + gripper = self.get_parameter("gripper").get_parameter_value().string_value + host = self.get_parameter("host").get_parameter_value().string_value + port = self.get_parameter("port").get_parameter_value().integer_value + self.get_logger().info(f"Connecting to OnRobot {gripper} at {host}:{port}") + return RG(gripper, host, port) + + def _width_m_to_register_units(self, width_m): + width_units = int(round(width_m * 10000.0)) + return max(0, min(width_units, self.gripper.max_width)) + + def _force_n_to_register_units(self, force_n): + force_units = int(round(force_n * 10.0)) + return max(0, min(force_units, self.gripper.max_force)) def on_set_gripper(self, request, response): command = request.command.lower() @@ -21,14 +59,49 @@ def on_set_gripper(self, request, response): response.message = f"unsupported command: {request.command}" return response + width_m = float(request.width_m) + force_n = float(request.force_n) + if force_n <= 0.0: + force_n = ( + self.get_parameter("default_force_n").get_parameter_value().double_value + ) + if command == "open" and width_m <= 0.0: + width_m = ( + self.get_parameter("default_open_width_m") + .get_parameter_value() + .double_value + ) + elif command == "close" and width_m <= 0.0: + width_m = ( + self.get_parameter("default_close_width_m") + .get_parameter_value() + .double_value + ) + self.get_logger().info( - f"gripper command={command} width_m={request.width_m:.3f} " - f"force_n={request.force_n:.1f}" + f"gripper command={command} width_m={width_m:.3f} force_n={force_n:.1f}" ) + if self.use_real_hardware: + try: + width_units = self._width_m_to_register_units(width_m) + force_units = self._force_n_to_register_units(force_n) + self.gripper.move_gripper(width_units, force_units) + except Exception as exc: + response.success = False + response.message = f"RG2 command failed: {exc}" + self.get_logger().error(response.message) + return response + + response.success = True + response.message = ( + f"sent RG2 {command} command " + f"width_units={width_units} force_units={force_units}" + ) + return response + response.success = True response.message = ( - "accepted no-motion placeholder command; does not command real RG2; " - "RG2 hardware binding and units 확인 필요" + "accepted dry-run command; real RG2 was not commanded" ) return response @@ -36,6 +109,11 @@ def on_set_gripper(self, request, response): def main(args=None): rclpy.init(args=args) node = RG2GripperNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() + try: + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() diff --git a/src/azas_gripper/package.xml b/src/azas_gripper/package.xml index 273aabf..a061a07 100644 --- a/src/azas_gripper/package.xml +++ b/src/azas_gripper/package.xml @@ -9,9 +9,9 @@ ament_python rclpy azas_interfaces + python3-pymodbus ament_python - From 5417aad1c429ee663cace473d46018e047345144 Mon Sep 17 00:00:00 2001 From: shining-b-02 Date: Fri, 15 May 2026 15:38:11 +0900 Subject: [PATCH 3/7] Add legacy MoveIt basic motion node --- .../azas_motion/mp_basic_legacy.py | 158 ++++++++++++++++++ src/azas_motion/setup.py | 1 + 2 files changed, 159 insertions(+) create mode 100644 src/azas_motion/azas_motion/mp_basic_legacy.py diff --git a/src/azas_motion/azas_motion/mp_basic_legacy.py b/src/azas_motion/azas_motion/mp_basic_legacy.py new file mode 100644 index 0000000..5ea41e6 --- /dev/null +++ b/src/azas_motion/azas_motion/mp_basic_legacy.py @@ -0,0 +1,158 @@ +#!/usr/bin/env python3 +"""Legacy MoveItPy basic motion example imported from dsr_practice.""" + +import math + +import rclpy +from geometry_msgs.msg import PoseStamped +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy +from rclpy.executors import ExternalShutdownException +from rclpy.logging import get_logger + + +GROUP_NAME = "manipulator" +BASE_FRAME = "base_link" +EE_LINK = "link_6" + +SAFE_X_MIN = 0.0 +SAFE_Y_MIN = -0.3 +SAFE_Y_MAX = 0.3 +SAFE_Z_MIN = 0.27 + + +def clamp_to_safe_workspace(x: float, y: float, z: float, logger): + """Clamp a requested pose target to the legacy safety workspace.""" + safe_x = x + safe_y = y + safe_z = z + + if safe_x < SAFE_X_MIN: + logger.warning( + f"Requested x ({safe_x:.3f} m) is below safety limit " + f"({SAFE_X_MIN:.3f} m). Clamping to SAFE_X_MIN." + ) + safe_x = SAFE_X_MIN + + if safe_y < SAFE_Y_MIN: + logger.warning( + f"Requested y ({safe_y:.3f} m) is below safety limit " + f"({SAFE_Y_MIN:.3f} m). Clamping to SAFE_Y_MIN." + ) + safe_y = SAFE_Y_MIN + elif safe_y > SAFE_Y_MAX: + logger.warning( + f"Requested y ({safe_y:.3f} m) is above safety limit " + f"({SAFE_Y_MAX:.3f} m). Clamping to SAFE_Y_MAX." + ) + safe_y = SAFE_Y_MAX + + if safe_z < SAFE_Z_MIN: + logger.warning( + f"Requested z ({safe_z:.3f} m) is below safety limit " + f"({SAFE_Z_MIN:.3f} m). Clamping to SAFE_Z_MIN." + ) + safe_z = SAFE_Z_MIN + + return safe_x, safe_y, safe_z + + +def plan_and_execute( + robot: MoveItPy, + planning_component, + logger, + pose_goal: PoseStamped = None, + plan_parameters=None, +): + """Plan and execute a legacy MoveItPy trajectory.""" + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + + planning_component.set_start_state_to_current_state() + planning_component.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link=EE_LINK, + ) + + logger.info("Planning trajectory") + if plan_parameters is not None: + plan_result = planning_component.plan(parameters=plan_parameters) + else: + plan_result = planning_component.plan() + + if not plan_result: + logger.error("Planning failed") + return False + + logger.info("Executing plan") + robot.execute( + group_name=GROUP_NAME, + robot_trajectory=plan_result.trajectory, + blocking=True, + ) + logger.info("Execution finished") + return True + + +def build_home_state(robot): + """Create the legacy M0609 home joint state.""" + home_state = RobotState(robot.get_robot_model()) + home_state.joint_positions = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(0.0), + } + home_state.update() + return home_state + + +def build_pose_goal(): + """Create the legacy pose goal used by the original example.""" + pose_goal = PoseStamped() + pose_goal.header.frame_id = BASE_FRAME + pose_goal.pose.position.x = 0.5 + pose_goal.pose.position.y = 0.0 + pose_goal.pose.position.z = 0.5 + pose_goal.pose.orientation.x = 0.0 + pose_goal.pose.orientation.y = 1.0 + pose_goal.pose.orientation.z = 0.0 + pose_goal.pose.orientation.w = 0.0 + return pose_goal + + +def main(args=None): + rclpy.init(args=args) + logger = get_logger("azas_motion.mp_basic_legacy") + + try: + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + logger.warning( + "Running legacy mp_basic example. This node can execute robot motion." + ) + + arm.set_start_state_to_current_state() + arm.set_goal_state(robot_state=build_home_state(robot)) + if not plan_and_execute(robot, arm, logger): + return + + plan_and_execute(robot, arm, logger, pose_goal=build_pose_goal()) + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/azas_motion/setup.py b/src/azas_motion/setup.py index af73e03..c73e15f 100644 --- a/src/azas_motion/setup.py +++ b/src/azas_motion/setup.py @@ -21,6 +21,7 @@ "console_scripts": [ "alignment_executor_node = azas_motion.alignment_executor_node:main", "dispenser_sequence_preview_node = azas_motion.dispenser_sequence_preview_node:main", + "mp_basic_legacy = azas_motion.mp_basic_legacy:main", "side_grasp_ik_preview_node = azas_motion.side_grasp_ik_preview_node:main", ], }, From 3109eb52b6ee7e2203d182f40ab0ddbf6ead3f47 Mon Sep 17 00:00:00 2001 From: shining-b-02 Date: Fri, 15 May 2026 15:38:55 +0900 Subject: [PATCH 4/7] Add legacy collision obstacle motion node --- .../azas_motion/collision_obstacle_legacy.py | 292 ++++++++++++++++++ src/azas_motion/package.xml | 2 + src/azas_motion/setup.py | 1 + 3 files changed, 295 insertions(+) create mode 100644 src/azas_motion/azas_motion/collision_obstacle_legacy.py diff --git a/src/azas_motion/azas_motion/collision_obstacle_legacy.py b/src/azas_motion/azas_motion/collision_obstacle_legacy.py new file mode 100644 index 0000000..3211162 --- /dev/null +++ b/src/azas_motion/azas_motion/collision_obstacle_legacy.py @@ -0,0 +1,292 @@ +#!/usr/bin/env python3 +import math + +import rclpy +from rclpy.logging import get_logger +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy + +from geometry_msgs.msg import Pose +from geometry_msgs.msg import PoseStamped +from shape_msgs.msg import SolidPrimitive +from moveit_msgs.msg import CollisionObject +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters + +# ================================ +# 기본 설정 +# ================================ +GROUP_NAME = "manipulator" # SRDF에 정의된 planning group 이름 +BASE_FRAME = "base_link" # 로봇 베이스 프레임 +EE_LINK = "link_6" # 엔드이펙터 링크 이름 (SRDF/URDF 기준) + +HOME_JOINTS_DEG = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0] +HOME_JOINTS_RAD = [math.radians(d) for d in HOME_JOINTS_DEG] + +# ====== 안전 작업 영역 정의 (base_link 기준) ====== +SAFE_X_MIN = 0.0 # x는 0 이상 +SAFE_Y_MIN = -0.3 # y 하한 +SAFE_Y_MAX = 0.3 # y 상한 +SAFE_Z_MIN = 0.27 # z는 이 값보다 낮아지면 안 됨 +# ================================================== + + +def clamp_to_safe_workspace(x: float, y: float, z: float, logger): + """안전 작업 영역으로 (x, y, z) 클램핑""" + safe_x = x + safe_y = y + safe_z = z + + if safe_x < SAFE_X_MIN: + logger.warning( + f"Requested x ({safe_x:.3f} m) is below safety limit " + f"({SAFE_X_MIN:.3f} m). Clamping to SAFE_X_MIN." + ) + safe_x = SAFE_X_MIN + + if safe_y < SAFE_Y_MIN: + logger.warning( + f"Requested y ({safe_y:.3f} m) is below safety limit " + f"({SAFE_Y_MIN:.3f} m). Clamping to SAFE_Y_MIN." + ) + safe_y = SAFE_Y_MIN + elif safe_y > SAFE_Y_MAX: + logger.warning( + f"Requested y ({safe_y:.3f} m) is above safety limit " + f"({SAFE_Y_MAX:.3f} m). Clamping to SAFE_Y_MAX." + ) + safe_y = SAFE_Y_MAX + + if safe_z < SAFE_Z_MIN: + logger.warning( + f"Requested z ({safe_z:.3f} m) is below safety limit " + f"({SAFE_Z_MIN:.3f} m). Clamping to SAFE_Z_MIN." + ) + safe_z = SAFE_Z_MIN + + return safe_x, safe_y, safe_z + + +def plan_and_execute( + robot: MoveItPy, + planning_component, + logger, + pose_goal: PoseStamped = None, + plan_parameters=None, +): + """ + 공식 문서 스타일 helper: 계획 후 곧바로 실행 + + - pose_goal이 주어지면: + · 안전 영역 클램핑 + · start_state = current + · pose 기반 goal 설정 (EE_LINK) + - 그 다음 plan_parameters 유무에 따라 plan() 호출 후 execute + """ + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + + planning_component.set_start_state_to_current_state() + planning_component.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link=EE_LINK, + ) + + logger.info("Planning trajectory") + + if plan_parameters is not None: + plan_result = planning_component.plan( + parameters=plan_parameters + ) + else: + plan_result = planning_component.plan() + + if not plan_result: + logger.error("Planning failed") + return False + + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + robot.execute( + group_name=GROUP_NAME, + robot_trajectory=robot_trajectory, + blocking=True, + ) + logger.info("Execution finished") + return True + + +def add_box_obstacle(node: Node, + object_id: str, + frame_id: str, + size_xyz=(0.2, 0.2, 0.2), + pos_xyz=(0.5, 0.0, 0.2)): + """ + collision_object 토픽으로 박스 장애물 추가(ADD). + - QoS를 TRANSIENT_LOCAL로 해서 구독자가 나중에 붙어도 유지되게 함(퍼블리셔가 살아있는 동안). + """ + qos = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) + pub = node.create_publisher(CollisionObject, "/collision_object", qos) + + box = CollisionObject() + box.id = object_id + box.header.frame_id = frame_id + + primitive = SolidPrimitive() + primitive.type = SolidPrimitive.BOX + primitive.dimensions = list(size_xyz) # [x, y, z] + + pose = Pose() + pose.position.x, pose.position.y, pose.position.z = pos_xyz + pose.orientation.w = 1.0 # 회전 없음 + + box.primitives.append(primitive) + box.primitive_poses.append(pose) + box.operation = CollisionObject.ADD + + pub.publish(box) + node.get_logger().info( + f"[scene] ADD box id={object_id} frame={frame_id} size={size_xyz} pos={pos_xyz}" + ) + + +def main(args=None): + # ================================ + # MoveItPy 인스턴스 생성 + # ================================ + rclpy.init(args=args) + logger = get_logger("m0609.moveit_py.waypoint_pilz") + + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + logger.info("MoveItPy instance created") + robot_model = robot.get_robot_model() + + # ================================ + # 플래닝 파라미터 + # ================================ + home_params = PlanRequestParameters(robot) + waypoint_params = PlanRequestParameters(robot) + + # MoveIt2 config의 planning_pipelines.pipeline_names와 일치해야 함 + home_params.planning_pipeline = "ompl" + home_params.planner_id = "RRTConnect" + + waypoint_params.planning_pipeline = "pilz_industrial_motion_planner" + waypoint_params.planner_id = "PTP" + + home_params.max_velocity_scaling_factor = 0.2 + home_params.max_acceleration_scaling_factor = 0.1 + home_params.planning_time = 2.0 + + waypoint_params.max_velocity_scaling_factor = 0.15 + waypoint_params.max_acceleration_scaling_factor = 0.1 + waypoint_params.planning_time = 5.0 + + home_state = RobotState(robot_model) + home_state.set_joint_group_positions(GROUP_NAME, HOME_JOINTS_RAD) + home_state.update() + + # HOME 이동 (조인트 목표 → pose_goal 없이, plan_parameters만 사용) + arm.set_start_state_to_current_state() + arm.set_goal_state(robot_state=home_state) + plan_and_execute( + robot, + arm, + logger, + plan_parameters=home_params, + ) + + # ================================ + # 기본 3개 waypoint + 수동 우회 2개 = 총 5개 경로점 + # ================================ + logger.info("=== Waypoints from HOME, orientation fixed (Pilz PTP) ===") + + WAYPOINTS = [ + { # waypoint_1 + "pos": {"x": 0.45, "y": 0.22, "z": 0.52}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + { # detour_1 + "pos": {"x": 0.45, "y": 0.22, "z": 0.28}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + { # waypoint_2 + "pos": {"x": 0.62, "y": -0.18, "z": 0.28}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + { # detour_2 + "pos": {"x": 0.36, "y": -0.26, "z": 0.28}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + { # waypoint_3 + "pos": {"x": 0.36, "y": -0.26, "z": 0.55}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + ] + + # ====== 여기(중간)에서 장애물 추가 (경로 사이 2개) ====== + scene_node = rclpy.create_node("scene_obstacle_publisher") + mid_12 = (0.535, 0.020, 0.600) + mid_23 = (0.590, -0.220, 0.615) + obstacles = [ + {"id": "mid_box_1", "size_xyz": (0.10, 0.10, 0.10), "pos_xyz": mid_12}, + {"id": "mid_box_2", "size_xyz": (0.10, 0.10, 0.10), "pos_xyz": mid_23}, + ] + for obstacle in obstacles: + add_box_obstacle( + node=scene_node, + object_id=obstacle["id"], + frame_id=BASE_FRAME, + size_xyz=obstacle["size_xyz"], + pos_xyz=obstacle["pos_xyz"], + ) + + pose_goal = PoseStamped() + pose_goal.header.frame_id = BASE_FRAME + + for i, wp in enumerate(WAYPOINTS, start=1): + pos = wp["pos"] + ori = wp["ori"] + + logger.info( + f"--- Waypoint {i}: " + f"x={pos['x']:.3f}, y={pos['y']:.3f}, z={pos['z']:.3f} ---" + ) + + # pose_goal에 의도 좌표/자세 설정 + pose_goal.pose.position.x = pos["x"] + pose_goal.pose.position.y = pos["y"] + pose_goal.pose.position.z = pos["z"] + + pose_goal.pose.orientation.x = ori["x"] + pose_goal.pose.orientation.y = ori["y"] + pose_goal.pose.orientation.z = ori["z"] + pose_goal.pose.orientation.w = ori["w"] + + # plan_and_execute 안에서 안전영역 + goal 설정 + Pilz PTP 플래너 호출 + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=waypoint_params, + ) + + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/azas_motion/package.xml b/src/azas_motion/package.xml index 36cf95a..5fa9d09 100644 --- a/src/azas_motion/package.xml +++ b/src/azas_motion/package.xml @@ -9,8 +9,10 @@ ament_python rclpy geometry_msgs + moveit_msgs nav_msgs sensor_msgs + shape_msgs visualization_msgs azas_interfaces dsr_moveit_config_m0609 diff --git a/src/azas_motion/setup.py b/src/azas_motion/setup.py index c73e15f..ece3f0d 100644 --- a/src/azas_motion/setup.py +++ b/src/azas_motion/setup.py @@ -20,6 +20,7 @@ entry_points={ "console_scripts": [ "alignment_executor_node = azas_motion.alignment_executor_node:main", + "collision_obstacle_legacy = azas_motion.collision_obstacle_legacy:main", "dispenser_sequence_preview_node = azas_motion.dispenser_sequence_preview_node:main", "mp_basic_legacy = azas_motion.mp_basic_legacy:main", "side_grasp_ik_preview_node = azas_motion.side_grasp_ik_preview_node:main", From 37e3e8221ed57723e640a9c22ea3de5d7af6ab76 Mon Sep 17 00:00:00 2001 From: shining-b-02 Date: Fri, 15 May 2026 15:39:32 +0900 Subject: [PATCH 5/7] Add legacy pick and place motion node --- .../azas_motion/pick_and_place_legacy.py | 305 ++++++++++++++++++ src/azas_motion/package.xml | 1 + src/azas_motion/setup.py | 1 + 3 files changed, 307 insertions(+) create mode 100644 src/azas_motion/azas_motion/pick_and_place_legacy.py diff --git a/src/azas_motion/azas_motion/pick_and_place_legacy.py b/src/azas_motion/azas_motion/pick_and_place_legacy.py new file mode 100644 index 0000000..096573b --- /dev/null +++ b/src/azas_motion/azas_motion/pick_and_place_legacy.py @@ -0,0 +1,305 @@ +#!/usr/bin/env python3 + +import math +import time + +import rclpy +from rclpy.logging import get_logger + +from geometry_msgs.msg import PoseStamped +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters + +# ====== OnRobot RG2 설정 ====== +from azas_gripper.onrobot import RG + +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 + +# 그리퍼 폭 (raw 단위: 1/10 mm) +GRIPPER_OPEN_WIDTH = 500 # 50.0 mm +GRIPPER_CLOSE_WIDTH = 150 # 20.0 mm +GRIPPER_FORCE = 300 # 약 20 N + +# ================================ +# 기본 설정 +# ================================ +GROUP_NAME = "manipulator" # SRDF에 정의된 planning group 이름 +BASE_FRAME = "base_link" # 로봇 베이스 프레임 +EE_LINK = "link_6" # 엔드이펙터 링크 이름 (SRDF/URDF 기준) + +HOME_JOINTS_DEG = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0] +HOME_JOINTS_RAD = [math.radians(d) for d in HOME_JOINTS_DEG] + +# ====== 안전 작업 영역 정의 (base_link 기준) ====== +SAFE_X_MIN = 0.0 # x는 0 이상 +SAFE_Y_MIN = -0.3 # y 하한 +SAFE_Y_MAX = 0.3 # y 상한 +SAFE_Z_MIN = 0.27 # z는 이 값보다 낮아지면 안 됨 +# ================================================== + +# ====== 기어 Pick/Place 포즈 (base_link 기준) ====== +GEAR_TASK = { + "pick": { + "pos": {"x": 0.427, "y": 0.148, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + "place": { + "pos": {"x": 0.426, "y": -0.153, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, +} + +APPROACH_OFFSET = 0.05 # 위에서 접근/후퇴할 거리 [m] + + +def clamp_to_safe_workspace(x: float, y: float, z: float, logger): + """안전 작업 영역으로 (x, y, z) 클램핑""" + safe_x = x + safe_y = y + safe_z = z + + if safe_x < SAFE_X_MIN: + logger.warning( + f"Requested x ({safe_x:.3f} m) is below safety limit " + f"({SAFE_X_MIN:.3f} m). Clamping to SAFE_X_MIN." + ) + safe_x = SAFE_X_MIN + + if safe_y < SAFE_Y_MIN: + logger.warning( + f"Requested y ({safe_y:.3f} m) is below safety limit " + f"({SAFE_Y_MIN:.3f} m). Clamping to SAFE_Y_MIN." + ) + safe_y = SAFE_Y_MIN + elif safe_y > SAFE_Y_MAX: + logger.warning( + f"Requested y ({safe_y:.3f} m) is above safety limit " + f"({SAFE_Y_MAX:.3f} m). Clamping to SAFE_Y_MAX." + ) + safe_y = SAFE_Y_MAX + + if safe_z < SAFE_Z_MIN: + logger.warning( + f"Requested z ({safe_z:.3f} m) is below safety limit " + f"({SAFE_Z_MIN:.3f} m). Clamping to SAFE_Z_MIN." + ) + safe_z = SAFE_Z_MIN + + return safe_x, safe_y, safe_z + + +def plan_and_execute( + robot: MoveItPy, + planning_component, + logger, + pose_goal: PoseStamped = None, + plan_parameters=None, +): + """ + 공식 문서 스타일 helper: 계획 후 곧바로 실행 + + - pose_goal이 주어지면: + · 안전 영역 클램핑 + · start_state = current + · pose 기반 goal 설정 (EE_LINK) + - 그 다음 plan_parameters 유무에 따라 plan() 호출 후 execute + """ + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + + planning_component.set_start_state_to_current_state() + planning_component.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link=EE_LINK, + ) + + logger.info("Planning trajectory") + + if plan_parameters is not None: + plan_result = planning_component.plan( + parameters=plan_parameters + ) + else: + plan_result = planning_component.plan() + + if not plan_result: + logger.error("Planning failed") + return False + + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + robot.execute( + group_name=GROUP_NAME, + robot_trajectory=robot_trajectory, + blocking=True, + ) + logger.info("Execution finished") + return True + + +def main(args=None): + rclpy.init(args=args) + logger = get_logger("gear_pick_place_simple") + + # ---- Gripper ---- + gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + time.sleep(0.5) + gripper.move_gripper(GRIPPER_OPEN_WIDTH, GRIPPER_FORCE) + + # ---- MoveIt ---- + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + robot_model = robot.get_robot_model() + + # ---- PlanRequestParameters (HOME / Pilz) ---- + home_params = PlanRequestParameters(robot) + home_params.planning_pipeline = "ompl" + home_params.planner_id = "RRTConnectkConfigDefault" + home_params.max_velocity_scaling_factor = 0.2 + home_params.max_acceleration_scaling_factor = 0.1 + home_params.planning_time = 2.0 + + pilz_params = PlanRequestParameters(robot) + pilz_params.planning_pipeline = "pilz_industrial_motion_planner" + pilz_params.planner_id = "PTP" + pilz_params.max_velocity_scaling_factor = 0.15 + pilz_params.max_acceleration_scaling_factor = 0.1 + pilz_params.planning_time = 2.0 + + # ---- HOME 자세로 이동 (joint goal) ---- + logger.info("=== HOME 자세로 이동 ===") + home_state = RobotState(robot_model) + home_state.set_joint_group_positions(GROUP_NAME, HOME_JOINTS_RAD) + home_state.update() + + arm.set_start_state_to_current_state() + arm.set_goal_state(robot_state=home_state) + plan_and_execute(robot, arm, logger, plan_parameters=home_params) + + logger.info("=== Gear Pick & Place 시작 ===") + + # ---- PoseStamped 공용 객체 준비 ---- + pose_goal = PoseStamped() + pose_goal.header.frame_id = BASE_FRAME + + pick = GEAR_TASK["pick"] + place = GEAR_TASK["place"] + + # ============================ + # PICK + # ============================ + pos = pick["pos"] + ori = pick["ori"] + + # 1) pick 위에서 접근 + pose_goal.pose.position.x = pos["x"] + pose_goal.pose.position.y = pos["y"] + pose_goal.pose.position.z = pos["z"] + APPROACH_OFFSET + + pose_goal.pose.orientation.x = ori["x"] + pose_goal.pose.orientation.y = ori["y"] + pose_goal.pose.orientation.z = ori["z"] + pose_goal.pose.orientation.w = ori["w"] + + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=pilz_params, + ) + + # 2) pick 높이까지 내려가기 + pose_goal.pose.position.z = pos["z"] + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=pilz_params, + ) + + # 3) 그리퍼 닫기 (집기) + logger.info("Gripper CLOSE – gear pick") + gripper.move_gripper(GRIPPER_CLOSE_WIDTH, GRIPPER_FORCE) + time.sleep(1.0) + + # 4) 다시 위로 + pose_goal.pose.position.z = pos["z"] + APPROACH_OFFSET + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=pilz_params, + ) + + # ============================ + # PLACE + # ============================ + pos = place["pos"] + ori = place["ori"] + + # 5) place 위에서 접근 + pose_goal.pose.position.x = pos["x"] + pose_goal.pose.position.y = pos["y"] + pose_goal.pose.position.z = pos["z"] + APPROACH_OFFSET + + pose_goal.pose.orientation.x = ori["x"] + pose_goal.pose.orientation.y = ori["y"] + pose_goal.pose.orientation.z = ori["z"] + pose_goal.pose.orientation.w = ori["w"] + + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=pilz_params, + ) + + # 6) place 높이까지 내려가기 + pose_goal.pose.position.z = pos["z"] + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=pilz_params, + ) + + # 7) 그리퍼 열기 (놓기) + logger.info("Gripper OPEN – gear place") + gripper.move_gripper(GRIPPER_OPEN_WIDTH, GRIPPER_FORCE) + time.sleep(1.0) + + # 8) 다시 위로 + pose_goal.pose.position.z = pos["z"] + APPROACH_OFFSET + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=pilz_params, + ) + + # 마지막으로 HOME으로 복귀 (선택) + arm.set_start_state_to_current_state() + arm.set_goal_state(robot_state=home_state) + plan_and_execute(robot, arm, logger, plan_parameters=home_params) + + logger.info("=== Gear Pick & Place 노드 종료 ===") + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/azas_motion/package.xml b/src/azas_motion/package.xml index 5fa9d09..7f8a9c0 100644 --- a/src/azas_motion/package.xml +++ b/src/azas_motion/package.xml @@ -14,6 +14,7 @@ sensor_msgs shape_msgs visualization_msgs + azas_gripper azas_interfaces dsr_moveit_config_m0609 moveit_py diff --git a/src/azas_motion/setup.py b/src/azas_motion/setup.py index ece3f0d..4bbcdf9 100644 --- a/src/azas_motion/setup.py +++ b/src/azas_motion/setup.py @@ -23,6 +23,7 @@ "collision_obstacle_legacy = azas_motion.collision_obstacle_legacy:main", "dispenser_sequence_preview_node = azas_motion.dispenser_sequence_preview_node:main", "mp_basic_legacy = azas_motion.mp_basic_legacy:main", + "pick_and_place_legacy = azas_motion.pick_and_place_legacy:main", "side_grasp_ik_preview_node = azas_motion.side_grasp_ik_preview_node:main", ], }, From 168af4b9660a22d6835043e5e2a28d0d958dd533 Mon Sep 17 00:00:00 2001 From: shining-b-02 Date: Fri, 15 May 2026 15:41:52 +0900 Subject: [PATCH 6/7] Add legacy YOLO cup pick node --- .../yolo_cup_pick_legacy_node.py | 1129 +++++++++++++++++ .../config/T_gripper2camera.npy | Bin 0 -> 256 bytes src/azas_perception/package.xml | 5 + src/azas_perception/setup.py | 3 + 4 files changed, 1137 insertions(+) create mode 100644 src/azas_perception/azas_perception/yolo_cup_pick_legacy_node.py create mode 100644 src/azas_perception/config/T_gripper2camera.npy diff --git a/src/azas_perception/azas_perception/yolo_cup_pick_legacy_node.py b/src/azas_perception/azas_perception/yolo_cup_pick_legacy_node.py new file mode 100644 index 0000000..cd19cda --- /dev/null +++ b/src/azas_perception/azas_perception/yolo_cup_pick_legacy_node.py @@ -0,0 +1,1129 @@ +#!/usr/bin/env python3 + +import math +import time +from collections import Counter +from pathlib import Path + +import cv2 +import numpy as np +import rclpy +from ament_index_python.packages import get_package_share_directory +from cv_bridge import CvBridge +from geometry_msgs.msg import PoseStamped +from rcl_interfaces.msg import ParameterDescriptor +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters +from rclpy.node import Node +from scipy.spatial.transform import Rotation +from sensor_msgs.msg import CameraInfo, Image +from ultralytics import YOLO + +from azas_gripper.onrobot import RG + + +GROUP_NAME = "manipulator" +BASE_FRAME = "base_link" +EE_LINK = "link_6" + +HOME_JOINTS = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(90.0), +} +HOME_JOINTS_RAD = [ + math.radians(0.0), + math.radians(0.0), + math.radians(90.0), + math.radians(0.0), + math.radians(90.0), + math.radians(90.0), +] + +SAFE_X_MIN = 0.0 +SAFE_Y_MIN = -0.35 +SAFE_Y_MAX = 0.35 +SAFE_Z_MIN = 0.20 + +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 +GRIPPER_OPEN_WIDTH = 1100 +GRIPPER_CLOSE_WIDTH = 120 +GRIPPER_FORCE = 250 +GRIPPER_OPEN_TIMEOUT_SEC = 5.0 +GRIPPER_STATUS_POLL_SEC = 0.15 + +DOWN_ORI = {"x": 0.0, "y": 1.0, "z": 0.0, "w": 0.0} + + +def clamp_to_safe_workspace(x, y, z, logger, z_min=SAFE_Z_MIN): + if x < SAFE_X_MIN: + logger.warning(f"x={x:.3f} -> {SAFE_X_MIN:.3f}") + x = SAFE_X_MIN + if y < SAFE_Y_MIN: + logger.warning(f"y={y:.3f} -> {SAFE_Y_MIN:.3f}") + y = SAFE_Y_MIN + elif y > SAFE_Y_MAX: + logger.warning(f"y={y:.3f} -> {SAFE_Y_MAX:.3f}") + y = SAFE_Y_MAX + if z < z_min: + logger.warning(f"z={z:.3f} -> {z_min:.3f}") + z = z_min + return x, y, z + + +def make_pose(x, y, z, ori=None): + if ori is None: + ori = DOWN_ORI + pose = PoseStamped() + pose.header.frame_id = BASE_FRAME + pose.pose.position.x = float(x) + pose.pose.position.y = float(y) + pose.pose.position.z = float(z) + pose.pose.orientation.x = ori["x"] + pose.pose.orientation.y = ori["y"] + pose.pose.orientation.z = ori["z"] + pose.pose.orientation.w = ori["w"] + return pose + + +def parse_bool(value): + if isinstance(value, bool): + return value + if isinstance(value, (int, float)): + return bool(value) + return str(value).strip().lower() in {"1", "true", "yes", "on"} + + +def parse_axis(value): + if isinstance(value, bool): + return "y" if value else "x" + + normalized = str(value).strip().lower() + if normalized in {"y", "y_axis", "axis_y", "true", "yes", "on"}: + return "y" + if normalized in {"x", "x_axis", "axis_x"}: + return "x" + return normalized + + +def quat_dict_from_matrix(matrix): + qx, qy, qz, qw = Rotation.from_matrix(matrix).as_quat() + return { + "x": float(qx), + "y": float(qy), + "z": float(qz), + "w": float(qw), + } + + +def quat_dict_from_euler(roll_deg, pitch_deg, yaw_deg): + qx, qy, qz, qw = Rotation.from_euler( + "xyz", + [roll_deg, pitch_deg, yaw_deg], + degrees=True, + ).as_quat() + return { + "x": float(qx), + "y": float(qy), + "z": float(qz), + "w": float(qw), + } + + +def get_ee_matrix(moveit_robot): + psm = moveit_robot.get_planning_scene_monitor() + with psm.read_only() as scene: + transform = scene.current_state.get_global_link_transform(EE_LINK) + return np.asarray(transform, dtype=float) + + +class YoloCupPickNode(Node): + def __init__(self): + super().__init__("yolo_cup_pick_node") + + self.declare_parameter( + "model_path", + "/home/ssu/ros2_ws/yolo_runs/cup_yolov8n_ft1/weights/best.pt", + ) + self.declare_parameter("conf", 0.35) + self.declare_parameter("imgsz", 640) + self.declare_parameter("device", "cpu") + self.declare_parameter("target_class", "cup") + self.declare_parameter("auto_pick", False) + self.declare_parameter("auto_pick_interval", 3.0) + self.declare_parameter("pick_depth_ratio", 0.55) + self.declare_parameter("depth_patch_radius", 7) + self.declare_parameter("min_depth_valid_ratio", 0.03) + self.declare_parameter("min_depth_m", 0.15) + self.declare_parameter("max_depth_m", 1.20) + self.declare_parameter("redetect_on_approach", True) + self.declare_parameter("redetect_settle_sec", 0.5) + self.declare_parameter("grasp_mode", "side") + dynamic_param = ParameterDescriptor(dynamic_typing=True) + self.declare_parameter("side_grasp_axis", "y_axis", dynamic_param) + self.declare_parameter("side_grasp_direction", -1.0) + self.declare_parameter("side_approach_offset", 0.12) + self.declare_parameter("side_staging_offset", 0.24) + self.declare_parameter("side_grasp_offset", 0.035) + self.declare_parameter("side_grasp_z_offset", 0.05) + self.declare_parameter("side_orientation_mode", "approach") + self.declare_parameter("side_tool_roll_deg", 0.0) + self.declare_parameter("side_roll_deg", 0.0) + self.declare_parameter("side_pitch_deg", 90.0) + self.declare_parameter("side_yaw_deg", 0.0) + self.declare_parameter("pick_z_offset", 0.20) + self.declare_parameter("approach_offset", 0.12) + self.declare_parameter("safe_z", 0.50) + self.declare_parameter("min_motion_z", 0.12) + self.declare_parameter("return_home_after_task", True) + self.declare_parameter("verify_motion", True) + self.declare_parameter("motion_verify_tolerance", 0.01) + self.declare_parameter("move_to_camera_home", True) + self.declare_parameter("camera_home_x", 0.45) + self.declare_parameter("camera_home_y", 0.00) + self.declare_parameter("camera_home_z", 0.62) + self.declare_parameter("place_x", 0.45) + self.declare_parameter("place_y", 0.0) + self.declare_parameter("place_z", 0.30) + + self.model_path = self.get_parameter("model_path").value + self.conf = float(self.get_parameter("conf").value) + self.imgsz = int(self.get_parameter("imgsz").value) + self.device = self.get_parameter("device").value + self.target_class = self.get_parameter("target_class").value + self.auto_pick = parse_bool(self.get_parameter("auto_pick").value) + self.auto_pick_interval = float(self.get_parameter("auto_pick_interval").value) + self.pick_depth_ratio = float(self.get_parameter("pick_depth_ratio").value) + self.depth_patch_radius = int(self.get_parameter("depth_patch_radius").value) + self.min_depth_valid_ratio = float( + self.get_parameter("min_depth_valid_ratio").value + ) + self.min_depth_m = float(self.get_parameter("min_depth_m").value) + self.max_depth_m = float(self.get_parameter("max_depth_m").value) + self.redetect_on_approach = parse_bool( + self.get_parameter("redetect_on_approach").value + ) + self.redetect_settle_sec = float(self.get_parameter("redetect_settle_sec").value) + self.grasp_mode = str(self.get_parameter("grasp_mode").value).strip().lower() + self.side_grasp_axis = parse_axis(self.get_parameter("side_grasp_axis").value) + self.side_grasp_direction = float( + self.get_parameter("side_grasp_direction").value + ) + self.side_approach_offset = float( + self.get_parameter("side_approach_offset").value + ) + self.side_staging_offset = float( + self.get_parameter("side_staging_offset").value + ) + self.side_grasp_offset = float(self.get_parameter("side_grasp_offset").value) + self.side_grasp_z_offset = float( + self.get_parameter("side_grasp_z_offset").value + ) + self.side_orientation_mode = str( + self.get_parameter("side_orientation_mode").value + ).strip().lower() + self.side_tool_roll_deg = float( + self.get_parameter("side_tool_roll_deg").value + ) + self.side_roll_deg = float(self.get_parameter("side_roll_deg").value) + self.side_pitch_deg = float(self.get_parameter("side_pitch_deg").value) + self.side_yaw_deg = float(self.get_parameter("side_yaw_deg").value) + self.pick_z_offset = float(self.get_parameter("pick_z_offset").value) + self.approach_offset = float(self.get_parameter("approach_offset").value) + self.safe_z = float(self.get_parameter("safe_z").value) + self.min_motion_z = float(self.get_parameter("min_motion_z").value) + self.return_home_after_task = parse_bool( + self.get_parameter("return_home_after_task").value + ) + self.verify_motion = parse_bool(self.get_parameter("verify_motion").value) + self.motion_verify_tolerance = float( + self.get_parameter("motion_verify_tolerance").value + ) + self.move_to_camera_home = parse_bool( + self.get_parameter("move_to_camera_home").value + ) + self.camera_home_x = float(self.get_parameter("camera_home_x").value) + self.camera_home_y = float(self.get_parameter("camera_home_y").value) + self.camera_home_z = float(self.get_parameter("camera_home_z").value) + self.place_x = float(self.get_parameter("place_x").value) + self.place_y = float(self.get_parameter("place_y").value) + self.place_z = float(self.get_parameter("place_z").value) + + model_file = Path(self.model_path).expanduser() + if not model_file.exists(): + raise FileNotFoundError(f"YOLO model not found: {model_file}") + + self.get_logger().info(f"Loading YOLO model: {model_file}") + self.model = YOLO(str(model_file)) + self.get_logger().info(f"YOLO classes: {self.model.names}") + if self.target_class not in self.model.names.values(): + raise ValueError( + f"target_class='{self.target_class}' is not in model classes " + f"{self.model.names}" + ) + if self.grasp_mode not in {"side", "top"}: + raise ValueError("grasp_mode must be 'side' or 'top'") + if self.side_grasp_axis not in {"x", "y"}: + raise ValueError("side_grasp_axis must be 'x' or 'y'") + self.side_grasp_direction = 1.0 if self.side_grasp_direction >= 0 else -1.0 + if self.side_orientation_mode not in {"approach", "euler", "home"}: + raise ValueError( + "side_orientation_mode must be 'approach', 'euler', or 'home'" + ) + if self.side_staging_offset < self.side_approach_offset: + self.get_logger().warning( + "side_staging_offset is smaller than side_approach_offset; " + "using side_approach_offset for staging." + ) + self.side_staging_offset = self.side_approach_offset + + self.bridge = CvBridge() + self.color_image = None + self.depth_image = None + self.intrinsics = None + self.last_detection = None + self.picking = False + self.has_picked_once = False + self.last_pick_time = 0.0 + self.last_status = "waiting for command" + + calib_file = ( + Path(get_package_share_directory("azas_perception")) + / "config" + / "T_gripper2camera.npy" + ) + self.gripper2cam = np.load(str(calib_file)).astype(float) + self.gripper2cam[:3, 3] /= 1000.0 + self.get_logger().info(f"Loaded hand-eye calibration: {calib_file}") + + self.gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + + self.get_logger().info("Initializing MoveItPy...") + self.robot = MoveItPy(node_name="yolo_cup_pick_moveit_py") + self.arm = self.robot.get_planning_component(GROUP_NAME) + self.robot_model = self.robot.get_robot_model() + self.get_logger().info("MoveItPy initialized") + + self.ompl_params = PlanRequestParameters(self.robot) + self.ompl_params.planning_pipeline = "ompl" + self.ompl_params.planner_id = "RRTConnect" + self.ompl_params.max_velocity_scaling_factor = 0.2 + self.ompl_params.max_acceleration_scaling_factor = 0.1 + self.ompl_params.planning_time = 3.0 + + self.pilz_params = PlanRequestParameters(self.robot) + self.pilz_params.planning_pipeline = "pilz_industrial_motion_planner" + self.pilz_params.planner_id = "PTP" + self.pilz_params.max_velocity_scaling_factor = 0.12 + self.pilz_params.max_acceleration_scaling_factor = 0.08 + self.pilz_params.planning_time = 3.0 + + self.home_ori = DOWN_ORI + + self.create_subscription( + CameraInfo, + "/camera/camera/color/camera_info", + self._camera_info_callback, + 10, + ) + self.create_subscription( + Image, + "/camera/camera/color/image_raw", + self._color_callback, + 10, + ) + self.create_subscription( + Image, + "/camera/camera/aligned_depth_to_color/image_raw", + self._depth_callback, + 10, + ) + + def _camera_info_callback(self, msg): + self.intrinsics = { + "fx": msg.k[0], + "fy": msg.k[4], + "cx": msg.k[2], + "cy": msg.k[5], + } + + def _color_callback(self, msg): + self.color_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") + + def _depth_callback(self, msg): + self.depth_image = self.bridge.imgmsg_to_cv2( + msg, desired_encoding="passthrough" + ) + + def plan_and_execute(self, pose_goal=None, state_goal=None, params=None): + log = self.get_logger() + self.arm.set_start_state_to_current_state() + start_matrix = get_ee_matrix(self.robot) + start_xyz = start_matrix[:3, 3].copy() + goal_xyz = None + + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + x, y, z = clamp_to_safe_workspace(x, y, z, log, self.min_motion_z) + pose_goal.pose.position.x = x + pose_goal.pose.position.y = y + pose_goal.pose.position.z = z + goal_xyz = np.array([x, y, z], dtype=float) + log.info( + f"Planning pose goal -> ({x:.3f}, {y:.3f}, {z:.3f}) " + f"from ({start_xyz[0]:.3f}, {start_xyz[1]:.3f}, {start_xyz[2]:.3f})" + ) + self.arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link=EE_LINK) + elif state_goal is not None: + log.info( + f"Planning joint/state goal from EE " + f"({start_xyz[0]:.3f}, {start_xyz[1]:.3f}, {start_xyz[2]:.3f})" + ) + self.arm.set_goal_state(robot_state=state_goal) + else: + log.error("No pose/state goal was provided") + return False + + plan_result = self.arm.plan(parameters=params) if params else self.arm.plan() + if not plan_result: + log.error("Planning failed") + return False + + self.robot.execute( + group_name=GROUP_NAME, + robot_trajectory=plan_result.trajectory, + blocking=True, + ) + self.spin_for_camera_update(0.2) + + end_matrix = get_ee_matrix(self.robot) + end_xyz = end_matrix[:3, 3].copy() + moved = float(np.linalg.norm(end_xyz - start_xyz)) + if goal_xyz is None: + log.info( + f"Execution finished. EE moved {moved:.3f} m -> " + f"({end_xyz[0]:.3f}, {end_xyz[1]:.3f}, {end_xyz[2]:.3f})" + ) + else: + goal_error = float(np.linalg.norm(end_xyz - goal_xyz)) + log.info( + f"Execution finished. EE moved {moved:.3f} m, " + f"goal_error={goal_error:.3f} m -> " + f"({end_xyz[0]:.3f}, {end_xyz[1]:.3f}, {end_xyz[2]:.3f})" + ) + if self.verify_motion and goal_error > self.motion_verify_tolerance: + log.error( + "MoveIt execution did not reach the requested pose. " + "Check that the real robot MoveIt/trajectory controller is running." + ) + return False + return True + + def move_joint_home(self): + home_state = RobotState(self.robot_model) + home_state.set_joint_group_positions(GROUP_NAME, HOME_JOINTS_RAD) + home_state.update() + if not self.plan_and_execute(state_goal=home_state, params=self.ompl_params): + return False + + transform = get_ee_matrix(self.robot) + self.update_home_orientation_from_matrix(transform) + return True + + def move_home(self): + if self.move_to_camera_home: + return self.move_camera_home() + return self.move_joint_home() + + def update_home_orientation_from_matrix(self, transform): + qx, qy, qz, qw = Rotation.from_matrix(transform[:3, :3]).as_quat() + self.home_ori = { + "x": float(qx), + "y": float(qy), + "z": float(qz), + "w": float(qw), + } + + def move_camera_home(self): + log = self.get_logger() + candidate_zs = [] + for z in (self.camera_home_z, 0.62, 0.58, 0.54): + if z > self.camera_home_z + 1e-6: + continue + if all(abs(z - candidate) > 1e-6 for candidate in candidate_zs): + candidate_zs.append(z) + + for idx, z in enumerate(candidate_zs): + if idx > 0: + log.warning( + f"Camera home IK failed at higher z; retrying z={z:.3f}" + ) + + log.info( + f"Move CAMERA HOME -> ({self.camera_home_x:.3f}, " + f"{self.camera_home_y:.3f}, {z:.3f})" + ) + if not self.plan_and_execute( + pose_goal=make_pose( + self.camera_home_x, + self.camera_home_y, + z, + self.home_ori, + ), + params=self.pilz_params, + ): + continue + + self.camera_home_z = z + transform = get_ee_matrix(self.robot) + self.update_home_orientation_from_matrix(transform) + return True + + return False + + def wait_until_gripper_idle(self, timeout_sec=GRIPPER_OPEN_TIMEOUT_SEC): + log = self.get_logger() + start_time = time.time() + while time.time() - start_time < timeout_sec: + status = self.gripper.get_status() + busy = bool(status[0]) + if not busy: + try: + width_mm = self.gripper.get_width_with_offset() + log.info(f"Gripper ready. current width={width_mm:.1f} mm") + except Exception as exc: + log.warning(f"Gripper width read failed: {exc}") + return True + time.sleep(GRIPPER_STATUS_POLL_SEC) + + log.warning("Timed out waiting for gripper to finish opening.") + return False + + def open_gripper_max(self, wait=False): + self.get_logger().info( + f"Open gripper to max width={GRIPPER_OPEN_WIDTH} " + f"({GRIPPER_OPEN_WIDTH / 10.0:.1f} mm)" + ) + self.gripper.move_gripper(GRIPPER_OPEN_WIDTH, GRIPPER_FORCE) + if wait: + return self.wait_until_gripper_idle() + return True + + def detect_objects(self, image): + results = self.model.predict( + source=image, + imgsz=self.imgsz, + conf=self.conf, + device=self.device, + verbose=False, + ) + boxes = results[0].boxes + if boxes is None or len(boxes) == 0: + self.last_detection = None + return [] + + detections = [] + for box in boxes: + cls_id = int(box.cls[0]) + class_name = self.model.names.get(cls_id, str(cls_id)) + x1, y1, x2, y2 = box.xyxy[0].cpu().numpy().tolist() + x1, y1, x2, y2 = int(x1), int(y1), int(x2), int(y2) + detections.append( + { + "bbox": (x1, y1, x2, y2), + "cx": int((x1 + x2) / 2), + "cy": int((y1 + y2) / 2), + "conf": float(box.conf[0]), + "class_name": class_name, + } + ) + + target_detections = [ + det for det in detections if det["class_name"] == self.target_class + ] + if not target_detections: + self.last_detection = None + else: + self.last_detection = max( + target_detections, + key=lambda det: det["conf"], + ) + + return detections + + def depth_candidates_from_bbox(self, bbox): + x1, y1, x2, y2 = bbox + h, w = self.depth_image.shape[:2] + x_ratios = [0.50, 0.35, 0.65, 0.25, 0.75] + y_ratios = [ + self.pick_depth_ratio, + 0.45, + 0.35, + 0.65, + 0.25, + 0.75, + ] + + points = [] + seen = set() + for yr in y_ratios: + for xr in x_ratios: + u = int(x1 + xr * (x2 - x1)) + v = int(y1 + yr * (y2 - y1)) + u = max(0, min(w - 1, u)) + v = max(0, min(h - 1, v)) + if (u, v) not in seen: + points.append((u, v)) + seen.add((u, v)) + return points + + def depth_patch_at(self, u, v): + h, w = self.depth_image.shape[:2] + r = self.depth_patch_radius + patch = self.depth_image[ + max(0, v - r) : min(h, v + r + 1), + max(0, u - r) : min(w, u + r + 1), + ] + valid = patch[patch > 0] + valid_ratio = valid.size / float(patch.size) + if valid.size == 0 or valid_ratio < self.min_depth_valid_ratio: + return None + + z_raw = float(np.median(valid)) + z_m = z_raw / 1000.0 if self.depth_image.dtype == np.uint16 else z_raw + if z_m < self.min_depth_m or z_m > self.max_depth_m: + return None + return u, v, z_m, valid_ratio + + def depth_from_bbox(self, bbox, log_reason=False): + log = self.get_logger() + if self.depth_image is None: + if log_reason: + log.warning("Depth image is not ready") + return None + + valid_samples = [] + for u, v in self.depth_candidates_from_bbox(bbox): + sample = self.depth_patch_at(u, v) + if sample is not None: + valid_samples.append(sample) + + if not valid_samples: + if log_reason: + log.warning( + "No valid depth found inside target bbox. " + "Try a larger depth_patch_radius or lower min_depth_valid_ratio." + ) + return None + + # Prefer the closest valid surface in the bbox. Transparent cups often + # expose background/table depth, so closest valid depth is usually safer. + u, v, z_m, valid_ratio = min(valid_samples, key=lambda sample: sample[2]) + if log_reason: + log.info( + f"Depth sample selected at ({u}, {v}): " + f"{z_m:.3f} m, valid_ratio={valid_ratio:.2f}" + ) + return u, v, z_m + + def pixel_to_camera(self, u, v, z_m): + fx = self.intrinsics["fx"] + fy = self.intrinsics["fy"] + cx = self.intrinsics["cx"] + cy = self.intrinsics["cy"] + + cam_x = (u - cx) * z_m / fx + cam_y = (v - cy) * z_m / fy + cam_z = z_m + return np.array([cam_x, cam_y, cam_z], dtype=float) + + def camera_to_base(self, camera_xyz): + coord = np.append(camera_xyz, 1.0) + base2ee = get_ee_matrix(self.robot) + base2cam = base2ee @ self.gripper2cam + return (base2cam @ coord)[:3] + + def side_unit_vector(self): + if self.side_grasp_axis == "x": + return np.array([self.side_grasp_direction, 0.0], dtype=float) + return np.array([0.0, self.side_grasp_direction], dtype=float) + + def side_grasp_orientation(self, side_vec): + if self.side_orientation_mode == "home": + return self.home_ori + + if self.side_orientation_mode == "euler": + return quat_dict_from_euler( + self.side_roll_deg, + self.side_pitch_deg, + self.side_yaw_deg, + ) + + # Make the tool's local +Z direction point horizontally into the cup. + # The local +Y axis is kept close to world +Z so the wrist is laid over + # the table instead of keeping the top-down grasp posture. + tool_z = np.array([-side_vec[0], -side_vec[1], 0.0], dtype=float) + tool_z_norm = np.linalg.norm(tool_z) + if tool_z_norm < 1e-6: + return self.home_ori + tool_z /= tool_z_norm + + world_up = np.array([0.0, 0.0, 1.0], dtype=float) + tool_x = np.cross(world_up, tool_z) + tool_x_norm = np.linalg.norm(tool_x) + if tool_x_norm < 1e-6: + return self.home_ori + tool_x /= tool_x_norm + tool_y = np.cross(tool_z, tool_x) + tool_y /= np.linalg.norm(tool_y) + + base_from_tool = np.column_stack((tool_x, tool_y, tool_z)) + if abs(self.side_tool_roll_deg) > 1e-6: + base_from_tool = ( + base_from_tool + @ Rotation.from_euler( + "z", + self.side_tool_roll_deg, + degrees=True, + ).as_matrix() + ) + return quat_dict_from_matrix(base_from_tool) + + def spin_for_camera_update(self, duration_sec): + end_time = time.time() + max(0.0, duration_sec) + while rclpy.ok() and time.time() < end_time: + rclpy.spin_once(self, timeout_sec=0.05) + + def select_redetect_target(self, detections): + if self.color_image is None: + return None + + candidates = [ + det for det in detections if det["class_name"] == self.target_class + ] + if not candidates: + return None + + h, w = self.color_image.shape[:2] + image_cx = w / 2.0 + image_cy = h / 2.0 + return min( + candidates, + key=lambda det: (det["cx"] - image_cx) ** 2 + + (det["cy"] - image_cy) ** 2, + ) + + def base_from_detection(self, detection, log_prefix): + depth_info = self.depth_from_bbox(detection["bbox"], log_reason=True) + if depth_info is None: + return None + + u, v, z_m = depth_info + camera_xyz = self.pixel_to_camera(u, v, z_m) + base_xyz = self.camera_to_base(camera_xyz) + self.get_logger().info( + f"{log_prefix} pixel=({u}, {v}), depth={z_m:.3f} m, " + f"camera=({camera_xyz[0]:.3f}, {camera_xyz[1]:.3f}, " + f"{camera_xyz[2]:.3f}) -> base=({base_xyz[0]:.3f}, " + f"{base_xyz[1]:.3f}, {base_xyz[2]:.3f})" + ) + return base_xyz + + def pick_and_place(self, base_xyz): + if self.grasp_mode == "side": + task_ok = self.pick_and_place_side(base_xyz) + else: + task_ok = self.pick_and_place_top(base_xyz) + + if task_ok and self.return_home_after_task: + self.get_logger().info("return home after task") + return self.move_home() + return task_ok + + def refine_target_from_current_view(self, log): + if not self.redetect_on_approach: + return None + + log.info("redetect target after approach") + self.spin_for_camera_update(self.redetect_settle_sec) + if self.color_image is None: + return None + + detections = self.detect_objects(self.color_image.copy()) + target = self.select_redetect_target(detections) + if target is None: + log.warning("redetect target not found; using initial target") + return None + return self.base_from_detection(target, "[redetect]") + + def pick_and_place_side(self, base_xyz): + log = self.get_logger() + bx, by, bz = [float(v) for v in base_xyz] + side_vec = self.side_unit_vector() + side_ori = self.side_grasp_orientation(side_vec) + stage_xy = ( + np.array([bx, by], dtype=float) + side_vec * self.side_staging_offset + ) + pre_xy = np.array([bx, by], dtype=float) + side_vec * self.side_approach_offset + grasp_xy = np.array([bx, by], dtype=float) + side_vec * self.side_grasp_offset + grasp_z = max(bz + self.side_grasp_z_offset, self.min_motion_z) + pre_z = grasp_z + lift_z = max(grasp_z + self.approach_offset, self.safe_z) + place_approach_z = max(self.place_z + self.approach_offset, self.safe_z) + + log.info( + f"Side grasp target base=({bx:.3f}, {by:.3f}, {bz:.3f}), " + f"axis={self.side_grasp_axis}, dir={self.side_grasp_direction:.0f}, " + f"ori_mode={self.side_orientation_mode}, " + f"tool_roll={self.side_tool_roll_deg:.1f}deg, " + f"stage=({stage_xy[0]:.3f}, {stage_xy[1]:.3f}, {pre_z:.3f}), " + f"pre=({pre_xy[0]:.3f}, {pre_xy[1]:.3f}, {pre_z:.3f}), " + f"grasp=({grasp_xy[0]:.3f}, {grasp_xy[1]:.3f}, {grasp_z:.3f})" + ) + + self.open_gripper_max(wait=False) + + steps = [ + ( + "move to outside side-staging pose", + make_pose(stage_xy[0], stage_xy[1], lift_z, side_ori), + ), + ( + "lower at outside side-staging pose", + make_pose(stage_xy[0], stage_xy[1], pre_z, side_ori), + ), + ( + "move horizontally to side pre-grasp", + make_pose(pre_xy[0], pre_xy[1], pre_z, side_ori), + ), + ] + for label, pose in steps: + log.info(label) + if not self.plan_and_execute(pose_goal=pose, params=self.pilz_params): + return False + + if not self.wait_until_gripper_idle(): + return False + + refined_base = self.refine_target_from_current_view(log) + if refined_base is not None: + bx, by, bz = [float(v) for v in refined_base] + pre_xy = np.array([bx, by], dtype=float) + side_vec * self.side_approach_offset + grasp_xy = np.array([bx, by], dtype=float) + side_vec * self.side_grasp_offset + grasp_z = max(bz + self.side_grasp_z_offset, self.min_motion_z) + pre_z = grasp_z + lift_z = max(grasp_z + self.approach_offset, self.safe_z) + log.info( + f"refined side grasp=({grasp_xy[0]:.3f}, {grasp_xy[1]:.3f}, " + f"{grasp_z:.3f})" + ) + if not self.plan_and_execute( + pose_goal=make_pose(pre_xy[0], pre_xy[1], pre_z, side_ori), + params=self.pilz_params, + ): + return False + + log.info("slide horizontally into cup side") + if not self.plan_and_execute( + pose_goal=make_pose(grasp_xy[0], grasp_xy[1], grasp_z, side_ori), + params=self.pilz_params, + ): + return False + + log.info("close gripper for side grasp") + self.gripper.move_gripper(GRIPPER_CLOSE_WIDTH, GRIPPER_FORCE) + time.sleep(1.0) + + move_steps = [ + ("lift cup", make_pose(grasp_xy[0], grasp_xy[1], lift_z, side_ori)), + ( + "move above syrup pump front", + make_pose(self.place_x, self.place_y, place_approach_z, side_ori), + ), + ( + "place cup", + make_pose(self.place_x, self.place_y, self.place_z, side_ori), + ), + ] + for label, pose in move_steps: + log.info(label) + if not self.plan_and_execute(pose_goal=pose, params=self.pilz_params): + return False + + log.info("open gripper") + self.open_gripper_max(wait=True) + + log.info("retract") + return self.plan_and_execute( + pose_goal=make_pose(self.place_x, self.place_y, place_approach_z, + side_ori), + params=self.pilz_params, + ) + + def pick_and_place_top(self, base_xyz): + log = self.get_logger() + bx, by, bz = [float(v) for v in base_xyz] + pick_z = bz + self.pick_z_offset + approach_z = max(pick_z + self.approach_offset, self.safe_z) + place_approach_z = max(self.place_z + self.approach_offset, self.safe_z) + + log.info( + f"Cup base point=({bx:.3f}, {by:.3f}, {bz:.3f}), " + f"pick_z={pick_z:.3f}" + ) + + self.open_gripper_max(wait=False) + + steps = [ + ("move above cup", make_pose(bx, by, approach_z, self.home_ori)), + ] + for label, pose in steps: + log.info(label) + if not self.plan_and_execute(pose_goal=pose, params=self.pilz_params): + return False + + if not self.wait_until_gripper_idle(): + return False + + refined_base = self.refine_target_from_current_view(log) + if refined_base is not None: + bx, by, bz = [float(v) for v in refined_base] + pick_z = bz + self.pick_z_offset + approach_z = max(pick_z + self.approach_offset, self.safe_z) + log.info( + f"refined cup base=({bx:.3f}, {by:.3f}, {bz:.3f}), " + f"pick_z={pick_z:.3f}" + ) + if not self.plan_and_execute( + pose_goal=make_pose(bx, by, approach_z, self.home_ori), + params=self.pilz_params, + ): + return False + + log.info("move down to cup") + if not self.plan_and_execute( + pose_goal=make_pose(bx, by, pick_z, self.home_ori), + params=self.pilz_params, + ): + return False + + log.info("close gripper") + self.gripper.move_gripper(GRIPPER_CLOSE_WIDTH, GRIPPER_FORCE) + time.sleep(1.0) + + move_steps = [ + ("lift cup", make_pose(bx, by, approach_z, self.home_ori)), + ( + "move above syrup pump front", + make_pose(self.place_x, self.place_y, place_approach_z, self.home_ori), + ), + ( + "place cup", + make_pose(self.place_x, self.place_y, self.place_z, self.home_ori), + ), + ] + for label, pose in move_steps: + log.info(label) + if not self.plan_and_execute(pose_goal=pose, params=self.pilz_params): + return False + + log.info("open gripper") + self.open_gripper_max(wait=True) + time.sleep(1.0) + + log.info("retract") + return self.plan_and_execute( + pose_goal=make_pose(self.place_x, self.place_y, place_approach_z, + self.home_ori), + params=self.pilz_params, + ) + + def start_pick_from_detection(self): + log = self.get_logger() + if self.picking: + log.warning("Already picking") + self.last_status = "already picking" + return + if self.color_image is None or self.depth_image is None or self.intrinsics is None: + log.warning("Waiting for color/depth/camera_info") + self.last_status = "waiting for color/depth/camera_info" + return + if self.last_detection is None: + log.warning(f"No {self.target_class} detection available") + self.last_status = f"no {self.target_class} detection" + return + + self.last_status = f"pick requested: {self.target_class}" + base_xyz = self.base_from_detection(self.last_detection, "[initial]") + if base_xyz is None: + log.error(f"No valid depth around {self.target_class} bbox") + self.last_status = f"no valid depth for {self.target_class}" + return + + self.picking = True + self.last_status = "moving robot" + try: + if self.pick_and_place(base_xyz): + self.has_picked_once = True + self.last_pick_time = time.time() + self.last_status = "pick finished" + else: + self.last_status = "pick failed" + finally: + self.picking = False + + def draw_detections(self, image, detections): + for detection in detections: + x1, y1, x2, y2 = detection["bbox"] + conf = detection["conf"] + class_name = detection.get("class_name", "") + + if class_name == self.target_class: + color = (0, 255, 0) + thickness = 2 + elif class_name == "lid": + color = (255, 0, 0) + thickness = 2 + else: + color = (180, 180, 180) + thickness = 1 + + cv2.rectangle(image, (x1, y1), (x2, y2), color, thickness) + + label = f"{class_name} {conf:.2f}" + if class_name == self.target_class: + depth_info = self.depth_from_bbox(detection["bbox"]) + if depth_info is not None: + u, v, z_m = depth_info + label += f" {z_m:.2f}m" + cv2.circle(image, (u, v), 5, (0, 0, 255), -1) + + cv2.putText( + image, + label, + (x1, max(20, y1 - 8)), + cv2.FONT_HERSHEY_SIMPLEX, + 0.6, + color, + 2, + cv2.LINE_AA, + ) + self.draw_hud(image, detections) + return image + + def draw_hud(self, image, detections): + counts = Counter(det["class_name"] for det in detections) + count_text = " ".join( + f"{name}:{counts[name]}" for name in sorted(counts) + ) or "none" + mode = "AUTO" if self.auto_pick else "MANUAL" + target_state = "ready" if self.last_detection is not None else "not found" + picked_state = "picked" if self.has_picked_once else "waiting" + + lines = [ + ( + f"[{mode}] {self.grasp_mode} target={self.target_class} " + f"conf>={self.conf:.2f} " + "p:pick a:auto r:reset ESC:quit" + ), + f"detections: {count_text} | target: {target_state} | {picked_state}", + f"status: {self.last_status}", + ] + color = (0, 255, 255) if self.auto_pick else (230, 230, 230) + for idx, text in enumerate(lines): + y = 26 + idx * 24 + cv2.putText( + image, + text, + (10, y), + cv2.FONT_HERSHEY_SIMPLEX, + 0.58, + color, + 2, + cv2.LINE_AA, + ) + + def run(self): + log = self.get_logger() + log.info("Move JOINT HOME") + if not self.move_joint_home(): + log.error("Joint home move failed") + return + + if self.move_to_camera_home: + log.info("Move HIGH CAMERA HOME") + if not self.move_camera_home(): + log.error("High camera home move failed") + return + + self.open_gripper_max(wait=True) + + window = "YOLO Cup Pick - p pick, a auto, r reset, esc quit" + cv2.namedWindow(window) + + while rclpy.ok(): + rclpy.spin_once(self, timeout_sec=0.01) + if self.color_image is None: + continue + + frame = self.color_image.copy() + detections = self.detect_objects(frame) + frame = self.draw_detections(frame, detections) + cv2.imshow(window, frame) + + now = time.time() + can_auto_pick = ( + self.auto_pick + and self.last_detection is not None + and not self.has_picked_once + and not self.picking + and (now - self.last_pick_time) >= self.auto_pick_interval + ) + if can_auto_pick: + self.start_pick_from_detection() + + key = cv2.waitKey(1) & 0xFF + if key == 27: + break + if key in (ord("p"), ord("P")): + log.info("pick key pressed") + self.start_pick_from_detection() + elif key in (ord("a"), ord("A")): + self.auto_pick = not self.auto_pick + self.last_pick_time = time.time() + self.last_status = f"auto_pick {'ON' if self.auto_pick else 'OFF'}" + log.info(f"auto_pick {'ON' if self.auto_pick else 'OFF'}") + elif key in (ord("r"), ord("R")): + self.has_picked_once = False + self.last_pick_time = 0.0 + self.last_status = "pick state reset" + log.info("pick state reset") + + cv2.destroyAllWindows() + + def destroy_node(self): + try: + self.gripper.close_connection() + finally: + super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + node = YoloCupPickNode() + try: + node.run() + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/azas_perception/config/T_gripper2camera.npy b/src/azas_perception/config/T_gripper2camera.npy new file mode 100644 index 0000000000000000000000000000000000000000..2a9474fc4635b9de2f2ce7ae3b2a2963d298f551 GIT binary patch literal 256 zcmbR27wQ`j$;eQ~P_3SlTAW;@Zl$1ZlV+i=qoAIaUsO_*m=~X4l#&V(cT3DEP6dh= zXCxM+0{I#yItnJ5ItsN4WCN~sdPnQ^UbxHmB!8Oyj>D6tnz~N1Uvitx;B2d% z!|&!N-Bxo__KU`TTsHCl`~5kcoR4|qTI|JEX!mov1UYOM+x-5w_aysMkGHK@?b>3$ aSnP3u*T47nR}_95_xyBpV89DL*aHBfCQbnW literal 0 HcmV?d00001 diff --git a/src/azas_perception/package.xml b/src/azas_perception/package.xml index b20e7c8..b18d82c 100644 --- a/src/azas_perception/package.xml +++ b/src/azas_perception/package.xml @@ -8,12 +8,17 @@ ament_python rclpy + ament_index_python + azas_gripper geometry_msgs + moveit_py + rcl_interfaces tf2_geometry_msgs tf2_ros sensor_msgs cv_bridge azas_interfaces + python3-scipy python3-pytest diff --git a/src/azas_perception/setup.py b/src/azas_perception/setup.py index f37ccbb..9856b9c 100644 --- a/src/azas_perception/setup.py +++ b/src/azas_perception/setup.py @@ -1,3 +1,4 @@ +from glob import glob from setuptools import find_packages, setup package_name = "azas_perception" @@ -9,6 +10,7 @@ data_files=[ ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]), (f"share/{package_name}", ["package.xml"]), + (f"share/{package_name}/config", glob("config/*.npy")), ], install_requires=["setuptools"], zip_safe=True, @@ -23,6 +25,7 @@ "cup_detection_pose_bridge_node = azas_perception.cup_detection_pose_bridge_node:main", "gpd_grasp_adapter_node = azas_perception.gpd_grasp_adapter_node:main", "simulated_cup_detection_node = azas_perception.simulated_cup_detection_node:main", + "yolo_cup_pick_legacy_node = azas_perception.yolo_cup_pick_legacy_node:main", ], }, ) From 93ba49ea624c52b02bfb534a6aa6f2b38e4fbbf8 Mon Sep 17 00:00:00 2001 From: shining-b-02 Date: Fri, 15 May 2026 16:17:09 +0900 Subject: [PATCH 7/7] Complete legacy dsr_practice integration --- .../azas_bringup/joint_state_relay_legacy.py | 41 ++ src/azas_bringup/config/moveit_py.yaml | 54 ++ .../launch/bar_sort_node_legacy.launch.py | 39 ++ .../launch/click_pick_node_legacy.launch.py | 42 ++ .../collision_obstacle_legacy.launch.py | 43 ++ .../launch/gear_assembly_legacy.launch.py | 42 ++ .../launch/mp_basic_legacy.launch.py | 43 ++ .../launch/mp_waypoint_legacy.launch.py | 43 ++ .../launch/mp_waypoint_pilz_legacy.launch.py | 43 ++ .../mp_waypoint_pilz_lin_legacy.launch.py | 43 ++ .../launch/pick_and_place_legacy.launch.py | 42 ++ .../realsense_data_collector_legacy.launch.py | 52 ++ .../stt_pick_and_place_legacy.launch.py | 54 ++ .../launch/stt_robot_control_legacy.launch.py | 54 ++ .../launch/syrup_pump_press_legacy.launch.py | 86 +++ .../yolo_cup_pick_node_legacy.launch.py | 274 +++++++++ src/azas_bringup/package.xml | 3 + src/azas_bringup/setup.py | 5 + .../calibration_test_legacy.py | 261 ++++++++ .../azas_calibration/data_recording_legacy.py | 72 +++ .../eye2hand_calibration_legacy.py | 287 +++++++++ .../handeye_calibration_legacy.py | 231 +++++++ .../azas_calibration/realsense_legacy.py | 41 ++ src/azas_calibration/package.xml | 4 + src/azas_calibration/setup.py | 4 + .../azas_motion/gear_assembly_legacy.py | 423 +++++++++++++ .../azas_motion/mp_waypoint_legacy.py | 189 ++++++ .../azas_motion/mp_waypoint_pilz_legacy.py | 239 ++++++++ .../mp_waypoint_pilz_lin_legacy.py | 228 +++++++ .../azas_motion/syrup_pump_press_legacy.py | 227 +++++++ src/azas_motion/setup.py | 5 + .../bar_detect_test_legacy_node.py | 116 ++++ .../azas_perception/bar_sort_legacy_node.py | 566 ++++++++++++++++++ .../azas_perception/click_pick_legacy_node.py | 408 +++++++++++++ .../realsense_data_collector_legacy_node.py | 213 +++++++ src/azas_perception/setup.py | 6 +- .../azas_voice/stt_pick_and_place_legacy.py | 387 ++++++++++++ .../azas_voice/stt_robot_control_legacy.py | 376 ++++++++++++ src/azas_voice/package.xml | 5 + src/azas_voice/setup.py | 6 +- 40 files changed, 5294 insertions(+), 3 deletions(-) create mode 100644 src/azas_bringup/azas_bringup/joint_state_relay_legacy.py create mode 100644 src/azas_bringup/config/moveit_py.yaml create mode 100644 src/azas_bringup/launch/bar_sort_node_legacy.launch.py create mode 100644 src/azas_bringup/launch/click_pick_node_legacy.launch.py create mode 100644 src/azas_bringup/launch/collision_obstacle_legacy.launch.py create mode 100644 src/azas_bringup/launch/gear_assembly_legacy.launch.py create mode 100644 src/azas_bringup/launch/mp_basic_legacy.launch.py create mode 100644 src/azas_bringup/launch/mp_waypoint_legacy.launch.py create mode 100644 src/azas_bringup/launch/mp_waypoint_pilz_legacy.launch.py create mode 100644 src/azas_bringup/launch/mp_waypoint_pilz_lin_legacy.launch.py create mode 100644 src/azas_bringup/launch/pick_and_place_legacy.launch.py create mode 100644 src/azas_bringup/launch/realsense_data_collector_legacy.launch.py create mode 100644 src/azas_bringup/launch/stt_pick_and_place_legacy.launch.py create mode 100644 src/azas_bringup/launch/stt_robot_control_legacy.launch.py create mode 100644 src/azas_bringup/launch/syrup_pump_press_legacy.launch.py create mode 100644 src/azas_bringup/launch/yolo_cup_pick_node_legacy.launch.py create mode 100644 src/azas_calibration/azas_calibration/calibration_test_legacy.py create mode 100644 src/azas_calibration/azas_calibration/data_recording_legacy.py create mode 100644 src/azas_calibration/azas_calibration/eye2hand_calibration_legacy.py create mode 100644 src/azas_calibration/azas_calibration/handeye_calibration_legacy.py create mode 100644 src/azas_calibration/azas_calibration/realsense_legacy.py create mode 100644 src/azas_motion/azas_motion/gear_assembly_legacy.py create mode 100644 src/azas_motion/azas_motion/mp_waypoint_legacy.py create mode 100644 src/azas_motion/azas_motion/mp_waypoint_pilz_legacy.py create mode 100644 src/azas_motion/azas_motion/mp_waypoint_pilz_lin_legacy.py create mode 100644 src/azas_motion/azas_motion/syrup_pump_press_legacy.py create mode 100644 src/azas_perception/azas_perception/bar_detect_test_legacy_node.py create mode 100644 src/azas_perception/azas_perception/bar_sort_legacy_node.py create mode 100644 src/azas_perception/azas_perception/click_pick_legacy_node.py create mode 100644 src/azas_perception/azas_perception/realsense_data_collector_legacy_node.py create mode 100644 src/azas_voice/azas_voice/stt_pick_and_place_legacy.py create mode 100644 src/azas_voice/azas_voice/stt_robot_control_legacy.py diff --git a/src/azas_bringup/azas_bringup/joint_state_relay_legacy.py b/src/azas_bringup/azas_bringup/joint_state_relay_legacy.py new file mode 100644 index 0000000..c727f8c --- /dev/null +++ b/src/azas_bringup/azas_bringup/joint_state_relay_legacy.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import JointState + + +class JointStateRelay(Node): + def __init__(self): + super().__init__("joint_state_relay") + self.declare_parameter("input_topic", "/dsr01/joint_states") + self.declare_parameter("output_topic", "/joint_states") + + input_topic = self.get_parameter("input_topic").value + output_topic = self.get_parameter("output_topic").value + + self.publisher = self.create_publisher(JointState, output_topic, 10) + self.subscription = self.create_subscription( + JointState, + input_topic, + self.callback, + 10, + ) + self.get_logger().info(f"Relaying {input_topic} -> {output_topic}") + + def callback(self, msg): + self.publisher.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = JointStateRelay() + try: + rclpy.spin(node) + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/azas_bringup/config/moveit_py.yaml b/src/azas_bringup/config/moveit_py.yaml new file mode 100644 index 0000000..0e89b41 --- /dev/null +++ b/src/azas_bringup/config/moveit_py.yaml @@ -0,0 +1,54 @@ +/**: + ros__parameters: + planning_scene_monitor_options: + name: "planning_scene_monitor" + robot_description: "robot_description" + joint_state_topic: "/joint_states" + attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor" + publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene" + monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene" + wait_for_initial_state_timeout: 10.0 + + planning_pipelines: + pipeline_names: ["ompl", "pilz_industrial_motion_planner", "chomp", "ompl_rrt_star"] + + plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + max_velocity_scaling_factor: 0.1 + max_acceleration_scaling_factor: 0.1 + + ompl_rrtc: + plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + planner_id: "RRTConnectkConfigDefault" + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + planning_time: 1.0 + + ompl_rrt_star: + plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl_rrt_star + planner_id: "RRTstarkConfigDefault" + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + planning_time: 1.5 + + pilz_lin: + plan_request_params: + planning_attempts: 1 + planning_pipeline: pilz_industrial_motion_planner + planner_id: "PTP" + max_velocity_scaling_factor: 0.1 + max_acceleration_scaling_factor: 0.1 + planning_time: 0.8 + + chomp: + plan_request_params: + planning_attempts: 1 + planning_pipeline: chomp + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + planning_time: 1.5 \ No newline at end of file diff --git a/src/azas_bringup/launch/bar_sort_node_legacy.launch.py b/src/azas_bringup/launch/bar_sort_node_legacy.launch.py new file mode 100644 index 0000000..7bf7d6e --- /dev/null +++ b/src/azas_bringup/launch/bar_sort_node_legacy.launch.py @@ -0,0 +1,39 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() + .robot_description_semantic(file_path="config/dsr.srdf") + .robot_description_kinematics() + .joint_limits() + .trajectory_execution() + .planning_scene_monitor() + .sensors_3d() + .to_moveit_configs() + ) + + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="azas_perception", + executable="bar_sort_legacy_node", + output="screen", + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) diff --git a/src/azas_bringup/launch/click_pick_node_legacy.launch.py b/src/azas_bringup/launch/click_pick_node_legacy.launch.py new file mode 100644 index 0000000..cd9aba7 --- /dev/null +++ b/src/azas_bringup/launch/click_pick_node_legacy.launch.py @@ -0,0 +1,42 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # Doosan M0609 MoveIt 기본 설정 (URDF, SRDF, kinematics, controllers 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() # m0609.urdf.xacro + .robot_description_semantic(file_path="config/dsr.srdf") # dsr.srdf + .robot_description_kinematics() # kinematics.yaml + .joint_limits() # joint_limits.yaml + .trajectory_execution() # moveit_controllers.yaml + .planning_scene_monitor() # sensors_3d.yaml 등과 연동 + .sensors_3d() + .to_moveit_configs() + ) + + # 🔹 MoveItPy 전용 YAML 추가 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="azas_perception", + executable="click_pick_legacy_node", + output="screen", + # MoveIt config + MoveItPy용 설정을 같이 넘김 + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) diff --git a/src/azas_bringup/launch/collision_obstacle_legacy.launch.py b/src/azas_bringup/launch/collision_obstacle_legacy.launch.py new file mode 100644 index 0000000..6c21fd2 --- /dev/null +++ b/src/azas_bringup/launch/collision_obstacle_legacy.launch.py @@ -0,0 +1,43 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # Doosan M0609 MoveIt 기본 설정 (URDF, SRDF, kinematics, controllers 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() # m0609.urdf.xacro + .robot_description_semantic() # dsr.srdf + .robot_description_kinematics() # kinematics.yaml + .joint_limits() # joint_limits.yaml + .trajectory_execution() # moveit_controllers.yaml + .planning_scene_monitor() # sensors_3d.yaml 등과 연동 + .sensors_3d() + .to_moveit_configs() + ) + + # 🔹 MoveItPy 전용 YAML 추가 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="azas_motion", + executable="collision_obstacle_legacy", + output="screen", + # MoveIt config + MoveItPy용 설정을 같이 넘김 + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) + diff --git a/src/azas_bringup/launch/gear_assembly_legacy.launch.py b/src/azas_bringup/launch/gear_assembly_legacy.launch.py new file mode 100644 index 0000000..90f3cea --- /dev/null +++ b/src/azas_bringup/launch/gear_assembly_legacy.launch.py @@ -0,0 +1,42 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # Doosan M0609 MoveIt 기본 설정 (URDF, SRDF, kinematics, controllers 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() # m0609.urdf.xacro + .robot_description_semantic() # dsr.srdf + .robot_description_kinematics() # kinematics.yaml + .joint_limits() # joint_limits.yaml + .trajectory_execution() # moveit_controllers.yaml + .planning_scene_monitor() # sensors_3d.yaml 등과 연동 + .sensors_3d() + .to_moveit_configs() + ) + + # 🔹 MoveItPy 전용 YAML 추가 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="azas_motion", + executable="gear_assembly_legacy", + output="screen", + # MoveIt config + MoveItPy용 설정을 같이 넘김 + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) diff --git a/src/azas_bringup/launch/mp_basic_legacy.launch.py b/src/azas_bringup/launch/mp_basic_legacy.launch.py new file mode 100644 index 0000000..f3f64a0 --- /dev/null +++ b/src/azas_bringup/launch/mp_basic_legacy.launch.py @@ -0,0 +1,43 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # Doosan M0609 MoveIt 기본 설정 (URDF, SRDF, kinematics, controllers 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() # m0609.urdf.xacro + .robot_description_semantic(file_path="config/dsr.srdf") # dsr.srdf + .robot_description_kinematics() # kinematics.yaml + .joint_limits() # joint_limits.yaml + .trajectory_execution() # moveit_controllers.yaml + .planning_scene_monitor() # sensors_3d.yaml 등과 연동 + .sensors_3d() + .to_moveit_configs() + ) + + # 🔹 MoveItPy 전용 YAML 추가 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="azas_motion", + executable="mp_basic_legacy", + output="screen", + # MoveIt config + MoveItPy용 설정을 같이 넘김 + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) + diff --git a/src/azas_bringup/launch/mp_waypoint_legacy.launch.py b/src/azas_bringup/launch/mp_waypoint_legacy.launch.py new file mode 100644 index 0000000..5dc5f69 --- /dev/null +++ b/src/azas_bringup/launch/mp_waypoint_legacy.launch.py @@ -0,0 +1,43 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # Doosan M0609 MoveIt 기본 설정 (URDF, SRDF, kinematics, controllers 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() # m0609.urdf.xacro + .robot_description_semantic() # dsr.srdf + .robot_description_kinematics() # kinematics.yaml + .joint_limits() # joint_limits.yaml + .trajectory_execution() # moveit_controllers.yaml + .planning_scene_monitor() # sensors_3d.yaml 등과 연동 + .sensors_3d() + .to_moveit_configs() + ) + + # 🔹 MoveItPy 전용 YAML 추가 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="azas_motion", + executable="mp_waypoint_legacy", + output="screen", + # MoveIt config + MoveItPy용 설정을 같이 넘김 + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) + diff --git a/src/azas_bringup/launch/mp_waypoint_pilz_legacy.launch.py b/src/azas_bringup/launch/mp_waypoint_pilz_legacy.launch.py new file mode 100644 index 0000000..ac890f8 --- /dev/null +++ b/src/azas_bringup/launch/mp_waypoint_pilz_legacy.launch.py @@ -0,0 +1,43 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # Doosan M0609 MoveIt 기본 설정 (URDF, SRDF, kinematics, controllers 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() # m0609.urdf.xacro + .robot_description_semantic() # dsr.srdf + .robot_description_kinematics() # kinematics.yaml + .joint_limits() # joint_limits.yaml + .trajectory_execution() # moveit_controllers.yaml + .planning_scene_monitor() # sensors_3d.yaml 등과 연동 + .sensors_3d() + .to_moveit_configs() + ) + + # 🔹 MoveItPy 전용 YAML 추가 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="azas_motion", + executable="mp_waypoint_pilz_legacy", + output="screen", + # MoveIt config + MoveItPy용 설정을 같이 넘김 + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) + diff --git a/src/azas_bringup/launch/mp_waypoint_pilz_lin_legacy.launch.py b/src/azas_bringup/launch/mp_waypoint_pilz_lin_legacy.launch.py new file mode 100644 index 0000000..58cea07 --- /dev/null +++ b/src/azas_bringup/launch/mp_waypoint_pilz_lin_legacy.launch.py @@ -0,0 +1,43 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # Doosan M0609 MoveIt 기본 설정 (URDF, SRDF, kinematics, controllers 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() # m0609.urdf.xacro + .robot_description_semantic() # dsr.srdf + .robot_description_kinematics() # kinematics.yaml + .joint_limits() # joint_limits.yaml + .trajectory_execution() # moveit_controllers.yaml + .planning_scene_monitor() # sensors_3d.yaml 등과 연동 + .sensors_3d() + .to_moveit_configs() + ) + + # 🔹 MoveItPy 전용 YAML 추가 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="azas_motion", + executable="mp_waypoint_pilz_lin_legacy", + output="screen", + # MoveIt config + MoveItPy용 설정을 같이 넘김 + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) + diff --git a/src/azas_bringup/launch/pick_and_place_legacy.launch.py b/src/azas_bringup/launch/pick_and_place_legacy.launch.py new file mode 100644 index 0000000..5e50e8e --- /dev/null +++ b/src/azas_bringup/launch/pick_and_place_legacy.launch.py @@ -0,0 +1,42 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # Doosan M0609 MoveIt 기본 설정 (URDF, SRDF, kinematics, controllers 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() # m0609.urdf.xacro + .robot_description_semantic() # dsr.srdf + .robot_description_kinematics() # kinematics.yaml + .joint_limits() # joint_limits.yaml + .trajectory_execution() # moveit_controllers.yaml + .planning_scene_monitor() # sensors_3d.yaml 등과 연동 + .sensors_3d() + .to_moveit_configs() + ) + + # 🔹 MoveItPy 전용 YAML 추가 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="azas_motion", + executable="pick_and_place_legacy", + output="screen", + # MoveIt config + MoveItPy용 설정을 같이 넘김 + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + ] + ) diff --git a/src/azas_bringup/launch/realsense_data_collector_legacy.launch.py b/src/azas_bringup/launch/realsense_data_collector_legacy.launch.py new file mode 100644 index 0000000..b01703d --- /dev/null +++ b/src/azas_bringup/launch/realsense_data_collector_legacy.launch.py @@ -0,0 +1,52 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + output_dir_arg = DeclareLaunchArgument( + "output_dir", + default_value="/home/ssu/ros2_ws/realsense_dataset/raw", + description="Directory where color, depth, and metadata files are saved.", + ) + save_interval_arg = DeclareLaunchArgument( + "save_interval_sec", + default_value="1.0", + description="Seconds between saved frames.", + ) + max_frames_arg = DeclareLaunchArgument( + "max_frames", + default_value="0", + description="Maximum number of frames to save. 0 means unlimited.", + ) + save_depth_arg = DeclareLaunchArgument( + "save_depth", + default_value="true", + description="Whether to save aligned depth images with RGB images.", + ) + + collector_node = Node( + package="azas_perception", + executable="realsense_data_collector_legacy_node", + name="realsense_data_collector", + output="screen", + parameters=[ + { + "output_dir": LaunchConfiguration("output_dir"), + "save_interval_sec": LaunchConfiguration("save_interval_sec"), + "max_frames": LaunchConfiguration("max_frames"), + "save_depth": LaunchConfiguration("save_depth"), + } + ], + ) + + return LaunchDescription( + [ + output_dir_arg, + save_interval_arg, + max_frames_arg, + save_depth_arg, + collector_node, + ] + ) diff --git a/src/azas_bringup/launch/stt_pick_and_place_legacy.launch.py b/src/azas_bringup/launch/stt_pick_and_place_legacy.launch.py new file mode 100644 index 0000000..50fa67e --- /dev/null +++ b/src/azas_bringup/launch/stt_pick_and_place_legacy.launch.py @@ -0,0 +1,54 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() + .robot_description_semantic(file_path="config/dsr.srdf") + .robot_description_kinematics() + .joint_limits() + .trajectory_execution() + .planning_scene_monitor() + .sensors_3d() + .to_moveit_configs() + ) + + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "config", "moveit_py.yaml"] + ) + + pick_place_node = Node( + package="azas_voice", + executable="stt_pick_and_place_legacy", + output="screen", + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + {"use_tts": True}, + ], + ) + + stt_node = Node( + package="azas_voice", + executable="stt_node", + output="screen", + parameters=[ + {"language": "ko-KR"}, + {"device_index": -1}, + {"energy_threshold": 300.0}, + {"pause_threshold": 0.8}, + {"phrase_time_limit": 5.0}, + {"dynamic_energy": True}, + {"ambient_duration": 1.0}, + ], + ) + + return LaunchDescription([pick_place_node, stt_node]) diff --git a/src/azas_bringup/launch/stt_robot_control_legacy.launch.py b/src/azas_bringup/launch/stt_robot_control_legacy.launch.py new file mode 100644 index 0000000..950b35f --- /dev/null +++ b/src/azas_bringup/launch/stt_robot_control_legacy.launch.py @@ -0,0 +1,54 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() + .robot_description_semantic(file_path="config/dsr.srdf") + .robot_description_kinematics() + .joint_limits() + .trajectory_execution() + .planning_scene_monitor() + .sensors_3d() + .to_moveit_configs() + ) + + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "config", "moveit_py.yaml"] + ) + + robot_control_node = Node( + package="azas_voice", + executable="stt_robot_control_legacy", + output="screen", + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + {"use_tts": True}, + ], + ) + + stt_node = Node( + package="azas_voice", + executable="stt_node", + output="screen", + parameters=[ + {"language": "ko-KR"}, + {"device_index": -1}, + {"energy_threshold": 300.0}, + {"pause_threshold": 0.8}, + {"phrase_time_limit": 5.0}, + {"dynamic_energy": True}, + {"ambient_duration": 1.0}, + ], + ) + + return LaunchDescription([robot_control_node, stt_node]) diff --git a/src/azas_bringup/launch/syrup_pump_press_legacy.launch.py b/src/azas_bringup/launch/syrup_pump_press_legacy.launch.py new file mode 100644 index 0000000..f82c5c5 --- /dev/null +++ b/src/azas_bringup/launch/syrup_pump_press_legacy.launch.py @@ -0,0 +1,86 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description(file_path="config/m0609.urdf.xacro") + .robot_description_semantic(file_path="config/dsr.srdf") + .robot_description_kinematics() + .joint_limits() + .trajectory_execution() + .planning_scene_monitor() + .sensors_3d() + .to_moveit_configs() + ) + + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "config", "moveit_py.yaml"] + ) + + pump_x_arg = DeclareLaunchArgument( + "pump_x", + default_value="0.45", + description="Syrup pump x position in base_link frame [m].", + ) + pump_y_arg = DeclareLaunchArgument( + "pump_y", + default_value="0.0", + description="Syrup pump y position in base_link frame [m].", + ) + start_z_arg = DeclareLaunchArgument( + "start_z", + default_value="0.50", + description="Vertical approach start height [m].", + ) + pump_top_z_arg = DeclareLaunchArgument( + "pump_top_z", + default_value="0.35", + description="Syrup pump top height [m].", + ) + press_depth_arg = DeclareLaunchArgument( + "press_depth", + default_value="0.05", + description="Press depth from pump top [m].", + ) + hold_sec_arg = DeclareLaunchArgument( + "hold_sec", + default_value="0.5", + description="Holding time at pressed position [sec].", + ) + + return LaunchDescription( + [ + pump_x_arg, + pump_y_arg, + start_z_arg, + pump_top_z_arg, + press_depth_arg, + hold_sec_arg, + Node( + package="azas_motion", + executable="syrup_pump_press_legacy", + output="screen", + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + { + "pump_x": LaunchConfiguration("pump_x"), + "pump_y": LaunchConfiguration("pump_y"), + "start_z": LaunchConfiguration("start_z"), + "pump_top_z": LaunchConfiguration("pump_top_z"), + "press_depth": LaunchConfiguration("press_depth"), + "hold_sec": LaunchConfiguration("hold_sec"), + }, + ], + ), + ] + ) diff --git a/src/azas_bringup/launch/yolo_cup_pick_node_legacy.launch.py b/src/azas_bringup/launch/yolo_cup_pick_node_legacy.launch.py new file mode 100644 index 0000000..b0025fd --- /dev/null +++ b/src/azas_bringup/launch/yolo_cup_pick_node_legacy.launch.py @@ -0,0 +1,274 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description(file_path="config/m0609.urdf.xacro") + .robot_description_semantic(file_path="config/dsr.srdf") + .robot_description_kinematics() + .joint_limits() + .trajectory_execution() + .planning_scene_monitor() + .sensors_3d() + .to_moveit_configs() + ) + + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "config", "moveit_py.yaml"] + ) + + model_path_arg = DeclareLaunchArgument( + "model_path", + default_value="/home/ssu/ros2_ws/yolo_runs/cup_yolov8n_ft1/weights/best.pt", + description="Path to trained cup YOLO weights.", + ) + conf_arg = DeclareLaunchArgument("conf", default_value="0.35") + imgsz_arg = DeclareLaunchArgument("imgsz", default_value="640") + device_arg = DeclareLaunchArgument("device", default_value="cpu") + target_class_arg = DeclareLaunchArgument("target_class", default_value="cup") + auto_pick_interval_arg = DeclareLaunchArgument( + "auto_pick_interval", default_value="3.0" + ) + pick_depth_ratio_arg = DeclareLaunchArgument( + "pick_depth_ratio", default_value="0.55" + ) + depth_patch_radius_arg = DeclareLaunchArgument( + "depth_patch_radius", default_value="7" + ) + min_depth_valid_ratio_arg = DeclareLaunchArgument( + "min_depth_valid_ratio", default_value="0.03" + ) + min_depth_m_arg = DeclareLaunchArgument("min_depth_m", default_value="0.15") + max_depth_m_arg = DeclareLaunchArgument("max_depth_m", default_value="1.20") + redetect_on_approach_arg = DeclareLaunchArgument( + "redetect_on_approach", default_value="true" + ) + redetect_settle_sec_arg = DeclareLaunchArgument( + "redetect_settle_sec", default_value="0.5" + ) + grasp_mode_arg = DeclareLaunchArgument("grasp_mode", default_value="side") + side_grasp_axis_arg = DeclareLaunchArgument( + "side_grasp_axis", default_value="y_axis" + ) + side_grasp_direction_arg = DeclareLaunchArgument( + "side_grasp_direction", default_value="-1.0" + ) + side_approach_offset_arg = DeclareLaunchArgument( + "side_approach_offset", default_value="0.12" + ) + side_staging_offset_arg = DeclareLaunchArgument( + "side_staging_offset", + default_value="0.24", + description="Far outside offset where the wrist first turns horizontal.", + ) + side_grasp_offset_arg = DeclareLaunchArgument( + "side_grasp_offset", default_value="0.035" + ) + side_grasp_z_offset_arg = DeclareLaunchArgument( + "side_grasp_z_offset", + default_value="0.05", + description="Side grasp height offset from detected base point.", + ) + side_orientation_mode_arg = DeclareLaunchArgument( + "side_orientation_mode", + default_value="approach", + description="Side grasp orientation: approach, euler, or home.", + ) + side_tool_roll_deg_arg = DeclareLaunchArgument( + "side_tool_roll_deg", + default_value="0.0", + description="Twist around the horizontal approach direction for RG2 finger alignment.", + ) + side_roll_deg_arg = DeclareLaunchArgument( + "side_roll_deg", + default_value="0.0", + description="Manual side grasp roll, used when side_orientation_mode:=euler.", + ) + side_pitch_deg_arg = DeclareLaunchArgument( + "side_pitch_deg", + default_value="90.0", + description="Manual side grasp pitch, used when side_orientation_mode:=euler.", + ) + side_yaw_deg_arg = DeclareLaunchArgument( + "side_yaw_deg", + default_value="0.0", + description="Manual side grasp yaw, used when side_orientation_mode:=euler.", + ) + verify_motion_arg = DeclareLaunchArgument("verify_motion", default_value="true") + motion_verify_tolerance_arg = DeclareLaunchArgument( + "motion_verify_tolerance", default_value="0.01" + ) + move_to_camera_home_arg = DeclareLaunchArgument( + "move_to_camera_home", default_value="true" + ) + camera_home_x_arg = DeclareLaunchArgument("camera_home_x", default_value="0.45") + camera_home_y_arg = DeclareLaunchArgument("camera_home_y", default_value="0.0") + camera_home_z_arg = DeclareLaunchArgument("camera_home_z", default_value="0.62") + min_motion_z_arg = DeclareLaunchArgument( + "min_motion_z", + default_value="0.12", + description="Minimum allowed commanded Z in base frame.", + ) + return_home_after_task_arg = DeclareLaunchArgument( + "return_home_after_task", default_value="true" + ) + place_x_arg = DeclareLaunchArgument("place_x", default_value="0.45") + place_y_arg = DeclareLaunchArgument("place_y", default_value="0.0") + place_z_arg = DeclareLaunchArgument("place_z", default_value="0.30") + auto_pick_arg = DeclareLaunchArgument("auto_pick", default_value="false") + + return LaunchDescription( + [ + model_path_arg, + conf_arg, + imgsz_arg, + device_arg, + target_class_arg, + auto_pick_interval_arg, + pick_depth_ratio_arg, + depth_patch_radius_arg, + min_depth_valid_ratio_arg, + min_depth_m_arg, + max_depth_m_arg, + redetect_on_approach_arg, + redetect_settle_sec_arg, + grasp_mode_arg, + side_grasp_axis_arg, + side_grasp_direction_arg, + side_approach_offset_arg, + side_staging_offset_arg, + side_grasp_offset_arg, + side_grasp_z_offset_arg, + side_orientation_mode_arg, + side_tool_roll_deg_arg, + side_roll_deg_arg, + side_pitch_deg_arg, + side_yaw_deg_arg, + verify_motion_arg, + motion_verify_tolerance_arg, + move_to_camera_home_arg, + camera_home_x_arg, + camera_home_y_arg, + camera_home_z_arg, + min_motion_z_arg, + return_home_after_task_arg, + place_x_arg, + place_y_arg, + place_z_arg, + auto_pick_arg, + Node( + package="azas_bringup", + executable="joint_state_relay_legacy", + name="joint_state_relay", + output="screen", + parameters=[ + { + "input_topic": "/dsr01/joint_states", + "output_topic": "/joint_states", + } + ], + ), + Node( + package="azas_perception", + executable="yolo_cup_pick_legacy_node", + output="screen", + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + { + "model_path": ParameterValue( + LaunchConfiguration("model_path"), + value_type=str, + ), + "conf": LaunchConfiguration("conf"), + "imgsz": LaunchConfiguration("imgsz"), + "device": ParameterValue( + LaunchConfiguration("device"), + value_type=str, + ), + "target_class": ParameterValue( + LaunchConfiguration("target_class"), + value_type=str, + ), + "auto_pick_interval": LaunchConfiguration( + "auto_pick_interval" + ), + "pick_depth_ratio": LaunchConfiguration("pick_depth_ratio"), + "depth_patch_radius": LaunchConfiguration( + "depth_patch_radius" + ), + "min_depth_valid_ratio": LaunchConfiguration( + "min_depth_valid_ratio" + ), + "min_depth_m": LaunchConfiguration("min_depth_m"), + "max_depth_m": LaunchConfiguration("max_depth_m"), + "redetect_on_approach": LaunchConfiguration( + "redetect_on_approach" + ), + "redetect_settle_sec": LaunchConfiguration( + "redetect_settle_sec" + ), + "grasp_mode": ParameterValue( + LaunchConfiguration("grasp_mode"), + value_type=str, + ), + "side_grasp_axis": ParameterValue( + LaunchConfiguration("side_grasp_axis"), + value_type=str, + ), + "side_grasp_direction": LaunchConfiguration( + "side_grasp_direction" + ), + "side_approach_offset": LaunchConfiguration( + "side_approach_offset" + ), + "side_staging_offset": LaunchConfiguration( + "side_staging_offset" + ), + "side_grasp_offset": LaunchConfiguration("side_grasp_offset"), + "side_grasp_z_offset": LaunchConfiguration( + "side_grasp_z_offset" + ), + "side_orientation_mode": ParameterValue( + LaunchConfiguration("side_orientation_mode"), + value_type=str, + ), + "side_tool_roll_deg": LaunchConfiguration( + "side_tool_roll_deg" + ), + "side_roll_deg": LaunchConfiguration("side_roll_deg"), + "side_pitch_deg": LaunchConfiguration("side_pitch_deg"), + "side_yaw_deg": LaunchConfiguration("side_yaw_deg"), + "verify_motion": LaunchConfiguration("verify_motion"), + "motion_verify_tolerance": LaunchConfiguration( + "motion_verify_tolerance" + ), + "move_to_camera_home": LaunchConfiguration( + "move_to_camera_home" + ), + "camera_home_x": LaunchConfiguration("camera_home_x"), + "camera_home_y": LaunchConfiguration("camera_home_y"), + "camera_home_z": LaunchConfiguration("camera_home_z"), + "min_motion_z": LaunchConfiguration("min_motion_z"), + "return_home_after_task": LaunchConfiguration( + "return_home_after_task" + ), + "place_x": LaunchConfiguration("place_x"), + "place_y": LaunchConfiguration("place_y"), + "place_z": LaunchConfiguration("place_z"), + "auto_pick": LaunchConfiguration("auto_pick"), + }, + ], + ), + ] + ) diff --git a/src/azas_bringup/package.xml b/src/azas_bringup/package.xml index d8153a3..b93f93a 100644 --- a/src/azas_bringup/package.xml +++ b/src/azas_bringup/package.xml @@ -13,7 +13,10 @@ azas_gripper azas_motion azas_perception + sensor_msgs dsr_description2 + dsr_moveit_config_m0609 + moveit_configs_utils robot_state_publisher rviz2 xacro diff --git a/src/azas_bringup/setup.py b/src/azas_bringup/setup.py index fafee50..3df40a3 100644 --- a/src/azas_bringup/setup.py +++ b/src/azas_bringup/setup.py @@ -20,4 +20,9 @@ maintainer_email="team@example.com", description="Launch and configuration package for Azas.", license="MIT", + entry_points={ + "console_scripts": [ + "joint_state_relay_legacy = azas_bringup.joint_state_relay_legacy:main", + ], + }, ) diff --git a/src/azas_calibration/azas_calibration/calibration_test_legacy.py b/src/azas_calibration/azas_calibration/calibration_test_legacy.py new file mode 100644 index 0000000..e256ee6 --- /dev/null +++ b/src/azas_calibration/azas_calibration/calibration_test_legacy.py @@ -0,0 +1,261 @@ +import cv2 +import rclpy +import time +import numpy as np +import threading +from scipy.spatial.transform import Rotation + +from azas_calibration.realsense_legacy import ImgNode +from azas_gripper.onrobot import RG +import DR_init + +# ====================== +# 로봇 / 그리퍼 설정 +# ====================== +ROBOT_ID = "dsr01" +ROBOT_MODEL = "m0609" +VELOCITY, ACC = 60, 60 + +DR_init.__dsr__id = ROBOT_ID +DR_init.__dsr__model = ROBOT_MODEL + +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 # 정수 + +# ====================== +# Z 관련 파라미터 +# ====================== +Z_OFFSET = 200.0 # 클릭한 지점 z에 더해 줄 오프셋 (mm) +SAFE_Z = 400.0 # 집은 뒤 올라갈 안전 높이 (mm) – 환경 보고 조정 + + +class TestNode: + def __init__(self): + # RealSense 노드 + self.img_node = ImgNode() + + # Intrinsic 수신될 때까지 대기 + while rclpy.ok() and self.img_node.get_camera_intrinsic() is None: + rclpy.spin_once(self.img_node, timeout_sec=0.1) + + self.intrinsics = self.img_node.get_camera_intrinsic() + + # Hand-eye 결과 (그리퍼 → 카메라) + self.gripper2cam = np.load("T_gripper2camera.npy") + + # 그리퍼 + self.gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + + # 준비자세(Joint) + self.JReady = posj([0, 0, 90, 0, 90, 90]) + + # 홈 XY (초기 자세에서 저장) + self.home_pose = None # [x, y, z, rx, ry, rz] + + # ========================= + # 카메라 → 베이스 좌표 변환 + # ========================= + def transform_to_base(self, camera_coords): + """ + camera_coords: (Xc, Yc, Zc) in mm (카메라 기준) + 반환: (Xb, Yb, Zb) in mm (base_link 기준) + """ + coord = np.append(np.array(camera_coords, dtype=float), 1.0) # [x,y,z,1] + + # 현재 베이스→그리퍼 + base2gripper = self.get_robot_pose_matrix(*get_current_posx()[0]) + + # 베이스→카메라 = 베이스→그리퍼 · 그리퍼→카메라 + base2cam = base2gripper @ self.gripper2cam + + td_coord = base2cam @ coord + + return td_coord[:3] + + def get_robot_pose_matrix(self, x, y, z, rx, ry, rz): + R = Rotation.from_euler("ZYZ", [rx, ry, rz], degrees=True).as_matrix() + T = np.eye(4) + T[:3, :3] = R + T[:3, 3] = [x, y, z] + return T + + # ========================= + # Pick 시퀀스 + # ========================= + def pick_and_place(self, x, y, z): + + print("\n========== PICK SEQUENCE ==========") + print(f"Base coord raw : x={x:.2f}, y={y:.2f}, z={z:.2f}") + print(f"Z_OFFSET : {Z_OFFSET}") + print(f"SAFE_Z : {SAFE_Z}") + print("===================================\n") + + # 현재 포즈 + cur = get_current_posx()[0] + cur_x, cur_y, cur_z, rx, ry, rz = cur + + # home pose (run()에서 설정) + if self.home_pose is None: + self.home_pose = cur + home_x, home_y, home_z, hrx, hry, hrz = self.home_pose + + # 0) 안전용으로 한 번 더 open (혹시 이전에 안 열려 있으면) + self.gripper.open_gripper() + time.sleep(0.5) + + # 1) 클릭한 위치 x,y로 이동 (z는 현재 값 유지) + target_xy = posx([x, y, cur_z, rx, ry, rz]) + print("[1] Move to XY only:", target_xy) + movel(target_xy, VELOCITY, ACC) + wait(0.5) + + # 2) z_correct 계산 및 이동 + z_correct = z + Z_OFFSET + target_xyz = posx([x, y, z_correct, rx, ry, rz]) + print(f"[2] Move down to z_correct={z_correct:.2f}:", target_xyz) + movel(target_xyz, VELOCITY, ACC) + wait(0.3) + + # 3) gripper close + print("[3] Gripper Close") + self.gripper.close_gripper() + time.sleep(1.0) + + # 4) z 상승: SAFE_Z까지 (x,y는 그대로) + up_pose = posx([x, y, SAFE_Z, rx, ry, rz]) + print(f"[4] Move up to SAFE_Z={SAFE_Z:.2f}:", up_pose) + movel(up_pose, VELOCITY, ACC) + wait(0.5) + + # 5) home xy 이동 (z는 SAFE_Z 유지) + home_xy_pose = posx([home_x, home_y, SAFE_Z, hrx, hry, hrz]) + print("[5] Move to home XY:", home_xy_pose) + movel(home_xy_pose, VELOCITY, ACC) + wait(0.5) + + # 6) place 높이 = pick 높이 이상으로 보장 + z_final = max(250.0, z_correct) + home_xy_z280 = posx([home_x, home_y, z_final, hrx, hry, hrz]) + print(f"[6] Move to home XY, z={z_final}:", home_xy_z280) + movel(home_xy_z280, VELOCITY, ACC) + wait(0.3) + + # 7) gripper open + print("[7] Gripper Open") + self.gripper.open_gripper() + time.sleep(1.0) + + # 8) 다시 SAFE_Z로 올려두고 끝낼지 여부 (원하면 유지) + back_up = posx([home_x, home_y, SAFE_Z, hrx, hry, hrz]) + print("[8] Back up to SAFE_Z:", back_up) + movel(back_up, VELOCITY, ACC) + wait(0.5) + + print("========== PICK END ==========\n") + + # ========================= + # 마우스 콜백 + # ========================= + def mouse_callback(self, event, x, y, flags, param): + if event == cv2.EVENT_LBUTTONDOWN and not hasattr(self, '_pick_thread_running'): + depth_frame = self.img_node.get_depth_frame() + if depth_frame is None: + print("No depth frame") + return + + # 픽셀 범위 체크 + h, w = depth_frame.shape + if not (0 <= x < w and 0 <= y < h): + print("Click out of range") + return + + z = depth_frame[y, x] + if z == 0: + print("Depth invalid at clicked point") + return + + # 카메라 좌표 (mm) 계산 + fx = self.intrinsics["fx"] + fy = self.intrinsics["fy"] + ppx = self.intrinsics["ppx"] + ppy = self.intrinsics["ppy"] + + X = (x - ppx) * z / fx + Y = (y - ppy) * z / fy + Z = z + + cam_coord = (X, Y, Z) + base_coord = self.transform_to_base(cam_coord) + + print("Camera:", cam_coord) + print("Base :", base_coord) + + def run_pick(): + self._pick_thread_running = True + self.pick_and_place(*base_coord) + del self._pick_thread_running + + threading.Thread(target=run_pick, daemon=True).start() + + # ========================= + # 메인 루프 + # ========================= + def run(self): + cv2.namedWindow("Webcam") + cv2.setMouseCallback("Webcam", self.mouse_callback) + + # rclpy spin을 별도 스레드로 분리 (pick 스레드와 충돌 방지) + executor = rclpy.executors.MultiThreadedExecutor() + executor.add_node(self.img_node) + spin_thread = threading.Thread(target=executor.spin, daemon=True) + spin_thread.start() + + # 초기 자세로 이동 + print("[Init] movej JReady") + movej(self.JReady, VELOCITY, ACC) + wait(1.0) + + # 현재 자세를 home_pose로 저장 + self.home_pose = get_current_posx()[0] + + # 초기 gripper open + print("[Init] Gripper Open") + self.gripper.open_gripper() + time.sleep(1.0) + + while True: + img = self.img_node.get_color_frame() + if img is None: + time.sleep(0.01) + continue + + cv2.imshow("Webcam", img) + + if cv2.waitKey(1) & 0xFF == 27: # ESC + break + + executor.shutdown() + cv2.destroyAllWindows() + + +def main(args=None): + rclpy.init() + node = rclpy.create_node("dsr_example_demo_py", namespace=ROBOT_ID) + DR_init.__dsr__node = node + + try: + from DSR_ROBOT2 import get_current_posx, movej, movel, wait + from DR_common2 import posx, posj + except ImportError as e: + print(f"Error importing DSR_ROBOT2 : {e}") + rclpy.shutdown() + raise SystemExit(1) + + test = TestNode() + test.run() + + rclpy.shutdown() + +if __name__ == "__main__": + main() diff --git a/src/azas_calibration/azas_calibration/data_recording_legacy.py b/src/azas_calibration/azas_calibration/data_recording_legacy.py new file mode 100644 index 0000000..ccbfcc3 --- /dev/null +++ b/src/azas_calibration/azas_calibration/data_recording_legacy.py @@ -0,0 +1,72 @@ +import os +import cv2 +import json +import rclpy +import DR_init + +# 로봇 설정 +ROBOT_ID = "dsr01" +ROBOT_MODEL = "m0609" +VELOCITY, ACC = 60, 60 +DEVICE_NUMBER = 4 + +DR_init.dsr__id = ROBOT_ID +DR_init.__dsr__model = ROBOT_MODEL + + +def main(args=None): + rclpy.init(args=args) + node = rclpy.create_node("dsr_example_demo_py", namespace=ROBOT_ID) + DR_init.__dsr__node = node + # 로봇 제어 모듈 가져오기 + try: + from DSR_ROBOT2 import ( + get_current_posx, + set_tool, + set_tcp, + ) + except ImportError as e: + print(f"Error importing DSR_ROBOT2 : {e}") + return + # 공구 및 TCP 설정 + set_tool("Tool Weight_2FG") + set_tcp("2FG_TCP") + + # 데이터 저장 경로 설정 + source_path = "./data" + os.makedirs(source_path, exist_ok=True) + # 카메라 연결 + print(f"현재 선택된 device number는 {DEVICE_NUMBER}입니다.") + cap = cv2.VideoCapture(DEVICE_NUMBER) # 4 is camera number, set your camera number + + write_data = {} + write_data["poses"] = [] + write_data["file_name"] = [] + + while True: + ret, frame = cap.read() + + if not ret: + print("카메라를 찾을 수 없습니다. DEVICE_NUMBER를 변경해주세요.") + exit(True) + cv2.imshow("camera", frame) + + if cv2.waitKey(1) & 0xFF == ord("q"): + pos = get_current_posx()[0] + file_name = f"{pos[0]}_{pos[1]}_{pos[2]}.jpg" + # 현재 위치 기반 이미지 저장 + cv2.imwrite(f"{source_path}/{file_name}", frame) + print("current position1 : ", pos) + write_data["file_name"].append(file_name) + write_data["poses"].append(pos) + print(f"save img to {source_path}/{file_name}") + with open(f"{source_path}/calibrate_data.json", "w") as json_file: + json.dump(write_data, json_file, indent=4) + + cap.release() + cv2.destroyAllWindows() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/azas_calibration/azas_calibration/eye2hand_calibration_legacy.py b/src/azas_calibration/azas_calibration/eye2hand_calibration_legacy.py new file mode 100644 index 0000000..7b0563f --- /dev/null +++ b/src/azas_calibration/azas_calibration/eye2hand_calibration_legacy.py @@ -0,0 +1,287 @@ +import json +from scipy.spatial.transform import Rotation +import numpy as np +import cv2 + +# 1) 로봇 그리퍼의 절대 좌표 (x, y, z, rx, ry, rz)를 행렬로 변환하는 함수 +def get_robot_pose_matrix(x, y, z, rx, ry, rz): + """ + 베이스->그리퍼 변환행렬 (4x4)을 반환. + """ + R = Rotation.from_euler('ZYZ', [rx, ry, rz], degrees=True).as_matrix() + T = np.eye(4) + T[:3, :3] = R + T[:3, 3] = [x, y, z] + return T + +# 2) 체커보드 코너 검출 (카메라→체커보드 변환 구하기) +def find_checkerboard_pose( + image, board_size, square_size, camera_matrix, dist_coeffs +): + """ + checkerboard_size = (7, 5) # 내부 코너 개수 + square_size = 25.0 # mm 단위 + 이미지에서 체커보드를 찾고, solvePnP로 카메라→체커보드 변환(R, t)을 구함. + 반환값: (R_camera2checker, t_camera2checker) + """ + objp = np.zeros((board_size[0] * board_size[1], 3), np.float32) + # 예: x 방향으로 square_size씩 증가, y 방향으로 square_size씩 증가 + objp[:, :2] = ( + np.mgrid[0 : board_size[0], 0 : board_size[1]].T.reshape(-1, 2) * 25 + ) + + gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) + found, corners = cv2.findChessboardCorners( + gray, + board_size, + flags=cv2.CALIB_CB_ADAPTIVE_THRESH + + cv2.CALIB_CB_FAST_CHECK + + cv2.CALIB_CB_NORMALIZE_IMAGE, + ) + if not found: + return None, None + + # 코너 좌표를 더 정확히 + corners_sub = cv2.cornerSubPix( + gray, + corners, + (11, 11), + (-1, -1), + criteria=(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001), + ) + + # solvePnP + retval, rvec, tvec = cv2.solvePnP(objp, corners_sub, camera_matrix, dist_coeffs) + if not retval: + return None, None + + # 회전벡터 -> 회전행렬 + R, _ = cv2.Rodrigues(rvec) + + return R, tvec + +# 체커보드 이미지를 이용한 카메라 보정 +def calibrate_camera_from_chessboard( + image_folder_path, + board_size, # (7, 5)처럼 내부 코너 개수 + square_size, # mm 단위 +): + """ + 지정된 폴더 안의 체커보드 이미지를 읽고, 카메라 행렬(camera_matrix)와 왜곡 계수(dist_coeffs)를 추정한다. + board_size: 체커보드 내부 코너 수 (cols, rows) + square_size: 체커보드 한 칸 크기 (mm) + """ + # 3D 세계 좌표계에 대한 좌표 생성 (z=0 평면 상에 체커보드) + objp = np.zeros((board_size[0] * board_size[1], 3), np.float32) + # 예: x 방향으로 square_size씩 증가, y 방향으로 square_size씩 증가 + objp[:, :2] = ( + np.mgrid[0 : board_size[0], 0 : board_size[1]].T.reshape(-1, 2) * square_size + ) + + # 모든 이미지에 대해 3D / 2D 포인트 누적 + obj_points = [] # 3D world points + img_points = [] # 2D image points + image_shape = None + + # 폴더 내에 있는 이미지 파일 읽기 + image_paths = image_folder_path # JPG, PNG 등 확장자 맞춰서 + # 필요하면 jpg 등 다른 확장자도 처리 가능 + + for fname in image_paths: + img = cv2.imread(fname) + if img is None: + continue + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + if image_shape is None: + image_shape = gray.shape[::-1] # (width, height) + + # 체커보드 코너 찾기 + ret, corners = cv2.findChessboardCorners(gray, board_size, None) + if ret: + # 코너를 더 정밀하게 + corners_sub = cv2.cornerSubPix( + gray, + corners, + (11, 11), + (-1, -1), + (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001), + ) + # 누적 + obj_points.append(objp) + img_points.append(corners_sub) + + # 내부 파라미터, 왜곡 계수, 외부 파라미터 구하기 + if len(obj_points) < 1: + print("체커보드 코너를 충분히 찾지 못하였습니다.") + return None, None, None, None + + # flags = cv2.CALIB_ZERO_TANGENT_DIST + cv2.CALIB_FIX_K3 등 필요에 따라 추가 + ret, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera( + obj_points, # 3D 실세계 점 + img_points, # 2D 이미지 점 + image_shape, # (width, height) + None, # 초기 camera_matrix + None, # 초기 dist_coeffs + ) + + if not ret: + print("캘리브레이션이 제대로 수렴하지 않았습니다.") + return None, None, None, None + + return camera_matrix, dist_coeffs, rvecs, tvecs + +from scipy.linalg import sqrtm +from numpy.linalg import inv + +# 4) 여러 개의 변환 행렬 조합 함수 +def compose_transformation_matrices(R_list, t_list): + T_list = [] + for R, t in zip(R_list, t_list): + T = np.eye(4) + T[:3, :3] = R + T[:3, 3] = np.ravel(t) # t가 벡터 형태여야 합니다. + T_list.append(T) + return T_list + +# 회전행렬을 로그변환하는 함수 +def logR(T): + R = T[0:3, 0:3] + theta = np.arccos((np.trace(R) - 1) / 2) + logr = np.array([ + R[2, 1] - R[1, 2], + R[0, 2] - R[2, 0], + R[1, 0] - R[0, 1] + ]) * theta / (2 * np.sin(theta)) + return logr + +# A와 B의 변환을 이용하여 보정 행렬 계산 +def Calibrate(A, B): + n_data = len(A) + M = np.zeros((3, 3)) + + for i in range(n_data - 1): + alpha = logR(A[i]) + beta = logR(B[i]) + alpha2 = logR(A[i + 1]) + beta2 = logR(B[i + 1]) + + alpha3 = np.cross(alpha, alpha2) + beta3 = np.cross(beta, beta2) + + M1 = np.dot(beta.reshape(3, 1), alpha.reshape(1, 3)) + M2 = np.dot(beta2.reshape(3, 1), alpha2.reshape(1, 3)) + M3 = np.dot(beta3.reshape(3, 1), alpha3.reshape(1, 3)) + + M += M1 + M2 + M3 + + theta = np.dot(sqrtm(inv(np.dot(M.T, M))), M.T) + + C = np.zeros((3 * n_data, 3)) + d = np.zeros((3 * n_data, 1)) + for i in range(n_data): + rot_a = A[i][:3, :3] + trans_a = A[i][:3, 3] + trans_b = B[i][:3, 3] + C[3 * i:3 * i + 3, :] = np.eye(3) - rot_a + d[3 * i:3 * i + 3, 0] = trans_a - np.dot(theta, trans_b) + + b_x = np.dot(inv(np.dot(C.T, C)), np.dot(C.T, d)) + return theta, b_x + +# Main Function +def main(args=None): + data = json.load(open("data/calibrate_data.json")) + robot_poses = np.array(data["poses"]) + + robot_poses[:, :3] = robot_poses[:, :3] + image_paths = ["data/" + d for d in data["file_name"]] + + valid_indices = [] + for i, pose in enumerate(robot_poses): + T_base2gripper = get_robot_pose_matrix(*pose) + det_T = np.linalg.det(T_base2gripper) + print(f"Index {i}: det(T_base2gripper) = {det_T}") + + if np.abs(det_T) > 1e-6: + valid_indices.append(i) + else: + print(f"⚠️ Warning: Singular T_base2gripper at index {i}!") + + robot_poses = robot_poses[valid_indices] + image_paths = [image_paths[i] for i in valid_indices] + + checkerboard_size = (8, 6) # 내부 코너 개수 + square_size = 25 + + camera_matrix, dist_coeffs, rvecs, tvecs = calibrate_camera_from_chessboard( + image_paths, checkerboard_size, square_size + ) + + R_gripper2base_list = [] + t_gripper2base_list = [] + R_camera2checker_list = [] + t_camera2checker_list = [] + R_checker2camera_list = [] + t_checker2camera_list = [] + + for img_path, pose in zip(image_paths, robot_poses): + # 1) 베이스->그리퍼 변환행렬 + T_base2gripper = get_robot_pose_matrix(*pose) + + # 2) 이미지 로딩 + image = cv2.imread(img_path) + if image is None: + continue + + # 3) 카메라->체커보드 변환 구하기 + R_cam2checker, t_cam2checker = find_checkerboard_pose( + image, checkerboard_size, square_size, camera_matrix, dist_coeffs + ) + if R_cam2checker is None: + continue + + T_gripper2base= np.linalg.inv(T_base2gripper) + + R_gripper2base = T_gripper2base[:3, :3] + t_gripper2base = T_gripper2base[:3, 3] + + R_gripper2base_list.append(R_gripper2base.copy()) + t_gripper2base_list.append(t_gripper2base.reshape(-1, 1).copy()) + + T_cam2checker = np.eye(4) + T_cam2checker[:3, :3] = R_cam2checker + T_cam2checker[:3, 3] = t_cam2checker.flatten() + T_checker2cam = np.linalg.inv(T_cam2checker) + + R_checker2camera_list.append(T_checker2cam[:3, :3].copy()) + t_checker2camera_list.append(T_checker2cam[:3, 3].copy()) + + T_gripper2base_list = compose_transformation_matrices(R_gripper2base_list, t_gripper2base_list) + T_checker2cam_list = compose_transformation_matrices(R_checker2camera_list, t_checker2camera_list) + A_list = [] + B_list = [] + num_pairs = min(len(T_gripper2base_list), len(T_checker2cam_list)) + + for i, T in enumerate(T_gripper2base_list): + det = np.linalg.det(T) + if np.abs(det) < 1e-6: + print(f"⚠️ Warning: T_gripper2base_list[{i}] is singular or nearly singular!") + + for i in range(num_pairs - 1): + A_i = np.dot(inv(T_gripper2base_list[i]), T_gripper2base_list[i + 1]) + B_i = np.dot(inv(T_checker2cam_list[i]), T_checker2cam_list[i + 1]) + A_list.append(A_i) + B_list.append(B_i) + + theta, b_x = Calibrate(A_list, B_list) + X = np.eye(4) + X[:3, :3] = theta + X[:3, 3] = b_x.flatten() + T_cam2base = X + print(T_cam2base) + print(T_cam2base[:3, 3]) + np.save("T_cam2base.npy", T_cam2base) + + +if __name__ == "__main__": + main() diff --git a/src/azas_calibration/azas_calibration/handeye_calibration_legacy.py b/src/azas_calibration/azas_calibration/handeye_calibration_legacy.py new file mode 100644 index 0000000..77ec9bc --- /dev/null +++ b/src/azas_calibration/azas_calibration/handeye_calibration_legacy.py @@ -0,0 +1,231 @@ +import cv2 +import numpy as np +import json +from scipy.spatial.transform import Rotation + +# 1) 로봇 그리퍼의 절대 좌표 (x, y, z, rx, ry, rz)를 행렬로 변환하는 함수 +def get_robot_pose_matrix(x, y, z, rx, ry, rz): + """ + 베이스->그리퍼 변환행렬 (4x4)을 반환. + """ + R = Rotation.from_euler('ZYZ', [rx, ry, rz], degrees=True).as_matrix() + T = np.eye(4) + T[:3, :3] = R + T[:3, 3] = [x, y, z] + return T + + +# 2) 체커보드 코너 검출 (카메라→체커보드 변환 구하기) +def find_checkerboard_pose( + image, board_size, square_size, camera_matrix, dist_coeffs +): + """ + checkerboard_size = (7, 5) # 내부 코너 개수 + square_size = 25.0 # mm 단위 + 이미지에서 체커보드를 찾고, solvePnP로 카메라→체커보드 변환(R, t)을 구함. + 반환값: (R_camera2checker, t_camera2checker) + """ + objp = np.zeros((board_size[0] * board_size[1], 3), np.float32) + # 예: x 방향으로 square_size씩 증가, y 방향으로 square_size씩 증가 + objp[:, :2] = ( + np.mgrid[0 : board_size[0], 0 : board_size[1]].T.reshape(-1, 2) * 25 + ) + + gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) + found, corners = cv2.findChessboardCorners( + gray, + board_size, + flags=cv2.CALIB_CB_ADAPTIVE_THRESH + + cv2.CALIB_CB_FAST_CHECK + + cv2.CALIB_CB_NORMALIZE_IMAGE, + ) + if not found: + return None, None + + # 코너 좌표를 더 정확히 + corners_sub = cv2.cornerSubPix( + gray, + corners, + (11, 11), + (-1, -1), + criteria=(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001), + ) + + # solvePnP + retval, rvec, tvec = cv2.solvePnP(objp, corners_sub, camera_matrix, dist_coeffs) + if not retval: + return None, None + + # 회전벡터 -> 회전행렬 + R, _ = cv2.Rodrigues(rvec) + + return R, tvec + + +def calibrate_camera_from_chessboard( + image_folder_path, + board_size, # (7, 5)처럼 내부 코너 개수 + square_size, # mm 단위 +): + """ + 지정된 폴더 안의 체커보드 이미지를 읽고, 카메라 행렬(camera_matrix)와 왜곡 계수(dist_coeffs)를 추정한다. + board_size: 체커보드 내부 코너 수 (cols, rows) + square_size: 체커보드 한 칸 크기 (mm) + """ + # 3D 세계 좌표계에 대한 좌표 생성 (z=0 평면 상에 체커보드) + objp = np.zeros((board_size[0] * board_size[1], 3), np.float32) + # 예: x 방향으로 square_size씩 증가, y 방향으로 square_size씩 증가 + objp[:, :2] = ( + np.mgrid[0 : board_size[0], 0 : board_size[1]].T.reshape(-1, 2) * square_size + ) + + # 모든 이미지에 대해 3D / 2D 포인트 누적 + obj_points = [] # 3D world points + img_points = [] # 2D image points + image_shape = None + + # 폴더 내에 있는 이미지 파일 읽기 + image_paths = image_folder_path # JPG, PNG 등 확장자 맞춰서 + # 필요하면 jpg 등 다른 확장자도 처리 가능 + + for fname in image_paths: + img = cv2.imread(fname) + if img is None: + continue + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + if image_shape is None: + image_shape = gray.shape[::-1] # (width, height) + + # 체커보드 코너 찾기 + ret, corners = cv2.findChessboardCorners(gray, board_size, None) + if ret: + # 코너를 더 정밀하게 + corners_sub = cv2.cornerSubPix( + gray, + corners, + (11, 11), + (-1, -1), + (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001), + ) + # 누적 + obj_points.append(objp) + img_points.append(corners_sub) + + # 내부 파라미터, 왜곡 계수, 외부 파라미터 구하기 + if len(obj_points) < 1: + print("체커보드 코너를 충분히 찾지 못하였습니다.") + return None, None, None, None + + # flags = cv2.CALIB_ZERO_TANGENT_DIST + cv2.CALIB_FIX_K3 등 필요에 따라 추가 + ret, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera( + obj_points, # 3D 실세계 점 + img_points, # 2D 이미지 점 + image_shape, # (width, height) + None, # 초기 camera_matrix + None, # 초기 dist_coeffs + ) + + if not ret: + print("캘리브레이션이 제대로 수렴하지 않았습니다.") + return None, None, None, None + + return camera_matrix, dist_coeffs, rvecs, tvecs + + + +# Main Function +def main(args=None): + # 캘리브레이션 데이터 로드 + data = json.load(open("data/calibrate_data.json")) + robot_poses = np.array(data["poses"]) + + robot_poses[:, :3] = robot_poses[:, :3] + image_paths = ["data/" + d for d in data["file_name"]] + + checkerboard_size = (10, 7) # 내부 코너 개수 + square_size = 25 + # 카메라 캘리브레이션 수행(내부 파라미터 왜곡 보정) + camera_matrix, dist_coeffs, rvecs, tvecs = calibrate_camera_from_chessboard( + image_paths, checkerboard_size, square_size + ) + + R_gripper2base_list = [] + t_gripper2base_list = [] + R_camera2checker_list = [] + t_camera2checker_list = [] + R_checker2camera_list = [] + t_checker2camera_list = [] + + for img_path, pose in zip(image_paths, robot_poses): + # 1) 베이스->그리퍼 변환행렬 + T_base2gripper = get_robot_pose_matrix(*pose) + + # 2) 이미지 로딩 + image = cv2.imread(img_path) + if image is None: + continue + + # 3) 카메라->체커보드 변환 구하기 + R_cam2checker, t_cam2checker = find_checkerboard_pose( + image, checkerboard_size, square_size, camera_matrix, dist_coeffs + ) + if R_cam2checker is None: + continue + + # T_gripper2base= np.linalg.inv(T_base2gripper) + T_gripper2base= T_base2gripper + + R_gripper2base = T_gripper2base[:3, :3] + t_gripper2base = T_gripper2base[:3, 3] + + R_gripper2base_list.append(R_gripper2base.copy()) + t_gripper2base_list.append(t_gripper2base.reshape(-1, 1).copy()) + + T_cam2checker = np.eye(4) + T_cam2checker[:3, :3] = R_cam2checker + T_cam2checker[:3, 3] = t_cam2checker.flatten() + + T_checker2cam = T_cam2checker + + R_checker2camera_list.append(T_checker2cam[:3, :3].copy()) + t_checker2camera_list.append(T_checker2cam[:3, 3].copy()) + + + # Hand-Eye 캘리브레이션 수행 + R_cam2gripper, t_cam2gripper = cv2.calibrateHandEye( + R_gripper2base_list, + t_gripper2base_list, + R_checker2camera_list, + t_checker2camera_list, + method=cv2.CALIB_HAND_EYE_PARK, + ) + + + T_base2gripper_example = get_robot_pose_matrix(*robot_poses[2]) + R_base2gripper_example = T_base2gripper_example[:3, :3] + t_base2gripper_example = T_base2gripper_example[:3, 3] + + # 그리퍼->카메라 변환행렬 + T_gripper2cam = np.eye(4) + T_gripper2cam[:3, :3] = R_cam2gripper + T_gripper2cam[:3, 3] = t_cam2gripper.flatten() + + # 최종 베이스->카메라 + T_base2cam = T_base2gripper_example @ T_gripper2cam + + print("===== Hand-Eye Calibration Results =====") + print("R_base2gripper:\n", T_base2gripper_example[:3, :3]) + print("T_base2gripper:\n", T_base2gripper_example[:3, 3]) + print("\n") + print("R_base2camera:\n", T_base2cam[:3, :3]) + print("T_base2camera:\n", T_base2cam[:3, 3]) + print("\n") + print("R_gripper2camera:\n", T_gripper2cam[:3, :3]) + print("T_gripper2camera:\n", T_gripper2cam[:3, 3].tolist()) + + # save T_grigper2camera + np.save("T_gripper2camera.npy", T_gripper2cam) + + +if __name__ == "__main__": + main() diff --git a/src/azas_calibration/azas_calibration/realsense_legacy.py b/src/azas_calibration/azas_calibration/realsense_legacy.py new file mode 100644 index 0000000..45a6813 --- /dev/null +++ b/src/azas_calibration/azas_calibration/realsense_legacy.py @@ -0,0 +1,41 @@ +from rclpy.node import Node +from sensor_msgs.msg import Image, CameraInfo +from cv_bridge import CvBridge + + +class ImgNode(Node): + def __init__(self): + super().__init__('img_node') + self.bridge = CvBridge() + self.color_frame = None + self.color_frame_stamp = None + self.depth_frame = None + self.intrinsics = None + self.color_subscription = self.create_subscription( + Image, '/camera/camera/color/image_raw', self.color_callback, 10) + self.depth_subscription = self.create_subscription( + Image, '/camera/camera/aligned_depth_to_color/image_raw', self.depth_callback, 10) + self.camera_info_subscription = self.create_subscription( + CameraInfo, '/camera/camera/color/camera_info', self.camera_info_callback, 10) + + def camera_info_callback(self, msg): + self.intrinsics = {"fx": msg.k[0], "fy": msg.k[4], "ppx": msg.k[2], "ppy": msg.k[5]} + + def color_callback(self, msg): + self.color_frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8') + self.color_frame_stamp = str(msg.header.stamp.sec) + str(msg.header.stamp.nanosec) + + def depth_callback(self, msg): + self.depth_frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough') + + def get_color_frame(self): + return self.color_frame + + def get_color_frame_stamp(self): + return self.color_frame_stamp + + def get_depth_frame(self): + return self.depth_frame + + def get_camera_intrinsic(self): + return self.intrinsics \ No newline at end of file diff --git a/src/azas_calibration/package.xml b/src/azas_calibration/package.xml index dab9b51..5328754 100644 --- a/src/azas_calibration/package.xml +++ b/src/azas_calibration/package.xml @@ -8,8 +8,12 @@ ament_python rclpy + azas_gripper + cv_bridge geometry_msgs + sensor_msgs azas_interfaces + python3-scipy ament_python diff --git a/src/azas_calibration/setup.py b/src/azas_calibration/setup.py index 2a8c43a..3c65c80 100644 --- a/src/azas_calibration/setup.py +++ b/src/azas_calibration/setup.py @@ -19,6 +19,10 @@ entry_points={ "console_scripts": [ "calibration_loader_node = azas_calibration.calibration_loader_node:main", + "calibration_test_legacy = azas_calibration.calibration_test_legacy:main", + "data_recording_legacy = azas_calibration.data_recording_legacy:main", + "eye2hand_calibration_legacy = azas_calibration.eye2hand_calibration_legacy:main", + "handeye_calibration_legacy = azas_calibration.handeye_calibration_legacy:main", ], }, ) diff --git a/src/azas_motion/azas_motion/gear_assembly_legacy.py b/src/azas_motion/azas_motion/gear_assembly_legacy.py new file mode 100644 index 0000000..dc5530e --- /dev/null +++ b/src/azas_motion/azas_motion/gear_assembly_legacy.py @@ -0,0 +1,423 @@ +#!/usr/bin/env python3 + +import math +import time + +import rclpy +from rclpy.logging import get_logger + +from geometry_msgs.msg import PoseStamped +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters + +# ====== OnRobot RG2 설정 ====== +from azas_gripper.onrobot import RG # 같은 패키지 내부의 onrobot.py 사용 + +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 + +# 그리퍼 폭 (raw 단위: 1/10 mm) +GRIPPER_OPEN_WIDTH = 500 # 50.0 mm +GRIPPER_CLOSE_WIDTH = 150 # 20.0 mm +GRIPPER_FORCE = 300 # 약 20 N + +# ================================ +# 기본 설정 +# ================================ +GROUP_NAME = "manipulator" # SRDF에 정의된 planning group 이름 +BASE_FRAME = "base_link" # 로봇 베이스 프레임 +EE_LINK = "link_6" # 엔드이펙터 링크 이름 (SRDF/URDF 기준) + +HOME_JOINTS_RAD = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(0.0), +} + +# ====== 안전 작업 영역 정의 (base_link 기준) ====== +SAFE_X_MIN = 0.0 # x는 0 이상 +SAFE_Y_MIN = -0.3 # y 하한 +SAFE_Y_MAX = 0.3 # y 상한 +SAFE_Z_MIN = 0.27 # z는 이 값보다 낮아지면 안 됨 +# ================================================== + +# ====== 기어 픽업/조립 포즈 (base_link 기준) ====== +GEAR_TASKS = [ + { # Gear 1 + "pick": { + "pos": {"x": 0.393, "y": 0.094, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + "place": { + "pos": {"x": 0.393, "y": -0.206, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + }, + { # Gear 2 + "pick": { + "pos": {"x": 0.392, "y": 0.200, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + "place": { + "pos": {"x": 0.392, "y": -0.101, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + }, + { # Gear 3 + "pick": { + "pos": {"x": 0.486, "y": 0.153, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + "place": { + "pos": {"x": 0.486, "y": -0.149, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + }, + { # Gear 4 + "pick": { + "pos": {"x": 0.427, "y": 0.148, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + "place": { + "pos": {"x": 0.426, "y": -0.153, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + }, +] + +APPROACH_OFFSET = 0.05 # 위에서 접근/후퇴할 거리 [m] + +# ----- 마지막 기어용 wiggle 파라미터 ----- +WIGGLE_Z = 0.295 # z축 회전할 높이 (place_z보다 약간 위) +WIGGLE_YAW_DEG = 5.0 # 좌우로 회전 각도 (deg) +WIGGLE_COUNT = 3 # 좌우 반복 횟수 + + +def quat_mul(q1, q2): + """쿼터니언 곱: q = q1 * q2""" + x1, y1, z1, w1 = q1 + x2, y2, z2, w2 = q2 + x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2 + y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2 + z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2 + w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2 + return x, y, z, w + + +def make_yaw_quat(yaw_rad): + """z축(yaw) 회전에 해당하는 쿼터니언 생성""" + half = yaw_rad / 2.0 + return (0.0, 0.0, math.sin(half), math.cos(half)) + + +def clamp_to_safe_workspace(x: float, y: float, z: float, logger): + """안전 작업 영역으로 (x, y, z) 클램핑""" + safe_x = x + safe_y = y + safe_z = z + + if safe_x < SAFE_X_MIN: + logger.warning( + f"Requested x ({safe_x:.3f} m) is below safety limit " + f"({SAFE_X_MIN:.3f} m). Clamping to SAFE_X_MIN." + ) + safe_x = SAFE_X_MIN + + if safe_y < SAFE_Y_MIN: + logger.warning( + f"Requested y ({safe_y:.3f} m) is below safety limit " + f"({SAFE_Y_MIN:.3f} m). Clamping to SAFE_Y_MIN." + ) + safe_y = SAFE_Y_MIN + elif safe_y > SAFE_Y_MAX: + logger.warning( + f"Requested y ({safe_y:.3f} m) is above safety limit " + f"({SAFE_Y_MAX:.3f} m). Clamping to SAFE_Y_MAX." + ) + safe_y = SAFE_Y_MAX + + if safe_z < SAFE_Z_MIN: + logger.warning( + f"Requested z ({safe_z:.3f} m) is below safety limit " + f"({SAFE_Z_MIN:.3f} m). Clamping to SAFE_Z_MIN." + ) + safe_z = SAFE_Z_MIN + + return safe_x, safe_y, safe_z + + +def plan_and_execute( + robot: MoveItPy, + planning_component, + logger, + pose_goal: PoseStamped = None, + plan_parameters=None, +): + """ + 공식 문서 스타일 helper: 계획 후 곧바로 실행 + + - pose_goal이 주어지면: + · 안전 영역 클램핑 + · start_state = current + · pose 기반 goal 설정 (EE_LINK) + - 그 다음 plan_parameters 유무에 따라 plan() 호출 후 execute + """ + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + + planning_component.set_start_state_to_current_state() + planning_component.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link=EE_LINK, + ) + + logger.info("Planning trajectory") + + if plan_parameters is not None: + plan_result = planning_component.plan( + parameters=plan_parameters + ) + else: + plan_result = planning_component.plan() + + if not plan_result: + logger.error("Planning failed") + return False + + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + robot.execute( + group_name=GROUP_NAME, + robot_trajectory=robot_trajectory, + blocking=True, + ) + logger.info("Execution finished") + return True + + +def main(args=None): + rclpy.init(args=args) + + logger = get_logger("m0609_gear_assembly") + logger.info("=== M0609 Gear Assembly 시작 ===") + + # ---- Gripper ---- + gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + time.sleep(0.5) + gripper.move_gripper(GRIPPER_OPEN_WIDTH, GRIPPER_FORCE) + + # ---- MoveIt ---- + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + robot_model = robot.get_robot_model() + + # ---- PlanRequestParameters (HOME / Pilz) ---- + home_params = PlanRequestParameters(robot) + home_params.planning_pipeline = "ompl" + home_params.planner_id = "RRTConnect" + home_params.max_velocity_scaling_factor = 0.2 + home_params.max_acceleration_scaling_factor = 0.1 + home_params.planning_time = 2.0 + + pilz_params = PlanRequestParameters(robot) + pilz_params.planning_pipeline = "pilz_industrial_motion_planner" + pilz_params.planner_id = "PTP" + pilz_params.max_velocity_scaling_factor = 0.15 + pilz_params.max_acceleration_scaling_factor = 0.1 + pilz_params.planning_time = 2.0 + + # ---- HOME 자세로 이동 (joint goal) ---- + logger.info("=== HOME 자세로 이동 ===") + home_state = RobotState(robot_model) + home_state.joint_positions = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(0.0), + } + home_state.update() + + arm.set_start_state_to_current_state() + arm.set_goal_state(robot_state=home_state) + plan_and_execute(robot, arm, logger, plan_parameters=home_params) + + # ---- PoseStamped 공용 객체 준비 ---- + pose_goal = PoseStamped() + pose_goal.header.frame_id = BASE_FRAME + + logger.info("=== Gear Pick & Place 시작 ===") + + total_gears = len(GEAR_TASKS) + + # ------------------------------- + # 각 기어에 대해 Pick & Place + # ------------------------------- + for gear_idx, task in enumerate(GEAR_TASKS, start=1): + pick = task["pick"] + place = task["place"] + + logger.info(f"--- Gear {gear_idx} 작업 시작 ---") + + # 1) Pick 위에서 접근 (z + offset) + pose_goal.pose.position.x = pick["pos"]["x"] + pose_goal.pose.position.y = pick["pos"]["y"] + pose_goal.pose.position.z = pick["pos"]["z"] + APPROACH_OFFSET + + pose_goal.pose.orientation.x = pick["ori"]["x"] + pose_goal.pose.orientation.y = pick["ori"]["y"] + pose_goal.pose.orientation.z = pick["ori"]["z"] + pose_goal.pose.orientation.w = pick["ori"]["w"] + + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # 2) Pick 위치로 내려가기 + pose_goal.pose.position.z = pick["pos"]["z"] + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # 3) Gripper 닫기 (집기) + logger.info("Gripper CLOSE (20mm) – 기어 집기") + gripper.move_gripper(width_val=GRIPPER_CLOSE_WIDTH, + force_val=GRIPPER_FORCE) + time.sleep(1.0) + + # 4) 다시 위로 올라가기 (Pick z + offset) + pose_goal.pose.position.z = pick["pos"]["z"] + APPROACH_OFFSET + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # 5) Place 위에서 접근 + pose_goal.pose.position.x = place["pos"]["x"] + pose_goal.pose.position.y = place["pos"]["y"] + pose_goal.pose.position.z = place["pos"]["z"] + APPROACH_OFFSET + + pose_goal.pose.orientation.x = place["ori"]["x"] + pose_goal.pose.orientation.y = place["ori"]["y"] + pose_goal.pose.orientation.z = place["ori"]["z"] + pose_goal.pose.orientation.w = place["ori"]["w"] + + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # ------------------------------- + # Place 동작 + # - 1~3번 기어: 바로 place_z로 하강 + # - 4번 기어(마지막): z=WIGGLE_Z에서 z축 회전 wiggle 후 place_z로 하강 + # ------------------------------- + if gear_idx < total_gears: + # 6) Place 위치로 내려가기 (일반) + pose_goal.pose.position.z = place["pos"]["z"] + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + else: + # ---- 마지막 기어: z축 wiggle ---- + logger.info( + f"마지막 기어: z={WIGGLE_Z:.3f}에서 " + f"±{WIGGLE_YAW_DEG}deg 좌우 회전 {WIGGLE_COUNT}회씩 수행" + ) + + # 우선 WIGGLE_Z까지 z만 이동 + pose_goal.pose.position.z = WIGGLE_Z + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # 기준 쿼터니언 (place 자세) + q_base = ( + place["ori"]["x"], + place["ori"]["y"], + place["ori"]["z"], + place["ori"]["w"], + ) + + yaw_rad = math.radians(WIGGLE_YAW_DEG) + + # 좌우로 WIGGLE_COUNT 번 반복 + for i in range(1, WIGGLE_COUNT + 1): + # +yaw + q_plus = quat_mul(make_yaw_quat(+yaw_rad), q_base) + pose_goal.pose.orientation.x = q_plus[0] + pose_goal.pose.orientation.y = q_plus[1] + pose_goal.pose.orientation.z = q_plus[2] + pose_goal.pose.orientation.w = q_plus[3] + + logger.info(f"Wiggle {i}/{WIGGLE_COUNT}: +{WIGGLE_YAW_DEG:.1f} deg") + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # -yaw + q_minus = quat_mul(make_yaw_quat(-yaw_rad), q_base) + pose_goal.pose.orientation.x = q_minus[0] + pose_goal.pose.orientation.y = q_minus[1] + pose_goal.pose.orientation.z = q_minus[2] + pose_goal.pose.orientation.w = q_minus[3] + + logger.info(f"Wiggle {i}/{WIGGLE_COUNT}: -{WIGGLE_YAW_DEG:.1f} deg") + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # 마지막에 기준 자세로 복귀 + pose_goal.pose.orientation.x = q_base[0] + pose_goal.pose.orientation.y = q_base[1] + pose_goal.pose.orientation.z = q_base[2] + pose_goal.pose.orientation.w = q_base[3] + + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # 그리고 place_z로 직선 하강 + pose_goal.pose.position.z = place["pos"]["z"] + logger.info(f"마지막 기어: wiggle 후 place_z={place['pos']['z']:.3f}로 하강") + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + # 7) Gripper 열기 (놓기) + logger.info("Gripper OPEN (50mm) – 기어 놓기") + gripper.move_gripper(width_val=GRIPPER_OPEN_WIDTH, + force_val=GRIPPER_FORCE) + time.sleep(1.0) + + # 8) 다시 위로 올라가기 (Place z + offset) + pose_goal.pose.position.z = place["pos"]["z"] + APPROACH_OFFSET + plan_and_execute(robot, arm, logger, + pose_goal=pose_goal, + plan_parameters=pilz_params) + + logger.info(f"--- Gear {gear_idx} 작업 완료 ---") + + logger.info("=== 모든 기어 조립 완료. HOME으로 복귀 ===") + + # 마지막으로 HOME으로 복귀 + arm.set_start_state_to_current_state() + arm.set_goal_state(robot_state=home_state) + plan_and_execute(robot, arm, logger, plan_parameters=home_params) + + logger.info("=== Gear Assembly 노드 종료 ===") + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/azas_motion/azas_motion/mp_waypoint_legacy.py b/src/azas_motion/azas_motion/mp_waypoint_legacy.py new file mode 100644 index 0000000..4b76f0e --- /dev/null +++ b/src/azas_motion/azas_motion/mp_waypoint_legacy.py @@ -0,0 +1,189 @@ +#!/usr/bin/env python3 +import math + +import rclpy +from rclpy.logging import get_logger + +from geometry_msgs.msg import PoseStamped +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy + +# ================================ +# 기본 설정 +# ================================ +GROUP_NAME = "manipulator" # SRDF에 정의된 planning group 이름 +BASE_FRAME = "base_link" # 로봇 베이스 프레임 +EE_LINK = "link_6" # 엔드이펙터 링크 이름 (SRDF/URDF 기준) + +HOME_JOINTS_DEG = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0] +HOME_JOINTS_RAD = [math.radians(d) for d in HOME_JOINTS_DEG] + +# ====== 안전 작업 영역 정의 (base_link 기준) ====== +SAFE_X_MIN = 0.0 # x는 0 이상 +SAFE_Y_MIN = -0.3 # y 하한 +SAFE_Y_MAX = 0.3 # y 상한 +SAFE_Z_MIN = 0.27 # z는 이 값보다 낮아지면 안 됨 +# ================================================== + + +def clamp_to_safe_workspace(x: float, y: float, z: float, logger): + """안전 작업 영역으로 (x, y, z) 클램핑""" + safe_x = x + safe_y = y + safe_z = z + + if safe_x < SAFE_X_MIN: + logger.warning( + f"Requested x ({safe_x:.3f} m) is below safety limit " + f"({SAFE_X_MIN:.3f} m). Clamping to SAFE_X_MIN." + ) + safe_x = SAFE_X_MIN + + if safe_y < SAFE_Y_MIN: + logger.warning( + f"Requested y ({safe_y:.3f} m) is below safety limit " + f"({SAFE_Y_MIN:.3f} m). Clamping to SAFE_Y_MIN." + ) + safe_y = SAFE_Y_MIN + elif safe_y > SAFE_Y_MAX: + logger.warning( + f"Requested y ({safe_y:.3f} m) is above safety limit " + f"({SAFE_Y_MAX:.3f} m). Clamping to SAFE_Y_MAX." + ) + safe_y = SAFE_Y_MAX + + if safe_z < SAFE_Z_MIN: + logger.warning( + f"Requested z ({safe_z:.3f} m) is below safety limit " + f"({SAFE_Z_MIN:.3f} m). Clamping to SAFE_Z_MIN." + ) + safe_z = SAFE_Z_MIN + + return safe_x, safe_y, safe_z + + +def plan_and_execute( + robot: MoveItPy, + planning_component, + logger, + pose_goal: PoseStamped = None, + plan_parameters=None, +): + """ + 공식 문서 스타일 helper: 계획 후 곧바로 실행 + + - pose_goal이 주어지면: + · 안전 영역 클램핑 + · start_state = current + · pose 기반 goal 설정 (EE_LINK) + - 그 다음 plan_parameters 유무에 따라 plan() 호출 후 execute + """ + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + + planning_component.set_start_state_to_current_state() + planning_component.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link=EE_LINK, + ) + + logger.info("Planning trajectory") + + if plan_parameters is not None: + plan_result = planning_component.plan( + parameters=plan_parameters + ) + else: + plan_result = planning_component.plan() + + if not plan_result: + logger.error("Planning failed") + return False + + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + robot.execute( + group_name=GROUP_NAME, + robot_trajectory=robot_trajectory, + blocking=True, + ) + logger.info("Execution finished") + return True + + +def main(args=None): + # ================================ + # Instantiating moveit_py and planning component + # ================================ + rclpy.init(args=args) + logger = get_logger("m0609.moveit_py.waypoint") + + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + logger.info("MoveItPy instance created") + + # 로봇 모델 / RobotState 준비 + robot_model = robot.get_robot_model() + home_state = RobotState(robot_model) + home_state.set_joint_group_positions(GROUP_NAME, HOME_JOINTS_RAD) + home_state.update() + + # 1) HOME 자세로 이동 (조인트 목표) + arm.set_start_state_to_current_state() + arm.set_goal_state(robot_state=home_state) + plan_and_execute(robot, arm, logger) + + # 2) WAYPOINT 이동 (안전영역 포함) + WAYPOINTS = [ + { # waypoint 1 + "pos": {"x": 0.493, "y": 0.010, "z": 0.417}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + { # waypoint 2 + "pos": {"x": 0.493, "y": -0.218, "z": 0.417}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + { # waypoint 3 + "pos": {"x": 0.371, "y": -0.218, "z": 0.419}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + ] + + pose_goal = PoseStamped() + pose_goal.header.frame_id = BASE_FRAME + + for i, wp in enumerate(WAYPOINTS, start=1): + pos = wp["pos"] + ori = wp["ori"] + + logger.info( + f"--- Waypoint {i}: " + f"x={pos['x']:.3f}, y={pos['y']:.3f}, z={pos['z']:.3f} ---" + ) + + # pose_goal에 원래 의도 좌표/자세 설정 + pose_goal.pose.position.x = pos["x"] + pose_goal.pose.position.y = pos["y"] + pose_goal.pose.position.z = pos["z"] + + pose_goal.pose.orientation.x = ori["x"] + pose_goal.pose.orientation.y = ori["y"] + pose_goal.pose.orientation.z = ori["z"] + pose_goal.pose.orientation.w = ori["w"] + + # plan_and_execute 안에서 안전영역 + goal 설정까지 처리 + plan_and_execute(robot, arm, logger, pose_goal=pose_goal) + + robot.shutdown() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/azas_motion/azas_motion/mp_waypoint_pilz_legacy.py b/src/azas_motion/azas_motion/mp_waypoint_pilz_legacy.py new file mode 100644 index 0000000..9eb6ff5 --- /dev/null +++ b/src/azas_motion/azas_motion/mp_waypoint_pilz_legacy.py @@ -0,0 +1,239 @@ +#!/usr/bin/env python3 +import math + +import rclpy +from rclpy.logging import get_logger + +from geometry_msgs.msg import PoseStamped +from moveit.planning import MoveItPy, PlanRequestParameters +from moveit_msgs.msg import Constraints, JointConstraint + +# ================================ +# 기본 설정 +# ================================ +GROUP_NAME = "manipulator" # SRDF에 정의된 planning group 이름 +BASE_FRAME = "base_link" # 로봇 베이스 프레임 +EE_LINK = "link_6" # 엔드이펙터 링크 이름 (SRDF/URDF 기준) + +HOME_JOINTS_DEG = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0] +HOME_JOINTS_RAD = [math.radians(d) for d in HOME_JOINTS_DEG] + +# ====== 안전 작업 영역 정의 (base_link 기준) ====== +SAFE_X_MIN = 0.0 # x는 0 이상 +SAFE_Y_MIN = -0.3 # y 하한 +SAFE_Y_MAX = 0.3 # y 상한 +SAFE_Z_MIN = 0.27 # z는 이 값보다 낮아지면 안 됨 +# ================================================== + + +def clamp_to_safe_workspace(x: float, y: float, z: float, logger): + """안전 작업 영역으로 (x, y, z) 클램핑""" + safe_x = x + safe_y = y + safe_z = z + + if safe_x < SAFE_X_MIN: + logger.warning( + f"Requested x ({safe_x:.3f} m) is below safety limit " + f"({SAFE_X_MIN:.3f} m). Clamping to SAFE_X_MIN." + ) + safe_x = SAFE_X_MIN + + if safe_y < SAFE_Y_MIN: + logger.warning( + f"Requested y ({safe_y:.3f} m) is below safety limit " + f"({SAFE_Y_MIN:.3f} m). Clamping to SAFE_Y_MIN." + ) + safe_y = SAFE_Y_MIN + elif safe_y > SAFE_Y_MAX: + logger.warning( + f"Requested y ({safe_y:.3f} m) is above safety limit " + f"({SAFE_Y_MAX:.3f} m). Clamping to SAFE_Y_MAX." + ) + safe_y = SAFE_Y_MAX + + if safe_z < SAFE_Z_MIN: + logger.warning( + f"Requested z ({safe_z:.3f} m) is below safety limit " + f"({SAFE_Z_MIN:.3f} m). Clamping to SAFE_Z_MIN." + ) + safe_z = SAFE_Z_MIN + + return safe_x, safe_y, safe_z + + +def build_home_joint_constraints() -> list[Constraints]: + constraints = Constraints() + joint_names = [f"joint_{i}" for i in range(1, 7)] + + for joint_name, position in zip(joint_names, HOME_JOINTS_RAD): + joint_constraint = JointConstraint() + joint_constraint.joint_name = joint_name + joint_constraint.position = position + joint_constraint.tolerance_above = 0.001 + joint_constraint.tolerance_below = 0.001 + joint_constraint.weight = 1.0 + constraints.joint_constraints.append(joint_constraint) + + return [constraints] + + +def plan_and_execute( + robot: MoveItPy, + planning_component, + logger, + pose_goal: PoseStamped = None, + plan_parameters=None, +): + """ + 공식 문서 스타일 helper: 계획 후 곧바로 실행 + + - pose_goal이 주어지면: + · 안전 영역 클램핑 + · start_state = current + · pose 기반 goal 설정 (EE_LINK) + - 그 다음 plan_parameters 유무에 따라 plan() 호출 후 execute + """ + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + + planning_component.set_start_state_to_current_state() + planning_component.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link=EE_LINK, + ) + + logger.info("Planning trajectory") + + if plan_parameters is not None: + plan_result = planning_component.plan( + parameters=plan_parameters + ) + else: + plan_result = planning_component.plan() + + if not plan_result: + logger.error("Planning failed") + return False + + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + robot.execute( + group_name=GROUP_NAME, + robot_trajectory=robot_trajectory, + blocking=True, + ) + logger.info("Execution finished") + return True + + +def main(args=None): + # ================================ + # MoveItPy 인스턴스 생성 + # ================================ + rclpy.init(args=args) + logger = get_logger("m0609.moveit_py.waypoint_pilz") + + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + logger.info("MoveItPy instance created") + # ================================ + # 플래닝 파라미터 + # ================================ + home_params = PlanRequestParameters(robot) + pilz_params = PlanRequestParameters(robot) + + # MoveIt2 config의 planning_pipelines.pipeline_names와 일치해야 함 + home_params.planning_pipeline = "ompl" + home_params.planner_id = "RRTConnectkConfigDefault" + + pilz_params.planning_pipeline = "pilz_industrial_motion_planner" + pilz_params.planner_id = "PTP" + + # HOME: OMPL + home_params.max_velocity_scaling_factor = 0.2 + home_params.max_acceleration_scaling_factor = 0.1 + home_params.planning_time = 2.0 + + # Waypoint: Pilz PTP + pilz_params.max_velocity_scaling_factor = 0.15 + pilz_params.max_acceleration_scaling_factor = 0.1 + pilz_params.planning_time = 2.0 + + logger.info("=== Move to HOME joints (OMPL + slow) ===") + + # HOME 이동 (조인트 목표 → pose_goal 없이, plan_parameters만 사용) + arm.set_start_state_to_current_state() + arm.set_goal_state( + motion_plan_constraints=build_home_joint_constraints() + ) + plan_and_execute( + robot, + arm, + logger, + plan_parameters=home_params, + ) + + # ================================ + # Waypoints (Pilz PTP, 안전영역 포함) + # ================================ + logger.info("=== Waypoints from HOME, orientation fixed (Pilz PTP) ===") + + WAYPOINTS = [ + { # waypoint 1 + "pos": {"x": 0.493, "y": 0.010, "z": 0.417}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + { # waypoint 2 + "pos": {"x": 0.493, "y": -0.218, "z": 0.417}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + { # waypoint 3 + "pos": {"x": 0.371, "y": -0.218, "z": 0.419}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + ] + + pose_goal = PoseStamped() + pose_goal.header.frame_id = BASE_FRAME + + for i, wp in enumerate(WAYPOINTS, start=1): + pos = wp["pos"] + ori = wp["ori"] + + logger.info( + f"--- Waypoint {i}: " + f"x={pos['x']:.3f}, y={pos['y']:.3f}, z={pos['z']:.3f} ---" + ) + + # pose_goal에 의도 좌표/자세 설정 + pose_goal.pose.position.x = pos["x"] + pose_goal.pose.position.y = pos["y"] + pose_goal.pose.position.z = pos["z"] + + pose_goal.pose.orientation.x = ori["x"] + pose_goal.pose.orientation.y = ori["y"] + pose_goal.pose.orientation.z = ori["z"] + pose_goal.pose.orientation.w = ori["w"] + + # plan_and_execute 안에서 안전영역 + goal 설정 + Pilz 플래너 호출 + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=pilz_params, + ) + + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/azas_motion/azas_motion/mp_waypoint_pilz_lin_legacy.py b/src/azas_motion/azas_motion/mp_waypoint_pilz_lin_legacy.py new file mode 100644 index 0000000..5863d36 --- /dev/null +++ b/src/azas_motion/azas_motion/mp_waypoint_pilz_lin_legacy.py @@ -0,0 +1,228 @@ +#!/usr/bin/env python3 +import math + +import rclpy +from rclpy.logging import get_logger + +from geometry_msgs.msg import PoseStamped +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters + +# ================================ +# 기본 설정 +# ================================ +GROUP_NAME = "manipulator" # SRDF에 정의된 planning group 이름 +BASE_FRAME = "base_link" # 로봇 베이스 프레임 +EE_LINK = "link_6" # 엔드이펙터 링크 이름 (SRDF/URDF 기준) + +HOME_JOINTS_DEG = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0] +HOME_JOINTS_RAD = [math.radians(d) for d in HOME_JOINTS_DEG] + +# ====== 안전 작업 영역 정의 (base_link 기준) ====== +SAFE_X_MIN = 0.0 # x는 0 이상 +SAFE_Y_MIN = -0.3 # y 하한 +SAFE_Y_MAX = 0.3 # y 상한 +SAFE_Z_MIN = 0.27 # z는 이 값보다 낮아지면 안 됨 +# ================================================== + + +def clamp_to_safe_workspace(x: float, y: float, z: float, logger): + """안전 작업 영역으로 (x, y, z) 클램핑""" + safe_x = x + safe_y = y + safe_z = z + + if safe_x < SAFE_X_MIN: + logger.warning( + f"Requested x ({safe_x:.3f} m) is below safety limit " + f"({SAFE_X_MIN:.3f} m). Clamping to SAFE_X_MIN." + ) + safe_x = SAFE_X_MIN + + if safe_y < SAFE_Y_MIN: + logger.warning( + f"Requested y ({safe_y:.3f} m) is below safety limit " + f"({SAFE_Y_MIN:.3f} m). Clamping to SAFE_Y_MIN." + ) + safe_y = SAFE_Y_MIN + elif safe_y > SAFE_Y_MAX: + logger.warning( + f"Requested y ({safe_y:.3f} m) is above safety limit " + f"({SAFE_Y_MAX:.3f} m). Clamping to SAFE_Y_MAX." + ) + safe_y = SAFE_Y_MAX + + if safe_z < SAFE_Z_MIN: + logger.warning( + f"Requested z ({safe_z:.3f} m) is below safety limit " + f"({SAFE_Z_MIN:.3f} m). Clamping to SAFE_Z_MIN." + ) + safe_z = SAFE_Z_MIN + + return safe_x, safe_y, safe_z + + +def plan_and_execute( + robot: MoveItPy, + planning_component, + logger, + pose_goal: PoseStamped = None, + plan_parameters=None, +): + """ + 공식 문서 스타일 helper: 계획 후 곧바로 실행 + + - pose_goal이 주어지면: + · 안전 영역 클램핑 + · start_state = current + · pose 기반 goal 설정 (EE_LINK) + - 그 다음 plan_parameters 유무에 따라 plan() 호출 후 execute + """ + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + + planning_component.set_start_state_to_current_state() + planning_component.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link=EE_LINK, + ) + + logger.info("Planning trajectory") + + if plan_parameters is not None: + plan_result = planning_component.plan( + parameters=plan_parameters + ) + else: + plan_result = planning_component.plan() + + if not plan_result: + logger.error("Planning failed") + return False + + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + robot.execute( + group_name=GROUP_NAME, + robot_trajectory=robot_trajectory, + blocking=True, + ) + logger.info("Execution finished") + return True + + +def main(args=None): + # ================================ + # MoveItPy 인스턴스 생성 + # ================================ + rclpy.init(args=args) + logger = get_logger("m0609.moveit_py.waypoint_pilz") + + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + logger.info("MoveItPy instance created") + robot_model = robot.get_robot_model() + + # ================================ + # 플래닝 파라미터 + # ================================ + home_params = PlanRequestParameters(robot) + pilz_params = PlanRequestParameters(robot) + + # MoveIt2 config의 planning_pipelines.pipeline_names와 일치해야 함 + home_params.planning_pipeline = "ompl" + home_params.planner_id = "RRTConnect" + + pilz_params.planning_pipeline = "pilz_industrial_motion_planner" + pilz_params.planner_id = "LIN" + + # HOME: OMPL + home_params.max_velocity_scaling_factor = 0.2 + home_params.max_acceleration_scaling_factor = 0.1 + home_params.planning_time = 2.0 + + # Waypoint: Pilz LIN + pilz_params.max_velocity_scaling_factor = 0.05 # 기존 0.10 -> 0.05 + pilz_params.max_acceleration_scaling_factor = 0.03 # 기존 0.10 -> 0.03 + pilz_params.planning_time = 2.0 + + logger.info("=== Move to HOME joints (OMPL + slow) ===") + + home_state = RobotState(robot_model) + home_state.set_joint_group_positions(GROUP_NAME, HOME_JOINTS_RAD) + home_state.update() + + # HOME 이동 (조인트 목표 → pose_goal 없이, plan_parameters만 사용) + arm.set_start_state_to_current_state() + arm.set_goal_state(robot_state=home_state) + plan_and_execute( + robot, + arm, + logger, + plan_parameters=home_params, + ) + + # ================================ + # Waypoints (Pilz LIN, 안전영역 포함) + # ================================ + logger.info("=== Waypoints from HOME, orientation fixed (Pilz LIN) ===") + + WAYPOINTS = [ + { # waypoint 1 (높게 시작) + "pos": {"x": 0.45, "y": 0.22, "z": 0.52}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + { # waypoint 2 (대각선으로 내려가며 이동) + "pos": {"x": 0.62, "y": -0.18, "z": 0.33}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + { # waypoint 3 (다시 대각선으로 올라가며 이동) + "pos": {"x": 0.36, "y": -0.26, "z": 0.55}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.001, "w": -0.009}, + }, + ] + + pose_goal = PoseStamped() + pose_goal.header.frame_id = BASE_FRAME + + for i, wp in enumerate(WAYPOINTS, start=1): + pos = wp["pos"] + ori = wp["ori"] + + logger.info( + f"--- Waypoint {i}: " + f"x={pos['x']:.3f}, y={pos['y']:.3f}, z={pos['z']:.3f} ---" + ) + + # pose_goal에 의도 좌표/자세 설정 + pose_goal.pose.position.x = pos["x"] + pose_goal.pose.position.y = pos["y"] + pose_goal.pose.position.z = pos["z"] + + pose_goal.pose.orientation.x = ori["x"] + pose_goal.pose.orientation.y = ori["y"] + pose_goal.pose.orientation.z = ori["z"] + pose_goal.pose.orientation.w = ori["w"] + + # plan_and_execute 안에서 안전영역 + goal 설정 + Pilz 플래너 호출 + plan_and_execute( + robot, + arm, + logger, + pose_goal=pose_goal, + plan_parameters=pilz_params, + ) + + robot.shutdown() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/azas_motion/azas_motion/syrup_pump_press_legacy.py b/src/azas_motion/azas_motion/syrup_pump_press_legacy.py new file mode 100644 index 0000000..ac0ea2d --- /dev/null +++ b/src/azas_motion/azas_motion/syrup_pump_press_legacy.py @@ -0,0 +1,227 @@ +#!/usr/bin/env python3 + +import math +import time + +import rclpy +from geometry_msgs.msg import PoseStamped +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters +from rclpy.node import Node + +from azas_gripper.onrobot import RG + + +GROUP_NAME = "manipulator" +BASE_FRAME = "base_link" +EE_LINK = "link_6" + +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 +GRIPPER_CLOSE_WIDTH = 0 +GRIPPER_FORCE = 200 + +HOME_JOINTS_DEG = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0] +HOME_JOINTS_RAD = [math.radians(d) for d in HOME_JOINTS_DEG] + +DOWN_ORI = { + "x": 0.0, + "y": 1.0, + "z": 0.0, + "w": 0.0, +} + + +class SyrupPumpPressNode(Node): + def __init__(self): + super().__init__("syrup_pump_press") + + self.declare_parameter("pump_x", 0.45) + self.declare_parameter("pump_y", 0.0) + self.declare_parameter("start_z", 0.50) + self.declare_parameter("pump_top_z", 0.35) + self.declare_parameter("press_depth", 0.05) + self.declare_parameter("hold_sec", 0.5) + self.declare_parameter("go_home_first", True) + self.declare_parameter("return_home", False) + + self.pump_x = self.get_parameter("pump_x").value + self.pump_y = self.get_parameter("pump_y").value + self.start_z = self.get_parameter("start_z").value + self.pump_top_z = self.get_parameter("pump_top_z").value + self.press_depth = self.get_parameter("press_depth").value + self.hold_sec = self.get_parameter("hold_sec").value + self.go_home_first = self.get_parameter("go_home_first").value + self.return_home = self.get_parameter("return_home").value + + self.press_z = self.pump_top_z - self.press_depth + + self.robot = MoveItPy(node_name="syrup_pump_press_moveit_py") + self.arm = self.robot.get_planning_component(GROUP_NAME) + self.robot_model = self.robot.get_robot_model() + + self.home_params = PlanRequestParameters(self.robot) + self.home_params.planning_pipeline = "ompl" + self.home_params.planner_id = "RRTConnectkConfigDefault" + self.home_params.max_velocity_scaling_factor = 0.2 + self.home_params.max_acceleration_scaling_factor = 0.1 + self.home_params.planning_time = 3.0 + + self.ptp_params = PlanRequestParameters(self.robot) + self.ptp_params.planning_pipeline = "pilz_industrial_motion_planner" + self.ptp_params.planner_id = "PTP" + self.ptp_params.max_velocity_scaling_factor = 0.15 + self.ptp_params.max_acceleration_scaling_factor = 0.1 + self.ptp_params.planning_time = 3.0 + + self.lin_params = PlanRequestParameters(self.robot) + self.lin_params.planning_pipeline = "pilz_industrial_motion_planner" + self.lin_params.planner_id = "LIN" + self.lin_params.max_velocity_scaling_factor = 0.08 + self.lin_params.max_acceleration_scaling_factor = 0.05 + self.lin_params.planning_time = 3.0 + + self.press_params = PlanRequestParameters(self.robot) + self.press_params.planning_pipeline = "pilz_industrial_motion_planner" + self.press_params.planner_id = "LIN" + self.press_params.max_velocity_scaling_factor = 0.02 + self.press_params.max_acceleration_scaling_factor = 0.02 + self.press_params.planning_time = 3.0 + + self.gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + time.sleep(0.5) + + def make_pose(self, x, y, z, ori=None): + if ori is None: + ori = DOWN_ORI + + pose = PoseStamped() + pose.header.frame_id = BASE_FRAME + pose.pose.position.x = float(x) + pose.pose.position.y = float(y) + pose.pose.position.z = float(z) + pose.pose.orientation.x = ori["x"] + pose.pose.orientation.y = ori["y"] + pose.pose.orientation.z = ori["z"] + pose.pose.orientation.w = ori["w"] + return pose + + def plan_and_execute(self, pose_goal=None, state_goal=None, params=None): + log = self.get_logger() + self.arm.set_start_state_to_current_state() + + if pose_goal is not None: + self.arm.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link=EE_LINK, + ) + elif state_goal is not None: + self.arm.set_goal_state(robot_state=state_goal) + else: + log.error("No pose/state goal was provided.") + return False + + plan_result = self.arm.plan(parameters=params) if params else self.arm.plan() + if not plan_result: + log.error("Planning failed.") + return False + + self.robot.execute( + group_name=GROUP_NAME, + robot_trajectory=plan_result.trajectory, + blocking=True, + ) + return True + + def move_home(self): + home_state = RobotState(self.robot_model) + home_state.set_joint_group_positions(GROUP_NAME, HOME_JOINTS_RAD) + home_state.update() + return self.plan_and_execute(state_goal=home_state, params=self.home_params) + + def run_task(self): + log = self.get_logger() + log.info("=== Syrup pump press task start ===") + log.info( + "Target pump pose: " + f"x={self.pump_x:.3f}, y={self.pump_y:.3f}, " + f"start_z={self.start_z:.3f}, pump_top_z={self.pump_top_z:.3f}, " + f"press_z={self.press_z:.3f}" + ) + + if self.press_z <= 0.0: + log.error("Invalid press_z. Check pump_top_z and press_depth.") + return False + + if self.go_home_first: + log.info("[0] Move HOME") + if not self.move_home(): + return False + + log.info("[1] Close gripper fully") + self.gripper.move_gripper( + width_val=GRIPPER_CLOSE_WIDTH, + force_val=GRIPPER_FORCE, + ) + time.sleep(1.0) + + log.info("[2] Move above syrup pump") + if not self.plan_and_execute( + pose_goal=self.make_pose(self.pump_x, self.pump_y, self.start_z), + params=self.ptp_params, + ): + return False + + log.info("[3] Move vertically down to pump top height") + if not self.plan_and_execute( + pose_goal=self.make_pose(self.pump_x, self.pump_y, self.pump_top_z), + params=self.lin_params, + ): + return False + + log.info("[4] Slowly press syrup pump by 5 cm") + if not self.plan_and_execute( + pose_goal=self.make_pose(self.pump_x, self.pump_y, self.press_z), + params=self.press_params, + ): + return False + + if self.hold_sec > 0.0: + log.info(f"[5] Hold for {self.hold_sec:.2f} sec") + time.sleep(self.hold_sec) + + log.info("[6] Retract vertically") + if not self.plan_and_execute( + pose_goal=self.make_pose(self.pump_x, self.pump_y, self.start_z), + params=self.lin_params, + ): + return False + + if self.return_home: + log.info("[7] Return HOME") + if not self.move_home(): + return False + + log.info("=== Syrup pump press task finished ===") + return True + + def destroy_node(self): + try: + self.gripper.close_connection() + finally: + super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + node = SyrupPumpPressNode() + try: + node.run_task() + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/azas_motion/setup.py b/src/azas_motion/setup.py index 4bbcdf9..d99d598 100644 --- a/src/azas_motion/setup.py +++ b/src/azas_motion/setup.py @@ -22,9 +22,14 @@ "alignment_executor_node = azas_motion.alignment_executor_node:main", "collision_obstacle_legacy = azas_motion.collision_obstacle_legacy:main", "dispenser_sequence_preview_node = azas_motion.dispenser_sequence_preview_node:main", + "gear_assembly_legacy = azas_motion.gear_assembly_legacy:main", "mp_basic_legacy = azas_motion.mp_basic_legacy:main", + "mp_waypoint_legacy = azas_motion.mp_waypoint_legacy:main", + "mp_waypoint_pilz_legacy = azas_motion.mp_waypoint_pilz_legacy:main", + "mp_waypoint_pilz_lin_legacy = azas_motion.mp_waypoint_pilz_lin_legacy:main", "pick_and_place_legacy = azas_motion.pick_and_place_legacy:main", "side_grasp_ik_preview_node = azas_motion.side_grasp_ik_preview_node:main", + "syrup_pump_press_legacy = azas_motion.syrup_pump_press_legacy:main", ], }, ) diff --git a/src/azas_perception/azas_perception/bar_detect_test_legacy_node.py b/src/azas_perception/azas_perception/bar_detect_test_legacy_node.py new file mode 100644 index 0000000..dacfa6c --- /dev/null +++ b/src/azas_perception/azas_perception/bar_detect_test_legacy_node.py @@ -0,0 +1,116 @@ +#!/usr/bin/env python3 +""" +bar_detect_test.py – 마우스 호버 depth 표시 (시각화 전용) + +RealSense 영상 위에서 마우스 커서 위치의 depth(mm)를 실시간으로 표시. +ESC 로 종료. +""" + +import cv2 +import numpy as np +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import Image +from cv_bridge import CvBridge + + +class BarDetectTest(Node): + def __init__(self): + super().__init__("bar_detect_test") + self.bridge = CvBridge() + self.color_image = None + self.depth_image = None + self.mouse_xy = None # (x, y) 커서 좌표 + + self.create_subscription( + Image, "/camera/camera/color/image_raw", + self._color_cb, 10) + self.create_subscription( + Image, "/camera/camera/aligned_depth_to_color/image_raw", + self._depth_cb, 10) + + def _color_cb(self, msg): + self.color_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") + + def _depth_cb(self, msg): + self.depth_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough") + + # ── 마우스 콜백 ── + def _mouse_cb(self, event, x, y, flags, param): + if event == cv2.EVENT_MOUSEMOVE: + self.mouse_xy = (x, y) + + # ── depth 샘플링 (5×5 median, mm) ── + def sample_depth_mm(self, px, py): + if self.depth_image is None: + return None + h, w = self.depth_image.shape[:2] + if not (0 <= px < w and 0 <= py < h): + return None + x0, x1 = max(0, px - 2), min(w, px + 3) + y0, y1 = max(0, py - 2), min(h, py + 3) + patch = self.depth_image[y0:y1, x0:x1] + valid = patch[patch > 0] + if valid.size == 0: + return None + z = float(np.median(valid)) + if self.depth_image.dtype != np.uint16: + z *= 1000.0 # m → mm + return z + + # ── 메인 ── + def run(self): + window = "DepthHover" + cv2.namedWindow(window) + cv2.setMouseCallback(window, self._mouse_cb) + + while rclpy.ok(): + rclpy.spin_once(self, timeout_sec=0.01) + if self.color_image is None: + continue + + vis = self.color_image.copy() + + if self.mouse_xy is not None: + mx, my = self.mouse_xy + # 십자선 + cv2.drawMarker(vis, (mx, my), (0, 255, 255), + markerType=cv2.MARKER_CROSS, + markerSize=20, thickness=1) + # depth + d = self.sample_depth_mm(mx, my) + if d is None: + text = f"({mx},{my}) d=?" + else: + text = f"({mx},{my}) d={d:.0f}mm" + # 좌상단 상태바 + cv2.putText(vis, text, (10, 25), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, + (255, 255, 255), 2) + # 커서 옆 + cv2.putText(vis, + "?" if d is None else f"{d:.0f}", + (mx + 10, my - 10), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, + (0, 255, 255), 2) + + cv2.imshow(window, vis) + if (cv2.waitKey(1) & 0xFF) == 27: + break + + cv2.destroyAllWindows() + + +def main(args=None): + rclpy.init(args=args) + node = BarDetectTest() + try: + node.run() + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/azas_perception/azas_perception/bar_sort_legacy_node.py b/src/azas_perception/azas_perception/bar_sort_legacy_node.py new file mode 100644 index 0000000..5671519 --- /dev/null +++ b/src/azas_perception/azas_perception/bar_sort_legacy_node.py @@ -0,0 +1,566 @@ +#!/usr/bin/env python3 +""" +bar_sort_node.py – 3 bars 크기순 자동 Pick & Place + +click_pick_node.py 기반: +- 카메라로 bar 3개 자동 검출 (OpenCV contour) +- 길이(minAreaRect 장변) 순으로 정렬 +- 미리 정의된 3개 위치에 작은→큰 순서로 배치 +""" + +import math +import time +from pathlib import Path + +import cv2 +import numpy as np +import rclpy +from rclpy.node import Node + +from scipy.spatial.transform import Rotation +from ament_index_python.packages import get_package_share_directory + +from geometry_msgs.msg import PoseStamped +from sensor_msgs.msg import Image, CameraInfo +from cv_bridge import CvBridge + +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters + +from azas_gripper.onrobot import RG + + +# ═══════════════════════════════════════════ +# 설정 +# ═══════════════════════════════════════════ +GROUP_NAME = "manipulator" +BASE_FRAME = "base_link" +EE_LINK = "link_6" + +HOME_JOINTS = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(90.0), +} + +# 안전 작업 영역 (m, base_link) +SAFE_X_MIN = 0.0 +SAFE_Y_MIN = -0.40 +SAFE_Y_MAX = 0.40 +SAFE_Z_MIN = 0.25 + +# Pick/Place 파라미터 (m) +Z_OFFSET = 0.20 # click_pick_node와 동일 +SAFE_Z = 0.40 + +# Place 위치 : x=0 에 1열 (작은 → 큰, base_link) +PLACE_POSITIONS = [ + (0.0, -0.20, 0.20), # 0 : 가장 작은 bar + (0.0, 0.00, 0.20), # 1 : 중간 + (0.0, 0.20, 0.20), # 2 : 가장 큰 bar +] + +# 시작 후 카메라 안정화를 위한 대기 시간 (초) +CAMERA_WARMUP_SEC = 3.0 + +# Bar 검출 파라미터 +MIN_CONTOUR_AREA_PX = 100 # 픽셀 노이즈 컷 + +# Depth(mm) 기반 bar 분류 +# (d_min, d_max, 이름, rank) rank 0=가장 작음, 2=가장 큼 +# LONG : 308~311 → 305~315 +# MEDIUM : 318~321 → 315~325 +# SHORT : 328~331 → 325~335 +BAR_CLASSES = [ + (325, 335, "SHORT", 0), + (315, 325, "MEDIUM", 1), + (305, 315, "LONG", 2), +] + + +def classify_depth_mm(d_mm): + """depth(mm) → (이름, rank) / 해당 없으면 (None, -1)""" + for d_min, d_max, name, rank in BAR_CLASSES: + if d_min <= d_mm < d_max: + return name, rank + return None, -1 + +# 그리퍼 +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 + +DOWN_ORI = {"x": 0.0, "y": 1.0, "z": 0.0, "w": 0.0} + + +# ═══════════════════════════════════════════ +# 유틸 +# ═══════════════════════════════════════════ +def clamp_to_safe_workspace(x, y, z, logger): + if x < SAFE_X_MIN: + logger.warning(f"x={x:.3f} → {SAFE_X_MIN}") + x = SAFE_X_MIN + if y < SAFE_Y_MIN: + logger.warning(f"y={y:.3f} → {SAFE_Y_MIN}") + y = SAFE_Y_MIN + elif y > SAFE_Y_MAX: + logger.warning(f"y={y:.3f} → {SAFE_Y_MAX}") + y = SAFE_Y_MAX + if z < SAFE_Z_MIN: + logger.warning(f"z={z:.3f} → {SAFE_Z_MIN}") + z = SAFE_Z_MIN + return x, y, z + + +def plan_and_execute(robot, arm, logger, pose_goal=None, + state_goal=None, params=None): + arm.set_start_state_to_current_state() + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link=EE_LINK) + elif state_goal is not None: + arm.set_goal_state(robot_state=state_goal) + else: + logger.error("pose/state 없음") + return False + + plan_result = (arm.plan(parameters=params) + if params is not None else arm.plan()) + if not plan_result: + logger.error("Planning 실패") + return False + + robot.execute(group_name=GROUP_NAME, + robot_trajectory=plan_result.trajectory, + blocking=True) + return True + + +def make_pose(x, y, z, ori=None): + if ori is None: + ori = DOWN_ORI + p = PoseStamped() + p.header.frame_id = BASE_FRAME + p.pose.position.x = float(x) + p.pose.position.y = float(y) + p.pose.position.z = float(z) + p.pose.orientation.x = ori["x"] + p.pose.orientation.y = ori["y"] + p.pose.orientation.z = ori["z"] + p.pose.orientation.w = ori["w"] + return p + + +def get_ee_matrix(moveit_robot): + psm = moveit_robot.get_planning_scene_monitor() + with psm.read_only() as scene: + T = scene.current_state.get_global_link_transform(EE_LINK) + return np.asarray(T, dtype=float) + + +def detect_bars(color_img, logger): + """ + bar 후보 검출: OTSU contour → 픽셀 장변(length). + 반환: [{'pixel':(cx,cy), 'length_px':..., 'rect':rect}, ...] + """ + gray = cv2.cvtColor(color_img, cv2.COLOR_BGR2GRAY) + blurred = cv2.GaussianBlur(gray, (5, 5), 0) + _, thresh = cv2.threshold( + blurred, 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU + ) + + kernel = np.ones((3, 3), np.uint8) + thresh = cv2.morphologyEx(thresh, cv2.MORPH_OPEN, kernel, iterations=1) + thresh = cv2.morphologyEx(thresh, cv2.MORPH_CLOSE, kernel, iterations=2) + + contours, _ = cv2.findContours( + thresh, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE + ) + + bars = [] + for c in contours: + if cv2.contourArea(c) < MIN_CONTOUR_AREA_PX: + continue + rect = cv2.minAreaRect(c) + (cx, cy), (rw, rh), _ = rect + bars.append({ + "pixel": (int(cx), int(cy)), + "length_px": float(max(rw, rh)), + "rect": rect, + }) + + logger.info(f"검출된 contour: {len(bars)} 개") + return bars, thresh + + +# ═══════════════════════════════════════════ +# BarSortNode +# ═══════════════════════════════════════════ +class BarSortNode(Node): + def __init__(self): + super().__init__("bar_sort_node") + self.bridge = CvBridge() + self.color_image = None + self.depth_image = None + self.intrinsics = None + + # Hand-Eye + calib_file = ( + Path(get_package_share_directory("azas_perception")) + / "config" / "T_gripper2camera.npy" + ) + self.gripper2cam = np.load(str(calib_file)).astype(float) + self.gripper2cam[:3, 3] /= 1000.0 # mm → m + self.get_logger().info(f"Hand-Eye 로드: {calib_file}") + + # 그리퍼 + self.gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + + # MoveIt + self.get_logger().info("MoveItPy 초기화 중…") + self.robot = MoveItPy(node_name="bar_sort_moveit_py") + self.arm = self.robot.get_planning_component(GROUP_NAME) + self.robot_model = self.robot.get_robot_model() + self.get_logger().info("MoveItPy 초기화 완료") + + # Plan 파라미터 + self.ompl_params = PlanRequestParameters(self.robot) + self.ompl_params.planning_pipeline = "ompl" + self.ompl_params.planner_id = "RRTConnect" + self.ompl_params.max_velocity_scaling_factor = 0.2 + self.ompl_params.max_acceleration_scaling_factor = 0.1 + self.ompl_params.planning_time = 2.0 + + self.pilz_params = PlanRequestParameters(self.robot) + self.pilz_params.planning_pipeline = "pilz_industrial_motion_planner" + self.pilz_params.planner_id = "PTP" + self.pilz_params.max_velocity_scaling_factor = 0.15 + self.pilz_params.max_acceleration_scaling_factor = 0.1 + self.pilz_params.planning_time = 2.0 + + self.home_xyz = None + self.home_ori = None + + # 구독 + self.create_subscription( + CameraInfo, "/camera/camera/color/camera_info", + self._cam_info_cb, 10) + self.create_subscription( + Image, "/camera/camera/color/image_raw", + self._color_cb, 10) + self.create_subscription( + Image, "/camera/camera/aligned_depth_to_color/image_raw", + self._depth_cb, 10) + + # ── 콜백 ── + def _cam_info_cb(self, msg): + self.intrinsics = { + "fx": msg.k[0], "fy": msg.k[4], + "ppx": msg.k[2], "ppy": msg.k[5], + } + + def _color_cb(self, msg): + self.color_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") + + def _depth_cb(self, msg): + self.depth_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough") + + # ── depth 샘플링 (5×5 median, mm) ── + def sample_depth_mm(self, px, py): + if self.depth_image is None: + return None + h, w = self.depth_image.shape[:2] + if not (0 <= px < w and 0 <= py < h): + return None + x0, x1 = max(0, px - 2), min(w, px + 3) + y0, y1 = max(0, py - 2), min(h, py + 3) + patch = self.depth_image[y0:y1, x0:x1] + valid = patch[patch > 0] + if valid.size == 0: + return None + z = float(np.median(valid)) + if self.depth_image.dtype != np.uint16: + z *= 1000.0 + return z + + # ── 좌표 변환 ── + def transform_to_base(self, cam_xyz_m): + coord = np.append(np.array(cam_xyz_m, dtype=float), 1.0) + base2ee = get_ee_matrix(self.robot) + base2cam = base2ee @ self.gripper2cam + return (base2cam @ coord)[:3] + + def pixel_to_base(self, px, py): + """(px,py) → base 좌표 (m). 주변 5x5 median으로 depth 안정화.""" + if self.depth_image is None or self.intrinsics is None: + return None + h, w = self.depth_image.shape[:2] + if not (0 <= px < w and 0 <= py < h): + return None + + x0, x1 = max(0, px - 2), min(w, px + 3) + y0, y1 = max(0, py - 2), min(h, py + 3) + patch = self.depth_image[y0:y1, x0:x1] + valid = patch[patch > 0] + if valid.size == 0: + return None + z_raw = float(np.median(valid)) + z_m = (z_raw / 1000.0 + if self.depth_image.dtype == np.uint16 else z_raw) + + fx, fy = self.intrinsics["fx"], self.intrinsics["fy"] + ppx, ppy = self.intrinsics["ppx"], self.intrinsics["ppy"] + cam_x = (px - ppx) * z_m / fx + cam_y = (py - ppy) * z_m / fy + return self.transform_to_base((cam_x, cam_y, z_m)) + + # ── Pick & Place ── + def pick_and_place(self, bx, by, bz, place_xyz): + """ + 1) 현재 z로 pick XY 이동 + 2) pick_z (= bz + Z_OFFSET) 하강 + 3) gripper close + 4) SAFE_Z 상승 + 5) place XY 이동 (SAFE_Z) + 6) place_z 하강 + 7) gripper open + 8) SAFE_Z 상승 + """ + log = self.get_logger() + ori = self.home_ori or DOWN_ORI + + pick_z = bz + Z_OFFSET + px, py, pz = place_xyz + place_z = max(pz, pick_z) # pick 높이 이상 보장 + + log.info( + f"Pick ({bx:.3f},{by:.3f},pick_z={pick_z:.3f}) → " + f"Place ({px:.3f},{py:.3f},{place_z:.3f})" + ) + + cur_ee = get_ee_matrix(self.robot) + cur_z = cur_ee[2, 3] + + # 0) gripper open + self.gripper.open_gripper() + time.sleep(0.5) + + # 1) pick XY + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(bx, by, cur_z, ori), + params=self.pilz_params): + log.error("[1] plan 실패"); return False + + # 2) pick_z 하강 + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(bx, by, pick_z, ori), + params=self.pilz_params): + log.error("[2] plan 실패"); return False + + # 3) close + log.info("[3] Gripper CLOSE") + self.gripper.close_gripper() + time.sleep(1.0) + + # 4) SAFE_Z 상승 + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(bx, by, SAFE_Z, ori), + params=self.pilz_params): + log.error("[4] plan 실패"); return False + + # 5) place XY (SAFE_Z) + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(px, py, SAFE_Z, ori), + params=self.pilz_params): + log.error("[5] plan 실패"); return False + + # 6) place_z 하강 + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(px, py, place_z, ori), + params=self.pilz_params): + log.error("[6] plan 실패"); return False + + # 7) open + log.info("[7] Gripper OPEN") + self.gripper.open_gripper() + time.sleep(1.0) + + # 8) SAFE_Z 상승 + plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(px, py, SAFE_Z, ori), + params=self.pilz_params) + return True + + # ── 정렬 시퀀스 ── + def sort_bars(self): + log = self.get_logger() + if (self.color_image is None or self.depth_image is None + or self.intrinsics is None): + log.error("이미지/내참 준비 안 됨"); return + + bars, thresh = detect_bars(self.color_image, log) + if not bars: + log.error("contour 없음 — 중단") + cv2.imshow("BarSort_Thresh", thresh) + cv2.waitKey(500) + return + + # 각 contour 중심의 depth 로 SHORT / MEDIUM / LONG 분류 + # rank당 후보가 여럿이면 contour 면적(length_px) 큰 것 선택 + classified = {} # rank -> target dict + for b in bars: + cx, cy = b["pixel"] + d_mm = self.sample_depth_mm(cx, cy) + if d_mm is None: + continue + name, rank = classify_depth_mm(d_mm) + if rank < 0: + log.info(f"skip: d={d_mm:.0f}mm 범위 밖 @ ({cx},{cy})") + continue + cand = { + "pixel": (cx, cy), + "length_px": b["length_px"], + "depth_mm": d_mm, + "name": name, + "rank": rank, + } + if rank not in classified \ + or cand["length_px"] > classified[rank]["length_px"]: + classified[rank] = cand + + missing = [r for r in (0, 1, 2) if r not in classified] + if missing: + names = ["SHORT", "MEDIUM", "LONG"] + log.error(f"분류 부족: 누락 {[names[r] for r in missing]}") + return + + # 시각화 + vis = self.color_image.copy() + colors = {0: (0, 255, 0), 1: (0, 200, 255), 2: (0, 0, 255)} + for rank in (0, 1, 2): + t = classified[rank] + cx, cy = t["pixel"] + col = colors[rank] + cv2.circle(vis, (cx, cy), 10, col, 2) + cv2.putText(vis, + f"{t['name']} d={t['depth_mm']:.0f}", + (cx + 12, cy), cv2.FONT_HERSHEY_SIMPLEX, + 0.5, col, 2) + cv2.imshow("BarSort", vis) + cv2.waitKey(500) + + log.info("─── 분류 결과 ───") + for rank in (0, 1, 2): + t = classified[rank] + log.info( + f"[{rank}] {t['name']:6s} " + f"d={t['depth_mm']:.0f}mm pixel={t['pixel']}" + ) + + # Pick & Place (SHORT(0) → MEDIUM(1) → LONG(2)) + for rank in (0, 1, 2): + t = classified[rank] + cx, cy = t["pixel"] + base = self.pixel_to_base(cx, cy) + if base is None: + log.error(f"{t['name']} 좌표 변환 실패 — 중단") + return + bx, by, bz = float(base[0]), float(base[1]), float(base[2]) + log.info( + f"═══ {t['name']} (d={t['depth_mm']:.0f}mm, " + f"base z={bz:.3f}) ═══" + ) + ok = self.pick_and_place(bx, by, bz, PLACE_POSITIONS[rank]) + if not ok: + log.error(f"{t['name']} 실패 — 시퀀스 중단") + return + time.sleep(0.5) + + log.info("========== 정렬 완료 ==========") + + # ── 메인 ── + def run(self): + log = self.get_logger() + window = "BarSort" + cv2.namedWindow(window) + + # Home 이동 + log.info("[Init] Home 이동") + home_state = RobotState(self.robot_model) + home_state.joint_positions = HOME_JOINTS + home_state.update() + if not plan_and_execute(self.robot, self.arm, log, + state_goal=home_state, + params=self.ompl_params): + log.error("Home 이동 실패 — 종료") + return + time.sleep(0.5) + + # Home pose 저장 + T = get_ee_matrix(self.robot) + self.home_xyz = (T[0, 3], T[1, 3], T[2, 3]) + qx, qy, qz, qw = Rotation.from_matrix(T[:3, :3]).as_quat() + self.home_ori = {"x": float(qx), "y": float(qy), + "z": float(qz), "w": float(qw)} + log.info(f"[Init] Home = ({T[0,3]:.3f},{T[1,3]:.3f},{T[2,3]:.3f})") + + self.gripper.open_gripper() + time.sleep(1.0) + + # 프레임 수신 대기 + log.info("프레임 수신 대기…") + t0 = time.time() + while rclpy.ok() and (self.color_image is None + or self.depth_image is None + or self.intrinsics is None): + rclpy.spin_once(self, timeout_sec=0.1) + if time.time() - t0 > 10.0: + log.error("타임아웃"); return + + # 카메라 안정화 대기 (spin 계속 돌려 최신 프레임 수신) + log.info(f"카메라 안정화 {CAMERA_WARMUP_SEC:.1f}초 대기…") + t0 = time.time() + while rclpy.ok() and (time.time() - t0) < CAMERA_WARMUP_SEC: + rclpy.spin_once(self, timeout_sec=0.05) + if self.color_image is not None: + cv2.imshow(window, self.color_image) + cv2.waitKey(1) + + # 자동 실행 + log.info("정렬 시퀀스 시작") + self.sort_bars() + + # 완료 후 이미지만 띄워두고 ESC 대기 + log.info("완료. ESC 로 종료") + while rclpy.ok(): + rclpy.spin_once(self, timeout_sec=0.01) + if self.color_image is None: + continue + cv2.imshow(window, self.color_image) + if (cv2.waitKey(1) & 0xFF) == 27: + break + + cv2.destroyAllWindows() + + +def main(args=None): + rclpy.init(args=args) + node = BarSortNode() + try: + node.run() + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/azas_perception/azas_perception/click_pick_legacy_node.py b/src/azas_perception/azas_perception/click_pick_legacy_node.py new file mode 100644 index 0000000..2970a62 --- /dev/null +++ b/src/azas_perception/azas_perception/click_pick_legacy_node.py @@ -0,0 +1,408 @@ +#!/usr/bin/env python3 +""" +click_pick_node.py – 카메라 클릭 → Pick & Place (MoveIt 기반) + +gear_assembly.py 구조 + test.py 시퀀스를 MoveIt/m 단위로 통합. +""" + +import math +import time +from pathlib import Path + +import cv2 +import numpy as np +import rclpy +from rclpy.node import Node +from rclpy.logging import get_logger + +from scipy.spatial.transform import Rotation +from ament_index_python.packages import get_package_share_directory + +from geometry_msgs.msg import PoseStamped +from sensor_msgs.msg import Image, CameraInfo +from cv_bridge import CvBridge + +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters + +from azas_gripper.onrobot import RG + + +# ═══════════════════════════════════════════ +# 설정 +# ═══════════════════════════════════════════ +GROUP_NAME = "manipulator" +BASE_FRAME = "base_link" +EE_LINK = "link_6" + +HOME_JOINTS = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(90.0), +} + +# 안전 작업 영역 (m, base_link 기준) +SAFE_X_MIN = 0.0 +SAFE_Y_MIN = -0.30 +SAFE_Y_MAX = 0.30 +SAFE_Z_MIN = 0.25 + +# Pick/Place 파라미터 (m) +Z_OFFSET = 0.20 # base_z에 더할 오프셋 (test.py의 200mm와 동일) +SAFE_Z = 0.40 # 안전 이동 높이 +APPROACH_OFFSET = 0.05 # pick/place 위에서 접근 거리 + +# 그리퍼 +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 + +# TCP 아래로 향한 자세 +DOWN_ORI = {"x": 0.0, "y": 1.0, "z": 0.0, "w": 0.0} + + +# ═══════════════════════════════════════════ +# 유틸 함수 (gear_assembly 스타일) +# ═══════════════════════════════════════════ +def clamp_to_safe_workspace(x, y, z, logger): + if x < SAFE_X_MIN: + logger.warning(f"x={x:.3f} → {SAFE_X_MIN}") + x = SAFE_X_MIN + if y < SAFE_Y_MIN: + logger.warning(f"y={y:.3f} → {SAFE_Y_MIN}") + y = SAFE_Y_MIN + elif y > SAFE_Y_MAX: + logger.warning(f"y={y:.3f} → {SAFE_Y_MAX}") + y = SAFE_Y_MAX + if z < SAFE_Z_MIN: + logger.warning(f"z={z:.3f} → {SAFE_Z_MIN}") + z = SAFE_Z_MIN + return x, y, z + + +def plan_and_execute(robot, arm, logger, pose_goal=None, + state_goal=None, params=None): + """plan 후 execute. 실패 시 False 반환.""" + arm.set_start_state_to_current_state() + + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link=EE_LINK) + elif state_goal is not None: + arm.set_goal_state(robot_state=state_goal) + else: + logger.error("pose/state 없음") + return False + + plan_result = (arm.plan(parameters=params) + if params is not None else arm.plan()) + if not plan_result: + logger.error("Planning 실패") + return False + + robot.execute(group_name=GROUP_NAME, + robot_trajectory=plan_result.trajectory, + blocking=True) + return True + + +def make_pose(x, y, z, ori=None): + if ori is None: + ori = DOWN_ORI + p = PoseStamped() + p.header.frame_id = BASE_FRAME + p.pose.position.x = float(x) + p.pose.position.y = float(y) + p.pose.position.z = float(z) + p.pose.orientation.x = ori["x"] + p.pose.orientation.y = ori["y"] + p.pose.orientation.z = ori["z"] + p.pose.orientation.w = ori["w"] + return p + + +def get_ee_matrix(moveit_robot): + """MoveIt FK로 base_link → EE_LINK 4×4 행렬 (m).""" + psm = moveit_robot.get_planning_scene_monitor() + with psm.read_only() as scene: + T = scene.current_state.get_global_link_transform(EE_LINK) + return np.asarray(T, dtype=float) + + +# ═══════════════════════════════════════════ +# ClickPickNode +# ═══════════════════════════════════════════ +class ClickPickNode(Node): + def __init__(self): + super().__init__("click_pick_moveit_node") + self.bridge = CvBridge() + self.color_image = None + self.depth_image = None + self.intrinsics = None + self.picking = False # pick 중복 방지 + + # Hand-Eye 변환행렬 로드 + calib_file = ( + Path(get_package_share_directory("azas_perception")) + / "config" / "T_gripper2camera.npy" + ) + self.gripper2cam = np.load(str(calib_file)).astype(float) + self.gripper2cam[:3, 3] /= 1000.0 # mm → m + self.get_logger().info(f"Hand-Eye 로드: {calib_file}") + + # 그리퍼 + self.gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + + # MoveIt + self.get_logger().info("MoveItPy 초기화 중…") + self.robot = MoveItPy(node_name="click_pick_moveit_py") + self.arm = self.robot.get_planning_component(GROUP_NAME) + self.robot_model = self.robot.get_robot_model() + self.get_logger().info("MoveItPy 초기화 완료") + + # Plan 파라미터 + self.ompl_params = PlanRequestParameters(self.robot) + self.ompl_params.planning_pipeline = "ompl" + self.ompl_params.planner_id = "RRTConnect" + self.ompl_params.max_velocity_scaling_factor = 0.2 + self.ompl_params.max_acceleration_scaling_factor = 0.1 + self.ompl_params.planning_time = 2.0 + + self.pilz_params = PlanRequestParameters(self.robot) + self.pilz_params.planning_pipeline = "pilz_industrial_motion_planner" + self.pilz_params.planner_id = "PTP" + self.pilz_params.max_velocity_scaling_factor = 0.15 + self.pilz_params.max_acceleration_scaling_factor = 0.1 + self.pilz_params.planning_time = 2.0 + + # Home pose (run에서 설정) + self.home_xyz = None # (x, y, z) in m + self.home_ori = None # dict {x, y, z, w} + + # 구독 + self.create_subscription( + CameraInfo, "/camera/camera/color/camera_info", + self._cam_info_cb, 10) + self.create_subscription( + Image, "/camera/camera/color/image_raw", + self._color_cb, 10) + self.create_subscription( + Image, "/camera/camera/aligned_depth_to_color/image_raw", + self._depth_cb, 10) + + # ── 콜백 ── + def _cam_info_cb(self, msg): + self.intrinsics = { + "fx": msg.k[0], "fy": msg.k[4], + "ppx": msg.k[2], "ppy": msg.k[5], + } + + def _color_cb(self, msg): + self.color_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") + + def _depth_cb(self, msg): + self.depth_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough") + + # ── 좌표 변환 ── + def transform_to_base(self, cam_xyz_m): + """카메라 좌표 (m) → 베이스 좌표 (m).""" + coord = np.append(np.array(cam_xyz_m, dtype=float), 1.0) + base2ee = get_ee_matrix(self.robot) + base2cam = base2ee @ self.gripper2cam + return (base2cam @ coord)[:3] + + # ── Pick & Place (test.py 시퀀스를 MoveIt으로) ── + def pick_and_place(self, bx, by, bz): + """ + 1) 현재 z 유지하며 XY 이동 + 2) pick_z (= bz + Z_OFFSET) 로 하강 + 3) gripper close + 4) SAFE_Z로 상승 + 5) home XY로 이동 (SAFE_Z 유지) + 6) place_z로 하강 (pick_z 이상 보장) + 7) gripper open + 8) SAFE_Z로 상승 + """ + log = self.get_logger() + ori = self.home_ori or DOWN_ORI + + pick_z = bz + Z_OFFSET + place_z = max(pick_z, 0.25) # pick 높이 이상 보장 + + log.info(f"Base raw: ({bx:.3f}, {by:.3f}, {bz:.3f}) m") + log.info(f"Z_OFFSET={Z_OFFSET}, pick_z={pick_z:.3f}, place_z={place_z:.3f}") + + # 현재 EE 위치 + cur_ee = get_ee_matrix(self.robot) + cur_z = cur_ee[2, 3] + + hx, hy, hz = self.home_xyz + + # 0) gripper open + self.gripper.open_gripper() + time.sleep(0.5) + + # 1) 현재 z 유지하며 클릭 XY로 이동 + log.info(f"[1] XY → ({bx:.3f}, {by:.3f}) @ cur_z={cur_z:.3f}") + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(bx, by, cur_z, ori), + params=self.pilz_params): + log.error("pick 중단: [1] plan 실패"); return + + # 2) pick_z로 하강 + log.info(f"[2] down to pick_z={pick_z:.3f}") + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(bx, by, pick_z, ori), + params=self.pilz_params): + log.error("pick 중단: [2] plan 실패"); return + + # 3) gripper close + log.info("[3] Gripper CLOSE") + self.gripper.close_gripper() + time.sleep(1.0) + + # 4) SAFE_Z로 상승 + log.info(f"[4] up to SAFE_Z={SAFE_Z:.3f}") + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(bx, by, SAFE_Z, ori), + params=self.pilz_params): + log.error("pick 중단: [4] plan 실패"); return + + # 5) home XY로 이동 (SAFE_Z 유지) + log.info(f"[5] home XY → ({hx:.3f}, {hy:.3f}) @ SAFE_Z") + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(hx, hy, SAFE_Z, ori), + params=self.pilz_params): + log.error("pick 중단: [5] plan 실패"); return + + # 6) place_z로 하강 + log.info(f"[6] down to place_z={place_z:.3f}") + if not plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(hx, hy, place_z, ori), + params=self.pilz_params): + log.error("pick 중단: [6] plan 실패"); return + + # 7) gripper open + log.info("[7] Gripper OPEN") + self.gripper.open_gripper() + time.sleep(1.0) + + # 8) SAFE_Z로 상승 + log.info(f"[8] up to SAFE_Z={SAFE_Z:.3f}") + plan_and_execute(self.robot, self.arm, log, + pose_goal=make_pose(hx, hy, SAFE_Z, ori), + params=self.pilz_params) + + log.info("========== PICK END ==========") + + # ── 마우스 콜백 ── + def mouse_callback(self, event, x, y, flags, param): + if event != cv2.EVENT_LBUTTONDOWN: + return + if self.picking: + self.get_logger().warn("pick 동작 중 — 무시") + return + if self.color_image is None or self.depth_image is None or self.intrinsics is None: + self.get_logger().warn("프레임/내참 아직 준비 안 됨") + return + + h, w = self.depth_image.shape[:2] + if not (0 <= x < w and 0 <= y < h): + self.get_logger().warn("클릭 범위 초과") + return + + z_raw = self.depth_image[y, x] + if z_raw == 0: + self.get_logger().warn("해당 픽셀 depth=0") + return + + # depth → m + z_m = float(z_raw) / 1000.0 if self.depth_image.dtype == np.uint16 else float(z_raw) + + fx, fy = self.intrinsics["fx"], self.intrinsics["fy"] + ppx, ppy = self.intrinsics["ppx"], self.intrinsics["ppy"] + + cam_x = (x - ppx) * z_m / fx + cam_y = (y - ppy) * z_m / fy + cam_z = z_m + + base = self.transform_to_base((cam_x, cam_y, cam_z)) + if base is None: + self.get_logger().error("좌표 변환 실패") + return + + bx, by, bz = float(base[0]), float(base[1]), float(base[2]) + self.get_logger().info( + f"Camera: ({cam_x:.3f}, {cam_y:.3f}, {cam_z:.3f}) m → " + f"Base: ({bx:.3f}, {by:.3f}, {bz:.3f}) m" + ) + + self.picking = True + try: + self.pick_and_place(bx, by, bz) + finally: + self.picking = False + + # ── 메인 루프 ── + def run(self): + log = self.get_logger() + window = "ClickToPick (MoveIt)" + cv2.namedWindow(window) + cv2.setMouseCallback(window, self.mouse_callback) + + # Home 이동 + log.info("[Init] Home 이동") + home_state = RobotState(self.robot_model) + home_state.joint_positions = HOME_JOINTS + home_state.update() + if not plan_and_execute(self.robot, self.arm, log, + state_goal=home_state, + params=self.ompl_params): + log.error("Home 이동 실패 — 종료") + return + + time.sleep(0.5) + + # Home pose 저장 + T = get_ee_matrix(self.robot) + self.home_xyz = (T[0, 3], T[1, 3], T[2, 3]) + qx, qy, qz, qw = Rotation.from_matrix(T[:3, :3]).as_quat() + self.home_ori = {"x": float(qx), "y": float(qy), + "z": float(qz), "w": float(qw)} + log.info(f"[Init] Home = ({T[0,3]:.3f}, {T[1,3]:.3f}, {T[2,3]:.3f}) m") + + self.gripper.open_gripper() + time.sleep(1.0) + + while rclpy.ok(): + rclpy.spin_once(self, timeout_sec=0.01) + if self.color_image is None: + continue + cv2.imshow(window, self.color_image) + if (cv2.waitKey(1) & 0xFF) == 27: + break + + cv2.destroyAllWindows() + + +def main(args=None): + rclpy.init(args=args) + node = ClickPickNode() + try: + node.run() + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/azas_perception/azas_perception/realsense_data_collector_legacy_node.py b/src/azas_perception/azas_perception/realsense_data_collector_legacy_node.py new file mode 100644 index 0000000..03e47ba --- /dev/null +++ b/src/azas_perception/azas_perception/realsense_data_collector_legacy_node.py @@ -0,0 +1,213 @@ +import csv +import json +import os +from pathlib import Path + +import cv2 +import rclpy +from cv_bridge import CvBridge +from rclpy.node import Node +from sensor_msgs.msg import CameraInfo, Image + + +class RealSenseDataCollector(Node): + def __init__(self): + super().__init__("realsense_data_collector") + + self.declare_parameter("color_topic", "/camera/camera/color/image_raw") + self.declare_parameter( + "depth_topic", "/camera/camera/aligned_depth_to_color/image_raw" + ) + self.declare_parameter("camera_info_topic", "/camera/camera/color/camera_info") + self.declare_parameter( + "output_dir", "/home/ssu/ros2_ws/realsense_dataset/raw" + ) + self.declare_parameter("save_interval_sec", 1.0) + self.declare_parameter("max_frames", 0) + self.declare_parameter("save_depth", True) + self.declare_parameter("jpeg_quality", 95) + + self.color_topic = ( + self.get_parameter("color_topic").get_parameter_value().string_value + ) + self.depth_topic = ( + self.get_parameter("depth_topic").get_parameter_value().string_value + ) + self.camera_info_topic = ( + self.get_parameter("camera_info_topic").get_parameter_value().string_value + ) + self.output_dir = Path( + self.get_parameter("output_dir").get_parameter_value().string_value + ).expanduser() + self.save_interval_sec = ( + self.get_parameter("save_interval_sec").get_parameter_value().double_value + ) + self.max_frames = ( + self.get_parameter("max_frames").get_parameter_value().integer_value + ) + self.save_depth = ( + self.get_parameter("save_depth").get_parameter_value().bool_value + ) + self.jpeg_quality = ( + self.get_parameter("jpeg_quality").get_parameter_value().integer_value + ) + + self.bridge = CvBridge() + self.latest_color = None + self.latest_depth = None + self.latest_color_stamp = None + self.latest_depth_stamp = None + self.camera_info_saved = False + self.frame_index = 0 + + self.color_dir = self.output_dir / "color" + self.depth_dir = self.output_dir / "depth" + self.meta_dir = self.output_dir / "meta" + self.color_dir.mkdir(parents=True, exist_ok=True) + if self.save_depth: + self.depth_dir.mkdir(parents=True, exist_ok=True) + self.meta_dir.mkdir(parents=True, exist_ok=True) + + self.metadata_path = self.meta_dir / "frames.csv" + self._init_metadata_file() + + self.create_subscription(Image, self.color_topic, self.color_callback, 10) + if self.save_depth: + self.create_subscription(Image, self.depth_topic, self.depth_callback, 10) + self.create_subscription( + CameraInfo, self.camera_info_topic, self.camera_info_callback, 10 + ) + self.create_timer(self.save_interval_sec, self.save_latest_frames) + + self.get_logger().info(f"Saving RealSense data to {self.output_dir}") + self.get_logger().info(f"Color topic: {self.color_topic}") + if self.save_depth: + self.get_logger().info(f"Depth topic: {self.depth_topic}") + self.get_logger().info("Press Ctrl+C to stop collecting data.") + + def _init_metadata_file(self): + if self.metadata_path.exists(): + return + + with self.metadata_path.open("w", newline="") as csv_file: + writer = csv.writer(csv_file) + writer.writerow( + [ + "frame_index", + "color_file", + "depth_file", + "color_stamp_sec", + "color_stamp_nanosec", + "depth_stamp_sec", + "depth_stamp_nanosec", + ] + ) + + def color_callback(self, msg): + self.latest_color = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") + self.latest_color_stamp = msg.header.stamp + + def depth_callback(self, msg): + self.latest_depth = self.bridge.imgmsg_to_cv2( + msg, desired_encoding="passthrough" + ) + self.latest_depth_stamp = msg.header.stamp + + def camera_info_callback(self, msg): + if self.camera_info_saved: + return + + camera_info = { + "width": msg.width, + "height": msg.height, + "distortion_model": msg.distortion_model, + "d": list(msg.d), + "k": list(msg.k), + "r": list(msg.r), + "p": list(msg.p), + "intrinsics": { + "fx": msg.k[0], + "fy": msg.k[4], + "cx": msg.k[2], + "cy": msg.k[5], + }, + } + camera_info_path = self.meta_dir / "camera_info.json" + with camera_info_path.open("w") as json_file: + json.dump(camera_info, json_file, indent=2) + + self.camera_info_saved = True + self.get_logger().info(f"Saved camera info to {camera_info_path}") + + def save_latest_frames(self): + if self.latest_color is None: + self.get_logger().warn("Waiting for color image...") + return + + if self.save_depth and self.latest_depth is None: + self.get_logger().warn("Waiting for aligned depth image...") + return + + self.frame_index += 1 + base_name = f"frame_{self.frame_index:06d}" + color_file = f"{base_name}.jpg" + depth_file = f"{base_name}.png" if self.save_depth else "" + + color_path = self.color_dir / color_file + cv2.imwrite( + str(color_path), + self.latest_color, + [int(cv2.IMWRITE_JPEG_QUALITY), self.jpeg_quality], + ) + + if self.save_depth: + depth_path = self.depth_dir / depth_file + cv2.imwrite(str(depth_path), self.latest_depth) + + self._append_metadata(color_file, depth_file) + self.get_logger().info(f"Saved {base_name}") + + if self.max_frames > 0 and self.frame_index >= self.max_frames: + self.get_logger().info(f"Reached max_frames={self.max_frames}. Stopping.") + rclpy.shutdown() + + def _append_metadata(self, color_file, depth_file): + color_stamp_sec = self.latest_color_stamp.sec if self.latest_color_stamp else "" + color_stamp_nanosec = ( + self.latest_color_stamp.nanosec if self.latest_color_stamp else "" + ) + depth_stamp_sec = self.latest_depth_stamp.sec if self.latest_depth_stamp else "" + depth_stamp_nanosec = ( + self.latest_depth_stamp.nanosec if self.latest_depth_stamp else "" + ) + + with self.metadata_path.open("a", newline="") as csv_file: + writer = csv.writer(csv_file) + writer.writerow( + [ + self.frame_index, + color_file, + depth_file, + color_stamp_sec, + color_stamp_nanosec, + depth_stamp_sec, + depth_stamp_nanosec, + ] + ) + + +def main(args=None): + rclpy.init(args=args) + node = RealSenseDataCollector() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/azas_perception/setup.py b/src/azas_perception/setup.py index 9856b9c..d718cae 100644 --- a/src/azas_perception/setup.py +++ b/src/azas_perception/setup.py @@ -21,11 +21,15 @@ tests_require=["pytest"], entry_points={ "console_scripts": [ - "yolo_tumbler_detector_node = azas_perception.yolo_tumbler_detector_node:main", + "bar_detect_test_legacy_node = azas_perception.bar_detect_test_legacy_node:main", + "bar_sort_legacy_node = azas_perception.bar_sort_legacy_node:main", + "click_pick_legacy_node = azas_perception.click_pick_legacy_node:main", "cup_detection_pose_bridge_node = azas_perception.cup_detection_pose_bridge_node:main", "gpd_grasp_adapter_node = azas_perception.gpd_grasp_adapter_node:main", + "realsense_data_collector_legacy_node = azas_perception.realsense_data_collector_legacy_node:main", "simulated_cup_detection_node = azas_perception.simulated_cup_detection_node:main", "yolo_cup_pick_legacy_node = azas_perception.yolo_cup_pick_legacy_node:main", + "yolo_tumbler_detector_node = azas_perception.yolo_tumbler_detector_node:main", ], }, ) diff --git a/src/azas_voice/azas_voice/stt_pick_and_place_legacy.py b/src/azas_voice/azas_voice/stt_pick_and_place_legacy.py new file mode 100644 index 0000000..e608a86 --- /dev/null +++ b/src/azas_voice/azas_voice/stt_pick_and_place_legacy.py @@ -0,0 +1,387 @@ +#!/usr/bin/env python3 +""" +STT 기반 Pick & Place 제어 노드 + +/stt_result (std_msgs/String) 구독 + -> 키워드 매핑 -> 명령 큐 -> 워커 스레드에서 MoveItPy + Gripper 실행 +""" +import math +import os +import queue +import tempfile +import threading +import time + +import rclpy +from rclpy.executors import MultiThreadedExecutor +from rclpy.logging import get_logger +from rclpy.node import Node +from std_msgs.msg import String + +from geometry_msgs.msg import PoseStamped +from moveit.planning import MoveItPy, PlanRequestParameters +from moveit_msgs.msg import Constraints, JointConstraint + +try: + from gtts import gTTS + import pygame + _TTS_OK = True +except ImportError: + _TTS_OK = False + +from azas_gripper.onrobot import RG + +# ----- Gripper ----- +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 +GRIPPER_OPEN_WIDTH = 500 +GRIPPER_CLOSE_WIDTH = 150 +GRIPPER_FORCE = 300 + +# ----- MoveIt / Robot ----- +GROUP_NAME = "manipulator" +BASE_FRAME = "base_link" +EE_LINK = "link_6" + +HOME_JOINTS_RAD = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(0.0), +} + +# ----- Safety bounds ----- +SAFE_X_MIN = 0.0 +SAFE_Y_MIN = -0.3 +SAFE_Y_MAX = 0.3 +SAFE_Z_MIN = 0.27 + +# ----- Task poses ----- +TASK_POSES = { + "pick": { + "pos": {"x": 0.427, "y": 0.148, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, + "place": { + "pos": {"x": 0.426, "y": -0.153, "z": 0.280}, + "ori": {"x": 0.000, "y": 1.000, "z": 0.000, "w": 0.000}, + }, +} +APPROACH_OFFSET = 0.05 + + +TTS_PHRASES = { + "home": "홈 위치로 이동합니다", + "pick": "물체를 집습니다", + "place": "물체를 내려놓습니다", + "pickplace": "픽 앤 플레이스를 시작합니다", + "stop": "정지합니다", + "done": "완료되었습니다", + "fail": "실행에 실패했습니다", +} + + +class _TtsPlayer: + """gTTS + pygame 기반 비동기 음성 재생 (캐시 사용).""" + + def __init__(self, lang: str = "ko", enabled: bool = True): + self._enabled = enabled and _TTS_OK + self._lang = lang + self._cache: dict[str, str] = {} + if self._enabled: + try: + pygame.mixer.init() + except Exception: + self._enabled = False + + def speak(self, text: str): + if not self._enabled or not text: + return + try: + path = self._cache.get(text) + if not path or not os.path.exists(path): + fd, path = tempfile.mkstemp(suffix=".mp3", prefix="dsr_tts_") + os.close(fd) + gTTS(text=text, lang=self._lang).save(path) + self._cache[text] = path + pygame.mixer.music.load(path) + pygame.mixer.music.play() + except Exception: + pass + + +KEYWORD_MAP: dict[str, str] = { + "홈": "home", + "home": "home", + "홈으로": "home", + "픽": "pick", + "집어": "pick", + "잡아": "pick", + "pick": "pick", + "플레이스": "place", + "놓아": "place", + "내려놔": "place", + "place": "place", + "픽앤플레이스": "pickplace", + "픽플레이스": "pickplace", + "pickandplace": "pickplace", + "pickplace": "pickplace", + "정지": "stop", + "멈춰": "stop", + "스톱": "stop", + "stop": "stop", +} + + +def _text_to_cmd(text: str) -> str | None: + normalized = text.lower().replace(" ", "") + for kw, cmd in KEYWORD_MAP.items(): + if kw in normalized: + return cmd + return None + + +def _clamp_to_safe_workspace(x: float, y: float, z: float, logger): + sx, sy, sz = x, y, z + + if sx < SAFE_X_MIN: + logger.warning(f"x safety clamp: {sx:.3f} -> {SAFE_X_MIN:.3f}") + sx = SAFE_X_MIN + if sy < SAFE_Y_MIN: + logger.warning(f"y safety clamp: {sy:.3f} -> {SAFE_Y_MIN:.3f}") + sy = SAFE_Y_MIN + elif sy > SAFE_Y_MAX: + logger.warning(f"y safety clamp: {sy:.3f} -> {SAFE_Y_MAX:.3f}") + sy = SAFE_Y_MAX + if sz < SAFE_Z_MIN: + logger.warning(f"z safety clamp: {sz:.3f} -> {SAFE_Z_MIN:.3f}") + sz = SAFE_Z_MIN + + return sx, sy, sz + + +def _build_home_joint_constraints() -> list[Constraints]: + constraints = Constraints() + joint_names = [f"joint_{i}" for i in range(1, 7)] + + for joint_name, position in zip(joint_names, HOME_JOINTS_RAD.values()): + joint_constraint = JointConstraint() + joint_constraint.joint_name = joint_name + joint_constraint.position = position + joint_constraint.tolerance_above = 0.001 + joint_constraint.tolerance_below = 0.001 + joint_constraint.weight = 1.0 + constraints.joint_constraints.append(joint_constraint) + + return [constraints] + + +def _plan_and_execute(robot, arm, logger, plan_params=None) -> bool: + result = arm.plan(parameters=plan_params) if plan_params else arm.plan() + if not result: + logger.error("Planning failed") + return False + robot.execute(group_name=GROUP_NAME, robot_trajectory=result.trajectory, blocking=True) + return True + + +def _move_home(robot, arm, logger, home_params) -> bool: + arm.set_start_state_to_current_state() + arm.set_goal_state(motion_plan_constraints=_build_home_joint_constraints()) + return _plan_and_execute(robot, arm, logger, plan_params=home_params) + + +def _move_pose(robot, arm, logger, pose_goal: PoseStamped, pilz_params) -> bool: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + sx, sy, sz = _clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + + arm.set_start_state_to_current_state() + arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link=EE_LINK) + return _plan_and_execute(robot, arm, logger, plan_params=pilz_params) + + +class SttPickAndPlaceNode(Node): + def __init__(self, robot: MoveItPy, arm, home_params, pilz_params, gripper): + super().__init__("stt_pick_and_place") + + self.declare_parameter("use_tts", True) + use_tts = self.get_parameter("use_tts").get_parameter_value().bool_value + + self._robot = robot + self._arm = arm + self._home = home_params + self._pilz = pilz_params + self._cmd_q: queue.Queue[str] = queue.Queue() + self._holding = False + self._tts = _TtsPlayer(enabled=use_tts) + self._gripper = gripper + + self.create_subscription(String, "/stt_result", self._stt_cb, 10) + threading.Thread(target=self._worker, daemon=True).start() + self.get_logger().info( + f"준비 완료 명령어: home / pick / place / pickplace / stop (tts={'on' if self._tts._enabled else 'off'})" + ) + + def _stt_cb(self, msg: String): + cmd = _text_to_cmd(msg.data) + if cmd is None: + self.get_logger().debug(f"매핑 없음: '{msg.data}'") + return + + if cmd == "stop": + n = 0 + while not self._cmd_q.empty(): + try: + self._cmd_q.get_nowait() + n += 1 + except queue.Empty: + break + self.get_logger().info(f"[STOP] 큐 {n}개 취소") + self._tts.speak(TTS_PHRASES["stop"]) + return + + self._cmd_q.put(cmd) + self.get_logger().info(f"[CMD] '{cmd}' 큐 추가 (크기={self._cmd_q.qsize()})") + + def _build_pose_goal(self, task_key: str, z_value: float) -> PoseStamped: + task = TASK_POSES[task_key] + pose_goal = PoseStamped() + pose_goal.header.frame_id = BASE_FRAME + pose_goal.pose.position.x = task["pos"]["x"] + pose_goal.pose.position.y = task["pos"]["y"] + pose_goal.pose.position.z = z_value + pose_goal.pose.orientation.x = task["ori"]["x"] + pose_goal.pose.orientation.y = task["ori"]["y"] + pose_goal.pose.orientation.z = task["ori"]["z"] + pose_goal.pose.orientation.w = task["ori"]["w"] + return pose_goal + + def _set_gripper(self, logger, width: int): + try: + self._gripper.move_gripper(width, GRIPPER_FORCE) + time.sleep(1.0) + except Exception as e: + logger.error(f"Gripper error: {e}") + + def _run_pick(self, logger) -> bool: + pick = TASK_POSES["pick"]["pos"] + approach = self._build_pose_goal("pick", pick["z"] + APPROACH_OFFSET) + target = self._build_pose_goal("pick", pick["z"]) + + if not _move_pose(self._robot, self._arm, logger, approach, self._pilz): + return False + if not _move_pose(self._robot, self._arm, logger, target, self._pilz): + return False + + logger.info("Gripper CLOSE") + self._set_gripper(logger, GRIPPER_CLOSE_WIDTH) + self._holding = True + + if not _move_pose(self._robot, self._arm, logger, approach, self._pilz): + return False + return True + + def _run_place(self, logger) -> bool: + place = TASK_POSES["place"]["pos"] + approach = self._build_pose_goal("place", place["z"] + APPROACH_OFFSET) + target = self._build_pose_goal("place", place["z"]) + + if not _move_pose(self._robot, self._arm, logger, approach, self._pilz): + return False + if not _move_pose(self._robot, self._arm, logger, target, self._pilz): + return False + + logger.info("Gripper OPEN") + self._set_gripper(logger, GRIPPER_OPEN_WIDTH) + self._holding = False + + if not _move_pose(self._robot, self._arm, logger, approach, self._pilz): + return False + return True + + def _worker(self): + logger = get_logger("stt_pick_and_place.worker") + while True: + try: + cmd = self._cmd_q.get(timeout=1.0) + except queue.Empty: + continue + + logger.info(f"===== '{cmd}' 실행 =====") + self._tts.speak(TTS_PHRASES.get(cmd, "")) + ok = True + + if cmd == "home": + ok = _move_home(self._robot, self._arm, logger, self._home) + elif cmd == "pick": + ok = self._run_pick(logger) + elif cmd == "place": + if not self._holding: + logger.warning("현재 물체를 쥐고 있지 않습니다. place를 계속 진행합니다.") + ok = self._run_place(logger) + elif cmd == "pickplace": + ok = self._run_pick(logger) + if ok: + ok = self._run_place(logger) + + if ok: + logger.info(f"===== '{cmd}' 완료 =====") + else: + logger.error(f"===== '{cmd}' 실패 =====") + self._tts.speak(TTS_PHRASES["done" if ok else "fail"]) + + +def main(args=None): + rclpy.init(args=args) + logger = get_logger("stt_pick_and_place.main") + + gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + time.sleep(0.5) + gripper.move_gripper(GRIPPER_OPEN_WIDTH, GRIPPER_FORCE) + + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + logger.info("MoveItPy 초기화 완료") + + home = PlanRequestParameters(robot) + home.planning_pipeline = "ompl" + home.planner_id = "RRTConnect" + home.max_velocity_scaling_factor = 0.2 + home.max_acceleration_scaling_factor = 0.1 + home.planning_time = 3.0 + + pilz = PlanRequestParameters(robot) + pilz.planning_pipeline = "pilz_industrial_motion_planner" + pilz.planner_id = "PTP" + pilz.max_velocity_scaling_factor = 0.15 + pilz.max_acceleration_scaling_factor = 0.1 + pilz.planning_time = 3.0 + + logger.info("시작 자세를 home으로 이동합니다") + _move_home(robot, arm, logger, home) + + node = SttPickAndPlaceNode(robot, arm, home, pilz, gripper) + executor = MultiThreadedExecutor() + executor.add_node(node) + + logger.info("음성 명령 대기 중 ... (Ctrl+C 종료)") + try: + executor.spin() + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + robot.shutdown() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/azas_voice/azas_voice/stt_robot_control_legacy.py b/src/azas_voice/azas_voice/stt_robot_control_legacy.py new file mode 100644 index 0000000..2888275 --- /dev/null +++ b/src/azas_voice/azas_voice/stt_robot_control_legacy.py @@ -0,0 +1,376 @@ +#!/usr/bin/env python3 +""" +STT 음성 명령 로봇 제어 노드 (오프셋 이동 방식) + +/stt_result (std_msgs/String) 구독 + → 키워드 매핑 → 명령 큐 → 워커 스레드에서 MoveItPy 실행 + +명령어: + home → HOME 조인트 자세 (OMPL RRTConnect) + 앞/뒤/왼쪽/오른쪽/위/아래 → 현재 EE 위치 기준 5cm 오프셋 이동 (Pilz PTP) + stop → 명령 큐 비우기 +""" +import math +import os +import queue +import tempfile +import threading + +import numpy as np +import rclpy +from rclpy.executors import MultiThreadedExecutor +from rclpy.logging import get_logger +from rclpy.node import Node +from std_msgs.msg import String + +from geometry_msgs.msg import PoseStamped +from moveit.planning import MoveItPy, PlanRequestParameters +from moveit_msgs.msg import Constraints, JointConstraint + +try: + from gtts import gTTS + import pygame + _TTS_OK = True +except ImportError: + _TTS_OK = False + +# ── 로봇 설정 ────────────────────────────────────────────── +GROUP_NAME = "manipulator" +BASE_FRAME = "base_link" +EE_LINK = "link_6" +HOME_JOINTS_RAD = { + "joint_1": math.radians(0.0), + "joint_2": math.radians(0.0), + "joint_3": math.radians(90.0), + "joint_4": math.radians(0.0), + "joint_5": math.radians(90.0), + "joint_6": math.radians(0.0), +} + +# ── 안전 작업 영역 (base_link 기준) ─────────────────────── +SAFE_X_MIN = 0.0 +SAFE_Y_MIN = -0.3 +SAFE_Y_MAX = 0.3 +SAFE_Z_MIN = 0.27 + +# ── 방향 → (dx, dy, dz) 오프셋 ─────────────────────────── +JOG_OFFSET = 0.05 # m +DIRECTIONS = { + "forward": ( JOG_OFFSET, 0.0, 0.0), + "backward": (-JOG_OFFSET, 0.0, 0.0), + "left": ( 0.0, JOG_OFFSET, 0.0), + "right": ( 0.0, -JOG_OFFSET, 0.0), + "up": ( 0.0, 0.0, JOG_OFFSET), + "down": ( 0.0, 0.0, -JOG_OFFSET), +} + +# ── 명령별 음성 안내 ────────────────────────────────────── +TTS_PHRASES = { + "home": "홈 위치로 이동합니다", + "forward": "앞으로 이동합니다", + "backward": "뒤로 이동합니다", + "left": "왼쪽으로 이동합니다", + "right": "오른쪽으로 이동합니다", + "up": "위로 이동합니다", + "down": "아래로 이동합니다", + "stop": "정지합니다", + "done": "완료되었습니다", + "fail": "실행에 실패했습니다", +} + +# ── 키워드 → 명령 매핑 ──────────────────────────────────── +KEYWORD_MAP: dict[str, str] = { + "홈": "home", "home": "home", "홈으로": "home", + "왼쪽": "left", "왼": "left", "left": "left", + "오른쪽": "right", "오른": "right", "right": "right", + "앞": "forward", "앞으로": "forward", "전방": "forward", + "front": "forward", "forward": "forward", + "뒤": "backward", "뒤로": "backward", "후방": "backward", + "back": "backward", "backward": "backward", + "위": "up", "위로": "up", "올려": "up", "올라가": "up", "up": "up", + "아래": "down", "아래로": "down", "내려": "down", "내려가": "down", "down": "down", + "정지": "stop", "멈춰": "stop", "스톱": "stop", "stop": "stop", +} + +VALID_CMDS = {"home"} | set(DIRECTIONS.keys()) + + +class _TtsPlayer: + """gTTS + pygame 기반 비동기 음성 재생 (캐시 사용).""" + + def __init__(self, lang: str = "ko", enabled: bool = True): + self._enabled = enabled and _TTS_OK + self._lang = lang + self._cache: dict[str, str] = {} + if self._enabled: + try: + pygame.mixer.init() + except Exception: + self._enabled = False + + def speak(self, text: str): + if not self._enabled or not text: + return + try: + path = self._cache.get(text) + if not path or not os.path.exists(path): + fd, path = tempfile.mkstemp(suffix=".mp3", prefix="dsr_tts_") + os.close(fd) + gTTS(text=text, lang=self._lang).save(path) + self._cache[text] = path + pygame.mixer.music.load(path) + pygame.mixer.music.play() + except Exception: + pass + + +def _text_to_cmd(text: str) -> str | None: + normalized = text.lower().replace(" ", "") + for kw, cmd in KEYWORD_MAP.items(): + if kw in normalized: + return cmd + return None + + +def _clamp_to_safe(x: float, y: float, z: float, logger) -> tuple[float, float, float]: + if x < SAFE_X_MIN: + logger.warning(f"x 클램핑: {x:.3f} → {SAFE_X_MIN:.3f}") + x = SAFE_X_MIN + if y < SAFE_Y_MIN: + logger.warning(f"y 클램핑: {y:.3f} → {SAFE_Y_MIN:.3f}") + y = SAFE_Y_MIN + elif y > SAFE_Y_MAX: + logger.warning(f"y 클램핑: {y:.3f} → {SAFE_Y_MAX:.3f}") + y = SAFE_Y_MAX + if z < SAFE_Z_MIN: + logger.warning(f"z 클램핑: {z:.3f} → {SAFE_Z_MIN:.3f}") + z = SAFE_Z_MIN + return x, y, z + + +def _rot_matrix_to_quat(R: np.ndarray) -> tuple[float, float, float, float]: + """3x3 회전행렬 → (x, y, z, w) 쿼터니언.""" + trace = R[0, 0] + R[1, 1] + R[2, 2] + if trace > 0: + s = 0.5 / math.sqrt(trace + 1.0) + w = 0.25 / s + x = (R[2, 1] - R[1, 2]) * s + y = (R[0, 2] - R[2, 0]) * s + z = (R[1, 0] - R[0, 1]) * s + elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]: + s = 2.0 * math.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) + w = (R[2, 1] - R[1, 2]) / s + x = 0.25 * s + y = (R[0, 1] + R[1, 0]) / s + z = (R[0, 2] + R[2, 0]) / s + elif R[1, 1] > R[2, 2]: + s = 2.0 * math.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) + w = (R[0, 2] - R[2, 0]) / s + x = (R[0, 1] + R[1, 0]) / s + y = 0.25 * s + z = (R[1, 2] + R[2, 1]) / s + else: + s = 2.0 * math.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) + w = (R[1, 0] - R[0, 1]) / s + x = (R[0, 2] + R[2, 0]) / s + y = (R[1, 2] + R[2, 1]) / s + z = 0.25 * s + return x, y, z, w + + +def _plan_and_execute(robot, arm, logger, plan_params=None) -> bool: + logger.info("Planning ...") + result = arm.plan(parameters=plan_params) if plan_params else arm.plan() + if not result: + logger.error("Planning failed") + return False + logger.info("Executing ...") + robot.execute(group_name=GROUP_NAME, robot_trajectory=result.trajectory, blocking=True) + logger.info("Done") + return True + + +def _build_home_joint_constraints() -> list[Constraints]: + constraints = Constraints() + joint_names = [f"joint_{i}" for i in range(1, 7)] + + for joint_name, position in zip(joint_names, HOME_JOINTS_RAD.values()): + joint_constraint = JointConstraint() + joint_constraint.joint_name = joint_name + joint_constraint.position = position + joint_constraint.tolerance_above = 0.001 + joint_constraint.tolerance_below = 0.001 + joint_constraint.weight = 1.0 + constraints.joint_constraints.append(joint_constraint) + + return [constraints] + + +def _move_home(robot, arm, logger, plan_params=None) -> bool: + arm.set_start_state_to_current_state() + arm.set_goal_state(motion_plan_constraints=_build_home_joint_constraints()) + return _plan_and_execute(robot, arm, logger, plan_params=plan_params) + + +# ══════════════════════════════════════════════════════════ +class SttRobotControlNode(Node): + + def __init__( + self, + robot: MoveItPy, + arm, + home_params: PlanRequestParameters, + pilz_params: PlanRequestParameters, + ): + super().__init__("stt_robot_control") + + self.declare_parameter("use_tts", True) + use_tts = self.get_parameter("use_tts").get_parameter_value().bool_value + + self._robot = robot + self._arm = arm + self._home = home_params + self._pilz = pilz_params + self._cmd_q: queue.Queue[str] = queue.Queue() + self._tts = _TtsPlayer(enabled=use_tts) + + threading.Thread(target=self._worker, daemon=True).start() + + self.create_subscription(String, "/stt_result", self._stt_cb, 10) + self.get_logger().info( + f"준비 완료 명령어: home / {' / '.join(DIRECTIONS)} / stop " + f"(jog={JOG_OFFSET*100:.0f}cm, tts={'on' if self._tts._enabled else 'off'})" + ) + + # ── ROS 콜백 ─────────────────────────────────────────── + def _stt_cb(self, msg: String): + cmd = _text_to_cmd(msg.data) + if cmd is None: + self.get_logger().debug(f"매핑 없음: '{msg.data}'") + return + + if cmd == "stop": + n = 0 + while not self._cmd_q.empty(): + try: + self._cmd_q.get_nowait() + n += 1 + except queue.Empty: + break + self.get_logger().info(f"[STOP] 큐 {n}개 취소") + self._tts.speak(TTS_PHRASES["stop"]) + else: + self._cmd_q.put(cmd) + self.get_logger().info(f"[CMD] '{cmd}' 큐 추가 (크기={self._cmd_q.qsize()})") + + # ── 워커 스레드 ──────────────────────────────────────── + def _worker(self): + logger = get_logger("stt_robot_control.worker") + + while True: + try: + cmd = self._cmd_q.get(timeout=1.0) + except queue.Empty: + continue + + logger.info(f"===== '{cmd}' 실행 =====") + self._tts.speak(TTS_PHRASES.get(cmd, "")) + ok = True + + if cmd == "home": + ok = _move_home( + self._robot, + self._arm, + logger, + plan_params=self._home, + ) + + elif cmd in DIRECTIONS: + ok = self._move_offset(logger, cmd) + + logger.info(f"===== '{cmd}' 완료 =====") + self._tts.speak(TTS_PHRASES["done" if ok else "fail"]) + + # ── 현재 EE 위치 기준 오프셋 이동 ────────────────────── + def _move_offset(self, logger, direction: str) -> bool: + try: + with self._robot.get_planning_scene_monitor().read_only() as scene: + state = scene.current_state + state.update() + tf = state.get_frame_transform(EE_LINK) + except Exception as e: + logger.error(f"현재 EE 상태 조회 실패: {e}") + return False + + cx = float(tf[0, 3]) + cy = float(tf[1, 3]) + cz = float(tf[2, 3]) + qx, qy, qz, qw = _rot_matrix_to_quat(tf[:3, :3]) + + dx, dy, dz = DIRECTIONS[direction] + tx, ty, tz = _clamp_to_safe(cx + dx, cy + dy, cz + dz, logger) + + logger.info( + f"JOG dir={direction} offset={JOG_OFFSET*100:.1f}cm " + f"({cx:.3f},{cy:.3f},{cz:.3f}) → ({tx:.3f},{ty:.3f},{tz:.3f})" + ) + + ps = PoseStamped() + ps.header.frame_id = BASE_FRAME + ps.pose.position.x = tx + ps.pose.position.y = ty + ps.pose.position.z = tz + ps.pose.orientation.x = qx + ps.pose.orientation.y = qy + ps.pose.orientation.z = qz + ps.pose.orientation.w = qw + + self._arm.set_start_state_to_current_state() + self._arm.set_goal_state(pose_stamped_msg=ps, pose_link=EE_LINK) + return _plan_and_execute(self._robot, self._arm, logger, plan_params=self._pilz) + + +# ══════════════════════════════════════════════════════════ +def main(args=None): + rclpy.init(args=args) + logger = get_logger("stt_robot_control.main") + + robot = MoveItPy(node_name="moveit_py") + arm = robot.get_planning_component(GROUP_NAME) + logger.info("MoveItPy 초기화 완료") + + home = PlanRequestParameters(robot) + home.planning_pipeline = "ompl" + home.planner_id = "RRTConnect" + home.max_velocity_scaling_factor = 0.2 + home.max_acceleration_scaling_factor = 0.1 + home.planning_time = 3.0 + + pilz = PlanRequestParameters(robot) + pilz.planning_pipeline = "pilz_industrial_motion_planner" + pilz.planner_id = "PTP" + pilz.max_velocity_scaling_factor = 0.2 + pilz.max_acceleration_scaling_factor = 0.1 + pilz.planning_time = 3.0 + + logger.info("시작 자세를 home으로 이동합니다") + _move_home(robot, arm, logger, plan_params=home) + + node = SttRobotControlNode(robot, arm, home, pilz) + + executor = MultiThreadedExecutor() + executor.add_node(node) + + logger.info("음성 명령 대기 중 ... (Ctrl+C 종료)") + try: + executor.spin() + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + robot.shutdown() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/azas_voice/package.xml b/src/azas_voice/package.xml index 9d1292a..45f94ea 100644 --- a/src/azas_voice/package.xml +++ b/src/azas_voice/package.xml @@ -8,7 +8,12 @@ ament_python rclpy + azas_gripper + geometry_msgs + moveit_msgs std_msgs + moveit_py + python3-numpy python3-pytest diff --git a/src/azas_voice/setup.py b/src/azas_voice/setup.py index d9a05cc..b77d21b 100644 --- a/src/azas_voice/setup.py +++ b/src/azas_voice/setup.py @@ -22,9 +22,11 @@ tests_require=["pytest"], entry_points={ "console_scripts": [ - "stt_node = azas_voice.stt_node:main", - "recipe_mapper_node = azas_voice.recipe_mapper_node:main", "llm_recipe_mapper_node = azas_voice.llm_recipe_mapper_node:main", + "recipe_mapper_node = azas_voice.recipe_mapper_node:main", + "stt_node = azas_voice.stt_node:main", + "stt_pick_and_place_legacy = azas_voice.stt_pick_and_place_legacy:main", + "stt_robot_control_legacy = azas_voice.stt_robot_control_legacy:main", ], }, )