Gazebo環境にカメラを追加 (#83)

* カメラ追加 * use_sim_timeオプション追加 * カメラの画角調整 * 色検出サンプルの色しきい値調整 * 把持位置調整 * 現バージョンのデフォルト設定を反映 * ArUcoマーカ付きの立方体を追加 * ワールド、launchファイル追加 * use_cameraの有効化 * use_cameraを引数で切り替えられるように変更 * カメラスタンドのURDF修正
rt-net · Dec 3, 2024 · 2f0964e · 2f0964e
1 parent 4f353e6
commit 2f0964e
Show file tree

Hide file tree

Showing 22 changed files with 703 additions and 45 deletions.
diff --git a/crane_plus_description/urdf/camera.urdf.xacro b/crane_plus_description/urdf/camera.urdf.xacro
@@ -7,14 +7,23 @@
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
-          <box size="0.05 0.05 0.05" />
+          <box size="0.03 0.08 0.05" />
         </geometry>
-        <material name="Black" />
+        <material name="black">
+          <color rgba="0.2 0.2 0.2 1.0"/>
+        </material>
       </visual>
+
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <box size="0.03 0.08 0.05" />
+        </geometry>
+      </collision>
     </link>
 
     <joint name="camera_joint" type="fixed">
-      <origin xyz="0.103 0.1025 0.46" rpy="0 0 0" />
+      <origin xyz="0.10626 0.10253 0.46" rpy="0 0 0" />
       <parent link="${parent}" />
       <child link="camera_link" />
     </joint>

diff --git a/crane_plus_description/urdf/camera_stand.urdf.xacro b/crane_plus_description/urdf/camera_stand.urdf.xacro
@@ -7,16 +7,23 @@
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
-          <box size="0.02 0.02 0.285" />
+          <box size="0.02 0.285 0.02" />
         </geometry>
         <material name="grey">
           <color rgba="0.8 0.8 0.8 1.0" />
         </material>
       </visual>
+
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <box size="0.02 0.285 0.02" />
+        </geometry>
+      </collision>
     </link>
 
     <joint name="camera_stand_joint1" type="fixed">
-      <origin xyz="-0.05 0.08 0.01" rpy="${radians(90)} 0 0" />
+      <origin xyz="-0.052 0.08 0.01" rpy="0 0 0" />
       <parent link="${parent}" />
       <child link="camera_stand_p1" />
     </joint>
@@ -31,65 +38,93 @@
           <color rgba="0.8 0.8 0.8 1.0" />
         </material>
       </visual>
+
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <box size="0.02 0.02 0.44" />
+        </geometry>
+      </collision>
     </link>
 
     <joint name="camera_stand_joint2" type="fixed">
-      <origin xyz="-0.05 0.1025 0.24" rpy="0 0 0" />
-      <parent link="${parent}" />
+      <origin xyz="0 0.02253 0.23" rpy="0 0 0" />
+      <parent link="camera_stand_p1" />
       <child link="camera_stand_p2" />
     </joint>
 
     <link name="camera_stand_p3">
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
-          <box size="0.02 0.02 0.13" />
+          <box size="0.13 0.02 0.02" />
         </geometry>
         <material name="grey">
           <color rgba="0.8 0.8 0.8 1.0" />
         </material>
       </visual>
+
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <box size="0.129 0.02 0.02" />
+        </geometry>
+      </collision>
     </link>
 
     <joint name="camera_stand_joint3" type="fixed">
-      <origin xyz="0.02 0.1025 0.45" rpy="0 ${radians(90)} 0" />
-      <parent link="${parent}" />
+      <origin xyz="0.075 0 0.21" rpy="0 0 0" />
+      <parent link="camera_stand_p2" />
       <child link="camera_stand_p3" />
     </joint>
 
     <link name="camera_stand_p4">
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
-          <box size="0.02 0.02 0.05" />
+          <box size="0.055 0.02 0.02" />
         </geometry>
         <material name="grey">
           <color rgba="0.8 0.8 0.8 1.0" />
         </material>
       </visual>
+
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <box size="0.054 0.019 0.02" />
+        </geometry>
+      </collision>
     </link>
 
     <joint name="camera_stand_joint4" type="fixed">
-      <origin xyz="-0.015 -0.0525 0.01" rpy="0 ${radians(90)} 0" />
-      <parent link="${parent}" />
+      <origin xyz="0.0375 -0.1325 0" rpy="0 0 0" />
+      <parent link="camera_stand_p1" />
       <child link="camera_stand_p4" />
     </joint>
 
     <link name="camera_stand_p5">
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
-          <box size="0.02 0.02 0.05" />
+          <box size="0.055 0.02 0.02" />
         </geometry>
         <material name="grey">
           <color rgba="0.8 0.8 0.8 1.0" />
         </material>
       </visual>
+
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <box size="0.054 0.02 0.02" />
+        </geometry>
+      </collision>
     </link>
 
     <joint name="camera_stand_joint5" type="fixed">
-      <origin xyz="-0.015 0.2125 0.01" rpy="0 ${radians(90)} 0" />
-      <parent link="${parent}" />
+      <origin xyz="0.0375 0.1325 0" rpy="0 0 0" />
+      <parent link="camera_stand_p1" />
       <child link="camera_stand_p5" />
     </joint>
 

diff --git a/crane_plus_description/urdf/crane_plus.gazebo.xacro b/crane_plus_description/urdf/crane_plus.gazebo.xacro
@@ -26,6 +26,7 @@
             name_link_3
             name_link_4
             name_link_hand
+            use_camera
             use_ignition
             config_package
             config_file_path
@@ -82,5 +83,54 @@
       <xacro:material_gazebo_white/>
     </gazebo>
 
+    <xacro:if value="$(arg use_camera)">
+      <gazebo>
+        <plugin filename="ignition-gazebo-sensors-system" name="ignition::gazebo::systems::Sensors">
+          <render_engine>ogre2</render_engine>
+        </plugin>
+      </gazebo>
+
+      <gazebo reference="camera_color_optical_frame">
+        <sensor name="camera" type="camera">
+          <pose relative_to="camera_color_optical_frame">0 0 0 ${radians(90)} ${radians(-90)} 0</pose>
+          <update_rate>10</update_rate>
+          <always_on>true</always_on>
+          <topic>image_raw</topic>
+          <gz_frame_id>camera_color_optical_frame</gz_frame_id>
+          <camera name="camera">
+            <xacro:property name="camera_width" value="640"/>
+            <xacro:property name="camera_height" value="480"/>
+            <xacro:property name="camera_hfov" value="${radians(70)}"/>
+            <xacro:property name="camera_focal_length" value="${camera_width / (2 * tan(camera_hfov / 2))}"/>
+            <horizontal_fov>${camera_hfov}</horizontal_fov>
+            <image>
+              <width>${camera_width}</width>
+              <height>${camera_height}</height>
+            </image>
+            <clip>
+              <near>0.1</near>
+              <far>10.0</far>
+            </clip>
+            <lens>
+              <intrinsics>
+                <fx>${camera_focal_length}</fx>
+                <fy>${camera_focal_length}</fy>
+                <cx>${(camera_width + 1) / 2}</cx>
+                <cy>${(camera_height + 1) / 2}</cy>
+                <s>1.0</s>
+              </intrinsics>
+              <projection>
+                <p_fx>${camera_focal_length}</p_fx>
+                <p_fy>${camera_focal_length}</p_fy>
+                <p_cx>${(camera_width + 1) / 2}</p_cx>
+                <p_cy>${(camera_height + 1) / 2}</p_cy>
+                <tx>0</tx>
+                <ty>0</ty>
+              </projection>
+            </lens>
+          </camera>
+        </sensor>
+      </gazebo>
+    </xacro:if>
   </xacro:macro>
 </robot>
diff --git a/crane_plus_description/urdf/crane_plus.urdf.xacro b/crane_plus_description/urdf/crane_plus.urdf.xacro
@@ -130,6 +130,7 @@
       name_link_3="${NAME_LINK_3}"
       name_link_4="${NAME_LINK_4}"
       name_link_hand="${NAME_LINK_HAND}"
+      use_camera="$(arg use_camera)"
       use_ignition="$(arg use_ignition)"
       config_package="$(arg gz_control_config_package)" 
       config_file_path="$(arg gz_control_config_file_path)" />

diff --git a/crane_plus_examples/README.md b/crane_plus_examples/README.md
@@ -53,6 +53,28 @@ $ ros2 launch crane_plus_examples demo.launch.py port_name:=/dev/ttyUSB0 use_cam
 $ ros2 launch crane_plus_gazebo crane_plus_with_table.launch.py
 ```
 
+#### Webカメラ搭載モデルを使用する場合
+
+Webカメラ搭載モデルの場合は、次のコマンドを実行してください。
+
+```sh
+$ ros2 launch crane_plus_gazebo crane_plus_with_table.launch.py use_camera:=true
+```
+
+CRANE+ V2の前にArUcoマーカ付きのBoxを置いたシミュレータ環境を使用する場合は次のコマンドを実行します。
+[aruco\_detection](#aruco_detection)サンプルを実行する際に使用することを想定しています。
+
+```sh
+$ ros2 launch crane_plus_gazebo crane_plus_with_aruco_cube.launch.py use_camera:=true
+```
+
+CRANE+ V2の前に赤いBoxを置いたシミュレータ環境を使用する場合は次のコマンドを実行します。
+[color\_detection](#color_detection)サンプルを実行する際に使用すること想定しています。
+
+```sh
+$ ros2 launch crane_plus_gazebo crane_plus_with_red_cube.launch.py use_camera:=true
+```
+
 ## サンプルプログラムを実行する
 
 準備ができたらサンプルプログラムを実行します。

diff --git a/crane_plus_examples/launch/camera_example.launch.py b/crane_plus_examples/launch/camera_example.launch.py
@@ -20,6 +20,7 @@
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
+from launch_ros.actions import SetParameter
 import yaml
 
 
@@ -46,7 +47,14 @@ def load_yaml(package_name, file_path):
 
 
 def generate_launch_description():
+    declare_use_camera = DeclareLaunchArgument(
+        'use_camera',
+        default_value='true',
+        description='Use camera.'
+    )
+
     description_loader = RobotDescriptionLoader()
+    description_loader.use_camera = LaunchConfiguration('use_camera')
 
     robot_description_semantic_config = load_file(
         'crane_plus_moveit_config', 'config/crane_plus.srdf')
@@ -61,6 +69,11 @@ def generate_launch_description():
                      '[color_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_plus_examples',
                         executable='pick_and_place_tf',
@@ -75,6 +88,9 @@ def generate_launch_description():
                           output='screen')
 
     return LaunchDescription([
+        declare_use_camera,
+        declare_use_sim_time,
+        SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')),
         detection_node,
         picking_node,
         declare_example_name

diff --git a/crane_plus_examples/launch/example.launch.py b/crane_plus_examples/launch/example.launch.py
@@ -47,7 +47,14 @@ def load_yaml(package_name, file_path):
 
 
 def generate_launch_description():
+    declare_use_camera = DeclareLaunchArgument(
+        'use_camera',
+        default_value='false',
+        description='Use camera.'
+    )
+
     description_loader = RobotDescriptionLoader()
+    description_loader.use_camera = LaunchConfiguration('use_camera')
 
     robot_description_semantic_config = load_file(
         'crane_plus_moveit_config', 'config/crane_plus.srdf')
@@ -76,6 +83,7 @@ def generate_launch_description():
                                     kinematics_yaml])
 
     return LaunchDescription([
+        declare_use_camera,
         declare_use_sim_time,
         SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')),
         declare_example_name,

diff --git a/crane_plus_examples/src/color_detection.cpp b/crane_plus_examples/src/color_detection.cpp
@@ -65,10 +65,10 @@ class ImageSubscriber : public rclcpp::Node
     if (camera_info_) {
       // 赤い物体を検出するようにHSVの範囲を設定
       // 周囲の明るさ等の動作環境に合わせて調整
-      const int LOW_H_1 = 0, HIGH_H_1 = 20;
-      const int LOW_H_2 = 160, HIGH_H_2 = 179;
+      const int LOW_H_1 = 0, HIGH_H_1 = 10;
+      const int LOW_H_2 = 170, HIGH_H_2 = 179;
       const int LOW_S = 100, HIGH_S = 255;
-      const int LOW_V = 50, HIGH_V = 255;
+      const int LOW_V = 100, HIGH_V = 255;
 
       // ウェブカメラの画像を受け取る
       auto cv_img = cv_bridge::toCvShare(msg, msg->encoding);

diff --git a/crane_plus_examples/src/pick_and_place_tf.cpp b/crane_plus_examples/src/pick_and_place_tf.cpp
@@ -155,7 +155,7 @@ class PickAndPlaceTf : public rclcpp::Node
     const double GRIPPER_OFFSET = 0.13;
     double gripper_offset_x = GRIPPER_OFFSET * std::cos(theta_rad);
     double gripper_offset_y = GRIPPER_OFFSET * std::sin(theta_rad);
-    if (!control_arm(x - gripper_offset_x, y - gripper_offset_y, 0.05, 0, 90, theta_deg)) {
+    if (!control_arm(x - gripper_offset_x, y - gripper_offset_y, 0.04, 0, 90, theta_deg)) {
       // アーム動作に失敗した時はpick_and_placeを中断して待機姿勢に戻る
       control_arm(0.0, 0.0, 0.17, 0, 0, 0);
       return;
@@ -168,7 +168,7 @@ class PickAndPlaceTf : public rclcpp::Node
     control_arm(0.0, 0.0, 0.17, 0, 90, 0);
 
     // 下ろす
-    control_arm(0.0, -0.15, 0.06, 0, 90, -90);
+    control_arm(0.0, -0.15, 0.05, 0, 90, -90);
 
     // ハンドを開く
     control_gripper(GRIPPER_OPEN);

diff --git a/crane_plus_gazebo/CMakeLists.txt b/crane_plus_gazebo/CMakeLists.txt
@@ -7,6 +7,7 @@ install(DIRECTORY
   launch
   worlds
   gui
+  models
   DESTINATION share/${PROJECT_NAME}/
 )
 

diff --git a/crane_plus_gazebo/README.md b/crane_plus_gazebo/README.md
@@ -15,4 +15,22 @@ CRANE+ V2 の[Gazebo](https://gazebosim.org/home)
 $ ros2 launch crane_plus_gazebo crane_plus_with_table.launch.py
 ```
 
+カメラ付きモデルを使用する場合は下記コマンドを実行します。
+
+```sh
+$ ros2 launch crane_plus_gazebo crane_plus_with_table.launch.py use_camera:=true
+```
+
+CRANE+ V2の前にArUcoマーカ付きのBoxを置いたシミュレータ環境を使用する場合は下記コマンドを実行します。
+
+```sh
+$ ros2 launch crane_plus_gazebo crane_plus_with_aruco_cube.launch.py
+```
+
+CRANE+ V2の前に赤いBoxを置いたシミュレータ環境を使用する場合は下記コマンドを実行します。
+
+```sh
+$ ros2 launch crane_plus_gazebo crane_plus_with_red_cube.launch.py
+```
+
 ![crane_plus_ignition](https://rt-net.github.io/images/crane-plus/crane_plus_ignition.png)
-Original file line number
+Diff line change
@@ Expand Up / @@ -7,6 +7,7 @@ install(DIRECTORY @@
       launch
       worlds
       gui
+      models
       DESTINATION share/${PROJECT_NAME}/
     )
@@ Expand Down @@