fixed state publish rate error (from 20hz to 50hz), gazebo scout v2 sim ready

.gitignore

@@ -0,0 +1,57 @@
+devel/
+logs/
+build/
+bin/
+lib/
+msg_gen/
+srv_gen/
+msg/*Action.msg
+msg/*ActionFeedback.msg
+msg/*ActionGoal.msg
+msg/*ActionResult.msg
+msg/*Feedback.msg
+msg/*Goal.msg
+msg/*Result.msg
+msg/_*.py
+build_isolated/
+devel_isolated/
+
+# Webots temporary files
+scout_webots_sim/worlds/.scout_base.wbproj
+
+# Generated by dynamic reconfigure
+*.cfgc
+/cfg/cpp/
+/cfg/*.py
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# Visual studio code stuff
+*.vscode
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+*.pcd
+*.pyc
+qtcreator-*
+*.user
+
+/planning/cfg
+/planning/docs
+/planning/src
+
+*~
+
+# Emacs
+.#*
+
+# Catkin custom files
+CATKIN_IGNORE

scout_base/CMakeLists.txt

@@ -51,6 +51,10 @@ set_property(TARGET scout_messenger PROPERTY POSITION_INDEPENDENT_CODE ON)
 add_executable(scout_base_node src/scout_base_node.cpp)
 target_link_libraries(scout_base_node scout_messenger ${catkin_LIBRARIES})
 
+add_executable(scout_base_sim_node src/scout_base_sim_node.cpp)
+target_link_libraries(scout_base_sim_node scout_messenger ${catkin_LIBRARIES})
+
+
 #############
 ## Install ##
 #############

scout_base/launch/scout_base_sim.launch

@@ -0,0 +1,22 @@
+<launch>
+    <!-- 
+        The robot can be controlled either through CAN bus or UART port. Make sure the hardware
+        interface is set up correctly before attempting to connect to the robot.
+        
+        You only need to specify the port name, such as "can0", "/dev/ttyUSB0". The port should 
+        operate with the following configuration:
+        
+        * CAN bus: 500k
+        * UART: 115200, Parity None, 8-bit Data, 1 Stop Bit 
+    -->
+    <arg name="simulated_robot" default="true" />
+    <arg name="control_rate" default="50" />
+
+    <node name="scout_base_sim_node" pkg="scout_base" type="scout_base_sim_node" output="screen">
+        <param name="simulated_robot" type="bool" value="$(arg simulated_robot)" />
+        <param name="control_rate" type="int" value="$(arg control_rate)" />
+
+        <param name="odom_frame" type="string" value="odom" />
+        <param name="base_frame" type="string" value="base_link" />
+    </node>
+</launch>

scout_base/src/scout_base_node.cpp

@@ -40,13 +40,13 @@ int main(int argc, char **argv)
     }
     messenger.SetupSubscription();
 
-    // publish robot state at 20Hz while listening to twist commands
+    // publish robot state at 50Hz while listening to twist commands
-    ros::Rate rate_20hz(20); // 20Hz
+    ros::Rate rate_50hz(50); // 50Hz
     while (true)
     {
         messenger.PublishStateToROS();
         ros::spinOnce();
-        rate_20hz.sleep();
+        rate_50hz.sleep();
     }
 
     return 0;

scout_base/src/scout_base_sim_node.cpp

@@ -0,0 +1,47 @@
+#include <string>
+
+#include <ros/ros.h>
+#include <sensor_msgs/JointState.h>
+#include <tf/transform_broadcaster.h>
+#include <nav_msgs/Odometry.h>
+
+#include "scout_sdk/scout_base.hpp"
+#include "scout_base/scout_messenger.hpp"
+
+using namespace wescore;
+
+int main(int argc, char **argv)
+{
+    // setup ROS node
+    ros::init(argc, argv, "scout_odom");
+    ros::NodeHandle node(""), private_node("~");
+
+    // instantiate a robot object
+    ScoutBase robot;
+    ScoutROSMessenger messenger(&robot, &node);
+
+    // fetch parameters before connecting to robot
+    std::string port_name;
+    private_node.param<std::string>("port_name", port_name, std::string("can0"));
+    private_node.param<std::string>("odom_frame", messenger.odom_frame_, std::string("odom"));
+    private_node.param<std::string>("base_frame", messenger.base_frame_, std::string("base_link"));
+    private_node.param<bool>("simulated_robot", messenger.simulated_robot_, true);
+    private_node.param<int>("control_rate", messenger.sim_control_rate_, 50);
+
+    // no connection for simulated robot
+    // setup ROS subscription
+    messenger.SetupSubscription();
+
+    // publish robot state at 50Hz while listening to twist commands
+    double linear, angular;
+    ros::Rate rate_50hz(50); // 50Hz
+    while (true)
+    {
+        messenger.GetCurrentMotionCmdForSim(linear, angular);
+        messenger.PublishSimStateToROS(linear, angular);
+        ros::spinOnce();
+        rate_50hz.sleep();
+    }
+
+    return 0;
+}

scout_bringup/launch/scout_base_gazebo_sim.launch

@@ -0,0 +1,4 @@
+<launch>
+    <include file="$(find scout_gazebo_sim)/launch/scout_empty_world.launch" />
+    <include file="$(find scout_base)/launch/scout_base_sim.launch" />
+</launch>

scout_description/rviz/model_display.rviz

@@ -63,11 +63,11 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
-        base_footprint:
+        base_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        base_link:
+        chassis_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
@@ -96,10 +96,6 @@ 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: ""
@@ -111,10 +107,10 @@ Visualization Manager:
       Frame Timeout: 15
       Frames:
         All Enabled: true
-        base_footprint:
-          Value: true
         base_link:
           Value: true
+        chassis_link:
+          Value: true
         front_left_wheel_link:
           Value: true
         front_right_wheel_link:
@@ -125,35 +121,31 @@ 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:
-        world:
         base_link:
-            base_footprint:
+          chassis_link:
-              {}
             front_left_wheel_link:
               {}
             front_right_wheel_link:
               {}
-            inertial_link:
-              {}
             rear_left_wheel_link:
               {}
             rear_right_wheel_link:
               {}
+          inertial_link:
+            {}
       Update Interval: 0
       Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
     Default Light: true
-    Fixed Frame: world
+    Fixed Frame: base_link
     Frame Rate: 30
   Name: root
   Tools:
@@ -214,4 +206,4 @@ Window Geometry:
     collapsed: true
   Width: 1869
   X: 632
-  Y: 381
+  Y: 291

scout_description/rviz/navigation.rviz

@@ -0,0 +1,209 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+      Splitter Ratio: 0.5
+    Tree Height: 732
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        chassis_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_left_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        inertial_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        rear_left_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base_link:
+          Value: true
+        chassis_link:
+          Value: true
+        front_left_wheel_link:
+          Value: true
+        front_right_wheel_link:
+          Value: true
+        inertial_link:
+          Value: true
+        rear_left_wheel_link:
+          Value: true
+        rear_right_wheel_link:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        base_link:
+          chassis_link:
+            front_left_wheel_link:
+              {}
+            front_right_wheel_link:
+              {}
+            rear_left_wheel_link:
+              {}
+            rear_right_wheel_link:
+              {}
+          inertial_link:
+            {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: odom
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 2.9432930946350098
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.027046792209148407
+        Y: -0.03490818291902542
+        Z: -0.09952529519796371
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5447957515716553
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.27542781829834
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1029
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000367fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000367000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000074d0000003efc0100000002fb0000000800540069006d006501000000000000074d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005f10000036700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1869
+  X: 632
+  Y: 291

scout_description/urdf/scout_v2.gazebo

@@ -1,10 +1,12 @@
 <?xml version="1.0"?>
 
+<!-- 
 Reference:
     [1] https://answers.ros.org/question/246914/four-wheeled-skid-steering-in-gazebo-and-ros-using-gazebo-ros-control/
     [2] https://answers.ros.org/question/10119/gazebo-controller-for-skid-steering/
     [3] https://answers.ros.org/question/9640/rotation-error-in-gazebo-simulation/
     [4] https://www.youtube.com/watch?v=fuRAv6PDwdw 
+-->
 
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
     <xacro:include filename="$(find scout_description)/urdf/scout_wheel.gazebo" />

scout_description/urdf/scout_v2.urdf

@@ -1,277 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from scout_v2.xacro                 | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
-<robot name="scout_v2">
-  <!-- Virtual link as root -->
-  <link name="vehicle_reference"/>
-  <joint name="veh_ref_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <parent link="vehicle_reference"/>
-    <child link="base_link"/>
-  </joint>
-  <!-- Base link -->
-  <link name="base_link">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://scout_description/meshes/base_link.dae"/>
-      </geometry>
-    </visual>
-    <inertial>
-      <mass value="40"/>
-      <!-- <mass value="50" /> -->
-      <!-- <origin xyz="-0.00065 -0.085 0.062" />
-            <inertia ixx="0.6022" ixy="-0.02364" ixz="-0.1197" iyy="1.7386" iyz="-0.001544" izz="2.0296" /> -->
-      <origin xyz="0.0 0.0 0.0"/>
-      <inertia ixx="2.288641" ixy="0" ixz="0" iyy="5.103976" iyz="0" izz="3.431465"/>
-    </inertial>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.008"/>
-      <geometry>
-        <box size="0.925 0.38 0.21"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.035"/>
-      <geometry>
-        <box size="0.154166666667 0.627 0.07"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Base footprint: base_link projected to the gound plane -->
-  <link name="base_footprint"/>
-  <joint name="base_footprint_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 -0.23479"/>
-    <parent link="base_link"/>
-    <child link="base_footprint"/>
-  </joint>
-  <link name="front_right_wheel_link">
-    <inertial>
-      <mass value="3"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="0.7171" ixy="0" ixz="0" iyy="0.1361" iyz="0" izz="0.7171"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://scout_description/meshes/wheel_type1.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="1.570795 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.11653" radius="0.16459"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="front_right_wheel" type="continuous">
-    <parent link="base_link"/>
-    <child link="front_right_wheel_link"/>
-    <origin rpy="3.14 0 0" xyz="0.249 -0.29153 -0.0702"/>
-    <axis rpy="0 0 0" xyz="0 -1 0"/>
-  </joint>
-  <link name="front_left_wheel_link">
-    <inertial>
-      <mass value="3"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="0.7171" ixy="0" ixz="0" iyy="0.1361" iyz="0" izz="0.7171"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://scout_description/meshes/wheel_type2.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="1.570795 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.11653" radius="0.16459"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="front_left_wheel" type="continuous">
-    <parent link="base_link"/>
-    <child link="front_left_wheel_link"/>
-    <origin rpy="0 0 0" xyz="0.249 0.29153 -0.0702"/>
-    <axis rpy="0 0 0" xyz="0 -1 0"/>
-  </joint>
-  <link name="rear_left_wheel_link">
-    <inertial>
-      <mass value="3"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="0.7171" ixy="0" ixz="0" iyy="0.1361" iyz="0" izz="0.7171"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://scout_description/meshes/wheel_type1.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="1.570795 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.11653" radius="0.16459"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="rear_left_wheel" type="continuous">
-    <parent link="base_link"/>
-    <child link="rear_left_wheel_link"/>
-    <origin rpy="0 0 0" xyz="-0.249 0.29153 -0.0702"/>
-    <axis rpy="0 0 0" xyz="0 -1 0"/>
-  </joint>
-  <link name="rear_right_wheel_link">
-    <inertial>
-      <mass value="3"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="0.7171" ixy="0" ixz="0" iyy="0.1361" iyz="0" izz="0.7171"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://scout_description/meshes/wheel_type2.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="1.570795 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.11653" radius="0.16459"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="rear_right_wheel" type="continuous">
-    <parent link="base_link"/>
-    <child link="rear_right_wheel_link"/>
-    <origin rpy="3.14 0 0" xyz="-0.249 -0.29153 -0.0702"/>
-    <axis rpy="0 0 0" xyz="0 -1 0"/>
-  </joint>
-  <!-- For testing, hang the robot up in the air -->
-  <!-- <link name="world" />
-    <joint name="world_to_base_link=" type="fixed">
-        <origin xyz="0 0 0.5" rpy="0 0 0" />
-        <parent link="world"/>
-        <child link="base_link"/>
-    </joint> -->
-  <!-- This file is a placeholder which is included by default from
-       husky.urdf.xacro. If a robot is being customized and requires
-       additional URDF, set the HUSKY_URDF_EXTRAS environment variable
-       to the full path of the file you would like included. -->
-  <!-- Additional definitions for simulation -->
-  <!-- <gazebo reference="base_link">
-        <material>Gazebo/Grey</material>
-    </gazebo> -->
-  <!-- kp spring constant, kd damping constant -->
-  <gazebo reference="front_right_wheel_link">
-    <mu1 value="1.0"/>
-    <mu2 value="1.0"/>
-    <kp value="10000000.0"/>
-    <kd value="1.0"/>
-    <fdir1 value="1 0 0"/>
-    <minDepth value="0.001"/>
-    <maxContacts value="64"/>
-  </gazebo>
-  <transmission name="front_right_wheel_trans" type="SimpleTransmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <actuator name="front_right_wheel_motor">
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-    <joint name="front_right_wheel">
-      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    </joint>
-  </transmission>
-  <!-- kp spring constant, kd damping constant -->
-  <gazebo reference="front_left_wheel_link">
-    <mu1 value="1.0"/>
-    <mu2 value="1.0"/>
-    <kp value="10000000.0"/>
-    <kd value="1.0"/>
-    <fdir1 value="1 0 0"/>
-    <minDepth value="0.001"/>
-    <maxContacts value="64"/>
-  </gazebo>
-  <transmission name="front_left_wheel_trans" type="SimpleTransmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <actuator name="front_left_wheel_motor">
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-    <joint name="front_left_wheel">
-      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    </joint>
-  </transmission>
-  <!-- kp spring constant, kd damping constant -->
-  <gazebo reference="rear_left_wheel_link">
-    <mu1 value="1.0"/>
-    <mu2 value="1.0"/>
-    <kp value="10000000.0"/>
-    <kd value="1.0"/>
-    <fdir1 value="1 0 0"/>
-    <minDepth value="0.001"/>
-    <maxContacts value="64"/>
-  </gazebo>
-  <transmission name="rear_left_wheel_trans" type="SimpleTransmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <actuator name="rear_left_wheel_motor">
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-    <joint name="rear_left_wheel">
-      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    </joint>
-  </transmission>
-  <!-- kp spring constant, kd damping constant -->
-  <gazebo reference="rear_right_wheel_link">
-    <mu1 value="1.0"/>
-    <mu2 value="1.0"/>
-    <kp value="10000000.0"/>
-    <kd value="1.0"/>
-    <fdir1 value="1 0 0"/>
-    <minDepth value="0.001"/>
-    <maxContacts value="64"/>
-  </gazebo>
-  <transmission name="rear_right_wheel_trans" type="SimpleTransmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <actuator name="rear_right_wheel_motor">
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-    <joint name="rear_right_wheel">
-      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    </joint>
-  </transmission>
-  <!-- Controller configurations -->
-  <gazebo>
-    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
-      <robotNamespace>/</robotNamespace>
-    </plugin>
-  </gazebo>
-  <!-- <gazebo>
-        <plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
-            <updateRate>100.0</updateRate>
-            <robotNamespace></robotNamespace>
-            <leftFrontJoint>front_left_wheel</leftFrontJoint>
-            <rightFrontJoint>front_right_wheel</rightFrontJoint>
-            <leftRearJoint>rear_left_wheel</leftRearJoint>
-            <rightRearJoint>rear_right_wheel</rightRearJoint>
-            <wheelSeparation>4</wheelSeparation>
-            <wheelDiameter>0.32918</wheelDiameter>
-            <robotBaseFrame>base_link</robotBaseFrame>
-            <torque>1000</torque>
-            <commandTopic>cmd_vel</commandTopic>
-            <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>
-        <plugin name="object_controller" filename="libgazebo_ros_planar_move.so">
-            <commandTopic>cmd_vel</commandTopic>
-            <odometryTopic>odom</odometryTopic>
-            <odometryFrame>odom</odometryFrame>
-            <odometryRate>20.0</odometryRate>
-            <robotBaseFrame>base_footprint</robotBaseFrame>
-        </plugin>
-    </gazebo> -->
-</robot>

scout_description/urdf/scout_v2.xacro

@@ -24,22 +24,6 @@
     <xacro:property name="wheel_length" value="1.1653e-01" />
     <xacro:property name="wheel_radius" value="1.6459e-01" />
 
-    <!-- Virtual link as root -->
-    <link name="vehicle_reference" />
-    <joint name="veh_ref_joint" type="fixed">
-        <origin xyz="0 0 0" rpy="0 0 0" />
-        <parent link="vehicle_reference" />
-        <child link="base_link" />
-    </joint>
-
-    <!-- For testing, hang the robot up in the air -->
-    <!-- <link name="world" />
-    <joint name="world_to_base_link=" type="fixed">
-        <origin xyz="0 0 0.5" rpy="0 0 0" />
-        <parent link="world"/>
-        <child link="base_link"/>
-    </joint> -->
-
     <!-- Base link -->
     <link name="base_link">
         <visual>
@@ -48,14 +32,6 @@
                 <mesh filename="package://scout_description/meshes/base_link.dae" />
             </geometry>
         </visual>
-        <inertial>
-            <mass value="40" />
-            <!-- <mass value="50" /> -->
-            <!-- <origin xyz="-0.00065 -0.085 0.062" />
-            <inertia ixx="0.6022" ixy="-0.02364" ixz="-0.1197" iyy="1.7386" iyz="-0.001544" izz="2.0296" /> -->
-            <origin xyz="0.0 0.0 0.0" />
-            <inertia ixx="2.288641" ixy="0" ixz="0" iyy="5.103976" iyz="0" izz="3.431465" />
-        </inertial>
         <collision>
             <origin xyz="0 0 0.008" rpy="0 0 0" />
             <geometry>
@@ -70,15 +46,37 @@
         </collision>
     </link>
 
-    <!-- Base footprint: base_link projected to the gound plane -->
+    <!-- <joint name="chassis_link_joint" type="fixed">
-    <link name="base_footprint"/>
+        <origin xyz="0 0 ${wheel_radius - wheel_vertical_offset}" rpy="0 0 0" />
-
-    <joint name="base_footprint_joint" type="fixed">
-        <origin xyz="0 0 ${wheel_vertical_offset - wheel_radius}" rpy="0 0 0" />
         <parent link="base_link" />
-        <child link="base_footprint" />
+        <child link="chassis_link" />
+    </joint> -->
+
+    <link name="inertial_link">
+        <inertial>
+            <mass value="40" />
+            <!-- <mass value="50" /> -->
+            <!-- <origin xyz="-0.00065 -0.085 0.062" />
+            <inertia ixx="0.6022" ixy="-0.02364" ixz="-0.1197" iyy="1.7386" iyz="-0.001544" izz="2.0296" /> -->
+            <origin xyz="0.0 0.0 0.0" />
+            <inertia ixx="2.288641" ixy="0" ixz="0" iyy="5.103976" iyz="0" izz="3.431465" />
+        </inertial>
+    </link>
+
+    <joint name="inertial_joint" type="fixed">
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <parent link="base_link" />
+        <child link="inertial_link" />
     </joint>
 
+    <!-- For testing, hang the robot up in the air -->
+    <!-- <link name="world" />
+    <joint name="world_to_base_link=" type="fixed">
+        <origin xyz="0 0 0.5" rpy="0 0 0" />
+        <parent link="world"/>
+        <child link="base_link"/>
+    </joint> -->
+
     <!-- Scout wheel macros -->
     <!-- wheel labeled from 0 to 3, conter-clockwise, starting from front right wheel -->
     <!-- motor 1 and 2 (left side) are mechanically installed in a reversed direction -->

scout_gazebo_sim/launch/scout_empty_world.launch

@@ -14,5 +14,5 @@
 
     <include file="$(find scout_gazebo_sim)/launch/spawn_scout_v2.launch"></include>
         
-    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find scout_description)/rviz/model_display.rviz" />
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find scout_description)/rviz/navigation.rviz" />
 </launch>

scout_gazebo_sim/launch/spawn_scout_v2.launch

@@ -9,7 +9,7 @@
 
     <!-- these are the arguments you can pass this launch file, for example paused:=true -->
     <arg name="paused" default="true" />
-    <arg name="use_sim_time" default="false" />
+    <arg name="use_sim_time" default="true" />
     <arg name="gui" default="true" />
     <arg name="headless" default="false" />
     <arg name="debug" default="true" />

scout_gazebo_sim/src/scout_skid_steer_controller.cpp

@@ -26,39 +26,5 @@ int main(int argc, char **argv) {
 
   ros::spin();
 
-  // // instantiate a robot object
-  // ScoutBase robot;
-  // ScoutROSMessenger messenger(&robot, &node);
-
-  // // fetch parameters before connecting to robot
-  // std::string port_name;
-  // private_node.param<std::string>("port_name", port_name,
-  // std::string("can0")); private_node.param<std::string>("odom_frame",
-  // messenger.odom_frame_, std::string("odom"));
-  // private_node.param<std::string>("base_frame", messenger.base_frame_,
-  // std::string("base_link")); private_node.param<bool>("simulated_robot",
-  // messenger.simulated_robot_, false);
-
-  // // connect to robot and setup ROS subscription
-  // if (port_name.find("can") != std::string::npos)
-  // {
-  //     robot.Connect(port_name);
-  //     ROS_INFO("Using CAN bus to talk with the robot");
-  // }
-  // else
-  // {
-  //     robot.Connect(port_name, 115200);
-  //     ROS_INFO("Using UART to talk with the robot");
-  // }
-  // messenger.SetupSubscription();
-
-  // publish robot state at 20Hz while listening to twist commands
-  //   ros::Rate rate_20hz(20);  // 20Hz
-  //   while (true) {
-  //     messenger.PublishStateToROS();
-  //     ros::spinOnce();
-  //     rate_20hz.sleep();
-  //   }
-
   return 0;
 }