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; 
    bool exact_arc_length; 
    double force_reinit_new_goal_dist; 
    int feasibility_check_no_poses; 
    bool publish_feedback; 
  } trajectory; 
    
  struct Robot
  {
    double max_vel_x; 
    double max_vel_x_backwards; 
    double max_vel_y; 
    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
  } robot; 
  
  struct GoalTolerance
  {
    double yaw_goal_tolerance; 
    double xy_goal_tolerance; 
    bool free_goal_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; 
  } 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_obstacle; 
    double weight_inflation; 
    double weight_dynamic_obstacle; 
    double weight_dynamic_obstacle_inflation; 
    double weight_viapoint; 
    double weight_prefer_rotdir; 
    
    double weight_adapt_factor; 
  } optim; 
  
  
  struct HomotopyClasses
  {
    bool enable_homotopy_class_planning; 
    bool enable_multithreading; 
    bool simple_exploration; 
    int max_number_classes; 
    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 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; 
  } 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; 
  } 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.exact_arc_length = false;
    trajectory.force_reinit_new_goal_dist = 1;
    trajectory.feasibility_check_no_poses = 5;
    trajectory.publish_feedback = false;
    
    // Robot
         
    robot.max_vel_x = 0.4;
    robot.max_vel_x_backwards = 0.2;
    robot.max_vel_y = 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;
    
    // GoalTolerance
    
    goal_tolerance.xy_goal_tolerance = 0.2;
    goal_tolerance.yaw_goal_tolerance = 0.2;
    goal_tolerance.free_goal_vel = false;
    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;
    
    // 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_obstacle = 50;
    optim.weight_inflation = 0.1;
    optim.weight_dynamic_obstacle = 50;
    optim.weight_dynamic_obstacle_inflation = 0.1;
    optim.weight_viapoint = 1;
    optim.weight_prefer_rotdir = 50;
    
    optim.weight_adapt_factor = 2.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.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;
    
    // 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