diff --git a/src/opendrive_planner.cpp b/src/opendrive_planner.cpp
index 5f8df05c759b8684d768f34f1972529243b67bf4..680fb9d85ab5b008d9af97d85a5a78e4433995fe 100644
--- a/src/opendrive_planner.cpp
+++ b/src/opendrive_planner.cpp
@@ -371,7 +371,7 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
       if(this->tf2_buffer.canTransform(target_frame, source_frame, time, timeout))
       {
         transform = this->tf2_buffer.lookupTransform(target_frame, source_frame, time);
-        point.header.frame_id=this->opendrive_frame_id_;
+        point.header.frame_id=this->frame_id_;
         point.header.stamp=time;
         for(unsigned int i=0;i<x.size();i++)
         {
@@ -382,7 +382,7 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
           orientation.setRPY(0.0,0.0,heading[i]);
           point.pose.orientation=tf2::toMsg(orientation);
           tf2::doTransform(point,plan[i],transform);
-          plan[i].header.frame_id=this->opendrive_frame_id_;
+          plan[i].header.frame_id=this->frame_id_;
           plan[i].header.stamp=time;
         }
       }else{