melodic noetic   Show EOL distros: 

Overview

Update: Two tutorials on planning with dynamic obstacles added (click here)

This package implements an online optimal local trajectory planner for navigation and control of mobile robots as a plugin for the ROS navigation package. The initial trajectory generated by a global planner is optimized during runtime w.r.t. minimizing the trajectory execution time (time-optimal objective), separation from obstacles and compliance with kinodynamic constraints such as satisfying maximum velocities and accelerations.

The current implementation complies with the kinematics of non-holonomic robots (differential drive and car-like robots). Support of holonomic robots is included since Kinetic.

The optimal trajectory is efficiently obtained by solving a sparse scalarized multi-objective optimization problem. The user can provide weights to the optimization problem in order to specify the behavior in case of conflicting objectives.

The approach called "Timed-Elastic-Band" is presented in:

• C. Rösmann, W. Feiten, T. Wösch, F. Hoffmann and T. Bertram: Trajectory modification considering dynamic constraints of autonomous robots. Proc. 7th German Conference on Robotics, Germany, Munich, 2012, pp 74–79.
• C. Rösmann, W. Feiten, T. Wösch, F. Hoffmann and T. Bertram: Efficient trajectory optimization using a sparse model. Proc. IEEE European Conference on Mobile Robots, Spain, Barcelona, 2013, pp. 138–143.

Since local planners such as the Timed-Elastic-Band get often stuck in a locally optimal trajectory as they are unable to transit across obstacles, an extension is implemented. A subset of admissible trajectories of distinctive topologies is optimized in parallel. The local planner is able to switch to the current globally optimal trajectory among the candidate set. Distinctive topologies are obtained by utilizing the concept of homology / homotopy classes. The following papers are describing the approach

• C. Rösmann, F. Hoffmann and T. Bertram: Integrated online trajectory planning and optimization in distinctive topologies, Robotics and Autonomous Systems, Vol. 88, 2017, pp. 142–153.
• C. Rösmann, F. Hoffmann and T. Bertram: Planning of Multiple Robot Trajectories in Distinctive Topologies, Proc. IEEE European Conference on Mobile Robots, UK, Lincoln, Sept. 2015

The extension to car-like robots is described in

• C. Rösmann, F. Hoffmann and T. Bertram: Kinodynamic Trajectory Optimization and Control for Car-Like Robots, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vancouver, BC, Canada, Sept. 2017.

Get started by completing the tutorials in the Tutorials section.

Use GitHub to report bugs or submit feature requests. [View active issues]

Video

The following video presents the features of the package and shows examples from simulation and real robot situations. Some spoken explanations are included in the audio track of the video.

Features introduced in version 0.2 are presented in the following video (supporting car-like robots and costmap conversion).

Node API

Topics

Published Topics

~<name>/global_plan (nav_msgs/Path)

• Global plan that the local planner is currently attempting to follow. Used primarily for visualization purposes.

~<name>/local_plan (nav_msgs/Path)

• The local plan or trajectory that the teb_local_planner optimizes and follows. Used primarily for visualization purposes.

~<name>/teb_poses (geometry_msgs/PoseArray)

• The discrete pose list (SE2) of the current local plan. Used primarily for visualization purposes.

~<name>/teb_markers (visualization_msgs/Marker)

• The teb_local_planner provides additional information of the planning scene via markers with different namespaces. Namespaces PointObstacles and PolyObstacles: visualize all point and polygon obstacles that are currently considered during optimization. Namespace TebContainer: Visualize all found and optimized trajectories that rest in alternative topologies (only if parallel planning is enabled). Some more information is published such as the optimization footprint model.

~<name>/teb_feedback (teb_local_planner/FeedbackMsg)

• The feedback message contains the planned trajectory including the velocity profile and temporal information as well as the obstacle list. Used primarily for evaluation and debugging. Parameter ~<name>/publish_feedback must be enabled.

Subscribed Topics

~<name>/odom (nav_msgs/Odometry)

• Odometry information that gives the local planner the current speed of the robot. Change this topic by either remapping or by changing the parameter ~<name>/odom_topic.

~<name>/obstacles (costmap_converter/ObstacleArrayMsg)

• Provide custom obstacles as point-, line- or polygon-shaped ones (additionally to or instead of the costmap obstacles).

Parameters

The teb_local_planner package allows the user to set Parameters in order to customize the behavior. These parameters are grouped into several categories: robot configuration, goal tolerance, trajectory configuration, obstacles, optimization, planning in distinctive topologies and miscellaneous parameters. Some of them are chosen to be compliant with the base_local_planner. Many (but not all) parameters can be modified at runtime using rqt_reconfigure.

Robot Configuration Parameters

~<name>/acc_lim_x (double, default: 0.5)

• Maximum translational acceleration of the robot in meters/sec^2

~<name>/acc_lim_theta (double, default: 0.5)

• Maximum angular acceleration of the robot in radians/sec^2

~<name>/max_vel_x (double, default: 0.4)

• Maximum translational velocity of the robot in meters/sec

~<name>/max_vel_x_backwards (double, default: 0.2)

• Maximum absolute translational velocity of the robot while driving backwards in meters/sec. See optimization parameter weight_kinematics_forward_drive

~<name>/max_vel_theta (double, default: 0.3)

• Maximum angular velocity of the robot in radians/sec

The following parameters are relevant only for carlike robots:

~<name>/min_turning_radius (double, default: 0.0)

• Minimum turning radius of a carlike robot (set to zero for a diff-drive robot).

~<name>/wheelbase (double, default: 1.0)

• The distance between the rear axle and the front axle. The value might be negative for back-wheeled robots (only required if ~<name>/cmd_angle_instead_rotvelis set to true).

~<name>/cmd_angle_instead_rotvel (bool, default: false)

• Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle [-pi/2,pi/2]. Note, changing the semantics of yaw rate depending on the application is not preferable. Here, it just complies with the inputs required by the stage simulator. Datatypes in ackermann_msgs are more appropriate, but are not supported by move_base. The local planner is not intended to send commands by itself.

The following parameters are relevant for the footprint model used for optimization (see Tutorial Obstacle Avoidance and Robot Footprint Model). New in version 0.3

~<name>/footprint_model/type (string, default: "point")

• Specify the robot footprint model type used for optimization. Different types are "point", "circular", "line", "two_circles" and "polygon." The type of the model significantly influences the required computation time.

~<name>/footprint_model/radius (double, default: 0.2)

• This parameter is only relevant for type "circular". It contains the radius of the circle. The center of the circle is located at the robot's axis of rotation.

~<name>/footprint_model/line_start (double[2], default: [-0.3, 0.0])

• This parameter is only relevant for type "line". It contains the start coordinates of the line segment.

~<name>/footprint_model/line_end (double[2], default: [0.3, 0.0])

• This parameter is only relevant for type "line". It contains the end coordinates of the line segment.

~<name>/footprint_model/front_offset (double, default: 0.2)

• This parameter is only relevant for type "two_circles". It describes how much the center of the front circle is shifted along the robot's x-axis. The robot's axis of rotation is assumed to be located at [0,0].

~<name>/footprint_model/front_radius (double, default: 0.2)

• This parameter is only relevant for type "two_circles". It contains the radius of front circle.

~<name>/footprint_model/rear_offset (double, default: 0.2)

• This parameter is only relevant for type "two_circles". It describes how much the center of the rear circle is shifted along the robot's negative x-axis. The robot's axis of rotation is assumed to be located at [0,0].

~<name>/footprint_model/rear_radius (double, default: 0.2)

• This parameter is only relevant for type "two_circles". It contains the radius of rear circle.

~<name>/footprint_model/vertices (double[], default: [ [0.25,-0.05], [...], ...])

• This parameter is only relevant for type "polygon". It contains the list of polygon vertices (2d coordinates each). The polygon is always closed: do not repeat the first vertex at the end.

~<name>/is_footprint_dynamic (bool, default: false)

• If true, updates the footprint before checking trajectory feasibility

Goal Tolerance Parameters

~<name>/xy_goal_tolerance (double, default: 0.2)

• Allowed final euclidean distance to the goal position in meters

~<name>/yaw_goal_tolerance (double, default: 0.2)

• Allowed final orientation error in radians

~<name>/free_goal_vel (bool, default: false)

• Remove the goal velocity constraint such that the robot can arrive at the goal with maximum speed

Trajectory Configuration Parameters

~<name>/dt_ref (double, default: 0.3)

• Desired temporal resolution of the trajectory (the trajectory is not fixed to dt_ref since the temporal resolution is part of the optimization, but the trajectory will be resized between iterations if dt_ref +-dt_hysteresis is violated.

~<name>/dt_hysteresis (double, default: 0.1)

• Hysteresis for automatic resizing depending on the current temporal resolution, usually approx. 10% of dt_ref is recommended

~<name>/min_samples (int, default: 3)

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

~<name>/global_plan_overwrite_orientation (bool, default: true)

• Overwrite orientation of local subgoals provided by the global planner (since they often provide only a 2D path)

~<name>/global_plan_viapoint_sep (double, default: -0.1 (disabled))

• If positive, via-points are extrected from the global plan (path-following mode). The value determines the resolution of the reference path (min. separation between each two consecutive via-points along the global plan, if negative: disabled). Refer to parameter weight_viapoint for adjusting the intensity. New in version 0.4

~<name>/max_global_plan_lookahead_dist (double, default: 3.0)

• Specify the maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization. The actual length is than determined by the logical conjunction of the local costmap size and this maximum bound. Set to zero or negative in order to deactivate this limitation.

~<name>/force_reinit_new_goal_dist (double, default: 1.0)

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

~<name>/feasibility_check_no_poses (int, default: 4)

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

~<name>/publish_feedback (bool, default: false)

• Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging). See list of publishers above.

~<name>/shrink_horizon_backup (bool, default: true)

• Allows the planner to shrink the horizon temporary (50%) in case of automatically detected issues (e.g. infeasibility). Also see parameter shrink_horizon_min_duration.

Obstacle Parameters

~<name>/min_obstacle_dist (double, default: 0.5)

• Minimum desired separation from obstacles in meters

~<name>/include_costmap_obstacles (bool, default: true)

• Specify if obstacles of the local costmap should be taken into account. Each cell that is marked as obstacle is considered as a point-obstacle. Therefore do not choose a very small resolution of the costmap since it increases computation time. In future releases this circumstance is going to be addressed as well as providing an additional api for dynamic obstacles.

~<name>/costmap_obstacles_behind_robot_dist (double, default: 1.0)

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

~<name>/obstacle_poses_affected (int, default: 30)

• Each obstacle position is attached to the closest pose on the trajectory in order to keep a distance. Additional neighbors can be taken into account as well. Note, this parameter might be removed in future versions, since the the obstacle association strategy has been modified in kinetic+. Refer to the parameter description of legacy_obstacle_association.

~<name>/inflation_dist (double, default: pre kinetic: 0.0, kinetic+: 0.6)

• Buffer zone around obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect). Also refer to the weight weight_inflation.

The following parameters are relevant only if costmap_converter plugins are desired (see tutorial):

~<name>/costmap_converter_plugin (string, default: "")

• Define plugin name in order to convert costmap cells to points/lines/polygons. Set an empty string to disable the conversion such that all cells are treated as point-obstacles.

~<name>/costmap_converter_spin_thread (bool, default: true)

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

~<name>/costmap_converter_rate (double, default: 5.0)

• 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) [in Hz].

Optimization Parameters

~<name>/no_inner_iterations (int, default: 5)

• Number of actual solver iterations called in each outerloop iteration. See param no_outer_iterations.

~<name>/no_outer_iterations (int, default: 4)

• Each outerloop iteration automatically resizes the trajectory according to the desired temporal resolution dt_ref and invokes the internal optimizer (that performs no_inner_iterations). The total number of solver iterations in each planning cycle is therefore the product of both values.

~<name>/penalty_epsilon (double, default: 0.1)

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

~<name>/weight_max_vel_x (double, default: 2.0)

• Optimization weight for satisfying the maximum allowed translational velocity

~<name>/weight_max_vel_theta (double, default: 1.0)

• Optimization weight for satisfying the maximum allowed angular velocity

~<name>/weight_acc_lim_x (double, default: 1.0)

• Optimization weight for satisfying the maximum allowed translational acceleration

~<name>/weight_acc_lim_theta (double, default: 1.0)

• Optimization weight for satisfying the maximum allowed angular acceleration

~<name>/weight_kinematics_nh (double, default: 1000.0)

• Optimization weight for satisfying the non-holonomic kinematics (this parameter must be high since the kinematics equation constitutes an equality constraint, even a value of 1000 does not imply a bad matrix condition due to small 'raw' cost values in comparison to other costs).

~<name>/weight_kinematics_forward_drive (double, default: 1.0)

• Optimization weight for forcing the robot to choose only forward directions (positive transl. velocities). A small weight (e.g. 1.0) still allows driving backwards. A value around 1000 almost prevents backward driving (but cannot be guaranteed).

~<name>/weight_kinematics_turning_radius (double, default: 1.0)

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

~<name>/weight_optimaltime (double, default: 1.0)

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

~<name>/weight_obstacle (double, default: 50.0)

• Optimization weight for keeping a minimum distance from obstacles

~<name>/weight_viapoint (double, default: 1.0)

• Optimization weight for minimzing the distance to via-points (resp. reference path). New in version 0.4

~<name>/weight_inflation (double, default: 0.1)

• Optimization weight for the inflation penalty (should be small).

Parallel Planning in distinctive Topologies

~<name>/enable_homotopy_class_planning (bool, default: true)

• Activate parallel planning in distinctive topologies (requires much more CPU resources, since multiple trajectories are optimized at once)

~<name>/enable_multithreading (bool, default: true)

• Activate multiple threading in order to plan each trajectory in a different thread

~<name>/max_number_classes (int, default: 4)

• Specify the maximum number of distinctive trajectories taken into account (limits computational effort)

~<name>/selection_cost_hysteresis (double, default: 1.0)

• Specify how much trajectory cost must a new candidate have w.r.t. a previously selected trajectory in order to be selected (selection if new_cost < old_cost*factor).

~<name>/selection_obst_cost_scale (double, default: 100.0)

• Extra scaling of obstacle cost terms just for selecting the 'best' candidate.

~<name>/selection_viapoint_cost_scale (double, default: 1.0)

• Extra scaling of via-point cost terms just for selecting the 'best' candidate. New in version 0.4

~<name>/selection_alternative_time_cost (bool, default: false)

• If true, time cost (sum of squared time differences) is replaced by the total transition time (sum of time differences).

~<name>/roadmap_graph_no_samples (int, default: 15)

• Specify the number of samples generated for creating the roadmap graph

~<name>/roadmap_graph_area_width (double, default: 6)

• Random keypoints/waypoints are sampled in a rectangular region between start and goal. Specify the width of that region in meters.

~<name>/h_signature_prescaler (double, default: 1.0)

• Scale internal parameter (H-signature) that is used to distinguish between homotopy classes. Warning: reduce this parameter only, if you observe problems with too many obstacles in the local cost map, do not choose it extremly low, otherwise obstacles cannot be distinguished from each other (0.2<value<=1).

~<name>/h_signature_threshold (double, default: 0.1)

• Two H-signatures are assumed to be equal, if both the difference of real parts and complex parts are below the specified threshold.

~<name>/obstacle_heading_threshold (double, default: 1.0)

• Specify the value of the scalar product between obstacle heading and goal heading in order to take them (obstacles) into account for exploration.

~<name>/visualize_hc_graph (bool, default: false)

• Visualize the graph that is created for exploring distinctive trajectories (check marker message in rviz)

~<name>/viapoints_all_candidates (bool, default: true)

• If true, all trajectories of different topologies are attached to the set of via-points, otherwise only the trajectory sharing the same topology as the initial/global plan is connected with them (no effect on test_optim_node). New in version 0.4

Miscellaneous Parameters

~<name>/odom_topic (string, default: "odom")

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

~<name>/map_frame (string, default: "odom")

• Global planning frame (in case of a static map, this parameter must be usually changed to "/map".

Roadmap

Some features and improvements that are currently planned for the future. Contributions are welcome!

• Add and improve safety functions in case of unavoidable obstacles (e.g. for obstacles that are located really close to the goal).
• Implementation of suitable escape behaviors.
• Improvements/Solutions for cases in which the planner oscillates between multiple locally optimal solutions (not on a topologic basis, but due to occuring noise etc.).