crane_plus_ignition パッケージを追加し、Gazeboの使用を非推奨にする

.docker/source/Dockerfile

@@ -29,6 +29,12 @@ RUN mkdir -p /root/.gazebo/models && \
     git pull origin master && \
     rm -rf .git
 
+# Download ignition model
+RUN ign fuel download -v 4 -u https://fuel.ignitionrobotics.org/1.0/openrobotics/models/sun && \
+    ign fuel download -v 4 -u https://fuel.ignitionrobotics.org/1.0/openrobotics/models/ground%20plane && \
+    ign fuel download -v 4 -u https://fuel.ignitionrobotics.org/1.0/openrobotics/models/wood%20cube%205cm && \
+    ign fuel download -v 4 -u https://fuel.ignitionrobotics.org/1.0/openrobotics/models/table
+
 # Edit entrypoint to source overlay setup file
 WORKDIR $OVERLAY_WS
 RUN sed --in-place --expression \

README.md

@@ -89,9 +89,9 @@ $ ros2 launch crane_plus_examples example.launch.py example:='gripper_control'
 - crane_plus_examples
   - [README](./crane_plus_examples/README.md)
   - CRANE+V2のサンプルコード集です
-- crane_plus_gazebo
-  - [README](./crane_plus_gazebo/README.md)
-  - CRANE+V2のGazeboシミュレーションパッケージです
+- crane_plus_ignition
+  - [README](./crane_plus_ignition/README.md)
+  - CRANE+V2のIgnition Gazeboシミュレーションパッケージです
 - crane_plus_moveit_config
   - [README](./crane_plus_moveit_config/README.md)
   - CRANE+V2の`moveit2`設定ファイルです

crane_plus_description/crane_plus_description/robot_description_loader.py

@@ -27,11 +27,13 @@ def __init__(self):
             'crane_plus.urdf.xacro')
         self.port_name = '/dev/ttyUSB0'
         self.use_gazebo = 'false'
+        self.use_ignition = 'false'
 
     def load(self):
         return Command([
                 'xacro ',
                 self.robot_description_path,
                 ' port_name:=', self.port_name,
-                ' use_gazebo:=', self.use_gazebo
+                ' use_gazebo:=', self.use_gazebo,
+                ' use_ignition:=', self.use_ignition
                 ])

crane_plus_description/package.xml

@@ -13,6 +13,7 @@
   <buildtool_depend>ament_cmake_python</buildtool_depend>
 
   <depend>gazebo_ros2_control</depend>
+  <depend>ign_ros2_control</depend>
   <depend>joint_state_publisher_gui</depend>
   <depend>launch</depend>
   <depend>robot_state_publisher</depend>

crane_plus_description/test/test_robot_description_loader.py

@@ -47,7 +47,15 @@ def test_port_name():
 
 
 def test_use_gazebo():
-    # use_gazeboが変更され、xacroにgazeboタグがセットされることを期待
+    # use_gazeboが変更され、xacroにgazebo_ros2_controlがセットされることを期待
     rdl = RobotDescriptionLoader()
     rdl.use_gazebo = 'true'
-    assert '<gazebo>' in exec_load(rdl)
+    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'
+    assert 'ign_ros2_control/IgnitionSystem' in exec_load(rdl)

crane_plus_description/urdf/crane_plus.gazebo.xacro

@@ -1,55 +1,85 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
+  <xacro:macro name="material_gazebo_black">
+    <visual>
+      <material>
+        <ambient>0 0 0 1</ambient>
+        <diffuse>0 0 0 1</diffuse>
+        <specular>0.1 0.1 0.1 5.0</specular>
+      </material>
+    </visual>
+  </xacro:macro>
+
+  <xacro:macro name="material_gazebo_white">
+    <visual>
+      <material>
+        <ambient>1 1 1 1</ambient>
+        <diffuse>1 1 1 1</diffuse>
+        <specular>0.1 0.1 0.1 128.0</specular>
+      </material>
+    </visual>
+  </xacro:macro>
+
   <xacro:macro name="gazebo_robot_settings"
     params="name_link_base
             name_link_1
             name_link_2
             name_link_3
             name_link_4
-            name_link_hand">
+            name_link_hand
+            use_ignition
+            ">
 
     <gazebo>
-      <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
-        <robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
-        <parameters>$(find crane_plus_control)/config/crane_plus_controllers.yaml</parameters>
-      </plugin>
+      <xacro:if value="${use_ignition}">
+        <plugin filename="libign_ros2_control-system.so" name="ign_ros2_control::IgnitionROS2ControlPlugin">
+          <parameters>$(find crane_plus_control)/config/crane_plus_controllers.yaml</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 crane_plus_control)/config/crane_plus_controllers.yaml</parameters>
+        </plugin>
+      </xacro:unless>
     </gazebo>
 
     <gazebo reference="${name_link_base}">
       <mu1>0.2</mu1>
       <mu2>0.2</mu2>
-      <material>Gazebo/Black</material>
+      <xacro:material_gazebo_black/>
     </gazebo>
 
     <gazebo reference="${name_link_1}">
       <mu1>0.2</mu1>
       <mu2>0.2</mu2>
-      <material>Gazebo/White</material>
+      <xacro:material_gazebo_white/>
     </gazebo>
 
     <gazebo reference="${name_link_2}">
       <mu1>0.2</mu1>
       <mu2>0.2</mu2>
-      <material>Gazebo/Black</material>
+      <xacro:material_gazebo_black/>
     </gazebo>
 
     <gazebo reference="${name_link_3}">
       <mu1>0.2</mu1>
       <mu2>0.2</mu2>
-      <material>Gazebo/White</material>
+      <xacro:material_gazebo_white/>
     </gazebo>
 
     <gazebo reference="${name_link_4}">
       <mu1>0.2</mu1>
       <mu2>0.2</mu2>
-      <material>Gazebo/Black</material>
+      <xacro:material_gazebo_black/>
     </gazebo>
 
     <gazebo reference="${name_link_hand}">
       <mu1>0.2</mu1>
       <mu2>0.2</mu2>
-      <material>Gazebo/White</material>
+      <xacro:material_gazebo_white/>
     </gazebo>
 
   </xacro:macro>

crane_plus_description/urdf/crane_plus.gazebo_ros2_control.xacro

@@ -17,11 +17,19 @@
             joint_4_lower_limit
             joint_4_upper_limit
             joint_hand_lower_limit
-            joint_hand_upper_limit">
+            joint_hand_upper_limit
+            use_ignition
+            ">
 
     <ros2_control name="${name}" type="system">
       <hardware>
-        <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+        <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>
       </hardware>
 
       <joint name="${name_joint_1}">

crane_plus_description/urdf/crane_plus.urdf.xacro

@@ -13,6 +13,7 @@
   <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="port_name" default="/dev/ttyUSB0" />
   <xacro:arg name="baudrate" default="1000000" />
   <xacro:arg name="timeout_seconds" default="5.0" />
@@ -124,7 +125,8 @@
       name_link_2="${NAME_LINK_2}"
       name_link_3="${NAME_LINK_3}"
       name_link_4="${NAME_LINK_4}"
-      name_link_hand="${NAME_LINK_HAND}" />
+      name_link_hand="${NAME_LINK_HAND}"
+      use_ignition="$(arg use_ignition)" />
 
     <xacro:crane_plus_gazebo_ros2_control_settings 
       name="crane_plus"
@@ -142,7 +144,8 @@
       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}" />
+      joint_hand_upper_limit="${JOINT_HAND_UPPER_LIMIT}"
+      use_ignition="$(arg use_ignition)" />
   </xacro:if>
 
 </robot>

crane_plus_examples/README.md

@@ -30,17 +30,17 @@ controller (`crane_plus_control`)を起動します。
 $ ros2 launch crane_plus_examples demo.launch.py port_name:=/dev/ttyUSB0
 ```
 
-## 準備（Gazeboを使う場合）
+## 準備（Ignition Gazeboを使う場合）
 
-![crane_plus_gazebo](https://rt-net.github.io/images/crane-plus/crane_plus_gazebo.png)
+![crane_plus_ignition](https://rt-net.github.io/images/crane-plus/crane_plus_ignition.png)
 
-### 1. move_groupとgazeboを起動する
+### 1. move_groupとIgnition Gazeboを起動する
 
 次のコマンドでmove_group (`crane_plus_moveit_config`)と
-Gazeboを起動します。
+Ignition Gazeboを起動します。
 
 ```sh
-$ ros2 launch crane_plus_gazebo crane_plus_gazebo.launch.py
+$ ros2 launch crane_plus_ignition crane_plus_ignition.launch.py
 ```
 
 ## サンプルプログラムを実行する

crane_plus_examples/src/pick_and_place.cpp

@@ -56,7 +56,7 @@ int main(int argc, char ** argv)
   auto gripper_joint_values = move_group_gripper.getCurrentJointValues();
   double GRIPPER_DEFAULT = 0.0;
   double GRIPPER_OPEN = to_radians(-30);
-  double GRIPPER_CLOSE = to_radians(-5);
+  double GRIPPER_CLOSE = to_radians(10);
 
   move_group_arm.setNamedTarget("vertical");
   move_group_arm.move();
@@ -88,7 +88,7 @@ int main(int argc, char ** argv)
   move_group_arm.setPoseTarget(target_pose);
   move_group_arm.move();
 
-  target_pose.position.z = 0.15;
+  target_pose.position.z = 0.14;
   move_group_arm.setPoseTarget(target_pose);
   move_group_arm.move();
 

crane_plus_gazebo/README.md

@@ -2,6 +2,11 @@
 
 CRANE+V2 のGazeboシミュレーションパッケージです。
 
+**[物体をつかめない問題](https://github.com/rt-net/crane_plus/issues/33)
+が発生しているため、
+[crane_plus_ignition](../crane_plus_ignition/README.md)
+の使用を推奨します。**
+
 ## ノードの起動
 
 次のコマンドを実行するとGazeboが起動し、CRANE+V2モデルとTableが表示されます。

crane_plus_ignition/CMakeLists.txt

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 3.5)
+project(crane_plus_ignition)
+
+find_package(ament_cmake REQUIRED)
+
+install(DIRECTORY
+  launch
+  worlds
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

crane_plus_ignition/README.md

@@ -0,0 +1,18 @@
+# crane_plus_ignition
+
+CRANE+V2 の[Ignition Gazebo](https://gazebosim.org/home)
+シミュレーションパッケージです。
+
+## ノードの起動
+
+次のコマンドを実行するとIgnition Gazeboが起動し、CRANE+V2モデルとTable、Boxが表示されます。
+
+初回起動時はTableとBoxのモデルをダウンロードするため、モデルの表示に時間がかかることがあります。
+
+実機との接続や`crane_plus_examples/launch/demo.launch/py`の実行は必要ありません。
+
+```sh
+$ ros2 launch crane_plus_ignitionw crane_plus_ignition.launch.py
+```
+
+![crane_plus_ignition](https://rt-net.github.io/images/crane-plus/crane_plus_ignition.png)

crane_plus_ignition/launch/crane_plus_ignition.launch.py

@@ -0,0 +1,88 @@
+# 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_plus_description.robot_description_loader import RobotDescriptionLoader
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    # PATHを追加で通さないとSTLファイルが読み込まれない
+    env = {'IGN_GAZEBO_SYSTEM_PLUGIN_PATH': os.environ['LD_LIBRARY_PATH'],
+           'IGN_GAZEBO_RESOURCE_PATH': os.path.dirname(
+               get_package_share_directory('crane_plus_description'))}
+    world_file = os.path.join(
+        get_package_share_directory('crane_plus_ignition'), 'worlds', 'table.sdf')
+    # -r オプションで起動時にシミュレーションをスタートしないと、コントローラが起動しない
+    ign_gazebo = ExecuteProcess(
+            cmd=['ign gazebo -r', world_file],
+            output='screen',
+            additional_env=env,
+            shell=True
+        )
+
+    ignition_spawn_entity = Node(
+        package='ros_ign_gazebo',
+        executable='create',
+        output='screen',
+        arguments=['-topic', '/robot_description',
+                   '-name', 'crane_plus',
+                   '-z', '1.015',
+                   '-allow_renaming', 'true'],
+    )
+
+    description_loader = RobotDescriptionLoader()
+    description_loader.use_gazebo = 'true'
+    description_loader.use_ignition = 'true'
+    description = description_loader.load()
+
+    move_group = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([
+                get_package_share_directory('crane_plus_moveit_config'),
+                '/launch/run_move_group.launch.py']),
+            launch_arguments={'loaded_description': description}.items()
+        )
+
+    spawn_joint_state_controller = ExecuteProcess(
+                cmd=['ros2 run controller_manager spawner.py joint_state_controller'],
+                shell=True,
+                output='screen',
+            )
+
+    spawn_arm_controller = ExecuteProcess(
+                cmd=['ros2 run controller_manager spawner.py crane_plus_arm_controller'],
+                shell=True,
+                output='screen',
+            )
+
+    spawn_gripper_controller = ExecuteProcess(
+                cmd=['ros2 run controller_manager spawner.py crane_plus_gripper_controller'],
+                shell=True,
+                output='screen',
+            )
+
+    return LaunchDescription([
+        ign_gazebo,
+        move_group,
+        ignition_spawn_entity,
+        spawn_joint_state_controller,
+        spawn_arm_controller,
+        spawn_gripper_controller
+    ])