diff --git a/.gitignore b/.gitignore
index 4581ef2eeefc7832704f4a78f3018455f7a06178..e69c986c20f3d99e09668785601e9dd7a9ac0eb5 100644
--- a/.gitignore
+++ b/.gitignore
@@ -27,3 +27,9 @@
 *.exe
 *.out
 *.app
+
+#colcon output
+build
+log
+install
+
diff --git a/Doxyfile b/Doxyfile
new file mode 100644
index 0000000000000000000000000000000000000000..834f11b1a1e9fa992c9be1eb06a6770a3b2c318f
--- /dev/null
+++ b/Doxyfile
@@ -0,0 +1,2427 @@
+# Doxyfile 1.8.11
+
+# This file describes the settings to be used by the documentation system
+# doxygen (www.doxygen.org) for a project.
+#
+# All text after a double hash (##) is considered a comment and is placed in
+# front of the TAG it is preceding.
+#
+# All text after a single hash (#) is considered a comment and will be ignored.
+# The format is:
+# TAG = value [value, ...]
+# For lists, items can also be appended using:
+# TAG += value [value, ...]
+# Values that contain spaces should be placed between quotes (\" \").
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+
+# This tag specifies the encoding used for all characters in the config file
+# that follow. The default is UTF-8 which is also the encoding used for all text
+# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv
+# built into libc) for the transcoding. See http://www.gnu.org/software/libiconv
+# for the list of possible encodings.
+# The default value is: UTF-8.
+
+DOXYFILE_ENCODING      = UTF-8
+
+# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by
+# double-quotes, unless you are using Doxywizard) that should identify the
+# project for which the documentation is generated. This name is used in the
+# title of most generated pages and in a few other places.
+# The default value is: My Project.
+
+PROJECT_NAME           = "ROS2 Navigation System"
+
+# The PROJECT_NUMBER tag can be used to enter a project or revision number. This
+# could be handy for archiving the generated documentation or if some version
+# control system is used.
+
+PROJECT_NUMBER         = 0.1
+
+# Using the PROJECT_BRIEF tag one can provide an optional one line description
+# for a project that appears at the top of each page and should give viewer a
+# quick idea about the purpose of the project. Keep the description short.
+
+PROJECT_BRIEF          =
+
+# With the PROJECT_LOGO tag one can specify a logo or an icon that is included
+# in the documentation. The maximum height of the logo should not exceed 55
+# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy
+# the logo to the output directory.
+
+PROJECT_LOGO           =
+
+# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path
+# into which the generated documentation will be written. If a relative path is
+# entered, it will be relative to the location where doxygen was started. If
+# left blank the current directory will be used.
+
+OUTPUT_DIRECTORY       = doc
+
+# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub-
+# directories (in 2 levels) under the output directory of each output format and
+# will distribute the generated files over these directories. Enabling this
+# option can be useful when feeding doxygen a huge amount of source files, where
+# putting all generated files in the same directory would otherwise causes
+# performance problems for the file system.
+# The default value is: NO.
+
+CREATE_SUBDIRS         = NO
+
+# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII
+# characters to appear in the names of generated files. If set to NO, non-ASCII
+# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode
+# U+3044.
+# The default value is: NO.
+
+ALLOW_UNICODE_NAMES    = NO
+
+# The OUTPUT_LANGUAGE tag is used to specify the language in which all
+# documentation generated by doxygen is written. Doxygen will use this
+# information to generate all constant output in the proper language.
+# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese,
+# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States),
+# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian,
+# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages),
+# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian,
+# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian,
+# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish,
+# Ukrainian and Vietnamese.
+# The default value is: English.
+
+OUTPUT_LANGUAGE        = English
+
+# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member
+# descriptions after the members that are listed in the file and class
+# documentation (similar to Javadoc). Set to NO to disable this.
+# The default value is: YES.
+
+BRIEF_MEMBER_DESC      = YES
+
+# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief
+# description of a member or function before the detailed description
+#
+# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the
+# brief descriptions will be completely suppressed.
+# The default value is: YES.
+
+REPEAT_BRIEF           = YES
+
+# This tag implements a quasi-intelligent brief description abbreviator that is
+# used to form the text in various listings. Each string in this list, if found
+# as the leading text of the brief description, will be stripped from the text
+# and the result, after processing the whole list, is used as the annotated
+# text. Otherwise, the brief description is used as-is. If left blank, the
+# following values are used ($name is automatically replaced with the name of
+# the entity):The $name class, The $name widget, The $name file, is, provides,
+# specifies, contains, represents, a, an and the.
+
+ABBREVIATE_BRIEF       =
+
+# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then
+# doxygen will generate a detailed section even if there is only a brief
+# description.
+# The default value is: NO.
+
+ALWAYS_DETAILED_SEC    = NO
+
+# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all
+# inherited members of a class in the documentation of that class as if those
+# members were ordinary class members. Constructors, destructors and assignment
+# operators of the base classes will not be shown.
+# The default value is: NO.
+
+INLINE_INHERITED_MEMB  = NO
+
+# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path
+# before files name in the file list and in the header files. If set to NO the
+# shortest path that makes the file name unique will be used
+# The default value is: YES.
+
+FULL_PATH_NAMES        = YES
+
+# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path.
+# Stripping is only done if one of the specified strings matches the left-hand
+# part of the path. The tag can be used to show relative paths in the file list.
+# If left blank the directory from which doxygen is run is used as the path to
+# strip.
+#
+# Note that you can specify absolute paths here, but also relative paths, which
+# will be relative from the directory where doxygen is started.
+# This tag requires that the tag FULL_PATH_NAMES is set to YES.
+
+STRIP_FROM_PATH        =
+
+# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the
+# path mentioned in the documentation of a class, which tells the reader which
+# header file to include in order to use a class. If left blank only the name of
+# the header file containing the class definition is used. Otherwise one should
+# specify the list of include paths that are normally passed to the compiler
+# using the -I flag.
+
+STRIP_FROM_INC_PATH    =
+
+# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but
+# less readable) file names. This can be useful is your file systems doesn't
+# support long names like on DOS, Mac, or CD-ROM.
+# The default value is: NO.
+
+SHORT_NAMES            = NO
+
+# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the
+# first line (until the first dot) of a Javadoc-style comment as the brief
+# description. If set to NO, the Javadoc-style will behave just like regular Qt-
+# style comments (thus requiring an explicit @brief command for a brief
+# description.)
+# The default value is: NO.
+
+JAVADOC_AUTOBRIEF      = NO
+
+# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first
+# line (until the first dot) of a Qt-style comment as the brief description. If
+# set to NO, the Qt-style will behave just like regular Qt-style comments (thus
+# requiring an explicit \brief command for a brief description.)
+# The default value is: NO.
+
+QT_AUTOBRIEF           = NO
+
+# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a
+# multi-line C++ special comment block (i.e. a block of //! or /// comments) as
+# a brief description. This used to be the default behavior. The new default is
+# to treat a multi-line C++ comment block as a detailed description. Set this
+# tag to YES if you prefer the old behavior instead.
+#
+# Note that setting this tag to YES also means that rational rose comments are
+# not recognized any more.
+# The default value is: NO.
+
+MULTILINE_CPP_IS_BRIEF = NO
+
+# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the
+# documentation from any documented member that it re-implements.
+# The default value is: YES.
+
+INHERIT_DOCS           = YES
+
+# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new
+# page for each member. If set to NO, the documentation of a member will be part
+# of the file/class/namespace that contains it.
+# The default value is: NO.
+
+SEPARATE_MEMBER_PAGES  = NO
+
+# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen
+# uses this value to replace tabs by spaces in code fragments.
+# Minimum value: 1, maximum value: 16, default value: 4.
+
+TAB_SIZE               = 2
+
+# This tag can be used to specify a number of aliases that act as commands in
+# the documentation. An alias has the form:
+# name=value
+# For example adding
+# "sideeffect=@par Side Effects:\n"
+# will allow you to put the command \sideeffect (or @sideeffect) in the
+# documentation, which will result in a user-defined paragraph with heading
+# "Side Effects:". You can put \n's in the value part of an alias to insert
+# newlines.
+
+ALIASES                =
+
+# This tag can be used to specify a number of word-keyword mappings (TCL only).
+# A mapping has the form "name=value". For example adding "class=itcl::class"
+# will allow you to use the command class in the itcl::class meaning.
+
+TCL_SUBST              =
+
+# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources
+# only. Doxygen will then generate output that is more tailored for C. For
+# instance, some of the names that are used will be different. The list of all
+# members will be omitted, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_FOR_C  = NO
+
+# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or
+# Python sources only. Doxygen will then generate output that is more tailored
+# for that language. For instance, namespaces will be presented as packages,
+# qualified scopes will look different, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_JAVA   = NO
+
+# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran
+# sources. Doxygen will then generate output that is tailored for Fortran.
+# The default value is: NO.
+
+OPTIMIZE_FOR_FORTRAN   = NO
+
+# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL
+# sources. Doxygen will then generate output that is tailored for VHDL.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_VHDL   = NO
+
+# Doxygen selects the parser to use depending on the extension of the files it
+# parses. With this tag you can assign which parser to use for a given
+# extension. Doxygen has a built-in mapping, but you can override or extend it
+# using this tag. The format is ext=language, where ext is a file extension, and
+# language is one of the parsers supported by doxygen: IDL, Java, Javascript,
+# C#, C, C++, D, PHP, Objective-C, Python, Fortran (fixed format Fortran:
+# FortranFixed, free formatted Fortran: FortranFree, unknown formatted Fortran:
+# Fortran. In the later case the parser tries to guess whether the code is fixed
+# or free formatted code, this is the default for Fortran type files), VHDL. For
+# instance to make doxygen treat .inc files as Fortran files (default is PHP),
+# and .f files as C (default is Fortran), use: inc=Fortran f=C.
+#
+# Note: For files without extension you can use no_extension as a placeholder.
+#
+# Note that for custom extensions you also need to set FILE_PATTERNS otherwise
+# the files are not read by doxygen.
+
+EXTENSION_MAPPING      =
+
+# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments
+# according to the Markdown format, which allows for more readable
+# documentation. See http://daringfireball.net/projects/markdown/ for details.
+# The output of markdown processing is further processed by doxygen, so you can
+# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in
+# case of backward compatibilities issues.
+# The default value is: YES.
+
+MARKDOWN_SUPPORT       = YES
+
+# When enabled doxygen tries to link words that correspond to documented
+# classes, or namespaces to their corresponding documentation. Such a link can
+# be prevented in individual cases by putting a % sign in front of the word or
+# globally by setting AUTOLINK_SUPPORT to NO.
+# The default value is: YES.
+
+AUTOLINK_SUPPORT       = YES
+
+# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want
+# to include (a tag file for) the STL sources as input, then you should set this
+# tag to YES in order to let doxygen match functions declarations and
+# definitions whose arguments contain STL classes (e.g. func(std::string);
+# versus func(std::string) {}). This also make the inheritance and collaboration
+# diagrams that involve STL classes more complete and accurate.
+# The default value is: NO.
+
+BUILTIN_STL_SUPPORT    = NO
+
+# If you use Microsoft's C++/CLI language, you should set this option to YES to
+# enable parsing support.
+# The default value is: NO.
+
+CPP_CLI_SUPPORT        = NO
+
+# Set the SIP_SUPPORT tag to YES if your project consists of sip (see:
+# http://www.riverbankcomputing.co.uk/software/sip/intro) sources only. Doxygen
+# will parse them like normal C++ but will assume all classes use public instead
+# of private inheritance when no explicit protection keyword is present.
+# The default value is: NO.
+
+SIP_SUPPORT            = NO
+
+# For Microsoft's IDL there are propget and propput attributes to indicate
+# getter and setter methods for a property. Setting this option to YES will make
+# doxygen to replace the get and set methods by a property in the documentation.
+# This will only work if the methods are indeed getting or setting a simple
+# type. If this is not the case, or you want to show the methods anyway, you
+# should set this option to NO.
+# The default value is: YES.
+
+IDL_PROPERTY_SUPPORT   = YES
+
+# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC
+# tag is set to YES then doxygen will reuse the documentation of the first
+# member in the group (if any) for the other members of the group. By default
+# all members of a group must be documented explicitly.
+# The default value is: NO.
+
+DISTRIBUTE_GROUP_DOC   = NO
+
+# If one adds a struct or class to a group and this option is enabled, then also
+# any nested class or struct is added to the same group. By default this option
+# is disabled and one has to add nested compounds explicitly via \ingroup.
+# The default value is: NO.
+
+GROUP_NESTED_COMPOUNDS = NO
+
+# Set the SUBGROUPING tag to YES to allow class member groups of the same type
+# (for instance a group of public functions) to be put as a subgroup of that
+# type (e.g. under the Public Functions section). Set it to NO to prevent
+# subgrouping. Alternatively, this can be done per class using the
+# \nosubgrouping command.
+# The default value is: YES.
+
+SUBGROUPING            = YES
+
+# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions
+# are shown inside the group in which they are included (e.g. using \ingroup)
+# instead of on a separate page (for HTML and Man pages) or section (for LaTeX
+# and RTF).
+#
+# Note that this feature does not work in combination with
+# SEPARATE_MEMBER_PAGES.
+# The default value is: NO.
+
+INLINE_GROUPED_CLASSES = NO
+
+# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions
+# with only public data fields or simple typedef fields will be shown inline in
+# the documentation of the scope in which they are defined (i.e. file,
+# namespace, or group documentation), provided this scope is documented. If set
+# to NO, structs, classes, and unions are shown on a separate page (for HTML and
+# Man pages) or section (for LaTeX and RTF).
+# The default value is: NO.
+
+INLINE_SIMPLE_STRUCTS  = NO
+
+# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or
+# enum is documented as struct, union, or enum with the name of the typedef. So
+# typedef struct TypeS {} TypeT, will appear in the documentation as a struct
+# with name TypeT. When disabled the typedef will appear as a member of a file,
+# namespace, or class. And the struct will be named TypeS. This can typically be
+# useful for C code in case the coding convention dictates that all compound
+# types are typedef'ed and only the typedef is referenced, never the tag name.
+# The default value is: NO.
+
+TYPEDEF_HIDES_STRUCT   = NO
+
+# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This
+# cache is used to resolve symbols given their name and scope. Since this can be
+# an expensive process and often the same symbol appears multiple times in the
+# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small
+# doxygen will become slower. If the cache is too large, memory is wasted. The
+# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range
+# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536
+# symbols. At the end of a run doxygen will report the cache usage and suggest
+# the optimal cache size from a speed point of view.
+# Minimum value: 0, maximum value: 9, default value: 0.
+
+LOOKUP_CACHE_SIZE      = 0
+
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+
+# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in
+# documentation are documented, even if no documentation was available. Private
+# class members and static file members will be hidden unless the
+# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES.
+# Note: This will also disable the warnings about undocumented members that are
+# normally produced when WARNINGS is set to YES.
+# The default value is: NO.
+
+EXTRACT_ALL            = YES
+
+# If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will
+# be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PRIVATE        = NO
+
+# If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal
+# scope will be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PACKAGE        = NO
+
+# If the EXTRACT_STATIC tag is set to YES, all static members of a file will be
+# included in the documentation.
+# The default value is: NO.
+
+EXTRACT_STATIC         = NO
+
+# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined
+# locally in source files will be included in the documentation. If set to NO,
+# only classes defined in header files are included. Does not have any effect
+# for Java sources.
+# The default value is: YES.
+
+EXTRACT_LOCAL_CLASSES  = YES
+
+# This flag is only useful for Objective-C code. If set to YES, local methods,
+# which are defined in the implementation section but not in the interface are
+# included in the documentation. If set to NO, only methods in the interface are
+# included.
+# The default value is: NO.
+
+EXTRACT_LOCAL_METHODS  = NO
+
+# If this flag is set to YES, the members of anonymous namespaces will be
+# extracted and appear in the documentation as a namespace called
+# 'anonymous_namespace{file}', where file will be replaced with the base name of
+# the file that contains the anonymous namespace. By default anonymous namespace
+# are hidden.
+# The default value is: NO.
+
+EXTRACT_ANON_NSPACES   = NO
+
+# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all
+# undocumented members inside documented classes or files. If set to NO these
+# members will be included in the various overviews, but no documentation
+# section is generated. This option has no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_MEMBERS     = NO
+
+# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all
+# undocumented classes that are normally visible in the class hierarchy. If set
+# to NO, these classes will be included in the various overviews. This option
+# has no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_CLASSES     = NO
+
+# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend
+# (class|struct|union) declarations. If set to NO, these declarations will be
+# included in the documentation.
+# The default value is: NO.
+
+HIDE_FRIEND_COMPOUNDS  = NO
+
+# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any
+# documentation blocks found inside the body of a function. If set to NO, these
+# blocks will be appended to the function's detailed documentation block.
+# The default value is: NO.
+
+HIDE_IN_BODY_DOCS      = NO
+
+# The INTERNAL_DOCS tag determines if documentation that is typed after a
+# \internal command is included. If the tag is set to NO then the documentation
+# will be excluded. Set it to YES to include the internal documentation.
+# The default value is: NO.
+
+INTERNAL_DOCS          = NO
+
+# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file
+# names in lower-case letters. If set to YES, upper-case letters are also
+# allowed. This is useful if you have classes or files whose names only differ
+# in case and if your file system supports case sensitive file names. Windows
+# and Mac users are advised to set this option to NO.
+# The default value is: system dependent.
+
+CASE_SENSE_NAMES       = YES
+
+# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with
+# their full class and namespace scopes in the documentation. If set to YES, the
+# scope will be hidden.
+# The default value is: NO.
+
+HIDE_SCOPE_NAMES       = NO
+
+# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will
+# append additional text to a page's title, such as Class Reference. If set to
+# YES the compound reference will be hidden.
+# The default value is: NO.
+
+HIDE_COMPOUND_REFERENCE= NO
+
+# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of
+# the files that are included by a file in the documentation of that file.
+# The default value is: YES.
+
+SHOW_INCLUDE_FILES     = YES
+
+# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each
+# grouped member an include statement to the documentation, telling the reader
+# which file to include in order to use the member.
+# The default value is: NO.
+
+SHOW_GROUPED_MEMB_INC  = NO
+
+# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include
+# files with double quotes in the documentation rather than with sharp brackets.
+# The default value is: NO.
+
+FORCE_LOCAL_INCLUDES   = NO
+
+# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the
+# documentation for inline members.
+# The default value is: YES.
+
+INLINE_INFO            = YES
+
+# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the
+# (detailed) documentation of file and class members alphabetically by member
+# name. If set to NO, the members will appear in declaration order.
+# The default value is: YES.
+
+SORT_MEMBER_DOCS       = YES
+
+# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief
+# descriptions of file, namespace and class members alphabetically by member
+# name. If set to NO, the members will appear in declaration order. Note that
+# this will also influence the order of the classes in the class list.
+# The default value is: NO.
+
+SORT_BRIEF_DOCS        = NO
+
+# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the
+# (brief and detailed) documentation of class members so that constructors and
+# destructors are listed first. If set to NO the constructors will appear in the
+# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS.
+# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief
+# member documentation.
+# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting
+# detailed member documentation.
+# The default value is: NO.
+
+SORT_MEMBERS_CTORS_1ST = NO
+
+# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy
+# of group names into alphabetical order. If set to NO the group names will
+# appear in their defined order.
+# The default value is: NO.
+
+SORT_GROUP_NAMES       = NO
+
+# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by
+# fully-qualified names, including namespaces. If set to NO, the class list will
+# be sorted only by class name, not including the namespace part.
+# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES.
+# Note: This option applies only to the class list, not to the alphabetical
+# list.
+# The default value is: NO.
+
+SORT_BY_SCOPE_NAME     = NO
+
+# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper
+# type resolution of all parameters of a function it will reject a match between
+# the prototype and the implementation of a member function even if there is
+# only one candidate or it is obvious which candidate to choose by doing a
+# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still
+# accept a match between prototype and implementation in such cases.
+# The default value is: NO.
+
+STRICT_PROTO_MATCHING  = NO
+
+# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo
+# list. This list is created by putting \todo commands in the documentation.
+# The default value is: YES.
+
+GENERATE_TODOLIST      = YES
+
+# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test
+# list. This list is created by putting \test commands in the documentation.
+# The default value is: YES.
+
+GENERATE_TESTLIST      = YES
+
+# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug
+# list. This list is created by putting \bug commands in the documentation.
+# The default value is: YES.
+
+GENERATE_BUGLIST       = YES
+
+# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO)
+# the deprecated list. This list is created by putting \deprecated commands in
+# the documentation.
+# The default value is: YES.
+
+GENERATE_DEPRECATEDLIST= YES
+
+# The ENABLED_SECTIONS tag can be used to enable conditional documentation
+# sections, marked by \if <section_label> ... \endif and \cond <section_label>
+# ... \endcond blocks.
+
+ENABLED_SECTIONS       =
+
+# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the
+# initial value of a variable or macro / define can have for it to appear in the
+# documentation. If the initializer consists of more lines than specified here
+# it will be hidden. Use a value of 0 to hide initializers completely. The
+# appearance of the value of individual variables and macros / defines can be
+# controlled using \showinitializer or \hideinitializer command in the
+# documentation regardless of this setting.
+# Minimum value: 0, maximum value: 10000, default value: 30.
+
+MAX_INITIALIZER_LINES  = 30
+
+# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at
+# the bottom of the documentation of classes and structs. If set to YES, the
+# list will mention the files that were used to generate the documentation.
+# The default value is: YES.
+
+SHOW_USED_FILES        = YES
+
+# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This
+# will remove the Files entry from the Quick Index and from the Folder Tree View
+# (if specified).
+# The default value is: YES.
+
+SHOW_FILES             = YES
+
+# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces
+# page. This will remove the Namespaces entry from the Quick Index and from the
+# Folder Tree View (if specified).
+# The default value is: YES.
+
+SHOW_NAMESPACES        = YES
+
+# The FILE_VERSION_FILTER tag can be used to specify a program or script that
+# doxygen should invoke to get the current version for each file (typically from
+# the version control system). Doxygen will invoke the program by executing (via
+# popen()) the command command input-file, where command is the value of the
+# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided
+# by doxygen. Whatever the program writes to standard output is used as the file
+# version. For an example see the documentation.
+
+FILE_VERSION_FILTER    =
+
+# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed
+# by doxygen. The layout file controls the global structure of the generated
+# output files in an output format independent way. To create the layout file
+# that represents doxygen's defaults, run doxygen with the -l option. You can
+# optionally specify a file name after the option, if omitted DoxygenLayout.xml
+# will be used as the name of the layout file.
+#
+# Note that if you run doxygen from a directory containing a file called
+# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE
+# tag is left empty.
+
+LAYOUT_FILE            =
+
+# The CITE_BIB_FILES tag can be used to specify one or more bib files containing
+# the reference definitions. This must be a list of .bib files. The .bib
+# extension is automatically appended if omitted. This requires the bibtex tool
+# to be installed. See also http://en.wikipedia.org/wiki/BibTeX for more info.
+# For LaTeX the style of the bibliography can be controlled using
+# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the
+# search path. See also \cite for info how to create references.
+
+CITE_BIB_FILES         =
+
+#---------------------------------------------------------------------------
+# Configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+
+# The QUIET tag can be used to turn on/off the messages that are generated to
+# standard output by doxygen. If QUIET is set to YES this implies that the
+# messages are off.
+# The default value is: NO.
+
+QUIET                  = NO
+
+# The WARNINGS tag can be used to turn on/off the warning messages that are
+# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES
+# this implies that the warnings are on.
+#
+# Tip: Turn warnings on while writing the documentation.
+# The default value is: YES.
+
+WARNINGS               = YES
+
+# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate
+# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag
+# will automatically be disabled.
+# The default value is: YES.
+
+WARN_IF_UNDOCUMENTED   = YES
+
+# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for
+# potential errors in the documentation, such as not documenting some parameters
+# in a documented function, or documenting parameters that don't exist or using
+# markup commands wrongly.
+# The default value is: YES.
+
+WARN_IF_DOC_ERROR      = YES
+
+# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that
+# are documented, but have no documentation for their parameters or return
+# value. If set to NO, doxygen will only warn about wrong or incomplete
+# parameter documentation, but not about the absence of documentation.
+# The default value is: NO.
+
+WARN_NO_PARAMDOC       = NO
+
+# If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when
+# a warning is encountered.
+# The default value is: NO.
+
+WARN_AS_ERROR          = NO
+
+# The WARN_FORMAT tag determines the format of the warning messages that doxygen
+# can produce. The string should contain the $file, $line, and $text tags, which
+# will be replaced by the file and line number from which the warning originated
+# and the warning text. Optionally the format may contain $version, which will
+# be replaced by the version of the file (if it could be obtained via
+# FILE_VERSION_FILTER)
+# The default value is: $file:$line: $text.
+
+WARN_FORMAT            = "$file:$line: $text"
+
+# The WARN_LOGFILE tag can be used to specify a file to which warning and error
+# messages should be written. If left blank the output is written to standard
+# error (stderr).
+
+WARN_LOGFILE           =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the input files
+#---------------------------------------------------------------------------
+
+# The INPUT tag is used to specify the files and/or directories that contain
+# documented source files. You may enter file names like myfile.cpp or
+# directories like /usr/src/myproject. Separate the files or directories with
+# spaces. See also FILE_PATTERNS and EXTENSION_MAPPING
+# Note: If this tag is empty the current directory is searched.
+
+INPUT = src
+
+# This tag can be used to specify the character encoding of the source files
+# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
+# libiconv (or the iconv built into libc) for the transcoding. See the libiconv
+# documentation (see: http://www.gnu.org/software/libiconv) for the list of
+# possible encodings.
+# The default value is: UTF-8.
+
+INPUT_ENCODING         = UTF-8
+
+# If the value of the INPUT tag contains directories, you can use the
+# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and
+# *.h) to filter out the source-files in the directories.
+#
+# Note that for custom extensions or not directly supported extensions you also
+# need to set EXTENSION_MAPPING for the extension otherwise the files are not
+# read by doxygen.
+#
+# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp,
+# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h,
+# *.hh, *.hxx, *.hpp, *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc,
+# *.m, *.markdown, *.md, *.mm, *.dox, *.py, *.pyw, *.f90, *.f, *.for, *.tcl,
+# *.vhd, *.vhdl, *.ucf, *.qsf, *.as and *.js.
+
+FILE_PATTERNS          =
+
+# The RECURSIVE tag can be used to specify whether or not subdirectories should
+# be searched for input files as well.
+# The default value is: NO.
+
+RECURSIVE              = YES
+
+# The EXCLUDE tag can be used to specify files and/or directories that should be
+# excluded from the INPUT source files. This way you can easily exclude a
+# subdirectory from a directory tree whose root is specified with the INPUT tag.
+#
+# Note that relative paths are relative to the directory from which doxygen is
+# run.
+
+EXCLUDE                =
+
+# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or
+# directories that are symbolic links (a Unix file system feature) are excluded
+# from the input.
+# The default value is: NO.
+
+EXCLUDE_SYMLINKS       = NO
+
+# If the value of the INPUT tag contains directories, you can use the
+# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude
+# certain files from those directories.
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories for example use the pattern */test/*
+
+EXCLUDE_PATTERNS       =
+
+# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names
+# (namespaces, classes, functions, etc.) that should be excluded from the
+# output. The symbol name can be a fully qualified name, a word, or if the
+# wildcard * is used, a substring. Examples: ANamespace, AClass,
+# AClass::ANamespace, ANamespace::*Test
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories use the pattern */test/*
+
+EXCLUDE_SYMBOLS        =
+
+# The EXAMPLE_PATH tag can be used to specify one or more files or directories
+# that contain example code fragments that are included (see the \include
+# command).
+
+EXAMPLE_PATH           =
+
+# If the value of the EXAMPLE_PATH tag contains directories, you can use the
+# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and
+# *.h) to filter out the source-files in the directories. If left blank all
+# files are included.
+
+EXAMPLE_PATTERNS       =
+
+# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be
+# searched for input files to be used with the \include or \dontinclude commands
+# irrespective of the value of the RECURSIVE tag.
+# The default value is: NO.
+
+EXAMPLE_RECURSIVE      = NO
+
+# The IMAGE_PATH tag can be used to specify one or more files or directories
+# that contain images that are to be included in the documentation (see the
+# \image command).
+
+IMAGE_PATH             =
+
+# The INPUT_FILTER tag can be used to specify a program that doxygen should
+# invoke to filter for each input file. Doxygen will invoke the filter program
+# by executing (via popen()) the command:
+#
+# <filter> <input-file>
+#
+# where <filter> is the value of the INPUT_FILTER tag, and <input-file> is the
+# name of an input file. Doxygen will then use the output that the filter
+# program writes to standard output. If FILTER_PATTERNS is specified, this tag
+# will be ignored.
+#
+# Note that the filter must not add or remove lines; it is applied before the
+# code is scanned, but not when the output code is generated. If lines are added
+# or removed, the anchors will not be placed correctly.
+#
+# Note that for custom extensions or not directly supported extensions you also
+# need to set EXTENSION_MAPPING for the extension otherwise the files are not
+# properly processed by doxygen.
+
+INPUT_FILTER           =
+
+# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern
+# basis. Doxygen will compare the file name with each pattern and apply the
+# filter if there is a match. The filters are a list of the form: pattern=filter
+# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how
+# filters are used. If the FILTER_PATTERNS tag is empty or if none of the
+# patterns match the file name, INPUT_FILTER is applied.
+#
+# Note that for custom extensions or not directly supported extensions you also
+# need to set EXTENSION_MAPPING for the extension otherwise the files are not
+# properly processed by doxygen.
+
+FILTER_PATTERNS        =
+
+# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using
+# INPUT_FILTER) will also be used to filter the input files that are used for
+# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES).
+# The default value is: NO.
+
+FILTER_SOURCE_FILES    = NO
+
+# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file
+# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and
+# it is also possible to disable source filtering for a specific pattern using
+# *.ext= (so without naming a filter).
+# This tag requires that the tag FILTER_SOURCE_FILES is set to YES.
+
+FILTER_SOURCE_PATTERNS =
+
+# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that
+# is part of the input, its contents will be placed on the main page
+# (index.html). This can be useful if you have a project on for instance GitHub
+# and want to reuse the introduction page also for the doxygen output.
+
+USE_MDFILE_AS_MAINPAGE =
+
+#---------------------------------------------------------------------------
+# Configuration options related to source browsing
+#---------------------------------------------------------------------------
+
+# If the SOURCE_BROWSER tag is set to YES then a list of source files will be
+# generated. Documented entities will be cross-referenced with these sources.
+#
+# Note: To get rid of all source code in the generated output, make sure that
+# also VERBATIM_HEADERS is set to NO.
+# The default value is: NO.
+
+SOURCE_BROWSER         = YES
+
+# Setting the INLINE_SOURCES tag to YES will include the body of functions,
+# classes and enums directly into the documentation.
+# The default value is: NO.
+
+INLINE_SOURCES         = NO
+
+# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any
+# special comment blocks from generated source code fragments. Normal C, C++ and
+# Fortran comments will always remain visible.
+# The default value is: YES.
+
+STRIP_CODE_COMMENTS    = YES
+
+# If the REFERENCED_BY_RELATION tag is set to YES then for each documented
+# function all documented functions referencing it will be listed.
+# The default value is: NO.
+
+REFERENCED_BY_RELATION = NO
+
+# If the REFERENCES_RELATION tag is set to YES then for each documented function
+# all documented entities called/used by that function will be listed.
+# The default value is: NO.
+
+REFERENCES_RELATION    = NO
+
+# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set
+# to YES then the hyperlinks from functions in REFERENCES_RELATION and
+# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will
+# link to the documentation.
+# The default value is: YES.
+
+REFERENCES_LINK_SOURCE = YES
+
+# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the
+# source code will show a tooltip with additional information such as prototype,
+# brief description and links to the definition and documentation. Since this
+# will make the HTML file larger and loading of large files a bit slower, you
+# can opt to disable this feature.
+# The default value is: YES.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+SOURCE_TOOLTIPS        = YES
+
+# If the USE_HTAGS tag is set to YES then the references to source code will
+# point to the HTML generated by the htags(1) tool instead of doxygen built-in
+# source browser. The htags tool is part of GNU's global source tagging system
+# (see http://www.gnu.org/software/global/global.html). You will need version
+# 4.8.6 or higher.
+#
+# To use it do the following:
+# - Install the latest version of global
+# - Enable SOURCE_BROWSER and USE_HTAGS in the config file
+# - Make sure the INPUT points to the root of the source tree
+# - Run doxygen as normal
+#
+# Doxygen will invoke htags (and that will in turn invoke gtags), so these
+# tools must be available from the command line (i.e. in the search path).
+#
+# The result: instead of the source browser generated by doxygen, the links to
+# source code will now point to the output of htags.
+# The default value is: NO.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+USE_HTAGS              = NO
+
+# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a
+# verbatim copy of the header file for each class for which an include is
+# specified. Set to NO to disable this.
+# See also: Section \class.
+# The default value is: YES.
+
+VERBATIM_HEADERS       = YES
+
+# If the CLANG_ASSISTED_PARSING tag is set to YES then doxygen will use the
+# clang parser (see: http://clang.llvm.org/) for more accurate parsing at the
+# cost of reduced performance. This can be particularly helpful with template
+# rich C++ code for which doxygen's built-in parser lacks the necessary type
+# information.
+# Note: The availability of this option depends on whether or not doxygen was
+# generated with the -Duse-libclang=ON option for CMake.
+# The default value is: NO.
+
+CLANG_ASSISTED_PARSING = NO
+
+# If clang assisted parsing is enabled you can provide the compiler with command
+# line options that you would normally use when invoking the compiler. Note that
+# the include paths will already be set by doxygen for the files and directories
+# specified with INPUT and INCLUDE_PATH.
+# This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES.
+
+CLANG_OPTIONS          =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+
+# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all
+# compounds will be generated. Enable this if the project contains a lot of
+# classes, structs, unions or interfaces.
+# The default value is: YES.
+
+ALPHABETICAL_INDEX     = YES
+
+# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in
+# which the alphabetical index list will be split.
+# Minimum value: 1, maximum value: 20, default value: 5.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+COLS_IN_ALPHA_INDEX    = 5
+
+# In case all classes in a project start with a common prefix, all classes will
+# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag
+# can be used to specify a prefix (or a list of prefixes) that should be ignored
+# while generating the index headers.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+IGNORE_PREFIX          =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the HTML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output
+# The default value is: YES.
+
+GENERATE_HTML          = YES
+
+# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_OUTPUT            = html
+
+# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each
+# generated HTML page (for example: .htm, .php, .asp).
+# The default value is: .html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FILE_EXTENSION    = .html
+
+# The HTML_HEADER tag can be used to specify a user-defined HTML header file for
+# each generated HTML page. If the tag is left blank doxygen will generate a
+# standard header.
+#
+# To get valid HTML the header file that includes any scripts and style sheets
+# that doxygen needs, which is dependent on the configuration options used (e.g.
+# the setting GENERATE_TREEVIEW). It is highly recommended to start with a
+# default header using
+# doxygen -w html new_header.html new_footer.html new_stylesheet.css
+# YourConfigFile
+# and then modify the file new_header.html. See also section "Doxygen usage"
+# for information on how to generate the default header that doxygen normally
+# uses.
+# Note: The header is subject to change so you typically have to regenerate the
+# default header when upgrading to a newer version of doxygen. For a description
+# of the possible markers and block names see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_HEADER            =
+
+# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each
+# generated HTML page. If the tag is left blank doxygen will generate a standard
+# footer. See HTML_HEADER for more information on how to generate a default
+# footer and what special commands can be used inside the footer. See also
+# section "Doxygen usage" for information on how to generate the default footer
+# that doxygen normally uses.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FOOTER            =
+
+# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style
+# sheet that is used by each HTML page. It can be used to fine-tune the look of
+# the HTML output. If left blank doxygen will generate a default style sheet.
+# See also section "Doxygen usage" for information on how to generate the style
+# sheet that doxygen normally uses.
+# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as
+# it is more robust and this tag (HTML_STYLESHEET) will in the future become
+# obsolete.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_STYLESHEET        =
+
+# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined
+# cascading style sheets that are included after the standard style sheets
+# created by doxygen. Using this option one can overrule certain style aspects.
+# This is preferred over using HTML_STYLESHEET since it does not replace the
+# standard style sheet and is therefore more robust against future updates.
+# Doxygen will copy the style sheet files to the output directory.
+# Note: The order of the extra style sheet files is of importance (e.g. the last
+# style sheet in the list overrules the setting of the previous ones in the
+# list). For an example see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_STYLESHEET  =
+
+# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the HTML output directory. Note
+# that these files will be copied to the base HTML output directory. Use the
+# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these
+# files. In the HTML_STYLESHEET file, use the file name only. Also note that the
+# files will be copied as-is; there are no commands or markers available.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_FILES       =
+
+# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen
+# will adjust the colors in the style sheet and background images according to
+# this color. Hue is specified as an angle on a colorwheel, see
+# http://en.wikipedia.org/wiki/Hue for more information. For instance the value
+# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300
+# purple, and 360 is red again.
+# Minimum value: 0, maximum value: 359, default value: 220.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_HUE    = 220
+
+# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors
+# in the HTML output. For a value of 0 the output will use grayscales only. A
+# value of 255 will produce the most vivid colors.
+# Minimum value: 0, maximum value: 255, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_SAT    = 100
+
+# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the
+# luminance component of the colors in the HTML output. Values below 100
+# gradually make the output lighter, whereas values above 100 make the output
+# darker. The value divided by 100 is the actual gamma applied, so 80 represents
+# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not
+# change the gamma.
+# Minimum value: 40, maximum value: 240, default value: 80.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_GAMMA  = 80
+
+# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML
+# page will contain the date and time when the page was generated. Setting this
+# to YES can help to show when doxygen was last run and thus if the
+# documentation is up to date.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_TIMESTAMP         = NO
+
+# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML
+# documentation will contain sections that can be hidden and shown after the
+# page has loaded.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_DYNAMIC_SECTIONS  = NO
+
+# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries
+# shown in the various tree structured indices initially; the user can expand
+# and collapse entries dynamically later on. Doxygen will expand the tree to
+# such a level that at most the specified number of entries are visible (unless
+# a fully collapsed tree already exceeds this amount). So setting the number of
+# entries 1 will produce a full collapsed tree by default. 0 is a special value
+# representing an infinite number of entries and will result in a full expanded
+# tree by default.
+# Minimum value: 0, maximum value: 9999, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_INDEX_NUM_ENTRIES = 100
+
+# If the GENERATE_DOCSET tag is set to YES, additional index files will be
+# generated that can be used as input for Apple's Xcode 3 integrated development
+# environment (see: http://developer.apple.com/tools/xcode/), introduced with
+# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a
+# Makefile in the HTML output directory. Running make will produce the docset in
+# that directory and running make install will install the docset in
+# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at
+# startup. See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html
+# for more information.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_DOCSET        = NO
+
+# This tag determines the name of the docset feed. A documentation feed provides
+# an umbrella under which multiple documentation sets from a single provider
+# (such as a company or product suite) can be grouped.
+# The default value is: Doxygen generated docs.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_FEEDNAME        = "Doxygen generated docs"
+
+# This tag specifies a string that should uniquely identify the documentation
+# set bundle. This should be a reverse domain-name style string, e.g.
+# com.mycompany.MyDocSet. Doxygen will append .docset to the name.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_BUNDLE_ID       = org.doxygen.Project
+
+# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify
+# the documentation publisher. This should be a reverse domain-name style
+# string, e.g. com.mycompany.MyDocSet.documentation.
+# The default value is: org.doxygen.Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_ID    = org.doxygen.Publisher
+
+# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher.
+# The default value is: Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_NAME  = Publisher
+
+# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three
+# additional HTML index files: index.hhp, index.hhc, and index.hhk. The
+# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop
+# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on
+# Windows.
+#
+# The HTML Help Workshop contains a compiler that can convert all HTML output
+# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML
+# files are now used as the Windows 98 help format, and will replace the old
+# Windows help format (.hlp) on all Windows platforms in the future. Compressed
+# HTML files also contain an index, a table of contents, and you can search for
+# words in the documentation. The HTML workshop also contains a viewer for
+# compressed HTML files.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_HTMLHELP      = NO
+
+# The CHM_FILE tag can be used to specify the file name of the resulting .chm
+# file. You can add a path in front of the file if the result should not be
+# written to the html output directory.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_FILE               =
+
+# The HHC_LOCATION tag can be used to specify the location (absolute path
+# including file name) of the HTML help compiler (hhc.exe). If non-empty,
+# doxygen will try to run the HTML help compiler on the generated index.hhp.
+# The file has to be specified with full path.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+HHC_LOCATION           =
+
+# The GENERATE_CHI flag controls if a separate .chi index file is generated
+# (YES) or that it should be included in the master .chm file (NO).
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+GENERATE_CHI           = NO
+
+# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc)
+# and project file content.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_INDEX_ENCODING     =
+
+# The BINARY_TOC flag controls whether a binary table of contents is generated
+# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it
+# enables the Previous and Next buttons.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+BINARY_TOC             = NO
+
+# The TOC_EXPAND flag can be set to YES to add extra items for group members to
+# the table of contents of the HTML help documentation and to the tree view.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+TOC_EXPAND             = NO
+
+# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and
+# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that
+# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help
+# (.qch) of the generated HTML documentation.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_QHP           = NO
+
+# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify
+# the file name of the resulting .qch file. The path specified is relative to
+# the HTML output folder.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QCH_FILE               =
+
+# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help
+# Project output. For more information please see Qt Help Project / Namespace
+# (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace).
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_NAMESPACE          = org.doxygen.Project
+
+# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt
+# Help Project output. For more information please see Qt Help Project / Virtual
+# Folders (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual-
+# folders).
+# The default value is: doc.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_VIRTUAL_FOLDER     = doc
+
+# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom
+# filter to add. For more information please see Qt Help Project / Custom
+# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
+# filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_NAME   =
+
+# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the
+# custom filter to add. For more information please see Qt Help Project / Custom
+# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
+# filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_ATTRS  =
+
+# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this
+# project's filter section matches. Qt Help Project / Filter Attributes (see:
+# http://qt-project.org/doc/qt-4.8/qthelpproject.html#filter-attributes).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_SECT_FILTER_ATTRS  =
+
+# The QHG_LOCATION tag can be used to specify the location of Qt's
+# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the
+# generated .qhp file.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHG_LOCATION           =
+
+# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be
+# generated, together with the HTML files, they form an Eclipse help plugin. To
+# install this plugin and make it available under the help contents menu in
+# Eclipse, the contents of the directory containing the HTML and XML files needs
+# to be copied into the plugins directory of eclipse. The name of the directory
+# within the plugins directory should be the same as the ECLIPSE_DOC_ID value.
+# After copying Eclipse needs to be restarted before the help appears.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_ECLIPSEHELP   = NO
+
+# A unique identifier for the Eclipse help plugin. When installing the plugin
+# the directory name containing the HTML and XML files should also have this
+# name. Each documentation set should have its own identifier.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES.
+
+ECLIPSE_DOC_ID         = org.doxygen.Project
+
+# If you want full control over the layout of the generated HTML pages it might
+# be necessary to disable the index and replace it with your own. The
+# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top
+# of each HTML page. A value of NO enables the index and the value YES disables
+# it. Since the tabs in the index contain the same information as the navigation
+# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+DISABLE_INDEX          = NO
+
+# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index
+# structure should be generated to display hierarchical information. If the tag
+# value is set to YES, a side panel will be generated containing a tree-like
+# index structure (just like the one that is generated for HTML Help). For this
+# to work a browser that supports JavaScript, DHTML, CSS and frames is required
+# (i.e. any modern browser). Windows users are probably better off using the
+# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can
+# further fine-tune the look of the index. As an example, the default style
+# sheet generated by doxygen has an example that shows how to put an image at
+# the root of the tree instead of the PROJECT_NAME. Since the tree basically has
+# the same information as the tab index, you could consider setting
+# DISABLE_INDEX to YES when enabling this option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_TREEVIEW      = YES
+
+# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that
+# doxygen will group on one line in the generated HTML documentation.
+#
+# Note that a value of 0 will completely suppress the enum values from appearing
+# in the overview section.
+# Minimum value: 0, maximum value: 20, default value: 4.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+ENUM_VALUES_PER_LINE   = 4
+
+# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used
+# to set the initial width (in pixels) of the frame in which the tree is shown.
+# Minimum value: 0, maximum value: 1500, default value: 250.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+TREEVIEW_WIDTH         = 250
+
+# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to
+# external symbols imported via tag files in a separate window.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+EXT_LINKS_IN_WINDOW    = NO
+
+# Use this tag to change the font size of LaTeX formulas included as images in
+# the HTML documentation. When you change the font size after a successful
+# doxygen run you need to manually remove any form_*.png images from the HTML
+# output directory to force them to be regenerated.
+# Minimum value: 8, maximum value: 50, default value: 10.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_FONTSIZE       = 10
+
+# Use the FORMULA_TRANPARENT tag to determine whether or not the images
+# generated for formulas are transparent PNGs. Transparent PNGs are not
+# supported properly for IE 6.0, but are supported on all modern browsers.
+#
+# Note that when changing this option you need to delete any form_*.png files in
+# the HTML output directory before the changes have effect.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_TRANSPARENT    = YES
+
+# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see
+# http://www.mathjax.org) which uses client side Javascript for the rendering
+# instead of using pre-rendered bitmaps. Use this if you do not have LaTeX
+# installed or if you want to formulas look prettier in the HTML output. When
+# enabled you may also need to install MathJax separately and configure the path
+# to it using the MATHJAX_RELPATH option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+USE_MATHJAX            = NO
+
+# When MathJax is enabled you can set the default output format to be used for
+# the MathJax output. See the MathJax site (see:
+# http://docs.mathjax.org/en/latest/output.html) for more details.
+# Possible values are: HTML-CSS (which is slower, but has the best
+# compatibility), NativeMML (i.e. MathML) and SVG.
+# The default value is: HTML-CSS.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_FORMAT         = HTML-CSS
+
+# When MathJax is enabled you need to specify the location relative to the HTML
+# output directory using the MATHJAX_RELPATH option. The destination directory
+# should contain the MathJax.js script. For instance, if the mathjax directory
+# is located at the same level as the HTML output directory, then
+# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax
+# Content Delivery Network so you can quickly see the result without installing
+# MathJax. However, it is strongly recommended to install a local copy of
+# MathJax from http://www.mathjax.org before deployment.
+# The default value is: http://cdn.mathjax.org/mathjax/latest.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_RELPATH        = http://cdn.mathjax.org/mathjax/latest
+
+# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax
+# extension names that should be enabled during MathJax rendering. For example
+# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_EXTENSIONS     =
+
+# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces
+# of code that will be used on startup of the MathJax code. See the MathJax site
+# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an
+# example see the documentation.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_CODEFILE       =
+
+# When the SEARCHENGINE tag is enabled doxygen will generate a search box for
+# the HTML output. The underlying search engine uses javascript and DHTML and
+# should work on any modern browser. Note that when using HTML help
+# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET)
+# there is already a search function so this one should typically be disabled.
+# For large projects the javascript based search engine can be slow, then
+# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to
+# search using the keyboard; to jump to the search box use <access key> + S
+# (what the <access key> is depends on the OS and browser, but it is typically
+# <CTRL>, <ALT>/<option>, or both). Inside the search box use the <cursor down
+# key> to jump into the search results window, the results can be navigated
+# using the <cursor keys>. Press <Enter> to select an item or <escape> to cancel
+# the search. The filter options can be selected when the cursor is inside the
+# search box by pressing <Shift>+<cursor down>. Also here use the <cursor keys>
+# to select a filter and <Enter> or <escape> to activate or cancel the filter
+# option.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+SEARCHENGINE           = YES
+
+# When the SERVER_BASED_SEARCH tag is enabled the search engine will be
+# implemented using a web server instead of a web client using Javascript. There
+# are two flavors of web server based searching depending on the EXTERNAL_SEARCH
+# setting. When disabled, doxygen will generate a PHP script for searching and
+# an index file used by the script. When EXTERNAL_SEARCH is enabled the indexing
+# and searching needs to be provided by external tools. See the section
+# "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SERVER_BASED_SEARCH    = NO
+
+# When EXTERNAL_SEARCH tag is enabled doxygen will no longer generate the PHP
+# script for searching. Instead the search results are written to an XML file
+# which needs to be processed by an external indexer. Doxygen will invoke an
+# external search engine pointed to by the SEARCHENGINE_URL option to obtain the
+# search results.
+#
+# Doxygen ships with an example indexer (doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see: http://xapian.org/).
+#
+# See the section "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH        = NO
+
+# The SEARCHENGINE_URL should point to a search engine hosted by a web server
+# which will return the search results when EXTERNAL_SEARCH is enabled.
+#
+# Doxygen ships with an example indexer (doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see: http://xapian.org/). See the section "External Indexing and
+# Searching" for details.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHENGINE_URL       =
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the unindexed
+# search data is written to a file for indexing by an external tool. With the
+# SEARCHDATA_FILE tag the name of this file can be specified.
+# The default file is: searchdata.xml.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHDATA_FILE        = searchdata.xml
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the
+# EXTERNAL_SEARCH_ID tag can be used as an identifier for the project. This is
+# useful in combination with EXTRA_SEARCH_MAPPINGS to search through multiple
+# projects and redirect the results back to the right project.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH_ID     =
+
+# The EXTRA_SEARCH_MAPPINGS tag can be used to enable searching through doxygen
+# projects other than the one defined by this configuration file, but that are
+# all added to the same external search index. Each project needs to have a
+# unique id set via EXTERNAL_SEARCH_ID. The search mapping then maps the id of
+# to a relative location where the documentation can be found. The format is:
+# EXTRA_SEARCH_MAPPINGS = tagname1=loc1 tagname2=loc2 ...
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTRA_SEARCH_MAPPINGS  =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_LATEX tag is set to YES, doxygen will generate LaTeX output.
+# The default value is: YES.
+
+GENERATE_LATEX         = NO
+
+# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_OUTPUT           = latex
+
+# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be
+# invoked.
+#
+# Note that when enabling USE_PDFLATEX this option is only used for generating
+# bitmaps for formulas in the HTML output, but not in the Makefile that is
+# written to the output directory.
+# The default file is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_CMD_NAME         = latex
+
+# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to generate
+# index for LaTeX.
+# The default file is: makeindex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+MAKEINDEX_CMD_NAME     = makeindex
+
+# If the COMPACT_LATEX tag is set to YES, doxygen generates more compact LaTeX
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+COMPACT_LATEX          = NO
+
+# The PAPER_TYPE tag can be used to set the paper type that is used by the
+# printer.
+# Possible values are: a4 (210 x 297 mm), letter (8.5 x 11 inches), legal (8.5 x
+# 14 inches) and executive (7.25 x 10.5 inches).
+# The default value is: a4.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PAPER_TYPE             = a4
+
+# The EXTRA_PACKAGES tag can be used to specify one or more LaTeX package names
+# that should be included in the LaTeX output. The package can be specified just
+# by its name or with the correct syntax as to be used with the LaTeX
+# \usepackage command. To get the times font for instance you can specify :
+# EXTRA_PACKAGES=times or EXTRA_PACKAGES={times}
+# To use the option intlimits with the amsmath package you can specify:
+# EXTRA_PACKAGES=[intlimits]{amsmath}
+# If left blank no extra packages will be included.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+EXTRA_PACKAGES         =
+
+# The LATEX_HEADER tag can be used to specify a personal LaTeX header for the
+# generated LaTeX document. The header should contain everything until the first
+# chapter. If it is left blank doxygen will generate a standard header. See
+# section "Doxygen usage" for information on how to let doxygen write the
+# default header to a separate file.
+#
+# Note: Only use a user-defined header if you know what you are doing! The
+# following commands have a special meaning inside the header: $title,
+# $datetime, $date, $doxygenversion, $projectname, $projectnumber,
+# $projectbrief, $projectlogo. Doxygen will replace $title with the empty
+# string, for the replacement values of the other commands the user is referred
+# to HTML_HEADER.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HEADER           =
+
+# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for the
+# generated LaTeX document. The footer should contain everything after the last
+# chapter. If it is left blank doxygen will generate a standard footer. See
+# LATEX_HEADER for more information on how to generate a default footer and what
+# special commands can be used inside the footer.
+#
+# Note: Only use a user-defined footer if you know what you are doing!
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_FOOTER           =
+
+# The LATEX_EXTRA_STYLESHEET tag can be used to specify additional user-defined
+# LaTeX style sheets that are included after the standard style sheets created
+# by doxygen. Using this option one can overrule certain style aspects. Doxygen
+# will copy the style sheet files to the output directory.
+# Note: The order of the extra style sheet files is of importance (e.g. the last
+# style sheet in the list overrules the setting of the previous ones in the
+# list).
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_EXTRA_STYLESHEET =
+
+# The LATEX_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the LATEX_OUTPUT output
+# directory. Note that the files will be copied as-is; there are no commands or
+# markers available.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_EXTRA_FILES      =
+
+# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated is
+# prepared for conversion to PDF (using ps2pdf or pdflatex). The PDF file will
+# contain links (just like the HTML output) instead of page references. This
+# makes the output suitable for online browsing using a PDF viewer.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PDF_HYPERLINKS         = YES
+
+# If the USE_PDFLATEX tag is set to YES, doxygen will use pdflatex to generate
+# the PDF file directly from the LaTeX files. Set this option to YES, to get a
+# higher quality PDF documentation.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+USE_PDFLATEX           = YES
+
+# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \batchmode
+# command to the generated LaTeX files. This will instruct LaTeX to keep running
+# if errors occur, instead of asking the user for help. This option is also used
+# when generating formulas in HTML.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BATCHMODE        = NO
+
+# If the LATEX_HIDE_INDICES tag is set to YES then doxygen will not include the
+# index chapters (such as File Index, Compound Index, etc.) in the output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HIDE_INDICES     = NO
+
+# If the LATEX_SOURCE_CODE tag is set to YES then doxygen will include source
+# code with syntax highlighting in the LaTeX output.
+#
+# Note that which sources are shown also depends on other settings such as
+# SOURCE_BROWSER.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_SOURCE_CODE      = NO
+
+# The LATEX_BIB_STYLE tag can be used to specify the style to use for the
+# bibliography, e.g. plainnat, or ieeetr. See
+# http://en.wikipedia.org/wiki/BibTeX and \cite for more info.
+# The default value is: plain.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BIB_STYLE        = plain
+
+# If the LATEX_TIMESTAMP tag is set to YES then the footer of each generated
+# page will contain the date and time when the page was generated. Setting this
+# to NO can help when comparing the output of multiple runs.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_TIMESTAMP        = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the RTF output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_RTF tag is set to YES, doxygen will generate RTF output. The
+# RTF output is optimized for Word 97 and may not look too pretty with other RTF
+# readers/editors.
+# The default value is: NO.
+
+GENERATE_RTF           = NO
+
+# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: rtf.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_OUTPUT             = rtf
+
+# If the COMPACT_RTF tag is set to YES, doxygen generates more compact RTF
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+COMPACT_RTF            = NO
+
+# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated will
+# contain hyperlink fields. The RTF file will contain links (just like the HTML
+# output) instead of page references. This makes the output suitable for online
+# browsing using Word or some other Word compatible readers that support those
+# fields.
+#
+# Note: WordPad (write) and others do not support links.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_HYPERLINKS         = NO
+
+# Load stylesheet definitions from file. Syntax is similar to doxygen's config
+# file, i.e. a series of assignments. You only have to provide replacements,
+# missing definitions are set to their default value.
+#
+# See also section "Doxygen usage" for information on how to generate the
+# default style sheet that doxygen normally uses.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_STYLESHEET_FILE    =
+
+# Set optional variables used in the generation of an RTF document. Syntax is
+# similar to doxygen's config file. A template extensions file can be generated
+# using doxygen -e rtf extensionFile.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_EXTENSIONS_FILE    =
+
+# If the RTF_SOURCE_CODE tag is set to YES then doxygen will include source code
+# with syntax highlighting in the RTF output.
+#
+# Note that which sources are shown also depends on other settings such as
+# SOURCE_BROWSER.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_SOURCE_CODE        = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the man page output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_MAN tag is set to YES, doxygen will generate man pages for
+# classes and files.
+# The default value is: NO.
+
+GENERATE_MAN           = NO
+
+# The MAN_OUTPUT tag is used to specify where the man pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it. A directory man3 will be created inside the directory specified by
+# MAN_OUTPUT.
+# The default directory is: man.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_OUTPUT             = man
+
+# The MAN_EXTENSION tag determines the extension that is added to the generated
+# man pages. In case the manual section does not start with a number, the number
+# 3 is prepended. The dot (.) at the beginning of the MAN_EXTENSION tag is
+# optional.
+# The default value is: .3.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_EXTENSION          = .3
+
+# The MAN_SUBDIR tag determines the name of the directory created within
+# MAN_OUTPUT in which the man pages are placed. If defaults to man followed by
+# MAN_EXTENSION with the initial . removed.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_SUBDIR             =
+
+# If the MAN_LINKS tag is set to YES and doxygen generates man output, then it
+# will generate one additional man file for each entity documented in the real
+# man page(s). These additional files only source the real man page, but without
+# them the man command would be unable to find the correct page.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_LINKS              = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the XML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_XML tag is set to YES, doxygen will generate an XML file that
+# captures the structure of the code including all documentation.
+# The default value is: NO.
+
+GENERATE_XML           = NO
+
+# The XML_OUTPUT tag is used to specify where the XML pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: xml.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_OUTPUT             = xml
+
+# If the XML_PROGRAMLISTING tag is set to YES, doxygen will dump the program
+# listings (including syntax highlighting and cross-referencing information) to
+# the XML output. Note that enabling this will significantly increase the size
+# of the XML output.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_PROGRAMLISTING     = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to the DOCBOOK output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_DOCBOOK tag is set to YES, doxygen will generate Docbook files
+# that can be used to generate PDF.
+# The default value is: NO.
+
+GENERATE_DOCBOOK       = NO
+
+# The DOCBOOK_OUTPUT tag is used to specify where the Docbook pages will be put.
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be put in
+# front of it.
+# The default directory is: docbook.
+# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
+
+DOCBOOK_OUTPUT         = docbook
+
+# If the DOCBOOK_PROGRAMLISTING tag is set to YES, doxygen will include the
+# program listings (including syntax highlighting and cross-referencing
+# information) to the DOCBOOK output. Note that enabling this will significantly
+# increase the size of the DOCBOOK output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
+
+DOCBOOK_PROGRAMLISTING = NO
+
+#---------------------------------------------------------------------------
+# Configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_AUTOGEN_DEF tag is set to YES, doxygen will generate an
+# AutoGen Definitions (see http://autogen.sf.net) file that captures the
+# structure of the code including all documentation. Note that this feature is
+# still experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_AUTOGEN_DEF   = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_PERLMOD tag is set to YES, doxygen will generate a Perl module
+# file that captures the structure of the code including all documentation.
+#
+# Note that this feature is still experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_PERLMOD       = NO
+
+# If the PERLMOD_LATEX tag is set to YES, doxygen will generate the necessary
+# Makefile rules, Perl scripts and LaTeX code to be able to generate PDF and DVI
+# output from the Perl module output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_LATEX          = NO
+
+# If the PERLMOD_PRETTY tag is set to YES, the Perl module output will be nicely
+# formatted so it can be parsed by a human reader. This is useful if you want to
+# understand what is going on. On the other hand, if this tag is set to NO, the
+# size of the Perl module output will be much smaller and Perl will parse it
+# just the same.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_PRETTY         = YES
+
+# The names of the make variables in the generated doxyrules.make file are
+# prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. This is useful
+# so different doxyrules.make files included by the same Makefile don't
+# overwrite each other's variables.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_MAKEVAR_PREFIX =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor
+#---------------------------------------------------------------------------
+
+# If the ENABLE_PREPROCESSING tag is set to YES, doxygen will evaluate all
+# C-preprocessor directives found in the sources and include files.
+# The default value is: YES.
+
+ENABLE_PREPROCESSING   = YES
+
+# If the MACRO_EXPANSION tag is set to YES, doxygen will expand all macro names
+# in the source code. If set to NO, only conditional compilation will be
+# performed. Macro expansion can be done in a controlled way by setting
+# EXPAND_ONLY_PREDEF to YES.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+MACRO_EXPANSION        = NO
+
+# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES then
+# the macro expansion is limited to the macros specified with the PREDEFINED and
+# EXPAND_AS_DEFINED tags.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_ONLY_PREDEF     = NO
+
+# If the SEARCH_INCLUDES tag is set to YES, the include files in the
+# INCLUDE_PATH will be searched if a #include is found.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SEARCH_INCLUDES        = YES
+
+# The INCLUDE_PATH tag can be used to specify one or more directories that
+# contain include files that are not input files but should be processed by the
+# preprocessor.
+# This tag requires that the tag SEARCH_INCLUDES is set to YES.
+
+INCLUDE_PATH           =
+
+# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard
+# patterns (like *.h and *.hpp) to filter out the header-files in the
+# directories. If left blank, the patterns specified with FILE_PATTERNS will be
+# used.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+INCLUDE_FILE_PATTERNS  =
+
+# The PREDEFINED tag can be used to specify one or more macro names that are
+# defined before the preprocessor is started (similar to the -D option of e.g.
+# gcc). The argument of the tag is a list of macros of the form: name or
+# name=definition (no spaces). If the definition and the "=" are omitted, "=1"
+# is assumed. To prevent a macro definition from being undefined via #undef or
+# recursively expanded use the := operator instead of the = operator.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+PREDEFINED             =
+
+# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this
+# tag can be used to specify a list of macro names that should be expanded. The
+# macro definition that is found in the sources will be used. Use the PREDEFINED
+# tag if you want to use a different macro definition that overrules the
+# definition found in the source code.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_AS_DEFINED      =
+
+# If the SKIP_FUNCTION_MACROS tag is set to YES then doxygen's preprocessor will
+# remove all references to function-like macros that are alone on a line, have
+# an all uppercase name, and do not end with a semicolon. Such function macros
+# are typically used for boiler-plate code, and will confuse the parser if not
+# removed.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SKIP_FUNCTION_MACROS   = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to external references
+#---------------------------------------------------------------------------
+
+# The TAGFILES tag can be used to specify one or more tag files. For each tag
+# file the location of the external documentation should be added. The format of
+# a tag file without this location is as follows:
+# TAGFILES = file1 file2 ...
+# Adding location for the tag files is done as follows:
+# TAGFILES = file1=loc1 "file2 = loc2" ...
+# where loc1 and loc2 can be relative or absolute paths or URLs. See the
+# section "Linking to external documentation" for more information about the use
+# of tag files.
+# Note: Each tag file must have a unique name (where the name does NOT include
+# the path). If a tag file is not located in the directory in which doxygen is
+# run, you must also specify the path to the tagfile here.
+
+TAGFILES               =
+
+# When a file name is specified after GENERATE_TAGFILE, doxygen will create a
+# tag file that is based on the input files it reads. See section "Linking to
+# external documentation" for more information about the usage of tag files.
+
+GENERATE_TAGFILE       =
+
+# If the ALLEXTERNALS tag is set to YES, all external class will be listed in
+# the class index. If set to NO, only the inherited external classes will be
+# listed.
+# The default value is: NO.
+
+ALLEXTERNALS           = NO
+
+# If the EXTERNAL_GROUPS tag is set to YES, all external groups will be listed
+# in the modules index. If set to NO, only the current project's groups will be
+# listed.
+# The default value is: YES.
+
+EXTERNAL_GROUPS        = YES
+
+# If the EXTERNAL_PAGES tag is set to YES, all external pages will be listed in
+# the related pages index. If set to NO, only the current project's pages will
+# be listed.
+# The default value is: YES.
+
+EXTERNAL_PAGES         = YES
+
+# The PERL_PATH should be the absolute path and name of the perl script
+# interpreter (i.e. the result of 'which perl').
+# The default file (with absolute path) is: /usr/bin/perl.
+
+PERL_PATH              = /usr/bin/perl
+
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool
+#---------------------------------------------------------------------------
+
+# If the CLASS_DIAGRAMS tag is set to YES, doxygen will generate a class diagram
+# (in HTML and LaTeX) for classes with base or super classes. Setting the tag to
+# NO turns the diagrams off. Note that this option also works with HAVE_DOT
+# disabled, but it is recommended to install and use dot, since it yields more
+# powerful graphs.
+# The default value is: YES.
+
+CLASS_DIAGRAMS         = YES
+
+# You can define message sequence charts within doxygen comments using the \msc
+# command. Doxygen will then run the mscgen tool (see:
+# http://www.mcternan.me.uk/mscgen/)) to produce the chart and insert it in the
+# documentation. The MSCGEN_PATH tag allows you to specify the directory where
+# the mscgen tool resides. If left empty the tool is assumed to be found in the
+# default search path.
+
+MSCGEN_PATH            =
+
+# You can include diagrams made with dia in doxygen documentation. Doxygen will
+# then run dia to produce the diagram and insert it in the documentation. The
+# DIA_PATH tag allows you to specify the directory where the dia binary resides.
+# If left empty dia is assumed to be found in the default search path.
+
+DIA_PATH               =
+
+# If set to YES the inheritance and collaboration graphs will hide inheritance
+# and usage relations if the target is undocumented or is not a class.
+# The default value is: YES.
+
+HIDE_UNDOC_RELATIONS   = YES
+
+# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is
+# available from the path. This tool is part of Graphviz (see:
+# http://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent
+# Bell Labs. The other options in this section have no effect if this option is
+# set to NO
+# The default value is: YES.
+
+HAVE_DOT               = YES
+
+# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed
+# to run in parallel. When set to 0 doxygen will base this on the number of
+# processors available in the system. You can set it explicitly to a value
+# larger than 0 to get control over the balance between CPU load and processing
+# speed.
+# Minimum value: 0, maximum value: 32, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_NUM_THREADS        = 0
+
+# When you want a differently looking font in the dot files that doxygen
+# generates you can specify the font name using DOT_FONTNAME. You need to make
+# sure dot is able to find the font, which can be done by putting it in a
+# standard location or by setting the DOTFONTPATH environment variable or by
+# setting DOT_FONTPATH to the directory containing the font.
+# The default value is: Helvetica.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTNAME           = Helvetica
+
+# The DOT_FONTSIZE tag can be used to set the size (in points) of the font of
+# dot graphs.
+# Minimum value: 4, maximum value: 24, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTSIZE           = 10
+
+# By default doxygen will tell dot to use the default font as specified with
+# DOT_FONTNAME. If you specify a different font using DOT_FONTNAME you can set
+# the path where dot can find it using this tag.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTPATH           =
+
+# If the CLASS_GRAPH tag is set to YES then doxygen will generate a graph for
+# each documented class showing the direct and indirect inheritance relations.
+# Setting this tag to YES will force the CLASS_DIAGRAMS tag to NO.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CLASS_GRAPH            = YES
+
+# If the COLLABORATION_GRAPH tag is set to YES then doxygen will generate a
+# graph for each documented class showing the direct and indirect implementation
+# dependencies (inheritance, containment, and class references variables) of the
+# class with other documented classes.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+COLLABORATION_GRAPH    = YES
+
+# If the GROUP_GRAPHS tag is set to YES then doxygen will generate a graph for
+# groups, showing the direct groups dependencies.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GROUP_GRAPHS           = YES
+
+# If the UML_LOOK tag is set to YES, doxygen will generate inheritance and
+# collaboration diagrams in a style similar to the OMG's Unified Modeling
+# Language.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LOOK               = NO
+
+# If the UML_LOOK tag is enabled, the fields and methods are shown inside the
+# class node. If there are many fields or methods and many nodes the graph may
+# become too big to be useful. The UML_LIMIT_NUM_FIELDS threshold limits the
+# number of items for each type to make the size more manageable. Set this to 0
+# for no limit. Note that the threshold may be exceeded by 50% before the limit
+# is enforced. So when you set the threshold to 10, up to 15 fields may appear,
+# but if the number exceeds 15, the total amount of fields shown is limited to
+# 10.
+# Minimum value: 0, maximum value: 100, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LIMIT_NUM_FIELDS   = 10
+
+# If the TEMPLATE_RELATIONS tag is set to YES then the inheritance and
+# collaboration graphs will show the relations between templates and their
+# instances.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+TEMPLATE_RELATIONS     = NO
+
+# If the INCLUDE_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are set to
+# YES then doxygen will generate a graph for each documented file showing the
+# direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDE_GRAPH          = YES
+
+# If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are
+# set to YES then doxygen will generate a graph for each documented file showing
+# the direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDED_BY_GRAPH      = YES
+
+# If the CALL_GRAPH tag is set to YES then doxygen will generate a call
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable call graphs for selected
+# functions only using the \callgraph command. Disabling a call graph can be
+# accomplished by means of the command \hidecallgraph.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALL_GRAPH             = NO
+
+# If the CALLER_GRAPH tag is set to YES then doxygen will generate a caller
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable caller graphs for selected
+# functions only using the \callergraph command. Disabling a caller graph can be
+# accomplished by means of the command \hidecallergraph.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALLER_GRAPH           = NO
+
+# If the GRAPHICAL_HIERARCHY tag is set to YES then doxygen will graphical
+# hierarchy of all classes instead of a textual one.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GRAPHICAL_HIERARCHY    = YES
+
+# If the DIRECTORY_GRAPH tag is set to YES then doxygen will show the
+# dependencies a directory has on other directories in a graphical way. The
+# dependency relations are determined by the #include relations between the
+# files in the directories.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DIRECTORY_GRAPH        = YES
+
+# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images
+# generated by dot. For an explanation of the image formats see the section
+# output formats in the documentation of the dot tool (Graphviz (see:
+# http://www.graphviz.org/)).
+# Note: If you choose svg you need to set HTML_FILE_EXTENSION to xhtml in order
+# to make the SVG files visible in IE 9+ (other browsers do not have this
+# requirement).
+# Possible values are: png, png:cairo, png:cairo:cairo, png:cairo:gd, png:gd,
+# png:gd:gd, jpg, jpg:cairo, jpg:cairo:gd, jpg:gd, jpg:gd:gd, gif, gif:cairo,
+# gif:cairo:gd, gif:gd, gif:gd:gd, svg, png:gd, png:gd:gd, png:cairo,
+# png:cairo:gd, png:cairo:cairo, png:cairo:gdiplus, png:gdiplus and
+# png:gdiplus:gdiplus.
+# The default value is: png.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_IMAGE_FORMAT       = png
+
+# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to
+# enable generation of interactive SVG images that allow zooming and panning.
+#
+# Note that this requires a modern browser other than Internet Explorer. Tested
+# and working are Firefox, Chrome, Safari, and Opera.
+# Note: For IE 9+ you need to set HTML_FILE_EXTENSION to xhtml in order to make
+# the SVG files visible. Older versions of IE do not have SVG support.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INTERACTIVE_SVG        = NO
+
+# The DOT_PATH tag can be used to specify the path where the dot tool can be
+# found. If left blank, it is assumed the dot tool can be found in the path.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_PATH               =
+
+# The DOTFILE_DIRS tag can be used to specify one or more directories that
+# contain dot files that are included in the documentation (see the \dotfile
+# command).
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOTFILE_DIRS           =
+
+# The MSCFILE_DIRS tag can be used to specify one or more directories that
+# contain msc files that are included in the documentation (see the \mscfile
+# command).
+
+MSCFILE_DIRS           =
+
+# The DIAFILE_DIRS tag can be used to specify one or more directories that
+# contain dia files that are included in the documentation (see the \diafile
+# command).
+
+DIAFILE_DIRS           =
+
+# When using plantuml, the PLANTUML_JAR_PATH tag should be used to specify the
+# path where java can find the plantuml.jar file. If left blank, it is assumed
+# PlantUML is not used or called during a preprocessing step. Doxygen will
+# generate a warning when it encounters a \startuml command in this case and
+# will not generate output for the diagram.
+
+PLANTUML_JAR_PATH      =
+
+# When using plantuml, the specified paths are searched for files specified by
+# the !include statement in a plantuml block.
+
+PLANTUML_INCLUDE_PATH  =
+
+# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of nodes
+# that will be shown in the graph. If the number of nodes in a graph becomes
+# larger than this value, doxygen will truncate the graph, which is visualized
+# by representing a node as a red box. Note that doxygen if the number of direct
+# children of the root node in a graph is already larger than
+# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note that
+# the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH.
+# Minimum value: 0, maximum value: 10000, default value: 50.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_GRAPH_MAX_NODES    = 50
+
+# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the graphs
+# generated by dot. A depth value of 3 means that only nodes reachable from the
+# root by following a path via at most 3 edges will be shown. Nodes that lay
+# further from the root node will be omitted. Note that setting this option to 1
+# or 2 may greatly reduce the computation time needed for large code bases. Also
+# note that the size of a graph can be further restricted by
+# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction.
+# Minimum value: 0, maximum value: 1000, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+MAX_DOT_GRAPH_DEPTH    = 0
+
+# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent
+# background. This is disabled by default, because dot on Windows does not seem
+# to support this out of the box.
+#
+# Warning: Depending on the platform used, enabling this option may lead to
+# badly anti-aliased labels on the edges of a graph (i.e. they become hard to
+# read).
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_TRANSPARENT        = NO
+
+# Set the DOT_MULTI_TARGETS tag to YES to allow dot to generate multiple output
+# files in one run (i.e. multiple -o and -T options on the command line). This
+# makes dot run faster, but since only newer versions of dot (>1.8.10) support
+# this, this feature is disabled by default.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_MULTI_TARGETS      = NO
+
+# If the GENERATE_LEGEND tag is set to YES doxygen will generate a legend page
+# explaining the meaning of the various boxes and arrows in the dot generated
+# graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GENERATE_LEGEND        = YES
+
+# If the DOT_CLEANUP tag is set to YES, doxygen will remove the intermediate dot
+# files that are used to generate the various graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_CLEANUP            = YES
diff --git a/doc/architecture/Navigation-System.eap b/doc/architecture/Navigation-System.eap
new file mode 100644
index 0000000000000000000000000000000000000000..3726366eb1fd08b2bff72953dabb697de07abd85
Binary files /dev/null and b/doc/architecture/Navigation-System.eap differ
diff --git a/doc/images/ros2-logo.png b/doc/images/ros2-logo.png
deleted file mode 100644
index 3eba505cea57d713da6490c045db0bf9847fcf06..0000000000000000000000000000000000000000
Binary files a/doc/images/ros2-logo.png and /dev/null differ
diff --git a/include/.gitignore b/src/control/nav2_control_msgs/.gitignore
similarity index 100%
rename from include/.gitignore
rename to src/control/nav2_control_msgs/.gitignore
diff --git a/src/.gitignore b/src/control/nav2_dwa_controller/CHANGELOG.rst
similarity index 100%
rename from src/.gitignore
rename to src/control/nav2_dwa_controller/CHANGELOG.rst
diff --git a/src/control/nav2_dwa_controller/CMakeLists.txt b/src/control/nav2_dwa_controller/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..2d42f50b4e26536195b76d573dae046f76dacdec
--- /dev/null
+++ b/src/control/nav2_dwa_controller/CMakeLists.txt
@@ -0,0 +1,63 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_dwa_controller)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(nav2_tasks REQUIRED)
+find_package(nav2_robot REQUIRED)
+find_package(nav2_planning_msgs REQUIRED)
+
+include_directories(
+  include
+)
+
+add_executable(dwa_controller
+  src/dwa_controller.cpp
+  src/main.cpp
+)
+
+ament_target_dependencies(dwa_controller
+  rclcpp
+  std_msgs
+  nav2_tasks
+  nav2_robot
+  nav2_planning_msgs
+)
+
+install(TARGETS dwa_controller
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_cppcheck REQUIRED)
+  find_package(ament_cmake_cpplint REQUIRED)
+  find_package(ament_cmake_lint_cmake REQUIRED)
+  find_package(ament_cmake_uncrustify REQUIRED)
+  # This forces cppcheck to consider all files in this project to be C++,
+  # including the headers which end with .h, which cppcheck would normally
+  # consider to be C instead.
+  ament_cppcheck(LANGUAGE "c++")
+  ament_cpplint()
+  ament_lint_cmake()
+  ament_uncrustify()
+endif()
+
+ament_export_include_directories(include)
+
+ament_package()
diff --git a/test/.gitignore b/src/control/nav2_dwa_controller/README.md
similarity index 100%
rename from test/.gitignore
rename to src/control/nav2_dwa_controller/README.md
diff --git a/src/control/nav2_dwa_controller/doc/.gitignore b/src/control/nav2_dwa_controller/doc/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/control/nav2_dwa_controller/include/nav2_dwa_controller/dwa_controller.hpp b/src/control/nav2_dwa_controller/include/nav2_dwa_controller/dwa_controller.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..5d84a59de38b0f6be70d758e829228b6221d0470
--- /dev/null
+++ b/src/control/nav2_dwa_controller/include/nav2_dwa_controller/dwa_controller.hpp
@@ -0,0 +1,35 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_DWA_CONTROLLER__DWA_CONTROLLER_HPP_
+#define NAV2_DWA_CONTROLLER__DWA_CONTROLLER_HPP_
+
+#include <string>
+#include "nav2_tasks/follow_path_task.hpp"
+
+namespace nav2_dwa_controller
+{
+
+class DwaController : public nav2_tasks::FollowPathTaskServer
+{
+public:
+  DwaController();
+  ~DwaController();
+
+  nav2_tasks::TaskStatus execute(const nav2_tasks::FollowPathCommand::SharedPtr path) override;
+};
+
+}  // namespace nav2_dwa_controller
+
+#endif  // NAV2_DWA_CONTROLLER__DWA_CONTROLLER_HPP_
diff --git a/src/control/nav2_dwa_controller/package.xml b/src/control/nav2_dwa_controller/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..d29052302ab9c408712623d855b22c866784d828
--- /dev/null
+++ b/src/control/nav2_dwa_controller/package.xml
@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_dwa_controller</name>
+  <version>0.1.0</version>
+  <description>TODO</description>
+  <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rclcpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>nav2_tasks</build_depend>
+  <build_depend>nav2_robot</build_depend>
+  <build_depend>nav2_planning_msgs</build_depend>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>nav2_planning_msgs</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/control/nav2_dwa_controller/src/dwa_controller.cpp b/src/control/nav2_dwa_controller/src/dwa_controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f2c0f9ba89f4ea656338e49974041dc0be6a3b2e
--- /dev/null
+++ b/src/control/nav2_dwa_controller/src/dwa_controller.cpp
@@ -0,0 +1,64 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+#include <chrono>
+#include "nav2_dwa_controller/dwa_controller.hpp"
+
+using namespace std::chrono_literals;
+using nav2_tasks::TaskStatus;
+
+namespace nav2_dwa_controller
+{
+
+DwaController::DwaController()
+: nav2_tasks::FollowPathTaskServer("FollowPathNode")
+{
+  RCLCPP_INFO(get_logger(), "DwaController::DwaController");
+}
+
+DwaController::~DwaController()
+{
+  RCLCPP_INFO(get_logger(), "DwaController::~DwaController");
+}
+
+TaskStatus
+DwaController::execute(const nav2_tasks::FollowPathCommand::SharedPtr /*command*/)
+{
+  RCLCPP_INFO(get_logger(), "DwaController::execute");
+
+  // Spin here for a bit to fake out some processing time
+  for (int i = 0; i < 10; i++) {
+    // Do a bit of the task
+    RCLCPP_INFO(get_logger(), "DwaController::execute: doing work: %d", i);
+    std::this_thread::sleep_for(250ms);
+
+    // Before we loop again to do more work, check if we've been canceled
+    if (cancelRequested()) {
+      RCLCPP_INFO(get_logger(), "DwaController::execute: task has been canceled");
+      setCanceled();
+      return TaskStatus::CANCELED;
+    }
+  }
+
+  // We've successfully completed the task, so return the result
+  RCLCPP_INFO(get_logger(), "DwaController::execute: task completed");
+
+  nav2_tasks::FollowPathResult result;
+  setResult(result);
+
+  return TaskStatus::SUCCEEDED;
+}
+
+}  // namespace nav2_dwa_controller
diff --git a/src/control/nav2_dwa_controller/src/main.cpp b/src/control/nav2_dwa_controller/src/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f41615ed8449542e15d97438db990e9893e38ff9
--- /dev/null
+++ b/src/control/nav2_dwa_controller/src/main.cpp
@@ -0,0 +1,26 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_dwa_controller/dwa_controller.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<nav2_dwa_controller::DwaController>());
+  rclcpp::shutdown();
+
+  return 0;
+}
diff --git a/src/control/nav2_dwa_controller/test/.gitignore b/src/control/nav2_dwa_controller/test/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/libs/nav2_libs_msgs/CHANGELOG.rst b/src/libs/nav2_libs_msgs/CHANGELOG.rst
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/libs/nav2_libs_msgs/CMakeLists.txt b/src/libs/nav2_libs_msgs/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..a1a6c95a4dfd317164fd12a6517ac9c84f42668d
--- /dev/null
+++ b/src/libs/nav2_libs_msgs/CMakeLists.txt
@@ -0,0 +1,41 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_libs_msgs)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/Costmap.msg"
+  "msg/CostmapMetaData.msg"
+  DEPENDENCIES builtin_interfaces geometry_msgs std_msgs
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_cppcheck REQUIRED)
+  find_package(ament_cmake_cpplint REQUIRED)
+  find_package(ament_cmake_lint_cmake REQUIRED)
+  find_package(ament_cmake_uncrustify REQUIRED)
+  # This forces cppcheck to consider all files in this project to be C++,
+  # including the headers which end with .h, which cppcheck would normally
+  # consider to be C instead.
+  ament_cppcheck(LANGUAGE "c++")
+  ament_cpplint()
+  ament_lint_cmake()
+  ament_uncrustify()
+endif()
+
+ament_export_dependencies(rosidl_default_runtime)
+
+ament_package()
diff --git a/src/libs/nav2_libs_msgs/README.md b/src/libs/nav2_libs_msgs/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/libs/nav2_libs_msgs/msg/Costmap.msg b/src/libs/nav2_libs_msgs/msg/Costmap.msg
new file mode 100644
index 0000000000000000000000000000000000000000..4c3e90ca70cd59d7afe0ff33b574addf2893a488
--- /dev/null
+++ b/src/libs/nav2_libs_msgs/msg/Costmap.msg
@@ -0,0 +1,9 @@
+# This represents a 2-D grid map, in which each cell has an associated cost
+
+std_msgs/Header header
+
+# MetaData for the map
+CostmapMetaData metadata
+
+# The cost data, in row-major order, starting with (0,0).
+uint8[] data
diff --git a/src/libs/nav2_libs_msgs/msg/CostmapMetaData.msg b/src/libs/nav2_libs_msgs/msg/CostmapMetaData.msg
new file mode 100644
index 0000000000000000000000000000000000000000..6338795dad803474a7a48248f85fe515ff17f07d
--- /dev/null
+++ b/src/libs/nav2_libs_msgs/msg/CostmapMetaData.msg
@@ -0,0 +1,23 @@
+# This hold basic information about the characterists of the Costmap
+
+# The time at which the static map was loaded
+builtin_interfaces/Time map_load_time
+
+# The time of the last update to costmap
+builtin_interfaces/Time update_time
+
+# The corresponding layer name
+string layer
+
+# The map resolution [m/cell]
+float32 resolution
+
+# Number of cells in the horizontal direction
+uint32 size_x
+
+# Number of cells in the vertical direction
+uint32 size_y
+
+# The origin of the costmap [m, m, rad].
+# This is the real-world pose of the cell (0,0) in the map.
+geometry_msgs/Pose origin
diff --git a/src/libs/nav2_libs_msgs/package.xml b/src/libs/nav2_libs_msgs/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..67b9e467390f4500048e61055a78426ed3f818e0
--- /dev/null
+++ b/src/libs/nav2_libs_msgs/package.xml
@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_libs_msgs</name>
+  <version>0.1.0</version>
+  <description>TODO</description>
+  <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rclcpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>builtin_interfaces</build_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>builtin_interfaces</exec_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/libs/nav2_robot/CHANGELOG.rst b/src/libs/nav2_robot/CHANGELOG.rst
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/libs/nav2_robot/CMakeLists.txt b/src/libs/nav2_robot/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..f7e09bc93561bb5fcf8b7f17e15457efa2d6b2ae
--- /dev/null
+++ b/src/libs/nav2_robot/CMakeLists.txt
@@ -0,0 +1,50 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_robot)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+
+include_directories(
+  include
+)
+
+add_library(nav2_robot STATIC
+  src/ros_robot.cpp
+)
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_cppcheck REQUIRED)
+  find_package(ament_cmake_cpplint REQUIRED)
+  find_package(ament_cmake_lint_cmake REQUIRED)
+  find_package(ament_cmake_uncrustify REQUIRED)
+  # This forces cppcheck to consider all files in this project to be C++,
+  # including the headers which end with .h, which cppcheck would normally
+  # consider to be C instead.
+  ament_cppcheck(LANGUAGE "c++")
+  ament_cpplint()
+  ament_lint_cmake()
+  ament_uncrustify()
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+
+ament_package()
diff --git a/src/libs/nav2_robot/README.md b/src/libs/nav2_robot/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/libs/nav2_robot/doc/.gitignore b/src/libs/nav2_robot/doc/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/libs/nav2_robot/include/nav2_robot/robot.hpp b/src/libs/nav2_robot/include/nav2_robot/robot.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..d647ba5e7389694af89d1e2a0580df67dd74c320
--- /dev/null
+++ b/src/libs/nav2_robot/include/nav2_robot/robot.hpp
@@ -0,0 +1,32 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_ROBOT__ROBOT_HPP_
+#define NAV2_ROBOT__ROBOT_HPP_
+
+namespace nav2_robot
+{
+
+class Robot
+{
+public:
+  virtual ~Robot() {}
+
+  // Commands to the robot
+  virtual void enterSafeState() = 0;
+};
+
+}  // namespace nav2_robot
+
+#endif  // NAV2_ROBOT__ROBOT_HPP_
diff --git a/src/libs/nav2_robot/include/nav2_robot/ros_robot.hpp b/src/libs/nav2_robot/include/nav2_robot/ros_robot.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..38709ab6ee424377642f3feb23948f5926c981a4
--- /dev/null
+++ b/src/libs/nav2_robot/include/nav2_robot/ros_robot.hpp
@@ -0,0 +1,41 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_ROBOT__ROS_ROBOT_HPP_
+#define NAV2_ROBOT__ROS_ROBOT_HPP_
+
+#include <string>
+#include "nav2_robot/robot.hpp"
+
+namespace nav2_robot
+{
+
+class RosRobot : public Robot
+{
+public:
+  /**
+   * Construct a RosRobot with a provided URDF file.
+   *
+   * @param[in] filename The filename of the URDF file describing this robot.
+   */
+  explicit RosRobot(const std::string & urdf_filename);
+  RosRobot() = delete;
+  ~RosRobot();
+
+  void enterSafeState() override;
+};
+
+}  // namespace nav2_robot
+
+#endif  // NAV2_ROBOT__ROS_ROBOT_HPP_
diff --git a/src/libs/nav2_robot/package.xml b/src/libs/nav2_robot/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..ef317f328cc29115486be1b0f73c607b8f0241b5
--- /dev/null
+++ b/src/libs/nav2_robot/package.xml
@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_robot</name>
+  <version>0.1.0</version>
+  <description>TODO</description>
+  <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/libs/nav2_robot/src/ros_robot.cpp b/src/libs/nav2_robot/src/ros_robot.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e77fe570a69357d568ab76bb16550e1487f9a45b
--- /dev/null
+++ b/src/libs/nav2_robot/src/ros_robot.cpp
@@ -0,0 +1,35 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+#include "nav2_robot/ros_robot.hpp"
+
+namespace nav2_robot
+{
+
+RosRobot::RosRobot(const std::string & /*urdf_filename*/)
+{
+  // Open and parser the URDF file
+}
+
+RosRobot::~RosRobot()
+{
+}
+
+void
+RosRobot::enterSafeState()
+{
+}
+
+}  // namespace nav2_robot
diff --git a/src/libs/nav2_robot/test/.gitignore b/src/libs/nav2_robot/test/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/libs/nav2_tasks/CHANGELOG.rst b/src/libs/nav2_tasks/CHANGELOG.rst
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/libs/nav2_tasks/CMakeLists.txt b/src/libs/nav2_tasks/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..0231535702e3291aa18a48a875659980a05670fd
--- /dev/null
+++ b/src/libs/nav2_tasks/CMakeLists.txt
@@ -0,0 +1,50 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_tasks)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+include_directories(
+  include
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+rosidl_generate_interfaces(nav2_tasks
+  "msg/TaskStatus.msg"
+  DEPENDENCIES builtin_interfaces geometry_msgs std_msgs
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_cppcheck REQUIRED)
+  find_package(ament_cmake_cpplint REQUIRED)
+  find_package(ament_cmake_lint_cmake REQUIRED)
+  find_package(ament_cmake_uncrustify REQUIRED)
+  # This forces cppcheck to consider all files in this project to be C++,
+  # including the headers which end with .h, which cppcheck would normally
+  # consider to be C instead.
+  ament_cppcheck(LANGUAGE "c++")
+  ament_cpplint()
+  ament_lint_cmake()
+  ament_uncrustify()
+endif()
+
+ament_export_include_directories(include)
+ament_export_dependencies(rosidl_default_runtime)
+
+ament_package()
diff --git a/src/libs/nav2_tasks/README.md b/src/libs/nav2_tasks/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/libs/nav2_tasks/doc/.gitignore b/src/libs/nav2_tasks/doc/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/libs/nav2_tasks/include/nav2_tasks/compute_path_to_pose_task.hpp b/src/libs/nav2_tasks/include/nav2_tasks/compute_path_to_pose_task.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..391e39725a7851d841c63146d9c7449e8e672446
--- /dev/null
+++ b/src/libs/nav2_tasks/include/nav2_tasks/compute_path_to_pose_task.hpp
@@ -0,0 +1,43 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_TASKS__COMPUTE_PATH_TO_POSE_TASK_HPP_
+#define NAV2_TASKS__COMPUTE_PATH_TO_POSE_TASK_HPP_
+
+#include "nav2_tasks/task_client.hpp"
+#include "nav2_tasks/task_server.hpp"
+#include "nav2_planning_msgs/msg/path_end_points.hpp"
+#include "nav2_planning_msgs/msg/path.hpp"
+
+namespace nav2_tasks
+{
+
+using ComputePathToPoseCommand = nav2_planning_msgs::msg::PathEndPoints;
+using ComputePathToPoseResult = nav2_planning_msgs::msg::Path;
+
+using ComputePathToPoseTaskClient =
+  TaskClient<ComputePathToPoseCommand, ComputePathToPoseResult>;
+
+using ComputePathToPoseTaskServer =
+  TaskServer<ComputePathToPoseCommand, ComputePathToPoseResult>;
+
+template<>
+inline const char * getTaskName<ComputePathToPoseCommand, ComputePathToPoseResult>()
+{
+  return "ComputePathToPoseTask";
+}
+
+}  // namespace nav2_tasks
+
+#endif  // NAV2_TASKS__COMPUTE_PATH_TO_POSE_TASK_HPP_
diff --git a/src/libs/nav2_tasks/include/nav2_tasks/execute_mission_task.hpp b/src/libs/nav2_tasks/include/nav2_tasks/execute_mission_task.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..74e528be82b3cd3f712675a5ea8f78ce098e7954
--- /dev/null
+++ b/src/libs/nav2_tasks/include/nav2_tasks/execute_mission_task.hpp
@@ -0,0 +1,40 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_TASKS__EXECUTE_MISSION_TASK_HPP_
+#define NAV2_TASKS__EXECUTE_MISSION_TASK_HPP_
+
+#include "nav2_tasks/task_client.hpp"
+#include "nav2_tasks/task_server.hpp"
+#include "nav2_mission_execution_msgs/msg/mission_plan.hpp"
+#include "std_msgs/msg/empty.hpp"
+
+namespace nav2_tasks
+{
+
+using ExecuteMissionCommand = nav2_mission_execution_msgs::msg::MissionPlan;
+using ExecuteMissionResult = std_msgs::msg::Empty;
+
+using ExecuteMissionTaskClient = TaskClient<ExecuteMissionCommand, ExecuteMissionResult>;
+using ExecuteMissionTaskServer = TaskServer<ExecuteMissionCommand, ExecuteMissionResult>;
+
+template<>
+inline const char * getTaskName<ExecuteMissionCommand, ExecuteMissionResult>()
+{
+  return "ExecuteMissionTask";
+}
+
+}  // namespace nav2_tasks
+
+#endif  // NAV2_TASKS__EXECUTE_MISSION_TASK_HPP_
diff --git a/src/libs/nav2_tasks/include/nav2_tasks/follow_path_task.hpp b/src/libs/nav2_tasks/include/nav2_tasks/follow_path_task.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..7ea2b2cc04a7126e0f82e64cddb4241a09041c6c
--- /dev/null
+++ b/src/libs/nav2_tasks/include/nav2_tasks/follow_path_task.hpp
@@ -0,0 +1,40 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_TASKS__FOLLOW_PATH_TASK_HPP_
+#define NAV2_TASKS__FOLLOW_PATH_TASK_HPP_
+
+#include "nav2_tasks/task_client.hpp"
+#include "nav2_tasks/task_server.hpp"
+#include "nav2_planning_msgs/msg/path.hpp"
+#include "std_msgs/msg/empty.hpp"
+
+namespace nav2_tasks
+{
+
+using FollowPathCommand = nav2_planning_msgs::msg::Path;
+using FollowPathResult = std_msgs::msg::Empty;
+
+using FollowPathTaskClient = TaskClient<FollowPathCommand, FollowPathResult>;
+using FollowPathTaskServer = TaskServer<FollowPathCommand, FollowPathResult>;
+
+template<>
+inline const char * getTaskName<FollowPathCommand, FollowPathResult>()
+{
+  return "FollowPathTask";
+}
+
+}  // namespace nav2_tasks
+
+#endif  // NAV2_TASKS__FOLLOW_PATH_TASK_HPP_
diff --git a/src/libs/nav2_tasks/include/nav2_tasks/navigate_to_pose_task.hpp b/src/libs/nav2_tasks/include/nav2_tasks/navigate_to_pose_task.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..3a29ecfef767e89349b8457b19694443da39a82a
--- /dev/null
+++ b/src/libs/nav2_tasks/include/nav2_tasks/navigate_to_pose_task.hpp
@@ -0,0 +1,40 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_TASKS__NAVIGATE_TO_POSE_TASK_HPP_
+#define NAV2_TASKS__NAVIGATE_TO_POSE_TASK_HPP_
+
+#include "nav2_tasks/task_client.hpp"
+#include "nav2_tasks/task_server.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "std_msgs/msg/empty.hpp"
+
+namespace nav2_tasks
+{
+
+using NavigateToPoseCommand = geometry_msgs::msg::PoseStamped;
+using NavigateToPoseResult = std_msgs::msg::Empty;
+
+using NavigateToPoseTaskClient = TaskClient<NavigateToPoseCommand, NavigateToPoseResult>;
+using NavigateToPoseTaskServer = TaskServer<NavigateToPoseCommand, NavigateToPoseResult>;
+
+template<>
+inline const char * getTaskName<NavigateToPoseCommand, NavigateToPoseResult>()
+{
+  return "NavigateToPoseTask";
+}
+
+}  // namespace nav2_tasks
+
+#endif  // NAV2_TASKS__NAVIGATE_TO_POSE_TASK_HPP_
diff --git a/src/libs/nav2_tasks/include/nav2_tasks/task_client.hpp b/src/libs/nav2_tasks/include/nav2_tasks/task_client.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..3819a1a97790dc6065e93eccc1a17bbec957d404
--- /dev/null
+++ b/src/libs/nav2_tasks/include/nav2_tasks/task_client.hpp
@@ -0,0 +1,204 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_TASKS__TASK_CLIENT_HPP_
+#define NAV2_TASKS__TASK_CLIENT_HPP_
+
+#include <atomic>
+#include <condition_variable>
+#include <string>
+#include <thread>
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/empty.hpp"
+#include "nav2_tasks/task_status.hpp"
+
+namespace nav2_tasks
+{
+
+constexpr std::chrono::milliseconds defaultServerTimeout = std::chrono::milliseconds(5000);
+
+template<class CommandMsg, class ResultMsg>
+const char * getTaskName();
+
+template<class CommandMsg, class ResultMsg>
+class TaskClient
+{
+public:
+  explicit TaskClient(rclcpp::Node * node)
+  : node_(node)
+  {
+    std::string taskName = getTaskName<CommandMsg, ResultMsg>();
+
+    // Create the publishers
+    commandPub_ = node_->create_publisher<CommandMsg>(taskName + "_command");
+    cancelPub_ = node_->create_publisher<CancelMsg>(taskName + "_cancel");
+
+    // Create the subscribers
+    resultSub_ = node_->create_subscription<ResultMsg>(taskName + "_result",
+        std::bind(&TaskClient::onResultReceived, this, std::placeholders::_1));
+    statusSub_ = node_->create_subscription<StatusMsg>(taskName + "_status",
+        std::bind(&TaskClient::onStatusReceived, this, std::placeholders::_1));
+  }
+
+  TaskClient() = delete;
+
+  ~TaskClient()
+  {
+  }
+
+  // The client can tell the TaskServer to execute its operation
+  void sendCommand(const typename CommandMsg::SharedPtr msg)
+  {
+    resultReceived_ = false;
+    statusReceived_ = false;
+    commandPub_->publish(msg);
+  }
+
+  // An in-flight operation can be canceled
+  void cancel()
+  {
+    CancelMsg msg;
+    cancelPub_->publish(msg);
+  }
+
+  bool waitForServer(std::chrono::milliseconds timeout = std::chrono::milliseconds::max())
+  {
+    std::string taskName = getTaskName<CommandMsg, ResultMsg>();
+    taskName += "_command";
+
+    auto t0 = std::chrono::high_resolution_clock::now();
+
+    // TODO(mjeronimo): Replace this with a legit way to wait for the server
+    while (node_->count_subscribers(taskName) < 1) {
+      rclcpp::spin_some(node_->get_node_base_interface());
+
+      auto t1 = std::chrono::high_resolution_clock::now();
+      auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(t1 - t0);
+
+      if (elapsedTime > timeout) {
+        return false;
+      }
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+
+    return true;
+  }
+
+  // The client can wait for a result with a timeout
+  TaskStatus waitForResult(
+    typename ResultMsg::SharedPtr & result,
+    std::chrono::milliseconds duration)
+  {
+    // Wait for a status message to come in
+    std::unique_lock<std::mutex> lock(statusMutex_);
+    if (!cvStatus_.wait_for(lock, std::chrono::milliseconds(duration),
+      [&] {return statusReceived_ == true;}))
+    {
+      return RUNNING;
+    }
+
+    // We've got a status message, indicating that the server task has finished (succeeded,
+    // failed, or canceled)
+    switch (statusMsg_->result) {
+      // If the task has failed or has been canceled, no result message is forthcoming and we
+      // can propagate the status code, using the TaskStatus type rather than the message-level
+      // implementation type
+      case nav2_tasks::msg::TaskStatus::FAILED:
+      case nav2_tasks::msg::TaskStatus::CANCELED:
+        return static_cast<TaskStatus>(statusMsg_->result);
+
+      case nav2_tasks::msg::TaskStatus::SUCCEEDED:
+        {
+          // The result message may be here already or it may come *after* the status
+          // message. If it's here, the wait will be satisfied immediately. Otherwise
+          // we'll wait for a bit longer to see if it comes in.
+          std::unique_lock<std::mutex> lock(resultMutex_);
+          if (cvResult_.wait_for(lock, std::chrono::milliseconds(100),
+            [&] {return resultReceived_ == true;}))
+          {
+            result = resultMsg_;
+            resultReceived_ = false;
+            return SUCCEEDED;
+          }
+
+          // Give up since we never received the result message
+          return FAILED;
+        }
+
+      default:
+        throw std::logic_error("Invalid status value from TaskServer");
+    }
+
+    // Not reachable; added to avoid a warning
+    return FAILED;
+  }
+
+protected:
+  // The result of this task
+  typename ResultMsg::SharedPtr resultMsg_;
+
+  // These messages are internal to the TaskClient implementation
+  typedef std_msgs::msg::Empty CancelMsg;
+  typedef nav2_tasks::msg::TaskStatus StatusMsg;
+  StatusMsg::SharedPtr statusMsg_;
+
+  // Variables to handle the communication of the status message to the waitForResult thread
+  std::mutex statusMutex_;
+  std::atomic<bool> statusReceived_;
+  std::condition_variable cvStatus_;
+
+  // Variables to handle the communication of the result message to the waitForResult thread
+  std::mutex resultMutex_;
+  std::atomic<bool> resultReceived_;
+  std::condition_variable cvResult_;
+
+  // Called when the TaskServer has sent its result
+  void onResultReceived(const typename ResultMsg::SharedPtr resultMsg)
+  {
+    {
+      std::lock_guard<std::mutex> lock(resultMutex_);
+      resultMsg_ = resultMsg;
+      resultReceived_ = true;
+    }
+
+    cvResult_.notify_one();
+  }
+
+  // Called when the TaskServer sends it status code (success or failure)
+  void onStatusReceived(const StatusMsg::SharedPtr statusMsg)
+  {
+    {
+      std::lock_guard<std::mutex> lock(statusMutex_);
+      statusMsg_ = statusMsg;
+      statusReceived_ = true;
+    }
+
+    cvStatus_.notify_one();
+  }
+
+  // The TaskClient isn't itself a node, so needs to know which one to use
+  rclcpp::Node * node_;
+
+  // The client's publishers: the command and cancel messages
+  typename rclcpp::Publisher<CommandMsg>::SharedPtr commandPub_;
+  rclcpp::Publisher<CancelMsg>::SharedPtr cancelPub_;
+
+  // The client's subscriptions: result, feedback, and status
+  typename rclcpp::Subscription<ResultMsg>::SharedPtr resultSub_;
+  rclcpp::Subscription<StatusMsg>::SharedPtr statusSub_;
+};
+
+}  // namespace nav2_tasks
+
+#endif  // NAV2_TASKS__TASK_CLIENT_HPP_
diff --git a/src/libs/nav2_tasks/include/nav2_tasks/task_server.hpp b/src/libs/nav2_tasks/include/nav2_tasks/task_server.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..16d2d482d0a66c2a148a801ba13f86457e941acc
--- /dev/null
+++ b/src/libs/nav2_tasks/include/nav2_tasks/task_server.hpp
@@ -0,0 +1,179 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_TASKS__TASK_SERVER_HPP_
+#define NAV2_TASKS__TASK_SERVER_HPP_
+
+#include <atomic>
+#include <condition_variable>
+#include <thread>
+#include <string>
+#include <chrono>
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/empty.hpp"
+#include "nav2_tasks/task_status.hpp"
+
+namespace nav2_tasks
+{
+
+template<class CommandMsg, class ResultMsg>
+const char * getTaskName();
+
+template<class CommandMsg, class ResultMsg>
+class TaskServer : public rclcpp::Node
+{
+public:
+  explicit TaskServer(const std::string & name)
+  : Node(name), workerThread_(nullptr)
+  {
+    std::string taskName = getTaskName<CommandMsg, ResultMsg>();
+    commandSub_ = create_subscription<CommandMsg>(taskName + "_command",
+        std::bind(&TaskServer::onCommandReceived, this, std::placeholders::_1));
+
+    cancelSub_ = create_subscription<std_msgs::msg::Empty>(taskName + "_cancel",
+        std::bind(&TaskServer::onCancelReceived, this, std::placeholders::_1));
+
+    resultPub_ = this->create_publisher<ResultMsg>(taskName + "_result");
+    statusPub_ = this->create_publisher<StatusMsg>(taskName + "_status");
+
+    startWorkerThread();
+  }
+
+  virtual ~TaskServer()
+  {
+    stopWorkerThread();
+  }
+
+  virtual TaskStatus execute(const typename CommandMsg::SharedPtr command) = 0;
+
+  // The user's execute method can check if the client is requesting a cancel
+  bool cancelRequested()
+  {
+    return cancelReceived_;
+  }
+
+  void setCanceled()
+  {
+    cancelReceived_ = false;
+  }
+
+  void setResult(const ResultMsg & result)
+  {
+    resultMsg_ = result;
+  }
+
+protected:
+  typename CommandMsg::SharedPtr commandMsg_;
+  ResultMsg resultMsg_;
+
+  // These messages are internal to the TaskClient implementation
+  typedef std_msgs::msg::Empty CancelMsg;
+  typedef nav2_tasks::msg::TaskStatus StatusMsg;
+
+  // The pointer to our private worker thread
+  std::thread * workerThread_;
+
+  // This class has the worker thread body which calls the user's execute() callback
+  void workerThread()
+  {
+    do {
+      std::unique_lock<std::mutex> lock(commandMutex_);
+      if (cvCommand_.wait_for(lock, std::chrono::milliseconds(10),
+        [&] {return commandReceived_ == true;}))
+      {
+        // Call the user's overridden method
+        TaskStatus status = execute(commandMsg_);
+
+        // Reset the execution flag now that we've executed the task
+        commandReceived_ = false;
+
+        nav2_tasks::msg::TaskStatus statusMsg;
+
+        // Check the result of the user's function and send the
+        // appropriate message
+
+        if (status == TaskStatus::SUCCEEDED) {
+          // If the task succeeded, first publish the result message
+          resultPub_->publish(resultMsg_);
+
+          // Then send the success code
+          statusMsg.result = nav2_tasks::msg::TaskStatus::SUCCEEDED;
+          statusPub_->publish(statusMsg);
+        } else if (status == TaskStatus::FAILED) {
+          // Otherwise, send the failure code
+          statusMsg.result = nav2_tasks::msg::TaskStatus::FAILED;
+          statusPub_->publish(statusMsg);
+        } else if (status == TaskStatus::CANCELED) {
+          // Or the canceled code
+          statusMsg.result = nav2_tasks::msg::TaskStatus::CANCELED;
+          statusPub_->publish(statusMsg);
+        } else {
+          throw std::logic_error("Unexpected status return from task");
+        }
+      }
+    } while (rclcpp::ok());
+  }
+
+  // Convenience routes for starting and stopping the worker thread (used from the ctor and dtor)
+  void startWorkerThread()
+  {
+    workerThread_ = new std::thread(&TaskServer::workerThread, this);
+  }
+
+  void stopWorkerThread()
+  {
+    workerThread_->join();
+    delete workerThread_;
+    workerThread_ = nullptr;
+  }
+
+  // Variables to handle the communication of the command to the execute thread
+  std::mutex commandMutex_;
+  bool commandReceived_;
+  std::condition_variable cvCommand_;
+
+  // Variables to handle the communication of the cancel request to the execute thread
+  std::atomic<bool> cancelReceived_;
+  std::condition_variable cvCancel_;
+
+  // The callbacks for our subscribers
+  void onCommandReceived(const typename CommandMsg::SharedPtr msg)
+  {
+    {
+      std::lock_guard<std::mutex> lock(commandMutex_);
+      commandMsg_ = msg;
+      commandReceived_ = true;
+    }
+
+    cvCommand_.notify_one();
+  }
+
+  void onCancelReceived(const CancelMsg::SharedPtr /*msg*/)
+  {
+    cancelReceived_ = true;
+    cvCancel_.notify_one();
+  }
+
+  // The subscribers: command and cancel
+  typename rclcpp::Subscription<CommandMsg>::SharedPtr commandSub_;
+  rclcpp::Subscription<CancelMsg>::SharedPtr cancelSub_;
+
+  // The publishers for the result from this task
+  typename rclcpp::Publisher<ResultMsg>::SharedPtr resultPub_;
+  typename rclcpp::Publisher<StatusMsg>::SharedPtr statusPub_;
+};
+
+}  // namespace nav2_tasks
+
+#endif  // NAV2_TASKS__TASK_SERVER_HPP_
diff --git a/src/libs/nav2_tasks/include/nav2_tasks/task_status.hpp b/src/libs/nav2_tasks/include/nav2_tasks/task_status.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..bf637c67543f7f771f12f5cc2f782b3dae65e426
--- /dev/null
+++ b/src/libs/nav2_tasks/include/nav2_tasks/task_status.hpp
@@ -0,0 +1,33 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_TASKS__TASK_STATUS_HPP_
+#define NAV2_TASKS__TASK_STATUS_HPP_
+
+#include "nav2_tasks/msg/task_status.hpp"
+
+namespace nav2_tasks
+{
+
+typedef enum
+{
+  SUCCEEDED = msg::TaskStatus::SUCCEEDED,
+  FAILED = msg::TaskStatus::FAILED,
+  RUNNING = msg::TaskStatus::RUNNING,
+  CANCELED = msg::TaskStatus::CANCELED
+} TaskStatus;
+
+}  // namespace nav2_tasks
+
+#endif  // NAV2_TASKS__TASK_STATUS_HPP_
diff --git a/src/libs/nav2_tasks/msg/TaskStatus.msg b/src/libs/nav2_tasks/msg/TaskStatus.msg
new file mode 100644
index 0000000000000000000000000000000000000000..92c689c9db19a29875d8b14c38dcbb7183d0a174
--- /dev/null
+++ b/src/libs/nav2_tasks/msg/TaskStatus.msg
@@ -0,0 +1,5 @@
+uint8 SUCCEEDED=0
+uint8 FAILED=1
+uint8 RUNNING=2
+uint8 CANCELED=3
+uint8 result
diff --git a/src/libs/nav2_tasks/package.xml b/src/libs/nav2_tasks/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..24806993835f194deb5306f8e5dd45b975c47c33
--- /dev/null
+++ b/src/libs/nav2_tasks/package.xml
@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_tasks</name>
+  <version>0.1.0</version>
+  <description>TODO</description>
+  <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rclcpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>builtin_interfaces</build_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>builtin_interfaces</exec_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/libs/nav2_tasks/test/.gitignore b/src/libs/nav2_tasks/test/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/libs/nav2_util/CHANGELOG.rst b/src/libs/nav2_util/CHANGELOG.rst
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/libs/nav2_util/CMakeLists.txt b/src/libs/nav2_util/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..4d3bd767b4922edb24bfaa99599a032c39a8fbe1
--- /dev/null
+++ b/src/libs/nav2_util/CMakeLists.txt
@@ -0,0 +1,56 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_util)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(nav2_libs_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2 REQUIRED)
+
+include_directories(include)
+
+add_library(nav2_util STATIC
+  src/costmap.cpp
+)
+
+ament_target_dependencies(nav2_util
+  nav2_libs_msgs
+  tf2_ros
+  tf2
+)
+
+install(TARGETS
+  nav2_util
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib)
+
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_cppcheck REQUIRED)
+  find_package(ament_cmake_cpplint REQUIRED)
+  find_package(ament_cmake_lint_cmake REQUIRED)
+  find_package(ament_cmake_uncrustify REQUIRED)
+  # This forces cppcheck to consider all files in this project to be C++,
+  # including the headers which end with .h, which cppcheck would normally
+  # consider to be C instead.
+  ament_cppcheck(LANGUAGE "c++")
+  ament_cpplint()
+  ament_lint_cmake()
+  ament_uncrustify()
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(nav2_util)
+
+ament_package()
diff --git a/src/libs/nav2_util/README.md b/src/libs/nav2_util/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/libs/nav2_util/doc/.gitignore b/src/libs/nav2_util/doc/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/libs/nav2_util/include/nav2_util/costmap.hpp b/src/libs/nav2_util/include/nav2_util/costmap.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..6d32f94310ead29335f39adaf4f8635e99460b0b
--- /dev/null
+++ b/src/libs/nav2_util/include/nav2_util/costmap.hpp
@@ -0,0 +1,53 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_UTIL__COSTMAP_HPP_
+#define NAV2_UTIL__COSTMAP_HPP_
+
+#include <vector>
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_libs_msgs/msg/costmap.hpp"
+#include "nav2_libs_msgs/msg/costmap_meta_data.hpp"
+
+namespace nav2_util
+{
+
+class Costmap
+{
+public:
+  explicit Costmap(rclcpp::Node * node);
+  Costmap() = delete;
+
+  nav2_libs_msgs::msg::Costmap getCostmap(
+    const nav2_libs_msgs::msg::CostmapMetaData & specifications);
+
+  typedef uint8_t CostValue;
+
+  // Mapping for often used cost values
+  static const CostValue no_information;
+  static const CostValue lethal_obstacle;
+  static const CostValue inscribed_inflated_obstacle;
+  static const CostValue medium_cost;
+  static const CostValue free_space;
+
+private:
+  // TODO(orduno): For now, the Costmap isn't itself a node
+  rclcpp::Node * node_;
+
+  std::vector<uint8_t> getTestData(const int size_x, const int size_y);
+};
+
+}  // namespace nav2_util
+
+#endif  // NAV2_UTIL__COSTMAP_HPP_
diff --git a/src/libs/nav2_util/msg/Costmap.msg b/src/libs/nav2_util/msg/Costmap.msg
new file mode 100644
index 0000000000000000000000000000000000000000..4c3e90ca70cd59d7afe0ff33b574addf2893a488
--- /dev/null
+++ b/src/libs/nav2_util/msg/Costmap.msg
@@ -0,0 +1,9 @@
+# This represents a 2-D grid map, in which each cell has an associated cost
+
+std_msgs/Header header
+
+# MetaData for the map
+CostmapMetaData metadata
+
+# The cost data, in row-major order, starting with (0,0).
+uint8[] data
diff --git a/src/libs/nav2_util/msg/CostmapMetaData.msg b/src/libs/nav2_util/msg/CostmapMetaData.msg
new file mode 100644
index 0000000000000000000000000000000000000000..6338795dad803474a7a48248f85fe515ff17f07d
--- /dev/null
+++ b/src/libs/nav2_util/msg/CostmapMetaData.msg
@@ -0,0 +1,23 @@
+# This hold basic information about the characterists of the Costmap
+
+# The time at which the static map was loaded
+builtin_interfaces/Time map_load_time
+
+# The time of the last update to costmap
+builtin_interfaces/Time update_time
+
+# The corresponding layer name
+string layer
+
+# The map resolution [m/cell]
+float32 resolution
+
+# Number of cells in the horizontal direction
+uint32 size_x
+
+# Number of cells in the vertical direction
+uint32 size_y
+
+# The origin of the costmap [m, m, rad].
+# This is the real-world pose of the cell (0,0) in the map.
+geometry_msgs/Pose origin
diff --git a/src/libs/nav2_util/package.xml b/src/libs/nav2_util/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..d06fd7e987b6cf2dcf436e2166b1ee50f4014d45
--- /dev/null
+++ b/src/libs/nav2_util/package.xml
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_util</name>
+  <version>0.1.0</version>
+  <description>TODO</description>
+  <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rclcpp</build_depend>
+  <build_depend>nav2_libs_msgs</build_depend>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>nav2_libs_msgs</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/libs/nav2_util/src/costmap.cpp b/src/libs/nav2_util/src/costmap.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..86a0011a8335254c85294698e714fe4bab123c99
--- /dev/null
+++ b/src/libs/nav2_util/src/costmap.cpp
@@ -0,0 +1,179 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <vector>
+#include "nav2_util/costmap.hpp"
+#include "tf2/LinearMath/Quaternion.h"
+
+using std::vector;
+
+namespace nav2_util
+{
+
+const Costmap::CostValue Costmap::no_information = 255;
+const Costmap::CostValue Costmap::lethal_obstacle = 254;
+const Costmap::CostValue Costmap::inscribed_inflated_obstacle = 253;
+const Costmap::CostValue Costmap::medium_cost = 128;
+const Costmap::CostValue Costmap::free_space = 0;
+
+// TODO(orduno): Port ROS1 Costmap package
+Costmap::Costmap(rclcpp::Node * node)
+: node_(node)
+{
+  RCLCPP_INFO(node_->get_logger(), "Costmap::Costmap");
+}
+
+nav2_libs_msgs::msg::Costmap
+Costmap::getCostmap(const nav2_libs_msgs::msg::CostmapMetaData & /*specifications*/)
+{
+  // TODO(orduno): build a costmap given the specifications
+
+  RCLCPP_INFO(node_->get_logger(), "Costmap::getCostmap");
+
+  // TODO(orduno): faking out a costmap for now
+
+  nav2_libs_msgs::msg::Costmap costmap;
+  costmap.header.stamp = node_->now();
+  costmap.header.frame_id = "/map";
+
+  // Fill map metadata
+  costmap.metadata.map_load_time = node_->now();
+
+  // Some arbitrary numbers
+  costmap.metadata.resolution = 1;  // m/cell
+  costmap.metadata.size_x = 10;  // cells
+  costmap.metadata.size_y = 10;  // cells
+
+  // The origin of the map [m, m, rad]. This is the real-world pose of the cell (0,0) in the map.
+  // Origin is lower-left pixel?
+  costmap.metadata.origin.position.x = 0.0;  // 2D pose of the lower-left pixel in the map
+  costmap.metadata.origin.position.y = 0.0;
+  costmap.metadata.origin.position.z = 0.0;  // ignored
+
+  // Define map rotation
+  // Provided as yaw with counterclockwise rotation, with yaw = 0 meaning no rotation
+
+  tf2::Quaternion quaternion;
+  quaternion.setRPY(0.0, 0.0, 0.0);  // set roll, pitch, yaw
+  costmap.metadata.origin.orientation.x = quaternion.x();
+  costmap.metadata.origin.orientation.y = quaternion.y();
+  costmap.metadata.origin.orientation.z = quaternion.z();
+  costmap.metadata.origin.orientation.w = quaternion.w();
+
+  costmap.data.resize(costmap.metadata.size_x, costmap.metadata.size_y);
+
+  // Fill with some fake data for testing
+  costmap.data = getTestData(costmap.metadata.size_x, costmap.metadata.size_y);
+
+  return costmap;
+}
+
+vector<uint8_t>
+Costmap::getTestData(const int /*size_x*/, const int /*size_y*/)
+{
+  // TODO(orduno): fixed size for now
+
+  // TODO(orduno): besides hardcoded costmaps, use a mathematical function
+
+  const uint8_t n = no_information;
+  const uint8_t x = lethal_obstacle;
+  const uint8_t i = inscribed_inflated_obstacle;
+  const uint8_t u = medium_cost;
+  const uint8_t o = free_space;
+
+  // TODO(orduno): make these vector of vectors, select maze by index
+  //               within the test program
+
+  vector<uint8_t> costmapFree =
+    // 0 1 2 3 4 5 6 7 8 9
+  {o, o, o, o, o, o, o, o, o, o,    // 0
+    o, o, o, o, o, o, o, o, o, o,   // 1
+    o, o, o, o, o, o, o, o, o, o,   // 2
+    o, o, o, o, o, o, o, o, o, o,   // 3
+    o, o, o, o, o, o, o, o, o, o,   // 4
+    o, o, o, o, o, o, o, o, o, o,   // 5
+    o, o, o, o, o, o, o, o, o, o,   // 6
+    o, o, o, o, o, o, o, o, o, o,   // 7
+    o, o, o, o, o, o, o, o, o, o,   // 8
+    o, o, o, o, o, o, o, o, o, o};  // 9
+
+  vector<uint8_t> costmapBounded =
+    // 0 1 2 3 4 5 6 7 8 9
+  {n, n, n, n, n, n, n, n, n, n,    // 0
+    n, o, o, o, o, o, o, o, o, n,   // 1
+    n, o, o, o, o, o, o, o, o, n,   // 2
+    n, o, o, o, o, o, o, o, o, n,   // 3
+    n, o, o, o, o, o, o, o, o, n,   // 4
+    n, o, o, o, o, o, o, o, o, n,   // 5
+    n, o, o, o, o, o, o, o, o, n,   // 6
+    n, o, o, o, o, o, o, o, o, n,   // 7
+    n, o, o, o, o, o, o, o, o, n,   // 8
+    n, n, n, n, n, n, n, n, n, n};  // 9
+
+  vector<uint8_t> costmapObstacleBL =
+    // 0 1 2 3 4 5 6 7 8 9
+  {n, n, n, n, n, n, n, n, n, n,    // 0
+    n, o, o, o, o, o, o, o, o, n,   // 1
+    n, o, o, o, o, o, o, o, o, n,   // 2
+    n, o, o, o, o, o, o, o, o, n,   // 3
+    n, o, o, o, o, o, o, o, o, n,   // 4
+    n, o, x, x, x, o, o, o, o, n,   // 5
+    n, o, x, x, x, o, o, o, o, n,   // 6
+    n, o, x, x, x, o, o, o, o, n,   // 7
+    n, o, o, o, o, o, o, o, o, n,   // 8
+    n, n, n, n, n, n, n, n, n, n};  // 9
+
+  vector<uint8_t> costmapObstacleTL =
+    // 0 1 2 3 4 5 6 7 8 9
+  {n, n, n, n, n, n, n, n, n, n,    // 0
+    n, o, o, o, o, o, o, o, o, n,   // 1
+    n, o, x, x, x, o, o, o, o, n,   // 2
+    n, o, x, x, x, o, o, o, o, n,   // 3
+    n, o, x, x, x, o, o, o, o, n,   // 4
+    n, o, o, o, o, o, o, o, o, n,   // 5
+    n, o, o, o, o, o, o, o, o, n,   // 6
+    n, o, o, o, o, o, o, o, o, n,   // 7
+    n, o, o, o, o, o, o, o, o, n,   // 8
+    n, n, n, n, n, n, n, n, n, n};  // 9
+
+  vector<uint8_t> costmapMaze =
+    // 0 1 2 3 4 5 6 7 8 9
+  {n, n, n, n, n, n, n, n, n, n,    // 0
+    n, o, o, o, o, o, o, o, o, n,   // 1
+    n, x, x, o, x, x, x, o, x, n,   // 2
+    n, o, o, o, o, x, o, o, o, n,   // 3
+    n, o, x, x, o, x, o, x, o, n,   // 4
+    n, o, x, x, o, x, o, x, o, n,   // 5
+    n, o, o, x, o, x, o, x, o, n,   // 6
+    n, x, o, x, o, x, o, x, o, n,   // 7
+    n, o, o, o, o, o, o, x, o, n,   // 8
+    n, n, n, n, n, n, n, n, n, n};  // 9
+
+  vector<uint8_t> costmapMaze2 =
+    // 0 1 2 3 4 5 6 7 8 9
+  {n, n, n, n, n, n, n, n, n, n,    // 0
+    n, o, o, o, o, o, o, o, o, n,   // 1
+    n, x, x, u, x, x, x, o, x, n,   // 2
+    n, o, o, o, o, o, o, o, u, n,   // 3
+    n, o, x, x, o, x, x, x, u, n,   // 4
+    n, o, x, x, o, o, o, x, u, n,   // 5
+    n, o, o, x, u, x, o, x, u, n,   // 6
+    n, x, o, x, u, x, i, x, u, n,   // 7
+    n, o, o, o, o, o, o, o, o, n,   // 8
+    n, n, n, n, n, n, n, n, n, n};  // 9
+
+  return costmapMaze2;
+}
+
+}  // namespace nav2_util
diff --git a/src/libs/nav2_util/test/.gitignore b/src/libs/nav2_util/test/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/localization/nav2_amcl/.gitignore b/src/localization/nav2_amcl/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/localization/nav2_gazebo_localizer/.gitignore b/src/localization/nav2_gazebo_localizer/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/localization/nav2_localization_msgs/.gitignore b/src/localization/nav2_localization_msgs/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/mapping/nav2_map_server/.gitignore b/src/mapping/nav2_map_server/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/mapping/nav2_mapping_msgs/.gitignore b/src/mapping/nav2_mapping_msgs/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/mission_execution/nav2_mission_execution_msgs/CHANGELOG.rst b/src/mission_execution/nav2_mission_execution_msgs/CHANGELOG.rst
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/mission_execution/nav2_mission_execution_msgs/CMakeLists.txt b/src/mission_execution/nav2_mission_execution_msgs/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d054aa34a6478b9a6ed85aa919a5ddf61cb3e22f
--- /dev/null
+++ b/src/mission_execution/nav2_mission_execution_msgs/CMakeLists.txt
@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_mission_execution_msgs)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/MissionPlan.msg"
+  DEPENDENCIES builtin_interfaces geometry_msgs std_msgs
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_cppcheck REQUIRED)
+  find_package(ament_cmake_cpplint REQUIRED)
+  find_package(ament_cmake_lint_cmake REQUIRED)
+  find_package(ament_cmake_uncrustify REQUIRED)
+  # This forces cppcheck to consider all files in this project to be C++,
+  # including the headers which end with .h, which cppcheck would normally
+  # consider to be C instead.
+  ament_cppcheck(LANGUAGE "c++")
+  ament_cpplint()
+  ament_lint_cmake()
+  ament_uncrustify()
+endif()
+
+ament_export_dependencies(rosidl_default_runtime)
+
+ament_package()
diff --git a/src/mission_execution/nav2_mission_execution_msgs/README.md b/src/mission_execution/nav2_mission_execution_msgs/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/mission_execution/nav2_mission_execution_msgs/msg/MissionPlan.msg b/src/mission_execution/nav2_mission_execution_msgs/msg/MissionPlan.msg
new file mode 100644
index 0000000000000000000000000000000000000000..690b4e8f6f71a3365fea7d6701e0f3ffafb7fdcb
--- /dev/null
+++ b/src/mission_execution/nav2_mission_execution_msgs/msg/MissionPlan.msg
@@ -0,0 +1,2 @@
+std_msgs/Header header
+string mission_plan
diff --git a/src/mission_execution/nav2_mission_execution_msgs/package.xml b/src/mission_execution/nav2_mission_execution_msgs/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..9c74cd4046a2dc766cdb2b46cf3af8599e506b5c
--- /dev/null
+++ b/src/mission_execution/nav2_mission_execution_msgs/package.xml
@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_mission_execution_msgs</name>
+  <version>0.1.0</version>
+  <description>TODO</description>
+  <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rclcpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>builtin_interfaces</build_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>builtin_interfaces</exec_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/mission_execution/nav2_mission_executor/CHANGELOG.rst b/src/mission_execution/nav2_mission_executor/CHANGELOG.rst
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/mission_execution/nav2_mission_executor/CMakeLists.txt b/src/mission_execution/nav2_mission_executor/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6674e59aa3df6bc70bcd5cd1bff36f4bb7572e92
--- /dev/null
+++ b/src/mission_execution/nav2_mission_executor/CMakeLists.txt
@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_mission_executor)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(nav2_mission_execution_msgs REQUIRED)
+find_package(nav2_tasks REQUIRED)
+
+include_directories(
+  include
+)
+
+add_executable(mission_executor
+  src/main.cpp
+  src/mission_executor.cpp
+)
+
+ament_target_dependencies(mission_executor
+  rclcpp
+  std_msgs
+  nav2_tasks
+  nav2_mission_execution_msgs
+)
+
+install(TARGETS mission_executor
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_cppcheck REQUIRED)
+  find_package(ament_cmake_cpplint REQUIRED)
+  find_package(ament_cmake_lint_cmake REQUIRED)
+  find_package(ament_cmake_uncrustify REQUIRED)
+  # This forces cppcheck to consider all files in this project to be C++,
+  # including the headers which end with .h, which cppcheck would normally
+  # consider to be C instead.
+  ament_cppcheck(LANGUAGE "c++")
+  ament_cpplint()
+  ament_lint_cmake()
+  ament_uncrustify()
+endif()
+
+ament_package()
diff --git a/src/mission_execution/nav2_mission_executor/README.md b/src/mission_execution/nav2_mission_executor/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/mission_execution/nav2_mission_executor/doc/.gitignore b/src/mission_execution/nav2_mission_executor/doc/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/mission_execution/nav2_mission_executor/include/nav2_mission_executor/mission_executor.hpp b/src/mission_execution/nav2_mission_executor/include/nav2_mission_executor/mission_executor.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..b69fb1c066463b0d4fc1dd54577b8036c693d4b8
--- /dev/null
+++ b/src/mission_execution/nav2_mission_executor/include/nav2_mission_executor/mission_executor.hpp
@@ -0,0 +1,41 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_MISSION_EXECUTOR__MISSION_EXECUTOR_HPP_
+#define NAV2_MISSION_EXECUTOR__MISSION_EXECUTOR_HPP_
+
+#include <string>
+#include <memory>
+#include "nav2_tasks/execute_mission_task.hpp"
+#include "nav2_tasks/navigate_to_pose_task.hpp"
+
+namespace nav2_mission_execution
+{
+
+class MissionExecutor : public nav2_tasks::ExecuteMissionTaskServer
+{
+public:
+  MissionExecutor();
+  ~MissionExecutor();
+
+  nav2_tasks::TaskStatus execute(
+    const nav2_tasks::ExecuteMissionCommand::SharedPtr command) override;
+
+private:
+  std::unique_ptr<nav2_tasks::NavigateToPoseTaskClient> navTaskClient_;
+};
+
+}  // namespace nav2_mission_execution
+
+#endif  // NAV2_MISSION_EXECUTOR__MISSION_EXECUTOR_HPP_
diff --git a/src/mission_execution/nav2_mission_executor/package.xml b/src/mission_execution/nav2_mission_executor/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..18b62412bc656e8da3c461a49d1cf875a51ce1bb
--- /dev/null
+++ b/src/mission_execution/nav2_mission_executor/package.xml
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_mission_executor</name>
+  <version>0.1.0</version>
+  <description>TODO</description>
+  <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rclcpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>nav2_tasks</build_depend>
+  <build_depend>nav2_mission_execution_msgs</build_depend>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>nav2_tasks</exec_depend>
+  <exec_depend>nav2_mission_execution_msgs</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/mission_execution/nav2_mission_executor/src/main.cpp b/src/mission_execution/nav2_mission_executor/src/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..05d8887bea5496fa3e251c1d819ed8f28e4096c0
--- /dev/null
+++ b/src/mission_execution/nav2_mission_executor/src/main.cpp
@@ -0,0 +1,26 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_mission_executor/mission_executor.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<nav2_mission_execution::MissionExecutor>());
+  rclcpp::shutdown();
+
+  return 0;
+}
diff --git a/src/mission_execution/nav2_mission_executor/src/mission_executor.cpp b/src/mission_execution/nav2_mission_executor/src/mission_executor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e05a680e55da0faa563bd2d85ba2ec9066c38e45
--- /dev/null
+++ b/src/mission_execution/nav2_mission_executor/src/mission_executor.cpp
@@ -0,0 +1,98 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+#include <memory>
+#include <exception>
+#include "nav2_mission_executor/mission_executor.hpp"
+
+using namespace std::chrono_literals;
+using nav2_tasks::TaskStatus;
+
+namespace nav2_mission_execution
+{
+
+MissionExecutor::MissionExecutor()
+: nav2_tasks::ExecuteMissionTaskServer("ExecuteMissionNode")
+{
+  RCLCPP_INFO(get_logger(), "MissionExecutor::MissionExecutor");
+  navTaskClient_ = std::make_unique<nav2_tasks::NavigateToPoseTaskClient>(this);
+
+  if (!navTaskClient_->waitForServer(nav2_tasks::defaultServerTimeout)) {
+    RCLCPP_ERROR(get_logger(), "MissionExecutor: NavigateToPoseTaskServer not running");
+    throw std::runtime_error("MissionExecutor: NavigateToPoseTaskServer not running");
+  }
+}
+
+MissionExecutor::~MissionExecutor()
+{
+  RCLCPP_INFO(get_logger(), "MissionExecutor::~MissionExecutor");
+}
+
+TaskStatus
+MissionExecutor::execute(const nav2_tasks::ExecuteMissionCommand::SharedPtr command)
+{
+  RCLCPP_INFO(get_logger(), "MissionExecutor:execute");
+  RCLCPP_INFO(get_logger(), "MissionExecutor:execute: plan: %s",
+    command->mission_plan.c_str());
+
+  // TODO(mjeronimo): Validate the mission plan for syntax and semantics
+
+  // TODO(mjeronimo): Get the goal pose from the task in the mission plan
+  auto goalPose = std::make_shared<nav2_tasks::NavigateToPoseCommand>();
+  navTaskClient_->sendCommand(goalPose);
+
+  auto navResult = std::make_shared<nav2_tasks::NavigateToPoseResult>();
+
+  // Loop until navigation reaches a terminal state
+  for (;; ) {
+    // Check to see if this task (mission execution) has been canceled. If so,
+    // cancel the navigation task first and then cancel this task
+    if (cancelRequested()) {
+      RCLCPP_INFO(get_logger(), "MissionExecutor::execute: task has been canceled");
+      navTaskClient_->cancel();
+      setCanceled();
+      return TaskStatus::CANCELED;
+    }
+
+    // This task hasn't been canceled, so see if the navigation task has finished
+    TaskStatus status = navTaskClient_->waitForResult(navResult, 100ms);
+
+    switch (status) {
+      case TaskStatus::SUCCEEDED:
+        {
+          RCLCPP_INFO(get_logger(), "MissionExecutor::execute: navigation task completed");
+
+          // No data to return from this task, just an empty result message
+          nav2_tasks::ExecuteMissionResult result;
+          setResult(result);
+
+          return TaskStatus::SUCCEEDED;
+        }
+
+      case TaskStatus::FAILED:
+        return TaskStatus::FAILED;
+
+      case TaskStatus::RUNNING:
+        RCLCPP_INFO(get_logger(), "MissionExecutor::execute: navigation task still running");
+        break;
+
+      default:
+        RCLCPP_ERROR(get_logger(), "MissionExecutor::execute: invalid status value");
+        throw std::logic_error("MissionExecutor::execute: invalid status value");
+    }
+  }
+}
+
+}  // namespace nav2_mission_execution
diff --git a/src/mission_execution/nav2_mission_executor/test/.gitignore b/src/mission_execution/nav2_mission_executor/test/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/navigation/nav2_bt_navigator/CHANGELOG.rst b/src/navigation/nav2_bt_navigator/CHANGELOG.rst
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/navigation/nav2_bt_navigator/CMakeLists.txt b/src/navigation/nav2_bt_navigator/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..229b37d44cfeeae9ee681bac5d07b5779e17b43d
--- /dev/null
+++ b/src/navigation/nav2_bt_navigator/CMakeLists.txt
@@ -0,0 +1,61 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_bt_navigator)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(nav2_tasks REQUIRED)
+find_package(nav2_planning_msgs REQUIRED)
+
+include_directories(
+  include
+)
+
+add_executable(bt_navigator
+  src/bt_navigator.cpp
+  src/main.cpp
+)
+
+ament_target_dependencies(bt_navigator
+  rclcpp
+  std_msgs
+  nav2_tasks
+  nav2_planning_msgs
+)
+
+install(TARGETS bt_navigator
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_cppcheck REQUIRED)
+  find_package(ament_cmake_cpplint REQUIRED)
+  find_package(ament_cmake_lint_cmake REQUIRED)
+  find_package(ament_cmake_uncrustify REQUIRED)
+  # This forces cppcheck to consider all files in this project to be C++,
+  # including the headers which end with .h, which cppcheck would normally
+  # consider to be C instead.
+  ament_cppcheck(LANGUAGE "c++")
+  ament_cpplint()
+  ament_lint_cmake()
+  ament_uncrustify()
+endif()
+
+ament_export_include_directories(include)
+
+ament_package()
diff --git a/src/navigation/nav2_bt_navigator/README.md b/src/navigation/nav2_bt_navigator/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/navigation/nav2_bt_navigator/doc/.gitignore b/src/navigation/nav2_bt_navigator/doc/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/navigation/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/src/navigation/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..2d9733f820b9b6873edf9cb492f68fdfb66a3d63
--- /dev/null
+++ b/src/navigation/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp
@@ -0,0 +1,42 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_BT_NAVIGATOR__BT_NAVIGATOR_HPP_
+#define NAV2_BT_NAVIGATOR__BT_NAVIGATOR_HPP_
+
+#include <string>
+#include <memory>
+#include "nav2_tasks/navigate_to_pose_task.hpp"
+#include "nav2_tasks/compute_path_to_pose_task.hpp"
+#include "nav2_tasks/follow_path_task.hpp"
+
+namespace nav2_bt_navigator
+{
+
+class BtNavigator : public nav2_tasks::NavigateToPoseTaskServer
+{
+public:
+  BtNavigator();
+  ~BtNavigator();
+
+  nav2_tasks::TaskStatus execute(const nav2_tasks::NavigateToPoseCommand::SharedPtr command);
+
+protected:
+  std::unique_ptr<nav2_tasks::ComputePathToPoseTaskClient> plannerTaskClient_;
+  std::unique_ptr<nav2_tasks::FollowPathTaskClient> controllerTaskClient_;
+};
+
+}  // namespace nav2_bt_navigator
+
+#endif  // NAV2_BT_NAVIGATOR__BT_NAVIGATOR_HPP_
diff --git a/src/navigation/nav2_bt_navigator/package.xml b/src/navigation/nav2_bt_navigator/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..4456a9f9c39b7c66a33b2022e04e8714bc6b652d
--- /dev/null
+++ b/src/navigation/nav2_bt_navigator/package.xml
@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_bt_navigator</name>
+  <version>0.1.0</version>
+  <description>TODO</description>
+  <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rclcpp</build_depend>
+  <build_depend>nav2_tasks</build_depend>
+  <build_depend>nav2_planning_msgs</build_depend>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>nav2_tasks</exec_depend>
+  <exec_depend>nav2_planning_msgs</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/navigation/nav2_bt_navigator/src/bt_navigator.cpp b/src/navigation/nav2_bt_navigator/src/bt_navigator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0670deb2075f2e22e9220bc5e4e602275930b2fc
--- /dev/null
+++ b/src/navigation/nav2_bt_navigator/src/bt_navigator.cpp
@@ -0,0 +1,143 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+#include <memory>
+#include <exception>
+#include <chrono>
+#include "nav2_bt_navigator/bt_navigator.hpp"
+
+using namespace std::chrono_literals;
+using nav2_tasks::TaskStatus;
+
+namespace nav2_bt_navigator
+{
+
+BtNavigator::BtNavigator()
+: nav2_tasks::NavigateToPoseTaskServer("NavigateToPoseNode")
+{
+  RCLCPP_INFO(get_logger(), "BtNavigator::BtNavigator");
+
+  plannerTaskClient_ = std::make_unique<nav2_tasks::ComputePathToPoseTaskClient>(this);
+  controllerTaskClient_ = std::make_unique<nav2_tasks::FollowPathTaskClient>(this);
+
+  if (!plannerTaskClient_->waitForServer(nav2_tasks::defaultServerTimeout)) {
+    RCLCPP_ERROR(get_logger(), "BtNavigator: Planner not running");
+    throw std::runtime_error("BtNavigator: planner not running");
+  }
+
+  if (!controllerTaskClient_->waitForServer(nav2_tasks::defaultServerTimeout)) {
+    RCLCPP_ERROR(get_logger(), "BtNavigator: Controller not running");
+    throw std::runtime_error("BtNavigator: controller not running");
+  }
+}
+
+BtNavigator::~BtNavigator()
+{
+  RCLCPP_INFO(get_logger(), "BtNavigator::~BtNavigator");
+}
+
+TaskStatus
+BtNavigator::execute(const nav2_tasks::NavigateToPoseCommand::SharedPtr command)
+{
+  RCLCPP_INFO(get_logger(), "BtNavigator::execute");
+
+  // Compose the PathEndPoints message for Navigation
+  auto endpoints = std::make_shared<nav2_tasks::ComputePathToPoseCommand>();
+  // TODO(mjeronimo): get the starting pose from Localization (fake it out for now)
+  endpoints->start = command->pose;
+  endpoints->goal = command->pose;
+
+  RCLCPP_INFO(get_logger(), "BtNavigator::execute: getting the path from the planner");
+  auto path = std::make_shared<nav2_tasks::ComputePathToPoseResult>();
+  plannerTaskClient_->sendCommand(endpoints);
+
+  // Loop until the subtasks are completed
+  for (;; ) {
+    // Check to see if this task (navigation) has been canceled. If so, cancel any child
+    // tasks and then cancel this task
+    if (cancelRequested()) {
+      RCLCPP_INFO(get_logger(), "BtNavigator::execute: task has been canceled");
+      plannerTaskClient_->cancel();
+      setCanceled();
+      return TaskStatus::CANCELED;
+    }
+
+    // Check if the planning task has completed
+    TaskStatus status = plannerTaskClient_->waitForResult(path, 100ms);
+
+    switch (status) {
+      case TaskStatus::SUCCEEDED:
+        RCLCPP_INFO(get_logger(), "BtNavigator::execute: planning task completed");
+        goto here;
+
+      case TaskStatus::FAILED:
+        return TaskStatus::FAILED;
+
+      case TaskStatus::RUNNING:
+        RCLCPP_INFO(get_logger(), "BtNavigator::execute: planning task still running");
+        break;
+
+      default:
+        RCLCPP_ERROR(get_logger(), "BtNavigator::execute: invalid status value");
+        throw std::logic_error("BtNavigator::execute: invalid status value");
+    }
+  }
+
+here:
+  RCLCPP_INFO(get_logger(),
+    "BtNavigator::execute: sending the path to the controller to execute");
+
+  controllerTaskClient_->sendCommand(path);
+
+  // Loop until the control task completes
+  for (;; ) {
+    // Check to see if this task (navigation) has been canceled. If so, cancel any child
+    // tasks and then cancel this task
+    if (cancelRequested()) {
+      RCLCPP_INFO(get_logger(), "BtNavigator::execute: task has been canceled");
+      controllerTaskClient_->cancel();
+      setCanceled();
+      return TaskStatus::CANCELED;
+    }
+
+    // Check if the control task has completed
+    auto controlResult = std::make_shared<nav2_tasks::FollowPathResult>();
+    TaskStatus status = controllerTaskClient_->waitForResult(controlResult, 10ms);
+
+    switch (status) {
+      case TaskStatus::SUCCEEDED:
+        {
+          RCLCPP_INFO(get_logger(), "BtNavigator::execute: control task completed");
+          nav2_tasks::NavigateToPoseResult navigationResult;
+          setResult(navigationResult);
+
+          return TaskStatus::SUCCEEDED;
+        }
+
+      case TaskStatus::FAILED:
+        return TaskStatus::FAILED;
+
+      case TaskStatus::RUNNING:
+        RCLCPP_INFO(get_logger(), "BtNavigator::execute: control task still running");
+        break;
+
+      default:
+        RCLCPP_ERROR(get_logger(), "BtNavigator::execute: invalid status value");
+        throw std::logic_error("BtNavigator::execute: invalid status value");
+    }
+  }
+}
+
+}  // namespace nav2_bt_navigator
diff --git a/src/navigation/nav2_bt_navigator/src/main.cpp b/src/navigation/nav2_bt_navigator/src/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..56a02c99784351ab8c73e13dea9dda6198b4fca1
--- /dev/null
+++ b/src/navigation/nav2_bt_navigator/src/main.cpp
@@ -0,0 +1,26 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_bt_navigator/bt_navigator.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<nav2_bt_navigator::BtNavigator>());
+  rclcpp::shutdown();
+
+  return 0;
+}
diff --git a/src/navigation/nav2_bt_navigator/test/.gitignore b/src/navigation/nav2_bt_navigator/test/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/navigation/nav2_navigation_msgs/.gitignore b/src/navigation/nav2_navigation_msgs/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/navigation/nav2_simple_navigator/CHANGELOG.rst b/src/navigation/nav2_simple_navigator/CHANGELOG.rst
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/navigation/nav2_simple_navigator/CMakeLists.txt b/src/navigation/nav2_simple_navigator/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..181d802fd80df0e15f94581a7eca3783adcf7a16
--- /dev/null
+++ b/src/navigation/nav2_simple_navigator/CMakeLists.txt
@@ -0,0 +1,61 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_simple_navigator)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(nav2_tasks REQUIRED)
+find_package(nav2_planning_msgs REQUIRED)
+
+include_directories(
+  include
+)
+
+add_executable(simple_navigator
+  src/simple_navigator.cpp
+  src/main.cpp
+)
+
+ament_target_dependencies(simple_navigator
+  rclcpp
+  std_msgs
+  nav2_tasks
+  nav2_planning_msgs
+)
+
+install(TARGETS simple_navigator
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_cppcheck REQUIRED)
+  find_package(ament_cmake_cpplint REQUIRED)
+  find_package(ament_cmake_lint_cmake REQUIRED)
+  find_package(ament_cmake_uncrustify REQUIRED)
+  # This forces cppcheck to consider all files in this project to be C++,
+  # including the headers which end with .h, which cppcheck would normally
+  # consider to be C instead.
+  ament_cppcheck(LANGUAGE "c++")
+  ament_cpplint()
+  ament_lint_cmake()
+  ament_uncrustify()
+endif()
+
+ament_export_include_directories(include)
+
+ament_package()
diff --git a/src/navigation/nav2_simple_navigator/README.md b/src/navigation/nav2_simple_navigator/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/navigation/nav2_simple_navigator/doc/.gitignore b/src/navigation/nav2_simple_navigator/doc/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/navigation/nav2_simple_navigator/include/nav2_simple_navigator/simple_navigator.hpp b/src/navigation/nav2_simple_navigator/include/nav2_simple_navigator/simple_navigator.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..de6a446e81c89a7936322a6502b5f180b1930ca8
--- /dev/null
+++ b/src/navigation/nav2_simple_navigator/include/nav2_simple_navigator/simple_navigator.hpp
@@ -0,0 +1,42 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_SIMPLE_NAVIGATOR__SIMPLE_NAVIGATOR_HPP_
+#define NAV2_SIMPLE_NAVIGATOR__SIMPLE_NAVIGATOR_HPP_
+
+#include <string>
+#include <memory>
+#include "nav2_tasks/navigate_to_pose_task.hpp"
+#include "nav2_tasks/compute_path_to_pose_task.hpp"
+#include "nav2_tasks/follow_path_task.hpp"
+
+namespace nav2_simple_navigator
+{
+
+class SimpleNavigator : public nav2_tasks::NavigateToPoseTaskServer
+{
+public:
+  SimpleNavigator();
+  ~SimpleNavigator();
+
+  nav2_tasks::TaskStatus execute(const nav2_tasks::NavigateToPoseCommand::SharedPtr command);
+
+protected:
+  std::unique_ptr<nav2_tasks::ComputePathToPoseTaskClient> plannerTaskClient_;
+  std::unique_ptr<nav2_tasks::FollowPathTaskClient> controllerTaskClient_;
+};
+
+}  // namespace nav2_simple_navigator
+
+#endif  // NAV2_SIMPLE_NAVIGATOR__SIMPLE_NAVIGATOR_HPP_
diff --git a/src/navigation/nav2_simple_navigator/package.xml b/src/navigation/nav2_simple_navigator/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..f9ed6e4c4fd489caeb98489b6b2aeba40ed90d85
--- /dev/null
+++ b/src/navigation/nav2_simple_navigator/package.xml
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_simple_navigator</name>
+  <version>0.1.0</version>
+  <description>TODO</description>
+  <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rclcpp</build_depend>
+  <build_depend>nav2_tasks</build_depend>
+  <build_depend>nav2_planning_msgs</build_depend>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>nav2_tasks</exec_depend>
+  <exec_depend>nav2_planning_msgs</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
+
diff --git a/src/navigation/nav2_simple_navigator/src/main.cpp b/src/navigation/nav2_simple_navigator/src/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..055ef609b2373c756373322e61b2a5ed08f98bb7
--- /dev/null
+++ b/src/navigation/nav2_simple_navigator/src/main.cpp
@@ -0,0 +1,26 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_simple_navigator/simple_navigator.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<nav2_simple_navigator::SimpleNavigator>());
+  rclcpp::shutdown();
+
+  return 0;
+}
diff --git a/src/navigation/nav2_simple_navigator/src/simple_navigator.cpp b/src/navigation/nav2_simple_navigator/src/simple_navigator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..faa4a3ca13e62ad61e9ffb86f1d043447b7b8972
--- /dev/null
+++ b/src/navigation/nav2_simple_navigator/src/simple_navigator.cpp
@@ -0,0 +1,158 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+#include <memory>
+#include <exception>
+#include <chrono>
+#include "nav2_simple_navigator/simple_navigator.hpp"
+
+using namespace std::chrono_literals;
+using nav2_tasks::TaskStatus;
+
+namespace nav2_simple_navigator
+{
+
+SimpleNavigator::SimpleNavigator()
+: nav2_tasks::NavigateToPoseTaskServer("NavigateToPoseNode")
+{
+  RCLCPP_INFO(get_logger(), "SimpleNavigator::SimpleNavigator");
+
+  plannerTaskClient_ = std::make_unique<nav2_tasks::ComputePathToPoseTaskClient>(this);
+
+  if (!plannerTaskClient_->waitForServer(nav2_tasks::defaultServerTimeout)) {
+    RCLCPP_ERROR(get_logger(), "SimpleNavigator: planner not running");
+    throw std::runtime_error("SimpleNavigator: planner not running");
+  }
+
+  controllerTaskClient_ = std::make_unique<nav2_tasks::FollowPathTaskClient>(this);
+
+  if (!controllerTaskClient_->waitForServer(nav2_tasks::defaultServerTimeout)) {
+    RCLCPP_ERROR(get_logger(), "SimpleNavigator: controller not running");
+    throw std::runtime_error("SimpleNavigator: controller not running");
+  }
+}
+
+SimpleNavigator::~SimpleNavigator()
+{
+  RCLCPP_INFO(get_logger(), "SimpleNavigator::~SimpleNavigator");
+}
+
+TaskStatus
+SimpleNavigator::execute(const nav2_tasks::NavigateToPoseCommand::SharedPtr /*command*/)
+{
+  RCLCPP_INFO(get_logger(), "SimpleNavigator::execute");
+
+  // Compose the PathEndPoints message for Navigation
+  auto endpoints = std::make_shared<nav2_tasks::ComputePathToPoseCommand>();
+  // TODO(mjeronimo): get the starting pose from Localization (fake it out for now)
+  endpoints->start.position.x = 1.0;
+  endpoints->start.position.y = 1.0;
+  endpoints->goal.position.x = 9.0;
+  endpoints->goal.position.y = 9.0;
+  endpoints->tolerance = 2.0;
+
+  RCLCPP_INFO(get_logger(), "SimpleNavigator::execute: getting the path from the planner");
+  auto path = std::make_shared<nav2_tasks::ComputePathToPoseResult>();
+  plannerTaskClient_->sendCommand(endpoints);
+
+  // Loop until the subtasks are completed
+  for (;; ) {
+    // Check to see if this task (navigation) has been canceled. If so, cancel any child
+    // tasks and then cancel this task
+    if (cancelRequested()) {
+      RCLCPP_INFO(get_logger(), "SimpleNavigator::execute: task has been canceled");
+      plannerTaskClient_->cancel();
+      setCanceled();
+      return TaskStatus::CANCELED;
+    }
+
+    // Check if the planning task has completed
+    TaskStatus status = plannerTaskClient_->waitForResult(path, 100ms);
+
+    switch (status) {
+      case TaskStatus::SUCCEEDED:
+        RCLCPP_INFO(get_logger(), "SimpleNavigator::execute: planning task completed");
+        goto planning_succeeded;
+
+      case TaskStatus::FAILED:
+        RCLCPP_ERROR(get_logger(), "SimpleNavigator::execute: planning task failed");
+        return TaskStatus::FAILED;
+
+      case TaskStatus::RUNNING:
+        RCLCPP_INFO(get_logger(), "SimpleNavigator::execute: planning task still running");
+        break;
+
+      default:
+        RCLCPP_ERROR(get_logger(), "SimpleNavigator::execute: invalid status value");
+        throw std::logic_error("SimpleNavigator::execute: invalid status value");
+    }
+  }
+
+planning_succeeded:
+
+  RCLCPP_INFO(get_logger(), "SimpleNavigator::execute: got path of size %u",
+    path->poses.size());
+  int index;
+  for (auto pose : path->poses) {
+    RCLCPP_INFO(get_logger(), "SimpleNavigator::execute: point %u x: %0.2f, y: %0.2f",
+      index, pose.position.x, pose.position.y);
+    index++;
+  }
+
+  RCLCPP_INFO(get_logger(),
+    "SimpleNavigator::execute: sending the path to the controller to execute");
+
+  controllerTaskClient_->sendCommand(path);
+
+  // Loop until the control task completes
+  for (;; ) {
+    // Check to see if this task (navigation) has been canceled. If so, cancel any child
+    // tasks and then cancel this task
+    if (cancelRequested()) {
+      RCLCPP_INFO(get_logger(), "SimpleNavigator::execute: task has been canceled");
+      controllerTaskClient_->cancel();
+      setCanceled();
+      return TaskStatus::CANCELED;
+    }
+
+    // Check if the control task has completed
+    auto controlResult = std::make_shared<nav2_tasks::FollowPathResult>();
+    TaskStatus status = controllerTaskClient_->waitForResult(controlResult, 10ms);
+
+    switch (status) {
+      case TaskStatus::SUCCEEDED:
+        {
+          RCLCPP_INFO(get_logger(), "SimpleNavigator::execute: control task completed");
+          nav2_tasks::NavigateToPoseResult navigationResult;
+          setResult(navigationResult);
+
+          return TaskStatus::SUCCEEDED;
+        }
+
+      case TaskStatus::FAILED:
+        RCLCPP_ERROR(get_logger(), "SimpleNavigator::execute: control task failed");
+        return TaskStatus::FAILED;
+
+      case TaskStatus::RUNNING:
+        break;
+
+      default:
+        RCLCPP_ERROR(get_logger(), "SimpleNavigator::execute: invalid status value");
+        throw std::logic_error("SimpleNavigator::execute: invalid status value");
+    }
+  }
+}
+
+}  // namespace nav2_simple_navigator
diff --git a/src/navigation/nav2_simple_navigator/test/.gitignore b/src/navigation/nav2_simple_navigator/test/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/planning/nav2_astar_planner/CHANGELOG.rst b/src/planning/nav2_astar_planner/CHANGELOG.rst
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/planning/nav2_astar_planner/CMakeLists.txt b/src/planning/nav2_astar_planner/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3f4f5fd0acb0740aa3d0e8fd1c019fe7f3a78357
--- /dev/null
+++ b/src/planning/nav2_astar_planner/CMakeLists.txt
@@ -0,0 +1,65 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_astar_planner)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(nav2_tasks REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(nav2_planning_msgs REQUIRED)
+
+include_directories(
+  include
+)
+
+add_executable(astar_planner
+  src/astar_planner.cpp
+  src/main.cpp
+)
+
+ament_target_dependencies(astar_planner
+  rclcpp
+  std_msgs
+  nav2_tasks
+  task
+  nav2_util
+  nav2_planning_msgs
+)
+
+install(TARGETS
+  astar_planner
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_cppcheck REQUIRED)
+  find_package(ament_cmake_cpplint REQUIRED)
+  find_package(ament_cmake_lint_cmake REQUIRED)
+  find_package(ament_cmake_uncrustify REQUIRED)
+  # This forces cppcheck to consider all files in this project to be C++,
+  # including the headers which end with .h, which cppcheck would normally
+  # consider to be C instead.
+  ament_cppcheck(LANGUAGE "c++")
+  ament_cpplint()
+  ament_lint_cmake()
+  ament_uncrustify()
+endif()
+
+ament_export_include_directories(include)
+
+ament_package()
diff --git a/src/planning/nav2_astar_planner/README.md b/src/planning/nav2_astar_planner/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/planning/nav2_astar_planner/doc/..gitignore b/src/planning/nav2_astar_planner/doc/..gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/planning/nav2_astar_planner/include/nav2_astar_planner/astar_planner.hpp b/src/planning/nav2_astar_planner/include/nav2_astar_planner/astar_planner.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a85d0cbe637e0702ae0f4e82b52e4e73f535bdcc
--- /dev/null
+++ b/src/planning/nav2_astar_planner/include/nav2_astar_planner/astar_planner.hpp
@@ -0,0 +1,36 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_ASTAR_PLANNER__ASTAR_PLANNER_HPP_
+#define NAV2_ASTAR_PLANNER__ASTAR_PLANNER_HPP_
+
+#include <string>
+#include "nav2_tasks/compute_path_to_pose_task.hpp"
+
+namespace nav2_astar_planner
+{
+
+class AStarPlanner : public nav2_tasks::ComputePathToPoseTaskServer
+{
+public:
+  AStarPlanner();
+  ~AStarPlanner();
+
+  nav2_tasks::TaskStatus execute(
+    const nav2_tasks::ComputePathToPoseCommand::SharedPtr command) override;
+};
+
+}  // namespace nav2_astar_planner
+
+#endif  // NAV2_ASTAR_PLANNER__ASTAR_PLANNER_HPP_
diff --git a/src/planning/nav2_astar_planner/package.xml b/src/planning/nav2_astar_planner/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..349367aec84c0cb8326246f7d8f0ff1a00aaa9e4
--- /dev/null
+++ b/src/planning/nav2_astar_planner/package.xml
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_astar_planner</name>
+  <version>0.1.0</version>
+  <description>TODO</description>
+  <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rclcpp</build_depend>
+  <build_depend>nav2_tasks</build_depend>
+  <build_depend>nav2_util</build_depend>
+  <build_depend>nav2_planning_msgs</build_depend>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>nav2_tasks</exec_depend>
+  <exec_depend>nav2_planning_msgs</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/planning/nav2_astar_planner/src/astar_planner.cpp b/src/planning/nav2_astar_planner/src/astar_planner.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4594d33ce07852dc70ea163f59256ae27b8826d5
--- /dev/null
+++ b/src/planning/nav2_astar_planner/src/astar_planner.cpp
@@ -0,0 +1,64 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+#include <chrono>
+#include "nav2_astar_planner/astar_planner.hpp"
+
+using namespace std::chrono_literals;
+using nav2_tasks::TaskStatus;
+
+namespace nav2_astar_planner
+{
+
+AStarPlanner::AStarPlanner()
+: nav2_tasks::ComputePathToPoseTaskServer("ComputePathToPoseNode")
+{
+  RCLCPP_INFO(get_logger(), "AStarPlanner::AStarPlanner");
+}
+
+AStarPlanner::~AStarPlanner()
+{
+  RCLCPP_INFO(get_logger(), "AStarPlanner::~AStarPlanner");
+}
+
+TaskStatus
+AStarPlanner::execute(const nav2_tasks::ComputePathToPoseCommand::SharedPtr /*command*/)
+{
+  RCLCPP_INFO(get_logger(), "AStarPlanner::execute");
+
+  // Spin here for a bit to fake out some processing time
+  for (int i = 0; i < 10; i++) {
+    // Do a bit of the task
+    RCLCPP_INFO(get_logger(), "AStarPlanner::execute: doing work: %d", i);
+    std::this_thread::sleep_for(100ms);
+
+    // Before we loop again to do more work, check if we've been canceled
+    if (cancelRequested()) {
+      RCLCPP_INFO(get_logger(), "AStarPlanner::execute: task has been canceled");
+      setCanceled();
+      return TaskStatus::CANCELED;
+    }
+  }
+
+  // We've successfully completed the task, so return the result
+  RCLCPP_INFO(get_logger(), "AStarPlanner::execute: task completed");
+
+  nav2_tasks::ComputePathToPoseResult result;
+  setResult(result);
+
+  return TaskStatus::SUCCEEDED;
+}
+
+}  // namespace nav2_astar_planner
diff --git a/src/planning/nav2_astar_planner/src/main.cpp b/src/planning/nav2_astar_planner/src/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..703416bf10008887388e7d380fd9019d7441445f
--- /dev/null
+++ b/src/planning/nav2_astar_planner/src/main.cpp
@@ -0,0 +1,26 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_astar_planner/astar_planner.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<nav2_astar_planner::AStarPlanner>());
+  rclcpp::shutdown();
+
+  return 0;
+}
diff --git a/src/planning/nav2_astar_planner/test/.gitignore b/src/planning/nav2_astar_planner/test/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/planning/nav2_dijkstra_planner/CHANGELOG.rst b/src/planning/nav2_dijkstra_planner/CHANGELOG.rst
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/planning/nav2_dijkstra_planner/CMakeLists.txt b/src/planning/nav2_dijkstra_planner/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d6a2b904041609fa8be1805766683898857e9d64
--- /dev/null
+++ b/src/planning/nav2_dijkstra_planner/CMakeLists.txt
@@ -0,0 +1,69 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_dijkstra_planner)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(nav2_tasks REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(nav2_planning_msgs REQUIRED)
+find_package(nav2_libs_msgs REQUIRED)
+find_package(nav2_world_model_msgs REQUIRED)
+
+include_directories(
+  include
+)
+
+add_executable(dijkstra_planner
+  src/dijkstra_planner.cpp
+  src/navfn.cpp
+  src/main.cpp
+)
+
+ament_target_dependencies(dijkstra_planner
+  rclcpp
+  std_msgs
+  nav2_tasks
+  nav2_util
+  nav2_planning_msgs
+  nav2_libs_msgs
+  nav2_world_model_msgs
+)
+
+install(TARGETS
+  dijkstra_planner
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_cppcheck REQUIRED)
+  find_package(ament_cmake_cpplint REQUIRED)
+  find_package(ament_cmake_lint_cmake REQUIRED)
+  find_package(ament_cmake_uncrustify REQUIRED)
+  # This forces cppcheck to consider all files in this project to be C++,
+  # including the headers which end with .h, which cppcheck would normally
+  # consider to be C instead.
+  ament_cppcheck(LANGUAGE "c++")
+  ament_cpplint()
+  ament_lint_cmake()
+  ament_uncrustify()
+endif()
+
+ament_export_include_directories(include)
+
+ament_package()
diff --git a/src/planning/nav2_dijkstra_planner/README.md b/src/planning/nav2_dijkstra_planner/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/planning/nav2_dijkstra_planner/doc/..gitignore b/src/planning/nav2_dijkstra_planner/doc/..gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/planning/nav2_dijkstra_planner/include/nav2_dijkstra_planner/dijkstra_planner.hpp b/src/planning/nav2_dijkstra_planner/include/nav2_dijkstra_planner/dijkstra_planner.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..8580caf4e2931a33f0af2df586cf32ddd825f4b5
--- /dev/null
+++ b/src/planning/nav2_dijkstra_planner/include/nav2_dijkstra_planner/dijkstra_planner.hpp
@@ -0,0 +1,119 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_DIJKSTRA_PLANNER__DIJKSTRA_PLANNER_HPP_
+#define NAV2_DIJKSTRA_PLANNER__DIJKSTRA_PLANNER_HPP_
+
+#include <string>
+#include <vector>
+#include <memory>
+#include <chrono>
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "nav2_dijkstra_planner/navfn.hpp"
+#include "nav2_tasks/compute_path_to_pose_task.hpp"
+#include "nav2_world_model_msgs/srv/get_costmap.hpp"
+#include "nav2_libs_msgs/msg/costmap.hpp"
+
+namespace nav2_dijkstra_planner
+{
+
+class DijkstraPlanner : public nav2_tasks::ComputePathToPoseTaskServer
+{
+public:
+  DijkstraPlanner();
+  ~DijkstraPlanner();
+
+  nav2_tasks::TaskStatus execute(
+    const nav2_tasks::ComputePathToPoseCommand::SharedPtr command) override;
+
+private:
+  // Compute a plan given start and goal poses, provided in global world frame.
+  bool makePlan(
+    const geometry_msgs::msg::Pose & start,
+    const geometry_msgs::msg::Pose & goal, double tolerance,
+    nav2_planning_msgs::msg::Path & plan);
+
+  // Compute the navigation function given a seed point in the world to start from
+  bool computePotential(const geometry_msgs::msg::Point & world_point);
+
+  // Compute a plan to a goal from a potential - must call computePotential first
+  bool getPlanFromPotential(
+    const geometry_msgs::msg::Pose & goal,
+    nav2_planning_msgs::msg::Path & plan);
+
+  // Compute the potential, or navigation cost, at a given point in the world
+  // - must call computePotential first
+  double getPointPotential(const geometry_msgs::msg::Point & world_point);
+
+  // Check for a valid potential value at a given point in the world
+  // - must call computePotential first
+  bool validPointPotential(const geometry_msgs::msg::Point & world_point);
+  bool validPointPotential(const geometry_msgs::msg::Point & world_point, double tolerance);
+
+  // Compute the squared distance between two points
+  inline double squared_distance(
+    const geometry_msgs::msg::Pose & p1,
+    const geometry_msgs::msg::Pose & p2)
+  {
+    double dx = p1.position.x - p2.position.x;
+    double dy = p1.position.y - p2.position.y;
+    return dx * dx + dy * dy;
+  }
+
+  // Transform a point from world to map frame
+  bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my);
+
+  // Transform a point from map to world frame
+  void mapToWorld(double mx, double my, double & wx, double & wy);
+
+  // Set the corresponding cell cost to be free space
+  void clearRobotCell(unsigned int mx, unsigned int my);
+
+  // Request costmap from world model
+  void getCostmap(
+    nav2_libs_msgs::msg::Costmap & costmap, const std::string layer = "master",
+    const std::chrono::milliseconds waitTime = std::chrono::milliseconds(100));
+
+  // Wait for costmap server to appear
+  void waitForCostmapServer(const std::chrono::seconds waitTime = std::chrono::seconds(10));
+
+  // Print costmap to terminal
+  void printCostmap(const nav2_libs_msgs::msg::Costmap & costmap);
+
+  // Planner based on ROS1 NavFn algorithm
+  std::shared_ptr<NavFn> planner_;
+
+  // Client for getting the costmap
+  rclcpp::Client<nav2_world_model_msgs::srv::GetCostmap>::SharedPtr costmap_client_;
+
+  // Computed path publisher
+  rclcpp::Publisher<nav2_planning_msgs::msg::Path>::SharedPtr plan_publisher_;
+
+  // The costmap to use
+  nav2_libs_msgs::msg::Costmap costmap_;
+
+  // The global frame of the costmap
+  std::string global_frame_;
+
+  // Whether or not the planner should be allowed to plan through unknown space
+  bool allow_unknown_;
+
+  // Amount the planner can relax the space constraint
+  double default_tolerance_;
+};
+
+}  // namespace nav2_dijkstra_planner
+
+#endif  // NAV2_DIJKSTRA_PLANNER__DIJKSTRA_PLANNER_HPP_
diff --git a/src/planning/nav2_dijkstra_planner/include/nav2_dijkstra_planner/navfn.hpp b/src/planning/nav2_dijkstra_planner/include/nav2_dijkstra_planner/navfn.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..4499805d16fad0f254875491e175dd3e81094ba0
--- /dev/null
+++ b/src/planning/nav2_dijkstra_planner/include/nav2_dijkstra_planner/navfn.hpp
@@ -0,0 +1,269 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  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 Willow Garage 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.
+*********************************************************************/
+
+//
+// Navigation function computation
+// Uses Dijkstra's method
+// Modified for Euclidean-distance computation
+//
+
+#ifndef NAV2_DIJKSTRA_PLANNER__NAVFN_HPP_
+#define NAV2_DIJKSTRA_PLANNER__NAVFN_HPP_
+
+#include <math.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdio.h>
+
+// cost defs
+#define COST_UNKNOWN_ROS 255  // 255 is unknown cost
+#define COST_OBS 254  // 254 for forbidden regions
+#define COST_OBS_ROS 253  // ROS values of 253 are obstacles
+
+// navfn cost values are set to
+// COST_NEUTRAL + COST_FACTOR * costmap_cost_value.
+// Incoming costmap cost values are in the range 0 to 252.
+// With COST_NEUTRAL of 50, the COST_FACTOR needs to be about 0.8 to
+// ensure the input values are spread evenly over the output range, 50
+// to 253.  If COST_FACTOR is higher, cost values will have a plateau
+// around obstacles and the planner will then treat (for example) the
+// whole width of a narrow hallway as equally undesirable and thus
+// will not plan paths down the center.
+
+#define COST_NEUTRAL 50  // Set this to "open space" value
+#define COST_FACTOR 0.8  // Used for translating costs in NavFn::setCostmap()
+
+// Define the cost type in the case that it is not set. However, this allows
+// clients to modify it without changing the file. Arguably, it is better to require it to
+// be defined by a user explicitly
+#ifndef COSTTYPE
+#define COSTTYPE unsigned char  // Whatever is used...
+#endif
+
+// potential defs
+#define POT_HIGH 1.0e10  // unassigned cell potential
+
+// priority buffers
+#define PRIORITYBUFSIZE 10000
+
+/**
+  Navigation function call.
+  \param costmap Cost map array, of type COSTTYPE; origin is upper left
+    NOTE: will be modified to have a border of obstacle costs
+  \param nx Width of map in cells
+  \param ny Height of map in cells
+  \param goal X,Y position of goal cell
+  \param start X,Y position of start cell
+
+Returns length of plan if found, and fills an array with x,y interpolated
+positions at about 1/2 cell resolution; else returns 0.
+*/
+int create_nav_plan_astar(
+  const COSTTYPE * costmap, int nx, int ny,
+  int * goal, int * start,
+  float * plan, int nplan);
+
+/**
+ * @class NavFn
+ * @brief Navigation function class. Holds buffers for costmap, navfn map. Maps are pixel-based.
+ *  Origin is upper left, x is right, y is down.
+ */
+class NavFn
+{
+public:
+  /**
+   * @brief  Constructs the planner
+   * @param nx The x size of the map
+   * @param ny The y size of the map
+   */
+  NavFn(int nx, int ny);
+
+  ~NavFn();
+
+  /**
+   * @brief  Sets or resets the size of the map
+   * @param nx The x size of the map
+   * @param ny The y size of the map
+   */
+  void setNavArr(int nx, int ny);
+  int nx, ny, ns;  /**< size of grid, in pixels */
+
+  /**
+   * @brief  Set up the cost array for the planner, usually from ROS
+   * @param cmap The costmap
+   * @param isROS Whether or not the costmap is coming in in ROS format
+   * @param allow_unknown Whether or not the planner should be allowed to plan through
+   *   unknown space
+   */
+  void setCostmap(const COSTTYPE * cmap, bool isROS = true, bool allow_unknown = true);
+
+  /**
+   * @brief  Calculates a plan using the A* heuristic, returns true if one is found
+   * @return True if a plan is found, false otherwise
+   */
+  bool calcNavFnAstar();
+
+  /**
+   * @brief Caclulates the full navigation function using Dijkstra
+   */
+  bool calcNavFnDijkstra(bool atStart = false);
+
+  /**
+   * @brief  Accessor for the x-coordinates of a path
+   * @return The x-coordinates of a path
+   */
+  float * getPathX();
+
+  /**
+   * @brief  Accessor for the y-coordinates of a path
+   * @return The y-coordinates of a path
+   */
+  float * getPathY();
+
+  /**
+   * @brief  Accessor for the length of a path
+   * @return The length of a path, 0 if not found
+   */
+  int getPathLen();
+
+  /**
+   * @brief  Gets the cost of the path found the last time a navigation function was computed
+   * @return The cost of the last path found
+   */
+  float getLastPathCost();
+
+  /** cell arrays */
+  COSTTYPE * costarr;  /**< cost array in 2D configuration space */
+  float * potarr;  /**< potential array, navigation function potential */
+  bool * pending;  /**< pending cells during propagation */
+  int nobs;  /**< number of obstacle cells */
+
+  /** block priority buffers */
+  int * pb1, * pb2, * pb3;  /**< storage buffers for priority blocks */
+  int * curP, * nextP, * overP;  /**< priority buffer block ptrs */
+  int curPe, nextPe, overPe;  /**< end points of arrays */
+
+  /** block priority thresholds */
+  float curT;  /**< current threshold */
+  float priInc;  /**< priority threshold increment */
+
+  /** goal and start positions */
+  /**
+   * @brief  Sets the goal position for the planner.
+   * Note: the navigation cost field computed gives the cost to get to a given point
+   * from the goal, not from the start.
+   * @param goal the goal position
+   */
+  void setGoal(int * goal);
+
+  /**
+   * @brief  Sets the start position for the planner.
+   * Note: the navigation cost field computed gives the cost to get to a given point
+   * from the goal, not from the start.
+   * @param start the start position
+   */
+  void setStart(int * start);
+
+  int goal[2];
+  int start[2];
+  /**
+   * @brief  Initialize cell k with cost v for propagation
+   * @param k the cell to initialize
+   * @param v the cost to give to the cell
+   */
+  void initCost(int k, float v);
+
+  /** propagation */
+
+  /**
+   * @brief  Updates the cell at index n
+   * @param n The index to update
+   */
+  void updateCell(int n);
+
+  /**
+   * @brief  Updates the cell at index n using the A* heuristic
+   * @param n The index to update
+   */
+  void updateCellAstar(int n);
+
+  void setupNavFn(bool keepit = false);
+
+  /**
+   * @brief  Run propagation for <cycles> iterations, or until start is reached using
+   * breadth-first Dijkstra method
+   * @param cycles The maximum number of iterations to run for
+   * @param atStart Whether or not to stop when the start point is reached
+   * @return true if the start point is reached
+   */
+  bool propNavFnDijkstra(int cycles, bool atStart = false);
+
+  /**
+   * @brief  Run propagation for <cycles> iterations, or until start is reached using
+   * the best-first A* method with Euclidean distance heuristic
+   * @param cycles The maximum number of iterations to run for
+   * @return true if the start point is reached
+   */
+  bool propNavFnAstar(int cycles);  /**< returns true if start point found */
+
+  /** gradient and paths */
+  float * gradx, * grady;  /**< gradient arrays, size of potential array */
+  float * pathx, * pathy;  /**< path points, as subpixel cell coordinates */
+  int npath;  /**< number of path points */
+  int npathbuf;  /**< size of pathx, pathy buffers */
+
+  float last_path_cost_;  /**< Holds the cost of the path found the last time A* was called */
+
+  /**
+   * @brief  Calculates the path for at mose <n> cycles
+   * @param n The maximum number of cycles to run for
+   * @return The lenght of the path found, 0 if none
+   */
+  int calcPath(int n, int * st = NULL);
+
+  float gradCell(int n);  /**< calculates gradient at cell <n>, returns norm */
+  float pathStep;  /**< step size for following gradient */
+
+  /** display callback */
+  /**< <n> is the number of cycles between updates  */
+  void display(void fn(NavFn * nav), int n = 100);
+  int displayInt;  /**< save second argument of display() above */
+  void (* displayFn)(NavFn * nav);  /**< display function itself */
+
+  /** save costmap */
+  /**< write out costmap and start/goal states as fname.pgm and fname.txt */
+  void savemap(const char * fname);
+};
+
+#endif  // NAV2_DIJKSTRA_PLANNER__NAVFN_HPP_
diff --git a/src/planning/nav2_dijkstra_planner/package.xml b/src/planning/nav2_dijkstra_planner/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..8bbb8c3f7f0d4fe97b35ef9d94f944e73e55d670
--- /dev/null
+++ b/src/planning/nav2_dijkstra_planner/package.xml
@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_dijkstra_planner</name>
+  <version>0.1.0</version>
+  <description>TODO</description>
+  <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rclcpp</build_depend>
+  <build_depend>nav2_tasks</build_depend>
+  <build_depend>nav2_util</build_depend>
+  <build_depend>nav2_planning_msgs</build_depend>
+  <build_depend>nav2_util_msgs</build_depend>
+  <build_depend>nav2_world_model_msgs</build_depend>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>nav2_tasks</exec_depend>
+  <exec_depend>nav2_planning_msgs</exec_depend>
+  <exec_depend>nav2_util_msgs</exec_depend>
+  <exec_depend>nav2_world_model_msgs</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/planning/nav2_dijkstra_planner/src/Logger.h b/src/planning/nav2_dijkstra_planner/src/Logger.h
new file mode 100644
index 0000000000000000000000000000000000000000..da06664a776baea16b29c8fa32182ab1d5259082
--- /dev/null
+++ b/src/planning/nav2_dijkstra_planner/src/Logger.h
@@ -0,0 +1,33 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef LOGGER_H_
+#define LOGGER_H_
+
+#include <cstdio>
+
+// TODO(orduno): Figure out how to get a reference to the node logger
+#define ROS_ERROR(...) std::fprintf(stderr, __VA_ARGS__)
+
+#define ROS_WARN(...) std::fprintf(stderr, __VA_ARGS__)
+
+#define ROS_INFO(...) std::fprintf(stderr, __VA_ARGS__)
+
+#define ROS_DEBUG(...) std::fprintf(stderr, __VA_ARGS__)
+
+#define ROS_DEBUG_NAMED(...) std::fprintf(stderr, __VA_ARGS__)
+
+#define ROS_INFO_ONCE(...) std::fprintf(stderr, __VA_ARGS__)
+
+#endif  // LOGGER_H_
diff --git a/src/planning/nav2_dijkstra_planner/src/dijkstra_planner.cpp b/src/planning/nav2_dijkstra_planner/src/dijkstra_planner.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..aa772ca815727cdb26ed10e58e5059576f325d5c
--- /dev/null
+++ b/src/planning/nav2_dijkstra_planner/src/dijkstra_planner.cpp
@@ -0,0 +1,452 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+// Navigation Strategy based on:
+// Brock, O. and Oussama K. (1999). High-Speed Navigation Using
+// the Global Dynamic Window Approach. IEEE.
+// https://cs.stanford.edu/group/manips/publications/pdfs/Brock_1999_ICRA.pdf
+
+#include <string>
+#include <vector>
+#include <memory>
+#include <chrono>
+#include <limits>
+#include <iostream>
+#include <iomanip>
+#include <algorithm>
+#include <exception>
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "nav2_dijkstra_planner/dijkstra_planner.hpp"
+#include "nav2_dijkstra_planner/navfn.hpp"
+#include "nav2_util/costmap.hpp"
+#include "nav2_libs_msgs/msg/costmap.hpp"
+#include "nav2_world_model_msgs/srv/get_costmap.hpp"
+
+using namespace std::chrono_literals;
+using nav2_tasks::TaskStatus;
+
+namespace nav2_dijkstra_planner
+{
+
+DijkstraPlanner::DijkstraPlanner()
+: nav2_tasks::ComputePathToPoseTaskServer("ComputePathToPoseNode"),
+  global_frame_("/map"), allow_unknown_(true), default_tolerance_(1.0)
+{
+  RCLCPP_INFO(get_logger(), "DijkstraPlanner::DijkstraPlanner");
+
+  // TODO(orduno): Enable parameter server
+
+  costmap_client_ = this->create_client<nav2_world_model_msgs::srv::GetCostmap>("CostmapService");
+  waitForCostmapServer();
+
+  // TODO(orduno): Service for getting the costmap sometimes fails,
+  //               check how the parent task can handle this. Might need to pull out
+  //               the getCostmap() method out of the constructor and have an initialize() one.
+  try {
+    getCostmap(costmap_);
+  } catch (...) {
+    RCLCPP_ERROR(this->get_logger(),
+      "DijkstraPlanner::makePlan: failed to obtain costmap from server");
+    throw std::runtime_error("makePlan: failed to obtain costmap from server");
+  }
+
+  printCostmap(costmap_);
+
+  planner_ = std::make_shared<NavFn>(costmap_.metadata.size_x, costmap_.metadata.size_y);
+
+  // Plan publisher for visualization purposes
+  plan_publisher_ = this->create_publisher<nav2_planning_msgs::msg::Path>("plan", 1);
+}
+
+DijkstraPlanner::~DijkstraPlanner()
+{
+  RCLCPP_INFO(get_logger(), "DijkstraPlanner::~DijkstraPlanner");
+}
+
+TaskStatus
+DijkstraPlanner::execute(const nav2_tasks::ComputePathToPoseCommand::SharedPtr command)
+{
+  RCLCPP_INFO(get_logger(), "DijkstraPlanner::execute");
+
+  nav2_tasks::ComputePathToPoseResult result;
+  try {
+    // TODO(orduno): Get an updated costmap
+    // getCostmap(costmap_);
+    makePlan(command->start, command->goal, command->tolerance, result);
+  } catch (...) {
+    RCLCPP_WARN(this->get_logger(), "DijkstraPlanner::execute: plan calculation failed");
+    // TODO(orduno): provide information about fail error to parent task,
+    //               for example: couldn't get costmap update
+    return TaskStatus::FAILED;
+  }
+
+  if (cancelRequested()) {
+    RCLCPP_INFO(get_logger(), "DijkstraPlanner::execute: task has been canceled");
+    setCanceled();
+    return TaskStatus::CANCELED;
+  }
+  // TODO(orduno): should check for cancel within the makePlan() method?
+
+  RCLCPP_INFO(get_logger(),
+    "DijkstraPlanner::execute: calculated path of size %u", result.poses.size());
+
+  // We've successfully completed the task, so return the result
+  RCLCPP_INFO(get_logger(), "DijkstraPlanner::execute: task completed");
+
+  // Pass the output to the Task Client
+  setResult(result);
+
+  // Publish the plan for visualization purposes
+  plan_publisher_->publish(result);
+
+  // TODO(orduno): Enable potential visualization
+
+  return TaskStatus::SUCCEEDED;
+}
+
+bool
+DijkstraPlanner::makePlan(
+  const geometry_msgs::msg::Pose & start,
+  const geometry_msgs::msg::Pose & goal, double tolerance,
+  nav2_planning_msgs::msg::Path & plan)
+{
+  // clear the plan, just in case
+  plan.poses.clear();
+
+  // TODO(orduno): add checks for start and goal reference frame -- should be in gobal frame
+
+  double wx = start.position.x;
+  double wy = start.position.y;
+
+  unsigned int mx, my;
+  if (!worldToMap(wx, wy, mx, my)) {
+    RCLCPP_WARN(
+      this->get_logger(),
+      "DijkstraPlanner::makePlan: The robot's start position is off the global costmap."
+      " Planning will always fail, are you sure the robot has been properly localized?");
+    return false;
+  }
+
+  // clear the starting cell within the costmap because we know it can't be an obstacle
+  clearRobotCell(mx, my);
+
+  // make sure to resize the underlying array that Navfn uses
+  planner_->setNavArr(costmap_.metadata.size_x, costmap_.metadata.size_y);
+
+  planner_->setCostmap(&costmap_.data[0], true, allow_unknown_);
+
+  int map_start[2];
+  map_start[0] = mx;
+  map_start[1] = my;
+
+  wx = goal.position.x;
+  wy = goal.position.y;
+
+  if (worldToMap(wx, wy, mx, my)) {
+    if (tolerance <= 0.0) {
+      std::cout << "tolerance: " << tolerance << std::endl;
+      RCLCPP_WARN(
+        this->get_logger(),
+        "DijkstraPlanner::makePlan: The goal sent to the planner is off the global costmap."
+        " Planning will always fail to this goal.");
+      return false;
+    }
+    mx = 0;
+    my = 0;
+  }
+
+  int map_goal[2];
+  map_goal[0] = mx;
+  map_goal[1] = my;
+
+  // TODO(orduno): Explain why we are providing 'map_goal' to setStart().
+  //               Same for setGoal, seems reversed. Computing backwards?
+
+  planner_->setStart(map_goal);
+  planner_->setGoal(map_start);
+  planner_->calcNavFnDijkstra(true);
+
+  double resolution = costmap_.metadata.resolution;
+  geometry_msgs::msg::Pose p, best_pose;
+  p = goal;
+
+  bool found_legal = false;
+  double best_sdist = std::numeric_limits<double>::max();
+
+  p.position.y = goal.position.y - tolerance;
+
+  while (p.position.y <= goal.position.y + tolerance) {
+    p.position.x = goal.position.x - tolerance;
+    while (p.position.x <= goal.position.x + tolerance) {
+      double potential = getPointPotential(p.position);
+      double sdist = squared_distance(p, goal);
+      if (potential < POT_HIGH && sdist < best_sdist) {
+        best_sdist = sdist;
+        best_pose = p;
+        found_legal = true;
+      }
+      p.position.x += resolution;
+    }
+    p.position.y += resolution;
+  }
+
+  if (found_legal) {
+    // extract the plan
+    if (getPlanFromPotential(best_pose, plan)) {
+      geometry_msgs::msg::Pose goal_copy = best_pose;
+      plan.poses.push_back(goal_copy);
+    } else {
+      RCLCPP_ERROR(
+        this->get_logger(),
+        "DijkstraPlanner::makePlan: Failed to get a plan from potential when a legal"
+        " potential was found. This shouldn't happen.");
+    }
+  }
+
+  return !plan.poses.empty();
+}
+
+bool
+DijkstraPlanner::computePotential(const geometry_msgs::msg::Point & world_point)
+{
+  // make sure to resize the underlying array that Navfn uses
+  planner_->setNavArr(costmap_.metadata.size_x, costmap_.metadata.size_y);
+
+  std::vector<unsigned char> costmapData = std::vector<unsigned char>(
+    costmap_.data.begin(), costmap_.data.end());
+
+  planner_->setCostmap(&costmapData[0], true, allow_unknown_);
+
+  unsigned int mx, my;
+  if (!worldToMap(world_point.x, world_point.y, mx, my)) {
+    return false;
+  }
+
+  int map_start[2];
+  map_start[0] = 0;
+  map_start[1] = 0;
+
+  int map_goal[2];
+  map_goal[0] = mx;
+  map_goal[1] = my;
+
+  planner_->setStart(map_start);
+  planner_->setGoal(map_goal);
+
+  return planner_->calcNavFnDijkstra();
+}
+
+bool
+DijkstraPlanner::getPlanFromPotential(
+  const geometry_msgs::msg::Pose & goal,
+  nav2_planning_msgs::msg::Path & plan)
+{
+  // clear the plan, just in case
+  plan.poses.clear();
+
+  // Goal should be in global frame
+  double wx = goal.position.x;
+  double wy = goal.position.y;
+
+  // the potential has already been computed, so we won't update our copy of the costmap
+  unsigned int mx, my;
+  if (!worldToMap(wx, wy, mx, my)) {
+    RCLCPP_WARN(
+      this->get_logger(),
+      "The goal sent to the navfn planner is off the global costmap."
+      " Planning will always fail to this goal.");
+    return false;
+  }
+
+  int map_goal[2];
+  map_goal[0] = mx;
+  map_goal[1] = my;
+
+  planner_->setStart(map_goal);
+
+  planner_->calcPath(costmap_.metadata.size_x * 4);
+
+  // extract the plan
+  float * x = planner_->getPathX();
+  float * y = planner_->getPathY();
+  int len = planner_->getPathLen();
+
+  plan.header.stamp = this->now();
+  plan.header.frame_id = global_frame_;
+
+  for (int i = len - 1; i >= 0; --i) {
+    // convert the plan to world coordinates
+    double world_x, world_y;
+    mapToWorld(x[i], y[i], world_x, world_y);
+
+    geometry_msgs::msg::Pose pose;
+    pose.position.x = world_x;
+    pose.position.y = world_y;
+    pose.position.z = 0.0;
+    pose.orientation.x = 0.0;
+    pose.orientation.y = 0.0;
+    pose.orientation.z = 0.0;
+    pose.orientation.w = 1.0;
+    plan.poses.push_back(pose);
+  }
+
+  return !plan.poses.empty();
+}
+
+double
+DijkstraPlanner::getPointPotential(const geometry_msgs::msg::Point & world_point)
+{
+  unsigned int mx, my;
+  if (!worldToMap(world_point.x, world_point.y, mx, my)) {
+    return std::numeric_limits<double>::max();
+  }
+
+  unsigned int index = my * planner_->nx + mx;
+  return planner_->potarr[index];
+}
+
+bool
+DijkstraPlanner::validPointPotential(const geometry_msgs::msg::Point & world_point)
+{
+  return validPointPotential(world_point, default_tolerance_);
+}
+
+bool
+DijkstraPlanner::validPointPotential(
+  const geometry_msgs::msg::Point & world_point, double tolerance)
+{
+  double resolution = costmap_.metadata.resolution;
+
+  geometry_msgs::msg::Point p;
+  p = world_point;
+
+  p.y = world_point.y - tolerance;
+
+  while (p.y <= world_point.y + tolerance) {
+    p.x = world_point.x - tolerance;
+    while (p.x <= world_point.x + tolerance) {
+      double potential = getPointPotential(p);
+      if (potential < POT_HIGH) {
+        return true;
+      }
+      p.x += resolution;
+    }
+    p.y += resolution;
+  }
+
+  return false;
+}
+
+bool
+DijkstraPlanner::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my)
+{
+  if (wx < costmap_.metadata.origin.position.x || wy < costmap_.metadata.origin.position.y) {
+    return false;
+  }
+
+  mx = static_cast<int>((wx - costmap_.metadata.origin.position.x) / costmap_.metadata.resolution);
+  my = static_cast<int>((wy - costmap_.metadata.origin.position.y) / costmap_.metadata.resolution);
+
+  if (mx < costmap_.metadata.size_x && my < costmap_.metadata.size_y) {
+    return true;
+  }
+
+  return false;
+}
+
+void
+DijkstraPlanner::mapToWorld(double mx, double my, double & wx, double & wy)
+{
+  wx = costmap_.metadata.origin.position.x + mx * costmap_.metadata.resolution;
+  wy = costmap_.metadata.origin.position.y + my * costmap_.metadata.resolution;
+}
+
+void
+DijkstraPlanner::clearRobotCell(unsigned int mx, unsigned int my)
+{
+  // TODO(orduno): check usage of this function, might instead be a request to
+  //               world_model / map server
+  unsigned int index = my * costmap_.metadata.size_x + mx;
+  costmap_.data[index] = nav2_util::Costmap::free_space;
+}
+
+void
+DijkstraPlanner::getCostmap(
+  nav2_libs_msgs::msg::Costmap & costmap, const std::string /*layer*/,
+  const std::chrono::milliseconds waitTime)
+{
+  RCLCPP_INFO(this->get_logger(), "DijkstraPlanner::getCostmap: requesting a new costmap");
+
+  // TODO(orduno): explicitly provide specifications for costmap using the costmap on the request,
+  //               including master (aggreate) layer
+  auto costmapServiceResult = costmap_client_->async_send_request(
+    std::make_shared<nav2_world_model_msgs::srv::GetCostmap::Request>());
+
+  if (rclcpp::spin_until_future_complete(
+      this->get_node_base_interface(), costmapServiceResult, waitTime) !=
+    rclcpp::executor::FutureReturnCode::SUCCESS)
+  {
+    RCLCPP_ERROR(this->get_logger(), "DijkstraPlanner::getCostmap: costmap service call failed");
+    throw std::runtime_error("getCostmap: service call failed");
+  }
+  costmap = costmapServiceResult.get()->map;
+}
+
+void
+DijkstraPlanner::waitForCostmapServer(const std::chrono::seconds waitTime)
+{
+  while (!costmap_client_->wait_for_service(waitTime)) {
+    if (!rclcpp::ok()) {
+      RCLCPP_ERROR(
+        this->get_logger(),
+        "DijkstraPlanner::waitForCostmapServer:"
+        " costmap client interrupted while waiting for the service to appear.");
+      throw std::runtime_error(
+              "waitForCostmapServer: interrupted while waiting for costmap server to appear");
+    }
+    RCLCPP_INFO(this->get_logger(),
+      "DijkstraPlanner::waitForCostmapServer: waiting for the costmap service to appear...")
+  }
+}
+
+void
+DijkstraPlanner::printCostmap(const nav2_libs_msgs::msg::Costmap & costmap)
+{
+  std::cout << "Costmap" << std::endl;
+  std::cout << "  size:       " <<
+    costmap.metadata.size_x << "," << costmap.metadata.size_x << std::endl;
+  std::cout << "  origin:     " <<
+    costmap.metadata.origin.position.x << "," << costmap.metadata.origin.position.y << std::endl;
+  std::cout << "  resolution: " << costmap.metadata.resolution << std::endl;
+  std::cout << "  data:       " <<
+    "(" << costmap.data.size() << " cells)" << std::endl << "    ";
+
+  const char separator = ' ';
+  const int valueWidth = 4;
+
+  unsigned int index = 0;
+  for (unsigned int h = 0; h < costmap.metadata.size_y; ++h) {
+    for (unsigned int w = 0; w < costmap.metadata.size_x; ++w) {
+      std::cout << std::left << std::setw(valueWidth) << std::setfill(separator) <<
+        static_cast<unsigned int>(costmap.data[index]);
+      index++;
+    }
+    std::cout << std::endl << "    ";
+  }
+  std::cout << std::endl;
+}
+
+}  // namespace nav2_dijkstra_planner
diff --git a/src/planning/nav2_dijkstra_planner/src/main.cpp b/src/planning/nav2_dijkstra_planner/src/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e06d89489fc90a8d4fa679e5ab2381c92a141981
--- /dev/null
+++ b/src/planning/nav2_dijkstra_planner/src/main.cpp
@@ -0,0 +1,26 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_dijkstra_planner/dijkstra_planner.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<nav2_dijkstra_planner::DijkstraPlanner>());
+  rclcpp::shutdown();
+
+  return 0;
+}
diff --git a/src/planning/nav2_dijkstra_planner/src/navfn.cpp b/src/planning/nav2_dijkstra_planner/src/navfn.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6cb0151430698f5251b9ae3cbc984b96734db75a
--- /dev/null
+++ b/src/planning/nav2_dijkstra_planner/src/navfn.cpp
@@ -0,0 +1,1034 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  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 Willow Garage 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.
+*********************************************************************/
+//
+// Navigation function computation
+// Uses Dijkstra's method
+// Modified for Euclidean-distance computation
+//
+// Path calculation uses no interpolation when pot field is at max in
+//   nearby cells
+//
+// Path calc has sanity check that it succeeded
+//
+
+#include <algorithm>
+#include "nav2_dijkstra_planner/navfn.hpp"
+
+// TODO(orduno): Define how to pass the node handle for ROS logging
+#include "Logger.h"
+
+//
+// function to perform nav fn calculation
+// keeps track of internal buffers, will be more efficient
+//   if the size of the environment does not change
+//
+
+int
+create_nav_plan_astar(
+  COSTTYPE * costmap, int nx, int ny,
+  int * goal, int * start,
+  float * plan, int nplan)
+{
+  static NavFn * nav = NULL;
+
+  if (nav == NULL) {
+    nav = new NavFn(nx, ny);
+  }
+
+  if (nav->nx != nx || nav->ny != ny) {  // check for compatibility with previous call
+    delete nav;
+    nav = new NavFn(nx, ny);
+  }
+
+  nav->setGoal(goal);
+  nav->setStart(start);
+
+  nav->costarr = costmap;
+  nav->setupNavFn(true);
+
+  // calculate the nav fn and path
+  nav->priInc = 2 * COST_NEUTRAL;
+  nav->propNavFnAstar(std::max(nx * ny / 20, nx + ny));
+
+  // path
+  int len = nav->calcPath(nplan);
+
+  if (len > 0) {  // found plan
+    ROS_DEBUG("[NavFn] Path found, %d steps\n", len);
+  } else {
+    ROS_DEBUG("[NavFn] No path found\n");
+  }
+
+  if (len > 0) {
+    for (int i = 0; i < len; i++) {
+      plan[i * 2] = nav->pathx[i];
+      plan[i * 2 + 1] = nav->pathy[i];
+    }
+  }
+
+  return len;
+}
+
+
+//
+// create nav fn buffers
+//
+
+NavFn::NavFn(int xs, int ys)
+{
+  // create cell arrays
+  costarr = NULL;
+  potarr = NULL;
+  pending = NULL;
+  gradx = grady = NULL;
+  setNavArr(xs, ys);
+
+  // priority buffers
+  pb1 = new int[PRIORITYBUFSIZE];
+  pb2 = new int[PRIORITYBUFSIZE];
+  pb3 = new int[PRIORITYBUFSIZE];
+
+  // for Dijkstra (breadth-first), set to COST_NEUTRAL
+  // for A* (best-first), set to COST_NEUTRAL
+  priInc = 2 * COST_NEUTRAL;
+
+  // goal and start
+  goal[0] = goal[1] = 0;
+  start[0] = start[1] = 0;
+
+  // display function
+  displayFn = NULL;
+  displayInt = 0;
+
+  // path buffers
+  npathbuf = npath = 0;
+  pathx = pathy = NULL;
+  pathStep = 0.5;
+}
+
+
+NavFn::~NavFn()
+{
+  if (costarr) {
+    delete[] costarr;
+  }
+  if (potarr) {
+    delete[] potarr;
+  }
+  if (pending) {
+    delete[] pending;
+  }
+  if (gradx) {
+    delete[] gradx;
+  }
+  if (grady) {
+    delete[] grady;
+  }
+  if (pathx) {
+    delete[] pathx;
+  }
+  if (pathy) {
+    delete[] pathy;
+  }
+  if (pb1) {
+    delete[] pb1;
+  }
+  if (pb2) {
+    delete[] pb2;
+  }
+  if (pb3) {
+    delete[] pb3;
+  }
+}
+
+
+//
+// set goal, start positions for the nav fn
+//
+
+void
+NavFn::setGoal(int * g)
+{
+  goal[0] = g[0];
+  goal[1] = g[1];
+  ROS_DEBUG("[NavFn] Setting goal to %d,%d\n", goal[0], goal[1]);
+}
+
+void
+NavFn::setStart(int * g)
+{
+  start[0] = g[0];
+  start[1] = g[1];
+  ROS_DEBUG("[NavFn] Setting start to %d,%d\n", start[0], start[1]);
+}
+
+//
+// Set/Reset map size
+//
+
+void
+NavFn::setNavArr(int xs, int ys)
+{
+  ROS_DEBUG("[NavFn] Array is %d x %d\n", xs, ys);
+
+  nx = xs;
+  ny = ys;
+  ns = nx * ny;
+
+  if (costarr) {
+    delete[] costarr;
+  }
+  if (potarr) {
+    delete[] potarr;
+  }
+  if (pending) {
+    delete[] pending;
+  }
+
+  if (gradx) {
+    delete[] gradx;
+  }
+  if (grady) {
+    delete[] grady;
+  }
+
+  costarr = new COSTTYPE[ns];  // cost array, 2d config space
+  memset(costarr, 0, ns * sizeof(COSTTYPE));
+  potarr = new float[ns];  // navigation potential array
+  pending = new bool[ns];
+  memset(pending, 0, ns * sizeof(bool));
+  gradx = new float[ns];
+  grady = new float[ns];
+}
+
+
+//
+// set up cost array, usually from ROS
+//
+
+void
+NavFn::setCostmap(const COSTTYPE * cmap, bool isROS, bool allow_unknown)
+{
+  COSTTYPE * cm = costarr;
+  if (isROS) {  // ROS-type cost array
+    for (int i = 0; i < ny; i++) {
+      int k = i * nx;
+      for (int j = 0; j < nx; j++, k++, cmap++, cm++) {
+        // This transforms the incoming cost values:
+        // COST_OBS                 -> COST_OBS (incoming "lethal obstacle")
+        // COST_OBS_ROS             -> COST_OBS (incoming "inscribed inflated obstacle")
+        // values in range 0 to 252 -> values from COST_NEUTRAL to COST_OBS_ROS.
+        *cm = COST_OBS;
+        int v = *cmap;
+        if (v < COST_OBS_ROS) {
+          v = COST_NEUTRAL + COST_FACTOR * v;
+          if (v >= COST_OBS) {
+            v = COST_OBS - 1;
+          }
+          *cm = v;
+        } else if (v == COST_UNKNOWN_ROS && allow_unknown) {
+          v = COST_OBS - 1;
+          *cm = v;
+        }
+      }
+    }
+  } else {  // not a ROS map, just a PGM
+    for (int i = 0; i < ny; i++) {
+      int k = i * nx;
+      for (int j = 0; j < nx; j++, k++, cmap++, cm++) {
+        *cm = COST_OBS;
+        if (i < 7 || i > ny - 8 || j < 7 || j > nx - 8) {
+          continue;  // don't do borders
+        }
+        int v = *cmap;
+        if (v < COST_OBS_ROS) {
+          v = COST_NEUTRAL + COST_FACTOR * v;
+          if (v >= COST_OBS) {
+            v = COST_OBS - 1;
+          }
+          *cm = v;
+        } else if (v == COST_UNKNOWN_ROS) {
+          v = COST_OBS - 1;
+          *cm = v;
+        }
+      }
+    }
+  }
+}
+
+bool
+NavFn::calcNavFnDijkstra(bool atStart)
+{
+  setupNavFn(true);
+
+  // calculate the nav fn and path
+  propNavFnDijkstra(std::max(nx * ny / 20, nx + ny), atStart);
+
+  // path
+  int len = calcPath(nx * ny / 2);
+
+  if (len > 0) {  // found plan
+    ROS_DEBUG("[NavFn] Path found, %d steps\n", len);
+    return true;
+  } else {
+    ROS_DEBUG("[NavFn] No path found\n");
+    return false;
+  }
+}
+
+
+//
+// calculate navigation function, given a costmap, goal, and start
+//
+
+bool
+NavFn::calcNavFnAstar()
+{
+  setupNavFn(true);
+
+  // calculate the nav fn and path
+  propNavFnAstar(std::max(nx * ny / 20, nx + ny));
+
+  // path
+  int len = calcPath(nx * 4);
+
+  if (len > 0) {  // found plan
+    ROS_DEBUG("[NavFn] Path found, %d steps\n", len);
+    return true;
+  } else {
+    ROS_DEBUG("[NavFn] No path found\n");
+    return false;
+  }
+}
+
+//
+// returning values
+//
+
+float * NavFn::getPathX() {return pathx;}
+float * NavFn::getPathY() {return pathy;}
+int NavFn::getPathLen() {return npath;}
+
+// inserting onto the priority blocks
+#define push_cur(n)  {if (n >= 0 && n < ns && !pending[n] && \
+      costarr[n] < COST_OBS && curPe < PRIORITYBUFSIZE) \
+    {curP[curPe++] = n; pending[n] = true;}}
+#define push_next(n) {if (n >= 0 && n < ns && !pending[n] && \
+      costarr[n] < COST_OBS && nextPe < PRIORITYBUFSIZE) \
+    {nextP[nextPe++] = n; pending[n] = true;}}
+#define push_over(n) {if (n >= 0 && n < ns && !pending[n] && \
+      costarr[n] < COST_OBS && overPe < PRIORITYBUFSIZE) \
+    {overP[overPe++] = n; pending[n] = true;}}
+
+
+// Set up navigation potential arrays for new propagation
+
+void
+NavFn::setupNavFn(bool keepit)
+{
+  // reset values in propagation arrays
+  for (int i = 0; i < ns; i++) {
+    potarr[i] = POT_HIGH;
+    if (!keepit) {
+      costarr[i] = COST_NEUTRAL;
+    }
+    gradx[i] = grady[i] = 0.0;
+  }
+
+  // outer bounds of cost array
+  COSTTYPE * pc;
+  pc = costarr;
+  for (int i = 0; i < nx; i++) {
+    *pc++ = COST_OBS;
+  }
+  pc = costarr + (ny - 1) * nx;
+  for (int i = 0; i < nx; i++) {
+    *pc++ = COST_OBS;
+  }
+  pc = costarr;
+  for (int i = 0; i < ny; i++, pc += nx) {
+    *pc = COST_OBS;
+  }
+  pc = costarr + nx - 1;
+  for (int i = 0; i < ny; i++, pc += nx) {
+    *pc = COST_OBS;
+  }
+
+  // priority buffers
+  curT = COST_OBS;
+  curP = pb1;
+  curPe = 0;
+  nextP = pb2;
+  nextPe = 0;
+  overP = pb3;
+  overPe = 0;
+  memset(pending, 0, ns * sizeof(bool));
+
+  // set goal
+  int k = goal[0] + goal[1] * nx;
+  initCost(k, 0);
+
+  // find # of obstacle cells
+  pc = costarr;
+  int ntot = 0;
+  for (int i = 0; i < ns; i++, pc++) {
+    if (*pc >= COST_OBS) {
+      ntot++;  // number of cells that are obstacles
+    }
+  }
+  nobs = ntot;
+}
+
+
+// initialize a goal-type cost for starting propagation
+
+void
+NavFn::initCost(int k, float v)
+{
+  potarr[k] = v;
+  push_cur(k + 1);
+  push_cur(k - 1);
+  push_cur(k - nx);
+  push_cur(k + nx);
+}
+
+
+//
+// Critical function: calculate updated potential value of a cell,
+//   given its neighbors' values
+// Planar-wave update calculation from two lowest neighbors in a 4-grid
+// Quadratic approximation to the interpolated value
+// No checking of bounds here, this function should be fast
+//
+
+#define INVSQRT2 0.707106781
+
+inline void
+NavFn::updateCell(int n)
+{
+  // get neighbors
+  float u, d, l, r;
+  l = potarr[n - 1];
+  r = potarr[n + 1];
+  u = potarr[n - nx];
+  d = potarr[n + nx];
+  // ROS_INFO("[Update] c: %0.1f  l: %0.1f  r: %0.1f  u: %0.1f  d: %0.1f\n",
+  //  potarr[n], l, r, u, d);
+  // ROS_INFO("[Update] cost: %d\n", costarr[n]);
+
+  // find lowest, and its lowest neighbor
+  float ta, tc;
+  if (l < r) {tc = l;} else {tc = r;}
+  if (u < d) {ta = u;} else {ta = d;}
+
+  // do planar wave update
+  if (costarr[n] < COST_OBS) {  // don't propagate into obstacles
+    float hf = static_cast<float>(costarr[n]);  // traversability factor
+    float dc = tc - ta;  // relative cost between ta,tc
+    if (dc < 0) {  // ta is lowest
+      dc = -dc;
+      ta = tc;
+    }
+
+    // calculate new potential
+    float pot;
+    if (dc >= hf) {  // if too large, use ta-only update
+      pot = ta + hf;
+    } else {  // two-neighbor interpolation update
+      // use quadratic approximation
+      // might speed this up through table lookup, but still have to
+      //   do the divide
+      float d = dc / hf;
+      float v = -0.2301 * d * d + 0.5307 * d + 0.7040;
+      pot = ta + hf * v;
+    }
+
+    //      ROS_INFO("[Update] new pot: %d\n", costarr[n]);
+
+    // now add affected neighbors to priority blocks
+    if (pot < potarr[n]) {
+      float le = INVSQRT2 * static_cast<float>(costarr[n - 1]);
+      float re = INVSQRT2 * static_cast<float>(costarr[n + 1]);
+      float ue = INVSQRT2 * static_cast<float>(costarr[n - nx]);
+      float de = INVSQRT2 * static_cast<float>(costarr[n + nx]);
+      potarr[n] = pot;
+      if (pot < curT) {  // low-cost buffer block
+        if (l > pot + le) {push_next(n - 1);}
+        if (r > pot + re) {push_next(n + 1);}
+        if (u > pot + ue) {push_next(n - nx);}
+        if (d > pot + de) {push_next(n + nx);}
+      } else {  // overflow block
+        if (l > pot + le) {push_over(n - 1);}
+        if (r > pot + re) {push_over(n + 1);}
+        if (u > pot + ue) {push_over(n - nx);}
+        if (d > pot + de) {push_over(n + nx);}
+      }
+    }
+  }
+}
+
+//
+// Use A* method for setting priorities
+// Critical function: calculate updated potential value of a cell,
+//   given its neighbors' values
+// Planar-wave update calculation from two lowest neighbors in a 4-grid
+// Quadratic approximation to the interpolated value
+// No checking of bounds here, this function should be fast
+//
+
+#define INVSQRT2 0.707106781
+
+inline void
+NavFn::updateCellAstar(int n)
+{
+  // get neighbors
+  float u, d, l, r;
+  l = potarr[n - 1];
+  r = potarr[n + 1];
+  u = potarr[n - nx];
+  d = potarr[n + nx];
+  // ROS_INFO("[Update] c: %0.1f  l: %0.1f  r: %0.1f  u: %0.1f  d: %0.1f\n",
+  // potarr[n], l, r, u, d);
+  // ROS_INFO("[Update] cost of %d: %d\n", n, costarr[n]);
+
+  // find lowest, and its lowest neighbor
+  float ta, tc;
+  if (l < r) {tc = l;} else {tc = r;}
+  if (u < d) {ta = u;} else {ta = d;}
+
+  // do planar wave update
+  if (costarr[n] < COST_OBS) {  // don't propagate into obstacles
+    float hf = static_cast<float>(costarr[n]);  // traversability factor
+    float dc = tc - ta;  // relative cost between ta,tc
+    if (dc < 0) {  // ta is lowest
+      dc = -dc;
+      ta = tc;
+    }
+
+    // calculate new potential
+    float pot;
+    if (dc >= hf) {  // if too large, use ta-only update
+      pot = ta + hf;
+    } else {  // two-neighbor interpolation update
+      // use quadratic approximation
+      // might speed this up through table lookup, but still have to
+      //   do the divide
+      float d = dc / hf;
+      float v = -0.2301 * d * d + 0.5307 * d + 0.7040;
+      pot = ta + hf * v;
+    }
+
+    // ROS_INFO("[Update] new pot: %d\n", costarr[n]);
+
+    // now add affected neighbors to priority blocks
+    if (pot < potarr[n]) {
+      float le = INVSQRT2 * static_cast<float>(costarr[n - 1]);
+      float re = INVSQRT2 * static_cast<float>(costarr[n + 1]);
+      float ue = INVSQRT2 * static_cast<float>(costarr[n - nx]);
+      float de = INVSQRT2 * static_cast<float>(costarr[n + nx]);
+
+      // calculate distance
+      int x = n % nx;
+      int y = n / nx;
+      float dist = hypot(x - start[0], y - start[1]) * static_cast<float>(COST_NEUTRAL);
+
+      potarr[n] = pot;
+      pot += dist;
+      if (pot < curT) {  // low-cost buffer block
+        if (l > pot + le) {push_next(n - 1);}
+        if (r > pot + re) {push_next(n + 1);}
+        if (u > pot + ue) {push_next(n - nx);}
+        if (d > pot + de) {push_next(n + nx);}
+      } else {
+        if (l > pot + le) {push_over(n - 1);}
+        if (r > pot + re) {push_over(n + 1);}
+        if (u > pot + ue) {push_over(n - nx);}
+        if (d > pot + de) {push_over(n + nx);}
+      }
+    }
+  }
+}
+
+
+//
+// main propagation function
+// Dijkstra method, breadth-first
+// runs for a specified number of cycles,
+//   or until it runs out of cells to update,
+//   or until the Start cell is found (atStart = true)
+//
+
+bool
+NavFn::propNavFnDijkstra(int cycles, bool atStart)
+{
+  int nwv = 0;  // max priority block size
+  int nc = 0;  // number of cells put into priority blocks
+  int cycle = 0;  // which cycle we're on
+
+  // set up start cell
+  int startCell = start[1] * nx + start[0];
+
+  for (; cycle < cycles; cycle++) {  // go for this many cycles, unless interrupted
+    if (curPe == 0 && nextPe == 0) {  // priority blocks empty
+      break;
+    }
+
+    // stats
+    nc += curPe;
+    if (curPe > nwv) {
+      nwv = curPe;
+    }
+
+    // reset pending flags on current priority buffer
+    int * pb = curP;
+    int i = curPe;
+    while (i-- > 0) {
+      pending[*(pb++)] = false;
+    }
+
+    // process current priority buffer
+    pb = curP;
+    i = curPe;
+    while (i-- > 0) {
+      updateCell(*pb++);
+    }
+
+    if (displayInt > 0 && (cycle % displayInt) == 0) {
+      displayFn(this);
+    }
+
+    // swap priority blocks curP <=> nextP
+    curPe = nextPe;
+    nextPe = 0;
+    pb = curP;  // swap buffers
+    curP = nextP;
+    nextP = pb;
+
+    // see if we're done with this priority level
+    if (curPe == 0) {
+      curT += priInc;  // increment priority threshold
+      curPe = overPe;  // set current to overflow block
+      overPe = 0;
+      pb = curP;  // swap buffers
+      curP = overP;
+      overP = pb;
+    }
+
+    // check if we've hit the Start cell
+    if (atStart) {
+      if (potarr[startCell] < POT_HIGH) {
+        break;
+      }
+    }
+  }
+
+  ROS_DEBUG("[NavFn] Used %d cycles, %d cells visited (%d%%), priority buf max %d\n",
+    cycle, nc, (int)((nc * 100.0) / (ns - nobs)), nwv);
+
+  if (cycle < cycles) {
+    return true;  // finished up here
+  } else {return false;}
+}
+
+
+//
+// main propagation function
+// A* method, best-first
+// uses Euclidean distance heuristic
+// runs for a specified number of cycles,
+//   or until it runs out of cells to update,
+//   or until the Start cell is found (atStart = true)
+//
+
+bool
+NavFn::propNavFnAstar(int cycles)
+{
+  int nwv = 0;  // max priority block size
+  int nc = 0;  // number of cells put into priority blocks
+  int cycle = 0;  // which cycle we're on
+
+  // set initial threshold, based on distance
+  float dist = hypot(goal[0] - start[0], goal[1] - start[1]) * static_cast<float>(COST_NEUTRAL);
+  curT = dist + curT;
+
+  // set up start cell
+  int startCell = start[1] * nx + start[0];
+
+  // do main cycle
+  for (; cycle < cycles; cycle++) {  // go for this many cycles, unless interrupted
+    if (curPe == 0 && nextPe == 0) {  // priority blocks empty
+      break;
+    }
+
+    // stats
+    nc += curPe;
+    if (curPe > nwv) {
+      nwv = curPe;
+    }
+
+    // reset pending flags on current priority buffer
+    int * pb = curP;
+    int i = curPe;
+    while (i-- > 0) {
+      pending[*(pb++)] = false;
+    }
+
+    // process current priority buffer
+    pb = curP;
+    i = curPe;
+    while (i-- > 0) {
+      updateCellAstar(*pb++);
+    }
+
+    if (displayInt > 0 && (cycle % displayInt) == 0) {
+      displayFn(this);
+    }
+
+    // swap priority blocks curP <=> nextP
+    curPe = nextPe;
+    nextPe = 0;
+    pb = curP;  // swap buffers
+    curP = nextP;
+    nextP = pb;
+
+    // see if we're done with this priority level
+    if (curPe == 0) {
+      curT += priInc;  // increment priority threshold
+      curPe = overPe;  // set current to overflow block
+      overPe = 0;
+      pb = curP;  // swap buffers
+      curP = overP;
+      overP = pb;
+    }
+
+    // check if we've hit the Start cell
+    if (potarr[startCell] < POT_HIGH) {
+      break;
+    }
+  }
+
+  last_path_cost_ = potarr[startCell];
+
+  ROS_DEBUG("[NavFn] Used %d cycles, %d cells visited (%d%%), priority buf max %d\n",
+    cycle, nc, (int)((nc * 100.0) / (ns - nobs)), nwv);
+
+  if (potarr[startCell] < POT_HIGH) {
+    return true;  // finished up here}
+  } else {
+    return false;
+  }
+}
+
+
+float NavFn::getLastPathCost()
+{
+  return last_path_cost_;
+}
+
+
+//
+// Path construction
+// Find gradient at array points, interpolate path
+// Use step size of pathStep, usually 0.5 pixel
+//
+// Some sanity checks:
+//  1. Stuck at same index position
+//  2. Doesn't get near goal
+//  3. Surrounded by high potentials
+//
+
+int
+NavFn::calcPath(int n, int * st)
+{
+  // test write
+  // savemap("test");
+
+  // check path arrays
+  if (npathbuf < n) {
+    if (pathx) {delete[] pathx;}
+    if (pathy) {delete[] pathy;}
+    pathx = new float[n];
+    pathy = new float[n];
+    npathbuf = n;
+  }
+
+  // set up start position at cell
+  // st is always upper left corner for 4-point bilinear interpolation
+  if (st == NULL) {st = start;}
+  int stc = st[1] * nx + st[0];
+
+  // set up offset
+  float dx = 0;
+  float dy = 0;
+  npath = 0;
+
+  // go for <n> cycles at most
+  for (int i = 0; i < n; i++) {
+    // check if near goal
+    int nearest_point = std::max(0,
+        std::min(nx * ny - 1, stc + static_cast<int>(round(dx)) +
+        static_cast<int>(nx * round(dy))));
+    if (potarr[nearest_point] < COST_NEUTRAL) {
+      pathx[npath] = static_cast<float>(goal[0]);
+      pathy[npath] = static_cast<float>(goal[1]);
+      return ++npath;  // done!
+    }
+
+    if (stc < nx || stc > ns - nx) {  // would be out of bounds
+      ROS_DEBUG("[PathCalc] Out of bounds");
+      return 0;
+    }
+
+    // add to path
+    pathx[npath] = stc % nx + dx;
+    pathy[npath] = stc / nx + dy;
+    npath++;
+
+    bool oscillation_detected = false;
+    if (npath > 2 &&
+      pathx[npath - 1] == pathx[npath - 3] &&
+      pathy[npath - 1] == pathy[npath - 3])
+    {
+      ROS_DEBUG("[PathCalc] oscillation detected, attempting fix.");
+      oscillation_detected = true;
+    }
+
+    int stcnx = stc + nx;
+    int stcpx = stc - nx;
+
+    // check for potentials at eight positions near cell
+    if (potarr[stc] >= POT_HIGH ||
+      potarr[stc + 1] >= POT_HIGH ||
+      potarr[stc - 1] >= POT_HIGH ||
+      potarr[stcnx] >= POT_HIGH ||
+      potarr[stcnx + 1] >= POT_HIGH ||
+      potarr[stcnx - 1] >= POT_HIGH ||
+      potarr[stcpx] >= POT_HIGH ||
+      potarr[stcpx + 1] >= POT_HIGH ||
+      potarr[stcpx - 1] >= POT_HIGH ||
+      oscillation_detected)
+    {
+      ROS_DEBUG("[Path] Pot fn boundary, following grid (%0.1f/%d)", potarr[stc], npath);
+      // check eight neighbors to find the lowest
+      int minc = stc;
+      int minp = potarr[stc];
+      int st = stcpx - 1;
+      if (potarr[st] < minp) {minp = potarr[st]; minc = st;}
+      st++;
+      if (potarr[st] < minp) {minp = potarr[st]; minc = st;}
+      st++;
+      if (potarr[st] < minp) {minp = potarr[st]; minc = st;}
+      st = stc - 1;
+      if (potarr[st] < minp) {minp = potarr[st]; minc = st;}
+      st = stc + 1;
+      if (potarr[st] < minp) {minp = potarr[st]; minc = st;}
+      st = stcnx - 1;
+      if (potarr[st] < minp) {minp = potarr[st]; minc = st;}
+      st++;
+      if (potarr[st] < minp) {minp = potarr[st]; minc = st;}
+      st++;
+      if (potarr[st] < minp) {minp = potarr[st]; minc = st;}
+      stc = minc;
+      dx = 0;
+      dy = 0;
+
+      ROS_DEBUG("[Path] Pot: %0.1f  pos: %0.1f,%0.1f",
+        potarr[stc], pathx[npath - 1], pathy[npath - 1]);
+
+      if (potarr[stc] >= POT_HIGH) {
+        ROS_DEBUG("[PathCalc] No path found, high potential");
+        // savemap("navfn_highpot");
+        return 0;
+      }
+    } else {  // have a good gradient here
+      // get grad at four positions near cell
+      gradCell(stc);
+      gradCell(stc + 1);
+      gradCell(stcnx);
+      gradCell(stcnx + 1);
+
+
+      // get interpolated gradient
+      float x1 = (1.0 - dx) * gradx[stc] + dx * gradx[stc + 1];
+      float x2 = (1.0 - dx) * gradx[stcnx] + dx * gradx[stcnx + 1];
+      float x = (1.0 - dy) * x1 + dy * x2;  // interpolated x
+      float y1 = (1.0 - dx) * grady[stc] + dx * grady[stc + 1];
+      float y2 = (1.0 - dx) * grady[stcnx] + dx * grady[stcnx + 1];
+      float y = (1.0 - dy) * y1 + dy * y2;  // interpolated y
+
+      // show gradients
+      ROS_DEBUG("[Path] %0.2f,%0.2f  %0.2f,%0.2f  %0.2f,%0.2f  %0.2f,%0.2f; final x=%.3f, y=%.3f\n",
+        gradx[stc], grady[stc], gradx[stc + 1], grady[stc + 1],
+        gradx[stcnx], grady[stcnx], gradx[stcnx + 1], grady[stcnx + 1],
+        x, y);
+
+      // check for zero gradient, failed
+      if (x == 0.0 && y == 0.0) {
+        ROS_DEBUG("[PathCalc] Zero gradient");
+        return 0;
+      }
+
+      // move in the right direction
+      float ss = pathStep / hypot(x, y);
+      dx += x * ss;
+      dy += y * ss;
+
+      // check for overflow
+      if (dx > 1.0) {stc++; dx -= 1.0;}
+      if (dx < -1.0) {stc--; dx += 1.0;}
+      if (dy > 1.0) {stc += nx; dy -= 1.0;}
+      if (dy < -1.0) {stc -= nx; dy += 1.0;}
+    }
+
+    //      ROS_INFO("[Path] Pot: %0.1f  grad: %0.1f,%0.1f  pos: %0.1f,%0.1f\n",
+    //      potarr[stc], x, y, pathx[npath-1], pathy[npath-1]);
+  }
+
+  //  return npath;  // out of cycles, return failure
+  ROS_DEBUG("[PathCalc] No path found, path too long");
+  // savemap("navfn_pathlong");
+  return 0;  // out of cycles, return failure
+}
+
+
+//
+// gradient calculations
+//
+
+// calculate gradient at a cell
+// positive value are to the right and down
+float
+NavFn::gradCell(int n)
+{
+  if (gradx[n] + grady[n] > 0.0) {  // check this cell
+    return 1.0;
+  }
+
+  if (n < nx || n > ns - nx) {  // would be out of bounds
+    return 0.0;
+  }
+
+  float cv = potarr[n];
+  float dx = 0.0;
+  float dy = 0.0;
+
+  // check for in an obstacle
+  if (cv >= POT_HIGH) {
+    if (potarr[n - 1] < POT_HIGH) {
+      dx = -COST_OBS;
+    } else if (potarr[n + 1] < POT_HIGH) {
+      dx = COST_OBS;
+    }
+    if (potarr[n - nx] < POT_HIGH) {
+      dy = -COST_OBS;
+    } else if (potarr[n + nx] < POT_HIGH) {
+      dy = COST_OBS;
+    }
+  } else {  // not in an obstacle
+    // dx calc, average to sides
+    if (potarr[n - 1] < POT_HIGH) {
+      dx += potarr[n - 1] - cv;
+    }
+    if (potarr[n + 1] < POT_HIGH) {
+      dx += cv - potarr[n + 1];
+    }
+
+    // dy calc, average to sides
+    if (potarr[n - nx] < POT_HIGH) {
+      dy += potarr[n - nx] - cv;
+    }
+    if (potarr[n + nx] < POT_HIGH) {
+      dy += cv - potarr[n + nx];
+    }
+  }
+
+  // normalize
+  float norm = hypot(dx, dy);
+  if (norm > 0) {
+    norm = 1.0 / norm;
+    gradx[n] = norm * dx;
+    grady[n] = norm * dy;
+  }
+  return norm;
+}
+
+
+//
+// display function setup
+// <n> is the number of cycles to wait before displaying,
+//     use 0 to turn it off
+
+void
+NavFn::display(void fn(NavFn * nav), int n)
+{
+  displayFn = fn;
+  displayInt = n;
+}
+
+
+//
+// debug writes
+// saves costmap and start/goal
+//
+
+void
+NavFn::savemap(const char * fname)
+{
+  char fn[4096];
+
+  ROS_DEBUG("[NavFn] Saving costmap and start/goal points");
+  // write start and goal points
+  snprintf(fn, sizeof(fn), "%s.txt", fname);
+  FILE * fp = fopen(fn, "w");
+  if (!fp) {
+    ROS_WARN("Can't open file %s", fn);
+    return;
+  }
+  fprintf(fp, "Goal: %d %d\nStart: %d %d\n", goal[0], goal[1], start[0], start[1]);
+  fclose(fp);
+
+  // write cost array
+  if (!costarr) {
+    return;
+  }
+  snprintf(fn, sizeof(fn), "%s.pgm", fname);
+  fp = fopen(fn, "wb");
+  if (!fp) {
+    ROS_WARN("Can't open file %s", fn);
+    return;
+  }
+  fprintf(fp, "P5\n%d\n%d\n%d\n", nx, ny, 0xff);
+  fwrite(costarr, 1, nx * ny, fp);
+  fclose(fp);
+}
diff --git a/src/planning/nav2_dijkstra_planner/test/.gitignore b/src/planning/nav2_dijkstra_planner/test/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/planning/nav2_planning_msgs/CHANGELOG.rst b/src/planning/nav2_planning_msgs/CHANGELOG.rst
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/planning/nav2_planning_msgs/CMakeLists.txt b/src/planning/nav2_planning_msgs/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..12d36e1115ecb28c4710abe19b8a985bd592a159
--- /dev/null
+++ b/src/planning/nav2_planning_msgs/CMakeLists.txt
@@ -0,0 +1,41 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_planning_msgs)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/Path.msg"
+  "msg/PathEndPoints.msg"
+  DEPENDENCIES builtin_interfaces geometry_msgs std_msgs
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_cppcheck REQUIRED)
+  find_package(ament_cmake_cpplint REQUIRED)
+  find_package(ament_cmake_lint_cmake REQUIRED)
+  find_package(ament_cmake_uncrustify REQUIRED)
+  # This forces cppcheck to consider all files in this project to be C++,
+  # including the headers which end with .h, which cppcheck would normally
+  # consider to be C instead.
+  ament_cppcheck(LANGUAGE "c++")
+  ament_cpplint()
+  ament_lint_cmake()
+  ament_uncrustify()
+endif()
+
+ament_export_dependencies(rosidl_default_runtime)
+
+ament_package()
diff --git a/src/planning/nav2_planning_msgs/README.md b/src/planning/nav2_planning_msgs/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/planning/nav2_planning_msgs/msg/Path.msg b/src/planning/nav2_planning_msgs/msg/Path.msg
new file mode 100644
index 0000000000000000000000000000000000000000..ce8ca16e7970272ae7b4a6f541ecebd01c1c2d54
--- /dev/null
+++ b/src/planning/nav2_planning_msgs/msg/Path.msg
@@ -0,0 +1,4 @@
+std_msgs/Header header
+
+# An array of poses that represents a Path for a robot to follow
+geometry_msgs/Pose[] poses
diff --git a/src/planning/nav2_planning_msgs/msg/PathEndPoints.msg b/src/planning/nav2_planning_msgs/msg/PathEndPoints.msg
new file mode 100644
index 0000000000000000000000000000000000000000..ebb268ae595165348d841f8b0a71fb4963cb5950
--- /dev/null
+++ b/src/planning/nav2_planning_msgs/msg/PathEndPoints.msg
@@ -0,0 +1,11 @@
+std_msgs/Header header
+
+# The start pose for the plan
+geometry_msgs/Pose start
+
+# The final pose of the goal
+geometry_msgs/Pose goal
+
+# If the goal is obstructed, how many meters the planner can
+# relax the constraint in x and y before failing.
+float32 tolerance
diff --git a/src/planning/nav2_planning_msgs/package.xml b/src/planning/nav2_planning_msgs/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..41277c50652e68d8a08f038d07de3363f1597dff
--- /dev/null
+++ b/src/planning/nav2_planning_msgs/package.xml
@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_planning_msgs</name>
+  <version>0.1.0</version>
+  <description>TODO</description>
+  <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rclcpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>builtin_interfaces</build_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>builtin_interfaces</exec_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/world_model/nav2_costmap_world_model/CHANGELOG.rst b/src/world_model/nav2_costmap_world_model/CHANGELOG.rst
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/world_model/nav2_costmap_world_model/CMakeLists.txt b/src/world_model/nav2_costmap_world_model/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..beaaaa97554a3b18379b33e63dfbbdf678ab3973
--- /dev/null
+++ b/src/world_model/nav2_costmap_world_model/CMakeLists.txt
@@ -0,0 +1,62 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_costmap_world_model)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(nav2_util)
+find_package(nav2_libs_msgs)
+find_package(nav2_world_model_msgs)
+
+include_directories(
+  include
+)
+
+add_executable(costmap_world_model
+  src/costmap_world_model.cpp
+  src/main.cpp
+)
+
+ament_target_dependencies(costmap_world_model
+  rclcpp
+  std_msgs
+  nav2_util
+  nav2_libs_msgs
+  nav2_world_model_msgs
+  tf2_ros
+  tf2
+)
+
+install(TARGETS
+  costmap_world_model
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_cppcheck REQUIRED)
+  find_package(ament_cmake_cpplint REQUIRED)
+  find_package(ament_cmake_lint_cmake REQUIRED)
+  find_package(ament_cmake_uncrustify REQUIRED)
+  # This forces cppcheck to consider all files in this project to be C++,
+  # including the headers which end with .h, which cppcheck would normally
+  # consider to be C instead.
+  ament_cppcheck(LANGUAGE "c++")
+  ament_cpplint()
+  ament_lint_cmake()
+  ament_uncrustify()
+endif()
+
+ament_export_include_directories(include)
+
+ament_package()
diff --git a/src/world_model/nav2_costmap_world_model/README.md b/src/world_model/nav2_costmap_world_model/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/world_model/nav2_costmap_world_model/doc/.gitignore b/src/world_model/nav2_costmap_world_model/doc/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/world_model/nav2_costmap_world_model/include/nav2_costmap_world_model/costmap_world_model.hpp b/src/world_model/nav2_costmap_world_model/include/nav2_costmap_world_model/costmap_world_model.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..7b254dfeaf8d9dc9befd2f11ab61e4ca056d7ed1
--- /dev/null
+++ b/src/world_model/nav2_costmap_world_model/include/nav2_costmap_world_model/costmap_world_model.hpp
@@ -0,0 +1,57 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_COSTMAP_WORLD_MODEL__COSTMAP_WORLD_MODEL_HPP_
+#define NAV2_COSTMAP_WORLD_MODEL__COSTMAP_WORLD_MODEL_HPP_
+
+#include <string>
+#include <vector>
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_util/costmap.hpp"
+#include "nav2_libs_msgs/msg/costmap.hpp"
+#include "nav2_world_model_msgs/srv/get_costmap.hpp"
+
+namespace nav2_costmap_world_model
+{
+
+class CostmapWorldModel : public rclcpp::Node
+{
+public:
+  explicit CostmapWorldModel(const std::string & name);
+  CostmapWorldModel();
+
+private:
+  // Server for providing a costmap
+  rclcpp::Service<nav2_world_model_msgs::srv::GetCostmap>::SharedPtr costmapServer_;
+
+  // TODO(orduno): Define a server for scoring trajectories
+  // rclcpp::Service<nav2_world_model_msgs::srv::ScoreTrajectory>::SharedPtr scoringServer_;
+
+  // TODO(orduno): Define a client for getting the static map
+  // rclcpp::Client<nav2_world_model_msgs::srv::GetMap>::SharedPtr mapClient_;
+
+  // TODO(orduno): Alternatively, obtain from a latched topic
+  // rclcpp::Subscription<nav2_world_model_msgs::OccupancyGrid>::SharedPtr mapSub_;
+
+  // TODO(orduno): Define a task for handling trajectory scoring
+  // std::unique_ptr<ScoreTrajectoryClient> scorer;
+
+  // TODO(orduno): std::unique_ptr<LayeredCostmap> layeredCostmap_;
+  std::unique_ptr<nav2_util::Costmap> costmap_;
+};
+
+}  // namespace nav2_costmap_world_model
+
+#endif  // NAV2_COSTMAP_WORLD_MODEL__COSTMAP_WORLD_MODEL_HPP_
diff --git a/src/world_model/nav2_costmap_world_model/package.xml b/src/world_model/nav2_costmap_world_model/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..61ab14f982299c05bbf7ad978850f418637eb822
--- /dev/null
+++ b/src/world_model/nav2_costmap_world_model/package.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_costmap_world_model</name>
+  <version>0.1.0</version>
+  <description>TODO</description>
+  <maintainer email="brian.wilcox@intel.com">Brian Wilcox</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rclcpp</build_depend>
+  <build_depend>nav2_tasks</build_depend>
+  <build_depend>nav2_util</build_depend>
+  <build_depend>nav2_util_msgs</build_depend>
+  <build_depend>nav2_world_model_msgs</build_depend>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>nav2_tasks</exec_depend>
+  <exec_depend>nav2_util_msgs</exec_depend>
+  <exec_depend>nav2_world_model_msgs</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/world_model/nav2_costmap_world_model/src/costmap_world_model.cpp b/src/world_model/nav2_costmap_world_model/src/costmap_world_model.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..edd85a96bf71136036dd8e91600e393bb70c512d
--- /dev/null
+++ b/src/world_model/nav2_costmap_world_model/src/costmap_world_model.cpp
@@ -0,0 +1,52 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <iostream>
+#include <memory>
+#include <vector>
+#include <string>
+#include "nav2_costmap_world_model/costmap_world_model.hpp"
+
+using std::vector;
+using std::string;
+
+namespace nav2_costmap_world_model
+{
+
+CostmapWorldModel::CostmapWorldModel(const string & name)
+: Node(name)
+{
+  costmap_ = std::make_unique<nav2_util::Costmap>(this);
+
+  auto costmap_service_callback = [this](
+    const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+    const std::shared_ptr<nav2_world_model_msgs::srv::GetCostmap::Request> request,
+    const std::shared_ptr<nav2_world_model_msgs::srv::GetCostmap::Response> response) -> void
+    {
+      RCLCPP_INFO(
+        this->get_logger(), "CostmapWorldModel::CostmapWorldModel:Incoming costmap request");
+      response->map = costmap_->getCostmap(request->specs);
+    };
+
+  // Create a service that will use the callback function to handle requests.
+  costmapServer_ = create_service<nav2_world_model_msgs::srv::GetCostmap>(name,
+      costmap_service_callback);
+}
+
+CostmapWorldModel::CostmapWorldModel()
+: CostmapWorldModel("CostmapWorldModel")
+{
+}
+
+}  // namespace nav2_costmap_world_model
diff --git a/src/world_model/nav2_costmap_world_model/src/main.cpp b/src/world_model/nav2_costmap_world_model/src/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c419e73dfc91b1f3f6d8d9b68e604c4c03a1b591
--- /dev/null
+++ b/src/world_model/nav2_costmap_world_model/src/main.cpp
@@ -0,0 +1,26 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_world_model/costmap_world_model.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<nav2_costmap_world_model::CostmapWorldModel>());
+  rclcpp::shutdown();
+
+  return 0;
+}
diff --git a/src/world_model/nav2_costmap_world_model/test/.gitignore b/src/world_model/nav2_costmap_world_model/test/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/world_model/nav2_world_model_msgs/CMakeLists.txt b/src/world_model/nav2_world_model_msgs/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..b15a975bce2920e23175f20826ce53c09c54e53b
--- /dev/null
+++ b/src/world_model/nav2_world_model_msgs/CMakeLists.txt
@@ -0,0 +1,41 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_world_model_msgs)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(nav2_libs_msgs REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/GetCostmap.srv"
+  DEPENDENCIES builtin_interfaces geometry_msgs std_msgs nav2_libs_msgs
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_cppcheck REQUIRED)
+  find_package(ament_cmake_cpplint REQUIRED)
+  find_package(ament_cmake_lint_cmake REQUIRED)
+  find_package(ament_cmake_uncrustify REQUIRED)
+  # This forces cppcheck to consider all files in this project to be C++,
+  # including the headers which end with .h, which cppcheck would normally
+  # consider to be C instead.
+  ament_cppcheck(LANGUAGE "c++")
+  ament_cpplint()
+  ament_lint_cmake()
+  ament_uncrustify()
+endif()
+
+ament_export_dependencies(rosidl_default_runtime)
+
+ament_package()
diff --git a/src/world_model/nav2_world_model_msgs/package.xml b/src/world_model/nav2_world_model_msgs/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..809400028bb9d76240ac5ecc6dbcb58463d95a66
--- /dev/null
+++ b/src/world_model/nav2_world_model_msgs/package.xml
@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_world_model_msgs</name>
+  <version>0.1.0</version>
+  <description>TODO</description>
+  <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rclcpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>builtin_interfaces</build_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>nav2_libs_msgs</build_depend>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>builtin_interfaces</exec_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>nav2_libs_msgs</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/world_model/nav2_world_model_msgs/srv/GetCostmap.srv b/src/world_model/nav2_world_model_msgs/srv/GetCostmap.srv
new file mode 100644
index 0000000000000000000000000000000000000000..a583755f446764571391db70152ff110ee03ff9b
--- /dev/null
+++ b/src/world_model/nav2_world_model_msgs/srv/GetCostmap.srv
@@ -0,0 +1,6 @@
+# Get the costmap
+
+# Specifications for the requested costmap
+nav2_libs_msgs/CostmapMetaData specs
+---
+nav2_libs_msgs/Costmap map