diff --git a/include/ackermann_odom_helper.h b/include/ackermann_odom_helper.h
index 8f09f7c3ff4b3a172b7879fe0345b073087be594..de57bcb7f93b43dff72216ef06535606dcbcb5d3 100644
--- a/include/ackermann_odom_helper.h
+++ b/include/ackermann_odom_helper.h
@@ -42,6 +42,7 @@
 #include <nav_msgs/Odometry.h>
 #include <ackermann_msgs/AckermannDriveStamped.h>
 #include <ackermann_msgs/AckermannDrive.h>
+#include <costmap_2d/costmap_2d_ros.h>
 #include <ros/ros.h>
 #include <boost/thread.hpp>
 
@@ -67,6 +68,7 @@ class AckermannOdomHelper
     void set_odom_topic(const std::string &odom_topic);
     void set_ackermann_topic(const std::string &ackermann_topic);
     void set_average_samples(int num_average_samples);
+    void set_costmap(costmap_2d::Costmap2DROS* costmap_ros);
 
     /** @brief Return the current odometry topic. */
     std::string get_odom_topic(void) const;
@@ -96,6 +98,7 @@ class AckermannOdomHelper
     ros::Subscriber odom_sub_;
     nav_msgs::Odometry base_odom_;
     boost::mutex odom_mutex_;
+    costmap_2d::Costmap2DROS* costmap_ros_;
 
     ros::Subscriber ackermann_sub_;
     std::vector<ackermann_msgs::AckermannDrive> ackermann_data_;
diff --git a/src/ackermann_odom_helper.cpp b/src/ackermann_odom_helper.cpp
index 5056a825c74f61c9f8fb6a0e6d8eb781e51add5e..9276b8ee6b8c047aa217c602f3ce1518f7288cf8 100644
--- a/src/ackermann_odom_helper.cpp
+++ b/src/ackermann_odom_helper.cpp
@@ -41,15 +41,26 @@ AckermannOdomHelper::AckermannOdomHelper(const std::string &odom_topic,const std
   set_odom_topic(odom_topic);
   set_ackermann_topic(ackermann_topic);
   ackermann_data_.resize(num_avg_samples);
+  this->costmap_ros_=NULL;
 }
 
 void AckermannOdomHelper::odom_callback(const nav_msgs::Odometry::ConstPtr& msg) 
 {
+  geometry_msgs::PoseStamped pose_in;
   ROS_INFO_ONCE("Odom received!");
-
+  
   //we assume that the odometry is published in the frame of the base
   boost::mutex::scoped_lock lock(odom_mutex_);
   base_odom_=*msg;
+  if(this->costmap_ros_!=NULL)
+  {
+    if(!this->costmap_ros_->getRobotPose(pose_in))
+    {
+      ROS_ERROR("AckermannOdomHelper: Could not get robot pose");
+      return;
+    }
+    base_odom_.pose.pose=pose_in.pose;
+  }
 }
 
 void AckermannOdomHelper::ackermann_callback(const ackermann_msgs::AckermannDriveStamped::ConstPtr& msg) 
@@ -147,6 +158,12 @@ void AckermannOdomHelper::set_average_samples(int num_average_samples)
     this->ackermann_data_.resize(num_average_samples);
 }
 
+void AckermannOdomHelper::set_costmap(costmap_2d::Costmap2DROS* costmap_ros)
+{
+  boost::mutex::scoped_lock lock(odom_mutex_);
+  this->costmap_ros_=costmap_ros;
+}
+
 std::string AckermannOdomHelper::get_odom_topic(void) const
 {
   return this->odom_topic_;
diff --git a/src/ackermann_planner_ros.cpp b/src/ackermann_planner_ros.cpp
index ca394c7320c8b307005fcfd2203be3c783802fb7..6f8c48851a56a73d96aab8d602d1e48f11c5e099 100644
--- a/src/ackermann_planner_ros.cpp
+++ b/src/ackermann_planner_ros.cpp
@@ -107,6 +107,7 @@ void AckermannPlannerROS::initialize(std::string name,tf2_ros::Buffer* tf,costma
 
     if( private_nh.getParam( "ackermann_topic", ackermann_topic_ ))
       odom_helper_.set_ackermann_topic( ackermann_topic_ );
+    odom_helper_.set_costmap(costmap_ros_);
 
     initialized_ = true;