teb_config.h

Go to the documentation of this file.

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2016,
 *  TU Dortmund - Institute of Control Theory and Systems Engineering.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the institute nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Christoph Rösmann
 *********************************************************************/

#ifndef TEB_CONFIG_H_
#define TEB_CONFIG_H_

#include <ros/console.h>
#include <ros/ros.h>
#include <Eigen/Core>
#include <Eigen/StdVector>

#include <teb_local_planner/TebLocalPlannerReconfigureConfig.h>


// Definitions
#define USE_ANALYTIC_JACOBI // if available for a specific edge, use analytic jacobi


namespace teb_local_planner
{

class TebConfig
{
public:

  std::string odom_topic; 
  std::string map_frame; 

  struct Trajectory
  {
    double teb_autosize; 
    double dt_ref; 
    double dt_hysteresis; 
    int min_samples; 
    int max_samples; 
    bool global_plan_overwrite_orientation; 
    bool allow_init_with_backwards_motion; 
    double global_plan_viapoint_sep; 
    bool via_points_ordered; 
    double max_global_plan_lookahead_dist; 
    double global_plan_prune_distance; 
    bool exact_arc_length; 
    double force_reinit_new_goal_dist; 
    double force_reinit_new_goal_angular; 
    int feasibility_check_no_poses; 
    bool publish_feedback; 
    double min_resolution_collision_check_angular; 
    int control_look_ahead_poses; 
    int prevent_look_ahead_poses_near_goal; 
  } trajectory; 

  struct Robot
  {
    double max_vel_x; 
    double max_vel_x_backwards; 
    double max_vel_y; 
    double max_vel_trans; 
    double max_vel_theta; 
    double acc_lim_x; 
    double acc_lim_y; 
    double acc_lim_theta; 
    double min_turning_radius; 
    double wheelbase; 
    bool cmd_angle_instead_rotvel; 
    bool is_footprint_dynamic; //<! If true, updated the footprint before checking trajectory feasibility
    bool use_proportional_saturation; //<! If true, reduce all twists components (linear x and y, and angular z) proportionally if any exceed its corresponding bounds, instead of saturating each one individually
    double transform_tolerance = 0.5; //<! Tolerance when querying the TF Tree for a transformation (seconds)
  } robot; 

  struct GoalTolerance
  {
    double yaw_goal_tolerance; 
    double xy_goal_tolerance; 
    bool free_goal_vel; 
    double trans_stopped_vel; 
    double theta_stopped_vel; 
    bool complete_global_plan; // true prevents the robot from ending the path early when it cross the end goal
  } goal_tolerance; 

  struct Obstacles
  {
    double min_obstacle_dist; 
    double inflation_dist; 
    double dynamic_obstacle_inflation_dist; 
    bool include_dynamic_obstacles; 
    bool include_costmap_obstacles; 
    double costmap_obstacles_behind_robot_dist; 
    int obstacle_poses_affected; 
    bool legacy_obstacle_association; 
    double obstacle_association_force_inclusion_factor; 
    double obstacle_association_cutoff_factor; 
    std::string costmap_converter_plugin; 
    bool costmap_converter_spin_thread; 
    int costmap_converter_rate; 
    double obstacle_proximity_ratio_max_vel; 
    double obstacle_proximity_lower_bound; 
    double obstacle_proximity_upper_bound; 
  } obstacles; 


  struct Optimization
  {
    int no_inner_iterations; 
    int no_outer_iterations; 

    bool optimization_activate; 
    bool optimization_verbose; 

    double penalty_epsilon; 

    double weight_max_vel_x; 
    double weight_max_vel_y; 
    double weight_max_vel_theta; 
    double weight_acc_lim_x; 
    double weight_acc_lim_y; 
    double weight_acc_lim_theta; 
    double weight_kinematics_nh; 
    double weight_kinematics_forward_drive; 
    double weight_kinematics_turning_radius; 
    double weight_optimaltime; 
    double weight_shortest_path; 
    double weight_obstacle; 
    double weight_inflation; 
    double weight_dynamic_obstacle; 
    double weight_dynamic_obstacle_inflation; 
    double weight_velocity_obstacle_ratio; 
    double weight_viapoint; 
    double weight_prefer_rotdir; 

    double weight_adapt_factor; 
    double obstacle_cost_exponent; 
  } optim; 


  struct HomotopyClasses
  {
    bool enable_homotopy_class_planning; 
    bool enable_multithreading; 
    bool simple_exploration; 
    int max_number_classes; 
    int max_number_plans_in_current_class; 
    double selection_cost_hysteresis; 
    double selection_prefer_initial_plan; 
    double selection_obst_cost_scale; 
    double selection_viapoint_cost_scale; 
    bool selection_alternative_time_cost; 
    double selection_dropping_probability; 
    double switching_blocking_period; 

    int roadmap_graph_no_samples; 
    double roadmap_graph_area_width; 
    double roadmap_graph_area_length_scale; 
    double h_signature_prescaler; 
    double h_signature_threshold; 

    double obstacle_keypoint_offset; 
    double obstacle_heading_threshold; 

    bool viapoints_all_candidates; 

    bool visualize_hc_graph; 
    double visualize_with_time_as_z_axis_scale; 
    bool delete_detours_backwards; 
    double detours_orientation_tolerance; 
    double length_start_orientation_vector; 
    double max_ratio_detours_duration_best_duration; 
  } hcp;

  struct Recovery
  {
    bool shrink_horizon_backup; 
    double shrink_horizon_min_duration; 
    bool oscillation_recovery; 
    double oscillation_v_eps; 
    double oscillation_omega_eps; 
    double oscillation_recovery_min_duration; 
    double oscillation_filter_duration; 
    bool divergence_detection_enable; 
    int divergence_detection_max_chi_squared; 
  } recovery; 


  TebConfig()
  {

    odom_topic = "odom";
    map_frame = "odom";

    // Trajectory

    trajectory.teb_autosize = true;
    trajectory.dt_ref = 0.3;
    trajectory.dt_hysteresis = 0.1;
    trajectory.min_samples = 3;
    trajectory.max_samples = 500;
    trajectory.global_plan_overwrite_orientation = true;
    trajectory.allow_init_with_backwards_motion = false;
    trajectory.global_plan_viapoint_sep = -1;
    trajectory.via_points_ordered = false;
    trajectory.max_global_plan_lookahead_dist = 1;
    trajectory.global_plan_prune_distance = 1;
    trajectory.exact_arc_length = false;
    trajectory.force_reinit_new_goal_dist = 1;
    trajectory.force_reinit_new_goal_angular = 0.5 * M_PI;
    trajectory.feasibility_check_no_poses = 5;
    trajectory.publish_feedback = false;
    trajectory.min_resolution_collision_check_angular = M_PI;
    trajectory.control_look_ahead_poses = 1;
    trajectory.prevent_look_ahead_poses_near_goal = 0;

    // Robot

    robot.max_vel_x = 0.4;
    robot.max_vel_x_backwards = 0.2;
    robot.max_vel_y = 0.0;
    robot.max_vel_trans = 0.0;
    robot.max_vel_theta = 0.3;
    robot.acc_lim_x = 0.5;
    robot.acc_lim_y = 0.5;
    robot.acc_lim_theta = 0.5;
    robot.min_turning_radius = 0;
    robot.wheelbase = 1.0;
    robot.cmd_angle_instead_rotvel = false;
    robot.is_footprint_dynamic = false;
    robot.use_proportional_saturation = false;

    // GoalTolerance

    goal_tolerance.xy_goal_tolerance = 0.2;
    goal_tolerance.yaw_goal_tolerance = 0.2;
    goal_tolerance.free_goal_vel = false;
    goal_tolerance.trans_stopped_vel = 0.1;
    goal_tolerance.theta_stopped_vel = 0.1;
    goal_tolerance.complete_global_plan = true;

    // Obstacles

    obstacles.min_obstacle_dist = 0.5;
    obstacles.inflation_dist = 0.6;
    obstacles.dynamic_obstacle_inflation_dist = 0.6;
    obstacles.include_dynamic_obstacles = true;
    obstacles.include_costmap_obstacles = true;
    obstacles.costmap_obstacles_behind_robot_dist = 1.5;
    obstacles.obstacle_poses_affected = 25;
    obstacles.legacy_obstacle_association = false;
    obstacles.obstacle_association_force_inclusion_factor = 1.5;
    obstacles.obstacle_association_cutoff_factor = 5;
    obstacles.costmap_converter_plugin = "";
    obstacles.costmap_converter_spin_thread = true;
    obstacles.costmap_converter_rate = 5;
    obstacles.obstacle_proximity_ratio_max_vel = 1;
    obstacles.obstacle_proximity_lower_bound = 0;
    obstacles.obstacle_proximity_upper_bound = 0.5;

    // Optimization

    optim.no_inner_iterations = 5;
    optim.no_outer_iterations = 4;
    optim.optimization_activate = true;
    optim.optimization_verbose = false;
    optim.penalty_epsilon = 0.1;
    optim.weight_max_vel_x = 2; //1
    optim.weight_max_vel_y = 2;
    optim.weight_max_vel_theta = 1;
    optim.weight_acc_lim_x = 1;
    optim.weight_acc_lim_y = 1;
    optim.weight_acc_lim_theta = 1;
    optim.weight_kinematics_nh = 1000;
    optim.weight_kinematics_forward_drive = 1;
    optim.weight_kinematics_turning_radius = 1;
    optim.weight_optimaltime = 1;
    optim.weight_shortest_path = 0;
    optim.weight_obstacle = 50;
    optim.weight_inflation = 0.1;
    optim.weight_dynamic_obstacle = 50;
    optim.weight_dynamic_obstacle_inflation = 0.1;
    optim.weight_velocity_obstacle_ratio = 0;
    optim.weight_viapoint = 1;
    optim.weight_prefer_rotdir = 50;

    optim.weight_adapt_factor = 2.0;
    optim.obstacle_cost_exponent = 1.0;

    // Homotopy Class Planner

    hcp.enable_homotopy_class_planning = true;
    hcp.enable_multithreading = true;
    hcp.simple_exploration = false;
    hcp.max_number_classes = 5;
    hcp.selection_cost_hysteresis = 1.0;
    hcp.selection_prefer_initial_plan = 0.95;
    hcp.selection_obst_cost_scale = 100.0;
    hcp.selection_viapoint_cost_scale = 1.0;
    hcp.selection_alternative_time_cost = false;
    hcp.selection_dropping_probability = 0.0;

    hcp.obstacle_keypoint_offset = 0.1;
    hcp.obstacle_heading_threshold = 0.45;
    hcp.roadmap_graph_no_samples = 15;
    hcp.roadmap_graph_area_width = 6; // [m]
    hcp.roadmap_graph_area_length_scale = 1.0;
    hcp.h_signature_prescaler = 1;
    hcp.h_signature_threshold = 0.1;
    hcp.switching_blocking_period = 0.0;

    hcp.viapoints_all_candidates = true;

    hcp.visualize_hc_graph = false;
    hcp.visualize_with_time_as_z_axis_scale = 0.0;
    hcp.delete_detours_backwards = true;
    hcp.detours_orientation_tolerance = M_PI / 2.0;
    hcp.length_start_orientation_vector = 0.4;
    hcp.max_ratio_detours_duration_best_duration = 3.0;

    // Recovery

    recovery.shrink_horizon_backup = true;
    recovery.shrink_horizon_min_duration = 10;
    recovery.oscillation_recovery = true;
    recovery.oscillation_v_eps = 0.1;
    recovery.oscillation_omega_eps = 0.1;
    recovery.oscillation_recovery_min_duration = 10;
    recovery.oscillation_filter_duration = 10;


  }

  void loadRosParamFromNodeHandle(const ros::NodeHandle& nh);

  void reconfigure(TebLocalPlannerReconfigureConfig& cfg);

  void checkParameters() const;

  void checkDeprecated(const ros::NodeHandle& nh) const;

  boost::mutex& configMutex() {return config_mutex_;}

private:
  boost::mutex config_mutex_; 

};


} // namespace teb_local_planner

#endif