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/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/aruco_markers.pdf b/sciurus17_examples/aruco_markers.pdf new file mode 100644 index 0000000..932ba46 Binary files /dev/null and b/sciurus17_examples/aruco_markers.pdf differ 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..26e8c57 --- /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::toCvCopy(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; +} 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 特定の色の物体を検出して掴むコード例です。 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', ], },