moveit _pyサンプルパッケージ追加

README.md

@@ -95,7 +95,10 @@ ros2 launch crane_plus_examples example.launch.py example:='gripper_control'
   - CRANE+ V2のモデルデータ（xacro）を定義するパッケージです
 - crane_plus_examples
   - [README](./crane_plus_examples/README.md)
-  - CRANE+ V2のサンプルコード集です
+  - CRANE+ V2のC++サンプルコード集です
+- crane_plus_examples_py
+  - [README](./crane_plus_examples_py/README.md)
+  - CRANE+ V2のPythonサンプルコード集です
 - crane_plus_gazebo
   - [README](./crane_plus_gazebo/README.md)
   - CRANE+ V2のGazeboシミュレーションパッケージです
@@ -105,7 +108,12 @@ ros2 launch crane_plus_examples example.launch.py example:='gripper_control'
 
 ## How to Use Examples
 
-サンプルプログラムは、`crane_plus_examples`パッケージの[README](./crane_plus_description/README.md)参照してください。
+サンプルプログラムは、C++とPythonの両方を用意しています。詳しくは、以下のリンクをご覧ください。
+
+- C++
+  - [crane_plus_examples](./crane_plus_examples/README.md)
+- Python
+  - [crane_plus_examples_py](./crane_plus_examples_py/README.md)
 
 ## License
 

crane_plus_control/src/crane_plus_hardware.cpp

@@ -205,7 +205,9 @@ return_type CranePlusHardware::read(
     RCLCPP_ERROR(
       rclcpp::get_logger("CranePlusHardware"),
       driver_->get_last_error_log().c_str());
-    return return_type::ERROR;
+    // readに失敗しても通信は継続させる。
+    // 不確かなデータをセットしないようにOKを返す。
+    return return_type::OK;
   } else {
     for (uint i = 0; i < hw_position_states_.size(); ++i) {
       hw_position_states_[i] = joint_positions[i];
@@ -271,7 +273,9 @@ return_type CranePlusHardware::write(
     RCLCPP_ERROR(
       rclcpp::get_logger("CranePlusHardware"),
       driver_->get_last_error_log().c_str());
-    return return_type::ERROR;
+    // writeに失敗しても通信は継続させる。
+    // 不確かなデータをセットしないようにOKを返す。
+    return return_type::OK;
   }
 
   prev_comm_timestamp_ = steady_clock_.now();

crane_plus_examples_py/README.md

@@ -0,0 +1,197 @@
+# crane_plus_examples_py
+
+このパッケージはCRANE+ V2 ROS 2パッケージのPythonによるサンプルコード集です。
+
+## Table of Contents
+
+- [crane\_plus\_examples\_py](#crane_plus_examples_py)
+  - [Table of Contents](#table-of-contents)
+  - [Setup](#setup)
+  - [How to Run Examples](#how-to-run-examples)
+  - [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)
+
+## Setup
+
+CRANE+ V2の起動方法は[crane_plus_examplesのREADME](../crane_plus_examples/README.md)を参照してください。
+
+## How to Run Examples
+
+準備ができたらPythonによるサンプルプログラムを実行します。
+例えばグリッパを開閉するサンプルは次のコマンドで実行できます。
+
+```sh
+ros2 launch crane_plus_examples_py example.launch.py example:='gripper_control'
+```
+
+終了するときは`Ctrl+c`を入力します。
+
+> [!NOTE]
+> Gazeboでサンプルプログラムを実行する場合は`use_sim_time`オプションを付けます。
+>
+> ```sh
+> ros2 launch crane_plus_examples_py example.launch.py example:='gripper_control' use_sim_time:=true
+> ```
+
+## Examples
+
+`demo.launch.py`を実行している状態で各サンプルを実行できます。
+
+- [gripper_control](#gripper_control)
+- [pose_groupstate](#pose_groupstate)
+- [joint_values](#joint_values)
+- [pick_and_place](#pick_and_place)
+
+> [!NOTE]
+> 実行できるサンプルの一覧は、`examples.launch.py`にオプション`-s`を付けて実行することで表示できます。
+>
+> ```sh
+> $ ros2 launch crane_plus_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: 'gripper_control')
+> ```
+
+---
+
+### gripper_control
+
+グリッパを開閉させるコード例です。
+
+次のコマンドを実行します。
+
+```sh
+ros2 launch crane_plus_examples_py example.launch.py example:='gripper_control'
+```
+
+<img src=https://rt-net.github.io/images/crane-plus/gripper_control.gif width=500px />
+
+[back to example list](#examples)
+
+---
+
+### pose_groupstate
+
+group_stateを使うコード例です。
+
+SRDFファイル[crane_plus_moveit_config/config/crane_plus.srdf](../crane_plus_moveit_config/config/crane_plus.srdf)
+に記載されている`home`と`vertical`の姿勢に移行します。
+
+次のコマンドを実行します。
+
+```sh
+ros2 launch crane_plus_examples_py example.launch.py example:='pose_groupstate'
+```
+
+<img src=https://rt-net.github.io/images/crane-plus/pose_groupstate.gif width=500px />
+
+[back to example list](#examples)
+
+---
+
+### joint_values 
+
+アームのジョイント角度を１つずつ変更するコード例です。
+
+次のコマンドを実行します。
+
+```sh
+ros2 launch crane_plus_examples_py example.launch.py example:='joint_values'
+```
+
+<img src=https://rt-net.github.io/images/crane-plus/joint_values.gif width=500px />
+
+[back to example list](#examples)
+
+---
+
+### pick_and_place
+
+モノを掴む・持ち上げる・運ぶ・置くコード例です。
+
+次のコマンドを実行します。
+
+```sh
+ros2 launch crane_plus_examples_py example.launch.py example:='pick_and_place'
+```
+
+<img src=https://rt-net.github.io/images/crane-plus/pick_and_place.gif width=500px />
+
+[back to example list](#examples)
+
+---
+
+## Camera Examples
+
+Webカメラ搭載モデルのカメラを使用したサンプルコードです。
+
+[crane_plus_examplesのREADME](../crane_plus_examples/README.md)に記載されている「Webカメラ搭載モデルを使用する場合」の手順に従って`demo.launch`を実行している状態で各サンプルを実行できます。
+
+- [aruco\_detection](#aruco_detection)
+- [color\_detection](#color_detection)
+
+> [!NOTE]
+> 実行できるサンプルの一覧は、`camera_example.launch.py`にオプション`-s`を付けて実行することで確認できます。
+>
+> ```sh
+> ros2 launch crane_plus_examples_py camera_example.launch.py -s
+> Arguments (pass arguments as '<name>:=<value>'):
+>
+>     'example':
+>         Set an example executable name: [color_detection]
+>         (default: 'color_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)を使用しています。
+
+次のコマンドを実行します。
+
+```bash
+ros2 launch crane_plus_examples_py camera_example.launch.py example:='aruco_detection'
+```
+
+#### Videos
+[![crane_plus_aruco_detection_demo](https://rt-net.github.io/images/crane-plus/aruco_detection.gif)](https://youtu.be/m9dus6LCocc)
+
+[back to example list](#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_plus_examples_py camera_example.launch.py example:='color_detection'
+```
+
+#### Videos
+[![crane_plus_color_detection_demo](https://rt-net.github.io/images/crane-plus/color_detection.gif)](https://youtu.be/Kn0eWA7sALY)
+
+[back to example list](#examples)
+
+---

crane_plus_examples_py/config/crane_plus_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: 5.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_plus_examples_py/crane_plus_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, 'image_raw', self.image_callback, 10
+        )
+        self.camera_info_subscription = self.create_subscription(
+            CameraInfo, '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()