color_detectionサンプルの追加 (#183)

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

* [WIP]color_detectionの実装

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

* Python版をcrane_x7_rosから移植

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

* pick_and_place_tf.py と camera用のluanchの追加

* README追記

* typo修正

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

* typo修正

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

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

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

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

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

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

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

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

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

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

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

* Potential fix for pull request finding

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

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

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

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

* CollorDetection2Dのクラス名の修正

* README内でのサンプル説明の表現修正

* 深度フォーマットについてのコメントを追加

* python版でjoint_valuesを受け取って範囲外であれば補正する処理の削除

---------

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

Author: KuraZuzu

sciurus17_examples/CMakeLists.txt

@@ -22,19 +22,22 @@ find_package(cv_bridge REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(control_msgs REQUIRED)
 find_package(image_geometry REQUIRED)
+find_package(image_transport REQUIRED)
+find_package(message_filters REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
 find_package(OpenCV REQUIRED COMPONENTS core)
 find_package(pcl_conversions REQUIRED)
 find_package(pcl_ros REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(tf2_ros REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_components REQUIRED)
 
 include_directories(include)
 
 set(components_list
-  color_detection
+  color_detection_2d
   neck_jt_control
   object_tracker
   waist_jt_control
@@ -51,18 +54,21 @@ foreach(loop_var IN LISTS components_list)
     cv_bridge
     geometry_msgs
     image_geometry
+    image_transport
+    message_filters
     OpenCV
     rclcpp
+    tf2_ros
     control_msgs)
 endforeach()
 
-rclcpp_components_register_nodes(color_detection "sciurus17_examples::ColorDetection")
+rclcpp_components_register_nodes(color_detection_2d "sciurus17_examples::ColorDetection2D")
 rclcpp_components_register_nodes(neck_jt_control "sciurus17_examples::NeckJtControl")
 rclcpp_components_register_nodes(object_tracker "sciurus17_examples::ObjectTracker")
 rclcpp_components_register_nodes(waist_jt_control "sciurus17_examples::WaistJtControl")
 
 install(TARGETS
-  color_detection
+  color_detection_2d
   neck_jt_control
   object_tracker
   waist_jt_control
@@ -121,6 +127,7 @@ set(executable_list
   pick_and_place_right_arm_waist
   pick_and_place_left_arm
   pick_and_place_tf
+  color_detection
   point_cloud_detection
 )
 foreach(loop_var IN LISTS executable_list)
@@ -131,11 +138,14 @@ foreach(loop_var IN LISTS executable_list)
     cv_bridge
     geometry_msgs
     image_geometry
+    image_transport
+    message_filters
     moveit_ros_planning_interface
     OpenCV
     pcl_ros
     pcl_conversions
     rclcpp
+    tf2_ros
     tf2_geometry_msgs
     control_msgs
   )

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)
+    - [color\_detection](#color_detection)
     - [point\_cloud\_detection](#point_cloud_detection)
 
 ## Setup
@@ -289,12 +290,31 @@ ros2 launch sciurus17_examples chest_camera_tracking.launch.py
 
 ---
 
+### color_detection
+
+特定の色の物体を検出して掴むコード例です。
+
+- デフォルトでは青い物体の位置をtfのフレームとして配信します。
+- 検出した物体のフレーム名は `target_0` です。
+- 色の検出にはOpenCVを使用しています。
+- 検出した物体の距離は深度画像から取得します。
+
+次のコマンドを実行します。
+
+```sh
+ros2 launch sciurus17_examples camera_example.launch.py example:='color_detection'
+```
+
+[Back to example list](#examples)
+
+---
+
 ### point_cloud_detection
 
 点群から物体を検出して掴むコード例です。
 
 - 検出された物体位置はtfのフレームとして配信されます。
-- tfの`frame_id`は検出された順に`target_0`、`target_1`、`target_2`…に設定されます。
+- 検出した物体のフレーム名は `target_0`、`target_1`、`target_2`…です。
 - 掴む対象はSciurus17前方の0.3 mの範囲にある`target_0`に設定されています。
 - 物体検出には[Point Cloud Library](https://pointclouds.org/)を使用しています。
 

…e/sciurus17_examples/color_detection.hpp …ciurus17_examples/color_detection_2d.hppsciurus17_examples/include/sciurus17_examples/color_detection.hpp renamed to sciurus17_examples/include/sciurus17_examples/color_detection_2d.hpp sciurus17_examples/include/sciurus17_examples/color_detection.hpp renamed to sciurus17_examples/include/sciurus17_examples/color_detection_2d.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef SCIURUS17_EXAMPLES__COLOR_DETECTION_HPP_
-#define SCIURUS17_EXAMPLES__COLOR_DETECTION_HPP_
+#ifndef SCIURUS17_EXAMPLES__COLOR_DETECTION_2D_HPP_
+#define SCIURUS17_EXAMPLES__COLOR_DETECTION_2D_HPP_
 
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/point_stamped.hpp"
@@ -23,10 +23,10 @@
 namespace sciurus17_examples
 {
 
-class ColorDetection : public rclcpp::Node
+class ColorDetection2D : public rclcpp::Node
 {
 public:
-  explicit ColorDetection(const rclcpp::NodeOptions & options);
+  explicit ColorDetection2D(const rclcpp::NodeOptions & options);
 
 private:
   rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_subscription_;
@@ -39,4 +39,4 @@ class ColorDetection : public rclcpp::Node
 
 }  // namespace sciurus17_examples
 
-#endif  // SCIURUS17_EXAMPLES__COLOR_DETECTION_HPP_
+#endif  // SCIURUS17_EXAMPLES__COLOR_DETECTION_2D_HPP_

sciurus17_examples/launch/camera_example.launch.py

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

sciurus17_examples/launch/chest_camera_tracking.launch.py

@@ -35,10 +35,10 @@ def generate_launch_description():
         output='screen',
         composable_node_descriptions=[
             ComposableNode(
-                name='color_detection',
+                name='color_detection_2d',
                 namespace='chest_camera_tracking',
                 package='sciurus17_examples',
-                plugin='sciurus17_examples::ColorDetection',
+                plugin='sciurus17_examples::ColorDetection2D',
                 remappings=[('/image_raw', '/chest_camera/image_raw')],
                 extra_arguments=[{'use_intra_process_comms': True}],
             ),

sciurus17_examples/launch/example.launch.py

@@ -28,10 +28,10 @@ def generate_launch_description():
         description=(
             'Set an example executable name: '
             '[gripper_control, pose_groupstate, joint_values, neck_control, waist_control,'
-            'pick_and_place_right_arm_waist, pick_and_place_left_arm]'
+            'pick_and_place_right_arm_waist, pick_and_place_left_arm, head_camera_tracking, '
+            'chest_camera_tracking]'
         ),
     )
-
     declare_use_sim_time = DeclareLaunchArgument(
         'use_sim_time',
         default_value='false',

sciurus17_examples/launch/head_camera_tracking.launch.py

@@ -35,10 +35,10 @@ def generate_launch_description():
         output='screen',
         composable_node_descriptions=[
             ComposableNode(
-                name='color_detection',
+                name='color_detection_2d',
                 namespace='head_camera_tracking',
                 package='sciurus17_examples',
-                plugin='sciurus17_examples::ColorDetection',
+                plugin='sciurus17_examples::ColorDetection2D',
                 remappings=[('/image_raw', '/head_camera/color/image_raw')],
                 extra_arguments=[{'use_intra_process_comms': True}],
             ),

sciurus17_examples/package.xml

@@ -22,11 +22,14 @@
   <depend>cv_bridge</depend>
   <depend>geometry_msgs</depend>
   <depend>image_geometry</depend>
+  <depend>image_transport</depend>
+  <depend>message_filters</depend>
   <depend>libopencv-dev</depend>
   <depend>moveit_ros_planning_interface</depend>
   <depend>pcl_ros</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>