Update crane_plus_gazebo

README.md

@@ -87,7 +87,6 @@ $ ros2 launch crane_plus_examples example.launch.py example:='gripper_control'
 - crane_plus_gazebo
   - [README](./crane_plus_gazebo/README.md)
   - CRANE+V2のGazeboシミュレーションパッケージです
-  - **現在、Gazebo上のCRANE+V2を制御することはできません**
 - crane_plus_moveit_config
   - [README](./crane_plus_moveit_config/README.md)
   - CRANE+V2の`moveit2`設定ファイルです

crane_plus_description/package.xml

@@ -11,6 +11,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>gazebo_ros2_control</depend>
   <depend>joint_state_publisher_gui</depend>
   <depend>robot_state_publisher</depend>
   <depend>rviz2</depend>

crane_plus_description/urdf/crane_plus.gazebo.xacro

@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <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">
+
+    <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>
+    </gazebo>
+
+    <gazebo reference="${name_link_base}">
+      <mu1>0.2</mu1>
+      <mu2>0.2</mu2>
+      <material>Gazebo/Black</material>
+    </gazebo>
+
+    <gazebo reference="${name_link_1}">
+      <mu1>0.2</mu1>
+      <mu2>0.2</mu2>
+      <material>Gazebo/White</material>
+    </gazebo>
+
+    <gazebo reference="${name_link_2}">
+      <mu1>0.2</mu1>
+      <mu2>0.2</mu2>
+      <material>Gazebo/Black</material>
+    </gazebo>
+
+    <gazebo reference="${name_link_3}">
+      <mu1>0.2</mu1>
+      <mu2>0.2</mu2>
+      <material>Gazebo/White</material>
+    </gazebo>
+
+    <gazebo reference="${name_link_4}">
+      <mu1>0.2</mu1>
+      <mu2>0.2</mu2>
+      <material>Gazebo/Black</material>
+    </gazebo>
+
+    <gazebo reference="${name_link_hand}">
+      <mu1>0.2</mu1>
+      <mu2>0.2</mu2>
+      <material>Gazebo/White</material>
+    </gazebo>
+
+  </xacro:macro>
+</robot>

crane_plus_description/urdf/crane_plus.gazebo_ros2_control.xacro

@@ -0,0 +1,79 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:macro name="crane_plus_gazebo_ros2_control_settings"
+    params="name
+            name_joint_1
+            name_joint_2
+            name_joint_3
+            name_joint_4
+            name_joint_hand
+            joint_1_lower_limit
+            joint_1_upper_limit
+            joint_2_lower_limit
+            joint_2_upper_limit
+            joint_3_lower_limit
+            joint_3_upper_limit
+            joint_4_lower_limit
+            joint_4_upper_limit
+            joint_hand_lower_limit
+            joint_hand_upper_limit">
+
+    <ros2_control name="${name}" type="system">
+      <hardware>
+        <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+      </hardware>
+
+      <joint name="${name_joint_1}">
+        <command_interface name="position">
+          <param name="min">${joint_1_lower_limit}</param>
+          <param name="max">${joint_1_upper_limit}</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
+
+      <joint name="${name_joint_2}">
+        <command_interface name="position">
+          <param name="min">${joint_2_lower_limit}</param>
+          <param name="max">${joint_2_upper_limit}</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
+
+      <joint name="${name_joint_3}">
+        <command_interface name="position">
+          <param name="min">${joint_3_lower_limit}</param>
+          <param name="max">${joint_3_upper_limit}</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
+
+      <joint name="${name_joint_4}">
+        <command_interface name="position">
+          <param name="min">${joint_4_lower_limit}</param>
+          <param name="max">${joint_4_upper_limit}</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
+
+      <joint name="${name_joint_hand}">
+        <command_interface name="position">
+          <param name="min">${joint_hand_lower_limit}</param>
+          <param name="max">${joint_hand_upper_limit}</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
+
+    </ros2_control>
+  </xacro:macro>
+</robot>

crane_plus_description/urdf/crane_plus.urdf.xacro

@@ -9,6 +9,10 @@
 
   <xacro:include filename="$(find crane_plus_description)/urdf/crane_plus.xacro"/>
   <xacro:include filename="$(find crane_plus_description)/urdf/crane_plus.ros2_control.xacro"/>
+  <xacro:include filename="$(find crane_plus_description)/urdf/crane_plus.gazebo.xacro"/>
+  <xacro:include filename="$(find crane_plus_description)/urdf/crane_plus.gazebo_ros2_control.xacro"/>
+
+  <xacro:arg name="use_gazebo" default="false" />
 
   <!-- Link to provide the location reference for the arm -->
   <link name="base_link"/>
@@ -79,22 +83,52 @@
     <origin xyz="0 0 0"/>
   </xacro:crane_plus>
 
-  <xacro:crane_plus_ros2_control_settings 
-    name="crane_plus"
-    name_joint_1="${NAME_JOINT_1}"
-    name_joint_2="${NAME_JOINT_2}"
-    name_joint_3="${NAME_JOINT_3}"
-    name_joint_4="${NAME_JOINT_4}"
-    name_joint_hand="${NAME_JOINT_HAND}"
-    joint_1_lower_limit="${JOINT_1_LOWER_LIMIT}"
-    joint_1_upper_limit="${JOINT_1_UPPER_LIMIT}"
-    joint_2_lower_limit="${JOINT_2_LOWER_LIMIT}"
-    joint_2_upper_limit="${JOINT_2_UPPER_LIMIT}"
-    joint_3_lower_limit="${JOINT_3_LOWER_LIMIT}"
-    joint_3_upper_limit="${JOINT_3_UPPER_LIMIT}"
-    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}" />
+  <xacro:unless value="$(arg use_gazebo)">
+    <xacro:crane_plus_ros2_control_settings 
+      name="crane_plus"
+      name_joint_1="${NAME_JOINT_1}"
+      name_joint_2="${NAME_JOINT_2}"
+      name_joint_3="${NAME_JOINT_3}"
+      name_joint_4="${NAME_JOINT_4}"
+      name_joint_hand="${NAME_JOINT_HAND}"
+      joint_1_lower_limit="${JOINT_1_LOWER_LIMIT}"
+      joint_1_upper_limit="${JOINT_1_UPPER_LIMIT}"
+      joint_2_lower_limit="${JOINT_2_LOWER_LIMIT}"
+      joint_2_upper_limit="${JOINT_2_UPPER_LIMIT}"
+      joint_3_lower_limit="${JOINT_3_LOWER_LIMIT}"
+      joint_3_upper_limit="${JOINT_3_UPPER_LIMIT}"
+      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}" />
+  </xacro:unless>
+
+  <xacro:if value="$(arg use_gazebo)">
+    <xacro:gazebo_robot_settings
+      name_link_base="${NAME_LINK_BASE}"
+      name_link_1="${NAME_LINK_1}"
+      name_link_2="${NAME_LINK_2}"
+      name_link_3="${NAME_LINK_3}"
+      name_link_4="${NAME_LINK_4}"
+      name_link_hand="${NAME_LINK_HAND}" />
+
+    <xacro:crane_plus_gazebo_ros2_control_settings 
+      name="crane_plus"
+      name_joint_1="${NAME_JOINT_1}"
+      name_joint_2="${NAME_JOINT_2}"
+      name_joint_3="${NAME_JOINT_3}"
+      name_joint_4="${NAME_JOINT_4}"
+      name_joint_hand="${NAME_JOINT_HAND}"
+      joint_1_lower_limit="${JOINT_1_LOWER_LIMIT}"
+      joint_1_upper_limit="${JOINT_1_UPPER_LIMIT}"
+      joint_2_lower_limit="${JOINT_2_LOWER_LIMIT}"
+      joint_2_upper_limit="${JOINT_2_UPPER_LIMIT}"
+      joint_3_lower_limit="${JOINT_3_LOWER_LIMIT}"
+      joint_3_upper_limit="${JOINT_3_UPPER_LIMIT}"
+      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}" />
+  </xacro:if>
 
 </robot>

crane_plus_examples/README.md

@@ -2,7 +2,9 @@
 
 このパッケージはCRANE+V2 ROS 2パッケージのサンプルコード集です。
 
-## 実行手順
+## 準備（実機を使う場合）
+
+![crane_plus](https://rt-net.github.io/images/crane-plus/CRANEV2-500x500.png)
 
 ### 1. CRANE+V2本体をPCに接続する
 
@@ -28,18 +30,29 @@ controller (`crane_plus_control`)を起動します。
 $ ros2 launch crane_plus_examples demo.launch.py
 ```
 
-### 4. サンプルプログラムを実行する
+## 準備（Gazeboを使う場合）
+
+![crane_plus_gazebo](https://rt-net.github.io/images/crane-plus/crane_plus_gazebo.png)
+
+### 1. move_groupとgazeboを起動する
+
+次のコマンドでmove_group (`crane_plus_moveit_config`)と
+Gazeboを起動します。
+
+```sh
+$ ros2 launch crane_plus_gazebo crane_plus_gazebo.launch.py
+```
+
+## サンプルプログラムを実行する
 
-サンプルプログラムを実行します。
+準備ができたらサンプルプログラムを実行します。
 例えばグリッパを開閉するサンプルは次のコマンドで実行できます。
 
 ```sh
 $ ros2 launch crane_plus_examples example.launch.py example:='gripper_control'
 ```
 
-### 5. サンプルプログラムを終了する
-
-`Ctrl+c`を入力してサンプルプログラムを終了します。
+終了するときは`Ctrl+c`を入力します。
 
 ## Examples
 

crane_plus_examples/src/pick_and_place.cpp

@@ -57,7 +57,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(10);
+  double GRIPPER_CLOSE = to_radians(-5);
 
   move_group_arm.setNamedTarget("vertical");
   move_group_arm.move();
@@ -89,11 +89,19 @@ int main(int argc, char ** argv)
   move_group_arm.setPoseTarget(target_pose);
   move_group_arm.move();
 
+  target_pose.position.z = 0.15;
+  move_group_arm.setPoseTarget(target_pose);
+  move_group_arm.move();
+
   // Grasp
   gripper_joint_values[0] = GRIPPER_CLOSE;
   move_group_gripper.setJointValueTarget(gripper_joint_values);
   move_group_gripper.move();
 
+  target_pose.position.z = 0.17;
+  move_group_arm.setPoseTarget(target_pose);
+  move_group_arm.move();
+
   // ----- Placing Preparation -----
   move_group_arm.setNamedTarget("home");
   move_group_arm.move();

crane_plus_gazebo/README.md

@@ -2,18 +2,13 @@
 
 CRANE+V2 のGazeboシミュレーションパッケージです。
 
-**現在、Gazebo上のCRANE+V2を動かすことは出来ません**
-
-`crane_plus_gazebo`は
-[gazebo_ros2_control](https://github.com/ros-simulation/gazebo_ros2_control)及び
-[ros2_control](https://github.com/ros-controls/ros2_control)
-の開発状況に依存しています。
-
 ## ノードの起動
 
-次のコマンドを実行するとGazeboが起動し、CRANE+V2モデルと、Table、Cubeが表示がされます。
+次のコマンドを実行するとGazeboが起動し、CRANE+V2モデルとTableが表示されます。
+
+初回起動時はTableのモデルをダウンロードするため、モデルの表示に時間がかかることがあります。
 
-初回起動時はTableとCubeのモデルをダウンロードするため、モデルの表示に時間がかかることがあります。
+実機との接続や`crane_plus_examples/launch/demo.launch/py`の実行は必要ありません。
 
 ```sh
 $ ros2 launch crane_plus_gazebo crane_plus_gazebo.launch.py

crane_plus_gazebo/launch/crane_plus_gazebo.launch.py

@@ -17,6 +17,7 @@
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
+from launch.actions import ExecuteProcess
 from launch.actions import IncludeLaunchDescription
 from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -57,18 +58,40 @@ def generate_launch_description():
             PythonLaunchDescriptionSource([
                 get_package_share_directory('crane_plus_moveit_config'),
                 '/launch/run_move_group.launch.py']),
+            launch_arguments={'xacro_use_gazebo': 'true'}.items(),
         )
 
     spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
                         arguments=['-entity', 'crane_plus', '-x', '0', '-y', '0',
                                    '-z', '1.02', '-topic', '/robot_description'],
                         output='screen')
 
+    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([
         declare_arg_gui,
         declare_arg_server,
         gzserver,
         gzclient,
         move_group,
         spawn_entity,
+        spawn_joint_state_controller,
+        spawn_arm_controller,
+        spawn_gripper_controller
     ])