add gazebo support to urdf

kroshu · Aug 21, 2023 · 4f20e33 · 4f20e33
1 parent dba2e8b
commit 4f20e33
Show file tree

Hide file tree

Showing 3 changed files with 28 additions and 11 deletions.
diff --git a/kuka_lbr_iisy_support/urdf/lbr_iisy3_r760.urdf.xacro b/kuka_lbr_iisy_support/urdf/lbr_iisy3_r760.urdf.xacro
@@ -19,6 +19,7 @@
 
 <!-- Read additional arguments  -->
 <xacro:arg name="use_fake_hardware" default="false" />
+<xacro:arg name="gazebo_sim" default="false" />
 
 <xacro:kuka_lbr3_r760_robot prefix=""
                   client_ip="${client_ip}" 
@@ -27,5 +28,6 @@
                   consequent_lost_packets="${consequent_lost_packets}" 
                   lost_packets_in_timeframe="${lost_packets_in_timeframe}" 
                   timeframe_ms="${timeframe_ms}" 
-                  use_fake_hardware="$(arg use_fake_hardware)"/>
+                  use_fake_hardware="$(arg use_fake_hardware)"
+                  gazebo_sim="$(arg gazebo_sim)"/>
 </robot>
diff --git a/kuka_lbr_iisy_support/urdf/lbr_iisy3_r760_macro.xacro b/kuka_lbr_iisy_support/urdf/lbr_iisy3_r760_macro.xacro
@@ -5,7 +5,7 @@
   <xacro:include filename="$(find kuka_resources)/urdf/common_materials.xacro"/>
   <xacro:include filename="$(find kuka_lbr_iisy_support)/urdf/lbr_iisy_transmission.xacro" />
 
-  <xacro:macro name="kuka_lbr3_r760_robot" params="prefix client_ip controller_ip control_mode consequent_lost_packets lost_packets_in_timeframe timeframe_ms use_fake_hardware:=false">
+  <xacro:macro name="kuka_lbr3_r760_robot" params="prefix client_ip controller_ip control_mode consequent_lost_packets lost_packets_in_timeframe timeframe_ms use_fake_hardware:=false gazebo_sim:=false">
     <!-- ros2 control instance -->
     <xacro:kuka_lbr_iisy_ros2_control
     name="LBRiisy3R760" 
@@ -16,7 +16,8 @@
     consequent_lost_packets="${consequent_lost_packets}"
     lost_packets_in_timeframe="${lost_packets_in_timeframe}"
     timeframe_ms="${timeframe_ms}" 
-    use_fake_hardware="${use_fake_hardware}" />
+    use_fake_hardware="${use_fake_hardware}"
+    gazebo_sim="${gazebo_sim}" />
 
     <!-- links - main serial chain -->
     <!-- links - root element cannot have inertia, therefore a dummy base_link is needed -->

diff --git a/kuka_lbr_iisy_support/urdf/lbr_iisy_ros2_control.xacro b/kuka_lbr_iisy_support/urdf/lbr_iisy_ros2_control.xacro
@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-  <xacro:macro name="kuka_lbr_iisy_ros2_control" params="name prefix client_ip controller_ip control_mode consequent_lost_packets lost_packets_in_timeframe timeframe_ms use_fake_hardware:=false">
+  <xacro:macro name="kuka_lbr_iisy_ros2_control" params="name prefix client_ip controller_ip control_mode consequent_lost_packets lost_packets_in_timeframe timeframe_ms use_fake_hardware:=false gazebo_sim:=false">
     <ros2_control name="${name}" type="system">
       <hardware>
         <xacro:if value="${use_fake_hardware}">
@@ -9,14 +9,20 @@
           <param name="state_following_offset">0.0</param>
         </xacro:if>
         <xacro:unless value="${use_fake_hardware}">
-          <plugin>kuka_rox::KukaRoXHardwareInterface</plugin>
-          <param name="client_ip">${client_ip}</param>
-          <param name="controller_ip">${controller_ip}</param>
-          <param name="control_mode">${control_mode}</param>
-          <param name="consequent_lost_packets">${consequent_lost_packets}</param>
-          <param name="lost_packets_in_timeframe">${lost_packets_in_timeframe}</param>
-          <param name="timeframe_ms">${timeframe_ms}</param>
+          <xacro:if value="${gazebo_sim}">
+            <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+          </xacro:if>
+          <xacro:unless value="${gazebo_sim}">
+            <plugin>kuka_rox::KukaRoXHardwareInterface</plugin>
+            <param name="client_ip">${client_ip}</param>
+            <param name="controller_ip">${controller_ip}</param>
+            <param name="control_mode">${control_mode}</param>
+            <param name="consequent_lost_packets">${consequent_lost_packets}</param>
+            <param name="lost_packets_in_timeframe">${lost_packets_in_timeframe}</param>
+            <param name="timeframe_ms">${timeframe_ms}</param>
+          </xacro:unless>
         </xacro:unless>
+        <plugin>gazebo_ros2_control/GazeboSystem</plugin>
       </hardware>
       <!-- define joints and command/state interfaces for each joint -->
       <joint name="${prefix}joint_a1">
@@ -68,5 +74,13 @@
         <state_interface name="effort"/>
       </joint>
     </ros2_control>
+    <gazebo>
+      <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
+        <robot_param>robot_description</robot_param>
+        <robot_param_node>robot_state_publisher</robot_param_node>
+        <parameters>$(find kuka_rox_hw_interface)/config/ros2_controller_config.yaml</parameters> 
+        <parameters>$(find kuka_rox_hw_interface)/config/joint_trajectory_controller_config.yaml</parameters> 
+      </plugin>
+    </gazebo>
   </xacro:macro>
 </robot>