merged with scout_sim

2026-01-12 03:15:01 +08:00 · 2020-08-21 17:10:00 +08:00
parent b6c781b8b2 dc3654ed8e
commit 1e321b74ef
70 changed files with 9021 additions and 39 deletions
--- a/.gitignore
+++ b/.gitignore
@@ -1 +1,61 @@
+<<<<<<< HEAD
 .vscode
+=======
+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_sim
--- a/NOTE.md
+++ b/NOTE.md
@@ -0,0 +1,26 @@
+```
+$ rostopic pub -1 /scout_motor_fl_controller/command std_msgs/Float64 "data: 0.5"
+```
+
+Convert xacro to urdf
+
+```
+$ rosrun xacro xacro -o model.urdf model.urdf.xacro
+```
+
+Generate proto for Webots
+
+```
+$ rosrun xacro xacro -o scout_description/urdf/scout_v2.urdf scout_description/urdf/scout_v2_webots.xacro
+$ python -m urdf2webots.importer --input=./scout_description/urdf/scout_v2.urdf --output=scout_description/proto/ScoutV2.proto
+```
+
+Convert urdf to sdf
+
+```
+$ gz sdf -p scout_v2.urdf > scout_v2.sdf
+```
+
+Reference:
+
+[1] https://github.com/cyberbotics/urdf2webots
--- a/README.md
+++ b/README.md
@@ -22,8 +22,14 @@ Please refer to the [README](https://github.com/westonrobot/wrp_sdk#hardware-int
 1. Install dependent libraries

    ```
+<<<<<<< HEAD
    $ sudo apt install libasio-dev
    $ sudo apt install ros-$ROS_DISTRO-teleop-twist-keyboard
+=======
+    $ 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
+>>>>>>> scout_sim
    ```

 2. Clone the packages into your catkin workspace and compile
@@ -40,7 +46,7 @@ Please refer to the [README](https://github.com/westonrobot/wrp_sdk#hardware-int

 4. Launch ROS nodes
 
-* Start the base node 
+* Start the base node for the real robot

    ```
    $ roslaunch scout_bringup scout_minimal.launch
@@ -52,6 +58,21 @@ Please refer to the [README](https://github.com/westonrobot/wrp_sdk#hardware-int
    $ roslaunch scout_bringup scout_minimal_uart.launch
    ```

+<<<<<<< HEAD
+=======
+* Start the Webots-based simulation (Scout V1)
+
+    ```
+    $ roslaunch scout_bringup scout_base_webots_sim.launch
+    ```
+
+* Start the Gazebo-based simulation (Scout V2)
+
+    ```
+    $ roslaunch scout_bringup scout_base_gazebo_sim.launch
+    ```
+
+>>>>>>> scout_sim
 * Start the keyboard tele-op node

    ```
--- a/scout_base/CMakeLists.txt
+++ b/scout_base/CMakeLists.txt
@@ -26,7 +26,7 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_package(
  INCLUDE_DIRS include
  LIBRARIES scout_messenger
-  CATKIN_DEPENDS scout_msgs roscpp sensor_msgs
+  CATKIN_DEPENDS scout_msgs roscpp sensor_msgs wrp_sdk
 #   DEPENDS Boost
 )

@@ -52,7 +52,7 @@ target_link_libraries(scout_base_node scout_messenger ${catkin_LIBRARIES})
 ## Install ##
 #############

-roslaunch_add_file_check(launch)
+# roslaunch_add_file_check(launch)

 install(TARGETS scout_messenger scout_base_node
  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
--- a/scout_base/launch/scout_base_sim.launch
+++ b/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_node" pkg="scout_base" type="scout_base_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>
--- a/scout_base/src/scout_base_node.cpp
+++ b/scout_base/src/scout_base_node.cpp
@@ -1,17 +1,14 @@
-#include <string>
-
+#include <nav_msgs/Odometry.h>
 #include <ros/ros.h>
 #include <sensor_msgs/JointState.h>
 #include <tf/transform_broadcaster.h>
-#include <nav_msgs/Odometry.h>

 #include "wrp_sdk/platforms/scout/scout_base.hpp"
 #include "scout_base/scout_messenger.hpp"

 using namespace westonrobot;

-int main(int argc, char **argv)
-{
+int main(int argc, char **argv) {
  // setup ROS node
  ros::init(argc, argv, "scout_odom");
  ros::NodeHandle node(""), private_node("~");
@@ -23,30 +20,38 @@ int main(int argc, char **argv)
  // 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);
+  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);
+  private_node.param<int>("control_rate", messenger.sim_control_rate_, 50);

+  if (!messenger.simulated_robot_) {
    // connect to robot and setup ROS subscription
-    if (port_name.find("can") != std::string::npos)
-    {
+    if (port_name.find("can") != std::string::npos) {
      robot.Connect(port_name);
      ROS_INFO("Using CAN bus to talk with the robot");
-    }
-    else
-    {
+    } 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)
-    {
+  // publish robot state at 50Hz while listening to twist commands
+  ros::Rate rate(50);
+  while (true) {
+    if (!messenger.simulated_robot_) {
      messenger.PublishStateToROS();
+    } else {
+      double linear, angular;
+      messenger.GetCurrentMotionCmdForSim(linear, angular);
+      messenger.PublishSimStateToROS(linear, angular);
+    }
    ros::spinOnce();
-        rate_20hz.sleep();
+    rate.sleep();
  }

  return 0;
--- a/scout_base/src/scout_base_sim_node.cpp
+++ b/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 "wrp_sdk/platforms/scout/scout_base.hpp"
+#include "scout_base/scout_messenger.hpp"
+
+using namespace westonrobot;
+
+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;
+}
--- a/scout_bringup/launch/scout_gazebo_sim_empty.launch
+++ b/scout_bringup/launch/scout_gazebo_sim_empty.launch
@@ -0,0 +1,4 @@
+<launch>
+    <!-- Scout base only (with no onboard sensors) in an empty world -->
+    <include file="$(find scout_gazebo_sim)/launch/scout_empty_world.launch" />
+</launch>
--- a/scout_bringup/launch/scout_gazebo_sim_playpen.launch
+++ b/scout_bringup/launch/scout_gazebo_sim_playpen.launch
@@ -0,0 +1,4 @@
+<launch>
+    <!-- Scout base with a hokuyo laser in an testing world -->
+    <include file="$(find scout_gazebo_sim)/launch/scout_playpen.launch" />
+</launch>
--- a/scout_bringup/launch/scout_webots_sim.launch
+++ b/scout_bringup/launch/scout_webots_sim.launch
--- a/scout_description/CMakeLists.txt
+++ b/scout_description/CMakeLists.txt
@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(scout_description)
+
+find_package(catkin REQUIRED COMPONENTS roslaunch)
+
+catkin_package()
+
+# roslaunch_add_file_check(launch)
+
+install(
+  DIRECTORY launch meshes urdf
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
--- a/scout_description/launch/display_model.launch
+++ b/scout_description/launch/display_model.launch
@@ -0,0 +1,10 @@
+<launch>
+        <arg name="model" />
+
+        <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" />
+
+        <node name="rviz" pkg="rviz" type="rviz" args="-d $(find scout_description)/rviz/model_display.rviz" />
+</launch>
--- a/scout_description/launch/scout_v2_sensors.launch
+++ b/scout_description/launch/scout_v2_sensors.launch
@@ -0,0 +1,10 @@
+
+<launch>
+    <arg name="robot_namespace" default="/"/>
+    <arg name="urdf_extras" default="$(find scout_description)/urdf/empty.urdf"/>
+
+    <param name="robot_description" command="$(find xacro)/xacro '$(find scout_description)/urdf/scout_v2.xacro'
+        robot_namespace:=$(arg robot_namespace) 
+        onboard_sensors:='true'
+        urdf_extras:=$(arg urdf_extras)" />
+</launch>
--- a/scout_description/launch/scout_v2_stock.launch
+++ b/scout_description/launch/scout_v2_stock.launch
@@ -0,0 +1,10 @@
+
+<launch>
+    <arg name="robot_namespace" default="/"/>
+    <arg name="urdf_extras" default="$(find scout_description)/urdf/empty.urdf"/>
+
+    <param name="robot_description" command="$(find xacro)/xacro '$(find scout_description)/urdf/scout_v2.xacro'
+        robot_namespace:=$(arg robot_namespace) 
+        onboard_sensors:='false'
+        urdf_extras:=$(arg urdf_extras)" />
+</launch>
--- a/scout_description/meshes/base_link.dae
+++ b/scout_description/meshes/base_link.dae
--- a/scout_description/meshes/base_link_full.dae
+++ b/scout_description/meshes/base_link_full.dae
@@ -0,0 +1,15 @@
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
+  <asset>
+    <contributor />
+    <created>2020-03-27T18:56:12.965296</created>
+    <modified>2020-03-27T18:56:12.965305</modified>
+    <unit name="meter" meter="1.0" />
+    <up_axis>Z_UP</up_axis>
+  </asset>
+  <library_visual_scenes>
+    <visual_scene id="myscene" />
+  </library_visual_scenes>
+  <scene>
+    <instance_visual_scene url="#myscene" />
+  </scene>
+</COLLADA>
--- a/scout_description/meshes/hokuyo.dae
+++ b/scout_description/meshes/hokuyo.dae
--- a/scout_description/meshes/wheel_type1.dae
+++ b/scout_description/meshes/wheel_type1.dae
--- a/scout_description/meshes/wheel_type2.dae
+++ b/scout_description/meshes/wheel_type2.dae
--- a/scout_description/package.xml
+++ b/scout_description/package.xml
@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<package>
+  <name>scout_description</name>
+  <version>0.4.2</version>
+  <description>AgileX Scout Husky URDF description</description>
+
+  <author email="rgariepy@clearpathrobotics.com">Ryan Gariepy</author>
+  <author email="pmukherj@clearpathrobotics.com">Prasenjit Mukherjee</author>
+  <author email="mpurvis@clearpathrobotics.com">Mike Purvis</author>
+  <author email="paul@bovbel.com">Paul Bovbel</author>
+  <author email="dash@clearpathrobotics.com">Devon Ash</author>
+
+  <maintainer email="paul@bovbel.com">Paul Bovbel</maintainer>
+  <maintainer email="tbaltovski@clearpathrobotics.com">Tony Baltovski</maintainer>
+
+  <url type="website">http://ros.org/wiki/scout_description</url>
+  <url type="bugtracker">https://github.com/husky/husky/issues</url>
+  <url type="repository">https://github.com/husky/husky</url>
+
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roslaunch</build_depend>
+  <run_depend>urdf</run_depend>
+  <run_depend>xacro</run_depend>
+  <run_depend>lms1xx</run_depend>
+
+  <export>
+  </export>
+</package>
--- a/scout_description/proto/ScoutV2.proto
+++ b/scout_description/proto/ScoutV2.proto
--- a/scout_description/rviz/model_display.rviz
+++ b/scout_description/rviz/model_display.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: base_link
+    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
--- a/scout_description/rviz/navigation.rviz
+++ b/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
--- a/scout_description/urdf/empty.urdf
+++ b/scout_description/urdf/empty.urdf
@@ -0,0 +1,6 @@
+<robot>
+  <!-- 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. -->
+</robot>
--- a/scout_description/urdf/onboard_sensors.urdf
+++ b/scout_description/urdf/onboard_sensors.urdf
@@ -0,0 +1,31 @@
+<robot>
+    <joint name="hokuyo_joint" type="fixed">
+        <origin xyz="0.26 0 0.3" rpy="0 0 0"/>
+        <parent link="base_link"/>
+        <axis xyz="0 1 0" />
+        <child link="hokuyo_link"/>
+    </joint>
+
+    <!-- Hokuyo Laser -->
+    <link name="hokuyo_link">
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+                <box size="0.1 0.1 0.1"/>
+            </geometry>
+        </collision>
+
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+                <mesh filename="package://scout_description/meshes/hokuyo.dae"/>
+            </geometry>
+        </visual>
+
+        <inertial>
+            <mass value="1e-5" />
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
+        </inertial>
+    </link>
+</robot>
--- a/scout_description/urdf/scout_v2.gazebo
+++ b/scout_description/urdf/scout_v2.gazebo
@@ -0,0 +1,111 @@
+<?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" />
+
+    <!-- Additional definitions for simulation -->
+    <!-- <gazebo reference="base_link">
+        <material>Gazebo/Grey</material>
+    </gazebo> -->
+
+    <!-- Actuator configurations -->
+    <xacro:scout_wheel_transmission wheel_prefix="front_right" />
+    <xacro:scout_wheel_transmission wheel_prefix="front_left" />
+    <xacro:scout_wheel_transmission wheel_prefix="rear_left" />
+    <xacro:scout_wheel_transmission wheel_prefix="rear_right" />
+
+    <!-- Controller configurations -->
+    <gazebo>
+        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+            <robotNamespace>$(arg robot_namespace)</robotNamespace>
+        </plugin>
+    </gazebo>
+
+    <!-- kp spring constant, kd damping constant -->
+    <gazebo>
+        <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>
+
+    <!-- hokuyo -->
+    <gazebo reference="hokuyo_link">
+        <sensor type="ray" name="head_hokuyo_sensor">
+            <pose>0 0 0 0 0 0</pose>
+            <visualize>false</visualize>
+            <update_rate>40</update_rate>
+            <ray>
+                <scan>
+                    <horizontal>
+                        <samples>720</samples>
+                        <resolution>1</resolution>
+                        <min_angle>-1.570796</min_angle>
+                        <max_angle>1.570796</max_angle>
+                    </horizontal>
+                </scan>
+                <range>
+                    <min>0.10</min>
+                    <max>30.0</max>
+                    <resolution>0.01</resolution>
+                </range>
+                <noise>
+                    <type>gaussian</type>
+                    <!-- Noise parameters based on published spec for Hokuyo laser
+               achieving "+-30mm" accuracy at range < 10m.  A mean of 0.0m and
+               stddev of 0.01m will put 99.7% of samples within 0.03m of the true
+               reading. -->
+                    <mean>0.0</mean>
+                    <stddev>0.01</stddev>
+                </noise>
+            </ray>
+            <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
+                <topicName>/scan</topicName>
+                <frameName>hokuyo_link</frameName>
+            </plugin>
+        </sensor>
+    </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>
--- a/scout_description/urdf/scout_v2.proto
+++ b/scout_description/urdf/scout_v2.proto
@@ -0,0 +1,6 @@
+#VRML_SIM R2019a utf8
+# license: Apache License 2.0
+# license url: http://www.apache.org/licenses/LICENSE-2.0
+# This is a proto file for Webots for the ScoutV2
+# Extracted from: ./scout_v2.urdf
+
--- a/scout_description/urdf/scout_v2.urdf
+++ b/scout_description/urdf/scout_v2.urdf
@@ -0,0 +1,137 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from scout_description/urdf/scout_v2_webots.xacro | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="scout_v2">
+  <!-- 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>
+    <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>
+    <inertial>
+      <mass value="40"/>
+      <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>
+  <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>
+  <!-- Additional definitions -->
+  <!-- <xacro:include filename="$(arg urdf_extras)" /> -->
+  <!-- Gazebo definitions  -->
+  <!-- <xacro:include filename="$(find scout_description)/urdf/scout_v2.gazebo" /> -->
+</robot>
--- a/scout_description/urdf/scout_v2.xacro
+++ b/scout_description/urdf/scout_v2.xacro
@@ -0,0 +1,115 @@
+<?xml version="1.0"?>
+
+<robot name="scout_v2" 
+    xmlns:xacro="http://ros.org/wiki/xacro">
+
+    <xacro:arg name="robot_namespace" default="/" />
+    <xacro:arg name="urdf_extras" default="empty.urdf" />
+    <xacro:arg name="onboard_sensors" default="true" />
+
+    <xacro:include filename="$(find scout_description)/urdf/scout_wheel_type1.xacro" />
+    <xacro:include filename="$(find scout_description)/urdf/scout_wheel_type2.xacro" />
+
+    <!-- Variables -->
+    <xacro:property name="M_PI" value="3.14159"/>
+
+    <!-- Vehicle Geometries -->
+    <xacro:property name="base_x_size" value="0.9250000" />
+    <xacro:property name="base_y_size" value="0.380000" />
+    <xacro:property name="base_z_size" value="0.210000" />
+
+    <xacro:property name="wheelbase" value="0.498" />
+    <xacro:property name="track" value="0.58306" />
+    <xacro:property name="wheel_vertical_offset" value="-0.0702" />
+
+    <xacro:property name="wheel_length" value="1.1653e-01" />
+    <xacro:property name="wheel_radius" value="1.6459e-01" />
+
+    <!-- Base link -->
+    <link name="base_link">
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://scout_description/meshes/base_link.dae" />
+            </geometry>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0.008" rpy="0 0 0" />
+            <geometry>
+                <box size="${base_x_size} ${base_y_size} ${base_z_size}"/>
+            </geometry>
+        </collision>
+        <collision>
+            <origin xyz="0 0 ${base_z_size/6}" rpy="0 0 0" />
+            <geometry>
+                <box size="${base_x_size/6} ${base_y_size*1.65} ${base_z_size/3}"/>
+            </geometry>
+        </collision>
+    </link>
+
+    <!-- <joint name="chassis_link_joint" type="fixed">
+        <origin xyz="0 0 ${wheel_radius - wheel_vertical_offset}" rpy="0 0 0" />
+        <parent link="base_link" />
+        <child link="chassis_link" />
+    </joint> -->
+
+    <link name="base_footprint"/>
+
+    <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" />
+    </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 -->
+    <xacro:scout_wheel_type1 wheel_prefix="front_right">
+        <origin xyz="${wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="3.14 0 0" />
+    </xacro:scout_wheel_type1>
+    <xacro:scout_wheel_type2 wheel_prefix="front_left">
+        <origin xyz="${wheelbase/2} ${track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
+    </xacro:scout_wheel_type2>
+    <xacro:scout_wheel_type1 wheel_prefix="rear_left">
+        <origin xyz="${-wheelbase/2} ${track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
+    </xacro:scout_wheel_type1>
+    <xacro:scout_wheel_type2 wheel_prefix="rear_right">
+        <origin xyz="${-wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="3.14 0 0" />
+    </xacro:scout_wheel_type2>
+
+    <!-- Onboard sensors -->
+    <xacro:if value="$(arg onboard_sensors)">
+        <xacro:include filename="$(find scout_description)/urdf/onboard_sensors.urdf" />
+    </xacro:if>
+
+    <!-- Additional definitions -->
+    <xacro:include filename="$(arg urdf_extras)" />
+
+    <!-- Gazebo definitions  -->
+    <xacro:include filename="$(find scout_description)/urdf/scout_v2.gazebo" />
+</robot>
--- a/scout_description/urdf/scout_v2_webots.xacro
+++ b/scout_description/urdf/scout_v2_webots.xacro
@@ -0,0 +1,83 @@
+<?xml version="1.0"?>
+
+<robot name="scout_v2" 
+    xmlns:xacro="http://ros.org/wiki/xacro">
+
+    <xacro:arg name="robot_namespace" default="/" />
+    <xacro:arg name="urdf_extras" default="empty.urdf" />
+    <xacro:arg name="onboard_sensors" default="false" />
+
+    <xacro:include filename="$(find scout_description)/urdf/scout_wheel_type1.xacro" />
+    <xacro:include filename="$(find scout_description)/urdf/scout_wheel_type2.xacro" />
+
+    <!-- Variables -->
+    <xacro:property name="M_PI" value="3.14159"/>
+
+    <!-- Vehicle Geometries -->
+    <xacro:property name="base_x_size" value="0.9250000" />
+    <xacro:property name="base_y_size" value="0.380000" />
+    <xacro:property name="base_z_size" value="0.210000" />
+
+    <xacro:property name="wheelbase" value="0.498" />
+    <xacro:property name="track" value="0.58306" />
+    <xacro:property name="wheel_vertical_offset" value="-0.0702" />
+
+    <xacro:property name="wheel_length" value="1.1653e-01" />
+    <xacro:property name="wheel_radius" value="1.6459e-01" />
+
+    <!-- Base link -->
+    <link name="base_link">
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://scout_description/meshes/base_link.dae" />
+            </geometry>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0.008" rpy="0 0 0" />
+            <geometry>
+                <box size="${base_x_size} ${base_y_size} ${base_z_size}"/>
+            </geometry>
+        </collision>
+        <collision>
+            <origin xyz="0 0 ${base_z_size/6}" rpy="0 0 0" />
+            <geometry>
+                <box size="${base_x_size/6} ${base_y_size*1.65} ${base_z_size/3}"/>
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="40" />
+            <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>
+
+    <!-- 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 -->
+    <xacro:scout_wheel_type1 wheel_prefix="front_right">
+        <origin xyz="${wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="3.14 0 0" />
+    </xacro:scout_wheel_type1>
+    <xacro:scout_wheel_type2 wheel_prefix="front_left">
+        <origin xyz="${wheelbase/2} ${track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
+    </xacro:scout_wheel_type2>
+    <xacro:scout_wheel_type1 wheel_prefix="rear_left">
+        <origin xyz="${-wheelbase/2} ${track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
+    </xacro:scout_wheel_type1>
+    <xacro:scout_wheel_type2 wheel_prefix="rear_right">
+        <origin xyz="${-wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="3.14 0 0" />
+    </xacro:scout_wheel_type2>
+
+    <!-- Onboard sensors -->
+    <xacro:if value="$(arg onboard_sensors)">
+        <!-- <xacro:include filename="$(find scout_description)/urdf/onboard_sensors.xacro" />
+        <xacro:sensor_frame /> -->
+        <xacro:include filename="$(find scout_description)/urdf/onboard_sensors.urdf" />
+    </xacro:if>
+
+    <!-- Additional definitions -->
+    <!-- <xacro:include filename="$(arg urdf_extras)" /> -->
+
+    <!-- Gazebo definitions  -->
+    <!-- <xacro:include filename="$(find scout_description)/urdf/scout_v2.gazebo" /> -->
+</robot>
--- a/scout_description/urdf/scout_wheel.gazebo
+++ b/scout_description/urdf/scout_wheel.gazebo
@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<!--
+Software License Agreement (BSD)
+
+\file      scout_wheel.xacro (adapted from wheel.urdf.xacro in husky_description)
+\authors   Paul Bovbel <pbovbel@clearpathrobotics.com>
+           Ruixiang Du <ruixiang.du@westonrobot.com>
+\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved.
+\copyright Copyright (c) 2020, Weston Robot Pte. Ltd., 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.
+-->
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="scout_wheel">
+
+    <xacro:macro name="scout_wheel_transmission" params="wheel_prefix">
+        <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>
--- a/scout_description/urdf/scout_wheel_type1.xacro
+++ b/scout_description/urdf/scout_wheel_type1.xacro
@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<!--
+Software License Agreement (BSD)
+
+\file      scout_wheel.xacro (adapted from wheel.urdf.xacro in husky_description)
+\authors   Paul Bovbel <pbovbel@clearpathrobotics.com>
+           Ruixiang Du <ruixiang.du@westonrobot.com>
+\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved.
+\copyright Copyright (c) 2020, Weston Robot Pte. Ltd., 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.
+-->
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="scout_wheel">
+
+    <xacro:macro name="scout_wheel_type1" params="wheel_prefix *joint_pose">
+        <link name="${wheel_prefix}_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 xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <mesh filename="package://scout_description/meshes/wheel_type1.dae" />
+                </geometry>
+            </visual>
+            <collision>
+                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
+                <geometry>
+                    <cylinder length="${wheel_length}" radius="${wheel_radius}" />
+                </geometry>
+            </collision>
+        </link>
+
+        <joint name="${wheel_prefix}_wheel" type="continuous">
+            <parent link="base_link"/>
+            <child link="${wheel_prefix}_wheel_link"/>
+            <xacro:insert_block name="joint_pose"/>
+            <axis xyz="0 -1 0" rpy="0 0 0" />
+        </joint>
+    </xacro:macro>
+    
+</robot>
--- a/scout_description/urdf/scout_wheel_type2.xacro
+++ b/scout_description/urdf/scout_wheel_type2.xacro
@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<!--
+Software License Agreement (BSD)
+
+\file      scout_wheel.xacro (adapted from wheel.urdf.xacro in husky_description)
+\authors   Paul Bovbel <pbovbel@clearpathrobotics.com>
+           Ruixiang Du <ruixiang.du@westonrobot.com>
+\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved.
+\copyright Copyright (c) 2020, Weston Robot Pte. Ltd., 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.
+-->
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="scout_wheel">
+
+    <xacro:macro name="scout_wheel_type2" params="wheel_prefix *joint_pose">
+        <link name="${wheel_prefix}_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 xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <mesh filename="package://scout_description/meshes/wheel_type2.dae" />
+                </geometry>
+            </visual>
+            <collision>
+                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
+                <geometry>
+                    <cylinder length="${wheel_length}" radius="${wheel_radius}" />
+                </geometry>
+            </collision>
+        </link>
+
+        <joint name="${wheel_prefix}_wheel" type="continuous">
+            <parent link="base_link"/>
+            <child link="${wheel_prefix}_wheel_link"/>
+            <xacro:insert_block name="joint_pose"/>
+            <axis xyz="0 -1 0" rpy="0 0 0" />
+        </joint>
+    </xacro:macro>
+
+</robot>
--- a/scout_gazebo_sim/CHANGELOG.md
+++ b/scout_gazebo_sim/CHANGELOG.md
@@ -0,0 +1,5 @@
+## Changelog
+
+0.0.1 (2020-03-25)
+------------------
+* Initial release for Melodic.
--- a/scout_gazebo_sim/CMakeLists.txt
+++ b/scout_gazebo_sim/CMakeLists.txt
@@ -0,0 +1,47 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(scout_gazebo_sim)
+
+add_compile_options(-std=c++11)
+
+find_package(catkin REQUIRED COMPONENTS    
+    roslaunch 
+    roslint 
+    roscpp 
+    std_msgs
+    sensor_msgs
+    geometry_msgs
+    tf2 
+    tf2_ros
+)
+catkin_package(
+  INCLUDE_DIRS include
+#   LIBRARIES 
+  CATKIN_DEPENDS roscpp sensor_msgs
+#   DEPENDS Boost
+)
+
+###########
+## Build ##
+###########
+
+include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+)
+
+add_library(scout_gazebo STATIC src/scout_skid_steer.cpp)
+target_link_libraries(scout_gazebo ${catkin_LIBRARIES})
+
+add_executable(scout_skid_steer_controller src/scout_skid_steer_controller.cpp)
+target_link_libraries(scout_skid_steer_controller scout_gazebo ${catkin_LIBRARIES})
+
+#############
+## Install ##
+#############
+
+# roslaunch_add_file_check(launch)
+
+install(
+  DIRECTORY launch worlds
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
--- a/scout_gazebo_sim/config/scout_v2_control.yaml
+++ b/scout_gazebo_sim/config/scout_v2_control.yaml
@@ -0,0 +1,37 @@
+# Publish all joint states -----------------------------------
+scout_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 50
+
+# Joint velocity controllers ---------------------------------
+scout_motor_fr_controller:
+  type: velocity_controllers/JointVelocityController
+  joint: front_right_wheel
+  pid:
+    p: 5.0
+    i: 20.0
+    i_clamp: 100.0
+
+scout_motor_fl_controller:
+  type: velocity_controllers/JointVelocityController
+  joint: front_left_wheel
+  pid:
+    p: 5.0
+    i: 20.0
+    i_clamp: 100.0
+
+scout_motor_rl_controller:
+  type: velocity_controllers/JointVelocityController
+  joint: rear_left_wheel
+  pid:
+    p: 5.0
+    i: 20.0
+    i_clamp: 100.0
+
+scout_motor_rr_controller:
+  type: velocity_controllers/JointVelocityController
+  joint: rear_right_wheel
+  pid:
+    p: 5.0
+    i: 20.0
+    i_clamp: 100.0
--- a/scout_gazebo_sim/include/scout_gazebo/scout_skid_steer.hpp
+++ b/scout_gazebo_sim/include/scout_gazebo/scout_skid_steer.hpp
@@ -0,0 +1,49 @@
+/*
+ * scout_skid_steer.hpp
+ *
+ * Created on: Mar 25, 2020 22:52
+ * Description:
+ *
+ * Copyright (c) 2019 Ruixiang Du (rdu)
+ */
+
+#ifndef SCOUT_SKID_STEER_HPP
+#define SCOUT_SKID_STEER_HPP
+
+#include <geometry_msgs/Twist.h>
+#include <ros/ros.h>
+
+#include <string>
+
+namespace westonrobot {
+class ScoutSkidSteer {
+ public:
+  ScoutSkidSteer(ros::NodeHandle *nh, std::string robot_name = "");
+
+  void SetupSubscription();
+
+ private:
+  std::string robot_name_;
+  std::string motor_fr_topic_;
+  std::string motor_fl_topic_;
+  std::string motor_rl_topic_;
+  std::string motor_rr_topic_;
+  std::string cmd_topic_;
+
+  const double SCOUT_WHEELBASE = 0.498;
+  const double SCOUT_WHEEL_RADIUS = 0.16459;
+
+  ros::NodeHandle *nh_;
+
+  ros::Publisher motor_fr_pub_;
+  ros::Publisher motor_fl_pub_;
+  ros::Publisher motor_rl_pub_;
+  ros::Publisher motor_rr_pub_;
+
+  ros::Subscriber cmd_sub_;
+
+  void TwistCmdCallback(const geometry_msgs::Twist::ConstPtr &msg);
+};
+}  // namespace westonrobot
+
+#endif /* SCOUT_SKID_STEER_HPP */
--- a/scout_gazebo_sim/launch/scout_empty_world.launch
+++ b/scout_gazebo_sim/launch/scout_empty_world.launch
@@ -0,0 +1,18 @@
+<launch>
+    <arg name="robot_namespace" default="/"/>
+    <arg name="world_name" default="$(find scout_gazebo_sim)worlds/weston_robot_empty.world"/>
+
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="world_name" value="$(arg world_name)"/>
+        <arg name="paused" value="false"/>
+        <arg name="use_sim_time" value="true"/>
+        <arg name="gui" value="true"/>
+        <arg name="headless" value="false"/>
+        <arg name="debug" value="false"/>
+    </include>
+
+    <include file="$(find scout_gazebo_sim)/launch/spawn_scout_v2_base.launch"></include>
+    <include file="$(find scout_base)/launch/scout_base_sim.launch" />
+
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find scout_description)/rviz/navigation.rviz" />
+</launch>
--- a/scout_gazebo_sim/launch/scout_playpen.launch
+++ b/scout_gazebo_sim/launch/scout_playpen.launch
@@ -0,0 +1,44 @@
+<?xml version="1.0"?>
+<!--
+Software License Agreement (BSD)
+
+\file      husky_empty_world.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="robot_namespace" default="/"/>
+    <arg name="world_name" default="$(find scout_gazebo_sim)/worlds/clearpath_playpen.world"/>
+
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="world_name" value="$(arg world_name)"/>
+        <arg name="paused" value="false"/>
+        <arg name="use_sim_time" value="true"/>
+        <arg name="gui" value="true"/>
+        <arg name="headless" value="false"/>
+        <arg name="debug" value="false"/>
+    </include>
+
+    <include file="$(find scout_gazebo_sim)/launch/spawn_scout_v2_sensors.launch"></include>
+    <include file="$(find scout_base)/launch/scout_base_sim.launch" />
+
+    <!-- <node name="rviz" pkg="rviz" type="rviz" args="-d $(find scout_description)/rviz/model_display.rviz" /> -->
+</launch>
--- a/scout_gazebo_sim/launch/spawn_scout_v2_base.launch
+++ b/scout_gazebo_sim/launch/spawn_scout_v2_base.launch
@@ -0,0 +1,40 @@
+<launch>
+    <!-- initial pose -->
+    <arg name="x" default="0.0"/>
+    <arg name="y" default="0.0"/>
+    <arg name="z" default="0.0"/>
+    <arg name="yaw" default="0.0"/>
+
+    <arg name="robot_namespace" default=""/>
+
+    <!-- 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="true" />
+    <arg name="gui" default="true" />
+    <arg name="headless" default="false" />
+    <arg name="debug" default="true" />
+
+    <include file="$(find scout_description)/launch/scout_v2_stock.launch">
+        <arg name="robot_namespace" value="$(arg robot_namespace)" />
+    </include>
+
+    <node name="spawn_scout_model" pkg="gazebo_ros" type="spawn_model" args="-x $(arg x)
+              -y $(arg y)
+              -z $(arg z)
+              -Y $(arg yaw)
+              -unpause
+              -urdf
+              -param robot_description
+              -model 'scout$(arg robot_namespace)'" />
+
+    <!-- Load joint controller configurations from YAML file to parameter server -->
+    <rosparam file="$(find scout_gazebo_sim)/config/scout_v2_control.yaml" command="load"/>
+
+    <!-- load the controllers -->
+    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="scout_state_controller scout_motor_fr_controller scout_motor_fl_controller scout_motor_rl_controller scout_motor_rr_controller"/>
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+
+    <node name="scout_skid_steer_controller" pkg="scout_gazebo_sim" type="scout_skid_steer_controller">
+        <param name="robot_namespace" type="string" value="$(arg robot_namespace)" />
+    </node>
+</launch> 
--- a/scout_gazebo_sim/launch/spawn_scout_v2_sensors.launch
+++ b/scout_gazebo_sim/launch/spawn_scout_v2_sensors.launch
@@ -0,0 +1,40 @@
+<launch>
+    <!-- initial pose -->
+    <arg name="x" default="0.0"/>
+    <arg name="y" default="0.0"/>
+    <arg name="z" default="0.0"/>
+    <arg name="yaw" default="0.0"/>
+
+    <arg name="robot_namespace" default=""/>
+
+    <!-- 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="true" />
+    <arg name="gui" default="true" />
+    <arg name="headless" default="false" />
+    <arg name="debug" default="true" />
+
+    <include file="$(find scout_description)/launch/scout_v2_sensors.launch">
+        <arg name="robot_namespace" value="$(arg robot_namespace)" />
+    </include>
+
+    <node name="spawn_scout_model" pkg="gazebo_ros" type="spawn_model" args="-x $(arg x)
+              -y $(arg y)
+              -z $(arg z)
+              -Y $(arg yaw)
+              -unpause
+              -urdf
+              -param robot_description
+              -model 'scout$(arg robot_namespace)'" />
+
+    <!-- Load joint controller configurations from YAML file to parameter server -->
+    <rosparam file="$(find scout_gazebo_sim)/config/scout_v2_control.yaml" command="load"/>
+
+    <!-- load the controllers -->
+    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="scout_state_controller scout_motor_fr_controller scout_motor_fl_controller scout_motor_rl_controller scout_motor_rr_controller"/>
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+
+    <node name="scout_skid_steer_controller" pkg="scout_gazebo_sim" type="scout_skid_steer_controller">
+        <param name="robot_namespace" type="string" value="$(arg robot_namespace)" />
+    </node>
+</launch> 
--- a/scout_gazebo_sim/package.xml
+++ b/scout_gazebo_sim/package.xml
@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<package>
+  <name>scout_gazebo_sim</name>
+  <version>0.0.1</version>
+  <description>AgileX Scout 2.0 Simulator bringup</description>
+
+  <author email="ruixiang.du@westonrobot.com">Ruixiang Du</author>
+  <maintainer email="ruixiang.du@westonrobot.com">Ruixiang Du</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/scout_gazebo</url>
+  <url type="bugtracker">https://github.com/husky/husky_simulator/issues</url>
+  <url type="repository">https://github.com/westonrobot/scout_ros</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roslaunch</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>controller_manager</run_depend>
+  <run_depend>gazebo_plugins</run_depend>
+  <run_depend>gazebo_ros</run_depend>
+  <run_depend>gazebo_ros_control</run_depend>
+  <run_depend>scout_description</run_depend>
+  <run_depend>rostopic</run_depend>
+
+  <export>
+  </export>
+</package>
--- a/scout_gazebo_sim/src/scout_skid_steer.cpp
+++ b/scout_gazebo_sim/src/scout_skid_steer.cpp
@@ -0,0 +1,60 @@
+/*
+ * scout_skid_steer.cpp
+ *
+ * Created on: Mar 25, 2020 22:54
+ * Description:
+ *
+ * Copyright (c) 2019 Ruixiang Du (rdu)
+ */
+
+#include "scout_gazebo/scout_skid_steer.hpp"
+
+#include <geometry_msgs/Twist.h>
+#include <std_msgs/Float64.h>
+
+namespace westonrobot {
+ScoutSkidSteer::ScoutSkidSteer(ros::NodeHandle *nh, std::string robot_name)
+    : nh_(nh), robot_name_(robot_name) {
+  motor_fr_topic_ = robot_name_ + "/scout_motor_fr_controller/command";
+  motor_fl_topic_ = robot_name_ + "/scout_motor_fl_controller/command";
+  motor_rl_topic_ = robot_name_ + "/scout_motor_rl_controller/command";
+  motor_rr_topic_ = robot_name_ + "/scout_motor_rr_controller/command";
+  cmd_topic_ = robot_name_ + "/cmd_vel";
+}
+
+void ScoutSkidSteer::SetupSubscription() {
+  // command subscriber
+  cmd_sub_ = nh_->subscribe<geometry_msgs::Twist>(
+      cmd_topic_, 5, &ScoutSkidSteer::TwistCmdCallback, this);
+
+  // motor command publisher
+  motor_fr_pub_ = nh_->advertise<std_msgs::Float64>(motor_fr_topic_, 50);
+  motor_fl_pub_ = nh_->advertise<std_msgs::Float64>(motor_fl_topic_, 50);
+  motor_rl_pub_ = nh_->advertise<std_msgs::Float64>(motor_rl_topic_, 50);
+  motor_rr_pub_ = nh_->advertise<std_msgs::Float64>(motor_rr_topic_, 50);
+}
+
+void ScoutSkidSteer::TwistCmdCallback(
+    const geometry_msgs::Twist::ConstPtr &msg) {
+  std_msgs::Float64 motor_cmd[4];
+
+  double driving_vel = msg->linear.x;
+  double steering_vel = msg->angular.z;
+
+  double left_side_velocity =
+      (driving_vel - steering_vel * SCOUT_WHEELBASE) / SCOUT_WHEEL_RADIUS;
+  double right_side_velocity =
+      (driving_vel + steering_vel * SCOUT_WHEELBASE) / SCOUT_WHEEL_RADIUS;
+
+  motor_cmd[0].data = right_side_velocity;
+  motor_cmd[1].data = -left_side_velocity;
+  motor_cmd[2].data = -left_side_velocity;
+  motor_cmd[3].data = right_side_velocity;
+
+  motor_fr_pub_.publish(motor_cmd[0]);
+  motor_fl_pub_.publish(motor_cmd[1]);
+  motor_rl_pub_.publish(motor_cmd[2]);
+  motor_rr_pub_.publish(motor_cmd[3]);
+}
+
+}  // namespace westonrobot
--- a/scout_gazebo_sim/src/scout_skid_steer_controller.cpp
+++ b/scout_gazebo_sim/src/scout_skid_steer_controller.cpp
@@ -0,0 +1,30 @@
+#include <nav_msgs/Odometry.h>
+#include <ros/ros.h>
+#include <sensor_msgs/JointState.h>
+#include <tf/transform_broadcaster.h>
+
+#include <string>
+
+#include "scout_gazebo/scout_skid_steer.hpp"
+
+using namespace westonrobot;
+
+int main(int argc, char **argv) {
+  // setup ROS node
+  ros::init(argc, argv, "scout_skid_steer");
+  ros::NodeHandle node(""), private_node("~");
+
+  // fetch parameters
+  std::string robot_namespace;
+  private_node.param<std::string>("robot_namespace", robot_namespace,
+                                  std::string("scout_default"));
+
+  ROS_INFO("Namespace: %s", robot_namespace.c_str());
+
+  ScoutSkidSteer skid_steer_controller(&node, robot_namespace);
+  skid_steer_controller.SetupSubscription();
+
+  ros::spin();
+
+  return 0;
+}
--- a/scout_gazebo_sim/worlds/clearpath_playpen.world
+++ b/scout_gazebo_sim/worlds/clearpath_playpen.world
--- a/scout_gazebo_sim/worlds/weston_robot_empty.world
+++ b/scout_gazebo_sim/worlds/weston_robot_empty.world
@@ -0,0 +1,22 @@
+<?xml version="1.0" ?>
+<sdf version="1.4">
+  <world name="default">
+    <!-- A global light source -->
+    <include>
+      <uri>model://sun</uri>
+    </include>
+
+    <!-- A ground plane -->
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+
+    <!-- Physics -->
+    <physics type='ode'>
+      <max_step_size>0.01</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <real_time_update_rate>100</real_time_update_rate>
+      <gravity>0 0 -9.8</gravity>
+    </physics>
+  </world>
+</sdf>
--- a/scout_navigation/CMakeLists.txt
+++ b/scout_navigation/CMakeLists.txt
@@ -0,0 +1,209 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(scout_navigation)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  sensor_msgs
+  std_msgs
+  urdf
+  xacro
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   sensor_msgs#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES scout_navigation
+#  CATKIN_DEPENDS roscpp rospy sensor_msgs std_msgs urdf xacro
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/scout_navigation.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/scout_navigation_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_scout_navigation.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
--- a/scout_navigation/launch/gmapping.launch
+++ b/scout_navigation/launch/gmapping.launch
@@ -0,0 +1,11 @@
+<!-- -->
+<launch>
+    <param name="/use_sim_time" value="true"/>
+
+    <node name="slam_gmapping" pkg="gmapping" type="slam_gmapping">
+        <remap from="scan" to="/scan"/>
+        <param name="base_link" value="base_footprint"/>
+    </node>
+
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find scout_robot)/rviz/mapping.rviz"/>
+</launch>
--- a/scout_navigation/launch/navigation.launch
+++ b/scout_navigation/launch/navigation.launch
@@ -0,0 +1,33 @@
+<launch>
+    <param name="/use_sim_time" value="true"/>
+    <arg name="map_file" default="$(find scout_navigation)/maps/play.yaml"/>
+
+    <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" output="screen">
+        <param name="frame_id" value="map"/>
+    </node>
+
+    <node pkg="amcl" type="amcl" name="amcl" output="screen">
+        <rosparam file="$(find scout_navigation)/param/amcl_params.yaml" command="load" />
+        <param name="initial_pose_x" value="0"/>
+        <param name="initial_pose_y" value="0"/>
+        <param name="initial_pose_a" value="0"/>
+    </node>
+
+    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+        <rosparam file="$(find scout_navigation)/param/diff_drive/costmap_common_params.yaml" command="load" ns="global_costmap" />
+        <rosparam file="$(find scout_navigation)/param/diff_drive/costmap_common_params.yaml" command="load" ns="local_costmap" />
+        <rosparam file="$(find scout_navigation)/param/diff_drive/local_costmap_params.yaml" command="load" />
+        <rosparam file="$(find scout_navigation)/param/diff_drive/global_costmap_params.yaml" command="load" />
+        <rosparam file="$(find scout_navigation)/param/diff_drive/teb_local_planner_params.yaml" command="load" />
+
+        <param name="base_global_planner" value="global_planner/GlobalPlanner" />
+        <param name="planner_frequency" value="1.0" />
+        <param name="planner_patience" value="5.0" />
+
+        <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
+        <param name="controller_frequency" value="5.0" />
+        <param name="controller_patience" value="15.0" />
+    </node>
+
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find scout_navigation)/rviz/rviz_navigation.rviz"/>
+</launch>
--- a/scout_navigation/maps/play.pgm
+++ b/scout_navigation/maps/play.pgm
--- a/scout_navigation/maps/play.yaml
+++ b/scout_navigation/maps/play.yaml
@@ -0,0 +1,7 @@
+image: play.pgm
+resolution: 0.010000
+origin: [-20.000000, -20.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+
--- a/scout_navigation/package.xml
+++ b/scout_navigation/package.xml
@@ -0,0 +1,77 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>scout_navigation</name>
+  <version>0.0.0</version>
+  <description>The scout_navigation package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="rdu@todo.todo">rdu</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/scout_navigation</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>urdf</build_depend>
+  <build_depend>xacro</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>urdf</build_export_depend>
+  <build_export_depend>xacro</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>urdf</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
--- a/scout_navigation/param/amcl_params.yaml
+++ b/scout_navigation/param/amcl_params.yaml
@@ -0,0 +1,38 @@
+use_map_topic: true
+
+odom_frame_id: "odom"
+base_frame_id: "base_footprint"
+global_frame_id: "map"
+
+## Publish scans from best pose at a max of 10 Hz
+odom_model_type: "diff"
+odom_alpha5: 0.1
+gui_publish_rate: 10.0
+laser_max_beams: 60
+laser_max_range: 12.0
+min_particles: 500
+max_particles: 2000
+kld_err: 0.05
+kld_z: 0.99
+odom_alpha1: 0.2
+odom_alpha2: 0.2
+## translation std dev, m 
+odom_alpha3: 0.2
+odom_alpha4: 0.2
+laser_z_hit: 0.5
+aser_z_short: 0.05
+laser_z_max: 0.05
+laser_z_rand: 0.5
+laser_sigma_hit: 0.2
+laser_lambda_short: 0.1
+laser_model_type: "likelihood_field" # "likelihood_field" or "beam"
+laser_likelihood_max_dist: 2.0
+update_min_d: 0.25
+update_min_a: 0.2
+
+resample_interval: 1
+
+## Increase tolerance because the computer can get quite busy 
+transform_tolerance: 1.0
+recovery_alpha_slow: 0.001
+recovery_alpha_fast: 0.1
--- a/scout_navigation/param/diff_drive/costmap_common_params.yaml
+++ b/scout_navigation/param/diff_drive/costmap_common_params.yaml
@@ -0,0 +1,33 @@
+
+#---standard pioneer footprint---
+#---(in meters)---
+#robot_radius: 0.25 #0.17
+#footprint_padding: 0.00
+
+footprint: [ [-0.465,-0.350], [0.465,-0.350], [0.465,0.350], [-0.465,0.350] ]
+
+transform_tolerance: 0.2
+map_type: costmap
+
+always_send_full_costmap: true
+
+obstacle_layer:
+ enabled: true
+ obstacle_range: 3.0
+ raytrace_range: 4.0
+ inflation_radius: 0.2
+ track_unknown_space: true
+ combination_method: 1
+
+ observation_sources: laser_scan_sensor
+ laser_scan_sensor: {data_type: LaserScan, topic: scan, marking: true, clearing: true}
+
+
+inflation_layer:
+  enabled:              true
+  cost_scaling_factor:  10.0  # exponential rate at which the obstacle cost drops off (default: 10)
+  inflation_radius:     0.5  # max. distance from an obstacle at which costs are incurred for planning paths.
+
+static_layer:
+  enabled:              true
+  map_topic:            "/map"
--- a/scout_navigation/param/diff_drive/costmap_converter_params.yaml
+++ b/scout_navigation/param/diff_drive/costmap_converter_params.yaml
@@ -0,0 +1,31 @@
+###########################################################################################
+## NOTE: Costmap conversion is experimental. Its purpose is to combine many point        ##
+## obstales into clusters, computed in a separate thread in order to improve the overall ## 
+## efficiency of local planning. However, the implemented conversion algorithms are in a ##
+## very early stage of development. Contributions are welcome!                           ##
+###########################################################################################
+
+TebLocalPlannerROS:
+
+  ## Costmap converter plugin   
+  #costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
+  costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC"
+  #costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSMCCH"
+  #costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSConcaveHull"
+  costmap_converter_spin_thread: True
+  costmap_converter_rate: 5
+ 
+ 
+  ## Configure plugins (namespace move_base/costmap_to_lines or move_base/costmap_to_polygons)
+  ## costmap_converter/CostmapToLinesDBSRANSAC, costmap_converter/CostmapToLinesDBSMCCH, costmap_converter/CostmapToPolygonsDBSMCCH
+  costmap_converter/CostmapToLinesDBSRANSAC:
+    cluster_max_distance: 0.4
+    cluster_min_pts: 2
+    ransac_inlier_distance: 0.15
+    ransac_min_inliers: 10
+    ransac_no_iterations: 1500
+    ransac_remainig_outliers: 3
+    ransac_convert_outlier_pts: True
+    ransac_filter_remaining_outlier_pts: False
+    convex_hull_min_pt_separation: 0.1
+ 
--- a/scout_navigation/param/diff_drive/dyn_obst/costmap_converter_params.yaml
+++ b/scout_navigation/param/diff_drive/dyn_obst/costmap_converter_params.yaml
@@ -0,0 +1,41 @@
+###########################################################################################
+## NOTE: Costmap conversion is experimental. Its purpose is to combine many point        ##
+## obstales into clusters, computed in a separate thread in order to improve the overall ## 
+## efficiency of local planning. However, the implemented conversion algorithms are in a ##
+## very early stage of development. Contributions are welcome!                           ##
+###########################################################################################
+
+TebLocalPlannerROS:
+
+  ## Costmap converter plugin   
+  costmap_converter_plugin: "costmap_converter::CostmapToDynamicObstacles"
+  costmap_converter_spin_thread: True
+  costmap_converter_rate: 5
+ 
+ 
+  ## Configure plugins (namespace move_base/CostmapToDynamicObstacles)
+  costmap_converter/CostmapToDynamicObstacles:
+    alpha_slow: 0.3
+    alpha_fast: 0.85
+    beta: 0.85
+    min_sep_between_slow_and_fast_filter: 80
+    min_occupancy_probability: 180
+    max_occupancy_neighbors: 100
+    morph_size: 1
+    filter_by_area: True
+    min_area: 3
+    max_area: 300
+    filter_by_circularity: True
+    min_circularity: 0.2
+    max_circularity: 1.0
+    filter_by_inertia: True
+    min_intertia_ratio: 0.2
+    max_inertia_ratio: 1.0
+    filter_by_convexity: False
+    min_convexity: 0.0
+    max_convexity: 1.0
+    dt: 0.2
+    dist_thresh: 60.0
+    max_allowed_skipped_frames: 3
+    max_trace_length: 10
+    static_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
--- a/scout_navigation/param/diff_drive/dyn_obst/global_costmap_params.yaml
+++ b/scout_navigation/param/diff_drive/dyn_obst/global_costmap_params.yaml
@@ -0,0 +1,16 @@
+global_costmap:
+  global_frame: map
+  robot_base_frame: robot_0/base_link
+  update_frequency: 1.0
+  publish_frequency: 0.5
+  static_map: true
+ 
+  transform_tolerance: 0.5
+  plugins:
+    - {name: static_layer,            type: "costmap_2d::StaticLayer"}
+    - {name: obstacle_layer,          type: "costmap_2d::VoxelLayer"}
+    - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}
+
+
+
+
--- a/scout_navigation/param/diff_drive/dyn_obst/local_costmap_params.yaml
+++ b/scout_navigation/param/diff_drive/dyn_obst/local_costmap_params.yaml
@@ -0,0 +1,15 @@
+local_costmap:
+  global_frame: map
+  robot_base_frame: robot_0/base_link
+  update_frequency: 5.0
+  publish_frequency: 5.001 # actually exactly 5.0Hz, see https://github.com/ros-planning/navigation/issues/383
+  static_map: false
+  rolling_window: true
+  width: 12
+  height: 12
+  resolution: 0.1
+  transform_tolerance: 0.5
+  
+  plugins:
+   - {name: static_layer,        type: "costmap_2d::StaticLayer"}
+   - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}
--- a/scout_navigation/param/diff_drive/dyn_obst/teb_local_planner_params.yaml
+++ b/scout_navigation/param/diff_drive/dyn_obst/teb_local_planner_params.yaml
@@ -0,0 +1,83 @@
+TebLocalPlannerROS:
+
+ odom_topic: odom
+    
+ # Trajectory
+  
+ teb_autosize: True
+ dt_ref: 0.3
+ dt_hysteresis: 0.1
+ global_plan_overwrite_orientation: True
+ allow_init_with_backwards_motion: False
+ max_global_plan_lookahead_dist: 3.0
+ feasibility_check_no_poses: 5
+    
+ # Robot
+         
+ max_vel_x: 0.4
+ max_vel_x_backwards: 0.2
+ max_vel_y: 0.0
+ max_vel_theta: 0.3
+ acc_lim_x: 0.5
+ acc_lim_theta: 0.5
+ min_turning_radius: 0.0 # diff-drive robot (can turn on place!)
+
+ footprint_model:
+   type: "point"
+
+ # GoalTolerance
+    
+ xy_goal_tolerance: 0.2
+ yaw_goal_tolerance: 0.1
+ free_goal_vel: False
+    
+ # Obstacles
+    
+ min_obstacle_dist: 0.6 # This value must also include our robot radius, since footprint_model is set to "point".
+ inflation_dist: 0.8
+ dynamic_obstacle_inflation_dist: 0.6
+ include_dynamic_obstacles: True
+ include_costmap_obstacles: True
+ costmap_obstacles_behind_robot_dist: 1.5
+ obstacle_poses_affected: 30
+ # costmap_converter parameters are defined in costmap_converter_params.yaml
+
+ # Optimization
+    
+ no_inner_iterations: 5
+ no_outer_iterations: 4
+ optimization_activate: True
+ optimization_verbose: False
+ penalty_epsilon: 0.1
+ weight_max_vel_x: 3
+ weight_max_vel_theta: 1
+ weight_acc_lim_x: 2
+ weight_acc_lim_theta: 2
+ weight_kinematics_nh: 1000
+ weight_kinematics_forward_drive: 1
+ weight_kinematics_turning_radius: 1
+ weight_optimaltime: 1
+ weight_obstacle: 50
+ weight_inflation: 0.3
+ weight_dynamic_obstacle: 50
+ weight_dynamic_obstacle_inflation: 0.3
+ weight_adapt_factor: 2
+
+ # Homotopy Class Planner
+
+ enable_homotopy_class_planning: True
+ enable_multithreading: True
+ simple_exploration: False
+ max_number_classes: 4
+ selection_cost_hysteresis: 1.0
+ selection_obst_cost_scale: 1.0
+ selection_alternative_time_cost: True
+ 
+ roadmap_graph_no_samples: 15
+ roadmap_graph_area_width: 5
+ h_signature_prescaler: 0.5
+ h_signature_threshold: 0.1
+ obstacle_keypoint_offset: 0.1
+ obstacle_heading_threshold: 0.45
+ visualize_hc_graph: False
+ visualize_with_time_as_z_axis_scale: 0.2
--- a/scout_navigation/param/diff_drive/global_costmap_params.yaml
+++ b/scout_navigation/param/diff_drive/global_costmap_params.yaml
@@ -0,0 +1,16 @@
+global_costmap:
+  global_frame: map
+  robot_base_frame: base_footprint
+  update_frequency: 1.0
+  publish_frequency: 0.5
+  static_map: true
+ 
+  transform_tolerance: 0.5
+  plugins:
+    - {name: static_layer,            type: "costmap_2d::StaticLayer"}
+    - {name: obstacle_layer,          type: "costmap_2d::VoxelLayer"}
+    - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}
+
+
+
+
--- a/scout_navigation/param/diff_drive/local_costmap_params.yaml
+++ b/scout_navigation/param/diff_drive/local_costmap_params.yaml
@@ -0,0 +1,15 @@
+local_costmap:
+  global_frame: map
+  robot_base_frame: base_footprint
+  update_frequency: 5.0
+  publish_frequency: 2.0
+  static_map: false
+  rolling_window: true
+  width: 5.5
+  height: 5.5
+  resolution: 0.1
+  transform_tolerance: 0.5
+  
+  plugins:
+   - {name: static_layer,        type: "costmap_2d::StaticLayer"}
+   - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}
--- a/scout_navigation/param/diff_drive/teb_local_planner_params.yaml
+++ b/scout_navigation/param/diff_drive/teb_local_planner_params.yaml
@@ -0,0 +1,112 @@
+TebLocalPlannerROS:
+
+ odom_topic: odom
+    
+ # Trajectory
+  
+ teb_autosize: True
+ dt_ref: 0.3
+ dt_hysteresis: 0.1
+ max_samples: 500
+ global_plan_overwrite_orientation: True
+ allow_init_with_backwards_motion: False
+ max_global_plan_lookahead_dist: 3.0
+ global_plan_viapoint_sep: -1
+ global_plan_prune_distance: 1
+ exact_arc_length: False
+ feasibility_check_no_poses: 5
+ publish_feedback: False
+    
+ # Robot
+         
+ max_vel_x: 0.8 #0.4
+ max_vel_x_backwards: 0.2
+ max_vel_y: 0.0
+ max_vel_theta: 1.8 #0.3
+ acc_lim_x: 0.5
+ acc_lim_theta: 1.8 #0.5
+ min_turning_radius: 0.0 # diff-drive robot (can turn on place!)
+
+ footprint_model:
+   type: "line"
+   line_start: [-0.4, 0.0] 
+   line_end: [0.4, 0.0]   
+
+ # GoalTolerance
+    
+ xy_goal_tolerance: 0.2
+ yaw_goal_tolerance: 0.1
+ free_goal_vel: False
+ complete_global_plan: True
+    
+ # Obstacles
+    
+ min_obstacle_dist: 0.45 #0.25 # This value must also include our robot radius, since footprint_model is set to "point".
+ inflation_dist: 0.6
+ include_costmap_obstacles: True
+ costmap_obstacles_behind_robot_dist: 1.5
+ obstacle_poses_affected: 15
+
+ dynamic_obstacle_inflation_dist: 0.6
+ include_dynamic_obstacles: True
+
+ costmap_converter_plugin: ""
+ costmap_converter_spin_thread: True
+ costmap_converter_rate: 5
+
+ # Optimization
+    
+ no_inner_iterations: 5
+ no_outer_iterations: 4
+ optimization_activate: True
+ optimization_verbose: False
+ penalty_epsilon: 0.1
+ obstacle_cost_exponent: 4
+ weight_max_vel_x: 2
+ weight_max_vel_theta: 1
+ weight_acc_lim_x: 1
+ weight_acc_lim_theta: 1
+ weight_kinematics_nh: 1000
+ weight_kinematics_forward_drive: 1
+ weight_kinematics_turning_radius: 1
+ weight_optimaltime: 1 # must be > 0
+ weight_shortest_path: 0
+ weight_obstacle: 100
+ weight_inflation: 0.2
+ weight_dynamic_obstacle: 10
+ weight_dynamic_obstacle_inflation: 0.2
+ weight_viapoint: 1
+ weight_adapt_factor: 2
+
+ # Homotopy Class Planner
+
+ enable_homotopy_class_planning: True
+ enable_multithreading: True
+ max_number_classes: 4
+ selection_cost_hysteresis: 1.0
+ selection_prefer_initial_plan: 0.9
+ selection_obst_cost_scale: 100.0
+ selection_alternative_time_cost: False
+ 
+ roadmap_graph_no_samples: 15
+ roadmap_graph_area_width: 5
+ roadmap_graph_area_length_scale: 1.0
+ h_signature_prescaler: 0.5
+ h_signature_threshold: 0.1
+ obstacle_heading_threshold: 0.45
+ switching_blocking_period: 0.0
+ viapoints_all_candidates: True
+ delete_detours_backwards: True
+ max_ratio_detours_duration_best_duration: 3.0
+ visualize_hc_graph: False
+ visualize_with_time_as_z_axis_scale: False
+
+# Recovery
+ 
+ shrink_horizon_backup: True
+ shrink_horizon_min_duration: 10
+ oscillation_recovery: True
+ oscillation_v_eps: 0.1
+ oscillation_omega_eps: 0.1
+ oscillation_recovery_min_duration: 10
+ oscillation_filter_duration: 10
--- a/scout_navigation/rviz/mapping.rviz
+++ b/scout_navigation/rviz/mapping.rviz
@@ -0,0 +1,252 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Map1
+        - /Odometry1
+        - /RobotModel1
+        - /TF1
+      Splitter Ratio: 0.5
+    Tree Height: 508
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        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: 0.7
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 1.12401e-37
+      Min Color: 0; 0; 0
+      Min Intensity: 1.12401e-37
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Flat Squares
+      Topic: /robot/laser/scan
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Angle Tolerance: 0.1
+      Class: rviz/Odometry
+      Color: 255; 25; 0
+      Enabled: true
+      Keep: 100
+      Length: 1
+      Name: Odometry
+      Position Tolerance: 0.1
+      Topic: /odom
+      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_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        hokuyo_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheel_1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheel_2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheel_3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheel_4:
+          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_footprint:
+          Value: true
+        base_link:
+          Value: true
+        camera_link:
+          Value: true
+        hokuyo_link:
+          Value: true
+        map:
+          Value: true
+        odom:
+          Value: true
+        wheel_1:
+          Value: true
+        wheel_2:
+          Value: true
+        wheel_3:
+          Value: true
+        wheel_4:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        map:
+          odom:
+            {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    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
+      Topic: /initialpose
+    - 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: 8.13564
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.860398
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.9304
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 681
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000013c0000028bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000000000028b000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000201fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004100000201000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000002f60000003efc0100000002fb0000000800540069006d00650000000000000002f6000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002f50000028b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1079
+  X: 783
+  Y: 50
--- a/scout_navigation/rviz/rviz_navigation.rviz
+++ b/scout_navigation/rviz/rviz_navigation.rviz
@@ -0,0 +1,329 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 81
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Robot1
+      Splitter Ratio: 0.605095983
+    Tree Height: 744
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+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.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 50
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.699999988
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 1
+          Arrow Length: 0.300000012
+          Axes Length: 0.300000012
+          Axes Radius: 0.00999999978
+          Class: rviz/PoseArray
+          Color: 255; 25; 0
+          Enabled: true
+          Head Length: 0.0700000003
+          Head Radius: 0.0299999993
+          Name: TebPoses
+          Shaft Length: 0.230000004
+          Shaft Radius: 0.00999999978
+          Shape: Arrow (Flat)
+          Topic: /move_base/TebLocalPlannerROS/teb_poses
+          Unreliable: false
+          Value: true
+        - Class: rviz/Marker
+          Enabled: true
+          Marker Topic: /move_base/TebLocalPlannerROS/teb_markers
+          Name: TebMarker
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz/Path
+          Color: 255; 0; 0
+          Enabled: true
+          Head Diameter: 0.300000012
+          Head Length: 0.200000003
+          Length: 0.300000012
+          Line Style: Lines
+          Line Width: 0.0299999993
+          Name: LocalPath
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Pose Color: 255; 85; 255
+          Pose Style: None
+          Radius: 0.0299999993
+          Shaft Diameter: 0.100000001
+          Shaft Length: 0.100000001
+          Topic: /move_base/TebLocalPlannerROS/local_plan
+          Unreliable: false
+          Value: true
+        - Alpha: 0.400000006
+          Class: rviz/Map
+          Color Scheme: map
+          Draw Behind: false
+          Enabled: true
+          Name: LocalCostmap
+          Topic: /move_base/local_costmap/costmap
+          Unreliable: false
+          Use Timestamp: false
+          Value: true
+      Enabled: true
+      Name: Local Planner
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz/Path
+          Color: 25; 255; 0
+          Enabled: true
+          Head Diameter: 0.300000012
+          Head Length: 0.200000003
+          Length: 0.300000012
+          Line Style: Lines
+          Line Width: 0.0299999993
+          Name: GlobalPath
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Pose Color: 255; 85; 255
+          Pose Style: None
+          Radius: 0.0299999993
+          Shaft Diameter: 0.100000001
+          Shaft Length: 0.100000001
+          Topic: /move_base/TebLocalPlannerROS/global_plan
+          Unreliable: false
+          Value: true
+        - Alpha: 0.699999988
+          Class: rviz/Map
+          Color Scheme: map
+          Draw Behind: false
+          Enabled: true
+          Name: GlobalCostmap
+          Topic: /move_base/global_costmap/costmap
+          Unreliable: false
+          Use Timestamp: false
+          Value: true
+      Enabled: true
+      Name: Global Planner
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 1
+          Class: rviz/Polygon
+          Color: 85; 0; 255
+          Enabled: true
+          Name: Robot Footprint
+          Topic: /move_base/local_costmap/footprint
+          Unreliable: false
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/LaserScan
+          Color: 255; 255; 255
+          Color Transformer: Intensity
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 999999
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: LaserScan
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.00999999978
+          Style: Flat Squares
+          Topic: /scan
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 1
+          Arrow Length: 0.100000001
+          Axes Length: 0.300000012
+          Axes Radius: 0.00999999978
+          Class: rviz/PoseArray
+          Color: 255; 25; 0
+          Enabled: true
+          Head Length: 0.0700000003
+          Head Radius: 0.0299999993
+          Name: AMCL Particles
+          Shaft Length: 0.230000004
+          Shaft Radius: 0.00999999978
+          Shape: Arrow (Flat)
+          Topic: /particlecloud
+          Unreliable: false
+          Value: true
+        - Class: rviz/TF
+          Enabled: false
+          Frame Timeout: 15
+          Frames:
+            All Enabled: true
+          Marker Scale: 1
+          Name: TF
+          Show Arrows: true
+          Show Axes: true
+          Show Names: true
+          Tree:
+            {}
+          Update Interval: 0
+          Value: false
+        - Angle Tolerance: 0.100000001
+          Class: rviz/Odometry
+          Covariance:
+            Orientation:
+              Alpha: 0.5
+              Color: 255; 255; 127
+              Color Style: Unique
+              Frame: Local
+              Offset: 1
+              Scale: 1
+              Value: true
+            Position:
+              Alpha: 0.300000012
+              Color: 204; 51; 204
+              Scale: 1
+              Value: true
+            Value: true
+          Enabled: false
+          Keep: 100
+          Name: Odometry
+          Position Tolerance: 0.100000001
+          Shape:
+            Alpha: 1
+            Axes Length: 1
+            Axes Radius: 0.100000001
+            Color: 255; 25; 0
+            Head Length: 0.300000012
+            Head Radius: 0.100000001
+            Shaft Length: 1
+            Shaft Radius: 0.0500000007
+            Value: Arrow
+          Topic: /odom
+          Unreliable: false
+          Value: false
+      Enabled: true
+      Name: Robot
+  Enabled: true
+  Global Options:
+    Background Color: 255; 255; 255
+    Default Light: true
+    Fixed Frame: map
+    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
+      Topic: /initialpose
+    - 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: 23.5006466
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.338720083
+        Y: 0.705890715
+        Z: -1.42741621
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 1.56480002
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.71043015
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1028
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000037afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000037a000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003a00000003efc0100000002fb0000000800540069006d00650100000000000003a00000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000002300000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 928
+  X: 65
+  Y: 24
--- a/scout_navigation/rviz/urdf.rviz
+++ b/scout_navigation/rviz/urdf.rviz
@@ -0,0 +1,156 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 775
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+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.0299999993
+        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
+        back left_Link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        back right_Link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front left_Link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front right_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
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: base_link
+    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
+      Topic: /initialpose
+    - 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: 1.64610708
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        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.0500000007
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 1.01039815
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.810398042
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1056
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1855
+  X: 65
+  Y: 24
--- a/scout_webots_sim/CMakeLists.txt
+++ b/scout_webots_sim/CMakeLists.txt
@@ -0,0 +1,219 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(scout_webots_sim)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roslaunch
+  geometry_msgs
+  roscpp
+  rospy
+  scout_base
+  sensor_msgs
+  std_msgs
+  message_generation 
+  tf
+  webots_ros
+  pcl_ros
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#     FILES
+#     BoolStamped.msg
+#     Float64Stamped.msg
+#     Int32Stamped.msg
+#   )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#     FILES
+#     camera_get_focus_info.srv
+#     camera_get_info.srv
+#   )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#     DEPENDENCIES
+#     std_msgs
+#     sensor_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES scout_webots_sim
+ CATKIN_DEPENDS geometry_msgs roscpp rospy scout_base sensor_msgs std_msgs message_runtime tf webots_ros pcl_ros
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# set(SCOUT_WEBOTS_SRC
+#     src/scout_webots_interface.cpp
+# )
+# add_library(scout_webots_sim STATIC ${SCOUT_WEBOTS_SRC})
+# target_link_libraries(scout_webots_sim ${catkin_LIBRARIES})
+# target_include_directories(scout_webots_sim PUBLIC
+#   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+#   $<INSTALL_INTERFACE:include>
+#   PRIVATE src)
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+## Declare a C++ executable
+add_executable(scout_webots_node src/scout_webots_node.cpp src/scout_webots_interface.cpp)
+add_dependencies(scout_webots_node webots_ros_generate_messages_cpp)
+target_link_libraries(scout_webots_node ${catkin_LIBRARIES})
+
+#############
+## Install ##
+#############
+
+# roslaunch_add_file_check(launch)
+
+install(TARGETS scout_webots_node
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+install(DIRECTORY launch urdf worlds
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_scout_webots_sim.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
--- a/scout_webots_sim/include/scout_webots_sim/scout_sim_params.hpp
+++ b/scout_webots_sim/include/scout_webots_sim/scout_sim_params.hpp
@@ -0,0 +1,36 @@
+/* 
+ * scout_sim_params.hpp
+ * 
+ * Created on: Sep 27, 2019 15:08
+ * Description: 
+ * 
+ * Copyright (c) 2019 Ruixiang Du (rdu)
+ */ 
+
+#ifndef SCOUT_SIM_PARAMS_HPP
+#define SCOUT_SIM_PARAMS_HPP
+
+#include <cstdint>
+
+namespace westonrobot
+{
+struct ScoutSimParams
+{
+    /* Scout Parameters */
+    static constexpr double max_steer_angle = 30.0; // in degree
+
+    static constexpr double track = 0.576;        // in meter (left & right wheel distance)
+    static constexpr double wheelbase = 0.648;    // in meter (front & rear wheel distance)
+    static constexpr double wheel_radius = 0.165; // in meter
+
+    // from user manual v1.2.8 P18
+    // max linear velocity: 1.5 m/s
+    // max angular velocity: 0.7853 rad/s
+    static constexpr double max_linear_speed = 1.5;  // in m/s
+    static constexpr double max_angular_speed = 0.7853; // in rad/s
+    static constexpr double max_speed_cmd = 10.0;       // in rad/s
+};
+} // namespace westonrobot
+
+
+#endif /* SCOUT_SIM_PARAMS_HPP */
--- a/scout_webots_sim/include/scout_webots_sim/scout_webots_interface.hpp
+++ b/scout_webots_sim/include/scout_webots_sim/scout_webots_interface.hpp
@@ -0,0 +1,41 @@
+/* 
+ * scout_webots_interface.hpp
+ * 
+ * Created on: Sep 26, 2019 23:04
+ * Description: 
+ * 
+ * Copyright (c) 2019 Ruixiang Du (rdu)
+ */
+
+#ifndef SCOUT_WEBOTS_INTERFACE_HPP
+#define SCOUT_WEBOTS_INTERFACE_HPP
+
+#include <string>
+
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud.h>
+#include <sensor_msgs/Imu.h>
+
+#include "scout_base/scout_messenger.hpp"
+
+namespace westonrobot
+{
+class ScoutWebotsInterface
+{
+public:
+    ScoutWebotsInterface(ros::NodeHandle *nh, ScoutROSMessenger* msger, uint32_t time_step);
+
+    void InitComponents(std::string controller_name);
+    void UpdateSimState();
+
+private:
+    ros::NodeHandle *nh_;
+    ScoutROSMessenger* messenger_;
+    uint32_t time_step_;
+
+    std::string robot_name_ = "agilex_scout";
+    const std::vector<std::string> motor_names_{"motor_fr", "motor_fl", "motor_rl", "motor_rr"};
+};
+} // namespace westonrobot
+
+#endif /* SCOUT_WEBOTS_INTERFACE_HPP */
--- a/scout_webots_sim/src/scout_webots_interface.cpp
+++ b/scout_webots_sim/src/scout_webots_interface.cpp
@@ -0,0 +1,129 @@
+/* 
+ * scout_webots_interface.cpp
+ * 
+ * Created on: Sep 26, 2019 23:19
+ * Description: 
+ * 
+ * Copyright (c) 2019 Ruixiang Du (rdu)
+ */
+
+#include "scout_webots_sim/scout_webots_interface.hpp"
+
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/point_cloud_conversion.h>
+#include <pcl_ros/transforms.h>
+
+#include <webots_ros/set_float.h>
+#include <webots_ros/get_float.h>
+#include <webots_ros/set_int.h>
+#include <webots_ros/set_bool.h>
+
+#include "scout_webots_sim/scout_sim_params.hpp"
+
+namespace westonrobot
+{
+ScoutWebotsInterface::ScoutWebotsInterface(ros::NodeHandle *nh, ScoutROSMessenger *msger, uint32_t time_step)
+    : nh_(nh), messenger_(msger), time_step_(time_step)
+{
+}
+
+void ScoutWebotsInterface::InitComponents(std::string controller_name)
+{
+    // reset controller name
+    robot_name_ = controller_name;
+
+    // init motors
+    for (int i = 0; i < 4; ++i)
+    {
+        // position
+        webots_ros::set_float set_position_srv;
+        ros::ServiceClient set_position_client = nh_->serviceClient<webots_ros::set_float>(robot_name_ + "/" + std::string(motor_names_[i]) +
+                                                                                           std::string("/set_position"));
+
+        set_position_srv.request.value = INFINITY;
+        if (set_position_client.call(set_position_srv) && set_position_srv.response.success)
+            ROS_INFO("Position set to INFINITY for motor %s.", motor_names_[i].c_str());
+        else
+            ROS_ERROR("Failed to call service set_position on motor %s.", motor_names_[i].c_str());
+
+        // speed
+        ros::ServiceClient set_velocity_client;
+        webots_ros::set_float set_velocity_srv;
+        set_velocity_client = nh_->serviceClient<webots_ros::set_float>(robot_name_ + "/" + std::string(motor_names_[i]) +
+                                                                        std::string("/set_velocity"));
+
+        set_velocity_srv.request.value = 0.0;
+        if (set_velocity_client.call(set_velocity_srv) && set_velocity_srv.response.success == 1)
+            ROS_INFO("Velocity set to 0.0 for motor %s.", motor_names_[i].c_str());
+        else
+            ROS_ERROR("Failed to call service set_velocity on motor %s.", motor_names_[i].c_str());
+    }
+}
+
+void ScoutWebotsInterface::UpdateSimState()
+{
+    // constants for calculation
+    constexpr double rotation_radius = std::hypot(ScoutSimParams::wheelbase / 2.0, ScoutSimParams::track / 2.0) * 2.0;
+    constexpr double rotation_theta = std::atan2(ScoutSimParams::wheelbase, ScoutSimParams::track);
+
+    // update robot state
+    double wheel_speeds[4];
+    for (int i = 0; i < 4; ++i)
+    {
+        webots_ros::get_float get_velocity_srv;
+        ros::ServiceClient get_velocity_client = nh_->serviceClient<webots_ros::get_float>(robot_name_ + "/" + std::string(motor_names_[i]) +
+                                                                                           std::string("/get_velocity"));
+
+        if (get_velocity_client.call(get_velocity_srv))
+        {
+            wheel_speeds[i] = get_velocity_srv.response.value;
+            ROS_INFO("Velocity set to 0.0 for motor %s.", motor_names_[i].c_str());
+        }
+        else
+            ROS_ERROR("Failed to call service set_velocity on motor %s.", motor_names_[i].c_str());
+    }
+    float left_speed = (wheel_speeds[1] + wheel_speeds[2]) / 2.0 * ScoutSimParams::wheel_radius;
+    float right_speed = (wheel_speeds[0] + wheel_speeds[3]) / 2.0 * ScoutSimParams::wheel_radius;
+    double linear_speed = (right_speed + left_speed) / 2.0;
+    double angular_speed = (right_speed - left_speed) * std::cos(rotation_theta) / rotation_radius;
+
+    messenger_->PublishSimStateToROS(linear_speed, angular_speed);
+
+    // send robot command
+    double linear, angular;
+    messenger_->GetCurrentMotionCmdForSim(linear, angular);
+
+    if (linear > ScoutSimParams::max_linear_speed)
+        linear = ScoutSimParams::max_linear_speed;
+    if (linear < -ScoutSimParams::max_linear_speed)
+        linear = -ScoutSimParams::max_linear_speed;
+
+    if (angular > ScoutSimParams::max_angular_speed)
+        angular = ScoutSimParams::max_angular_speed;
+    if (angular < -ScoutSimParams::max_angular_speed)
+        angular = -ScoutSimParams::max_angular_speed;
+
+    double vel_left_cmd = (linear - angular * rotation_radius / std::cos(rotation_theta)) / ScoutSimParams::wheel_radius;
+    double vel_right_cmd = (linear + angular * rotation_radius / std::cos(rotation_theta)) / ScoutSimParams::wheel_radius;
+
+    double wheel_cmds[4];
+    wheel_cmds[0] = vel_right_cmd;
+    wheel_cmds[1] = vel_left_cmd;
+    wheel_cmds[2] = vel_left_cmd;
+    wheel_cmds[3] = vel_right_cmd;
+    for (int i = 0; i < 4; ++i)
+    {
+        ros::ServiceClient set_velocity_client;
+        webots_ros::set_float set_velocity_srv;
+        set_velocity_client = nh_->serviceClient<webots_ros::set_float>(robot_name_ + "/" + std::string(motor_names_[i]) +
+                                                                        std::string("/set_velocity"));
+
+        set_velocity_srv.request.value = wheel_cmds[i];
+        if (set_velocity_client.call(set_velocity_srv) && set_velocity_srv.response.success == 1)
+            ROS_INFO("Velocity set to 0.0 for motor %s.", motor_names_[i].c_str());
+        else
+            ROS_ERROR("Failed to call service set_velocity on motor %s.", motor_names_[i].c_str());
+    }
+}
+
+} // namespace westonrobot
--- a/scout_webots_sim/src/scout_webots_node.cpp
+++ b/scout_webots_sim/src/scout_webots_node.cpp
@@ -0,0 +1,119 @@
+/* 
+ * scout_webots_node.cpp
+ * 
+ * Created on: Sep 26, 2019 23:03
+ * Description: 
+ * 
+ * Copyright (c) 2019 Ruixiang Du (rdu)
+ */
+
+#include <signal.h>
+
+#include <iostream>
+
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+
+#include <webots_ros/set_float.h>
+#include <webots_ros/set_int.h>
+
+#include "scout_webots_sim/scout_webots_interface.hpp"
+
+using namespace westonrobot;
+
+ros::ServiceClient timeStepClient;
+webots_ros::set_int timeStepSrv;
+
+static int controllerCount;
+static std::vector<std::string> controllerList;
+
+void quit(int sig)
+{
+    ROS_INFO("User stopped the 'agilex_scout' node.");
+    timeStepSrv.request.value = 0;
+    timeStepClient.call(timeStepSrv);
+    ros::shutdown();
+    exit(0);
+}
+
+// catch names of the controllers availables on ROS network
+void controllerNameCallback(const std_msgs::String::ConstPtr &name)
+{
+    controllerCount++;
+    controllerList.push_back(name->data);
+    ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str());
+}
+
+int main(int argc, char *argv[])
+{
+    ros::init(argc, argv, "scout_webots_node", ros::init_options::AnonymousName);
+    ros::NodeHandle nh, private_node("~");
+
+    ScoutROSMessenger messenger(&nh);
+    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<int>("sim_control_rate", messenger.sim_control_rate_, 50);
+    private_node.param<bool>("simulated_robot", messenger.simulated_robot_, true);
+    messenger.SetupSubscription();
+
+    const uint32_t time_step = 1000 / messenger.sim_control_rate_;
+    ScoutWebotsInterface scout_webots(&nh, &messenger, time_step);
+
+    signal(SIGINT, quit);
+
+    // subscribe to the topic model_name to get the list of availables controllers
+    std::string controllerName;
+    ros::Subscriber nameSub = nh.subscribe("model_name", 100, controllerNameCallback);
+    while (controllerCount == 0 || controllerCount < nameSub.getNumPublishers())
+    {
+        ros::spinOnce();
+        ros::spinOnce();
+        ros::spinOnce();
+    }
+    ros::spinOnce();
+
+    // if there is more than one controller available, it let the user choose
+    if (controllerCount == 1)
+        controllerName = controllerList[0];
+    else
+    {
+        int wantedController = 0;
+        std::cout << "Choose the # of the controller you want to use:\n";
+        std::cin >> wantedController;
+        if (1 <= wantedController && wantedController <= controllerCount)
+            controllerName = controllerList[wantedController - 1];
+        else
+        {
+            ROS_ERROR("Invalid number for controller choice.");
+            return 1;
+        }
+    }
+    ROS_INFO("Using controller: '%s'", controllerName.c_str());
+
+    // leave topic once it is not necessary anymore
+    nameSub.shutdown();
+
+    // init robot components
+    scout_webots.InitComponents(controllerName);
+
+    ROS_INFO("Entering ROS main loop...");
+
+    // main loop
+    timeStepClient = nh.serviceClient<webots_ros::set_int>(controllerName + "/robot/time_step");
+    timeStepSrv.request.value = time_step;
+    while (ros::ok())
+    {
+        if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success)
+        {
+            ROS_ERROR("Failed to call service time_step for next step.");
+            break;
+        }
+        scout_webots.UpdateSimState();
+        ros::spinOnce();
+    }
+    timeStepSrv.request.value = 0;
+    timeStepClient.call(timeStepSrv);
+
+    ros::shutdown();
+    return 0;
+}