aron_conversions_teb.cpp

Go to the documentation of this file.

#include "aron_conversions.h"
 
#include <teb_local_planner/teb_config.h>
 
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
 
#include <armarx/navigation/local_planning/TimedElasticBands.h>
#include <armarx/navigation/local_planning/aron/TimedElasticBands.aron.generated.h>
 
namespace armarx::navigation::local_planning
{
 
    void
    toTebCfg(const arondto::TimedElasticBandsParams& dto, ::teb_local_planner::TebConfig& bo)
    {
        bo.pse.pse_costum_obstacle_penalties = dto.teb_config.pse.pse_costum_obstacle_penalties;
        bo.pse.pse_costum_obstacle_penalties_dynamic =
            dto.teb_config.pse.pse_costum_obstacle_penalties_dynamic;
        bo.pse.weight_costmap = dto.teb_config.pse.weight_costmap;
        bo.pse.weight_global_path_position = dto.teb_config.pse.weight_global_path_position;
        bo.pse.weight_global_path_orientation = dto.teb_config.pse.weight_global_path_orientation;
        bo.pse.lrk_use_alternative_approach = dto.teb_config.pse.lrk_use_alternative_approach;
        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.teb_config.pse.robot_diff_circumscribed_inscribed_radius;
 
        // Trajectory
        bo.trajectory.teb_autosize = bo.trajectory.teb_autosize;
        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.teb_config.trajectory.global_plan_overwrite_orientation;
        bo.trajectory.allow_init_with_backwards_motion =
            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.teb_config.trajectory.feasibility_check_lookahead_distance;
        bo.trajectory.min_resolution_collision_check_angular =
            dto.teb_config.trajectory.min_resolution_collision_check_angular;
 
        // Robot
        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.teb_config.goal_tolerance.xy_goal_tolerance;
 
        // Obstacles
        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.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.teb_config.obstacles.obstacle_association_force_inclusion_factor;
        bo.obstacles.obstacle_association_cutoff_factor =
            dto.teb_config.obstacles.obstacle_association_cutoff_factor;
        bo.obstacles.obstacle_proximity_ratio_max_vel =
            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.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.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.teb_config.hcp.max_ratio_detours_duration_best_duration;
 
        bo.checkParameters();
    }
 
 
} // namespace armarx::navigation::local_planning