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_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
-
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/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_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/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/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/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/package.xml b/src/azas_motion/package.xml
index 36cf95a..7f8a9c0 100644
--- a/src/azas_motion/package.xml
+++ b/src/azas_motion/package.xml
@@ -9,9 +9,12 @@
ament_python
rclpy
geometry_msgs
+ moveit_msgs
nav_msgs
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 af73e03..d99d598 100644
--- a/src/azas_motion/setup.py
+++ b/src/azas_motion/setup.py
@@ -20,8 +20,16 @@
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",
+ "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/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 0000000..2a9474f
Binary files /dev/null and b/src/azas_perception/config/T_gripper2camera.npy differ
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..d718cae 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,
@@ -19,10 +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",
],
},
)
diff --git a/src/dsr_practice/config/T_gripper2camera.npy b/src/dsr_practice/config/T_gripper2camera.npy
new file mode 100644
index 0000000..2a9474f
Binary files /dev/null and b/src/dsr_practice/config/T_gripper2camera.npy differ
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 0000000..2a9474f
Binary files /dev/null and b/src/dsr_practice/dsr_practice/config/T_gripper2camera.npy differ
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'