diff --git a/source/armarx/navigation/algorithms/Costmap.h b/source/armarx/navigation/algorithms/Costmap.h
index 6f452b7dcc5e0fb3f8265f344c54b6b3701cd13b..d168ef7d9c2973d7e55bf045d304d892c87afb66 100644
--- a/source/armarx/navigation/algorithms/Costmap.h
+++ b/source/armarx/navigation/algorithms/Costmap.h
@@ -68,8 +68,10 @@ namespace armarx::navigation::algorithms
         centerPose() const
         {
             const Eigen::Vector2f costmap_P_center{
-                (getLocalSceneBounds().max.x() - getLocalSceneBounds().min.x())/2 + getLocalSceneBounds().min.x(),
-                (getLocalSceneBounds().max.y() - getLocalSceneBounds().min.y())/2 + getLocalSceneBounds().min.y()};
+                (getLocalSceneBounds().max.x() - getLocalSceneBounds().min.x()) / 2 +
+                    getLocalSceneBounds().min.x(),
+                (getLocalSceneBounds().max.y() - getLocalSceneBounds().min.y()) / 2 +
+                    getLocalSceneBounds().min.y()};
 
             return global_T_costmap * Eigen::Translation2f(costmap_P_center);
         }
diff --git a/source/armarx/navigation/local_planning/CMakeLists.txt b/source/armarx/navigation/local_planning/CMakeLists.txt
index ae184519cebee4c6ee46881b72c09702441377df..4b5d36b1235915c6e8ac01bc3b3a57a76dfb5fd3 100644
--- a/source/armarx/navigation/local_planning/CMakeLists.txt
+++ b/source/armarx/navigation/local_planning/CMakeLists.txt
@@ -10,6 +10,7 @@ armarx_add_library(local_planning
         ArViz
         armarx_navigation::core
         armarx_navigation::conversions
+        armarx_navigation::algorithms
         armarx_navigation::local_planning_aron
         armarx_navigation::teb_human
         teb_extension::obstacles
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index 91483e74e63a8f260f2893968ab25f9d2942658a..1b86f5d3af36cf35428b35f014e7b4780e3a8302 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -67,8 +67,12 @@ namespace armarx::navigation::local_planning
 
         hcp_ = std::make_unique<teb_local_planner::HomotopyClassPlanner>();
         hcp_->initialize(
-            cfg_, &teb_obstacles, robot_model, teb_local_planner::TebVisualizationPtr(), nullptr);
-
+            cfg_, &teb_obstacles, robot_model, teb_local_planner::TebVisualizationPtr(), nullptr);       
+        //set member teb_costmap
+        setTebCostmap();
+        if (teb_costmap) {
+            hcp_->setCostmap(&teb_costmap.value());
+        }
         ros::Time::init(); // we have to init time before we can use the planner
     }
 
@@ -198,4 +202,17 @@ namespace armarx::navigation::local_planning
         ARMARX_INFO << "TEB: added " << obstManager.size() << " obstacles";
     }
 
+    //export algorithms::Costmap to costmap type of teb local planner and provide costmap to planner
+    void
+    TimedElasticBands::setTebCostmap()
+    {
+        if (!scene.staticScene)
+            return;
+        if (!scene.staticScene->distanceToObstaclesCostmap)
+            return;
+        const algorithms::Costmap& navigationCostmap =
+            scene.staticScene->distanceToObstaclesCostmap.value();
+        teb_costmap.emplace(conv::toRos(navigationCostmap));
+    }
+
 } // namespace armarx::navigation::local_planning
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.h b/source/armarx/navigation/local_planning/TimedElasticBands.h
index 9eba4d52739af83a79e0adcbc6cd8b0b19386b27..1c1db279a8bf343adccf20a8cc4ac67843d46180 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.h
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.h
@@ -61,6 +61,7 @@ namespace armarx::navigation::local_planning
     private:
         void readDefaultConfig(arondto::TimedElasticBandsParams& target);
         void fillObstacles();
+        void setTebCostmap();
 
     protected:
         Params params;
@@ -72,6 +73,7 @@ namespace armarx::navigation::local_planning
         teb_local_planner::ObstContainer teb_obstacles;
         TebObstacleManager obstManager;
         teb_local_planner::PoseSequence teb_globalPath;
+        std::optional<teb_local_planner::Costmap> teb_costmap;
         std::unique_ptr<teb_local_planner::HomotopyClassPlanner> hcp_{nullptr};
     };
 } // namespace armarx::navigation::local_planning
diff --git a/source/armarx/navigation/local_planning/ros_conversions.cpp b/source/armarx/navigation/local_planning/ros_conversions.cpp
index 86fd6a2c954ab6a9d44a44e7255eea850b2d1711..91736c3229a4b35e9a9ee5df6bcf917188703896 100644
--- a/source/armarx/navigation/local_planning/ros_conversions.cpp
+++ b/source/armarx/navigation/local_planning/ros_conversions.cpp
@@ -164,4 +164,29 @@ namespace armarx::navigation::conv
         return {.a = ellipse.b / 1000, .b = ellipse.a / 1000}; // [mm] to [m]
     }
 
+    teb_local_planner::Costmap
+    toRos(const algorithms::Costmap& costmap)
+    {
+        const algorithms::Costmap::Parameters& params = costmap.params();
+        const teb_local_planner::Costmap::Parameters teb_params = {
+            params.binaryGrid, params.cellSize / 1000}; // [mm] to [m]
+
+        const algorithms::SceneBounds& bounds = costmap.getLocalSceneBounds();
+        const teb_local_planner::Costmap::SceneBounds teb_bounds = {
+            toRos2D(bounds.min).cast<float>(), toRos2D(bounds.max).cast<float>()};
+
+        const std::optional<algorithms::Costmap::Mask>& mask = costmap.getMask();
+        boost::optional<teb_local_planner::Costmap::Mask> teb_mask = boost::none;
+        if (mask)
+        {
+            teb_mask = mask.value();
+        }
+
+        teb_local_planner::Costmap::Pose2D teb_origin = costmap.origin();
+        teb_origin.translation() /= 1000; // [mm] to[m]
+
+        return teb_local_planner::Costmap{
+            costmap.getGrid(), teb_params, teb_bounds, teb_mask, teb_origin};
+    }
+
 } // namespace armarx::navigation::conv
diff --git a/source/armarx/navigation/local_planning/ros_conversions.h b/source/armarx/navigation/local_planning/ros_conversions.h
index 548dce7300dd93a373e92fff9dc4701667449b1c..05b52e77803dcb52a2e6ec8aca447bb9664cae6f 100644
--- a/source/armarx/navigation/local_planning/ros_conversions.h
+++ b/source/armarx/navigation/local_planning/ros_conversions.h
@@ -27,6 +27,8 @@
 #include <geometry_msgs/Twist.h>
 #include <teb_local_planner/pose_se2.h>
 #include <teb_local_planner/timed_elastic_band.h>
+#include <teb_local_planner/costmap.h>
+#include <armarx/navigation/algorithms/Costmap.h>
 
 namespace armarx::navigation::conv
 {
@@ -49,4 +51,6 @@ namespace armarx::navigation::conv
 
     human::shapes::Ellipse toRos(const human::shapes::Ellipse& ellipse);
 
+    teb_local_planner::Costmap toRos(const algorithms::Costmap& costmap);
+
 } // namespace armarx::navigation::conv