From 4eaa0f8b4b376a8906056e32a4ed3e8aff92d81f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Wed, 3 Aug 2022 15:01:24 +0200
Subject: [PATCH 01/18] Small fix (add const to iterator)

---
 source/armarx/navigation/local_planning/TimedElasticBands.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index eeb9c238..70b692b0 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -153,7 +153,7 @@ namespace armarx::navigation::local_planning
 
         if (scene.staticScene.has_value())
         {
-            for (auto& obst : scene.staticScene.value().objects->getCollisionModels())
+            for (const auto& obst : scene.staticScene.value().objects->getCollisionModels())
             {
                 obstManager.addBoxObstacle(obst->getGlobalBoundingBox());
             }
-- 
GitLab


From 169d91b4471301ccdb78d71463c2e63f849f9ec3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Wed, 3 Aug 2022 15:01:49 +0200
Subject: [PATCH 02/18] Add teb default config as json

---
 .../TimedElasticBands/default.json            | 150 ++++++++++++++++++
 1 file changed, 150 insertions(+)
 create mode 100644 data/armarx_navigation/local_planner_config/TimedElasticBands/default.json

diff --git a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
new file mode 100644
index 00000000..344df69e
--- /dev/null
+++ b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
@@ -0,0 +1,150 @@
+
+{
+  "odom_topic": "odom",
+  "map_frame": "odom",
+
+  "pse": {
+    "pse_costum_obstacle_penalties": true,
+    "pse_costum_obstacle_penalties_dynamic": true,
+    "weight_global_path": 0.1,
+    "lrk_use_alternative_approach": false,
+    "lrk_enable_collision_check": true,
+    "hybrid_homotopy_calculation": true,
+    "robot_diff_circumscribed_inscribed_radius": 0.0
+  },
+
+  "trajectory": {
+    "teb_autosize": true,
+    "dt_ref": 0.3,
+    "dt_hysteresis": 0.1,
+    "min_samples": 3,
+    "max_samples": 500,
+    "global_plan_overwrite_orientation": true,
+    "allow_init_with_backwards_motion": false,
+    "global_plan_viapoint_sep": -1,
+    "via_points_ordered": false,
+    "max_global_plan_lookahead_dist": 1,
+    "global_plan_prune_distance": 1,
+    "exact_arc_length": false,
+    "force_reinit_new_goal_dist": 1,
+    "force_reinit_new_goal_angular": 0.5 * M_PI,
+    "feasibility_check_no_poses": 5,
+    "feasibility_check_lookahead_distance": -1,
+    "publish_feedback": false,
+    "min_resolution_collision_check_angular": M_PI,
+    "control_look_ahead_poses": 1
+  },
+
+  "robot": {
+    "max_vel_x": 0.4,
+    "max_vel_x_backwards": 0.2,
+    "max_vel_y": 0.0,
+    "max_vel_theta": 0.3,
+    "acc_lim_x": 0.5,
+    "acc_lim_y": 0.5,
+    "acc_lim_theta": 0.5,
+    "min_turning_radius": 0,
+    "wheelbase": 1.0,
+    "cmd_angle_instead_rotvel": false,
+    "is_footprint_dynamic": false,
+    "use_proportional_saturation": false,
+    "transform_tolerance": 0.5
+  },
+
+  "goal_tolerance": {
+    "xy_goal_tolerance": 0.2,
+    "yaw_goal_tolerance": 0.2,
+    "free_goal_vel": false,
+    "complete_global_plan": true
+
+  },
+
+  "obstacles": {
+    "min_obstacle_dist": 0.5,
+    "inflation_dist": 0.0,
+    "dynamic_obstacle_inflation_dist": 0.6,
+    "include_dynamic_obstacles": true,
+    "include_costmap_obstacles": true,
+    "costmap_obstacles_behind_robot_dist": 1.5,
+    "obstacle_poses_affected": 25,
+    "legacy_obstacle_association": false,
+    "obstacle_association_force_inclusion_factor": 1.5,
+    "obstacle_association_cutoff_factor": 5,
+    "costmap_converter_plugin": "",
+    "costmap_converter_spin_thread": true,
+    "costmap_converter_rate": 5,
+    "obstacle_proximity_ratio_max_vel": 1,
+    "obstacle_proximity_lower_bound": 0,
+    "obstacle_proximity_upper_bound": 0.5
+  },
+
+  "optim": {
+    "no_inner_iterations": 5,
+    "no_outer_iterations": 4,
+    "optimization_activate": true,
+    "optimization_verbose": false,
+    "penalty_epsilon": 0.05,
+    "weight_max_vel_x": 2,
+    "weight_max_vel_y": 2,
+    "weight_max_vel_theta": 1,
+    "weight_acc_lim_x": 1,
+    "weight_acc_lim_y": 1,
+    "weight_acc_lim_theta": 1,
+    "weight_kinematics_nh": 1000,
+    "weight_kinematics_forward_drive": 1,
+    "weight_kinematics_turning_radius": 1,
+    "weight_optimaltime": 1,
+    "weight_shortest_path": 0,
+    "weight_obstacle": 50,
+    "weight_inflation": 0.1,
+    "weight_dynamic_obstacle": 50,
+    "weight_dynamic_obstacle_inflation": 0.1,
+    "weight_velocity_obstacle_ratio": 0,
+    "weight_viapoint": 1,
+    "weight_prefer_rotdir": 50,
+    "weight_adapt_factor": 2.0,
+    "obstacle_cost_exponent": 1.0
+  },
+
+  "hcp": {
+    "enable_homotopy_class_planning": true,
+    "enable_multithreading": true,
+    "simple_exploration": false,
+    "max_number_classes": 5,
+    "max_number_plans_in_current_class" 0,
+    "selection_cost_hysteresis": 1.0,
+    "selection_prefer_initial_plan": 0.95,
+    "selection_obst_cost_scale": 100.0,
+    "selection_viapoint_cost_scale": 1.0,
+    "selection_alternative_time_cost": false,
+    "selection_dropping_probability": 0.0,
+    "switching_blocking_period": 0.0,
+
+    "obstacle_keypoint_offset": 0.1,
+    "obstacle_heading_threshold": 0.45,
+    "roadmap_graph_no_samples": 20,
+    "roadmap_graph_area_width": 10,
+    "roadmap_graph_area_length_scale": 1.0,
+    "h_signature_prescaler": 1,
+    "h_signature_threshold": 0.1,
+
+    "viapoints_all_candidates": true,
+
+    "visualize_hc_graph": false,
+    "visualize_with_time_as_z_axis_scale": 0.0,
+    "delete_detours_backwards": true,
+    "detours_orientation_tolerance": M_PI / 2.0,
+    "length_start_orientation_vector": 0.4,
+    "max_ratio_detours_duration_best_duration": 3.0,
+  },
+
+  "recovery": {
+    "shrink_horizon_backup": true,
+    "shrink_horizon_min_duration": 10,
+    "oscillation_recovery": true,
+    "oscillation_v_eps": 0.1,
+    "oscillation_omega_eps": 0.1,
+    "oscillation_recovery_min_duration": 10,
+    "oscillation_filter_duration": 10
+  }
+}
-- 
GitLab


From fde23ddbaddfa53441b9a3e284a26b56bdb7b60d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Wed, 3 Aug 2022 17:43:15 +0200
Subject: [PATCH 03/18] Fix json formatting

---
 .../TimedElasticBands/default.json                    | 11 +++++------
 1 file changed, 5 insertions(+), 6 deletions(-)

diff --git a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
index 344df69e..6849fbb4 100644
--- a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
+++ b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
@@ -1,4 +1,3 @@
-
 {
   "odom_topic": "odom",
   "map_frame": "odom",
@@ -27,11 +26,11 @@
     "global_plan_prune_distance": 1,
     "exact_arc_length": false,
     "force_reinit_new_goal_dist": 1,
-    "force_reinit_new_goal_angular": 0.5 * M_PI,
+    "force_reinit_new_goal_angular": 1.5707963267948966,
     "feasibility_check_no_poses": 5,
     "feasibility_check_lookahead_distance": -1,
     "publish_feedback": false,
-    "min_resolution_collision_check_angular": M_PI,
+    "min_resolution_collision_check_angular": 3.141592653589793,
     "control_look_ahead_poses": 1
   },
 
@@ -111,7 +110,7 @@
     "enable_multithreading": true,
     "simple_exploration": false,
     "max_number_classes": 5,
-    "max_number_plans_in_current_class" 0,
+    "max_number_plans_in_current_class": 0,
     "selection_cost_hysteresis": 1.0,
     "selection_prefer_initial_plan": 0.95,
     "selection_obst_cost_scale": 100.0,
@@ -133,9 +132,9 @@
     "visualize_hc_graph": false,
     "visualize_with_time_as_z_axis_scale": 0.0,
     "delete_detours_backwards": true,
-    "detours_orientation_tolerance": M_PI / 2.0,
+    "detours_orientation_tolerance": 1.5707963267948966,
     "length_start_orientation_vector": 0.4,
-    "max_ratio_detours_duration_best_duration": 3.0,
+    "max_ratio_detours_duration_best_duration": 3.0
   },
 
   "recovery": {
-- 
GitLab


From 427dd3b3cacf5c72348d78938a37961422338b04 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Wed, 3 Aug 2022 18:14:42 +0200
Subject: [PATCH 04/18] Add teb config to aron

---
 .../local_planning/aron/TimedElasticBands.xml | 517 +++++++++++++++++-
 1 file changed, 515 insertions(+), 2 deletions(-)

diff --git a/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml b/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
index 2c352aca..23dd0836 100644
--- a/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
+++ b/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
@@ -5,10 +5,523 @@
     </AronIncludes>
 
     <GenerateTypes>
-        <Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams'>
-            <ObjectChild key='foo'>
+        <Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams::pse'>
+            <ObjectChild key='pse_costum_obstacle_penalties'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='pse_costum_obstacle_penalties_dynamic'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='weight_global_path'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='lrk_use_alternative_approach'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='lrk_enable_collision_check'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='hybrid_homotopy_calculation'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='robot_diff_circumscribed_inscribed_radius'>
+                <float />
+            </ObjectChild>
+        </Object>
+
+        <Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams::trajectory'>
+            <ObjectChild key='teb_autosize'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='dt_ref'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='dt_hysteresis'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='min_samples'>
+                <int />
+            </ObjectChild>
+
+            <ObjectChild key='max_samples'>
+                <int />
+            </ObjectChild>
+
+            <ObjectChild key='global_plan_overwrite_orientation'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='allow_init_with_backwards_motion'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='global_plan_viapoint_sep'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='via_points_ordered'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='max_global_plan_lookahead_dist'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='global_plan_prune_distance'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='exact_arc_length'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='force_reinit_new_goal_dist'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='force_reinit_new_goal_angular'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='feasibility_check_no_poses'>
+                <int />
+            </ObjectChild>
+
+            <ObjectChild key='feasibility_check_lookahead_distance'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='publish_feedback'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='min_resolution_collision_check_angular'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='control_look_ahead_poses'>
+                <int />
+            </ObjectChild>
+        </Object>
+
+        <Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams::robot'>
+            <ObjectChild key='max_vel_x'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='max_vel_x_backwards'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='max_vel_y'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='max_vel_theta'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='acc_lim_x'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='acc_lim_y'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='acc_lim_theta'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='min_turning_radius'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='wheelbase'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='cmd_angle_instead_rotvel'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='is_footprint_dynamic'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='use_proportional_saturation'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='transform_tolerance'>
+                <float />
+            </ObjectChild>
+        </Object>
+
+        <Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams::goal_tolerance'>
+            <ObjectChild key='xy_goal_tolerance'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='yaw_goal_tolerance'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='free_goal_vel'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='complete_global_plan'>
+                <bool />
+            </ObjectChild>
+        </Object>
+
+        <Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams::obstacles'>
+            <ObjectChild key='min_obstacle_dist'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='inflation_dist'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='dynamic_obstacle_inflation_dist'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='include_dynamic_obstacles'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='include_costmap_obstacles'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='costmap_obstacles_behind_robot_dist'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='obstacle_poses_affected'>
+                <int />
+            </ObjectChild>
+
+            <ObjectChild key='legacy_obstacle_association'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='obstacle_association_force_inclusion_factor'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='obstacle_association_cutoff_factor'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='costmap_converter_plugin'>
+                <string />
+            </ObjectChild>
+
+            <ObjectChild key='costmap_converter_spin_thread'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='costmap_converter_rate'>
+                <int />
+            </ObjectChild>
+
+            <ObjectChild key='obstacle_proximity_ratio_max_vel'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='obstacle_proximity_lower_bound'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='obstacle_proximity_upper_bound'>
+                <float />
+            </ObjectChild>
+        </Object>
+
+        <Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams::optim'>
+            <ObjectChild key='no_inner_iterations'>
+                <int />
+            </ObjectChild>
+
+            <ObjectChild key='no_outer_iterations'>
+                <int />
+            </ObjectChild>
+
+            <ObjectChild key='optimization_activate'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='optimization_verbose'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='penalty_epsilon'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='weight_max_vel_x'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='weight_max_vel_y'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='weight_max_vel_theta'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='weight_acc_lim_x'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='weight_acc_lim_y'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='weight_acc_lim_theta'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='weight_kinematics_nh'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='weight_kinematics_forward_drive'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='weight_kinematics_turning_radius'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='weight_optimaltime'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='weight_shortest_path'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='weight_obstacle'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='weight_inflation'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='weight_dynamic_obstacle'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='weight_dynamic_obstacle_inflation'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='weight_velocity_obstacle_ratio'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='weight_viapoint'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='weight_prefer_rotdir'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='weight_adapt_factor'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='obstacle_cost_exponent'>
+                <float />
+            </ObjectChild>
+        </Object>
+
+        <Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams::hcp'>
+            <ObjectChild key='enable_homotopy_class_planning'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='enable_multithreading'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='simple_exploration'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='max_number_classes'>
+                <int />
+            </ObjectChild>
+
+            <ObjectChild key='max_number_plans_in_current_class'>
+                <int />
+            </ObjectChild>
+
+            <ObjectChild key='selection_cost_hysteresis'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='selection_prefer_initial_plan'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='selection_obst_cost_scale'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='selection_viapoint_cost_scale'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='selection_alternative_time_cost'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='selection_dropping_probability'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='switching_blocking_period'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='obstacle_keypoint_offset'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='obstacle_heading_threshold'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='roadmap_graph_no_samples'>
+                <int />
+            </ObjectChild>
+
+            <ObjectChild key='roadmap_graph_area_width'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='roadmap_graph_area_length_scale'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='h_signature_prescaler'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='h_signature_threshold'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='viapoints_all_candidates'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='visualize_hc_graph'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='visualize_with_time_as_z_axis_scale'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='delete_detours_backwards'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='detours_orientation_tolerance'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='length_start_orientation_vector'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='max_ratio_detours_duration_best_duration'>
                 <float />
             </ObjectChild>
         </Object>
+
+        <Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams::recovery'>
+            <ObjectChild key='shrink_horizon_backup'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='shrink_horizon_min_duration'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='oscillation_recovery'>
+                <bool />
+            </ObjectChild>
+
+            <ObjectChild key='oscillation_v_eps'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='oscillation_omega_eps'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='oscillation_recovery_min_duration'>
+                <float />
+            </ObjectChild>
+
+            <ObjectChild key='oscillation_filter_duration'>
+                <float />
+            </ObjectChild>
+        </Object>
+
+        <Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams'>
+            <ObjectChild key='odom_topic'>
+                <string />
+            </ObjectChild>
+            <ObjectChild key='map_frame'>
+                <string />
+            </ObjectChild>
+
+            <ObjectChild key='pse'>
+                <armarx::navigation::local_planning::arondto::TimedElasticBandsParams::pse />
+            </ObjectChild>
+            <ObjectChild key='trajectory'>
+                <armarx::navigation::local_planning::arondto::TimedElasticBandsParams::trajectory />
+            </ObjectChild>
+            <ObjectChild key='robot'>
+                <armarx::navigation::local_planning::arondto::TimedElasticBandsParams::robot />
+            </ObjectChild>
+            <ObjectChild key='goal_tolerance'>
+                <armarx::navigation::local_planning::arondto::TimedElasticBandsParams::goal_tolerance />
+            </ObjectChild>
+            <ObjectChild key='obstacles'>
+                <armarx::navigation::local_planning::arondto::TimedElasticBandsParams::obstacles />
+            </ObjectChild>
+            <ObjectChild key='optim'>
+                <armarx::navigation::local_planning::arondto::TimedElasticBandsParams::optim />
+            </ObjectChild>
+            <ObjectChild key='hcp'>
+                <armarx::navigation::local_planning::arondto::TimedElasticBandsParams::hcp />
+            </ObjectChild>
+            <ObjectChild key='recovery'>
+                <armarx::navigation::local_planning::arondto::TimedElasticBandsParams::recovery />
+            </ObjectChild>
+        </Object>
+
     </GenerateTypes>
 </AronTypeDefinition>
-- 
GitLab


From 200aa17151c68628106f6bd27498b647518d55d2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Wed, 3 Aug 2022 19:34:30 +0200
Subject: [PATCH 05/18] setup aron dto

---
 .../TimedElasticBands/default.json            |  1 -
 .../local_planning/TimedElasticBands.cpp      | 11 ++-----
 .../local_planning/TimedElasticBands.h        |  4 +--
 .../local_planning/aron/TimedElasticBands.xml | 32 +++++++++----------
 .../local_planning/aron_conversions.cpp       |  2 ++
 .../local_planning/aron_conversions.h         |  4 +++
 6 files changed, 26 insertions(+), 28 deletions(-)

diff --git a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
index 6849fbb4..1a09f281 100644
--- a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
+++ b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
@@ -55,7 +55,6 @@
     "yaw_goal_tolerance": 0.2,
     "free_goal_vel": false,
     "complete_global_plan": true
-
   },
 
   "obstacles": {
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index 70b692b0..37e77cd1 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -30,23 +30,16 @@ namespace armarx::navigation::local_planning
     aron::data::DictPtr
     TimedElasticBandsParams::toAron() const
     {
-        arondto::TimedElasticBandsParams dto;
-
-        TimedElasticBandsParams bo;
-        armarx::navigation::local_planning::toAron(dto, bo);
-
-        return dto.toAron();
+        return cfg.toAron();
     }
 
     TimedElasticBandsParams
     TimedElasticBandsParams::FromAron(const aron::data::DictPtr& dict)
     {
         ARMARX_CHECK_NOT_NULL(dict);
-        arondto::TimedElasticBandsParams dto;
-        dto.fromAron(dict);
 
         TimedElasticBandsParams bo;
-        fromAron(dto, bo);
+        bo.cfg.fromAron(dict);
 
         return bo;
     }
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.h b/source/armarx/navigation/local_planning/TimedElasticBands.h
index 26826650..654343d5 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.h
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.h
@@ -28,6 +28,7 @@
 #include <armarx/navigation/core/Trajectory.h>
 #include <armarx/navigation/local_planning/LocalPlanner.h>
 #include <armarx/navigation/local_planning/TebObstacleManager.h>
+#include <armarx/navigation/local_planning/aron/TimedElasticBands.aron.generated.h>
 #include <armarx/navigation/local_planning/core.h>
 #include <teb_local_planner/homotopy_class_planner.h>
 #include <teb_local_planner/teb_config.h>
@@ -38,8 +39,7 @@ namespace armarx::navigation::local_planning
 
     struct TimedElasticBandsParams : public LocalPlannerParams
     {
-        bool includeStartPose{false};
-
+        arondto::TimedElasticBandsParams cfg;
 
         Algorithms algorithm() const override;
         aron::data::DictPtr toAron() const override;
diff --git a/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml b/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
index 23dd0836..9a2adb44 100644
--- a/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
+++ b/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
@@ -5,7 +5,7 @@
     </AronIncludes>
 
     <GenerateTypes>
-        <Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams::pse'>
+        <Object name='armarx::navigation::local_planning::arondto::pse'>
             <ObjectChild key='pse_costum_obstacle_penalties'>
                 <bool />
             </ObjectChild>
@@ -35,7 +35,7 @@
             </ObjectChild>
         </Object>
 
-        <Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams::trajectory'>
+        <Object name='armarx::navigation::local_planning::arondto::trajectory'>
             <ObjectChild key='teb_autosize'>
                 <bool />
             </ObjectChild>
@@ -113,7 +113,7 @@
             </ObjectChild>
         </Object>
 
-        <Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams::robot'>
+        <Object name='armarx::navigation::local_planning::arondto::robot'>
             <ObjectChild key='max_vel_x'>
                 <float />
             </ObjectChild>
@@ -167,7 +167,7 @@
             </ObjectChild>
         </Object>
 
-        <Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams::goal_tolerance'>
+        <Object name='armarx::navigation::local_planning::arondto::goal_tolerance'>
             <ObjectChild key='xy_goal_tolerance'>
                 <float />
             </ObjectChild>
@@ -185,7 +185,7 @@
             </ObjectChild>
         </Object>
 
-        <Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams::obstacles'>
+        <Object name='armarx::navigation::local_planning::arondto::obstacles'>
             <ObjectChild key='min_obstacle_dist'>
                 <float />
             </ObjectChild>
@@ -251,7 +251,7 @@
             </ObjectChild>
         </Object>
 
-        <Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams::optim'>
+        <Object name='armarx::navigation::local_planning::arondto::optim'>
             <ObjectChild key='no_inner_iterations'>
                 <int />
             </ObjectChild>
@@ -353,7 +353,7 @@
             </ObjectChild>
         </Object>
 
-        <Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams::hcp'>
+        <Object name='armarx::navigation::local_planning::arondto::hcp'>
             <ObjectChild key='enable_homotopy_class_planning'>
                 <bool />
             </ObjectChild>
@@ -459,7 +459,7 @@
             </ObjectChild>
         </Object>
 
-        <Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams::recovery'>
+        <Object name='armarx::navigation::local_planning::arondto::recovery'>
             <ObjectChild key='shrink_horizon_backup'>
                 <bool />
             </ObjectChild>
@@ -498,28 +498,28 @@
             </ObjectChild>
 
             <ObjectChild key='pse'>
-                <armarx::navigation::local_planning::arondto::TimedElasticBandsParams::pse />
+                <armarx::navigation::local_planning::arondto::pse />
             </ObjectChild>
             <ObjectChild key='trajectory'>
-                <armarx::navigation::local_planning::arondto::TimedElasticBandsParams::trajectory />
+                <armarx::navigation::local_planning::arondto::trajectory />
             </ObjectChild>
             <ObjectChild key='robot'>
-                <armarx::navigation::local_planning::arondto::TimedElasticBandsParams::robot />
+                <armarx::navigation::local_planning::arondto::robot />
             </ObjectChild>
             <ObjectChild key='goal_tolerance'>
-                <armarx::navigation::local_planning::arondto::TimedElasticBandsParams::goal_tolerance />
+                <armarx::navigation::local_planning::arondto::goal_tolerance />
             </ObjectChild>
             <ObjectChild key='obstacles'>
-                <armarx::navigation::local_planning::arondto::TimedElasticBandsParams::obstacles />
+                <armarx::navigation::local_planning::arondto::obstacles />
             </ObjectChild>
             <ObjectChild key='optim'>
-                <armarx::navigation::local_planning::arondto::TimedElasticBandsParams::optim />
+                <armarx::navigation::local_planning::arondto::optim />
             </ObjectChild>
             <ObjectChild key='hcp'>
-                <armarx::navigation::local_planning::arondto::TimedElasticBandsParams::hcp />
+                <armarx::navigation::local_planning::arondto::hcp />
             </ObjectChild>
             <ObjectChild key='recovery'>
-                <armarx::navigation::local_planning::arondto::TimedElasticBandsParams::recovery />
+                <armarx::navigation::local_planning::arondto::recovery />
             </ObjectChild>
         </Object>
 
diff --git a/source/armarx/navigation/local_planning/aron_conversions.cpp b/source/armarx/navigation/local_planning/aron_conversions.cpp
index 44599c44..87ead8dc 100644
--- a/source/armarx/navigation/local_planning/aron_conversions.cpp
+++ b/source/armarx/navigation/local_planning/aron_conversions.cpp
@@ -14,11 +14,13 @@ namespace armarx::navigation::local_planning
     void
     toAron(arondto::TimedElasticBandsParams& dto, const TimedElasticBandsParams& bo)
     {
+        dto = bo.cfg;
     }
 
     void
     fromAron(const arondto::TimedElasticBandsParams& dto, TimedElasticBandsParams& bo)
     {
+        bo.cfg = dto;
     }
 
 
diff --git a/source/armarx/navigation/local_planning/aron_conversions.h b/source/armarx/navigation/local_planning/aron_conversions.h
index 1b7d2382..22eb26d1 100644
--- a/source/armarx/navigation/local_planning/aron_conversions.h
+++ b/source/armarx/navigation/local_planning/aron_conversions.h
@@ -1,5 +1,7 @@
 #pragma once
 
+#include <teb_local_planner/teb_config.h>
+
 namespace armarx::navigation::local_planning
 {
     // struct LocalPlannerParams;
@@ -19,4 +21,6 @@ namespace armarx::navigation::local_planning
     void toAron(arondto::TimedElasticBandsParams& dto, const TimedElasticBandsParams& bo);
     void fromAron(const arondto::TimedElasticBandsParams& dto, TimedElasticBandsParams& bo);
 
+    void toTebCfg(const arondto::TimedElasticBandsParams& dto, teb_local_planner::TebConfig& bo);
+
 } // namespace armarx::navigation::local_planning
-- 
GitLab


From ea2b32b7c94dcfe016a68e11c784cdf6a578d2ad Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Thu, 4 Aug 2022 05:03:05 +0200
Subject: [PATCH 06/18] remove unused config parameters

---
 .../TimedElasticBands/default.json            | 45 ++-----------------
 1 file changed, 4 insertions(+), 41 deletions(-)

diff --git a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
index 1a09f281..33389b94 100644
--- a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
+++ b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
@@ -1,7 +1,4 @@
 {
-  "odom_topic": "odom",
-  "map_frame": "odom",
-
   "pse": {
     "pse_costum_obstacle_penalties": true,
     "pse_costum_obstacle_penalties_dynamic": true,
@@ -20,18 +17,12 @@
     "max_samples": 500,
     "global_plan_overwrite_orientation": true,
     "allow_init_with_backwards_motion": false,
-    "global_plan_viapoint_sep": -1,
-    "via_points_ordered": false,
-    "max_global_plan_lookahead_dist": 1,
-    "global_plan_prune_distance": 1,
     "exact_arc_length": false,
     "force_reinit_new_goal_dist": 1,
     "force_reinit_new_goal_angular": 1.5707963267948966,
     "feasibility_check_no_poses": 5,
     "feasibility_check_lookahead_distance": -1,
-    "publish_feedback": false,
-    "min_resolution_collision_check_angular": 3.141592653589793,
-    "control_look_ahead_poses": 1
+    "min_resolution_collision_check_angular": 3.141592653589793
   },
 
   "robot": {
@@ -42,19 +33,11 @@
     "acc_lim_x": 0.5,
     "acc_lim_y": 0.5,
     "acc_lim_theta": 0.5,
-    "min_turning_radius": 0,
-    "wheelbase": 1.0,
-    "cmd_angle_instead_rotvel": false,
-    "is_footprint_dynamic": false,
-    "use_proportional_saturation": false,
-    "transform_tolerance": 0.5
+    "min_turning_radius": 0
   },
 
   "goal_tolerance": {
-    "xy_goal_tolerance": 0.2,
-    "yaw_goal_tolerance": 0.2,
-    "free_goal_vel": false,
-    "complete_global_plan": true
+    "xy_goal_tolerance": 0.2
   },
 
   "obstacles": {
@@ -62,15 +45,8 @@
     "inflation_dist": 0.0,
     "dynamic_obstacle_inflation_dist": 0.6,
     "include_dynamic_obstacles": true,
-    "include_costmap_obstacles": true,
-    "costmap_obstacles_behind_robot_dist": 1.5,
-    "obstacle_poses_affected": 25,
-    "legacy_obstacle_association": false,
     "obstacle_association_force_inclusion_factor": 1.5,
     "obstacle_association_cutoff_factor": 5,
-    "costmap_converter_plugin": "",
-    "costmap_converter_spin_thread": true,
-    "costmap_converter_rate": 5,
     "obstacle_proximity_ratio_max_vel": 1,
     "obstacle_proximity_lower_bound": 0,
     "obstacle_proximity_upper_bound": 0.5
@@ -105,7 +81,6 @@
   },
 
   "hcp": {
-    "enable_homotopy_class_planning": true,
     "enable_multithreading": true,
     "simple_exploration": false,
     "max_number_classes": 5,
@@ -118,7 +93,6 @@
     "selection_dropping_probability": 0.0,
     "switching_blocking_period": 0.0,
 
-    "obstacle_keypoint_offset": 0.1,
     "obstacle_heading_threshold": 0.45,
     "roadmap_graph_no_samples": 20,
     "roadmap_graph_area_width": 10,
@@ -128,21 +102,10 @@
 
     "viapoints_all_candidates": true,
 
-    "visualize_hc_graph": false,
-    "visualize_with_time_as_z_axis_scale": 0.0,
     "delete_detours_backwards": true,
     "detours_orientation_tolerance": 1.5707963267948966,
     "length_start_orientation_vector": 0.4,
     "max_ratio_detours_duration_best_duration": 3.0
-  },
-
-  "recovery": {
-    "shrink_horizon_backup": true,
-    "shrink_horizon_min_duration": 10,
-    "oscillation_recovery": true,
-    "oscillation_v_eps": 0.1,
-    "oscillation_omega_eps": 0.1,
-    "oscillation_recovery_min_duration": 10,
-    "oscillation_filter_duration": 10
   }
+
 }
-- 
GitLab


From 55075b4716e905a01e4adfdfbeb3ee1630410124 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Thu, 4 Aug 2022 05:10:20 +0200
Subject: [PATCH 07/18] adjust aron file

---
 .../local_planning/aron/TimedElasticBands.xml | 138 ------------------
 1 file changed, 138 deletions(-)

diff --git a/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml b/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
index 9a2adb44..b4bd0e76 100644
--- a/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
+++ b/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
@@ -64,22 +64,6 @@
                 <bool />
             </ObjectChild>
 
-            <ObjectChild key='global_plan_viapoint_sep'>
-                <float />
-            </ObjectChild>
-
-            <ObjectChild key='via_points_ordered'>
-                <bool />
-            </ObjectChild>
-
-            <ObjectChild key='max_global_plan_lookahead_dist'>
-                <float />
-            </ObjectChild>
-
-            <ObjectChild key='global_plan_prune_distance'>
-                <float />
-            </ObjectChild>
-
             <ObjectChild key='exact_arc_length'>
                 <bool />
             </ObjectChild>
@@ -100,17 +84,9 @@
                 <float />
             </ObjectChild>
 
-            <ObjectChild key='publish_feedback'>
-                <bool />
-            </ObjectChild>
-
             <ObjectChild key='min_resolution_collision_check_angular'>
                 <float />
             </ObjectChild>
-
-            <ObjectChild key='control_look_ahead_poses'>
-                <int />
-            </ObjectChild>
         </Object>
 
         <Object name='armarx::navigation::local_planning::arondto::robot'>
@@ -145,44 +121,12 @@
             <ObjectChild key='min_turning_radius'>
                 <float />
             </ObjectChild>
-
-            <ObjectChild key='wheelbase'>
-                <float />
-            </ObjectChild>
-
-            <ObjectChild key='cmd_angle_instead_rotvel'>
-                <bool />
-            </ObjectChild>
-
-            <ObjectChild key='is_footprint_dynamic'>
-                <bool />
-            </ObjectChild>
-
-            <ObjectChild key='use_proportional_saturation'>
-                <bool />
-            </ObjectChild>
-
-            <ObjectChild key='transform_tolerance'>
-                <float />
-            </ObjectChild>
         </Object>
 
         <Object name='armarx::navigation::local_planning::arondto::goal_tolerance'>
             <ObjectChild key='xy_goal_tolerance'>
                 <float />
             </ObjectChild>
-
-            <ObjectChild key='yaw_goal_tolerance'>
-                <float />
-            </ObjectChild>
-
-            <ObjectChild key='free_goal_vel'>
-                <bool />
-            </ObjectChild>
-
-            <ObjectChild key='complete_global_plan'>
-                <bool />
-            </ObjectChild>
         </Object>
 
         <Object name='armarx::navigation::local_planning::arondto::obstacles'>
@@ -202,22 +146,6 @@
                 <bool />
             </ObjectChild>
 
-            <ObjectChild key='include_costmap_obstacles'>
-                <bool />
-            </ObjectChild>
-
-            <ObjectChild key='costmap_obstacles_behind_robot_dist'>
-                <float />
-            </ObjectChild>
-
-            <ObjectChild key='obstacle_poses_affected'>
-                <int />
-            </ObjectChild>
-
-            <ObjectChild key='legacy_obstacle_association'>
-                <bool />
-            </ObjectChild>
-
             <ObjectChild key='obstacle_association_force_inclusion_factor'>
                 <float />
             </ObjectChild>
@@ -226,18 +154,6 @@
                 <float />
             </ObjectChild>
 
-            <ObjectChild key='costmap_converter_plugin'>
-                <string />
-            </ObjectChild>
-
-            <ObjectChild key='costmap_converter_spin_thread'>
-                <bool />
-            </ObjectChild>
-
-            <ObjectChild key='costmap_converter_rate'>
-                <int />
-            </ObjectChild>
-
             <ObjectChild key='obstacle_proximity_ratio_max_vel'>
                 <float />
             </ObjectChild>
@@ -354,10 +270,6 @@
         </Object>
 
         <Object name='armarx::navigation::local_planning::arondto::hcp'>
-            <ObjectChild key='enable_homotopy_class_planning'>
-                <bool />
-            </ObjectChild>
-
             <ObjectChild key='enable_multithreading'>
                 <bool />
             </ObjectChild>
@@ -402,9 +314,6 @@
                 <float />
             </ObjectChild>
 
-            <ObjectChild key='obstacle_keypoint_offset'>
-                <float />
-            </ObjectChild>
 
             <ObjectChild key='obstacle_heading_threshold'>
                 <float />
@@ -434,13 +343,6 @@
                 <bool />
             </ObjectChild>
 
-            <ObjectChild key='visualize_hc_graph'>
-                <bool />
-            </ObjectChild>
-
-            <ObjectChild key='visualize_with_time_as_z_axis_scale'>
-                <float />
-            </ObjectChild>
 
             <ObjectChild key='delete_detours_backwards'>
                 <bool />
@@ -459,44 +361,7 @@
             </ObjectChild>
         </Object>
 
-        <Object name='armarx::navigation::local_planning::arondto::recovery'>
-            <ObjectChild key='shrink_horizon_backup'>
-                <bool />
-            </ObjectChild>
-
-            <ObjectChild key='shrink_horizon_min_duration'>
-                <float />
-            </ObjectChild>
-
-            <ObjectChild key='oscillation_recovery'>
-                <bool />
-            </ObjectChild>
-
-            <ObjectChild key='oscillation_v_eps'>
-                <float />
-            </ObjectChild>
-
-            <ObjectChild key='oscillation_omega_eps'>
-                <float />
-            </ObjectChild>
-
-            <ObjectChild key='oscillation_recovery_min_duration'>
-                <float />
-            </ObjectChild>
-
-            <ObjectChild key='oscillation_filter_duration'>
-                <float />
-            </ObjectChild>
-        </Object>
-
         <Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams'>
-            <ObjectChild key='odom_topic'>
-                <string />
-            </ObjectChild>
-            <ObjectChild key='map_frame'>
-                <string />
-            </ObjectChild>
-
             <ObjectChild key='pse'>
                 <armarx::navigation::local_planning::arondto::pse />
             </ObjectChild>
@@ -518,9 +383,6 @@
             <ObjectChild key='hcp'>
                 <armarx::navigation::local_planning::arondto::hcp />
             </ObjectChild>
-            <ObjectChild key='recovery'>
-                <armarx::navigation::local_planning::arondto::recovery />
-            </ObjectChild>
         </Object>
 
     </GenerateTypes>
-- 
GitLab


From 953dcbc965c024a4f7f1bd1b67a690d00cd14a46 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Thu, 4 Aug 2022 05:56:15 +0200
Subject: [PATCH 08/18] Read config from dto, read default config

---
 .../local_planning/TimedElasticBands.cpp      |  58 +++++----
 .../local_planning/TimedElasticBands.h        |   2 +-
 .../local_planning/aron/TimedElasticBands.xml |   1 +
 .../local_planning/aron_conversions.cpp       | 118 ++++++++++++++++++
 4 files changed, 154 insertions(+), 25 deletions(-)

diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index 37e77cd1..8084e1fb 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -1,11 +1,15 @@
 #include "TimedElasticBands.h"
 
+#include <SimoxUtility/json/json.hpp>
 #include <VirtualRobot/MathTools.h>
 #include <VirtualRobot/Robot.h>
 #include <VirtualRobot/SceneObjectSet.h>
 
+#include <ArmarXCore/core/PackagePath.h>
 #include <ArmarXCore/core/logging/Logging.h>
 
+#include "RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.h"
+
 #include "armarx/navigation/conversions/eigen.h"
 #include "armarx/navigation/core/Trajectory.h"
 #include <armarx/navigation/local_planning/LocalPlanner.h>
@@ -51,7 +55,14 @@ namespace armarx::navigation::local_planning
         LocalPlanner(ctx), params(params), scene(ctx), obstManager(teb_obstacles)
     {
         //TODO (SALt): init configuration, robot footprint
-        initConfig();
+        if (false)
+        {
+            toTebCfg(params.cfg, cfg_);
+        }
+        else
+        {
+            initDefaultConfig();
+        }
         auto robot_model = boost::make_shared<teb_local_planner::CircularRobotFootprint>(0.2);
 
         hcp_ = std::make_unique<teb_local_planner::HomotopyClassPlanner>();
@@ -62,30 +73,29 @@ namespace armarx::navigation::local_planning
     }
 
     void
-    TimedElasticBands::initConfig()
+    TimedElasticBands::initDefaultConfig()
     {
-        cfg_.robot.max_vel_x = 1.0;
-        cfg_.robot.max_vel_x_backwards = 1.0;
-        cfg_.robot.max_vel_y = 1.0;
-        cfg_.robot.max_vel_theta = 1.0;
-        cfg_.robot.acc_lim_x = 1.0;
-        cfg_.robot.acc_lim_y = 1.0;
-        cfg_.robot.acc_lim_theta = 1.0;
-
-        cfg_.optim.weight_max_vel_x = 1.0;
-        cfg_.optim.weight_max_vel_y = 1.0;
-        cfg_.optim.weight_max_vel_theta = 1.0;
-        cfg_.optim.weight_acc_lim_x = 1.0;
-        cfg_.optim.weight_acc_lim_y = 1.0;
-        cfg_.optim.weight_acc_lim_theta = 1.0;
-
-        cfg_.optim.weight_optimaltime = 0.6;
-        cfg_.optim.weight_shortest_path = 0.6;
-
-        cfg_.hcp.selection_cost_hysteresis = 1.0;
-        cfg_.hcp.switching_blocking_period = 0;
-
-        cfg_.pse.weight_global_path = 0.6;
+        arondto::TimedElasticBandsParams dto;
+
+        const armarx::PackagePath configPath("armarx_navigation",
+                                             "local_planner_config/TimedElasticBands/default.json");
+        const std::filesystem::path file = configPath.toSystemPath();
+
+        ARMARX_CHECK(std::filesystem::is_regular_file(file)) << file;
+
+        ARMARX_INFO << "Loading config from file `" << file << "`.";
+        std::ifstream ifs{file};
+
+        nlohmann::json jsonConfig;
+        ifs >> jsonConfig;
+
+        ARMARX_INFO << "Initializing config";
+
+        armarx::aron::data::reader::NlohmannJSONReaderWithoutTypeCheck reader;
+
+        dto.read(reader, jsonConfig);
+
+        toTebCfg(params.cfg, cfg_);
     }
 
     std::optional<LocalPlannerResult>
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.h b/source/armarx/navigation/local_planning/TimedElasticBands.h
index 654343d5..caf2c832 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.h
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.h
@@ -66,7 +66,7 @@ namespace armarx::navigation::local_planning
             bool planToDestination;
         };
 
-        void initConfig();
+        void initDefaultConfig();
         FindTargetResult findTarget(const core::Pose& currentPose,
                                     const core::GlobalTrajectory& globalPath);
         void fillObstacles();
diff --git a/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml b/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
index b4bd0e76..b0f90f17 100644
--- a/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
+++ b/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
@@ -339,6 +339,7 @@
                 <float />
             </ObjectChild>
 
+
             <ObjectChild key='viapoints_all_candidates'>
                 <bool />
             </ObjectChild>
diff --git a/source/armarx/navigation/local_planning/aron_conversions.cpp b/source/armarx/navigation/local_planning/aron_conversions.cpp
index 87ead8dc..4a93ffcd 100644
--- a/source/armarx/navigation/local_planning/aron_conversions.cpp
+++ b/source/armarx/navigation/local_planning/aron_conversions.cpp
@@ -23,5 +23,123 @@ namespace armarx::navigation::local_planning
         bo.cfg = dto;
     }
 
+    void
+    toTebCfg(const arondto::TimedElasticBandsParams& dto, teb_local_planner::TebConfig& bo)
+    {
+        bo.pse.pse_costum_obstacle_penalties = dto.pse.pse_costum_obstacle_penalties;
+        bo.pse.pse_costum_obstacle_penalties_dynamic =
+            dto.pse.pse_costum_obstacle_penalties_dynamic;
+        bo.pse.weight_global_path = dto.pse.weight_global_path;
+        bo.pse.lrk_use_alternative_approach = dto.pse.lrk_use_alternative_approach;
+        bo.pse.lrk_enable_collision_check = dto.pse.lrk_enable_collision_check;
+        bo.pse.hybrid_homotopy_calculation = dto.pse.hybrid_homotopy_calculation;
+        bo.pse.robot_diff_circumscribed_inscribed_radius =
+            dto.pse.robot_diff_circumscribed_inscribed_radius;
+
+        // Trajectory
+        bo.trajectory.teb_autosize = bo.trajectory.teb_autosize;
+        bo.trajectory.dt_ref = dto.trajectory.dt_ref;
+        bo.trajectory.dt_hysteresis = dto.trajectory.dt_hysteresis;
+        bo.trajectory.min_samples = dto.trajectory.min_samples;
+        bo.trajectory.max_samples = dto.trajectory.max_samples;
+        bo.trajectory.global_plan_overwrite_orientation =
+            dto.trajectory.global_plan_overwrite_orientation;
+        bo.trajectory.allow_init_with_backwards_motion =
+            dto.trajectory.allow_init_with_backwards_motion;
+        bo.trajectory.exact_arc_length = dto.trajectory.exact_arc_length;
+        bo.trajectory.force_reinit_new_goal_dist = dto.trajectory.force_reinit_new_goal_dist;
+        bo.trajectory.force_reinit_new_goal_angular = dto.trajectory.force_reinit_new_goal_angular;
+        bo.trajectory.feasibility_check_no_poses = dto.trajectory.feasibility_check_no_poses;
+        bo.trajectory.feasibility_check_lookahead_distance =
+            dto.trajectory.feasibility_check_lookahead_distance;
+        bo.trajectory.min_resolution_collision_check_angular =
+            dto.trajectory.min_resolution_collision_check_angular;
+
+        // Robot
+        bo.robot.max_vel_x = dto.robot.max_vel_x;
+        bo.robot.max_vel_x_backwards = dto.robot.max_vel_x_backwards;
+        bo.robot.max_vel_y = dto.robot.max_vel_y;
+        bo.robot.max_vel_theta = dto.robot.max_vel_theta;
+        bo.robot.acc_lim_x = dto.robot.acc_lim_x;
+        bo.robot.acc_lim_y = dto.robot.acc_lim_y;
+        bo.robot.acc_lim_theta = dto.robot.acc_lim_theta;
+        bo.robot.min_turning_radius = dto.robot.min_turning_radius;
+
+        // GoalTolerance
+        bo.goal_tolerance.xy_goal_tolerance = dto.goal_tolerance.xy_goal_tolerance;
+
+        // Obstacles
+        bo.obstacles.min_obstacle_dist = dto.obstacles.min_obstacle_dist;
+        bo.obstacles.inflation_dist = dto.obstacles.inflation_dist;
+        bo.obstacles.dynamic_obstacle_inflation_dist =
+            dto.obstacles.dynamic_obstacle_inflation_dist;
+        bo.obstacles.include_dynamic_obstacles = dto.obstacles.include_dynamic_obstacles;
+        bo.obstacles.obstacle_association_force_inclusion_factor =
+            dto.obstacles.obstacle_association_force_inclusion_factor;
+        bo.obstacles.obstacle_association_cutoff_factor =
+            dto.obstacles.obstacle_association_cutoff_factor;
+        bo.obstacles.obstacle_proximity_ratio_max_vel =
+            dto.obstacles.obstacle_proximity_ratio_max_vel;
+        bo.obstacles.obstacle_proximity_lower_bound = dto.obstacles.obstacle_proximity_lower_bound;
+        bo.obstacles.obstacle_proximity_upper_bound = dto.obstacles.obstacle_proximity_upper_bound;
+
+        // Optimization
+        bo.optim.no_inner_iterations = dto.optim.no_inner_iterations;
+        bo.optim.no_outer_iterations = dto.optim.no_outer_iterations;
+        bo.optim.optimization_activate = dto.optim.optimization_activate;
+        bo.optim.optimization_verbose = dto.optim.optimization_verbose;
+        bo.optim.penalty_epsilon = dto.optim.penalty_epsilon;
+        bo.optim.weight_max_vel_x = dto.optim.weight_max_vel_x;
+        bo.optim.weight_max_vel_y = dto.optim.weight_max_vel_y;
+        bo.optim.weight_max_vel_theta = dto.optim.weight_max_vel_theta;
+        bo.optim.weight_acc_lim_x = dto.optim.weight_acc_lim_x;
+        bo.optim.weight_acc_lim_y = dto.optim.weight_acc_lim_y;
+        bo.optim.weight_acc_lim_theta = dto.optim.weight_acc_lim_theta;
+        bo.optim.weight_kinematics_nh = dto.optim.weight_kinematics_nh;
+        bo.optim.weight_kinematics_forward_drive = dto.optim.weight_kinematics_forward_drive;
+        bo.optim.weight_kinematics_turning_radius = dto.optim.weight_kinematics_turning_radius;
+        bo.optim.weight_optimaltime = dto.optim.weight_optimaltime;
+        bo.optim.weight_shortest_path = dto.optim.weight_shortest_path;
+        bo.optim.weight_obstacle = dto.optim.weight_obstacle;
+        bo.optim.weight_inflation = dto.optim.weight_inflation;
+        bo.optim.weight_dynamic_obstacle = dto.optim.weight_dynamic_obstacle;
+        bo.optim.weight_dynamic_obstacle_inflation = dto.optim.weight_dynamic_obstacle_inflation;
+        bo.optim.weight_velocity_obstacle_ratio = dto.optim.weight_velocity_obstacle_ratio;
+        bo.optim.weight_viapoint = dto.optim.weight_viapoint;
+        bo.optim.weight_prefer_rotdir = dto.optim.weight_prefer_rotdir;
+        bo.optim.weight_adapt_factor = dto.optim.weight_adapt_factor;
+        bo.optim.obstacle_cost_exponent = dto.optim.obstacle_cost_exponent;
+
+        // Homotopy Class Planner
+        bo.hcp.enable_multithreading = dto.hcp.enable_multithreading;
+        bo.hcp.simple_exploration = dto.hcp.simple_exploration;
+        bo.hcp.max_number_classes = dto.hcp.max_number_classes;
+        bo.hcp.max_number_plans_in_current_class = dto.hcp.max_number_plans_in_current_class;
+        bo.hcp.selection_cost_hysteresis = dto.hcp.selection_cost_hysteresis;
+        bo.hcp.selection_prefer_initial_plan = dto.hcp.selection_prefer_initial_plan;
+        bo.hcp.selection_obst_cost_scale = dto.hcp.selection_obst_cost_scale;
+        bo.hcp.selection_viapoint_cost_scale = dto.hcp.selection_viapoint_cost_scale;
+        bo.hcp.selection_alternative_time_cost = dto.hcp.selection_alternative_time_cost;
+        bo.hcp.selection_dropping_probability = dto.hcp.selection_dropping_probability;
+        bo.hcp.switching_blocking_period = dto.hcp.switching_blocking_period;
+
+        bo.hcp.obstacle_heading_threshold = dto.hcp.obstacle_heading_threshold;
+        bo.hcp.roadmap_graph_no_samples = dto.hcp.roadmap_graph_no_samples;
+        bo.hcp.roadmap_graph_area_width = dto.hcp.roadmap_graph_area_width;
+        bo.hcp.roadmap_graph_area_length_scale = dto.hcp.roadmap_graph_area_length_scale;
+        bo.hcp.h_signature_prescaler = dto.hcp.h_signature_prescaler;
+        bo.hcp.h_signature_threshold = dto.hcp.h_signature_threshold;
+
+        bo.hcp.viapoints_all_candidates = dto.hcp.viapoints_all_candidates;
+
+        bo.hcp.delete_detours_backwards = dto.hcp.delete_detours_backwards;
+        bo.hcp.detours_orientation_tolerance = dto.hcp.detours_orientation_tolerance;
+        bo.hcp.length_start_orientation_vector = dto.hcp.length_start_orientation_vector;
+        bo.hcp.max_ratio_detours_duration_best_duration =
+            dto.hcp.max_ratio_detours_duration_best_duration;
+
+        bo.checkParameters();
+    }
+
 
 } // namespace armarx::navigation::local_planning
-- 
GitLab


From b63a6c0e677e188f0abe5885d1d9b4aa75d26875 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Thu, 11 Aug 2022 20:05:22 +0200
Subject: [PATCH 09/18] Fix config parameters

Upstream merge of teb_local_planner added a new parameter.
Other parameter is only used when an initial plan is used which we do not do.
---
 .../local_planner_config/TimedElasticBands/default.json         | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
index 33389b94..ed871ab9 100644
--- a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
+++ b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
@@ -29,6 +29,7 @@
     "max_vel_x": 0.4,
     "max_vel_x_backwards": 0.2,
     "max_vel_y": 0.0,
+    "max_vel_trans": 0.5,
     "max_vel_theta": 0.3,
     "acc_lim_x": 0.5,
     "acc_lim_y": 0.5,
@@ -86,7 +87,6 @@
     "max_number_classes": 5,
     "max_number_plans_in_current_class": 0,
     "selection_cost_hysteresis": 1.0,
-    "selection_prefer_initial_plan": 0.95,
     "selection_obst_cost_scale": 100.0,
     "selection_viapoint_cost_scale": 1.0,
     "selection_alternative_time_cost": false,
-- 
GitLab


From 4c95256a7ca6e27dc9d297b1f7cdcc2ef3eed484 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Thu, 11 Aug 2022 20:06:38 +0200
Subject: [PATCH 10/18] Adjust default velocity

---
 .../local_planner_config/TimedElasticBands/default.json     | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
index ed871ab9..a689e85d 100644
--- a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
+++ b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
@@ -26,9 +26,9 @@
   },
 
   "robot": {
-    "max_vel_x": 0.4,
-    "max_vel_x_backwards": 0.2,
-    "max_vel_y": 0.0,
+    "max_vel_x": 0.5,
+    "max_vel_x_backwards": 0.5,
+    "max_vel_y": 0.5,
     "max_vel_trans": 0.5,
     "max_vel_theta": 0.3,
     "acc_lim_x": 0.5,
-- 
GitLab


From 2522bc14e273f676d6276cf2962c9e9b2c6f91aa Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Thu, 11 Aug 2022 20:07:11 +0200
Subject: [PATCH 11/18] Implement paramter changes in aron and add conversion

---
 .../navigation/local_planning/aron/TimedElasticBands.xml  | 8 ++++----
 .../armarx/navigation/local_planning/aron_conversions.cpp | 2 +-
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml b/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
index b0f90f17..452a953b 100644
--- a/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
+++ b/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
@@ -102,6 +102,10 @@
                 <float />
             </ObjectChild>
 
+            <ObjectChild key='max_vel_trans'>
+                <float />
+            </ObjectChild>
+
             <ObjectChild key='max_vel_theta'>
                 <float />
             </ObjectChild>
@@ -290,10 +294,6 @@
                 <float />
             </ObjectChild>
 
-            <ObjectChild key='selection_prefer_initial_plan'>
-                <float />
-            </ObjectChild>
-
             <ObjectChild key='selection_obst_cost_scale'>
                 <float />
             </ObjectChild>
diff --git a/source/armarx/navigation/local_planning/aron_conversions.cpp b/source/armarx/navigation/local_planning/aron_conversions.cpp
index 4a93ffcd..000eabca 100644
--- a/source/armarx/navigation/local_planning/aron_conversions.cpp
+++ b/source/armarx/navigation/local_planning/aron_conversions.cpp
@@ -59,6 +59,7 @@ namespace armarx::navigation::local_planning
         bo.robot.max_vel_x = dto.robot.max_vel_x;
         bo.robot.max_vel_x_backwards = dto.robot.max_vel_x_backwards;
         bo.robot.max_vel_y = dto.robot.max_vel_y;
+        bo.robot.max_vel_trans = dto.robot.max_vel_trans;
         bo.robot.max_vel_theta = dto.robot.max_vel_theta;
         bo.robot.acc_lim_x = dto.robot.acc_lim_x;
         bo.robot.acc_lim_y = dto.robot.acc_lim_y;
@@ -116,7 +117,6 @@ namespace armarx::navigation::local_planning
         bo.hcp.max_number_classes = dto.hcp.max_number_classes;
         bo.hcp.max_number_plans_in_current_class = dto.hcp.max_number_plans_in_current_class;
         bo.hcp.selection_cost_hysteresis = dto.hcp.selection_cost_hysteresis;
-        bo.hcp.selection_prefer_initial_plan = dto.hcp.selection_prefer_initial_plan;
         bo.hcp.selection_obst_cost_scale = dto.hcp.selection_obst_cost_scale;
         bo.hcp.selection_viapoint_cost_scale = dto.hcp.selection_viapoint_cost_scale;
         bo.hcp.selection_alternative_time_cost = dto.hcp.selection_alternative_time_cost;
-- 
GitLab


From 8ae12c873c0fff8334c02e6c924eaf4dfdff2edd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Thu, 11 Aug 2022 21:23:18 +0200
Subject: [PATCH 12/18] Fix bug in loadDefaultConfig

---
 source/armarx/navigation/local_planning/TimedElasticBands.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index 8084e1fb..0d61b9ed 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -95,7 +95,7 @@ namespace armarx::navigation::local_planning
 
         dto.read(reader, jsonConfig);
 
-        toTebCfg(params.cfg, cfg_);
+        toTebCfg(dto, cfg_);
     }
 
     std::optional<LocalPlannerResult>
-- 
GitLab


From 97748d9d2b10c7ac6958933f1f188bf925063cbf Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Thu, 11 Aug 2022 21:26:18 +0200
Subject: [PATCH 13/18] Increase default velocity limit

---
 .../local_planner_config/TimedElasticBands/default.json         | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
index a689e85d..f862ff02 100644
--- a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
+++ b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
@@ -30,7 +30,7 @@
     "max_vel_x_backwards": 0.5,
     "max_vel_y": 0.5,
     "max_vel_trans": 0.5,
-    "max_vel_theta": 0.3,
+    "max_vel_theta": 0.5,
     "acc_lim_x": 0.5,
     "acc_lim_y": 0.5,
     "acc_lim_theta": 0.5,
-- 
GitLab


From cecccfb086433209b563212207fe04c9c69ac1f6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Thu, 11 Aug 2022 22:39:48 +0200
Subject: [PATCH 14/18] Add new config parameters

---
 .../TimedElasticBands/default.json            | 201 +++++++++---------
 .../local_planning/TimedElasticBands.cpp      |  34 ++-
 .../local_planning/TimedElasticBands.h        |   4 +-
 .../local_planning/aron/TimedElasticBands.xml |  16 +-
 .../local_planning/aron_conversions.cpp       | 184 ++++++++--------
 5 files changed, 230 insertions(+), 209 deletions(-)

diff --git a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
index f862ff02..e2c40a1b 100644
--- a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
+++ b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
@@ -1,111 +1,114 @@
 {
-  "pse": {
-    "pse_costum_obstacle_penalties": true,
-    "pse_costum_obstacle_penalties_dynamic": true,
-    "weight_global_path": 0.1,
-    "lrk_use_alternative_approach": false,
-    "lrk_enable_collision_check": true,
-    "hybrid_homotopy_calculation": true,
-    "robot_diff_circumscribed_inscribed_radius": 0.0
-  },
+  "robot_footprint_radius": 0.2,
+  "planning_distance": 2,
+  "teb_config": {
+    "pse": {
+      "pse_costum_obstacle_penalties": true,
+      "pse_costum_obstacle_penalties_dynamic": true,
+      "weight_global_path": 0.1,
+      "lrk_use_alternative_approach": false,
+      "lrk_enable_collision_check": true,
+      "hybrid_homotopy_calculation": true,
+      "robot_diff_circumscribed_inscribed_radius": 0.0
+    },
 
-  "trajectory": {
-    "teb_autosize": true,
-    "dt_ref": 0.3,
-    "dt_hysteresis": 0.1,
-    "min_samples": 3,
-    "max_samples": 500,
-    "global_plan_overwrite_orientation": true,
-    "allow_init_with_backwards_motion": false,
-    "exact_arc_length": false,
-    "force_reinit_new_goal_dist": 1,
-    "force_reinit_new_goal_angular": 1.5707963267948966,
-    "feasibility_check_no_poses": 5,
-    "feasibility_check_lookahead_distance": -1,
-    "min_resolution_collision_check_angular": 3.141592653589793
-  },
+    "trajectory": {
+      "teb_autosize": true,
+      "dt_ref": 0.3,
+      "dt_hysteresis": 0.1,
+      "min_samples": 3,
+      "max_samples": 500,
+      "global_plan_overwrite_orientation": true,
+      "allow_init_with_backwards_motion": false,
+      "exact_arc_length": false,
+      "force_reinit_new_goal_dist": 1,
+      "force_reinit_new_goal_angular": 1.5707963267948966,
+      "feasibility_check_no_poses": 5,
+      "feasibility_check_lookahead_distance": -1,
+      "min_resolution_collision_check_angular": 3.141592653589793
+    },
 
-  "robot": {
-    "max_vel_x": 0.5,
-    "max_vel_x_backwards": 0.5,
-    "max_vel_y": 0.5,
-    "max_vel_trans": 0.5,
-    "max_vel_theta": 0.5,
-    "acc_lim_x": 0.5,
-    "acc_lim_y": 0.5,
-    "acc_lim_theta": 0.5,
-    "min_turning_radius": 0
-  },
+    "robot": {
+      "max_vel_x": 0.5,
+      "max_vel_x_backwards": 0.5,
+      "max_vel_y": 0.5,
+      "max_vel_trans": 0.5,
+      "max_vel_theta": 0.5,
+      "acc_lim_x": 0.5,
+      "acc_lim_y": 0.5,
+      "acc_lim_theta": 0.5,
+      "min_turning_radius": 0
+    },
 
-  "goal_tolerance": {
-    "xy_goal_tolerance": 0.2
-  },
+    "goal_tolerance": {
+      "xy_goal_tolerance": 0.2
+    },
 
-  "obstacles": {
-    "min_obstacle_dist": 0.5,
-    "inflation_dist": 0.0,
-    "dynamic_obstacle_inflation_dist": 0.6,
-    "include_dynamic_obstacles": true,
-    "obstacle_association_force_inclusion_factor": 1.5,
-    "obstacle_association_cutoff_factor": 5,
-    "obstacle_proximity_ratio_max_vel": 1,
-    "obstacle_proximity_lower_bound": 0,
-    "obstacle_proximity_upper_bound": 0.5
-  },
+    "obstacles": {
+      "min_obstacle_dist": 0.5,
+      "inflation_dist": 0.0,
+      "dynamic_obstacle_inflation_dist": 0.6,
+      "include_dynamic_obstacles": true,
+      "obstacle_association_force_inclusion_factor": 1.5,
+      "obstacle_association_cutoff_factor": 5,
+      "obstacle_proximity_ratio_max_vel": 1,
+      "obstacle_proximity_lower_bound": 0,
+      "obstacle_proximity_upper_bound": 0.5
+    },
 
-  "optim": {
-    "no_inner_iterations": 5,
-    "no_outer_iterations": 4,
-    "optimization_activate": true,
-    "optimization_verbose": false,
-    "penalty_epsilon": 0.05,
-    "weight_max_vel_x": 2,
-    "weight_max_vel_y": 2,
-    "weight_max_vel_theta": 1,
-    "weight_acc_lim_x": 1,
-    "weight_acc_lim_y": 1,
-    "weight_acc_lim_theta": 1,
-    "weight_kinematics_nh": 1000,
-    "weight_kinematics_forward_drive": 1,
-    "weight_kinematics_turning_radius": 1,
-    "weight_optimaltime": 1,
-    "weight_shortest_path": 0,
-    "weight_obstacle": 50,
-    "weight_inflation": 0.1,
-    "weight_dynamic_obstacle": 50,
-    "weight_dynamic_obstacle_inflation": 0.1,
-    "weight_velocity_obstacle_ratio": 0,
-    "weight_viapoint": 1,
-    "weight_prefer_rotdir": 50,
-    "weight_adapt_factor": 2.0,
-    "obstacle_cost_exponent": 1.0
-  },
+    "optim": {
+      "no_inner_iterations": 5,
+      "no_outer_iterations": 4,
+      "optimization_activate": true,
+      "optimization_verbose": false,
+      "penalty_epsilon": 0.05,
+      "weight_max_vel_x": 2,
+      "weight_max_vel_y": 2,
+      "weight_max_vel_theta": 1,
+      "weight_acc_lim_x": 1,
+      "weight_acc_lim_y": 1,
+      "weight_acc_lim_theta": 1,
+      "weight_kinematics_nh": 1000,
+      "weight_kinematics_forward_drive": 1,
+      "weight_kinematics_turning_radius": 1,
+      "weight_optimaltime": 1,
+      "weight_shortest_path": 0,
+      "weight_obstacle": 50,
+      "weight_inflation": 0.1,
+      "weight_dynamic_obstacle": 50,
+      "weight_dynamic_obstacle_inflation": 0.1,
+      "weight_velocity_obstacle_ratio": 0,
+      "weight_viapoint": 1,
+      "weight_prefer_rotdir": 50,
+      "weight_adapt_factor": 2.0,
+      "obstacle_cost_exponent": 1.0
+    },
 
-  "hcp": {
-    "enable_multithreading": true,
-    "simple_exploration": false,
-    "max_number_classes": 5,
-    "max_number_plans_in_current_class": 0,
-    "selection_cost_hysteresis": 1.0,
-    "selection_obst_cost_scale": 100.0,
-    "selection_viapoint_cost_scale": 1.0,
-    "selection_alternative_time_cost": false,
-    "selection_dropping_probability": 0.0,
-    "switching_blocking_period": 0.0,
+    "hcp": {
+      "enable_multithreading": true,
+      "simple_exploration": false,
+      "max_number_classes": 5,
+      "max_number_plans_in_current_class": 0,
+      "selection_cost_hysteresis": 1.0,
+      "selection_obst_cost_scale": 100.0,
+      "selection_viapoint_cost_scale": 1.0,
+      "selection_alternative_time_cost": false,
+      "selection_dropping_probability": 0.0,
+      "switching_blocking_period": 0.0,
 
-    "obstacle_heading_threshold": 0.45,
-    "roadmap_graph_no_samples": 20,
-    "roadmap_graph_area_width": 10,
-    "roadmap_graph_area_length_scale": 1.0,
-    "h_signature_prescaler": 1,
-    "h_signature_threshold": 0.1,
+      "obstacle_heading_threshold": 0.45,
+      "roadmap_graph_no_samples": 20,
+      "roadmap_graph_area_width": 10,
+      "roadmap_graph_area_length_scale": 1.0,
+      "h_signature_prescaler": 1,
+      "h_signature_threshold": 0.1,
 
-    "viapoints_all_candidates": true,
+      "viapoints_all_candidates": true,
 
-    "delete_detours_backwards": true,
-    "detours_orientation_tolerance": 1.5707963267948966,
-    "length_start_orientation_vector": 0.4,
-    "max_ratio_detours_duration_best_duration": 3.0
+      "delete_detours_backwards": true,
+      "detours_orientation_tolerance": 1.5707963267948966,
+      "length_start_orientation_vector": 0.4,
+      "max_ratio_detours_duration_best_duration": 3.0
+    }
   }
-
 }
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index 35881b66..446b5743 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -51,19 +51,18 @@ namespace armarx::navigation::local_planning
 
     // TimedElasticBands
 
-    TimedElasticBands::TimedElasticBands(const Params& params, const core::Scene& ctx) :
-        LocalPlanner(ctx), params(params), scene(ctx), obstManager(teb_obstacles)
+    TimedElasticBands::TimedElasticBands(const Params& i_params, const core::Scene& ctx) :
+        LocalPlanner(ctx), params(i_params), scene(ctx), obstManager(teb_obstacles)
     {
-        //TODO (SALt): init configuration, robot footprint
-        if (false)
+        //TODO (SALt): find proper place to init with default config
+        if (true)
         {
-            toTebCfg(params.cfg, cfg_);
+            readDefaultConfig(params.cfg);
         }
-        else
-        {
-            initDefaultConfig();
-        }
-        auto robot_model = boost::make_shared<teb_local_planner::CircularRobotFootprint>(0.2);
+        toTebCfg(params.cfg, cfg_);
+
+        auto robot_model = boost::make_shared<teb_local_planner::CircularRobotFootprint>(
+            params.cfg.robot_footprint_radius);
 
         hcp_ = std::make_unique<teb_local_planner::HomotopyClassPlanner>();
         hcp_->initialize(
@@ -73,10 +72,8 @@ namespace armarx::navigation::local_planning
     }
 
     void
-    TimedElasticBands::initDefaultConfig()
+    TimedElasticBands::readDefaultConfig(arondto::TimedElasticBandsParams& target)
     {
-        arondto::TimedElasticBandsParams dto;
-
         const armarx::PackagePath configPath("armarx_navigation",
                                              "local_planner_config/TimedElasticBands/default.json");
         const std::filesystem::path file = configPath.toSystemPath();
@@ -89,13 +86,10 @@ namespace armarx::navigation::local_planning
         nlohmann::json jsonConfig;
         ifs >> jsonConfig;
 
-        ARMARX_INFO << "Initializing config";
+        ARMARX_INFO << "Reading config";
 
         armarx::aron::data::reader::NlohmannJSONReaderWithoutTypeCheck reader;
-
-        dto.read(reader, jsonConfig);
-
-        toTebCfg(dto, cfg_);
+        target.read(reader, jsonConfig);
     }
 
     std::optional<LocalPlannerResult>
@@ -103,10 +97,9 @@ namespace armarx::navigation::local_planning
     {
         const core::Pose currentPose{scene.robot->getGlobalPose()};
 
-        // TODO (SALt): put distance parameter into config (note: distance is in mm)
         // prune global trajectory
         const auto& [prunedGoal, planToDest] =
-            goal.getSubTrajectory(currentPose.translation(), 2000);
+            goal.getSubTrajectory(currentPose.translation(), params.cfg.planning_distance * 1000);
 
         const teb_local_planner::TimedElasticBand globalPath = conv::toRos(prunedGoal);
         teb_globalPath = globalPath.poses();
@@ -124,6 +117,7 @@ namespace armarx::navigation::local_planning
         try
         {
             // TODO (SALt): free goal velocity is not respected inside homotopy planner
+            ARMARX_VERBOSE << "Plan to dest: " << planToDest << ", freeGoal Vel: " << !planToDest;
             hcp_->plan(start, end, &velocity_start, !planToDest);
         }
         catch (std::exception& e)
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.h b/source/armarx/navigation/local_planning/TimedElasticBands.h
index 6bded067..9eba4d52 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.h
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.h
@@ -59,11 +59,11 @@ namespace armarx::navigation::local_planning
         std::optional<LocalPlannerResult> plan(const core::GlobalTrajectory& goal) override;
 
     private:
-        void initDefaultConfig();
+        void readDefaultConfig(arondto::TimedElasticBandsParams& target);
         void fillObstacles();
 
     protected:
-        const Params params;
+        Params params;
 
     private:
         const core::Scene& scene;
diff --git a/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml b/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
index 452a953b..4c3c9249 100644
--- a/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
+++ b/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
@@ -362,7 +362,7 @@
             </ObjectChild>
         </Object>
 
-        <Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams'>
+        <Object name='armarx::navigation::local_planning::arondto::teb_config'>
             <ObjectChild key='pse'>
                 <armarx::navigation::local_planning::arondto::pse />
             </ObjectChild>
@@ -386,5 +386,19 @@
             </ObjectChild>
         </Object>
 
+        <Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams'>
+            <ObjectChild key='robot_footprint_radius'>
+                <float />
+            </ObjectChild>
+            <ObjectChild key='planning_distance'>
+                <float />
+            </ObjectChild>
+            <ObjectChild key='teb_config'>
+                <armarx::navigation::local_planning::arondto::teb_config />
+            </ObjectChild>
+        </Object>
+
+
+
     </GenerateTypes>
 </AronTypeDefinition>
diff --git a/source/armarx/navigation/local_planning/aron_conversions.cpp b/source/armarx/navigation/local_planning/aron_conversions.cpp
index 000eabca..220a02b3 100644
--- a/source/armarx/navigation/local_planning/aron_conversions.cpp
+++ b/source/armarx/navigation/local_planning/aron_conversions.cpp
@@ -26,117 +26,127 @@ namespace armarx::navigation::local_planning
     void
     toTebCfg(const arondto::TimedElasticBandsParams& dto, teb_local_planner::TebConfig& bo)
     {
-        bo.pse.pse_costum_obstacle_penalties = dto.pse.pse_costum_obstacle_penalties;
+        bo.pse.pse_costum_obstacle_penalties = dto.teb_config.pse.pse_costum_obstacle_penalties;
         bo.pse.pse_costum_obstacle_penalties_dynamic =
-            dto.pse.pse_costum_obstacle_penalties_dynamic;
-        bo.pse.weight_global_path = dto.pse.weight_global_path;
-        bo.pse.lrk_use_alternative_approach = dto.pse.lrk_use_alternative_approach;
-        bo.pse.lrk_enable_collision_check = dto.pse.lrk_enable_collision_check;
-        bo.pse.hybrid_homotopy_calculation = dto.pse.hybrid_homotopy_calculation;
+            dto.teb_config.pse.pse_costum_obstacle_penalties_dynamic;
+        bo.pse.weight_global_path = dto.teb_config.pse.weight_global_path;
+        bo.pse.lrk_use_alternative_approach = dto.teb_config.pse.lrk_use_alternative_approach;
+        bo.pse.lrk_enable_collision_check = dto.teb_config.pse.lrk_enable_collision_check;
+        bo.pse.hybrid_homotopy_calculation = dto.teb_config.pse.hybrid_homotopy_calculation;
         bo.pse.robot_diff_circumscribed_inscribed_radius =
-            dto.pse.robot_diff_circumscribed_inscribed_radius;
+            dto.teb_config.pse.robot_diff_circumscribed_inscribed_radius;
 
         // Trajectory
         bo.trajectory.teb_autosize = bo.trajectory.teb_autosize;
-        bo.trajectory.dt_ref = dto.trajectory.dt_ref;
-        bo.trajectory.dt_hysteresis = dto.trajectory.dt_hysteresis;
-        bo.trajectory.min_samples = dto.trajectory.min_samples;
-        bo.trajectory.max_samples = dto.trajectory.max_samples;
+        bo.trajectory.dt_ref = dto.teb_config.trajectory.dt_ref;
+        bo.trajectory.dt_hysteresis = dto.teb_config.trajectory.dt_hysteresis;
+        bo.trajectory.min_samples = dto.teb_config.trajectory.min_samples;
+        bo.trajectory.max_samples = dto.teb_config.trajectory.max_samples;
         bo.trajectory.global_plan_overwrite_orientation =
-            dto.trajectory.global_plan_overwrite_orientation;
+            dto.teb_config.trajectory.global_plan_overwrite_orientation;
         bo.trajectory.allow_init_with_backwards_motion =
-            dto.trajectory.allow_init_with_backwards_motion;
-        bo.trajectory.exact_arc_length = dto.trajectory.exact_arc_length;
-        bo.trajectory.force_reinit_new_goal_dist = dto.trajectory.force_reinit_new_goal_dist;
-        bo.trajectory.force_reinit_new_goal_angular = dto.trajectory.force_reinit_new_goal_angular;
-        bo.trajectory.feasibility_check_no_poses = dto.trajectory.feasibility_check_no_poses;
+            dto.teb_config.trajectory.allow_init_with_backwards_motion;
+        bo.trajectory.exact_arc_length = dto.teb_config.trajectory.exact_arc_length;
+        bo.trajectory.force_reinit_new_goal_dist =
+            dto.teb_config.trajectory.force_reinit_new_goal_dist;
+        bo.trajectory.force_reinit_new_goal_angular =
+            dto.teb_config.trajectory.force_reinit_new_goal_angular;
+        bo.trajectory.feasibility_check_no_poses =
+            dto.teb_config.trajectory.feasibility_check_no_poses;
         bo.trajectory.feasibility_check_lookahead_distance =
-            dto.trajectory.feasibility_check_lookahead_distance;
+            dto.teb_config.trajectory.feasibility_check_lookahead_distance;
         bo.trajectory.min_resolution_collision_check_angular =
-            dto.trajectory.min_resolution_collision_check_angular;
+            dto.teb_config.trajectory.min_resolution_collision_check_angular;
 
         // Robot
-        bo.robot.max_vel_x = dto.robot.max_vel_x;
-        bo.robot.max_vel_x_backwards = dto.robot.max_vel_x_backwards;
-        bo.robot.max_vel_y = dto.robot.max_vel_y;
-        bo.robot.max_vel_trans = dto.robot.max_vel_trans;
-        bo.robot.max_vel_theta = dto.robot.max_vel_theta;
-        bo.robot.acc_lim_x = dto.robot.acc_lim_x;
-        bo.robot.acc_lim_y = dto.robot.acc_lim_y;
-        bo.robot.acc_lim_theta = dto.robot.acc_lim_theta;
-        bo.robot.min_turning_radius = dto.robot.min_turning_radius;
+        bo.robot.max_vel_x = dto.teb_config.robot.max_vel_x;
+        bo.robot.max_vel_x_backwards = dto.teb_config.robot.max_vel_x_backwards;
+        bo.robot.max_vel_y = dto.teb_config.robot.max_vel_y;
+        bo.robot.max_vel_trans = dto.teb_config.robot.max_vel_trans;
+        bo.robot.max_vel_theta = dto.teb_config.robot.max_vel_theta;
+        bo.robot.acc_lim_x = dto.teb_config.robot.acc_lim_x;
+        bo.robot.acc_lim_y = dto.teb_config.robot.acc_lim_y;
+        bo.robot.acc_lim_theta = dto.teb_config.robot.acc_lim_theta;
+        bo.robot.min_turning_radius = dto.teb_config.robot.min_turning_radius;
 
         // GoalTolerance
-        bo.goal_tolerance.xy_goal_tolerance = dto.goal_tolerance.xy_goal_tolerance;
+        bo.goal_tolerance.xy_goal_tolerance = dto.teb_config.goal_tolerance.xy_goal_tolerance;
 
         // Obstacles
-        bo.obstacles.min_obstacle_dist = dto.obstacles.min_obstacle_dist;
-        bo.obstacles.inflation_dist = dto.obstacles.inflation_dist;
+        bo.obstacles.min_obstacle_dist = dto.teb_config.obstacles.min_obstacle_dist;
+        bo.obstacles.inflation_dist = dto.teb_config.obstacles.inflation_dist;
         bo.obstacles.dynamic_obstacle_inflation_dist =
-            dto.obstacles.dynamic_obstacle_inflation_dist;
-        bo.obstacles.include_dynamic_obstacles = dto.obstacles.include_dynamic_obstacles;
+            dto.teb_config.obstacles.dynamic_obstacle_inflation_dist;
+        bo.obstacles.include_dynamic_obstacles = dto.teb_config.obstacles.include_dynamic_obstacles;
         bo.obstacles.obstacle_association_force_inclusion_factor =
-            dto.obstacles.obstacle_association_force_inclusion_factor;
+            dto.teb_config.obstacles.obstacle_association_force_inclusion_factor;
         bo.obstacles.obstacle_association_cutoff_factor =
-            dto.obstacles.obstacle_association_cutoff_factor;
+            dto.teb_config.obstacles.obstacle_association_cutoff_factor;
         bo.obstacles.obstacle_proximity_ratio_max_vel =
-            dto.obstacles.obstacle_proximity_ratio_max_vel;
-        bo.obstacles.obstacle_proximity_lower_bound = dto.obstacles.obstacle_proximity_lower_bound;
-        bo.obstacles.obstacle_proximity_upper_bound = dto.obstacles.obstacle_proximity_upper_bound;
+            dto.teb_config.obstacles.obstacle_proximity_ratio_max_vel;
+        bo.obstacles.obstacle_proximity_lower_bound =
+            dto.teb_config.obstacles.obstacle_proximity_lower_bound;
+        bo.obstacles.obstacle_proximity_upper_bound =
+            dto.teb_config.obstacles.obstacle_proximity_upper_bound;
 
         // Optimization
-        bo.optim.no_inner_iterations = dto.optim.no_inner_iterations;
-        bo.optim.no_outer_iterations = dto.optim.no_outer_iterations;
-        bo.optim.optimization_activate = dto.optim.optimization_activate;
-        bo.optim.optimization_verbose = dto.optim.optimization_verbose;
-        bo.optim.penalty_epsilon = dto.optim.penalty_epsilon;
-        bo.optim.weight_max_vel_x = dto.optim.weight_max_vel_x;
-        bo.optim.weight_max_vel_y = dto.optim.weight_max_vel_y;
-        bo.optim.weight_max_vel_theta = dto.optim.weight_max_vel_theta;
-        bo.optim.weight_acc_lim_x = dto.optim.weight_acc_lim_x;
-        bo.optim.weight_acc_lim_y = dto.optim.weight_acc_lim_y;
-        bo.optim.weight_acc_lim_theta = dto.optim.weight_acc_lim_theta;
-        bo.optim.weight_kinematics_nh = dto.optim.weight_kinematics_nh;
-        bo.optim.weight_kinematics_forward_drive = dto.optim.weight_kinematics_forward_drive;
-        bo.optim.weight_kinematics_turning_radius = dto.optim.weight_kinematics_turning_radius;
-        bo.optim.weight_optimaltime = dto.optim.weight_optimaltime;
-        bo.optim.weight_shortest_path = dto.optim.weight_shortest_path;
-        bo.optim.weight_obstacle = dto.optim.weight_obstacle;
-        bo.optim.weight_inflation = dto.optim.weight_inflation;
-        bo.optim.weight_dynamic_obstacle = dto.optim.weight_dynamic_obstacle;
-        bo.optim.weight_dynamic_obstacle_inflation = dto.optim.weight_dynamic_obstacle_inflation;
-        bo.optim.weight_velocity_obstacle_ratio = dto.optim.weight_velocity_obstacle_ratio;
-        bo.optim.weight_viapoint = dto.optim.weight_viapoint;
-        bo.optim.weight_prefer_rotdir = dto.optim.weight_prefer_rotdir;
-        bo.optim.weight_adapt_factor = dto.optim.weight_adapt_factor;
-        bo.optim.obstacle_cost_exponent = dto.optim.obstacle_cost_exponent;
+        bo.optim.no_inner_iterations = dto.teb_config.optim.no_inner_iterations;
+        bo.optim.no_outer_iterations = dto.teb_config.optim.no_outer_iterations;
+        bo.optim.optimization_activate = dto.teb_config.optim.optimization_activate;
+        bo.optim.optimization_verbose = dto.teb_config.optim.optimization_verbose;
+        bo.optim.penalty_epsilon = dto.teb_config.optim.penalty_epsilon;
+        bo.optim.weight_max_vel_x = dto.teb_config.optim.weight_max_vel_x;
+        bo.optim.weight_max_vel_y = dto.teb_config.optim.weight_max_vel_y;
+        bo.optim.weight_max_vel_theta = dto.teb_config.optim.weight_max_vel_theta;
+        bo.optim.weight_acc_lim_x = dto.teb_config.optim.weight_acc_lim_x;
+        bo.optim.weight_acc_lim_y = dto.teb_config.optim.weight_acc_lim_y;
+        bo.optim.weight_acc_lim_theta = dto.teb_config.optim.weight_acc_lim_theta;
+        bo.optim.weight_kinematics_nh = dto.teb_config.optim.weight_kinematics_nh;
+        bo.optim.weight_kinematics_forward_drive =
+            dto.teb_config.optim.weight_kinematics_forward_drive;
+        bo.optim.weight_kinematics_turning_radius =
+            dto.teb_config.optim.weight_kinematics_turning_radius;
+        bo.optim.weight_optimaltime = dto.teb_config.optim.weight_optimaltime;
+        bo.optim.weight_shortest_path = dto.teb_config.optim.weight_shortest_path;
+        bo.optim.weight_obstacle = dto.teb_config.optim.weight_obstacle;
+        bo.optim.weight_inflation = dto.teb_config.optim.weight_inflation;
+        bo.optim.weight_dynamic_obstacle = dto.teb_config.optim.weight_dynamic_obstacle;
+        bo.optim.weight_dynamic_obstacle_inflation =
+            dto.teb_config.optim.weight_dynamic_obstacle_inflation;
+        bo.optim.weight_velocity_obstacle_ratio =
+            dto.teb_config.optim.weight_velocity_obstacle_ratio;
+        bo.optim.weight_viapoint = dto.teb_config.optim.weight_viapoint;
+        bo.optim.weight_prefer_rotdir = dto.teb_config.optim.weight_prefer_rotdir;
+        bo.optim.weight_adapt_factor = dto.teb_config.optim.weight_adapt_factor;
+        bo.optim.obstacle_cost_exponent = dto.teb_config.optim.obstacle_cost_exponent;
 
         // Homotopy Class Planner
-        bo.hcp.enable_multithreading = dto.hcp.enable_multithreading;
-        bo.hcp.simple_exploration = dto.hcp.simple_exploration;
-        bo.hcp.max_number_classes = dto.hcp.max_number_classes;
-        bo.hcp.max_number_plans_in_current_class = dto.hcp.max_number_plans_in_current_class;
-        bo.hcp.selection_cost_hysteresis = dto.hcp.selection_cost_hysteresis;
-        bo.hcp.selection_obst_cost_scale = dto.hcp.selection_obst_cost_scale;
-        bo.hcp.selection_viapoint_cost_scale = dto.hcp.selection_viapoint_cost_scale;
-        bo.hcp.selection_alternative_time_cost = dto.hcp.selection_alternative_time_cost;
-        bo.hcp.selection_dropping_probability = dto.hcp.selection_dropping_probability;
-        bo.hcp.switching_blocking_period = dto.hcp.switching_blocking_period;
-
-        bo.hcp.obstacle_heading_threshold = dto.hcp.obstacle_heading_threshold;
-        bo.hcp.roadmap_graph_no_samples = dto.hcp.roadmap_graph_no_samples;
-        bo.hcp.roadmap_graph_area_width = dto.hcp.roadmap_graph_area_width;
-        bo.hcp.roadmap_graph_area_length_scale = dto.hcp.roadmap_graph_area_length_scale;
-        bo.hcp.h_signature_prescaler = dto.hcp.h_signature_prescaler;
-        bo.hcp.h_signature_threshold = dto.hcp.h_signature_threshold;
-
-        bo.hcp.viapoints_all_candidates = dto.hcp.viapoints_all_candidates;
-
-        bo.hcp.delete_detours_backwards = dto.hcp.delete_detours_backwards;
-        bo.hcp.detours_orientation_tolerance = dto.hcp.detours_orientation_tolerance;
-        bo.hcp.length_start_orientation_vector = dto.hcp.length_start_orientation_vector;
+        bo.hcp.enable_multithreading = dto.teb_config.hcp.enable_multithreading;
+        bo.hcp.simple_exploration = dto.teb_config.hcp.simple_exploration;
+        bo.hcp.max_number_classes = dto.teb_config.hcp.max_number_classes;
+        bo.hcp.max_number_plans_in_current_class =
+            dto.teb_config.hcp.max_number_plans_in_current_class;
+        bo.hcp.selection_cost_hysteresis = dto.teb_config.hcp.selection_cost_hysteresis;
+        bo.hcp.selection_obst_cost_scale = dto.teb_config.hcp.selection_obst_cost_scale;
+        bo.hcp.selection_viapoint_cost_scale = dto.teb_config.hcp.selection_viapoint_cost_scale;
+        bo.hcp.selection_alternative_time_cost = dto.teb_config.hcp.selection_alternative_time_cost;
+        bo.hcp.selection_dropping_probability = dto.teb_config.hcp.selection_dropping_probability;
+        bo.hcp.switching_blocking_period = dto.teb_config.hcp.switching_blocking_period;
+
+        bo.hcp.obstacle_heading_threshold = dto.teb_config.hcp.obstacle_heading_threshold;
+        bo.hcp.roadmap_graph_no_samples = dto.teb_config.hcp.roadmap_graph_no_samples;
+        bo.hcp.roadmap_graph_area_width = dto.teb_config.hcp.roadmap_graph_area_width;
+        bo.hcp.roadmap_graph_area_length_scale = dto.teb_config.hcp.roadmap_graph_area_length_scale;
+        bo.hcp.h_signature_prescaler = dto.teb_config.hcp.h_signature_prescaler;
+        bo.hcp.h_signature_threshold = dto.teb_config.hcp.h_signature_threshold;
+
+        bo.hcp.viapoints_all_candidates = dto.teb_config.hcp.viapoints_all_candidates;
+
+        bo.hcp.delete_detours_backwards = dto.teb_config.hcp.delete_detours_backwards;
+        bo.hcp.detours_orientation_tolerance = dto.teb_config.hcp.detours_orientation_tolerance;
+        bo.hcp.length_start_orientation_vector = dto.teb_config.hcp.length_start_orientation_vector;
         bo.hcp.max_ratio_detours_duration_best_duration =
-            dto.hcp.max_ratio_detours_duration_best_duration;
+            dto.teb_config.hcp.max_ratio_detours_duration_best_duration;
 
         bo.checkParameters();
     }
-- 
GitLab


From 73bdc265aa93c8ddcee893f1c63f8db50faeb74e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Fri, 12 Aug 2022 01:17:11 +0200
Subject: [PATCH 15/18] adjust config

---
 .../local_planner_config/TimedElasticBands/default.json    | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
index e2c40a1b..4b298708 100644
--- a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
+++ b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
@@ -1,6 +1,7 @@
 {
-  "robot_footprint_radius": 0.2,
-  "planning_distance": 2,
+  "robot_footprint_radius": 0.5,
+  "planning_distance": 3,
+
   "teb_config": {
     "pse": {
       "pse_costum_obstacle_penalties": true,
@@ -45,7 +46,7 @@
     },
 
     "obstacles": {
-      "min_obstacle_dist": 0.5,
+      "min_obstacle_dist": 0.3,
       "inflation_dist": 0.0,
       "dynamic_obstacle_inflation_dist": 0.6,
       "include_dynamic_obstacles": true,
-- 
GitLab


From 753dd2f4e3f22625f870b10f0070fb4251f1c4d6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Fri, 12 Aug 2022 01:17:44 +0200
Subject: [PATCH 16/18] Remove todo, change velocity visualization upper limit

---
 source/armarx/navigation/local_planning/TimedElasticBands.cpp   | 2 --
 .../navigation/server/introspection/ArvizIntrospector.cpp       | 2 +-
 2 files changed, 1 insertion(+), 3 deletions(-)

diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index 446b5743..dc02d4c5 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -116,8 +116,6 @@ namespace armarx::navigation::local_planning
 
         try
         {
-            // TODO (SALt): free goal velocity is not respected inside homotopy planner
-            ARMARX_VERBOSE << "Plan to dest: " << planToDest << ", freeGoal Vel: " << !planToDest;
             hcp_->plan(start, end, &velocity_start, !planToDest);
         }
         catch (std::exception& e)
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
index 694ff174..cb1f306e 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
@@ -172,7 +172,7 @@ namespace armarx::navigation::server
 
         simox::ColorMap cm = simox::color::cmaps::inferno();
         cm.set_vmin(0);
-        cm.set_vmax(1);
+        cm.set_vmax(0.6);
 
         for (size_t i = 0; i < trajectory.points().size() - 1; i++)
         {
-- 
GitLab


From d56f1cd7d9b4c0b8964049a6b3874e8e1c803064 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Fri, 12 Aug 2022 01:48:00 +0200
Subject: [PATCH 17/18] Adjust default config

---
 .../local_planner_config/TimedElasticBands/default.json       | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
index 4b298708..0dcf777b 100644
--- a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
+++ b/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_global_path": 0.1,
+      "weight_global_path": 0.3,
       "lrk_use_alternative_approach": false,
       "lrk_enable_collision_check": true,
       "hybrid_homotopy_calculation": true,
@@ -73,7 +73,7 @@
       "weight_kinematics_forward_drive": 1,
       "weight_kinematics_turning_radius": 1,
       "weight_optimaltime": 1,
-      "weight_shortest_path": 0,
+      "weight_shortest_path": 1,
       "weight_obstacle": 50,
       "weight_inflation": 0.1,
       "weight_dynamic_obstacle": 50,
-- 
GitLab


From 81972ba7a4bf41755af799e97e1a518c90bac3b6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Fri, 12 Aug 2022 15:43:29 +0200
Subject: [PATCH 18/18] Add comment

---
 source/armarx/navigation/local_planning/TimedElasticBands.cpp | 4 ++--
 source/armarx/navigation/local_planning/ros_conversions.cpp   | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index dc02d4c5..de0514f6 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -98,8 +98,8 @@ namespace armarx::navigation::local_planning
         const core::Pose currentPose{scene.robot->getGlobalPose()};
 
         // prune global trajectory
-        const auto& [prunedGoal, planToDest] =
-            goal.getSubTrajectory(currentPose.translation(), params.cfg.planning_distance * 1000);
+        const auto& [prunedGoal, planToDest] = goal.getSubTrajectory(
+            currentPose.translation(), params.cfg.planning_distance * 1000); // [m] to [mm]
 
         const teb_local_planner::TimedElasticBand globalPath = conv::toRos(prunedGoal);
         teb_globalPath = globalPath.poses();
diff --git a/source/armarx/navigation/local_planning/ros_conversions.cpp b/source/armarx/navigation/local_planning/ros_conversions.cpp
index 3ad81d9c..c23adf1a 100644
--- a/source/armarx/navigation/local_planning/ros_conversions.cpp
+++ b/source/armarx/navigation/local_planning/ros_conversions.cpp
@@ -18,7 +18,7 @@ namespace armarx::navigation::conv
     Eigen::Vector2d
     toRos(const Eigen::Vector3f& vec)
     {
-        return conv::to2D(vec).cast<double>() / 1000;
+        return conv::to2D(vec).cast<double>() / 1000; // [mm] to [m]
     }
 
 
-- 
GitLab