From 9c26ce9c34aaf9a17a6d90e51fed487bc710c1b8 Mon Sep 17 00:00:00 2001
From: Ruixiang Du <ruixiang.du@gmail.com>
Date: Wed, 25 Mar 2020 00:03:18 +0800
Subject: [PATCH] scout joint controller setup

---
 README.md                                     |  1 +
 scout_description/launch/display_model.launch | 27 +---------
 .../launch/scout_v2_stock.launch              | 26 ---------
 scout_description/rviz/model_display.rviz     | 53 +++++++++++--------
 scout_description/urdf/scout_v2.gazebo        |  9 ++--
 scout_description/urdf/scout_v2.xacro         |  2 +-
 .../urdf/scout_wheel_type1.xacro              | 21 ++++----
 .../urdf/scout_wheel_type2.xacro              | 20 +++----
 8 files changed, 57 insertions(+), 102 deletions(-)

diff --git a/README.md b/README.md
index 54a9b90..ae98b76 100644
--- a/README.md
+++ b/README.md
@@ -70,6 +70,7 @@ Two scripts inside the "scout_bringup/scripts" folder are provided for easy setu
     ```
     $ sudo apt install ros-melodic-teleop-twist-keyboard
     $ sudo apt-get install ros-melodic-joint-state-publisher-gui
+    $ sudo apt install ros-melodic-ros-controllers
     ```
 
     Change ros-melodic-* in the command to ros-kinetic-* if you're using ROS Kinetic.
diff --git a/scout_description/launch/display_model.launch b/scout_description/launch/display_model.launch
index ef513b7..4e76d4b 100644
--- a/scout_description/launch/display_model.launch
+++ b/scout_description/launch/display_model.launch
@@ -1,29 +1,3 @@
-<?xml version="1.0"?>
-<!--
-Software License Agreement (BSD)
-
-\file      description.launch
-\authors   Paul Bovbel <pbovbel@clearpathrobotics.com>, Devon Ash <dash@clearpathrobotics.com>
-\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved.
-
-Redistribution and use in source and binary forms, with or without modification, are permitted provided that
-the following conditions are met:
- * Redistributions of source code must retain the above copyright notice, this list of conditions and the
-   following disclaimer.
- * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
-   following disclaimer in the documentation and/or other materials provided with the distribution.
- * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
-   products derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
-RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
-PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
-DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
-OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
--->
-
 <launch>
         <arg name="model" />
 
@@ -31,5 +5,6 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
         <!-- <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node> -->
         <node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" ></node>
         <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+
         <node name="rviz" pkg="rviz" type="rviz" args="-d $(find scout_description)/rviz/model_display.rviz" />
 </launch>
\ No newline at end of file
diff --git a/scout_description/launch/scout_v2_stock.launch b/scout_description/launch/scout_v2_stock.launch
index 6a7cb19..37d50b5 100644
--- a/scout_description/launch/scout_v2_stock.launch
+++ b/scout_description/launch/scout_v2_stock.launch
@@ -1,31 +1,5 @@
 <?xml version="1.0"?>
-<!--
-Software License Agreement (BSD)
 
-\file      description.launch
-\authors   Paul Bovbel <pbovbel@clearpathrobotics.com>, Devon Ash <dash@clearpathrobotics.com>
-\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved.
-
-Redistribution and use in source and binary forms, with or without modification, are permitted provided that
-the following conditions are met:
- * Redistributions of source code must retain the above copyright notice, this list of conditions and the
-   following disclaimer.
- * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
-   following disclaimer in the documentation and/or other materials provided with the distribution.
- * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
-   products derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
-RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
-PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
-DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
-OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
--->
 <launch>
     <param name="robot_description" command="$(find xacro)/xacro '$(find scout_description)/urdf/scout_v2.xacro'" />
-    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"></node>
-    <!-- <node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"></node> -->
-    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
 </launch>
diff --git a/scout_description/rviz/model_display.rviz b/scout_description/rviz/model_display.rviz
index bf3e6ae..ed89270 100644
--- a/scout_description/rviz/model_display.rviz
+++ b/scout_description/rviz/model_display.rviz
@@ -8,7 +8,7 @@ Panels:
         - /Status1
         - /RobotModel1
       Splitter Ratio: 0.5
-    Tree Height: 1088
+    Tree Height: 732
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -96,6 +96,10 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        world:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
       Name: RobotModel
       Robot Description: robot_description
       TF Prefix: ""
@@ -121,32 +125,35 @@ Visualization Manager:
           Value: true
         rear_right_wheel_link:
           Value: true
+        world:
+          Value: true
       Marker Scale: 1
       Name: TF
       Show Arrows: true
       Show Axes: true
       Show Names: true
       Tree:
-        base_link:
-          base_footprint:
-            {}
-          front_left_wheel_link:
-            {}
-          front_right_wheel_link:
-            {}
-          inertial_link:
-            {}
-          rear_left_wheel_link:
-            {}
-          rear_right_wheel_link:
-            {}
+        world:
+          base_link:
+            base_footprint:
+              {}
+            front_left_wheel_link:
+              {}
+            front_right_wheel_link:
+              {}
+            inertial_link:
+              {}
+            rear_left_wheel_link:
+              {}
+            rear_right_wheel_link:
+              {}
       Update Interval: 0
       Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
     Default Light: true
-    Fixed Frame: base_link
+    Fixed Frame: world
     Frame Rate: 30
   Name: root
   Tools:
@@ -170,7 +177,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 2.0112123489379883
+      Distance: 2.9432930946350098
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -185,18 +192,18 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.6247956156730652
+      Pitch: 0.5447957515716553
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 3.785417318344116
+      Yaw: 4.27542781829834
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1385
+  Height: 1029
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000004cbfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004cb000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009bd0000003efc0100000002fb0000000800540069006d00650100000000000009bd000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000861000004cb00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000367fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000367000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000074d0000003efc0100000002fb0000000800540069006d006501000000000000074d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005f10000036700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -205,6 +212,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 2493
-  X: 67
-  Y: 27
+  Width: 1869
+  X: 632
+  Y: 381
diff --git a/scout_description/urdf/scout_v2.gazebo b/scout_description/urdf/scout_v2.gazebo
index 7890ceb..86ab624 100644
--- a/scout_description/urdf/scout_v2.gazebo
+++ b/scout_description/urdf/scout_v2.gazebo
@@ -10,7 +10,7 @@
         </plugin>
     </gazebo>
 
-    <gazebo>
+    <!-- <gazebo>
         <plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
             <updateRate>100.0</updateRate>
             <robotNamespace></robotNamespace>
@@ -22,16 +22,15 @@
             <wheelDiameter>0.32918</wheelDiameter>
             <robotBaseFrame>base_link</robotBaseFrame>
             <torque>1000</torque>
-            <!-- <topicName>cmd_vel</topicName> -->
             <commandTopic>cmd_vel</commandTopic>
-            <broadcastTF>1</broadcastTF>
+            <broadcastTF>true</broadcastTF>
             <odometryTopic>odom</odometryTopic>
             <odometryFrame>odom</odometryFrame>
             <covariance_x>0.000100</covariance_x>
             <covariance_y>0.000100</covariance_y>
             <covariance_yaw>0.010000</covariance_yaw>
         </plugin>
-    </gazebo>
+    </gazebo> -->
 
     <!-- <gazebo>
         <plugin name="object_controller" filename="libgazebo_ros_planar_move.so">
@@ -41,5 +40,5 @@
             <odometryRate>20.0</odometryRate>
             <robotBaseFrame>base_footprint</robotBaseFrame>
         </plugin>
-    </gazebo> -->
+    </gazebo> -->    
 </robot>
\ No newline at end of file
diff --git a/scout_description/urdf/scout_v2.xacro b/scout_description/urdf/scout_v2.xacro
index 9e17da3..216e8fd 100644
--- a/scout_description/urdf/scout_v2.xacro
+++ b/scout_description/urdf/scout_v2.xacro
@@ -89,7 +89,7 @@
     <!-- For testing, hang the robot up in the air -->
     <!-- <link name="world" />
     <joint name="world_to_base_link=" type="fixed">
-        <origin xyz="0 0 1.0" rpy="0 0 0" />
+        <origin xyz="0 0 0.5" rpy="0 0 0" />
         <parent link="world"/>
         <child link="base_link"/>
     </joint> -->
diff --git a/scout_description/urdf/scout_wheel_type1.xacro b/scout_description/urdf/scout_wheel_type1.xacro
index ddfc915..0a36557 100644
--- a/scout_description/urdf/scout_wheel_type1.xacro
+++ b/scout_description/urdf/scout_wheel_type1.xacro
@@ -55,16 +55,6 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
             <axis xyz="0 -1 0" rpy="0 0 0" />
         </joint>
 
-        <!-- <transmission name="${wheel_prefix}_wheel_trans" type="SimpleTransmission">
-			<type>transmission_interface/SimpleTransmission</type>
-			<actuator name="${wheel_prefix}_wheel_motor">
-				<mechanicalReduction>1</mechanicalReduction>
-			</actuator>
-			<joint name="${wheel_prefix}_wheel">
-				<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-			</joint>
-		</transmission> -->
-
         <!-- Gazebo definitions -->
         <gazebo reference="${wheel_prefix}_wheel_link">
             <mu1 value="1.0"/>
@@ -73,6 +63,15 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
             <kd value="1.0" />
             <fdir1 value="1 0 0"/>
         </gazebo>
-
+        
+        <transmission name="${wheel_prefix}_wheel_trans" type="SimpleTransmission">
+			<type>transmission_interface/SimpleTransmission</type>
+			<actuator name="${wheel_prefix}_wheel_motor">
+				<mechanicalReduction>1</mechanicalReduction>
+			</actuator>
+			<joint name="${wheel_prefix}_wheel">
+				<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
+			</joint>
+		</transmission>
     </xacro:macro>
 </robot>
diff --git a/scout_description/urdf/scout_wheel_type2.xacro b/scout_description/urdf/scout_wheel_type2.xacro
index 166ba0b..8331ca9 100644
--- a/scout_description/urdf/scout_wheel_type2.xacro
+++ b/scout_description/urdf/scout_wheel_type2.xacro
@@ -55,16 +55,6 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
             <axis xyz="0 -1 0" rpy="0 0 0" />
         </joint>
 
-        <!-- <transmission name="${wheel_prefix}_wheel_trans" type="SimpleTransmission">
-            <type>transmission_interface/SimpleTransmission</type>
-            <actuator name="${wheel_prefix}_wheel_motor">
-                <mechanicalReduction>1</mechanicalReduction>
-            </actuator>
-            <joint name="${wheel_prefix}_wheel">
-                <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-            </joint>
-        </transmission> -->
-
         <!-- Gazebo definitions -->
         <gazebo reference="${wheel_prefix}_wheel_link">
             <mu1 value="1.0"/>
@@ -73,6 +63,16 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
             <kd value="1.0" />
             <fdir1 value="1 0 0"/>
         </gazebo>
+
+        <transmission name="${wheel_prefix}_wheel_trans" type="SimpleTransmission">
+            <type>transmission_interface/SimpleTransmission</type>
+            <actuator name="${wheel_prefix}_wheel_motor">
+                <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+            <joint name="${wheel_prefix}_wheel">
+                <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
+            </joint>
+        </transmission>        
     </xacro:macro>
 
 </robot>