From 40321b0dfba73917995b21a3a30e089ca3be2c0a Mon Sep 17 00:00:00 2001
From: Zach Ghera <zpghera00@gmail.com>
Date: Mon, 28 Dec 2020 18:34:09 -0500
Subject: [PATCH 01/10] Slight formatting changes in kart_rosbag.launch.

---
 src/kart_2dnav/launch/kart_rosbag.launch | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/kart_2dnav/launch/kart_rosbag.launch b/src/kart_2dnav/launch/kart_rosbag.launch
index 2d9dd90..fe3e51f 100644
--- a/src/kart_2dnav/launch/kart_rosbag.launch
+++ b/src/kart_2dnav/launch/kart_rosbag.launch
@@ -2,6 +2,7 @@
 
 <launch>
   <!-- This parameter instructs the system to use a simulated clock. 
+
        This file is meant soely for testing with bagged environment data 
        (rosbags) so that is why we set it here.
 
@@ -88,5 +89,6 @@
        Note: This should only be set when running the system with simulated 
              sensor data (rosbags).
   -->
-  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find kart_2dnav)/params/kart_rviz.rviz" />
+  <node name="rviz" pkg="rviz" type="rviz" 
+        args="-d $(find kart_2dnav)/params/kart_rviz.rviz" />
 </launch>

From 4e24aed081acd061a2af50927f2ccb70510669b5 Mon Sep 17 00:00:00 2001
From: Zach Ghera <zpghera00@gmail.com>
Date: Wed, 30 Dec 2020 11:04:33 -0500
Subject: [PATCH 02/10] Move previous sim launch files to archive.

---
 ...m_autocross_track_only_goal_setting.launch | 69 +++++++++++++++++++
 .../sim_square_track_only_goal_setting.launch | 68 ++++++++++++++++++
 2 files changed, 137 insertions(+)
 create mode 100644 src/kart_2dnav/launch/archived/sim_autocross_track_only_goal_setting.launch
 create mode 100644 src/kart_2dnav/launch/archived/sim_square_track_only_goal_setting.launch

diff --git a/src/kart_2dnav/launch/archived/sim_autocross_track_only_goal_setting.launch b/src/kart_2dnav/launch/archived/sim_autocross_track_only_goal_setting.launch
new file mode 100644
index 0000000..ac04c29
--- /dev/null
+++ b/src/kart_2dnav/launch/archived/sim_autocross_track_only_goal_setting.launch
@@ -0,0 +1,69 @@
+<?xml version="1.0"?>
+
+<!-- 
+  Simulate a carlike robot with the teb_local_planner in stage:
+  - stage
+  - map_server
+  - move_base
+  - static map
+  - amcl
+  - rviz view
+ -->
+<launch>
+
+  <!--  ************** Global Parameters ***************  -->
+  <param name="/use_sim_time" value="true"/>
+
+  <!--  ************** Stage Simulator ***************  -->
+	<node pkg="stage_ros" type="stageros" name="stageros" args="$(find kart_2dnav)/stage/autocross_track.world">
+		<!-- Use top/scan to match naming used for output scan topic from pointcloud_to_laserscan -->
+		<remap from="base_scan" to="top/scan"/>
+  </node>
+
+  <!--  ************** ROS Navigation Stack (move_base) ***************  -->
+	<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+      <rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/costmap_common_params.yaml" command="load" ns="global_costmap" />
+			<!-- Modified teb_local_planner_tutorials version to set laserscan input to move_base come from top/scan -->
+			<rosparam file="$(find kart_2dnav)/params/sim_params/costmap_common_params.yaml" command="load" ns="local_costmap" />
+  		<rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/local_costmap_params.yaml" command="load" />
+  		<rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/global_costmap_params.yaml" command="load" />
+  		<rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/teb_local_planner_params.yaml" command="load" />
+
+		<param name="base_global_planner" value="global_planner/GlobalPlanner" />
+		<param name="planner_frequency" value="1.0" />
+		<param name="planner_patience" value="5.0" />
+
+		<param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
+		<param name="controller_frequency" value="5.0" />
+		<param name="controller_patience" value="15.0" />
+
+		<!-- Our carlike robot is not able to rotate in place -->
+    <param name="clearing_rotation_allowed" value="false" />  
+	</node>
+
+  <!-- Run the goal setting script to navigate in SLAM mode -->
+  <node pkg="slam_mode_goal" type="slam_mode_goal" name="slam_mode_goal" output="screen">
+  </node>
+
+  <!-- MICROCONTROLLER COMM -->
+  <!-- Run the communication script to enable and start communicating with microcontroller -->
+  <node pkg="ti_comm" type="ti_comm_node" name="ti_comm_node" output="screen" />
+
+	<!--  ****** Use a Known (Static) Map *****  -->
+	<node name="map_server" pkg="map_server" type="map_server" args="$(find kart_2dnav)/maps/autocross.yaml" output="screen">
+		<param name="frame_id" value="map"/>
+	</node>
+
+	<node pkg="amcl" type="amcl" name="amcl" output="screen">
+		<!-- Change default subscribed scan topic from scan to top/scan -->
+		<remap from="scan" to="top/scan"/>
+
+    <param name="initial_pose_x" value="4"/>
+		<param name="initial_pose_y" value="1.5"/>
+		<param name="initial_pose_a" value="0"/>
+	</node>
+
+  <!--  **************** Visualisation ****************  -->
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find teb_local_planner_tutorials)/cfg/rviz_navigation.rviz"/>
+
+</launch>
diff --git a/src/kart_2dnav/launch/archived/sim_square_track_only_goal_setting.launch b/src/kart_2dnav/launch/archived/sim_square_track_only_goal_setting.launch
new file mode 100644
index 0000000..b7e7f54
--- /dev/null
+++ b/src/kart_2dnav/launch/archived/sim_square_track_only_goal_setting.launch
@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+
+<!-- 
+  Simulate a carlike robot with the teb_local_planner in stage:
+  - stage
+  - map_server
+  - move_base
+  - static map
+  - amcl
+  - rviz view
+ -->
+<launch>
+
+  <!--  ************** Global Parameters ***************  -->
+  <param name="/use_sim_time" value="true"/>
+
+  <!--  ************** Stage Simulator ***************  -->
+	<node pkg="stage_ros" type="stageros" name="stageros" args="$(find kart_2dnav)/stage/square_track.world">
+		<!-- Use top/scan to match naming used for output scan topic from pointcloud_to_laserscan -->
+		<remap from="base_scan" to="top/scan"/>
+  </node>
+
+  <!--  ************** Navigation ***************  -->
+	<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+      <rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/costmap_common_params.yaml" command="load" ns="global_costmap" />
+			<!-- Modified teb_local_planner_tutorials version to set laserscan input to move_base come from top/scan -->
+			<rosparam file="$(find kart_2dnav)/params/sim_params/costmap_common_params.yaml" command="load" ns="local_costmap" />
+  		<rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/local_costmap_params.yaml" command="load" />
+  		<rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/global_costmap_params.yaml" command="load" />
+  		<rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/teb_local_planner_params.yaml" command="load" />
+
+		<param name="base_global_planner" value="global_planner/GlobalPlanner" />
+		<param name="planner_frequency" value="1.0" />
+		<param name="planner_patience" value="5.0" />
+
+		<param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
+		<param name="controller_frequency" value="5.0" />
+		<param name="controller_patience" value="15.0" />
+
+    <param name="clearing_rotation_allowed" value="false" />  <!-- Our carlike robot is not able to rotate in place -->
+	</node>
+
+  <!-- Run the goal setting script to navigate in SLAM mode -->
+  <node pkg="slam_mode_goal" type="slam_mode_goal" name="slam_mode_goal" output="screen">
+  </node>
+
+  <!-- MICROCONTROLLER COMM -->
+  <!-- Run the communication script to enable and start communicating with microcontroller -->
+  <node pkg="ti_comm" type="ti_comm_node" name="ti_comm_node" output="screen" />
+
+	<!--  ****** Use a Known (Static) Map *****  -->
+	<node name="map_server" pkg="map_server" type="map_server" args="$(find kart_2dnav)/maps/square.yaml" output="screen">
+		<param name="frame_id" value="map"/>
+	</node>
+
+	<node pkg="amcl" type="amcl" name="amcl" output="screen">
+		<!-- Change default subscribed scan topic from scan to top/scan -->
+		<remap from="scan" to="top/scan"/>
+
+    <param name="initial_pose_x" value="4"/>
+		<param name="initial_pose_y" value="1.5"/>
+		<param name="initial_pose_a" value="0"/>
+	</node>
+
+  <!--  **************** Visualisation ****************  -->
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find teb_local_planner_tutorials)/cfg/rviz_navigation.rviz"/>
+
+</launch>

From f21e092d40d8eb5810095d409bcc14808dbd6e63 Mon Sep 17 00:00:00 2001
From: Zach Ghera <zpghera00@gmail.com>
Date: Wed, 30 Dec 2020 19:14:01 -0500
Subject: [PATCH 03/10] Failed attempt to try to remove/ignore stageros'
 frames.

I tried to use tf_remap to change the names of the tf frames so that our nodes can publish the tfs we have like in the kart launch files. But this does not work because there is no good way to remove tfs from stageros. See a pretty good explination here: https://answers.ros.org/question/175013/removingdeletingignoring-odom-tf-data/. Like option 1 in that post, I am also going to try to directly modify stageros to force it to not publish the base_footprint and odom frames.
---
 .../launch/sim_autocross_track.launch         | 112 ++++++++++--------
 1 file changed, 63 insertions(+), 49 deletions(-)

diff --git a/src/kart_2dnav/launch/sim_autocross_track.launch b/src/kart_2dnav/launch/sim_autocross_track.launch
index ac04c29..c5fba1c 100644
--- a/src/kart_2dnav/launch/sim_autocross_track.launch
+++ b/src/kart_2dnav/launch/sim_autocross_track.launch
@@ -1,69 +1,83 @@
 <?xml version="1.0"?>
 
-<!-- 
-  Simulate a carlike robot with the teb_local_planner in stage:
-  - stage
-  - map_server
-  - move_base
-  - static map
-  - amcl
-  - rviz view
- -->
 <launch>
-
-  <!--  ************** Global Parameters ***************  -->
+  <!-- SIMULATION CONFIGURATION -->
   <param name="/use_sim_time" value="true"/>
 
-  <!--  ************** Stage Simulator ***************  -->
-	<node pkg="stage_ros" type="stageros" name="stageros" args="$(find kart_2dnav)/stage/autocross_track.world">
-		<!-- Use top/scan to match naming used for output scan topic from pointcloud_to_laserscan -->
+  <!-- Launch the RViz simulation -->
+	<node pkg="stage_ros" type="stageros" name="stageros" 
+        args="$(find kart_2dnav)/stage/autocross_track.world">
+		<!-- Remap the scan topic to top/scan to match naming used for output 
+         scan topic from pointcloud_to_laserscan -->
 		<remap from="base_scan" to="top/scan"/>
+    <!-- Remap the odom published by the simulation to a topic that will 
+         not be used by move_base. Rather, the odom publushed by 
+         laser_scan_matcher_odom will be used. -->
+		<remap from="odom" to="sim/odom"/>
+    <!-- Remap to tf_old so we can use tf_remap to make the frames consistent 
+         with what the kart launch files use.-->
+		<!-- <remap from="tf" to="tf_old"/> -->
   </node>
 
-  <!--  ************** ROS Navigation Stack (move_base) ***************  -->
-	<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
-      <rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/costmap_common_params.yaml" command="load" ns="global_costmap" />
-			<!-- Modified teb_local_planner_tutorials version to set laserscan input to move_base come from top/scan -->
-			<rosparam file="$(find kart_2dnav)/params/sim_params/costmap_common_params.yaml" command="load" ns="local_costmap" />
-  		<rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/local_costmap_params.yaml" command="load" />
-  		<rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/global_costmap_params.yaml" command="load" />
-  		<rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/teb_local_planner_params.yaml" command="load" />
-
-		<param name="base_global_planner" value="global_planner/GlobalPlanner" />
-		<param name="planner_frequency" value="1.0" />
-		<param name="planner_patience" value="5.0" />
+  <!--
+  <node pkg="tf" type="tf_remap" name="tf_remap">
+        <rosparam param="mappings">
+          [ {old: odom,            new: /sim_odom}, 
+            {old: base_footprint,  new: /sim_base_footprint}, 
+            {old: base_link,       new: /sim_base_link},
+            {old: base_laser_link, new: /sim_velodyne} ]
+        </rosparam>
+  </node>
+  -->
 
-		<param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
-		<param name="controller_frequency" value="5.0" />
-		<param name="controller_patience" value="15.0" />
+  <!-- LASER ODOMETRY -->
+  <!-- This node is responsible for taking incremental 2D laserscans (/top/scan)
+	     and outputting estimated odometry information as a tf (odom->base_link) 
+			 and by publishing nav_msgs/Odometry messages to the /odom topic. -->
+  <node pkg="laser_scan_matcher_odom" type="laser_scan_matcher_node_odom" 
+	      name="laser_scan_matcher_node_odom" output="screen">
+      <remap from="scan" to="/top/scan" /> 
+      <rosparam file="$(find kart_2dnav)/params/laser_scan_matcher_params.yaml" 
+			          command="load" />
+  </node>
 
-		<!-- Our carlike robot is not able to rotate in place -->
-    <param name="clearing_rotation_allowed" value="false" />  
-	</node>
+  <!-- SLAM: LOCALIZATION & MAPPING -->
+  <!-- Run the SLAM package, HectorMapping, that takes in 2D laser scans 
+	     (/top/scan) and publishes topics for the created map (/map) and 
+			 estimated pose (/slam_out_pose) of the kart relative to the map. 
+			 This node is also responsible for publishing the tf map->odom. --> 
+  <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
+      <remap from="scan" to="/top/scan" />
+      <rosparam file="$(find kart_2dnav)/params/hector_mapping_params.yaml" command="load" />
+  </node>
 
-  <!-- Run the goal setting script to navigate in SLAM mode -->
+  <!-- GOAL SETTING -->
+  <!-- Run the goal setting algorithm to navigate without a preliminary map (lap 1).
+	     This node takes 2D laser scans (/top/scan) then constructs a goal message and 
+			 creates an ActionClient to sends the goal as ROS actions to SimpleActionServer 
+			 on move_base. -->
+  <!-- TODO: Figure out how to incorporate ROS params with goal setting script -->
   <node pkg="slam_mode_goal" type="slam_mode_goal" name="slam_mode_goal" output="screen">
   </node>
 
+  <!-- ROS NAVIGATION STACK -->
+  <!-- Configure and run move_base to set up the ROS Navigation Stack. -->
+  <!-- More details on inputs/outputs can be found in move_base.launch -->
+  <include file="$(find kart_2dnav)/launch/move_base.launch" />
+
   <!-- MICROCONTROLLER COMM -->
-  <!-- Run the communication script to enable and start communicating with microcontroller -->
+  <!-- Run the communication script to enable and start communicating with the 
+	     microcontroller. This node subscribed to the /cmd_vel topic published by 
+			 move_base and then then sends packets containing the velocity and steering 
+			 angle to the MCU over a serial connection. -->
   <node pkg="ti_comm" type="ti_comm_node" name="ti_comm_node" output="screen" />
 
-	<!--  ****** Use a Known (Static) Map *****  -->
-	<node name="map_server" pkg="map_server" type="map_server" args="$(find kart_2dnav)/maps/autocross.yaml" output="screen">
-		<param name="frame_id" value="map"/>
-	</node>
-
-	<node pkg="amcl" type="amcl" name="amcl" output="screen">
-		<!-- Change default subscribed scan topic from scan to top/scan -->
-		<remap from="scan" to="top/scan"/>
-
-    <param name="initial_pose_x" value="4"/>
-		<param name="initial_pose_y" value="1.5"/>
-		<param name="initial_pose_a" value="0"/>
-	</node>
+  <!-- Launch rviz with the configuration file in order to visualize incoming data
 
-  <!--  **************** Visualisation ****************  -->
-  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find teb_local_planner_tutorials)/cfg/rviz_navigation.rviz"/>
+       Note: This should only be set when running the system with simulated 
+             sensor data (rosbags).
+  -->
+  <node name="rviz" pkg="rviz" type="rviz" 
+        args="-d $(find kart_2dnav)/params/kart_rviz.rviz" />
 
 </launch>

From 79109aea21352076245676f0c68328021901b4b3 Mon Sep 17 00:00:00 2001
From: Zach Ghera <zpghera00@gmail.com>
Date: Sat, 2 Jan 2021 19:38:30 -0500
Subject: [PATCH 04/10] Remove the duplicated (old) stage_ros node.

---
 src/kart_2dnav/launch/sim_autocross_track.launch | 2 --
 1 file changed, 2 deletions(-)

diff --git a/src/kart_2dnav/launch/sim_autocross_track.launch b/src/kart_2dnav/launch/sim_autocross_track.launch
index efaa1f8..91e7ac0 100644
--- a/src/kart_2dnav/launch/sim_autocross_track.launch
+++ b/src/kart_2dnav/launch/sim_autocross_track.launch
@@ -5,8 +5,6 @@
   <param name="/use_sim_time" value="true"/>
 
   <!-- Launch the RViz simulation -->
-	<node pkg="stage_ros" type="stageros" name="stageros" 
-        args="$(find kart_2dnav)/stage/autocross_track.world">
 	<node pkg="stage_ros_mod_tf" type="stage_ros_mod_tf_node" 
         name="stage_ros_mod_tf_node" 
         args="$(find kart_2dnav)/stage/autocross_track.world">

From 49761bf6a654b8829d792724407172df0b6513c5 Mon Sep 17 00:00:00 2001
From: Zach Ghera <zpghera00@gmail.com>
Date: Sat, 2 Jan 2021 19:40:51 -0500
Subject: [PATCH 05/10] Get rid of duplicate sim launch for square track.

---
 src/kart_2dnav/launch/sim_square_track.launch | 68 -------------------
 1 file changed, 68 deletions(-)
 delete mode 100644 src/kart_2dnav/launch/sim_square_track.launch

diff --git a/src/kart_2dnav/launch/sim_square_track.launch b/src/kart_2dnav/launch/sim_square_track.launch
deleted file mode 100644
index b7e7f54..0000000
--- a/src/kart_2dnav/launch/sim_square_track.launch
+++ /dev/null
@@ -1,68 +0,0 @@
-<?xml version="1.0"?>
-
-<!-- 
-  Simulate a carlike robot with the teb_local_planner in stage:
-  - stage
-  - map_server
-  - move_base
-  - static map
-  - amcl
-  - rviz view
- -->
-<launch>
-
-  <!--  ************** Global Parameters ***************  -->
-  <param name="/use_sim_time" value="true"/>
-
-  <!--  ************** Stage Simulator ***************  -->
-	<node pkg="stage_ros" type="stageros" name="stageros" args="$(find kart_2dnav)/stage/square_track.world">
-		<!-- Use top/scan to match naming used for output scan topic from pointcloud_to_laserscan -->
-		<remap from="base_scan" to="top/scan"/>
-  </node>
-
-  <!--  ************** Navigation ***************  -->
-	<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
-      <rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/costmap_common_params.yaml" command="load" ns="global_costmap" />
-			<!-- Modified teb_local_planner_tutorials version to set laserscan input to move_base come from top/scan -->
-			<rosparam file="$(find kart_2dnav)/params/sim_params/costmap_common_params.yaml" command="load" ns="local_costmap" />
-  		<rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/local_costmap_params.yaml" command="load" />
-  		<rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/global_costmap_params.yaml" command="load" />
-  		<rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/teb_local_planner_params.yaml" command="load" />
-
-		<param name="base_global_planner" value="global_planner/GlobalPlanner" />
-		<param name="planner_frequency" value="1.0" />
-		<param name="planner_patience" value="5.0" />
-
-		<param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
-		<param name="controller_frequency" value="5.0" />
-		<param name="controller_patience" value="15.0" />
-
-    <param name="clearing_rotation_allowed" value="false" />  <!-- Our carlike robot is not able to rotate in place -->
-	</node>
-
-  <!-- Run the goal setting script to navigate in SLAM mode -->
-  <node pkg="slam_mode_goal" type="slam_mode_goal" name="slam_mode_goal" output="screen">
-  </node>
-
-  <!-- MICROCONTROLLER COMM -->
-  <!-- Run the communication script to enable and start communicating with microcontroller -->
-  <node pkg="ti_comm" type="ti_comm_node" name="ti_comm_node" output="screen" />
-
-	<!--  ****** Use a Known (Static) Map *****  -->
-	<node name="map_server" pkg="map_server" type="map_server" args="$(find kart_2dnav)/maps/square.yaml" output="screen">
-		<param name="frame_id" value="map"/>
-	</node>
-
-	<node pkg="amcl" type="amcl" name="amcl" output="screen">
-		<!-- Change default subscribed scan topic from scan to top/scan -->
-		<remap from="scan" to="top/scan"/>
-
-    <param name="initial_pose_x" value="4"/>
-		<param name="initial_pose_y" value="1.5"/>
-		<param name="initial_pose_a" value="0"/>
-	</node>
-
-  <!--  **************** Visualisation ****************  -->
-  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find teb_local_planner_tutorials)/cfg/rviz_navigation.rviz"/>
-
-</launch>

From 235b75bdc1c7c64d334233ad15c068aef1ffda71 Mon Sep 17 00:00:00 2001
From: Zach Ghera <zpghera00@gmail.com>
Date: Sat, 2 Jan 2021 20:32:45 -0500
Subject: [PATCH 06/10] Add new stage_ros modified package to top level files
 of kart_2dnav.

---
 src/kart_2dnav/CMakeLists.txt | 1 +
 src/kart_2dnav/package.xml    | 4 +---
 2 files changed, 2 insertions(+), 3 deletions(-)

diff --git a/src/kart_2dnav/CMakeLists.txt b/src/kart_2dnav/CMakeLists.txt
index b9eac04..aa025a6 100644
--- a/src/kart_2dnav/CMakeLists.txt
+++ b/src/kart_2dnav/CMakeLists.txt
@@ -15,6 +15,7 @@ find_package(catkin REQUIRED COMPONENTS
   velodyne_pointcloud
 	pointcloud_to_laserscan
 	laser_scan_matcher_odom
+	stage_ros_mod_tf
 	hector_mapping
   base_local_planner
 	global_planner
diff --git a/src/kart_2dnav/package.xml b/src/kart_2dnav/package.xml
index be1381f..a661c48 100644
--- a/src/kart_2dnav/package.xml
+++ b/src/kart_2dnav/package.xml
@@ -57,15 +57,13 @@
   <depend>velodyne_pointcloud</depend>
   <depend>pointcloud_to_laserscan</depend>
   <depend>laser_scan_matcher_odom</depend>
+  <depend>stage_ros_mod_tf</depend>
   <depend>hector_mapping</depend>
   <depend>base_local_planner</depend>
   <depend>global_planner</depend>
   <depend>move_base</depend>
   <depend>teb_local_planner</depend>
   <depend>teb_local_planner_tutorials</depend>
-  <!-- <depend>sbg_driver</depend>
-  <depend>sbg_to_imu</depend>
-  <depend>imu_filter_madgwick</depend> -->
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

From 0677821280a8c3f5269063932e63e63ec61b2528 Mon Sep 17 00:00:00 2001
From: Zach Ghera <zpghera00@gmail.com>
Date: Tue, 5 Jan 2021 18:43:27 -0500
Subject: [PATCH 07/10] No need for tf_remap node now due to work in #12 and
 #13.

---
 src/kart_2dnav/launch/sim_autocross_track.launch | 11 -----------
 1 file changed, 11 deletions(-)

diff --git a/src/kart_2dnav/launch/sim_autocross_track.launch b/src/kart_2dnav/launch/sim_autocross_track.launch
index 91e7ac0..b4eb08e 100644
--- a/src/kart_2dnav/launch/sim_autocross_track.launch
+++ b/src/kart_2dnav/launch/sim_autocross_track.launch
@@ -13,17 +13,6 @@
 		<remap from="base_scan" to="top/scan"/>
   </node>
 
-  <!--
-  <node pkg="tf" type="tf_remap" name="tf_remap">
-        <rosparam param="mappings">
-          [ {old: odom,            new: /sim_odom}, 
-            {old: base_footprint,  new: /sim_base_footprint}, 
-            {old: base_link,       new: /sim_base_link},
-            {old: base_laser_link, new: /sim_velodyne} ]
-        </rosparam>
-  </node>
-  -->
-
   <!-- LASER ODOMETRY -->
   <!-- This node is responsible for taking incremental 2D laserscans (/top/scan)
 	     and outputting estimated odometry information as a tf (odom->base_link) 

From 4e2e5bad707d0517fd94c21ff86acde540e14a42 Mon Sep 17 00:00:00 2001
From: Zach Ghera <zpghera00@gmail.com>
Date: Tue, 5 Jan 2021 20:43:30 -0500
Subject: [PATCH 08/10] Temporary use of teb_local_planner's config of
 move_base to get the simulation working before having to tune move_base
 params.

---
 .../launch/sim_autocross_track.launch         | 27 +++++++++++++++++++
 1 file changed, 27 insertions(+)

diff --git a/src/kart_2dnav/launch/sim_autocross_track.launch b/src/kart_2dnav/launch/sim_autocross_track.launch
index b4eb08e..ca3115e 100644
--- a/src/kart_2dnav/launch/sim_autocross_track.launch
+++ b/src/kart_2dnav/launch/sim_autocross_track.launch
@@ -46,7 +46,34 @@
   <!-- ROS NAVIGATION STACK -->
   <!-- Configure and run move_base to set up the ROS Navigation Stack. -->
   <!-- More details on inputs/outputs can be found in move_base.launch -->
+  <!--
   <include file="$(find kart_2dnav)/launch/move_base.launch" />
+  -->
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+      <rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/costmap_common_params.yaml" 
+                command="load" ns="global_costmap" />
+      <!-- Modified teb_local_planner_tutorials version to set laserscan input 
+           to move_base come from top/scan -->
+      <rosparam file="$(find kart_2dnav)/params/sim_params/costmap_common_params.yaml" 
+                command="load" ns="local_costmap" />
+      <rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/local_costmap_params.yaml" 
+                command="load" />
+      <rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/global_costmap_params.yaml" 
+                command="load" />
+      <rosparam 
+            file="$(find teb_local_planner_tutorials)/cfg/carlike/teb_local_planner_params.yaml" 
+            command="load" />
+      <param name="base_global_planner" value="global_planner/GlobalPlanner" />
+      <param name="planner_frequency" value="1.0" />
+      <param name="planner_patience" value="5.0" />
+
+      <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
+      <param name="controller_frequency" value="5.0" />
+      <param name="controller_patience" value="15.0" />
+      <!-- Our carlike robot is not able to rotate in place -->
+      <param name="clearing_rotation_allowed" value="false" />  
+  </node>
+
 
   <!-- MICROCONTROLLER COMM -->
   <!-- Run the communication script to enable and start communicating with the 

From ebd2377b70652932c27930367c8cb239905da106 Mon Sep 17 00:00:00 2001
From: Zach Ghera <zpghera00@gmail.com>
Date: Mon, 5 Apr 2021 23:25:11 -0400
Subject: [PATCH 09/10] Tune move_base.launch params for successful sim run.

The param files associated with move_base.launch were modified to (mostly) match teb_local_planner_tutorials' param files as the simulated kart needed to 'fit' on the simulation track we took from teb_local_planner_tutorials. Create Issue #19 for determine a way to modify the params to meet our karts physical params while also maintaining our ability to test in simulation.
---
 .../launch/sim_autocross_track.launch         | 27 -------------------
 .../params/costmap_common_params.yaml         | 24 ++++++++++-------
 .../params/global_costmap_params.yaml         |  7 ++---
 .../params/local_costmap_params.yaml          |  7 +++--
 .../params/teb_local_planner_params.yaml      | 18 ++++++++-----
 5 files changed, 32 insertions(+), 51 deletions(-)

diff --git a/src/kart_2dnav/launch/sim_autocross_track.launch b/src/kart_2dnav/launch/sim_autocross_track.launch
index ca3115e..b4eb08e 100644
--- a/src/kart_2dnav/launch/sim_autocross_track.launch
+++ b/src/kart_2dnav/launch/sim_autocross_track.launch
@@ -46,34 +46,7 @@
   <!-- ROS NAVIGATION STACK -->
   <!-- Configure and run move_base to set up the ROS Navigation Stack. -->
   <!-- More details on inputs/outputs can be found in move_base.launch -->
-  <!--
   <include file="$(find kart_2dnav)/launch/move_base.launch" />
-  -->
-  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
-      <rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/costmap_common_params.yaml" 
-                command="load" ns="global_costmap" />
-      <!-- Modified teb_local_planner_tutorials version to set laserscan input 
-           to move_base come from top/scan -->
-      <rosparam file="$(find kart_2dnav)/params/sim_params/costmap_common_params.yaml" 
-                command="load" ns="local_costmap" />
-      <rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/local_costmap_params.yaml" 
-                command="load" />
-      <rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/global_costmap_params.yaml" 
-                command="load" />
-      <rosparam 
-            file="$(find teb_local_planner_tutorials)/cfg/carlike/teb_local_planner_params.yaml" 
-            command="load" />
-      <param name="base_global_planner" value="global_planner/GlobalPlanner" />
-      <param name="planner_frequency" value="1.0" />
-      <param name="planner_patience" value="5.0" />
-
-      <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
-      <param name="controller_frequency" value="5.0" />
-      <param name="controller_patience" value="15.0" />
-      <!-- Our carlike robot is not able to rotate in place -->
-      <param name="clearing_rotation_allowed" value="false" />  
-  </node>
-
 
   <!-- MICROCONTROLLER COMM -->
   <!-- Run the communication script to enable and start communicating with the 
diff --git a/src/kart_2dnav/params/costmap_common_params.yaml b/src/kart_2dnav/params/costmap_common_params.yaml
index e152782..2ec8c51 100644
--- a/src/kart_2dnav/params/costmap_common_params.yaml
+++ b/src/kart_2dnav/params/costmap_common_params.yaml
@@ -2,7 +2,11 @@
 # See http://wiki.ros.org/costmap_2d for details.
 
 # TODO(Issue #8): Correct this values for our kart's specific physical attr/sensors.
-footprint: [ [0.7112, 0.4064], [0.7112, -1.524], [-0.7112, -1.524], [-0.7112, 0.4064] ]
+# robot footprint in meters
+# 1. (maybe) footprint of our go-kart
+# footprint: [ [0.7112, 0.4064], [0.7112, -1.524], [-0.7112, -1.524], [-0.7112, 0.4064] ]
+# 2. footprint used by teb_local_planner tutorials. Use this for simulation as of now
+footprint: [ [-0.1,-0.125], [0.5,-0.125], [0.5,0.125], [-0.1,0.125] ]
 
 transform_tolerance: 0.2 
 map_type: costmap
@@ -10,12 +14,14 @@ map_type: costmap
 obstacle_layer:
     enabled: true 
     # TODO(Issue #8): Correct this values for our kart's specific physical attr/sensors.
-    obstacle_range: 2.5 
-    # TODO(Issue #8): Correct this values for our kart's specific physical attr/sensors.
-    max_obstacle_height: 2.0 
+    obstacle_range: 3.0
     # TODO(Issue #8): Will likely change in tandem with obstacle_range above.
-    raytrace_range: 3.0
-    inflation_radius: 0.5 
+    raytrace_range: 3.5
+    # TODO(Issue #8): Correct this values for our kart's specific physical attr/sensors.
+    max_obstacle_height: 2.0
+    min_obstacle_height: 0.0
+    # ------------------------
+    inflation_radius: 0.2
     track_unknown_space: false
     combination_method: 1
     observation_sources: laser_scan_sensor point_cloud_sensor
@@ -26,10 +32,8 @@ obstacle_layer:
 
 inflation_layer:
     enabled: true
-    cost_scaling_factor: 10.0 # exponential rate at which the obstacle 
-                              # cost drops off [default]
-    inflation_radius: 0.5 # max distance from an obstacle at which costs 
-                          # are incurred for planning paths [default]
+    cost_scaling_factor: 10.0
+    inflation_radius: 0.5
 
 static_layer:
     enabled: true
diff --git a/src/kart_2dnav/params/global_costmap_params.yaml b/src/kart_2dnav/params/global_costmap_params.yaml
index 1bb3b8b..9a6211c 100644
--- a/src/kart_2dnav/params/global_costmap_params.yaml
+++ b/src/kart_2dnav/params/global_costmap_params.yaml
@@ -5,11 +5,12 @@ global_costmap:
     global_frame: "map"
     robot_base_frame: "base_link"
     update_frequency: 5.0
-    public_frequency: 2.5
-    static_map: false
+    public_frequency: 0.5 # match teb_local_planner_tutorials
+    static_map: false     # we are not using a map server / existing map
 
-    transform_tolerance: 0.5
+    transform_tolerance: 0.2
     plugins:
         - {name: static_layer, type:    "costmap_2d::StaticLayer"}
         - {name: obstacle_layer, type:  "costmap_2d::VoxelLayer"}
         - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+
diff --git a/src/kart_2dnav/params/local_costmap_params.yaml b/src/kart_2dnav/params/local_costmap_params.yaml
index be6a4ca..6f96d8b 100644
--- a/src/kart_2dnav/params/local_costmap_params.yaml
+++ b/src/kart_2dnav/params/local_costmap_params.yaml
@@ -1,7 +1,6 @@
 # All parameters are set to default values unless otherwise noted.
 # See http://wiki.ros.org/costmap_2d for details.
 
-
 local_costmap:
     global_frame: "map"
     robot_base_frame: "base_link"
@@ -11,9 +10,9 @@ local_costmap:
     rolling_window: true
     # TODO: Revisit width, height, resolution, etc. here once we start looking 
     #       at creating a map from first lap.
-    width: 6.0
-    height: 6.0
-    resolution: 0.05
+    width: 5.5
+    height: 5.5
+    resolution: 0.1
     transform_tolerance: 0.5
 
     plugins:
diff --git a/src/kart_2dnav/params/teb_local_planner_params.yaml b/src/kart_2dnav/params/teb_local_planner_params.yaml
index 0b3a04c..27335b6 100644
--- a/src/kart_2dnav/params/teb_local_planner_params.yaml
+++ b/src/kart_2dnav/params/teb_local_planner_params.yaml
@@ -5,14 +5,14 @@ TebLocalPlannerROS:
 
     odom_topic: "odom"
 
-    # Trajectory of the kart
+    # Trajectory
 
     teb_autosize: True
     dt_ref: 1
     dt_hysteresis: 0.1
     global_plan_overwrite_orientation: True
     allow_init_with_backwards_motion: True
-    max_global_plan_lookahead_dist: 5.0
+    max_global_plan_lookahead_dist: 3.0
     global_plan_viapoint_sep: -0.1
     global_plan_prune_distance: 1 
     exact_arc_length: False
@@ -32,14 +32,18 @@ TebLocalPlannerROS:
     acc_lim_x: 0.5
     acc_lim_theta: 0.5
 
-    min_turning_radius: 0.51
-    wheelbase: 1.0541
+    min_turning_radius: 0.5         # (maybe) phyiscal val: 0.51
+    wheelbase: 0.4                  # (maybe) phyiscal val: 1.0541
     cmd_angle_instead_rotvel: True
 
     footprint_model:
         type: "polygon"
-        vertices: [ [0.7112, 0.4064],  [0.7112, -1.524], 
-                    [-0.7112, -1.524], [-0.7112, 0.4064] ]
+        # 1. (maybe) footprint of our go-kart
+        #vertices: [ [0.7112, 0.4064],  [0.7112, -1.524],
+        #            [-0.7112, -1.524], [-0.7112, 0.4064] ]
+        # 2. footprint used by teb_local_planner tutorials.
+        #    Use this for simulation as of now
+        vertices: [[-0.1,-0.125], [0.5,-0.125], [0.5,0.125], [-0.1,0.125]]
     # ---------------------------------
 
     # Goal Tolerance
@@ -51,7 +55,7 @@ TebLocalPlannerROS:
 
     # Obstacles
 
-    min_obstacle_dist: 0.5 
+    min_obstacle_dist: 0.25
     inflation_dist: 0.6
     include_costmap_obstacles: True
     costmap_obstacles_behind_robot_dist: 1.0

From f19e5a50704f56be682855b098ca3fc486fab87c Mon Sep 17 00:00:00 2001
From: Zach Ghera <zpghera00@gmail.com>
Date: Sat, 25 Sep 2021 11:42:54 -0400
Subject: [PATCH 10/10] Fix weird spacing in comments.

---
 .../launch/sim_autocross_track.launch         | 24 +++++++++----------
 1 file changed, 12 insertions(+), 12 deletions(-)

diff --git a/src/kart_2dnav/launch/sim_autocross_track.launch b/src/kart_2dnav/launch/sim_autocross_track.launch
index b4eb08e..a6bb1a5 100644
--- a/src/kart_2dnav/launch/sim_autocross_track.launch
+++ b/src/kart_2dnav/launch/sim_autocross_track.launch
@@ -8,15 +8,15 @@
 	<node pkg="stage_ros_mod_tf" type="stage_ros_mod_tf_node" 
         name="stage_ros_mod_tf_node" 
         args="$(find kart_2dnav)/stage/autocross_track.world">
-		<!-- Remap the scan topic to top/scan to match naming used for output 
+		<!-- Remap the scan topic to top/scan to match naming used for output
          scan topic from pointcloud_to_laserscan -->
 		<remap from="base_scan" to="top/scan"/>
   </node>
 
   <!-- LASER ODOMETRY -->
   <!-- This node is responsible for taking incremental 2D laserscans (/top/scan)
-	     and outputting estimated odometry information as a tf (odom->base_link) 
-			 and by publishing nav_msgs/Odometry messages to the /odom topic. -->
+       and outputting estimated odometry information as a tf (odom->base_link) 
+       and by publishing nav_msgs/Odometry messages to the /odom topic. -->
   <node pkg="laser_scan_matcher_odom" type="laser_scan_matcher_node_odom" 
 	      name="laser_scan_matcher_node_odom" output="screen">
       <remap from="scan" to="/top/scan" /> 
@@ -26,9 +26,9 @@
 
   <!-- SLAM: LOCALIZATION & MAPPING -->
   <!-- Run the SLAM package, HectorMapping, that takes in 2D laser scans 
-	     (/top/scan) and publishes topics for the created map (/map) and 
-			 estimated pose (/slam_out_pose) of the kart relative to the map. 
-			 This node is also responsible for publishing the tf map->odom. --> 
+       (/top/scan) and publishes topics for the created map (/map) and 
+       estimated pose (/slam_out_pose) of the kart relative to the map. 
+       This node is also responsible for publishing the tf map->odom. --> 
   <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
       <remap from="scan" to="/top/scan" />
       <rosparam file="$(find kart_2dnav)/params/hector_mapping_params.yaml" command="load" />
@@ -36,9 +36,9 @@
 
   <!-- GOAL SETTING -->
   <!-- Run the goal setting algorithm to navigate without a preliminary map (lap 1).
-	     This node takes 2D laser scans (/top/scan) then constructs a goal message and 
-			 creates an ActionClient to sends the goal as ROS actions to SimpleActionServer 
-			 on move_base. -->
+       This node takes 2D laser scans (/top/scan) then constructs a goal message and 
+       creates an ActionClient to sends the goal as ROS actions to SimpleActionServer 
+       on move_base. -->
   <!-- TODO: Figure out how to incorporate ROS params with goal setting script -->
   <node pkg="slam_mode_goal" type="slam_mode_goal" name="slam_mode_goal" output="screen">
   </node>
@@ -50,9 +50,9 @@
 
   <!-- MICROCONTROLLER COMM -->
   <!-- Run the communication script to enable and start communicating with the 
-	     microcontroller. This node subscribed to the /cmd_vel topic published by 
-			 move_base and then then sends packets containing the velocity and steering 
-			 angle to the MCU over a serial connection. -->
+       microcontroller. This node subscribed to the /cmd_vel topic published by 
+       move_base and then then sends packets containing the velocity and steering 
+       angle to the MCU over a serial connection. -->
   <node pkg="ti_comm" type="ti_comm_node" name="ti_comm_node" output="screen" />
 
   <!-- Launch rviz with the configuration file in order to visualize incoming data