aruco_detectionサンプルの追加

sciurus17_examples/CMakeLists.txt

@@ -127,6 +127,7 @@ set(executable_list
   pick_and_place_right_arm_waist
   pick_and_place_left_arm
   pick_and_place_tf
+  aruco_detection
   color_detection
   point_cloud_detection
 )

sciurus17_examples/README.md

@@ -19,6 +19,7 @@
     - [pick\_and\_place\_left\_arm](#pick_and_place_left_arm)
     - [head\_camera\_tracking](#head_camera_tracking)
     - [chest\_camera\_tracking](#chest_camera_tracking)
+    - [aruco\_detection](#aruco_detection)
     - [color\_detection](#color_detection)
     - [point\_cloud\_detection](#point_cloud_detection)
 
@@ -290,6 +291,26 @@ ros2 launch sciurus17_examples chest_camera_tracking.launch.py
 
 ---
 
+### aruco_detection
+
+モノに取り付けたArUcoマーカをカメラで検出し、マーカ位置に合わせて掴むコード例です。
+
+- マーカは[aruco_markers.pdf](./aruco_markers.pdf)をA4紙に印刷し、一辺50mmの立方体に取り付けます。
+- 検出されたマーカの位置姿勢はtfのフレームとして配信されます。
+- 各マーカはIDに対応した`tf`フレームとして配信され、ID0のマーカは`target_0`フレームになります。
+- 掴む対象は`target_0`に設定されています。
+- マーカ検出には[OpenCV](https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html)を使用しています。
+
+次のコマンドを実行します。
+
+```sh
+ros2 launch sciurus17_examples camera_example.launch.py example:='aruco_detection'
+```
+
+[Back to camera example list](#camera-examples)
+
+---
+
 ### color_detection
 
 特定の色の物体を検出して掴むコード例です。

sciurus17_examples/launch/camera_example.launch.py

@@ -27,7 +27,7 @@ def generate_launch_description():
         default_value='point_cloud_detection',
         description=(
             'Set an example executable name: '
-            '[color_detection, point_cloud_detection]'
+            '[aruco_detection, color_detection, point_cloud_detection]'
         ),
     )
 

sciurus17_examples/src/aruco_detection.cpp

@@ -0,0 +1,118 @@
+// Copyright 2026 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.
+
+// Reference:
+// https://docs.opencv.org/4.2.0/d5/dae/tutorial_aruco_detection.html
+// https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Broadcaster-Cpp.html
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "sensor_msgs/msg/camera_info.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include "opencv2/opencv.hpp"
+#include "opencv2/aruco.hpp"
+#include "opencv2/core/quaternion.hpp"
+#include "cv_bridge/cv_bridge.hpp"
+#include "tf2_ros/transform_broadcaster.h"
+#include "image_transport/image_transport.hpp"
+#include "image_transport/camera_subscriber.hpp"
+
+using std::placeholders::_1;
+using std::placeholders::_2;
+
+class ImageSubscriber : public rclcpp::Node
+{
+public:
+  ImageSubscriber()
+  : Node("aruco_detection")
+  {
+    camera_subscription_ = image_transport::create_camera_subscription(
+      this,
+      "/head_camera/color/image_raw",
+      std::bind(&ImageSubscriber::camera_callback, this, _1, _2),
+      "raw");
+
+    // ArUcoマーカのデータセットを読み込む
+    // DICT_6x6_50は6x6ビットのマーカが50個収録されたもの
+    marker_dict_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_50);
+
+    tf_broadcaster_ =
+      std::make_unique<tf2_ros::TransformBroadcaster>(*this);
+  }
+
+private:
+  image_transport::CameraSubscriber camera_subscription_;
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+  cv::Ptr<cv::aruco::Dictionary> marker_dict_;
+
+  void camera_callback(
+    const sensor_msgs::msg::Image::ConstSharedPtr & img_msg,
+    const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg)
+  {
+    auto cv_img = cv_bridge::toCvCopy(img_msg, img_msg->encoding);
+    cv::cvtColor(cv_img->image, cv_img->image, cv::COLOR_RGB2BGR);
+
+    // マーカID
+    std::vector<int> ids;
+    // 画像座標系上のマーカ頂点位置
+    std::vector<std::vector<cv::Point2f>> corners;
+    // マーカの検出
+    cv::aruco::detectMarkers(cv_img->image, marker_dict_, corners, ids);
+    // マーカの検出数
+    int n_markers = ids.size();
+
+    if (n_markers <= 0) {
+      return;
+    }
+
+    // カメラパラメータの読み込み
+    const auto CAMERA_MATRIX = cv::Mat(3, 3, CV_64F, const_cast<double *>(info_msg->k.data()));
+    const auto DIST_COEFFS = cv::Mat(1, 5, CV_64F, const_cast<double *>(info_msg->d.data()));
+    // マーカ一辺の長さ 0.04 [m]
+    const float MARKER_LENGTH = 0.04;
+    // マーカの回転ベクトルと位置ベクトル
+    std::vector<cv::Vec3d> rvecs, tvecs;
+    // 画像座標系上のマーカ位置を三次元のカメラ座標系に変換
+    cv::aruco::estimatePoseSingleMarkers(
+      corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS, rvecs, tvecs);
+
+    // tfの配信
+    for (int i = 0; i < n_markers; i++) {
+      geometry_msgs::msg::TransformStamped t;
+      t.header = img_msg->header;
+      t.child_frame_id = "target_" + std::to_string(ids[i]);
+      t.transform.translation.x = tvecs[i][0];
+      t.transform.translation.y = tvecs[i][1];
+      t.transform.translation.z = tvecs[i][2];
+      cv::Quatd cv_q = cv::Quatd::createFromRvec(rvecs[i]);
+      t.transform.rotation.x = cv_q.x;
+      t.transform.rotation.y = cv_q.y;
+      t.transform.rotation.z = cv_q.z;
+      t.transform.rotation.w = cv_q.w;
+      tf_broadcaster_->sendTransform(t);
+    }
+  }
+};
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<ImageSubscriber>());
+  rclcpp::shutdown();
+  return 0;
+}

sciurus17_examples_py/README.md

@@ -16,6 +16,7 @@
     - [waist\_control](#waist_control)
     - [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist)
     - [pick\_and\_place\_left\_arm](#pick_and_place_left_arm)
+    - [aruco\_detection](#aruco_detection)
     - [color\_detection](#color_detection)
 
 ## Setup
@@ -162,6 +163,25 @@ ros2 launch sciurus17_examples_py example.launch.py example:='pick_and_place_lef
 
 ---
 
+---
+
+### aruco_detection
+
+モノに取り付けたArUcoマーカをカメラで検出し、マーカ位置に合わせて掴むコード例です。
+
+- マーカは[aruco_markers.pdf](../sciurus17_examples/aruco_markers.pdf)をA4紙に印刷し、一辺50mmの立方体に取り付けます。
+- 検出されたマーカの位置姿勢はtfのフレームとして配信されます。
+- 各マーカはIDに対応した`tf`フレームとして配信され、ID0のマーカは`target_0`フレームになります。
+- 掴む対象は`target_0`に設定されています。
+- マーカ検出には[OpenCV](https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html)を使用しています。
+
+次のコマンドを実行します。
+
+```sh
+ros2 launch sciurus17_examples_py camera_example.launch.py example:='aruco_detection'
+```
+
+[Back to camera example list](#camera-examples)
 ### color_detection
 
 特定の色の物体を検出して掴むコード例です。

sciurus17_examples_py/launch/camera_example.launch.py

@@ -25,7 +25,7 @@ def generate_launch_description():
     declare_example_name = DeclareLaunchArgument(
         'example',
         default_value='color_detection',
-        description=('Set an example executable name: [color_detection]'),
+        description=('Set an example executable name: [aruco_detection, color_detection]'),
     )
 
     declare_use_sim_time = DeclareLaunchArgument(

sciurus17_examples_py/sciurus17_examples_py/aruco_detection.py

@@ -0,0 +1,107 @@
+# Copyright 2026 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 message_filters
+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_sub = message_filters.Subscriber(
+            self, Image, '/head_camera/color/image_raw'
+        )
+        self.info_sub = message_filters.Subscriber(
+            self, CameraInfo, '/head_camera/color/camera_info'
+        )
+        self.ts = message_filters.TimeSynchronizer([self.image_sub, self.info_sub], 10)
+        self.ts.registerCallback(self.camera_callback)
+
+        # ArUcoマーカのデータセットを読み込む
+        # DICT_6x6_50は6x6ビットのマーカが50個収録されたもの
+        self.marker_dict = aruco.getPredefinedDictionary(aruco.DICT_6X6_50)
+
+        self.tf_broadcaster = TransformBroadcaster(self)
+        self.bridge = CvBridge()
+
+    def camera_callback(self, img_msg, info_msg):
+        # 画像データをROSのメッセージからOpenCVの配列に変換
+        cv_img = self.bridge.imgmsg_to_cv2(img_msg, desired_encoding=img_msg.encoding)
+        cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2BGR)
+
+        # 画像座標系上のマーカ頂点位置
+        corners = []
+        # マーカID
+        ids = []
+        # マーカの検出
+        corners, ids, _ = aruco.detectMarkers(cv_img, self.marker_dict)
+
+        if ids is None:
+            return
+        # マーカの検出数
+        n_markers = len(ids)
+
+        # カメラパラメータ
+        CAMERA_MATRIX = np.array(info_msg.k).reshape(3, 3)
+        DIST_COEFFS = np.array(info_msg.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):
+            t = TransformStamped()
+            t.header = img_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 main(args=None):
+    rclpy.init(args=args)
+
+    image_subscriber = ImageSubscriber()
+    rclpy.spin(image_subscriber)
+
+    image_subscriber.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

sciurus17_examples_py/setup.py

@@ -34,6 +34,7 @@
             sciurus17_examples_py.pick_and_place_right_arm_waist:main',
             'pick_and_place_left_arm = sciurus17_examples_py.pick_and_place_left_arm:main',
             'pick_and_place_tf = sciurus17_examples_py.pick_and_place_tf:main',
+            'aruco_detection = sciurus17_examples_py.aruco_detection:main',
             'color_detection = sciurus17_examples_py.color_detection:main',
         ],
     },