ArUcoサンプルとピッキング動作の追加

.ci.rosinstall

@@ -5,4 +5,4 @@
 - git:
     uri: /~https://github.com/ros-controls/gz_ros2_control.git
     local-name: gz_ros2_control
-    version: master
+    version: humble

README.en.md

@@ -10,7 +10,7 @@ ROS 2 package suite of CRANE-X7.
 
 ## Table of Contents
 
-- [crane_x7_ros](#crane_x7_ros)
+- [crane\_x7\_ros](#crane_x7_ros)
   - [Table of Contents](#table-of-contents)
   - [Supported ROS 2 distributions](#supported-ros-2-distributions)
     - [ROS](#ros)
@@ -55,7 +55,7 @@ $ git clone -b ros2 /~https://github.com/rt-net/crane_x7_ros.git
 $ git clone -b ros2 /~https://github.com/rt-net/crane_x7_description.git
 
 # Install dependencies
-$ git clone /~https://github.com/ros-controls/gz_ros2_control.git
+$ git clone -b humble /~https://github.com/ros-controls/gz_ros2_control.git
 $ rosdep install -r -y -i --from-paths .
 
 # Build & Install

README.md

@@ -10,7 +10,7 @@ ROS 2 package suite of CRANE-X7.
 
 ## Table of Contents
 
-- [crane_x7_ros](#crane_x7_ros)
+- [crane\_x7\_ros](#crane_x7_ros)
   - [Table of Contents](#table-of-contents)
   - [Supported ROS 2 distributions](#supported-ros-2-distributions)
     - [ROS](#ros)
@@ -57,7 +57,7 @@ $ git clone -b ros2 /~https://github.com/rt-net/crane_x7_ros.git
 $ git clone -b ros2 /~https://github.com/rt-net/crane_x7_description.git
 
 # Install dependencies
-$ git clone /~https://github.com/ros-controls/gz_ros2_control.git
+$ git clone -b humble /~https://github.com/ros-controls/gz_ros2_control.git
 $ rosdep install -r -y -i --from-paths .
 
 # Build & Install

crane_x7_examples/CMakeLists.txt

@@ -17,8 +17,10 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(cv_bridge REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
+find_package(OpenCV REQUIRED COMPONENTS core)
 find_package(rclcpp REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 
@@ -29,12 +31,16 @@ set(executable_list
   pose_groupstate
   joint_values
   pick_and_place
+  pick_and_place_tf
+  aruco_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
     moveit_ros_planning_interface
+    OpenCV
     rclcpp
     tf2_geometry_msgs
   )

crane_x7_examples/README.md

@@ -2,25 +2,31 @@
 
 このパッケージはCRANE-X7 ROS 2パッケージのサンプルコード集です。
 
-- [crane_x7_examples](#crane_x7_examples)
+- [crane\_x7\_examples](#crane_x7_examples)
   - [準備（実機を使う場合）](#準備実機を使う場合)
     - [1. CRANE-X7本体をPCに接続する](#1-crane-x7本体をpcに接続する)
     - [2. USB通信ポートの接続を確認する](#2-usb通信ポートの接続を確認する)
-    - [3. move_groupとcontrollerを起動する](#3-move_groupとcontrollerを起動する)
-      - [標準モデルを使用する場合](#標準モデルを使用する場合)
+    - [3. move\_groupとcontrollerを起動する](#3-move_groupとcontrollerを起動する)
+      - [標準のCRANE-X7を使用する場合](#標準のcrane-x7を使用する場合)
       - [RealSense D435マウンタ搭載モデルを使用する場合](#realsense-d435マウンタ搭載モデルを使用する場合)
   - [準備 (Gazeboを使う場合)](#準備-gazeboを使う場合)
-    - [1. move_groupとGazeboを起動する](#1-move_groupとgazeboを起動する)
+    - [1. move\_groupとGazeboを起動する](#1-move_groupとgazeboを起動する)
   - [サンプルプログラムを実行する](#サンプルプログラムを実行する)
     - [Gazeboでサンプルプログラムを実行する場合](#gazeboでサンプルプログラムを実行する場合)
   - [Examples](#examples)
-    - [gripper_control](#gripper_control)
-    - [pose_groupstate](#pose_groupstate)
-    - [joint_values](#joint_values)
-    - [cartesian_path](#cartesian_path)
+    - [gripper\_control](#gripper_control)
       - [Videos](#videos)
-    - [pick_and_place](#pick_and_place)
+    - [pose\_groupstate](#pose_groupstate)
       - [Videos](#videos-1)
+    - [joint\_values](#joint_values)
+      - [Videos](#videos-2)
+    - [cartesian\_path](#cartesian_path)
+      - [Videos](#videos-3)
+    - [pick\_and\_place](#pick_and_place)
+      - [Videos](#videos-4)
+    - [aruco\_detection](#aruco_detection)
+      - [実行手順](#実行手順)
+      - [Videos](#videos-5)
 
 ## 準備（実機を使う場合）
 
@@ -42,7 +48,7 @@ USB通信ポートの設定については`crane_x7_control`の
 
 ### 3. move_groupとcontrollerを起動する
 
-#### 標準モデルを使用する場合
+#### 標準のCRANE-X7を使用する場合
 次のコマンドでmove_group (`crane_x7_moveit_config`)と
 controller (`crane_x7_control`)を起動します。
 
@@ -124,6 +130,9 @@ ros2 launch crane_x7_examples example.launch.py example:='gripper_control'
 
 <img src=https://rt-net.github.io/images/crane-x7/gazebo_gripper_example.gif width=500px />
 
+#### Videos
+[![crane_x7_gripper_control_demo](http://img.youtube.com/vi/uLRLkwbXUP0/hqdefault.jpg)](https://youtu.be/uLRLkwbXUP0)
+
 [back to example list](#examples)
 
 ---
@@ -143,6 +152,9 @@ ros2 launch crane_x7_examples example.launch.py example:='pose_groupstate'
 
 <img src=https://rt-net.github.io/images/crane-x7/gazebo_pose_groupstate.gif width=500px />
 
+#### Videos
+[![crane_x7_pose_groupstate_demo](http://img.youtube.com/vi/FH18dA_xcjM/hqdefault.jpg)](https://youtu.be/FH18dA_xcjM)
+
 [back to example list](#examples)
 
 ---
@@ -158,6 +170,9 @@ ros2 launch crane_x7_examples example.launch.py example:='joint_values'
 ```
 <img src= https://rt-net.github.io/images/crane-x7/gazebo_joint_values_example.gif width = 500px />
 
+#### Videos
+[![crane_x7_joint_values_demo](http://img.youtube.com/vi/skRwrrlUl4c/hqdefault.jpg)](https://youtu.be/skRwrrlUl4c)
+
 [back to example list](#examples)
 
 ---
@@ -174,8 +189,7 @@ ros2 launch crane_x7_examples example.launch.py example:='cartesian_path'
 ```
 
 #### Videos
-
-[![](http://img.youtube.com/vi/-Rt3zc3UXMM/sddefault.jpg)](https://youtu.be/-Rt3zc3UXMM)
+[![crane_x7_cartesian_path_demo](http://img.youtube.com/vi/XLhbUqsP2WA/hqdefault.jpg)](https://youtu.be/XLhbUqsP2WA)
 
 [back to example list](#examples)
 
@@ -202,6 +216,29 @@ CRANE-X7から20cm離れた位置にピッキング対象を設置します。
 
 #### Videos
 
-[![crane_x7_pick_and_place_demo](http://img.youtube.com/vi/_8xBgpgMhk8/hqdefault.jpg)](https://youtu.be/_8xBgpgMhk8)
+[![crane_x7_pick_and_place_demo](http://img.youtube.com/vi/S_MwSvG2tKw/hqdefault.jpg)](https://youtu.be/S_MwSvG2tKw)
+
+[back to example list](#examples)
+
+---
+
+### aruco_detection
+
+モノに取り付けたArUcoマーカをカメラで認識し、マーカ位置に合わせて掴むコード例です。[RealSense D435マウンタ](/~https://github.com/rt-net/crane_x7_Hardware/blob/master/3d_print_parts/v1.0/CRANE-X7_HandA_RealSenseD435マウンタ.stl)搭載モデルで実行することを想定しています。
+
+認識されたマーカの位置姿勢は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)を使用しています。
+
+#### 実行手順
+1. [aruco_markers.pdf](./aruco_markers.pdf)をA4紙に印刷し、一辺50mmの立方体に取り付けます
+1. [「RealSense D435マウンタ搭載モデルを使用する場合」](#realsense-d435マウンタ搭載モデルを使用する場合)の手順でCRANE-X7を起動します
+1. 次のコマンドを実行します
+    ```sh
+    ros2 launch crane_x7_examples camera_example.launch.py example:='aruco_detection'
+    ```
+1. マーカのついた立方体をカメラに写る位置に置きます
+
+#### Videos
+[![crane_x7_aruco_detection_demo](http://img.youtube.com/vi/eWzmG_jbTmM/hqdefault.jpg)](https://youtu.be/eWzmG_jbTmM)
 
 [back to example list](#examples)

crane_x7_examples/launch/camera_example.launch.py

@@ -0,0 +1,89 @@
+# Copyright 2022 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 os
+
+from ament_index_python.packages import get_package_share_directory
+from crane_x7_description.robot_description_loader import RobotDescriptionLoader
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import SetParameter
+from launch_ros.actions import Node
+import yaml
+
+
+def load_file(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, 'r') as file:
+            return file.read()
+    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+
+def load_yaml(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, 'r') as file:
+            return yaml.safe_load(file)
+    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+
+def generate_launch_description():
+    description_loader = RobotDescriptionLoader()
+
+    robot_description_semantic_config = load_file(
+        'crane_x7_moveit_config', 'config/crane_x7.srdf')
+    robot_description_semantic = {
+        'robot_description_semantic': robot_description_semantic_config}
+
+    kinematics_yaml = load_yaml('crane_x7_moveit_config', 'config/kinematics.yaml')
+
+    declare_example_name = DeclareLaunchArgument(
+        'example', default_value='aruco_detection',
+        description=('Set an example executable name: '
+                     '[aruco_detection]')
+    )
+
+    declare_use_sim_time = DeclareLaunchArgument(
+        'use_sim_time', default_value='false',
+        description=('Set true when using the gazebo simulator.')
+    )
+
+    picking_node = Node(name="pick_and_place_tf",
+                        package='crane_x7_examples',
+                        executable='pick_and_place_tf',
+                        output='screen',
+                        parameters=[{'robot_description': description_loader.load()},
+                                    robot_description_semantic,
+                                    kinematics_yaml])
+
+    detection_node = Node(name=[LaunchConfiguration('example'), '_node'],
+                          package='crane_x7_examples',
+                          executable=LaunchConfiguration('example'),
+                          output='screen')
+
+    return LaunchDescription([
+        declare_use_sim_time,
+        SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')),
+        picking_node,
+        detection_node,
+        declare_example_name
+    ])