Add support to ros2_control.

diff --git a/config/controller_manager.yaml b/config/controller_manager.yaml
new file mode 100644 (file)
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 (file)
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 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- a/urdf/plier.urdf
+++ b/urdf/plier.urdf
@@ -1,118 +1,167 @@
-<?xml version="1.0"?>\r
-<robot name="plier">\r
-\r
-  <link name="plier_origin"/>\r
-  \r
-  <joint name="plier_origin_joint" type="fixed">\r
-       <parent link="plier_origin"/>\r
-       <child link="base_link"/>\r
-  </joint>\r
-\r
-  <link name="base_link">\r
-    <inertial>\r
-      <origin xyz="-2.9946E-09 2.3647E-09 0.038028" rpy="0 0 0" />\r
-      <mass value="0.065887" />\r
-      <inertia ixx="0.0001476" ixy="5.724E-13" ixz="-6.0829E-11" iyy="0.0001476" iyz="4.4824E-11" izz="8.4481E-05" />\r
-    </inertial>\r
-\r
-    <visual>\r
-      <origin  xyz="0 0 0" rpy="0 0 0" />\r
-      <geometry>\r
-        <mesh filename="package://plier_description/meshes/base_link.stl" />\r
-      </geometry>\r
-      <material name="">\r
-        <color rgba="0.75294 0.75294 0.75294 1" />\r
-      </material>\r
-    </visual>\r
-\r
-    <collision>\r
-      <origin xyz="0 0 0" rpy="0 0 0" />\r
-      <geometry>\r
-        <mesh filename="package://plier_description/meshes/base_link.stl" />\r
-      </geometry>\r
-    </collision>\r
-  </link>\r
-\r
-  <link name="link1">\r
-    <inertial>\r
-      <origin xyz="0.00018812 -0.011115 0.0015573" rpy="0 0 0" />\r
-      <mass value="0.00011808" />\r
-      <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" />\r
-    </inertial>\r
-\r
-    <visual>\r
-      <origin xyz="0 0 0" rpy="0 0 0" />\r
-      <geometry>\r
-        <mesh filename="package://plier_description/meshes/link1.stl" />\r
-      </geometry>\r
-      <material name="">\r
-        <color rgba="0.75294 0.75294 0.75294 1" />\r
-      </material>\r
-    </visual>\r
-\r
-    <collision>\r
-      <origin xyz="0 0 0" rpy="0 0 0" />\r
-      <geometry>\r
-        <mesh filename="package://plier_description/meshes/link1.stl" />\r
-      </geometry>\r
-    </collision>\r
-  </link>\r
-\r
-  <joint name="mimic_joint" type="revolute">\r
-    <origin xyz="-0.00125 0.00098997 0.3459" rpy="-1.5708 -6.123E-17 -1.5708" />\r
-    <parent link="base_link" />\r
-    <child link="link1" />\r
-    <axis xyz="0 0 1" />\r
-    <limit lower="-0.05" upper="0.0" effort="0.31" velocity="0.25" />\r
-    <mimic joint="joint" multiplier="-1.0" offset="0.0"/>\r
-  </joint>\r
-\r
-  <link name="link2">\r
-    <inertial>\r
-      <origin xyz="-0.0001883 -0.011122 0.00021282" rpy="0 0 0" />\r
-      <mass value="0.00011791" />\r
-      <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" />\r
-    </inertial>\r
-\r
-    <visual>\r
-      <origin xyz="0 0 0" rpy="0 0 0" />\r
-      <geometry>\r
-        <mesh filename="package://plier_description/meshes/link2.stl" />\r
-      </geometry>\r
-      <material name="">\r
-        <color rgba="0.75294 0.75294 0.75294 1" />\r
-      </material>\r
-    </visual>\r
-\r
-    <collision>\r
-      <origin xyz="0 0 0" rpy="0 0 0" />\r
-      <geometry>\r
-        <mesh filename="package://plier_description/meshes/link2.stl" />\r
-      </geometry>\r
-    </collision>\r
-  </link>\r
-\r
-  <joint name="joint" type="revolute">\r
-    <origin xyz="0.00025 -0.00098997 0.3459" rpy="-1.5708 -6.123E-17 -1.5708" />\r
-    <parent link="base_link" />\r
-    <child link="link2" />\r
-    <axis xyz="0 0 1" />\r
-    <limit lower="0.0" upper="0.05" effort="0.31" velocity="0.25" />\r
-    \r
-  </joint>\r
-\r
+<?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>\r
-\r
-\r
+  </gazebo>
+
+
   <gazebo reference="link1">
     <material>Gazebo/White</material>
-  </gazebo>\r
-\r
-\r
+  </gazebo>
+
+
   <gazebo reference="link2">
     <material>Gazebo/Grey</material>
-  </gazebo>\r
-\r
-</robot>\r
+  </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>