From 623a2b9aefc0b4c159e3f87bc47ab76558715192 Mon Sep 17 00:00:00 2001
From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Mon, 20 Mar 2023 14:43:48 -0300
Subject: [PATCH] Add support to ros2_control.

---
 config/controller_manager.yaml |   4 +
 config/gazebo.yaml             |   4 +
 launch/gazebo.launch.xml       |   1 +
 launch/ignition.launch.xml     |  19 +--
 launch/plier.launch.xml        |   3 +-
 urdf/plier.urdf                | 273 ++++++++++++++++++++++++-----------------
 6 files changed, 182 insertions(+), 122 deletions(-)
 create mode 100644 config/controller_manager.yaml
 create mode 100644 config/gazebo.yaml

diff --git a/config/controller_manager.yaml b/config/controller_manager.yaml
new file mode 100644
index 0000000..c12730f
--- /dev/null
+++ b/config/controller_manager.yaml
@@ -0,0 +1,4 @@
+controller_manager:
+        ros__parameters:
+                update_rate: 1000
+                use_sim_time: true
diff --git a/config/gazebo.yaml b/config/gazebo.yaml
new file mode 100644
index 0000000..c9cdc3c
--- /dev/null
+++ b/config/gazebo.yaml
@@ -0,0 +1,4 @@
+gazebo:
+        ros__parameters:
+                # This is the /clock update rate
+                publish_rate: 1000.0
diff --git a/launch/gazebo.launch.xml b/launch/gazebo.launch.xml
index 3fc4fc8..7ecdb3c 100644
--- a/launch/gazebo.launch.xml
+++ b/launch/gazebo.launch.xml
@@ -33,6 +33,7 @@
 
 	<include file="$(find-pkg-share plier_description)/launch/plier.launch.xml">
 		<arg name="use_sim_time" value="$(var use_sim_time)"/>
+		<arg name="extra_gazebo_args" value="--ros-args --params-file $(find-pkg-share plier_description)/config/gazebo.yaml"/>
 	</include>
 
 	<node name="plier_spawner" pkg="gazebo_ros" exec="spawn_entity.py" args="-topic robot_description -entity plier" />
diff --git a/launch/ignition.launch.xml b/launch/ignition.launch.xml
index 0a1ef9e..ea7cee3 100644
--- a/launch/ignition.launch.xml
+++ b/launch/ignition.launch.xml
@@ -1,7 +1,7 @@
 <!--******************************************************************************
 	                     Plier Description
 	                  Gazebo Ignitio Launch File
-          Copyright (C) 2022 Walter Fetter Lages <w.fetter@ieee.org>
+          Copyright (C) 2023 Walter Fetter Lages <w.fetter@ieee.org>
 
         This program is free software: you can redistribute it and/or modify
         it under the terms of the GNU General Public License as published by
@@ -24,23 +24,24 @@
 	<arg name="gui" default="true"/>
 	<arg name="use_sim_time" default="true"/>
 
-	<arg unless="$(var pause)" name="ign_pause" default="-r"/>
-	<arg if="$(var pause)" name="ign_pause" default=""/>
+	<arg unless="$(var pause)" name="gz_pause" default="-r"/>
+	<arg if="$(var pause)" name="gz_pause" default=""/>
 	
-	<arg unless="$(var gui)" name="ign_gui" default="-s"/>
-	<arg if="$(var gui)" name="ign_gui" default=""/>
+	<arg unless="$(var gui)" name="gz_gui" default="-s"/>
+	<arg if="$(var gui)" name="gz_gui" default=""/>
 
-	<include file="$(find-pkg-share ros_ign_gazebo)/launch/ign_gazebo.launch.py">
-		<arg name="ign_args" value="$(var ign_pause) $(var ign_gui) empty.sdf"/>
+	<include file="$(find-pkg-share ros_gz_sim)/launch/gz_sim.launch.py">
+		<arg name="gz_args" value="$(var gz_pause) $(var gz_gui) empty.sdf"/>
 	</include>
 	
 	<include file="$(find-pkg-share plier_description)/launch/plier.launch.xml">
 		<arg name="use_sim_time" value="$(var use_sim_time)"/>
+		<arg name="ignition" value="true"/>
 	</include>
 
-	<node name="plier_spawner" pkg="ros_ign_gazebo" exec="create" args="-topic robot_description -name plier"/>
+	<node name="plier_spawner" pkg="ros_gz_sim" exec="create" args="-topic robot_description -name plier"/>
 
-	<node name="clock_bridge" pkg="ros_ign_bridge" exec="parameter_bridge" args="/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock">
+	<node name="clock_bridge" pkg="ros_gz_bridge" exec="parameter_bridge" args="/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock">
 		<param name="use_sim_time" value="$(var use_sim_time)"/>
 	</node>
 </launch>
diff --git a/launch/plier.launch.xml b/launch/plier.launch.xml
index 87f8500..eb41296 100644
--- a/launch/plier.launch.xml
+++ b/launch/plier.launch.xml
@@ -21,9 +21,10 @@
 
 <launch>
 	<arg name="use_sim_time" default="false"/>
+	<arg name="ignition" default="false"/>
 
 	<node name="robot_state_publisher" pkg="robot_state_publisher" exec="robot_state_publisher">
-		<param name="robot_description" value="$(command 'xacro $(find-pkg-share plier_description)/urdf/plier.urdf')" type="str"/>
+		<param name="robot_description" value="$(command 'xacro $(find-pkg-share plier_description)/urdf/plier.urdf ignition:=$(var ignition)')" type="str"/>
 		<param name="use_sim_time" value="$(var use_sim_time)"/>
 	</node>
 </launch>
diff --git a/urdf/plier.urdf b/urdf/plier.urdf
index dbb1de5..82be6a3 100644
--- a/urdf/plier.urdf
+++ b/urdf/plier.urdf
@@ -1,118 +1,167 @@
-<?xml version="1.0"?>
-<robot name="plier">
-
-  <link name="plier_origin"/>
-  
-  <joint name="plier_origin_joint" type="fixed">
-	<parent link="plier_origin"/>
-	<child link="base_link"/>
-  </joint>
-
-  <link name="base_link">
-    <inertial>
-      <origin xyz="-2.9946E-09 2.3647E-09 0.038028" rpy="0 0 0" />
-      <mass value="0.065887" />
-      <inertia ixx="0.0001476" ixy="5.724E-13" ixz="-6.0829E-11" iyy="0.0001476" iyz="4.4824E-11" izz="8.4481E-05" />
-    </inertial>
-
-    <visual>
-      <origin  xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="package://plier_description/meshes/base_link.stl" />
-      </geometry>
-      <material name="">
-        <color rgba="0.75294 0.75294 0.75294 1" />
-      </material>
-    </visual>
-
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="package://plier_description/meshes/base_link.stl" />
-      </geometry>
-    </collision>
-  </link>
-
-  <link name="link1">
-    <inertial>
-      <origin xyz="0.00018812 -0.011115 0.0015573" rpy="0 0 0" />
-      <mass value="0.00011808" />
-      <inertia ixx="5.1612E-09" ixy="-9.0621E-11" ixz="2.0497E-12" iyy="1.6125E-10" iyz="-1.1473E-10" izz="5.0583E-09" />
-    </inertial>
-
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="package://plier_description/meshes/link1.stl" />
-      </geometry>
-      <material name="">
-        <color rgba="0.75294 0.75294 0.75294 1" />
-      </material>
-    </visual>
-
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="package://plier_description/meshes/link1.stl" />
-      </geometry>
-    </collision>
-  </link>
-
-  <joint name="mimic_joint" type="revolute">
-    <origin xyz="-0.00125 0.00098997 0.3459" rpy="-1.5708 -6.123E-17 -1.5708" />
-    <parent link="base_link" />
-    <child link="link1" />
-    <axis xyz="0 0 1" />
-    <limit lower="-0.05" upper="0.0" effort="0.31" velocity="0.25" />
-    <mimic joint="joint" multiplier="-1.0" offset="0.0"/>
-  </joint>
-
-  <link name="link2">
-    <inertial>
-      <origin xyz="-0.0001883 -0.011122 0.00021282" rpy="0 0 0" />
-      <mass value="0.00011791" />
-      <inertia ixx="5.1222E-09" ixy="9.017E-11" ixz="1.1618E-12" iyy="1.4783E-10" iyz="6.6059E-11" izz="5.0326E-09" />
-    </inertial>
-
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="package://plier_description/meshes/link2.stl" />
-      </geometry>
-      <material name="">
-        <color rgba="0.75294 0.75294 0.75294 1" />
-      </material>
-    </visual>
-
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="package://plier_description/meshes/link2.stl" />
-      </geometry>
-    </collision>
-  </link>
-
-  <joint name="joint" type="revolute">
-    <origin xyz="0.00025 -0.00098997 0.3459" rpy="-1.5708 -6.123E-17 -1.5708" />
-    <parent link="base_link" />
-    <child link="link2" />
-    <axis xyz="0 0 1" />
-    <limit lower="0.0" upper="0.05" effort="0.31" velocity="0.25" />
-    
-  </joint>
-
+<?xml version="1.0"?>
+<robot name="plier" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:arg name="ignition" default="false"/>
+	
+  <link name="plier_origin"/>
+
+  <joint name="plier_origin_joint" type="fixed">
+	<parent link="plier_origin"/>
+	<child link="base_link"/>
+  </joint>
+
+  <link name="base_link">
+    <inertial>
+      <origin xyz="-2.9946E-09 2.3647E-09 0.038028" rpy="0 0 0" />
+      <mass value="0.065887" />
+      <inertia ixx="0.0001476" ixy="5.724E-13" ixz="-6.0829E-11" iyy="0.0001476" iyz="4.4824E-11" izz="8.4481E-05" />
+    </inertial>
+
+    <visual>
+      <origin  xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://plier_description/meshes/base_link.stl" />
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://plier_description/meshes/base_link.stl" />
+      </geometry>
+    </collision>
+  </link>
+
+  <link name="link1">
+    <inertial>
+      <origin xyz="0.00018812 -0.011115 0.0015573" rpy="0 0 0" />
+      <mass value="0.00011808" />
+      <inertia ixx="5.1612E-09" ixy="-9.0621E-11" ixz="2.0497E-12" iyy="1.6125E-10" iyz="-1.1473E-10" izz="5.0583E-09" />
+    </inertial>
+
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://plier_description/meshes/link1.stl" />
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://plier_description/meshes/link1.stl" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="mimic_joint" type="revolute">
+    <origin xyz="-0.00125 0.00098997 0.3459" rpy="-1.5708 -6.123E-17 -1.5708" />
+    <parent link="base_link" />
+    <child link="link1" />
+    <axis xyz="0 0 1" />
+    <limit lower="-0.05" upper="0.0" effort="0.31" velocity="0.25" />
+    <mimic joint="joint" multiplier="-1.0" offset="0.0"/>
+  </joint>
+
+  <link name="link2">
+    <inertial>
+      <origin xyz="-0.0001883 -0.011122 0.00021282" rpy="0 0 0" />
+      <mass value="0.00011791" />
+      <inertia ixx="5.1222E-09" ixy="9.017E-11" ixz="1.1618E-12" iyy="1.4783E-10" iyz="6.6059E-11" izz="5.0326E-09" />
+    </inertial>
+
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://plier_description/meshes/link2.stl" />
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://plier_description/meshes/link2.stl" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="joint" type="revolute">
+    <origin xyz="0.00025 -0.00098997 0.3459" rpy="-1.5708 -6.123E-17 -1.5708" />
+    <parent link="base_link" />
+    <child link="link2" />
+    <axis xyz="0 0 1" />
+    <limit lower="0.0" upper="0.05" effort="0.31" velocity="0.25" />
+  </joint>
+
   <gazebo reference="base_link">
     <material>Gazebo/Yellow</material>
-  </gazebo>
-
-
+  </gazebo>
+
+
   <gazebo reference="link1">
     <material>Gazebo/White</material>
-  </gazebo>
-
-
+  </gazebo>
+
+
   <gazebo reference="link2">
     <material>Gazebo/Grey</material>
-  </gazebo>
-
-</robot>
+  </gazebo>
+
+	<ros2_control name="BhandSystem" type="system">
+		<!-- Gazebo Classic -->
+		<xacro:unless value="$(arg ignition)">
+			<hardware>
+				<plugin>gazebo_ros2_control/GazeboSystem</plugin>
+			</hardware>
+	</xacro:unless>
+	<xacro:if value="$(arg ignition)">
+                <!-- Gazebo Ignition -->
+		<hardware>
+			<plugin>ign_ros2_control/IgnitionSystem</plugin>
+		</hardware>
+	</xacro:if>
+
+		<joint name="joint">
+			<command_interface name="position">
+				<param name="min">0.0</param>
+				<param name="max">0.05</param>
+			</command_interface>
+			<state_interface name="position"/>
+		</joint>
+		
+		<joint name="mimic_joint">
+ 			<param name="mimic">joint</param>
+ 			<param name="multiplier">-1.0</param>
+ 			<command_interface name="position"/>
+			<state_interface name="position"/>
+		</joint>
+	</ros2_control>
+
+	<gazebo>
+		<xacro:unless value="$(arg ignition)">
+			<!-- Gazebo Classic -->
+			<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 plier_description)/config/controller_manager.yaml</parameters>
+			</plugin>
+		</xacro:unless>
+		<xacro:if value="$(arg ignition)">
+			<!-- Gazebo Ignition -->
+			<plugin filename="libign_ros2_control-system.so" name="ign_ros2_control::IgnitionROS2ControlPlugin">
+				<robot_param>robot_description</robot_param>
+				<robot_param_node>robot_state_publisher</robot_param_node>
+				<parameters>$(find plier_description)/config/controller_manager.yaml</parameters>
+			</plugin>
+		</xacro:if>
+	</gazebo>
+</robot>
-- 
2.12.0