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