aron_conversions_teb.cpp
Go to the documentation of this file.
1 #include "aron_conversions.h"
2 
3 #include <teb_local_planner/teb_config.h>
4 
8 
10 #include <armarx/navigation/local_planning/aron/TimedElasticBands.aron.generated.h>
11 
13 {
14 
15  void
16  toTebCfg(const arondto::TimedElasticBandsParams& dto, ::teb_local_planner::TebConfig& bo)
17  {
18  bo.pse.pse_costum_obstacle_penalties = dto.teb_config.pse.pse_costum_obstacle_penalties;
19  bo.pse.pse_costum_obstacle_penalties_dynamic =
20  dto.teb_config.pse.pse_costum_obstacle_penalties_dynamic;
21  bo.pse.weight_costmap = dto.teb_config.pse.weight_costmap;
22  bo.pse.weight_global_path_position = dto.teb_config.pse.weight_global_path_position;
23  bo.pse.weight_global_path_orientation = dto.teb_config.pse.weight_global_path_orientation;
24  bo.pse.lrk_use_alternative_approach = dto.teb_config.pse.lrk_use_alternative_approach;
25  bo.pse.lrk_enable_collision_check = dto.teb_config.pse.lrk_enable_collision_check;
26  bo.pse.hybrid_homotopy_calculation = dto.teb_config.pse.hybrid_homotopy_calculation;
27  bo.pse.robot_diff_circumscribed_inscribed_radius =
28  dto.teb_config.pse.robot_diff_circumscribed_inscribed_radius;
29 
30  // Trajectory
31  bo.trajectory.teb_autosize = bo.trajectory.teb_autosize;
32  bo.trajectory.dt_ref = dto.teb_config.trajectory.dt_ref;
33  bo.trajectory.dt_hysteresis = dto.teb_config.trajectory.dt_hysteresis;
34  bo.trajectory.min_samples = dto.teb_config.trajectory.min_samples;
35  bo.trajectory.max_samples = dto.teb_config.trajectory.max_samples;
36  bo.trajectory.global_plan_overwrite_orientation =
37  dto.teb_config.trajectory.global_plan_overwrite_orientation;
38  bo.trajectory.allow_init_with_backwards_motion =
39  dto.teb_config.trajectory.allow_init_with_backwards_motion;
40  bo.trajectory.exact_arc_length = dto.teb_config.trajectory.exact_arc_length;
41  bo.trajectory.force_reinit_new_goal_dist =
42  dto.teb_config.trajectory.force_reinit_new_goal_dist;
43  bo.trajectory.force_reinit_new_goal_angular =
44  dto.teb_config.trajectory.force_reinit_new_goal_angular;
45  bo.trajectory.feasibility_check_no_poses =
46  dto.teb_config.trajectory.feasibility_check_no_poses;
47  bo.trajectory.feasibility_check_lookahead_distance =
48  dto.teb_config.trajectory.feasibility_check_lookahead_distance;
49  bo.trajectory.min_resolution_collision_check_angular =
50  dto.teb_config.trajectory.min_resolution_collision_check_angular;
51 
52  // Robot
53  bo.robot.max_vel_x = dto.teb_config.robot.max_vel_x;
54  bo.robot.max_vel_x_backwards = dto.teb_config.robot.max_vel_x_backwards;
55  bo.robot.max_vel_y = dto.teb_config.robot.max_vel_y;
56  bo.robot.max_vel_trans = dto.teb_config.robot.max_vel_trans;
57  bo.robot.max_vel_theta = dto.teb_config.robot.max_vel_theta;
58  bo.robot.acc_lim_x = dto.teb_config.robot.acc_lim_x;
59  bo.robot.acc_lim_y = dto.teb_config.robot.acc_lim_y;
60  bo.robot.acc_lim_theta = dto.teb_config.robot.acc_lim_theta;
61  bo.robot.min_turning_radius = dto.teb_config.robot.min_turning_radius;
62 
63  // GoalTolerance
64  bo.goal_tolerance.xy_goal_tolerance = dto.teb_config.goal_tolerance.xy_goal_tolerance;
65 
66  // Obstacles
67  bo.obstacles.min_obstacle_dist = dto.teb_config.obstacles.min_obstacle_dist;
68  bo.obstacles.inflation_dist = dto.teb_config.obstacles.inflation_dist;
69  bo.obstacles.dynamic_obstacle_inflation_dist =
70  dto.teb_config.obstacles.dynamic_obstacle_inflation_dist;
71  bo.obstacles.include_dynamic_obstacles = dto.teb_config.obstacles.include_dynamic_obstacles;
72  bo.obstacles.obstacle_association_force_inclusion_factor =
73  dto.teb_config.obstacles.obstacle_association_force_inclusion_factor;
74  bo.obstacles.obstacle_association_cutoff_factor =
75  dto.teb_config.obstacles.obstacle_association_cutoff_factor;
76  bo.obstacles.obstacle_proximity_ratio_max_vel =
77  dto.teb_config.obstacles.obstacle_proximity_ratio_max_vel;
78  bo.obstacles.obstacle_proximity_lower_bound =
79  dto.teb_config.obstacles.obstacle_proximity_lower_bound;
80  bo.obstacles.obstacle_proximity_upper_bound =
81  dto.teb_config.obstacles.obstacle_proximity_upper_bound;
82 
83  // Optimization
84  bo.optim.no_inner_iterations = dto.teb_config.optim.no_inner_iterations;
85  bo.optim.no_outer_iterations = dto.teb_config.optim.no_outer_iterations;
86  bo.optim.optimization_activate = dto.teb_config.optim.optimization_activate;
87  bo.optim.optimization_verbose = dto.teb_config.optim.optimization_verbose;
88  bo.optim.penalty_epsilon = dto.teb_config.optim.penalty_epsilon;
89  bo.optim.weight_max_vel_x = dto.teb_config.optim.weight_max_vel_x;
90  bo.optim.weight_max_vel_y = dto.teb_config.optim.weight_max_vel_y;
91  bo.optim.weight_max_vel_theta = dto.teb_config.optim.weight_max_vel_theta;
92  bo.optim.weight_acc_lim_x = dto.teb_config.optim.weight_acc_lim_x;
93  bo.optim.weight_acc_lim_y = dto.teb_config.optim.weight_acc_lim_y;
94  bo.optim.weight_acc_lim_theta = dto.teb_config.optim.weight_acc_lim_theta;
95  bo.optim.weight_kinematics_nh = dto.teb_config.optim.weight_kinematics_nh;
96  bo.optim.weight_kinematics_forward_drive =
97  dto.teb_config.optim.weight_kinematics_forward_drive;
98  bo.optim.weight_kinematics_turning_radius =
99  dto.teb_config.optim.weight_kinematics_turning_radius;
100  bo.optim.weight_optimaltime = dto.teb_config.optim.weight_optimaltime;
101  bo.optim.weight_shortest_path = dto.teb_config.optim.weight_shortest_path;
102  bo.optim.weight_obstacle = dto.teb_config.optim.weight_obstacle;
103  bo.optim.weight_inflation = dto.teb_config.optim.weight_inflation;
104  bo.optim.weight_dynamic_obstacle = dto.teb_config.optim.weight_dynamic_obstacle;
105  bo.optim.weight_dynamic_obstacle_inflation =
106  dto.teb_config.optim.weight_dynamic_obstacle_inflation;
107  bo.optim.weight_velocity_obstacle_ratio =
108  dto.teb_config.optim.weight_velocity_obstacle_ratio;
109  bo.optim.weight_viapoint = dto.teb_config.optim.weight_viapoint;
110  bo.optim.weight_prefer_rotdir = dto.teb_config.optim.weight_prefer_rotdir;
111  bo.optim.weight_adapt_factor = dto.teb_config.optim.weight_adapt_factor;
112  bo.optim.obstacle_cost_exponent = dto.teb_config.optim.obstacle_cost_exponent;
113 
114  // Homotopy Class Planner
115  bo.hcp.enable_multithreading = dto.teb_config.hcp.enable_multithreading;
116  bo.hcp.simple_exploration = dto.teb_config.hcp.simple_exploration;
117  bo.hcp.max_number_classes = dto.teb_config.hcp.max_number_classes;
118  bo.hcp.max_number_plans_in_current_class =
119  dto.teb_config.hcp.max_number_plans_in_current_class;
120  bo.hcp.selection_cost_hysteresis = dto.teb_config.hcp.selection_cost_hysteresis;
121  bo.hcp.selection_obst_cost_scale = dto.teb_config.hcp.selection_obst_cost_scale;
122  bo.hcp.selection_viapoint_cost_scale = dto.teb_config.hcp.selection_viapoint_cost_scale;
123  bo.hcp.selection_alternative_time_cost = dto.teb_config.hcp.selection_alternative_time_cost;
124  bo.hcp.selection_dropping_probability = dto.teb_config.hcp.selection_dropping_probability;
125  bo.hcp.switching_blocking_period = dto.teb_config.hcp.switching_blocking_period;
126 
127  bo.hcp.obstacle_heading_threshold = dto.teb_config.hcp.obstacle_heading_threshold;
128  bo.hcp.roadmap_graph_no_samples = dto.teb_config.hcp.roadmap_graph_no_samples;
129  bo.hcp.roadmap_graph_area_width = dto.teb_config.hcp.roadmap_graph_area_width;
130  bo.hcp.roadmap_graph_area_length_scale = dto.teb_config.hcp.roadmap_graph_area_length_scale;
131  bo.hcp.h_signature_prescaler = dto.teb_config.hcp.h_signature_prescaler;
132  bo.hcp.h_signature_threshold = dto.teb_config.hcp.h_signature_threshold;
133 
134  bo.hcp.viapoints_all_candidates = dto.teb_config.hcp.viapoints_all_candidates;
135 
136  bo.hcp.delete_detours_backwards = dto.teb_config.hcp.delete_detours_backwards;
137  bo.hcp.detours_orientation_tolerance = dto.teb_config.hcp.detours_orientation_tolerance;
138  bo.hcp.length_start_orientation_vector = dto.teb_config.hcp.length_start_orientation_vector;
139  bo.hcp.max_ratio_detours_duration_best_duration =
140  dto.teb_config.hcp.max_ratio_detours_duration_best_duration;
141 
142  bo.checkParameters();
143  }
144 
145 
146 } // namespace armarx::navigation::local_planning
TimedElasticBands.h
stl.h
armarx::navigation::local_planning
This file is part of ArmarX.
Definition: fwd.h:35
core.h
aron_conversions.h
armarx::navigation::local_planning::toTebCfg
void toTebCfg(const arondto::TimedElasticBandsParams &dto, ::teb_local_planner::TebConfig &bo)
Definition: aron_conversions_teb.cpp:16
armarx::aron::bo
const std::optional< BoT > & bo
Definition: aron_conversions.h:166