From 7dd52da38accbcd53bf44afa86e9ac0534a166ab Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Thu, 2 Jul 2026 14:18:53 +0900 Subject: [PATCH 1/5] =?UTF-8?q?Python=E7=89=88=E3=81=AEaruco=5Fdetection?= =?UTF-8?q?=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/camera_example.launch.py | 2 +- .../sciurus17_examples_py/aruco_detection.py | 107 ++++++++++++++++++ sciurus17_examples_py/setup.py | 1 + 3 files changed, 109 insertions(+), 1 deletion(-) create mode 100644 sciurus17_examples_py/sciurus17_examples_py/aruco_detection.py diff --git a/sciurus17_examples_py/launch/camera_example.launch.py b/sciurus17_examples_py/launch/camera_example.launch.py index 437993f..2aab23d 100644 --- a/sciurus17_examples_py/launch/camera_example.launch.py +++ b/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( diff --git a/sciurus17_examples_py/sciurus17_examples_py/aruco_detection.py b/sciurus17_examples_py/sciurus17_examples_py/aruco_detection.py new file mode 100644 index 0000000..aa95120 --- /dev/null +++ b/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() diff --git a/sciurus17_examples_py/setup.py b/sciurus17_examples_py/setup.py index 50feab6..e2e5356 100644 --- a/sciurus17_examples_py/setup.py +++ b/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', ], }, From cc2265806ef3e51e591523e611f68da739fc9eb6 Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Mon, 6 Jul 2026 18:20:25 +0900 Subject: [PATCH 2/5] =?UTF-8?q?C++=E3=82=B5=E3=83=B3=E3=83=97=E3=83=AB?= =?UTF-8?q?=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/CMakeLists.txt | 1 + .../launch/camera_example.launch.py | 2 +- sciurus17_examples/src/aruco_detection.cpp | 118 ++++++++++++++++++ 3 files changed, 120 insertions(+), 1 deletion(-) create mode 100644 sciurus17_examples/src/aruco_detection.cpp diff --git a/sciurus17_examples/CMakeLists.txt b/sciurus17_examples/CMakeLists.txt index 0d14b67..f34e2fb 100644 --- a/sciurus17_examples/CMakeLists.txt +++ b/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 ) diff --git a/sciurus17_examples/launch/camera_example.launch.py b/sciurus17_examples/launch/camera_example.launch.py index 23422cb..23d9cb8 100644 --- a/sciurus17_examples/launch/camera_example.launch.py +++ b/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]' ), ) diff --git a/sciurus17_examples/src/aruco_detection.cpp b/sciurus17_examples/src/aruco_detection.cpp new file mode 100644 index 0000000..c853d4b --- /dev/null +++ b/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 +#include +#include + +#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(*this); + } + +private: + image_transport::CameraSubscriber camera_subscription_; + std::unique_ptr tf_broadcaster_; + cv::Ptr 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::toCvShare(img_msg, img_msg->encoding); + cv::cvtColor(cv_img->image, cv_img->image, cv::COLOR_RGB2BGR); + + // マーカID + std::vector ids; + // 画像座標系上のマーカ頂点位置 + std::vector> 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(info_msg->k.data())); + const auto DIST_COEFFS = cv::Mat(1, 5, CV_64F, const_cast(info_msg->d.data())); + // マーカ一辺の長さ 0.04 [m] + const float MARKER_LENGTH = 0.04; + // マーカの回転ベクトルと位置ベクトル + std::vector 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()); + rclcpp::shutdown(); + return 0; +} From a382c66c8e63a10224077dbb9319fd0b65ce25a5 Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Tue, 7 Jul 2026 11:45:03 +0900 Subject: [PATCH 3/5] =?UTF-8?q?=E5=85=B1=E6=9C=89=E7=94=BB=E5=83=8F?= =?UTF-8?q?=E3=82=92=E7=9B=B4=E6=8E=A5=E7=B7=A8=E9=9B=86=E3=81=97=E3=81=A6?= =?UTF-8?q?=E3=81=84=E3=81=9F=E3=82=B3=E3=83=BC=E3=83=89=E3=81=AE=E4=BF=AE?= =?UTF-8?q?=E6=AD=A3=E3=80=82=E7=94=BB=E5=83=8F=E3=82=92=E3=82=B3=E3=83=94?= =?UTF-8?q?=E3=83=BC=E3=81=99=E3=82=8B=E3=82=88=E3=81=86=E3=81=AB=E5=A4=89?= =?UTF-8?q?=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/aruco_detection.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_examples/src/aruco_detection.cpp b/sciurus17_examples/src/aruco_detection.cpp index c853d4b..26e8c57 100644 --- a/sciurus17_examples/src/aruco_detection.cpp +++ b/sciurus17_examples/src/aruco_detection.cpp @@ -64,7 +64,7 @@ class ImageSubscriber : public rclcpp::Node const sensor_msgs::msg::Image::ConstSharedPtr & img_msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg) { - auto cv_img = cv_bridge::toCvShare(img_msg, img_msg->encoding); + auto cv_img = cv_bridge::toCvCopy(img_msg, img_msg->encoding); cv::cvtColor(cv_img->image, cv_img->image, cv::COLOR_RGB2BGR); // マーカID From 9b2b0ed337214d4fbfe263848df0600cbbc5c5c6 Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Tue, 7 Jul 2026 15:27:13 +0900 Subject: [PATCH 4/5] =?UTF-8?q?aruco=5Fmarker=E3=81=AEpdf=E3=83=95?= =?UTF-8?q?=E3=82=A1=E3=82=A4=E3=83=AB=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/aruco_markers.pdf | Bin 0 -> 28922 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 sciurus17_examples/aruco_markers.pdf diff --git a/sciurus17_examples/aruco_markers.pdf b/sciurus17_examples/aruco_markers.pdf new file mode 100644 index 0000000000000000000000000000000000000000..932ba468ce774e83d7dc2e6e98c57f465631f7b7 GIT binary patch literal 28922 zcmeHw2UJtb)-Z%nBOMJ2NGuRhBnd4+AaoE#0%8H_y$FQfTPR8sL{v~I78D!B3kp&L zQY=VNq$n*A5s)q|)O;tX_g=5}{qOy2ecyWTzKhE>?3pvOXV0F!=ggk7Gh56+Q+q#3 zN`^zMZt%2);{){_O^ICdl*8`-rdp1 z2_`KA!(utSeZ1_g-8cdm_Z?$9w&z%j|0}s;9xqO^ff+;}GL3TByW4%M0YG%DsNZ2f z7U%(>Gq84CPrdBjePGhSJ+Zx&Xz)XQYHML>pFoZGA#>L*& z2ev*`Hxx{69XS?DAJ{hp^rG(w{*)VE2k{*f!pPUgXMI@OcrQ1AaX`9W0|i5~kp5v}GLOFUib8IxSl&0wgbHJyx?&M@`l<$L>zONW32$49 z`z#qR>r#BKg1Me_ybXH%mhp)PgI4%R@$7a+cd6C5ZG42wBeb%!Oyn`b8Xf@8WkE@e; z@o2zZfh(_x>D(>_n_s@1xv%GJUkER>+H%flkoZxVk5G8OiO_WeltLZ$^{>Xazq)jh)xf*ry^K7pZSB`ZM8jJehWe@_bP@P2 zhIE0}1q2%NOp}D11OQMn=A}PSIBi(4u<-seY$S)9Dt0{%YV=XI=1f= z|0A(qryPbj>1+oq5i&?53~|uj+0n`8E36A>v6ly)zM8BHFvsu5gZip|2z6&4Zv%TT z4ZNEN-krWEV18S3FmiH#zA6Eu|8*%EZ&gbYLvPYl>!ZDW(jjlvr0R7nA*52MiM2>j zcXe7PoPGCsRnUKY(k(zOs_p`zN1fY2gf7<33Kgi?k$t@a!>`Str6#zdEo^4hL>8&i zt0yWXT2h}%c@$+i?B^*BBXF8|$mInXU59&{lO2>_pMrU;f`m%RE~lpNsNz1A5nl&g zeXL4eVwU%)X)7K$Lda_JC0v`6Jxw6aL0rj^51i|-ft0U|e(QM@+DT+S*&2^#4wCL* zc5iQe$PDU06cQl$cQ)fWX6^DJ-4An3AmDsUE2eJ*zWD1BD5@}8))~&dlEQ)ye-|Fb zP3&ZYM86FHYk`zQfZ9qF@bH?r4Wl&Xm?v_ddzSiPW?N=|AngL_Nk zs&~I9ts#f9Qf0}89;Q^m*Rrb_QrAdbuENPev!0a0^*I#f)kvV}kgn%W@%k>e*k=IR zX)&9((^+9^-Pe<6axlE91LN{_{Vbu~R6f*50InJaU=BQ*5vx1xCp6#@g>?^!OZTbB zwd4W1P?FHE%Lypw^aOCCt2Ffv^tK{mfv&Oy;5J7qdd^?~oI;j6#YHAZpmm%}7+gF* zN}|_YLm0q$4#At6v@i=9Ue--`IGr`pPut6Y?mivZh05Y}pbz%vsfI)kiB+#ZS$# zEU$Gi2SpX2I~S^@T7*I>YpQ6zgkJI}=lp5`WR}GeUpN_VejQB>iVCIu{ZOb7`Zf~_ z*8Ei_R~9HxCAdp%35bI8S&8iH#f=Po1&7cr(Rk*D>|rW%b!GNj(pQj#zs}yrk*M-! z8WI+)ayuKc4rcYK@>gvZ48KW%uFQuWQh~06O4S7MkbTHC%|--ueDz|O)H)at#&m>w z!fGWh$#SY|Sz^`fE4adPbt;cY8XK{CH%9*WkqW)bq{q zlQs;ki|cG{v?c)yf8BHr0G`9QAYu;#i@E052&Hi74909A;3xn#pneI}L!ug?PBMNwgDW;ZQ);5InXv&!hYe!j+30Q}pLU&zdV z5Jmv={pTiY`=_AS74C-oY>?0`gn#e$UIq!+0DpSu@W<`Fodgf~O%?67qk}SJ_q!E% zY;pG`Yz)PLH+*e2K^44{hrp%@1M$ZGg}{m<Q5Z6mnYPGy_ISuORYmHP zY6}{c;5QX|RxV&8Pv?h9c%fP{1S&a!VN{X9Ok*C+#gz7hdyX#5&V_-(96WZOi0FD8 z)j0|E1Tq8AmM#7Re&giY`hD3eO=L|;P|0c5M}3F^AXA69zJH=_{6kGl!b$Anm6|wX zuzLrWO|@kmJ!7eA+8qaAfR22hQclsFUzNlKn!X@n14nO5?wnfB}@ zQ#F-jvfxq}Y_pJXcC2H=OdWG$V<6DYbW=c9DVd7FdxY%F+DhVw3X2ZWk;_mvkpDBb zG|bF0KLQNplmX2|fs*AIj4NQVrp2Zt4UIr-0Lye&eVm|#`vSzX6R>a*yt_n5t5vj!~a3e7|xC7dM>VAXC1Ea-!t z<(9F}0E;^yPH-e3s|qF$Sk{2WoigB8&2yG+q4S~MPAi3~0W<^xM&z#g@vt3Sbdy-j zCWjKZ6baBUbI^0Ac6`^Wx-ihV;c(r-#*G2|HUsHuq!m{i7Sfm568jbzm22rWMM|q~ zTW6a{;to@ixNuVz<(Z z|MmEwEb-Uc<%S#~TiykWZ28&zSpO>VZ?->VWEB1d`y*8?Nf@<>X@QpAOZ{}%N{pfW z!_kfm7AFmXgm`vn@@(8b{6<);cE@V$E7!HV1 zJWQM0jjXy!;hZ1KG{u7``wsMyo8N{je|@YRJk)-TNR3ZImln-h;?^Hqk$RubiLbYr zkX-FZU4PtJ8TQ(ETFuQNqx7K*qc28!^A^F@^}%n>%0~qTMcwG0 z7sRR=dzy+Yaeo@&YzVG{v+Zk$>yhW7=ODz5+4ap>yBnG-#UTK%uXwSr(l9qKP^QrW}1ouE)GgF}1Q0RbVqfjFMKGrV>t?MtS&NGg=zp!6G2Y$KbqHvfG9Bnm1(=gLqWLT zcD8a;uMMOdJpIoVseADzpy@;1^2me-84X>ClV55E!PI7AJTknq=Ry?cUMQUC&vGJ> zUy(byCopOVb1Gy|k!fG81aYE54bunzNUw`iRcxX^Ju6ujGC+T-w(<0Dzzxg6zm}yy zv}9jl)c5@0@FzCh&+eVyUDAH@jQqcK5tBhm1HP7j=BoMQHzX9-ZZr?LW|@{NcI8Ww z_KueH*N@v_#$$K0RKl0U{sR7C)lyTWj__=RAe0)2Q~Aw1$Oky91kNP00k@v8-d65k z-GG#ULr=Op(a_<#oa}{JreFPiaKPy);5W3e2>2NdDX&HV@K?X2R^Sv9a1aUw9Ez4U zx=0uJzhczNN<@J5TEeoD#P&iK6jdpoWhjSQyRD8qQ@Sl9bpUdjCyGRI@z9S)JXG6+ zO@2_LAW^kZ@qeE^8Bw-x7nf;4JGKKpe8;adH8M{%2XHFU-SNcS0oYZP<|PC?tAzP{ z`h}r)D~`YlxY!lff03PR4D765Pz=^pOpAZNbEt_{iqgaxY&H`y~*$Yg;497@h{rU2a4SlxXFAPm8l z`BXB(zrl#i2srlb_uLoZ@32fw>jDUyV7?)fMjsr<#^8ROxhcgcegtLV33!d4Xc?XS zU=W-Hp(cai+ZQGS*~y67#^au#M&=bA0)O)ILIrpV+E2NUAy7bTw}^3#tHBl^YQmJ~ z@eHCTBh8RpM_E%wFs^7(OZV!NOCxA|JO(^zS!oPwJ44Ji!#&K1;R2ZH(s0sYhHzeN zemp-$imbws5zt*dBo4jd6i06 z5PCamfVPSiUDAjfO6eY5Lu#p*m6N>twS;z8)cRl=?V$pFwF;4D-nVFOjk%GwwXfN7~UUePtVhv z@T#k=%L4pq-#VJR%`AmKi4W#YFVKYA5YjFy(gkA!_5U(M^!~$tAVF*duQBHz{3Yd2tPuL| zSpMB#lF+jMf)$ebhEoK!=~)@=t($mkP=uuvh@(~Gfk)-AjET;kg?lt}(L#Mm4&X$v@!)kuhFnFzp4D3)mLgJALnCxJ z6x-u^nEQPiC#rC(lS>i4HiqYcLTykFBc|uk=!9rnJ8L5NGn3NM&PV!VQfxr$h0xF8 z9jp)e6C!~^6yGoxaU{vypLX{N)@rwbC~(G@$a!&zV+hFB?Dl3t1m)1BASsh^PBhNz0Xg;8-nh~fEZ3G+=)ruHi zYh|ahQ`T1#=*%#8DVM*+4@8@W;Jp`B{Kr=vBcaH{x?E!n9R>eeQ~-Z zf|whrAj~d#H=5T9^ynB@$WDfATsbosySb_Vpp}|fA_ITyZ7+J|kPrJ|Q}?X6bU={3 z*5Gti$xBSxY{a2fcSZt^*&D-WY{0&Y2)4N=Ox0w-X8S?dLiVOVOCd%c5@ZzTIXp;& zgQgaXO;yQ8HNr{YEyrrvkObqstXSR%Qf?0jl+E$UV|U?oKJ5thx}yqHaSS2Y@hv>m zW5~0+2+Ws|AU)XSmjoP`JmdU#Avr{B;7|`E7B<#de|sB_L8|Fdl*|`KYn}95KVOiC zbx4#idG632HvYROcXH$d3#1RLRn~_#cAw=b6_+_S{~DT!^K6rHRd{$dgQZG3yC{!e zbfC*gI0N)kXWOoB=EaCY%Gal6f_+vZ)Jgv)&Pnj+ORt$wdUu!*%W&z%L@za#F%N$b^uCJ$- z&d^7kW3JG%T?m&4MVG?>mQ^$^OiuDgKqx{FnP{s?Px~`fEBOqkfR`k`+i^)X zGd&W?SVb6Rsy%2;XFf4T!4KfQ8Zc`C$&-D30Jj^^#;j}0BHqsw2|;2v$@EuTDQT8) z0L5$#3SsJCjZUdmdi((B!h7s9??hyXYUlL!zUpdV!V~d@Tq_HTpU(J$<_>Ar&|grV z*&|H8HXOBJDoZ{$+pd_vDl1k?kIEiMCdxLAxTM3#I%c8s7|KT6Y>r=0cWmkuD_k=q zGM0B3jx&Qiv^u&fTeiyhu|EtK@E}h39t$Qgn7x_vL-;f#nuA6~nS~33McOQ23@3dy zQ2uMYOcEzns|lttj)KXM{E`QvdpK$jQm}K3!2at_SDTO^v$c(9_EH3+HK=v`GiPEX z@=+-+lo){|jQGv5Xse|gcjscQRMUw*iH7CBcwKe9OW;Zb2S# zWKVuC5zzF6<#()`*!a`kPvWfvqT}jAKQpmJv+I>SKqEP7>jpNu{VOn8Rq-$6lt#BH zV1#fsm3fgrG4p7@W^L4*Iu~-|6#Chn4EX4x`eXMqM-*`FGchhbo343f zJsg6xm2H(DR5Bq!@B7NQz6|^&3vq1Jq4VO5owDWK_Sc0HLob4=4D$ZYh#cEkz@J28 zD#%3L;z&tY_Hc-zLe&ROwk8#hz#2lT6SIjb_RXT7SIA=_p7d~mFOf{}Hypo2gxs7N z|44NKE`g!#`N+(we{-`PkcU7x3!{-pL49$JdL0{sUOHwHhdLoD`x6VU_ zF>xo(Gk@gI#KRj;mNOC-b#@>y9(;K)$aDCCKK-NuKPJ#jvbu!_dG{%`E$4AQRhhz; z%_ZX#z)bG_B|>8do=$dlz}e^-5=hy$gchQT6mc)Yg8jtLch$sFsf$eoB+H>sYKwKenJ>G7C?lP<5z2jY=%@$%s?MfI+&Am>etBLk7h(4EJE6+M?sCZY{I9W zb}lBjtuT*LeF+r9M=Dgav6V)bwMgHUGUD{zPSFsljqd;Jn2-98zDt09%>ixy6t@Xt zOz}_l=fA(Xd-p|?A{-_Rc$=SF21W`z|^gsV(5_89UXA*3F1lYTlF};O*hFJqvzCbBly}Uo5q?a&R99Vm+4!fg;&v5`ofzx z8O|Ph^Gl*sjMKf(A7b>52Cj8h@R{dI1x1wIODjvbZ!7j?QKDANroM8fY=$W&dT@z6 zzt%jwAJ-!`aaZ$3uoe1nQW-ep!k(3@F7KrbFVh`8SGw*gwp|PeV zZgZK>R~EEw9+>6rUN&6bciZL^Sto*YuYP)jrRvE;(%?3#=&K`JB1Ct6{&3th;|Rkf z)AoH`{6$3a$lKx%cKihy?G9N{tJuzy(AT?cCiKbp2au=6kHeI!oL9qwpos{-1 z^{{s1uQBsR%^lkgzGUius`T{vunn38k1 z)3m6el@ERr3N(4>@=;T2s?d5OU@r1(y_uT>pWjFO8L4fbN%n}exj5HdYoV{R1pH77 z@%9110;g?D#!jidOET+c4fH+X#MV4z{AN2z)+6qdy4HZ*Tafmp`^CB(oA+qwcJKeC zUm{hXMN-&;3({yL<50Lud~$J>>JY8I8UKm1>0Qc>wgvV!q~$G@np|c*j@whmqVm+c zW~lp0-;+@$+zCZYR8c#lF&-i7TRDZf?`2#QZ(EDlvocMDn2Ojaqf_dK0@=4dg`&^K zBW_5i#S4aOMeFJlTW-`J!sqpM3_o1GnQswFX&;oaoN%enf7AxeZ`W1`8Drl?v5~ z_c1Kn9lAesS7urj+d#?W(`x_7tYBZZYj@Sh8XYRf<88uU>IRmq6lPprc%J;yNbTaz zh36Vyaw?!%J!<}^%1uUE)N*ZG9^Px8e_HpVg-P5=qzoF`Zn{;mxl!`O!&Y{W4+fTF zMSC;VU6&P3AFEsH%-?)9qf1x*?{qNALK)Y~OeF!KOwEZZCVt=h6##_m^2O-n8D6^~%t7!Xx-f+I%Wi zKR!^i1k$^JD4ub7^$B~}XBa=-d;0yS)0#uVH?tZ|pM+Hi`B&%^FGZg!N(;qu#8kn( zUb3F495RrM^Y0+dExA=ZyNTy@t1>B|En8PsE%H*)u~OS;j5M(T$j^%s{r*h)r(xg{ zP+biTb!%^XI~eNg&70=*+Vp3r@0IA+DE{+>OIjYdIPsr)oMvA3KW6lP%UMz-y0V3D zCroU1TM#?!t>VGWV8b}w2m{cWXmLMHW^Dmpi4%6mh>SXCciL(RK%ydTI@rJP7&O!v zYHAJkB%WrU%#)kxy}H_H()WI-q~pEgWN1ejgTYE9xYoy*F+%o+vB1P=jTs;RtGWe{ zAP*y#F(a|x!2zs8)dam;sZ_plv%1*S#ZWaa(tu}e3 zFmdQ0=%P_k{Z?rj_~q;+3F$K^=`A%WxDt`k?MQbX9DeSwG+ku0XoK2!@!667*Z@7!Z1JWf zBs8J`e_4xB%*5@AQ_6Ur@c6h%`zNQx>i2!y!;Dsyp9d`;pB5h@WvL}*ne@4iJw#M) zVVK{2>Y7MV)urPM#cnLBgZ&S2lQ4!a%nZf788h3@xLRDO8`uPgB#e!@dNXF9mfAOr zUe1ohzpOr^uyEzo#PSKnJ5UwEF5S&C_F98?a_=10Y--xXl}55#UErZnhzicHUp$k{ z$Rd`W&y! zps|Z-x4TLZD0I}62gUKcs=_HRrNBE`I%sWZNfcrh@7}Mcw$td-{HA0(FR`1FTW|G6 zyVd}|D`oY$&LK;XLV_f%IE5o*JeYc64ABt>d6+IUNgV_^tAc+~!*()osG&L;{i0kv z7~T<1?mW*0)x{R^yMlY8?%&WbP@di8v#pHHJZkAWYYR-4WtA~Bnl~NPYR%cn z@Fe1Wp`tLGPVFnQX29kQRw%Kn|4```ELTY_v|nh7K{Cd&HkD2FIUCa(ZDTf}X-)O> z#=9ALcbrbynFG3~b@e<~j?mL{2U9#hGO?V!VVy3xrC}@OtVv?O%?ayvn?uO9bldb3 zdeBeu4m>i1#mL(>ZIA94!yl^nvCH#bJGZ~?gjKSYX<^zUrcUcIMPaT+;?@CSkFZif zGii6Zd~gYOM`C%z=rhiK@#%nyy}bgHk=!-jQ#K(xsyM=h!?*V(i#All&$FKbpN{0- zaT&JTb@wrzZEQnqtI@o*vWA4aIG>$ocmnIpY$P5fb*fe93XAKf3h18M4L!PB;z}{$ zn6ArCb8%(y${ndM4%q2<37V){;PQ88!z{(PFQ^z?+#@c4J0z^7{$5|<&{New)j*Bg zM>+4hzcap{UY2rO~utG2F$DH2*Y9scg9Aj_^Zk zcUDS4hBc}+Muc01CvIr>7uPDs=xSZPdC4Z$=BiDcP1Jx~y`fme1;T|AKHNRkk;1oP zw|8t)Z@nG(wzT+2@qrKaACMwC=$z%&)RFpG(c#Kjc6%{<)4@H1!h_oei(VdOsYO~* zWsYvWotTc3sRU*HehU@c*aj-DW>9uahbXobkDswV$}Jr_n1`~ zshG6s^d~2A*#~mvjW-LsO-vqYw_jIyru01IR^YY8eGugYU!7ZBPp9(x)zamC+=sbSbP^gb z=B5s9BS9=;(+W_hRfz|!LZVFCW&{We;f8g=Cdlb^qRu9I+X%i%5l+AnnKwc2!!Xa#GfX&p{8mNY5) zsGWE!5q3v;i>T1%;N5;+_ZHsv96n1K8{nW!NXeL8E*ksb^?2~!D^mV>%Ni5Uz1xLE zMf!MXas0LS2KDl1EuZ=if;&>n&n{mWYS=e)ZrYq`zmft@i~c40jb>V%d7Q0ozV2pS z3J9n6nC$zD-fTb<}%9gO2jkSTS$$vm(Fby z$N6unzDd#B<89(lo}RR(H+L{`_cgI76G)NEFTIXB_Sr8Dn|MFXe!Tk8{gudznHS4p zat?C-zK&IXYiCw!z}I{O?HF19eh9aO8eD0d!at}_}+Jh z*9{A+telD_KQ*X7RX6Of+}~`<$|fp(xL#D{ZI6n>m|T1MvemvF;0_eI}tN z7kirKqsJ?6&);75C;PGaclV5x%@|*?F|l~xl210z8*KV=^Giwj$+4;v)~23T+|J2% z$;Hxlf2llF;G)x(`#I+`VO#RmPnDm8OU5%lGy`PicV&2QhFa4A9{ZTzM4J%q~%onsT=Lx?Qu_XMtLNIcmuFqJ91C+Ec|7S>IpR^Y zdYO`!RK*-Rv}C@F=$$VM9ZaHS3Na9t=f)J zZo)Zmd$aDVI0up$-3Iwqef)j-fLEf(OOF_Z3*_GuSuf7Z#(sg_uKvT*6mgX}Q>SIn;Z%N@1ye-fhTk_+zWT z^W>e}_}&LqD)WTZ8_Tw{jVU8zI4bT^_Bj>*5cZWX0pe~BpZXS}XO@57ysqE${nPH1 zzFR#C`)&97`)j&4cCQ<|*Nxrl#_n}v_qwrr-PpZu>|Qr^uN%A9f7|Z$dn~0N&R)NU zsQGpG`twhB=<%Vv{}}NJh+wlG496D;4+D5$9d@?!2I4w>jb&p_4}bLiDEH?}tu?HD ztX=VrUjt8he~&nI!rsf<8Sf55prmAf*It?)D(Cx8iGV0QcD}YiJghy=I3w8i7AQF> zG|X&|y05b>pS8ZF~V<_6{6Km^6w5`RhNJyquhj9Lxds zy$nS^;HLkBxqmB@MoVLWs6juJ0U2q4`+q2tmIlm=Kb8Rzrv6xlQTQW1X%t!(nA{)R zu7BkFLzyg!9)#$}dT1H?b$~yVp^zAvKlXu=MauqE4=o1-*ZQ%ow5+VcAIAakK=!9H zq#PQM(jWQ-#_hc{vjgUPjT12x>bG+(e}p}Y)A1vbTL%`Vm$p8 zjtx*3B*%~O*yv%a4Co=uzK3`FO?>}>dacpS05-bO6Ki^6OHXk0#E722kS)j+WGQk8 zIYZB)=!pzHk)kKaUm+BL28BenB1e%g=!Me1CTes@mY%HFM$#*;qk+-O4C#q4Jz4MF ziw^nFlOTGsUd4b8Y0?vIdZIy3@btu;o~&24r$Yh1W+my+dfWBRJ?Po>WWC;cGdp_O z5qh$YYCXIDbiLU+ed{AcNpSpOkpM2vN6Diw(nvYvZ|lhK*PDO5nEWnS-uK1i`?^6V z_{X(G8iSJm-C6=n-?xQF8Y2zJ0nh+w0xUAW7j(K50}`r67h)|av!N{Psw9!Z`U5tLvm0vZ;z{}nb cwl30u&iqb+fZ%07P_SQR9wU#ze3SYA0WP5+L;wH) literal 0 HcmV?d00001 From f8b2798273cfff37c51a594cd96e9abaf8b1a19c Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Tue, 7 Jul 2026 15:27:39 +0900 Subject: [PATCH 5/5] =?UTF-8?q?README=E3=81=AB=E3=82=B5=E3=83=B3=E3=83=97?= =?UTF-8?q?=E3=83=AB=E3=81=AE=E8=AA=AC=E6=98=8E=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/README.md | 21 +++++++++++++++++++++ sciurus17_examples_py/README.md | 20 ++++++++++++++++++++ 2 files changed, 41 insertions(+) diff --git a/sciurus17_examples/README.md b/sciurus17_examples/README.md index b07b7d4..c172417 100644 --- a/sciurus17_examples/README.md +++ b/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 特定の色の物体を検出して掴むコード例です。 diff --git a/sciurus17_examples_py/README.md b/sciurus17_examples_py/README.md index a259acf..fc18dd5 100644 --- a/sciurus17_examples_py/README.md +++ b/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 特定の色の物体を検出して掴むコード例です。