Merge branch 'feature/add-costmap-weight' into 'dev'

Feature/add costmap weight

See merge request ArmarX/skills/navigation!52

data/armarx_navigation/local_planner_config/TimedElasticBands/default.json

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

source/armarx/navigation/local_planning/aron/TimedElasticBands.xml

@@ -14,6 +14,10 @@
                 <bool />
             </ObjectChild>
 
+            <ObjectChild key='weight_costmap'>
+                <float />
+            </ObjectChild>
+
             <ObjectChild key='weight_global_path_position'>
                 <float />
             </ObjectChild>

source/armarx/navigation/local_planning/aron_conversions.cpp

@@ -29,6 +29,7 @@ namespace armarx::navigation::local_planning
         bo.pse.pse_costum_obstacle_penalties = dto.teb_config.pse.pse_costum_obstacle_penalties;
         bo.pse.pse_costum_obstacle_penalties_dynamic =
             dto.teb_config.pse.pse_costum_obstacle_penalties_dynamic;
+        bo.pse.weight_costmap = dto.teb_config.pse.weight_costmap;
         bo.pse.weight_global_path_position = dto.teb_config.pse.weight_global_path_position;
         bo.pse.weight_global_path_orientation = dto.teb_config.pse.weight_global_path_orientation;
         bo.pse.lrk_use_alternative_approach = dto.teb_config.pse.lrk_use_alternative_approach;