From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Mon, 20 Mar 2023 01:08:57 +0000 (-0300)
Subject: Add supports to ros2_control in Ignition.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=bab34fc19df0309a7dacf863936fe967b577fa56;p=q2d.git

Add supports to ros2_control in Ignition.
---

diff --git a/q2d_bringup/config/bypass.yaml b/q2d_bringup/config/bypass.yaml
index 2a7bfe6..29d9a17 100644
--- a/q2d_bringup/config/bypass.yaml
+++ b/q2d_bringup/config/bypass.yaml
@@ -1,10 +1,10 @@
-/shoulder_controller:
+shoulder_controller:
         ros__parameters:
                 joints:
                         - shoulder_active_joint
                 interface_name: effort
 
-/elbow_controller:
+elbow_controller:
         ros__parameters:
                 joints:
                         - elbow_active_joint
diff --git a/q2d_bringup/config/computed_torque.yaml b/q2d_bringup/config/computed_torque.yaml
index ebadbfd..c16f2f0 100644
--- a/q2d_bringup/config/computed_torque.yaml
+++ b/q2d_bringup/config/computed_torque.yaml
@@ -1,4 +1,4 @@
-/computed_torque_controller:
+computed_torque_controller:
         ros__parameters:
                 joints: 
                         - shoulder_active_joint
diff --git a/q2d_bringup/config/group_bypass.yaml b/q2d_bringup/config/group_bypass.yaml
index 8d815e0..61eef34 100644
--- a/q2d_bringup/config/group_bypass.yaml
+++ b/q2d_bringup/config/group_bypass.yaml
@@ -1,4 +1,4 @@
-/group_controller:
+group_controller:
         ros__parameters:
                 joints: 
                         - shoulder_active_joint
diff --git a/q2d_bringup/config/group_pid.yaml b/q2d_bringup/config/group_pid.yaml
index 8cc2bec..1bcbd0b 100644
--- a/q2d_bringup/config/group_pid.yaml
+++ b/q2d_bringup/config/group_pid.yaml
@@ -1,4 +1,4 @@
-/group_controller:
+group_controller:
         ros__parameters:
                 joints:
                         - shoulder_active_joint
diff --git a/q2d_bringup/config/pid.yaml b/q2d_bringup/config/pid.yaml
index 281d110..962b7f6 100644
--- a/q2d_bringup/config/pid.yaml
+++ b/q2d_bringup/config/pid.yaml
@@ -1,9 +1,9 @@
-/shoulder_controller:
+shoulder_controller:
         ros__parameters:
                 joint: shoulder_active_joint
                 pid: {p: 2310, i: 4640, d: 0.299}
 
-/elbow_controller:
+elbow_controller:
         ros__parameters:
                 joint: elbow_active_joint
                 pid: {p: 339, i: 851, d: 0.351}
diff --git a/q2d_bringup/config/pid_plus_gravity.yaml b/q2d_bringup/config/pid_plus_gravity.yaml
index 6a8e286..a0ad17a 100644
--- a/q2d_bringup/config/pid_plus_gravity.yaml
+++ b/q2d_bringup/config/pid_plus_gravity.yaml
@@ -1,4 +1,4 @@
-/pid_plus_gravity_controller:
+pid_plus_gravity_controller:
         ros__parameters:
                 joints: 
                         - shoulder_active_joint
diff --git a/q2d_bringup/launch/gazebo.launch.xml b/q2d_bringup/launch/gazebo.launch.xml
index 6b53402..f685019 100644
--- a/q2d_bringup/launch/gazebo.launch.xml
+++ b/q2d_bringup/launch/gazebo.launch.xml
@@ -23,11 +23,18 @@
 	<arg name="pause" default="true"/>
 	<arg name="gui" default="true"/>
 	<arg name="use_sim_time" default="true"/>
+	<arg name="ignition" default="false"/>
 
 	<arg name="controller" default="pid"/>
 	<arg name="config" default="$(find-pkg-share q2d_bringup)/config/$(var controller).yaml"/>
 
-	<include file="$(find-pkg-share q2d_description)/launch/gazebo.launch.xml" >
+	<include unless="$(var ignition)" file="$(find-pkg-share q2d_description)/launch/gazebo.launch.xml" >
+		<arg name="pause" value="$(var pause)"/>
+		<arg name="gui" value="$(var gui)"/>
+		<arg name="use_sim_time" value="$(var use_sim_time)"/>
+	</include>
+	
+	<include if="$(var ignition)" file="$(find-pkg-share q2d_description)/launch/ignition.launch.xml" >
 		<arg name="pause" value="$(var pause)"/>
 		<arg name="gui" value="$(var gui)"/>
 		<arg name="use_sim_time" value="$(var use_sim_time)"/>
diff --git a/q2d_bringup/launch/ignition.launch.xml b/q2d_bringup/launch/ignition.launch.xml
deleted file mode 100644
index d2305bd..0000000
--- a/q2d_bringup/launch/ignition.launch.xml
+++ /dev/null
@@ -1,40 +0,0 @@
-<!--******************************************************************************
-	                   Quanser 2DSFJE Bringup
-	                 Ignition Gazebo Launch File
-          Copyright (C) 2018, 2022 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
-        the Free Software Foundation, either version 3 of the License, or
-        (at your option) any later version.
-
-        This program is distributed in the hope that it will be useful, but
-        WITHOUT ANY WARRANTY; without even the implied warranty of
-        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
-        Geneal Public License for more details.
-
-        You should have received a copy of the GNU General Public License
-        along with this program.  If not, see
-        <http://www.gnu.org/licenses/>.
-        
-*******************************************************************************-->
-
-<launch>
-	<arg name="pause" default="true"/>
-	<arg name="gui" default="true"/>
-	<arg name="use_sim_time" default="true"/>
-
-	<arg name="controller" default="pid"/>
-	<arg name="config" default="$(find-pkg-share q2d_bringup)/config/$(var controller).yaml"/>
-
-	<include file="$(find-pkg-share q2d_description)/launch/ignition.launch.xml" >
-		<arg name="pause" value="$(var pause)"/>
-		<arg name="gui" value="$(var gui)"/>
-		<arg name="use_sim_time" value="$(var use_sim_time)"/>
-	</include>
-
-	<include file="$(find-pkg-share q2d_bringup)/launch/$(var controller).launch.xml" >
-		<arg name="config" value="$(var config)"/>
-		<arg name="use_sim_time" value="$(var use_sim_time)"/>
-	</include>
-</launch>
diff --git a/q2d_description/launch/ignition.launch.xml b/q2d_description/launch/ignition.launch.xml
index 4e39ec5..7bab226 100644
--- a/q2d_description/launch/ignition.launch.xml
+++ b/q2d_description/launch/ignition.launch.xml
@@ -1,7 +1,7 @@
 <!--******************************************************************************
 	                   Quanser 2DSFJE Description
 	                   Ignition Gazebo Launch File
-          Copyright (C) 2018..2022 Walter Fetter Lages <w.fetter@ieee.org>
+          Copyright (C) 2018..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) -v 4 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) -v 4 empty.sdf"/>
 	</include>
 
 	<include file="$(find-pkg-share q2d_description)/launch/q2d.launch.xml">
 		<arg name="use_sim_time" value="$(var use_sim_time)"/>
+		<arg name="ignition" value="true"/>
 	</include>
 
-	<node name="q2d_spawner" pkg="ros_ign_gazebo" exec="create" args="-topic robot_description -name q2d"/>
+	<node name="q2d_spawner" pkg="ros_gz_sim" exec="create" args="-topic robot_description -name q2d"/>
 
-	<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>
+</launch>
diff --git a/q2d_description/launch/q2d.launch.xml b/q2d_description/launch/q2d.launch.xml
index 7237d75..bd25d05 100644
--- a/q2d_description/launch/q2d.launch.xml
+++ b/q2d_description/launch/q2d.launch.xml
@@ -1,7 +1,7 @@
 <!--******************************************************************************
 	                   Quanser 2DSFJE Description
 	                   Quanser 2DSFJE Launch File
-          Copyright (C) 2018, 2021 Walter Fetter Lages <w.fetter@ieee.org>
+          Copyright (C) 2018..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
@@ -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 q2d_description)/urdf/q2d.urdf')" type="str"/>
+		<param name="robot_description" value="$(command 'xacro $(find-pkg-share q2d_description)/urdf/q2d.urdf ignition:=$(var ignition)')" type="str"/>
 		<param name="use_sim_time" value="$(var use_sim_time)"/>
 	</node>
 </launch>
diff --git a/q2d_description/urdf/q2d.urdf b/q2d_description/urdf/q2d.urdf
index 11f63e0..8e6fb7c 100644
--- a/q2d_description/urdf/q2d.urdf
+++ b/q2d_description/urdf/q2d.urdf
@@ -1,6 +1,8 @@
 <?xml version="1.0"?>
-<robot name="q2d">
+<robot name="q2d" xmlns:xacro="http://www.ros.org/wiki/xacro">
 
+	<xacro:arg name="ignition" default="false"/>
+	
 	<link name="origin_link"/>
 
 	<link name="base_link">
@@ -247,10 +249,19 @@
 	</gazebo>
 
 	<ros2_control name="GazeboSystem" type="system">
-		<hardware>
-			<plugin>gazebo_ros2_control/GazeboSystem</plugin>
-		</hardware>
-
+		<xacro:unless value="$(arg ignition)">
+			<!-- Gazebo Classic -->
+			<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="shoulder_active_joint">
 			<command_interface name="effort">
 				<param name="min">-27.94</param>
@@ -273,45 +284,22 @@
 	</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 q2d_description)/config/controller_manager.yaml</parameters>
-		</plugin>
-	</gazebo>
-<!--
-	<ros2_control name="IgnitionSystem" type="system">
-		<hardware>
-			<plugin>ign_ros2_control/IgnitionSystem</plugin>
-		</hardware>
-
-		<joint name="shoulder_active_joint">
-			<command_interface name="effort">
-				<param name="min">-27.94</param>
-				<param name="max">27.94</param>
-			</command_interface>
-			<state_interface name="position"/>
-			<state_interface name="velocity"/>
-			<state_interface name="effort"/>
-		</joint>
-
-		<joint name="elbow_active_joint">
-			<command_interface name="effort">
-				<param name="min">-13.62</param>
-				<param name="max">13.62</param>
-			</command_interface>
-			<state_interface name="position"/>
-			<state_interface name="velocity"/>
-			<state_interface name="effort"/>
-		</joint>
-	</ros2_control>
-
-	<gazebo>
-		<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 q2d_description)/config/controller_manager.yaml</parameters>
-		</plugin>
+		<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 q2d_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 q2d_description)/config/controller_manager.yaml</parameters>
+			</plugin>
+		</xacro:if>
 	</gazebo>
--->
+
 </robot>
diff --git a/q2d_teleop/launch/gazebo.launch.xml b/q2d_teleop/launch/gazebo.launch.xml
index aae3501..1e78fa7 100644
--- a/q2d_teleop/launch/gazebo.launch.xml
+++ b/q2d_teleop/launch/gazebo.launch.xml
@@ -23,15 +23,18 @@
 -->
 
 <launch>
+	<arg name="ignition" default="false"/>
+	
 	<include file="$(find-pkg-share q2d_bringup)/launch/gazebo.launch.xml">
-		<arg name="pause" value="false" />
+		<arg name="pause" value="false"/>
+		<arg name="ignition" value="$(arg ignition)"/>
 	</include>
 
-  	<include file="$(find-pkg-share q2d_teleop)/launch/q2d_teleop_tablet.launch.xml" />
+  	<include file="$(find-pkg-share q2d_teleop)/launch/q2d_teleop_tablet.launch.xml"/>
 
-  	<include file="$(find-pkg-share q2d_teleop)/launch/q2d_teleop_rviz.launch.xml" />
+  	<include file="$(find-pkg-share q2d_teleop)/launch/q2d_teleop_rviz.launch.xml"/>
 
-  	<include file="$(find-pkg-share q2d_teleop)/launch/display.launch.xml" />
+  	<include file="$(find-pkg-share q2d_teleop)/launch/display.launch.xml"/>
 
-  	<include file="$(find-pkg-share gfxtablet_ros)/launch/gfxtablet.launch.xml" />
+  	<include file="$(find-pkg-share gfxtablet_ros)/launch/gfxtablet.launch.xml"/>
 </launch>