Merge branch 'scout_gazebo' into melodic

.gitignore

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

NOTE.md

@@ -0,0 +1,15 @@
+```
+$ 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
+```
+
+Convert urdf to sdf
+
+```
+$ gz sdf -p scout_v2.urdf > scout_v2.sdf
+```

README.md

@@ -69,6 +69,8 @@ Two scripts inside the "scout_bringup/scripts" folder are provided for easy setu
 
     ```
     $ sudo apt install ros-melodic-teleop-twist-keyboard
+    $ sudo apt-get install ros-melodic-joint-state-publisher-gui
+    $ sudo apt install ros-melodic-ros-controllers
     ```
 
     Change ros-melodic-* in the command to ros-kinetic-* if you're using ROS Kinetic.

scout_base/CMakeLists.txt

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

scout_base/launch/scout_base_sim.launch

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

scout_base/src/scout_base_node.cpp

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

scout_base/src/scout_base_sim_node.cpp

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

scout_bringup/launch/scout_base_gazebo_sim.launch

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

scout_description/CHANGELOG.rst

@@ -0,0 +1,132 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package husky_description
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.4.2 (2019-12-11)
+------------------
+
+0.4.1 (2019-09-30)
+------------------
+
+0.4.0 (2019-08-01)
+------------------
+
+0.3.4 (2019-08-01)
+------------------
+
+0.3.3 (2019-04-18)
+------------------
+* Fixed bumper extensions, cleaned up collision meshes
+* Contributors: Dave Niewinski
+
+0.3.2 (2019-03-25)
+------------------
+* Added some additional frames on the top plates and an environment variable for diabling the user rails
+* Added env var to allow a 7cm forward bumper extension (`#92 <https://github.com/husky/husky/issues/92>`_)
+  * Added env var to allow for extendable front bumper
+  * Fix weird spacing
+  * Uploaded bumper extension meshes
+  * Allowed for different lengths of bumper extensions
+* Contributors: Dave Niewinski, Guy Stoppi
+
+0.3.1 (2018-08-02)
+------------------
+* Removed unnecessary dae objects and duplicate vertices
+* Contributors: Dave Niewinski
+
+0.3.0 (2018-04-11)
+------------------
+* Updated all package versions to 0.2.6.
+* Added a large top plate (used for waterproofing upgrade and UR5 upgrade) and an environment variable for controlling it HUSKY_LARGE_TOP_PLATE
+* changed Husky wheel radius, a Husky outdoor tire is 13 inchs (0.3302m)
+* [husky_description] Updated inertial parameters.
+* [husky_description] Fixed depreciated syntax.
+* Remove defunct email address
+* Updated maintainers.
+* Changes for xacro updates in kinetic.
+* Add interface definitions
+* Purge more UR; Implement urdf_extras
+* Update URDF for multirobot
+* Move packages into monorepo for kinetic; strip out ur packages
+* wheel.urdf.xacro: swap iyy, izz inertias
+  Fixes `#34 <https://github.com/husky/husky/issues/34>`_.
+* Contributors: Dave Niewinski, Martin Cote, Paul Bovbel, Steven Peters, Tony Baltovski
+
+0.2.7 (2015-12-31)
+------------------
+* Fixed indent.
+* Added Sick LMS1XX URDF.
+* Contributors: Tony Baltovski
+
+0.2.6 (2015-07-08)
+------------------
+* Adjust Kinect angle so it doesn't hit top plate
+* Contributors: Paul Bovbel
+
+0.2.5 (2015-04-16)
+------------------
+* Add standard mount for lms1xx
+* Contributors: Paul Bovbel
+
+0.2.4 (2015-04-13)
+------------------
+* Add argument to enable/disable top plate
+* Fix sensor arch name
+* Fix conflict with underlay
+  When using -z check, underlayed instances of husky_gazebo would override overlays.
+* Update top plate model
+* Contributors: Paul Bovbel
+
+0.2.3 (2015-04-08)
+------------------
+* Integrate husky_customization workflow
+* Disable all accessories by default
+* Contributors: Paul Bovbel
+
+0.2.2 (2015-03-23)
+------------------
+* Fix package urls
+* Contributors: Paul Bovbel
+
+0.2.1 (2015-03-23)
+------------------
+* Port stl to dae format, removing material/gazebo colours
+* Make base_footprint a child of base_link
+* Contributors: Paul Bovbel
+
+0.2.0 (2015-03-23)
+------------------
+* Add Kinect, UR5 peripherals
+* Contributors: Paul Bovbel, Devon Ash
+
+0.1.2 (2015-01-30)
+------------------
+* Update maintainers and description
+* Get rid of chassis_link, switch to base_footprint and base_link
+* Switch to NED orientation for UM6 standard package
+* Contributors: Paul Bovbel
+
+0.1.1 (2015-01-14)
+------------------
+* Remove multirobot changes, experiment later
+* Contributors: Paul Bovbel
+
+0.1.0 (2015-01-13)
+------------------
+* Major refactor for indigo release:
+  * base_link is now located on the ground plane, while chassis_link
+  * refactored joint names for consistency with Jackal and Grizzly for ros_control
+  * moved plugins requiring gazebo dependencies to husky_gazebo (imu, gps, lidar, ros_control)
+  * initial prefixing for multirobot
+* Contributors: Alex Bencz, James Servos, Mike Purvis, Paul Bovbel, Prasenjit Mukherjee, y22ma
+
+0.0.2 (2013-09-30)
+------------------
+* Renamed /models folder to /meshes to follow the convention of other gazebo simulation packages.
+* Changed the base.urdf.xacro to use base_footprint as the parent frame. For some reason, the new Gazebo paints all parts the same color as base_link when base_link is the parent.
+
+0.0.1 (2013-09-11)
+------------------
+* Move to model-only launchfile.
+* Catkinize package, add install targets.
+* husky_description moved up to repository root.

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}
+)

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>

scout_description/launch/scout_v2_stock.launch

@@ -0,0 +1,9 @@
+
+<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) 
+        urdf_extras:=$(arg urdf_extras)" />
+</launch>

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>

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>

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

scout_description/rviz/navigation.rviz

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

scout_description/urdf/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>

scout_description/urdf/scout_v2.gazebo

@@ -0,0 +1,74 @@
+<?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="0.9"/>
+        <kp value="10000000.0" />
+        <kd value="1.0" />
+        <fdir1 value="1 0 0"/>
+        <minDepth value="0.001" />
+        <maxContacts value="64"/>
+    </gazebo>
+
+    <!-- <gazebo>
+        <plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
+            <updateRate>100.0</updateRate>
+            <robotNamespace></robotNamespace>
+            <leftFrontJoint>front_left_wheel</leftFrontJoint>
+            <rightFrontJoint>front_right_wheel</rightFrontJoint>
+            <leftRearJoint>rear_left_wheel</leftRearJoint>
+            <rightRearJoint>rear_right_wheel</rightRearJoint>
+            <wheelSeparation>4</wheelSeparation>
+            <wheelDiameter>0.32918</wheelDiameter>
+            <robotBaseFrame>base_link</robotBaseFrame>
+            <torque>1000</torque>
+            <commandTopic>cmd_vel</commandTopic>
+            <broadcastTF>true</broadcastTF>
+            <odometryTopic>odom</odometryTopic>
+            <odometryFrame>odom</odometryFrame>
+            <covariance_x>0.000100</covariance_x>
+            <covariance_y>0.000100</covariance_y>
+            <covariance_yaw>0.010000</covariance_yaw>
+        </plugin>
+    </gazebo> -->
+
+    <!-- <gazebo>
+        <plugin name="object_controller" filename="libgazebo_ros_planar_move.so">
+            <commandTopic>cmd_vel</commandTopic>
+            <odometryTopic>odom</odometryTopic>
+            <odometryFrame>odom</odometryFrame>
+            <odometryRate>20.0</odometryRate>
+            <robotBaseFrame>base_footprint</robotBaseFrame>
+        </plugin>
+    </gazebo> -->
+</robot>

scout_description/urdf/scout_v2.xacro

@@ -0,0 +1,101 @@
+<?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: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="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>
+
+    <!-- Additional definitions -->
+    <xacro:include filename="$(arg urdf_extras)" />
+
+    <!-- Gazebo definitions  -->
+    <xacro:include filename="$(find scout_description)/urdf/scout_v2.gazebo" />
+</robot>

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>

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>

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>

scout_gazebo_sim/CHANGELOG.md

@@ -0,0 +1,5 @@
+## Changelog
+
+0.0.1 (2020-03-25)
+------------------
+* Initial release for Melodic.

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}
+)

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

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 wescore {
+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 wescore
+
+#endif /* SCOUT_SKID_STEER_HPP */

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.launch"></include>
+        
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find scout_description)/rviz/navigation.rviz" />
+</launch>

scout_gazebo_sim/launch/scout_playpen.launch

@@ -0,0 +1,43 @@
+<?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.launch"></include>
+        
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find scout_description)/rviz/model_display.rviz" />
+</launch>

scout_gazebo_sim/launch/spawn_scout_v2.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> 

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>

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 wescore {
+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 wescore

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 wescore;
+
+int main(int argc, char **argv) {
+  // setup ROS node
+  ros::init(argc, argv, "scout_odom");
+  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;
+}

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>