add src

2024-10-30 15:31:05 +08:00 · 2024-10-30 15:31:05 +08:00 · 3a80599928
parent 28a9b88877
commit 3a80599928
65 changed files with 9320 additions and 0 deletions
--- a/src/jackal_description/CHANGELOG.rst
+++ b/src/jackal_description/CHANGELOG.rst
@ -0,0 +1,155 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package jackal_description
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.8.6 (2022-11-16)
+------------------
+* Set the GPS plugin's reference heading to 90 so it's ENU
+* Use xacro properties defined from environment variables for Microstrain URDF (`#123 <https://github.com/jackal/jackal/issues/123>`_)
+* Add GAZEBO_WORLD\_{LAT|LON} envars to change the reference coordinate of the robot's integral GPS
+* Contributors: Chris Iverach-Brereton, Joey Yang
+
+0.8.5 (2022-05-17)
+------------------
+* Added Blackfly  entry to URDF
+* Added Blackfly description to package.xml
+* Contributors: Luis Camero
+
+0.8.4 (2022-05-09)
+------------------
+
+0.8.3 (2022-03-08)
+------------------
+* Added the option to remove tower from VLP16 mount
+* Added SICK TIM551 to URDF and package.xml
+* Added UTM30 (`#106 <https://github.com/jackal/jackal/issues/106>`_)
+* Updated Navsat and LMS1xx mounts (`#103 <https://github.com/jackal/jackal/issues/103>`_)
+  * Updated hokuyo_ust10_mount to include min and max angle
+  * Removed extra spaces
+  * Updated SICK LMS1XX mount and NAVSAT mount
+  * Maintained backward compatibility with LMS1xx standard upright poisition by adding mount types
+* Updated hokuyo_ust10_mount to include min and max angle (`#102 <https://github.com/jackal/jackal/issues/102>`_)
+  * Updated hokuyo_ust10_mount to include min and max angle
+  * Removed extra spaces
+* Contributors: Luis Camero, luis-camero
+
+0.8.2 (2022-02-15)
+------------------
+* Moved microstrain link to accessories.urdf and updated envvars
+* Added velodyne tower mesh
+* Added Microstrain GX5 to description
+* Removed unnecessary URDF
+* Added Wibotic mesh and STL
+* Contributors: Luis Camero
+
+0.8.1 (2022-01-18)
+------------------
+* Updated to match melodic-devel
+* Contributors: Luis Camero
+
+0.8.0 (2021-04-23)
+------------------
+
+0.7.5 (2021-03-24)
+------------------
+* Add the origin block to the fender UST-10 macros; otherwise enabling them crashes
+* Contributors: Chris I-B
+
+0.7.4 (2021-03-16)
+------------------
+* Bumped CMake version to avoid author warning.
+* Contributors: Tony Baltovski
+
+0.7.3 (2021-03-08)
+------------------
+*  Add VLP16 support, refactor main/secondary laser envar support (#79)
+* Contributors: Chris I-B
+
+0.7.2 (2020-09-29)
+------------------
+
+0.7.1 (2020-08-24)
+------------------
+
+0.7.0 (2020-04-20)
+------------------
+* [jackal_description] Re-added pointgrey_camera_description as run depend.
+* Contributors: Tony Baltovski
+
+0.6.4 (2020-03-04)
+------------------
+* Modify the hokuyo accessory so that it works properly in gazebo/rviz.  Add an additional environment var JACKAL_LASER_HOKUYO which overrides the default lms1xx sensor with the ust10.
+* use env_run.bat on Windows (`#3 <https://github.com/jackal/jackal/issues/3>`_)
+* add setlocal
+* Fix jackal_description install location & fold xacro includes (`#2 <https://github.com/jackal/jackal/issues/2>`_)
+  * Fix install location.
+  * Fold xacro includes
+* add env-hook batch scripts (`#1 <https://github.com/jackal/jackal/issues/1>`_)
+* Contributors: Chris I-B, James Xu, Sean Yen, Tony Baltovski
+
+0.6.3 (2019-07-18)
+------------------
+* Added all extra fender changes
+* Contributors: Dave Niewinski
+
+* Made minor changes to syntax for kinetic warnings
+* Contributors: Dave Niewinski
+
+* Added stereo camera accessory.
+* Removed unused variable jackal_description_dir
+* Make urdf refer explicitly to jackal_description, rather than relying on current working directory being correct, for easier external includes
+* Contributors: Arnold Kalmbach, Tony Baltovski, akalmbach
+
+0.5.1 (2015-02-02)
+------------------
+* Modified the accessories.urdf.xacro to include both the GPS and mount plate, including standoffs.
+* Eliminate rosrun from the xacro wrapper.
+* Contributors: BryceVoort, Mike Purvis
+
+0.5.0 (2015-01-20)
+------------------
+* Add hook for custom URDF insertion to jackal.urdf.xacro.
+* Add xacro wrapper script to provide some pre-cooked "configs", especially for simulated Jackal.
+* Switch to parameterizing URDF with optenv.
+* Add laser bracket STL.
+* Contributors: Mike Purvis
+
+0.4.2 (2015-01-14)
+------------------
+
+0.4.1 (2015-01-07)
+------------------
+
+0.4.0 (2014-12-12)
+------------------
+* add pointgrey camera
+* Removed inertial and geometry of the base_link.
+* hector gazebo plugin for gps is added.
+* hector gazebo plugin for imu sensor is added
+* Contributors: Mike Purvis, spourmehr
+
+0.3.0 (2014-09-10)
+------------------
+* Add comment about accessory args.
+* Add front laser accessory to description.
+* Contributors: Mike Purvis
+
+0.2.1 (2014-09-10)
+------------------
+
+0.2.0 (2014-09-09)
+------------------
+* Changed physical and collision properties.
+* Fixed inertia parameters. Added imu plugin--not working
+* Install launch directory.
+* Contributors: Mike Purvis, Shokoofeh
+
+0.1.1 (2014-09-06)
+------------------
+* Remove unnecessary find packages.
+* Contributors: Mike Purvis
+
+0.1.0 (2014-09-05)
+------------------
+* Updated description with v0.9 hardware changes.
+* Contributors: Mike Purvis
--- a/src/jackal_description/CMakeLists.txt
+++ b/src/jackal_description/CMakeLists.txt
@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(jackal_description)
+
+find_package(catkin REQUIRED COMPONENTS roslaunch)
+
+catkin_package()
+
+roslaunch_add_file_check(launch/description.launch)
+
+install(DIRECTORY meshes launch urdf
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+if(WIN32)
+install(PROGRAMS scripts/env_run.bat
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+else()
+install(PROGRAMS scripts/env_run
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+endif()
--- a/src/jackal_description/README.md
+++ b/src/jackal_description/README.md
@ -0,0 +1,219 @@
+# Jackal Description
+
+This packages contains the meshes and URDF of the Jackal robot, its supported sensors, and their supported mounts. 
+
+# Jackal Payloads
+
+## Microstrain IMU
+```bash
+export JACKAL_IMU_MICROSTRAIN=1
+```
+###### Launch
+```bash
+export JACKAL_IMU_MICROSTRAIN_NAME="microstrain"
+```
+###### Description
+```bash
+export JACKAL_IMU_MICROSTRAIN_LINK="microstrain_link"
+export JACKAL_IMU_MICROSTRAIN_PARENT="base_link"
+export JACKAL_IMU_MICROSTRAIN_OFFSET="-0.139 0.096 0.100"
+export JACKAL_IMU_MICROSTRAIN_RPY="3.14159 0 -1.5707"
+```
+
+## 2D Laser
+#### Primary
+```bash
+export JACKAL_LASER=1
+export JACKAL_LASER_MODEL=lms1xx # or tim551 or ust10 or utm30
+```
+###### Launch
+```bash
+export JACKAL_LASER_TOPIC="front/scan"
+export JACKAL_LASER_HOST="192.168.131.20"
+```
+###### Description
+```bash
+export JACKAL_LASER_TOWER=1
+export JACKAL_LASER_MOUNT="front"
+export JACKAL_LASER_PREFIX="front"
+export JACKAL_LASER_PARENT="front_mount"
+export JACKAL_LASER_MOUNT_TYPE="upright" # or "inverted"
+export JACKAL_LASER_OFFSET="0 0 0"
+export JACKAL_LASER_RPY="0 0 0"
+```
+#### Secondary
+```bash
+export JACKAL_LASER_SECONDARY=1
+export JACKAL_LASER_SECONDARY_MODEL="lms1xx" # or "tim551" or "ust10" or "utm30"
+```
+###### Launch
+```bash
+export JACKAL_LASER_SECONDARY_HOST="192.168.131.21"
+export JACKAL_LASER_SECONDARY_TOPIC="rear/scan"
+```
+###### Description
+```bash
+export JACKAL_LASER_SECONDARY_TOWER=1
+export JACKAL_LASER_SECONDARY_MOUNT="rear"
+export JACKAL_LASER_SECONDARY_PREFIX="rear"
+export JACKAL_LASER_SECONDARY_PARENT="rear_mount"
+export JACKAL_LASER_SECONDARY_OFFSET="0 0 0"
+export JACKAL_LASER_SECONDARY_RPY="0 0 3.13159"
+```
+
+## 3D Laser
+```bash
+export JACKAL_LASER_3D=1
+```
+###### Launch
+```bash
+export JACKAL_LASER_3D_HOST="192.168.131.20"
+export JACKAL_LASER_3D_TOPIC="mid/points"
+```
+###### Description
+```bash
+export JACKAL_LASER_3D_TOWER=1
+export JACKAL_LASER_3D_MOUNT="mid"
+export JACKAL_LASER_3D_PREFIX="mid"
+export JACKAL_LASER_3D_PARENT="mid_mount"
+export JACKAL_LASER_3D_MODEL="vlp16" # or "hdl32e"
+export JACKAL_LASER_3D_OFFSET="0 0 0"
+export JACKAL_LASER_3D_RPY="0 0 0"
+```
+
+## NAVSAT
+```bash
+export JACKAL_NAVSAT=1
+```
+###### Launch
+```bash
+export JACKAL_NAVSAT_PORT="/dev/clearpath/gps"
+export JACKAL_NAVSAT_BAUD=57600
+export JACKAL_NAVSAT_RTK=0
+export JACKAL_NAVSAT_RTK_DEVICE=wlan0
+export JACKAL_NAVSAT_RTK_BAUD=57600
+```
+###### Description
+```bash
+export JACKAL_NAVSAT_TOWER=1
+export JACKAL_NAVSAT_HEIGHT=0.1 # in meters
+export JACKAL_NAVSAT_MOUNT="rear"
+export JACKAL_NAVSAT_PREFIX="rear"
+export JACKAL_NAVSAT_PARENT="rear_mount"
+export JACKAL_NAVSAT_MODEL="smart6" # or "smart7"
+export JACKAL_NAVSAT_OFFSET="0 0 0"
+export JACKAL_NAVSAT_RPY="0 0 0"
+```
+
+## Pointgrey Flea3
+```bash
+export JACKAL_FLEA3=1
+```
+###### Launch
+```bash
+export JACKAL_FLEA3_SERIAL=0
+export JACKAL_FLEA3_FRAME_RATE=30
+export JACKAL_FLEA3_CALIBRATION=0
+```
+###### Description
+```bash
+export JACKAL_FLEA3_TOWER=1
+export JACKAL_FLEA3_TILT=0.5236
+export JACKAL_FLEA3_NAME="front"
+export JACKAL_FLEA3_MOUNT="front"
+export JACKAL_FLEA3_PREFIX="front"
+export JACKAL_FLEA3_PARENT="front_mount"
+export JACKAL_FLEA3_OFFSET="0 0 0"
+export JACKAL_FLEA3_RPY="0 0 0"
+```
+
+## Stereo Pointgrey Flea3
+```bash
+export JACKAL_STEREO_FLEA3=1
+```
+###### Launch
+```bash
+export JACKAL_FLEA3_FRAME_RATE=30
+export JACKAL_FLEA3_LEFT_SERIAL=0
+export JACKAL_FLEA3_LEFT_CALIBRATION=0
+export JACKAL_FLEA3_RIGHT_SERIAL=0
+export JACKAL_FLEA3_RIGHT_CALIBRATION=0
+```
+###### Description
+```bash
+export JACKAL_STEREO_SEPERATION=0.16
+export JACKAL_FLEA3_TILT="0.5236"
+export JACKAL_FLEA3_MOUNT="front"
+export JACKAL_FLEA3_PREFIX="front"
+export JACKAL_FLEA3_PARENT="front_mount"
+export JACKAL_FLEA3_LEFT_NAME="front/left"
+export JACKAL_FLEA3_RIGHT_NAME="front/right"
+export JACKAL_FLEA3_OFFSET="0 0 0"
+export JACKAL_FLEA3_RPY="0 0 0"
+```
+
+## Bumblebee2
+```bash
+export JACKAL_BB2=1
+```
+###### Launch
+```bash
+export JACKAL_BB2_SERIAL=0
+export JACKAL_BB2_CALIBRATION=0
+```
+###### Description
+```bash
+export JACKAL_BB2_TILT=0
+export JACKAL_BB2_TOWER=1
+export JACKAL_BB2_NAME="front"
+export JACKAL_BB2_MOUNT="front"
+export JACKAL_BB2_PREFIX="front"
+export JACKAL_BB2_PARENT="front_mount"
+export JACKAL_BB2_OFFSET="0 0 0"
+export JACKAL_BB2_RPY="0 0 0"
+```
+
+## Flir Blackfly
+```bash
+export JACKAL_BLACKFLY=1
+```
+###### Launch
+```bash
+export JACKAL_BLACKFLY_SERIAL=0
+export JACKAL_BLACKFLY_DEVICE="USB" # or "GigE"
+export JACKAL_BLACKFLY_ENCODING="BayerRGB"
+export JACKAL_BLACKFLY_FRAMERATE=30
+```
+###### Description
+```bash
+export JACKAL_BLACKFLY_PREFIX="front_camera"
+export JACKAL_BLACKFLY_PARENT="front_mount"
+export JACKAL_BLACKFLY_OFFSET="0 0 0"
+export JACKAL_BLACKFLY_RPY="0 0 0"
+```
+
+## Front and Rear Accessory Fender
+```bash
+export JACKAL_FRONT_ACCESSORY_FENDER=1
+export JACKAL_REAR_ACCESSORY_FENDER=1
+export JACKAL_FRONT_FENDER_UST10=1
+export JACKAL_REAR_FENDER_UST10=1
+```
+###### Launch
+```bash
+export JACKAL_FRONT_LASER_TOPIC=front/scan
+export JACKAL_FRONT_LASER_HOST="192.168.131.20"
+export JACKAL_REAR_LASER_TOPIC=rear/scan
+export JACKAL_REAR_LASER_HOST="192.168.131.21"
+```
+
+## Decorations
+Description only accessories (i.e. no driver).
+###### Wibotic Q-Charging Bumper:
+```bash
+export JACKAL_WIBOTIC_BUMPER=1
+```
+###### Backpack Computer Enclosure:
+```bash
+export JACKAL_ARK_ENCLOSURE=1
+```
--- a/src/jackal_description/launch/description.launch
+++ b/src/jackal_description/launch/description.launch
@ -0,0 +1,14 @@
+<launch>
+  <arg name="config" default="base" />
+  <!-- fix for oneweek project -->
+  <arg name="env_runner" value="$(eval 'env_run' if not optenv('OS', 'unknown').lower().startswith('windows') else 'env_run.bat')" />
+  <!-- the following seems to work too when in devel space, but not in install_isolated -->
+  <!-- <arg name="env_runner" value="env_run" /> -->
+
+  <param name="robot_description"
+         command="$(find jackal_description)/scripts/$(arg env_runner)
+                    $(find jackal_description)/urdf/configs/$(arg config)
+                    $(find xacro)/xacro $(find jackal_description)/urdf/jackal.urdf.xacro
+                    --inorder" />
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+</launch>
--- a/src/jackal_description/meshes/accessory_fender.stl
+++ b/src/jackal_description/meshes/accessory_fender.stl
--- a/src/jackal_description/meshes/ark_enclosure.stl
+++ b/src/jackal_description/meshes/ark_enclosure.stl
--- a/src/jackal_description/meshes/bridge-plate.stl
+++ b/src/jackal_description/meshes/bridge-plate.stl
--- a/src/jackal_description/meshes/camera-beam.stl
+++ b/src/jackal_description/meshes/camera-beam.stl
--- a/src/jackal_description/meshes/camera-bracket.stl
+++ b/src/jackal_description/meshes/camera-bracket.stl
--- a/src/jackal_description/meshes/hokuyo_ust10.stl
+++ b/src/jackal_description/meshes/hokuyo_ust10.stl
--- a/src/jackal_description/meshes/hokuyo_utm30.stl
+++ b/src/jackal_description/meshes/hokuyo_utm30.stl
--- a/src/jackal_description/meshes/jackal-base.stl
+++ b/src/jackal_description/meshes/jackal-base.stl
--- a/src/jackal_description/meshes/jackal-fender.stl
+++ b/src/jackal_description/meshes/jackal-fender.stl
--- a/src/jackal_description/meshes/jackal-fenders.stl
+++ b/src/jackal_description/meshes/jackal-fenders.stl
--- a/src/jackal_description/meshes/jackal-wheel.stl
+++ b/src/jackal_description/meshes/jackal-wheel.stl
--- a/src/jackal_description/meshes/kinect.dae
+++ b/src/jackal_description/meshes/kinect.dae
--- a/src/jackal_description/meshes/kinect_mount.stl
+++ b/src/jackal_description/meshes/kinect_mount.stl
--- a/src/jackal_description/meshes/novatel-smart6.stl
+++ b/src/jackal_description/meshes/novatel-smart6.stl
--- a/src/jackal_description/meshes/novatel-smart7.stl
+++ b/src/jackal_description/meshes/novatel-smart7.stl
--- a/src/jackal_description/meshes/sick-lms1xx-inverted-bracket.stl
+++ b/src/jackal_description/meshes/sick-lms1xx-inverted-bracket.stl
--- a/src/jackal_description/meshes/sick-lms1xx-upright-bracket.stl
+++ b/src/jackal_description/meshes/sick-lms1xx-upright-bracket.stl
--- a/src/jackal_description/meshes/stereo-camera-beam.stl
+++ b/src/jackal_description/meshes/stereo-camera-beam.stl
--- a/src/jackal_description/meshes/stereo-camera-bracket.stl
+++ b/src/jackal_description/meshes/stereo-camera-bracket.stl
--- a/src/jackal_description/meshes/velodyne_tower.stl
+++ b/src/jackal_description/meshes/velodyne_tower.stl
--- a/src/jackal_description/meshes/wibotic_bumper.stl
+++ b/src/jackal_description/meshes/wibotic_bumper.stl
--- a/src/jackal_description/package.xml
+++ b/src/jackal_description/package.xml
@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<package>
+  <name>jackal_description</name>
+  <version>0.8.6</version>
+  <description>URDF robot description for Jackal</description>
+
+  <maintainer email="mpurvis@clearpathrobotics.com">Mike Purvis</maintainer>
+
+  <license>BSD</license>
+
+  <author email="mpurvis@clearpathrobotics.com">Mike Purvis</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roslaunch</build_depend>
+  <run_depend>flir_camera_description</run_depend>
+  <run_depend>robot_state_publisher</run_depend>
+  <run_depend>urdf</run_depend>
+  <run_depend>xacro</run_depend>
+  <run_depend>lms1xx</run_depend>
+  <run_depend>pointgrey_camera_description</run_depend>
+  <run_depend>sick_tim</run_depend>
+  <run_depend>velodyne_description</run_depend>
+
+  <export>
+  </export>
+</package>
--- a/src/jackal_description/scripts/env_run
+++ b/src/jackal_description/scripts/env_run
@ -0,0 +1,12 @@
+#!/bin/bash
+# This simple wrapper allowing us to pass a set of
+# environment variables to be sourced prior to running
+# another command. Used in the launch file for setting
+# robot configurations prior to xacro.
+
+ENVVARS_FILE=$1
+shift 1
+
+set -a
+source $ENVVARS_FILE 
+$@
--- a/src/jackal_description/scripts/env_run.bat
+++ b/src/jackal_description/scripts/env_run.bat
@ -0,0 +1,25 @@
+@echo off
+setlocal
+REM This simple wrapper allowing us to pass a set of
+REM environment variables to be sourced prior to running
+REM another command. Used in the launch file for setting
+REM robot configurations prior to xacro.
+
+set ENVVARS_FILE=%1
+
+REM get arguments starting from %2
+shift
+set RUNNER_COMMAND=
+:getrunnerloop
+if "%1"=="" goto after_loop
+if "%RUNNER_COMMAND%" == "" (
+    set RUNNER_COMMAND=%1
+) else (
+    set RUNNER_COMMAND=%RUNNER_COMMAND% %1
+)
+shift
+goto getrunnerloop
+
+:after_loop
+call %ENVVARS_FILE%
+call %RUNNER_COMMAND%
--- a/src/jackal_description/urdf/accessories.urdf.xacro
+++ b/src/jackal_description/urdf/accessories.urdf.xacro
@ -0,0 +1,552 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <!--
+    As you add to this URDF, please be aware that both the robot and
+    simulation include it. You must retain compatibility with all of
+    the following launch files:
+
+    jackal_viz/launch/view_model.launch
+    jackal_gazebo/launch/jackal_world.launch
+    jackal_base/launch/base.launch
+  -->
+
+  <!-- Macros to generate standoffs for offset payload mounting. -->
+  <xacro:include filename="$(find jackal_description)/urdf/accessories/standoffs.urdf.xacro" />
+
+  <!-- This optional plate mounts both the upgraded GPS and the Velodyne 3D LIDAR. -->
+  <xacro:include filename="$(find jackal_description)/urdf/accessories/bridge_plate.urdf.xacro" />
+
+  <!-- 2D Lidars -->
+  <xacro:include filename="$(find sick_tim)/urdf/sick_tim.urdf.xacro" />
+  <xacro:include filename="$(find lms1xx)/urdf/sick_lms1xx.urdf.xacro" />
+  <xacro:include filename="$(find jackal_description)/urdf/accessories/sick_lms1xx_inverted_mount.urdf.xacro" />
+  <xacro:include filename="$(find jackal_description)/urdf/accessories/sick_lms1xx_upright_mount.urdf.xacro" />
+  <xacro:include filename="$(find jackal_description)/urdf/accessories/hokuyo_utm30.urdf.xacro" />
+  <xacro:include filename="$(find jackal_description)/urdf/accessories/hokuyo_ust10.urdf.xacro" />
+  <xacro:include filename="$(find jackal_description)/urdf/accessories/vlp16_mount.urdf.xacro" />
+  <xacro:include filename="$(find jackal_description)/urdf/accessories/hdl32_mount.urdf.xacro" />
+
+  <!--
+    Add Microstrain IMU. By default the Jackal comes equipped with an IMU linked to "imu_link". These extra
+    optional microstrain IMU's will be added to another frame, by default, "microstrain_link".
+  -->
+  <xacro:if value="$(optenv JACKAL_IMU_MICROSTRAIN 0)">
+    <xacro:property name="frame"  value="$(optenv JACKAL_IMU_MICROSTRAIN_LINK microstrain_link)"/>
+    <xacro:property name="parent" value="$(optenv JACKAL_IMU_MICROSTRAIN_PARENT base_link)"/>
+    <xacro:property name="xyz"    value="$(optenv JACKAL_IMU_MICROSTRAIN_OFFSET -0.139 0.096 0.100)"/>
+    <xacro:property name="rpy"    value="$(optenv JACKAL_IMU_MICROSTRAIN_RPY ${PI} 0 -${PI/2})"/>
+    <link name="${frame}" />
+    <joint name="${frame}_joint" type="fixed">
+      <parent link="${parent}" />
+      <child link="${frame}" />
+      <origin xyz="${xyz}" rpy="${rpy}" />
+    </joint>
+  </xacro:if>
+
+  <!--
+    Add primary/secondary 2D lidar sensors.  By default these are SICK LMS1xx but can be changed with the
+    JACKAL_LASER_MODEL and JACKAL_LASER_SECONDARY_MODEL environment variables. Valid model designations are:
+    - lms1xx (default) :: SICK LMS1xx
+    - ust10            :: Hokuyo UST10
+    - utm30            :: Hokuyo UTM30
+  -->
+  <!-- Primary 2D Lidar -->
+  <xacro:if value="$(optenv JACKAL_LASER 1)">
+    <xacro:property name="mount" value="$(optenv JACKAL_LASER_MOUNT front)" />
+    <xacro:property name="topic" value="$(optenv JACKAL_LASER_TOPIC front/scan)" />
+    <xacro:property name="tower" value="$(optenv JACKAL_LASER_TOWER 1)" />
+    <xacro:property name="prefix" value="$(optenv JACKAL_LASER_PREFIX ${mount})" />
+    <xacro:property name="parent" value="$(optenv JACKAL_LASER_PARENT ${mount}_mount)" />
+    <xacro:property name="lidar_model" value="$(optenv JACKAL_LASER_MODEL tim551)" />
+    <xacro:property name="mount_type" value="$(optenv JACKAL_LASER_MOUNT_TYPE upright)" />
+    <!-- SICK LMS1xx -->
+    <xacro:if value="${lidar_model == 'lms1xx'}">
+      <!-- Mount Enabled -->
+      <xacro:if value="${tower}">
+        <xacro:if value="${mount_type == 'upright'}">
+          <xacro:sick_lms1xx_upright_mount topic="${topic}" prefix="${prefix}" parent_link="${parent}">
+            <origin xyz="$(optenv JACKAL_LASER_OFFSET 0 0 0)" rpy="$(optenv JACKAL_LASER_RPY 0 0 0)" />
+          </xacro:sick_lms1xx_upright_mount>
+        </xacro:if>
+        <xacro:if value="${mount_type == 'inverted'}">
+          <xacro:sick_lms1xx_inverted_mount topic="${topic}" prefix="${prefix}" parent_link="${parent}">
+            <origin xyz="$(optenv JACKAL_LASER_OFFSET 0 0 0)" rpy="$(optenv JACKAL_LASER_RPY 0 0 0)" />
+          </xacro:sick_lms1xx_inverted_mount>
+        </xacro:if>
+      </xacro:if>
+      <!-- Mount Disabled -->
+      <xacro:unless value="${tower}">
+        <xacro:sick_lms1xx frame="${prefix}_laser" topic="${topic}" />
+        <link name="${prefix}_laser_base"/>
+        <joint name="${prefix}_laser_base_joint" type="fixed">
+          <origin xyz="$(optenv JACKAL_LASER_OFFSET 0 0 0)" rpy="$(optenv JACKAL_LASER_RPY 0 0 0)"/>
+          <parent link="${parent}"/>
+          <child link="${prefix}_laser_base"/>
+        </joint>
+        <joint name="${prefix}_laser_joint" type="fixed">
+          <origin xyz="0 0 0.116" rpy="0 0 0"/>
+          <parent link="${prefix}_laser_base"/>
+          <child link="${prefix}_laser"/>
+        </joint>
+      </xacro:unless>
+    </xacro:if>
+    <!-- SICK Tim -->
+    <xacro:if value="${lidar_model == 'tim551'}">
+      <xacro:property name="prefix" value="$(optenv JACKAL_LASER_PREFIX tim551)" />
+      <xacro:sick_tim551 name="${prefix}" ros_topic="${topic}"/>
+      <joint name="${prefix}_mount_link_joint" type="fixed">
+        <origin xyz="$(optenv JACKAL_LASER_OFFSET 0 0 0)" rpy="$(optenv JACKAL_LASER_RPY 0 0 0)"/>
+        <parent link="${parent}"/>
+        <child link="${prefix}_mount_link"/>
+      </joint>
+    </xacro:if>
+    <!-- Hokuyo UST10 -->
+    <xacro:if value="${lidar_model == 'ust10'}">
+      <xacro:hokuyo_ust10_mount topic="${topic}" prefix="${prefix}" parent_link="${parent}">
+        <origin xyz="$(optenv JACKAL_LASER_OFFSET 0 0 0)" rpy="$(optenv JACKAL_LASER_RPY 0 0 0)" />
+      </xacro:hokuyo_ust10_mount>
+    </xacro:if>
+    <!-- Hokuyo UTM30 -->
+    <xacro:if value="${lidar_model == 'utm30'}">
+      <xacro:hokuyo_utm30_mount topic="${topic}" prefix="${prefix}" parent_link="${parent}">
+        <origin xyz="$(optenv JACKAL_LASER_OFFSET 0 0 0)" rpy="$(optenv JACKAL_LASER_RPY 0 0 0)" />
+      </xacro:hokuyo_utm30_mount>
+    </xacro:if>
+  </xacro:if>
+
+  <!-- Secondary 2D Lidar -->
+  <xacro:if value="$(optenv JACKAL_LASER_SECONDARY 0)">
+    <xacro:property name="mount" value="$(optenv JACKAL_LASER_SECONDARY_MOUNT rear)" />
+    <xacro:property name="topic" value="$(optenv JACKAL_LASER_SECONDARY_TOPIC rear/scan)" />
+    <xacro:property name="tower" value="$(optenv JACKAL_LASER_SECONDARY_TOWER 1)" />
+    <xacro:property name="prefix" value="$(optenv JACKAL_LASER_SECONDARY_PREFIX ${mount})" />
+    <xacro:property name="parent" value="$(optenv JACKAL_LASER_SECONDARY_PARENT ${mount}_mount)" />
+    <xacro:property name="lidar_model" value="$(optenv JACKAL_LASER_SECONDARY_MODEL lms1xx)" />
+    <!-- SICK LMS1xx -->
+    <xacro:if value="${lidar_model == 'lms1xx'}">
+      <!-- Mount Enabled -->
+      <xacro:if value="${tower}">
+        <xacro:sick_lms1xx_mount topic="${topic}" prefix="${prefix}" parent_link="${parent}">
+          <origin xyz="$(optenv JACKAL_LASER_SECONDARY_OFFSET 0 0 0)" rpy="$(optenv JACKAL_LASER_SECONDARY_RPY 0 0 3.14159)" />
+        </xacro:sick_lms1xx_mount>
+      </xacro:if>
+      <!-- Mount Disabled -->
+      <xacro:unless value="${tower}">
+        <xacro:sick_lms1xx frame="${prefix}_laser" topic="${topic}" />
+        <link name="${prefix}_laser_base"/>
+        <joint name="${prefix}_laser_base_joint" type="fixed">
+          <origin xyz="$(optenv JACKAL_LASER_SECONDARY_OFFSET 0 0 0)" rpy="$(optenv JACKAL_LASER_SECONDARY_RPY 0 0 3.14159)"/>
+          <parent link="${parent}"/>
+          <child link="${prefix}_laser_base"/>
+        </joint>
+        <joint name="${prefix}_laser_joint" type="fixed">
+          <origin xyz="0 0 0.116" rpy="0 0 0"/>
+          <parent link="${prefix}_laser_base"/>
+          <child link="${prefix}_laser"/>
+        </joint>
+      </xacro:unless>
+    </xacro:if>
+    <!-- SICK Tim -->
+    <xacro:if value="${lidar_model == 'tim551'}">
+      <xacro:property name="prefix" value="$(optenv JACKAL_LASER_SECONDARY_PREFIX tim551_2)" />
+      <xacro:sick_tim551 name="${prefix}" ros_topic="${topic}"/>
+      <joint name="${prefix}_mount_link_joint" type="fixed">
+        <origin xyz="$(optenv JACKAL_LASER_SECONDARY_OFFSET 0 0 0)" rpy="$(optenv JACKAL_LASER_SECONDARY_RPY 0 0 3.14159)"/>
+        <parent link="${parent}"/>
+        <child link="${prefix}_mount_link"/>
+      </joint>
+    </xacro:if>
+    <!-- Hokuyo UST10 -->
+    <xacro:if value="${lidar_model == 'ust10'}">
+      <xacro:hokuyo_ust10_mount topic="${topic}" prefix="${prefix}" parent_link="${parent}">
+        <origin xyz="$(optenv JACKAL_LASER_SECONDARY_OFFSET 0 0 0)" rpy="$(optenv JACKAL_LASER_SECONDARY_RPY 0 0 3.14159)" />
+      </xacro:hokuyo_ust10_mount>
+    </xacro:if>
+    <!-- Hokuyo UTM30 -->
+    <xacro:if value="${lidar_model == 'utm30'}">
+      <xacro:hokuyo_utm30_mount topic="${topic}" prefix="${prefix}" parent_link="${parent}">
+        <origin xyz="$(optenv JACKAL_LASER_SECONDARY_OFFSET 0 0 0)" rpy="$(optenv JACKAL_LASER_SECONDARY_RPY 0 0 3.14159)" />
+      </xacro:hokuyo_utm30_mount>
+    </xacro:if>
+  </xacro:if>
+
+  <!--
+    Add a 3D lidar sensor.  By default this is a Velodyne VLP16 but can be changed with the
+    JACKAL_LASER_3D_MODEL environment variable. Valid model designations are:
+    - vlp16 (default)  :: Velodyne VLP16
+    - vlp32e           :: Velodyne HDL-32E
+  -->
+  <xacro:if value="$(optenv JACKAL_LASER_3D 1)">
+    <xacro:property name="mount" value="$(optenv JACKAL_LASER_3D_MOUNT mid)" />
+    <xacro:property name="topic" value="$(optenv JACKAL_LASER_3D_TOPIC mid/points)" />
+    <xacro:property name="tower" value="$(optenv JACKAL_LASER_3D_TOWER 1)" />
+    <xacro:property name="prefix" value="$(optenv JACKAL_LASER_3D_PREFIX ${mount})" />
+    <xacro:property name="parent" value="$(optenv JACKAL_LASER_3D_PARENT ${mount}_mount)" />
+    <xacro:property name="lidar_3d_model" value="$(optenv JACKAL_LASER_3D_MODEL vlp16)" />
+    <!-- Velodyne VLP16 -->
+    <xacro:if value="${lidar_3d_model == 'vlp16'}">
+      <xacro:vlp16_mount topic="${topic}" tower="${tower}" prefix="${prefix}" parent_link="${parent}" >
+        <origin xyz="$(optenv JACKAL_LASER_3D_OFFSET 0 0 0)" rpy="$(optenv JACKAL_LASER_3D_RPY 0 0 0)" />
+      </xacro:vlp16_mount>
+    </xacro:if>
+    <!-- Velodyne HDL-32E -->
+    <xacro:if value="${lidar_3d_model == 'hdl32e'}">
+      <xacro:hdl32e_mount topic="${topic}" tower="${tower}" prefix="${prefix}" parent_link="${parent}" >
+        <origin xyz="$(optenv JACKAL_LASER_3D_OFFSET 0 0 0)" rpy="$(optenv JACKAL_LASER_3D_RPY 0 0 0)" />
+      </xacro:hdl32e_mount>
+    </xacro:if>
+  </xacro:if>
+
+  <!--
+    Add a Nav. Sat. By default this is a Novatel Smart6, but can be changed with the 
+    JACKAL_NAVSAT_MODEL environment variable. Valid model designations are: 
+    - smart6 (default)    :: Novatel Smart6
+    - smart7              :: Novatel Smart7
+  -->
+  <xacro:include filename="$(find jackal_description)/urdf/accessories/novatel_smart6.urdf.xacro" />
+  <xacro:include filename="$(find jackal_description)/urdf/accessories/novatel_smart7.urdf.xacro" />
+
+  <xacro:if value="$(optenv JACKAL_NAVSAT 0)">
+    <xacro:property name="mount" value="$(optenv JACKAL_NAVSAT_MOUNT rear)" />
+    <xacro:property name="tower" value="$(optenv JACKAL_NAVSAT_TOWER 1)" />
+    <xacro:property name="prefix" value="$(optenv JACKAL_NAVSAT_PREFIX ${mount})" />
+    <xacro:property name="parent" value="$(optenv JACKAL_NAVSAT_PARENT ${mount}_mount)" />
+    <xacro:property name="navsat_model" value="$(optenv JACKAL_NAVSAT_MODEL smart6)" />
+    <!--- Novotel Smart6 -->
+    <xacro:if value="${navsat_model == 'smart6'}">
+      <xacro:novatel_smart6 prefix="${prefix}"/>
+      <!-- Tower Enabled -->
+      <xacro:if value="${tower}">
+        <xacro:bridge_plate prefix="${prefix}" parent="${parent}" height="$(optenv JACKAL_NAVSAT_HEIGHT 0.1)">
+          <origin xyz="$(optenv JACKAL_NAVSAT_OFFSET 0 0 0)" rpy="$(optenv JACKAL_NAVSAT_RPY 0 0 0)"/>
+        </xacro:bridge_plate>
+
+        <joint name="${prefix}_navsat_joint" type="fixed">
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <parent link="${prefix}_bridge" />
+          <child link="${prefix}_navsat" />
+        </joint>
+      </xacro:if>
+      <!-- Tower Disabled -->
+      <xacro:unless value="${tower}">
+        <joint name="${prefix}_navsat_joint" type="fixed">
+          <origin xyz="$(optenv JACKAL_NAVSAT_OFFSET 0 0 0)" rpy="$(optenv JACKAL_NAVSAT_RPY 0 0 0)" />
+          <parent link="${parent}" />
+          <child link="${prefix}_navsat" />
+        </joint>
+      </xacro:unless>
+    </xacro:if>
+
+    <!-- Novotel Smart7 -->
+    <xacro:if value="${navsat_model == 'smart7'}">
+      <xacro:novatel_smart7 prefix="${prefix}"/>
+      <!-- Tower Enabled -->
+      <xacro:if value="${tower}">
+        <xacro:bridge_plate prefix="${prefix}" parent="${parent}" height="$(optenv JACKAL_NAVSAT_HEIGHT 0.1)">
+          <origin xyz="$(optenv JACKAL_NAVSAT_OFFSET 0 0 0)" rpy="$(optenv JACKAL_NAVSAT_RPY 0 0 0)"/>
+        </xacro:bridge_plate>
+
+        <joint name="${prefix}_navsat_joint" type="fixed">
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <parent link="${prefix}_bridge" />
+          <child link="${prefix}_navsat" />
+        </joint>
+      </xacro:if>
+      <!-- Tower Disabled -->
+      <xacro:unless value="${tower}">
+        <joint name="${prefix}_navsat_joint" type="fixed">
+          <origin xyz="$(optenv JACKAL_NAVSAT_OFFSET 0 0 0)" rpy="$(optenv JACKAL_NAVSAT_RPY 0 0 0)" />
+          <parent link="${parent}" />
+          <child link="${prefix}_navsat" />
+        </joint>
+      </xacro:unless>
+    </xacro:if>
+  </xacro:if>
+
+  <!-- 
+    Add Cameras. 
+    - JACKAL_FLEA3
+    - JACKAL_STEREO_FLEA3
+    - JACKAL_BB2
+    - Kinect
+  -->
+
+  <!-- Common camera mounts and accessory URDFs. -->
+  <xacro:include filename="$(find jackal_description)/urdf/accessories/camera_mount.urdf.xacro" />
+  <xacro:include filename="$(find pointgrey_camera_description)/urdf/pointgrey_flea3.urdf.xacro" />
+
+  <!-- If enabled, generate the flea3 camera payload with a tilt of 30 degrees. -->
+  <xacro:if value="$(optenv JACKAL_FLEA3 0)">
+    <xacro:property name="name" value="$(optenv JACKAL_FLEA3_NAME front)" />
+    <xacro:property name="tilt" value="$(optenv JACKAL_FLEA3_TILT 0.)" />
+    <xacro:property name="tower" value="$(optenv JACKAL_FLEA3_TOWER 1)" />
+    <xacro:property name="mount" value="$(optenv JACKAL_FLEA3_MOUNT front)" />
+    <xacro:property name="prefix" value="$(optenv JACKAL_FLEA3_PREFIX ${mount})" />
+    <xacro:property name="parent" value="$(optenv JACKAL_FLEA3_PARENT ${mount}_mount)" />
+
+    <xacro:pointgrey_flea3 frame="${prefix}_camera" name="${name}"
+      camera_x="0.0754" camera_y="0.029" camera_z="0.029"
+      camera_mass="0.085" hfov="1.0471975512" fps="60" width="640" height="512"/>
+
+    <!-- Mount Enabled -->
+    <xacro:if value="${tower}">
+      <xacro:camera_mount prefix="${prefix}" tilt="${tilt}"/>
+
+      <joint name="${prefix}_camera_mount_joint" type="fixed">
+        <origin xyz="$(optenv JACKAL_FLEA3_OFFSET 0 0 0)"
+                rpy="$(optenv JACKAL_FLEA3_RPY 0 0 0)" />
+        <parent link="${parent}" />
+        <child link="${prefix}_camera_mount" />
+      </joint>
+
+      <joint name="${prefix}_camera_bracket_joint" type="fixed">
+        <origin xyz="0.020 0 0.0245" rpy="0 0 0" />
+        <parent link="${prefix}_camera_beam" />
+        <child link="${prefix}_camera" />
+      </joint>
+    </xacro:if>
+
+    <!-- Mount Disabled -->
+    <xacro:unless value="${tower}">
+      <link name="${prefix}_camera_base"/>
+      <joint name="${prefix}_camera_base_joint" type="fixed">
+        <origin xyz="$(optenv JACKAL_FLEA3_OFFSET 0 0 0)"
+                rpy="$(optenv JACKAL_FLEA3_RPY 0 0 0)" />
+        <parent link="${parent}" />
+        <child link="${prefix}_camera_base" />
+      </joint>
+      <joint name="${prefix}_camera_joint" type="fixed">
+        <origin xyz="0 0 0.0145"
+                rpy="0 0 0" />
+        <parent link="${prefix}_camera_base" />
+        <child link="${prefix}_camera" />
+      </joint>
+    </xacro:unless>
+  </xacro:if>
+
+  <xacro:include filename="$(find jackal_description)/urdf/accessories/stereo_camera_mount.urdf.xacro" />
+  <!-- If enabled, generates a pair of flea3 cameras for stereo vision with a tilt of 30 degrees. -->
+  <!-- Disabled temporarily due to metapackage issue. -->
+  <xacro:if value="$(optenv JACKAL_STEREO_FLEA3 0)">
+    <xacro:property name="tilt" value="$(optenv JACKAL_FLEA3_TILT 0.5236)"/>
+    <xacro:property name="mount" value="$(optenv JACKAL_FLEA3_MOUNT front)"/>
+    <xacro:property name="prefix" value="$(optenv JACKAL_FLEA3_PREFIX ${mount})"/>
+    <xacro:property name="parent" value="$(optenv JACKAL_FLEA3_PARENT ${mount}_mount)"/>
+    <xacro:property name="left_name" value="$(optenv JACKAL_FLEA3_LEFT_NAME front/left)"/>
+    <xacro:property name="right_name" value="$(optenv JACKAL_FLEA3_RIGHT_NAME front/right)"/>
+    <xacro:property name="separation" value="$(optenv JACKAL_STEREO_SEPERATION 0.16)"/>
+
+    <xacro:stereo_camera_mount prefix="${prefix}" tilt="${tilt}"/>
+    <joint name="${prefix}_stereo_camera_mount_joint" type="fixed">
+      <origin xyz="$(optenv JACKAL_FLEA3_OFFSET 0 0 0)"
+              rpy="$(optenv JACKAL_FLEA3_RPY 0 0 0)" />
+      <parent link="${parent}" />
+      <child link="${prefix}_stereo_camera_mount" />
+    </joint>
+
+    <xacro:pointgrey_flea3 frame="${prefix}_left_camera" name="${left_name}"
+      camera_x="0.0754" camera_y="0.029" camera_z="0.029"
+      camera_mass="0.085" hfov="1.0471975512" fps="60" width="640" height="512"/>
+    <joint name="${prefix}_left_stereo_camera_bracket_joint" type="fixed">
+      <origin xyz="0.015 ${separation} 0.0599" rpy="0 0 0" />
+      <parent link="${prefix}_stereo_camera_beam" />
+      <child link="${prefix}_left_camera" />
+    </joint>
+
+    <xacro:pointgrey_flea3 frame="${prefix}_right_camera" name="${right_name}"
+      camera_x="0.0754" camera_y="0.029" camera_z="0.029"
+      camera_mass="0.085" hfov="1.0471975512" fps="60" width="640" height="512"/>
+    <joint name="${prefix}_right_stereo_camera_bracket_joint" type="fixed">
+      <origin xyz="0.015 -${separation} 0.0599" rpy="0 0 0" />
+      <parent link="${prefix}_stereo_camera_beam" />
+      <child link="${prefix}_right_camera" />
+    </joint>
+  </xacro:if>
+
+  <!-- If enabled, generate the bumblebee2 camera payload with a tilt of 0 degrees. -->
+  <xacro:include filename="$(find pointgrey_camera_description)/urdf/pointgrey_bumblebee2.urdf.xacro" />
+
+  <!-- If enabled, generate the bumblebee2 camera payload with a tilt of 0 degrees. -->
+  <!-- Disabled temporarily due to metapackage issue. -->
+  <xacro:if value="$(optenv JACKAL_BB2 0)">
+    <xacro:property name="tilt" value="$(optenv JACKAL_BB2_TILT 0)" />
+    <xacro:property name="name" value="$(optenv JACKAL_BB2_NAME front)" />
+    <xacro:property name="tower" value="$(optenv JACKAL_BB2_TOWER 1)"/>
+    <xacro:property name="mount" value="$(optenv JACKAL_BB2_MOUNT front)" />
+    <xacro:property name="prefix" value="$(optenv JACKAL_BB2_PREFIX ${mount})"/>
+    <xacro:property name="parent" value="$(optenv JACKAL_BB2_PARENT ${mount}_mount)"/>
+
+    <xacro:BB2-08S2C-38 frame="${prefix}_camera" name="${name}" />
+
+    <!-- Mount Enabled -->
+    <xacro:if value="${tower}">
+      <xacro:camera_mount prefix="${prefix}" tilt="${tilt}"/>
+
+      <joint name="${prefix}_camera_mount_joint" type="fixed">
+        <origin xyz="$(optenv JACKAL_BB2_OFFSET 0 0 0)"
+                rpy="$(optenv JACKAL_BB2_RPY 0 0 0)" />
+        <parent link="${parent}" />
+        <child link="${prefix}_camera_mount" />
+      </joint>
+
+      <joint name="${prefix}_camera_bracket_joint" type="fixed">
+        <origin xyz="0.007 0 0.02450" rpy="0 0 0" />
+        <parent link="${prefix}_camera_beam" />
+        <child link="${prefix}_camera" />
+      </joint>
+    </xacro:if>
+    <!-- Mount Disabled -->
+    <xacro:unless value="${tower}">
+      <link name="${prefix}_camera_base" type="fixed"/>
+      <joint name="${prefix}_camera_base_joint" type="fixed">
+        <origin xyz="$(optenv JACKAL_BB2_OFFSET 0 0 0)"
+                rpy="$(optenv JACKAL_BB2_RPY 0 0 0)" />
+        <parent link="${parent}" />
+        <child link="${prefix}_camera_base" />
+      </joint>
+      <joint name="${prefix}_camera_joint" type="fixed">
+        <origin xyz="0 0 0.018" rpy="0 0 0" />
+        <parent link="${prefix}_camera_base" />
+        <child link="${prefix}_camera" />
+      </joint>
+    </xacro:unless>
+  </xacro:if>
+
+
+
+
+  <xacro:if value="$(optenv KINECT 1)">
+    <xacro:property name="name" value="$(optenv KINECT_NAME front)" />
+    <xacro:property name="tilt" value="$(optenv KINECT_TILT 0.)" />
+    <xacro:property name="tower" value="$(optenv KINECT_TOWER 1)" />
+    <xacro:property name="mount" value="$(optenv KINECT_MOUNT front)" />
+    <xacro:property name="prefix" value="$(optenv KINECT_PREFIX ${mount})" />
+    <xacro:property name="parent" value="$(optenv KINECT_PARENT ${mount}_mount)" />
+
+    <xacro:include filename="$(find jackal_description)/urdf/accessories/kinect.urdf.xacro"/>
+    <xacro:kinect_camera prefix="front"/>
+
+    <!-- Mount Enabled -->
+    <xacro:if value="${tower}">
+      <xacro:camera_mount prefix="${prefix}" tilt="${tilt}"/>
+
+      <joint name="${prefix}_camera_mount_joint" type="fixed">
+        <origin xyz="$(optenv JACKAL_KINECT_OFFSET 0 0 0)"
+                rpy="$(optenv JACKAL_KINECT_RPY 0 0 0)" />
+        <parent link="${parent}" />
+        <child link="${prefix}_camera_mount" />
+      </joint>
+
+      <joint name="${prefix}_camera_bracket_joint" type="fixed">
+        <origin xyz="0.020 0 -0.0245" rpy="0 0 0" />
+        <parent link="${prefix}_camera_beam" />
+        <child link="${prefix}_kinect" />
+      </joint>
+    </xacro:if>
+
+    <!-- Mount Disabled -->
+    <xacro:unless value="${tower}">
+      <link name="${prefix}_camera_base"/>
+      <joint name="${prefix}_camera_base_joint" type="fixed">
+        <origin xyz="$(optenv JACKAL_KINECT_OFFSET 0 0 0)"
+                rpy="$(optenv JACKAL_KINECT_RPY 0 0 0)" />
+        <parent link="${parent}" />
+        <child link="${prefix}_camera_base" />
+      </joint>
+      <joint name="${prefix}_camera_joint" type="fixed">
+        <origin xyz="0 0 0.0145"
+                rpy="0 0 0" />
+        <parent link="${prefix}_camera_base" />
+        <child link="${prefix}_kinect" />
+      </joint>
+    </xacro:unless>
+  </xacro:if>
+  
+
+  <!-- BlackflyS Camera -->
+  <xacro:include filename="$(find flir_camera_description)/urdf/flir_blackflys.urdf.xacro"/>
+  <xacro:if value="$(optenv JACKAL_BLACKFLY 0)">
+    <xacro:property name="prefix" value="$(optenv JACKAL_BLACKFLY_PREFIX front_camera)"/>
+    <xacro:property name="parent" value="$(optenv JACKAL_BLACKFLY_PARENT front_mount)"/>
+    <xacro:property name="xyz"    value="$(optenv JACKAL_BLACKFLY_OFFSET 0 0 0)"/>
+    <xacro:property name="rpy"    value="$(optenv JACKAL_BLACKFLY_RPY    0 0 0)"/>
+    <xacro:flir_blackflys frame="${prefix}"/>
+    <joint name="${prefix}_mount_joint" type="fixed">
+      <child link="${prefix}"/>
+      <parent link="${parent}"/>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+    </joint>
+  </xacro:if>
+
+  <!-- Fender Accessories --> 
+  <xacro:if value="$(optenv JACKAL_FRONT_ACCESSORY_FENDER 0)">
+    <xacro:include filename="$(find jackal_description)/urdf/accessories/hokuyo_ust10.urdf.xacro" />
+    <xacro:if value="$(optenv JACKAL_FRONT_FENDER_UST10 0)">
+      <xacro:hokuyo_ust10_mount
+        prefix="front"
+        parent_link="front_fender_accessory_link"
+        min_angle="${-pi/2}"
+        max_angle="${pi/2}"
+        topic="$(optenv JACKAL_FRONT_LASER_TOPIC front/scan)">
+        <origin xyz="0 0 0" rpy="0 0 0" />
+      </xacro:hokuyo_ust10_mount>
+    </xacro:if>
+  </xacro:if>
+
+  <xacro:if value="$(optenv JACKAL_REAR_ACCESSORY_FENDER 0)">
+    <xacro:if value="$(optenv JACKAL_REAR_FENDER_UST10 0)">
+      <xacro:hokuyo_ust10_mount
+        prefix="rear"
+        parent_link="rear_fender_accessory_link"
+        min_angle="${-pi/2}"
+        max_angle="${pi/2}"
+        topic="$(optenv JACKAL_REAR_LASER_TOPIC rear/scan)">
+        <origin xyz="0 0 0" rpy="0 0 0" />
+      </xacro:hokuyo_ust10_mount>
+    </xacro:if>
+  </xacro:if>
+
+  <!-- Bumper Accessories -->
+  <xacro:if value="$(optenv JACKAL_WIBOTIC_BUMPER 0)">
+    <link name="wibotic_bumper">
+      <visual>
+        <geometry>
+          <mesh filename="package://jackal_description/meshes/wibotic_bumper.stl" />
+        </geometry>
+        <material name="dark_grey" />
+      </visual>
+      <collision>
+        <geometry>
+          <box size="0.115 0.25 0.145"/>
+        </geometry>
+        <origin xyz="-0.017575 0 0.0575" rpy="0 0 0"/>
+      </collision>
+    </link>
+    <joint name="wibotic_bumper_joint" type="fixed">
+      <origin xyz="0.285 0 0.07455" rpy="0 0 0"/>
+      <parent link="base_link"/>
+      <child link="wibotic_bumper"/>
+    </joint>
+  </xacro:if>
+
+  <xacro:if value="$(optenv JACKAL_ARK_ENCLOSURE 0)">
+    <link name="ark_enclosure_link">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://jackal_description/meshes/ark_enclosure.stl"/>
+        </geometry>
+        <material name="dark_grey" />
+      </visual>
+    </link>
+    <joint name="ark_enclosure_joint" type="fixed">
+      <parent link="mid_mount" />
+      <child link="ark_enclosure_link" />
+    </joint>
+    <gazebo reference="ark_enclosure_link">
+      <material>Gazebo/DarkGrey</material>
+      <turnGravityOff>false</turnGravityOff>
+    </gazebo>
+  </xacro:if>
+
+</robot>
--- a/src/jackal_description/urdf/accessories/bridge_plate.urdf.xacro
+++ b/src/jackal_description/urdf/accessories/bridge_plate.urdf.xacro
@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="bridge_plate" params="prefix parent height *origin">
+    <link name="${prefix}_bridge_base"/>
+    <joint name="${prefix}_bridge_base_joint" type="fixed">
+      <parent link="${parent}"/>
+      <child link="${prefix}_bridge_base" />
+      <xacro:insert_block name="origin" />
+    </joint>
+
+    <joint name="${prefix}_bridge_joint" type="fixed">
+      <parent link="${prefix}_bridge_base"/>
+      <child link="${prefix}_bridge"/>
+      <origin xyz="0.0 0.0 ${height}" rpy="0 0 0"/>
+    </joint>
+
+    <link name="${prefix}_bridge">
+      <visual>
+        <origin xyz="0 0 0" rpy="1.5707 0.0 1.5707" />
+        <geometry>
+          <mesh filename="package://jackal_description/meshes/bridge-plate.stl"/>
+        </geometry>
+        <material name="dark_grey" />
+      </visual>
+    </link>
+
+    <xacro:standoff_box prefix="${prefix}" parent="${prefix}_bridge_base" height="${height}" />
+  </xacro:macro>
+</robot>
--- a/src/jackal_description/urdf/accessories/camera_mount.urdf.xacro
+++ b/src/jackal_description/urdf/accessories/camera_mount.urdf.xacro
@ -0,0 +1,41 @@
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:macro name="camera_mount" params="prefix tilt:=0">
+
+    <link name="${prefix}_camera_mount">
+      <visual>
+        <origin xyz="0 0 0" rpy="1.5707 0 1.5707" />
+        <geometry>
+          <mesh filename="package://jackal_description/meshes/camera-bracket.stl" scale="0.001 0.001 0.001" />
+        </geometry>
+        <material name="dark_grey" />
+      </visual>
+    </link>
+
+    <link name="${prefix}_camera_beam">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 1.5707" />
+        <geometry>
+          <mesh filename="package://jackal_description/meshes/camera-beam.stl" scale="0.001 0.001 0.001" />
+        </geometry>
+        <material name="dark_grey" />
+      </visual>
+    </link>
+
+    <joint name="${prefix}_camera_pivot_joint" type="fixed">
+      <origin xyz="0.102 0 0.008" rpy="0 ${tilt} 0" />
+      <parent link="${prefix}_camera_mount" />
+      <child link="${prefix}_camera_beam" />
+    </joint>
+
+    <gazebo reference="${prefix}_camera_beam">
+      <material>Gazebo/DarkGrey</material>
+    </gazebo>
+
+    <gazebo reference="${prefix}_camera_mount">
+      <material>Gazebo/DarkGrey</material>
+    </gazebo>
+
+  </xacro:macro>
+
+</robot>
--- a/src/jackal_description/urdf/accessories/hdl32_mount.urdf.xacro
+++ b/src/jackal_description/urdf/accessories/hdl32_mount.urdf.xacro
@ -0,0 +1,46 @@
+<robot xmlns:xacro="http://wiki.ros.org/xacro">
+  <xacro:include filename="$(find velodyne_description)/urdf/HDL-32E.urdf.xacro" />
+
+  <xacro:macro name="hdl32e_mount" params="prefix parent_link topic tower *origin">    
+    <xacro:if value="${tower}">
+      <link name="${prefix}_hdl32e_mount">
+        <visual>
+          <geometry>
+            <mesh filename="package://jackal_description/meshes/velodyne_tower.stl"/>
+          </geometry>
+          <origin xyz="0 0 0" rpy="0 0 0" /> 
+          <material name="black" />
+        </visual>
+        <collision>
+          <geometry>
+            <mesh filename="package://jackal_description/meshes/velodyne_tower.stl"/>
+          </geometry>
+          <origin xyz="0 0 0" rpy="0 0 0" /> 
+        </collision>
+      </link>
+
+      <joint name="${prefix}_hdl32e_mount_joint" type="fixed">
+        <parent link="${parent_link}" />
+        <child link="${prefix}_hdl32e_mount" />
+        <xacro:insert_block name="origin" />
+      </joint>
+
+      <!-- Velodyne HDL-32E Macro treats Y-axis as forward in pointcloud --> 
+      <xacro:HDL-32E parent="${prefix}_hdl32e_mount" topic="${topic}">
+        <origin xyz="0 0 0.081" rpy="0 0 -1.5707" />
+      </xacro:HDL-32E>
+    </xacro:if>
+    <xacro:unless value="${tower}">
+      <link name="${prefix}_hdl32e_mount"/>
+      <joint name="${prefix}_hdl32e_mount_joint" type="fixed">
+        <parent link="${parent_link}"/>
+        <child link="${prefix}_hdl32e_mount"/>
+        <xacro:insert_block name="origin" />      
+      </joint>
+      <!-- Velodyne HDL-32E Macro treats Y-axis as forward in pointcloud --> 
+      <xacro:HDL-32E parent="${prefix}_hdl32e_mount" topic="${topic}">
+        <origin xyz="0 0 0" rpy="0 0 -1.5707"/>
+      </xacro:HDL-32E>
+    </xacro:unless>
+  </xacro:macro>
+</robot>
--- a/src/jackal_description/urdf/accessories/hokuyo_ust10.urdf.xacro
+++ b/src/jackal_description/urdf/accessories/hokuyo_ust10.urdf.xacro
@ -0,0 +1,84 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="hokuyo_ust10_mount" params="prefix topic parent_link min_angle:=-2.35619 max_angle:=2.35619 *origin">
+
+    <xacro:macro name="hokuyo_ust10" params="frame:=laser topic:=scan sample_size:=720 update_rate:=50
+               min_angle:=-2.35619 max_angle:=2.35619 min_range:=0.1 max_range:=30.0 robot_namespace:=/">
+      <link name="${frame}">
+        <inertial>
+          <mass value="1.1" />
+          <origin xyz="0 0 0" />
+          <inertia ixx="${0.0833333 * 1.1 * (0.102*0.102 + 0.152*0.152)}" ixy="0.0" ixz="0.0"
+            iyy="${0.0833333 * 1.1 * (0.105*0.105 + 0.152*0.152)}" iyz="0.0"
+            izz="${0.0833333 * 1.1 * (0.105*0.105 + 0.102*0.102)}" />
+        </inertial>
+      </link>
+
+      <gazebo reference="${frame}">
+        <turnGravityOff>true</turnGravityOff>
+        <sensor type="ray" name="${frame}">
+          <pose>0 0 0 0 0 0</pose>
+          <visualize>false</visualize>
+          <update_rate>${update_rate}</update_rate>
+          <ray>
+            <scan>
+              <horizontal>
+                <samples>${sample_size}</samples>
+                <resolution>1</resolution>
+                <min_angle>${min_angle}</min_angle>
+                <max_angle>${max_angle}</max_angle>
+              </horizontal>
+            </scan>
+            <range>
+              <min>${min_range}</min>
+              <max>${max_range}</max>
+              <resolution>0.01</resolution>
+            </range>
+            <noise>
+              <type>gaussian</type>
+              <mean>0.0</mean>
+              <stddev>0.001</stddev>
+            </noise>
+          </ray>
+          <plugin name="gazebo_ros_laser" filename="libgazebo_ros_laser.so">
+            <topicName>${topic}</topicName>
+            <frameName>${frame}</frameName>
+            <robotNamespace>${robot_namespace}</robotNamespace>
+          </plugin>
+        </sensor>
+      </gazebo>
+    </xacro:macro>
+
+    <xacro:hokuyo_ust10 frame="${prefix}_laser" topic="${topic}"/>
+
+    <joint name="${prefix}_laser_mount_joint" type="fixed">
+      <xacro:insert_block name="origin" />
+      <parent link="${parent_link}" />
+      <child link="${prefix}_laser_mount" />
+    </joint>
+
+    <link name="${prefix}_laser_mount">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <!-- Origin of this mesh is the base of the bracket. -->
+          <mesh filename="package://jackal_description/meshes/hokuyo_ust10.stl" />
+        </geometry>
+        <material name="dark_grey" />
+      </visual>
+    </link>
+
+    <joint name="${prefix}_laser_joint" type="fixed">
+      <!-- This offset is from the base of the bracket to the LIDAR's focal point. -->
+      <origin xyz="0 0 0.0474" rpy="0 0 0" />
+      <parent link="${prefix}_laser_mount" />
+      <child link="${prefix}_laser" />
+    </joint>
+
+    <gazebo reference="${prefix}_laser_mount">
+      <material>Gazebo/DarkGrey</material>
+    </gazebo>
+
+  </xacro:macro>
+
+</robot>
--- a/src/jackal_description/urdf/accessories/hokuyo_utm30.urdf.xacro
+++ b/src/jackal_description/urdf/accessories/hokuyo_utm30.urdf.xacro
@ -0,0 +1,87 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="hokuyo_utm30_mount" params="prefix topic parent_link min_angle:=-2.2689  max_angle:=2.2689 *origin">
+
+    <xacro:macro name="hokuyo_utm30" params="frame:=laser topic:=scan sample_size:=1440 update_rate:=50
+               min_angle:=-2.2689 max_angle:=2.2689 min_range:=0.1 max_range:=30.0 robot_namespace:=/">
+      <xacro:property name="mass" value="0.370"/>
+      <xacro:property name="base" value="0.06" />
+      <xacro:property name="height" value="0.087"/>
+      <link name="${frame}">
+        <inertial>
+          <mass value="${mass}" />
+          <origin xyz="0 0 0" />
+          <inertia ixx="${mass/12 * (base*base + height*height)}" ixy="0.0" ixz="0.0"
+                   iyy="${mass/12 * (base*base + height*height)}" iyz="0.0"
+                   izz="${mass/12 * 2 * (base * base)}" />
+        </inertial>
+      </link>
+
+      <gazebo reference="${frame}">
+        <turnGravityOff>true</turnGravityOff>
+        <sensor type="ray" name="${frame}">
+          <pose>0 0 0 0 0 0</pose>
+          <visualize>false</visualize>
+          <update_rate>${update_rate}</update_rate>
+          <ray>
+            <scan>
+              <horizontal>
+                <samples>${sample_size}</samples>
+                <resolution>1</resolution>
+                <min_angle>${min_angle}</min_angle>
+                <max_angle>${max_angle}</max_angle>
+              </horizontal>
+            </scan>
+            <range>
+              <min>${min_range}</min>
+              <max>${max_range}</max>
+              <resolution>0.01</resolution>
+            </range>
+            <noise>
+              <type>gaussian</type>
+              <mean>0.0</mean>
+              <stddev>0.001</stddev>
+            </noise>
+          </ray>
+          <plugin name="gazebo_ros_laser" filename="libgazebo_ros_laser.so">
+            <topicName>${topic}</topicName>
+            <frameName>${frame}</frameName>
+            <robotNamespace>${robot_namespace}</robotNamespace>
+          </plugin>
+        </sensor>
+      </gazebo>
+    </xacro:macro>
+
+    <xacro:hokuyo_utm30 frame="${prefix}_laser" topic="${topic}"/>
+
+    <joint name="${prefix}_laser_mount_joint" type="fixed">
+      <xacro:insert_block name="origin" />
+      <parent link="${parent_link}" />
+      <child link="${prefix}_laser_mount" />
+    </joint>
+
+    <link name="${prefix}_laser_mount">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <!-- Origin of this mesh is the base of the bracket. -->
+          <mesh filename="package://jackal_description/meshes/hokuyo_utm30.stl" />
+        </geometry>
+        <material name="dark_grey" />
+      </visual>
+    </link>
+
+    <joint name="${prefix}_laser_joint" type="fixed">
+      <!-- This offset is from the base of the bracket to the LIDAR's focal point. -->
+      <origin xyz="0 0 0.055" rpy="0 0 0" />
+      <parent link="${prefix}_laser_mount" />
+      <child link="${prefix}_laser" />
+    </joint>
+
+    <gazebo reference="${prefix}_laser_mount">
+      <material>Gazebo/DarkGrey</material>
+    </gazebo>
+
+  </xacro:macro>
+
+</robot>
--- a/src/jackal_description/urdf/accessories/kinect.urdf.xacro
+++ b/src/jackal_description/urdf/accessories/kinect.urdf.xacro
@ -0,0 +1,67 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="kinect_camera">
+    <xacro:macro name="kinect_camera" params="prefix:=camera">
+        <!-- Create kinect reference frame -->
+        <!-- Add mesh for kinect -->
+        <link name="${prefix}_kinect">
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0"/>
+                <geometry>
+                    <mesh filename="package://jackal_description/meshes/kinect.dae" scale="0.4 0.4 0.4" />
+                </geometry>
+            </visual>
+            <collision>
+                <geometry>
+                    <box size="0.07 0.3 0.09"/>
+                </geometry>
+            </collision>
+        </link>
+ 
+        <joint name="${prefix}_optical_joint" type="fixed">
+            <origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/>
+            <parent link="${prefix}_kinect"/>
+            <child link="${prefix}_frame_optical"/>
+        </joint>
+ 
+        <link name="${prefix}_frame_optical"/>
+ 
+        <gazebo reference="${prefix}_kinect">
+            <sensor type="depth" name="${prefix}">
+                <always_on>true</always_on>
+                <update_rate>20.0</update_rate>
+                <camera>
+                    <horizontal_fov>${60.0*M_PI/180.0}</horizontal_fov>
+                    <image>
+                        <format>R8G8B8</format>
+                        <width>640</width>
+                        <height>480</height>
+                    </image>
+                    <clip>
+                        <near>0.05</near>
+                        <far>8.0</far>
+                    </clip>
+                </camera>
+                <plugin name="kinect_${prefix}_controller" filename="libgazebo_ros_openni_kinect.so">
+                    <cameraName>${prefix}</cameraName>
+                    <alwaysOn>true</alwaysOn>
+                    <updateRate>10</updateRate>
+                    <imageTopicName>rgb/image_raw</imageTopicName>
+                    <depthImageTopicName>depth/image_raw</depthImageTopicName>
+                    <pointCloudTopicName>depth/points</pointCloudTopicName>
+                    <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
+                    <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
+                    <frameName>${prefix}_frame_optical</frameName>
+                    <baseline>0.1</baseline>
+                    <distortion_k1>0.0</distortion_k1>
+                    <distortion_k2>0.0</distortion_k2>
+                    <distortion_k3>0.0</distortion_k3>
+                    <distortion_t1>0.0</distortion_t1>
+                    <distortion_t2>0.0</distortion_t2>
+                    <pointCloudCutoff>0.4</pointCloudCutoff>
+                </plugin>
+            </sensor>
+        </gazebo>
+ 
+    </xacro:macro>
+</robot>
--- a/src/jackal_description/urdf/accessories/novatel_smart6.urdf.xacro
+++ b/src/jackal_description/urdf/accessories/novatel_smart6.urdf.xacro
@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="novatel_smart6" params="prefix">
+  <link name="${prefix}_navsat">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://jackal_description/meshes/novatel-smart6.stl"/>
+      </geometry>
+      <material name="novatel_white">
+        <color rgba="0.8 0.8 0.8 1.0" />
+      </material>
+    </visual>
+  </link>
+  </xacro:macro>
+</robot>
--- a/src/jackal_description/urdf/accessories/novatel_smart7.urdf.xacro
+++ b/src/jackal_description/urdf/accessories/novatel_smart7.urdf.xacro
@ -0,0 +1,46 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="novatel_smart7" params="prefix">
+    <link name="${prefix}_navsat">
+      <visual>
+        <origin xyz="-0.095 0 0" rpy="${pi/2} 0 -${pi/2}" />
+        <geometry>
+          <mesh filename="package://jackal_description/meshes/novatel-smart7.stl"/>
+        </geometry>
+        <material name="novatel_white">
+          <color rgba="0.8 0.8 0.8 1.0" />
+        </material>
+      </visual>
+      <collision>
+        <origin xyz="-0.095 0 0" rpy="${pi/2} 0 -${pi/2}" />
+        <geometry>
+          <mesh filename="package://jackal_description/meshes/novatel-smart7.stl"/>
+        </geometry>
+      </collision>
+    </link>
+  </xacro:macro>
+
+  <xacro:macro name="novatel_smart7_stand" params="prefix parent height:=14.5 *origin">
+    <link name="${prefix}_navsat_stand">
+      <visual>
+        <origin xyz="0 0 ${height/2}" rpy="0 0 0"/>
+        <geometry>
+          <box size="0.045 0.025 ${height}" />
+        </geometry>
+        <material name="black" />
+      </visual>
+      <collision>
+        <origin xyz="0 0 ${height/2}" rpy="0 0 0"/>
+        <geometry>
+          <box size="0.045 0.025 ${height}" />
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${prefix}_navsat_stand_joint" type="fixed">
+      <xacro:insert_block name="origin" />
+      <parent link="${parent}" />
+      <child link="${prefix}_navsat_stand" />
+    </joint>
+  </xacro:macro>
+</robot>
--- a/src/jackal_description/urdf/accessories/sick_lms1xx_inverted_mount.urdf.xacro
+++ b/src/jackal_description/urdf/accessories/sick_lms1xx_inverted_mount.urdf.xacro
@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<!--
+Copyright (c) 2013, Goncalo Cabrita, ISR University of Coimbra
+Copyright (c) 2014, 2015 Clearpath Robotics
+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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE FOR ANY
+DIRECT, INDIRECT, 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://www.ros.org/wiki/xacro">
+
+  <xacro:include filename="$(find lms1xx)/urdf/sick_lms1xx.urdf.xacro" />
+
+  <xacro:macro name="sick_lms1xx_inverted_mount" params="prefix topic parent_link *origin">
+
+    <xacro:sick_lms1xx frame="${prefix}_laser" topic="${topic}" />
+
+    <joint name="${prefix}_laser_mount_joint" type="fixed">
+      <xacro:insert_block name="origin" />
+      <parent link="${parent_link}" />
+      <child link="${prefix}_laser_mount" />
+    </joint>
+
+    <link name="${prefix}_laser_mount">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <!-- Origin of this mesh is the base of the bracket. -->
+          <mesh filename="package://jackal_description/meshes/sick-lms1xx-inverted-bracket.stl" />
+        </geometry>
+        <material name="dark_grey" />
+      </visual>
+    </link>
+
+    <joint name="${prefix}_laser_joint" type="fixed">
+      <!-- This offset is from the base of the bracket to the LIDAR's focal point. -->
+      <origin xyz="0.04 0 0.066" rpy="3.141592 0 0" />
+      <parent link="${prefix}_laser_mount" />
+      <child link="${prefix}_laser" />
+    </joint>
+
+    <gazebo reference="${prefix}_laser_mount">
+      <material>Gazebo/DarkGrey</material>
+    </gazebo>
+
+  </xacro:macro>
+
+</robot>
--- a/src/jackal_description/urdf/accessories/sick_lms1xx_upright_mount.urdf.xacro
+++ b/src/jackal_description/urdf/accessories/sick_lms1xx_upright_mount.urdf.xacro
@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<!--
+Copyright (c) 2013, Goncalo Cabrita, ISR University of Coimbra
+Copyright (c) 2014, 2015 Clearpath Robotics
+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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE FOR ANY
+DIRECT, INDIRECT, 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://www.ros.org/wiki/xacro">
+
+  <xacro:include filename="$(find lms1xx)/urdf/sick_lms1xx.urdf.xacro" />
+
+  <xacro:macro name="sick_lms1xx_upright_mount" params="prefix topic parent_link *origin">
+
+    <xacro:sick_lms1xx frame="${prefix}_laser" topic="${topic}" />
+
+    <joint name="${prefix}_laser_mount_joint" type="fixed">
+      <xacro:insert_block name="origin" />
+      <parent link="${parent_link}" />
+      <child link="${prefix}_laser_mount" />
+    </joint>
+
+    <link name="${prefix}_laser_mount">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <!-- Origin of this mesh is the base of the bracket. -->
+          <mesh filename="package://jackal_description/meshes/sick-lms1xx-upright-bracket.stl" />
+        </geometry>
+        <material name="dark_grey" />
+      </visual>
+    </link>
+
+    <joint name="${prefix}_laser_joint" type="fixed">
+      <!-- This offset is from the base of the bracket to the LIDAR's focal point. -->
+      <origin xyz="0 0 0.149" rpy="0 0 0" />
+      <parent link="${prefix}_laser_mount" />
+      <child link="${prefix}_laser" />
+    </joint>
+
+    <gazebo reference="${prefix}_laser_mount">
+      <material>Gazebo/DarkGrey</material>
+    </gazebo>
+
+  </xacro:macro>
+
+</robot>
--- a/src/jackal_description/urdf/accessories/standoffs.urdf.xacro
+++ b/src/jackal_description/urdf/accessories/standoffs.urdf.xacro
@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <!-- Generate a single payload plate standoff. -->
+  <xacro:macro name="standoff" params="name length parent *origin">
+    <link name="${name}">
+      <visual>
+        <origin xyz="0 0 ${length/2}" rpy="0 0 0" />
+        <geometry>
+          <cylinder radius="0.004" length="${length}" />
+        </geometry>
+        <material name="light_grey" />
+      </visual>
+    </link>
+
+    <joint name="${name}_joint" type="fixed">
+      <xacro:insert_block name="origin" />
+      <parent link="${parent}" />
+      <child link="${name}" />
+    </joint>
+  </xacro:macro>
+  
+  <!-- Generate a set of four standoffs from a payload mount point. -->
+  <xacro:macro name="standoff_box" params="prefix parent height">
+    <xacro:standoff name="${prefix}_standoff0" length="${height}" parent="${parent}">
+      <origin xyz="0.060 0.060 0" rpy="0 0 0"/>
+    </xacro:standoff>
+    <xacro:standoff name="${prefix}_standoff1" length="${height}" parent="${parent}">
+      <origin xyz="0.060 -0.060 0" rpy="0 0 0"/>
+    </xacro:standoff>
+    <xacro:standoff name="${prefix}_standoff2" length="${height}" parent="${parent}">
+      <origin xyz="-0.060 0.060 0" rpy="0 0 0"/>
+    </xacro:standoff>
+    <xacro:standoff name="${prefix}_standoff3" length="${height}" parent="${parent}">
+      <origin xyz="-0.060 -0.060 0" rpy="0 0 0"/>
+    </xacro:standoff>
+  </xacro:macro>
+</robot>
--- a/src/jackal_description/urdf/accessories/stereo_camera_mount.urdf.xacro
+++ b/src/jackal_description/urdf/accessories/stereo_camera_mount.urdf.xacro
@ -0,0 +1,49 @@
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:macro name="stereo_camera_mount" params="prefix tilt:=0">
+
+    <link name="${prefix}_stereo_camera_mount" />
+
+    <link name="${prefix}_stereo_camera_bracket">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://jackal_description/meshes/stereo-camera-bracket.stl" />
+        </geometry>
+        <material name="dark_grey" />
+      </visual>
+    </link>
+
+    <link name="${prefix}_stereo_camera_beam">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://jackal_description/meshes/stereo-camera-beam.stl" />
+        </geometry>
+        <material name="dark_grey" />
+      </visual>
+    </link>
+
+    <joint name="${prefix}_stereo_camera_bracket_joint" type="fixed">
+      <origin xyz="0.060 0 0.0225" rpy="0 0 0" />
+      <parent link="${prefix}_stereo_camera_mount" />
+      <child link="${prefix}_stereo_camera_bracket" />
+    </joint>
+
+    <joint name="${prefix}_stereo_camera_pivot_joint" type="fixed">
+      <origin xyz="0 0 0" rpy="0 ${tilt} 0" />
+      <parent link="${prefix}_stereo_camera_bracket" />
+      <child link="${prefix}_stereo_camera_beam" />
+    </joint>
+
+    <gazebo reference="${prefix}_stereo_camera_beam">
+      <material>Gazebo/DarkGrey</material>
+    </gazebo>
+
+    <gazebo reference="${prefix}_stereo_camera_bracket">
+      <material>Gazebo/DarkGrey</material>
+    </gazebo>
+
+  </xacro:macro>
+
+</robot>
--- a/src/jackal_description/urdf/accessories/vlp16_mount.urdf.xacro
+++ b/src/jackal_description/urdf/accessories/vlp16_mount.urdf.xacro
@ -0,0 +1,104 @@
+<robot xmlns:xacro="http://wiki.ros.org/xacro">
+  <xacro:include filename="$(find velodyne_description)/urdf/VLP-16.urdf.xacro" />
+
+  <xacro:macro name="vlp16_mount" params="prefix parent_link topic tower height:=0.1 *origin">    
+    <xacro:if value="${tower}">
+      <link name="${prefix}_vlp16_mount" />
+
+      <link name="${prefix}_vlp16_plate">
+        <visual>
+          <geometry>
+            <box size="0.1 0.1 0.002" />
+          </geometry>
+          <material name="black" />
+          <origin xyz="0 0 -0.001" rpy="0 0 0" />
+        </visual>
+      </link>
+      
+      <link name="${prefix}_vlp16_leg1">
+        <visual>
+          <geometry>
+            <cylinder length="${height}" radius="0.003" />
+          </geometry>
+          <material name="white">
+            <color rgba="1 1 1 1" />
+          </material>
+          <origin xyz="0 0 ${height/2}" rpy="0 0 0" />
+        </visual>
+      </link>
+      <link name="${prefix}_vlp16_leg2">
+        <visual>
+          <geometry>
+            <cylinder length="${height}" radius="0.003" />
+          </geometry>
+          <material name="white">
+            <color rgba="1 1 1 1" />
+          </material>
+          <origin xyz="0 0 ${height/2}" rpy="0 0 0" />
+        </visual>
+      </link>
+      <link name="${prefix}_vlp16_leg3">
+        <visual>
+          <geometry>
+            <cylinder length="${height}" radius="0.003" />
+          </geometry>
+          <material name="white">
+            <color rgba="1 1 1 1" />
+          </material>
+          <origin xyz="0 0 ${height/2}" rpy="0 0 0" />
+        </visual>
+      </link>
+      <link name="${prefix}_vlp16_leg4">
+        <visual>
+          <geometry>
+            <cylinder length="${height}" radius="0.003" />
+          </geometry>
+          <material name="white">
+            <color rgba="1 1 1 1" />
+          </material>
+          <origin xyz="0 0 ${height/2}" rpy="0 0 0" />
+        </visual>
+      </link>
+      <joint name="${prefix}_vlp16_mount_joint" type="fixed">
+        <parent link="${parent_link}" />
+        <child link="${prefix}_vlp16_mount" />
+        <xacro:insert_block name="origin" />
+      </joint>
+
+      <joint name="${prefix}_vlp16_leg1_joint" type="fixed">
+        <parent link="${prefix}_vlp16_mount" />
+        <child link="${prefix}_vlp16_leg1" />
+        <origin xyz="0.05 0.05 0" rpy="0 0 0" />
+      </joint>
+      <joint name="${prefix}_vlp16_leg2_joint" type="fixed">
+        <parent link="${prefix}_vlp16_mount" />
+        <child link="${prefix}_vlp16_leg2" />
+        <origin xyz="0.05 -0.05 0" rpy="0 0 0" />
+      </joint>
+      <joint name="${prefix}_vlp16_leg3_joint" type="fixed">
+        <parent link="${prefix}_vlp16_mount" />
+        <child link="${prefix}_vlp16_leg3" />
+        <origin xyz="-0.05 0.05 0" rpy="0 0 0" />
+      </joint>
+      <joint name="${prefix}_vlp16_leg4_joint" type="fixed">
+        <parent link="${prefix}_vlp16_mount" />
+        <child link="${prefix}_vlp16_leg4" />
+        <origin xyz="-0.05 -0.05 0" rpy="0 0 0" />
+      </joint>
+      <joint name="${prefix}_vlp16_plate_joint" type="fixed">
+        <parent link="${prefix}_vlp16_mount" />
+        <child link="${prefix}_vlp16_plate" />
+        <origin xyz="0 0 ${height}" rpy="0 0 0" />
+      </joint>
+
+      <xacro:VLP-16 parent="${prefix}_vlp16_plate" topic="${topic}">
+        <origin xyz="0 0 0" rpy="0 0 0" />
+      </xacro:VLP-16>
+    </xacro:if>
+    <xacro:unless value="${tower}">
+      <xacro:VLP-16 parent="${parent_link}" topic="${topic}">
+        <xacro:insert_block name="origin" />
+      </xacro:VLP-16>
+    </xacro:unless>
+  </xacro:macro>
+</robot>
--- a/src/jackal_description/urdf/configs/base
+++ b/src/jackal_description/urdf/configs/base
@ -0,0 +1,3 @@
+# The base Jackal configuration has no accessories at all,
+# so nothing need be specified here; the defaults as given
+# in the URDF suffice to define this config.
--- a/src/jackal_description/urdf/configs/base.bat
+++ b/src/jackal_description/urdf/configs/base.bat
@ -0,0 +1,5 @@
+@echo off
+
+REM The base Jackal configuration has no accessories at all,
+REM so nothing need be specified here; the defaults as given
+REM in the URDF suffice to define this config.
--- a/src/jackal_description/urdf/configs/front_bumblebee2
+++ b/src/jackal_description/urdf/configs/front_bumblebee2
@ -0,0 +1,2 @@
+
+JACKAL_BB2=1
--- a/src/jackal_description/urdf/configs/front_bumblebee2.bat
+++ b/src/jackal_description/urdf/configs/front_bumblebee2.bat
@ -0,0 +1,3 @@
+@echo off
+
+set JACKAL_BB2=1
--- a/src/jackal_description/urdf/configs/front_flea3
+++ b/src/jackal_description/urdf/configs/front_flea3
@ -0,0 +1,2 @@
+
+JACKAL_FLEA3=1
--- a/src/jackal_description/urdf/configs/front_flea3.bat
+++ b/src/jackal_description/urdf/configs/front_flea3.bat
@ -0,0 +1,3 @@
+@echo off
+
+set JACKAL_FLEA3=1
--- a/src/jackal_description/urdf/configs/front_laser
+++ b/src/jackal_description/urdf/configs/front_laser
@ -0,0 +1,6 @@
+# The front_laser configuration of Jackal is sufficient for
+# basic gmapping and navigation. It is mostly the default
+# config, but with a SICK LMS100 series LIDAR on the front,
+# pointing forward.
+
+JACKAL_LASER=1
--- a/src/jackal_description/urdf/configs/front_laser.bat
+++ b/src/jackal_description/urdf/configs/front_laser.bat
@ -0,0 +1,8 @@
+@echo off
+
+REM The front_laser configuration of Jackal is sufficient for
+REM basic gmapping and navigation. It is mostly the default
+REM config, but with a SICK LMS100 series LIDAR on the front,
+REM pointing forward.
+
+set JACKAL_LASER=1
--- a/src/jackal_description/urdf/custom_example.urdf
+++ b/src/jackal_description/urdf/custom_example.urdf
@ -0,0 +1,64 @@
+<robot>
+  <!-- This file is an example that can be included from jackal.urdf.xacro
+       by setting the JACKAL_URDF_EXTRAS environment variable to the full
+       path of this file. -->
+
+  <xacro:camera_mount prefix="front" tilt="0.5236"/>
+  <joint name="front_camera_mount_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0" />
+    <parent link="front_mount" />
+    <child link="front_camera_mount" />
+  </joint>
+  <xacro:BB2-08S2C-38 frame="front_camera" name="front" />
+  <joint name="front_camera_bracket_joint" type="fixed">
+    <origin xyz="0.007 0 0.02450" rpy="0 0 0" />
+    <parent link="front_camera_beam" />
+    <child link="front_camera" />
+  </joint>
+
+  <xacro:sick_lms1xx_mount prefix="front" topic="front/scan"/>
+
+  <joint name="front_laser_mount_joint" type="fixed">
+    <origin xyz="0 0 0.002" rpy="0 0 0" />
+    <parent link="front_mount" />
+    <child link="front_laser_mount" />
+  </joint>
+
+  <xacro:camera_mount prefix="rear" tilt="0.5236"/>
+  <joint name="rear_camera_mount_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 3.141" />
+    <parent link="rear_mount" />
+    <child link="rear_camera_mount" />
+  </joint>
+  <xacro:BB2-08S2C-38 frame="rear_camera" name="rear" />
+  <joint name="rear_camera_bracket_joint" type="fixed">
+    <origin xyz="0.007 0 0.02450" rpy="0 0 0" />
+    <parent link="rear_camera_beam" />
+    <child link="rear_camera" />
+  </joint>
+
+  <xacro:sick_lms1xx_mount prefix="rear" topic="rear/scan"/>
+
+  <joint name="rear_laser_mount_joint" type="fixed">
+    <origin xyz="0 0 0.002" rpy="0 0 3.141" />>
+    <parent link="rear_mount" />
+    <child link="rear_laser_mount" />
+  </joint>
+
+  <xacro:bridge_plate mount="rear" height="0.20" />
+  <xacro:camera_mount prefix="rear_upper" tilt="0"/>
+  <joint name="rear_upper_camera_mount_joint" type="fixed">
+    <origin xyz="0 0 0.001" rpy="0 0 0" />
+    <parent link="rear_bridge" />
+    <child link="rear_upper_camera_mount" />
+  </joint>
+
+  <xacro:pointgrey_flea3 frame="rear_upper_camera" name="rear_upper"
+    camera_x="0.0754" camera_y="0.029" camera_z="0.035"
+    camera_mass="0.085" hfov="1.0471975512" fps="60" width="640" height="512"/>
+  <joint name="rear_upper_camera_bracket_joint" type="fixed">
+    <origin xyz="0.020 0 0.0245" rpy="0 0 0" />
+    <parent link="rear_upper_camera_beam" />
+    <child link="rear_upper_camera" />
+  </joint>
+</robot>
--- a/src/jackal_description/urdf/empty.urdf
+++ b/src/jackal_description/urdf/empty.urdf
@ -0,0 +1,6 @@
+<robot>
+  <!-- This file is a placeholder which is included by default from
+       jackal.urdf.xacro. If a robot is being customized and requires
+       additional URDF, set the JACKAL_URDF_EXTRAS environment variable
+       to the full path of the file you would like included. -->
+</robot>
--- a/src/jackal_description/urdf/jackal.gazebo
+++ b/src/jackal_description/urdf/jackal.gazebo
@ -0,0 +1,77 @@
+<?xml version="1.0"?>
+<robot>
+  <gazebo>
+    <plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
+      <alwaysOn>true</alwaysOn>
+      <updateRate>50.0</updateRate>
+      <bodyName>base_link</bodyName>
+      <topicName>/gazebo/ground_truth/state</topicName>
+      <gaussianNoise>0.0</gaussianNoise>
+      <frameName>world</frameName>
+      <xyzOffsets>0 0 0</xyzOffsets>
+      <rpyOffsets>0 0 0</rpyOffsets>
+    </plugin>
+  </gazebo>
+
+  <gazebo>
+    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+      <robotNamespace>/</robotNamespace>
+    </plugin>
+  </gazebo>
+
+  <gazebo>
+    <plugin name="imu_controller" filename="libhector_gazebo_ros_imu.so">
+      <robotNamespace>/</robotNamespace>
+      <updateRate>50.0</updateRate>
+      <bodyName>imu_link</bodyName>
+      <topicName>imu/data</topicName>
+      <accelDrift>0.005 0.005 0.005</accelDrift>
+      <accelGaussianNoise>0.005 0.005 0.005</accelGaussianNoise>
+      <rateDrift>0.005 0.005 0.005 </rateDrift>
+      <rateGaussianNoise>0.005 0.005 0.005 </rateGaussianNoise>
+      <headingDrift>0.005</headingDrift>
+      <headingGaussianNoise>0.005</headingGaussianNoise>
+    </plugin>
+  </gazebo>
+
+  <gazebo>
+    <plugin name="gps_controller" filename="libhector_gazebo_ros_gps.so">
+      <updateRate>40</updateRate>
+      <robotNamespace>/</robotNamespace>
+      <bodyName>navsat_link</bodyName>
+      <frameId>base_link</frameId>
+      <topicName>/navsat/fix</topicName>
+      <velocityTopicName>/navsat/vel</velocityTopicName>
+      <referenceLatitude>$(optenv GAZEBO_WORLD_LAT 49.9)</referenceLatitude>
+      <referenceLongitude>$(optenv GAZEBO_WORLD_LON 8.9)</referenceLongitude>
+      <referenceHeading>90</referenceHeading>
+      <referenceAltitude>0</referenceAltitude>
+      <drift>0.0001 0.0001 0.0001</drift>
+    </plugin>
+  </gazebo>
+
+  <gazebo reference="base_link">
+    <turnGravityOff>false</turnGravityOff>
+  </gazebo>
+
+  <gazebo reference="chassis_link">
+    <material>Gazebo/DarkGrey</material>
+    <turnGravityOff>false</turnGravityOff>
+  </gazebo>
+  <gazebo reference="imu_link">
+    <turnGravityOff>false</turnGravityOff>
+  </gazebo>
+
+  <gazebo reference="navsat_link">
+    <material>Gazebo/DarkGrey</material>
+    <turnGravityOff>false</turnGravityOff>
+  </gazebo>
+  <gazebo reference="front_fender_link">
+    <material>Gazebo/Yellow</material>
+    <turnGravityOff>false</turnGravityOff>
+  </gazebo>
+  <gazebo reference="rear_fender_link">
+    <material>Gazebo/Yellow</material>
+    <turnGravityOff>false</turnGravityOff>
+  </gazebo>
+</robot>
--- a/src/jackal_description/urdf/jackal.urdf.xacro
+++ b/src/jackal_description/urdf/jackal.urdf.xacro
@ -0,0 +1,262 @@
+<?xml version="1.0"?>
+<robot name="jackal" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:property name="PI" value="3.1415926535897931" />
+
+  <xacro:property name="wheelbase" value="0.262" />
+  <xacro:property name="track" value="0.37559" />
+  <xacro:property name="wheel_vertical_offset" value="0.0345" />
+  <xacro:property name="footprint_vertical_offset" value="-0.0655" />
+
+  <xacro:property name="wheel_radius" value="0.098" />
+  <xacro:property name="wheel_width" value="0.040" />
+
+  <xacro:property name="chassis_length" value="0.420" />
+  <xacro:property name="chassis_width" value="0.310" />
+  <xacro:property name="chassis_height" value="0.184" />
+
+  <xacro:property name="dummy_inertia" value="1e-09"/>
+
+  <xacro:property name="mount_spacing" value="0.120" />
+
+  <material name="dark_grey"><color rgba="0.2 0.2 0.2 1.0" /></material>
+  <material name="light_grey"><color rgba="0.4 0.4 0.4 1.0" /></material>
+  <material name="yellow"><color rgba="0.8 0.8 0.0 1.0" /></material>
+  <material name="black"><color rgba="0.15 0.15 0.15 1.0" /></material>
+
+  <xacro:macro name="wheel" params="prefix *joint_pose">
+
+    <link name="${prefix}_wheel_link">
+      <visual>
+        <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
+        <geometry>
+          <mesh filename="package://jackal_description/meshes/jackal-wheel.stl"/>
+        </geometry>
+        <material name="black" />
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
+        <geometry>
+          <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="0.477"/>
+        <inertia
+          ixx="0.0013" ixy="0" ixz="0"
+          iyy="0.0024" iyz="0"
+          izz="0.0013"/>
+      </inertial>
+    </link>
+
+    <gazebo reference="${prefix}_wheel_link">
+      <material>Gazebo/DarkGrey</material>
+      <selfCollide>false</selfCollide>
+      <mu1 value="0.5"/>
+      <mu2 value="0.5"/>
+      <kp value="10000000.0" />
+      <kd value="1" />
+      <fdir1 value="1 0 0"/>
+    </gazebo>
+
+    <joint name="${prefix}_wheel" type="continuous">
+      <parent link="chassis_link"/>
+      <child link="${prefix}_wheel_link" />
+      <xacro:insert_block name="joint_pose" />
+      <axis xyz="0 1 0" />
+    </joint>
+
+    <!-- In reality, Jackal has only two motors, one per side. However, it's more
+         straightforward for Gazebo to simulate as if there's an actuator per wheel. -->
+    <transmission name="${prefix}_wheel_trans">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}_wheel">
+        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}_actuator">
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+    </transmission>
+  </xacro:macro>
+
+  <xacro:wheel prefix="front_left">
+    <origin xyz="${wheelbase/2} ${track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
+  </xacro:wheel>
+  <xacro:wheel prefix="front_right">
+    <origin xyz="${wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
+  </xacro:wheel>
+  <xacro:wheel prefix="rear_left">
+    <origin xyz="${-wheelbase/2} ${track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
+  </xacro:wheel>
+  <xacro:wheel prefix="rear_right">
+    <origin xyz="${-wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
+  </xacro:wheel>
+
+  <link name="base_link"></link>
+
+  <joint name="base_link_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0" />
+    <parent link="base_link"/>
+    <child link="chassis_link" />
+  </joint>
+
+  <link name="chassis_link">
+    <visual>
+      <origin xyz="0 0 ${footprint_vertical_offset}" rpy="${PI/2} 0 ${PI/2}"/>
+      <geometry>
+        <mesh filename="package://jackal_description/meshes/jackal-base.stl"/>
+      </geometry>
+      <material name="dark_grey" />
+    </visual>
+    <collision>
+      <origin xyz="0 0 ${chassis_height/2}"/>
+      <geometry>
+        <box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <!-- Center of mass -->
+      <origin xyz="0.012  0.002 0.067" rpy="0 0 0"/>
+      <mass value="16.523"/>
+      <!-- Moments of inertia: ( chassis without wheels ) -->
+      <inertia
+        ixx="0.3136" ixy="-0.0008" ixz="0.0164"
+        iyy="0.3922" iyz="-0.0009"
+        izz="0.4485"/>
+      </inertial>
+  </link>
+
+  <xacro:unless value="$(optenv JACKAL_FRONT_ACCESSORY_FENDER 0)">
+    <link name="front_fender_link">
+      <visual>
+        <geometry>
+          <mesh filename="package://jackal_description/meshes/jackal-fender.stl"/>
+        </geometry>
+        <material name="yellow" />
+      </visual>
+    </link>
+  </xacro:unless>
+
+  <xacro:if value="$(optenv JACKAL_FRONT_ACCESSORY_FENDER 0)">
+    <link name="front_fender_link">
+      <visual>
+        <geometry>
+          <mesh filename="package://jackal_description/meshes/accessory_fender.stl"/>
+        </geometry>
+        <material name="yellow" />
+      </visual>
+    </link>
+
+    <link name="front_fender_accessory_link"/>
+    <joint name="front_fender_accessory_joint" type="fixed">
+      <origin xyz="0.25629 0 0.07455" rpy="${PI} 0 0" />
+      <parent link="front_fender_link" />
+      <child link="front_fender_accessory_link" />
+    </joint>
+  </xacro:if>
+
+  <joint name="front_fender_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="chassis_link" />
+    <child link="front_fender_link" />
+  </joint>
+
+  <xacro:unless value="$(optenv JACKAL_REAR_ACCESSORY_FENDER 0)">
+    <link name="rear_fender_link">
+      <visual>
+        <geometry>
+          <mesh filename="package://jackal_description/meshes/jackal-fender.stl"/>
+        </geometry>
+        <material name="yellow" />
+      </visual>
+    </link>
+  </xacro:unless>
+
+  <xacro:if value="$(optenv JACKAL_REAR_ACCESSORY_FENDER 0)">
+    <link name="rear_fender_link">
+      <visual>
+        <geometry>
+          <mesh filename="package://jackal_description/meshes/accessory_fender.stl"/>
+        </geometry>
+        <material name="yellow" />
+      </visual>
+    </link>
+
+    <link name="rear_fender_accessory_link"/>
+    <joint name="rear_fender_accessory_joint" type="fixed">
+      <origin xyz="0.25629 0 0.07455" rpy="${PI} 0 0" />
+      <parent link="rear_fender_link" />
+      <child link="rear_fender_accessory_link" />
+    </joint>
+  </xacro:if>
+
+  <joint name="rear_fender_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 ${PI}"/>
+    <parent link="chassis_link" />
+    <child link="rear_fender_link" />
+  </joint>
+
+  <!-- Default Internal IMU Link -->
+  <link name="imu_link">
+    <inertial>
+      <mass value="0.001"/>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <inertia ixx="${dummy_inertia}" ixy="0.0" ixz="0.0" iyy="${dummy_inertia}" iyz="0.0" izz="${dummy_inertia}"/>
+    </inertial>
+  </link>
+  <joint name="imu_joint" type="fixed">
+    <parent link="chassis_link" />
+    <child link="imu_link" />
+  </joint>
+
+  <!-- Default NAVSAT Link -->
+  <link name="navsat_link">
+    <visual>
+      <geometry>
+        <cylinder radius="0.026" length="0.016" />
+      </geometry>
+      <origin xyz="0 0 0.008" />
+      <material name="black" />
+    </visual>
+  </link>
+  <joint name="navsat_joint" type="fixed">
+    <parent link="chassis_link" />
+    <child link="navsat_link" />
+    <origin xyz="-0.180 0.126 0.1815" />
+  </joint>
+
+  <link name="mid_mount"></link>
+  <joint name="mid_mount_joint" type="fixed">
+    <parent link="chassis_link" />
+    <child link="mid_mount" />
+    <origin xyz="0 0 ${chassis_height}" />
+  </joint>
+
+  <link name="rear_mount"></link>
+  <joint name="rear_mount_joint" type="fixed">
+    <parent link="mid_mount" />
+    <child link="rear_mount" />
+    <origin xyz="${-mount_spacing} 0 0" />
+  </joint>
+
+  <link name="front_mount"></link>
+  <joint name="front_mount_joint" type="fixed">
+    <parent link="mid_mount" />
+    <child link="front_mount" />
+    <origin xyz="${mount_spacing} 0 0" />
+  </joint>
+
+  <!-- Bring in simulation data for Gazebo. -->
+  <xacro:include filename="$(find jackal_description)/urdf/jackal.gazebo" />
+
+  <!-- Optional standard accessories, including their simulation data. The rendering
+       of these into the final description is controlled by optenv variables, which
+       default each one to off.-->
+  <xacro:include filename="$(find jackal_description)/urdf/accessories.urdf.xacro" />
+
+  <!-- Optional custom includes. -->
+  <xacro:include filename="$(optenv JACKAL_URDF_EXTRAS empty.urdf)" />
+
+  <!-- Optional for Clearpath internal softwares -->
+  <xacro:include filename="$(optenv CPR_URDF_EXTRAS empty.urdf)" />
+</robot>
--- a/src/launch/amcl.launch
+++ b/src/launch/amcl.launch
@ -0,0 +1,64 @@
+<launch>
+
+  <arg name="use_map_topic" default="false"/>
+  <arg name="scan_topic" default="$(eval optenv('JACKAL_LASER_TOPIC', 'front/scan'))" />
+
+  <node pkg="amcl" type="amcl" name="amcl">
+    <param name="use_map_topic" value="$(arg use_map_topic)"/>
+    <!-- Publish scans from best pose at a max of 10 Hz -->
+    <param name="odom_model_type" value="diff"/>
+    <param name="odom_alpha5" value="0.1"/>
+    <param name="gui_publish_rate" value="20.0"/>
+    <!-- <param name="laser_max_beams" value="720"/> -->
+    <param name="laser_max_beams" value="2000"/>
+    <!-- <param name="laser_min_range" value="0.1"/>
+    <param name="laser_max_range" value="30.0"/> -->
+    <param name="laser_min_range" value="-1"/>
+    <param name="laser_max_range" value="-1"/>
+    <param name="min_particles" value="2000"/>
+    <param name="max_particles" value="5000"/>
+    <!-- Maximum error between the true distribution and the estimated distribution. -->
+    <param name="kld_err" value="0.05"/>
+    <param name="kld_z" value="0.99"/>
+    <param name="odom_alpha1" value="0.2"/>
+    <param name="odom_alpha2" value="0.2"/>
+    <!-- translation std dev, m -->
+    <param name="odom_alpha3" value="0.2"/>
+    <param name="odom_alpha4" value="0.2"/>
+    <param name="laser_z_hit" value="0.5"/>
+    <param name="laser_z_short" value="0.05"/>
+    <param name="laser_z_max" value="0.05"/>
+    <param name="laser_z_rand" value="0.5"/>
+    <param name="laser_sigma_hit" value="0.2"/>
+    <param name="laser_lambda_short" value="0.1"/>
+    <param name="laser_model_type" value="likelihood_field"/>
+    <!-- Maximum distance to do obstacle inflation on map, for use in likelihood_field model. -->
+    <param name="laser_likelihood_max_dist" value="2.0"/>
+    <!-- Translational movement required before performing a filter update.  -->
+    <param name="update_min_d" value="0.1"/>
+    <!--Rotational movement required before performing a filter update. -->
+    <param name="update_min_a" value="0.2"/> 
+    <!-- <param name="update_min_a" value="0.314"/>  -->
+    <param name="odom_frame_id" value="odom"/>
+    <param name="base_frame_id" value="base_link"/>
+    <param name="global_frame_id" value="map"/>
+    <!-- Number of filter updates required before resampling. -->
+    <param name="resample_interval" value="1"/>
+    <!-- Increase tolerance because the computer can get quite busy -->
+    <param name="transform_tolerance" value="1.0"/>
+    <!-- Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.001. -->
+    <param name="recovery_alpha_slow" value="0.0"/>
+    <!--Exponential decay rate for the fast average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.1. -->
+    <param name="recovery_alpha_fast" value="0.1"/>
+    <!-- Initial pose mean -->
+    <param name="initial_pose_x" value="0.0" />
+    <param name="initial_pose_y" value="0.0" />
+    <param name="initial_pose_a" value="0.0" />
+    <!-- When set to true, AMCL will subscribe to the map topic rather than making a service call to receive its map.-->
+    <param name="receive_map_topic" value="true"/>
+    <!--  When set to true, AMCL will only use the first map it subscribes to, rather than updating each time a new one is received. -->
+    <param name="first_map_only" value="false"/>
+    <remap from="scan" to="$(arg scan_topic)"/>
+  </node>
+
+</launch>
--- a/src/launch/move_base_teb.launch
+++ b/src/launch/move_base_teb.launch
@ -0,0 +1,26 @@
+<launch>
+ 
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+
+    <rosparam file="$(find me5413_world)/params/common_costmap_config.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find me5413_world)/params/common_costmap_config.yaml" command="load" ns="local_costmap" />
+    
+    <rosparam file="$(find me5413_world)/params/map_nav_params/local_costmap_params.yaml" command="load" />
+    <rosparam file="$(find me5413_world)/params/map_nav_params/global_costmap_params.yaml" command="load" />
+    
+    <rosparam file="$(find me5413_world)/params/prohibition_areas.yaml" command="load" ns="global_costmap/prohibition_layer" />
+    <rosparam file="$(find me5413_world)/params/prohibition_areas.yaml" command="load" ns="local_costmap/prohibition_layer" />
+
+    <rosparam file="$(find me5413_world)/params/teb_planner/teb_planner_param.yaml" command="load" />
+    <rosparam file="$(find me5413_world)/params/teb_planner/costmap_converter_params.yaml" command="load" />
+
+    <param name="base_global_planner" value="global_planner/GlobalPlanner" />
+    <param name="planner_frequency" value="1.0" />
+
+    <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
+    <param name="controller_frequency" value="10.0" />
+    
+    <remap from="odom" to="odometry/filtered" />
+  </node>
+
+</launch>
--- a/src/launch/navigation.launch
+++ b/src/launch/navigation.launch
@ -0,0 +1,24 @@
+<launch>
+
+  <!-- Connect the robot to a keyboard teleop controller -->
+  <!-- <node name="teleop_twist_keyboard" pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" output="screen" respawn="true"/> -->
+
+  <!-- Run the map server -->
+  <arg name="map_file" default="$(find me5413_world)/maps/my_map.yaml"/>
+  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />
+  
+  <!-- Launch the AMCL Localizer -->
+  <include file="$(find me5413_world)/launch/amcl.launch" />
+  <!-- Launch Move Base -->
+  <include file="$(find me5413_world)/launch/move_base_teb.launch" />
+  <!-- <include file="$(find me5413_world)/launch/move_base.launch" /> -->
+
+  <!-- Launch Rviz with our settings -->
+  <node type="rviz" name="rviz" pkg="rviz" args="-d $(find me5413_world)/rviz/navigation.rviz" output="log" respawn="true"/>
+
+  <node ns="me5413_world" pkg="me5413_world" type="goal_publisher_node" name="goal_publisher_node" output="screen" />
+  <node pkg="me5413_world" type="detector.py" name="Detector" output="screen" />
+  <node pkg="me5413_world" type="roi_coordinate_calculator.py" name="ROICoordinateCalculator" output="screen" />
+  <node pkg="me5413_world" type="viz.py" name="ObjectDetector" output="screen" />
+
+</launch>
--- a/src/launch/spawn_jackal.launch
+++ b/src/launch/spawn_jackal.launch
@ -0,0 +1,35 @@
+<launch>
+  <!-- Configuration of Jackal which you would like to simulate.
+       See jackal_description for details. -->
+  <arg name="config" default="base" />
+
+  <!-- Load Jackal's description, controllers, and teleop nodes. -->
+  <!-- <include file="$(find jackal_description)/launch/description.launch">
+    <arg name="config" value="$(arg config)" />
+  </include> -->
+
+  <!-- fix for oneweek project -->
+  <arg name="env_runner" value="$(eval 'env_run' if not optenv('OS', 'unknown').lower().startswith('windows') else 'env_run.bat')" />
+  <!-- the following seems to work too when in devel space, but not in install_isolated -->
+  <!-- <arg name="env_runner" value="env_run" /> -->
+
+  <param name="robot_description"
+         command="$(find jackal_description)/scripts/$(arg env_runner)
+                    $(find jackal_description)/urdf/configs/$(arg config)
+                    $(find xacro)/xacro $(find jackal_description)/urdf/jackal.urdf.xacro
+                    --inorder" />
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+
+  <!-- <include file="$(find jackal_control)/launch/control.launch" /> -->
+  <include file="$(find me5413_world)/launch/include/control.launch" />
+  <include file="$(find jackal_control)/launch/teleop.launch">
+    <arg name="joystick" value="false" />
+  </include>
+
+  <!-- Spawn Jackal -->
+  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
+	      args="-urdf -model jackal -param robot_description -x 0 -y 0 -z 1 -R 0 -P 0 -Y 0" />
+
+  <!-- Add a static world frame -->
+  <!-- <node pkg="tf2_ros" type="static_transform_publisher" name="world_broadcast" args="0 0 0 0 0 0 1 world odom" /> -->
+</launch>
--- a/src/launch/world.launch
+++ b/src/launch/world.launch
@ -0,0 +1,21 @@
+<launch>
+    <!-- Using the simulation clock -->
+    <param name="/use_sim_time" value="true"/>
+
+    <!-- Load the project world into Gazebo -->
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="world_name" value="$(find me5413_world)/worlds/me5413_project.world"/>
+        <arg name="debug" value="false" />
+        <arg name="gui" value="true" />
+        <arg name="paused" value="false"/>
+        <arg name="use_sim_time" value="true"/>
+        <arg name="headless" value="false"/>
+    </include>
+
+    <!-- Add our jackal robot into the simulation -->
+    <include file="$(find me5413_world)/launch/include/spawn_jackal.launch"/>
+    
+    <!-- Load the destination configuration -->
+    <rosparam command="load" file="$(find me5413_world)/config/config.yaml" />
+
+</launch>
--- a/src/rviz/manual.rviz
+++ b/src/rviz/manual.rviz
@ -0,0 +1,422 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5117647051811218
+    Tree Height: 420
+  - 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.6096654534339905
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: PointCloud2
+  - Class: rviz_panel::simplePanel
+    Name: simplePanel
+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
+    - Class: rviz/TF
+      Enabled: false
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+      Marker Alpha: 1
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+      Value: false
+    - 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_camera:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_camera_beam:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_camera_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_camera_optical:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_fender_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_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        mid_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        mid_vlp16_leg1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        mid_vlp16_leg2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        mid_vlp16_leg3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        mid_vlp16_leg4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        mid_vlp16_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        mid_vlp16_plate:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        navsat_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_fender_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_left_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        rear_right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        tim551:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        tim551_mount_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        velodyne:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        velodyne_base_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/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: true
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /twist_marker_server/update
+      Value: true
+    - Class: rviz/Group
+      Displays:
+        - Acceleration properties:
+            Acc. vector alpha: 1
+            Acc. vector color: 255; 0; 0
+            Acc. vector scale: 1
+            Derotate acceleration: true
+            Enable acceleration: false
+          Axes properties:
+            Axes scale: 1
+            Enable axes: true
+          Box properties:
+            Box alpha: 1
+            Box color: 255; 0; 0
+            Enable box: false
+            x_scale: 1
+            y_scale: 1
+            z_scale: 1
+          Class: rviz_imu_plugin/Imu
+          Enabled: true
+          Name: Imu
+          Queue Size: 10
+          Topic: /imu/data
+          Unreliable: false
+          Value: true
+          fixed_frame_orientation: true
+        - Class: rviz/Image
+          Enabled: true
+          Image Topic: /front/rgb/image_raw
+          Max Value: 1
+          Median window: 5
+          Min Value: 0
+          Name: Image
+          Normalize Range: true
+          Queue Size: 2
+          Transport Hint: raw
+          Unreliable: false
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 6.684496879577637
+            Min Value: -0.751370906829834
+            Value: true
+          Axis: X
+          Channel Name: intensity
+          Class: rviz/LaserScan
+          Color: 255; 255; 255
+          Color Transformer: AxisColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Min Color: 0; 0; 0
+          Name: LaserScan
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.05000000074505806
+          Style: Spheres
+          Topic: /front/scan
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 2.438411235809326
+            Min Value: -0.0709778368473053
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: AxisColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Min Color: 0; 0; 0
+          Name: PointCloud2
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.019999999552965164
+          Style: Points
+          Topic: /mid/points
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+      Enabled: true
+      Name: Sensing
+  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/ThirdPersonFollower
+      Distance: 4
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.8999999761581421
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.6000000238418579
+      Target Frame: base_link
+      Yaw: 3.1500000953674316
+    Saved:
+      - Class: rviz/ThirdPersonFollower
+        Distance: 4
+        Enable Stereo Rendering:
+          Stereo Eye Separation: 0.05999999865889549
+          Stereo Focal Distance: 1
+          Swap Stereo Eyes: false
+          Value: false
+        Field of View: 0.8999999761581421
+        Focal Point:
+          X: 0
+          Y: 0
+          Z: 0
+        Focal Shape Fixed Size: true
+        Focal Shape Size: 0.05000000074505806
+        Invert Z Axis: false
+        Name: ThirdPersonFollower
+        Near Clip Distance: 0.009999999776482582
+        Pitch: 0.6000000238418579
+        Target Frame: base_link
+        Yaw: 3.1500000953674316
+      - Angle: -1.5700000524520874
+        Class: rviz/TopDownOrtho
+        Enable Stereo Rendering:
+          Stereo Eye Separation: 0.05999999865889549
+          Stereo Focal Distance: 1
+          Swap Stereo Eyes: false
+          Value: false
+        Invert Z Axis: false
+        Name: TopDownOrtho
+        Near Clip Distance: 0.009999999776482582
+        Scale: 100
+        Target Frame: base_link
+        X: 0
+        Y: 0
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1016
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001560000035ffc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000022f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002720000012a0000001600ffffff00000001000001990000035ffc0200000005fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035f000000a400fffffffb0000001600730069006d0070006c006500500061006e0065006c00000002700000012c0000001600fffffffb0000000c00540065006c0065006f00700000000287000001150000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073800000039fc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000043d0000035f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1848
+  X: 72
+  Y: 27
+  simplePanel:
+    collapsed: false
--- a/src/rviz/navigation.rviz
+++ b/src/rviz/navigation.rviz
@ -0,0 +1,771 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 87
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Sensing1
+        - /Planning1
+        - /Planning1/Local CostMap1
+        - /Evaluation1
+      Splitter Ratio: 0.4637681245803833
+    Tree Height: 529
+  - 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.6096654534339905
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: Image
+  - Class: rviz_panel::simplePanel
+    Name: simplePanel
+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_camera:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_camera_beam:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_camera_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_camera_optical:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_fender_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_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        mid_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        mid_vlp16_leg1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        mid_vlp16_leg2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        mid_vlp16_leg3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        mid_vlp16_leg4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        mid_vlp16_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        mid_vlp16_plate:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        navsat_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_fender_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_left_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        rear_right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        tim551:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        tim551_mount_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        velodyne:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        velodyne_base_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/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: true
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: false
+      Show Visual Aids: false
+      Update Topic: /twist_marker_server/update
+      Value: true
+    - Class: rviz/Group
+      Displays:
+        - Acceleration properties:
+            Acc. vector alpha: 1
+            Acc. vector color: 255; 0; 0
+            Acc. vector scale: 1
+            Derotate acceleration: true
+            Enable acceleration: false
+          Axes properties:
+            Axes scale: 1
+            Enable axes: true
+          Box properties:
+            Box alpha: 1
+            Box color: 255; 0; 0
+            Enable box: false
+            x_scale: 1
+            y_scale: 1
+            z_scale: 1
+          Class: rviz_imu_plugin/Imu
+          Enabled: true
+          Name: Imu
+          Queue Size: 10
+          Topic: /imu/data
+          Unreliable: false
+          Value: true
+          fixed_frame_orientation: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 5.794246673583984
+            Min Value: -0.9823746681213379
+            Value: true
+          Axis: X
+          Channel Name: intensity
+          Class: rviz/LaserScan
+          Color: 255; 255; 255
+          Color Transformer: AxisColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Min Color: 0; 0; 0
+          Name: LaserScan
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.05000000074505806
+          Style: Spheres
+          Topic: /front/scan
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 3.1792826652526855
+            Min Value: -0.07012569904327393
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: AxisColor
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Min Color: 0; 0; 0
+          Name: PointCloud2
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.019999999552965164
+          Style: Points
+          Topic: /mid/points
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Class: rviz/Camera
+          Enabled: false
+          Image Rendering: background and overlay
+          Image Topic: /front/rgb/image_raw
+          Name: Camera
+          Overlay Alpha: 0.5
+          Queue Size: 2
+          Transport Hint: compressed
+          Unreliable: false
+          Value: false
+          Visibility:
+            Evaluation:
+              "": true
+              Value: true
+            Grid: true
+            InteractiveMarkers: true
+            Localization:
+              AMCL PoseArray: true
+              AMCL PoseWithCovariance: true
+              Map: true
+              Value: true
+            Planning:
+              "": true
+              Value: true
+            RobotModel: true
+            Sensing:
+              Image: true
+              Imu: true
+              LaserScan: true
+              PointCloud2: true
+              Value: true
+            Value: true
+          Zoom Factor: 1
+        - Class: rviz/Image
+          Enabled: true
+          Image Topic: /front/rgb/image_raw
+          Max Value: 1
+          Median window: 5
+          Min Value: 0
+          Name: Image
+          Normalize Range: true
+          Queue Size: 2
+          Transport Hint: compressed
+          Unreliable: false
+          Value: true
+      Enabled: true
+      Name: Sensing
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 0.699999988079071
+          Class: rviz/Map
+          Color Scheme: map
+          Draw Behind: true
+          Enabled: true
+          Name: Map
+          Topic: /map
+          Unreliable: false
+          Use Timestamp: false
+          Value: true
+        - Alpha: 1
+          Arrow Length: 0.30000001192092896
+          Axes Length: 0.30000001192092896
+          Axes Radius: 0.009999999776482582
+          Class: rviz/PoseArray
+          Color: 255; 25; 0
+          Enabled: true
+          Head Length: 0.07000000029802322
+          Head Radius: 0.029999999329447746
+          Name: AMCL PoseArray
+          Queue Size: 10
+          Shaft Length: 0.23000000417232513
+          Shaft Radius: 0.009999999776482582
+          Shape: Arrow (Flat)
+          Topic: /particlecloud
+          Unreliable: false
+          Value: true
+        - Alpha: 1
+          Axes Length: 1
+          Axes Radius: 0.10000000149011612
+          Class: rviz/PoseWithCovariance
+          Color: 255; 25; 0
+          Covariance:
+            Orientation:
+              Alpha: 0.5
+              Color: 255; 255; 127
+              Color Style: Unique
+              Frame: Local
+              Offset: 1
+              Scale: 1
+              Value: true
+            Position:
+              Alpha: 0.30000001192092896
+              Color: 204; 51; 204
+              Scale: 1
+              Value: true
+            Value: true
+          Enabled: false
+          Head Length: 0.30000001192092896
+          Head Radius: 0.10000000149011612
+          Name: AMCL PoseWithCovariance
+          Queue Size: 10
+          Shaft Length: 1
+          Shaft Radius: 0.05000000074505806
+          Shape: Arrow
+          Topic: /amcl_pose
+          Unreliable: false
+          Value: false
+      Enabled: true
+      Name: Localization
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 1
+          Class: rviz/Polygon
+          Color: 25; 255; 0
+          Enabled: true
+          Name: Foot Print
+          Queue Size: 10
+          Topic: /move_base/local_costmap/footprint
+          Unreliable: false
+          Value: true
+        - Alpha: 1
+          Axes Length: 1
+          Axes Radius: 0.10000000149011612
+          Class: rviz/Pose
+          Color: 255; 25; 0
+          Enabled: true
+          Head Length: 0.20000000298023224
+          Head Radius: 0.10000000149011612
+          Name: Goal Pose
+          Queue Size: 10
+          Shaft Length: 0.5
+          Shaft Radius: 0.05000000074505806
+          Shape: Arrow
+          Topic: /move_base_simple/goal
+          Unreliable: false
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz/Path
+          Color: 25; 255; 0
+          Enabled: true
+          Head Diameter: 0.30000001192092896
+          Head Length: 0.20000000298023224
+          Length: 0.30000001192092896
+          Line Style: Billboards
+          Line Width: 0.029999999329447746
+          Name: Full Path
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Pose Color: 255; 85; 255
+          Pose Style: None
+          Queue Size: 10
+          Radius: 0.029999999329447746
+          Shaft Diameter: 0.10000000149011612
+          Shaft Length: 0.10000000149011612
+          Topic: /move_base/TebLocalPlannerROS/global_plan
+          Unreliable: false
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz/Path
+          Color: 25; 255; 0
+          Enabled: true
+          Head Diameter: 0.30000001192092896
+          Head Length: 0.20000000298023224
+          Length: 0.30000001192092896
+          Line Style: Lines
+          Line Width: 0.029999999329447746
+          Name: Local Path
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Pose Color: 255; 85; 255
+          Pose Style: None
+          Queue Size: 10
+          Radius: 0.029999999329447746
+          Shaft Diameter: 0.10000000149011612
+          Shaft Length: 0.10000000149011612
+          Topic: /move_base/TebLocalPlannerROS/local_plan
+          Unreliable: false
+          Value: true
+        - Alpha: 0.699999988079071
+          Class: rviz/Map
+          Color Scheme: map
+          Draw Behind: false
+          Enabled: true
+          Name: Global CostMap
+          Topic: /move_base/global_costmap/costmap
+          Unreliable: false
+          Use Timestamp: false
+          Value: true
+        - Alpha: 0.699999988079071
+          Class: rviz/Map
+          Color Scheme: map
+          Draw Behind: false
+          Enabled: true
+          Name: Local CostMap
+          Topic: /move_base/local_costmap/costmap
+          Unreliable: false
+          Use Timestamp: false
+          Value: true
+      Enabled: true
+      Name: Planning
+    - Class: rviz/Group
+      Displays:
+        - Class: rviz/Group
+          Displays:
+            - Buffer length: 100
+              Class: jsk_rviz_plugin/Plotter2D
+              Enabled: true
+              Name: ADE (m)
+              Show Value: true
+              Topic: /me5413_world/absolute/position_error
+              Value: true
+              auto color change: true
+              auto scale: true
+              background color: 0; 0; 0
+              backround alpha: 0
+              border: true
+              foreground alpha: 0.699999988079071
+              foreground color: 25; 255; 240
+              height: 128
+              left: 20
+              linewidth: 2
+              max color: 255; 0; 0
+              max value: 10
+              min value: 0
+              show caption: true
+              text size: 14
+              top: 20
+              update interval: 0.03999999910593033
+              width: 128
+            - Buffer length: 100
+              Class: jsk_rviz_plugin/Plotter2D
+              Enabled: true
+              Name: AHE (deg)
+              Show Value: true
+              Topic: /me5413_world/absolute/heading_error
+              Value: true
+              auto color change: true
+              auto scale: true
+              background color: 0; 0; 0
+              backround alpha: 0
+              border: true
+              foreground alpha: 0.699999988079071
+              foreground color: 25; 255; 240
+              height: 128
+              left: 160
+              linewidth: 2
+              max color: 255; 0; 0
+              max value: 10
+              min value: 0
+              show caption: true
+              text size: 14
+              top: 20
+              update interval: 0.03999999910593033
+              width: 128
+            - Class: jsk_rviz_plugin/PieChart
+              Enabled: false
+              Name: AHE (deg)
+              Topic: /me5413_world/absolute/heading_error
+              Value: false
+              auto color change: true
+              background color: 0; 0; 0
+              backround alpha: 0
+              clockwise rotate direction: false
+              foreground alpha: 0.699999988079071
+              foreground alpha 2: 0.4000000059604645
+              foreground color: 25; 255; 240
+              left: 160
+              max color: 255; 0; 0
+              max color change threthold: 90
+              max value: 180
+              med color: 138; 226; 52
+              med color change threthold: 30
+              min value: -180
+              show caption: true
+              size: 128
+              text size: 14
+              top: 20
+          Enabled: true
+          Name: Absolute Error
+        - Class: rviz/Group
+          Displays:
+            - Buffer length: 100
+              Class: jsk_rviz_plugin/Plotter2D
+              Enabled: true
+              Name: RDE (m)
+              Show Value: true
+              Topic: /me5413_world/relative/position_error
+              Value: true
+              auto color change: true
+              auto scale: true
+              background color: 0; 0; 0
+              backround alpha: 0
+              border: true
+              foreground alpha: 0.699999988079071
+              foreground color: 25; 255; 240
+              height: 128
+              left: 20
+              linewidth: 2
+              max color: 255; 0; 0
+              max value: 10
+              min value: 0
+              show caption: true
+              text size: 14
+              top: 180
+              update interval: 0.03999999910593033
+              width: 128
+            - Buffer length: 100
+              Class: jsk_rviz_plugin/Plotter2D
+              Enabled: true
+              Name: RHE (deg)
+              Show Value: true
+              Topic: /me5413_world/relative/heading_error
+              Value: true
+              auto color change: true
+              auto scale: true
+              background color: 0; 0; 0
+              backround alpha: 0
+              border: true
+              foreground alpha: 0.699999988079071
+              foreground color: 25; 255; 240
+              height: 128
+              left: 160
+              linewidth: 2
+              max color: 255; 0; 0
+              max value: 10
+              min value: 0
+              show caption: true
+              text size: 14
+              top: 180
+              update interval: 0.03999999910593033
+              width: 128
+            - Class: jsk_rviz_plugin/PieChart
+              Enabled: false
+              Name: RHE (deg)
+              Topic: /me5413_world/relative/heading_error
+              Value: false
+              auto color change: true
+              background color: 0; 0; 0
+              backround alpha: 0
+              clockwise rotate direction: false
+              foreground alpha: 0.699999988079071
+              foreground alpha 2: 0.4000000059604645
+              foreground color: 25; 255; 240
+              left: 160
+              max color: 255; 0; 0
+              max color change threthold: 90
+              max value: 180
+              med color: 138; 226; 52
+              med color change threthold: 30
+              min value: -180
+              show caption: true
+              size: 128
+              text size: 14
+              top: 180
+          Enabled: true
+          Name: Relative Error
+        - Class: rviz/MarkerArray
+          Enabled: true
+          Marker Topic: /gazebo/ground_truth/box_markers
+          Name: BoxMarkerArray
+          Namespaces:
+            gazebo: true
+          Queue Size: 100
+          Value: true
+      Enabled: true
+      Name: Evaluation
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      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/ThirdPersonFollower
+      Distance: 23.740150451660156
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: -1.2324113845825195
+        Y: -0.22771888971328735
+        Z: 0
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.519796371459961
+      Target Frame: base_link
+      Yaw: 2.8004043102264404
+    Saved:
+      - Class: rviz/ThirdPersonFollower
+        Distance: 4
+        Enable Stereo Rendering:
+          Stereo Eye Separation: 0.05999999865889549
+          Stereo Focal Distance: 1
+          Swap Stereo Eyes: false
+          Value: false
+        Field of View: 0.8999999761581421
+        Focal Point:
+          X: 0
+          Y: 0
+          Z: 0
+        Focal Shape Fixed Size: true
+        Focal Shape Size: 0.05000000074505806
+        Invert Z Axis: false
+        Name: ThirdPersonFollower
+        Near Clip Distance: 0.009999999776482582
+        Pitch: 0.6000000238418579
+        Target Frame: base_link
+        Yaw: 3.1500000953674316
+      - Angle: -1.5700000524520874
+        Class: rviz/TopDownOrtho
+        Enable Stereo Rendering:
+          Stereo Eye Separation: 0.05999999865889549
+          Stereo Focal Distance: 1
+          Swap Stereo Eyes: false
+          Value: false
+        Invert Z Axis: false
+        Name: TopDownOrtho
+        Near Clip Distance: 0.009999999776482582
+        Scale: 100
+        Target Frame: base_link
+        X: 0
+        Y: 0
+Window Geometry:
+  Camera:
+    collapsed: false
+  Displays:
+    collapsed: false
+  Height: 1329
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000002ba00000442fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007e01000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000005a000002c70000010f01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000003220000017a0000002701000003fb0000000c00430061006d00650072006100000002d5000000c70000002701000003000000010000014f00000442fc0200000005fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000005a000002fd000000f201000003fb0000001600730069006d0070006c006500500061006e0065006c010000035800000144000000ef000001b7fb0000000c00540065006c0065006f00700000000287000001150000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a0000000060fc0100000002fb0000000800540069006d0065010000000000000a000000053201000003fb0000000800540069006d00650100000000000004500000000000000000000005f50000044200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 2560
+  X: 0
+  Y: 0
+  simplePanel:
+    collapsed: false
--- a/src/src/goal_publisher_node.cpp
+++ b/src/src/goal_publisher_node.cpp
@ -0,0 +1,415 @@
+/* goal_publisher_node.cpp
+
+ * Copyright (C) 2023 SS47816
+
+ * ROS Node for publishing goal poses
+
+**/
+
+#include "me5413_world/goal_publisher_node.hpp"
+
+namespace me5413_world
+{
+
+GoalPublisherNode::GoalPublisherNode() : tf2_listener_(tf2_buffer_)
+{
+  this->pub_goal_ = nh_.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1);
+  this->pub_absolute_position_error_ = nh_.advertise<std_msgs::Float32>("/me5413_world/absolute/position_error", 1);
+  this->pub_absolute_heading_error_ = nh_.advertise<std_msgs::Float32>("/me5413_world/absolute/heading_error", 1);
+  this->pub_relative_position_error_ = nh_.advertise<std_msgs::Float32>("/me5413_world/relative/position_error", 1);
+  this->pub_relative_heading_error_ = nh_.advertise<std_msgs::Float32>("/me5413_world/relative/heading_error", 1);
+
+  this->timer_ = nh_.createTimer(ros::Duration(0.2), &GoalPublisherNode::timerCallback, this);
+  this->sub_robot_odom_ = nh_.subscribe("/gazebo/ground_truth/state", 1, &GoalPublisherNode::robotOdomCallback, this);
+  this->sub_goal_name_ = nh_.subscribe("/rviz_panel/goal_name", 1, &GoalPublisherNode::goalNameCallback, this);
+  this->sub_goal_pose_ = nh_.subscribe("/move_base_simple/goal", 1, &GoalPublisherNode::goalPoseCallback, this);
+  this->sub_box_markers_ = nh_.subscribe("/gazebo/ground_truth/box_markers", 1, &GoalPublisherNode::boxMarkersCallback, this);
+  this->sub_move_base_status_ = nh_.subscribe("/move_base/status", 10, &GoalPublisherNode::moveBaseStatusCallback, this);
+  this->sub_map_ = nh_.subscribe("/move_base/global_costmap/costmap", 10, &GoalPublisherNode::globalCostmapCallback, this);
+  this->sub_calculated_target_ = nh_.subscribe("/calculated/target", 1, &GoalPublisherNode::calculatedTargetCallback, this);
+
+  // Initialization
+  this->robot_frame_ = "base_link";
+  this->map_frame_ = "map";
+  this->world_frame_ = "world";
+  this->absolute_position_error_.data = 0.0;
+  this->absolute_heading_error_.data = 0.0;
+  this->relative_position_error_.data = 0.0;
+  this->relative_heading_error_.data = 0.0;
+  this->last_responded_goal_id_ = "";
+  this->last_goal_time_ = ros::Time::now();
+  this->globalCostmapData = nav_msgs::OccupancyGrid();
+  this->gt_box_pose = geometry_msgs::PoseStamped();
+  this->calculated_target_ = geometry_msgs::PoseStamped();
+};
+
+void GoalPublisherNode::timerCallback(const ros::TimerEvent&)
+{
+  // Calculate absolute errors (wrt to world frame)
+  const std::pair<double, double> error_absolute = calculatePoseError(this->pose_world_robot_, this->pose_world_goal_);
+  // Calculate relative errors (wrt to map frame)
+  const std::pair<double, double> error_relative = calculatePoseError(this->pose_map_robot_, this->pose_map_goal_);
+
+  this->absolute_position_error_.data = error_absolute.first;
+  this->absolute_heading_error_.data = error_absolute.second;
+  this->relative_position_error_.data = error_relative.first;
+  this->relative_heading_error_.data = error_relative.second;
+
+  if (this->goal_type_ == "box")
+  {
+    this->absolute_heading_error_.data = 0.0;
+    this->relative_heading_error_.data = 0.0;
+  }
+
+  // Publish errors
+  this->pub_absolute_position_error_.publish(this->absolute_position_error_);
+  this->pub_absolute_heading_error_.publish(this->absolute_heading_error_);
+  this->pub_relative_position_error_.publish(this->relative_position_error_);
+  this->pub_relative_heading_error_.publish(this->relative_heading_error_);
+
+  return;
+};
+
+void GoalPublisherNode::robotOdomCallback(const nav_msgs::Odometry::ConstPtr& odom)
+{
+  this->world_frame_ = odom->header.frame_id;
+  this->robot_frame_ = odom->child_frame_id;
+  this->pose_world_robot_ = odom->pose.pose;
+
+  const tf2::Transform T_world_robot = convertPoseToTransform(this->pose_world_robot_);
+  const tf2::Transform T_robot_world = T_world_robot.inverse();
+
+  geometry_msgs::TransformStamped transformStamped;
+  transformStamped.header.stamp = ros::Time::now();
+  transformStamped.header.frame_id = this->robot_frame_;
+  transformStamped.child_frame_id = this->world_frame_;
+  transformStamped.transform.translation.x = T_robot_world.getOrigin().getX();
+  transformStamped.transform.translation.y = T_robot_world.getOrigin().getY();
+  transformStamped.transform.translation.z = 0.0;
+  transformStamped.transform.rotation.x = T_robot_world.getRotation().getX();
+  transformStamped.transform.rotation.y = T_robot_world.getRotation().getY();
+  transformStamped.transform.rotation.z = T_robot_world.getRotation().getZ();
+  transformStamped.transform.rotation.w = T_robot_world.getRotation().getW();
+
+  this->tf2_bcaster_.sendTransform(transformStamped);
+
+  return;
+};
+
+void GoalPublisherNode::calculatedTargetCallback(const geometry_msgs::PoseStamped& target)
+{
+  this->calculated_target_.pose = target.pose;
+  return;
+};
+
+void GoalPublisherNode::goalNameCallback(const std_msgs::String::ConstPtr& name)
+{
+  const std::string goal_name = name->data;
+  const int end = goal_name.find_last_of("_");
+  this->goal_type_ = goal_name.substr(1, end - 1);
+  const int goal_box_id = stoi(goal_name.substr(end + 1, 1));
+
+  geometry_msgs::PoseStamped P_world_goal;
+  if (this->goal_type_ == "box")
+  {
+    if (box_poses_.empty())
+    {
+      ROS_ERROR_STREAM("Box poses unknown, please spawn boxes first!");
+      return;
+    }
+    else if (goal_box_id >= box_poses_.size())
+    {
+      ROS_ERROR_STREAM("Box id is outside the available range, please select a smaller id!");
+      return;
+    }
+
+    P_world_goal = getRandomTargetInPackingArea();
+    // The following line is commented out because the goal pose is now randomly generated
+    // It represents the ground truth pose of the box in the world frame
+
+    this->gt_box_pose = box_poses_[goal_box_id - 1];
+  }
+  else if (this->goal_type_ == "done")
+  {
+    P_world_goal = this->gt_box_pose;
+    // The box search is done, so the robot should stay still
+    // return;
+  }
+  else
+  {
+    // Get the Pose of the goal in world frame
+    P_world_goal = getGoalPoseFromConfig(goal_name);
+  }
+
+  this->pose_world_goal_ = P_world_goal.pose;
+  // Get the Transform from world to map from the tf_listener
+  geometry_msgs::TransformStamped transform_map_world;
+  try
+  {
+    transform_map_world = this->tf2_buffer_.lookupTransform(this->map_frame_, this->world_frame_, ros::Time(0));
+  }
+  catch (tf2::TransformException& ex)
+  {
+    ROS_WARN("%s", ex.what());
+    return;
+  }
+
+  // Transform the goal pose to map frame
+  geometry_msgs::PoseStamped P_map_goal;
+  tf2::doTransform(P_world_goal, P_map_goal, transform_map_world);
+  P_map_goal.header.stamp = ros::Time::now();
+  P_map_goal.header.frame_id = map_frame_;
+
+  // Transform the robot pose to map frame
+  tf2::doTransform(this->pose_world_robot_, this->pose_map_robot_, transform_map_world);
+
+  if (this->goal_type_ == "done")
+  {
+    double x1 = this->calculated_target_.pose.position.x;
+    double y1 = this->calculated_target_.pose.position.y;
+    double z1 = this->calculated_target_.pose.position.z;
+
+    double x2 = P_world_goal.pose.position.x;
+    double y2 = P_world_goal.pose.position.y;
+    double z2 = P_world_goal.pose.position.z;
+    ROS_INFO("Ground truth position is: x=%f, y=%f, z=%f", x2, y2, z2);
+
+    // 计算差值
+    double x_diff = x1 - x2;
+    double y_diff = y1 - y2;
+    double z_diff = z1 - z2;
+    // Euclidean distance for position error
+    double position_error = sqrt(x_diff * x_diff + y_diff * y_diff + z_diff * z_diff);
+    ROS_INFO("Position error: %f", position_error);
+    
+    return;
+  }
+
+  // Publish goal pose in map frame
+  this->pub_goal_.publish(P_map_goal);
+
+  return;
+};
+
+// This callback is implemented to detect the status of the move_base action server
+// It would give a new random goal if the previous goal is either succeeded or aborted
+void GoalPublisherNode::moveBaseStatusCallback(const actionlib_msgs::GoalStatusArray& status)
+{
+  // std::lock_guard<std::mutex> lock(mutex_);
+  // Go through the status list
+  for (const auto& goalStatus : status.status_list)
+  {
+    // Ensure the goal id is not the same as the last responded goal id
+    double time_diff = (ros::Time::now() - last_goal_time_).toSec();
+    // std::cout << "Time difference: " << time_diff << std::endl;
+    if (goalStatus.goal_id.id != last_responded_goal_id_ && 
+            time_diff > 0.3)
+    {
+      last_goal_time_ = ros::Time::now();
+      if (goalStatus.status == actionlib_msgs::GoalStatus::SUCCEEDED ||
+          goalStatus.status == actionlib_msgs::GoalStatus::ABORTED)
+      {
+        // std::cout << "Goal Type: " << this->goal_type_ << std::endl;
+        if (this->goal_type_ == "box")
+        {
+          this->last_responded_goal_id_ = goalStatus.goal_id.id;
+
+          // All conditions satisfied, target still not found, generate a new random goal
+          geometry_msgs::PoseStamped newGoal = this->getRandomTargetInPackingArea();
+
+          // Transform the new goal to map frame
+          geometry_msgs::TransformStamped transform_map_world;
+          try
+          {
+            transform_map_world = this->tf2_buffer_.lookupTransform(this->map_frame_, this->world_frame_, ros::Time(0));
+          }
+          catch (tf2::TransformException& ex)
+          {
+            ROS_WARN("%s", ex.what());
+            return;
+          }
+          geometry_msgs::PoseStamped P_map_goal;
+          tf2::doTransform(newGoal, P_map_goal, transform_map_world);
+          P_map_goal.header.stamp = ros::Time::now();
+          P_map_goal.header.frame_id = map_frame_;
+
+          // Publish the new random goal
+          this->pub_goal_.publish(P_map_goal);
+
+          break;
+        }
+      }
+    }
+  }
+}
+
+void GoalPublisherNode::goalPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& goal_pose)
+{
+  this->pose_map_goal_ = goal_pose->pose;
+}
+
+tf2::Transform GoalPublisherNode::convertPoseToTransform(const geometry_msgs::Pose& pose)
+{
+  tf2::Transform T;
+  T.setOrigin(tf2::Vector3(pose.position.x, pose.position.y, 0));
+  tf2::Quaternion q;
+  q.setValue(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
+  T.setRotation(q);
+
+  return T;
+};
+
+void GoalPublisherNode::boxMarkersCallback(const visualization_msgs::MarkerArray::ConstPtr& box_markers)
+{
+  this->box_poses_.clear();
+  for (const auto& box : box_markers->markers)
+  {
+    geometry_msgs::PoseStamped pose;
+    pose.pose = box.pose;
+    this->box_poses_.emplace_back(pose);
+  }
+
+  return;
+};
+
+void GoalPublisherNode::globalCostmapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg)
+{
+  this->globalCostmapData = *msg;  // Monitor the global costmap data
+}
+
+bool GoalPublisherNode::isPointInObstacle(double x, double y)
+{
+  // Calculate the map coordinates of the target point
+  float resolution = this->globalCostmapData.info.resolution;
+  int map_x = static_cast<int>((x - this->globalCostmapData.info.origin.position.x) / resolution);
+  int map_y = static_cast<int>((y - this->globalCostmapData.info.origin.position.y) / resolution);
+
+  // Define the radius of the circle to check
+  double check_radius = 0.3;
+  int radius_cells = static_cast<int>(check_radius / resolution);
+
+  for (int dx = -radius_cells; dx <= radius_cells; dx++)
+  {
+    for (int dy = -radius_cells; dy <= radius_cells; dy++)
+    {
+      int check_x = map_x + dx;
+      int check_y = map_y + dy;
+
+      // Ensure the point is within the circle
+      if (dx * dx + dy * dy <= radius_cells * radius_cells)
+      {
+        // Ignore points outside the map
+        if (check_x < 0 || check_x >= this->globalCostmapData.info.width || check_y < 0 || check_y >= this->globalCostmapData.info.height)
+        {
+          continue; 
+        }
+
+        // Get the index of the point in the costmap data
+        int index = check_y * this->globalCostmapData.info.width + check_x;
+
+        // If the map data is greater than 50, it is an obstacle
+        if (this->globalCostmapData.data[index] >= 50)
+        {
+          ROS_INFO("Point is in obstacle. Regenerating a new random point");
+          return true;
+        }
+      }
+    }
+  }
+
+  // Return false if the point is not in an obstacle
+  return false;
+}
+
+geometry_msgs::PoseStamped GoalPublisherNode::getRandomTargetInPackingArea()
+{
+  // x: 8.0 to 16.0, y: -6.25 to 1.0, yaw: -3.14 to 3.14
+  double x, y, yaw;
+  bool inObstacle = false;
+
+  do
+  {
+    x = std::round((static_cast<double>(std::rand()) / RAND_MAX * 8.0 + 8) * 10) / 10.0;
+    y = std::round((-6.25 + static_cast<double>(std::rand()) / RAND_MAX * 7.25) * 10) / 10.0;
+    yaw = std::round((static_cast<double>(std::rand()) / RAND_MAX * 6.28 - 3.14) * 10) / 10.0;
+    // Check if the point is in an obstacle
+    inObstacle = isPointInObstacle(x, y);
+  } while (inObstacle);
+
+  std::cout << "Packing area dimensions: "
+            << "x = " << x << ", "
+            << "y = " << y << ", "
+            << "yaw = " << yaw << std::endl;
+
+  tf2::Quaternion q;
+  q.setRPY(0, 0, yaw);
+  q.normalize();
+  geometry_msgs::PoseStamped random_goal_in_packing_area;
+  random_goal_in_packing_area.pose.position.x = x;
+  random_goal_in_packing_area.pose.position.y = y;
+  random_goal_in_packing_area.pose.orientation = tf2::toMsg(q);
+
+  return random_goal_in_packing_area;
+};
+
+geometry_msgs::PoseStamped GoalPublisherNode::getGoalPoseFromConfig(const std::string& name)
+{
+  /**
+   * Get the Transform from goal to world from the file
+   */
+
+  double x, y, yaw;
+  nh_.getParam("/me5413_world" + name + "/x", x);
+  nh_.getParam("/me5413_world" + name + "/y", y);
+  nh_.getParam("/me5413_world" + name + "/yaw", yaw);
+  nh_.getParam("/me5413_world/frame_id", this->world_frame_);
+
+  std::cout << "Heading to " << name << ": "
+            << "x = " << x << ", "
+            << "y = " << y << ", "
+            << "yaw = " << yaw << std::endl;
+
+  tf2::Quaternion q;
+  q.setRPY(0, 0, yaw);
+  q.normalize();
+
+  geometry_msgs::PoseStamped P_world_goal;
+  P_world_goal.pose.position.x = x;
+  P_world_goal.pose.position.y = y;
+  P_world_goal.pose.orientation = tf2::toMsg(q);
+
+  return P_world_goal;
+};
+
+std::pair<double, double> GoalPublisherNode::calculatePoseError(const geometry_msgs::Pose& pose_robot, const geometry_msgs::Pose& pose_goal)
+{
+  // Positional Error
+  const double position_error = std::sqrt(
+      std::pow(pose_robot.position.x - pose_goal.position.x, 2) +
+      std::pow(pose_robot.position.y - pose_goal.position.y, 2));
+
+  // Heading Error
+  tf2::Quaternion q_robot, q_goal;
+  tf2::fromMsg(pose_robot.orientation, q_robot);
+  tf2::fromMsg(pose_goal.orientation, q_goal);
+  const tf2::Matrix3x3 m_robot = tf2::Matrix3x3(q_robot);
+  const tf2::Matrix3x3 m_goal = tf2::Matrix3x3(q_goal);
+
+  double roll, pitch, yaw_robot, yaw_goal;
+  m_robot.getRPY(roll, pitch, yaw_robot);
+  m_goal.getRPY(roll, pitch, yaw_goal);
+
+  const double heading_error = (yaw_robot - yaw_goal) / M_PI * 180.0;
+
+  return std::pair<double, double>(position_error, heading_error);
+}
+
+}  // namespace me5413_world
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "goal_publisher_node");
+  me5413_world::GoalPublisherNode goal_publisher_node;
+  ros::spin();  // spin the ros node.
+  return 0;
+}
--- a/src/src/joy_control.cpp
+++ b/src/src/joy_control.cpp
@ -0,0 +1,47 @@
+#include <ros/ros.h>
+#include <sensor_msgs/Joy.h>
+#include <geometry_msgs/Twist.h>
+
+class TeleopRobot
+{
+public:
+    TeleopRobot()
+    {
+        nh_.param<int>("axis_linear_x", axis_lin_x, 1);
+        nh_.param<int>("axis_linear_y", axis_lin_y, 0);
+        nh_.param<int>("axis_angular", axis_ang, 3);
+        nh_.param<double>("vel_linear", vlinear, 0.6);
+        nh_.param<double>("vel_angular", vangular, 0.5);
+        nh_.param<int>("button", ton, 4);
+
+        pub_ = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
+        sub_ = nh_.subscribe<sensor_msgs::Joy>("joy", 10, &TeleopRobot::joyCallback, this);
+    }
+
+private:
+    void joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
+    {
+        geometry_msgs::Twist cmd_vel;
+        if (joy->buttons[ton])
+        {
+            cmd_vel.linear.x = joy->axes[axis_lin_x] * vlinear;
+            cmd_vel.linear.y = joy->axes[axis_lin_y] * vlinear;
+            cmd_vel.angular.z = joy->axes[axis_ang] * vangular;
+            pub_.publish(cmd_vel);
+        }
+    }
+
+    ros::NodeHandle nh_;
+    ros::Publisher pub_;
+    ros::Subscriber sub_;
+    int axis_ang, axis_lin_x, axis_lin_y, ton;
+    double vlinear, vangular;
+};
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "joy_control");
+    TeleopRobot teleop_robot;
+    ros::spin();
+    return 0;
+}
--- a/src/src/object_spawner_gz_plugin.cpp
+++ b/src/src/object_spawner_gz_plugin.cpp
@ -0,0 +1,224 @@
+/* object_spawner_gz_plugin.cpp
+
+ * Copyright (C) 2024 nuslde, SS47816
+
+ * Gazebo Plugin for spawning objects
+ 
+**/
+
+#include "me5413_world/object_spawner_gz_plugin.hpp"
+
+namespace gazebo
+{
+
+ObjectSpawner::ObjectSpawner() : WorldPlugin() {};
+
+ObjectSpawner::~ObjectSpawner() {};
+  
+void ObjectSpawner::Load(physics::WorldPtr _world, sdf::ElementPtr _sdf)
+{
+  // Create a new transport node
+  transport::NodePtr node(new transport::Node());
+  node->Init(_world->Name());
+  clt_delete_objects_ = nh_.serviceClient<gazebo_msgs::DeleteModel>("/gazebo/delete_model");
+  this->timer_ = nh_.createTimer(ros::Duration(0.1), &ObjectSpawner::timerCallback, this);
+  this->pub_factory_ = node->Advertise<msgs::Factory>("~/factory");
+  this->sub_respawn_objects_ = nh_.subscribe("/rviz_panel/respawn_objects", 1, &ObjectSpawner::respawnCmdCallback, this);
+  this->pub_rviz_markers_ = nh_.advertise<visualization_msgs::MarkerArray>("/gazebo/ground_truth/box_markers", 0);
+  
+  return;
+};
+
+void ObjectSpawner::timerCallback(const ros::TimerEvent&)
+{
+  // publish rviz markers
+  this->pub_rviz_markers_.publish(this->box_markers_msg_);
+
+  return;
+};
+
+void ObjectSpawner::spawnRandomCones()
+{
+  this->cone_name = "Construction Cone_0";
+
+  msgs::Factory cone_msg;
+  cone_msg.set_sdf_filename("model://construction_cone");
+
+  std::srand(std::time(0));
+  if (std::rand() % 2 == 0)
+  {
+    msgs::Set(cone_msg.mutable_pose(), ignition::math::Pose3d(
+                ignition::math::Vector3d(12.7, 2.5, 0.1),
+                ignition::math::Quaterniond(0, 0, 0)));
+  }
+  else
+  {
+    msgs::Set(cone_msg.mutable_pose(), ignition::math::Pose3d(
+                ignition::math::Vector3d(16.9, 2.5, 0.1),
+                ignition::math::Quaterniond(0, 0, 0)));
+  }
+  this->pub_factory_->Publish(cone_msg);
+  
+  return;
+};
+
+void ObjectSpawner::spawnRandomBoxes(const int num)
+{
+  if (num > 9)
+  {
+    ROS_WARN_STREAM("Maximum 9 boxes are allowed!, Spawning 9 boxes instead");
+  }
+
+  std::srand(std::time(0));
+  this->box_names.clear();
+  this->box_points.clear();
+  this->box_markers_msg_.markers.clear();
+  this->box_points.emplace_back(ignition::math::Vector3d(16.576, -5.96, 1.0)); // add tree point
+
+  visualization_msgs::MarkerArray text_markers_msg;
+  for (int i = 1; i <= std::min(num, 9); ++i)
+  {
+    ignition::math::Vector3d point;
+    // Generate random box_points within a 10 by 8 area with a distance greater than 4.3
+    bool has_collision = true;
+    while (has_collision)
+    {
+      has_collision = false;
+      point = ignition::math::Vector3d(static_cast<double>(std::rand()) / RAND_MAX * 8.0 + 8,
+                                       -6.25 + static_cast<double>(std::rand()) / RAND_MAX * 7.25,
+                                       0.4);
+      for (const auto& pre_point : this->box_points)
+      {
+        const double dist = (point - pre_point).Length();
+        if (dist <= 1.2)
+        {
+          has_collision = true;
+          break;
+        }
+      }
+    } 
+
+    // Add this box to the list
+    this->box_points.push_back(point);
+    const std::string box_name = "number" + std::to_string(i);
+    this->box_names.push_back(box_name);
+
+    msgs::Factory box_msg;
+    box_msg.set_sdf_filename("model://" + box_name); // TODO: change to our own file
+    // ignition::math::Vector3d spawn_point = ignition::math::Vector3d(point.X(), point.Y(), static_cast<double>(std::rand()) / RAND_MAX * 1.5 + 0.5);
+    msgs::Set(box_msg.mutable_pose(), ignition::math::Pose3d(point, ignition::math::Quaterniond(0, 0, 0)));
+    this->pub_factory_->Publish(box_msg);
+
+    visualization_msgs::Marker box_marker;
+    box_marker.header.frame_id = "world";
+    box_marker.header.stamp = ros::Time();
+    box_marker.ns = "gazebo";
+    box_marker.id = i;
+    box_marker.type = visualization_msgs::Marker::CUBE;
+    box_marker.action = visualization_msgs::Marker::ADD;
+    box_marker.frame_locked = true;
+    box_marker.lifetime = ros::Duration(0.2);
+    box_marker.pose.position.x = point.X();
+    box_marker.pose.position.y = point.Y();
+    box_marker.pose.position.z = point.Z();
+    box_marker.pose.orientation.x = 0.0;
+    box_marker.pose.orientation.y = 0.0;
+    box_marker.pose.orientation.z = 0.0;
+    box_marker.pose.orientation.w = 1.0;
+    box_marker.scale.x = 0.8;
+    box_marker.scale.y = 0.8;
+    box_marker.scale.z = 0.8;
+    box_marker.color.a = 0.7;
+    box_marker.color.r = static_cast<double>(std::rand()) / RAND_MAX * 0.5 + 0.25;
+    box_marker.color.g = static_cast<double>(std::rand()) / RAND_MAX * 0.5 + 0.25;
+    box_marker.color.b = static_cast<double>(std::rand()) / RAND_MAX * 0.5 + 0.25;
+    this->box_markers_msg_.markers.emplace_back(box_marker);
+
+    visualization_msgs::Marker text_marker = box_marker;
+    text_marker.id = num + i;
+    text_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+    text_marker.text = std::to_string(i);
+    text_marker.pose.position.z += 0.5;
+    text_marker.scale.z = 0.5;
+    text_marker.color.a = 0.8;
+    text_marker.color.r = 0.0;
+    text_marker.color.g = 0.0;
+    text_marker.color.b = 0.0;
+    text_markers_msg.markers.emplace_back(text_marker);
+  }
+
+  // remove the tree point
+  this->box_points.erase(this->box_points.begin());
+  // merge the two marker arrays
+  this->box_markers_msg_.markers.insert(this->box_markers_msg_.markers.end(), text_markers_msg.markers.begin(), text_markers_msg.markers.end());
+
+  return;
+};
+
+void ObjectSpawner::deleteObject(const std::string& object_name)
+{
+  gazebo_msgs::DeleteModel delete_model_srv;
+  delete_model_srv.request.model_name = object_name;
+  this->clt_delete_objects_.call(delete_model_srv);
+  if (delete_model_srv.response.success == true)
+  {
+    ROS_INFO_STREAM("Object: " << object_name << "successfully deleted");
+  }
+  else
+  {
+    ROS_ERROR_STREAM("Failed to delete Object: " << object_name << std::endl);
+  }
+
+  return;
+};
+
+void ObjectSpawner::deleteCone()
+{
+  deleteObject(this->cone_name);
+  this->cone_name = "";
+  this->cone_point = ignition::math::Vector3d();
+
+  return;
+};
+
+void ObjectSpawner::deleteBoxs()
+{
+  this->box_markers_msg_.markers.clear();
+  this->pub_rviz_markers_.publish(this->box_markers_msg_);
+
+  for (const auto& box_name: this->box_names)
+  {
+    deleteObject(box_name);
+  }
+  this->box_names.clear();
+  this->box_points.clear();
+
+  return;
+};
+
+void ObjectSpawner::respawnCmdCallback(const std_msgs::Int16::ConstPtr& respawn_msg)
+{
+  const int cmd = respawn_msg->data;
+  if (cmd == 0)
+  {
+    deleteCone();
+    deleteBoxs();
+    ROS_INFO_STREAM("Random Objects Cleared!");
+  }
+  else if (cmd == 1)
+  {
+    deleteCone();
+    deleteBoxs();
+    spawnRandomCones();
+    spawnRandomBoxes(9);
+    ROS_INFO_STREAM("Random Objects Respawned!");
+  }
+  else
+  {
+    ROS_INFO_STREAM("Respawn Command Not Recognized!");
+  }
+
+  return;
+};
+
+} // namespace gazebo
--- a/src/worlds/me5413_project.world
+++ b/src/worlds/me5413_project.world