diff --git a/src/base_autonomy/local_planner/config/unitree/unitree_g1.yaml b/src/base_autonomy/local_planner/config/unitree/unitree_g1.yaml
index 65e70665..65fc6559 100644
--- a/src/base_autonomy/local_planner/config/unitree/unitree_g1.yaml
+++ b/src/base_autonomy/local_planner/config/unitree/unitree_g1.yaml
@@ -26,10 +26,6 @@ localPlanner:
     # obstacle height threshold
     obstacleHeightThre: 0.1 # 0.1 for flat terrain
 
-    # Sensor offset
-    sensorOffsetX: 0.0
-    sensorOffsetY: 0.0
-  
     # Vehicle dimensions
     vehicleLength: 0.5
     vehicleWidth: 0.5
diff --git a/src/base_autonomy/local_planner/launch/local_planner.launch b/src/base_autonomy/local_planner/launch/local_planner.launch
index 4ef035ff..ff22080d 100644
--- a/src/base_autonomy/local_planner/launch/local_planner.launch
+++ b/src/base_autonomy/local_planner/launch/local_planner.launch
@@ -78,8 +78,8 @@
   </node>
 
   <!-- Static transform using sensor offsets from launch arguments (should match robot_config file values) -->
-  <node pkg="tf2_ros" exec="static_transform_publisher" name="vehicleTransPublisher" args="$(var sensorOffsetX) $(var sensorOffsetY) $(var sensorOffsetZ) 0 0 0 /sensor /vehicle"/>
+  <node pkg="tf2_ros" exec="static_transform_publisher" name="vehicleTransPublisher" args="$(var sensorOffsetX) $(var sensorOffsetY) $(var sensorOffsetZ) 0 0 0 sensor base_link"/>
 
-  <node pkg="tf2_ros" exec="static_transform_publisher" name="sensorTransPublisher" args="0 0 $(var cameraOffsetZ) -1.5707963 0 -1.5707963 /sensor /camera"/>
+  <node pkg="tf2_ros" exec="static_transform_publisher" name="sensorTransPublisher" args="0 0 $(var cameraOffsetZ) -1.5707963 0 -1.5707963 sensor camera"/>
 
 </launch>
diff --git a/src/base_autonomy/local_planner/launch/local_planner.launch.py b/src/base_autonomy/local_planner/launch/local_planner.launch.py
index abbf5154..4d52e70c 100644
--- a/src/base_autonomy/local_planner/launch/local_planner.launch.py
+++ b/src/base_autonomy/local_planner/launch/local_planner.launch.py
@@ -185,6 +185,21 @@ def generate_launch_description():
         ]
     )
 
+    # Odom transformer: re-expresses /state_estimation (sensor frame) as
+    # odom_base_link (base_link frame) using the sensor->base_link static TF.
+    odomTransformer_node = Node(
+        package='odom_transformer',
+        executable='odom_transformer_node',
+        name='odom_transformer',
+        output='screen',
+        parameters=[{
+            'source_frame': 'sensor',
+            'target_frame': 'base_link',
+            'odom_topic_in': '/state_estimation',
+            'odom_topic_out': '/odom_base_link',
+        }]
+    )
+
     # Static transform publishers with sensor offsets from config
     vehicleTransPublisher_node = Node(
         package='tf2_ros',
@@ -195,8 +210,8 @@ def generate_launch_description():
             str(-sensor_offsets['sensorOffsetY']),
             str(-sensor_offsets['sensorOffsetZ']),
             '0', '0', '0',
-            '/sensor',
-            '/vehicle'
+            'sensor',
+            'base_link'
         ]
     )
 
@@ -211,7 +226,7 @@ def generate_launch_description():
             str(camera_offsets['cameraOffsetRoll']),
             str(camera_offsets['cameraOffsetPitch']),
             str(camera_offsets['cameraOffsetYaw']),
-            '/sensor', camera_link
+            'sensor', camera_link
         ]
     )
 
@@ -227,4 +242,5 @@ def generate_launch_description():
         pathFollower_node,
         vehicleTransPublisher_node,
         sensorTransPublisher_node,
+        odomTransformer_node,
     ])
\ No newline at end of file
diff --git a/src/base_autonomy/local_planner/src/localPlanner.cpp b/src/base_autonomy/local_planner/src/localPlanner.cpp
index f41d33a3..2b3d7146 100644
--- a/src/base_autonomy/local_planner/src/localPlanner.cpp
+++ b/src/base_autonomy/local_planner/src/localPlanner.cpp
@@ -52,8 +52,6 @@ double normalizeAngle(double angle) {
 string pathFolder;
 double vehicleLength = 0.6;
 double vehicleWidth = 0.6;
-double sensorOffsetX = 0;
-double sensorOffsetY = 0;
 bool twoWayDrive = true;
 double laserVoxelSize = 0.05;
 double terrainVoxelSize = 0.2;
@@ -169,8 +167,8 @@ void odometryHandler(const nav_msgs::msg::Odometry::ConstSharedPtr odom)
   vehicleRoll = roll;
   vehiclePitch = pitch;
   vehicleYaw = yaw;
-  vehicleX = odom->pose.pose.position.x - cos(yaw) * sensorOffsetX + sin(yaw) * sensorOffsetY;
-  vehicleY = odom->pose.pose.position.y - sin(yaw) * sensorOffsetX - cos(yaw) * sensorOffsetY;
+  vehicleX = odom->pose.pose.position.x;
+  vehicleY = odom->pose.pose.position.y;
   vehicleZ = odom->pose.pose.position.z;
 }
 
@@ -562,8 +560,6 @@ int main(int argc, char** argv)
   nh->declare_parameter<std::string>("pathFolder", pathFolder);
   nh->declare_parameter<double>("vehicleLength", vehicleLength);
   nh->declare_parameter<double>("vehicleWidth", vehicleWidth);
-  nh->declare_parameter<double>("sensorOffsetX", sensorOffsetX);
-  nh->declare_parameter<double>("sensorOffsetY", sensorOffsetY);
   nh->declare_parameter<bool>("twoWayDrive", twoWayDrive);
   nh->declare_parameter<double>("laserVoxelSize", laserVoxelSize);
   nh->declare_parameter<double>("terrainVoxelSize", terrainVoxelSize);
@@ -610,8 +606,6 @@ int main(int argc, char** argv)
   nh->get_parameter("pathFolder", pathFolder);
   nh->get_parameter("vehicleLength", vehicleLength);
   nh->get_parameter("vehicleWidth", vehicleWidth);
-  nh->get_parameter("sensorOffsetX", sensorOffsetX);
-  nh->get_parameter("sensorOffsetY", sensorOffsetY);
   nh->get_parameter("twoWayDrive", twoWayDrive);
   nh->get_parameter("laserVoxelSize", laserVoxelSize);
   nh->get_parameter("terrainVoxelSize", terrainVoxelSize);
@@ -655,7 +649,7 @@ int main(int argc, char** argv)
   nh->get_parameter("goalX", goalX);
   nh->get_parameter("goalY", goalY);
 
-  auto subOdometry = nh->create_subscription<nav_msgs::msg::Odometry>("/state_estimation", 5, odometryHandler);
+  auto subOdometry = nh->create_subscription<nav_msgs::msg::Odometry>("/odom_base_link", 5, odometryHandler);
 
   auto subLaserCloud = nh->create_subscription<sensor_msgs::msg::PointCloud2>("/registered_scan", 5, laserCloudHandler);
 
@@ -1061,7 +1055,7 @@ int main(int argc, char** argv)
           }
 
           path.header.stamp = rclcpp::Time(static_cast<uint64_t>(odomTime * 1e9));
-          path.header.frame_id = "vehicle";
+          path.header.frame_id = "base_link";
           pubPath->publish(path);
 
           #if PLOTPATHSET == 1
@@ -1107,7 +1101,7 @@ int main(int argc, char** argv)
           sensor_msgs::msg::PointCloud2 freePaths2;
           pcl::toROSMsg(*freePaths, freePaths2);
           freePaths2.header.stamp = rclcpp::Time(static_cast<uint64_t>(odomTime * 1e9));
-          freePaths2.header.frame_id = "vehicle";
+          freePaths2.header.frame_id = "base_link";
           pubFreePaths->publish(freePaths2);
           #endif
         }
@@ -1137,7 +1131,7 @@ int main(int argc, char** argv)
         path.poses[0].pose.orientation.w = 0;
 
         path.header.stamp = rclcpp::Time(static_cast<uint64_t>(odomTime * 1e9));
-        path.header.frame_id = "vehicle";
+        path.header.frame_id = "base_link";
         pubPath->publish(path);
 
         #if PLOTPATHSET == 1
@@ -1145,7 +1139,7 @@ int main(int argc, char** argv)
         sensor_msgs::msg::PointCloud2 freePaths2;
         pcl::toROSMsg(*freePaths, freePaths2);
         freePaths2.header.stamp = rclcpp::Time(static_cast<uint64_t>(odomTime * 1e9));
-        freePaths2.header.frame_id = "vehicle";
+        freePaths2.header.frame_id = "base_link";
         pubFreePaths->publish(freePaths2);
         #endif
       }
@@ -1153,7 +1147,7 @@ int main(int argc, char** argv)
       /*sensor_msgs::msg::PointCloud2 plannerCloud2;
       pcl::toROSMsg(*plannerCloudCrop, plannerCloud2);
       plannerCloud2.header.stamp = rclcpp::Time(static_cast<uint64_t>(odomTime * 1e9));
-      plannerCloud2.header.frame_id = "vehicle";
+      plannerCloud2.header.frame_id = "base_link";
       pubLaserCloud->publish(plannerCloud2);*/
     }
 
diff --git a/src/base_autonomy/local_planner/src/pathFollower.cpp b/src/base_autonomy/local_planner/src/pathFollower.cpp
index 6805f113..29e6be3f 100644
--- a/src/base_autonomy/local_planner/src/pathFollower.cpp
+++ b/src/base_autonomy/local_planner/src/pathFollower.cpp
@@ -46,8 +46,6 @@ double normalizeAngle(double angle) {
 bool realRobot = false;
 string serialPort = "/dev/ttyACM0";
 int baudrate = 115200;
-double sensorOffsetX = 0;
-double sensorOffsetY = 0;
 int pubSkipNum = 1;
 int pubSkipCount = 0;
 bool twoWayDrive = true;
@@ -133,8 +131,8 @@ void odomHandler(const nav_msgs::msg::Odometry::ConstSharedPtr odomIn)
   vehicleRoll = roll;
   vehiclePitch = pitch;
   vehicleYaw = yaw;
-  vehicleX = odomIn->pose.pose.position.x - cos(yaw) * sensorOffsetX + sin(yaw) * sensorOffsetY;
-  vehicleY = odomIn->pose.pose.position.y - sin(yaw) * sensorOffsetX - cos(yaw) * sensorOffsetY;
+  vehicleX = odomIn->pose.pose.position.x;
+  vehicleY = odomIn->pose.pose.position.y;
   vehicleZ = odomIn->pose.pose.position.z;
 
   if ((fabs(roll) > inclThre * PI / 180.0 || fabs(pitch) > inclThre * PI / 180.0) && useInclToStop) {
@@ -246,8 +244,6 @@ int main(int argc, char** argv)
   nh->declare_parameter<bool>("realRobot", realRobot);
   nh->declare_parameter<string>("serialPort", serialPort);
   nh->declare_parameter<int>("baudrate", baudrate);
-  nh->declare_parameter<double>("sensorOffsetX", sensorOffsetX);
-  nh->declare_parameter<double>("sensorOffsetY", sensorOffsetY);
   nh->declare_parameter<int>("pubSkipNum", pubSkipNum);
   nh->declare_parameter<bool>("twoWayDrive", twoWayDrive);
   nh->declare_parameter<double>("lookAheadDis", lookAheadDis);
@@ -282,8 +278,6 @@ int main(int argc, char** argv)
   nh->get_parameter("realRobot", realRobot);
   nh->get_parameter("serialPort", serialPort);
   nh->get_parameter("baudrate", baudrate);
-  nh->get_parameter("sensorOffsetX", sensorOffsetX);
-  nh->get_parameter("sensorOffsetY", sensorOffsetY);
   nh->get_parameter("pubSkipNum", pubSkipNum);
   nh->get_parameter("twoWayDrive", twoWayDrive);
   nh->get_parameter("lookAheadDis", lookAheadDis);
@@ -315,7 +309,7 @@ int main(int argc, char** argv)
   nh->get_parameter("joyToSpeedDelay", joyToSpeedDelay);
   nh->get_parameter("goalYawGain", goalYawGain);
 
-  auto subOdom = nh->create_subscription<nav_msgs::msg::Odometry>("/state_estimation", 5, odomHandler);
+  auto subOdom = nh->create_subscription<nav_msgs::msg::Odometry>("/odom_base_link", 5, odomHandler);
 
   auto subPath = nh->create_subscription<nav_msgs::msg::Path>("/path", 5, pathHandler);
 
@@ -327,9 +321,9 @@ int main(int argc, char** argv)
 
   auto subSlowDown = nh->create_subscription<std_msgs::msg::Int8>("/slow_down", 5, slowDownHandler);
 
-  auto pubSpeed = nh->create_publisher<geometry_msgs::msg::TwistStamped>("/cmd_vel", 5);
+  auto pubSpeed = nh->create_publisher<geometry_msgs::msg::TwistStamped>("/navigation_cmd_vel", 5);
   geometry_msgs::msg::TwistStamped cmd_vel;
-  cmd_vel.header.frame_id = "vehicle";
+  cmd_vel.header.frame_id = "base_link";
 
   if (autonomyMode) {
     joySpeed = autonomySpeed / maxSpeed;
diff --git a/src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch.py
index fc78986c..4c96c032 100644
--- a/src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch.py
+++ b/src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch.py
@@ -86,6 +86,12 @@ def generate_launch_description():
   		}]
   )
 
+  start_cmd_vel_mux = IncludeLaunchDescription(
+    PythonLaunchDescriptionSource(os.path.join(
+      get_package_share_directory('cmd_vel_mux'), 'launch', 'cmd_vel_mux-launch.py')
+    )
+  )
+
   ld = LaunchDescription()
 
   # Add the actions
@@ -105,4 +111,6 @@ def generate_launch_description():
   ld.add_action(start_visualization_tools)
   ld.add_action(start_joy)
 
+  ld.add_action(start_cmd_vel_mux)
+
   return ld
diff --git a/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch.py
index 9f8328aa..b558a940 100644
--- a/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch.py
+++ b/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch.py
@@ -96,6 +96,12 @@ def generate_launch_description():
     }.items()
   )
 
+  start_cmd_vel_mux = IncludeLaunchDescription(
+    PythonLaunchDescriptionSource(os.path.join(
+      get_package_share_directory('cmd_vel_mux'), 'launch', 'cmd_vel_mux-launch.py')
+    )
+  )
+
   ld = LaunchDescription()
 
   # Add the actions
@@ -117,4 +123,6 @@ def generate_launch_description():
   ld.add_action(start_joy)
   ld.add_action(start_tare_planner)
 
+  ld.add_action(start_cmd_vel_mux)
+
   return ld
diff --git a/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_route_planner.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_route_planner.launch.py
index 1c3d3d15..a521e74e 100644
--- a/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_route_planner.launch.py
+++ b/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_route_planner.launch.py
@@ -114,6 +114,12 @@ def generate_launch_description():
     condition=IfCondition(use_pct_planner)
   )
 
+  start_cmd_vel_mux = IncludeLaunchDescription(
+    PythonLaunchDescriptionSource(os.path.join(
+      get_package_share_directory('cmd_vel_mux'), 'launch', 'cmd_vel_mux-launch.py')
+    )
+  )
+
   ld = LaunchDescription()
 
   # Add the actions
@@ -137,4 +143,6 @@ def generate_launch_description():
   ld.add_action(start_far_planner)
   ld.add_action(start_pct_planner)
 
+  ld.add_action(start_cmd_vel_mux)
+
   return ld
diff --git a/src/base_autonomy/vehicle_simulator/launch/system_real_robot.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_real_robot.launch.py
index 38fbf22c..40739917 100644
--- a/src/base_autonomy/vehicle_simulator/launch/system_real_robot.launch.py
+++ b/src/base_autonomy/vehicle_simulator/launch/system_real_robot.launch.py
@@ -91,6 +91,12 @@ def generate_launch_description():
       [get_package_share_directory('livox_ros_driver2'), '/launch_ROS2/msg_MID360_launch.py']),
   )
 
+  start_cmd_vel_mux = IncludeLaunchDescription(
+    PythonLaunchDescriptionSource(os.path.join(
+      get_package_share_directory('cmd_vel_mux'), 'launch', 'cmd_vel_mux-launch.py')
+    )
+  )
+
   ld = LaunchDescription()
 
   # Add the actions
@@ -111,4 +117,6 @@ def generate_launch_description():
   ld.add_action(start_joy)
   ld.add_action(start_mid360)
 
+  ld.add_action(start_cmd_vel_mux)
+
   return ld
diff --git a/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_exploration_planner.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_exploration_planner.launch.py
index 0a554c86..94b863f3 100644
--- a/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_exploration_planner.launch.py
+++ b/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_exploration_planner.launch.py
@@ -101,6 +101,12 @@ def generate_launch_description():
       [get_package_share_directory('livox_ros_driver2'), '/launch_ROS2/msg_MID360_launch.py']),
   )
 
+  start_cmd_vel_mux = IncludeLaunchDescription(
+    PythonLaunchDescriptionSource(os.path.join(
+      get_package_share_directory('cmd_vel_mux'), 'launch', 'cmd_vel_mux-launch.py')
+    )
+  )
+
   ld = LaunchDescription()
 
   # Add the actions
@@ -123,4 +129,6 @@ def generate_launch_description():
   ld.add_action(start_tare_planner)
   ld.add_action(start_mid360)
 
+  ld.add_action(start_cmd_vel_mux)
+
   return ld
diff --git a/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_route_planner.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_route_planner.launch.py
index bf394c7a..c483113a 100644
--- a/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_route_planner.launch.py
+++ b/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_route_planner.launch.py
@@ -119,6 +119,12 @@ def generate_launch_description():
       [get_package_share_directory('livox_ros_driver2'), '/launch_ROS2/msg_MID360_launch.py']),
   )
 
+  start_cmd_vel_mux = IncludeLaunchDescription(
+    PythonLaunchDescriptionSource(os.path.join(
+      get_package_share_directory('cmd_vel_mux'), 'launch', 'cmd_vel_mux-launch.py')
+    )
+  )
+
   ld = LaunchDescription()
 
   # Add the actions
@@ -143,4 +149,6 @@ def generate_launch_description():
   ld.add_action(start_pct_planner)
   ld.add_action(start_mid360)
 
+  ld.add_action(start_cmd_vel_mux)
+
   return ld
diff --git a/src/base_autonomy/vehicle_simulator/launch/system_simulation.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_simulation.launch.py
index 7da08415..3b8f2245 100644
--- a/src/base_autonomy/vehicle_simulator/launch/system_simulation.launch.py
+++ b/src/base_autonomy/vehicle_simulator/launch/system_simulation.launch.py
@@ -119,6 +119,12 @@ def generate_launch_description():
   		}]
   )
 
+  start_cmd_vel_mux = IncludeLaunchDescription(
+    PythonLaunchDescriptionSource(os.path.join(
+      get_package_share_directory('cmd_vel_mux'), 'launch', 'cmd_vel_mux-launch.py')
+    )
+  )
+
   ld = LaunchDescription()
 
   # Add the actions
@@ -141,4 +147,6 @@ def generate_launch_description():
   ld.add_action(start_visualization_tools)
   ld.add_action(start_joy)
 
+  ld.add_action(start_cmd_vel_mux)
+
   return ld
diff --git a/src/base_autonomy/vehicle_simulator/launch/system_simulation_with_exploration_planner.launch.py b/src/base_autonomy/vehicle_simulator/launch/system_simulation_with_exploration_planner.launch.py
index 6725f501..4800757a 100644
--- a/src/base_autonomy/vehicle_simulator/launch/system_simulation_with_exploration_planner.launch.py
+++ b/src/base_autonomy/vehicle_simulator/launch/system_simulation_with_exploration_planner.launch.py
@@ -129,6 +129,12 @@ def generate_launch_description():
     }.items()
   )
 
+  start_cmd_vel_mux = IncludeLaunchDescription(
+    PythonLaunchDescriptionSource(os.path.join(
+      get_package_share_directory('cmd_vel_mux'), 'launch', 'cmd_vel_mux-launch.py')
+    )
+  )
+
   ld = LaunchDescription()
 
   # Add the actions
@@ -153,4 +159,6 @@ def generate_launch_description():
   ld.add_action(start_joy)
   ld.add_action(start_tare_planner)
 
+  ld.add_action(start_cmd_vel_mux)
+
   return ld
diff --git a/src/base_autonomy/vehicle_simulator/package.xml b/src/base_autonomy/vehicle_simulator/package.xml
index 6fbb4732..6733ff1f 100644
--- a/src/base_autonomy/vehicle_simulator/package.xml
+++ b/src/base_autonomy/vehicle_simulator/package.xml
@@ -20,6 +20,8 @@
   <depend>ros_tcp_endpoint</depend>
   <depend>cv_bridge</depend>
 
+  <exec_depend>cmd_vel_mux</exec_depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
diff --git a/src/base_autonomy/vehicle_simulator/rviz/vehicle_simulator.rviz b/src/base_autonomy/vehicle_simulator/rviz/vehicle_simulator.rviz
index d3341823..2dbe581a 100644
--- a/src/base_autonomy/vehicle_simulator/rviz/vehicle_simulator.rviz
+++ b/src/base_autonomy/vehicle_simulator/rviz/vehicle_simulator.rviz
@@ -33,7 +33,7 @@ Visualization Manager:
       Length: 1.5
       Name: Vehicle
       Radius: 0.20000000298023224
-      Reference Frame: vehicle
+      Reference Frame: base_link
       Value: true
     - Class: rviz_default_plugins/Image
       Enabled: true
diff --git a/src/base_autonomy/visualization_tools/launch/visualization_tools.launch b/src/base_autonomy/visualization_tools/launch/visualization_tools.launch
index 1fc04423..7e9686b1 100755
--- a/src/base_autonomy/visualization_tools/launch/visualization_tools.launch
+++ b/src/base_autonomy/visualization_tools/launch/visualization_tools.launch
@@ -3,10 +3,10 @@
   <arg name="world_name" default="garage"/>
 
   <node pkg="visualization_tools" exec="visualizationTools" name="visualizationTools" output="screen">
-    <param name="metricFile" value="$(find-pkg-prefix vehicle_simulator)/log/metrics" />
-    <param name="trajFile" value="$(find-pkg-prefix vehicle_simulator)/log/trajectory" />
-    <param name="pcdFile" value="$(find-pkg-prefix vehicle_simulator)/log/pointcloud" />
-    <param name="mapFile" value="$(find-pkg-prefix vehicle_simulator)/mesh/$(var world_name)/map.ply" />
+    <param name="metricFile" value="$(find-pkg-share vehicle_simulator)/log/metrics" />
+    <param name="trajFile" value="$(find-pkg-share vehicle_simulator)/log/trajectory" />
+    <param name="pcdFile" value="$(find-pkg-share vehicle_simulator)/log/pointcloud" />
+    <param name="mapFile" value="$(find-pkg-share vehicle_simulator)/mesh/$(var world_name)/map.ply" />
     <param name="overallMapVoxelSize" value="0.1" />
     <param name="exploredAreaVoxelSize" value="0.05" />
     <param name="exploredVolumeVoxelSize" value="1.0" />
diff --git a/src/base_autonomy/visualization_tools/src/visualizationTools.cpp b/src/base_autonomy/visualization_tools/src/visualizationTools.cpp
index 68a5b59b..9c37ab46 100644
--- a/src/base_autonomy/visualization_tools/src/visualizationTools.cpp
+++ b/src/base_autonomy/visualization_tools/src/visualizationTools.cpp
@@ -404,11 +404,6 @@ int main(int argc, char** argv)
   nh->get_parameter("saveTraj", saveTraj);
   nh->get_parameter("savePcd", savePcd);
 
-  // No direct replacement present for $(find pkg) in ROS2. Edit file path.
-  mapFile.replace(mapFile.find("/install/"), 8, "/src/base_autonomy");
-  metricFile.replace(metricFile.find("/install/"), 8, "/src/base_autonomy");
-  trajFile.replace(trajFile.find("/install/"), 8, "/src/base_autonomy");
-  pcdFile.replace(pcdFile.find("/install/"), 8, "/src/base_autonomy");
 
   auto subOdometry = nh->create_subscription<nav_msgs::msg::Odometry>("/state_estimation", 5, odometryHandler);
 
diff --git a/src/route_planner/far_planner/launch/far_planner.launch b/src/route_planner/far_planner/launch/far_planner.launch
index d33606c0..77db6d60 100644
--- a/src/route_planner/far_planner/launch/far_planner.launch
+++ b/src/route_planner/far_planner/launch/far_planner.launch
@@ -27,7 +27,7 @@ def generate_launch_description():
                 '.yaml"'])
             ],
             remappings=[
-                ('/odom_world', '/state_estimation'),
+                ('/odom_world', '/odom_base_link'),
                 ('/terrain_cloud', '/terrain_map_ext'),
                 ('/scan_cloud', '/terrain_map'),
                 ('/terrain_local_cloud', '/registered_scan')
diff --git a/src/utilities/cmd_vel_mux/CMakeLists.txt b/src/utilities/cmd_vel_mux/CMakeLists.txt
new file mode 100644
index 00000000..a426a08e
--- /dev/null
+++ b/src/utilities/cmd_vel_mux/CMakeLists.txt
@@ -0,0 +1,64 @@
+cmake_minimum_required(VERSION 3.5)
+project(cmd_vel_mux)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rcl_interfaces REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(rcpputils REQUIRED)
+find_package(std_msgs REQUIRED)
+
+add_library(${PROJECT_NAME} SHARED src/cmd_vel_mux.cpp)
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+ament_target_dependencies(${PROJECT_NAME}
+  geometry_msgs
+  rcl_interfaces
+  rclcpp
+  rclcpp_components
+  rcpputils
+  std_msgs
+)
+
+add_executable(cmd_vel_mux_node src/cmd_vel_mux_node.cpp)
+target_link_libraries(cmd_vel_mux_node ${PROJECT_NAME})
+ament_target_dependencies(cmd_vel_mux_node rclcpp)
+
+rclcpp_components_register_nodes(${PROJECT_NAME}
+  "cmd_vel_mux::CmdVelMux")
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(TARGETS cmd_vel_mux_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION include/${PROJECT_NAME}
+)
+
+install(DIRECTORY config launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/src/utilities/cmd_vel_mux/README.md b/src/utilities/cmd_vel_mux/README.md
new file mode 100644
index 00000000..e7f8ed17
--- /dev/null
+++ b/src/utilities/cmd_vel_mux/README.md
@@ -0,0 +1,33 @@
+# cmd_vel_mux
+
+Velocity command multiplexer for ROS2. Arbitrates incoming velocity commands from multiple topics based on configurable priorities. Supports both `geometry_msgs/msg/Twist` and `geometry_msgs/msg/TwistStamped` per-subscriber and on the output. Includes an emergency stop that immediately zeroes output.
+
+## Topics
+
+**Published:**
+- `cmd_vel` (`TwistStamped` or `Twist`, controlled by `output_stamped` parameter) — Highest-priority velocity command
+- `active` (`std_msgs/msg/String`) — Name of the currently active input, or "idle"
+
+**Subscribed:**
+- `/stop` (`std_msgs/msg/Int8`) — Value >= 1 immediately zeroes output and blocks all input; 0 resumes
+- Per-subscriber input topics configured via parameters, each independently `Twist` or `TwistStamped`
+
+## Parameters
+
+**Node-level:**
+- `output_stamped` (bool, default `true`) — Publish `TwistStamped` (with timestamp and frame "vehicle") or plain `Twist`
+
+**Per-subscriber** under `subscribers.<name>`:
+- `topic` (string) — Input topic name
+- `timeout` (double) — Seconds of silence before input is deactivated
+- `priority` (int) — Higher value = higher priority, must be unique
+- `short_desc` (string) — Human-readable label
+- `stamped` (bool, default `false`) — Whether the input topic is `TwistStamped` or `Twist`
+
+See `config/cmd_vel_mux_params.yaml` for an example configuration.
+
+## Usage
+
+```bash
+ros2 launch cmd_vel_mux cmd_vel_mux-launch.py
+```
diff --git a/src/utilities/cmd_vel_mux/config/cmd_vel_mux_params.yaml b/src/utilities/cmd_vel_mux/config/cmd_vel_mux_params.yaml
new file mode 100644
index 00000000..7c9107ea
--- /dev/null
+++ b/src/utilities/cmd_vel_mux/config/cmd_vel_mux_params.yaml
@@ -0,0 +1,28 @@
+cmd_vel_mux:
+  ros__parameters:
+    output_stamped: true
+    subscribers:
+        navigation:
+          topic:       "/navigation_cmd_vel"
+          timeout:     0.5
+          priority:    3
+          short_desc:  "Autonomous navigation stack"
+          stamped:     true
+        safety:
+          topic:       "/safety_cmd_vel"
+          timeout:     0.1
+          priority:    8
+          short_desc:  "Safety controller / obstacle avoidance"
+          stamped:     true
+        local_movement:
+          topic:       "/local_movement_cmd_vel"
+          timeout:     0.3
+          priority:    5
+          short_desc:  "Local precise movements"
+          stamped:     true
+        teleop:
+          topic:       "/teleop_cmd_vel"
+          timeout:     0.1
+          priority:    10
+          short_desc:  "Teleoperation / joystick"
+          stamped:     true
\ No newline at end of file
diff --git a/src/utilities/cmd_vel_mux/include/cmd_vel_mux/cmd_vel_mux.hpp b/src/utilities/cmd_vel_mux/include/cmd_vel_mux/cmd_vel_mux.hpp
new file mode 100644
index 00000000..9f483551
--- /dev/null
+++ b/src/utilities/cmd_vel_mux/include/cmd_vel_mux/cmd_vel_mux.hpp
@@ -0,0 +1,97 @@
+#ifndef CMD_VEL_MUX__CMD_VEL_MUX_HPP_
+#define CMD_VEL_MUX__CMD_VEL_MUX_HPP_
+
+#include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <rcl_interfaces/msg/set_parameters_result.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/int8.hpp>
+#include <std_msgs/msg/string.hpp>
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace cmd_vel_mux
+{
+
+struct ParameterValues
+{
+  std::string topic;
+  double timeout{-1.0};
+  int64_t priority{-1};
+  std::string short_desc;
+  bool stamped{false};
+};
+
+inline bool operator==(const ParameterValues & p1, const ParameterValues & p2)
+{
+  return p1.topic == p2.topic &&
+         p1.timeout == p2.timeout &&
+         p1.priority == p2.priority &&
+         p1.short_desc == p2.short_desc &&
+         p1.stamped == p2.stamped;
+}
+
+class CmdVelMux final : public rclcpp::Node
+{
+public:
+  explicit CmdVelMux(rclcpp::NodeOptions options);
+  ~CmdVelMux() override = default;
+  CmdVelMux(CmdVelMux && c) = delete;
+  CmdVelMux & operator=(CmdVelMux && c) = delete;
+  CmdVelMux(const CmdVelMux & c) = delete;
+  CmdVelMux & operator=(const CmdVelMux & c) = delete;
+
+private:
+  static const char * const VACANT;
+
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_stamped_pub_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr active_subscriber_pub_;
+  rclcpp::Subscription<std_msgs::msg::Int8>::SharedPtr stop_sub_;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_;
+
+  std::string allowed_;
+  int8_t stop_level_{0};
+  bool output_stamped_{true};
+
+  void publishTwist(const geometry_msgs::msg::Twist & twist);
+  void publishZero();
+
+  void timerCallback(const std::string & key);
+  void cmdVelCallback(
+    const geometry_msgs::msg::Twist & twist,
+    const std::string & key);
+  void stopCallback(const std_msgs::msg::Int8::SharedPtr msg);
+
+  rcl_interfaces::msg::SetParametersResult parameterUpdate(
+    const std::vector<rclcpp::Parameter> & update_parameters);
+
+  struct CmdVelSub final
+  {
+    std::string name_;
+    ParameterValues values_;
+    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_sub_;
+    rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_stamped_sub_;
+    rclcpp::TimerBase::SharedPtr timer_;
+  };
+
+  bool addInputToParameterMap(
+    std::map<std::string, ParameterValues> & parsed_parameters,
+    const std::string & input_name, const std::string & parameter_name,
+    const rclcpp::Parameter & parameter_value);
+  bool parametersAreValid(
+    const std::map<std::string, ParameterValues> & parameters) const;
+  void configureFromParameters(
+    const std::map<std::string, ParameterValues> & parameters);
+  std::map<std::string, ParameterValues> parseFromParametersMap(
+    const std::map<std::string, rclcpp::Parameter> & parameters);
+
+  std::map<std::string, std::shared_ptr<CmdVelSub>> map_;
+};
+
+}  // namespace cmd_vel_mux
+
+#endif  // CMD_VEL_MUX__CMD_VEL_MUX_HPP_
diff --git a/src/utilities/cmd_vel_mux/launch/cmd_vel_mux-launch.py b/src/utilities/cmd_vel_mux/launch/cmd_vel_mux-launch.py
new file mode 100644
index 00000000..b7c20873
--- /dev/null
+++ b/src/utilities/cmd_vel_mux/launch/cmd_vel_mux-launch.py
@@ -0,0 +1,23 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    params_file = os.path.join(
+        get_package_share_directory('cmd_vel_mux'),
+        'config',
+        'cmd_vel_mux_params.yaml'
+    )
+
+    return LaunchDescription([
+        Node(
+            package='cmd_vel_mux',
+            executable='cmd_vel_mux_node',
+            name='cmd_vel_mux',
+            output='screen',
+            parameters=[params_file],
+        ),
+    ])
diff --git a/src/utilities/cmd_vel_mux/package.xml b/src/utilities/cmd_vel_mux/package.xml
new file mode 100644
index 00000000..413efed9
--- /dev/null
+++ b/src/utilities/cmd_vel_mux/package.xml
@@ -0,0 +1,26 @@
+<package format="3">
+  <name>cmd_vel_mux</name>
+  <version>1.0.0</version>
+  <description>
+    Multiplexer for geometry_msgs/Twist velocity commands.
+    Arbitrates multiple input topics based on priority, with timeout-based fallback and emergency stop.
+  </description>
+  <maintainer email="alex@vectorrobotics.com">Alex Lin</maintainer>
+  <license>BSD</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>rcl_interfaces</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>rcpputils</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/utilities/cmd_vel_mux/src/cmd_vel_mux.cpp b/src/utilities/cmd_vel_mux/src/cmd_vel_mux.cpp
new file mode 100644
index 00000000..227ae255
--- /dev/null
+++ b/src/utilities/cmd_vel_mux/src/cmd_vel_mux.cpp
@@ -0,0 +1,412 @@
+#include "cmd_vel_mux/cmd_vel_mux.hpp"
+
+#include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <rcl_interfaces/msg/set_parameters_result.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_components/register_node_macro.hpp>
+#include <rcpputils/split.hpp>
+#include <std_msgs/msg/int8.hpp>
+#include <std_msgs/msg/string.hpp>
+
+#include <chrono>
+#include <functional>
+#include <limits>
+#include <map>
+#include <memory>
+#include <set>
+#include <stdexcept>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace cmd_vel_mux
+{
+
+const char * const CmdVelMux::VACANT = "empty";
+
+CmdVelMux::CmdVelMux(rclcpp::NodeOptions options)
+: rclcpp::Node("cmd_vel_mux", options.allow_undeclared_parameters(
+      true).automatically_declare_parameters_from_overrides(true)), allowed_(VACANT)
+{
+  if (!this->has_parameter("output_stamped")) {
+    this->declare_parameter<bool>("output_stamped", true);
+  }
+  this->get_parameter("output_stamped", output_stamped_);
+
+  std::map<std::string, rclcpp::Parameter> parameters;
+  if (!get_parameters("subscribers", parameters) || parameters.size() < 1) {
+    RCLCPP_WARN(get_logger(), "No subscribers configured!");
+  } else {
+    std::map<std::string, ParameterValues> parsed_parameters = parseFromParametersMap(parameters);
+    if (parsed_parameters.empty()) {
+      throw std::runtime_error("Invalid parameters");
+    }
+    if (!parametersAreValid(parsed_parameters)) {
+      throw std::runtime_error("Incomplete parameters");
+    }
+    configureFromParameters(parsed_parameters);
+  }
+
+  param_cb_ =
+    add_on_set_parameters_callback(std::bind(&CmdVelMux::parameterUpdate, this,
+      std::placeholders::_1));
+
+  if (output_stamped_) {
+    twist_stamped_pub_ = this->create_publisher<geometry_msgs::msg::TwistStamped>("cmd_vel", 10);
+  } else {
+    twist_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);
+  }
+
+  active_subscriber_pub_ = this->create_publisher<std_msgs::msg::String>(
+    "active", rclcpp::QoS(1).transient_local());
+
+  stop_sub_ = this->create_subscription<std_msgs::msg::Int8>(
+    "/stop", 10,
+    [this](const std_msgs::msg::Int8::SharedPtr msg) { stopCallback(msg); });
+
+  auto active_msg = std::make_unique<std_msgs::msg::String>();
+  active_msg->data = "idle";
+  active_subscriber_pub_->publish(std::move(active_msg));
+
+  RCLCPP_INFO(get_logger(), "CmdVelMux : initialized (output_stamped=%s)",
+    output_stamped_ ? "true" : "false");
+}
+
+void CmdVelMux::publishTwist(const geometry_msgs::msg::Twist & twist)
+{
+  if (output_stamped_) {
+    auto msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
+    msg->header.stamp = this->now();
+    msg->header.frame_id = "vehicle";
+    msg->twist = twist;
+    twist_stamped_pub_->publish(std::move(msg));
+  } else {
+    twist_pub_->publish(twist);
+  }
+}
+
+void CmdVelMux::publishZero()
+{
+  geometry_msgs::msg::Twist zero;
+  publishTwist(zero);
+}
+
+bool CmdVelMux::parametersAreValid(const std::map<std::string, ParameterValues> & parameters) const
+{
+  std::set<int64_t> used_priorities;
+
+  for (const auto & parameter : parameters) {
+    if (parameter.second.topic.empty()) {
+      RCLCPP_WARN(get_logger(), "Empty topic for '%s'", parameter.first.c_str());
+      return false;
+    }
+    if (parameter.second.timeout < 0.0) {
+      RCLCPP_WARN(get_logger(), "Missing timeout for '%s', ignoring", parameter.first.c_str());
+      return false;
+    }
+    if (parameter.second.priority < 0) {
+      RCLCPP_WARN(get_logger(), "Missing priority for '%s', ignoring", parameter.first.c_str());
+      return false;
+    }
+    if (parameter.second.short_desc.empty()) {
+      RCLCPP_WARN(get_logger(), "Empty short_desc for '%s', ignoring", parameter.first.c_str());
+      return false;
+    }
+
+    if (used_priorities.count(parameter.second.priority) != 0) {
+      RCLCPP_WARN(get_logger(), "Cannot have duplicate priorities, ignoring");
+      return false;
+    }
+    used_priorities.insert(parameter.second.priority);
+  }
+
+  return true;
+}
+
+void CmdVelMux::configureFromParameters(const std::map<std::string, ParameterValues> & parameters)
+{
+  std::map<std::string, std::shared_ptr<CmdVelSub>> new_map;
+
+  for (const auto & parameter : parameters) {
+    const std::string & key = parameter.first;
+    const ParameterValues & parameter_values = parameter.second;
+    if (map_.count(key) != 0) {
+      new_map[key] = map_[key];
+    } else {
+      new_map[key] = std::make_shared<CmdVelSub>();
+    }
+
+    new_map[key]->name_ = key;
+    new_map[key]->values_.priority = parameter_values.priority;
+    new_map[key]->values_.short_desc = parameter_values.short_desc;
+
+    bool topic_changed = parameter_values.topic != new_map[key]->values_.topic;
+    bool stamped_changed = parameter_values.stamped != new_map[key]->values_.stamped;
+
+    if (topic_changed || stamped_changed) {
+      new_map[key]->values_.topic = parameter_values.topic;
+      new_map[key]->values_.stamped = parameter_values.stamped;
+      new_map[key]->twist_sub_ = nullptr;
+      new_map[key]->twist_stamped_sub_ = nullptr;
+    }
+
+    if (parameter_values.timeout != new_map[key]->values_.timeout) {
+      new_map[key]->values_.timeout = parameter_values.timeout;
+      new_map[key]->timer_ = nullptr;
+    }
+  }
+
+  if (allowed_ != VACANT && new_map.count(allowed_) == 0) {
+    allowed_ = VACANT;
+    auto active_msg = std::make_unique<std_msgs::msg::String>();
+    active_msg->data = "idle";
+    active_subscriber_pub_->publish(std::move(active_msg));
+  }
+
+  map_ = new_map;
+
+  for (auto & [key, values] : map_) {
+    bool needs_sub = !values->twist_sub_ && !values->twist_stamped_sub_;
+    if (needs_sub) {
+      if (values->values_.stamped) {
+        values->twist_stamped_sub_ =
+          this->create_subscription<geometry_msgs::msg::TwistStamped>(
+            values->values_.topic, 10,
+            [this, key](const geometry_msgs::msg::TwistStamped::SharedPtr msg) {
+              cmdVelCallback(msg->twist, key);
+            });
+      } else {
+        values->twist_sub_ =
+          this->create_subscription<geometry_msgs::msg::Twist>(
+            values->values_.topic, 10,
+            [this, key](const geometry_msgs::msg::Twist::SharedPtr msg) {
+              cmdVelCallback(*msg, key);
+            });
+      }
+      RCLCPP_DEBUG(get_logger(), "CmdVelMux : subscribed to '%s' on topic '%s' (stamped=%s)",
+        values->name_.c_str(), values->values_.topic.c_str(),
+        values->values_.stamped ? "true" : "false");
+    }
+
+    if (!values->timer_) {
+      values->timer_ =
+        this->create_wall_timer(std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::
+          duration<double>(values->values_.timeout)), [this, key]() {timerCallback(key);});
+    }
+  }
+
+  RCLCPP_INFO(get_logger(), "CmdVelMux : (re)configured");
+}
+
+bool CmdVelMux::addInputToParameterMap(
+  std::map<std::string, ParameterValues> & parsed_parameters,
+  const std::string & input_name,
+  const std::string & parameter_name,
+  const rclcpp::Parameter & parameter_value)
+{
+  if (parsed_parameters.count(input_name) == 0) {
+    parsed_parameters.emplace(std::make_pair(input_name, ParameterValues()));
+  }
+
+  if (parameter_name == "topic") {
+    if (parameter_value.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) {
+      parsed_parameters[input_name].topic.clear();
+    } else if (parameter_value.get_type() == rclcpp::ParameterType::PARAMETER_STRING) {
+      parsed_parameters[input_name].topic = parameter_value.as_string();
+    } else {
+      RCLCPP_WARN(get_logger(), "topic must be a string; ignoring");
+      return false;
+    }
+  } else if (parameter_name == "timeout") {
+    if (parameter_value.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) {
+      parsed_parameters[input_name].timeout = -1.0;
+    } else if (parameter_value.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) {
+      parsed_parameters[input_name].timeout = parameter_value.as_double();
+    } else {
+      RCLCPP_WARN(get_logger(), "timeout must be a double; ignoring");
+      return false;
+    }
+  } else if (parameter_name == "priority") {
+    if (parameter_value.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) {
+      parsed_parameters[input_name].priority = -1;
+    } else if (parameter_value.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) {
+      int64_t priority = parameter_value.as_int();
+      if (priority < 0 || priority > std::numeric_limits<uint32_t>::max()) {
+        RCLCPP_WARN(get_logger(), "Priority out of range, must be between 0 and MAX_UINT32");
+        return false;
+      }
+      parsed_parameters[input_name].priority = priority;
+    } else {
+      RCLCPP_WARN(get_logger(), "priority must be an integer; ignoring");
+      return false;
+    }
+  } else if (parameter_name == "short_desc") {
+    if (parameter_value.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) {
+      parsed_parameters[input_name].short_desc.clear();
+    } else if (parameter_value.get_type() == rclcpp::ParameterType::PARAMETER_STRING) {
+      parsed_parameters[input_name].short_desc = parameter_value.as_string();
+    } else {
+      RCLCPP_WARN(get_logger(), "short_desc must be a string; ignoring");
+      return false;
+    }
+  } else if (parameter_name == "stamped") {
+    if (parameter_value.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) {
+      parsed_parameters[input_name].stamped = false;
+    } else if (parameter_value.get_type() == rclcpp::ParameterType::PARAMETER_BOOL) {
+      parsed_parameters[input_name].stamped = parameter_value.as_bool();
+    } else {
+      RCLCPP_WARN(get_logger(), "stamped must be a bool; ignoring");
+      return false;
+    }
+  } else {
+    RCLCPP_WARN(get_logger(), "Invalid input variable '%s'; ignored", parameter_name.c_str());
+    return false;
+  }
+
+  return true;
+}
+
+std::map<std::string, ParameterValues> CmdVelMux::parseFromParametersMap(
+  const std::map<std::string,
+  rclcpp::Parameter> & parameters)
+{
+  std::map<std::string, ParameterValues> parsed_parameters;
+  for (const auto & parameter : parameters) {
+    std::vector<std::string> splits = rcpputils::split(parameter.first, '.');
+    if (splits.size() != 2) {
+      RCLCPP_WARN(get_logger(), "Invalid or unknown parameter '%s', ignoring",
+        parameter.first.c_str());
+      continue;
+    }
+
+    const std::string & input_name = splits[0];
+    const std::string & parameter_name = splits[1];
+    const rclcpp::Parameter & parameter_value = parameter.second;
+    if (!addInputToParameterMap(parsed_parameters, input_name, parameter_name, parameter_value)) {
+      parsed_parameters.clear();
+      break;
+    }
+  }
+
+  return parsed_parameters;
+}
+
+void CmdVelMux::cmdVelCallback(
+  const geometry_msgs::msg::Twist & twist,
+  const std::string & key)
+{
+  if (stop_level_ >= 1) {
+    return;
+  }
+
+  if (map_.count(key) == 0) {
+    return;
+  }
+
+  map_[key]->timer_->reset();
+
+  if ((allowed_ == VACANT) ||
+    (allowed_ == key) ||
+    (map_[key]->values_.priority > map_[allowed_]->values_.priority))
+  {
+    if (allowed_ != key) {
+      allowed_ = key;
+
+      auto active_msg = std::make_unique<std_msgs::msg::String>();
+      active_msg->data = map_[key]->name_;
+      active_subscriber_pub_->publish(std::move(active_msg));
+    }
+
+    publishTwist(twist);
+  }
+}
+
+void CmdVelMux::stopCallback(const std_msgs::msg::Int8::SharedPtr msg)
+{
+  int8_t prev = stop_level_;
+  stop_level_ = msg->data;
+  if (stop_level_ >= 1 && prev < 1) {
+    publishZero();
+    RCLCPP_WARN(get_logger(), "CmdVelMux : STOP level %d, all input cut", stop_level_);
+  } else if (stop_level_ < 1 && prev >= 1) {
+    RCLCPP_INFO(get_logger(), "CmdVelMux : STOP released, resuming");
+  }
+}
+
+void CmdVelMux::timerCallback(const std::string & key)
+{
+  if (allowed_ == key) {
+    allowed_ = VACANT;
+
+    auto active_msg = std::make_unique<std_msgs::msg::String>();
+    active_msg->data = "idle";
+    active_subscriber_pub_->publish(std::move(active_msg));
+  }
+}
+
+rcl_interfaces::msg::SetParametersResult CmdVelMux::parameterUpdate(
+  const std::vector<rclcpp::Parameter> & update_parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+
+  std::map<std::string, rclcpp::Parameter> old_parameters;
+  if (!get_parameters("subscribers", old_parameters) || old_parameters.size() <= 1) {
+    result.successful = false;
+    result.reason = "no parameters loaded";
+    return result;
+  }
+
+  std::map<std::string, ParameterValues> parameters = parseFromParametersMap(old_parameters);
+
+  for (const rclcpp::Parameter & parameter : update_parameters) {
+    std::vector<std::string> splits = rcpputils::split(parameter.get_name(), '.');
+    if (splits.size() != 3) {
+      RCLCPP_WARN(get_logger(), "Invalid or unknown parameter '%s', ignoring",
+        parameter.get_name().c_str());
+      result.successful = false;
+      result.reason = "Invalid or unknown parameter";
+      break;
+    }
+    if (splits[0] != "subscribers") {
+      RCLCPP_WARN(get_logger(), "Unknown parameter prefix '%s', ignoring",
+        parameter.get_name().c_str());
+      result.successful = false;
+      result.reason = "Unknown parameter prefix";
+      break;
+    }
+
+    const std::string & input_name = splits[1];
+    const std::string & parameter_name = splits[2];
+
+    if (!addInputToParameterMap(parameters, input_name, parameter_name, parameter)) {
+      result.successful = false;
+      result.reason = "Invalid parameter";
+      break;
+    }
+  }
+
+  for (auto it = parameters.begin(); it != parameters.end(); ) {
+    if (it->second == ParameterValues()) {
+      it = parameters.erase(it);
+    } else {
+      ++it;
+    }
+  }
+
+  if (result.successful) {
+    if (parametersAreValid(parameters)) {
+      configureFromParameters(parameters);
+    } else {
+      result.successful = false;
+      result.reason = "Incomplete parameters";
+    }
+  }
+
+  return result;
+}
+
+}  // namespace cmd_vel_mux
+
+RCLCPP_COMPONENTS_REGISTER_NODE(cmd_vel_mux::CmdVelMux)
diff --git a/src/utilities/cmd_vel_mux/src/cmd_vel_mux_node.cpp b/src/utilities/cmd_vel_mux/src/cmd_vel_mux_node.cpp
new file mode 100644
index 00000000..d86e8317
--- /dev/null
+++ b/src/utilities/cmd_vel_mux/src/cmd_vel_mux_node.cpp
@@ -0,0 +1,18 @@
+#include "cmd_vel_mux/cmd_vel_mux.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <memory>
+
+int main(int argc, char ** argv)
+{
+  setvbuf(stdout, nullptr, _IONBF, BUFSIZ);
+
+  rclcpp::init(argc, argv);
+  rclcpp::spin(
+    std::make_shared<cmd_vel_mux::CmdVelMux>(
+      rclcpp::NodeOptions()));
+  rclcpp::shutdown();
+
+  return 0;
+}
diff --git a/src/utilities/odom_transformer/CMakeLists.txt b/src/utilities/odom_transformer/CMakeLists.txt
new file mode 100644
index 00000000..c60dbc49
--- /dev/null
+++ b/src/utilities/odom_transformer/CMakeLists.txt
@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.5)
+project(odom_transformer)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
+add_executable(odom_transformer_node src/odom_transformer_node.cpp)
+ament_target_dependencies(odom_transformer_node
+  rclcpp
+  nav_msgs
+  geometry_msgs
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+)
+
+install(TARGETS
+  odom_transformer_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_package()
diff --git a/src/utilities/odom_transformer/package.xml b/src/utilities/odom_transformer/package.xml
new file mode 100644
index 00000000..feba2277
--- /dev/null
+++ b/src/utilities/odom_transformer/package.xml
@@ -0,0 +1,24 @@
+<package format="3">
+  <name>odom_transformer</name>
+  <version>1.0.0</version>
+  <description>
+    Transforms nav_msgs/Odometry from one child frame to another using the TF
+    static transform. Properly handles both pose and twist (including the lever-arm
+    coupling between angular velocity and linear velocity at the new origin).
+  </description>
+  <maintainer email="alex@vectorrobotics.com">Alex Lin</maintainer>
+  <license>BSD</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/utilities/odom_transformer/src/odom_transformer_node.cpp b/src/utilities/odom_transformer/src/odom_transformer_node.cpp
new file mode 100644
index 00000000..835ad4b3
--- /dev/null
+++ b/src/utilities/odom_transformer/src/odom_transformer_node.cpp
@@ -0,0 +1,145 @@
+#include <math.h>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+
+#include "tf2/LinearMath/Transform.h"
+#include "tf2/LinearMath/Matrix3x3.h"
+#include "tf2/LinearMath/Vector3.h"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+// Transforms an Odometry message from one child frame to another.
+//
+// The SLAM output (e.g. /state_estimation) typically reports the pose of the
+// *sensor* in the map frame.  When the sensor is not co-located with the
+// robot's centre of mass (base_link), both the pose and the twist need to be
+// re-expressed for the base_link origin.
+//
+// Pose:  T_map_base = T_map_sensor * T_sensor_base^{-1}
+//
+// Twist (body-frame velocities):
+//   w_base = R * w_sensor
+//   v_base = R * v_sensor  +  arm x w_base
+//
+// where R is the rotation from source to target frame and arm is the position
+// of the source (sensor) origin expressed in the target (base_link) frame,
+// both taken from the static TF.
+
+std::string sourceFrame, targetFrame;
+
+std::shared_ptr<tf2_ros::Buffer> tfBuffer;
+std::shared_ptr<tf2_ros::TransformListener> tfListener;
+rclcpp::TimerBase::SharedPtr tfTimer;
+
+rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubOdom;
+
+tf2::Transform sourceToTargetTf;
+tf2::Matrix3x3 rotation;
+tf2::Vector3 arm;
+bool transformReady = false;
+
+void lookupTransform(rclcpp::Node::SharedPtr nh)
+{
+  try {
+    auto tf = tfBuffer->lookupTransform(targetFrame, sourceFrame, tf2::TimePointZero);
+    tf2::fromMsg(tf.transform, sourceToTargetTf);
+
+    rotation = sourceToTargetTf.getBasis();
+    arm = sourceToTargetTf.getOrigin();
+
+    transformReady = true;
+    RCLCPP_INFO(nh->get_logger(), "Acquired static TF  %s -> %s  (t = [%.3f, %.3f, %.3f])",
+                sourceFrame.c_str(), targetFrame.c_str(),
+                arm.x(), arm.y(), arm.z());
+    tfTimer->cancel();
+  } catch (tf2::TransformException &ex) {
+    RCLCPP_WARN_THROTTLE(nh->get_logger(), *nh->get_clock(), 5000,
+                         "Waiting for TF %s -> %s: %s",
+                         sourceFrame.c_str(), targetFrame.c_str(), ex.what());
+  }
+}
+
+void odomHandler(const nav_msgs::msg::Odometry::ConstSharedPtr msg)
+{
+  if (!transformReady) return;
+
+  nav_msgs::msg::Odometry out;
+  out.header = msg->header;
+  out.child_frame_id = targetFrame;
+
+  // --- Pose: T_map_target = T_map_source * T_source_target^{-1} -----------
+  tf2::Transform sensorInMap;
+  tf2::fromMsg(msg->pose.pose, sensorInMap);
+  tf2::Transform baseInMap = sensorInMap * sourceToTargetTf.inverse();
+  tf2::toMsg(baseInMap, out.pose.pose);
+
+  // Pass through covariance unchanged (rotation-only transform of covariance
+  // can be added later if the source publishes meaningful values).
+  out.pose.covariance = msg->pose.covariance;
+
+  // --- Twist --------------------------------------------------------------
+  // Input twist is the velocity of the source (sensor) in the source frame.
+  // Output twist is the velocity of the target (base_link) in the target frame.
+  tf2::Vector3 vSrc(msg->twist.twist.linear.x,
+                     msg->twist.twist.linear.y,
+                     msg->twist.twist.linear.z);
+  tf2::Vector3 wSrc(msg->twist.twist.angular.x,
+                     msg->twist.twist.angular.y,
+                     msg->twist.twist.angular.z);
+
+  tf2::Vector3 wTgt = rotation * wSrc;
+  tf2::Vector3 vTgt = rotation * vSrc + arm.cross(wTgt);
+
+  out.twist.twist.linear.x  = vTgt.x();
+  out.twist.twist.linear.y  = vTgt.y();
+  out.twist.twist.linear.z  = vTgt.z();
+  out.twist.twist.angular.x = wTgt.x();
+  out.twist.twist.angular.y = wTgt.y();
+  out.twist.twist.angular.z = wTgt.z();
+
+  out.twist.covariance = msg->twist.covariance;
+
+  pubOdom->publish(out);
+}
+
+int main(int argc, char **argv)
+{
+  rclcpp::init(argc, argv);
+  auto nh = rclcpp::Node::make_shared("odom_transformer");
+
+  nh->declare_parameter<std::string>("source_frame", "sensor");
+  nh->declare_parameter<std::string>("target_frame", "base_link");
+  nh->declare_parameter<std::string>("odom_topic_in", "/state_estimation");
+  nh->declare_parameter<std::string>("odom_topic_out", "/odom_base_link");
+
+  nh->get_parameter("source_frame", sourceFrame);
+  nh->get_parameter("target_frame", targetFrame);
+
+  std::string topicIn, topicOut;
+  nh->get_parameter("odom_topic_in", topicIn);
+  nh->get_parameter("odom_topic_out", topicOut);
+
+  tfBuffer = std::make_shared<tf2_ros::Buffer>(nh->get_clock());
+  tfListener = std::make_shared<tf2_ros::TransformListener>(*tfBuffer);
+
+  tfTimer = nh->create_wall_timer(
+      std::chrono::seconds(1),
+      [nh]() { lookupTransform(nh); });
+
+  auto subOdom = nh->create_subscription<nav_msgs::msg::Odometry>(
+      topicIn, 50, odomHandler);
+
+  pubOdom = nh->create_publisher<nav_msgs::msg::Odometry>(topicOut, 50);
+
+  RCLCPP_INFO(nh->get_logger(),
+              "odom_transformer: %s (%s) -> %s (%s)",
+              topicIn.c_str(), sourceFrame.c_str(),
+              topicOut.c_str(), targetFrame.c_str());
+
+  rclcpp::spin(nh);
+  rclcpp::shutdown();
+  return 0;
+}
diff --git a/src/utilities/teleop_joy_controller/src/teleopJoyController.cpp b/src/utilities/teleop_joy_controller/src/teleopJoyController.cpp
index 8a253b5a..01fcf44c 100644
--- a/src/utilities/teleop_joy_controller/src/teleopJoyController.cpp
+++ b/src/utilities/teleop_joy_controller/src/teleopJoyController.cpp
@@ -52,7 +52,7 @@ int main(int argc, char** argv)
 
   auto pubSpeed = nh->create_publisher<geometry_msgs::msg::TwistStamped>("/cmd_vel", 5);
   geometry_msgs::msg::TwistStamped cmd_vel;
-  cmd_vel.header.frame_id = "vehicle";
+  cmd_vel.header.frame_id = "base_link";
 
   serial::Serial *motorCtrSerial = new serial::Serial();
   serial::Timeout timeout(serial::Timeout::simpleTimeout(500));
diff --git a/system_bagfile.sh b/system_bagfile.sh
index 8b8647c9..cb2e9d6c 100755
--- a/system_bagfile.sh
+++ b/system_bagfile.sh
@@ -1,9 +1,10 @@
 #!/bin/bash
 
 SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )"
+WS_DIR="$( cd "$SCRIPT_DIR/../.." && pwd )"
 
-cd $SCRIPT_DIR
-source ./install/setup.bash
+cd "$SCRIPT_DIR"
+source "$WS_DIR/install/setup.bash"
 ros2 launch vehicle_simulator system_bagfile.launch.py &
 sleep 1
 ros2 run rviz2 rviz2 -d src/base_autonomy/vehicle_simulator/rviz/vehicle_simulator.rviz
diff --git a/system_bagfile_with_exploration_planner.sh b/system_bagfile_with_exploration_planner.sh
index fdaaa437..322fda40 100755
--- a/system_bagfile_with_exploration_planner.sh
+++ b/system_bagfile_with_exploration_planner.sh
@@ -1,9 +1,10 @@
 #!/bin/bash
 
 SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )"
+WS_DIR="$( cd "$SCRIPT_DIR/../.." && pwd )"
 
-cd $SCRIPT_DIR
-source ./install/setup.bash
+cd "$SCRIPT_DIR"
+source "$WS_DIR/install/setup.bash"
 ros2 launch vehicle_simulator system_bagfile_with_exploration_planner.launch.py &
 sleep 1
 ros2 run rviz2 rviz2 -d src/exploration_planner/tare_planner/rviz/tare_planner_ground.rviz
diff --git a/system_bagfile_with_route_planner.sh b/system_bagfile_with_route_planner.sh
index deda4ae9..8d86c4f0 100755
--- a/system_bagfile_with_route_planner.sh
+++ b/system_bagfile_with_route_planner.sh
@@ -1,9 +1,10 @@
 #!/bin/bash
 
 SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )"
+WS_DIR="$( cd "$SCRIPT_DIR/../.." && pwd )"
 
-cd $SCRIPT_DIR
-source ./install/setup.bash
+cd "$SCRIPT_DIR"
+source "$WS_DIR/install/setup.bash"
 ros2 launch vehicle_simulator system_bagfile_with_route_planner.launch.py &
 sleep 1
 ros2 run rviz2 rviz2 -d src/route_planner/far_planner/rviz/default.rviz
diff --git a/system_real_robot.sh b/system_real_robot.sh
index 51e7d583..dc0df733 100755
--- a/system_real_robot.sh
+++ b/system_real_robot.sh
@@ -1,9 +1,10 @@
 #!/bin/bash
 
 SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )"
+WS_DIR="$( cd "$SCRIPT_DIR/../.." && pwd )"
 
-cd $SCRIPT_DIR
-source ./install/setup.bash
+cd "$SCRIPT_DIR"
+source "$WS_DIR/install/setup.bash"
 ros2 launch vehicle_simulator system_real_robot.launch.py &
 sleep 1
 ros2 run rviz2 rviz2 -d src/base_autonomy/vehicle_simulator/rviz/vehicle_simulator.rviz
diff --git a/system_real_robot_with_exploration_planner.sh b/system_real_robot_with_exploration_planner.sh
index 13a3ac4b..ad5a90f1 100755
--- a/system_real_robot_with_exploration_planner.sh
+++ b/system_real_robot_with_exploration_planner.sh
@@ -1,9 +1,10 @@
 #!/bin/bash
 
 SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )"
+WS_DIR="$( cd "$SCRIPT_DIR/../.." && pwd )"
 
-cd $SCRIPT_DIR
-source ./install/setup.bash
+cd "$SCRIPT_DIR"
+source "$WS_DIR/install/setup.bash"
 ros2 launch vehicle_simulator system_real_robot_with_exploration_planner.launch.py &
 sleep 1
 ros2 run rviz2 rviz2 -d src/exploration_planner/tare_planner/rviz/tare_planner_ground.rviz
diff --git a/system_real_robot_with_route_planner.sh b/system_real_robot_with_route_planner.sh
index abb64845..a932611b 100755
--- a/system_real_robot_with_route_planner.sh
+++ b/system_real_robot_with_route_planner.sh
@@ -1,9 +1,10 @@
 #!/bin/bash
 
 SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )"
+WS_DIR="$( cd "$SCRIPT_DIR/../.." && pwd )"
 
-cd $SCRIPT_DIR
-source ./install/setup.bash
+cd "$SCRIPT_DIR"
+source "$WS_DIR/install/setup.bash"
 ros2 launch vehicle_simulator system_real_robot_with_route_planner.launch.py &
 sleep 1
 ros2 run rviz2 rviz2 -d src/route_planner/far_planner/rviz/default.rviz
diff --git a/system_simulation.sh b/system_simulation.sh
index e4f8791a..7c62cf29 100755
--- a/system_simulation.sh
+++ b/system_simulation.sh
@@ -1,9 +1,10 @@
 #!/bin/bash
 
 SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )"
+WS_DIR="$( cd "$SCRIPT_DIR/../.." && pwd )"
 
-cd $SCRIPT_DIR
-source ./install/setup.bash
+cd "$SCRIPT_DIR"
+source "$WS_DIR/install/setup.bash"
 ./src/base_autonomy/vehicle_simulator/mesh/unity/environment/Model.x86_64 &
 sleep 3 
 ros2 launch vehicle_simulator system_simulation.launch.py &
diff --git a/system_simulation_with_exploration_planner.sh b/system_simulation_with_exploration_planner.sh
index a86c3eba..1afb7474 100755
--- a/system_simulation_with_exploration_planner.sh
+++ b/system_simulation_with_exploration_planner.sh
@@ -1,9 +1,10 @@
 #!/bin/bash
 
 SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )"
+WS_DIR="$( cd "$SCRIPT_DIR/../.." && pwd )"
 
-cd $SCRIPT_DIR
-source ./install/setup.bash
+cd "$SCRIPT_DIR"
+source "$WS_DIR/install/setup.bash"
 ./src/base_autonomy/vehicle_simulator/mesh/unity/environment/Model.x86_64 &
 sleep 3 
 ros2 launch vehicle_simulator system_simulation_with_exploration_planner.launch.py &
diff --git a/system_simulation_with_route_planner.sh b/system_simulation_with_route_planner.sh
index e77d3500..3a9b08dc 100755
--- a/system_simulation_with_route_planner.sh
+++ b/system_simulation_with_route_planner.sh
@@ -1,9 +1,10 @@
 #!/bin/bash
 
 SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )"
+WS_DIR="$( cd "$SCRIPT_DIR/../.." && pwd )"
 
-cd $SCRIPT_DIR
-source ./install/setup.bash
+cd "$SCRIPT_DIR"
+source "$WS_DIR/install/setup.bash"
 ./src/base_autonomy/vehicle_simulator/mesh/unity/environment/Model.x86_64 &
 sleep 3 
 ros2 launch vehicle_simulator system_simulation_with_route_planner.launch.py &