Planning to Plan:
New Flexible
Navigation Interfaces

David V. Lu!!

ROSCon 2018

Central Dogma of ROS Navigation

Current Location + Goal Location
Global Costmap + Planner
Global Plan
Local Costmap + Planner
Command Velocities23

Animating with the Navigation Stack
ForethoughtGet the Global Plan, Look that Direction, Start Driving
ReactionIf the robot failed, How it failed, When it failed
Planning
Plan around people, for people4Navigation is often about more thandriving from point A to point B
efficiently. Context is key.

5

Planning to Talk about "Planning to Plan"
●●
●
●
●

6

The State of Navigation
Navigation Interface Design
New Global Planner
Locomotor
ROS2: The Next Generation

Why the fork?
Hard to swap out
single components
Feature creep of theuniversal solution
Move slow anddon't break things
github.com/ros-planning/navigation
7

State of the Navigation Packages
6DOF Poses Everywhere
nav_core - Interfaces haven't changed much in almost a decade
costmap_2d - forces you to use layers, transmits OccupancyGrids
Global Planners- navfn is prematurely optimized, hard to edit
- global_planner has bugs
Local Planners - difficult to debug/customize
move_base - Black box, static list of recovery behaviors

8Design Methodology
Build reusable pieces
Meant to be extensible
No black boxes

9nav_grid
nav_grid::NavGridInfo
unsigned int width = 0;
unsigned int height = 0;
double resolution = 1.0;
std::string frame_id= "map";
double origin_x = 0.0;
double origin_y = 0.0;

relative of nav_msgs::MapMetaData
10

NavGrid<T> Operations●●●●

Abstracted Data Storage
Template Based TypingCoordinate Translation
New Message Types○○○○

NavGridOfChars
NavGridOfCharsUpdate
NavGridOfDoubles
NavGridOfDoublesUpdate● Iterators11

nav_core2
it's plugins all the way down
BaseGlobalPlanner
BaseLocalPlanner
RecoveryBehaviors
CostmapLayer
DWBLocalPlannerand more...
12

nav_core2::Costmap
class Costmap : nav_grid::NavGrid<unsigned char>
Still Chars{

void initialize(NodeHandle parent, string name,
No Required TFs

TFListenerPtr tf);void update();mutex_t getMutex();

No UpdateThreadUse Any UpdateAlgorithm

bool canTrackChanges();
UIntBounds getChangeBounds(string ns);};

13

Option to Track
ChangesCompatible with
Costmap2DROS

dwb_local_planner
dwb_local_planner
Velocity Iterator &Trajectory Generator

14

Trajectory
Critic[ ]GoalChecker

nav_core2::GlobalPlannerclass GlobalPlanner{
public:void initialize(

NodeHandle parent,string name,TFListenerPtr tf,Costmap::Ptr costmap);
nav_2d_msgs

Path2D makePlan(

Pose2DStamped start,Pose2DStamped goal);};
15

ThrowsExceptions

dlux_global_planner
dlux_global_planner
Potential
Calculator
dlux_plugins::Dijkstra
dlux_plugins::AStar
16

Tracebackdlux_plugins::VonNeumannPath
dlux_plugins::GridPath
dlux_plugins::GradientPath

The many
paths ofdlux
globalplanner17

move_base● Four Components○○○○

Global Costmap
Global Planner
Local Costmap
Local Planner● Pass Global Plan to Local Planner● Try to Recover when Planning Fails

18

locomotor● Extensible Path Planning
Coordination Engine● Control what to do when path
planning succeeds and fails● Built on nav_core2 Interfaces● Leverages ROS Callback Queues

19Primary Components
onXCostmapUpdate
requestXCostmapUpdate

doCostmapUpdate
onXCostmapException
onNewGlobalPlan

requestGlobalPlan

makeGlobalPlan
onGlobalPlanningException
onNewLocalPlan

requestLocalPlan
20

makeLocalPlan
onLocalPlanningException

Single Thread Control Flow21

doGlobal
CostmapUpdate

doLocalCostmapUpdate

makeGlobalPlan

makeLocalPlan

Dealing with Failure
doGlobal
CostmapUpdate

makeGlobalPlan

22

InvalidStartPose
Exception

Clear Global
Costmap

InvalidGoalPose
Exception

Choose New Goal

Double the Threads - Double the Fun

23

doGlobal
CostmapUpdate

doLocalCostmapUpdate

makeGlobalPlan

makeLocalPlan

Four Shall Be the Number of the Timers

24

doGlobal
CostmapUpdate

doLocalCostmapUpdate

makeGlobalPlan

makeLocalPlan

locomove_base● Drop in replacement for move_base● Uses nav_core_adapter::CostmapAdapter● Loads nav_core::RecoveryBehaviors
doGlobal
CostmapUpdate

makeGlobalPlan

25

Any
Exception

Recovery
Behavior 1

RecoveryBehavior 2

Plugin Mux●●●●Load Multiple Pluginlib Plugins
Not nav_core2 specific
Switch Plugin via C++ method or ROS Service
Triggers Callback when Switching

makeLocalPlan

26

LocalPlanner
Exception

Switch toRotate Planner

Showing Forethought in Navigation
doGlobal
CostmapUpdate

doLocalCostmapUpdate

makeGlobalPlan

makeLocalPlan
Look in Direction ofGlobal Plan27

move_base Actions# move_base_msgs/MoveBase.action
geometry_msgs/PoseStamped target_pose----geometry_msgs/PoseStamped base_position28

Locomotor Action
nav_2d_msgs/Pose2DStamped goal--int64 state_info--nav_2d_msgs/Pose2DStamped current_position
nav_2d_msgs/Twist2D current_speed
nav_2d_msgs/Path2D global_plan
float32 percent_complete
float32 distance_traveled
float32 estimated_distance_remaining
int64 state_info
29

ROS 2● Full reimplementation of navigation stack
○
○
○
○
○

DluxGlobalPlanner
DWBLocalPlanner
Locomotor
LayeredCostmap and CostmapLayers
NavGridServer / Saver● Out by year's end
https://github.com/ros-planning/navigation2
Matt Hansen, Intel30

github.com/locusrobotics/robot_navigation

davidvlu@gmail.com@probablydavid@LocusRoboticsLocusRobotics.com/careers31