From 1399f005354e062951548ebf6248533573395b01 Mon Sep 17 00:00:00 2001
From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com>
Date: Wed, 10 Feb 2021 23:14:02 -0500
Subject: [PATCH] Add thermal camera to Marble HD2 (#773)

* starting to add thermal camera to marble_hd2_sensor_config_1

Signed-off-by: Ashton Larkin <ashton@openrobotics.org>

* add optical frame publisher, camera info topic, and more thermal camera sensor parameters

Signed-off-by: Ashton Larkin <ashton@openrobotics.org>

* fix topic mapping (#776)

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* add thermal camera visual, fix location and textures

Signed-off-by: Ashton Larkin <ashton@openrobotics.org>

* update hfov assuming degrees in spec

Signed-off-by: Ashton Larkin <ashton@openrobotics.org>

* break out thermal camera addition to HD2 configs 3 and 4

Signed-off-by: Ashton Larkin <ashton@openrobotics.org>

* set thermal default to 0 for HD2 vehicle_topics.launch

Signed-off-by: Ashton Larkin <ashton@openrobotics.org>

* New config with thermal camera: CERBERUS_M100_SENSOR_CONFIG_2 (#777)

* adding thermal camera to cerberus m100

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* fix bridge

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* update thermal camera spec

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* new config_2

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* remove config_2

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* reuse config_1 files

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

Co-authored-by: Ian Chen <ichen@osrfoundation.org>
---
 .../launch/vehicle_topics.launch              |  32 ++-
 .../CMakeLists.txt                            |   8 +
 .../launch/spawner.rb                         | 214 ++++++++++++++++++
 .../cerberus_m100_sensor_config_2/package.xml |  23 ++
 .../launch/vehicle_topics.launch              |  29 +++
 .../marble_hd2_sensor_config_1/model.sdf      |  36 +--
 .../marble_hd2_sensor_config_2/package.xml    |   2 +-
 .../marble_hd2_sensor_config_3/CMakeLists.txt |   8 +
 .../launch/spawner.rb                         | 149 ++++++++++++
 .../marble_hd2_sensor_config_3/package.xml    |  30 +++
 .../marble_hd2_sensor_config_4/CMakeLists.txt |   8 +
 .../launch/spawner.rb                         | 149 ++++++++++++
 .../marble_hd2_sensor_config_4/package.xml    |  31 +++
 13 files changed, 699 insertions(+), 20 deletions(-)
 create mode 100644 submitted_models/cerberus_m100_sensor_config_2/CMakeLists.txt
 create mode 100755 submitted_models/cerberus_m100_sensor_config_2/launch/spawner.rb
 create mode 100644 submitted_models/cerberus_m100_sensor_config_2/package.xml
 create mode 100644 submitted_models/marble_hd2_sensor_config_3/CMakeLists.txt
 create mode 100644 submitted_models/marble_hd2_sensor_config_3/launch/spawner.rb
 create mode 100644 submitted_models/marble_hd2_sensor_config_3/package.xml
 create mode 100644 submitted_models/marble_hd2_sensor_config_4/CMakeLists.txt
 create mode 100644 submitted_models/marble_hd2_sensor_config_4/launch/spawner.rb
 create mode 100644 submitted_models/marble_hd2_sensor_config_4/package.xml

diff --git a/submitted_models/cerberus_m100_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/cerberus_m100_sensor_config_1/launch/vehicle_topics.launch
index 3ec08126..048dd4da 100755
--- a/submitted_models/cerberus_m100_sensor_config_1/launch/vehicle_topics.launch
+++ b/submitted_models/cerberus_m100_sensor_config_1/launch/vehicle_topics.launch
@@ -11,8 +11,9 @@
   <arg name="top_scan" default="0"/>
   <arg name="bottom_scan" default="0"/>
   <arg name="lidar_3d" default="1"/>
+  <arg name="thermal" default="0"/>
   <group ns="$(arg name)">
-    <!--Create multipe bridges so that it can run in parallel-->
+    <!--Create multiple bridges so that it can run in parallel-->
     <node
       pkg="ros_ign_bridge"
       type="parameter_bridge"
@@ -71,6 +72,35 @@
       <remap from="/model/$(arg name)/gas_detected" to="gas_detected"/>
     </node>
 
+    <group if="$(arg thermal)">
+      <!-- Thermal Camera -->
+      <!-- Note camera_info topic generated by ign-gazebo incorrectly removes
+           sensor name in topic string -->
+      <node
+        pkg="ros_ign_bridge"
+        type="parameter_bridge"
+        name="ros_ign_bridge_thermal_camera"
+        args="$(arg sensor_prefix)/camera_info@sensor_msgs/CameraInfo[ignition.msgs.CameraInfo">
+        <remap from="$(arg sensor_prefix)/camera_info" to="thermal_camera/camera_info"/>
+      </node>
+      <node
+        pkg="ros_ign_image"
+        type="image_bridge"
+        name="ros_ign_image_thermal_camera"
+        args="$(arg sensor_prefix)/thermal_camera">
+        <remap from="$(arg sensor_prefix)/thermal_camera" to="thermal_camera/image_raw"/>
+      </node>
+      <node
+        pkg="subt_ros"
+        type="optical_frame_publisher"
+        name="optical_frame_publisher_image_thermal_camera">
+        <remap from="input/image" to="thermal_camera/image_raw" />
+        <remap from="output/image" to="thermal_camera/optical/image_raw" />
+        <remap from="input/camera_info" to="thermal_camera/camera_info" />
+        <remap from="output/camera_info" to="thermal_camera/optical/camera_info" />
+      </node>
+    </group>
+
     <!-- stereo -->
     <group if="$(arg stereo_cam)">
       <node
diff --git a/submitted_models/cerberus_m100_sensor_config_2/CMakeLists.txt b/submitted_models/cerberus_m100_sensor_config_2/CMakeLists.txt
new file mode 100644
index 00000000..a42e8d8a
--- /dev/null
+++ b/submitted_models/cerberus_m100_sensor_config_2/CMakeLists.txt
@@ -0,0 +1,8 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(cerberus_m100_sensor_config_2)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/submitted_models/cerberus_m100_sensor_config_2/launch/spawner.rb b/submitted_models/cerberus_m100_sensor_config_2/launch/spawner.rb
new file mode 100755
index 00000000..b1bb4b70
--- /dev/null
+++ b/submitted_models/cerberus_m100_sensor_config_2/launch/spawner.rb
@@ -0,0 +1,214 @@
+def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw)
+  <<-HEREDOC
+    _config=8
+    uav=1
+    laserScan=0
+    stereoCam=0
+    rgbdCam=0
+    if _config == "2"
+      rgbdCam=1
+    end
+    if _config == "3" or _config == "4"
+      laserScan=1
+    end
+    if _config == "5"
+      stereoCam=1
+    if _config == "6"
+      laserScan=1
+      topScan=1
+      bottomScan=1
+      rgbdCam=1
+    end
+    if _config == "8"
+      laserScan=1
+      topScan=1
+      bottomScan=1
+      rgbdCam=1
+    end
+    <spawn name="${_name}">
+      <name>#{_name}</name>
+      <allow_renaming>false</allow_renaming>
+      <pose>#{_x} #{_y} #{_z + 0.187499} #{_roll} #{_pitch} #{_yaw}</pose>
+      <world>#{_worldName}</world>
+      <is_performer>true</is_performer>
+      <sdf version='1.6'>
+      <include>
+        <name>#{_name}</name>
+        <uri>#{_modelURI}</uri>
+        <!-- Publish robot state information -->
+        <plugin filename="libignition-gazebo-pose-publisher-system.so"
+          name="ignition::gazebo::systems::PosePublisher">
+          <publish_link_pose>true</publish_link_pose>
+          <publish_sensor_pose>true</publish_sensor_pose>
+          <publish_collision_pose>false</publish_collision_pose>
+          <publish_visual_pose>false</publish_visual_pose>
+          <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>
+          <use_pose_vector_msg>true</use_pose_vector_msg>
+          <static_publisher>true</static_publisher>
+          <static_update_frequency>1</static_update_frequency>
+        </plugin>
+        <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
+          name="ignition::gazebo::systems::MulticopterMotorModel">
+          <robotNamespace>model/#{_name}</robotNamespace>
+          <jointName>rotor_0_joint</jointName>
+          <linkName>rotor_0</linkName>
+          <turningDirection>ccw</turningDirection>
+          <timeConstantUp>0.0182</timeConstantUp>
+          <timeConstantDown>0.0182</timeConstantDown>
+          <maxRotVelocity>1000.0</maxRotVelocity>
+          <motorConstant>0.02246</motorConstant>
+          <momentConstant>0.016754</momentConstant>
+          <commandSubTopic>command/motor_speed</commandSubTopic>
+          <motorNumber>0</motorNumber>
+          <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
+          <rollingMomentCoefficient>0</rollingMomentCoefficient>
+          <motorSpeedPubTopic>motor_speed/0</motorSpeedPubTopic>
+          <rotorVelocitySlowdownSim>0.5</rotorVelocitySlowdownSim>
+          <motorType>velocity</motorType>
+        </plugin>
+        <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
+          name="ignition::gazebo::systems::MulticopterMotorModel">
+          <robotNamespace>model/#{_name}</robotNamespace>
+          <jointName>rotor_1_joint</jointName>
+          <linkName>rotor_1</linkName>
+          <turningDirection>ccw</turningDirection>
+          <timeConstantUp>0.0182</timeConstantUp>
+          <timeConstantDown>0.0182</timeConstantDown>
+          <maxRotVelocity>1000.0</maxRotVelocity>
+          <motorConstant>0.02246</motorConstant>
+          <momentConstant>0.016754</momentConstant>
+          <commandSubTopic>command/motor_speed</commandSubTopic>
+          <motorNumber>1</motorNumber>
+          <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
+          <rollingMomentCoefficient>0</rollingMomentCoefficient>
+          <motorSpeedPubTopic>motor_speed/1</motorSpeedPubTopic>
+          <rotorVelocitySlowdownSim>0.5</rotorVelocitySlowdownSim>
+          <motorType>velocity</motorType>
+        </plugin>
+        <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
+          name="ignition::gazebo::systems::MulticopterMotorModel">
+          <robotNamespace>model/#{_name}</robotNamespace>
+          <jointName>rotor_2_joint</jointName>
+          <linkName>rotor_2</linkName>
+          <turningDirection>cw</turningDirection>
+          <timeConstantUp>0.0182</timeConstantUp>
+          <timeConstantDown>0.0182</timeConstantDown>
+          <maxRotVelocity>1000.0</maxRotVelocity>
+          <motorConstant>0.02246</motorConstant>
+          <momentConstant>0.016754</momentConstant>
+          <commandSubTopic>command/motor_speed</commandSubTopic>
+          <motorNumber>2</motorNumber>
+          <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
+          <rollingMomentCoefficient>0</rollingMomentCoefficient>
+          <motorSpeedPubTopic>motor_speed/2</motorSpeedPubTopic>
+          <rotorVelocitySlowdownSim>0.5</rotorVelocitySlowdownSim>
+          <motorType>velocity</motorType>
+        </plugin>
+        <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
+          name="ignition::gazebo::systems::MulticopterMotorModel">
+          <robotNamespace>model/#{_name}</robotNamespace>
+          <jointName>rotor_3_joint</jointName>
+          <linkName>rotor_3</linkName>
+          <turningDirection>cw</turningDirection>
+          <timeConstantUp>0.0182</timeConstantUp>
+          <timeConstantDown>0.0182</timeConstantDown>
+          <maxRotVelocity>1000.0</maxRotVelocity>
+          <motorConstant>0.02246</motorConstant>
+          <momentConstant>0.016754</momentConstant>
+          <commandSubTopic>command/motor_speed</commandSubTopic>
+          <motorNumber>3</motorNumber>
+          <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
+          <rollingMomentCoefficient>0</rollingMomentCoefficient>
+          <motorSpeedPubTopic>motor_speed/3</motorSpeedPubTopic>
+          <rotorVelocitySlowdownSim>0.5</rotorVelocitySlowdownSim>
+          <motorType>velocity</motorType>
+        </plugin>
+        <plugin
+          filename="libignition-gazebo-multicopter-control-system.so"
+          name="ignition::gazebo::systems::MulticopterVelocityControl">
+          <robotNamespace>model/#{_name}</robotNamespace>
+          <commandSubTopic>cmd_vel</commandSubTopic>
+          <motorControlPubTopic>command/motor_speed</motorControlPubTopic>
+          <enableSubTopic>velocity_controller/enable</enableSubTopic>
+          <comLinkName>base_link</comLinkName>
+          <velocityGain>6 6 10</velocityGain>
+          <attitudeGain>4 4 2</attitudeGain>
+          <angularRateGain>0.7 0.7 0.7</angularRateGain>
+          <maximumLinearAcceleration>4 4 5</maximumLinearAcceleration>
+          <maximumLinearVelocity>3 3 1.8</maximumLinearVelocity>
+          <maximumAngularVelocity>3 3 1.9</maximumAngularVelocity>
+          <linearVelocityNoiseMean>0 0 0</linearVelocityNoiseMean>
+          <!-- linearVelocityNoiseStdDev based on error values reported in the paper Shen et. al., -->
+          <!-- Vision-Based State Estimation and Trajectory Control Towards High-Speed Flight with a Quadrotor -->
+          <!-- http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.490.7958&rep=rep1&type=pdf -->
+          <linearVelocityNoiseStdDev>0.1105 0.1261 0.0947</linearVelocityNoiseStdDev>
+          <angularVelocityNoiseMean>0 0 0</angularVelocityNoiseMean>
+          <!-- angularVelocityNoiseStdDev values based on ADIS16448's Rate Noise Density with a sample  -->
+          <!-- time of 0.004 ms. -->
+          <angularVelocityNoiseStdDev>0.004 0.004 0.004</angularVelocityNoiseStdDev>
+
+          <rotorConfiguration>
+            <rotor>
+              <jointName>rotor_0_joint</jointName>
+              <forceConstant>0.02246</forceConstant>
+              <momentConstant>1.6754e-2</momentConstant>
+              <direction>1</direction>
+            </rotor>
+            <rotor>
+              <jointName>rotor_1_joint</jointName>
+              <forceConstant>0.02246</forceConstant>
+              <momentConstant>1.6754e-2</momentConstant>
+              <direction>1</direction>
+            </rotor>
+            <rotor>
+              <jointName>rotor_2_joint</jointName>
+              <forceConstant>0.02246</forceConstant>
+              <momentConstant>1.6754e-2</momentConstant>
+              <direction>-1</direction>
+            </rotor>
+            <rotor>
+              <jointName>rotor_3_joint</jointName>
+              <forceConstant>0.02246</forceConstant>
+              <momentConstant>1.6754e-2</momentConstant>
+              <direction>-1</direction>
+            </rotor>
+          </rotorConfiguration>
+        </plugin>
+        <!-- Battery plugin -->
+        <plugin filename="libignition-gazebo-linearbatteryplugin-system.so"
+          name="ignition::gazebo::systems::LinearBatteryPlugin">
+          <battery_name>linear_battery</battery_name>
+          <voltage>22.2</voltage>
+          <open_circuit_voltage_constant_coef>22.2</open_circuit_voltage_constant_coef>
+          <open_circuit_voltage_linear_coef>-3.1424</open_circuit_voltage_linear_coef>
+          <initial_charge>45.0</initial_charge>
+          <capacity>45.0</capacity>
+          <resistance>0.061523</resistance>
+          <smooth_current_tau>1.9499</smooth_current_tau>
+          <power_load>70</power_load>
+          <start_on_motion>true</start_on_motion>
+        </plugin>
+        <!-- Gas Detector plugin -->
+        <plugin filename="libGasEmitterDetectorPlugin.so"
+          name="subt::GasDetector">
+          <topic>/model/#{_name}/gas_detected</topic>
+          <update_rate>10</update_rate>
+          <type>gas</type>
+        </plugin>
+        </include>
+      </sdf>
+    </spawn>
+
+  HEREDOC
+end
+
+def rosExecutables(_name, _worldName)
+  <<-HEREDOC
+    <executable name='m100_description'>
+      <command>roslaunch --wait cerberus_m100_sensor_config_1 description.launch world_name:=#{_worldName} name:=#{_name}</command>
+    </executable>
+    <executable name='m100_ros_ign_bridge'>
+      <command>roslaunch --wait cerberus_m100_sensor_config_1 vehicle_topics.launch world_name:=#{_worldName} name:=#{_name} uav:=1 lidar_3d:=1 laser_scan:=0 top_scan:=0 bottom_scan:=0 stereo_cam:=0 thermal:=1</command>
+  </executable>
+  HEREDOC
+end
diff --git a/submitted_models/cerberus_m100_sensor_config_2/package.xml b/submitted_models/cerberus_m100_sensor_config_2/package.xml
new file mode 100644
index 00000000..6870575b
--- /dev/null
+++ b/submitted_models/cerberus_m100_sensor_config_2/package.xml
@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>cerberus_m100_sensor_config_2</name>
+  <version>0.1.0</version>
+  <description>
+    The cerberus_m100_sensor_config_2 package is derived from OSRF's X4 vehicle model.
+    This sensor configuration has a 3D lidar (copied from the X1 3D medium range lidar)
+    and a thermal camera
+  </description>
+
+  <author email="mihirk284@gmail.com">Mihir Kulkarni</author>
+  <author email="pdepetris@nevada.unr.edu">Paolo De Petris</author>
+  <maintainer email="mihirk284@gmail.com">Mihir Kulkarni</maintainer>
+  <maintainer email="pdepetris@nevada.unr.edu">Paolo De Petris</maintainer>
+
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>roslaunch</buildtool_depend>
+  <buildtool_depend>urdf</buildtool_depend>
+  <buildtool_depend>xacro</buildtool_depend>
+
+</package>
diff --git a/submitted_models/marble_hd2_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/marble_hd2_sensor_config_1/launch/vehicle_topics.launch
index 4a263b17..29ec9375 100644
--- a/submitted_models/marble_hd2_sensor_config_1/launch/vehicle_topics.launch
+++ b/submitted_models/marble_hd2_sensor_config_1/launch/vehicle_topics.launch
@@ -7,6 +7,7 @@
   <arg name="sensor_prefix" value="/world/$(arg world_name)/model/$(arg name)/link/$(arg link_name)/sensor"/>
   <arg name="gimbal_sensor_prefix" value="/world/$(arg world_name)/model/$(arg name)/link/tilt_gimbal_link/sensor"/>
   <arg name="breadcrumbs" default="0"/>
+  <arg name="thermal" default="0"/>
   <group ns="$(arg name)">
     <!--Create multipe bridges so that it can run in parallel-->
     <node
@@ -256,6 +257,34 @@
       <remap from="/model/$(arg name)/gas_detected" to="gas_detected"/>
     </node>
 
+    <group if="$(arg thermal)">
+      <!-- Note camera_info topic generated by ign-gazebo incorrectly removes
+           sensor name in topic string -->
+      <node
+        pkg="ros_ign_bridge"
+        type="parameter_bridge"
+        name="ros_ign_bridge_thermal_camera"
+        args="$(arg sensor_prefix)/camera_info@sensor_msgs/CameraInfo[ignition.msgs.CameraInfo">
+        <remap from="$(arg sensor_prefix)/camera_info" to="thermal_camera/camera_info"/>
+      </node>
+      <node
+        pkg="ros_ign_image"
+        type="image_bridge"
+        name="ros_ign_image_thermal_camera"
+        args="$(arg sensor_prefix)/thermal_camera">
+        <remap from="$(arg sensor_prefix)/thermal_camera" to="thermal_camera/image_raw"/>
+      </node>
+      <node
+        pkg="subt_ros"
+        type="optical_frame_publisher"
+        name="optical_frame_publisher_image_thermal_camera">
+        <remap from="input/image" to="thermal_camera/image_raw" />
+        <remap from="output/image" to="thermal_camera/optical/image_raw" />
+        <remap from="input/camera_info" to="thermal_camera/camera_info" />
+        <remap from="output/camera_info" to="thermal_camera/optical/camera_info" />
+      </node>
+    </group>
+
     <group if="$(arg breadcrumbs)">
       <node
         pkg="ros_ign_bridge"
diff --git a/submitted_models/marble_hd2_sensor_config_1/model.sdf b/submitted_models/marble_hd2_sensor_config_1/model.sdf
index 83647ddc..4cd0928d 100644
--- a/submitted_models/marble_hd2_sensor_config_1/model.sdf
+++ b/submitted_models/marble_hd2_sensor_config_1/model.sdf
@@ -142,7 +142,7 @@
         <pose>0 0 -0.135 0 0 1.57</pose>
         <geometry>
           <mesh>
-            <uri>model://MARBLE_HD2_SENSOR_CONFIG_1/meshes/hd2.dae</uri>
+            <uri>meshes/hd2.dae</uri>
             <submesh>
               <name>Body</name>
               <center>false</center>
@@ -154,15 +154,15 @@
           <specular>1.0 1.0 1.0</specular>
           <pbr>
             <metal>
-              <albedo_map>model://MARBLE_HD2_SENSOR_CONFIG_1/materials/textures/HD2_Albedo.png</albedo_map>
-              <metalness_map>model://MARBLE_HD2_SENSOR_CONFIG_1/materials/textures/HD2_Metalness.png</metalness_map>
-              <roughness_map>model://MARBLE_HD2_SENSOR_CONFIG_1/materials/textures/HD2_Roughness.png</roughness_map>
+              <albedo_map>materials/textures/HD2_Albedo.png</albedo_map>
+              <metalness_map>materials/textures/HD2_Metalness.png</metalness_map>
+              <roughness_map>materials/textures/HD2_Roughness.png</roughness_map>
             </metal>
           </pbr>
           <!-- fallback to script if no PBR support-->
           <script>
-            <uri>model://MARBLE_HD2_SENSOR_CONFIG_1/materials/scripts/</uri>
-            <uri>model://MARBLE_HD2_SENSOR_CONFIG_1/materials/textures/</uri>
+            <uri>materials/scripts/</uri>
+            <uri>materials/textures/</uri>
             <name>HD/HD2_Diffuse</name>
           </script>
         </material>
@@ -895,7 +895,7 @@
                       <pose>0 0 0 0 0 1.57</pose>
                       <geometry>
                         <mesh>
-                          <uri>model://MARBLE_HD2_SENSOR_CONFIG_1/meshes/hd2.dae</uri>
+                          <uri>meshes/hd2.dae</uri>
                           <submesh>
                             <name>CameraPivot</name>
                             <center>true</center>
@@ -907,14 +907,14 @@
                         <specular>1.0 1.0 1.0</specular>
                         <pbr>
                           <metal>
-                            <albedo_map>model://MARBLE_HD2_SENSOR_CONFIG_1/materials/textures/HD2_Albedo.png</albedo_map>
-                            <metalness_map>model://MARBLE_HD2_SENSOR_CONFIG_1/materials/textures/HD2_Metalness.png</metalness_map>
-                            <roughness_map>model://MARBLE_HD2_SENSOR_CONFIG_1/materials/textures/HD2_Roughness.png</roughness_map>
+                            <albedo_map>materials/textures/HD2_Albedo.png</albedo_map>
+                            <metalness_map>materials/textures/HD2_Metalness.png</metalness_map>
+                            <roughness_map>materials/textures/HD2_Roughness.png</roughness_map>
                           </metal>
                         </pbr>
                         <script>
-                          <uri>model://MARBLE_HD2_SENSOR_CONFIG_1/materials/scripts/</uri>
-                          <uri>model://MARBLE_HD2_SENSOR_CONFIG_1/materials/textures/</uri>
+                          <uri>materials/scripts/</uri>
+                          <uri>materials/textures/</uri>
                           <name>HD2/HD2_Diffuse</name>
                         </script>
                       </material>
@@ -957,7 +957,7 @@
                       <pose>0.005 0.015 0.038 0 0 -1.570796</pose>
                       <geometry>
                         <mesh>
-                          <uri>model://MARBLE_HD2_SENSOR_CONFIG_1/meshes/hd2.dae</uri>
+                          <uri>meshes/hd2.dae</uri>
                           <submesh>
                             <name>CameraTilt</name>
                             <center>true</center>
@@ -969,15 +969,15 @@
                         <specular>1.0 1.0 1.0</specular>
                         <pbr>
                           <metal>
-                            <albedo_map>model://MARBLE_HD2_SENSOR_CONFIG_1/materials/textures/HD2_Albedo.png</albedo_map>
-                            <metalness_map>model://MARBLE_HD2_SENSOR_CONFIG_1/materials/textures/HD2_Metalness.png</metalness_map>
-                            <roughness_map>model://MARBLE_HD2_SENSOR_CONFIG_1/materials/textures/HD2_Roughness.png</roughness_map>
+                            <albedo_map>materials/textures/HD2_Albedo.png</albedo_map>
+                            <metalness_map>materials/textures/HD2_Metalness.png</metalness_map>
+                            <roughness_map>materials/textures/HD2_Roughness.png</roughness_map>
                           </metal>
                         </pbr>
                         <!-- fallback to script if no PBR support-->
                         <script>
-                          <uri>model://MARBLE_HD2_SENSOR_CONFIG_1/materials/scripts/</uri>
-                          <uri>model://MARBLE_HD2_SENSOR_CONFIG_1/materials/textures/</uri>
+                          <uri>materials/scripts/</uri>
+                          <uri>materials/textures/</uri>
                           <name>HD/HD2_Diffuse</name>
                         </script>
                       </material>
diff --git a/submitted_models/marble_hd2_sensor_config_2/package.xml b/submitted_models/marble_hd2_sensor_config_2/package.xml
index 179ebd2a..4faf16d7 100644
--- a/submitted_models/marble_hd2_sensor_config_2/package.xml
+++ b/submitted_models/marble_hd2_sensor_config_2/package.xml
@@ -2,7 +2,7 @@
 <package format="2">
   <name>marble_hd2_sensor_config_2</name>
   <version>0.0.1</version>
-  <description>The marble_hd2_sensor_config_1 track vehicle package
+  <description>The marble_hd2_sensor_config_2 track vehicle package
     - Has two rgbd camera Realsense D435
     - An RP Lidar S1
     - OS1-64 3D Laser C
diff --git a/submitted_models/marble_hd2_sensor_config_3/CMakeLists.txt b/submitted_models/marble_hd2_sensor_config_3/CMakeLists.txt
new file mode 100644
index 00000000..c0482752
--- /dev/null
+++ b/submitted_models/marble_hd2_sensor_config_3/CMakeLists.txt
@@ -0,0 +1,8 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(marble_hd2_sensor_config_3)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/submitted_models/marble_hd2_sensor_config_3/launch/spawner.rb b/submitted_models/marble_hd2_sensor_config_3/launch/spawner.rb
new file mode 100644
index 00000000..9534405f
--- /dev/null
+++ b/submitted_models/marble_hd2_sensor_config_3/launch/spawner.rb
@@ -0,0 +1,149 @@
+def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw)
+  <<-HEREDOC
+  <spawn name='#{_name}'>
+    <name>#{_name}</name>
+    <allow_renaming>false</allow_renaming>
+    <pose>#{_x} #{_y} #{_z + 0.2} #{_roll} #{_pitch} #{_yaw}</pose>
+    <world>#{_worldName}</world>
+    <is_performer>true</is_performer>
+    <sdf version='1.6'>
+    <include>
+      <name>#{_name}</name>
+      <uri>#{_modelURI}</uri>
+      <!-- Diff drive -->
+      <plugin filename="libignition-gazebo-diff-drive-system.so"
+              name="ignition::gazebo::systems::DiffDrive">
+        <left_joint>front_left_wheel_joint</left_joint>
+        <left_joint>front_middle_left_wheel_joint</left_joint>
+        <left_joint>rear_middle_left_wheel_joint</left_joint>
+        <left_joint>rear_left_wheel_joint</left_joint>
+        <right_joint>front_right_wheel_joint</right_joint>
+        <right_joint>front_middle_right_wheel_joint</right_joint>
+        <right_joint>rear_middle_right_wheel_joint</right_joint>
+        <right_joint>rear_right_wheel_joint</right_joint>
+        <wheel_separation>#{0.525}</wheel_separation>
+        <wheel_radius>0.129</wheel_radius>
+        <topic>/model/#{_name}/cmd_vel_relay</topic>
+        <min_velocity>-1</min_velocity>
+        <max_velocity>1</max_velocity>
+        <min_acceleration>-3</min_acceleration>
+        <max_acceleration>3</max_acceleration>
+      </plugin>
+      <!-- Publish robot state information -->
+      <plugin filename="libignition-gazebo-pose-publisher-system.so"
+        name="ignition::gazebo::systems::PosePublisher">
+        <publish_link_pose>true</publish_link_pose>
+        <publish_sensor_pose>true</publish_sensor_pose>
+        <publish_collision_pose>false</publish_collision_pose>
+        <publish_visual_pose>false</publish_visual_pose>
+        <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>
+        <use_pose_vector_msg>true</use_pose_vector_msg>
+        <static_publisher>true</static_publisher>
+        <static_update_frequency>1</static_update_frequency>
+      </plugin>
+      <!-- Battery plugin -->
+      <plugin filename="libignition-gazebo-linearbatteryplugin-system.so"
+        name="ignition::gazebo::systems::LinearBatteryPlugin">
+        <battery_name>linear_battery</battery_name>
+        <voltage>12.694</voltage>
+        <open_circuit_voltage_constant_coef>12.694</open_circuit_voltage_constant_coef>
+        <open_circuit_voltage_linear_coef>-3.1424</open_circuit_voltage_linear_coef>
+        <initial_charge>78.4</initial_charge>
+        <capacity>78.4</capacity>
+        <resistance>0.061523</resistance>
+        <smooth_current_tau>1.9499</smooth_current_tau>
+        <power_load>9.9</power_load>
+        <start_on_motion>true</start_on_motion>
+      </plugin>
+      <!-- Gas Sensor plugin -->
+      <plugin filename="libGasEmitterDetectorPlugin.so"
+        name="subt::GasDetector">
+        <topic>/model/#{_name}/gas_detected</topic>
+        <update_rate>10</update_rate>
+        <type>gas</type>
+      </plugin>
+      <plugin filename="libignition-gazebo-breadcrumbs-system.so"
+            name="ignition::gazebo::systems::Breadcrumbs">
+        <topic>/model/#{_name}/breadcrumb/deploy</topic>
+        <max_deployments>12</max_deployments>
+        <disable_physics_time>3.0</disable_physics_time>
+        <topic_statistics>true</topic_statistics>
+        <breadcrumb>
+          <sdf version="1.6">
+            <model name="#{_name}__breadcrumb__">
+              <pose>-0.45 0 0 0 0 0</pose>
+              <include>
+                <uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Breadcrumb Node</uri>
+             </include>
+           </model>
+         </sdf>
+        </breadcrumb>
+      </plugin>
+      <!-- Wheel slip -->
+      <plugin filename="libignition-gazebo-wheel-slip-system.so"
+        name="ignition::gazebo::systems::WheelSlip">
+        <wheel link_name="front_left_wheel_link">
+          <slip_compliance_lateral>0.086</slip_compliance_lateral>
+          <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
+          <wheel_normal_force>41.47103925</wheel_normal_force>
+          <wheel_radius>0.129</wheel_radius>
+        </wheel>
+        <wheel link_name="front_middle_left_wheel_link">
+          <slip_compliance_lateral>0.086</slip_compliance_lateral>
+          <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
+          <wheel_normal_force>41.47103925</wheel_normal_force>
+          <wheel_radius>0.129</wheel_radius>
+        </wheel>
+        <wheel link_name="rear_middle_left_wheel_link">
+          <slip_compliance_lateral>0.086</slip_compliance_lateral>
+          <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
+          <wheel_normal_force>41.47103925</wheel_normal_force>
+          <wheel_radius>0.129</wheel_radius>
+        </wheel>
+        <wheel link_name="rear_left_wheel_link">
+          <slip_compliance_lateral>0.086</slip_compliance_lateral>
+          <slip_compliance_longitudinal>0.25</slip_compliance_longitudinal>
+          <wheel_normal_force>41.47103925</wheel_normal_force>
+          <wheel_radius>0.129</wheel_radius>
+        </wheel>
+        <wheel link_name="front_right_wheel_link">
+          <slip_compliance_lateral>0.086</slip_compliance_lateral>
+          <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
+          <wheel_normal_force>41.47103925</wheel_normal_force>
+          <wheel_radius>0.129</wheel_radius>
+        </wheel>
+        <wheel link_name="front_middle_right_wheel_link">
+          <slip_compliance_lateral>0.086</slip_compliance_lateral>
+          <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
+          <wheel_normal_force>41.47103925</wheel_normal_force>
+          <wheel_radius>0.129</wheel_radius>
+        </wheel>
+        <wheel link_name="rear_middle_right_wheel_link">
+          <slip_compliance_lateral>0.086</slip_compliance_lateral>
+          <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
+          <wheel_normal_force>41.47103925</wheel_normal_force>
+          <wheel_radius>0.129</wheel_radius>
+        </wheel>
+        <wheel link_name="rear_right_wheel_link">
+          <slip_compliance_lateral>0.086</slip_compliance_lateral>
+          <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
+          <wheel_normal_force>41.47103925</wheel_normal_force>
+          <wheel_radius>0.129</wheel_radius>
+        </wheel>
+      </plugin>
+    </include>
+    </sdf>
+  </spawn>
+  HEREDOC
+end
+
+def rosExecutables(_name, _worldName)
+  <<-HEREDOC
+  <executable name='robot_description'>
+    <command>roslaunch --wait marble_hd2_sensor_config_1 description.launch world_name:=#{_worldName} name:=#{_name}</command>
+  </executable>
+  <executable name='topics'>
+    <command>roslaunch --wait marble_hd2_sensor_config_1 vehicle_topics.launch world_name:=#{_worldName} name:=#{_name} breadcrumbs:=1 thermal:=1</command>
+  </executable>
+  HEREDOC
+end
diff --git a/submitted_models/marble_hd2_sensor_config_3/package.xml b/submitted_models/marble_hd2_sensor_config_3/package.xml
new file mode 100644
index 00000000..e3558bf8
--- /dev/null
+++ b/submitted_models/marble_hd2_sensor_config_3/package.xml
@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>marble_hd2_sensor_config_3</name>
+  <version>0.0.1</version>
+  <description>The marble_hd2_sensor_config_3 track vehicle package
+    - Has two rgbd camera Realsense D435
+    - An RP Lidar S1
+    - OS1-64 3D Laser C
+    - IMU
+    - gas sensor
+    - A pan/tilt gimbal (Trossen Dynamixel MX-64) with a Realsense D435
+      and 100 W laser mounted on it
+    - A planar lidar (RPLidar S1)
+    - A microstrain 3DMGX5-25 IMU (AHRS)
+    - Vividia HTI-301 LWIR Camera (thermal camera)
+    - Weight: about 75 lbs
+  </description>
+  <maintainer email="dvasquez@ssci.com">Derek Vasquez</maintainer>
+  <author email="dvasquez@ssci.com">Derek Vasquez</author>
+  <author email="njohnson@ssci.com">Neil Johnson</author>
+  <author email="hescobar@ssci.com">Hector Escobar</author>
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roslaunch</build_depend>
+  <exec_depend>lms1xx</exec_depend>
+  <exec_depend>urdf</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+</package>
diff --git a/submitted_models/marble_hd2_sensor_config_4/CMakeLists.txt b/submitted_models/marble_hd2_sensor_config_4/CMakeLists.txt
new file mode 100644
index 00000000..4c0d0eaf
--- /dev/null
+++ b/submitted_models/marble_hd2_sensor_config_4/CMakeLists.txt
@@ -0,0 +1,8 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(marble_hd2_sensor_config_4)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/submitted_models/marble_hd2_sensor_config_4/launch/spawner.rb b/submitted_models/marble_hd2_sensor_config_4/launch/spawner.rb
new file mode 100644
index 00000000..9534405f
--- /dev/null
+++ b/submitted_models/marble_hd2_sensor_config_4/launch/spawner.rb
@@ -0,0 +1,149 @@
+def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw)
+  <<-HEREDOC
+  <spawn name='#{_name}'>
+    <name>#{_name}</name>
+    <allow_renaming>false</allow_renaming>
+    <pose>#{_x} #{_y} #{_z + 0.2} #{_roll} #{_pitch} #{_yaw}</pose>
+    <world>#{_worldName}</world>
+    <is_performer>true</is_performer>
+    <sdf version='1.6'>
+    <include>
+      <name>#{_name}</name>
+      <uri>#{_modelURI}</uri>
+      <!-- Diff drive -->
+      <plugin filename="libignition-gazebo-diff-drive-system.so"
+              name="ignition::gazebo::systems::DiffDrive">
+        <left_joint>front_left_wheel_joint</left_joint>
+        <left_joint>front_middle_left_wheel_joint</left_joint>
+        <left_joint>rear_middle_left_wheel_joint</left_joint>
+        <left_joint>rear_left_wheel_joint</left_joint>
+        <right_joint>front_right_wheel_joint</right_joint>
+        <right_joint>front_middle_right_wheel_joint</right_joint>
+        <right_joint>rear_middle_right_wheel_joint</right_joint>
+        <right_joint>rear_right_wheel_joint</right_joint>
+        <wheel_separation>#{0.525}</wheel_separation>
+        <wheel_radius>0.129</wheel_radius>
+        <topic>/model/#{_name}/cmd_vel_relay</topic>
+        <min_velocity>-1</min_velocity>
+        <max_velocity>1</max_velocity>
+        <min_acceleration>-3</min_acceleration>
+        <max_acceleration>3</max_acceleration>
+      </plugin>
+      <!-- Publish robot state information -->
+      <plugin filename="libignition-gazebo-pose-publisher-system.so"
+        name="ignition::gazebo::systems::PosePublisher">
+        <publish_link_pose>true</publish_link_pose>
+        <publish_sensor_pose>true</publish_sensor_pose>
+        <publish_collision_pose>false</publish_collision_pose>
+        <publish_visual_pose>false</publish_visual_pose>
+        <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>
+        <use_pose_vector_msg>true</use_pose_vector_msg>
+        <static_publisher>true</static_publisher>
+        <static_update_frequency>1</static_update_frequency>
+      </plugin>
+      <!-- Battery plugin -->
+      <plugin filename="libignition-gazebo-linearbatteryplugin-system.so"
+        name="ignition::gazebo::systems::LinearBatteryPlugin">
+        <battery_name>linear_battery</battery_name>
+        <voltage>12.694</voltage>
+        <open_circuit_voltage_constant_coef>12.694</open_circuit_voltage_constant_coef>
+        <open_circuit_voltage_linear_coef>-3.1424</open_circuit_voltage_linear_coef>
+        <initial_charge>78.4</initial_charge>
+        <capacity>78.4</capacity>
+        <resistance>0.061523</resistance>
+        <smooth_current_tau>1.9499</smooth_current_tau>
+        <power_load>9.9</power_load>
+        <start_on_motion>true</start_on_motion>
+      </plugin>
+      <!-- Gas Sensor plugin -->
+      <plugin filename="libGasEmitterDetectorPlugin.so"
+        name="subt::GasDetector">
+        <topic>/model/#{_name}/gas_detected</topic>
+        <update_rate>10</update_rate>
+        <type>gas</type>
+      </plugin>
+      <plugin filename="libignition-gazebo-breadcrumbs-system.so"
+            name="ignition::gazebo::systems::Breadcrumbs">
+        <topic>/model/#{_name}/breadcrumb/deploy</topic>
+        <max_deployments>12</max_deployments>
+        <disable_physics_time>3.0</disable_physics_time>
+        <topic_statistics>true</topic_statistics>
+        <breadcrumb>
+          <sdf version="1.6">
+            <model name="#{_name}__breadcrumb__">
+              <pose>-0.45 0 0 0 0 0</pose>
+              <include>
+                <uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Breadcrumb Node</uri>
+             </include>
+           </model>
+         </sdf>
+        </breadcrumb>
+      </plugin>
+      <!-- Wheel slip -->
+      <plugin filename="libignition-gazebo-wheel-slip-system.so"
+        name="ignition::gazebo::systems::WheelSlip">
+        <wheel link_name="front_left_wheel_link">
+          <slip_compliance_lateral>0.086</slip_compliance_lateral>
+          <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
+          <wheel_normal_force>41.47103925</wheel_normal_force>
+          <wheel_radius>0.129</wheel_radius>
+        </wheel>
+        <wheel link_name="front_middle_left_wheel_link">
+          <slip_compliance_lateral>0.086</slip_compliance_lateral>
+          <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
+          <wheel_normal_force>41.47103925</wheel_normal_force>
+          <wheel_radius>0.129</wheel_radius>
+        </wheel>
+        <wheel link_name="rear_middle_left_wheel_link">
+          <slip_compliance_lateral>0.086</slip_compliance_lateral>
+          <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
+          <wheel_normal_force>41.47103925</wheel_normal_force>
+          <wheel_radius>0.129</wheel_radius>
+        </wheel>
+        <wheel link_name="rear_left_wheel_link">
+          <slip_compliance_lateral>0.086</slip_compliance_lateral>
+          <slip_compliance_longitudinal>0.25</slip_compliance_longitudinal>
+          <wheel_normal_force>41.47103925</wheel_normal_force>
+          <wheel_radius>0.129</wheel_radius>
+        </wheel>
+        <wheel link_name="front_right_wheel_link">
+          <slip_compliance_lateral>0.086</slip_compliance_lateral>
+          <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
+          <wheel_normal_force>41.47103925</wheel_normal_force>
+          <wheel_radius>0.129</wheel_radius>
+        </wheel>
+        <wheel link_name="front_middle_right_wheel_link">
+          <slip_compliance_lateral>0.086</slip_compliance_lateral>
+          <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
+          <wheel_normal_force>41.47103925</wheel_normal_force>
+          <wheel_radius>0.129</wheel_radius>
+        </wheel>
+        <wheel link_name="rear_middle_right_wheel_link">
+          <slip_compliance_lateral>0.086</slip_compliance_lateral>
+          <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
+          <wheel_normal_force>41.47103925</wheel_normal_force>
+          <wheel_radius>0.129</wheel_radius>
+        </wheel>
+        <wheel link_name="rear_right_wheel_link">
+          <slip_compliance_lateral>0.086</slip_compliance_lateral>
+          <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
+          <wheel_normal_force>41.47103925</wheel_normal_force>
+          <wheel_radius>0.129</wheel_radius>
+        </wheel>
+      </plugin>
+    </include>
+    </sdf>
+  </spawn>
+  HEREDOC
+end
+
+def rosExecutables(_name, _worldName)
+  <<-HEREDOC
+  <executable name='robot_description'>
+    <command>roslaunch --wait marble_hd2_sensor_config_1 description.launch world_name:=#{_worldName} name:=#{_name}</command>
+  </executable>
+  <executable name='topics'>
+    <command>roslaunch --wait marble_hd2_sensor_config_1 vehicle_topics.launch world_name:=#{_worldName} name:=#{_name} breadcrumbs:=1 thermal:=1</command>
+  </executable>
+  HEREDOC
+end
diff --git a/submitted_models/marble_hd2_sensor_config_4/package.xml b/submitted_models/marble_hd2_sensor_config_4/package.xml
new file mode 100644
index 00000000..0361d18f
--- /dev/null
+++ b/submitted_models/marble_hd2_sensor_config_4/package.xml
@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>marble_hd2_sensor_config_4</name>
+  <version>0.0.1</version>
+  <description>The marble_hd2_sensor_config_4 track vehicle package
+    - Has two rgbd camera Realsense D435
+    - An RP Lidar S1
+    - OS1-64 3D Laser C
+    - IMU
+    - gas sensor
+    - A pan/tilt gimbal (Trossen Dynamixel MX-64) with a Realsense D435
+      and 100 W laser mounted on it
+    - A planar lidar (RPLidar S1)
+    - A microstrain 3DMGX5-25 IMU (AHRS)
+    - Vividia HTI-301 LWIR Camera (thermal camera)
+    - 12 breadcrumbs
+    - Weight: about 75 lbs
+  </description>
+  <maintainer email="dvasquez@ssci.com">Derek Vasquez</maintainer>
+  <author email="dvasquez@ssci.com">Derek Vasquez</author>
+  <author email="njohnson@ssci.com">Neil Johnson</author>
+  <author email="hescobar@ssci.com">Hector Escobar</author>
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roslaunch</build_depend>
+  <exec_depend>lms1xx</exec_depend>
+  <exec_depend>urdf</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+</package>