From f5ccac1576dc1a116b822c2a817a1467e25a93b2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?=
 <christophfroehlich@users.noreply.github.com>
Date: Mon, 1 May 2023 22:27:41 +0200
Subject: [PATCH] [Example 4+5] Fix tool link and slowdown (#266)

* Fix slowdown of hw interface of example_4

* Readd missing joints for tool_link
---
 example_4/description/rviz/rrbot.rviz                  |  3 ++-
 .../description/urdf/rrbot_description.urdf.xacro      | 10 ++++++++++
 example_4/hardware/rrbot_system_with_sensor.cpp        |  3 +--
 .../description/urdf/rrbot_description.urdf.xacro      | 10 ++++++++++
 4 files changed, 23 insertions(+), 3 deletions(-)

diff --git a/example_4/description/rviz/rrbot.rviz b/example_4/description/rviz/rrbot.rviz
index 0efbecc38..adb274d83 100644
--- a/example_4/description/rviz/rrbot.rviz
+++ b/example_4/description/rviz/rrbot.rviz
@@ -103,7 +103,8 @@ Visualization Manager:
       Update Interval: 0
       Value: true
       Visual Enabled: true
-    - Alpha: 1
+    - Accept NaN Values: true
+      Alpha: 1
       Arrow Width: 0.5
       Class: rviz_default_plugins/Wrench
       Enabled: true
diff --git a/example_4/description/urdf/rrbot_description.urdf.xacro b/example_4/description/urdf/rrbot_description.urdf.xacro
index cfcb0388b..d4ddcb744 100644
--- a/example_4/description/urdf/rrbot_description.urdf.xacro
+++ b/example_4/description/urdf/rrbot_description.urdf.xacro
@@ -114,6 +114,16 @@
           </inertial>
         </link>
 
+  <joint name="${prefix}tool_joint" type="fixed">
+    <origin xyz="0 0 1" rpy="0 0 0" />
+    <parent link="${prefix}link2"/>
+    <child link="${prefix}tool_link" />
+  </joint>
+
+  <!-- Tool Link -->
+  <link name="${prefix}tool_link">
+  </link>
+
   </xacro:macro>
 
 </robot>
diff --git a/example_4/hardware/rrbot_system_with_sensor.cpp b/example_4/hardware/rrbot_system_with_sensor.cpp
index 65dcd929c..eea370e1f 100644
--- a/example_4/hardware/rrbot_system_with_sensor.cpp
+++ b/example_4/hardware/rrbot_system_with_sensor.cpp
@@ -216,8 +216,7 @@ hardware_interface::return_type RRBotSystemWithSensorHardware::read(
   for (uint i = 0; i < hw_joint_states_.size(); i++)
   {
     // Simulate RRBot's movement
-    hw_joint_states_[i] =
-      hw_joint_commands_[i] + (hw_joint_states_[i] - hw_joint_commands_[i]) / hw_slowdown_;
+    hw_joint_states_[i] += (hw_joint_commands_[i] - hw_joint_states_[i]) / hw_slowdown_;
     RCLCPP_INFO(
       rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Got state %.5f for joint %u!",
       hw_joint_states_[i], i);
diff --git a/example_5/description/urdf/rrbot_description.urdf.xacro b/example_5/description/urdf/rrbot_description.urdf.xacro
index cfcb0388b..8ef1f053e 100644
--- a/example_5/description/urdf/rrbot_description.urdf.xacro
+++ b/example_5/description/urdf/rrbot_description.urdf.xacro
@@ -114,6 +114,16 @@
           </inertial>
         </link>
 
+        <joint name="${prefix}tool_joint" type="fixed">
+          <origin xyz="0 0 1" rpy="0 0 0" />
+          <parent link="${prefix}link2"/>
+          <child link="${prefix}tool_link" />
+        </joint>
+
+        <!-- Tool Link -->
+        <link name="${prefix}tool_link">
+        </link>
+
   </xacro:macro>
 
 </robot>