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] 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