From 6bc710456379c3be8ec63d0b6fdcddde4b1e87c4 Mon Sep 17 00:00:00 2001
From: Bryan Zheng <ashuh223@gmail.com>
Date: Mon, 6 Dec 2021 22:26:29 +0800
Subject: [PATCH 01/16] fix(cws): Catch unhandled exception

CollisionChecker may throw runtime_error during start up
---
 collision_warning_system/src/collision_warning_system.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/collision_warning_system/src/collision_warning_system.cpp b/collision_warning_system/src/collision_warning_system.cpp
index 297043d..5984b03 100644
--- a/collision_warning_system/src/collision_warning_system.cpp
+++ b/collision_warning_system/src/collision_warning_system.cpp
@@ -89,6 +89,11 @@ void CollisionWarningSystem::timerCallback(const ros::TimerEvent& timer_event)
     ROS_WARN("%s", ex.what());
     return;
   }
+  catch (const std::runtime_error& ex)
+  {
+    ROS_WARN("%s", ex.what());
+    return;
+  }
 }
 
 void CollisionWarningSystem::odomCallback(const nav_msgs::Odometry& odom_msg)

From 2e987077b836a61b4621e510ed86fb01fdc986a3 Mon Sep 17 00:00:00 2001
From: Bryan Zheng <ashuh223@gmail.com>
Date: Mon, 6 Dec 2021 22:29:22 +0800
Subject: [PATCH 02/16] refactor(cws): Remove namespace

---
 .../include/collision_warning_system/bicycle_model.h      | 6 ------
 .../include/collision_warning_system/bicycle_state.h      | 7 -------
 .../collision_warning_system/collision_warning_system.h   | 7 +------
 collision_warning_system/src/bicycle_model.cpp            | 6 ------
 collision_warning_system/src/bicycle_state.cpp            | 6 ------
 collision_warning_system/src/collision_warning_system.cpp | 8 +-------
 6 files changed, 2 insertions(+), 38 deletions(-)

diff --git a/collision_warning_system/include/collision_warning_system/bicycle_model.h b/collision_warning_system/include/collision_warning_system/bicycle_model.h
index c2dad9e..8cc043e 100644
--- a/collision_warning_system/include/collision_warning_system/bicycle_model.h
+++ b/collision_warning_system/include/collision_warning_system/bicycle_model.h
@@ -9,10 +9,6 @@
 
 #include "collision_warning_system/bicycle_state.h"
 
-namespace f1tenth_racecar
-{
-namespace safety
-{
 class BicycleModel
 {
 private:
@@ -29,7 +25,5 @@ class BicycleModel
                                    const double velocity, const double steering_angle, const double delta_t,
                                    const double t_max);
 };
-}  // namespace safety
-}  // namespace f1tenth_racecar
 
 #endif  // COLLISION_WARNING_SYSTEM_BICYCLE_MODEL_H
diff --git a/collision_warning_system/include/collision_warning_system/bicycle_state.h b/collision_warning_system/include/collision_warning_system/bicycle_state.h
index db77f9d..cb101e2 100644
--- a/collision_warning_system/include/collision_warning_system/bicycle_state.h
+++ b/collision_warning_system/include/collision_warning_system/bicycle_state.h
@@ -1,10 +1,6 @@
 #ifndef COLLISION_WARNING_SYSTEM_BICYCLE_STATE_H
 #define COLLISION_WARNING_SYSTEM_BICYCLE_STATE_H
 
-namespace f1tenth_racecar
-{
-namespace safety
-{
 class BicycleState
 {
 private:
@@ -24,7 +20,4 @@ class BicycleState
   double steering_angle() const;
 };
 
-}  // namespace safety
-}  // namespace f1tenth_racecar
-
 #endif  // COLLISION_WARNING_SYSTEM_BICYCLE_STATE_H
diff --git a/collision_warning_system/include/collision_warning_system/collision_warning_system.h b/collision_warning_system/include/collision_warning_system/collision_warning_system.h
index 93660ad..423c578 100644
--- a/collision_warning_system/include/collision_warning_system/collision_warning_system.h
+++ b/collision_warning_system/include/collision_warning_system/collision_warning_system.h
@@ -14,10 +14,6 @@
 #include "collision_warning_system/bicycle_state.h"
 #include "costmap_generator/collision_checker.h"
 
-namespace f1tenth_racecar
-{
-namespace safety
-{
 class CollisionWarningSystem
 {
 public:
@@ -58,6 +54,5 @@ class CollisionWarningSystem
 
   void visualizeProjectedTrajectory(const nav_msgs::Path& path);
 };
-}  // namespace safety
-}  // namespace f1tenth_racecar
+
 #endif  // COLLISION_WARNING_SYSTEM_COLLISION_WARNING_SYSTEM_H
diff --git a/collision_warning_system/src/bicycle_model.cpp b/collision_warning_system/src/bicycle_model.cpp
index b2349c3..d908cb2 100644
--- a/collision_warning_system/src/bicycle_model.cpp
+++ b/collision_warning_system/src/bicycle_model.cpp
@@ -8,10 +8,6 @@
 
 #include "collision_warning_system/bicycle_model.h"
 
-namespace f1tenth_racecar
-{
-namespace safety
-{
 BicycleModel::BicycleModel(const double wheelbase) : wheelbase_(wheelbase)
 {
 }
@@ -84,5 +80,3 @@ geometry_msgs::PoseStamped BicycleModel::bicycleStateToPoseStamped(const Bicycle
 
   return pose_stamped;
 }
-}  // namespace safety
-}  // namespace f1tenth_racecar
diff --git a/collision_warning_system/src/bicycle_state.cpp b/collision_warning_system/src/bicycle_state.cpp
index cc3249b..c733d13 100644
--- a/collision_warning_system/src/bicycle_state.cpp
+++ b/collision_warning_system/src/bicycle_state.cpp
@@ -1,9 +1,5 @@
 #include "collision_warning_system/bicycle_state.h"
 
-namespace f1tenth_racecar
-{
-namespace safety
-{
 BicycleState::BicycleState(double x, double y, double v, double yaw, double steering_angle)
 {
   x_ = x;
@@ -37,5 +33,3 @@ double BicycleState::steering_angle() const
 {
   return steering_angle_;
 }
-}  // namespace safety
-}  // namespace f1tenth_racecar
diff --git a/collision_warning_system/src/collision_warning_system.cpp b/collision_warning_system/src/collision_warning_system.cpp
index 5984b03..10d3c2e 100644
--- a/collision_warning_system/src/collision_warning_system.cpp
+++ b/collision_warning_system/src/collision_warning_system.cpp
@@ -12,10 +12,6 @@
 #include "collision_warning_system/collision_warning_system.h"
 #include "f1tenth_utils/tf2_wrapper.h"
 
-namespace f1tenth_racecar
-{
-namespace safety
-{
 CollisionWarningSystem::CollisionWarningSystem()
 {
   ros::NodeHandle private_nh("~");
@@ -176,13 +172,11 @@ void CollisionWarningSystem::visualizeProjectedTrajectory(const nav_msgs::Path&
 
   viz_pub_.publish(projected_marker);
 }
-}  // namespace safety
-}  // namespace f1tenth_racecar
 
 int main(int argc, char* argv[])
 {
   ros::init(argc, argv, "collision_warning_system");
-  f1tenth_racecar::safety::CollisionWarningSystem cws;
+  CollisionWarningSystem cws;
   ros::spin();
   return 0;
 }

From fd4d1311fcd60817ba535bf11d549c1803ee2df5 Mon Sep 17 00:00:00 2001
From: Bryan Zheng <ashuh223@gmail.com>
Date: Mon, 6 Dec 2021 22:33:51 +0800
Subject: [PATCH 03/16] refactor(cws): Add node name to ROS_WARN statement

---
 collision_warning_system/src/collision_warning_system.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/collision_warning_system/src/collision_warning_system.cpp b/collision_warning_system/src/collision_warning_system.cpp
index 10d3c2e..a0de8b8 100644
--- a/collision_warning_system/src/collision_warning_system.cpp
+++ b/collision_warning_system/src/collision_warning_system.cpp
@@ -78,7 +78,8 @@ void CollisionWarningSystem::timerCallback(const ros::TimerEvent& timer_event)
     }
 
     time_to_collision_pub_.publish(ttc);
-    ROS_WARN_COND(ttc.data < std::numeric_limits<double>::max(), "Time to collision: %.1f", ttc.data);
+    ROS_WARN_COND(ttc.data < std::numeric_limits<double>::max(), "[Collision Warning System] Time to collision: %.1f",
+                  ttc.data);
   }
   catch (const tf2::TransformException& ex)
   {

From 99546b355f1cc2f5566829d6805ee7156bcaaf71 Mon Sep 17 00:00:00 2001
From: Bryan Zheng <ashuh223@gmail.com>
Date: Tue, 7 Dec 2021 03:09:53 +0800
Subject: [PATCH 04/16] refactor(bringup): Organize launch files

Get vehicle params from the global namespace

Create individual yaml files for each node

Hardcode topic names and use topic remapping instead of params
---
 .../launch/collision_warning_system.launch    | 34 +++----
 .../src/collision_warning_system.cpp          | 26 ++----
 .../launch/costmap_generator.launch           | 18 ++--
 costmap_generator/src/costmap_generator.cpp   | 10 +--
 .../launch/drive_controller.launch            | 20 ++---
 drive_controller/src/drive_controller.cpp     | 17 +---
 .../launch/drive_mode_selector.launch         | 11 ++-
 .../src/drive_mode_selector.cpp               | 19 +---
 .../include/ds4_controller/ds4_controller.h   |  2 -
 ds4_controller/launch/ds4_controller.launch   | 28 +-----
 ds4_controller/src/ds4_controller.cpp         | 19 ++--
 f1tenth_racecar_bringup/config/amcl.yaml      | 30 +++++++
 f1tenth_racecar_bringup/config/autonomy.yaml  | 89 -------------------
 .../config/collision_warning_system.yaml      |  4 +
 f1tenth_racecar_bringup/config/common.yaml    | 22 -----
 .../config/costmap_generator.yaml             |  3 +
 .../config/drive_controller.yaml              |  2 +
 .../config/ds4_controller.yaml                |  6 ++
 .../config/ds4_driver.yaml                    |  6 ++
 f1tenth_racecar_bringup/config/gmapping.yaml  | 34 +++++++
 f1tenth_racecar_bringup/config/joy.yaml       | 12 ---
 .../config/local_planner.yaml                 | 16 ++++
 .../config/vehicle_params.yaml                |  5 ++
 .../config/vesc_ackermann.yaml                |  4 +
 .../config/vesc_driver.yaml                   | 13 +++
 .../launch/autonomy.launch                    | 31 ++++++-
 .../launch/f1tenth_racecar.launch             | 82 +++--------------
 .../launch/f1tenth_racecar_manual.launch      | 46 ----------
 .../launch/f1tenth_sim.launch                 | 23 +++--
 .../launch/f1tenth_slam.launch                | 16 ++++
 f1tenth_racecar_bringup/launch/joy.launch     |  6 +-
 f1tenth_racecar_bringup/launch/test.launch    | 11 +++
 f1tenth_racecar_bringup/launch/vesc.launch    | 16 ++++
 local_planner/launch/local_planner.launch     | 56 ++++++------
 local_planner/src/local_planner.cpp           | 29 ++----
 .../launch/path_recorder_publish.launch       | 15 ++--
 .../launch/path_recorder_record.launch        | 17 ++--
 path_recorder/src/path_recorder.cpp           |  4 +-
 .../launch/safety_controller.launch           | 12 +--
 safety_controller/src/safety_controller.cpp   | 19 +---
 40 files changed, 332 insertions(+), 501 deletions(-)
 create mode 100644 f1tenth_racecar_bringup/config/amcl.yaml
 delete mode 100644 f1tenth_racecar_bringup/config/autonomy.yaml
 create mode 100644 f1tenth_racecar_bringup/config/collision_warning_system.yaml
 delete mode 100644 f1tenth_racecar_bringup/config/common.yaml
 create mode 100644 f1tenth_racecar_bringup/config/costmap_generator.yaml
 create mode 100644 f1tenth_racecar_bringup/config/drive_controller.yaml
 create mode 100644 f1tenth_racecar_bringup/config/ds4_controller.yaml
 create mode 100644 f1tenth_racecar_bringup/config/ds4_driver.yaml
 create mode 100644 f1tenth_racecar_bringup/config/gmapping.yaml
 delete mode 100644 f1tenth_racecar_bringup/config/joy.yaml
 create mode 100644 f1tenth_racecar_bringup/config/local_planner.yaml
 create mode 100644 f1tenth_racecar_bringup/config/vehicle_params.yaml
 create mode 100644 f1tenth_racecar_bringup/config/vesc_ackermann.yaml
 create mode 100644 f1tenth_racecar_bringup/config/vesc_driver.yaml
 delete mode 100644 f1tenth_racecar_bringup/launch/f1tenth_racecar_manual.launch
 create mode 100644 f1tenth_racecar_bringup/launch/f1tenth_slam.launch
 create mode 100644 f1tenth_racecar_bringup/launch/test.launch
 create mode 100644 f1tenth_racecar_bringup/launch/vesc.launch

diff --git a/collision_warning_system/launch/collision_warning_system.launch b/collision_warning_system/launch/collision_warning_system.launch
index 37cacf4..530cb21 100644
--- a/collision_warning_system/launch/collision_warning_system.launch
+++ b/collision_warning_system/launch/collision_warning_system.launch
@@ -1,21 +1,16 @@
 <launch>
-    <arg name="circle_offsets" value="[0.1, 0.3]" />
-    <arg name="circle_radius" value="0.2" />
-    <arg name="t_max" value="2"/>
-    <arg name="delta_t" value="0.1"/>
+    <arg name="circle_offsets" default="[0.1, 0.3]" />
+    <arg name="circle_radius" default="0.2" />
+    <arg name="t_max" default="2"/>
+    <arg name="delta_t" default="0.1"/>
 
-    <arg name="wheelbase" value="0.3"/>
-    <arg name="vehicle_width" value="0.3"/>
-    <arg name="vehicle_length" value="0.5"/>
-    <arg name="base_link_to_center_dist" value="0.1"/>
+    <arg name="wheelbase" default="0.3"/>
+    <arg name="vehicle_width" default="0.3"/>
+    <arg name="vehicle_length" default="0.5"/>
+    <arg name="base_link_to_center_dist" default="0.1"/>
 
-    <arg name="odom_topic" value="odom"/>
-    <arg name="drive_topic" value="drive"/>
-    <arg name="obstacle_topic" value="obstacles"/>
-    <arg name="time_to_collision_topic" value="time_to_collision"/>
-    <arg name="trajectory_topic" value="trajectory"/>
-    <arg name="vehicle_footprints_viz_topic" value="vehicle_footprints_viz"/>
-    <arg name="collision_viz_topic" value="collision_viz"/>
+    <arg name="odom_topic" default="odom"/>
+    <arg name="drive_topic" default="drive"/>
 
     <node name="collision_warning_system" pkg="collision_warning_system" type="collision_warning_system_node" output="screen">
         <rosparam param="circle_offsets" subst_value="True">$(arg circle_offsets)</rosparam>
@@ -28,12 +23,7 @@
         <param name="vehicle_length" value="$(arg vehicle_length)"/>
         <param name="base_link_to_center_dist" value="$(arg base_link_to_center_dist)"/>
 
-        <param name="odom_topic" value="$(arg odom_topic)"/>
-        <param name="drive_topic" value="$(arg drive_topic)"/>
-        <param name="obstacle_topic" value="$(arg obstacle_topic)"/>
-        <param name="time_to_collision_topic" value="$(arg time_to_collision_topic)"/>
-        <param name="trajectory_topic" value="$(arg trajectory_topic)"/>
-        <param name="vehicle_footprints_viz_topic" value="$(arg vehicle_footprints_viz_topic)"/>
-        <param name="collision_viz_topic" value="$(arg collision_viz_topic)"/>
+        <remap from="odom" to="$(arg odom_topic)"/>
+        <remap from="drive" to="$(arg drive_topic)"/>
     </node>
 </launch>
\ No newline at end of file
diff --git a/collision_warning_system/src/collision_warning_system.cpp b/collision_warning_system/src/collision_warning_system.cpp
index a0de8b8..a76bff7 100644
--- a/collision_warning_system/src/collision_warning_system.cpp
+++ b/collision_warning_system/src/collision_warning_system.cpp
@@ -20,33 +20,23 @@ CollisionWarningSystem::CollisionWarningSystem()
   double circle_radius;
   double wheelbase;
 
-  std::string odom_topic;
-  std::string drive_topic;
-  std::string time_to_collision_topic;
-  std::string collision_viz_topic;
-
   private_nh.param("circle_offsets", circle_offsets, std::vector<double>{ 0.1, 0.3 });
   private_nh.param("circle_radius", circle_radius, 0.2);
   private_nh.getParam("t_max", t_max_);
   private_nh.getParam("delta_t", delta_t_);
 
-  private_nh.getParam("wheelbase", wheelbase);
-  private_nh.getParam("vehicle_width", vehicle_width_);
-  private_nh.getParam("vehicle_length", vehicle_length_);
-  private_nh.getParam("base_link_to_center_dist", base_link_to_center_dist_);
-
-  private_nh.getParam("odom_topic", odom_topic);
-  private_nh.getParam("drive_topic", drive_topic);
-  private_nh.getParam("time_to_collision_topic", time_to_collision_topic);
-  private_nh.getParam("collision_viz_topic", collision_viz_topic);
+  nh_.getParam("wheelbase", wheelbase);
+  nh_.getParam("vehicle_width", vehicle_width_);
+  nh_.getParam("vehicle_length", vehicle_length_);
+  nh_.getParam("base_link_to_center_dist", base_link_to_center_dist_);
 
   biycle_model_ = new BicycleModel(wheelbase);
   collision_checker_ = std::make_unique<CollisionChecker>(circle_offsets, circle_radius);
-  time_to_collision_pub_ = nh_.advertise<std_msgs::Float64>(time_to_collision_topic, 1);
-  viz_pub_ = nh_.advertise<visualization_msgs::Marker>(collision_viz_topic, 1);
+  time_to_collision_pub_ = nh_.advertise<std_msgs::Float64>("time_to_collision", 1);
+  viz_pub_ = nh_.advertise<visualization_msgs::Marker>("vizualization/collision_warning_system", 1);
 
-  odom_sub_ = nh_.subscribe(odom_topic, 1, &CollisionWarningSystem::odomCallback, this);
-  drive_sub_ = nh_.subscribe(drive_topic, 1, &CollisionWarningSystem::driveCallback, this);
+  odom_sub_ = nh_.subscribe("odom", 1, &CollisionWarningSystem::odomCallback, this);
+  drive_sub_ = nh_.subscribe("drive", 1, &CollisionWarningSystem::driveCallback, this);
   timer_ = nh_.createTimer(ros::Duration(0.1), &CollisionWarningSystem::timerCallback, this);
 }
 
diff --git a/costmap_generator/launch/costmap_generator.launch b/costmap_generator/launch/costmap_generator.launch
index 44db5a7..a96e09c 100644
--- a/costmap_generator/launch/costmap_generator.launch
+++ b/costmap_generator/launch/costmap_generator.launch
@@ -1,16 +1,14 @@
 <launch>
-    <arg name="grid_size_x" value="10" />
-    <arg name="grid_size_y" value="10" />
-    <arg name="grid_resolution" value="0.05" />
-    <arg name="map_topic" value="map" />
-    <arg name="scan_topic" value="scan" />
+    <arg name="grid_size_x" default="10"/>
+    <arg name="grid_size_y" default="10"/>
+    <arg name="grid_resolution" default="0.05"/>
+    <arg name="scan_topic" default="scan"/>
 
     <node name="costmap_generator" pkg="costmap_generator" type="costmap_generator_node" output="screen">
-        <param name="grid_size_x" value="$(arg grid_size_x)" />
-        <param name="grid_size_y" value="$(arg grid_size_y)" />
-        <param name="grid_resolution" value="$(arg grid_resolution)" />
+        <param name="grid_size_x" value="$(arg grid_size_x)"/>
+        <param name="grid_size_y" value="$(arg grid_size_y)"/>
+        <param name="grid_resolution" value="$(arg grid_resolution)"/>
 
-        <param name="map_topic" value="$(arg map_topic)" />
-        <param name="scan_topic" value="$(arg scan_topic)" />
+        <remap from="scan" to="$(arg scan_topic)"/>
     </node>
 </launch>
diff --git a/costmap_generator/src/costmap_generator.cpp b/costmap_generator/src/costmap_generator.cpp
index 1340c0c..782b9a4 100644
--- a/costmap_generator/src/costmap_generator.cpp
+++ b/costmap_generator/src/costmap_generator.cpp
@@ -24,18 +24,12 @@ CostmapGenerator::CostmapGenerator()
   double grid_size_y;
   double grid_resolution;
 
-  std::string map_topic;
-  std::string scan_topic;
-
   private_nh.param("grid_resolution", grid_resolution, 0.05);
   private_nh.param("grid_size_x", grid_size_x, 10.0);
   private_nh.param("grid_size_y", grid_size_y, 10.0);
 
-  private_nh.param("map_topic", map_topic, std::string("map"));
-  private_nh.param("scan_topic", scan_topic, std::string("scan"));
-
-  map_sub_ = nh_.subscribe(map_topic, 1, &CostmapGenerator::mapCallback, this);
-  scan_sub_ = nh_.subscribe(scan_topic, 1, &CostmapGenerator::scanCallback, this);
+  map_sub_ = nh_.subscribe("map", 1, &CostmapGenerator::mapCallback, this);
+  scan_sub_ = nh_.subscribe("scan", 1, &CostmapGenerator::scanCallback, this);
   costmap_pub_ = nh_.advertise<grid_map_msgs::GridMap>("costmap", 1, true);
   timer_ = nh_.createTimer(ros::Duration(0.1), &CostmapGenerator::timerCallback, this);
 
diff --git a/drive_controller/launch/drive_controller.launch b/drive_controller/launch/drive_controller.launch
index 1adbb53..498739e 100644
--- a/drive_controller/launch/drive_controller.launch
+++ b/drive_controller/launch/drive_controller.launch
@@ -1,19 +1,15 @@
 <launch>
-    <arg name="look_ahead_dist" value="1" />
-    <arg name="gain" value="0.25" />
+    <arg name="look_ahead_dist" default="1"/>
+    <arg name="gain" default="0.40"/>
 
-    <arg name="trajectory_topic" value="path/local" />
-    <arg name="odom_topic" value="odom" />
-    <arg name="auto_drive_topic" value="drive" />
-    <arg name="viz_topic" value="drive_controller_viz" />
+    <arg name="trajectory_topic" default="trajectory"/>
+    <arg name="odom_topic" default="odom"/>
 
     <node name="drive_controller" pkg="drive_controller" type="drive_controller_node" output="screen">
-        <param name="look_ahead_dist" value="$(arg look_ahead_dist)" />
-        <param name="gain" value="$(arg gain)" />
+        <param name="look_ahead_dist" value="$(arg look_ahead_dist)"/>
+        <param name="gain" value="$(arg gain)"/>
 
-        <param name="trajectory_topic" value="$(arg trajectory_topic)" />
-        <param name="odom_topic" value="$(arg odom_topic)" />
-        <param name="auto_drive_topic" value="$(arg auto_drive_topic)" />
-        <param name="viz_topic" value="$(arg viz_topic)" />
+        <remap from="trajectory" to="$(arg trajectory_topic)"/>
+        <remap from="odom" to="$(arg odom_topic)"/>
     </node>
 </launch>
diff --git a/drive_controller/src/drive_controller.cpp b/drive_controller/src/drive_controller.cpp
index c36013d..3b34eff 100644
--- a/drive_controller/src/drive_controller.cpp
+++ b/drive_controller/src/drive_controller.cpp
@@ -18,22 +18,13 @@ DriveController::DriveController()
   double look_ahead_dist;
   double gain;
 
-  std::string trajectory_topic;
-  std::string odom_topic;
-  std::string drive_topic;
-  std::string viz_topic;
-
   private_nh.getParam("look_ahead_dist", look_ahead_dist);
   private_nh.getParam("gain", gain);
-  private_nh.getParam("trajectory_topic", trajectory_topic);
-  private_nh.getParam("odom_topic", odom_topic);
-  private_nh.getParam("auto_drive_topic", drive_topic);
-  private_nh.getParam("viz_topic", viz_topic);
 
-  trajectory_sub_ = nh_.subscribe(trajectory_topic, 1, &DriveController::trajectoryCallback, this);
-  odom_sub_ = nh_.subscribe(odom_topic, 1, &DriveController::odomCallback, this);
-  drive_pub_ = nh_.advertise<ackermann_msgs::AckermannDriveStamped>(drive_topic, 1);
-  viz_pub_ = nh_.advertise<visualization_msgs::MarkerArray>(viz_topic, 1);
+  trajectory_sub_ = nh_.subscribe("trajectory", 1, &DriveController::trajectoryCallback, this);
+  odom_sub_ = nh_.subscribe("odom", 1, &DriveController::odomCallback, this);
+  drive_pub_ = nh_.advertise<ackermann_msgs::AckermannDriveStamped>("drive_auto", 1);
+  viz_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("visualization/drive_controller", 1);
   timer_ = nh_.createTimer(ros::Duration(0.1), &DriveController::timerCallback, this);
 
   viz_ptr_ = std::make_shared<visualization_msgs::MarkerArray>();
diff --git a/drive_mode_selector/launch/drive_mode_selector.launch b/drive_mode_selector/launch/drive_mode_selector.launch
index 9d4a699..9ab5a13 100644
--- a/drive_mode_selector/launch/drive_mode_selector.launch
+++ b/drive_mode_selector/launch/drive_mode_selector.launch
@@ -1,13 +1,16 @@
 <launch>
-    <arg name="drive_mode_topic" value="drive_mode" />
-    <arg name="joy_drive_topic" value="drive/joy" />
-    <arg name="auto_drive_topic" value="drive/auto" />
-    <arg name="selected_drive_topic" value="drive/selected" />
+    <arg name="drive_mode_topic" default="drive_mode" />
+    <arg name="joy_drive_topic" default="drive_joy" />
+    <arg name="auto_drive_topic" default="drive_auto" />
 
     <node name="drive_mode_selector" pkg="drive_mode_selector" type="drive_mode_selector_node" output="screen">
         <param name="drive_mode_topic" value="$(arg drive_mode_topic)" />
         <param name="joy_drive_topic" value="$(arg joy_drive_topic)" />
         <param name="auto_drive_topic" value="$(arg auto_drive_topic)" />
         <param name="selected_drive_topic" value="$(arg selected_drive_topic)" />
+
+        <remap from="drive_mode_topic" to="$(arg drive_mode_topic)"/>
+        <remap from="joy_drive_topic" to="$(arg joy_drive_topic)"/>
+        <remap from="auto_drive_topic" to="$(arg auto_drive_topic)"/>
     </node>
 </launch>
\ No newline at end of file
diff --git a/drive_mode_selector/src/drive_mode_selector.cpp b/drive_mode_selector/src/drive_mode_selector.cpp
index 80a2eb0..af44c56 100644
--- a/drive_mode_selector/src/drive_mode_selector.cpp
+++ b/drive_mode_selector/src/drive_mode_selector.cpp
@@ -12,21 +12,10 @@ DriveModeSelector::DriveModeSelector()
 {
   ros::NodeHandle private_nh("~");
 
-  std::string drive_mode_topic;
-  std::string joy_drive_topic;
-  std::string auto_drive_topic;
-  std::string selected_drive_topic;
-
-  getParam("drive_mode_topic", drive_mode_topic);
-  getParam("joy_drive_topic", joy_drive_topic);
-  getParam("auto_drive_topic", auto_drive_topic);
-  getParam("selected_drive_topic", selected_drive_topic);
-
-  drive_mode_sub_ = nh_.subscribe(drive_mode_topic, 1, &DriveModeSelector::driveModeCallback, this);
-  joy_drive_sub_ = nh_.subscribe(joy_drive_topic, 1, &DriveModeSelector::joyDriveCallback, this);
-  auto_drive_sub_ = nh_.subscribe(auto_drive_topic, 1, &DriveModeSelector::autoDriveCallback, this);
-
-  selected_drive_pub_ = nh_.advertise<ackermann_msgs::AckermannDriveStamped>(selected_drive_topic, 1);
+  drive_mode_sub_ = nh_.subscribe("drive_mode", 1, &DriveModeSelector::driveModeCallback, this);
+  joy_drive_sub_ = nh_.subscribe("drive_joy", 1, &DriveModeSelector::joyDriveCallback, this);
+  auto_drive_sub_ = nh_.subscribe("drive_auto", 1, &DriveModeSelector::autoDriveCallback, this);
+  selected_drive_pub_ = nh_.advertise<ackermann_msgs::AckermannDriveStamped>("drive_selected", 1);
 
   drive_mode_ = Mode::PARK;
 }
diff --git a/ds4_controller/include/ds4_controller/ds4_controller.h b/ds4_controller/include/ds4_controller/ds4_controller.h
index 6fdcca5..18dbd82 100644
--- a/ds4_controller/include/ds4_controller/ds4_controller.h
+++ b/ds4_controller/include/ds4_controller/ds4_controller.h
@@ -24,8 +24,6 @@ class DS4Controller
   ros::Publisher drive_pub_;
   ros::Publisher drive_mode_pub_;
 
-  std::string status_topic_;
-
   ros::Time last_message_time_;
   double timeout_;
 
diff --git a/ds4_controller/launch/ds4_controller.launch b/ds4_controller/launch/ds4_controller.launch
index f3d57dd..93a7ae4 100644
--- a/ds4_controller/launch/ds4_controller.launch
+++ b/ds4_controller/launch/ds4_controller.launch
@@ -1,32 +1,10 @@
 <launch>
-  <arg name="use_standard_msgs" value="false"/>
-  <arg name="deadzone" value="0"/>
-
-  <arg name="status_topic" value="status"/>
-  <arg name="feedback_topic" value="set_feedback"/>
-  <arg name="joy_drive_topic" value="drive/joy"/>
-  <arg name="drive_mode_topic" value="drive_mode"/>
-
-  <arg name="timeout" value="0.5"/>
-
-  <arg name="max_steering_angle" value="0.3"/>
-  <arg name="max_speed" value="5"/>
-
-  <node name="ds4_driver" pkg="ds4_driver" type="ds4_driver_node.py" output="screen">
-    <param name="use_standard_msgs" value="$(arg use_standard_msgs)"/>
-    <param name="deadzone" value="$(arg deadzone)"/>
-  </node>
-
-  <node pkg="tf2_ros" type="static_transform_publisher" name="ds4_to_imu" args="0 0.05 -0.01 -1.5707 0 1.5707 ds4 ds4_imu"/>
+  <arg name="timeout" default="0.5"/>
+  <arg name="max_steering_angle" default="0.3"/>
+  <arg name="max_speed" default="5"/>
 
   <node name="ds4_controller" pkg="ds4_controller" type="ds4_controller_node" output="screen">
-    <param name="status_topic" value="$(arg status_topic)" />
-    <param name="feedback_topic" value="$(arg feedback_topic)" />
-    <param name="joy_drive_topic" value="$(arg joy_drive_topic)" />
-    <param name="drive_mode_topic" value="$(arg drive_mode_topic)" />
-
     <param name="timeout" value="$(arg timeout)" />
-
     <param name="max_steering_angle" value="$(arg max_steering_angle)"/>
     <param name="max_speed" value="$(arg max_speed)"/>
   </node>
diff --git a/ds4_controller/src/ds4_controller.cpp b/ds4_controller/src/ds4_controller.cpp
index 662d395..60f494e 100644
--- a/ds4_controller/src/ds4_controller.cpp
+++ b/ds4_controller/src/ds4_controller.cpp
@@ -17,23 +17,14 @@ DS4Controller::DS4Controller()
 {
   ros::NodeHandle private_nh("~");
 
-  std::string feedback_topic;
-  std::string drive_topic;
-  std::string drive_mode_topic;
-
-  getParam("status_topic", status_topic_);
-  getParam("feedback_topic", feedback_topic);
-  getParam("joy_drive_topic", drive_topic);
-  getParam("drive_mode_topic", drive_mode_topic);
   getParam("timeout", timeout_);
   getParam("max_steering_angle", max_steering_angle_);
   getParam("max_speed", max_speed_);
 
-  status_sub_ = nh_.subscribe(status_topic_, 1, &DS4Controller::statusCallback, this);
-  drive_pub_ = nh_.advertise<ackermann_msgs::AckermannDriveStamped>(drive_topic, 1);
-  drive_mode_pub_ = nh_.advertise<f1tenth_msgs::DriveMode>(drive_mode_topic, 1);
-
-  feedback_pub_ = nh_.advertise<ds4_driver::Feedback>(feedback_topic, 1);
+  status_sub_ = nh_.subscribe("status", 1, &DS4Controller::statusCallback, this);
+  drive_pub_ = nh_.advertise<ackermann_msgs::AckermannDriveStamped>("drive_joy", 1);
+  drive_mode_pub_ = nh_.advertise<f1tenth_msgs::DriveMode>("drive_mode", 1);
+  feedback_pub_ = nh_.advertise<ds4_driver::Feedback>("set_feedback", 1);
 
   waitForConnection();
 
@@ -104,7 +95,7 @@ void DS4Controller::waitForConnection()
 
   ROS_INFO("[DS4 Controller] DS4 Driver Started");
 
-  while (ros::topic::waitForMessage<ds4_driver::Status>(status_topic_, ros::Duration(0.1)) == NULL)
+  while (ros::topic::waitForMessage<ds4_driver::Status>(status_sub_.getTopic(), ros::Duration(0.1)) == NULL)
   {
     ROS_WARN_THROTTLE(1, "[DS4 Controller] Waiting for Connection to DualShock 4");
   }
diff --git a/f1tenth_racecar_bringup/config/amcl.yaml b/f1tenth_racecar_bringup/config/amcl.yaml
new file mode 100644
index 0000000..ed1a51d
--- /dev/null
+++ b/f1tenth_racecar_bringup/config/amcl.yaml
@@ -0,0 +1,30 @@
+use_map_topic: true
+odom_model_type: diff
+gui_publish_rate: 10
+laser_max_beams: 30
+min_particles: 500
+max_particles: 5000
+kld_err: 0.05
+kld_z: 0.99
+
+odom_alpha1: 0.2
+odom_alpha2: 0.2
+odom_alpha3: 0.8
+odom_alpha4: 0.2
+odom_alpha5: 0.1
+
+laser_z_hit: 0.5
+laser_z_short: 0.05
+laser_z_max: 0.05
+laser_z_rand: 0.5
+laser_sigma_hit: 0.2
+laser_lambda_short: 0.1
+laser_model_type: likelihood_field
+laser_likelihood_max_dist: 2.0
+update_min_d: 0.05
+update_min_a: 0.5
+odom_frame_id: odom
+resample_interval: 1
+transform_tolerance: 0.1
+recovery_alpha_slow: 0.0
+recovery_alpha_fast: 0.0
diff --git a/f1tenth_racecar_bringup/config/autonomy.yaml b/f1tenth_racecar_bringup/config/autonomy.yaml
deleted file mode 100644
index 4a902bb..0000000
--- a/f1tenth_racecar_bringup/config/autonomy.yaml
+++ /dev/null
@@ -1,89 +0,0 @@
-path_recorder:
-  mode: 0
-  interval: 0.1
-  odom_topic: odom
-  path_topic: path/global
-
-amcl:
-  use_map_topic: true
-  odom_model_type: diff
-  gui_publish_rate: 10
-  laser_max_beams: 30
-  min_particles: 500
-  max_particles: 5000
-  kld_err: 0.05
-  kld_z: 0.99
-
-  odom_alpha1: 0.2
-  odom_alpha2: 0.2
-  odom_alpha3: 0.8
-  odom_alpha4: 0.2
-  odom_alpha5: 0.1
-
-  laser_z_hit: 0.5
-  laser_z_short: 0.05
-  laser_z_max: 0.05
-  laser_z_rand: 0.5
-  laser_sigma_hit: 0.2
-  laser_lambda_short: 0.1
-  laser_model_type: likelihood_field
-  laser_likelihood_max_dist: 2.0
-  update_min_d: 0.05
-  update_min_a: 0.5
-  odom_frame_id: odom
-  resample_interval: 1
-  transform_tolerance: 0.1
-  recovery_alpha_slow: 0.0
-  recovery_alpha_fast: 0.0
-
-costmap_generator:
-  grid_size_x: 10
-  grid_size_y: 10
-  grid_resolution: 0.05
-  map_topic: map
-  scan_topic: scan
-
-local_planner:
-  circle_offsets: [0.1, 0.3]
-  circle_radius: 0.2
-  lattice_num_layers: 8
-  lattice_num_lateral_samples_per_side: 3
-  lattice_layer_spacing: 2.0
-  lattice_lateral_spacing: 0.05
-  lattice_k_length: 0.5
-
-  ref_max_speed: 5.0
-  ref_max_lat_acc: 1.0
-  ref_max_lon_acc: 1.0
-  ref_max_lon_dec: 1.0
-
-  track_num_paths: 9
-  wheelbase: 0.3
-  max_steering_angle: 0.4
-  track_path_lateral_spacing: 0.05
-  track_look_ahead_time: 0.5
-
-  global_path_topic: path/global
-  local_path_topic: path/local
-  odom_topic: odom
-  viz_topic: viz/local_planner
-
-drive_controller:
-  look_ahead_dist: 1
-  gain: 0.5
-  trajectory_topic: path/local
-  odom_topic: odom
-  auto_drive_topic: drive/auto
-  viz_topic: drive_controller/viz
-
-drive_mode_selector:
-  drive_mode_topic: drive_mode
-  joy_drive_topic: drive/joy
-  auto_drive_topic: drive/auto
-  selected_drive_topic: drive/selected
-
-safety_controller:
-  odom_topic: odom
-  time_to_collision_topic: time_to_collision
-  selected_drive_topic: drive/selected
-  safe_drive_topic: drive
diff --git a/f1tenth_racecar_bringup/config/collision_warning_system.yaml b/f1tenth_racecar_bringup/config/collision_warning_system.yaml
new file mode 100644
index 0000000..67436f3
--- /dev/null
+++ b/f1tenth_racecar_bringup/config/collision_warning_system.yaml
@@ -0,0 +1,4 @@
+circle_offsets: [0.1, 0.3]
+circle_radius: 0.2
+t_max: 2
+delta_t: 0.1
diff --git a/f1tenth_racecar_bringup/config/common.yaml b/f1tenth_racecar_bringup/config/common.yaml
deleted file mode 100644
index cedd871..0000000
--- a/f1tenth_racecar_bringup/config/common.yaml
+++ /dev/null
@@ -1,22 +0,0 @@
-# Vehicle Params
-wheelbase: 0.3302 # meters
-width: 0.2032 # meters
-max_steering_angle: 0.4189 # radians
-
-# VESC Params
-speed_to_erpm_gain: 4614
-speed_to_erpm_offset: 0
-steering_angle_to_servo_gain: -1.2135
-steering_angle_to_servo_offset: 0.5
-
-# Topics
-odom_topic: odom
-
-drive_mode_topic: drive_mode
-
-joy_drive_topic: ackermann_joy
-auto_drive_topic: ackermann_auto
-selected_drive_topic: ackermann_selected
-safe_drive_topic: ackermann_cmd
-
-time_to_collision_topic: ttc
diff --git a/f1tenth_racecar_bringup/config/costmap_generator.yaml b/f1tenth_racecar_bringup/config/costmap_generator.yaml
new file mode 100644
index 0000000..9f8f596
--- /dev/null
+++ b/f1tenth_racecar_bringup/config/costmap_generator.yaml
@@ -0,0 +1,3 @@
+grid_size_x: 10
+grid_size_y: 10
+grid_resolution: 0.05
diff --git a/f1tenth_racecar_bringup/config/drive_controller.yaml b/f1tenth_racecar_bringup/config/drive_controller.yaml
new file mode 100644
index 0000000..16ab902
--- /dev/null
+++ b/f1tenth_racecar_bringup/config/drive_controller.yaml
@@ -0,0 +1,2 @@
+look_ahead_dist: 1
+gain: 0.5
diff --git a/f1tenth_racecar_bringup/config/ds4_controller.yaml b/f1tenth_racecar_bringup/config/ds4_controller.yaml
new file mode 100644
index 0000000..a1540bf
--- /dev/null
+++ b/f1tenth_racecar_bringup/config/ds4_controller.yaml
@@ -0,0 +1,6 @@
+status_topic: status
+feedback_topic: set_feedback
+joy_drive_topic: drive/joy
+drive_mode_topic: drive_mode
+timeout: 0.5
+max_speed: 5
diff --git a/f1tenth_racecar_bringup/config/ds4_driver.yaml b/f1tenth_racecar_bringup/config/ds4_driver.yaml
new file mode 100644
index 0000000..c1c6136
--- /dev/null
+++ b/f1tenth_racecar_bringup/config/ds4_driver.yaml
@@ -0,0 +1,6 @@
+device_addr: 84:30:95:56:EE:1A
+backend: hidraw
+use_standard_msgs: false
+deadzone: 0.1
+frame_id: ds4
+imu_frame_id: ds4_imu
diff --git a/f1tenth_racecar_bringup/config/gmapping.yaml b/f1tenth_racecar_bringup/config/gmapping.yaml
new file mode 100644
index 0000000..0820e73
--- /dev/null
+++ b/f1tenth_racecar_bringup/config/gmapping.yaml
@@ -0,0 +1,34 @@
+base_frame: base_link
+odom_frame: odom
+map_update_interval: 5.0
+maxUrange: 6.0
+maxRange: 8.0
+sigma: 0.05
+kernelSize: 1
+lstep: 0.05
+astep: 0.05
+iterations: 5
+lsigma: 0.075
+ogain: 3.0
+lskip: 0
+minimumScore: 100
+srr: 0.01
+srt: 0.02
+str: 0.01
+stt: 0.02
+linearUpdate: 0.1
+angularUpdate: 0.3
+temporalUpdate: -1.0
+resampleThreshold: 0.5
+particles: 80
+
+xmin: -50.0
+ymin: -50.0
+xmax: 50.0
+ymax: 50.0
+
+delta: 0.05
+llsamplerange: 0.01
+llsamplestep: 0.01
+lasamplerange: 0.005
+lasamplestep: 0.005
diff --git a/f1tenth_racecar_bringup/config/joy.yaml b/f1tenth_racecar_bringup/config/joy.yaml
deleted file mode 100644
index 1b3fafb..0000000
--- a/f1tenth_racecar_bringup/config/joy.yaml
+++ /dev/null
@@ -1,12 +0,0 @@
-ds4_driver:
-  use_standard_msgs: false
-  deadzone: 0
-
-ds4_controller:
-  status_topic: status
-  feedback_topic: set_feedback
-  joy_drive_topic: drive/joy
-  drive_mode_topic: drive_mode
-  timeout: 0.5
-  max_steering_angle: 0.3
-  max_speed: 5
diff --git a/f1tenth_racecar_bringup/config/local_planner.yaml b/f1tenth_racecar_bringup/config/local_planner.yaml
new file mode 100644
index 0000000..ad1af27
--- /dev/null
+++ b/f1tenth_racecar_bringup/config/local_planner.yaml
@@ -0,0 +1,16 @@
+circle_offsets: [0.1, 0.3]
+circle_radius: 0.2
+lattice_num_layers: 8
+lattice_num_lateral_samples_per_side: 3
+lattice_layer_spacing: 2.0
+lattice_lateral_spacing: 0.05
+lattice_k_length: 0.5
+
+ref_max_speed: 5.0
+ref_max_lat_acc: 1.0
+ref_max_lon_acc: 1.0
+ref_max_lon_dec: 1.0
+
+track_num_paths: 9
+track_path_lateral_spacing: 0.05
+track_look_ahead_time: 0.5
diff --git a/f1tenth_racecar_bringup/config/vehicle_params.yaml b/f1tenth_racecar_bringup/config/vehicle_params.yaml
new file mode 100644
index 0000000..a0c3428
--- /dev/null
+++ b/f1tenth_racecar_bringup/config/vehicle_params.yaml
@@ -0,0 +1,5 @@
+vehicle_length: 0.5 # meters
+vehicle_width: 0.27 # meters
+base_link_to_center_dist: 0.165 # meters
+wheelbase: 0.33 # meters
+max_steering_angle: 0.4189 # radians
diff --git a/f1tenth_racecar_bringup/config/vesc_ackermann.yaml b/f1tenth_racecar_bringup/config/vesc_ackermann.yaml
new file mode 100644
index 0000000..8ff481b
--- /dev/null
+++ b/f1tenth_racecar_bringup/config/vesc_ackermann.yaml
@@ -0,0 +1,4 @@
+speed_to_erpm_gain: 4614
+speed_to_erpm_offset: 0
+steering_angle_to_servo_gain: -1.2135
+steering_angle_to_servo_offset: 0.5
diff --git a/f1tenth_racecar_bringup/config/vesc_driver.yaml b/f1tenth_racecar_bringup/config/vesc_driver.yaml
new file mode 100644
index 0000000..bc08994
--- /dev/null
+++ b/f1tenth_racecar_bringup/config/vesc_driver.yaml
@@ -0,0 +1,13 @@
+port: /dev/sensors/vesc
+brake_max: 200000.0
+brake_min: -20000.0
+current_max: 100.0
+current_min: 0.0
+duty_cycle_max: 1.0
+duty_cycle_min: -1.0
+position_max: 0.0
+position_min: 0.0
+servo_max: 1.0
+servo_min: 0.0
+speed_max: 23250.0
+speed_min: -23250.0
diff --git a/f1tenth_racecar_bringup/launch/autonomy.launch b/f1tenth_racecar_bringup/launch/autonomy.launch
index 92fd5cc..98d9292 100644
--- a/f1tenth_racecar_bringup/launch/autonomy.launch
+++ b/f1tenth_racecar_bringup/launch/autonomy.launch
@@ -1,28 +1,51 @@
 <launch>
-    <arg name="param_file_name" default="autonomy.yaml"/>
-    <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/$(arg param_file_name)"/>
+    <arg name="map_name" default="map"/>
+    <arg name="path_name" default="path"/>
 
-    <arg name="file_name" value="$(find f1tenth_racecar_bringup)/path/berlin.csv"/>
+    <node pkg="map_server" name="map_server" type="map_server" args="$(find f1tenth_racecar_bringup)/map/$(arg map_name).yaml"/>
 
     <node name="path_recorder" pkg="path_recorder" type="path_recorder_node" output="screen">
-        <param name="file_name" value="$(arg file_name)" />
+        <param name="mode" value="0"/>
+        <param name="file_name" value="$(find f1tenth_racecar_bringup)/path/$(arg path_name).csv"/>
     </node>
 
     <node name="amcl" pkg="amcl" type="amcl" output="screen">
+        <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/amcl.yaml"/>
+        <remap from="scan" to="scan"/>
     </node>
 
     <node name="costmap_generator" pkg="costmap_generator" type="costmap_generator_node" output="screen">
+        <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/costmap_generator.yaml"/>
+        <remap from="scan" to="scan"/>
     </node>
 
     <node name="local_planner" pkg="local_planner" type="local_planner_node" output="screen">
+        <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/local_planner.yaml"/>
+        <remap from="global_path" to="global_path"/>
+        <remap from="odom" to="odom"/>
+        <remap from="drive_feedback" to="drive_safe"/>
     </node>
 
     <node name="drive_controller" pkg="drive_controller" type="drive_controller_node" output="screen">
+        <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/drive_controller.yaml"/>
+        <remap from="trajectory" to="local_trajectory"/>
+        <remap from="odom" to="odom"/>
     </node>
 
     <node name="drive_mode_selector" pkg="drive_mode_selector" type="drive_mode_selector_node" output="screen">
+        <remap from="drive_mode" to="drive_mode"/>
+        <remap from="drive_joy" to="drive_joy"/>
+        <remap from="drive_auto" to="drive_auto"/>
+    </node>
+
+    <node name="collision_warning_system" pkg="collision_warning_system" type="collision_warning_system_node" output="screen">
+        <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/collision_warning_system.yaml"/>
+        <remap from="odom" to="odom"/>
+        <remap from="drive" to="drive_safe"/>
     </node>
 
     <node name="safety_controller" pkg="safety_controller" type="safety_controller_node" output="screen">
+        <remap from="odom" to="odom"/>
+        <remap from="drive_in" to="drive_selected"/>
     </node>
 </launch>
diff --git a/f1tenth_racecar_bringup/launch/f1tenth_racecar.launch b/f1tenth_racecar_bringup/launch/f1tenth_racecar.launch
index 6265aea..32a0104 100644
--- a/f1tenth_racecar_bringup/launch/f1tenth_racecar.launch
+++ b/f1tenth_racecar_bringup/launch/f1tenth_racecar.launch
@@ -1,75 +1,17 @@
 <launch>
-    <arg name="use_standard_msgs" value="false"/>
-    <arg name="deadzone" value="0"/>
+    <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/vehicle_params.yaml"/>
 
-    <arg name="status_topic" value="status"/>
-    <arg name="feedback_topic" value="set_feedback"/>
-    <arg name="drive_topic" value="drive_raw"/>
+    <include file="$(find f1tenth_racecar_bringup)/launch/joy.launch">
+    </include>
 
-    <arg name="timeout" value="0.5"/>
+    <include file="$(find f1tenth_racecar_bringup)/launch/autonomy.launch">
+        <arg name="map_name" value="map"/>
+        <arg name="path_name" value="path"/>
+    </include>
 
-    <arg name="max_steering_angle" value="0.3"/>
-    <arg name="max_speed" value="5"/>
+    <include file="$(find f1tenth_racecar_bringup)/launch/vesc.launch">
+        <arg name="vesc_drive_topic" value="drive_safe"/>
+    </include>
 
-    <node name="ds4_driver" pkg="ds4_driver" type="ds4_driver_node.py" output="screen">
-        <param name="use_standard_msgs" value="$(arg use_standard_msgs)"/>
-        <param name="deadzone" value="$(arg deadzone)"/>
-    </node>
-
-    <node pkg="tf2_ros" type="static_transform_publisher" name="ds4_to_imu" args="0 0.05 -0.01 -1.5707 0 1.5707 ds4 ds4_imu"/>
-
-    <node name="ds4_controller" pkg="ds4_controller" type="ds4_controller_node" output="screen">
-        <param name="status_topic" value="$(arg status_topic)" />
-        <param name="feedback_topic" value="$(arg feedback_topic)" />
-        <param name="drive_topic" value="$(arg drive_topic)" />
-
-        <param name="timeout" value="$(arg timeout)" />
-
-        <param name="max_steering_angle" value="$(arg max_steering_angle)"/>
-        <param name="max_speed" value="$(arg max_speed)"/>
-    </node>
-
-    <arg name="t_max" value="2"/>
-    <arg name="delta_t" value="0.2"/>
-
-    <arg name="wheelbase" value="0.3"/>
-    <arg name="vehicle_width" value="0.3"/>
-    <arg name="vehicle_length" value="0.8"/>
-    <arg name="base_link_to_center_dist" value="0.4"/>
-
-    <arg name="odom_topic" value="odom"/>
-    <arg name="cws_drive_topic" value="drive"/>
-    <arg name="obstacle_topic" value="obstacles"/>
-    <arg name="time_to_collision_topic" value="time_to_collision"/>
-    <arg name="trajectory_topic" value="trajectory"/>
-    <arg name="vehicle_footprints_viz_topic" value="vehicle_footprints_viz"/>
-    <arg name="collision_viz_topic" value="collision_viz"/>
-
-    <node name="collision_warning_system" pkg="collision_warning_system" type="collision_warning_system_node" output="screen">
-        <param name="t_max" value="$(arg t_max)"/>
-        <param name="delta_t" value="$(arg delta_t)"/>
-
-        <param name="wheelbase" value="$(arg wheelbase)"/>
-        <param name="vehicle_width" value="$(arg vehicle_width)"/>
-        <param name="vehicle_length" value="$(arg vehicle_length)"/>
-        <param name="base_link_to_center_dist" value="$(arg base_link_to_center_dist)"/>
-
-        <param name="odom_topic" value="$(arg odom_topic)"/>
-        <param name="drive_topic" value="$(arg cws_drive_topic)"/>
-        <param name="obstacle_topic" value="$(arg obstacle_topic)"/>
-        <param name="time_to_collision_topic" value="$(arg time_to_collision_topic)"/>
-        <param name="trajectory_topic" value="$(arg trajectory_topic)"/>
-        <param name="vehicle_footprints_viz_topic" value="$(arg vehicle_footprints_viz_topic)"/>
-        <param name="collision_viz_topic" value="$(arg collision_viz_topic)"/>
-    </node>
-
-    <arg name="raw_drive_topic" value="drive_raw"/>
-    <arg name="safe_drive_topic" value="drive"/>
-
-    <node name="safety_controller" pkg="safety_controller" type="safety_controller_node" output="screen">
-        <param name="odom_topic" value="$(arg odom_topic)" />
-        <param name="time_to_collision_topic" value="$(arg time_to_collision_topic)" />
-        <param name="raw_drive_topic" value="$(arg raw_drive_topic)" />
-        <param name="safe_drive_topic" value="$(arg safe_drive_topic)" />
-    </node>
-</launch>
\ No newline at end of file
+    <include file="$(find f1tenth_simulator)/launch/racecar_model.launch"/>
+</launch>
diff --git a/f1tenth_racecar_bringup/launch/f1tenth_racecar_manual.launch b/f1tenth_racecar_bringup/launch/f1tenth_racecar_manual.launch
deleted file mode 100644
index 2699045..0000000
--- a/f1tenth_racecar_bringup/launch/f1tenth_racecar_manual.launch
+++ /dev/null
@@ -1,46 +0,0 @@
-<launch>
-    <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/common.yaml"/>
-
-    <!-- DS4 Driver -->
-    <arg name="use_standard_msgs" value="false"/>
-    <arg name="deadzone" value="0.02"/>
-
-    <node name="ds4_driver" pkg="ds4_driver" type="ds4_driver_node.py" output="screen">
-        <param name="use_standard_msgs" value="$(arg use_standard_msgs)"/>
-        <param name="deadzone" value="$(arg deadzone)"/>
-    </node>
-
-    <node pkg="tf2_ros" type="static_transform_publisher" name="ds4_to_imu" args="0 0.05 -0.01 -1.5707 0 1.5707 ds4 ds4_imu"/>
-
-    <!-- DS4 Controller -->
-    <arg name="status_topic" value="status"/>
-    <arg name="feedback_topic" value="set_feedback"/>
-    <arg name="timeout" value="0.5"/>
-    <arg name="max_speed" value="10"/>
-
-    <node name="ds4_controller" pkg="ds4_controller" type="ds4_controller_node" output="screen">
-        <param name="status_topic" value="$(arg status_topic)" />
-        <param name="feedback_topic" value="$(arg feedback_topic)" />
-        <param name="timeout" value="$(arg timeout)" />
-        <param name="max_speed" value="$(arg max_speed)"/>
-    </node>
-
-    <!-- Drive Mode Selector -->
-    <node name="drive_mode_selector" pkg="drive_mode_selector" type="drive_mode_selector_node" output="screen">
-    </node>
-
-    <!-- Safety Controller -->
-    <node name="safety_controller" pkg="safety_controller" type="safety_controller_node" output="screen">
-    </node>
-
-    <!-- Ackermann To VESC -->
-    <node name="ackermann_to_vesc" pkg="vesc_ackermann" type="ackermann_to_vesc_node" output="screen">
-    </node>
-
-    <!-- VESC Driver  -->
-    <arg name="port" value="/dev/sensors/vesc" />
-
-    <node name="vesc_driver_node" pkg="vesc_driver" type="vesc_driver_node" output="screen">
-        <param name="port" value="$(arg port)" />
-    </node>
-</launch>
diff --git a/f1tenth_racecar_bringup/launch/f1tenth_sim.launch b/f1tenth_racecar_bringup/launch/f1tenth_sim.launch
index 573aadb..89e7dfe 100644
--- a/f1tenth_racecar_bringup/launch/f1tenth_sim.launch
+++ b/f1tenth_racecar_bringup/launch/f1tenth_sim.launch
@@ -1,23 +1,20 @@
 <launch>
-  <!-- Launch a map from the maps folder-->
-  <arg name="map" value="$(find f1tenth_simulator)/maps/berlin.yaml"/>
-  <node pkg="map_server" name="map_server" type="map_server" args="$(arg map)"/>
-
-  <!-- Launch the racecar model -->
-  <include file="$(find f1tenth_simulator)/launch/racecar_model.launch"/>
-
-  <!-- Begin the simulator with the parameters from params.yaml -->
-  <node pkg="f1tenth_simulator" name="f1tenth_simulator" type="simulator" output="screen">
-    <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/simulator.yaml"/>
-  </node>
+  <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/vehicle_params.yaml"/>
 
   <include file="$(find f1tenth_racecar_bringup)/launch/joy.launch">
-    <arg name="param_file_name" value="joy.yaml"/>
   </include>
 
   <include file="$(find f1tenth_racecar_bringup)/launch/autonomy.launch">
-    <arg name="param_file_name" value="autonomy.yaml"/>
+    <arg name="map_name" value="berlin"/>
+    <arg name="path_name" value="berlin"/>
   </include>
 
+  <node pkg="f1tenth_simulator" name="f1tenth_simulator" type="simulator" output="screen">
+    <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/simulator.yaml"/>
+    <remap from="drive" to="drive_safe"/>
+  </node>
+
+  <include file="$(find f1tenth_simulator)/launch/racecar_model.launch"/>
+
   <node pkg="rviz" type="rviz" name="rviz" args="-d $(find f1tenth_racecar_bringup)/rviz/f1tenth_racecar.rviz" output="screen"/>
 </launch>
diff --git a/f1tenth_racecar_bringup/launch/f1tenth_slam.launch b/f1tenth_racecar_bringup/launch/f1tenth_slam.launch
new file mode 100644
index 0000000..752f7b3
--- /dev/null
+++ b/f1tenth_racecar_bringup/launch/f1tenth_slam.launch
@@ -0,0 +1,16 @@
+<launch>
+    <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/vehicle_params.yaml"/>
+
+    <include file="$(find f1tenth_racecar_bringup)/launch/joy.launch">
+    </include>
+
+    <include file="$(find f1tenth_racecar_bringup)/launch/vesc.launch">
+        <param name="vesc_drive_topic" value="drive_joy"/>
+    </include>
+
+    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
+        <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/gmapping.yaml"/>
+        <remap from="scan" to="scan"/>
+        <remap from="odom" to="odom"/>
+    </node>
+</launch>
diff --git a/f1tenth_racecar_bringup/launch/joy.launch b/f1tenth_racecar_bringup/launch/joy.launch
index 486efed..97fd666 100644
--- a/f1tenth_racecar_bringup/launch/joy.launch
+++ b/f1tenth_racecar_bringup/launch/joy.launch
@@ -1,11 +1,9 @@
 <launch>
-    <arg name="param_file_name" default="joy.yaml"/>
-    <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/$(arg param_file_name)"/>
-
     <node name="ds4_driver" pkg="ds4_driver" type="ds4_driver_node.py" output="screen">
+        <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/ds4_driver.yaml"/>
     </node>
 
     <node name="ds4_controller" pkg="ds4_controller" type="ds4_controller_node" output="screen">
+        <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/ds4_controller.yaml"/>
     </node>
 </launch>
-  
\ No newline at end of file
diff --git a/f1tenth_racecar_bringup/launch/test.launch b/f1tenth_racecar_bringup/launch/test.launch
new file mode 100644
index 0000000..4124c86
--- /dev/null
+++ b/f1tenth_racecar_bringup/launch/test.launch
@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="odom_topic" default="odom"/>
+
+    <node name="collision_warning_system" pkg="collision_warning_system" type="collision_warning_system_node" output="screen">
+        <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/collision_warning_system.yaml"/>
+
+        <remap from="odom" to="$(arg odom_topic)"/>
+        <remap from="drive" to="drive_safe"/>
+    </node>
+</launch>
diff --git a/f1tenth_racecar_bringup/launch/vesc.launch b/f1tenth_racecar_bringup/launch/vesc.launch
new file mode 100644
index 0000000..1fa852f
--- /dev/null
+++ b/f1tenth_racecar_bringup/launch/vesc.launch
@@ -0,0 +1,16 @@
+<launch>
+    <arg name="vesc_drive_topic" default="ackermann_cmd"/>
+
+    <node name="vesc_driver_node" pkg="vesc_driver" type="vesc_driver_node" output="screen">
+        <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/vesc_driver.yaml"/>
+    </node>
+
+    <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/vesc_ackermann.yaml"/>
+
+    <node name="ackermann_to_vesc" pkg="vesc_ackermann" type="ackermann_to_vesc_node" output="screen">
+        <remap from="ackermann_cmd" to="$(arg vesc_cmd_topic)"/>
+    </node>
+
+    <node name="vesc_to_odom" pkg="vesc_ackermann" type="vesc_to_odom_node" output="screen">
+    </node>
+</launch>
diff --git a/local_planner/launch/local_planner.launch b/local_planner/launch/local_planner.launch
index a73cb54..708c7e0 100644
--- a/local_planner/launch/local_planner.launch
+++ b/local_planner/launch/local_planner.launch
@@ -1,29 +1,27 @@
 <launch>
-    <arg name="circle_offsets" value="[0.1, 0.3]" />
-    <arg name="circle_radius" value="0.2" />
-
-    <arg name="lattice_num_layers" value="8" />
-    <arg name="lattice_num_lateral_samples_per_side" value="8" />
-    <arg name="lattice_layer_spacing" value="2.0" />
-    <arg name="lattice_lateral_spacing" value="0.05" />
-    <arg name="lattice_k_length" value="0.5" />
-
-    <arg name="ref_max_speed" value="5.0" />
-    <arg name="ref_max_lat_acc" value="1.0" />
-    <arg name="ref_max_lon_acc" value="1.0" />
-    <arg name="ref_max_lon_dec" value="1.0" />
-
-    <arg name="track_num_paths" value="9" />
-    <arg name="wheelbase" value="0.3" />
-    <arg name="max_steering_angle" value="0.4" />
-    <arg name="track_path_lateral_spacing" value="0.06" />
-    <arg name="track_look_ahead_time" value="0.5" />
-
-    <arg name="global_path_topic" value="path/global" />
-    <arg name="local_path_topic" value="path/local" />
-    <arg name="odom_topic" value="odom" />
-    <arg name="inflated_costmap_topic" value="inflated_costmap" />
-    <arg name="viz_topic" value="viz/local_planner" />
+    <arg name="circle_offsets" default="[0.1, 0.3]" />
+    <arg name="circle_radius" default="0.2" />
+
+    <arg name="lattice_num_layers" default="8" />
+    <arg name="lattice_num_lateral_samples_per_side" default="8" />
+    <arg name="lattice_layer_spacing" default="2.0" />
+    <arg name="lattice_lateral_spacing" default="0.05" />
+    <arg name="lattice_k_length" default="0.5" />
+
+    <arg name="ref_max_speed" default="5.0" />
+    <arg name="ref_max_lat_acc" default="1.0" />
+    <arg name="ref_max_lon_acc" default="1.0" />
+    <arg name="ref_max_lon_dec" default="1.0" />
+
+    <arg name="track_num_paths" default="9" />
+    <arg name="wheelbase" default="0.3" />
+    <arg name="max_steering_angle" default="0.4" />
+    <arg name="track_path_lateral_spacing" default="0.06" />
+    <arg name="track_look_ahead_time" default="0.5" />
+
+    <arg name="global_path_topic" default="global_path" />
+    <arg name="odom_topic" default="odom" />
+    <arg name="drive_feedback_topic" default="drive_feedback" />
 
     <node name="local_planner" pkg="local_planner" type="local_planner_node" output="screen">
         <rosparam param="circle_offsets" subst_value="True">$(arg circle_offsets)</rosparam>
@@ -46,10 +44,8 @@
         <param name="track_path_lateral_spacing" value="$(arg track_path_lateral_spacing)" />
         <param name="track_look_ahead_time" value="$(arg track_look_ahead_time)" />
 
-        <param name="global_path_topic" value="$(arg global_path_topic)" />
-        <param name="local_path_topic" value="$(arg local_path_topic)" />
-        <param name="odom_topic" value="$(arg odom_topic)" />
-        <param name="inflated_costmap_topic" value="$(arg inflated_costmap_topic)" />
-        <param name="viz_topic" value="$(arg viz_topic)" />
+        <remap from="global_path" to="$(arg global_path_topic)"/>
+        <remap from="odom" to="$(arg odom_topic)"/>
+        <remap from="drive_feedback" to="$(arg drive_feedback_topic)"/>
     </node>
 </launch>
diff --git a/local_planner/src/local_planner.cpp b/local_planner/src/local_planner.cpp
index 49dd409..9bfd3c5 100644
--- a/local_planner/src/local_planner.cpp
+++ b/local_planner/src/local_planner.cpp
@@ -34,27 +34,13 @@ LocalPlanner::LocalPlanner()
 
 void LocalPlanner::initCallbacks(const ros::NodeHandle& private_nh)
 {
-  std::string global_path_topic;
-  std::string local_path_topic;
-  std::string odom_topic;
-  std::string inflated_costmap_topic;
-  std::string viz_topic;
-  std::string drive_topic;
-
-  private_nh.param("global_path_topic", global_path_topic, std::string("path/global"));
-  private_nh.param("local_path_topic", local_path_topic, std::string("path/local"));
-  private_nh.param("odom_topic", odom_topic, std::string("odom"));
-  private_nh.param("drive_topic", drive_topic, std::string("drive"));
-  private_nh.param("inflated_costmap_topic", inflated_costmap_topic, std::string("inflated_costmap"));
-  private_nh.param("viz_topic", viz_topic, std::string("viz/local_planner"));
-
-  global_path_sub_ = nh_.subscribe(global_path_topic, 1, &LocalPlanner::globalPathCallback, this);
-  odom_sub_ = nh_.subscribe(odom_topic, 1, &LocalPlanner::odomCallback, this);
-  drive_sub_ = nh_.subscribe(drive_topic, 1, &LocalPlanner::driveCallback, this);
+  global_path_sub_ = nh_.subscribe("global_path", 1, &LocalPlanner::globalPathCallback, this);
+  odom_sub_ = nh_.subscribe("odom", 1, &LocalPlanner::odomCallback, this);
+  drive_sub_ = nh_.subscribe("drive_feedback", 1, &LocalPlanner::driveCallback, this);
   timer_ = nh_.createTimer(ros::Duration(0.1), &LocalPlanner::timerCallback, this);
 
-  trajectory_pub_ = nh_.advertise<f1tenth_msgs::Trajectory>(local_path_topic, 1);
-  viz_pub_ = nh_.advertise<visualization_msgs::MarkerArray>(viz_topic, 1);
+  trajectory_pub_ = nh_.advertise<f1tenth_msgs::Trajectory>("local_trajectory", 1);
+  viz_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("visualization/local_planner", 1);
 }
 
 void LocalPlanner::initPlanner(const ros::NodeHandle& private_nh)
@@ -80,6 +66,9 @@ void LocalPlanner::initPlanner(const ros::NodeHandle& private_nh)
   double track_k_spatial;
   double track_k_temporal;
 
+  nh_.param("wheelbase", wheelbase_, 0.3);
+  nh_.param("max_steering_angle", max_steering_angle, 0.4);
+
   private_nh.param("circle_offsets", circle_offsets, std::vector<double>{ 0.1, 0.3 });
   private_nh.param("circle_radius", circle_radius, 0.2);
 
@@ -94,8 +83,6 @@ void LocalPlanner::initPlanner(const ros::NodeHandle& private_nh)
   private_nh.param("ref_max_lon_dec", ref_max_lon_dec, 1.0);
 
   private_nh.param("track_num_paths", track_num_paths, 9);
-  private_nh.param("wheelbase", wheelbase_, 0.3);
-  private_nh.param("max_steering_angle", max_steering_angle, 0.4);
   private_nh.param("track_path_lateral_spacing", track_path_lateral_spacing, 0.05);
   private_nh.param("track_look_ahead_time", track_look_ahead_time, 0.5);
   private_nh.param("track_k_spatial", track_k_spatial, 1.0);
diff --git a/path_recorder/launch/path_recorder_publish.launch b/path_recorder/launch/path_recorder_publish.launch
index 1582b7c..c7e49b6 100644
--- a/path_recorder/launch/path_recorder_publish.launch
+++ b/path_recorder/launch/path_recorder_publish.launch
@@ -1,19 +1,14 @@
 <launch>
-    <!-- Publish: 0, Record: 1-->
-    <arg name="mode" value="0"/>
-
-    <arg name="file_name" value="$(find path_recorder)/path.csv"/>
-    <arg name="interval" value="0.1"/>
-
-    <arg name="odom_topic" value="odom"/>
-    <arg name="path_topic" value="path"/>
+    <arg name="mode" default="0" doc="Publish: 0, Record: 1"/>
+    <arg name="file_name" default="$(find path_recorder)/path.csv"/>
+    <arg name="interval" default="0.1"/>
+    <arg name="odom_topic" default="odom"/>
 
     <node name="path_recorder" pkg="path_recorder" type="path_recorder_node" output="screen">
         <param name="mode" value="$(arg mode)" />
         <param name="file_name" value="$(arg file_name)" />
         <param name="interval" value="$(arg interval)" />
 
-        <param name="odom_topic" value="$(arg odom_topic)"/>
-        <param name="path_topic" value="$(arg path_topic)" />
+        <remap from="odom" to="$(arg odom_topic)"/>
     </node>
 </launch>
\ No newline at end of file
diff --git a/path_recorder/launch/path_recorder_record.launch b/path_recorder/launch/path_recorder_record.launch
index 08fb95a..ece4792 100644
--- a/path_recorder/launch/path_recorder_record.launch
+++ b/path_recorder/launch/path_recorder_record.launch
@@ -1,19 +1,14 @@
 <launch>
-    <!-- Publish: 0, Record: 1-->
-    <arg name="mode" value="1"/>
-
-    <arg name="file_name" value="$(find path_recorder)/path.csv"/>
-    <arg name="interval" value="0.1"/>
-
-    <arg name="odom_topic" value="odom"/>
-    <arg name="path_topic" value="path"/>
+    <arg name="mode" default="0" doc="Publish: 0, Record: 1"/>
+    <arg name="file_name" default="$(find path_recorder)/path.csv"/>
+    <arg name="interval" default="0.1"/>
+    <arg name="odom_topic" default="odom"/>
 
     <node name="path_recorder" pkg="path_recorder" type="path_recorder_node" output="screen">
         <param name="mode" value="$(arg mode)" />
         <param name="file_name" value="$(arg file_name)" />
         <param name="interval" value="$(arg interval)" />
-
-        <param name="odom_topic" value="$(arg odom_topic)"/>
-        <param name="path_topic" value="$(arg path_topic)" />
+        
+        <remap from="odom" to="$(arg odom_topic)"/>
     </node>
 </launch>
\ No newline at end of file
diff --git a/path_recorder/src/path_recorder.cpp b/path_recorder/src/path_recorder.cpp
index 4865594..a90ee03 100644
--- a/path_recorder/src/path_recorder.cpp
+++ b/path_recorder/src/path_recorder.cpp
@@ -18,7 +18,6 @@ PathRecorder::PathRecorder()
 
   int mode;
   std::string file_name;
-  std::string path_topic;
 
   private_nh.param("mode", mode, 1);
 
@@ -27,10 +26,9 @@ PathRecorder::PathRecorder()
     throw std::invalid_argument("Invalid file specified");
   }
 
-  private_nh.param("path_topic", path_topic, std::string("path"));
   private_nh.param("interval", interval_, 0.1);
 
-  path_pub_ = nh_.advertise<nav_msgs::Path>(path_topic, 1, true);
+  path_pub_ = nh_.advertise<nav_msgs::Path>("global_path", 1, true);
 
   if (mode == static_cast<int>(Mode::Record))
   {
diff --git a/safety_controller/launch/safety_controller.launch b/safety_controller/launch/safety_controller.launch
index 22b8724..f7b9b3d 100644
--- a/safety_controller/launch/safety_controller.launch
+++ b/safety_controller/launch/safety_controller.launch
@@ -1,13 +1,9 @@
 <launch>
-    <arg name="odom_topic" value="odom"/>
-    <arg name="time_to_collision_topic" value="time_to_collision"/>
-    <arg name="selected_drive_topic" value="drive/selected"/>
-    <arg name="safe_drive_topic" value="drive/safe"/>
+    <arg name="odom_topic" default="odom"/>
+    <arg name="drive_in_topic" default="drive_in"/>
 
     <node name="safety_controller" pkg="safety_controller" type="safety_controller_node" output="screen">
-        <param name="odom_topic" value="$(arg odom_topic)" />
-        <param name="time_to_collision_topic" value="$(arg time_to_collision_topic)" />
-        <param name="selected_drive_topic" value="$(arg selected_drive_topic)" />
-        <param name="safe_drive_topic" value="$(arg safe_drive_topic)" />
+        <remap from="odom" to="$(arg odom_topic)"/>
+        <remap from="drive_in" to="$(arg drive_in_topic)"/>
     </node>
 </launch>
\ No newline at end of file
diff --git a/safety_controller/src/safety_controller.cpp b/safety_controller/src/safety_controller.cpp
index a3b9be4..b177a55 100644
--- a/safety_controller/src/safety_controller.cpp
+++ b/safety_controller/src/safety_controller.cpp
@@ -14,21 +14,10 @@ SafetyController::SafetyController()
 {
   ros::NodeHandle private_nh("~");
 
-  std::string odom_topic;
-  std::string time_to_collision_topic;
-  std::string selected_drive_topic;
-  std::string safe_drive_topic;
-
-  getParam("odom_topic", odom_topic);
-  getParam("time_to_collision_topic", time_to_collision_topic);
-  getParam("selected_drive_topic", selected_drive_topic);
-  getParam("safe_drive_topic", safe_drive_topic);
-
-  odom_sub_ = nh_.subscribe(odom_topic, 1, &SafetyController::odomCallback, this);
-  time_to_collision_sub_ = nh_.subscribe(time_to_collision_topic, 1, &SafetyController::timeToCollisionCallback, this);
-  selected_drive_sub_ = nh_.subscribe(selected_drive_topic, 1, &SafetyController::driveCallback, this);
-
-  safe_drive_pub_ = nh_.advertise<ackermann_msgs::AckermannDriveStamped>(safe_drive_topic, 1);
+  odom_sub_ = nh_.subscribe("odom", 1, &SafetyController::odomCallback, this);
+  time_to_collision_sub_ = nh_.subscribe("time_to_collision", 1, &SafetyController::timeToCollisionCallback, this);
+  selected_drive_sub_ = nh_.subscribe("drive_in", 1, &SafetyController::driveCallback, this);
+  safe_drive_pub_ = nh_.advertise<ackermann_msgs::AckermannDriveStamped>("drive_safe", 1);
 }
 
 void SafetyController::driveCallback(const ackermann_msgs::AckermannDriveStamped drive_msg)

From 6df3355797862b6c2b47ba7a4b328f3ad5bbedfb Mon Sep 17 00:00:00 2001
From: Bryan Zheng <ashuh223@gmail.com>
Date: Wed, 8 Dec 2021 22:43:02 +0800
Subject: [PATCH 05/16] chore(bringup): Add vesc_to_odom.yaml

---
 f1tenth_racecar_bringup/config/vesc_to_odom.yaml | 4 ++++
 f1tenth_racecar_bringup/launch/vesc.launch       | 3 ++-
 2 files changed, 6 insertions(+), 1 deletion(-)
 create mode 100644 f1tenth_racecar_bringup/config/vesc_to_odom.yaml

diff --git a/f1tenth_racecar_bringup/config/vesc_to_odom.yaml b/f1tenth_racecar_bringup/config/vesc_to_odom.yaml
new file mode 100644
index 0000000..6a1a53c
--- /dev/null
+++ b/f1tenth_racecar_bringup/config/vesc_to_odom.yaml
@@ -0,0 +1,4 @@
+odom_frame: odom
+base_frame: base_link
+publish_tf: true
+use_servo_cmd_to_calc_angular_velocity: true
\ No newline at end of file
diff --git a/f1tenth_racecar_bringup/launch/vesc.launch b/f1tenth_racecar_bringup/launch/vesc.launch
index 1fa852f..b79e650 100644
--- a/f1tenth_racecar_bringup/launch/vesc.launch
+++ b/f1tenth_racecar_bringup/launch/vesc.launch
@@ -8,9 +8,10 @@
     <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/vesc_ackermann.yaml"/>
 
     <node name="ackermann_to_vesc" pkg="vesc_ackermann" type="ackermann_to_vesc_node" output="screen">
-        <remap from="ackermann_cmd" to="$(arg vesc_cmd_topic)"/>
+        <remap from="ackermann_cmd" to="$(arg vesc_drive_topic)"/>
     </node>
 
     <node name="vesc_to_odom" pkg="vesc_ackermann" type="vesc_to_odom_node" output="screen">
+        <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/vesc_to_odom.yaml"/>
     </node>
 </launch>

From 944c3433e2045bf7f260501dcef3001f95c1fd56 Mon Sep 17 00:00:00 2001
From: Bryan Zheng <ashuh223@gmail.com>
Date: Wed, 8 Dec 2021 22:44:29 +0800
Subject: [PATCH 06/16] chore(bringup): Add lidar.launch

---
 f1tenth_racecar_bringup/launch/lidar.launch | 13 +++++++++++++
 1 file changed, 13 insertions(+)
 create mode 100644 f1tenth_racecar_bringup/launch/lidar.launch

diff --git a/f1tenth_racecar_bringup/launch/lidar.launch b/f1tenth_racecar_bringup/launch/lidar.launch
new file mode 100644
index 0000000..e6bce61
--- /dev/null
+++ b/f1tenth_racecar_bringup/launch/lidar.launch
@@ -0,0 +1,13 @@
+<launch>
+  <node name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" output="screen">
+    <param name="serial_port" type="string" value="/dev/sensors/rplidar"/>
+    <param name="serial_baudrate" type="int" value="256000"/>
+    <!--A3 -->
+    <param name="frame_id" type="string" value="scan"/>
+    <param name="inverted" type="bool" value="false"/>
+    <param name="angle_compensate" type="bool" value="true"/>
+    <param name="scan_mode" type="string" value="Sensitivity"/>
+  </node>
+
+  <node pkg="tf" type="static_transform_publisher" name="rplidar_broadcaster" args="0.3 0 0.1 3.14 0 0 base_link scan 100" />
+</launch>

From 732a2d250591bf14da005ddc6e0f1a404b8dd210 Mon Sep 17 00:00:00 2001
From: Bryan Zheng <ashuh223@gmail.com>
Date: Wed, 8 Dec 2021 22:46:56 +0800
Subject: [PATCH 07/16] chore(bringup): Update f1tenth_slam.launch

---
 f1tenth_racecar_bringup/launch/f1tenth_slam.launch | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/f1tenth_racecar_bringup/launch/f1tenth_slam.launch b/f1tenth_racecar_bringup/launch/f1tenth_slam.launch
index 752f7b3..071a9c7 100644
--- a/f1tenth_racecar_bringup/launch/f1tenth_slam.launch
+++ b/f1tenth_racecar_bringup/launch/f1tenth_slam.launch
@@ -5,7 +5,10 @@
     </include>
 
     <include file="$(find f1tenth_racecar_bringup)/launch/vesc.launch">
-        <param name="vesc_drive_topic" value="drive_joy"/>
+        <arg name="vesc_drive_topic" value="drive_joy"/>
+    </include>
+
+    <include file="$(find f1tenth_racecar_bringup)/launch/lidar.launch">
     </include>
 
     <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
@@ -13,4 +16,6 @@
         <remap from="scan" to="scan"/>
         <remap from="odom" to="odom"/>
     </node>
+
+    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find f1tenth_racecar_bringup)/rviz/f1tenth_slam.rviz" output="screen"/>
 </launch>

From 427d61a9522ffc36a61e9771fcda590f3ad18ad1 Mon Sep 17 00:00:00 2001
From: Bryan Zheng <ashuh223@gmail.com>
Date: Wed, 8 Dec 2021 22:47:25 +0800
Subject: [PATCH 08/16] chore(bringup): Add f1tenth_slam_record.launch

---
 .../launch/f1tenth_slam_record.launch             | 15 +++++++++++++++
 1 file changed, 15 insertions(+)
 create mode 100644 f1tenth_racecar_bringup/launch/f1tenth_slam_record.launch

diff --git a/f1tenth_racecar_bringup/launch/f1tenth_slam_record.launch b/f1tenth_racecar_bringup/launch/f1tenth_slam_record.launch
new file mode 100644
index 0000000..bb2fd29
--- /dev/null
+++ b/f1tenth_racecar_bringup/launch/f1tenth_slam_record.launch
@@ -0,0 +1,15 @@
+<launch>
+    <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/vehicle_params.yaml"/>
+
+    <include file="$(find f1tenth_racecar_bringup)/launch/joy.launch">
+    </include>
+
+    <include file="$(find f1tenth_racecar_bringup)/launch/vesc.launch">
+        <arg name="vesc_drive_topic" value="drive_joy"/>
+    </include>
+
+    <include file="$(find f1tenth_racecar_bringup)/launch/lidar.launch">
+    </include>
+
+    <node pkg="rosbag" type="record" name="rosbag_record" args="record -a -O $(find f1tenth_racecar_bringup)/bag/slam_record.bag" />
+</launch>

From 6ac36756eef6360ef45ae3ccf4771d5e118fa508 Mon Sep 17 00:00:00 2001
From: Bryan Zheng <ashuh223@gmail.com>
Date: Wed, 8 Dec 2021 22:49:18 +0800
Subject: [PATCH 09/16] chore(bringup): Update gmapping default parameters

---
 f1tenth_racecar_bringup/config/gmapping.yaml | 24 ++++++++++----------
 1 file changed, 12 insertions(+), 12 deletions(-)

diff --git a/f1tenth_racecar_bringup/config/gmapping.yaml b/f1tenth_racecar_bringup/config/gmapping.yaml
index 0820e73..0e08925 100644
--- a/f1tenth_racecar_bringup/config/gmapping.yaml
+++ b/f1tenth_racecar_bringup/config/gmapping.yaml
@@ -1,8 +1,8 @@
 base_frame: base_link
 odom_frame: odom
 map_update_interval: 5.0
-maxUrange: 6.0
-maxRange: 8.0
+maxUrange: 10.0
+maxRange: 25.0
 sigma: 0.05
 kernelSize: 1
 lstep: 0.05
@@ -12,20 +12,20 @@ lsigma: 0.075
 ogain: 3.0
 lskip: 0
 minimumScore: 100
-srr: 0.01
-srt: 0.02
-str: 0.01
-stt: 0.02
+srr: 0.3
+srt: 0.2
+str: 0.0
+stt: 0.2
 linearUpdate: 0.1
-angularUpdate: 0.3
+angularUpdate: 0.1
 temporalUpdate: -1.0
 resampleThreshold: 0.5
-particles: 80
+particles: 10
 
-xmin: -50.0
-ymin: -50.0
-xmax: 50.0
-ymax: 50.0
+xmin: -10.0
+ymin: -10.0
+xmax: 10.0
+ymax: 10.0
 
 delta: 0.05
 llsamplerange: 0.01

From a0acc152bfa4491043781c27f40a80f635252dd0 Mon Sep 17 00:00:00 2001
From: Bryan Zheng <ashuh223@gmail.com>
Date: Wed, 8 Dec 2021 22:51:55 +0800
Subject: [PATCH 10/16] chore(bringup): Add f1tenth_slam.rviz

---
 .../rviz/f1tenth_slam.rviz                    | 214 ++++++++++++++++++
 1 file changed, 214 insertions(+)
 create mode 100644 f1tenth_racecar_bringup/rviz/f1tenth_slam.rviz

diff --git a/f1tenth_racecar_bringup/rviz/f1tenth_slam.rviz b/f1tenth_racecar_bringup/rviz/f1tenth_slam.rviz
new file mode 100644
index 0000000..6c04d89
--- /dev/null
+++ b/f1tenth_racecar_bringup/rviz/f1tenth_slam.rviz
@@ -0,0 +1,214 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Status1
+        - /Odometry1/Shape1
+        - /TF1/Tree1
+      Splitter Ratio: 0.5117647051811218
+    Tree Height: 571
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+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: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: false
+      Enabled: true
+      Keep: 5
+      Name: Odometry
+      Position Tolerance: 0.10000000149011612
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 255; 25; 0
+        Head Length: 0.05000000074505806
+        Head Radius: 0.10000000149011612
+        Shaft Length: 0.10000000149011612
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic: /odom
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base_link:
+          Value: true
+        map:
+          Value: true
+        odom:
+          Value: true
+        scan:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        map:
+          odom:
+            base_link:
+              scan:
+                {}
+      Update Interval: 0
+      Value: true
+  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:
+      Angle: 0
+      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: Current View
+      Near Clip Distance: 0.009999999776482582
+      Scale: 28.89838981628418
+      Target Frame: <Fixed Frame>
+      Value: TopDownOrtho (rviz)
+      X: 1.1936955451965332
+      Y: 0.06486502289772034
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002c6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000027000002c6000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000027000002c6000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000003efc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000397000002c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1267
+  X: 65
+  Y: 60

From a1f97835bc0b38d3f1a325b8b9ddb4c37f5c12b2 Mon Sep 17 00:00:00 2001
From: Bryan Zheng <ashuh223@gmail.com>
Date: Thu, 9 Dec 2021 17:03:52 +0800
Subject: [PATCH 11/16] feat(bringup): Add launch files

---
 .../launch/f1tenth_sim_record_path.launch     | 29 +++++++++++++++++++
 .../launch/f1tenth_slam_play.launch           | 13 +++++++++
 2 files changed, 42 insertions(+)
 create mode 100644 f1tenth_racecar_bringup/launch/f1tenth_sim_record_path.launch
 create mode 100644 f1tenth_racecar_bringup/launch/f1tenth_slam_play.launch

diff --git a/f1tenth_racecar_bringup/launch/f1tenth_sim_record_path.launch b/f1tenth_racecar_bringup/launch/f1tenth_sim_record_path.launch
new file mode 100644
index 0000000..27da93e
--- /dev/null
+++ b/f1tenth_racecar_bringup/launch/f1tenth_sim_record_path.launch
@@ -0,0 +1,29 @@
+<launch>
+    <arg name="map_name" value="map"/>
+    <arg name="path_name" value="path"/>
+
+    <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/vehicle_params.yaml"/>
+
+    <node pkg="map_server" name="map_server" type="map_server" args="$(find f1tenth_racecar_bringup)/map/$(arg map_name).yaml"/>
+
+    <include file="$(find f1tenth_racecar_bringup)/launch/f1tenth_teleop.launch">
+    </include>
+
+    <node name="path_recorder" pkg="path_recorder" type="path_recorder_node" output="screen">
+        <param name="mode" value="1"/>
+        <param name="odom_topic" value="odom"/>
+        <param name="interval" value="0.1"/>
+        <param name="file_name" value="$(find f1tenth_racecar_bringup)/path/$(arg path_name).csv"/>
+    </node>
+
+    <node pkg="f1tenth_simulator" name="f1tenth_simulator" type="simulator" output="screen">
+        <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/f1tenth_simulator.yaml"/>
+        <param name="map_frame" value="map"/>
+        <param name="base_frame" value="base_link"/>
+        <remap from="drive" to="drive_joy"/>
+    </node>
+
+    <include file="$(find f1tenth_simulator)/launch/racecar_model.launch"/>
+
+    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find f1tenth_racecar_bringup)/rviz/f1tenth.rviz" output="screen"/>
+</launch>
diff --git a/f1tenth_racecar_bringup/launch/f1tenth_slam_play.launch b/f1tenth_racecar_bringup/launch/f1tenth_slam_play.launch
new file mode 100644
index 0000000..4569ead
--- /dev/null
+++ b/f1tenth_racecar_bringup/launch/f1tenth_slam_play.launch
@@ -0,0 +1,13 @@
+<launch>
+    <param name="/use_sim_time" value="true"/>
+
+    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
+        <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/gmapping.yaml"/>
+        <remap from="scan" to="scan"/>
+        <remap from="odom" to="odom"/>
+    </node>
+
+    <node pkg="rosbag" type="play" name="player" output="screen" args="--clock $(find f1tenth_racecar_bringup)/bag/slam_record.bag"/>
+
+    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find f1tenth_racecar_bringup)/rviz/f1tenth_slam.rviz" output="screen"/>
+</launch>

From 5a9b88f84a815549935dfbf404d4428f77be6c19 Mon Sep 17 00:00:00 2001
From: Bryan Zheng <ashuh223@gmail.com>
Date: Thu, 9 Dec 2021 17:18:09 +0800
Subject: [PATCH 12/16] refactor(bringup): Rename files

---
 .../config/{simulator.yaml => f1tenth_simulator.yaml}    | 0
 .../launch/{f1tenth_racecar.launch => f1tenth.launch}    | 9 ++++++---
 .../launch/{lidar.launch => f1tenth_lidar.launch}        | 0
 .../{autonomy.launch => f1tenth_navigation.launch}       | 0
 f1tenth_racecar_bringup/launch/f1tenth_sim.launch        | 8 ++++----
 f1tenth_racecar_bringup/launch/f1tenth_slam.launch       | 6 +++---
 .../launch/f1tenth_slam_record.launch                    | 6 +++---
 .../launch/{joy.launch => f1tenth_teleop.launch}         | 0
 .../launch/{vesc.launch => f1tenth_vesc.launch}          | 0
 9 files changed, 16 insertions(+), 13 deletions(-)
 rename f1tenth_racecar_bringup/config/{simulator.yaml => f1tenth_simulator.yaml} (100%)
 rename f1tenth_racecar_bringup/launch/{f1tenth_racecar.launch => f1tenth.launch} (52%)
 rename f1tenth_racecar_bringup/launch/{lidar.launch => f1tenth_lidar.launch} (100%)
 rename f1tenth_racecar_bringup/launch/{autonomy.launch => f1tenth_navigation.launch} (100%)
 rename f1tenth_racecar_bringup/launch/{joy.launch => f1tenth_teleop.launch} (100%)
 rename f1tenth_racecar_bringup/launch/{vesc.launch => f1tenth_vesc.launch} (100%)

diff --git a/f1tenth_racecar_bringup/config/simulator.yaml b/f1tenth_racecar_bringup/config/f1tenth_simulator.yaml
similarity index 100%
rename from f1tenth_racecar_bringup/config/simulator.yaml
rename to f1tenth_racecar_bringup/config/f1tenth_simulator.yaml
diff --git a/f1tenth_racecar_bringup/launch/f1tenth_racecar.launch b/f1tenth_racecar_bringup/launch/f1tenth.launch
similarity index 52%
rename from f1tenth_racecar_bringup/launch/f1tenth_racecar.launch
rename to f1tenth_racecar_bringup/launch/f1tenth.launch
index 32a0104..bda4847 100644
--- a/f1tenth_racecar_bringup/launch/f1tenth_racecar.launch
+++ b/f1tenth_racecar_bringup/launch/f1tenth.launch
@@ -1,15 +1,18 @@
 <launch>
     <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/vehicle_params.yaml"/>
 
-    <include file="$(find f1tenth_racecar_bringup)/launch/joy.launch">
+    <include file="$(find f1tenth_racecar_bringup)/launch/f1tenth_teleop.launch">
     </include>
 
-    <include file="$(find f1tenth_racecar_bringup)/launch/autonomy.launch">
+    <include file="$(find f1tenth_racecar_bringup)/launch/f1tenth_lidar.launch">
+    </include>
+
+    <include file="$(find f1tenth_racecar_bringup)/launch/f1tenth_navigation.launch">
         <arg name="map_name" value="map"/>
         <arg name="path_name" value="path"/>
     </include>
 
-    <include file="$(find f1tenth_racecar_bringup)/launch/vesc.launch">
+    <include file="$(find f1tenth_racecar_bringup)/launch/f1tenth_vesc.launch">
         <arg name="vesc_drive_topic" value="drive_safe"/>
     </include>
 
diff --git a/f1tenth_racecar_bringup/launch/lidar.launch b/f1tenth_racecar_bringup/launch/f1tenth_lidar.launch
similarity index 100%
rename from f1tenth_racecar_bringup/launch/lidar.launch
rename to f1tenth_racecar_bringup/launch/f1tenth_lidar.launch
diff --git a/f1tenth_racecar_bringup/launch/autonomy.launch b/f1tenth_racecar_bringup/launch/f1tenth_navigation.launch
similarity index 100%
rename from f1tenth_racecar_bringup/launch/autonomy.launch
rename to f1tenth_racecar_bringup/launch/f1tenth_navigation.launch
diff --git a/f1tenth_racecar_bringup/launch/f1tenth_sim.launch b/f1tenth_racecar_bringup/launch/f1tenth_sim.launch
index 89e7dfe..c14c140 100644
--- a/f1tenth_racecar_bringup/launch/f1tenth_sim.launch
+++ b/f1tenth_racecar_bringup/launch/f1tenth_sim.launch
@@ -1,20 +1,20 @@
 <launch>
   <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/vehicle_params.yaml"/>
 
-  <include file="$(find f1tenth_racecar_bringup)/launch/joy.launch">
+  <include file="$(find f1tenth_racecar_bringup)/launch/f1tenth_teleop.launch">
   </include>
 
-  <include file="$(find f1tenth_racecar_bringup)/launch/autonomy.launch">
+  <include file="$(find f1tenth_racecar_bringup)/launch/f1tenth_navigation.launch">
     <arg name="map_name" value="berlin"/>
     <arg name="path_name" value="berlin"/>
   </include>
 
   <node pkg="f1tenth_simulator" name="f1tenth_simulator" type="simulator" output="screen">
-    <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/simulator.yaml"/>
+    <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/f1tenth_simulator.yaml"/>
     <remap from="drive" to="drive_safe"/>
   </node>
 
   <include file="$(find f1tenth_simulator)/launch/racecar_model.launch"/>
 
-  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find f1tenth_racecar_bringup)/rviz/f1tenth_racecar.rviz" output="screen"/>
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find f1tenth_racecar_bringup)/rviz/f1tenth_sim.rviz" output="screen"/>
 </launch>
diff --git a/f1tenth_racecar_bringup/launch/f1tenth_slam.launch b/f1tenth_racecar_bringup/launch/f1tenth_slam.launch
index 071a9c7..11276a2 100644
--- a/f1tenth_racecar_bringup/launch/f1tenth_slam.launch
+++ b/f1tenth_racecar_bringup/launch/f1tenth_slam.launch
@@ -1,14 +1,14 @@
 <launch>
     <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/vehicle_params.yaml"/>
 
-    <include file="$(find f1tenth_racecar_bringup)/launch/joy.launch">
+    <include file="$(find f1tenth_racecar_bringup)/launch/f1tenth_teleop.launch">
     </include>
 
-    <include file="$(find f1tenth_racecar_bringup)/launch/vesc.launch">
+    <include file="$(find f1tenth_racecar_bringup)/launch/f1tenth_vesc.launch">
         <arg name="vesc_drive_topic" value="drive_joy"/>
     </include>
 
-    <include file="$(find f1tenth_racecar_bringup)/launch/lidar.launch">
+    <include file="$(find f1tenth_racecar_bringup)/launch/f1tenth_lidar.launch">
     </include>
 
     <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
diff --git a/f1tenth_racecar_bringup/launch/f1tenth_slam_record.launch b/f1tenth_racecar_bringup/launch/f1tenth_slam_record.launch
index bb2fd29..0da5647 100644
--- a/f1tenth_racecar_bringup/launch/f1tenth_slam_record.launch
+++ b/f1tenth_racecar_bringup/launch/f1tenth_slam_record.launch
@@ -1,14 +1,14 @@
 <launch>
     <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/vehicle_params.yaml"/>
 
-    <include file="$(find f1tenth_racecar_bringup)/launch/joy.launch">
+    <include file="$(find f1tenth_racecar_bringup)/launch/f1tenth_teleop.launch">
     </include>
 
-    <include file="$(find f1tenth_racecar_bringup)/launch/vesc.launch">
+    <include file="$(find f1tenth_racecar_bringup)/launch/f1tenth_vesc.launch">
         <arg name="vesc_drive_topic" value="drive_joy"/>
     </include>
 
-    <include file="$(find f1tenth_racecar_bringup)/launch/lidar.launch">
+    <include file="$(find f1tenth_racecar_bringup)/launch/f1tenth_lidar.launch">
     </include>
 
     <node pkg="rosbag" type="record" name="rosbag_record" args="record -a -O $(find f1tenth_racecar_bringup)/bag/slam_record.bag" />
diff --git a/f1tenth_racecar_bringup/launch/joy.launch b/f1tenth_racecar_bringup/launch/f1tenth_teleop.launch
similarity index 100%
rename from f1tenth_racecar_bringup/launch/joy.launch
rename to f1tenth_racecar_bringup/launch/f1tenth_teleop.launch
diff --git a/f1tenth_racecar_bringup/launch/vesc.launch b/f1tenth_racecar_bringup/launch/f1tenth_vesc.launch
similarity index 100%
rename from f1tenth_racecar_bringup/launch/vesc.launch
rename to f1tenth_racecar_bringup/launch/f1tenth_vesc.launch

From 3cd821d24cf10d1c1a1d4c3f8c1b44f49c9297bd Mon Sep 17 00:00:00 2001
From: Bryan Zheng <ashuh223@gmail.com>
Date: Thu, 9 Dec 2021 17:18:31 +0800
Subject: [PATCH 13/16] refactor(bringup): Remove test.launch

---
 f1tenth_racecar_bringup/launch/test.launch | 11 -----------
 1 file changed, 11 deletions(-)
 delete mode 100644 f1tenth_racecar_bringup/launch/test.launch

diff --git a/f1tenth_racecar_bringup/launch/test.launch b/f1tenth_racecar_bringup/launch/test.launch
deleted file mode 100644
index 4124c86..0000000
--- a/f1tenth_racecar_bringup/launch/test.launch
+++ /dev/null
@@ -1,11 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-    <arg name="odom_topic" default="odom"/>
-
-    <node name="collision_warning_system" pkg="collision_warning_system" type="collision_warning_system_node" output="screen">
-        <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/collision_warning_system.yaml"/>
-
-        <remap from="odom" to="$(arg odom_topic)"/>
-        <remap from="drive" to="drive_safe"/>
-    </node>
-</launch>

From 5d02db53029ced556f2e1f2db63058ca0209afec Mon Sep 17 00:00:00 2001
From: Bryan Zheng <ashuh223@gmail.com>
Date: Thu, 9 Dec 2021 17:29:38 +0800
Subject: [PATCH 14/16] feat(bringup): Add f1tenth_sim.rviz

---
 f1tenth_racecar_bringup/rviz/f1tenth_sim.rviz | 341 ++++++++++++++++++
 1 file changed, 341 insertions(+)
 create mode 100644 f1tenth_racecar_bringup/rviz/f1tenth_sim.rviz

diff --git a/f1tenth_racecar_bringup/rviz/f1tenth_sim.rviz b/f1tenth_racecar_bringup/rviz/f1tenth_sim.rviz
new file mode 100644
index 0000000..5cf74d4
--- /dev/null
+++ b/f1tenth_racecar_bringup/rviz/f1tenth_sim.rviz
@@ -0,0 +1,341 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 87
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Status1
+        - /Local Planner1/Namespaces1
+      Splitter Ratio: 0.7277777791023254
+    Tree Height: 710
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  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
+    - Class: rviz/TF
+      Enabled: false
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+      Value: false
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: true
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /racecar_sim/update
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 14.848588943481445
+      Min Color: 0; 0; 225
+      Min Intensity: 1.96762216091156
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.10000000149011612
+      Style: Points
+      Topic: /scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        back_left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        back_right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_left_hinge:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_right_hinge:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        laser_model:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: /racecar/robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Alpha: 0.5
+      Autocompute Intensity Bounds: true
+      Class: grid_map_rviz_plugin/GridMap
+      Color: 200; 200; 200
+      Color Layer: STATIC
+      Color Transformer: IntensityLayer
+      Enabled: true
+      Height Layer: STATIC
+      Height Transformer: Flat
+      History Length: 1
+      Invert Rainbow: false
+      Max Color: 255; 25; 0
+      Max Intensity: 10
+      Min Color: 255; 255; 255
+      Min Intensity: 0
+      Name: GridMap
+      Show Grid Lines: false
+      Topic: /costmap
+      Unreliable: false
+      Use Rainbow: false
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /vizualization/collision_warning_system
+      Name: Collision Warning System
+      Namespaces:
+        "": true
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /visualization/drive_controller
+      Name: Drive Controller
+      Namespaces:
+        arc: true
+        look_ahead_point: true
+      Queue Size: 100
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 46; 52; 54
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Billboards
+      Line Width: 0.05000000074505806
+      Name: Global Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic: /global_path
+      Unreliable: false
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /visualization/local_planner
+      Name: Local Planner
+      Namespaces:
+        final_trajectory/path: true
+        final_trajectory/velocity: false
+        lattice/edges: false
+        lattice/vertices: false
+        lattice/weights: false
+        reference_trajectory/path: true
+        reference_trajectory/velocity: false
+        safe_paths: true
+        sssp: true
+      Queue Size: 100
+      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: false
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: AMCL Pose
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic: /amcl_pose
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz/Pose
+      Color: 25; 255; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Ground Truth Pose
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic: /gt_pose
+      Unreliable: false
+      Value: true
+  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: 5.63323974609375
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.6303983330726624
+      Target Frame: base_link
+      Value: ThirdPersonFollower (rviz)
+      Yaw: 3.1054089069366455
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1022
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000260000001370000000000000000fb0000000a0049006d00610067006500000002d0000000c70000000000000000000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000055e00000044fc0100000002fb0000000800540069006d006501000000000000055e000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004020000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1374
+  X: 101
+  Y: 27

From 1a3f3d8d8d2b591a5335afe2f3a37ecaa8a9a457 Mon Sep 17 00:00:00 2001
From: Bryan Zheng <ashuh223@gmail.com>
Date: Sun, 12 Dec 2021 05:23:03 +0800
Subject: [PATCH 15/16] chore(bringup): Update vesc params

---
 f1tenth_racecar_bringup/config/vehicle_params.yaml | 2 +-
 f1tenth_racecar_bringup/config/vesc_ackermann.yaml | 6 +++---
 f1tenth_racecar_bringup/config/vesc_driver.yaml    | 4 ++--
 3 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/f1tenth_racecar_bringup/config/vehicle_params.yaml b/f1tenth_racecar_bringup/config/vehicle_params.yaml
index a0c3428..68ff6c4 100644
--- a/f1tenth_racecar_bringup/config/vehicle_params.yaml
+++ b/f1tenth_racecar_bringup/config/vehicle_params.yaml
@@ -2,4 +2,4 @@ vehicle_length: 0.5 # meters
 vehicle_width: 0.27 # meters
 base_link_to_center_dist: 0.165 # meters
 wheelbase: 0.33 # meters
-max_steering_angle: 0.4189 # radians
+max_steering_angle: 0.4698 # radians
diff --git a/f1tenth_racecar_bringup/config/vesc_ackermann.yaml b/f1tenth_racecar_bringup/config/vesc_ackermann.yaml
index 8ff481b..d713953 100644
--- a/f1tenth_racecar_bringup/config/vesc_ackermann.yaml
+++ b/f1tenth_racecar_bringup/config/vesc_ackermann.yaml
@@ -1,4 +1,4 @@
-speed_to_erpm_gain: 4614
+speed_to_erpm_gain: 7800
 speed_to_erpm_offset: 0
-steering_angle_to_servo_gain: -1.2135
-steering_angle_to_servo_offset: 0.5
+steering_angle_to_servo_gain: -1.0643
+steering_angle_to_servo_offset: 0.51
diff --git a/f1tenth_racecar_bringup/config/vesc_driver.yaml b/f1tenth_racecar_bringup/config/vesc_driver.yaml
index bc08994..42134d6 100644
--- a/f1tenth_racecar_bringup/config/vesc_driver.yaml
+++ b/f1tenth_racecar_bringup/config/vesc_driver.yaml
@@ -9,5 +9,5 @@ position_max: 0.0
 position_min: 0.0
 servo_max: 1.0
 servo_min: 0.0
-speed_max: 23250.0
-speed_min: -23250.0
+speed_max: 200000.0
+speed_min: -200000.0

From 8d5ff3fa358969927c7d0f038e0ddcad8ea2582a Mon Sep 17 00:00:00 2001
From: Bryan Zheng <ashuh223@gmail.com>
Date: Mon, 20 Dec 2021 14:17:46 +0800
Subject: [PATCH 16/16] style(bringup): Add newline at end of file

---
 f1tenth_racecar_bringup/config/vesc_to_odom.yaml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/f1tenth_racecar_bringup/config/vesc_to_odom.yaml b/f1tenth_racecar_bringup/config/vesc_to_odom.yaml
index 6a1a53c..6115833 100644
--- a/f1tenth_racecar_bringup/config/vesc_to_odom.yaml
+++ b/f1tenth_racecar_bringup/config/vesc_to_odom.yaml
@@ -1,4 +1,4 @@
 odom_frame: odom
 base_frame: base_link
 publish_tf: true
-use_servo_cmd_to_calc_angular_velocity: true
\ No newline at end of file
+use_servo_cmd_to_calc_angular_velocity: true