Skip to content
Snippets Groups Projects
Commit 953dcbc9 authored by Tobias Gröger's avatar Tobias Gröger
Browse files

Read config from dto, read default config

parent 55075b47
No related branches found
No related tags found
2 merge requests!39Feature/teb config as json,!28Draft: Dev -> Main
Hide whitespace changes
Inline Side-by-side
source/armarx/navigation/local_planning/TimedElasticBands.cpp
+ 34
− 24
View file @ 953dcbc9
#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>
......
source/armarx/navigation/local_planning/TimedElasticBands.h
+ 1
− 1
View file @ 953dcbc9
......@@ -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();
......
source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
+ 1
− 0
View file @ 953dcbc9
......@@ -339,6 +339,7 @@
<float />
</ObjectChild>
<ObjectChild key='viapoints_all_candidates'>
<bool />
</ObjectChild>
......
source/armarx/navigation/local_planning/aron_conversions.cpp
+ 118
− 0
View file @ 953dcbc9
......@@ -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
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment