[Example 4+5] Fix tool link and slowdown (ros-controls#266)

* Fix slowdown of hw interface of example_4 * Readd missing joints for tool_link
StoglRobotics-forks · May 1, 2023 · f5ccac1 · f5ccac1
1 parent 269654f
commit f5ccac1
Show file tree

Hide file tree

Showing 4 changed files with 23 additions and 3 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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>