Fix costmap conversion

Convert armarx costmap from mm to m to teb.

data/armarx_navigation/local_planner_config/TimedElasticBands/default.json

@@ -6,7 +6,7 @@
     "pse": {
       "pse_costum_obstacle_penalties": true,
       "pse_costum_obstacle_penalties_dynamic": true,
-      "weight_costmap": 0.5,
+      "weight_costmap": 1,
       "weight_global_path_position": 0.3,
       "weight_global_path_orientation": 0.3,
       "lrk_use_alternative_approach": false,

source/armarx/navigation/local_planning/ros_conversions.cpp

@@ -185,8 +185,10 @@ namespace armarx::navigation::conv
         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};
+        algorithms::Costmap::Grid teb_grid = costmap.getGrid();
+        teb_grid /= 1000; // [mm] to [m]
+
+        return teb_local_planner::Costmap{teb_grid, teb_params, teb_bounds, teb_mask, teb_origin};
     }
 
 } // namespace armarx::navigation::conv