Class HATebConfig

Nested Relationships

Nested Types

Class Documentation

class HATebConfig

Config class for the hateb_local_planner and its components.

Public Functions

inline HATebConfig()

Construct the HATebConfig using default values.

Warning

If the rosparam server or/and dynamic_reconfigure

(rqt_reconfigure) node are used, the default variables will be overwritten:

E.g. if

base_local_planner is utilized as plugin for the navigation stack, the initialize() method will register a dynamic_reconfigure server. A subset (not all but most) of the parameters are considered for dynamic modifications. All parameters considered by the dynamic_reconfigure server (and their default values) are set in PROJECT_SRC/cfg/HATebLocalPlannerReconfigure.cfg

.

In addition the rosparam server can be queried to get parameters e.g. defiend in a launch file. The plugin source (or a possible binary source) can call

loadRosParamFromNodeHandle() to update the parameters. In summary, default parameters are loaded in the following order (the right one overrides the left ones): HATebConfig Constructor defaults << dynamic_reconfigure defaults << rosparam server defaults

void loadRosParamFromNodeHandle(const ros::NodeHandle &nh)

Load parmeters from the ros param server.

Parameters:

nh – const reference to the local ros::NodeHandle

void reconfigure(HATebLocalPlannerReconfigureConfig &cfg)

Reconfigure parameters from the dynamic_reconfigure config. Change parameters dynamically (e.g. with rosrun rqt_reconfigure rqt_reconfigure). A reconfigure server needs to be instantiated that calls this method in it’s callback. In case of the plugin hateb_local_planner default values are defined in PROJECT_SRC/cfg/HATebLocalPlannerReconfigure.cfg.

Parameters:

cfg – Config class autogenerated by dynamic_reconfigure according to the cfg-file mentioned above.

void checkParameters() const

Check parameters and print warnings in case of discrepancies.

Call this method whenever parameters are changed using public interfaces to inform the user about some improper uses.

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

Check if some deprecated parameters are found and print warnings.

Parameters:

nh – const reference to the local ros::NodeHandle

inline boost::mutex &configMutex()

Return the internal config mutex.

Public Members

std::string ns_

Namespace of the planner, used for topic, service and parameter names.

std::string odom_topic

Topic name of the odometry message, provided by the robot driver or simulator.

std::string map_frame

Global planning frame.

FootprintModelPtr robot_model

model of the robot’s footprint

CircularFootprintPtr human_model

model of the agent’s footprint

int planning_mode

Planning mode (autonomous vs human-aware)

struct hateb_local_planner::HATebConfig::Trajectory trajectory
struct hateb_local_planner::HATebConfig::Robot robot
struct hateb_local_planner::HATebConfig::Agent agent
struct hateb_local_planner::HATebConfig::GoalTolerance goal_tolerance
struct hateb_local_planner::HATebConfig::Obstacles obstacles

Obstacle related parameters.

struct hateb_local_planner::HATebConfig::Optimization optim

Optimization related parameters.

struct hateb_local_planner::HATebConfig::Hateb hateb
struct hateb_local_planner::HATebConfig::Recovery recovery

Parameters related to recovery and backup strategies.

struct hateb_local_planner::HATebConfig::Visualization visualization

Private Members

boost::mutex config_mutex_

Mutex for config accesses and changes.

struct Agent

Agent related parameters.

Public Members

double radius

Radius of the agent for collision checking.

double max_vel_x

Maximum translational velocity of the agent.

double max_vel_y

Maximum strafing velocity of the agent.

double max_vel_x_backwards

Maximum backwards velocity of the agent.

double min_vel_x_backwards

Minimum backwards velocity of the agent.

double max_vel_theta

Maximum angular velocity of the agent.

double min_vel_theta

Minimum angular velocity of the agent.

double acc_lim_x

Maximum translational acceleration of the agent.

double acc_lim_y

Maximum strafing acceleration of the agent.

double acc_lim_theta

Maximum angular acceleration of the agent.

double fov

Field of view angle of the agent in radians.

int num_moving_avg

Number of samples for moving average calculation.

struct GoalTolerance

Goal tolerance related parameters.

Public Members

double yaw_goal_tolerance

Allowed final orientation error.

double xy_goal_tolerance

Allowed final euclidean distance to the goal position.

bool free_goal_vel

Allow the robot’s velocity to be nonzero (usally max_vel) for planning purposes.

bool complete_global_plan
struct Hateb

Public Members

int planning_mode

Mode of planning (autonomous vs human-aware)

bool use_agent_robot_safety_c

Enable agent-robot safety constraints.

bool use_agent_agent_safety_c

Enable agent-agent safety constraints.

bool use_agent_robot_rel_vel_c

Enable relative velocity constraints.

bool add_invisible_humans

Include invisible humans in planning.

bool use_agent_robot_visi_c

Enable visibility constraints.

bool use_agent_elastic_vel

Enable elastic velocity constraints for agents.

double pose_prediction_reset_time

Time after which pose prediction is reset.

double min_agent_robot_dist

Minimum distance that should be maintained agent and robot.

double min_agent_agent_dist

Minimum distance that should be maintained between agents.

double rel_vel_cost_threshold

Threshold value for Relative Velocity Constraint (lower means higher reaction)

double invisible_human_threshold

Threshold value for Invisible Humans Constraint (higher means higher reaction)

double visibility_cost_threshold

Threshold value for Visibility Constraint (lower means higher reaction)

struct Obstacles

Obstacle related parameters.

Public Members

double min_obstacle_dist

Minimum desired separation from obstacles.

double inflation_dist

buffer zone around obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)

bool use_nonlinear_obstacle_penalty
double obstacle_cost_mult
double dynamic_obstacle_inflation_dist

Buffer zone around predicted locations of dynamic obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)

bool include_dynamic_obstacles

Specify whether the movement of dynamic obstacles should be predicted by a constant velocity model (this also effects homotopy class planning); If false, all obstacles are considered to be static.

bool include_costmap_obstacles

Specify whether the obstacles in the costmap should be taken into account directly.

double costmap_obstacles_behind_robot_dist

Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters)

int obstacle_poses_affected

The obstacle position is attached to the closest pose on the trajectory to reduce computational effort, but take a number of neighbors into account as well.

bool legacy_obstacle_association

If true, the old association strategy is used (for each obstacle, find the nearest TEB pose), otherwise the new one (for each teb pose, find only “relevant” obstacles).

double obstacle_association_force_inclusion_factor

The non-legacy obstacle association technique tries to connect only relevant obstacles with the discretized trajectory during optimization, all obstacles within a specifed distance are forced to be included (as a multiple of min_obstacle_dist), e.g. choose 2.0 in order to consider obstacles within a radius of 2.0*min_obstacle_dist.

double obstacle_association_cutoff_factor

See obstacle_association_force_inclusion_factor, but beyond a multiple of [value]*min_obstacle_dist all obstacles are ignored during optimization. obstacle_association_force_inclusion_factor is processed first.

std::string costmap_converter_plugin

Define a plugin name of the costmap_converter package (costmap cells are converted to points/lines/polygons)

bool costmap_converter_spin_thread

If true, the costmap converter invokes its callback queue in a different thread.

int costmap_converter_rate

The rate that defines how often the costmap_converter plugin processes the current costmap (the value should not be much higher than the costmap update rate)

struct Optimization

Optimization related parameters.

Public Members

int no_inner_iterations

Number of solver iterations called in each outerloop iteration.

int no_outer_iterations

Each outerloop iteration automatically resizes the trajectory and invokes the internal optimizer with no_inner_iterations.

bool optimization_activate

Activate the optimization.

bool optimization_verbose

Print verbose information.

double penalty_epsilon

Add a small safety margin to penalty functions for hard-constraint approximations.

double time_penalty_epsilon

Time safety margin for penalty functions.

bool cap_optimaltime_penalty

Whether to cap the optimal time penalty.

double weight_max_vel_x

Optimization weight for satisfying the maximum allowed translational velocity.

double weight_max_vel_y

Optimization weight for satisfying the maximum allowed strafing velocity (in use only for holonomic robots)

double weight_max_vel_theta

Optimization weight for satisfying the maximum allowed angular velocity.

double weight_acc_lim_x

Optimization weight for satisfying the maximum allowed translational acceleration.

double weight_acc_lim_y

Optimization weight for satisfying the maximum allowed strafing acceleration (in use only for holonomic robots)

double weight_acc_lim_theta

Optimization weight for satisfying the maximum allowed angular acceleration.

double weight_kinematics_nh

Optimization weight for satisfying the non-holonomic kinematics.

double weight_kinematics_forward_drive

Optimization weight for forcing the robot to choose only forward directions (positive transl. velocities, only diffdrive robot)

double weight_kinematics_turning_radius

Optimization weight for enforcing a minimum turning radius (carlike robots)

double weight_optimaltime

Optimization weight for contracting the trajectory w.r.t. transition time.

double weight_shortest_path

Optimization weight for contracting the trajectory w.r.t. path length.

double weight_obstacle

Optimization weight for satisfying a minimum separation from obstacles.

double weight_inflation

Optimization weight for the inflation penalty (should be small)

double weight_dynamic_obstacle

Optimization weight for satisfying a minimum separation from dynamic obstacles.

double weight_dynamic_obstacle_inflation

Optimization weight for the inflation penalty of dynamic obstacles (should be small)

double weight_viapoint

Optimization weight for minimizing the distance to via-points.

double weight_prefer_rotdir

Optimization weight for preferring a specific turning direction.

double weight_adapt_factor

Some special weights (currently ‘weight_obstacle’) are repeatedly scaled by this factor in each outer TEB iteration (weight_new = weight_old*factor); Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions of the underlying optimization problem.

double obstacle_cost_exponent

Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default)

double weight_max_agent_vel_x

Optimization weight for satisfying maximum agent translational velocity.

double weight_max_agent_vel_y

Optimization weight for satisfying maximum agent straffing velocity.

double weight_nominal_agent_vel_x

Optimization weight for maintaining nominal agent velocity.

double weight_max_agent_vel_theta

Optimization weight for satisfying maximum agent angular velocity.

double weight_agent_acc_lim_x

Optimization weight for satisfying agent translational acceleration limits.

double weight_agent_acc_lim_y

Optimization weight for satisfying agent straffing acceleration limits.

double weight_agent_acc_lim_theta

Optimization weight for satisfying agent angular acceleration limits.

double weight_agent_optimaltime

Optimization weight for agent trajectory transition time.

double weight_agent_viapoint

Optimization weight for agent via-points.

double weight_invisible_human

Optimization weight for invisible human penalties.

double weight_agent_robot_safety

Optimization weight for agent-robot safety constraints.

double weight_agent_agent_safety

Optimization weight for agent-agent safety constraints.

double weight_agent_robot_rel_vel

Optimization weight for relative velocity constraint between agent and robot.

double weight_agent_robot_visibility

Optimization weight for agent visibility to robot.

bool disable_warm_start

If true, disables warm start in optimization.

bool disable_rapid_omega_chage

If true, prevents rapid changes in angular velocity.

double omega_chage_time_seperation

Minimum time separation for angular velocity changes.

struct Recovery

Recovery/backup related parameters.

Public Members

bool shrink_horizon_backup

Allows the planner to shrink the horizon temporary (50%) in case of automatically detected issues.

double shrink_horizon_min_duration

Specify minimum duration for the reduced horizon in case an infeasible trajectory is detected.

bool oscillation_recovery

Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards)

double oscillation_v_eps

Threshold for the average normalized linear velocity: if oscillation_v_eps and oscillation_omega_eps are not exceeded both, a possible oscillation is detected.

double oscillation_omega_eps

Threshold for the average normalized angular velocity: if oscillation_v_eps and oscillation_omega_eps are not exceeded both, a possible oscillation is detected.

double oscillation_recovery_min_duration

Minumum duration [sec] for which the recovery mode is activated after an oscillation is detected.

double oscillation_filter_duration

Filter length/duration [sec] for the detection of oscillations.

struct Robot

Robot related parameters.

Public Members

int type
double max_vel_x

Maximum translational velocity of the robot.

double max_vel_x_backwards

Maximum translational velocity of the robot for driving backwards.

double max_vel_y

Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)

double max_vel_theta

Maximum angular velocity of the robot.

double acc_lim_x

Maximum translational acceleration of the robot.

double acc_lim_y

Maximum strafing acceleration of the robot.

double acc_lim_theta

Maximum angular acceleration of the robot.

double min_turning_radius

Minimum turning radius of a carlike robot (diff-drive robot: zero);.

double wheelbase

The distance between the drive shaft and steering axle (only required for a carlike robot with ‘cmd_angle_instead_rotvel’ enabled); The value might be negative for back-wheeled robots!

bool cmd_angle_instead_rotvel

Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check ‘axles_distance’)

bool is_footprint_dynamic
bool is_real
struct Trajectory

Trajectory related parameters.

Public Members

bool teb_autosize

Enable automatic resizing of the trajectory w.r.t to the temporal resolution (recommended)

double dt_ref

Desired temporal resolution of the trajectory (should be in the magniture of the underlying control rate)

double dt_hysteresis

Hysteresis for automatic resizing depending on the current temporal resolution (dt): usually 10% of dt_ref.

int min_samples

Minimum number of samples (should be always greater than 2)

int max_samples

Maximum number of samples; Warning: if too small the discretization/resolution might not be sufficient for the given robot model or obstacle avoidance does not work anymore.

int agent_min_samples

Minimum number of samples for agent’s teb (should be always greater than 2)

bool global_plan_overwrite_orientation

Overwrite orientation of local subgoals provided by the global planner.

bool allow_init_with_backwards_motion

If true, the underlying trajectories might be initialized with backwards motions in case the goal is behind the start within the local costmap (this is only recommended if the robot is equipped with rear sensors)

double global_plan_viapoint_sep

Min. separation between each two consecutive via-points extracted from the global plan (if negative: disabled)

bool via_points_ordered

If true, the planner adheres to the order of via-points in the storage container.

double max_global_plan_lookahead_dist

Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if <=0: disabled; the length is also bounded by the local costmap size!]

double global_plan_prune_distance

Distance between robot and via_points of global plan which is used for pruning.

bool exact_arc_length

If true, the planner uses the exact arc length in velocity, acceleration and turning rate computations [-> increased cpu time], otherwise the euclidean approximation is used.

double force_reinit_new_goal_dist

Reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting)

double force_reinit_new_goal_angular

Reinitialize the trajectory if a previous goal is updated with an angular difference of more than the specified value in radians (skip hot-starting)

int feasibility_check_no_poses

Specify up to which pose on the predicted plan the feasibility should be checked each sampling interval.

bool publish_feedback

Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes)

double min_resolution_collision_check_angular

Min angular resolution used during the costmap collision check. If not respected, intermediate samples are added. [rad].

int control_look_ahead_poses

Index of the pose used to extract the velocity command.

double teb_init_skip_dist

How much distance needs to be skipped before calculating the plan.

double visualize_with_time_as_z_axis_scale

Scaling factor for visualizing the trajectory in 3D with time as z-axis.

struct Visualization

Visualization.

Public Members

bool publish_robot_global_plan

If true, publish the global plan for the robot.

bool publish_robot_local_plan

If true, publish the local plan for the robot.

bool publish_robot_local_plan_poses

If true, publish poses from the robot’s local plan.

bool publish_robot_local_plan_fp_poses

If true, publish footprint (time colored and vel scaled) poses from the robot’s local plan.

bool publish_agents_global_plans

If true, publish the global plans for all agents.

bool publish_agents_local_plans

If true, publish the local plans for all agents.

bool publish_agents_local_plan_poses

If true, publish poses from agents’ local plans.

bool publish_agents_local_plan_fp_poses

If true, publish footprint (time colored and vel scaled) poses from agents’ local plans.

double pose_array_z_scale

Multiplier to show time on z value of pose array for agents and robot.