moveit_pyサンプルパッケージ追加

README.en.md

@@ -85,7 +85,10 @@ Please refer to [crane_x7_examples](./crane_x7_examples/README.md) for details.
   - This package includes a hardware driver for CRANE-X7.
 - crane_x7_examples
   - [README](./crane_x7_examples/README.md)
-  - This package includes example codes for CRANE-X7.
+  - This package includes C++ example codes for CRANE-X7.
+- crane_x7_examples_py
+  - [README](./crane_x7_examples_py/README.md)
+  - This package includes Python example codes for CRANE-X7.
 - crane_x7_gazebo
   - [README](./crane_x7_gazebo/README.md)
   - This package includes Gazebo simulation environments for CRANE-X7.

README.md

@@ -87,7 +87,10 @@ $ ros2 launch crane_x7_examples example.launch.py example:='gripper_control'
   - USB通信ポートの設定方法をREAMDEに記載してます
 - crane_x7_examples
   - [README](./crane_x7_examples/README.md)
-  - CRANE-X7のサンプルコード集です
+  - CRANE-X7のC++サンプルコード集です
+- crane_x7_examples_py
+  - [README](./crane_x7_examples_py/README.md)
+  - CRANE-X7のPythonサンプルコード集です  
 - crane_x7_gazebo
   - [README](./crane_x7_gazebo/README.md)
   - CRANE-X7のGazeboシミュレーションパッケージです

crane_x7_examples_py/README.md

@@ -0,0 +1,198 @@
+# crane_x7_examples_py
+
+このパッケージはCRANE-X7 ROS 2パッケージのサンプルコード集です。
+
+- [crane\_x7\_examples\_py](#crane_x7_examples_py)
+  - [起動方法](#起動方法)
+  - [サンプルプログラムを実行する](#サンプルプログラムを実行する)
+    - [Gazeboでサンプルプログラムを実行する場合](#gazeboでサンプルプログラムを実行する場合)
+  - [Examples](#examples)
+    - [gripper\_control](#gripper_control)
+    - [pose\_groupstate](#pose_groupstate)
+    - [joint\_values](#joint_values)
+    - [pick\_and\_place](#pick_and_place)
+  - [Camera Examples](#camera-examples)
+    - [aruco\_detection](#aruco_detection)
+    - [color\_detection](#color_detection)
+
+## 起動方法
+CRANE-X7の起動方法は[crane_x7_examplesのREADME](../crane_x7_examples/README.md)を参照してください。
+
+## サンプルプログラムを実行する
+
+準備ができたらサンプルプログラムを実行します。
+例えばグリッパを開閉するサンプルは次のコマンドで実行できます。
+
+```sh
+ros2 launch crane_x7_examples_py example.launch.py example:='gripper_control'
+```
+
+終了するときは`Ctrl+c`を入力します。
+
+### Gazeboでサンプルプログラムを実行する場合
+
+Gazeboでサンプルプログラムを実行する場合は`use_sim_time`オプションを付けます。
+
+```sh
+ros2 launch crane_x7_examples_py example.launch.py example:='gripper_control' use_sim_time:='true'
+```
+
+## Examples
+
+`demo.launch`を実行している状態で各サンプルを実行できます。
+
+- [gripper_control](#gripper_control)
+- [pose_groupstate](#pose_groupstate)
+- [joint_values](#joint_values)
+- [pick_and_place](#pick_and_place)
+
+実行できるサンプルの一覧は、`example.launch.py`にオプション`-s`を付けて実行することで表示できます。
+
+```sh
+$ ros2 launch crane_x7_examples_py example.launch.py -s
+Arguments (pass arguments as '<name>:=<value>'):
+
+    'example':
+        Set an example executable name: [gripper_control, pose_groupstate, joint_values, pick_and_place]
+        (default: 'pose_groupstate')
+```
+
+---
+
+### gripper_control
+
+ハンドを開閉させるコード例です。
+
+次のコマンドを実行します。
+
+```sh
+ros2 launch crane_x7_examples_py example.launch.py example:='gripper_control'
+```
+
+<img src=https://rt-net.github.io/images/crane-x7/gazebo_gripper_example.gif width=500px />
+
+[![crane_x7_gripper_control_demo](http://img.youtube.com/vi/uLRLkwbXUP0/hqdefault.jpg)](https://youtu.be/uLRLkwbXUP0)
+
+[back to example list](#examples)
+
+---
+
+### pose_groupstate
+
+group_stateを使うコード例です。
+
+SRDFファイル[crane_x7_moveit_config/config/crane_x7.srdf](../crane_x7_moveit_config/config/crane_x7.srdf)
+に記載されている`home`と`vertical`の姿勢に移行します。
+
+次のコマンドを実行します。
+
+```sh
+ros2 launch crane_x7_examples_py example.launch.py example:='pose_groupstate'
+```
+
+<img src=https://rt-net.github.io/images/crane-x7/gazebo_pose_groupstate.gif width=500px />
+
+[![crane_x7_pose_groupstate_demo](http://img.youtube.com/vi/FH18dA_xcjM/hqdefault.jpg)](https://youtu.be/FH18dA_xcjM)
+
+[back to example list](#examples)
+
+---
+
+### joint_values
+
+アームのジョイント角度を１つずつ変更させるコード例です。
+
+次のコマンドを実行します。
+
+```sh
+ros2 launch crane_x7_examples_py example.launch.py example:='joint_values'
+```
+<img src= https://rt-net.github.io/images/crane-x7/gazebo_joint_values_example.gif width = 500px />
+
+[![crane_x7_joint_values_demo](http://img.youtube.com/vi/skRwrrlUl4c/hqdefault.jpg)](https://youtu.be/skRwrrlUl4c)
+
+[back to example list](#examples)
+
+---
+
+### pick_and_place
+
+モノを掴む・持ち上げる・運ぶ・置くコード例です。
+
+次のコマンドを実行します。
+
+```sh
+ros2 launch crane_x7_examples_py example.launch.py example:='pick_and_place'
+```
+<img src = https://rt-net.github.io/images/crane-x7/bringup_rviz.gif width = 500px />
+
+**実機を使う場合**
+
+CRANE-X7から20cm離れた位置にピッキング対象を設置します。
+
+<img src = https://rt-net.github.io/images/crane-x7/bringup.jpg width = 500px />
+
+サンプルで使用しているこのオレンジ色のソフトボールはRT ROBOT SHOPの[こちらのページ](https://www.rt-shop.jp/index.php?main_page=product_info&cPath=1299_1307&products_id=3701)から入手することができます。
+
+[![crane_x7_pick_and_place_demo](http://img.youtube.com/vi/S_MwSvG2tKw/hqdefault.jpg)](https://youtu.be/S_MwSvG2tKw)
+
+[back to example list](#examples)
+
+## Camera Examples
+
+[RealSense D435マウンタ](https://github.com/rt-net/crane_x7_Hardware/blob/master/3d_print_parts/v1.0/CRANE-X7_HandA_RealSenseD435マウンタ.stl)搭載モデルのカメラを使用したサンプルコードです。
+
+[crane_x7_examplesのREADME](../crane_x7_examples/README.md)に記載されている「RealSense D435マウンタ搭載モデルを使用する場合」の手順に従って`demo.launch`を実行している状態で各サンプルを実行できます。
+
+- [aruco\_detection](#aruco_detection)
+- [color\_detection](#color_detection)
+
+実行できるサンプルの一覧は、`camera_example.launch.py`にオプション`-s`を付けて実行することで表示できます。
+
+```sh
+$ ros2 launch crane_x7_examples_py camera_example.launch.py -s
+Arguments (pass arguments as '<name>:=<value>'):
+
+    'example':
+        Set an example executable name: [aruco_detection, color_detection]
+        (default: 'aruco_detection')
+```
+
+### aruco_detection
+
+モノに取り付けたArUcoマーカをカメラで検出し、マーカ位置に合わせて掴むコード例です。
+マーカは[aruco_markers.pdf](./aruco_markers.pdf)をA4紙に印刷し、一辺50mmの立方体に取り付けて使用します。
+
+検出されたマーカの位置姿勢はtfのフレームとして配信されます。
+tfの`frame_id`はマーカIDごとに異なりID0のマーカの`frame_id`は`target_0`になります。掴む対象は`target_0`に設定されています。マーカ検出には[OpenCV](https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html)を使用しています。
+
+次のコマンドを実行します
+```sh
+ros2 launch crane_x7_examples_py camera_example.launch.py example:='aruco_detection'
+```
+
+[![crane_x7_aruco_detection_demo](http://img.youtube.com/vi/eWzmG_jbTmM/hqdefault.jpg)](https://youtu.be/eWzmG_jbTmM)
+
+[back to camera example list](#camera-examples)
+
+---
+
+### color_detection
+
+特定の色の物体を検出して掴むコード例です。
+
+デフォルトでは青い物体の位置をtfのフレームとして配信します。
+tfの`frame_id`は`target_0`です。
+色の検出には[OpenCV](https://docs.opencv.org/4.x/db/d8e/tutorial_threshold.html)を使用しています。
+検出した物体の距離は深度画像から取得します。
+
+次のコマンドを実行します
+```sh
+ros2 launch crane_x7_examples_py camera_example.launch.py example:='color_detection'
+```
+
+[![crane_x7_color_detection_demo](http://img.youtube.com/vi/O8lqw7yemAI/hqdefault.jpg)](https://youtu.be/O8lqw7yemAI)
+
+[back to camera example list](#camera-examples)
+
+---

crane_x7_examples_py/config/crane_x7_moveit_py_examples.yaml

@@ -0,0 +1,26 @@
+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"]
+
+plan_request_params:
+  planning_attempts: 1
+  planning_pipeline: ompl
+  max_velocity_scaling_factor: 1.0
+  max_acceleration_scaling_factor: 1.0
+
+ompl_rrtc:  # Namespace for individual plan request
+  plan_request_params:  # PlanRequestParameters similar to the ones that are used by the single pipeline planning of moveit_cpp
+    planning_attempts: 1  # Number of attempts the planning pipeline tries to solve a given motion planning problem
+    planning_pipeline: ompl  # Name of the pipeline that is being used
+    planner_id: "RRTConnectkConfigDefault:"  # Name of the specific planner to be used by the pipeline
+    max_velocity_scaling_factor: 1.0  # Velocity scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning
+    max_acceleration_scaling_factor: 1.0  # Acceleration scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning
+    planning_time: 1.0  # Time budget for the motion plan request. If the planning problem cannot be solved within this time, an empty solution with error code is returned

crane_x7_examples_py/crane_x7_examples_py/aruco_detection.py

@@ -0,0 +1,106 @@
+# Copyright 2025 RT Corporation
+#
+# 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.
+
+import cv2
+from cv2 import aruco
+from cv_bridge import CvBridge
+from geometry_msgs.msg import TransformStamped
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from scipy.spatial.transform import Rotation
+from sensor_msgs.msg import CameraInfo, Image
+from tf2_ros import TransformBroadcaster
+
+
+class ImageSubscriber(Node):
+    def __init__(self):
+        super().__init__('aruco_detection')
+        self.image_subscription = self.create_subscription(
+            Image, '/camera/color/image_raw', self.image_callback, 10
+        )
+        self.camera_info_subscription = self.create_subscription(
+            CameraInfo, '/camera/color/camera_info', self.camera_info_callback, 10
+        )
+
+        self.tf_broadcaster = TransformBroadcaster(self)
+
+        self.camera_info = None
+
+        self.bridge = CvBridge()
+
+    def image_callback(self, msg):
+        # 画像データをROSのメッセージからOpenCVの配列に変換
+        cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding)
+        cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2BGR)
+
+        if not self.camera_info:
+            return
+
+        # ArUcoマーカのデータセットを読み込む
+        # DICT_6x6_50は6x6ビットのマーカが50個収録されたもの
+        MARKER_DICT = aruco.getPredefinedDictionary(aruco.DICT_6X6_50)
+        # マーカID
+        ids = []
+        # 画像座標系上のマーカ頂点位置
+        corners = []
+        # マーカの検出
+        corners, ids, _ = aruco.detectMarkers(cv_img, MARKER_DICT)
+        if ids is None:
+            return
+        # マーカの検出数
+        n_markers = len(ids)
+        # カメラパラメータ
+        CAMERA_MATRIX = np.array(self.camera_info.k).reshape(3, 3)
+        DIST_COEFFS = np.array(self.camera_info.d).reshape(1, 5)
+        # マーカ一辺の長さ 0.04 [m]
+        MARKER_LENGTH = 0.04
+        # 画像座標系上のマーカ位置を三次元のカメラ座標系に変換
+        rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
+                            corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS)
+
+        # マーカの位置姿勢をtfで配信
+        for i in range(n_markers):
+            # tfの配信
+            t = TransformStamped()
+            t.header = msg.header
+            t.child_frame_id = 'target_' + str(ids[i][0])
+            t.transform.translation.x = tvecs[i][0][0]
+            t.transform.translation.y = tvecs[i][0][1]
+            t.transform.translation.z = tvecs[i][0][2]
+            # 回転ベクトルをクォータニオンに変換
+            marker_orientation_rot = Rotation.from_rotvec(rvecs[i][0])
+            marker_orientation_quat = marker_orientation_rot.as_quat()
+            t.transform.rotation.x = marker_orientation_quat[0]
+            t.transform.rotation.y = marker_orientation_quat[1]
+            t.transform.rotation.z = marker_orientation_quat[2]
+            t.transform.rotation.w = marker_orientation_quat[3]
+            self.tf_broadcaster.sendTransform(t)
+
+    def camera_info_callback(self, msg):
+        self.camera_info = msg
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    image_subscriber = ImageSubscriber()
+    rclpy.spin(image_subscriber)
+
+    image_subscriber.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()