color_detectionサンプルの追加 by KuraZuzu · Pull Request #183 · rt-net/sciurus17_ros

KuraZuzu · 2026-06-22T08:22:29Z

What does this implement/fix?

color_detectionサンプルを追加します。

crane_x7_rosのcolor_detectionサンプルをベースに実装しています。

移植に加えて、Sciurus17向けに以下の変更・修正を行っています。

sciurus17_examples/src/color_detection_2d.cpp
- 既存の2Dトラッキング用 color_detection コンポーネントを color_detection_2d にリネームしました。
- C++クラス名は ColorDetection2D に変更しています。
- head_camera_tracking.launch.py / chest_camera_tracking.launch.py の component 名・plugin 名も更新しています。
- 処理内容は変更していません。
sciurus17_examples/src/color_detection.cpp
- C++版 color_detection として、 Sciurus17 のカメラ構成に合わせて調整しました。
- 入力トピックを /head_camera/color/image_raw、/head_camera/aligned_depth_to_color/image_raw、/head_camera/color/camera_info に変更しています。
- 検出結果は target_0 の TF として配信します。
- Gazeboと実機でのRealsenseのエンコードデータが違うため、深度画像の encoding として 16UC1 と 32FC1 の両方に対応しています。
- HSV変換時に共有されたROS画像バッファを書き換えないよう、画像をコピーして処理するようにしました。
- TFの姿勢 quaternion に恒等回転 w=1.0 を明示しました。
- cv::moments() の名前空間を明示しました。
- 深度画像アクセス時の座標変換を明示的な static_cast<int> にしました。
sciurus17_examples/src/pick_and_place_tf.cpp
- pick_and_place_tf.cpp を実機/Gazeboでの動作に合わせて調整しました。
- 把持対象のZ方向下限が0.03程度となって青色キューブが把持対象外となってしまうため、0.04 から 0.02 に緩和しています。
sciurus17_examples_py/sciurus17_examples_py/color_detection.py
- Python版の color_detection サンプルとして追加しました。
- C++版と同様にRGB画像・深度画像・CameraInfoを同期して使用します。
- 16UC1 / 32FC1 の深度画像 encoding に対応しています。
- 検出結果を target_0 の TF として配信します。
- TFの姿勢 quaternion に恒等回転 w=1.0 を明示しています。
sciurus17_examples_py/sciurus17_examples_py/pick_and_place_tf.py
- Python版の pick_and_place_tf.py を追加しました。
- Sciurus17の左右腕・腰・左右グリッパ・首のMoveIt groupに合わせています。
- 物体位置のY座標に応じて左右の腕を切り替えるようにしています。
- グリッパの開始状態が可動範囲外にならないよう、現在角度を範囲内に丸める処理を追加しています。
その他
- C++版/Python版それぞれの launch、依存関係、README を更新しました。
- camera_example.launch.py から color_detection を起動できるようにしました。
- image_transport、message_filters、image_geometry、tf2_ros、cv_bridge など必要な依存関係を追加しています。
- README に color_detection の実行方法と、配信される TF frame の説明を追記しました。

Does this close any currently open issues?

しません。

How has this been tested?

実機とGazeboの両方で動作を確認しました。

Sciurus17を起動する

# 実機
$ ros2 launch sciurus17_examples demo.launch.py
# Gazebo
$ ros2 launch sciurus17_gazebo sciurus17_with_blue_cube.launch.py

color_detectioサンプルを実行する

# C++版サンプル
$ ros2 launch sciurus17_examples camera_example.launch.py example:='color_detection'
# Python版サンプル
$ ros2 launch sciurus17_examples_py camera_example.launch.py example:='color_detection'

Any other comments?

なし

Checklists

I have read the CONTRIBUTING guidelines.
- If there is no guideline in the repository, rt-net organization's guideline applies.
I have checked to ensure there aren't other open Pull Requests for the same change.

Copilot

Pull request overview

Sciurus17向けに、特定色（デフォルト青）の物体を深度画像込みで検出し、TF（target_0）として配信してピッキングにつなぐ color_detection サンプル（C++/Python）を追加するPRです。既存の2D版 color_detection コンポーネントは color_detection_2d にリネームし、追従系launchから参照する構成に整理しています。

Changes:

C++: 深度+CameraInfoを同期して3D位置を算出しTF配信する color_detection 実行ファイルを追加
C++: 既存2D検出コンポーネントを color_detection_2d に改名し、関連launchを更新
Python: color_detection と、それに対応する pick_and_place_tf とカメラ例launchを追加

Reviewed changes

Copilot reviewed 16 out of 16 changed files in this pull request and generated 12 comments.

Show a summary per file

File	Description
sciurus17_examples/src/color_detection.cpp	深度/CameraInfo同期で3D位置を算出しTF配信する新しい実行ファイルノードを実装
sciurus17_examples/src/color_detection_2d.cpp	既存2D検出を `ColorDetection2d` としてコンポーネント化
sciurus17_examples/include/sciurus17_examples/color_detection_2d.hpp	`ColorDetection2d` の新規ヘッダ/ガード名更新
sciurus17_examples/CMakeLists.txt	`color_detection_2d` をコンポーネント登録、`color_detection` を実行ファイルとして追加、依存追加
sciurus17_examples/package.xml	image_transport/message_filters/tf2_ros 依存追加
sciurus17_examples/README.md	`color_detection` サンプル説明を追加
sciurus17_examples/launch/head_camera_tracking.launch.py	参照コンポーネントを `color_detection_2d` に更新
sciurus17_examples/launch/chest_camera_tracking.launch.py	参照コンポーネントを `color_detection_2d` に更新
sciurus17_examples/launch/example.launch.py	例示リスト文言を更新
sciurus17_examples/launch/camera_example.launch.py	`color_detection` を例示リストに追加
sciurus17_examples_py/setup.py	`pick_and_place_tf` と `color_detection` エントリポイント追加
sciurus17_examples_py/sciurus17_examples_py/pick_and_place_tf.py	TFターゲットを見てピック&プレースするノードを新規追加
sciurus17_examples_py/sciurus17_examples_py/color_detection.py	Python版の深度同期・3D算出・TF配信ノードを新規追加
sciurus17_examples_py/launch/camera_example.launch.py	Python版のカメラ例launchを新規追加
sciurus17_examples_py/package.xml	Pythonサンプル実行に必要な依存を追加
sciurus17_examples_py/README.md	Python版 `color_detection` サンプル説明を追加

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>

KuraZuzu · 2026-06-23T06:22:22Z

@Kuwamai レビューをお願いします。

Kuwamai

@KuraZuzu
取り急ぎドキュメントについてコメントしました。
実装について後ほど確認します 🙇

Kuwamai · 2026-06-24T06:32:50Z

+特定の色の物体を検出して掴むコード例です。
+
+- デフォルトでは青い物体の位置をtfのフレームとして配信します。
+- `target_0`は、カメラ画像の`frame_id`を親フレームとするtfフレームとして配信します。


この文章では配信するフレーム名について説明してほしいです。
親フレームについてはユーザーがサンプルを使う上で影響しないと思うので、言及しなくて大丈夫です。

Kuwamai

@KuraZuzu コードについてもコメントしました 🙇

Kuwamai · 2026-06-24T07:22:37Z

+        cv_depth = self.bridge.imgmsg_to_cv2(depth_msg, desired_encoding=depth_msg.encoding)
+
+        # カメラから把持対象物の表面までの距離
+        depth = cv_depth[int(point[1]), int(point[0])]
+        if depth_msg.encoding == '16UC1':
+            front_distance = float(depth) / 1000.0
+        elif depth_msg.encoding == '32FC1':
+            front_distance = float(depth)
+        else:
+            self.get_logger().warn(f'Unsupported depth encoding: {depth_msg.encoding}')
+            return


この部分の実装の背景が知りたいです。
X7の実装では動作しませんでしたか?

X7の実装のまま動作させたところ、実機では動作したものの、Gazebo上では動作しませんでした。
実機とGazeboでRealsenseのエンコード方式が違っていたため、ひとまずここのサンプル上の条件分岐で対応としたという背景です。

Kuwamai · 2026-06-24T07:24:48Z

+    // 深度画像アクセス用に小数座標を整数インデックスに変換
+    const cv::Point point_int(static_cast<int>(point.x), static_cast<int>(point.y));
+
+    // エンコーディングの違いによる深度値の取得方法の違いに対応
+    if (depth_msg->encoding == "16UC1") {
+      front_distance = cv_depth->image.at<uint16_t>(point_int) / 1000.0;
+    } else if (depth_msg->encoding == "32FC1") {
+      front_distance = cv_depth->image.at<float>(point_int);
+    } else {
+      RCLCPP_WARN(
+        this->get_logger(),
+        "Unsupported depth encoding: %s",
+        depth_msg->encoding.c_str());
+      return;
+    }


Python版にコメントしてしまいましたが、どうようにこちらの意図も知りたいです。

KuraZuzu added 7 commits June 19, 2026 17:45

[WIP]color_detectionコンポーネントをcolor_detection2にリネーム

c22cb7a

[WIP]color_detectionの実装

56840ca

launchのメッセージやREADME.mdを更新

d4305e2

Python版をcrane_x7_rosから移植

a8c40c2

トピックをS17に向けたものにして構文ミスも修正

605284a

pick_and_place_tf.py と camera用のluanchの追加

3802a10

README追記

f8ed402

KuraZuzu self-assigned this Jun 22, 2026

KuraZuzu added the Type: Feature New Feature label Jun 22, 2026

KuraZuzu requested a review from Copilot June 22, 2026 08:22

Copilot started reviewing on behalf of KuraZuzu June 22, 2026 08:23 View session

Copilot AI reviewed Jun 22, 2026

View reviewed changes

KuraZuzu and others added 14 commits June 22, 2026 17:47

typo修正

4cdf77c

Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>

typo修正

57f00f9

Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>

READMEのframe_idに関する表現の繻子栄

30778b5

Python版のREADMEのframe_idの表現も修正

8f97323

現状だと高すぎて対象範囲に入らないため、把持対象の範囲を0.04->0.02mmに緩和

452ea5e

Realsense実機とGazebo上でのエンコーディングの違いを吸収

6dc5883

coments()をcv::moments()へと名前空間の明示

9382fa6

暗黙的な型変換を明示的なキャストに修正

aacd260

クォータニオンのデフォルトに恒等回転（w=1.0）を明示。Potential fix for pull request finding

fc91b8c

Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>

HSV変換時に共有されたROS画像バッファを書き換えないようにする Potential fix for pull request finding

cda1781

Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>

Potential fix for pull request finding

3629acb

Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>

Python版にもクォータニオンの恒常姿勢のデフォルト値を追加 Potential fix for pull request finding

3ce79a1

Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>

CIに従ってコードフォーマット修正

8399996

CollorDetection2Dのクラス名の修正

a7a77b2

KuraZuzu requested a review from Kuwamai June 23, 2026 06:21

KuraZuzu marked this pull request as ready for review June 23, 2026 06:29

Kuwamai requested changes Jun 24, 2026

View reviewed changes

Merge branch 'ros2' into feature/color_detection

c938571

Uh oh!

Conversation

KuraZuzu commented Jun 22, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

What does this implement/fix?

Does this close any currently open issues?

How has this been tested?

Any other comments?

Checklists

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

KuraZuzu commented Jun 23, 2026

Uh oh!

Kuwamai left a comment

Choose a reason for hiding this comment

Uh oh!

Kuwamai Jun 24, 2026

Choose a reason for hiding this comment

Uh oh!

Kuwamai left a comment

Choose a reason for hiding this comment

Uh oh!

Kuwamai Jun 24, 2026

Choose a reason for hiding this comment

Uh oh!

KuraZuzu Jun 24, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Kuwamai Jun 24, 2026

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

KuraZuzu commented Jun 22, 2026 •

edited

Loading

KuraZuzu Jun 24, 2026 •

edited

Loading