path planner

Path Planner

Receives goal descriptions by path_control or directly by some high level node and finds a path on the map. The map is optional and has to be provided by some external node. The currently seen obstacles are integrated into the map before the search begins. If no map is available, only the obstacles are used for planning.

There are three planners available:

1. Kinematic Path Planner: Performs A* search using car-line kinematics
2. Static Planner: Generates a predefined path, starting at the goal pose
3. Course Planner: [experimental] Generates longer paths using predefined course lanes.

Kinematic Path Planner

The node path_planner_node performs A* search with a car-like kinematic model. Other kinematic models are also supported, however they are statically compiled.

Parameters

Common Parameters

Name	Type	Default	Description
~target_topic	string	/goal	Topic to subscribe for goal specification.
~use_map_topic	bool	false	If false, use service to request map, otherwise subscribe map topic.
~map_topic	string	/map	Name of the topic, that provides the map (only used if use_map_topic=true).
~map_service	string	/dynamic_map	Name of the service, that provides the map (only used if use_map_topic=false).
~base_frame	string	/base_link	Name of the robot frame.
~algorithm	string	generic	The type of kinematic model to use. Can be one of generic, ackermann, 2d or also more specialized variants: summit, patsy, patsy_forward, summit_forward.
~grow_obstacles	double	0.0	Radius which obstacles are grown by.
~use_cloud	bool	true	When true, obstacle points in /obstacle_cloud are integrated into the map before path planning.
~use_scan_front	bool	false	When true, laser measurements in /scan/front/filtered are integrated into the map before path planning.
~use_scan_back	bool	false	When true, laser measurements in /scan/back/filtered are integrated into the map before path planning.
~use_cost_map	bool	false	When true, use the costmap published on cost_map_service for optimization.
~use_unknown_cells	bool	true	Allow planning through unknown cells.
~oversearch_distance	double	0.0	Specify for how long the search continues after a first candidate solution has been found. The search stops at the first node that has a distance to the start larger than the first candidate + oversearch_distance.
~penalty/backward	double	2.5	Penalty weight for path segments that have to be traversed in reverse.
~penalty/turn	double	4.0	Penalty weight for switching direction on the path.
~preprocess	bool	true	If true, the costmap for optimization is generated from the map using distance transform.
~postprocess	bool	true	If true, the resulting path is interpolated and smoothed.
~render_open_cells	bool	false	If true, the list of open cells is periodically published as grid cells.

Collision Model

Name	Type	Default	Description
~use_collision_gridmap	bool	false	When true, the search uses a box model for collision checking (parameter size). Otherwise the robot is treated as a point.
~size/backward	double	-0.6	Distance from the center to the rear most point in the (forward direction.)
~size/forward	double	0.4	Distance from the center to the front most point.
~size/width	double	0.5	Width of the robot.

Optimization Parameters (unstable)

Name	Type	Default	Description
~optimization/optimize_cost	bool	false	Activate path optimization (unstable.) Minimizes the a weighted sum of a cost map, the original path and a smoothness term
~optimization/publish_gradient	bool	false	Publish a vector field as Markers representing the optimization gradient.
~optimization/tolerance	double	1e-5	Maximum change in one iteration of the optimization considered as converged.
~optimization/weight/cost	double	0.75	Weight of the cost term in optimization.
~optimization/weight/data	double	0.9	Weight of the original path.
~optimization/weight/smooth	double	0.3	Smoothness weight.

Topics

Subscriptions

Name	Type	Description
goal (param: ~target_topic)	geometry_msgs/PoseStamped	Goal pose
map (param: ~map_topic)	nav_msgs/OccupancyGrid	Map
obstacle_cloud	sensor_msgs/PointCloud2	(optional) Obstacles
scan/front/filtered	sensor_msgs/LaserScan	(optional) Front facing laser scanner
scan/back/filtered	sensor_msgs/LaserScan	(optional) Rear facing laser scanner
tf

Publications

Name	Type	Description
path	path_msgs/PathSequence	The final (refined) path
path_raw	nav_msgs/Path	Raw, unrefined path
visualization_marker	visualization_msgs/Marker	Individual Markers
visualization_marker_array	visualization_msgs/MarkerArray	Marker Arrays
rosout	rosgraph_msgs/Log	ROS logging
~cells	nav_msgs/GridCells	List of open cells, if ~render_open_cells is true
~cost	nav_msgs/OccupancyGrid	The currently used cost map.
~map	nav_msgs/OccupancyGrid	The final map used for planning after integrating all obstacle information.

Static Planner

The node static_path_node publishes a path that has been defined beforehand. The path is specified as a sequence of straight lines and curves, starting at the goal pose specified in the request. Using RViz, this means that the pre-planned path will start at the user-defined 2d nav goal (red arrow in the example below.)

The sequence has to be specified as a list [<segment_0>, ..., <segment_n>]. A segment <segment_i> can be one of:

• [<distance>] for a straight line segment of distance <distance>. T he sign of the distance determines the direction, where a negative distance is meant to be driven in reverse.
• [<angle>, <radius>] for a curve with radius <radius> and the central angle <angle>.

Exemplary usage:

rosparam set /static_path/segments "[ [3.141592, 4.0], [4.0], [-3.141592, 4.0], [-4.0] ]"

This path consists of four segments:

1. (left) Curve segment with central angle π and radius 4m.
2. (forward) Straight line of length 4m.
3. (right) Curve segment with central angle π and radius 4m.
4. (reverse) Straight line of length 4m.

Course Planner (experimental)

Similarly to the static planner, the course planner uses hand designed segments for path planning. This planner, however, is more powerful: It plans a topological path on the segments.

The topological map consists of directed path segments. These segments are used for path planning, as demonstrated in the following image.

The path network shown above is a partial view of the segments generated by the following example:

<node name="course_planner" type="course_planner_node" pkg="path_planner" output="screen" respawn="true">
    <remap from="plan_path" to="/plan_path_course" />

    <param name="course/radius" value="0.9" />
    <param name="course/penalty/backwards" value="10.0" />
    <param name="course/penalty/turn" value="50.0" />

    <param name="course/turning/straight" value="0.7" />

    <rosparam param="map_segments">
        [
            [ [-2.0, 1.0], [-2.0, 0.0] ],
            [ [-2.0, 0.0], [13.5, 0.0] ],
            [ [13.5, -30.0], [13.5, 52.4] ],
            [ [13.5, 52.4],[8.0, 52.4] ],
            [ [8.0, 52.4], [8.0, 50.4] ],
            [ [8.0, 50.4], [13.5, 50.4] ],
            [ [12.5, 50.4], [12.5, -30.0] ],
            [ [13.5, 1.0], [-2.0, 1.0] ],
            [ [9.5, 52.4],[8.7, 59.0] ],
            [ [7.2, 59.0], [8.0, 52.4] ],
            [ [8.7, 59.0], [7.2, 59.0] ],
            [ [13.5, -24.5], [2.5, -24.5] ],
            [ [13.5, -25.5], [2.5, -25.5] ],
        ]
    </rosparam>
</node>

Transitions between the lines specified by map_segments are automatically generated at crossings. A graph search is then performed:

1. A generic search is performed starting at the robot's position. The search terminates once a map_segment is reached.
2. Another search is performed from the goal pose in reverse. This search also terminates once a map_segment is reached.
3. The topological graph is used to plan between the two points found by step 1. and 2.

Parameters

Name	Type	Default	Description
~course/map_segments	list		A list of map segments. See the example below.
~course/radius	double	1.0	Curve radius for generating transitions between segments.
~course/resolution	double	0.1	Segments are interpolated to this resolution.
~course/penalty/backwards	double	2.5	Multiplicative penalty for backwards driving for the topological search.
~course/penalty/turn	double	5.0	Additive penalty for changing driving direction for the topological search.
~course/penalty/straight	double	0.7	Multiplicative penalty for backwards straight for the topological search. (Values < 1 encourage driving straight.)
~max_distance_for_direct_try	double	7.0	First try to find a direct path, if start and goal are closer than this distance.
~max_time_for_direct_try	double	1.0	When trying to find a direct path, stop after this timeout.

Exemplary usage in parallel to another planner:

Assuming the course_planner_node is running with the topic plan_path_course, the high level dummy can be ordered to use the course planner by changing the planning_channel parameter:

rosparam set /highlevel_dummy/planning_channel /plan_path_course

Then the goal selected in RViz is sent to the course planner while other planners can still be used at the same time on other channels.