From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Thu, 16 Sep 2021 19:46:15 +0000 (-0300)
Subject: Port q2d_bringup to Galactic.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=d402f1ac738fae4943c6f3f62b7a271636853fcd;p=q2d.git

Port q2d_bringup to Galactic.
---

diff --git a/q2d_bringup/CMakeLists.txt b/q2d_bringup/CMakeLists.txt
index a764fc3..5b75510 100644
--- a/q2d_bringup/CMakeLists.txt
+++ b/q2d_bringup/CMakeLists.txt
@@ -7,10 +7,12 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-find_package(effort_controllers REQUIRED)
-find_package(joint_state_controller REQUIRED)
 
-install(DIRECTORY config launch scripts
+install(PROGRAMS scripts/ijc_step.sh scripts/ijc_square.py
+	DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY config launch
 	DESTINATION share/${PROJECT_NAME}
 )
 
diff --git a/q2d_bringup/config/bypass.yaml b/q2d_bringup/config/bypass.yaml
index 285561d..2a7bfe6 100644
--- a/q2d_bringup/config/bypass.yaml
+++ b/q2d_bringup/config/bypass.yaml
@@ -1,11 +1,11 @@
-shoulder_controller:
-        type: effort_controllers/JointEffortController
-        joint: shoulder_active_joint
+/shoulder_controller:
+        ros__parameters:
+                joints:
+                        - shoulder_active_joint
+                interface_name: effort
 
-elbow_controller:
-        type: effort_controllers/JointEffortController
-        joint: elbow_active_joint
-
-joint_state_broadcaster:
-        type: joint_state_broadcaster/JointStateBroadcaster
-        publish_rate: 100
+/elbow_controller:
+        ros__parameters:
+                joints:
+                        - elbow_active_joint
+                interface_name: effort
diff --git a/q2d_bringup/config/controller_manager.yaml b/q2d_bringup/config/controller_manager.yaml
new file mode 100644
index 0000000..c12730f
--- /dev/null
+++ b/q2d_bringup/config/controller_manager.yaml
@@ -0,0 +1,4 @@
+controller_manager:
+        ros__parameters:
+                update_rate: 1000
+                use_sim_time: true
diff --git a/q2d_bringup/config/group_bypass.yaml b/q2d_bringup/config/group_bypass.yaml
index a777684..8d815e0 100644
--- a/q2d_bringup/config/group_bypass.yaml
+++ b/q2d_bringup/config/group_bypass.yaml
@@ -1,9 +1,5 @@
-group_controller:
-        type: effort_controllers/JointGroupEffortController
-        joints:
-                - shoulder_active_joint
-                - elbow_active_joint
-
-joint_state_broadcaster:
-        type: joint_state_broadcaster/JointStateBroadcaster
-        publish_rate: 100
+/group_controller:
+        ros__parameters:
+                joints: 
+                        - shoulder_active_joint
+                        - elbow_active_joint
diff --git a/q2d_bringup/config/group_pid.yaml b/q2d_bringup/config/group_pid.yaml
index 6fe21f8..8cc2bec 100644
--- a/q2d_bringup/config/group_pid.yaml
+++ b/q2d_bringup/config/group_pid.yaml
@@ -1,11 +1,7 @@
-group_controller:
-        type: effort_controllers/JointGroupEffortController
-        joints:
-                - shoulder_active_joint
-                - elbow_active_joint
-        shoulder_active_joint/pid: {p: 2310, i: 4640, d: 0.299}
-        elbow_active_joint/pid: {p: 339, i: 851, d: 0.351}
-
-joint_state_broadcaster:
-        type: joint_state_broadcaster/JointStateBroadcaster
-        publish_rate: 100
+/group_controller:
+        ros__parameters:
+                joints:
+                        - shoulder_active_joint
+                        - elbow_active_joint
+                shoulder_active_joint/pid: {p: 2310, i: 4640, d: 0.299}
+                elbow_active_joint/pid: {p: 339, i: 851, d: 0.351}
diff --git a/q2d_bringup/config/pid.yaml b/q2d_bringup/config/pid.yaml
index 1a3d087..281d110 100644
--- a/q2d_bringup/config/pid.yaml
+++ b/q2d_bringup/config/pid.yaml
@@ -1,13 +1,9 @@
-shoulder_controller:
-        type: effort_controllers/JointPositionController
-        joint: shoulder_active_joint
-        pid: {p: 2310, i: 4640, d: 0.299}
+/shoulder_controller:
+        ros__parameters:
+                joint: shoulder_active_joint
+                pid: {p: 2310, i: 4640, d: 0.299}
 
-elbow_controller:
-        type: effort_controllers/JointPositionController
-        joint: elbow_active_joint
-        pid: {p: 339, i: 851, d: 0.351}
-
-joint_state_broadcaster:
-        type: joint_state_broadcaster/JointStateBroadcaster
-        publish_rate: 100
+/elbow_controller:
+        ros__parameters:
+                joint: elbow_active_joint
+                pid: {p: 339, i: 851, d: 0.351}
diff --git a/q2d_bringup/launch/bypass.launch.xml b/q2d_bringup/launch/bypass.launch.xml
new file mode 100644
index 0000000..b6b37c7
--- /dev/null
+++ b/q2d_bringup/launch/bypass.launch.xml
@@ -0,0 +1,33 @@
+<!--******************************************************************************
+	                   Quanser 2DSFJE Bringup
+	                Bypass controller Launch File
+          Copyright (C) 2018, 2021 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="config" default="$(find-pkg-share q2d_bringup)/config/bypass.yaml"/>
+	
+	<node name="shoulder_controller_spawner" pkg="controller_manager" exec="spawner"
+		args="-t forward_command_controller/ForwardCommandController -p $(var config) shoulder_controller"/>
+
+	<node name="elbow_controller_spawner" pkg="controller_manager" exec="spawner"
+		args="-t forward_command_controller/ForwardCommandController -p $(var config) elbow_controller"/>
+		
+	<node name="join_state_broadcaster_spawner" pkg="controller_manager" exec="spawner"
+		args="-t joint_state_broadcaster/JointStateBroadcaster joint_state_broadcaster"/>
+</launch>
diff --git a/q2d_bringup/launch/gazebo.launch.xml b/q2d_bringup/launch/gazebo.launch.xml
index b0b70ae..6b53402 100644
--- a/q2d_bringup/launch/gazebo.launch.xml
+++ b/q2d_bringup/launch/gazebo.launch.xml
@@ -1,3 +1,24 @@
+<!--******************************************************************************
+	                   Quanser 2DSFJE Bringup
+	                     Gazebo Launch File
+          Copyright (C) 2018, 2021 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"/>
@@ -5,16 +26,15 @@
 
 	<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" >
 		<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/$(arg controller).launch.xml" >
-		<arg name="controller" value="$(var controller)"/>
+	<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_bringup/launch/group_bypass.launch.xml b/q2d_bringup/launch/group_bypass.launch.xml
new file mode 100644
index 0000000..1320138
--- /dev/null
+++ b/q2d_bringup/launch/group_bypass.launch.xml
@@ -0,0 +1,30 @@
+<!--******************************************************************************
+	                   Quanser 2DSFJE Bringup
+	             Group Bypass Controller Launch File
+          Copyright (C) 2018, 2021 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="config" default="$(find-pkg-share q2d_bringup)/config/group_bypass.yaml"/>
+
+	<node name="group_controller_spawner" pkg="controller_manager" exec="spawner"
+		args="-t effort_controllers/JointGroupEffortController -p $(var config) group_controller"/>
+
+	<node name="join_state_broadcaster_spawner" pkg="controller_manager" exec="spawner"
+		args="-t joint_state_broadcaster/JointStateBroadcaster joint_state_broadcaster"/>
+</launch>
diff --git a/q2d_bringup/launch/group_ijc.launch.xml b/q2d_bringup/launch/group_ijc.launch.xml
deleted file mode 100644
index 12baaa5..0000000
--- a/q2d_bringup/launch/group_ijc.launch.xml
+++ /dev/null
@@ -1,10 +0,0 @@
-<launch>
-	<arg name="controller" default="group_pid"/>
-	<arg name="config" default="$(find-pkg-share q2d_bringup)/config/$(var controller).yaml"/>
-
-	<node name="controller_spawner" pkg="controller_manager"
-		exec="spawner" output="screen"
-		args="group_controller joint_states_broadcaster">
-		<param from="$(var config)"/>
-	</node>
-</launch>
diff --git a/q2d_bringup/launch/group_pid.launch.xml b/q2d_bringup/launch/group_pid.launch.xml
new file mode 100644
index 0000000..ce38d18
--- /dev/null
+++ b/q2d_bringup/launch/group_pid.launch.xml
@@ -0,0 +1,30 @@
+<!--******************************************************************************
+	                   Quanser 2DSFJE Bringup
+	                  PID Controller Launch File
+          Copyright (C) 2018, 2021 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="config" default="$(find-pkg-share q2d_bringup)/config/group_pid.yaml"/>
+	
+	<node name="shoulder_controller_spawner" pkg="controller_manager" exec="spawner"
+		args="-t effort_controllers/JointGroupPositionController -p $(var config) group_controller"/>
+		
+	<node name="join_state_broadcaster_spawner" pkg="controller_manager" exec="spawner"
+		args="-t joint_state_broadcaster/JointStateBroadcaster joint_state_broadcaster"/>
+</launch>
diff --git a/q2d_bringup/launch/ijc.launch.xml b/q2d_bringup/launch/ijc.launch.xml
deleted file mode 100644
index 0fc4a1a..0000000
--- a/q2d_bringup/launch/ijc.launch.xml
+++ /dev/null
@@ -1,7 +0,0 @@
-<launch>
-	<node name="controller_spawner" pkg="controller_manager"
-		exec="spawner" output="screen"
-		args="shoulder_controller elbow_controller joint_states_broadcaster">
-		<param from="$(var config)"/>
-	</nodde>
-</launch>
diff --git a/q2d_bringup/launch/pid.launch.xml b/q2d_bringup/launch/pid.launch.xml
new file mode 100644
index 0000000..749569c
--- /dev/null
+++ b/q2d_bringup/launch/pid.launch.xml
@@ -0,0 +1,39 @@
+<!--******************************************************************************
+	                   Quanser 2DSFJE Bringup
+	              Group PID Controller Launch File
+          Copyright (C) 2018, 2021 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="config" default="$(find-pkg-share q2d_bringup)/config/pid.yaml"/>
+
+	<node pkg="controller_manager" exec="ros2_control_node">
+		<param name="robot_description" value="$(command 'cat $(find-pkg-share q2d_description)/urdf/q2d.urdf')" type="str"/>
+		<param from="$(var config)"/>
+		<!--param name="use_sim_time" value="true"/-->
+	</node>
+
+	<node name="shoulder_controller_spawner" pkg="controller_manager" exec="spawner"
+		args="-t effort_controllers/JointPositionController -p $(var config) shoulder_controller"/>
+
+	<node name="elbow_controller_spawner" pkg="controller_manager" exec="spawner"
+		args="-t effort_controllers/JointPositionController -p $(var config)elbow_controller"/>
+		
+	<node name="join_state_broadcaster_spawner" pkg="controller_manager" exec="spawner"
+		args="-t joint_state_broadcaster/JointStateBroadcaster joint_state_broadcaster"/>
+</launch>
diff --git a/q2d_bringup/package.xml b/q2d_bringup/package.xml
index df2dabd..6af289e 100644
--- a/q2d_bringup/package.xml
+++ b/q2d_bringup/package.xml
@@ -9,8 +9,8 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <depend>effort_controllers</depend>
-  <depend>joint_state_controller</depend>
+  <exec_depend>effort_controllers</exec_depend>
+  <exec_depend>joint_state_broadcaster</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/q2d_description/launch/gazebo.launch.xml b/q2d_description/launch/gazebo.launch.xml
index 32d9e42..1d74ea1 100644
--- a/q2d_description/launch/gazebo.launch.xml
+++ b/q2d_description/launch/gazebo.launch.xml
@@ -3,7 +3,7 @@
 	<arg name="gui" default="true"/>
 	<arg name="use_sim_time" default="true"/>
 
-	<set_env name="GAZEBO_MODEL_PATH" value="$(env GAZEBO_MODEL_PATH):$(find-pkg-prefix q2d_description)/share/"/>
+	<set_env name="GAZEBO_MODEL_PATH" value="$(env GAZEBO_MODEL_PATH ''):$(find-pkg-prefix q2d_description)/share/"/>
 	
 	<include file="$(find-pkg-share gazebo_ros)/launch/gazebo.launch.py">
 		<arg name="pause" value="$(var pause)"/>
@@ -12,7 +12,9 @@
 		<arg name="world" value="worlds/empty_sky.world" />
 	</include>
 
-	<include file="$(find-pkg-share q2d_description)/launch/q2d.launch.xml"/>
+	<include file="$(find-pkg-share q2d_description)/launch/q2d.launch.xml">
+		<arg name="use_sim_time" value="$(var use_sim_time)"/>
+	</include>
 
 	<node name="q2d_spawner" pkg="gazebo_ros" exec="spawn_entity.py" args="-topic robot_description -entity q2d" />
 </launch>
diff --git a/q2d_description/launch/q2d.launch.xml b/q2d_description/launch/q2d.launch.xml
index 2fe6ec1..1972173 100644
--- a/q2d_description/launch/q2d.launch.xml
+++ b/q2d_description/launch/q2d.launch.xml
@@ -1,5 +1,8 @@
 <launch>
+	<arg name="use_sim_time" default="false"/>
+
 	<node name="robot_state_publisher" pkg="robot_state_publisher" exec="robot_state_publisher">
-		<param name="robot_description" value="$(command 'cat $(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')" 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 e05403a..d231ba1 100644
--- a/q2d_description/urdf/q2d.urdf
+++ b/q2d_description/urdf/q2d.urdf
@@ -1,3 +1,4 @@
+<?xml version="1.0"?>
 <robot name="q2d">
 
 	<link name="origin_link"/>
@@ -190,13 +191,6 @@
 		</actuator>
 	</transmission>
 
-	<gazebo>
-		<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
-			<!-- <robotNamespace>/q2d</robotNamespace> -->
-			<controlPeriod>0.001</controlPeriod>
-		</plugin>
-	</gazebo>
-
 	<gazebo reference="base_link">
 		<visual>
 			<material>
@@ -251,4 +245,38 @@
 			</material>
 		</visual>
 	</gazebo>
+
+	<ros2_control name="GazeboSystem" type="system">
+		<hardware>
+			<plugin>gazebo_ros2_control/GazeboSystem</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="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_bringup)/config/controller_manager.yaml</parameters>
+		</plugin>
+	</gazebo>
 </robot>
diff --git a/q2d_teleop/CMakeLists.txt b/q2d_teleop/CMakeLists.txt
index bc6a3be..1e1de62 100644
--- a/q2d_teleop/CMakeLists.txt
+++ b/q2d_teleop/CMakeLists.txt
@@ -8,8 +8,8 @@ endif()
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
 find_package(Eigen3 REQUIRED)
-find_package(cmake_modules REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(kdl_parser REQUIRED)
 find_package(orocos_kdl REQUIRED)
@@ -26,7 +26,6 @@ ament_target_dependencies(
   q2d_teleop_tablet
   "rclcpp"
   "Eigen3"
-  "cmake_modules"
   "geometry_msgs"
   "kdl_parser"
   "orocos_kdl"
@@ -35,8 +34,8 @@ ament_target_dependencies(
   "urdf"
 )
 
-install(TARGETS q2d_teleop_tablet
-  DESTINATION lib/${PROJECT_NAME})
+#install(TARGETS q2d_teleop_tablet
+#  DESTINATION lib/${PROJECT_NAME})
 
 add_executable(q2d_teleop_rviz src/q2d_teleop_rviz.cpp)
 target_include_directories(q2d_teleop_rviz PUBLIC
@@ -47,7 +46,6 @@ ament_target_dependencies(
   q2d_teleop_rviz
   "rclcpp"
   "Eigen3"
-  "cmake_modules"
   "geometry_msgs"
   "kdl_parser"
   "orocos_kdl"
@@ -56,7 +54,7 @@ ament_target_dependencies(
   "urdf"
 )
 
-install(TARGETS q2d_teleop_rviz
+install(TARGETS q2d_teleop_tablet q2d_teleop_rviz
   DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY launch rviz
diff --git a/q2d_teleop/launch/display.launch.xml b/q2d_teleop/launch/display.launch.xml
index 61e7a99..2c73b72 100644
--- a/q2d_teleop/launch/display.launch.xml
+++ b/q2d_teleop/launch/display.launch.xml
@@ -1,5 +1,29 @@
+<!--
+  q2d_teleop: A ROS 2 package node to teloperate the Quanser 2DSFJE robot.
+  
+  Copyright (c) 2018, 2019, 2021 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 2 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 General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+    You can also obtain a copy of the GNU General Public License
+    at <http://www.gnu.org/licenses>.
+
+-->
+
 <launch>
         <include file="$(find-pkg-share q2d_description)/launch/q2d.launch.xml" />
 	<node pkg="tf2_ros" exec="static_transform_publisher" name="q2d_origin_publisher" args="0 0 0 0 0 0 1 map origin_link" />
-        <node name="rviz" pkg="rviz2" exec="rviz2" args="-d $(find-pkg-share q2d_teleop)/rviz/display.rviz" required="true" />
+        <node name="rviz" pkg="rviz2" exec="rviz2" args="-d $(find-pkg-share q2d_teleop)/rviz/display.rviz" />
 </launch>
diff --git a/q2d_teleop/launch/gazebo.launch.xml b/q2d_teleop/launch/gazebo.launch.xml
index 3ea4c31..aae3501 100644
--- a/q2d_teleop/launch/gazebo.launch.xml
+++ b/q2d_teleop/launch/gazebo.launch.xml
@@ -1,13 +1,37 @@
+<!--
+  q2d_teleop: A ROS 2 package node to teloperate the Quanser 2DSFJE robot.
+  
+  Copyright (c) 2018, 2019, 2021 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 2 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 General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+    You can also obtain a copy of the GNU General Public License
+    at <http://www.gnu.org/licenses>.
+
+-->
+
 <launch>
 	<include file="$(find-pkg-share q2d_bringup)/launch/gazebo.launch.xml">
-		<arg name="paused" value="false" />
+		<arg name="pause" value="false" />
 	</include>
 
   	<include file="$(find-pkg-share q2d_teleop)/launch/q2d_teleop_tablet.launch.xml" />
 
-  	<include file="$(find q2d_teleop)/launch/q2d_teleop_rviz.launch.xml" />
+  	<include file="$(find-pkg-share q2d_teleop)/launch/q2d_teleop_rviz.launch.xml" />
 
-  	<include file="$(find q2d_teleop)/launch/display.launch.xml" />
+  	<include file="$(find-pkg-share q2d_teleop)/launch/display.launch.xml" />
 
-  	<include file="$(find gfxtablet_ros)/launch/gfxtablet.launch.xml" />
+  	<include file="$(find-pkg-share gfxtablet_ros)/launch/gfxtablet.launch.xml" />
 </launch>
diff --git a/q2d_teleop/launch/q2d_teleop_rviz.launch.xml b/q2d_teleop/launch/q2d_teleop_rviz.launch.xml
index 7424bc4..70bd272 100644
--- a/q2d_teleop/launch/q2d_teleop_rviz.launch.xml
+++ b/q2d_teleop/launch/q2d_teleop_rviz.launch.xml
@@ -1,4 +1,27 @@
+<!--
+  q2d_teleop: A ROS 2 package node to teloperate the Quanser 2DSFJE robot.
+  
+  Copyright (c) 2018, 2019, 2021 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 2 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 General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+    You can also obtain a copy of the GNU General Public License
+    at <http://www.gnu.org/licenses>.
+
+-->
+
 <launch>
-	<node name="q2d_teleop_rviz" pkg="q2d_teleop" exec="q2d_teleop_rviz"
-		output="screen" />
+	<node name="q2d_teleop_rviz" pkg="q2d_teleop" exec="q2d_teleop_rviz" />
 </launch>
diff --git a/q2d_teleop/launch/q2d_teleop_tablet.launch.xml b/q2d_teleop/launch/q2d_teleop_tablet.launch.xml
index 83ea1c1..4fd4242 100644
--- a/q2d_teleop/launch/q2d_teleop_tablet.launch.xml
+++ b/q2d_teleop/launch/q2d_teleop_tablet.launch.xml
@@ -1,4 +1,27 @@
+<!--
+  q2d_teleop: A ROS 2 package node to teloperate the Quanser 2DSFJE robot.
+  
+  Copyright (c) 2018, 2019, 2021 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 2 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 General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+    You can also obtain a copy of the GNU General Public License
+    at <http://www.gnu.org/licenses>.
+
+-->
+
 <launch>
-	<node name="q2d_teleop_tablet" pkg="q2d_teleop" exec="q2d_teleop_tablet"
-		output="screen" />
+	<node name="q2d_teleop_tablet" pkg="q2d_teleop" exec="q2d_teleop_tablet" />
 </launch>
diff --git a/q2d_teleop/package.xml b/q2d_teleop/package.xml
index 2340893..1ea599a 100644
--- a/q2d_teleop/package.xml
+++ b/q2d_teleop/package.xml
@@ -10,14 +10,15 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>rclcpp</depend>
-  <depend>Eigen3</depend>
-  <depend>cmake_modules</depend>
   <depend>geometry_msgs</depend>
   <depend>kdl_parser</depend>
   <depend>orocos_kdl</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>urdf</depend>
+  
+  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
+  <build_depend>eigen</build_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/q2d_teleop/rviz/display.rviz b/q2d_teleop/rviz/display.rviz
index 40ef76e..1cfaa99 100644
--- a/q2d_teleop/rviz/display.rviz
+++ b/q2d_teleop/rviz/display.rviz
@@ -1,33 +1,27 @@
 Panels:
-  - Class: rviz/Displays
+  - Class: rviz_common/Displays
     Help Height: 78
     Name: Displays
     Property Tree Widget:
       Expanded:
         - /Global Options1
-        - /Status1
-        - /Grid1/Offset1
         - /RobotModel1
-        - /RobotModel1/Links1
-        - /RobotModel1/Links1/tool_link1
-        - /RobotModel1/Links1/tool_link1/Position1
       Splitter Ratio: 0.5
-    Tree Height: 559
-  - Class: rviz/Selection
+    Tree Height: 549
+  - Class: rviz_common/Selection
     Name: Selection
-  - Class: rviz/Tool Properties
+  - Class: rviz_common/Tool Properties
     Expanded:
-      - /2D Pose Estimate1
-      - /2D Nav Goal1
+      - /2D Goal Pose1
       - /Publish Point1
     Name: Tool Properties
-    Splitter Ratio: 0.588679
-  - Class: rviz/Views
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
     Expanded:
       - /Current View1
     Name: Views
     Splitter Ratio: 0.5
-  - Class: rviz/Time
+  - Class: rviz_common/Time
     Experimental: false
     Name: Time
     SyncMode: 0
@@ -36,12 +30,12 @@ Visualization Manager:
   Class: ""
   Displays:
     - Alpha: 0.5
-      Cell Size: 0.2
-      Class: rviz/Grid
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
       Color: 160; 160; 164
       Enabled: true
       Line Style:
-        Line Width: 0.03
+        Line Width: 0.029999999329447746
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -50,16 +44,16 @@ Visualization Manager:
         Y: 0
         Z: 0
       Plane: XY
-      Plane Cell Count: 8
+      Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: true
     - Alpha: 0
-      Cell Size: 0.001
-      Class: rviz/Grid
-      Color: 59; 59; 59
+      Cell Size: 0.0010000000474974513
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
       Enabled: true
       Line Style:
-        Line Width: 0.03
+        Line Width: 0.029999999329447746
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -72,8 +66,16 @@ Visualization Manager:
       Reference Frame: <Fixed Frame>
       Value: true
     - Alpha: 1
-      Class: rviz/RobotModel
+      Class: rviz_default_plugins/RobotModel
       Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
       Enabled: true
       Links:
         All Links Enabled: true
@@ -112,10 +114,9 @@ Visualization Manager:
           Value: true
         tool_link:
           Alpha: 1
-          Show Axes: true
-          Show Trail: true
+          Show Axes: false
+          Show Trail: false
       Name: RobotModel
-      Robot Description: robot_description
       TF Prefix: ""
       Update Interval: 0
       Value: true
@@ -127,36 +128,64 @@ Visualization Manager:
     Frame Rate: 30
   Name: root
   Tools:
-    - Class: rviz/Interact
+    - Class: rviz_default_plugins/Interact
       Hide Inactive Objects: true
-    - Class: rviz/MoveCamera
-    - Class: rviz/Select
-    - Class: rviz/FocusCamera
-    - Class: rviz/Measure
-    - Class: rviz/SetInitialPose
-      Topic: /initialpose
-    - Class: rviz/SetGoal
-      Topic: /move_base_simple/goal
-    - Class: rviz/PublishPoint
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
       Single click: true
-      Topic: /clicked_point
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
   Value: true
   Views:
     Current:
-      Angle: 0
-      Class: rviz/TopDownOrtho
+      Class: rviz_default_plugins/Orbit
+      Distance: 10
       Enable Stereo Rendering:
-        Stereo Eye Separation: 0.06
+        Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
       Name: Current View
-      Near Clip Distance: 0.01
-      Scale: 348.433
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.785398006439209
       Target Frame: <Fixed Frame>
-      Value: TopDownOrtho (rviz)
-      X: 0.00473011
-      Y: -0.00390336
+      Value: Orbit (rviz)
+      Yaw: 0.785398006439209
     Saved: ~
 Window Geometry:
   Displays:
@@ -164,7 +193,7 @@ Window Geometry:
   Height: 846
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000013c000002bafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000036000002ba000000b700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002bafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000036000002ba0000009b00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024700fffffffb0000000800540069006d0065010000000000000450000000000000000000000259000002ba00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025600fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
diff --git a/q2d_teleop/src/q2d_teleop_rviz.cpp b/q2d_teleop/src/q2d_teleop_rviz.cpp
index 59d17a6..aa79d84 100644
--- a/q2d_teleop/src/q2d_teleop_rviz.cpp
+++ b/q2d_teleop/src/q2d_teleop_rviz.cpp
@@ -1,7 +1,7 @@
 /*
-  q2d_teleop_rviz: A ROS node to teloperate the Quanser 2DSFJE robot.
+  q2d_teleop_rviz: A ROS 2 node to teloperate the Quanser 2DSFJE robot.
   
-  Copyright (c) 2018,2019 Walter Fetter Lages <w.fetter@ieee.org>
+  Copyright (c) 2018, 2019, 2021 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
@@ -22,54 +22,60 @@
 
 */
 
-#include <ros/ros.h>
-#include <geometry_msgs/PointStamped.h>
-#include <std_msgs/Float64.h>
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/point_stamped.hpp>
+#include <std_msgs/msg/float64.hpp>
+#include <std_msgs/msg/string.hpp>
 #include <kdl/chainiksolverpos_lma.hpp>
 #include <kdl_parser/kdl_parser.hpp>
 
 #define sqr(x) ((x)*(x))
 
-class Q2dTeleop
+class Q2dTeleop: public rclcpp::Node
 {
 	public:
-	Q2dTeleop(ros::NodeHandle node);
+	Q2dTeleop(void);
 	~Q2dTeleop(void);
 	void publish(void);
 			
 	private:
-	ros::NodeHandle node_;
-
-	ros::Subscriber clickSub_;
-	ros::Publisher	shoulderCmdPub_;
-	ros::Publisher	elbowCmdPub_;
+	rclcpp::Subscription<geometry_msgs::msg::PointStamped>::SharedPtr clickSub_;
+	rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr shoulderCmdPub_;
+	rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr elbowCmdPub_;
 	
 	KDL::Frame goal_;
+
+	std::string robotDescription_;
 	KDL::Chain chain_;
 	KDL::ChainIkSolverPos_LMA *ikSolverPos_;
 	KDL::JntArray q_;
 		
-	void clickCB(const geometry_msgs::PointStamped &click);
+	void clickCB(const geometry_msgs::msg::PointStamped::SharedPtr click);
+	void robotDescriptionCB(const std_msgs::msg::String::SharedPtr robotDescription);
 };
 
-Q2dTeleop::Q2dTeleop(ros::NodeHandle node): q_(2)
+Q2dTeleop::Q2dTeleop(void): Node("Q2d_teleop_rviz"), q_(2)
 {
-	node_=node;
+	using std::placeholders::_1;
+	clickSub_=create_subscription<geometry_msgs::msg::PointStamped>("clicked_point",100,std::bind(&Q2dTeleop::clickCB,this,_1));
+	shoulderCmdPub_=create_publisher<std_msgs::msg::Float64>("shoulder_controller/command",100);
+        elbowCmdPub_=create_publisher<std_msgs::msg::Float64>("elbow_controller/command",100);
 	
-	clickSub_=node_.subscribe("/clicked_point",100,&Q2dTeleop::clickCB,this);
-	shoulderCmdPub_=node_.advertise<std_msgs::Float64>("shoulder_controller/command",100);
-        elbowCmdPub_=node_.advertise<std_msgs::Float64>("elbow_controller/command",100);
-	
-	std::string robotDescription;
-	if(!node.getParam("robot_description",robotDescription))
-		ROS_ERROR_STREAM("Could not find 'robot_description'.");
-		
+        rclcpp::QoS qos(rclcpp::KeepLast(1));
+        qos.transient_local();
+        auto robotDescriptionSubscriber_=create_subscription<std_msgs::msg::String>("robot_description",qos,std::bind(&Q2dTeleop::robotDescriptionCB,this,_1));
+        while(robotDescription_.empty())
+        {
+                RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(get_logger(),*get_clock(),1000,"Waiting for robot model on /robot_description.");
+                rclcpp::spin_some(get_node_base_interface());
+        }
+
 	KDL::Tree tree;
-	if (!kdl_parser::treeFromString(robotDescription,tree))
-		ROS_ERROR_STREAM("Failed to construct KDL tree.");
+	if (!kdl_parser::treeFromString(robotDescription_,tree))
+		RCLCPP_ERROR_STREAM(get_logger(),"Failed to construct KDL tree.");
 		
 	if (!tree.getChain("origin_link","tool_link",chain_))
-		ROS_ERROR_STREAM("Failed to get chain from KDL tree.");
+		RCLCPP_ERROR_STREAM(get_logger(),"Failed to get chain from KDL tree.");
 
 	q_.resize(chain_.getNrOfJoints());
 
@@ -87,22 +93,24 @@ Q2dTeleop::Q2dTeleop(ros::NodeHandle node): q_(2)
 
 Q2dTeleop::~Q2dTeleop(void)
 {
-	clickSub_.shutdown();
-	shoulderCmdPub_.shutdown();
-        elbowCmdPub_.shutdown();
 	delete ikSolverPos_;
 }
 
-void Q2dTeleop::clickCB(const geometry_msgs::PointStamped &click)
+void Q2dTeleop::clickCB(const geometry_msgs::msg::PointStamped::SharedPtr click)
 {
-	goal_.p.x(click.point.x);
-	goal_.p.y(click.point.y);
+	goal_.p.x(click->point.x);
+	goal_.p.y(click->point.y);
 	goal_.p.z(0.1477);
-	goal_.M.RotZ(atan2(click.point.y,click.point.x));
+	goal_.M.RotZ(atan2(click->point.y,click->point.x));
 	
 	publish();
 }
 
+void Q2dTeleop::robotDescriptionCB(const std_msgs::msg::String::SharedPtr robotDescription)
+{
+        robotDescription_=robotDescription->data;
+}
+
 #define KDL_IK
 void Q2dTeleop::publish(void)
 {
@@ -110,7 +118,7 @@ void Q2dTeleop::publish(void)
 	KDL::JntArray q_out=q_;
 	int error=0;
 	if((error=ikSolverPos_->CartToJnt(q_,goal_,q_out)) < 0)
-		ROS_ERROR_STREAM("Failed to compute invere kinematics: (" << error << ") " 
+		RCLCPP_ERROR_STREAM(get_logger(),"Failed to compute invere kinematics: (" << error << ") "
 		        << ikSolverPos_->strError(error));
         q_=q_out;
 #else
@@ -141,25 +149,21 @@ void Q2dTeleop::publish(void)
         }
 #endif
 
-	std_msgs::Float64 shoulderCmd;
-	std_msgs::Float64 elbowCmd;
+	std_msgs::msg::Float64 shoulderCmd;
+	std_msgs::msg::Float64 elbowCmd;
 	
 	shoulderCmd.data=q_(0);
 	elbowCmd.data=q_(1);
 	
-	shoulderCmdPub_.publish(shoulderCmd);
-	elbowCmdPub_.publish(elbowCmd);
+	shoulderCmdPub_->publish(shoulderCmd);
+	elbowCmdPub_->publish(elbowCmd);
 }
 
 
 int main(int argc,char* argv[])
 {
-        ros::init(argc,argv,"q2d_teleop_rviz");
-        ros::NodeHandle node;
-	
-	Q2dTeleop q2dTeleop(node);
-
-	ros::spin();
-
+	rclcpp::init(argc,argv);
+	rclcpp::spin(std::make_shared<Q2dTeleop>());
+	rclcpp::shutdown();
 	return 0;
 }
diff --git a/q2d_teleop/src/q2d_teleop_tablet.cpp b/q2d_teleop/src/q2d_teleop_tablet.cpp
index ab2de87..ecd06b3 100644
--- a/q2d_teleop/src/q2d_teleop_tablet.cpp
+++ b/q2d_teleop/src/q2d_teleop_tablet.cpp
@@ -1,7 +1,7 @@
 /*
-  q2d_teleop_tablet: A ROS node to teloperate the Quanser 2DSFJE robot.
+  q2d_teleop_tablet: A ROS 2 node to teloperate the Quanser 2DSFJE robot.
   
-  Copyright (c) 2018,2019 Walter Fetter Lages <w.fetter@ieee.org>
+  Copyright (c) 2018, 2019, 2021 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
@@ -22,54 +22,61 @@
 
 */
 
-#include <ros/ros.h>
-#include <sensor_msgs/Joy.h>
-#include <std_msgs/Float64.h>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/logging.hpp>
+#include <sensor_msgs/msg/joy.hpp>
+#include <std_msgs/msg/float64.hpp>
+#include <std_msgs/msg/string.hpp>
 #include <kdl/chainiksolverpos_lma.hpp>
 #include <kdl_parser/kdl_parser.hpp>
 
 #define sqr(x) ((x)*(x))
 
-class Q2dTeleop
+class Q2dTeleop: public rclcpp::Node
 {
 	public:
-	Q2dTeleop(ros::NodeHandle node);
+	Q2dTeleop(void);
 	~Q2dTeleop(void);
 	void publish(void);
 			
 	private:
-	ros::NodeHandle node_;
-
-	ros::Subscriber joySub_;
-	ros::Publisher	shoulderCmdPub_;
-	ros::Publisher	elbowCmdPub_;
+	rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joySub_;
+	rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr shoulderCmdPub_;
+	rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr elbowCmdPub_;
 	
 	KDL::Frame goal_;
+
+	std::string robotDescription_;
 	KDL::Chain chain_;
 	KDL::ChainIkSolverPos_LMA *ikSolverPos_;
 	KDL::JntArray q_;
 		
-	void joyCB(const sensor_msgs::Joy &joy);
+	void joyCB(const sensor_msgs::msg::Joy::SharedPtr joy);
+	void robotDescriptionCB(const std_msgs::msg::String::SharedPtr robotDescription);
 };
 
-Q2dTeleop::Q2dTeleop(ros::NodeHandle node): q_(2)
+Q2dTeleop::Q2dTeleop(void): Node("q2d_teleop_tablet"), q_(2)
 {
-	node_=node;
-	
-	joySub_=node_.subscribe("joy",100,&Q2dTeleop::joyCB,this);
-	shoulderCmdPub_=node_.advertise<std_msgs::Float64>("shoulder_controller/command",100);
-        elbowCmdPub_=node_.advertise<std_msgs::Float64>("elbow_controller/command",100);
+	using std::placeholders::_1;
+	joySub_=create_subscription<sensor_msgs::msg::Joy>("joy",100,std::bind(&Q2dTeleop::joyCB,this,_1));
+	shoulderCmdPub_=create_publisher<std_msgs::msg::Float64>("shoulder_controller/command",100);
+        elbowCmdPub_=create_publisher<std_msgs::msg::Float64>("elbow_controller/command",100);
 	
-	std::string robotDescription;
-	if(!node.getParam("robot_description",robotDescription))
-		ROS_ERROR_STREAM("Could not find 'robot_description'.");
+        rclcpp::QoS qos(rclcpp::KeepLast(1));
+        qos.transient_local();
+        auto robotDescriptionSubscriber_=create_subscription<std_msgs::msg::String>("robot_description",qos,std::bind(&Q2dTeleop::robotDescriptionCB,this,_1));
+        while(robotDescription_.empty())
+        {
+                RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(get_logger(),*get_clock(),1000,"Waiting for robot model on /robot_description.");
+                rclcpp::spin_some(get_node_base_interface());
+        }
 		
 	KDL::Tree tree;
-	if (!kdl_parser::treeFromString(robotDescription,tree))
-		ROS_ERROR_STREAM("Failed to construct KDL tree.");
+	if (!kdl_parser::treeFromString(robotDescription_,tree))
+		RCLCPP_ERROR_STREAM(get_logger(),"Failed to construct KDL tree.");
 		
 	if (!tree.getChain("origin_link","tool_link",chain_))
-		ROS_ERROR_STREAM("Failed to get chain from KDL tree.");
+		RCLCPP_ERROR_STREAM(get_logger(),"Failed to get chain from KDL tree.");
 
 	q_.resize(chain_.getNrOfJoints());
 
@@ -87,24 +94,26 @@ Q2dTeleop::Q2dTeleop(ros::NodeHandle node): q_(2)
 
 Q2dTeleop::~Q2dTeleop(void)
 {
-	joySub_.shutdown();
-	shoulderCmdPub_.shutdown();
-        elbowCmdPub_.shutdown();
 	delete ikSolverPos_;
 }
 
-void Q2dTeleop::joyCB(const sensor_msgs::Joy &joy)
+void Q2dTeleop::joyCB(const sensor_msgs::msg::Joy::SharedPtr joy)
 {
-//	goal_.p.x(joy.axes[0]*0.61/2.0);
-//	goal_.p.y(-joy.axes[1]*0.61);
-	goal_.p.x(joy.axes[0]*0.8);
-	goal_.p.y(-joy.axes[1]*0.8);	// 20 cm/division in GfxTablet
+//	goal_.p.x(joy->axes[0]*0.61/2.0);
+//	goal_.p.y(-joy->axes[1]*0.61);
+	goal_.p.x(joy->axes[0]*0.8);
+	goal_.p.y(-joy->axes[1]*0.8);	// 20 cm/division in GfxTablet
 	goal_.p.z(0.1477);
-	goal_.M.RotZ(atan2(joy.axes[1],joy.axes[0]));
+	goal_.M.RotZ(atan2(joy->axes[1],joy->axes[0]));
 	
 	publish();
 }
 
+void Q2dTeleop::robotDescriptionCB(const std_msgs::msg::String::SharedPtr robotDescription)
+{
+        robotDescription_=robotDescription->data;
+}
+
 #define KDL_IK
 void Q2dTeleop::publish(void)
 {
@@ -112,7 +121,7 @@ void Q2dTeleop::publish(void)
 	KDL::JntArray q_out=q_;
 	int error=0;
 	if((error=ikSolverPos_->CartToJnt(q_,goal_,q_out)) < 0)
-		ROS_ERROR_STREAM("Failed to compute invere kinematics: (" << error << ") " 
+		RCLCPP_ERROR_STREAM(get_logger(),"Failed to compute invere kinematics: (" << error << ") "
 		        << ikSolverPos_->strError(error));
         q_=q_out;
 #else
@@ -143,25 +152,20 @@ void Q2dTeleop::publish(void)
         }
 #endif
 
-	std_msgs::Float64 shoulderCmd;
-	std_msgs::Float64 elbowCmd;
+	std_msgs::msg::Float64 shoulderCmd;
+	std_msgs::msg::Float64 elbowCmd;
 	
 	shoulderCmd.data=q_(0);
 	elbowCmd.data=q_(1);
 	
-	shoulderCmdPub_.publish(shoulderCmd);
-	elbowCmdPub_.publish(elbowCmd);
+	shoulderCmdPub_->publish(shoulderCmd);
+	elbowCmdPub_->publish(elbowCmd);
 }
 
-
 int main(int argc,char* argv[])
 {
-        ros::init(argc,argv,"q2d_teleop_tablet");
-        ros::NodeHandle node;
-	
-	Q2dTeleop q2dTeleop(node);
-	
-	ros::spin();
-
+	rclcpp::init(argc,argv);
+	rclcpp::spin(std::make_shared<Q2dTeleop>());
+	rclcpp::shutdown();
 	return 0;
 }