ros2ブランチの更新内容を反映

crane_x7/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>crane_x7</name>
-  <version>4.2.0</version>
+  <version>4.3.0</version>
   <description>ROS 2 package suite of CRANE-X7</description>
   <maintainer email="[email protected]">RT Corporation</maintainer>
   <license>Apache License 2.0</license>

crane_x7_control/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>crane_x7_control</name>
-  <version>4.2.0</version>
+  <version>4.3.0</version>
   <description>The CRANE-X7 control package</description>
   <maintainer email="[email protected]">RT Corporation</maintainer>
   <license>Apache License 2.0</license>

crane_x7_examples/CMakeLists.txt

@@ -19,6 +19,7 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(image_geometry REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
 find_package(OpenCV REQUIRED COMPONENTS core)
 find_package(pcl_conversions REQUIRED)
@@ -35,13 +36,15 @@ set(executable_list
   pick_and_place
   pick_and_place_tf
   aruco_detection
+  color_detection
   point_cloud_detection
 )
 foreach(loop_var IN LISTS executable_list)
   add_executable(${loop_var} src/${loop_var}.cpp)
   ament_target_dependencies(${loop_var}
     cv_bridge
     geometry_msgs
+    image_geometry
     moveit_ros_planning_interface
     OpenCV
     pcl_ros

crane_x7_examples/README.md

@@ -27,8 +27,10 @@
   - [Camera Examples](#camera-examples)
     - [aruco\_detection](#aruco_detection)
       - [Videos](#videos-5)
-    - [point\_cloud\_detection](#point_cloud_detection)
+    - [color\_detection](#color_detection)
       - [Videos](#videos-6)
+    - [point\_cloud\_detection](#point_cloud_detection)
+      - [Videos](#videos-7)
 
 ## 準備（実機を使う場合）
 
@@ -229,6 +231,7 @@ CRANE-X7から20cm離れた位置にピッキング対象を設置します。
 [「RealSense D435マウンタ搭載モデルを使用する場合」](#realsense-d435マウンタ搭載モデルを使用する場合)の手順に従って`demo.launch`を実行している状態で各サンプルを実行できます。
 
 - [aruco\_detection](#aruco_detection)
+- [color\_detection](#color_detection)
 - [point\_cloud\_detection](#point_cloud_detection)
 
 実行できるサンプルの一覧は、`camera_example.launch.py`にオプション`-s`を付けて実行することで表示できます。
@@ -244,11 +247,11 @@ Arguments (pass arguments as '<name>:=<value>'):
 
 ### aruco_detection
 
-モノに取り付けたArUcoマーカをカメラで認識し、マーカ位置に合わせて掴むコード例です。
+モノに取り付けたArUcoマーカをカメラで検出し、マーカ位置に合わせて掴むコード例です。
 マーカは[aruco_markers.pdf](./aruco_markers.pdf)をA4紙に印刷し、一辺50mmの立方体に取り付けて使用します。
 
-認識されたマーカの位置姿勢はtfのフレームとして配信されます。
-tfの`frame_id`はマーカIDごとに異なりID0のマーカの`frame_id`は`target_0`になります。掴む対象は`target_0`に設定されています。マーカ認識には[OpenCV](https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html)を使用しています。
+検出されたマーカの位置姿勢はtfのフレームとして配信されます。
+tfの`frame_id`はマーカIDごとに異なりID0のマーカの`frame_id`は`target_0`になります。掴む対象は`target_0`に設定されています。マーカ検出には[OpenCV](https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html)を使用しています。
 
 次のコマンドを実行します
 ```sh
@@ -262,13 +265,34 @@ ros2 launch crane_x7_examples camera_example.launch.py example:='aruco_detection
 
 ---
 
+### color_detection
+
+特定の色の物体を検出して掴むコード例です。
+
+デフォルトでは青い物体の位置をtfのフレームとして配信します。
+tfの`frame_id`は`target_0`です。
+色の検出には[OpenCV](https://docs.opencv.org/4.x/db/d8e/tutorial_threshold.html)を使用しています。
+検出した物体の距離は深度画像から取得します。
+
+次のコマンドを実行します
+```sh
+ros2 launch crane_x7_examples camera_example.launch.py example:='color_detection'
+```
+
+#### Videos
+[![crane_x7_color_detection_demo](http://img.youtube.com/vi/O8lqw7yemAI/hqdefault.jpg)](https://youtu.be/O8lqw7yemAI)
+
+[back to camera example list](#camera-examples)
+
+---
+
 ### point_cloud_detection
 
-点群から物体位置を認識して掴むコード例です。
+点群から物体を検出して掴むコード例です。
 
-認識された物体位置はtfのフレームとして配信されます。
-tfの`frame_id`は認識された順に`target_0`、`target_1`、`target_2`…に設定されます。掴む対象は`target_0`に設定されています。
-物体認識には[Point Cloud Library](https://pointclouds.org/)を使用しています。
+検出された物体位置はtfのフレームとして配信されます。
+tfの`frame_id`は検出された順に`target_0`、`target_1`、`target_2`…に設定されます。掴む対象は`target_0`に設定されています。
+物体検出には[Point Cloud Library](https://pointclouds.org/)を使用しています。
 
 次のコマンドを実行します
 ```sh

crane_x7_examples/launch/camera_example.launch.py

@@ -57,9 +57,9 @@ def generate_launch_description():
     kinematics_yaml = load_yaml('crane_x7_moveit_config', 'config/kinematics.yaml')
 
     declare_example_name = DeclareLaunchArgument(
-        'example', default_value='aruco_detection',
+        'example', default_value='color_detection',
         description=('Set an example executable name: '
-                     '[aruco_detection, point_cloud_detection]')
+                     '[color_detection, aruco_detection, point_cloud_detection]')
     )
 
     declare_use_sim_time = DeclareLaunchArgument(

crane_x7_examples/launch/camera_example.rviz

@@ -5,7 +5,7 @@ Panels:
     Property Tree Widget:
       Expanded: ~
       Splitter Ratio: 0.5
-    Tree Height: 154
+    Tree Height: 257
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -405,8 +405,6 @@ Visualization Manager:
           Value: false
         target_0:
           Value: true
-        target_1:
-          Value: true
         world:
           Value: false
       Marker Scale: 0.5
@@ -446,18 +444,15 @@ Visualization Manager:
                                 camera_link:
                                   camera_color_frame:
                                     camera_color_optical_frame:
-                                      {}
+                                      target_0:
+                                        {}
                                   camera_depth_frame:
                                     camera_depth_optical_frame:
                                       {}
                               crane_x7_gripper_finger_a_link:
                                 {}
                               crane_x7_gripper_finger_b_link:
                                 {}
-            target_0:
-              {}
-            target_1:
-              {}
       Update Interval: 0
       Value: true
     - Alpha: 1
@@ -509,6 +504,7 @@ Visualization Manager:
       Value: true
       Visibility:
         Grid: false
+        Image: true
         MotionPlanning: false
         PointCloud2: true
         TF: true
@@ -548,6 +544,20 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /image_thresholded
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -622,11 +632,13 @@ Window Geometry:
   Height: 1016
   Hide Left Dock: false
   Hide Right Dock: false
+  Image:
+    collapsed: false
   MotionPlanning:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000266000003a2fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000011b000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000015c0000018a0000016900fffffffb0000000c00430061006d00650072006101000002ec000000f10000002800ffffff000000010000010f000003a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000003a2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003b7000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000266000003a2fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000182000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001c30000021a0000016900ffffff00000001000001b2000003a2fc0200000005fb0000000c00430061006d006500720061010000003b000001c60000002800fffffffb0000000a0049006d0061006700650100000207000001d60000002800fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003a2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000314000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Tool Properties:

crane_x7_examples/launch/demo.launch.py

@@ -104,6 +104,7 @@ def generate_launch_description():
             launch_arguments={
                 'device_type': 'd435',
                 'pointcloud.enable': 'true',
+                'align_depth.enable': 'true',
             }.items()
         )
 

crane_x7_examples/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>crane_x7_examples</name>
-  <version>4.2.0</version>
+  <version>4.3.0</version>
   <description>CRANE-X7 examples package</description>
   <maintainer email="[email protected]">RT Corporation</maintainer>
   <license>Apache License 2.0</license>
@@ -18,6 +18,7 @@
   <depend>crane_x7_moveit_config</depend>
   <depend>cv_bridge</depend>
   <depend>geometry_msgs</depend>
+  <depend>image_geometry</depend>
   <depend>libopencv-dev</depend>
   <depend>moveit_ros_planning_interface</depend>
   <depend>pcl_ros</depend>