Merge branch 'master' into moveit_py

rt-net · Dec 27, 2024 · 2611004 · 2611004
2 parents 8daa08f + 8198372
commit 2611004
Show file tree

Hide file tree

Showing 33 changed files with 1,125 additions and 219 deletions.
diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml
@@ -15,13 +15,13 @@ on:
 jobs:
   industrial_ci:
     strategy:
+      fail-fast: false
       matrix:
         env:
           - { ROS_DISTRO: jazzy, ROS_REPO: ros }
+          - { ROS_DISTRO: rolling, ROS_REPO: ros }
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v2
-      - name: Set environment variable for pip
-        run: echo "PIP_BREAK_SYSTEM_PACKAGES=1" >> $GITHUB_ENV
+      - uses: actions/checkout@v4
       - uses: "ros-industrial/industrial_ci@master"
         env: ${{ matrix.env }}
diff --git a/.gitignore b/.gitignore
@@ -55,4 +55,5 @@ docs/_build/
 
 # PyBuilder
 target/
-.pytest_cache
+.pytest_cache
+.vscode
diff --git a/crane_plus_description/crane_plus_description/robot_description_loader.py b/crane_plus_description/crane_plus_description/robot_description_loader.py
@@ -27,7 +27,6 @@ def __init__(self):
             'crane_plus.urdf.xacro')
         self.port_name = '/dev/ttyUSB0'
         self.use_gazebo = 'false'
-        self.use_ignition = 'false'
         self.use_camera = 'false'
         self.gz_control_config_package = ''
         self.gz_control_config_file_path = ''
@@ -38,7 +37,6 @@ def load(self):
                 self.robot_description_path,
                 ' port_name:=', self.port_name,
                 ' use_gazebo:=', self.use_gazebo,
-                ' use_ignition:=', self.use_ignition,
                 ' use_camera:=', self.use_camera,
                 ' gz_control_config_package:=', self.gz_control_config_package,
                 ' gz_control_config_file_path:=', self.gz_control_config_file_path

diff --git a/crane_plus_description/test/test_robot_description_loader.py b/crane_plus_description/test/test_robot_description_loader.py
@@ -52,17 +52,7 @@ def test_use_gazebo():
     rdl.use_gazebo = 'true'
     rdl.gz_control_config_package = 'crane_plus_description'
     rdl.gz_control_config_file_path = 'test/dummy_controllers.yaml'
-    assert 'gazebo_ros2_control/GazeboSystem' in exec_load(rdl)
-
-
-def test_use_ignition():
-    # use_gazeboとuse_ignitionが変更され、xacroにign_ros2_controlがセットされることを期待
-    rdl = RobotDescriptionLoader()
-    rdl.use_gazebo = 'true'
-    rdl.use_ignition = 'true'
-    rdl.gz_control_config_package = 'crane_plus_description'
-    rdl.gz_control_config_file_path = 'test/dummy_controllers.yaml'
-    assert 'ign_ros2_control/IgnitionSystem' in exec_load(rdl)
+    assert 'gz_ros2_control/GazeboSimSystem' in exec_load(rdl)
 
 
 def test_use_camera():

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,24 +26,15 @@
             name_link_3
             name_link_4
             name_link_hand
-            use_ignition
+            use_camera
             config_package
             config_file_path
             ">
 
     <gazebo>
-      <xacro:if value="${use_ignition}">
-        <plugin filename="libign_ros2_control-system.so" name="ign_ros2_control::IgnitionROS2ControlPlugin">
-          <parameters>$(find ${config_package})/${config_file_path}</parameters>
-        </plugin>
-      </xacro:if>
-
-      <xacro:unless value="${use_ignition}">
-        <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
-          <robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
-          <parameters>$(find ${config_package})/${config_file_path}</parameters>
-        </plugin>
-      </xacro:unless>
+      <plugin filename="libgz_ros2_control-system.so" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+      <parameters>$(find ${config_package})/${config_file_path}</parameters>
+      </plugin>
     </gazebo>
 
     <gazebo reference="${name_link_base}">
@@ -82,5 +73,54 @@
       <xacro:material_gazebo_white/>
     </gazebo>
 
+    <xacro:if value="$(arg use_camera)">
+      <gazebo>
+          <plugin filename="gz-sim-sensors-system" name="gz::sim::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.gazebo_ros2_control.xacro b/crane_plus_description/urdf/crane_plus.gazebo_ros2_control.xacro
@@ -18,18 +18,11 @@
             joint_4_upper_limit
             joint_hand_lower_limit
             joint_hand_upper_limit
-            use_ignition
             ">
 
     <ros2_control name="${name}" type="system">
       <hardware>
-        <xacro:if value="${use_ignition}">
-          <plugin>ign_ros2_control/IgnitionSystem</plugin>
-        </xacro:if>
-
-        <xacro:unless value="${use_ignition}">
-          <plugin>gazebo_ros2_control/GazeboSystem</plugin>
-        </xacro:unless>
+        <plugin>gz_ros2_control/GazeboSimSystem</plugin>
       </hardware>
 
       <joint name="${name_joint_1}">

diff --git a/crane_plus_description/urdf/crane_plus.urdf.xacro b/crane_plus_description/urdf/crane_plus.urdf.xacro
@@ -13,7 +13,6 @@
   <xacro:include filename="$(find crane_plus_description)/urdf/crane_plus.gazebo_ros2_control.xacro"/>
 
   <xacro:arg name="use_gazebo" default="false" />
-  <xacro:arg name="use_ignition" default="false" />
   <xacro:arg name="use_camera" default="false" />
   <xacro:arg name="port_name" default="/dev/ttyUSB0" />
   <xacro:arg name="baudrate" default="1000000" />
@@ -130,7 +129,7 @@
       name_link_3="${NAME_LINK_3}"
       name_link_4="${NAME_LINK_4}"
       name_link_hand="${NAME_LINK_HAND}"
-      use_ignition="$(arg use_ignition)"
+      use_camera="$(arg use_camera)"
       config_package="$(arg gz_control_config_package)" 
       config_file_path="$(arg gz_control_config_file_path)" />
 
@@ -150,8 +149,7 @@
       joint_4_lower_limit="${JOINT_4_LOWER_LIMIT}"
       joint_4_upper_limit="${JOINT_4_UPPER_LIMIT}"
       joint_hand_lower_limit="${JOINT_HAND_LOWER_LIMIT}"
-      joint_hand_upper_limit="${JOINT_HAND_UPPER_LIMIT}"
-      use_ignition="$(arg use_ignition)" />
+      joint_hand_upper_limit="${JOINT_HAND_UPPER_LIMIT}"/> 
   </xacro:if>
 
 </robot>
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
+```
+
 ## サンプルプログラムを実行する
 
 準備ができたらサンプルプログラムを実行します。