diff --git a/carla_ad_agent/src/carla_ad_agent/local_planner.py b/carla_ad_agent/src/carla_ad_agent/local_planner.py
index 202eae51..f2e56781 100755
--- a/carla_ad_agent/src/carla_ad_agent/local_planner.py
+++ b/carla_ad_agent/src/carla_ad_agent/local_planner.py
@@ -59,6 +59,7 @@ def __init__(self):
 
         self.data_lock = threading.Lock()
 
+        self._current_header = None
         self._current_pose = None
         self._current_speed = None
         self._target_speed = 0.0
@@ -100,6 +101,8 @@ def __init__(self):
 
     def odometry_cb(self, odometry_msg):
         with self.data_lock:
+            self._current_header = odometry_msg.header
+            self._current_header.frame_id = odometry_msg.child_frame_id
             self._current_pose = odometry_msg.pose.pose
             self._current_speed = math.sqrt(odometry_msg.twist.twist.linear.x ** 2 +
                                             odometry_msg.twist.twist.linear.y ** 2 +
@@ -138,6 +141,11 @@ def run_step(self):
                 self.emergency_stop()
                 return
 
+            if (self._current_pose is None) or (self._current_speed is None) or (self._current_header is None):
+                self.loginfo("Waiting for first odometry message...")
+                self.emergency_stop()
+                return
+
             # when target speed is 0, brake.
             if self._target_speed == 0.0:
                 self.emergency_stop()
@@ -158,6 +166,7 @@ def run_step(self):
             # move using PID controllers
             control_msg = self._vehicle_controller.run_step(
                 self._target_speed, self._current_speed, self._current_pose, target_pose)
+            control_msg.header = self._current_header
 
             # purge the queue of obsolete waypoints
             max_index = -1
@@ -176,6 +185,8 @@ def run_step(self):
 
     def emergency_stop(self):
         control_msg = CarlaEgoVehicleControl()
+        if self._current_header:
+            control_msg.header = self._current_header
         control_msg.steer = 0.0
         control_msg.throttle = 0.0
         control_msg.brake = 1.0