diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/.gitignore b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/.gitignore new file mode 100644 index 00000000..e7455725 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/.gitignore @@ -0,0 +1,2 @@ +deps/ +temp/ \ No newline at end of file diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/AutonomyLib.vcxproj b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/AutonomyLib.vcxproj new file mode 100644 index 00000000..071e44c9 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/AutonomyLib.vcxproj @@ -0,0 +1,488 @@ + + + + + true + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + RelWithDebInfo + Win32 + + + RelWithDebInfo + x64 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + {8510c7a4-bf63-41d2-94f6-d8731d137a5a} + + + + {4BFB7231-077A-4671-BD21-D3ADE3EA36E7} + Win32Proj + AutonomyLib + + + + StaticLibrary + true + v143 + Unicode + + + StaticLibrary + false + v143 + true + Unicode + + + StaticLibrary + false + v143 + true + Unicode + + + StaticLibrary + true + v143 + Unicode + + + StaticLibrary + false + v143 + true + Unicode + + + StaticLibrary + false + v143 + true + Unicode + + + + + + + + + + + + + + + + + + + + + + + + + true + $(ProjectDir)temp\$(ProjectName)\$(Platform)\$(Configuration)\ + $(ProjectDir)lib\$(Platform)\$(Configuration)\ + + + true + $(ProjectDir)temp\$(ProjectName)\$(Platform)\$(Configuration)\ + $(ProjectDir)lib\$(Platform)\$(Configuration)\ + + + false + $(ProjectDir)temp\$(ProjectName)\$(Platform)\$(Configuration)\ + $(ProjectDir)lib\$(Platform)\$(Configuration)\ + + + false + $(ProjectDir)temp\$(ProjectName)\$(Platform)\$(Configuration)\ + $(ProjectDir)lib\$(Platform)\$(Configuration)\ + + + false + $(ProjectDir)temp\$(ProjectName)\$(Platform)\$(Configuration)\ + $(ProjectDir)lib\$(Platform)\$(Configuration)\ + + + false + $(ProjectDir)temp\$(ProjectName)\$(Platform)\$(Configuration)\ + $(ProjectDir)lib\$(Platform)\$(Configuration)\ + + + + + Level4 + Disabled + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + include;deps\eigen3;deps\rpclib\include;$(ProjectDir)..\MavLinkCom\include + true + /w34263 /w34266 %(AdditionalOptions) + stdcpp17 + + + Console + true + MavLinkCom.lib;%(AdditionalDependencies) + %(AdditionalLibraryDirectories) + + + rpc.lib + + + deps\rpclib\lib\$(Platform)\$(Configuration) + + + + + + Level4 + Disabled + _SCL_SECURE_NO_WARNINGS;_CRT_SECURE_NO_WARNINGS;_DEBUG;_LIB;%(PreprocessorDefinitions) + include;deps\eigen3;deps\rpclib\include;$(ProjectDir)..\MavLinkCom\include + true + /w34263 /w34266 %(AdditionalOptions) + 4100;4505;4820;4464;4514;4710;4571;%(DisableSpecificWarnings) + stdcpp17 + + + Windows + true + deps\rpclib\lib\Debug;deps\MavLinkCom\lib\x64\Debug;%(AdditionalLibraryDirectories) + MavLinkCom.lib;Setupapi.lib;Cfgmgr32.lib;rpc.lib;%(AdditionalDependencies) + + + rpc.lib + + + deps\rpclib\lib\$(Platform)\$(Configuration) + + + + + Level4 + + MaxSpeed + true + true + WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions) + include;deps\eigen3;deps\rpclib\include;$(ProjectDir)..\MavLinkCom\include + true + /w34263 /w34266 %(AdditionalOptions) + stdcpp17 + + + Windows + true + true + true + %(AdditionalLibraryDirectories) + MavLinkCom.lib;Setupapi.lib;Cfgmgr32.lib;%(AdditionalDependencies) + + + rpc.lib + + + deps\rpclib\lib\$(Platform)\$(Configuration) + + + + + Level4 + + Disabled + true + true + WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions) + include;deps\eigen3;deps\rpclib\include;$(ProjectDir)..\MavLinkCom\include + true + /w34263 /w34266 %(AdditionalOptions) + stdcpp17 + false + + + Windows + true + true + true + %(AdditionalLibraryDirectories) + MavLinkCom.lib;Setupapi.lib;Cfgmgr32.lib;%(AdditionalDependencies) + + + rpc.lib + + + deps\rpclib\lib\$(Platform)\$(Configuration) + + + + + Level4 + + MaxSpeed + true + true + NDEBUG;_SCL_SECURE_NO_WARNINGS;_CRT_SECURE_NO_WARNINGS;_LIB;%(PreprocessorDefinitions) + include;deps\eigen3;deps\rpclib\include;$(ProjectDir)..\MavLinkCom\include + true + /w34263 /w34266 %(AdditionalOptions) + stdcpp17 + + + Windows + true + true + true + deps\rpclib\lib\Debug;deps\MavLinkCom\lib\x64\Release;%(AdditionalLibraryDirectories) + MavLinkCom.lib;Setupapi.lib;Cfgmgr32.lib;%(AdditionalDependencies) + + + rpc.lib + + + deps\rpclib\lib\$(Platform)\$(Configuration) + + + + + Level4 + + Disabled + true + true + NDEBUG;_SCL_SECURE_NO_WARNINGS;_CRT_SECURE_NO_WARNINGS;_LIB;%(PreprocessorDefinitions) + include;deps\eigen3;deps\rpclib\include;$(ProjectDir)..\MavLinkCom\include + true + /w34263 /w34266 %(AdditionalOptions) + stdcpp17 + false + + + Windows + true + true + true + deps\rpclib\lib\Debug;deps\MavLinkCom\lib\x64\Release;%(AdditionalLibraryDirectories) + MavLinkCom.lib;Setupapi.lib;Cfgmgr32.lib;%(AdditionalDependencies) + + + rpc.lib + + + deps\rpclib\lib\$(Platform)\$(Configuration) + + + + + \ No newline at end of file diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/AutonomyLib.vcxproj.filters b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/AutonomyLib.vcxproj.filters new file mode 100644 index 00000000..186d9394 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/AutonomyLib.vcxproj.filters @@ -0,0 +1,493 @@ + + + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hh;hpp;hxx;hm;inl;inc;xsd + + + {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} + rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms + + + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + \ No newline at end of file diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/Doxyfile b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/Doxyfile new file mode 100644 index 00000000..6c96db32 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/Doxyfile @@ -0,0 +1,415 @@ +# Doxyfile 1.10.0 + +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- +DOXYFILE_ENCODING = UTF-8 +PROJECT_NAME = AutonomyLib +PROJECT_NUMBER = +PROJECT_BRIEF = "The simulation engine for autonomous systems" +PROJECT_LOGO = +PROJECT_ICON = +OUTPUT_DIRECTORY = ./AutonomyLib/docs/doxyxml +CREATE_SUBDIRS = NO +CREATE_SUBDIRS_LEVEL = 8 +ALLOW_UNICODE_NAMES = NO +OUTPUT_LANGUAGE = English +BRIEF_MEMBER_DESC = YES +REPEAT_BRIEF = YES +ABBREVIATE_BRIEF = "The $name class" \ + "The $name widget" \ + "The $name file" \ + is \ + provides \ + specifies \ + contains \ + represents \ + a \ + an \ + the +ALWAYS_DETAILED_SEC = NO +INLINE_INHERITED_MEMB = NO +FULL_PATH_NAMES = YES +STRIP_FROM_PATH = +STRIP_FROM_INC_PATH = +SHORT_NAMES = NO +JAVADOC_AUTOBRIEF = NO +JAVADOC_BANNER = NO +QT_AUTOBRIEF = NO +MULTILINE_CPP_IS_BRIEF = NO +PYTHON_DOCSTRING = YES +INHERIT_DOCS = YES +SEPARATE_MEMBER_PAGES = NO +TAB_SIZE = 4 +ALIASES = +OPTIMIZE_OUTPUT_FOR_C = NO +OPTIMIZE_OUTPUT_JAVA = NO +OPTIMIZE_FOR_FORTRAN = NO +OPTIMIZE_OUTPUT_VHDL = NO +OPTIMIZE_OUTPUT_SLICE = NO +EXTENSION_MAPPING = +MARKDOWN_SUPPORT = YES +TOC_INCLUDE_HEADINGS = 5 +MARKDOWN_ID_STYLE = DOXYGEN +AUTOLINK_SUPPORT = YES +BUILTIN_STL_SUPPORT = NO +CPP_CLI_SUPPORT = NO +SIP_SUPPORT = NO +IDL_PROPERTY_SUPPORT = YES +DISTRIBUTE_GROUP_DOC = NO +GROUP_NESTED_COMPOUNDS = NO +SUBGROUPING = YES +INLINE_GROUPED_CLASSES = NO +INLINE_SIMPLE_STRUCTS = NO +TYPEDEF_HIDES_STRUCT = NO +LOOKUP_CACHE_SIZE = 0 +NUM_PROC_THREADS = 1 +TIMESTAMP = NO +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- +EXTRACT_ALL = YES +EXTRACT_PRIVATE = NO +EXTRACT_PRIV_VIRTUAL = NO +EXTRACT_PACKAGE = NO +EXTRACT_STATIC = NO +EXTRACT_LOCAL_CLASSES = YES +EXTRACT_LOCAL_METHODS = NO +EXTRACT_ANON_NSPACES = NO +RESOLVE_UNNAMED_PARAMS = YES +HIDE_UNDOC_MEMBERS = NO +HIDE_UNDOC_CLASSES = NO +HIDE_FRIEND_COMPOUNDS = NO +HIDE_IN_BODY_DOCS = NO +INTERNAL_DOCS = NO +CASE_SENSE_NAMES = SYSTEM +HIDE_SCOPE_NAMES = NO +HIDE_COMPOUND_REFERENCE= NO +SHOW_HEADERFILE = YES +SHOW_INCLUDE_FILES = YES +SHOW_GROUPED_MEMB_INC = NO +FORCE_LOCAL_INCLUDES = NO +INLINE_INFO = YES +SORT_MEMBER_DOCS = YES +SORT_BRIEF_DOCS = NO +SORT_MEMBERS_CTORS_1ST = NO +SORT_GROUP_NAMES = NO +SORT_BY_SCOPE_NAME = NO +STRICT_PROTO_MATCHING = NO +GENERATE_TODOLIST = YES +GENERATE_TESTLIST = YES +GENERATE_BUGLIST = YES +GENERATE_DEPRECATEDLIST= YES +ENABLED_SECTIONS = +MAX_INITIALIZER_LINES = 30 +SHOW_USED_FILES = YES +SHOW_FILES = YES +SHOW_NAMESPACES = YES +FILE_VERSION_FILTER = +LAYOUT_FILE = +CITE_BIB_FILES = +#--------------------------------------------------------------------------- +# Configuration options related to warning and progress messages +#--------------------------------------------------------------------------- +QUIET = NO +WARNINGS = YES +WARN_IF_UNDOCUMENTED = YES +WARN_IF_DOC_ERROR = YES +WARN_IF_INCOMPLETE_DOC = YES +WARN_NO_PARAMDOC = NO +WARN_IF_UNDOC_ENUM_VAL = NO +WARN_AS_ERROR = NO +WARN_FORMAT = "$file:$line: $text" +WARN_LINE_FORMAT = "at line $line of file $file" +WARN_LOGFILE = +#--------------------------------------------------------------------------- +# Configuration options related to the input files +#--------------------------------------------------------------------------- +INPUT = ./AutonomyLib +INPUT_ENCODING = UTF-8 +INPUT_FILE_ENCODING = +FILE_PATTERNS = *.c \ + *.cc \ + *.cxx \ + *.cxxm \ + *.cpp \ + *.cppm \ + *.ccm \ + *.c++ \ + *.c++m \ + *.java \ + *.ii \ + *.ixx \ + *.ipp \ + *.i++ \ + *.inl \ + *.idl \ + *.ddl \ + *.odl \ + *.h \ + *.hh \ + *.hxx \ + *.hpp \ + *.h++ \ + *.ixx \ + *.l \ + *.cs \ + *.d \ + *.php \ + *.php4 \ + *.php5 \ + *.phtml \ + *.inc \ + *.m \ + *.markdown \ + *.md \ + *.mm \ + *.dox \ + *.py \ + *.pyw \ + *.f90 \ + *.f95 \ + *.f03 \ + *.f08 \ + *.f18 \ + *.f \ + *.for \ + *.vhd \ + *.vhdl \ + *.ucf \ + *.qsf \ + *.ice +RECURSIVE = YES +EXCLUDE = ./AutonomyLib/include/common/ \ + ./AutonomyLib/src/common/ \ + ./AutonomyLib/include/sensors/imu/ImuBase.hpp +EXCLUDE_SYMLINKS = NO +EXCLUDE_PATTERNS = +EXCLUDE_SYMBOLS = +EXAMPLE_PATH = +EXAMPLE_PATTERNS = * +EXAMPLE_RECURSIVE = NO +IMAGE_PATH = +INPUT_FILTER = +FILTER_PATTERNS = +FILTER_SOURCE_FILES = NO +FILTER_SOURCE_PATTERNS = +USE_MDFILE_AS_MAINPAGE = +FORTRAN_COMMENT_AFTER = 72 +#--------------------------------------------------------------------------- +# Configuration options related to source browsing +#--------------------------------------------------------------------------- +SOURCE_BROWSER = NO +INLINE_SOURCES = NO +STRIP_CODE_COMMENTS = YES +REFERENCED_BY_RELATION = NO +REFERENCES_RELATION = NO +REFERENCES_LINK_SOURCE = YES +SOURCE_TOOLTIPS = YES +USE_HTAGS = NO +VERBATIM_HEADERS = YES +CLANG_ASSISTED_PARSING = NO +CLANG_ADD_INC_PATHS = YES +CLANG_OPTIONS = +CLANG_DATABASE_PATH = +#--------------------------------------------------------------------------- +# Configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- +ALPHABETICAL_INDEX = YES +IGNORE_PREFIX = +#--------------------------------------------------------------------------- +# Configuration options related to the HTML output +#--------------------------------------------------------------------------- +GENERATE_HTML = NO +HTML_OUTPUT = html +HTML_FILE_EXTENSION = .html +HTML_HEADER = +HTML_FOOTER = +HTML_STYLESHEET = +HTML_EXTRA_STYLESHEET = +HTML_EXTRA_FILES = +HTML_COLORSTYLE = AUTO_LIGHT +HTML_COLORSTYLE_HUE = 220 +HTML_COLORSTYLE_SAT = 100 +HTML_COLORSTYLE_GAMMA = 80 +HTML_DYNAMIC_MENUS = YES +HTML_DYNAMIC_SECTIONS = NO +HTML_CODE_FOLDING = YES +HTML_COPY_CLIPBOARD = YES +HTML_PROJECT_COOKIE = +HTML_INDEX_NUM_ENTRIES = 100 +GENERATE_DOCSET = NO +DOCSET_FEEDNAME = "Doxygen generated docs" +DOCSET_FEEDURL = +DOCSET_BUNDLE_ID = org.doxygen.Project +DOCSET_PUBLISHER_ID = org.doxygen.Publisher +DOCSET_PUBLISHER_NAME = Publisher +GENERATE_HTMLHELP = NO +CHM_FILE = +HHC_LOCATION = +GENERATE_CHI = NO +CHM_INDEX_ENCODING = +BINARY_TOC = NO +TOC_EXPAND = NO +SITEMAP_URL = +GENERATE_QHP = NO +QCH_FILE = +QHP_NAMESPACE = org.doxygen.Project +QHP_VIRTUAL_FOLDER = doc +QHP_CUST_FILTER_NAME = +QHP_CUST_FILTER_ATTRS = +QHP_SECT_FILTER_ATTRS = +QHG_LOCATION = +GENERATE_ECLIPSEHELP = NO +ECLIPSE_DOC_ID = org.doxygen.Project +DISABLE_INDEX = NO +GENERATE_TREEVIEW = NO +FULL_SIDEBAR = NO +ENUM_VALUES_PER_LINE = 4 +TREEVIEW_WIDTH = 250 +EXT_LINKS_IN_WINDOW = NO +OBFUSCATE_EMAILS = YES +HTML_FORMULA_FORMAT = png +FORMULA_FONTSIZE = 10 +FORMULA_MACROFILE = +USE_MATHJAX = NO +MATHJAX_VERSION = MathJax_2 +MATHJAX_FORMAT = HTML-CSS +MATHJAX_RELPATH = +MATHJAX_EXTENSIONS = +MATHJAX_CODEFILE = +SEARCHENGINE = YES +SERVER_BASED_SEARCH = NO +EXTERNAL_SEARCH = NO +SEARCHENGINE_URL = +SEARCHDATA_FILE = searchdata.xml +EXTERNAL_SEARCH_ID = +EXTRA_SEARCH_MAPPINGS = +#--------------------------------------------------------------------------- +# Configuration options related to the LaTeX output +#--------------------------------------------------------------------------- +GENERATE_LATEX = NO +LATEX_OUTPUT = latex +LATEX_CMD_NAME = +MAKEINDEX_CMD_NAME = makeindex +LATEX_MAKEINDEX_CMD = makeindex +COMPACT_LATEX = NO +PAPER_TYPE = a4 +EXTRA_PACKAGES = +LATEX_HEADER = +LATEX_FOOTER = +LATEX_EXTRA_STYLESHEET = +LATEX_EXTRA_FILES = +PDF_HYPERLINKS = YES +USE_PDFLATEX = YES +LATEX_BATCHMODE = NO +LATEX_HIDE_INDICES = NO +LATEX_BIB_STYLE = plain +LATEX_EMOJI_DIRECTORY = +#--------------------------------------------------------------------------- +# Configuration options related to the RTF output +#--------------------------------------------------------------------------- +GENERATE_RTF = NO +RTF_OUTPUT = rtf +COMPACT_RTF = NO +RTF_HYPERLINKS = NO +RTF_STYLESHEET_FILE = +RTF_EXTENSIONS_FILE = +#--------------------------------------------------------------------------- +# Configuration options related to the man page output +#--------------------------------------------------------------------------- +GENERATE_MAN = NO +MAN_OUTPUT = man +MAN_EXTENSION = .3 +MAN_SUBDIR = +MAN_LINKS = NO +#--------------------------------------------------------------------------- +# Configuration options related to the XML output +#--------------------------------------------------------------------------- +GENERATE_XML = YES +XML_OUTPUT = xml +XML_PROGRAMLISTING = YES +XML_NS_MEMB_FILE_SCOPE = NO +#--------------------------------------------------------------------------- +# Configuration options related to the DOCBOOK output +#--------------------------------------------------------------------------- +GENERATE_DOCBOOK = NO +DOCBOOK_OUTPUT = docbook +#--------------------------------------------------------------------------- +# Configuration options for the AutoGen Definitions output +#--------------------------------------------------------------------------- +GENERATE_AUTOGEN_DEF = NO +#--------------------------------------------------------------------------- +# Configuration options related to Sqlite3 output +#--------------------------------------------------------------------------- +GENERATE_SQLITE3 = NO +SQLITE3_OUTPUT = sqlite3 +SQLITE3_RECREATE_DB = YES +#--------------------------------------------------------------------------- +# Configuration options related to the Perl module output +#--------------------------------------------------------------------------- +GENERATE_PERLMOD = NO +PERLMOD_LATEX = NO +PERLMOD_PRETTY = YES +PERLMOD_MAKEVAR_PREFIX = +#--------------------------------------------------------------------------- +# Configuration options related to the preprocessor +#--------------------------------------------------------------------------- +ENABLE_PREPROCESSING = YES +MACRO_EXPANSION = YES +EXPAND_ONLY_PREDEF = NO +SEARCH_INCLUDES = YES +INCLUDE_PATH = +INCLUDE_FILE_PATTERNS = +PREDEFINED = +EXPAND_AS_DEFINED = +SKIP_FUNCTION_MACROS = YES +#--------------------------------------------------------------------------- +# Configuration options related to external references +#--------------------------------------------------------------------------- +TAGFILES = +GENERATE_TAGFILE = +ALLEXTERNALS = NO +EXTERNAL_GROUPS = YES +EXTERNAL_PAGES = YES +#--------------------------------------------------------------------------- +# Configuration options related to diagram generator tools +#--------------------------------------------------------------------------- +HIDE_UNDOC_RELATIONS = YES +HAVE_DOT = NO +DOT_NUM_THREADS = 0 +DOT_COMMON_ATTR = "fontname=Helvetica,fontsize=10" +DOT_EDGE_ATTR = "labelfontname=Helvetica,labelfontsize=10" +DOT_NODE_ATTR = "shape=box,height=0.2,width=0.4" +DOT_FONTPATH = +CLASS_GRAPH = YES +COLLABORATION_GRAPH = YES +GROUP_GRAPHS = YES +UML_LOOK = NO +UML_LIMIT_NUM_FIELDS = 10 +DOT_UML_DETAILS = NO +DOT_WRAP_THRESHOLD = 17 +TEMPLATE_RELATIONS = NO +INCLUDE_GRAPH = YES +INCLUDED_BY_GRAPH = YES +CALL_GRAPH = NO +CALLER_GRAPH = NO +GRAPHICAL_HIERARCHY = YES +DIRECTORY_GRAPH = YES +DIR_GRAPH_MAX_DEPTH = 1 +DOT_IMAGE_FORMAT = png +INTERACTIVE_SVG = NO +DOT_PATH = +DOTFILE_DIRS = +DIA_PATH = +DIAFILE_DIRS = +PLANTUML_JAR_PATH = +PLANTUML_CFG_FILE = +PLANTUML_INCLUDE_PATH = +DOT_GRAPH_MAX_NODES = 50 +MAX_DOT_GRAPH_DEPTH = 0 +DOT_MULTI_TARGETS = NO +GENERATE_LEGEND = YES +DOT_CLEANUP = YES +MSCGEN_TOOL = +MSCFILE_DIRS = diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/Makefile b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/Makefile new file mode 100644 index 00000000..d4bb2cbb --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/Makefile @@ -0,0 +1,20 @@ +# Minimal makefile for Sphinx documentation +# + +# You can set these variables from the command line, and also +# from the environment for the first two. +SPHINXOPTS ?= +SPHINXBUILD ?= sphinx-build +SOURCEDIR = . +BUILDDIR = _build + +# Put it first so that "make" without argument is like "make help". +help: + @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) + +.PHONY: help Makefile + +# Catch-all target: route all unknown targets to Sphinx using the new +# "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). +%: Makefile + @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/css/extra.css b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/css/extra.css new file mode 100644 index 00000000..1ba3d62e --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/css/extra.css @@ -0,0 +1,189 @@ +/* extra.css +see: https://github.com/squidfunk/mkdocs-material/blob/master/src/templates/assets/stylesheets/palette/_scheme.scss +*/ + +.center { + display: block; + margin: 0 auto; +} + +/* +Nervosys tripartite color scheme: + violet: #5000cd + magenta: #ff008a + seafoam: #00ffcd +*/ + +/* Template + + // Indicate that the site is rendered with a dark color scheme + color-scheme: dark; + + // Default color shades + --md-default-fg-color: hsla(var(--md-hue), 15%, 90%, 0.82); + --md-default-fg-color--light: hsla(var(--md-hue), 15%, 90%, 0.56); + --md-default-fg-color--lighter: hsla(var(--md-hue), 15%, 90%, 0.32); + --md-default-fg-color--lightest: hsla(var(--md-hue), 15%, 90%, 0.12); + --md-default-bg-color: hsla(var(--md-hue), 15%, 14%, 1); + --md-default-bg-color--light: hsla(var(--md-hue), 15%, 14%, 0.54); + --md-default-bg-color--lighter: hsla(var(--md-hue), 15%, 14%, 0.26); + --md-default-bg-color--lightest: hsla(var(--md-hue), 15%, 14%, 0.07); + + // Code color shades + --md-code-fg-color: hsla(var(--md-hue), 18%, 86%, 0.82); + --md-code-bg-color: hsla(var(--md-hue), 15%, 18%, 1); + + // Code highlighting color shades + --md-code-hl-color: hsla(#{hex2hsl($clr-blue-a400)}, 1); + --md-code-hl-color--light: hsla(#{hex2hsl($clr-blue-a400)}, 0.1); + + // Code highlighting syntax color shades + --md-code-hl-number-color: hsla(6, 74%, 63%, 1); + --md-code-hl-special-color: hsla(340, 83%, 66%, 1); + --md-code-hl-function-color: hsla(291, 57%, 65%, 1); + --md-code-hl-constant-color: hsla(250, 62%, 70%, 1); + --md-code-hl-keyword-color: hsla(219, 66%, 64%, 1); + --md-code-hl-string-color: hsla(150, 58%, 44%, 1); + --md-code-hl-name-color: var(--md-code-fg-color); + --md-code-hl-operator-color: var(--md-default-fg-color--light); + --md-code-hl-punctuation-color: var(--md-default-fg-color--light); + --md-code-hl-comment-color: var(--md-default-fg-color--light); + --md-code-hl-generic-color: var(--md-default-fg-color--light); + --md-code-hl-variable-color: var(--md-default-fg-color--light); + + // Typeset color shades + --md-typeset-color: var(--md-default-fg-color); + + // Typeset `a` color shades + --md-typeset-a-color: var(--md-primary-fg-color); + + // Typeset `kbd` color shades + --md-typeset-kbd-color: hsla(var(--md-hue), 15%, 90%, 0.12); + --md-typeset-kbd-accent-color: hsla(var(--md-hue), 15%, 90%, 0.2); + --md-typeset-kbd-border-color: hsla(var(--md-hue), 15%, 14%, 1); + + // Typeset `mark` color shades + --md-typeset-mark-color: hsla(#{hex2hsl($clr-blue-a200)}, 0.3); + + // Typeset `table` color shades + --md-typeset-table-color: hsla(var(--md-hue), 15%, 95%, 0.12); + --md-typeset-table-color--light: hsla(var(--md-hue), 15%, 95%, 0.035); + + // Admonition color shades + --md-admonition-fg-color: var(--md-default-fg-color); + --md-admonition-bg-color: var(--md-default-bg-color); + + // Footer color shades + --md-footer-bg-color: hsla(var(--md-hue), 15%, 10%, 0.87); + --md-footer-bg-color--dark: hsla(var(--md-hue), 15%, 8%, 1); + +*/ + +/* nervosys: light mode */ +[data-md-color-scheme="nervosys_light"] { + + color-scheme: light; + + /* primary color shades */ + --md-primary-fg-color: #5000cd; + --md-default-bg-color: #ffffff; + --md-primary-bg-color: hsla(0, 0%, 100%, 1); + + /* text color shades */ + --md-typeset-color: #000000; + --md-text-link-color: hsla(168, 100%, 50%, 1); + + /* accent color shades */ + --md-accent-fg-color: hsl(263, 100%, 30%); + --md-accent-fg-color--transparent: hsla(189, 100%, 37%, 0.1); + --md-accent-bg-color: hsla(0, 0%, 100%, 1); +} + +[data-md-color-scheme="nervosys_light"] > * { + + /* code block color shades */ + --md-code-fg-color: hsla(200, 18%, 26%, 1); + --md-code-bg-color: hsla(0, 0%, 96%, 1); + + /* footer */ + --md-footer-fg-color: hsla(0, 0%, 100%, 1); + --md-footer-bg-color: #5000cd; +} + +/* nervosys: dark mode */ +[data-md-color-scheme="nervosys_dark"] { + + color-scheme: dark; + + /* primary color shades */ + --md-primary-fg-color: #ff008a; + --md-default-fg-color: hsl(0, 0%, 50%); + --md-default-fg-color--light: hsl(0, 0%, 60%); + --md-default-fg-color--lighter: hsl(0, 0%, 90%); + --md-default-fg-color--lightest: hsl(0, 0%, 10%); /* background */ + + --md-primary-bg-color: hsl(0, 0%, 100%); /* header */ + --md-default-bg-color: hsl(0, 14%, 4%); + + /* text color shades */ + --md-typeset-color: hsl(0, 0%, 90%); + --md-typeset-a-color: hsl(328, 100%, 50%); /* links */ + --md-text-link-color: hsla(168, 100%, 50%, 1); + + /* accent color shades */ + --md-accent-fg-color: hsl(328, 100%, 40%); + --md-accent-fg-color--transparent: hsla(189, 100%, 37%, 0.9); + --md-accent-bg-color: hsl(0, 0%, 50%); +} + +[data-md-color-scheme="nervosys_dark"] > * { + + /* code color shades */ + --md-code-fg-color: hsl(200, 18%, 74%); + --md-code-bg-color: hsl(0, 0%, 10%); + + /* code highlighting color shades */ + --md-code-hl-color: hsl(200, 18%, 54%); + --md-code-hl-color--light: hsl(200, 18%, 64%); + + /* Code highlighting syntax color shades */ + --md-code-hl-number-color: hsl(0, 74%, 60%); + --md-code-hl-special-color: hsl(340, 83%, 60%); + --md-code-hl-function-color: hsl(290, 57%, 60%); + --md-code-hl-constant-color: hsl(250, 62%, 70%); + --md-code-hl-keyword-color: hsl(220, 66%, 70%); + --md-code-hl-string-color: hsl(30, 100%, 60%); + --md-code-hl-name-color: hsl(210, 80%, 70%); + --md-code-hl-operator-color: hsl(250, 100%, 90%); + --md-code-hl-punctuation-color: hsl(320, 100%, 70%); + --md-code-hl-comment-color: hsl(0, 0%, 50%); + --md-code-hl-generic-color: hsl(150, 100%, 50%); + --md-code-hl-variable-color: hsl(200, 100%, 70%); + + /* typeset color shades */ + /* + --md-typeset-color: hsl(0, 0%, 90%); + --md-typeset-a-color: hsl(0, 0%, 50%); + */ + + /* typeset `kbd` color shades */ + --md-typeset-kbd-color: hsl(200, 15%, 70%); + --md-typeset-kbd-accent-color: hsl(200, 15%, 80%); + --md-typeset-kbd-border-color: hsl(200, 15%, 90%); + + /* typeset `mark` color shades */ + --md-typeset-mark-color: hsl(200, 18%, 34%); + + /* typeset `table` color shades */ + --md-typeset-table-color: hsl(200, 15%, 95%); + --md-typeset-table-color--light: hsl(200, 15%, 75%); + + /* admonition color shades */ + --md-admonition-fg-color: hsl(0, 0%, 80%); + --md-admonition-bg-color: hsl(0, 0%, 06%); + + /* footer */ + --md-footer-fg-color: hsla(0, 0%, 100%, 1); + --md-footer-bg-color: #ff008a; + /* --md-footer-bg-color--dark: hsl(328, 100%, 30%); */ +} diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-Black.ttf b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-Black.ttf new file mode 100644 index 00000000..0112e7da Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-Black.ttf differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-BlackItalic.ttf b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-BlackItalic.ttf new file mode 100644 index 00000000..b2c6aca5 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-BlackItalic.ttf differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-Bold.ttf b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-Bold.ttf new file mode 100644 index 00000000..43da14d8 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-Bold.ttf differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-BoldItalic.ttf b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-BoldItalic.ttf new file mode 100644 index 00000000..bcfdab43 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-BoldItalic.ttf differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-Italic.ttf b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-Italic.ttf new file mode 100644 index 00000000..1b5eaa36 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-Italic.ttf differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-Light.ttf b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-Light.ttf new file mode 100644 index 00000000..e7307e72 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-Light.ttf differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-LightItalic.ttf b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-LightItalic.ttf new file mode 100644 index 00000000..2d277afb Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-LightItalic.ttf differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-Medium.ttf b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-Medium.ttf new file mode 100644 index 00000000..ac0f908b Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-Medium.ttf differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-MediumItalic.ttf b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-MediumItalic.ttf new file mode 100644 index 00000000..fc36a478 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-MediumItalic.ttf differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-Regular.ttf b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-Regular.ttf new file mode 100644 index 00000000..ddf4bfac Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-Regular.ttf differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-Thin.ttf b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-Thin.ttf new file mode 100644 index 00000000..2e0dee6a Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-Thin.ttf differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-ThinItalic.ttf b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-ThinItalic.ttf new file mode 100644 index 00000000..084f9c0f Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/fonts/Roboto-ThinItalic.ttf differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/js/extra.js b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/js/extra.js new file mode 100644 index 00000000..f39b165b --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/js/extra.js @@ -0,0 +1,3 @@ +// if (window.matchMedia && window.matchMedia('(prefers-color-scheme: dark)').matches) { +// // dark mode +// } diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/js/mathjax.js b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/js/mathjax.js new file mode 100644 index 00000000..e69de29b diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/diagrams/mavlink/Design.dgml b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/diagrams/mavlink/Design.dgml new file mode 100644 index 00000000..57961395 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/diagrams/mavlink/Design.dgml @@ -0,0 +1,138 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/diagrams/mavlink/FtpDesign.dgml b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/diagrams/mavlink/FtpDesign.dgml new file mode 100644 index 00000000..25f7fa8d --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/diagrams/mavlink/FtpDesign.dgml @@ -0,0 +1,115 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/diagrams/mavlink/OffboardControl.dgml b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/diagrams/mavlink/OffboardControl.dgml new file mode 100644 index 00000000..0e1e97d1 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/diagrams/mavlink/OffboardControl.dgml @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/diagrams/mavlink/images/example1.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/diagrams/mavlink/images/example1.png new file mode 100644 index 00000000..45af34b1 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/diagrams/mavlink/images/example1.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/diagrams/mavlink/images/example2.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/diagrams/mavlink/images/example2.png new file mode 100644 index 00000000..a95dc911 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/diagrams/mavlink/images/example2.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/diagrams/mavlink/images/example3.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/diagrams/mavlink/images/example3.png new file mode 100644 index 00000000..437356d1 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/diagrams/mavlink/images/example3.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/diagrams/mavlink/images/example4.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/diagrams/mavlink/images/example4.png new file mode 100644 index 00000000..1fb7de88 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/diagrams/mavlink/images/example4.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/diagrams/mavlink/images/overview.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/diagrams/mavlink/images/overview.png new file mode 100644 index 00000000..e3978188 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/diagrams/mavlink/images/overview.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/DJI S900.zip b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/DJI S900.zip new file mode 100644 index 00000000..831c1fc5 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/DJI S900.zip differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/Flamewheel.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/Flamewheel.png new file mode 100644 index 00000000..d174876d Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/Flamewheel.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/HelloSpawnedDrones.gif b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/HelloSpawnedDrones.gif new file mode 100644 index 00000000..7b1fb8a1 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/HelloSpawnedDrones.gif differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/PIA14840-full.jpg b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/PIA14840-full.jpg new file mode 100644 index 00000000..cce71092 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/PIA14840-full.jpg differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/Playback.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/Playback.png new file mode 100644 index 00000000..b024d24e Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/Playback.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/RealFlight.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/RealFlight.png new file mode 100644 index 00000000..7f8df960 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/RealFlight.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/androidsdkmanagericon.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/androidsdkmanagericon.png new file mode 100644 index 00000000..65014a26 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/androidsdkmanagericon.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/authorizegithubscreen2.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/authorizegithubscreen2.png new file mode 100644 index 00000000..2f6b6ca0 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/authorizegithubscreen2.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/authorizeongithub.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/authorizeongithub.png new file mode 100644 index 00000000..9380f80c Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/authorizeongithub.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/autonomysim_car_manual.gif b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/autonomysim_car_manual.gif new file mode 100644 index 00000000..eb153682 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/autonomysim_car_manual.gif differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/autonomysim_drone_manual.gif b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/autonomysim_drone_manual.gif new file mode 100644 index 00000000..d4056d86 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/autonomysim_drone_manual.gif differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/autonomysim_simple_bw_1000w.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/autonomysim_simple_bw_1000w.png new file mode 100644 index 00000000..ca53dda5 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/autonomysim_simple_bw_1000w.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/autonomysim_startup.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/autonomysim_startup.png new file mode 100644 index 00000000..53d042d4 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/autonomysim_startup.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/blocks_video.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/blocks_video.png new file mode 100644 index 00000000..658dc8b1 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/blocks_video.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/busy_hard_drive.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/busy_hard_drive.png new file mode 100644 index 00000000..de1152cd Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/busy_hard_drive.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/camera_noise_demo.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/camera_noise_demo.png new file mode 100644 index 00000000..b9ab8856 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/camera_noise_demo.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/cameras.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/cameras.png new file mode 100644 index 00000000..67a5e001 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/cameras.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/car_demo_video.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/car_demo_video.png new file mode 100644 index 00000000..8218b254 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/car_demo_video.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/car_demo_video_large.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/car_demo_video_large.png new file mode 100644 index 00000000..5bc17b0e Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/car_demo_video_large.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/cooked_ssd.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/cooked_ssd.png new file mode 100644 index 00000000..c78738bb Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/cooked_ssd.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/cpu_views.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/cpu_views.png new file mode 100644 index 00000000..5739b271 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/cpu_views.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/current_version.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/current_version.png new file mode 100644 index 00000000..36dfac22 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/current_version.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/demo_multi_vehicles.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/demo_multi_vehicles.png new file mode 100644 index 00000000..631af6b6 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/demo_multi_vehicles.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/demo_video.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/demo_video.png new file mode 100644 index 00000000..8da76b56 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/demo_video.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/demo_video_large.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/demo_video_large.png new file mode 100644 index 00000000..a8dba617 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/demo_video_large.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/depth.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/depth.png new file mode 100644 index 00000000..1b1612b4 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/depth.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/detection_python.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/detection_python.png new file mode 100644 index 00000000..24c72e85 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/detection_python.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/detection_ue4.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/detection_ue4.png new file mode 100644 index 00000000..20c7b418 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/detection_ue4.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/dqn_car.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/dqn_car.png new file mode 100644 index 00000000..e26c9b17 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/dqn_car.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/dqn_quadcopter.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/dqn_quadcopter.png new file mode 100644 index 00000000..18a32a3a Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/dqn_quadcopter.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/drone_depth_materials.PNG b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/drone_depth_materials.PNG new file mode 100644 index 00000000..2fce65e9 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/drone_depth_materials.PNG differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/drone_front_camera_forest.PNG b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/drone_front_camera_forest.PNG new file mode 100644 index 00000000..5568756e Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/drone_front_camera_forest.PNG differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/drone_front_camera_view.PNG b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/drone_front_camera_view.PNG new file mode 100644 index 00000000..6a8ed782 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/drone_front_camera_view.PNG differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/epic_launcher_install.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/epic_launcher_install.png new file mode 100644 index 00000000..6b7fc3f7 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/epic_launcher_install.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/event_sim.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/event_sim.png new file mode 100644 index 00000000..7bd9794f Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/event_sim.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/exceptions.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/exceptions.png new file mode 100644 index 00000000..c94f17b6 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/exceptions.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/forces.PNG b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/forces.PNG new file mode 100644 index 00000000..7900df54 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/forces.PNG differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/fps_views.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/fps_views.png new file mode 100644 index 00000000..324adb73 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/fps_views.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/game_controllers.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/game_controllers.png new file mode 100644 index 00000000..e6ca9ebf Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/game_controllers.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/helpapi-01.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/helpapi-01.png new file mode 100644 index 00000000..51308a16 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/helpapi-01.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/helpapi.svg b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/helpapi.svg new file mode 100644 index 00000000..0ddc9227 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/helpapi.svg @@ -0,0 +1,1661 @@ + + + + + + + + + + + + + + + + + sample help text sample help text sample help text sample help text sample help text sample help text sample help + + + + + + + + + + + + + + + + + + + + + + + Getting Started text sample help text sample help text sample help text sample help text sample help text sample help text sample + + + + + + + + + + + + + Learning Coursesample help text sample help text sample help text sample help text sample help text sample help text sample help + + + + + + + + + + + + + + + + + + + sample help text sample help text sample help text sample help text sample help text sample + + + + + + + + + + + + + + + Help API + + + +pulling from API + + + +pulling from API + + + +pulling from API + + + +pulling from API + + + + + + + website #1 + + website #2 + + website #4 + + website #3 + + diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/illustratoroptions.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/illustratoroptions.png new file mode 100644 index 00000000..d043971d Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/illustratoroptions.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/itermexample.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/itermexample.png new file mode 100644 index 00000000..e412af02 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/itermexample.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/jekyll.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/jekyll.png new file mode 100644 index 00000000..ebaf27d0 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/jekyll.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/killalljekyll.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/killalljekyll.png new file mode 100644 index 00000000..aa8adaa2 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/killalljekyll.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/landscape_mountains.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/landscape_mountains.png new file mode 100644 index 00000000..92d9d79e Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/landscape_mountains.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/liningup.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/liningup.png new file mode 100644 index 00000000..ede4dc07 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/liningup.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/lm_player_start_pos.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/lm_player_start_pos.png new file mode 100644 index 00000000..d0a3b321 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/lm_player_start_pos.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/log_viewer.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/log_viewer.png new file mode 100644 index 00000000..79ce0604 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/log_viewer.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/log_viewer_connect.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/log_viewer_connect.png new file mode 100644 index 00000000..91d7e782 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/log_viewer_connect.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/logo_block_raven_rgi_modern_blueye_xwerx_bw_1000h.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/logo_block_raven_rgi_modern_blueye_xwerx_bw_1000h.png new file mode 100644 index 00000000..5e92fe1f Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/logo_block_raven_rgi_modern_blueye_xwerx_bw_1000h.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/micronav_gcs.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/micronav_gcs.png new file mode 100644 index 00000000..d6f63b3b Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/micronav_gcs.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/mildef_rc2100.jpg b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/mildef_rc2100.jpg new file mode 100644 index 00000000..8861a79f Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/mildef_rc2100.jpg differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/octomap.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/octomap.png new file mode 100644 index 00000000..05788763 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/octomap.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/orbit.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/orbit.png new file mode 100644 index 00000000..adf171d8 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/orbit.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/overview.PNG b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/overview.PNG new file mode 100644 index 00000000..d93c545d Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/overview.PNG differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/package_unreal.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/package_unreal.png new file mode 100644 index 00000000..be4407cf Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/package_unreal.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/plugin_contents.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/plugin_contents.png new file mode 100644 index 00000000..bcebcda6 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/plugin_contents.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/point_cloud.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/point_cloud.png new file mode 100644 index 00000000..2140120e Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/point_cloud.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/px4_debugging.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/px4_debugging.png new file mode 100644 index 00000000..d4309ce5 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/px4_debugging.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/px4_nice.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/px4_nice.png new file mode 100644 index 00000000..91a1bae6 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/px4_nice.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/qgc_joystick.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/qgc_joystick.png new file mode 100644 index 00000000..8855dd0d Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/qgc_joystick.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/record_data.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/record_data.png new file mode 100644 index 00000000..0f14ccc9 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/record_data.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/record_data_large.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/record_data_large.png new file mode 100644 index 00000000..c73ebc80 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/record_data_large.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/regen_sln.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/regen_sln.png new file mode 100644 index 00000000..2a3cded9 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/regen_sln.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/rune.svg b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/rune.svg new file mode 100644 index 00000000..65628ad4 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/rune.svg differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/KiteDemo1.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/KiteDemo1.png new file mode 100644 index 00000000..46019c26 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/KiteDemo1.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/KiteDemo2.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/KiteDemo2.png new file mode 100644 index 00000000..755ae6f0 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/KiteDemo2.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/KiteDemo3.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/KiteDemo3.png new file mode 100644 index 00000000..d126c2e8 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/KiteDemo3.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/KiteDemo4.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/KiteDemo4.png new file mode 100644 index 00000000..383f0de8 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/KiteDemo4.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/KiteDemo5.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/KiteDemo5.png new file mode 100644 index 00000000..a3642480 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/KiteDemo5.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/KiteDemo6.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/KiteDemo6.png new file mode 100644 index 00000000..09c28ddc Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/KiteDemo6.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/drone_flight.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/drone_flight.png new file mode 100644 index 00000000..622b099e Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/drone_flight.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/drone_front_camera_forest.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/drone_front_camera_forest.png new file mode 100644 index 00000000..5568756e Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/drone_front_camera_forest.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/drone_front_camera_view.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/drone_front_camera_view.png new file mode 100644 index 00000000..6a8ed782 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/drone_front_camera_view.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/drone_one_ground.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/drone_one_ground.png new file mode 100644 index 00000000..e9d8ed11 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/screenshots/drone_one_ground.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/simAddVehicle_Car.gif b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/simAddVehicle_Car.gif new file mode 100644 index 00000000..426ccf5e Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/simAddVehicle_Car.gif differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/sim_game_mode.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/sim_game_mode.png new file mode 100644 index 00000000..49d4c228 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/sim_game_mode.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/steering_wheel_instructions_1.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/steering_wheel_instructions_1.png new file mode 100644 index 00000000..8ebf560d Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/steering_wheel_instructions_1.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/steering_wheel_instructions_2.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/steering_wheel_instructions_2.png new file mode 100644 index 00000000..1aa868de Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/steering_wheel_instructions_2.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/survey.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/survey.png new file mode 100644 index 00000000..649f3c5b Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/survey.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/tex_shuffle_actor.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/tex_shuffle_actor.png new file mode 100644 index 00000000..53e11366 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/tex_shuffle_actor.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/tex_swap_demo.gif b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/tex_swap_demo.gif new file mode 100644 index 00000000..d9d18e0a Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/tex_swap_demo.gif differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/tex_swap_group_editing.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/tex_swap_group_editing.png new file mode 100644 index 00000000..650d3962 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/tex_swap_group_editing.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/tex_swap_material.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/tex_swap_material.png new file mode 100644 index 00000000..06f0289b Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/tex_swap_material.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/tex_swap_subset.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/tex_swap_subset.png new file mode 100644 index 00000000..eec0eba3 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/tex_swap_subset.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/ue_hard_drive.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/ue_hard_drive.png new file mode 100644 index 00000000..55f79bea Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/ue_hard_drive.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/ue_install.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/ue_install.png new file mode 100644 index 00000000..7246729a Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/ue_install.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/ue_install_location.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/ue_install_location.png new file mode 100644 index 00000000..1493806d Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/ue_install_location.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/unreal_editor.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/unreal_editor.png new file mode 100644 index 00000000..6f80a746 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/unreal_editor.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/unreal_editor_blocks.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/unreal_editor_blocks.png new file mode 100644 index 00000000..58fc59c3 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/unreal_editor_blocks.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/unreal_versions.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/unreal_versions.png new file mode 100644 index 00000000..11a71e3a Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/unreal_versions.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/voxel_grid.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/voxel_grid.png new file mode 100644 index 00000000..e2460ec7 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/voxel_grid.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/vsbuild_config.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/vsbuild_config.png new file mode 100644 index 00000000..08bee233 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/vsbuild_config.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/weather_menu.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/weather_menu.png new file mode 100644 index 00000000..b7ed08c3 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/weather_menu.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/workflowarrow.png b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/workflowarrow.png new file mode 100644 index 00000000..91a3e816 Binary files /dev/null and b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/_static/media/images/workflowarrow.png differ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/conf.py b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/conf.py new file mode 100644 index 00000000..bb75bd87 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/conf.py @@ -0,0 +1,358 @@ +# -*- coding: utf-8 -*- +# +# conf.py +# +# Sphinx configuration file for Doxygen XML -> Breathe -> Sphinx +# +# Generate XML on Windows: `. "C:\Program Files\doxygen\bin\doxygen.exe" ".\Doxyfile"` +# +# For the full list of built-in configuration values, see the documentation: +# https://www.sphinx-doc.org/en/master/usage/configuration.html +# +# Copyright 2024 Nervosys, LLC +# + + +# -- Path setup -------------------------------------------------------------- +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. + +import os +import sys + +sys.path.insert(0, os.path.abspath("./ext/breathe/")) + +import sphinx_immaterial + + +# -- Project information ----------------------------------------------------- +# https://www.sphinx-doc.org/en/master/usage/configuration.html#project-information + +project = "AutonomySim C++ API" +copyright = "Copyright © 2024 Nervosys, LLC" +author = "Nervosys" + +# short X.Y version +# version = __version__ +version = "" +# full version, including alpha/beta/rc tags +# release = version +release = "" + + +# -- General configuration --------------------------------------------------- +# https://www.sphinx-doc.org/en/master/usage/configuration.html#general-configuration + +# If your documentation needs a minimal Sphinx version, state it here. +# needs_sphinx = '1.0' + +# Add any Sphinx extension module names here, as strings. They can be +# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom +# ones. +extensions = [ + "sphinx.ext.autodoc", + "sphinx.ext.autosummary", + "sphinx.ext.autosectionlabel", + "sphinx.ext.doctest", + "sphinx.ext.duration", + "sphinx.ext.intersphinx", + "sphinx.ext.inheritance_diagram", + "sphinx.ext.todo", + "sphinx.ext.mathjax", + "sphinx.ext.coverage", + "sphinx.ext.napoleon", + "sphinx.ext.viewcode", + "sphinx_immaterial", + "breathe", + "exhale", +] + +autodoc_default_flags = ["members"] +autosummary_generate = True +autosectionlabel_prefix_document = True +autosectionlabel_maxdepth = 4 + +# Breathe Configuration +breathe_projects = {"AutonomyLib": "./doxyxml/xml"} +# breathe_projects_source = {"AutonomyLib": "./doxyxml/xml"} +breathe_default_project = "AutonomyLib" +# breathe_default_members = ("members", "undoc-members") + +exhale_args = { + # These arguments are required + "containmentFolder": "./api", + "rootFileName": "library_root.rst", + "doxygenStripFromPath": "..", + # Heavily encouraged optional argument (see docs) + # "rootFileTitle": "C++ Library API", + # Suggested optional arguments + "createTreeView": True, + # TIP: if using the sphinx-bootstrap-theme, you need + # "treeViewIsBootstrap": True, + "exhaleExecutesDoxygen": False, + # "exhaleDoxygenStdin": "INPUT = ../include" +} + +# Tell sphinx what the primary language being documented is. +primary_domain = "cpp" + +# Tell sphinx what the pygments highlight language should be. +highlight_language = "cpp" + +# Add any paths that contain templates here, relative to this directory. +templates_path = ["_templates"] + +# The suffix(es) of source filenames. +# You can specify multiple suffix as a list of string: +# source_suffix = ['.rst', '.md'] +source_suffix = ".rst" + +# The master toctree document. +master_doc = "index" + +# The language for content autogenerated by Sphinx. Refer to documentation +# for a list of supported languages. This is also used if you do content +# translation via gettext catalogs. Usually you set "language" from the +# command line for these cases. +language = "en" + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +# This pattern also affects html_static_path and html_extra_path. +exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] + +# The name of the Pygments (syntax highlighting) style to use. +# See all: `python -c "import pygments; from pygments.styles import get_all_styles; print(list(get_all_styles()))"` +pygments_style = None # "monokai" + + +# -- Options for HTML output ------------------------------------------------- +# https://www.sphinx-doc.org/en/master/usage/configuration.html#options-for-html-output +# https://jbms.github.io/sphinx-immaterial/customization.html +# see `theme.conf` for more information + +# Title; default: v documentation +html_title = project + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +# html_theme = 'alabaster' +html_theme = "sphinx_immaterial" + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = ["_static"] + +# Output file base name for HTML help builder. +sphinx_immaterial_external_resource_cache_dir = "_static/fonts" + +# custom CSS file +html_css_files = [ + "css/extra.css", +] + +# custom JS file +html_js_files = [ + "js/extra.css", +] + +# style override +html_style = "css/extra.css" + +# Theme options are theme-specific and customize the look and feel of a theme +# further. For a list of options available for each theme, see the +# documentation. +# html_theme_options = {} +html_theme_options = { + "site_url": "https://nervosys.github.io/AutonomySim", + "repo_url": "https://github.com/nervosys/AutonomySim", + "repo_name": "AutonomySim", + "edit_uri": "edit/master/docs", + "globaltoc_collapse": True, + "toc_title_is_page_title": False, + "version_dropdown": True, + # "version_info": [ + # { + # "version": "https://nervosys.github.io/AutonomySim", + # "title": "Main", + # "aliases": [], + # }, + # ], + "features": [ + "announce.dismiss", + "content.code.annotate", + "content.code.copy", + "content.tabs.link", + # 'header.autohide', + # 'mkdocstrings', + "navigation.expand", + # 'navigation.instant', + "navigation.top", + # 'navigation.tabs', + # 'navigation.sections', + "navigation.instant", + "navigation.path", + "navigation.footer", + # 'navigation.tracking', + # 'search.highlight', + "search.suggest", + "search.share", + "toc.follow", + "toc.integrate", + ], + "font": { + "text": "Roboto", + "code": "Roboto Mono", + }, + # "language": "en", + "palette": [ + # { + # "media": "(prefers-color-scheme)", + # "toggle": { + # "icon": "material/brightness-auto", + # "name": "Switch to light mode", + # }, + # }, + { + "media": "(prefers-color-scheme: light)", + "scheme": "nervosys_light", + "toggle": { + "icon": "material/brightness-7", + "name": "Switch to dark mode", + }, + }, + { + "media": "(prefers-color-scheme: dark)", + "scheme": "nervosys_dark", + "toggle": { + "icon": "material/brightness-4", + "name": "Switch to system preference", + }, + }, + ], + # "favicon": "media/images/rune.svg", + "icon": { + "logo": "material/book-open-page-variant", + "repo": "fontawesome/brands/git-alt", + "edit": "material/pencil", + "view": "material/eye", + }, + "social": [ + { + "icon": "fontawesome/brands/github-alt", + "link": "https://github.com/nervosys/AutonomySim", + }, + { + "icon": "fontawesome/brands/discord", + "link": "https://discord.gg/x84JXYje", + }, + { + "icon": "fontawesome/brands/twitter", # x-twitter, square-x-twitter missing in sphinx-immaterial (needs update) + "link": "https://x.com/nervosys", + }, + ], +} + +# Custom sidebar templates, must be a dictionary that maps document names +# to template names. +# The default sidebars (for documents that don't match any pattern) are +# defined by theme itself. Builtin themes are using these templates by +# default: ``['localtoc.html', 'relations.html', 'sourcelink.html', +# 'searchbox.html']``. +# html_sidebars = {} + + +# -- Options for HTMLHelp output --------------------------------------------- + +# Output file base name for HTML help builder. +htmlhelp_basename = "AutonomySimDoc" + + +# -- Options for LaTeX output ------------------------------------------------ + +# Elements +latex_elements = { + # The paper size ('letterpaper' or 'a4paper'). + # + # 'papersize': 'letterpaper', + # The font size ('10pt', '11pt' or '12pt'). + # + # 'pointsize': '10pt', + # Additional stuff for the LaTeX preamble. + # + # 'preamble': '', + # Latex figure (float) alignment + # + # 'figure_align': 'htbp', +} + +# Grouping the document tree into LaTeX files. List of tuples +# (source start file, target name, title, +# author, documentclass [howto, manual, or own class]). +latex_documents = [ + ( + master_doc, + "AutonomySim.tex", + "AutonomySim Documentation", + "Adam Erickson", + "manual", + ), +] + + +# -- Options for manual page output ------------------------------------------ + +# One entry per manual page. List of tuples +# (source start file, name, description, authors, manual section). +man_pages = [(master_doc, "AutonomySim", "AutonomySim Documentation", [author], 1)] + + +# -- Options for Texinfo output ---------------------------------------------- + +# Grouping the document tree into Texinfo files. List of tuples +# (source start file, target name, title, author, +# dir menu entry, description, category) +texinfo_documents = [ + ( + master_doc, + "AutonomySim", + "AutonomySim Documentation", + author, + "AutonomySim", + "The simulation engine for autonomous systems", + "Miscellaneous", + ), +] + + +# -- Options for Epub output ------------------------------------------------- + +# Bibliographic Dublin Core info. +epub_title = project + +# The unique identifier of the text. This can be a ISBN number +# or the project homepage. +# epub_identifier = '' + +# A unique identification for the text. +# epub_uid = '' + +# A list of files that should not be packed into the epub file. +epub_exclude_files = ["search.html"] + + +# -- Extension configuration ------------------------------------------------- + + +# -- Options for intersphinx extension --------------------------------------- + +# Example configuration for intersphinx: refer to the Python standard library. +intersphinx_mapping = {"https://docs.python.org/": None} + + +# -- Options for todo extension ---------------------------------------------- + +# If true, `todo` and `todoList` produce output, else they produce nothing. +todo_include_todos = True diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/index.rst b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/index.rst new file mode 100644 index 00000000..37c122b4 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/index.rst @@ -0,0 +1,17 @@ +.. AutonomyLib documentation master file, created by + sphinx-quickstart on Mon Feb 12 15:24:11 2024. + You can adapt this file completely to your liking, but it should at least + contain the root `toctree` directive. + +AutonomyLib +=========== + +This page documents `AutonomyLib`_, the C++ library of `AutonomySim`_. + +.. _`AutonomySim`: https://github.com/nervosys/AutonomySim + +.. toctree:: + :maxdepth: 2 + + about + api/library_root diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/make.bat b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/make.bat new file mode 100644 index 00000000..954237b9 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/docs/make.bat @@ -0,0 +1,35 @@ +@ECHO OFF + +pushd %~dp0 + +REM Command file for Sphinx documentation + +if "%SPHINXBUILD%" == "" ( + set SPHINXBUILD=sphinx-build +) +set SOURCEDIR=. +set BUILDDIR=_build + +%SPHINXBUILD% >NUL 2>NUL +if errorlevel 9009 ( + echo. + echo.The 'sphinx-build' command was not found. Make sure you have Sphinx + echo.installed, then set the SPHINXBUILD environment variable to point + echo.to the full path of the 'sphinx-build' executable. Alternatively you + echo.may add the Sphinx directory to PATH. + echo. + echo.If you don't have Sphinx installed, grab it from + echo.https://www.sphinx-doc.org/ + exit /b 1 +) + +if "%1" == "" goto help + +%SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% +goto end + +:help +%SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% + +:end +popd diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/api/ApiProvider.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/api/ApiProvider.hpp new file mode 100644 index 00000000..169a5c08 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/api/ApiProvider.hpp @@ -0,0 +1,68 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_api_ApiProvider_hpp +#define autonomylib_api_ApiProvider_hpp + +#include "VehicleApiBase.hpp" +#include "VehicleSimApiBase.hpp" +#include "WorldSimApiBase.hpp" +#include "common/utils/UniqueValueMap.hpp" +#include + +namespace nervosys { +namespace autonomylib { + +class ApiProvider { + public: + ApiProvider(WorldSimApiBase *world_sim_api) : world_sim_api_(world_sim_api) {} + virtual ~ApiProvider() = default; + + // vehicle API + virtual VehicleApiBase *getVehicleApi(const std::string &vehicle_name) { + return vehicle_apis_.findOrDefault(vehicle_name, nullptr); + } + + // world simulation API + virtual WorldSimApiBase *getWorldSimApi() { return world_sim_api_; } + + // vehicle simulation API + virtual VehicleSimApiBase *getVehicleSimApi(const std::string &vehicle_name) const { + return vehicle_sim_apis_.findOrDefault(vehicle_name, nullptr); + } + + size_t getVehicleCount() const { return vehicle_apis_.valsSize(); } + + void insert_or_assign(const std::string &vehicle_name, VehicleApiBase *vehicle_api, + VehicleSimApiBase *vehicle_sim_api) { + vehicle_apis_.insert_or_assign(vehicle_name, vehicle_api); + vehicle_sim_apis_.insert_or_assign(vehicle_name, vehicle_sim_api); + } + + const common_utils::UniqueValueMap &getVehicleApis() { return vehicle_apis_; } + + const common_utils::UniqueValueMap &getVehicleSimApis() { + return vehicle_sim_apis_; + } + + bool hasDefaultVehicle() const { + return !(vehicle_apis_.findOrDefault("", nullptr) == nullptr && + vehicle_sim_apis_.findOrDefault("", nullptr) == nullptr); + } + + void makeDefaultVehicle(const std::string &vehicle_name) { + vehicle_apis_.insert_or_assign("", vehicle_apis_.at(vehicle_name)); + vehicle_sim_apis_.insert_or_assign("", vehicle_sim_apis_.at(vehicle_name)); + } + + private: + WorldSimApiBase *world_sim_api_; + + common_utils::UniqueValueMap vehicle_apis_; + common_utils::UniqueValueMap vehicle_sim_apis_; +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif \ No newline at end of file diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/api/ApiServerBase.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/api/ApiServerBase.hpp new file mode 100644 index 00000000..49eec51e --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/api/ApiServerBase.hpp @@ -0,0 +1,24 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_api_ApiServerBase_hpp +#define autonomylib_api_ApiServerBase_hpp + +#include "common/Common.hpp" +#include + +namespace nervosys { +namespace autonomylib { + +class ApiServerBase { + public: + virtual void start(bool block, std::size_t thread_count) = 0; + virtual void stop() = 0; + + virtual ~ApiServerBase() = default; +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/api/RpcLibAdaptorsBase.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/api/RpcLibAdaptorsBase.hpp new file mode 100644 index 00000000..e285f24f --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/api/RpcLibAdaptorsBase.hpp @@ -0,0 +1,784 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_api_RpcLibAdaptorsBase_hpp +#define autonomylib_api_RpcLibAdaptorsBase_hpp + +#include "api/WorldSimApiBase.hpp" +#include "common/Common.hpp" +#include "common/CommonStructs.hpp" +#include "common/ImageCaptureBase.hpp" +#include "physics/Environment.hpp" +#include "physics/Kinematics.hpp" +#include "safety/SafetyEval.hpp" + +#include "common/utils/WindowsApisCommonPost.hpp" +#include "common/utils/WindowsApisCommonPre.hpp" +#include "rpc/msgpack.hpp" + +namespace nervosys { +namespace autonomylib_rpclib { + +class RpcLibAdaptorsBase { + public: + template static void to(const std::vector &s, std::vector &d) { + d.clear(); + for (size_t i = 0; i < s.size(); ++i) + d.push_back(s.at(i).to()); + } + + template static void from(const std::vector &s, std::vector &d) { + d.clear(); + for (size_t i = 0; i < s.size(); ++i) + d.push_back(TDest(s.at(i))); + } + + struct Vector2r { + nervosys::autonomylib::real_T x_val = 0, y_val = 0; + MSGPACK_DEFINE_MAP(x_val, y_val); + + Vector2r() {} + + Vector2r(const nervosys::autonomylib::Vector2r &s) { + x_val = s.x(); + y_val = s.y(); + } + nervosys::autonomylib::Vector2r to() const { return nervosys::autonomylib::Vector2r(x_val, y_val); } + }; + + struct Vector3r { + nervosys::autonomylib::real_T x_val = 0, y_val = 0, z_val = 0; + MSGPACK_DEFINE_MAP(x_val, y_val, z_val); + + Vector3r() {} + + Vector3r(const nervosys::autonomylib::Vector3r &s) { + x_val = s.x(); + y_val = s.y(); + z_val = s.z(); + } + nervosys::autonomylib::Vector3r to() const { return nervosys::autonomylib::Vector3r(x_val, y_val, z_val); } + }; + + struct CollisionInfo { + bool has_collided = false; + Vector3r normal; + Vector3r impact_point; + Vector3r position; + nervosys::autonomylib::real_T penetration_depth = 0; + nervosys::autonomylib::TTimePoint time_stamp = 0; + std::string object_name; + int object_id = -1; + + MSGPACK_DEFINE_MAP(has_collided, penetration_depth, time_stamp, normal, impact_point, position, object_name, + object_id); + + CollisionInfo() {} + + CollisionInfo(const nervosys::autonomylib::CollisionInfo &s) { + has_collided = s.has_collided; + normal = s.normal; + impact_point = s.impact_point; + position = s.position; + penetration_depth = s.penetration_depth; + time_stamp = s.time_stamp; + object_name = s.object_name; + object_id = s.object_id; + } + + nervosys::autonomylib::CollisionInfo to() const { + return nervosys::autonomylib::CollisionInfo(has_collided, normal.to(), impact_point.to(), position.to(), + penetration_depth, time_stamp, object_name, object_id); + } + }; + + struct Quaternionr { + nervosys::autonomylib::real_T w_val = 1, x_val = 0, y_val = 0, z_val = 0; + MSGPACK_DEFINE_MAP(w_val, x_val, y_val, z_val); + + Quaternionr() {} + + Quaternionr(const nervosys::autonomylib::Quaternionr &s) { + w_val = s.w(); + x_val = s.x(); + y_val = s.y(); + z_val = s.z(); + } + nervosys::autonomylib::Quaternionr to() const { + return nervosys::autonomylib::Quaternionr(w_val, x_val, y_val, z_val); + } + }; + + struct Pose { + Vector3r position; + Quaternionr orientation; + MSGPACK_DEFINE_MAP(position, orientation); + + Pose() {} + Pose(const nervosys::autonomylib::Pose &s) { + position = s.position; + orientation = s.orientation; + } + nervosys::autonomylib::Pose to() const { return nervosys::autonomylib::Pose(position.to(), orientation.to()); } + }; + + struct GeoPoint { + double latitude = 0, longitude = 0; + float altitude = 0; + MSGPACK_DEFINE_MAP(latitude, longitude, altitude); + + GeoPoint() {} + + GeoPoint(const nervosys::autonomylib::GeoPoint &s) { + latitude = s.latitude; + longitude = s.longitude; + altitude = s.altitude; + } + nervosys::autonomylib::GeoPoint to() const { + return nervosys::autonomylib::GeoPoint(latitude, longitude, altitude); + } + }; + + struct RCData { + uint64_t timestamp = 0; + float pitch = 0, roll = 0, throttle = 0, yaw = 0; + float left_z = 0, right_z = 0; + uint16_t switches = 0; + std::string vendor_id = ""; + bool is_initialized = false; // is RC connected? + bool is_valid = false; // must be true for data to be valid + + MSGPACK_DEFINE_MAP(timestamp, pitch, roll, throttle, yaw, left_z, right_z, switches, vendor_id, is_initialized, + is_valid); + + RCData() {} + + RCData(const nervosys::autonomylib::RCData &s) { + timestamp = s.timestamp; + pitch = s.pitch; + roll = s.roll; + throttle = s.throttle; + yaw = s.yaw; + left_z = s.left_z; + right_z = s.right_z; + switches = s.switches; + vendor_id = s.vendor_id; + is_initialized = s.is_initialized; + is_valid = s.is_valid; + } + + nervosys::autonomylib::RCData to() const { + nervosys::autonomylib::RCData d; + d.timestamp = timestamp; + d.pitch = pitch; + d.roll = roll; + d.throttle = throttle; + d.yaw = yaw; + d.left_z = left_z; + d.right_z = right_z; + d.switches = switches; + d.vendor_id = vendor_id; + d.is_initialized = is_initialized; + d.is_valid = is_valid; + + return d; + } + }; + + struct ProjectionMatrix { + float matrix[4][4]; + + MSGPACK_DEFINE_MAP(matrix); + + ProjectionMatrix() {} + + ProjectionMatrix(const nervosys::autonomylib::ProjectionMatrix &s) { + for (auto i = 0; i < 4; ++i) + for (auto j = 0; j < 4; ++j) + matrix[i][j] = s.matrix[i][j]; + } + + nervosys::autonomylib::ProjectionMatrix to() const { + nervosys::autonomylib::ProjectionMatrix s; + for (auto i = 0; i < 4; ++i) + for (auto j = 0; j < 4; ++j) + s.matrix[i][j] = matrix[i][j]; + return s; + } + }; + + struct Box2D { + Vector2r min; + Vector2r max; + + MSGPACK_DEFINE_MAP(min, max); + + Box2D() {} + + Box2D(const nervosys::autonomylib::Box2D &s) { + min = s.min; + max = s.max; + } + + nervosys::autonomylib::Box2D to() const { + nervosys::autonomylib::Box2D s; + s.min = min.to(); + s.max = max.to(); + + return s; + } + }; + + struct Box3D { + Vector3r min; + Vector3r max; + + MSGPACK_DEFINE_MAP(min, max); + + Box3D() {} + + Box3D(const nervosys::autonomylib::Box3D &s) { + min = s.min; + max = s.max; + } + + nervosys::autonomylib::Box3D to() const { + nervosys::autonomylib::Box3D s; + s.min = min.to(); + s.max = max.to(); + + return s; + } + }; + + struct DetectionInfo { + std::string name; + GeoPoint geo_point; + Box2D box2D; + Box3D box3D; + Pose relative_pose; + + MSGPACK_DEFINE_MAP(name, geo_point, box2D, box3D, relative_pose); + + DetectionInfo() {} + + DetectionInfo(const nervosys::autonomylib::DetectionInfo &d) { + name = d.name; + geo_point = d.geo_point; + box2D = d.box2D; + box3D = d.box3D; + relative_pose = d.relative_pose; + } + + nervosys::autonomylib::DetectionInfo to() const { + nervosys::autonomylib::DetectionInfo d; + d.name = name; + d.geo_point = geo_point.to(); + d.box2D = box2D.to(); + d.box3D = box3D.to(); + d.relative_pose = relative_pose.to(); + + return d; + } + + static std::vector from(const std::vector &request) { + std::vector request_adaptor; + for (const auto &item : request) + request_adaptor.push_back(DetectionInfo(item)); + + return request_adaptor; + } + static std::vector to(const std::vector &request_adapter) { + std::vector request; + for (const auto &item : request_adapter) + request.push_back(item.to()); + + return request; + } + }; + + struct CameraInfo { + Pose pose; + float fov; + ProjectionMatrix proj_mat; + + MSGPACK_DEFINE_MAP(pose, fov, proj_mat); + + CameraInfo() {} + + CameraInfo(const nervosys::autonomylib::CameraInfo &s) { + pose = s.pose; + fov = s.fov; + proj_mat = ProjectionMatrix(s.proj_mat); + } + + nervosys::autonomylib::CameraInfo to() const { + nervosys::autonomylib::CameraInfo s; + s.pose = pose.to(); + s.fov = fov; + s.proj_mat = proj_mat.to(); + + return s; + } + }; + + struct KinematicsState { + Vector3r position; + Quaternionr orientation; + + Vector3r linear_velocity; + Vector3r angular_velocity; + + Vector3r linear_acceleration; + Vector3r angular_acceleration; + + MSGPACK_DEFINE_MAP(position, orientation, linear_velocity, angular_velocity, linear_acceleration, + angular_acceleration); + + KinematicsState() {} + + KinematicsState(const nervosys::autonomylib::Kinematics::State &s) { + position = s.pose.position; + orientation = s.pose.orientation; + linear_velocity = s.twist.linear; + angular_velocity = s.twist.angular; + linear_acceleration = s.accelerations.linear; + angular_acceleration = s.accelerations.angular; + } + + nervosys::autonomylib::Kinematics::State to() const { + nervosys::autonomylib::Kinematics::State s; + s.pose.position = position.to(); + s.pose.orientation = orientation.to(); + s.twist.linear = linear_velocity.to(); + s.twist.angular = angular_velocity.to(); + s.accelerations.linear = linear_acceleration.to(); + s.accelerations.angular = angular_acceleration.to(); + + return s; + } + }; + + struct EnvironmentState { + Vector3r position; + GeoPoint geo_point; + + // these fields are computed + Vector3r gravity; + float air_pressure; + float temperature; + float air_density; + + MSGPACK_DEFINE_MAP(position, geo_point, gravity, air_pressure, temperature, air_density); + + EnvironmentState() {} + + EnvironmentState(const nervosys::autonomylib::Environment::State &s) { + position = s.position; + geo_point = s.geo_point; + gravity = s.gravity; + air_pressure = s.air_pressure; + temperature = s.temperature; + air_density = s.air_density; + } + + nervosys::autonomylib::Environment::State to() const { + nervosys::autonomylib::Environment::State s; + s.position = position.to(); + s.geo_point = geo_point.to(); + s.gravity = gravity.to(); + s.air_pressure = air_pressure; + s.temperature = temperature; + s.air_density = air_density; + + return s; + } + }; + + struct ImageRequest { + std::string camera_name; + nervosys::autonomylib::ImageCaptureBase::ImageType image_type; + bool pixels_as_float; + bool compress; + + MSGPACK_DEFINE_MAP(camera_name, image_type, pixels_as_float, compress); + + ImageRequest() {} + + ImageRequest(const nervosys::autonomylib::ImageCaptureBase::ImageRequest &s) + : camera_name(s.camera_name), image_type(s.image_type), pixels_as_float(s.pixels_as_float), + compress(s.compress) {} + + nervosys::autonomylib::ImageCaptureBase::ImageRequest to() const { + return {camera_name, image_type, pixels_as_float, compress}; + } + + static std::vector + from(const std::vector &request) { + std::vector request_adaptor; + for (const auto &item : request) + request_adaptor.push_back(ImageRequest(item)); + + return request_adaptor; + } + static std::vector + to(const std::vector &request_adapter) { + std::vector request; + for (const auto &item : request_adapter) + request.push_back(item.to()); + + return request; + } + }; + + struct ImageResponse { + std::vector image_data_uint8; + std::vector image_data_float; + + std::string camera_name; + Vector3r camera_position; + Quaternionr camera_orientation; + nervosys::autonomylib::TTimePoint time_stamp; + std::string message; + bool pixels_as_float; + bool compress; + int width, height; + nervosys::autonomylib::ImageCaptureBase::ImageType image_type; + + MSGPACK_DEFINE_MAP(image_data_uint8, image_data_float, camera_position, camera_name, camera_orientation, + time_stamp, message, pixels_as_float, compress, width, height, image_type); + + ImageResponse() {} + + ImageResponse(const nervosys::autonomylib::ImageCaptureBase::ImageResponse &s) { + pixels_as_float = s.pixels_as_float; + + image_data_uint8 = s.image_data_uint8; + image_data_float = s.image_data_float; + + camera_name = s.camera_name; + camera_position = Vector3r(s.camera_position); + camera_orientation = Quaternionr(s.camera_orientation); + time_stamp = s.time_stamp; + message = s.message; + compress = s.compress; + width = s.width; + height = s.height; + image_type = s.image_type; + } + + nervosys::autonomylib::ImageCaptureBase::ImageResponse to() const { + nervosys::autonomylib::ImageCaptureBase::ImageResponse d; + + d.pixels_as_float = pixels_as_float; + + if (!pixels_as_float) + d.image_data_uint8 = image_data_uint8; + else + d.image_data_float = image_data_float; + + d.camera_name = camera_name; + d.camera_position = camera_position.to(); + d.camera_orientation = camera_orientation.to(); + d.time_stamp = time_stamp; + d.message = message; + d.compress = compress; + d.width = width; + d.height = height; + d.image_type = image_type; + + return d; + } + + static std::vector + to(const std::vector &response_adapter) { + std::vector response; + for (const auto &item : response_adapter) + response.push_back(item.to()); + + return response; + } + static std::vector + from(const std::vector &response) { + std::vector response_adapter; + for (const auto &item : response) + response_adapter.push_back(ImageResponse(item)); + + return response_adapter; + } + }; + + struct LidarData { + + nervosys::autonomylib::TTimePoint time_stamp; // timestamp + std::vector point_cloud; // data + Pose pose; + std::vector segmentation; + + MSGPACK_DEFINE_MAP(time_stamp, point_cloud, pose, segmentation); + + LidarData() {} + + LidarData(const nervosys::autonomylib::LidarData &s) { + time_stamp = s.time_stamp; + point_cloud = s.point_cloud; + pose = s.pose; + segmentation = s.segmentation; + } + + nervosys::autonomylib::LidarData to() const { + nervosys::autonomylib::LidarData d; + + d.time_stamp = time_stamp; + d.point_cloud = point_cloud; + d.pose = pose.to(); + d.segmentation = segmentation; + + return d; + } + }; + + struct ImuData { + nervosys::autonomylib::TTimePoint time_stamp; + Quaternionr orientation; + Vector3r angular_velocity; + Vector3r linear_acceleration; + + MSGPACK_DEFINE_MAP(time_stamp, orientation, angular_velocity, linear_acceleration); + + ImuData() {} + + ImuData(const nervosys::autonomylib::ImuBase::Output &s) { + time_stamp = s.time_stamp; + orientation = s.orientation; + angular_velocity = s.angular_velocity; + linear_acceleration = s.linear_acceleration; + } + + nervosys::autonomylib::ImuBase::Output to() const { + nervosys::autonomylib::ImuBase::Output d; + + d.time_stamp = time_stamp; + d.orientation = orientation.to(); + d.angular_velocity = angular_velocity.to(); + d.linear_acceleration = linear_acceleration.to(); + + return d; + } + }; + + struct BarometerData { + nervosys::autonomylib::TTimePoint time_stamp; + nervosys::autonomylib::real_T altitude; + nervosys::autonomylib::real_T pressure; + nervosys::autonomylib::real_T qnh; + + MSGPACK_DEFINE_MAP(time_stamp, altitude, pressure, qnh); + + BarometerData() {} + + BarometerData(const nervosys::autonomylib::BarometerBase::Output &s) { + time_stamp = s.time_stamp; + altitude = s.altitude; + pressure = s.pressure; + qnh = s.qnh; + } + + nervosys::autonomylib::BarometerBase::Output to() const { + nervosys::autonomylib::BarometerBase::Output d; + + d.time_stamp = time_stamp; + d.altitude = altitude; + d.pressure = pressure; + d.qnh = qnh; + + return d; + } + }; + + struct MagnetometerData { + nervosys::autonomylib::TTimePoint time_stamp; + Vector3r magnetic_field_body; + std::vector magnetic_field_covariance; // not implemented in MagnetometerBase.hpp + + MSGPACK_DEFINE_MAP(time_stamp, magnetic_field_body, magnetic_field_covariance); + + MagnetometerData() {} + + MagnetometerData(const nervosys::autonomylib::MagnetometerBase::Output &s) { + time_stamp = s.time_stamp; + magnetic_field_body = s.magnetic_field_body; + magnetic_field_covariance = s.magnetic_field_covariance; + } + + nervosys::autonomylib::MagnetometerBase::Output to() const { + nervosys::autonomylib::MagnetometerBase::Output d; + + d.time_stamp = time_stamp; + d.magnetic_field_body = magnetic_field_body.to(); + d.magnetic_field_covariance = magnetic_field_covariance; + + return d; + } + }; + + struct GnssReport { + GeoPoint geo_point; + nervosys::autonomylib::real_T eph = 0.0, epv = 0.0; + Vector3r velocity; + nervosys::autonomylib::GpsBase::GnssFixType fix_type; + uint64_t time_utc = 0; + + MSGPACK_DEFINE_MAP(geo_point, eph, epv, velocity, fix_type, time_utc); + + GnssReport() {} + + GnssReport(const nervosys::autonomylib::GpsBase::GnssReport &s) { + geo_point = s.geo_point; + eph = s.eph; + epv = s.epv; + velocity = s.velocity; + fix_type = s.fix_type; + time_utc = s.time_utc; + } + + nervosys::autonomylib::GpsBase::GnssReport to() const { + nervosys::autonomylib::GpsBase::GnssReport d; + + d.geo_point = geo_point.to(); + d.eph = eph; + d.epv = epv; + d.velocity = velocity.to(); + d.fix_type = fix_type; + d.time_utc = time_utc; + + return d; + } + }; + + struct GpsData { + nervosys::autonomylib::TTimePoint time_stamp; + GnssReport gnss; + bool is_valid = false; + + MSGPACK_DEFINE_MAP(time_stamp, gnss, is_valid); + + GpsData() {} + + GpsData(const nervosys::autonomylib::GpsBase::Output &s) { + time_stamp = s.time_stamp; + gnss = s.gnss; + is_valid = s.is_valid; + } + + nervosys::autonomylib::GpsBase::Output to() const { + nervosys::autonomylib::GpsBase::Output d; + + d.time_stamp = time_stamp; + d.gnss = gnss.to(); + d.is_valid = is_valid; + + return d; + } + }; + + struct DistanceSensorData { + nervosys::autonomylib::TTimePoint time_stamp; + nervosys::autonomylib::real_T distance; // meters + nervosys::autonomylib::real_T min_distance; // m + nervosys::autonomylib::real_T max_distance; // m + Pose relative_pose; + + MSGPACK_DEFINE_MAP(time_stamp, distance, min_distance, max_distance, relative_pose); + + DistanceSensorData() {} + + DistanceSensorData(const nervosys::autonomylib::DistanceSensorData &s) { + time_stamp = s.time_stamp; + distance = s.distance; + min_distance = s.min_distance; + max_distance = s.max_distance; + relative_pose = s.relative_pose; + } + + nervosys::autonomylib::DistanceSensorData to() const { + nervosys::autonomylib::DistanceSensorData d; + + d.time_stamp = time_stamp; + d.distance = distance; + d.min_distance = min_distance; + d.max_distance = max_distance; + d.relative_pose = relative_pose.to(); + + return d; + } + }; + + struct MeshPositionVertexBuffersResponse { + Vector3r position; + Quaternionr orientation; + + std::vector vertices; + std::vector indices; + std::string name; + + MSGPACK_DEFINE_MAP(position, orientation, vertices, indices, name); + + MeshPositionVertexBuffersResponse() {} + + MeshPositionVertexBuffersResponse(const nervosys::autonomylib::MeshPositionVertexBuffersResponse &s) { + position = Vector3r(s.position); + orientation = Quaternionr(s.orientation); + + vertices = s.vertices; + indices = s.indices; + + if (vertices.size() == 0) + vertices.push_back(0); + if (indices.size() == 0) + indices.push_back(0); + + name = s.name; + } + + nervosys::autonomylib::MeshPositionVertexBuffersResponse to() const { + nervosys::autonomylib::MeshPositionVertexBuffersResponse d; + d.position = position.to(); + d.orientation = orientation.to(); + d.vertices = vertices; + d.indices = indices; + d.name = name; + + return d; + } + + static std::vector + to(const std::vector &response_adapter) { + std::vector response; + for (const auto &item : response_adapter) + response.push_back(item.to()); + + return response; + } + + static std::vector + from(const std::vector &response) { + std::vector response_adapter; + for (const auto &item : response) + response_adapter.push_back(MeshPositionVertexBuffersResponse(item)); + + return response_adapter; + } + }; +}; + +} // namespace autonomylib_rpclib +} // namespace nervosys + +MSGPACK_ADD_ENUM(nervosys::autonomylib::SafetyEval::SafetyViolationType_); +MSGPACK_ADD_ENUM(nervosys::autonomylib::SafetyEval::ObsAvoidanceStrategy); +MSGPACK_ADD_ENUM(nervosys::autonomylib::ImageCaptureBase::ImageType); +MSGPACK_ADD_ENUM(nervosys::autonomylib::WorldSimApiBase::WeatherParameter); +MSGPACK_ADD_ENUM(nervosys::autonomylib::GpsBase::GnssFixType); + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/api/RpcLibClientBase.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/api/RpcLibClientBase.hpp new file mode 100644 index 00000000..1cc3665f --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/api/RpcLibClientBase.hpp @@ -0,0 +1,224 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_api_RpcLibClientBase_hpp +#define autonomylib_api_RpcLibClientBase_hpp + +#include "api/WorldSimApiBase.hpp" +#include "common/Common.hpp" +#include "common/CommonStructs.hpp" +#include "common/ImageCaptureBase.hpp" +#include "physics/Environment.hpp" +#include "physics/Kinematics.hpp" +#include "sensors/barometer/BarometerBase.hpp" +#include "sensors/distance/DistanceBase.hpp" +#include "sensors/gps/GpsBase.hpp" +#include "sensors/imu/ImuBase.hpp" +#include "sensors/magnetometer/MagnetometerBase.hpp" + +namespace nervosys { +namespace autonomylib { + +// common methods for RCP clients of different vehicles +class RpcLibClientBase { + public: + enum class ConnectionState : uint { Initial = 0, Connected, Disconnected, Reset, Unknown }; + + public: + RpcLibClientBase(const string &ip_address = "localhost", uint16_t port = RpcLibPort, float timeout_sec = 60); + virtual ~RpcLibClientBase(); // required for pimpl + + void confirmConnection(); + void reset(); + + ConnectionState getConnectionState(); + bool ping(); + int getClientVersion() const; + int getServerVersion() const; + int getMinRequiredServerVersion() const; + int getMinRequiredClientVersion() const; + + bool simIsPaused() const; + void simPause(bool is_paused); + void simContinueForTime(double seconds); + void simContinueForFrames(uint32_t frames); + + void simSetTimeOfDay(bool is_enabled, const string &start_datetime = "", bool is_start_datetime_dst = false, + float celestial_clock_speed = 1, float update_interval_secs = 60, bool move_sun = true); + + void simEnableWeather(bool enable); + void simSetWeatherParameter(WorldSimApiBase::WeatherParameter param, float val); + + vector simListSceneObjects(const string &name_regex = string(".*")) const; + Pose simGetObjectPose(const std::string &object_name) const; + bool simLoadLevel(const string &level_name); + Vector3r simGetObjectScale(const std::string &object_name) const; + bool simSetObjectPose(const std::string &object_name, const Pose &pose, bool teleport = true); + bool simSetObjectScale(const std::string &object_name, const Vector3r &scale); + std::string simSpawnObject(const std::string &object_name, const std::string &load_component, const Pose &pose, + const Vector3r &scale, bool physics_enabled); + bool simDestroyObject(const std::string &object_name); + + // task management APIs + void cancelLastTask(const std::string &vehicle_name = ""); + virtual RpcLibClientBase *waitOnLastTask(bool *task_result = nullptr, float timeout_sec = Utils::nan()); + + bool simSetSegmentationObjectID(const std::string &mesh_name, int object_id, bool is_name_regex = false); + int simGetSegmentationObjectID(const std::string &mesh_name) const; + void simPrintLogMessage(const std::string &message, std::string message_param = "", unsigned char severity = 0); + + void simAddDetectionFilterMeshName(const std::string &camera_name, ImageCaptureBase::ImageType type, + const std::string &mesh_name, const std::string &vehicle_name = "", + bool external = false); + void simSetDetectionFilterRadius(const std::string &camera_name, ImageCaptureBase::ImageType type, + const float radius_cm, const std::string &vehicle_name = "", + bool external = false); + void simClearDetectionMeshNames(const std::string &camera_name, ImageCaptureBase::ImageType type, + const std::string &vehicle_name = "", bool external = false); + vector simGetDetections(const std::string &camera_name, ImageCaptureBase::ImageType image_type, + const std::string &vehicle_name = "", bool external = false); + + void simFlushPersistentMarkers(); + void simPlotPoints(const vector &points, const vector &color_rgba, float size, float duration, + bool is_persistent); + void simPlotLineStrip(const vector &points, const vector &color_rgba, float thickness, + float duration, bool is_persistent); + void simPlotLineList(const vector &points, const vector &color_rgba, float thickness, + float duration, bool is_persistent); + void simPlotArrows(const vector &points_start, const vector &points_end, + const vector &color_rgba, float thickness, float arrow_size, float duration, + bool is_persistent); + void simPlotStrings(const vector &strings, const vector &positions, float scale, + const vector &color_rgba, float duration); + void simPlotTransforms(const vector &poses, float scale, float thickness, float duration, bool is_persistent); + void simPlotTransformsWithNames(const vector &poses, const vector &names, float tf_scale, + float tf_thickness, float text_scale, const vector &text_color_rgba, + float duration); + + bool armDisarm(bool arm, const std::string &vehicle_name = ""); + bool isApiControlEnabled(const std::string &vehicle_name = "") const; + void enableApiControl(bool is_enabled, const std::string &vehicle_name = ""); + + nervosys::autonomylib::GeoPoint getHomeGeoPoint(const std::string &vehicle_name = "") const; + + bool simRunConsoleCommand(const std::string &command); + + // sensor APIs + nervosys::autonomylib::LidarData getLidarData(const std::string &lidar_name = "", + const std::string &vehicle_name = "") const; + nervosys::autonomylib::ImuBase::Output getImuData(const std::string &imu_name = "", + const std::string &vehicle_name = "") const; + nervosys::autonomylib::BarometerBase::Output getBarometerData(const std::string &barometer_name = "", + const std::string &vehicle_name = "") const; + nervosys::autonomylib::MagnetometerBase::Output getMagnetometerData(const std::string &magnetometer_name = "", + const std::string &vehicle_name = "") const; + nervosys::autonomylib::GpsBase::Output getGpsData(const std::string &gps_name = "", + const std::string &vehicle_name = "") const; + nervosys::autonomylib::DistanceSensorData getDistanceSensorData(const std::string &distance_sensor_name = "", + const std::string &vehicle_name = "") const; + + Pose simGetVehiclePose(const std::string &vehicle_name = "") const; + void simSetVehiclePose(const Pose &pose, bool ignore_collision, const std::string &vehicle_name = ""); + void simSetTraceLine(const std::vector &color_rgba, float thickness = 3.0f, + const std::string &vehicle_name = ""); + + vector simGetImages(vector request, + const std::string &vehicle_name = "", bool external = false); + vector simGetImage(const std::string &camera_name, ImageCaptureBase::ImageType type, + const std::string &vehicle_name = "", bool external = false); + + // CinemAutonomySim + std::vector simGetPresetLensSettings(const std::string &camera_name, + const std::string &vehicle_name = "", bool external = false); + std::string simGetLensSettings(const std::string &camera_name, const std::string &vehicle_name = "", + bool external = false); + void simSetPresetLensSettings(const std::string &preset_lens_settings, const std::string &camera_name, + const std::string &vehicle_name = "", bool external = false); + std::vector simGetPresetFilmbackSettings(const std::string &camera_name, + const std::string &vehicle_name = "", bool external = false); + void simSetPresetFilmbackSettings(const std::string &preset_filmback_settings, const std::string &camera_name, + const std::string &vehicle_name = "", bool external = false); + std::string simGetFilmbackSettings(const std::string &camera_name, const std::string &vehicle_name = "", + bool external = false); + float simSetFilmbackSettings(const float sensor_width, const float sensor_heigth, const std::string &camera_name, + const std::string &vehicle_name = "", bool external = false); + float simGetFocalLength(const std::string &camera_name, const std::string &vehicle_name = "", + bool external = false); + void simSetFocalLength(float focal_length, const std::string &camera_name, const std::string &vehicle_name = "", + bool external = false); + void simEnableManualFocus(const bool enable, const std::string &camera_name, const std::string &vehicle_name = "", + bool external = false); + float simGetFocusDistance(const std::string &camera_name, const std::string &vehicle_name = "", + bool external = false); + void simSetFocusDistance(float focus_distance, const std::string &camera_name, const std::string &vehicle_name = "", + bool external = false); + float simGetFocusAperture(const std::string &camera_name, const std::string &vehicle_name = "", + bool external = false); + void simSetFocusAperture(const float focus_aperture, const std::string &camera_name, + const std::string &vehicle_name = "", bool external = false); + void simEnableFocusPlane(const bool enable, const std::string &camera_name, const std::string &vehicle_name = "", + bool external = false); + std::string simGetCurrentFieldOfView(const std::string &camera_name, const std::string &vehicle_name = "", + bool external = false); + // end CinemAutonomySim + bool simTestLineOfSightToPoint(const nervosys::autonomylib::GeoPoint &point, const std::string &vehicle_name = ""); + bool simTestLineOfSightBetweenPoints(const nervosys::autonomylib::GeoPoint &point1, + const nervosys::autonomylib::GeoPoint &point2); + vector simGetWorldExtents(); + + vector simGetMeshPositionVertexBuffers(); + bool simAddVehicle(const std::string &vehicle_name, const std::string &vehicle_type, const Pose &pose, + const std::string &pawn_path = ""); + + CollisionInfo simGetCollisionInfo(const std::string &vehicle_name = "") const; + + CameraInfo simGetCameraInfo(const std::string &camera_name, const std::string &vehicle_name = "", + bool external = false) const; + void simSetDistortionParam(const std::string &camera_name, const std::string ¶m_name, float value, + const std::string &vehicle_name = "", bool external = false); + std::vector simGetDistortionParams(const std::string &camera_name, const std::string &vehicle_name = "", + bool external = false); + void simSetCameraPose(const std::string &camera_name, const Pose &pose, const std::string &vehicle_name = "", + bool external = false); + void simSetCameraFov(const std::string &camera_name, float fov_degrees, const std::string &vehicle_name = "", + bool external = false); + + bool simCreateVoxelGrid(const Vector3r &position, const int &x_size, const int &y_size, const int &z_size, + const float &res, const std::string &output_file); + nervosys::autonomylib::Kinematics::State simGetGroundTruthKinematics(const std::string &vehicle_name = "") const; + void simSetKinematics(const Kinematics::State &state, bool ignore_collision, const std::string &vehicle_name = ""); + nervosys::autonomylib::Environment::State simGetGroundTruthEnvironment(const std::string &vehicle_name = "") const; + std::vector simSwapTextures(const std::string &tags, int tex_id = 0, int component_id = 0, + int material_id = 0); + bool simSetObjectMaterial(const std::string &object_name, const std::string &material_name, + const int component_id = 0); + bool simSetObjectMaterialFromTexture(const std::string &object_name, const std::string &texture_path, + const int component_id = 0); + + // Recording APIs + void startRecording(); + void stopRecording(); + bool isRecording(); + + void simSetWind(const Vector3r &wind) const; + void simSetExtForce(const Vector3r &ext_force) const; + + vector listVehicles(); + + std::string getSettingsString() const; + + std::vector simListAssets() const; + + protected: + void *getClient(); + const void *getClient() const; + + private: + struct impl; + std::unique_ptr pimpl_; +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/api/RpcLibServerBase.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/api/RpcLibServerBase.hpp new file mode 100644 index 00000000..0171d50d --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/api/RpcLibServerBase.hpp @@ -0,0 +1,67 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_api_RpcLibServerBase_hpp +#define autonomylib_api_RpcLibServerBase_hpp + +#include "api/ApiProvider.hpp" +#include "api/ApiServerBase.hpp" +#include "common/Common.hpp" + +namespace nervosys { +namespace autonomylib { + +class RpcLibServerBase : public ApiServerBase { + public: + RpcLibServerBase(ApiProvider *api_provider, const std::string &server_address, uint16_t port = RpcLibPort); + virtual ~RpcLibServerBase() override; + + virtual void start(bool block, std::size_t thread_count) override; + virtual void stop() override; + + class ApiNotSupported : public std::runtime_error { + public: + ApiNotSupported(const std::string &message) : std::runtime_error(message) {} + }; + + protected: + void *getServer() const; + + virtual VehicleApiBase *getVehicleApi(const std::string &vehicle_name) { + auto *api = api_provider_->getVehicleApi(vehicle_name); + if (api) + return api; + else + throw ApiNotSupported("Vehicle API for '" + vehicle_name + + "' is not available. This could either because this is simulation-only API or this " + "vehicle does not exist"); + } + virtual VehicleSimApiBase *getVehicleSimApi(const std::string &vehicle_name) { + auto *api = api_provider_->getVehicleSimApi(vehicle_name); + if (api) + return api; + else + throw ApiNotSupported("Vehicle Sim-API for '" + vehicle_name + + "' is not available. This could either because this is not a simulation or this " + "vehicle does not exist"); + } + virtual WorldSimApiBase *getWorldSimApi() { + auto *api = api_provider_->getWorldSimApi(); + if (api) + return api; + else + throw ApiNotSupported("World-Sim API " + "' is not available. This could be because this is not a simulation"); + } + + private: + ApiProvider *api_provider_; + + struct impl; + std::unique_ptr pimpl_; +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif \ No newline at end of file diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/api/VehicleApiBase.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/api/VehicleApiBase.hpp new file mode 100644 index 00000000..28f1f8a4 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/api/VehicleApiBase.hpp @@ -0,0 +1,199 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_api_VehicleApiBase_hpp +#define autonomylib_api_VehicleApiBase_hpp + +#include "common/Common.hpp" +#include "common/CommonStructs.hpp" +#include "common/ImageCaptureBase.hpp" +#include "common/UpdatableObject.hpp" +#include "common/Waiter.hpp" +#include "safety/SafetyEval.hpp" +#include "sensors/SensorCollection.hpp" +#include "sensors/barometer/BarometerBase.hpp" +#include "sensors/distance/DistanceBase.hpp" +#include "sensors/gps/GpsBase.hpp" +#include "sensors/imu/ImuBase.hpp" +#include "sensors/lidar/LidarBase.hpp" +#include "sensors/magnetometer/MagnetometerBase.hpp" +#include +#include + +namespace nervosys { +namespace autonomylib { + +/* +Vehicle controller allows to obtain state from vehicle and send control commands to the vehicle. +State can include many things including sensor data, logs, estimated state from onboard computer etc. +Control commands can be low level actuation commands or high level movement commands. +The base class defines usually available methods that all vehicle controllers may implement. +Some methods may not be applicable to specific vehicle in which case an exception may be raised or call may be ignored. +*/ +class VehicleApiBase : public UpdatableObject { + public: + virtual void enableApiControl(bool is_enabled) = 0; + virtual bool isApiControlEnabled() const = 0; + virtual bool armDisarm(bool arm) = 0; + virtual GeoPoint getHomeGeoPoint() const = 0; + + virtual void update() override { UpdatableObject::update(); } + + virtual void cancelLastTask() { + // if derived class supports async task then override this method + } + + virtual bool isReady(std::string &message) const { + unused(message); + return true; + } + + virtual bool canArm() const { return true; } + + // if vehicle supports it, call this method to send + // kinematics and other info to somewhere (ex. log viewer, file, cloud etc) + virtual void sendTelemetry(float last_interval = -1) { + // no default action + unused(last_interval); + } + + // below APIs are used by FastPhysicsEngine + virtual real_T getActuation(unsigned int actuator_index) const { + unused(actuator_index); + throw VehicleCommandNotImplementedException("getActuation API is not supported for this vehicle"); + } + + virtual size_t getActuatorCount() const { + throw VehicleCommandNotImplementedException("getActuatorCount API is not supported for this vehicle"); + } + + virtual void getStatusMessages(std::vector &messages) { + unused(messages); + // default implementation + } + + /* + For RCs, there are two cases: (1) vehicle may be configured to use + RC bound to its hardware (2) vehicle may be configured to get RC data + supplied via API calls. Below two APIs are not symmetrical, i.e., + getRCData() may or may not return same thing as setRCData(). + */ + // get reading from RC bound to vehicle (if unsupported then RCData::is_valid = false) + virtual RCData getRCData() const { + static const RCData invalid_rc_data{}; + return invalid_rc_data; + } + + // set external RC data to vehicle (if unsupported then returns false) + virtual bool setRCData(const RCData &rc_data) { + unused(rc_data); + return false; + } + + // Sensors APIs + virtual const SensorCollection &getSensors() const { + throw VehicleCommandNotImplementedException("getSensors API is not supported for this vehicle"); + } + + // Lidar APIs + virtual const LidarData &getLidarData(const std::string &lidar_name) const { + auto *lidar = static_cast(findSensorByName(lidar_name, SensorBase::SensorType::Lidar)); + if (lidar == nullptr) { + throw VehicleControllerException( + Utils::stringf("No lidar with name %s exist on vehicle", lidar_name.c_str())); + } + return lidar->getOutput(); + } + + // IMU API + virtual const ImuBase::Output &getImuData(const std::string &imu_name) const { + auto *imu = static_cast(findSensorByName(imu_name, SensorBase::SensorType::Imu)); + if (imu == nullptr) { + throw VehicleControllerException(Utils::stringf("No IMU with name %s exist on vehicle", imu_name.c_str())); + } + return imu->getOutput(); + } + + // Barometer API + virtual const BarometerBase::Output &getBarometerData(const std::string &barometer_name) const { + auto *barometer = + static_cast(findSensorByName(barometer_name, SensorBase::SensorType::Barometer)); + if (barometer == nullptr) { + throw VehicleControllerException( + Utils::stringf("No barometer with name %s exist on vehicle", barometer_name.c_str())); + } + return barometer->getOutput(); + } + + // Magnetometer API + virtual const MagnetometerBase::Output &getMagnetometerData(const std::string &magnetometer_name) const { + auto *magnetometer = static_cast( + findSensorByName(magnetometer_name, SensorBase::SensorType::Magnetometer)); + if (magnetometer == nullptr) { + throw VehicleControllerException( + Utils::stringf("No magnetometer with name %s exist on vehicle", magnetometer_name.c_str())); + } + return magnetometer->getOutput(); + } + + // Gps API + virtual const GpsBase::Output &getGpsData(const std::string &gps_name) const { + auto *gps = static_cast(findSensorByName(gps_name, SensorBase::SensorType::Gps)); + if (gps == nullptr) { + throw VehicleControllerException(Utils::stringf("No gps with name %s exist on vehicle", gps_name.c_str())); + } + return gps->getOutput(); + } + + // Distance Sensor API + virtual const DistanceSensorData &getDistanceSensorData(const std::string &distance_sensor_name) const { + auto *distance_sensor = + static_cast(findSensorByName(distance_sensor_name, SensorBase::SensorType::Distance)); + if (distance_sensor == nullptr) { + throw VehicleControllerException( + Utils::stringf("No distance sensor with name %s exist on vehicle", distance_sensor_name.c_str())); + } + return distance_sensor->getOutput(); + } + + virtual ~VehicleApiBase() = default; + + // exceptions + class VehicleControllerException : public std::runtime_error { + public: + VehicleControllerException(const std::string &message) : runtime_error(message) {} + }; + + class VehicleCommandNotImplementedException : public VehicleControllerException { + public: + VehicleCommandNotImplementedException(const std::string &message) : VehicleControllerException(message) {} + }; + + class VehicleMoveException : public VehicleControllerException { + public: + VehicleMoveException(const std::string &message) : VehicleControllerException(message) {} + }; + + private: + const SensorBase *findSensorByName(const std::string &sensor_name, const SensorBase::SensorType type) const { + const SensorBase *sensor = nullptr; + + // Find sensor with the given name (for empty input name, return the first one found) + // Not efficient but should suffice given small number of sensors + uint count_sensors = getSensors().size(type); + for (uint i = 0; i < count_sensors; i++) { + const SensorBase *current_sensor = getSensors().getByType(type, i); + if (current_sensor != nullptr && (current_sensor->getName() == sensor_name || sensor_name == "")) { + sensor = current_sensor; + break; + } + } + + return sensor; + } +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/api/VehicleSimApiBase.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/api/VehicleSimApiBase.hpp new file mode 100644 index 00000000..5284b9ec --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/api/VehicleSimApiBase.hpp @@ -0,0 +1,70 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_api_VehicleSimApiBase_hpp +#define autonomylib_api_VehicleSimApiBase_hpp + +#include "common/AutonomySimSettings.hpp" +#include "common/CommonStructs.hpp" +#include "common/ImageCaptureBase.hpp" +#include "common/UpdatableObject.hpp" +#include "physics/Environment.hpp" +#include "physics/Kinematics.hpp" + +namespace nervosys { +namespace autonomylib { + +class VehicleSimApiBase : public nervosys::autonomylib::UpdatableObject { + public: + virtual ~VehicleSimApiBase() = default; + + virtual void update() override { UpdatableObject::update(); } + + // this method is called at every render tick when we want to transfer state from + // physics engine to render engine. As physics engine is halted while + // this happens, this method should do minimal processing + virtual void updateRenderedState(float dt) { + unused(dt); + // derived class should override if needed + } + + // called when render changes are required at every render tick + virtual void updateRendering(float dt) { + unused(dt); + // derived class should override if needed + } + + virtual const ImageCaptureBase *getImageCapture() const = 0; + virtual ImageCaptureBase *getImageCapture() { + return const_cast(static_cast(this)->getImageCapture()); + } + + virtual void initialize() = 0; + + virtual bool testLineOfSightToPoint(const GeoPoint &point) const = 0; + + virtual Pose getPose() const = 0; + virtual void setPose(const Pose &pose, bool ignore_collision) = 0; + virtual const Kinematics::State *getGroundTruthKinematics() const = 0; + virtual void setKinematics(const Kinematics::State &state, bool ignore_collision) = 0; + virtual const nervosys::autonomylib::Environment *getGroundTruthEnvironment() const = 0; + + virtual CollisionInfo getCollisionInfo() const = 0; + virtual CollisionInfo getCollisionInfoAndReset() = 0; + virtual int getRemoteControlID() const = 0; // which RC to use, 0 is first one, -1 means disable RC (use keyborad) + virtual RCData getRCData() const = 0; // get reading from RC from simulator's host OS + virtual std::string getVehicleName() const = 0; + virtual std::string getRecordFileLine(bool is_header_line) const = 0; + virtual void toggleTrace() = 0; + virtual void setTraceLine(const std::vector &color_rgba, float thickness) = 0; + + // use pointer here because of derived classes for VehicleSetting + const AutonomySimSettings::VehicleSetting *getVehicleSetting() const { + return AutonomySimSettings::singleton().getVehicleSetting(getVehicleName()); + } +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/api/WorldSimApiBase.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/api/WorldSimApiBase.hpp new file mode 100644 index 00000000..059e9d20 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/api/WorldSimApiBase.hpp @@ -0,0 +1,155 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_api_WorldSimApiBase_hpp +#define autonomylib_api_WorldSimApiBase_hpp + +#include "common/CommonStructs.hpp" +#include "common/ImageCaptureBase.hpp" + +namespace nervosys { +namespace autonomylib { + +class WorldSimApiBase { + public: + enum class WeatherParameter { + Rain = 0, + Roadwetness = 1, + Snow = 2, + RoadSnow = 3, + MapleLeaf = 4, + RoadLeaf = 5, + Dust = 6, + Fog = 7, + Enabled = 8 + }; + + virtual ~WorldSimApiBase() = default; + + // ------ Level setting apis ----- // + virtual bool loadLevel(const std::string &level_name) = 0; + virtual string spawnObject(const std::string &object_name, const std::string &load_component, const Pose &pose, + const Vector3r &scale, bool physics_enabled, bool is_blueprint) = 0; + virtual bool destroyObject(const std::string &object_name) = 0; + virtual std::vector listAssets() const = 0; + + virtual bool isPaused() const = 0; + virtual void reset() = 0; + virtual void pause(bool is_paused) = 0; + virtual void continueForTime(double seconds) = 0; + virtual void continueForFrames(uint32_t frames) = 0; + + virtual void setTimeOfDay(bool is_enabled, const std::string &start_datetime, bool is_start_datetime_dst, + float celestial_clock_speed, float update_interval_secs, bool move_sun) = 0; + + virtual void enableWeather(bool enable) = 0; + virtual void setWeatherParameter(WeatherParameter param, float val) = 0; + + virtual bool setSegmentationObjectID(const std::string &mesh_name, int object_id, bool is_name_regex = false) = 0; + virtual int getSegmentationObjectID(const std::string &mesh_name) const = 0; + + virtual bool addVehicle(const std::string &vehicle_name, const std::string &vehicle_type, const Pose &pose, + const std::string &pawn_path = "") = 0; + + virtual void printLogMessage(const std::string &message, const std::string &message_param = "", + unsigned char severity = 0) = 0; + + //----------- Plotting APIs ----------/ + virtual void simFlushPersistentMarkers() = 0; + virtual void simPlotPoints(const vector &points, const vector &color_rgba, float size, + float duration, bool is_persistent) = 0; + virtual void simPlotLineStrip(const vector &points, const vector &color_rgba, float thickness, + float duration, bool is_persistent) = 0; + virtual void simPlotLineList(const vector &points, const vector &color_rgba, float thickness, + float duration, bool is_persistent) = 0; + virtual void simPlotArrows(const vector &points_start, const vector &points_end, + const vector &color_rgba, float thickness, float arrow_size, float duration, + bool is_persistent) = 0; + virtual void simPlotStrings(const vector &strings, const vector &positions, float scale, + const vector &color_rgba, float duration) = 0; + virtual void simPlotTransforms(const vector &poses, float scale, float thickness, float duration, + bool is_persistent) = 0; + virtual void simPlotTransformsWithNames(const vector &poses, const vector &names, float tf_scale, + float tf_thickness, float text_scale, const vector &text_color_rgba, + float duration) = 0; + + virtual std::vector listSceneObjects(const std::string &name_regex) const = 0; + virtual Pose getObjectPose(const std::string &object_name) const = 0; + virtual Vector3r getObjectScale(const std::string &object_name) const = 0; + virtual bool setObjectPose(const std::string &object_name, const Pose &pose, bool teleport) = 0; + virtual bool runConsoleCommand(const std::string &command) = 0; + virtual bool setObjectScale(const std::string &object_name, const Vector3r &scale) = 0; + virtual std::unique_ptr> swapTextures(const std::string &tag, int tex_id = 0, + int component_id = 0, int material_id = 0) = 0; + virtual bool setLightIntensity(const std::string &light_name, float intensity) = 0; + virtual bool setObjectMaterial(const std::string &object_name, const std::string &material_name, + const int component_id = 0) = 0; + virtual bool setObjectMaterialFromTexture(const std::string &object_name, const std::string &texture_path, + const int component_id = 0) = 0; + virtual vector getMeshPositionVertexBuffers() const = 0; + + virtual bool createVoxelGrid(const Vector3r &position, const int &x_size, const int &y_size, const int &z_size, + const float &res, const std::string &output_file) = 0; + + // Recording APIs + virtual void startRecording() = 0; + virtual void stopRecording() = 0; + virtual bool isRecording() const = 0; + + virtual void setWind(const Vector3r &wind) const = 0; + virtual void setExtForce(const Vector3r &ext_force) const = 0; + virtual vector listVehicles() const = 0; + + virtual std::string getSettingsString() const = 0; + + virtual bool testLineOfSightBetweenPoints(const nervosys::autonomylib::GeoPoint &point1, + const nervosys::autonomylib::GeoPoint &point2) const = 0; + virtual vector getWorldExtents() const = 0; + + // Camera APIs + virtual CameraInfo getCameraInfo(const CameraDetails &camera_details) const = 0; + virtual void setCameraPose(const nervosys::autonomylib::Pose &pose, const CameraDetails &camera_details) = 0; + virtual void setCameraFoV(float fov_degrees, const CameraDetails &camera_details) = 0; + virtual void setDistortionParam(const std::string ¶m_name, float value, + const CameraDetails &camera_details) = 0; + virtual std::vector getDistortionParams(const CameraDetails &camera_details) const = 0; + + virtual std::vector + getImages(const std::vector &requests, const std::string &vehicle_name, + bool external) const = 0; + virtual std::vector getImage(ImageCaptureBase::ImageType image_type, + const CameraDetails &camera_details) const = 0; + + // CinemAutonomySim + virtual std::vector getPresetLensSettings(const CameraDetails &camera_details) = 0; + virtual std::string getLensSettings(const CameraDetails &camera_details) = 0; + virtual void setPresetLensSettings(std::string, const CameraDetails &camera_details) = 0; + virtual std::vector getPresetFilmbackSettings(const CameraDetails &camera_details) = 0; + virtual void setPresetFilmbackSettings(std::string, const CameraDetails &camera_details) = 0; + virtual std::string getFilmbackSettings(const CameraDetails &camera_details) = 0; + virtual float setFilmbackSettings(float width, float height, const CameraDetails &camera_details) = 0; + virtual float getFocalLength(const CameraDetails &camera_details) = 0; + virtual void setFocalLength(float focal_length, const CameraDetails &camera_details) = 0; + virtual void enableManualFocus(bool enable, const CameraDetails &camera_details) = 0; + virtual float getFocusDistance(const CameraDetails &camera_details) = 0; + virtual void setFocusDistance(float focus_distance, const CameraDetails &camera_details) = 0; + virtual float getFocusAperture(const CameraDetails &camera_details) = 0; + virtual void setFocusAperture(float focus_aperture, const CameraDetails &camera_details) = 0; + virtual void enableFocusPlane(bool enable, const CameraDetails &camera_details) = 0; + virtual std::string getCurrentFieldOfView(const CameraDetails &camera_details) = 0; + // end CinemAutonomySim + + virtual void addDetectionFilterMeshName(ImageCaptureBase::ImageType image_type, const std::string &mesh_name, + const CameraDetails &camera_details) = 0; + virtual void setDetectionFilterRadius(ImageCaptureBase::ImageType image_type, float radius_cm, + const CameraDetails &camera_details) = 0; + virtual void clearDetectionMeshNames(ImageCaptureBase::ImageType image_type, + const CameraDetails &camera_details) = 0; + virtual std::vector getDetections(ImageCaptureBase::ImageType image_type, + const CameraDetails &camera_details) = 0; +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/AutonomySimSettings.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/AutonomySimSettings.hpp new file mode 100644 index 00000000..92dda092 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/AutonomySimSettings.hpp @@ -0,0 +1,1338 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_AutonomySimSettings_hpp +#define autonomylib_common_AutonomySimSettings_hpp + +#include +#include +#include +#include +#include + +#include "CommonStructs.hpp" +#include "ImageCaptureBase.hpp" +#include "Settings.hpp" +#include "common/utils/Utils.hpp" +#include "sensors/SensorBase.hpp" + +namespace nervosys { +namespace autonomylib { + +struct AutonomySimSettings { + private: + typedef common_utils::Utils Utils; + typedef ImageCaptureBase::ImageType ImageType; + + public: // types + static constexpr int kSubwindowCount = 3; // must be >= 3 for now + static constexpr char const *kVehicleTypePX4 = "px4multirotor"; + static constexpr char const *kVehicleTypeArduCopterSolo = "arducoptersolo"; + static constexpr char const *kVehicleTypeSimpleFlight = "simpleflight"; + static constexpr char const *kVehicleTypeArduCopter = "arducopter"; + static constexpr char const *kVehicleTypePhysXCar = "physxcar"; + static constexpr char const *kVehicleTypeArduRover = "ardurover"; + static constexpr char const *kVehicleTypeComputerVision = "computervision"; + + static constexpr char const *kVehicleInertialFrame = "VehicleInertialFrame"; + static constexpr char const *kSensorLocalFrame = "SensorLocalFrame"; + + static constexpr char const *kSimModeTypeMultirotor = "Multirotor"; + static constexpr char const *kSimModeTypeCar = "Car"; + static constexpr char const *kSimModeTypeComputerVision = "ComputerVision"; + + struct SubwindowSetting { + int window_index; + ImageType image_type; + bool visible; + std::string camera_name; + std::string vehicle_name; + bool external; + + SubwindowSetting(int window_index_val = 0, ImageType image_type_val = ImageType::Scene, + bool visible_val = false, const std::string &camera_name_val = "", + const std::string &vehicle_name_val = "", bool external_val = false) + : window_index(window_index_val), image_type(image_type_val), visible(visible_val), + camera_name(camera_name_val), vehicle_name(vehicle_name_val), external(external_val) {} + }; + + struct RecordingSetting { + bool record_on_move = false; + float record_interval = 0.05f; + std::string folder = ""; + bool enabled = false; + + std::map> requests; + + RecordingSetting() {} + + RecordingSetting(bool record_on_move_val, float record_interval_val, const std::string &folder_val, + bool enabled_val) + : record_on_move(record_on_move_val), record_interval(record_interval_val), folder(folder_val), + enabled(enabled_val) {} + }; + + struct PawnPath { + std::string pawn_bp; + std::string slippery_mat; + std::string non_slippery_mat; + + PawnPath(const std::string &pawn_bp_val = "", + const std::string &slippery_mat_val = "/AutonomySim/VehicleAdv/PhysicsMaterials/Slippery.Slippery", + const std::string &non_slippery_mat_val = + "/AutonomySim/VehicleAdv/PhysicsMaterials/NonSlippery.NonSlippery") + : pawn_bp(pawn_bp_val), slippery_mat(slippery_mat_val), non_slippery_mat(non_slippery_mat_val) {} + }; + + struct RCSettings { + int remote_control_id = -1; + bool allow_api_when_disconnected = false; + }; + + struct Rotation { + float yaw = 0; + float pitch = 0; + float roll = 0; + + Rotation() {} + + Rotation(float yaw_val, float pitch_val, float roll_val) : yaw(yaw_val), pitch(pitch_val), roll(roll_val) {} + + static Rotation nanRotation() noexcept { + static const Rotation val(Utils::nan(), Utils::nan(), Utils::nan()); + return val; + } + }; + + struct GimbalSetting { + float stabilization = 0; + // bool is_world_frame = false; + Rotation rotation = Rotation::nanRotation(); + }; + + struct CaptureSetting { + // below settings_json are obtained by using Unreal console command (press ~): + // ShowFlag.VisualizeHDR 1. + // to replicate camera settings_json to SceneCapture2D + // TODO: should we use UAutonomyBlueprintLib::GetDisplayGamma()? + static constexpr float kSceneTargetGamma = 1.4f; + + int image_type = 0; + + unsigned int width = 256, height = 144; // 960 X 540 + float fov_degrees = Utils::nan(); // 90.0f + int auto_exposure_method = -1; // histogram + float auto_exposure_speed = Utils::nan(); // 100.0f; + float auto_exposure_bias = Utils::nan(); // 0; + float auto_exposure_max_brightness = Utils::nan(); // 0.64f; + float auto_exposure_min_brightness = Utils::nan(); // 0.03f; + float auto_exposure_low_percent = Utils::nan(); // 80.0f; + float auto_exposure_high_percent = Utils::nan(); // 98.3f; + float auto_exposure_histogram_log_min = Utils::nan(); // -8; + float auto_exposure_histogram_log_max = Utils::nan(); // 4; + float motion_blur_amount = Utils::nan(); + float target_gamma = + Utils::nan(); // 1.0f; //This would be reset to kSceneTargetGamma for scene as default + int projection_mode = 0; // ECameraProjectionMode::Perspective + float ortho_width = Utils::nan(); + }; + + struct NoiseSetting { + int ImageType = 0; + + bool Enabled = false; + + float RandContrib = 0.2f; + float RandSpeed = 100000.0f; + float RandSize = 500.0f; + float RandDensity = 2.0f; + + float HorzWaveContrib = 0.03f; + float HorzWaveStrength = 0.08f; + float HorzWaveVertSize = 1.0f; + float HorzWaveScreenSize = 1.0f; + + float HorzNoiseLinesContrib = 1.0f; + float HorzNoiseLinesDensityY = 0.01f; + float HorzNoiseLinesDensityXY = 0.5f; + + float HorzDistortionContrib = 1.0f; + float HorzDistortionStrength = 0.002f; + }; + + struct PixelFormatOverrideSetting { + int pixel_format = 0; + }; + + struct UnrealEngineSetting { + std::map pixel_format_override_settings; + }; + + using CaptureSettingsMap = std::map; + using NoiseSettingsMap = std::map; + struct CameraSetting { + // nan means keep the default values set in components + Vector3r position = VectorMath::nanVector(); + Rotation rotation = Rotation::nanRotation(); + + GimbalSetting gimbal; + CaptureSettingsMap capture_settings; + NoiseSettingsMap noise_settings; + + UnrealEngineSetting ue_setting; + + CameraSetting() { + initializeCaptureSettings(capture_settings); + initializeNoiseSettings(noise_settings); + } + }; + using CameraSettingMap = std::map; + + struct CameraDirectorSetting { + Vector3r position = VectorMath::nanVector(); + Rotation rotation = Rotation::nanRotation(); + float follow_distance = Utils::nan(); + }; + + struct SensorSetting { + SensorBase::SensorType sensor_type; + std::string sensor_name; + bool enabled = true; + Settings settings; // imported json data that needs to be parsed by specific sensors. + }; + + struct BarometerSetting : SensorSetting {}; + + struct ImuSetting : SensorSetting {}; + + struct GpsSetting : SensorSetting {}; + + struct MagnetometerSetting : SensorSetting {}; + + struct DistanceSetting : SensorSetting {}; + + struct LidarSetting : SensorSetting { + enum class DataFrame { VehicleInertialFrame, SensorLocalFrame }; + }; + + struct VehicleSetting { + // required + std::string vehicle_name; + std::string vehicle_type; + + // optional + std::string default_vehicle_state; + std::string pawn_path; + bool allow_api_always = true; + bool auto_create = true; + bool enable_collision_passthrough = false; + bool enable_trace = false; + bool enable_collisions = true; + bool is_fpv_vehicle = false; + + // nan means use player start + Vector3r position = VectorMath::nanVector(); // in global NED + Rotation rotation = Rotation::nanRotation(); + + CameraSettingMap cameras; + std::map> sensors; + + RCSettings rc; + + VehicleSetting() {} + + VehicleSetting(const std::string &vehicle_name_val, const std::string &vehicle_type_val) + : vehicle_name(vehicle_name_val), vehicle_type(vehicle_type_val) {} + }; + + struct MavLinkConnectionInfo { + /* Default values are requires so uninitialized instance doesn't have random values */ + + bool use_serial = true; // false means use UDP or TCP instead + + // Used to connect via HITL: needed only if use_serial = true + std::string serial_port = "*"; + int baud_rate = 115200; + + // Used to connect to drone over UDP: needed only if use_serial = false and use_tcp == false + std::string udp_address = "127.0.0.1"; + int udp_port = 14560; + + // Used to accept connections from drone over TCP: needed only if use_tcp = true + bool lock_step = true; + bool use_tcp = false; + int tcp_port = 4560; + + // The PX4 SITL app requires receiving drone commands over a different mavlink channel called + // the "ground control station" (or GCS) channel. + std::string control_ip_address = "127.0.0.1"; + int control_port_local = 14540; + int control_port_remote = 14580; + + // The log viewer can be on a different machine, so you can configure it's ip address and port here. + int logviewer_ip_port = 14388; + int logviewer_ip_sport = 14389; // for logging all messages we send to the vehicle. + std::string logviewer_ip_address = ""; + + // The QGroundControl app can be on a different machine, and AutonomySim can act as a proxy to forward + // the mavlink stream over to that machine if you configure it's ip address and port here. + int qgc_ip_port = 0; + std::string qgc_ip_address = ""; + + // mavlink vehicle identifiers + uint8_t sim_sysid = 142; + int sim_compid = 42; + uint8_t offboard_sysid = 134; + int offboard_compid = 1; + uint8_t vehicle_sysid = 135; + int vehicle_compid = 1; + + // if you want to select a specific local network adapter so you can reach certain remote machines (e.g. wifi + // versus ethernet) then you will want to change the LocalHostIp accordingly. This default only works when log + // viewer and QGC are also on the same machine. Whatever network you choose it has to be the same one for + // external + std::string local_host_ip = "127.0.0.1"; + + std::string model = "Generic"; + + std::map params; + std::string logs; + }; + + struct MavLinkVehicleSetting : public VehicleSetting { + MavLinkConnectionInfo connection_info; + }; + + struct SegmentationSetting { + enum class InitMethodType { None, CommonObjectsRandomIDs }; + + enum class MeshNamingMethodType { OwnerName, StaticMeshName }; + + InitMethodType init_method = InitMethodType::CommonObjectsRandomIDs; + bool override_existing = false; + MeshNamingMethodType mesh_naming_method = MeshNamingMethodType::OwnerName; + }; + + struct TimeOfDaySetting { + bool enabled = false; + std::string start_datetime = ""; // format: %Y-%m-%d %H:%M:%S + bool is_start_datetime_dst = false; + float celestial_clock_speed = 1; + float update_interval_secs = 60; + bool move_sun = true; + }; + + private: // fields + float settings_version_actual; + float settings_version_minimum = 1.2f; + + public: // fields + std::string simmode_name = ""; + std::string level_name = ""; + + std::vector subwindow_settings; + RecordingSetting recording_setting; + SegmentationSetting segmentation_setting; + TimeOfDaySetting tod_setting; + + std::vector warning_messages; + std::vector error_messages; + + bool is_record_ui_visible = false; + int initial_view_mode = 2; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FLY_WITH_ME + bool enable_rpc = true; + std::string api_server_address = ""; + int api_port = RpcLibPort; + std::string physics_engine_name = ""; + + std::string clock_type = ""; + float clock_speed = 1.0f; + bool engine_sound = false; + bool log_messages_visible = true; + bool show_los_debug_lines_ = false; + HomeGeoPoint origin_geopoint{ + GeoPoint(47.641468, -122.140165, 122)}; // The geo-coordinate assigned to Unreal coordinate 0,0,0 + std::map pawn_paths; // path for pawn blueprint + std::map> vehicles; + CameraSetting camera_defaults; + CameraDirectorSetting camera_director; + float speed_unit_factor = 1.0f; + std::string speed_unit_label = "m\\s"; + std::map> sensor_defaults; + Vector3r wind = Vector3r::Zero(); + Vector3r ext_force = Vector3r::Zero(); + CameraSettingMap external_cameras; + + std::string settings_text_ = ""; + + public: // methods + static AutonomySimSettings &singleton() { + static AutonomySimSettings instance; + return instance; + } + + AutonomySimSettings() { + initializeSubwindowSettings(subwindow_settings); + initializePawnPaths(pawn_paths); + } + + // returns number of warnings + void load(std::function simmode_getter) { + warning_messages.clear(); + error_messages.clear(); + const Settings &settings_json = Settings::singleton(); + checkSettingsVersion(settings_json); + + loadCoreSimModeSettings(settings_json, simmode_getter); + loadLevelSettings(settings_json); + loadDefaultCameraSetting(settings_json, camera_defaults); + loadCameraDirectorSetting(settings_json, camera_director, simmode_name); + loadSubWindowsSettings(settings_json, subwindow_settings); + loadViewModeSettings(settings_json); + loadSegmentationSetting(settings_json, segmentation_setting); + loadPawnPaths(settings_json, pawn_paths); + loadOtherSettings(settings_json); + loadDefaultSensorSettings(simmode_name, settings_json, sensor_defaults); + loadVehicleSettings(simmode_name, settings_json, vehicles, sensor_defaults, camera_defaults); + loadExternalCameraSettings(settings_json, external_cameras, camera_defaults); + + // this should be done last because it depends on vehicles (and/or their type) we have + loadRecordingSetting(settings_json); + loadClockSettings(settings_json); + } + + static void initializeSettings(const std::string &json_settings_text) { + singleton().settings_text_ = json_settings_text; + Settings &settings_json = Settings::loadJSonString(json_settings_text); + if (!settings_json.isLoadSuccess()) + throw std::invalid_argument("Cannot parse JSON settings_json string."); + } + + static void createDefaultSettingsFile() { + initializeSettings("{}"); + + Settings &settings_json = Settings::singleton(); + // write some settings_json in new file otherwise the string "null" is written if all settings_json are empty + settings_json.setString("SeeDocsAt", "https://github.com/nervosys/AutonomySim/blob/master/docs/settings.md"); + settings_json.setDouble("SettingsVersion", 1.2); + + std::string settings_filename = Settings::getUserDirectoryFullPath("settings.json"); + // TODO: there is a crash in Linux due to settings_json.saveJSonString(). Remove this workaround after we only + // support Unreal 4.17 + // https://answers.unrealengine.com/questions/664905/unreal-crashes-on-two-lines-of-extremely-simple-st.html + settings_json.saveJSonFile(settings_filename); + } + + // This is for the case when a new vehicle is made on the fly, at runtime + void addVehicleSetting(const std::string &vehicle_name, const std::string &vehicle_type, const Pose &pose, + const std::string &pawn_path = "") { + // No Mavlink-type vehicles currently + auto vehicle_setting = std::unique_ptr(new VehicleSetting(vehicle_name, vehicle_type)); + vehicle_setting->position = pose.position; + vehicle_setting->pawn_path = pawn_path; + + vehicle_setting->sensors = sensor_defaults; + + VectorMath::toEulerianAngle(pose.orientation, vehicle_setting->rotation.pitch, vehicle_setting->rotation.roll, + vehicle_setting->rotation.yaw); + + vehicles[vehicle_name] = std::move(vehicle_setting); + } + + const VehicleSetting *getVehicleSetting(const std::string &vehicle_name) const { + auto it = vehicles.find(vehicle_name); + if (it == vehicles.end()) + // pre-existing flying pawns in Unreal Engine don't have name 'SimpleFlight' + it = vehicles.find("SimpleFlight"); + return it->second.get(); + } + + static Vector3r createVectorSetting(const Settings &settings_json, const Vector3r &default_vec) { + return Vector3r(settings_json.getFloat("X", default_vec.x()), settings_json.getFloat("Y", default_vec.y()), + settings_json.getFloat("Z", default_vec.z())); + } + static Rotation createRotationSetting(const Settings &settings_json, const Rotation &default_rot) { + return Rotation(settings_json.getFloat("Yaw", default_rot.yaw), + settings_json.getFloat("Pitch", default_rot.pitch), + settings_json.getFloat("Roll", default_rot.roll)); + } + + private: + void checkSettingsVersion(const Settings &settings_json) { + bool has_default_settings = hasDefaultSettings(settings_json, settings_version_actual); + bool upgrade_required = settings_version_actual < settings_version_minimum; + if (upgrade_required) { + bool auto_upgrade = false; + + // if we have default setting file not modified by user then we will + // just auto-upgrade it + if (has_default_settings) { + auto_upgrade = true; + } else { + // check if auto-upgrade is possible + if (settings_version_actual == 1) { + const std::vector all_changed_keys = {"AdditionalCameras", "CaptureSettings", + "NoiseSettings", "UsageScenario", + "SimpleFlight", "PX4"}; + std::stringstream detected_keys_ss; + for (const auto &changed_key : all_changed_keys) { + if (settings_json.hasKey(changed_key)) + detected_keys_ss << changed_key << ","; + } + std::string detected_keys = detected_keys_ss.str(); + if (detected_keys.length()) { + std::string error_message = + "You are using newer version of AutonomySim with older version of settings.json. " + "You can either delete your settings.json and restart AutonomySim or use the upgrade " + "instructions at https://git.io/vjefh. \n\n" + "Following keys in your settings.json needs updating: "; + + error_messages.push_back(error_message + detected_keys); + } else + auto_upgrade = true; + } else + auto_upgrade = true; + } + + if (auto_upgrade) { + warning_messages.push_back( + "You are using newer version of AutonomySim with older version of settings.json. " + "You should delete your settings.json file and restart AutonomySim."); + } + } + // else no action necessary + } + + bool hasDefaultSettings(const Settings &settings_json, float &version) { + // if empty settings file + bool has_default = settings_json.size() == 0; + + bool has_docs = + settings_json.getString("SeeDocsAt", "") != "" || settings_json.getString("see_docs_at", "") != ""; + // we had spelling mistake so we are currently supporting SettingsVersion or SettingdVersion :( + version = settings_json.getFloat("SettingsVersion", settings_json.getFloat("SettingdVersion", 0)); + + // If we have pre-V1 settings and only element is docs link + has_default |= settings_json.size() == 1 && has_docs; + + // if we have V1 settings and only elements are docs link and version + has_default |= settings_json.size() == 2 && has_docs && version > 0; + + return has_default; + } + + void loadCoreSimModeSettings(const Settings &settings_json, std::function simmode_getter) { + // get the simmode from user if not specified + simmode_name = settings_json.getString("SimMode", ""); + if (simmode_name == "") { + if (simmode_getter) + simmode_name = simmode_getter(); + else + throw std::invalid_argument("simmode_name is not expected empty in SimModeBase"); + } + + physics_engine_name = settings_json.getString("PhysicsEngineName", ""); + if (physics_engine_name == "") { + if (simmode_name == kSimModeTypeMultirotor) + physics_engine_name = "FastPhysicsEngine"; + else + physics_engine_name = "PhysX"; // this value is only informational for now + } + } + + void loadLevelSettings(const Settings &settings_json) { + level_name = settings_json.getString("Default Environment", ""); + } + + void loadViewModeSettings(const Settings &settings_json) { + std::string view_mode_string = settings_json.getString("ViewMode", ""); + + if (view_mode_string == "") { + if (simmode_name == kSimModeTypeMultirotor) + view_mode_string = "FlyWithMe"; + else if (simmode_name == kSimModeTypeComputerVision) + view_mode_string = "Fpv"; + else + view_mode_string = "SpringArmChase"; + } + + if (view_mode_string == "Fpv") + initial_view_mode = 0; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FPV; + else if (view_mode_string == "GroundObserver") + initial_view_mode = 1; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_GROUND_OBSERVER; + else if (view_mode_string == "FlyWithMe") + initial_view_mode = 2; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FLY_WITH_ME; + else if (view_mode_string == "Manual") + initial_view_mode = 3; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_MANUAL; + else if (view_mode_string == "SpringArmChase") + initial_view_mode = 4; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE; + else if (view_mode_string == "Backup") + initial_view_mode = 5; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_BACKUP; + else if (view_mode_string == "NoDisplay") + initial_view_mode = 6; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_NODISPLAY; + else if (view_mode_string == "Front") + initial_view_mode = 7; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_FRONT; + else + error_messages.push_back("ViewMode setting is not recognized: " + view_mode_string); + } + + static void loadRCSetting(const std::string &simmode_name, const Settings &settings_json, RCSettings &rc_setting) { + Settings rc_json; + if (settings_json.getChild("RC", rc_json)) { + rc_setting.remote_control_id = + rc_json.getInt("RemoteControlID", simmode_name == kSimModeTypeMultirotor ? 0 : -1); + rc_setting.allow_api_when_disconnected = + rc_json.getBool("AllowAPIWhenDisconnected", rc_setting.allow_api_when_disconnected); + } + } + + static std::string getCameraName(const Settings &settings_json) { + return settings_json.getString( + "CameraName", + // TODO: below exist only due to legacy reason and can be replaced by "" in future + std::to_string(settings_json.getInt("CameraID", 0))); + } + + void loadDefaultRecordingSettings() { + recording_setting.requests.clear(); + // Add Scene image for each vehicle + for (const auto &vehicle : vehicles) { + recording_setting.requests[vehicle.first].push_back( + ImageCaptureBase::ImageRequest("", ImageType::Scene, false, true)); + } + } + + void loadRecordingSetting(const Settings &settings_json) { + loadDefaultRecordingSettings(); + + Settings recording_json; + if (settings_json.getChild("Recording", recording_json)) { + recording_setting.record_on_move = recording_json.getBool("RecordOnMove", recording_setting.record_on_move); + recording_setting.record_interval = + recording_json.getFloat("RecordInterval", recording_setting.record_interval); + recording_setting.folder = recording_json.getString("Folder", recording_setting.folder); + recording_setting.enabled = recording_json.getBool("Enabled", recording_setting.enabled); + + Settings req_cameras_settings; + if (recording_json.getChild("Cameras", req_cameras_settings)) { + // If 'Cameras' field is present, clear defaults + recording_setting.requests.clear(); + // Get name of the default vehicle to be used if "VehicleName" isn't specified + // Map contains a default vehicle if vehicles haven't been specified + std::string default_vehicle_name = vehicles.begin()->first; + + for (size_t child_index = 0; child_index < req_cameras_settings.size(); ++child_index) { + Settings req_camera_settings; + + if (req_cameras_settings.getChild(child_index, req_camera_settings)) { + std::string camera_name = getCameraName(req_camera_settings); + ImageType image_type = Utils::toEnum(req_camera_settings.getInt("ImageType", 0)); + bool compress = req_camera_settings.getBool("Compress", true); + bool pixels_as_float = req_camera_settings.getBool("PixelsAsFloat", false); + std::string vehicle_name = req_camera_settings.getString("VehicleName", default_vehicle_name); + + recording_setting.requests[vehicle_name].push_back( + ImageCaptureBase::ImageRequest(camera_name, image_type, pixels_as_float, compress)); + } + } + } + } + } + + static void initializeCaptureSettings(CaptureSettingsMap &capture_settings) { + capture_settings.clear(); + for (int i = -1; i < Utils::toNumeric(ImageType::Count); ++i) { + capture_settings[i] = CaptureSetting(); + } + capture_settings.at(Utils::toNumeric(ImageType::Scene)).target_gamma = CaptureSetting::kSceneTargetGamma; + } + + static void loadCaptureSettings(const Settings &settings_json, CaptureSettingsMap &capture_settings) { + // We don't call initializeCaptureSettings here since it's already called in CameraSettings constructor + // And to avoid overwriting any defaults already set from CameraDefaults + + Settings json_parent; + if (settings_json.getChild("CaptureSettings", json_parent)) { + for (size_t child_index = 0; child_index < json_parent.size(); ++child_index) { + Settings json_settings_child; + if (json_parent.getChild(child_index, json_settings_child)) { + CaptureSetting capture_setting; + createCaptureSettings(json_settings_child, capture_setting); + capture_settings[capture_setting.image_type] = capture_setting; + } + } + } + } + + static std::unique_ptr createMavLinkVehicleSetting(const Settings &settings_json) { + // these settings_json are expected in same section, not in another child + std::unique_ptr vehicle_setting_p = + std::unique_ptr(new MavLinkVehicleSetting()); + MavLinkVehicleSetting *vehicle_setting = static_cast(vehicle_setting_p.get()); + + // TODO: we should be selecting remote if available else keyboard + // currently keyboard is not supported so use rc as default + vehicle_setting->rc.remote_control_id = 0; + + MavLinkConnectionInfo &connection_info = vehicle_setting->connection_info; + connection_info.sim_sysid = static_cast(settings_json.getInt("SimSysID", connection_info.sim_sysid)); + connection_info.sim_compid = settings_json.getInt("SimCompID", connection_info.sim_compid); + + connection_info.vehicle_sysid = + static_cast(settings_json.getInt("VehicleSysID", connection_info.vehicle_sysid)); + connection_info.vehicle_compid = settings_json.getInt("VehicleCompID", connection_info.vehicle_compid); + + connection_info.offboard_sysid = + static_cast(settings_json.getInt("OffboardSysID", connection_info.offboard_sysid)); + connection_info.offboard_compid = settings_json.getInt("OffboardCompID", connection_info.offboard_compid); + + connection_info.logviewer_ip_address = + settings_json.getString("LogViewerHostIp", connection_info.logviewer_ip_address); + connection_info.logviewer_ip_port = settings_json.getInt("LogViewerPort", connection_info.logviewer_ip_port); + connection_info.logviewer_ip_sport = + settings_json.getInt("LogViewerSendPort", connection_info.logviewer_ip_sport); + + connection_info.qgc_ip_address = settings_json.getString("QgcHostIp", connection_info.qgc_ip_address); + connection_info.qgc_ip_port = settings_json.getInt("QgcPort", connection_info.qgc_ip_port); + + connection_info.control_ip_address = settings_json.getString("ControlIp", connection_info.control_ip_address); + connection_info.control_port_local = + settings_json.getInt("ControlPort", connection_info.control_port_local); // legacy + connection_info.control_port_local = + settings_json.getInt("ControlPortLocal", connection_info.control_port_local); + connection_info.control_port_remote = + settings_json.getInt("ControlPortRemote", connection_info.control_port_remote); + + std::string sitlip = settings_json.getString("SitlIp", connection_info.control_ip_address); + if (sitlip.size() > 0 && connection_info.control_ip_address.size() == 0) { + // backwards compat + connection_info.control_ip_address = sitlip; + } + if (settings_json.hasKey("SitlPort")) { + // backwards compat + connection_info.control_port_local = settings_json.getInt("SitlPort", connection_info.control_port_local); + } + + connection_info.local_host_ip = settings_json.getString("LocalHostIp", connection_info.local_host_ip); + + connection_info.use_serial = settings_json.getBool("UseSerial", connection_info.use_serial); + connection_info.udp_address = settings_json.getString("UdpIp", connection_info.udp_address); + connection_info.udp_port = settings_json.getInt("UdpPort", connection_info.udp_port); + connection_info.use_tcp = settings_json.getBool("UseTcp", connection_info.use_tcp); + connection_info.lock_step = settings_json.getBool("LockStep", connection_info.lock_step); + connection_info.tcp_port = settings_json.getInt("TcpPort", connection_info.tcp_port); + connection_info.serial_port = settings_json.getString("SerialPort", connection_info.serial_port); + connection_info.baud_rate = settings_json.getInt("SerialBaudRate", connection_info.baud_rate); + connection_info.model = settings_json.getString("Model", connection_info.model); + connection_info.logs = settings_json.getString("Logs", connection_info.logs); + + Settings params; + if (settings_json.getChild("Parameters", params)) { + std::vector keys; + params.getChildNames(keys); + for (auto key : keys) { + connection_info.params[key] = params.getFloat(key, 0); + } + } + + return vehicle_setting_p; + } + + static std::unique_ptr + createVehicleSetting(const std::string &simmode_name, const Settings &settings_json, const std::string vehicle_name, + std::map> &sensor_defaults, + const CameraSetting &camera_defaults) { + auto vehicle_type = Utils::toLower(settings_json.getString("VehicleType", "")); + + std::unique_ptr vehicle_setting; + if (vehicle_type == kVehicleTypePX4 || vehicle_type == kVehicleTypeArduCopterSolo || + vehicle_type == kVehicleTypeArduCopter || vehicle_type == kVehicleTypeArduRover) + vehicle_setting = createMavLinkVehicleSetting(settings_json); + // for everything else we don't need derived class yet + else { + vehicle_setting = std::unique_ptr(new VehicleSetting()); + if (vehicle_type == kVehicleTypeSimpleFlight) { + // TODO: we should be selecting remote if available else keyboard + // currently keyboard is not supported so use rc as default + vehicle_setting->rc.remote_control_id = 0; + } + } + vehicle_setting->vehicle_name = vehicle_name; + + // required settings_json + vehicle_setting->vehicle_type = vehicle_type; + + // optional settings_json + vehicle_setting->pawn_path = settings_json.getString("PawnPath", ""); + vehicle_setting->default_vehicle_state = settings_json.getString("DefaultVehicleState", ""); + vehicle_setting->allow_api_always = settings_json.getBool("AllowAPIAlways", vehicle_setting->allow_api_always); + vehicle_setting->auto_create = settings_json.getBool("AutoCreate", vehicle_setting->auto_create); + vehicle_setting->enable_collision_passthrough = + settings_json.getBool("EnableCollisionPassthrogh", vehicle_setting->enable_collision_passthrough); + vehicle_setting->enable_trace = settings_json.getBool("EnableTrace", vehicle_setting->enable_trace); + vehicle_setting->enable_collisions = + settings_json.getBool("EnableCollisions", vehicle_setting->enable_collisions); + vehicle_setting->is_fpv_vehicle = settings_json.getBool("IsFpvVehicle", vehicle_setting->is_fpv_vehicle); + + loadRCSetting(simmode_name, settings_json, vehicle_setting->rc); + + vehicle_setting->position = createVectorSetting(settings_json, vehicle_setting->position); + vehicle_setting->rotation = createRotationSetting(settings_json, vehicle_setting->rotation); + + loadCameraSettings(settings_json, vehicle_setting->cameras, camera_defaults); + loadSensorSettings(settings_json, "Sensors", vehicle_setting->sensors, sensor_defaults); + + return vehicle_setting; + } + + static void createDefaultVehicle(const std::string &simmode_name, + std::map> &vehicles, + const std::map> &sensor_defaults) { + vehicles.clear(); + + // NOTE: Do not set defaults for vehicle type here. If you do then make sure + // to sync code in createVehicleSetting() as well. + if (simmode_name == kSimModeTypeMultirotor) { + // create simple flight as default multirotor + auto simple_flight_setting = + std::unique_ptr(new VehicleSetting("SimpleFlight", kVehicleTypeSimpleFlight)); + // TODO: we should be selecting remote if available else keyboard + // currently keyboard is not supported so use rc as default + simple_flight_setting->rc.remote_control_id = 0; + simple_flight_setting->sensors = sensor_defaults; + vehicles[simple_flight_setting->vehicle_name] = std::move(simple_flight_setting); + } else if (simmode_name == kSimModeTypeCar) { + // create PhysX as default car vehicle + auto physx_car_setting = + std::unique_ptr(new VehicleSetting("PhysXCar", kVehicleTypePhysXCar)); + physx_car_setting->sensors = sensor_defaults; + vehicles[physx_car_setting->vehicle_name] = std::move(physx_car_setting); + } else if (simmode_name == kSimModeTypeComputerVision) { + // create default computer vision vehicle + auto cv_setting = + std::unique_ptr(new VehicleSetting("ComputerVision", kVehicleTypeComputerVision)); + cv_setting->sensors = sensor_defaults; + vehicles[cv_setting->vehicle_name] = std::move(cv_setting); + } else { + throw std::invalid_argument( + Utils::stringf("Unknown SimMode: %s, failed to set default vehicle settings", simmode_name.c_str()) + .c_str()); + } + } + + static void loadVehicleSettings(const std::string &simmode_name, const Settings &settings_json, + std::map> &vehicles, + std::map> &sensor_defaults, + const CameraSetting &camera_defaults) { + createDefaultVehicle(simmode_name, vehicles, sensor_defaults); + + nervosys::autonomylib::Settings vehicles_child; + if (settings_json.getChild("Vehicles", vehicles_child)) { + std::vector keys; + vehicles_child.getChildNames(keys); + + // remove default vehicles, if values are specified in settings + if (keys.size()) + vehicles.clear(); + + for (const auto &key : keys) { + nervosys::autonomylib::Settings child; + vehicles_child.getChild(key, child); + vehicles[key] = createVehicleSetting(simmode_name, child, key, sensor_defaults, camera_defaults); + } + } + } + + static void initializePawnPaths(std::map &pawn_paths) { + pawn_paths.clear(); + pawn_paths.emplace("BareboneCar", + PawnPath("Class'/AutonomySim/VehicleAdv/Vehicle/VehicleAdvPawn.VehicleAdvPawn_C'")); + pawn_paths.emplace("DefaultCar", PawnPath("Class'/AutonomySim/VehicleAdv/SUV/SuvCarPawn.SuvCarPawn_C'")); + pawn_paths.emplace("DefaultQuadrotor", + PawnPath("Class'/AutonomySim/Blueprints/BP_FlyingPawn.BP_FlyingPawn_C'")); + pawn_paths.emplace("DefaultComputerVision", + PawnPath("Class'/AutonomySim/Blueprints/BP_ComputerVisionPawn.BP_ComputerVisionPawn_C'")); + } + + static void loadPawnPaths(const Settings &settings_json, std::map &pawn_paths) { + initializePawnPaths(pawn_paths); + + nervosys::autonomylib::Settings pawn_paths_child; + if (settings_json.getChild("PawnPaths", pawn_paths_child)) { + std::vector keys; + pawn_paths_child.getChildNames(keys); + + for (const auto &key : keys) { + nervosys::autonomylib::Settings child; + pawn_paths_child.getChild(key, child); + pawn_paths[key] = createPathPawn(child); + } + } + } + + static PawnPath createPathPawn(const Settings &settings_json) { + auto paths = PawnPath(); + paths.pawn_bp = settings_json.getString("PawnBP", ""); + auto slippery_mat = settings_json.getString("SlipperyMat", ""); + auto non_slippery_mat = settings_json.getString("NonSlipperyMat", ""); + + if (slippery_mat != "") + paths.slippery_mat = slippery_mat; + if (non_slippery_mat != "") + paths.non_slippery_mat = non_slippery_mat; + + return paths; + } + + static void loadSegmentationSetting(const Settings &settings_json, SegmentationSetting &segmentation_setting) { + Settings json_parent; + if (settings_json.getChild("SegmentationSettings", json_parent)) { + std::string init_method = Utils::toLower(json_parent.getString("InitMethod", "")); + if (init_method == "" || init_method == "commonobjectsrandomids") + segmentation_setting.init_method = SegmentationSetting::InitMethodType::CommonObjectsRandomIDs; + else if (init_method == "none") + segmentation_setting.init_method = SegmentationSetting::InitMethodType::None; + else + // TODO: below exception doesn't actually get raised right now because of issue in Unreal Engine? + throw std::invalid_argument( + std::string("SegmentationSetting init_method has invalid value in settings_json ") + init_method); + + segmentation_setting.override_existing = json_parent.getBool("OverrideExisting", true); + + std::string mesh_naming_method = Utils::toLower(json_parent.getString("MeshNamingMethod", "")); + if (mesh_naming_method == "" || mesh_naming_method == "ownername") + segmentation_setting.mesh_naming_method = SegmentationSetting::MeshNamingMethodType::OwnerName; + else if (mesh_naming_method == "staticmeshname") + segmentation_setting.mesh_naming_method = SegmentationSetting::MeshNamingMethodType::StaticMeshName; + else + throw std::invalid_argument( + std::string("SegmentationSetting MeshNamingMethod has invalid value in settings_json ") + + mesh_naming_method); + } + } + + static void initializeNoiseSettings(NoiseSettingsMap &noise_settings) { + const int image_count = Utils::toNumeric(ImageType::Count); + noise_settings.clear(); + for (int i = -1; i < image_count; ++i) + noise_settings[i] = NoiseSetting(); + } + + static void loadNoiseSettings(const Settings &settings_json, NoiseSettingsMap &noise_settings) { + // We don't call initializeNoiseSettings here since it's already called in CameraSettings constructor + // And to avoid overwriting any defaults already set from CameraDefaults + + Settings json_parent; + if (settings_json.getChild("NoiseSettings", json_parent)) { + for (size_t child_index = 0; child_index < json_parent.size(); ++child_index) { + Settings json_settings_child; + if (json_parent.getChild(child_index, json_settings_child)) { + NoiseSetting noise_setting; + loadNoiseSetting(json_settings_child, noise_setting); + noise_settings[noise_setting.ImageType] = noise_setting; + } + } + } + } + + static void loadNoiseSetting(const Settings &settings_json, NoiseSetting &noise_setting) { + noise_setting.Enabled = settings_json.getBool("Enabled", noise_setting.Enabled); + noise_setting.ImageType = settings_json.getInt("ImageType", noise_setting.ImageType); + + noise_setting.HorzWaveStrength = settings_json.getFloat("HorzWaveStrength", noise_setting.HorzWaveStrength); + noise_setting.RandSpeed = settings_json.getFloat("RandSpeed", noise_setting.RandSpeed); + noise_setting.RandSize = settings_json.getFloat("RandSize", noise_setting.RandSize); + noise_setting.RandDensity = settings_json.getFloat("RandDensity", noise_setting.RandDensity); + noise_setting.RandContrib = settings_json.getFloat("RandContrib", noise_setting.RandContrib); + noise_setting.HorzWaveContrib = settings_json.getFloat("HorzWaveContrib", noise_setting.HorzWaveContrib); + noise_setting.HorzWaveVertSize = settings_json.getFloat("HorzWaveVertSize", noise_setting.HorzWaveVertSize); + noise_setting.HorzWaveScreenSize = + settings_json.getFloat("HorzWaveScreenSize", noise_setting.HorzWaveScreenSize); + noise_setting.HorzNoiseLinesContrib = + settings_json.getFloat("HorzNoiseLinesContrib", noise_setting.HorzNoiseLinesContrib); + noise_setting.HorzNoiseLinesDensityY = + settings_json.getFloat("HorzNoiseLinesDensityY", noise_setting.HorzNoiseLinesDensityY); + noise_setting.HorzNoiseLinesDensityXY = + settings_json.getFloat("HorzNoiseLinesDensityXY", noise_setting.HorzNoiseLinesDensityXY); + noise_setting.HorzDistortionStrength = + settings_json.getFloat("HorzDistortionStrength", noise_setting.HorzDistortionStrength); + noise_setting.HorzDistortionContrib = + settings_json.getFloat("HorzDistortionContrib", noise_setting.HorzDistortionContrib); + } + + static GimbalSetting createGimbalSetting(const Settings &settings_json) { + GimbalSetting gimbal; + // capture_setting.gimbal.is_world_frame = settings_json.getBool("IsWorldFrame", false); + gimbal.stabilization = settings_json.getFloat("Stabilization", false); + gimbal.rotation = createRotationSetting(settings_json, gimbal.rotation); + return gimbal; + } + + static void loadUnrealEngineSetting(const nervosys::autonomylib::Settings &settings_json, + UnrealEngineSetting &ue_setting) { + Settings ue_settings_json; + if (settings_json.getChild("UnrealEngine", ue_settings_json)) { + Settings pixel_format_override_settings_json; + ue_setting.pixel_format_override_settings.clear(); + + for (int i = 0; i < Utils::toNumeric(ImageType::Count); i++) { + PixelFormatOverrideSetting pixel_format_setting; + pixel_format_setting.pixel_format = 0; // EXPixelformat::PF_Unknown + ue_setting.pixel_format_override_settings[i] = pixel_format_setting; + } + + if (ue_settings_json.getChild("PixelFormatOverride", pixel_format_override_settings_json)) { + for (size_t child_index = 0; child_index < pixel_format_override_settings_json.size(); ++child_index) { + Settings pixel_format_child_json; + if (pixel_format_override_settings_json.getChild(child_index, pixel_format_child_json)) { + int image_type = pixel_format_child_json.getInt("ImageType", 0); + PixelFormatOverrideSetting &pixel_format_setting = + ue_setting.pixel_format_override_settings.at(image_type); + pixel_format_setting.pixel_format = + pixel_format_child_json.getInt("PixelFormat", 0); // default to EXPixelformat::PF_Unknown + } + } + } + } + } + + static CameraSetting createCameraSetting(const Settings &settings_json, const CameraSetting &camera_defaults) { + CameraSetting setting = camera_defaults; + + setting.position = createVectorSetting(settings_json, setting.position); + setting.rotation = createRotationSetting(settings_json, setting.rotation); + + loadCaptureSettings(settings_json, setting.capture_settings); + loadNoiseSettings(settings_json, setting.noise_settings); + Settings json_gimbal; + if (settings_json.getChild("Gimbal", json_gimbal)) + setting.gimbal = createGimbalSetting(json_gimbal); + + loadUnrealEngineSetting(settings_json, setting.ue_setting); + + return setting; + } + + static void loadCameraSettings(const Settings &settings_json, CameraSettingMap &cameras, + const CameraSetting &camera_defaults) { + cameras.clear(); + + Settings json_parent; + if (settings_json.getChild("Cameras", json_parent)) { + std::vector keys; + json_parent.getChildNames(keys); + + for (const auto &key : keys) { + nervosys::autonomylib::Settings child; + json_parent.getChild(key, child); + cameras[key] = createCameraSetting(child, camera_defaults); + } + } + } + + static void createCaptureSettings(const nervosys::autonomylib::Settings &settings_json, + CaptureSetting &capture_setting) { + capture_setting.width = settings_json.getInt("Width", capture_setting.width); + capture_setting.height = settings_json.getInt("Height", capture_setting.height); + capture_setting.fov_degrees = settings_json.getFloat("FOV_Degrees", capture_setting.fov_degrees); + capture_setting.auto_exposure_speed = + settings_json.getFloat("AutoExposureSpeed", capture_setting.auto_exposure_speed); + capture_setting.auto_exposure_bias = + settings_json.getFloat("AutoExposureBias", capture_setting.auto_exposure_bias); + capture_setting.auto_exposure_max_brightness = + settings_json.getFloat("AutoExposureMaxBrightness", capture_setting.auto_exposure_max_brightness); + capture_setting.auto_exposure_min_brightness = + settings_json.getFloat("AutoExposureMinBrightness", capture_setting.auto_exposure_min_brightness); + capture_setting.motion_blur_amount = + settings_json.getFloat("MotionBlurAmount", capture_setting.motion_blur_amount); + capture_setting.image_type = settings_json.getInt("ImageType", 0); + capture_setting.target_gamma = settings_json.getFloat( + "TargetGamma", capture_setting.image_type == 0 ? CaptureSetting::kSceneTargetGamma : Utils::nan()); + + std::string projection_mode = Utils::toLower(settings_json.getString("ProjectionMode", "")); + if (projection_mode == "" || projection_mode == "perspective") + capture_setting.projection_mode = 0; // Perspective + else if (projection_mode == "orthographic") + capture_setting.projection_mode = 1; // Orthographic + else + throw std::invalid_argument( + std::string("CaptureSettings projection_mode has invalid value in settings_json ") + projection_mode); + + capture_setting.ortho_width = settings_json.getFloat("OrthoWidth", capture_setting.ortho_width); + } + + static void loadSubWindowsSettings(const Settings &settings_json, + std::vector &subwindow_settings) { + // load default subwindows + initializeSubwindowSettings(subwindow_settings); + + Settings json_parent; + if (settings_json.getChild("SubWindows", json_parent)) { + for (size_t child_index = 0; child_index < json_parent.size(); ++child_index) { + Settings json_settings_child; + if (json_parent.getChild(child_index, json_settings_child)) { + int window_index = json_settings_child.getInt("WindowID", 0); + SubwindowSetting &subwindow_setting = subwindow_settings.at(window_index); + subwindow_setting.window_index = window_index; + subwindow_setting.image_type = Utils::toEnum(json_settings_child.getInt("ImageType", 0)); + subwindow_setting.visible = json_settings_child.getBool("Visible", false); + subwindow_setting.camera_name = getCameraName(json_settings_child); + subwindow_setting.vehicle_name = json_settings_child.getString("VehicleName", ""); + subwindow_setting.external = json_settings_child.getBool("External", false); + } + } + } + } + + static void initializeSubwindowSettings(std::vector &subwindow_settings) { + subwindow_settings.clear(); + subwindow_settings.push_back(SubwindowSetting(0, ImageType::DepthVis, false, "", "", false)); // depth + subwindow_settings.push_back(SubwindowSetting(1, ImageType::Segmentation, false, "", "", false)); // seg + subwindow_settings.push_back(SubwindowSetting(2, ImageType::Scene, false, "", "", false)); // vis + } + + void loadOtherSettings(const Settings &settings_json) { + // by default we spawn server at local endpoint. Do not use 127.0.0.1 as default below + // because for docker container default is 0.0.0.0 and people get really confused why things + // don't work + api_server_address = settings_json.getString("LocalHostIp", ""); + api_port = settings_json.getInt("ApiServerPort", RpcLibPort); + is_record_ui_visible = settings_json.getBool("RecordUIVisible", true); + engine_sound = settings_json.getBool("EngineSound", false); + enable_rpc = settings_json.getBool("EnableRpc", enable_rpc); + speed_unit_factor = settings_json.getFloat("SpeedUnitFactor", 1.0f); + speed_unit_label = settings_json.getString("SpeedUnitLabel", "m\\s"); + log_messages_visible = settings_json.getBool("LogMessagesVisible", true); + show_los_debug_lines_ = settings_json.getBool("ShowLosDebugLines", false); + + { // load origin geopoint + Settings origin_geopoint_json; + if (settings_json.getChild("OriginGeopoint", origin_geopoint_json)) { + GeoPoint origin = origin_geopoint.home_geo_point; + origin.latitude = origin_geopoint_json.getDouble("Latitude", origin.latitude); + origin.longitude = origin_geopoint_json.getDouble("Longitude", origin.longitude); + origin.altitude = origin_geopoint_json.getFloat("Altitude", origin.altitude); + origin_geopoint.initialize(origin); + } + } + + { // time of day settings_json + Settings tod_settings_json; + if (settings_json.getChild("TimeOfDay", tod_settings_json)) { + tod_setting.enabled = tod_settings_json.getBool("Enabled", tod_setting.enabled); + tod_setting.start_datetime = tod_settings_json.getString("StartDateTime", tod_setting.start_datetime); + tod_setting.celestial_clock_speed = + tod_settings_json.getFloat("CelestialClockSpeed", tod_setting.celestial_clock_speed); + tod_setting.is_start_datetime_dst = + tod_settings_json.getBool("StartDateTimeDst", tod_setting.is_start_datetime_dst); + tod_setting.update_interval_secs = + tod_settings_json.getFloat("UpdateIntervalSecs", tod_setting.update_interval_secs); + tod_setting.move_sun = tod_settings_json.getBool("MoveSun", tod_setting.move_sun); + } + } + + { + // Wind Settings + Settings child_json; + if (settings_json.getChild("Wind", child_json)) { + wind = createVectorSetting(child_json, wind); + } + } + { + // External Force Settings + Settings child_json; + if (settings_json.getChild("ExternalForce", child_json)) { + ext_force = createVectorSetting(child_json, ext_force); + } + } + } + + static void loadDefaultCameraSetting(const Settings &settings_json, CameraSetting &camera_defaults) { + Settings child_json; + if (settings_json.getChild("CameraDefaults", child_json)) { + camera_defaults = createCameraSetting(child_json, camera_defaults); + } + } + static void loadCameraDirectorSetting(const Settings &settings_json, CameraDirectorSetting &camera_director, + const std::string &simmode_name) { + camera_director = CameraDirectorSetting(); + + Settings child_json; + if (settings_json.getChild("CameraDirector", child_json)) { + camera_director.position = createVectorSetting(child_json, camera_director.position); + camera_director.rotation = createRotationSetting(child_json, camera_director.rotation); + camera_director.follow_distance = child_json.getFloat("FollowDistance", camera_director.follow_distance); + } + + if (std::isnan(camera_director.follow_distance)) { + if (simmode_name == kSimModeTypeCar) + camera_director.follow_distance = -8; + else + camera_director.follow_distance = -3; + } + if (std::isnan(camera_director.position.x())) + camera_director.position.x() = camera_director.follow_distance; + if (std::isnan(camera_director.position.y())) + camera_director.position.y() = 0; + if (std::isnan(camera_director.position.z())) { + if (simmode_name == kSimModeTypeCar) + camera_director.position.z() = -4; + else + camera_director.position.z() = -2; + } + } + + void loadClockSettings(const Settings &settings_json) { + clock_type = settings_json.getString("ClockType", ""); + + if (clock_type == "") { + // default value + clock_type = "ScalableClock"; + + // override if multirotor simmode with simple_flight + if (simmode_name == kSimModeTypeMultirotor) { + // TODO: this won't work if simple_flight and PX4 is combined together! + + // for multirotors we select steppable fixed interval clock unless we have + // PX4 enabled vehicle + clock_type = "SteppableClock"; + for (auto const &vehicle : vehicles) { + if (vehicle.second->auto_create && vehicle.second->vehicle_type == kVehicleTypePX4) { + clock_type = "ScalableClock"; + break; + } + } + } + } + + clock_speed = settings_json.getFloat("ClockSpeed", 1.0f); + } + + static std::shared_ptr createSensorSetting(SensorBase::SensorType sensor_type, + const std::string &sensor_name, bool enabled) { + std::shared_ptr sensor_setting; + + switch (sensor_type) { + case SensorBase::SensorType::Barometer: + sensor_setting = std::shared_ptr(new BarometerSetting()); + break; + case SensorBase::SensorType::Imu: + sensor_setting = std::shared_ptr(new ImuSetting()); + break; + case SensorBase::SensorType::Gps: + sensor_setting = std::shared_ptr(new GpsSetting()); + break; + case SensorBase::SensorType::Magnetometer: + sensor_setting = std::shared_ptr(new MagnetometerSetting()); + break; + case SensorBase::SensorType::Distance: + sensor_setting = std::shared_ptr(new DistanceSetting()); + break; + case SensorBase::SensorType::Lidar: + sensor_setting = std::shared_ptr(new LidarSetting()); + break; + default: + throw std::invalid_argument("Unexpected sensor type"); + } + + sensor_setting->sensor_type = sensor_type; + sensor_setting->sensor_name = sensor_name; + sensor_setting->enabled = enabled; + + return sensor_setting; + } + + static void initializeSensorSetting(SensorSetting *sensor_setting, const Settings &settings_json) { + sensor_setting->enabled = settings_json.getBool("Enabled", sensor_setting->enabled); + + // pass the json Settings property bag through to the specific sensor params object where it is + // extracted there. This way default values can be kept in one place. For example, see the + // BarometerSimpleParams::initializeFromSettings method. + sensor_setting->settings = settings_json; + } + + // creates and intializes sensor settings from json + static void loadSensorSettings(const Settings &settings_json, const std::string &collectionName, + std::map> &sensors, + std::map> &sensor_defaults) + + { + // NOTE: Increase type if number of sensors goes above 8 + uint8_t present_sensors_bitmask = 0; + + nervosys::autonomylib::Settings sensors_child; + if (settings_json.getChild(collectionName, sensors_child)) { + std::vector keys; + sensors_child.getChildNames(keys); + + for (const auto &key : keys) { + nervosys::autonomylib::Settings child; + sensors_child.getChild(key, child); + + auto sensor_type = Utils::toEnum(child.getInt("SensorType", 0)); + auto enabled = child.getBool("Enabled", false); + + sensors[key] = createSensorSetting(sensor_type, key, enabled); + initializeSensorSetting(sensors[key].get(), child); + + // Mark sensor types already added + present_sensors_bitmask |= 1U << Utils::toNumeric(sensor_type); + } + } + + // Only add default sensors which are not present + for (const auto &p : sensor_defaults) { + auto type = Utils::toNumeric(p.second->sensor_type); + + if ((present_sensors_bitmask & (1U << type)) == 0) + sensors[p.first] = p.second; + } + } + + // creates default sensor list when none specified in json + static void createDefaultSensorSettings(const std::string &simmode_name, + std::map> &sensors) { + if (simmode_name == kSimModeTypeMultirotor) { + sensors["imu"] = createSensorSetting(SensorBase::SensorType::Imu, "imu", true); + sensors["magnetometer"] = createSensorSetting(SensorBase::SensorType::Magnetometer, "magnetometer", true); + sensors["gps"] = createSensorSetting(SensorBase::SensorType::Gps, "gps", true); + sensors["barometer"] = createSensorSetting(SensorBase::SensorType::Barometer, "barometer", true); + } else if (simmode_name == kSimModeTypeCar) { + sensors["gps"] = createSensorSetting(SensorBase::SensorType::Gps, "gps", true); + } else { + // no sensors added for other modes + } + } + + // loads or creates default sensor list + static void loadDefaultSensorSettings(const std::string &simmode_name, const Settings &settings_json, + std::map> &sensors) { + nervosys::autonomylib::Settings sensors_child; + if (settings_json.getChild("DefaultSensors", sensors_child)) + loadSensorSettings(settings_json, "DefaultSensors", sensors, sensors); + else + createDefaultSensorSettings(simmode_name, sensors); + } + + static void loadExternalCameraSettings(const Settings &settings_json, CameraSettingMap &external_cameras, + const CameraSetting &camera_defaults) { + external_cameras.clear(); + + Settings json_parent; + if (settings_json.getChild("ExternalCameras", json_parent)) { + std::vector keys; + json_parent.getChildNames(keys); + + for (const auto &key : keys) { + Settings child; + json_parent.getChild(key, child); + external_cameras[key] = createCameraSetting(child, camera_defaults); + } + } + } +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/CancelToken.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/CancelToken.hpp new file mode 100644 index 00000000..e22bc954 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/CancelToken.hpp @@ -0,0 +1,80 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_CancelToken_hpp +#define autonomylib_common_CancelToken_hpp + +#include "common/Common.hpp" +#include "common/utils/Utils.hpp" +#include +#include + +namespace nervosys { +namespace autonomylib { + +class CancelToken { + public: + CancelToken() : is_cancelled_(false), is_complete_(false), recursion_count_(0) {} + + void reset() { + is_cancelled_ = false; + is_complete_ = false; + recursion_count_ = 0; + } + + bool isCancelled() const { return is_cancelled_; } + + void cancel() { is_cancelled_ = true; } + + bool sleep(double secs) { + // We can pass duration directly to sleep_for however it is known that on + // some systems, sleep_for makes system call anyway even if passed duration + // is <= 0. This can cause 50us of delay due to context switch. + if (isCancelled()) { + return false; + } + TTimePoint start = ClockFactory::get()->nowNanos(); + static constexpr std::chrono::duration MinSleepDuration(0); + while (secs > 0 && !isCancelled() && ClockFactory::get()->elapsedSince(start) < secs) { + std::this_thread::sleep_for(MinSleepDuration); + } + + return !isCancelled(); + } + + void complete(bool is_complete = true) { is_complete_ = is_complete; } + + bool isComplete() const { return is_complete_; } + + int getRecursionCount() { return recursion_count_; } + + bool try_lock() { + bool result = wait_mutex_.try_lock(); + if (result) + ++recursion_count_; + return result; + } + + void unlock() { + wait_mutex_.unlock(); + if (recursion_count_ > 0) + --recursion_count_; + } + + void lock() { + wait_mutex_.lock(); + ++recursion_count_; + } + + private: + std::atomic is_cancelled_; + std::atomic is_complete_; + std::atomic recursion_count_; + + std::recursive_mutex wait_mutex_; +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/ClockBase.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/ClockBase.hpp new file mode 100644 index 00000000..e0dd769c --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/ClockBase.hpp @@ -0,0 +1,82 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_ClockBase_hpp +#define autonomylib_common_ClockBase_hpp + +#include "Common.hpp" +#include +#include + +namespace nervosys { +namespace autonomylib { + +class ClockBase { + public: + // returns value indicating nanoseconds elapsed since some reference timepoint in history + // typically nanoseconds from Unix epoch + virtual TTimePoint nowNanos() const = 0; + virtual TTimePoint getStart() const = 0; + + ClockBase() { wall_clock_start_ = Utils::getTimeSinceEpochNanos(); } + + virtual ~ClockBase() = default; + + TTimeDelta elapsedSince(TTimePoint since) const { return elapsedBetween(nowNanos(), since); } + static TTimeDelta elapsedBetween(TTimePoint second, TTimePoint first) { return (second - first) / 1.0E9; } + TTimePoint addTo(TTimePoint t, TTimeDelta dt) { return static_cast(t + dt * 1.0E9); } + TTimeDelta updateSince(TTimePoint &since) const { + TTimePoint cur = nowNanos(); + TTimeDelta elapsed = elapsedBetween(cur, since); + since = cur; + return elapsed; + } + + virtual TTimePoint stepBy(TTimeDelta amount) { + unused(amount); + return step(); + } + + virtual TTimePoint step() { + // by default step doesn't do anything + // for steppeble clock, this would advance to next tick + // for wall clocks, step() is no-op + ++step_count_; + + return nowNanos(); + } + + uint64_t getStepCount() const { return step_count_; } + + virtual void sleep_for(TTimeDelta dt) { + if (dt <= 0) + return; + + static constexpr std::chrono::duration MinSleepDuration(0); + TTimePoint start = nowNanos(); + // spin wait + while (elapsedSince(start) < dt) + std::this_thread::sleep_for(MinSleepDuration); + } + + double getTrueScaleWrtWallClock() { + TTimePoint wall_clock_now = Utils::getTimeSinceEpochNanos(); + TTimeDelta wall_clock_elapsed = elapsedBetween(wall_clock_now, wall_clock_start_); + + TTimePoint clock_now = nowNanos(); + TTimeDelta clock_elapsed = elapsedBetween(clock_now, getStart()); + + return static_cast(clock_elapsed) / wall_clock_elapsed; + } + + private: + template using duration = std::chrono::duration; + + uint64_t step_count_ = 0; + TTimePoint wall_clock_start_; +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/ClockFactory.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/ClockFactory.hpp new file mode 100644 index 00000000..55d9981e --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/ClockFactory.hpp @@ -0,0 +1,40 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_ClockFactory_hpp +#define autonomylib_common_ClockFactory_hpp + +#include "ScalableClock.hpp" +#include + +namespace nervosys { +namespace autonomylib { + +class ClockFactory { + public: + // output of this function should not be stored as pointer might change + static ClockBase *get(std::shared_ptr val = nullptr) { + static std::shared_ptr clock; + + if (val != nullptr) + clock = val; + + if (clock == nullptr) + clock = std::make_shared(); + + return clock.get(); + } + + // don't allow multiple instances of this class + ClockFactory(ClockFactory const &) = delete; + void operator=(ClockFactory const &) = delete; + + private: + // disallow instance creation + ClockFactory() {} +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/Common.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/Common.hpp new file mode 100644 index 00000000..92f7fba8 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/Common.hpp @@ -0,0 +1,72 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_Common_hpp +#define autonomylib_common_Common_hpp + +#include +#include +#include +#include +#include +#include +#include + +#include "VectorMath.hpp" +#include "common/utils/RandomGenerator.hpp" +#include "common/utils/Utils.hpp" + +#ifndef _CRT_SECURE_NO_WARNINGS +#define _CRT_SECURE_NO_WARNINGS 1 +#endif + +#define RpcLibPort 41451 + +namespace nervosys { +namespace autonomylib { + +// numericals +typedef float real_T; +// this is not required for most compilers +typedef unsigned int uint; + +// well known types +typedef nervosys::autonomylib::VectorMathf VectorMath; +typedef VectorMath::Vector3f Vector3r; +typedef VectorMath::Vector2f Vector2r; +typedef VectorMath::Vector1f Vector1r; +typedef VectorMath::Array3f Array3r; +typedef VectorMath::Pose Pose; +typedef VectorMath::Quaternionf Quaternionr; +typedef VectorMath::Matrix3x3f Matrix3x3r; +typedef VectorMath::AngleAxisf AngleAxisr; +typedef common_utils::RandomGeneratorF RandomGeneratorR; +typedef common_utils::RandomGeneratorGaussianF RandomGeneratorGausianR; +typedef std::string string; +typedef common_utils::Utils Utils; +typedef VectorMath::RandomVectorGaussianT RandomVectorGaussianR; +typedef VectorMath::RandomVectorT RandomVectorR; +typedef uint64_t TTimePoint; +typedef double TTimeDelta; + +template using vector = std::vector; +template using unordered_map = std::unordered_map; +template using unordered_set = std::unordered_set; +template using unique_ptr = std::unique_ptr; +template using shared_ptr = std::shared_ptr; +template using vector_size_type = typename std::vector::size_type; + +inline std::ostream &operator<<(std::ostream &os, Quaternionr const &q) { + float p, r, y; + VectorMath::toEulerianAngle(q, p, r, y); + return os << "(" << r << "\t" << p << "\t" << y << ")" << q.w() << q.x() << "\t" << q.y() << "\t" << q.z() << "\t"; +} + +inline std::ostream &operator<<(std::ostream &os, Vector3r const &vec) { + return os << vec.x() << "\t" << vec.y() << "\t" << vec.z() << "\t"; +} + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/CommonStructs.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/CommonStructs.hpp new file mode 100644 index 00000000..b829ae4f --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/CommonStructs.hpp @@ -0,0 +1,343 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_CommonStructs_hpp +#define autonomylib_common_CommonStructs_hpp + +#include + +#include "common/Common.hpp" + +namespace nervosys { +namespace autonomylib { + +// velocity +struct Twist { + Vector3r linear, angular; + + Twist() {} + + Twist(const Vector3r &linear_val, const Vector3r &angular_val) : linear(linear_val), angular(angular_val) {} + + static const Twist zero() { + static const Twist zero_twist(Vector3r::Zero(), Vector3r::Zero()); + return zero_twist; + } +}; + +// force & torque +struct Wrench { + Vector3r force, torque; + + Wrench() {} + + Wrench(const Vector3r &force_val, const Vector3r &torque_val) : force(force_val), torque(torque_val) {} + + // support basic arithmatic + Wrench operator+(const Wrench &other) const { + Wrench result; + result.force = this->force + other.force; + result.torque = this->torque + other.torque; + return result; + } + Wrench operator+=(const Wrench &other) { + force += other.force; + torque += other.torque; + return *this; + } + Wrench operator-(const Wrench &other) const { + Wrench result; + result.force = this->force - other.force; + result.torque = this->torque - other.torque; + return result; + } + Wrench operator-=(const Wrench &other) { + force -= other.force; + torque -= other.torque; + return *this; + } + + static const Wrench zero() { + static const Wrench zero_wrench(Vector3r::Zero(), Vector3r::Zero()); + return zero_wrench; + } +}; + +struct Momentums { + Vector3r linear; + Vector3r angular; + + Momentums() {} + + Momentums(const Vector3r &linear_val, const Vector3r &angular_val) : linear(linear_val), angular(angular_val) {} + + static const Momentums zero() { + static const Momentums zero_val(Vector3r::Zero(), Vector3r::Zero()); + return zero_val; + } +}; + +struct Accelerations { + Vector3r linear; + Vector3r angular; + + Accelerations() {} + + Accelerations(const Vector3r &linear_val, const Vector3r &angular_val) : linear(linear_val), angular(angular_val) {} + + static const Accelerations zero() { + static const Accelerations zero_val(Vector3r::Zero(), Vector3r::Zero()); + return zero_val; + } +}; + +struct PoseWithCovariance { + VectorMath::Pose pose; + vector covariance; // 36 elements, 6x6 matrix + + PoseWithCovariance() : covariance(36, 0) {} +}; + +struct PowerSupply { + vector voltage, current; +}; + +struct TwistWithCovariance { + Twist twist; + vector covariance; // 36 elements, 6x6 matrix + + TwistWithCovariance() : covariance(36, 0) {} +}; + +struct Joystick { + vector axes; + vector buttons; +}; + +struct Odometry { + PoseWithCovariance pose; + TwistWithCovariance twist; +}; + +struct GeoPoint { + double latitude = 0, longitude = 0; + float altitude = 0; + + GeoPoint() {} + + GeoPoint(double latitude_val, double longitude_val, float altitude_val) { + set(latitude_val, longitude_val, altitude_val); + } + + void set(double latitude_val, double longitude_val, float altitude_val) { + latitude = latitude_val, longitude = longitude_val; + altitude = altitude_val; + } + + friend std::ostream &operator<<(std::ostream &os, GeoPoint const &g) { + return os << "[" << g.latitude << ", " << g.longitude << ", " << g.altitude << "]"; + } + + std::string to_string() const { + return std::to_string(latitude) + string(", ") + std::to_string(longitude) + string(", ") + + std::to_string(altitude); + } +}; + +struct HomeGeoPoint { + GeoPoint home_geo_point; + double lat_rad, lon_rad; + double cos_lat, sin_lat; + + HomeGeoPoint() {} + HomeGeoPoint(const GeoPoint &home_geo_point_val) { initialize(home_geo_point_val); } + void initialize(const GeoPoint &home_geo_point_val) { + home_geo_point = home_geo_point_val; + lat_rad = Utils::degreesToRadians(home_geo_point.latitude); + lon_rad = Utils::degreesToRadians(home_geo_point.longitude); + cos_lat = cos(lat_rad); + sin_lat = sin(lat_rad); + } +}; + +struct ProjectionMatrix { + float matrix[4][4]; + + void setTo(float val) { + for (auto i = 0; i < 4; ++i) + for (auto j = 0; j < 4; ++j) + matrix[i][j] = val; + } +}; + +struct CollisionInfo { + bool has_collided = false; + Vector3r normal = Vector3r::Zero(); + Vector3r impact_point = Vector3r::Zero(); + Vector3r position = Vector3r::Zero(); + real_T penetration_depth = 0; + TTimePoint time_stamp = 0; + unsigned int collision_count = 0; + std::string object_name; + int object_id = -1; + + CollisionInfo() {} + + CollisionInfo(bool has_collided_val, const Vector3r &normal_val, const Vector3r &impact_point_val, + const Vector3r &position_val, real_T penetration_depth_val, TTimePoint time_stamp_val, + const std::string &object_name_val, int object_id_val) + : has_collided(has_collided_val), normal(normal_val), impact_point(impact_point_val), position(position_val), + penetration_depth(penetration_depth_val), time_stamp(time_stamp_val), object_name(object_name_val), + object_id(object_id_val) {} +}; + +struct CameraInfo { + Pose pose; + float fov; + ProjectionMatrix proj_mat; + + CameraInfo() {} + + CameraInfo(const Pose &pose_val, float fov_val, const ProjectionMatrix &proj_mat_val) + : pose(pose_val), fov(fov_val), proj_mat(proj_mat_val) {} +}; + +struct Box2D { + Vector2r min; + Vector2r max; + + Box2D() {} + + Box2D(Vector2r min_val, Vector2r max_val) : min(min_val), max(max_val) {} +}; + +struct Box3D { + Vector3r min; + Vector3r max; + + Box3D() {} + + Box3D(Vector3r min_val, Vector3r max_val) : min(min_val), max(max_val) {} +}; + +struct DetectionInfo { + std::string name = ""; + GeoPoint geo_point = GeoPoint(); + Box2D box2D = Box2D(); + Box3D box3D = Box3D(); + Pose relative_pose = Pose(); + + DetectionInfo() {} + + DetectionInfo(const std::string &name_val, const GeoPoint &geo_point_val, const Box2D &box2D_val, + const Box3D &box3D_val, const Pose &relative_pose_val) + : name(name_val), geo_point(geo_point_val), box2D(box2D_val), box3D(box3D_val), + relative_pose(relative_pose_val) {} +}; + +struct CollisionResponse { + unsigned int collision_count_raw = 0; + unsigned int collision_count_non_resting = 0; + TTimePoint collision_time_stamp = 0; +}; + +struct GeoPose { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Quaternionr orientation; + GeoPoint position; +}; + +struct RCData { + TTimePoint timestamp = 0; + // pitch, roll, yaw should be in range -1 to 1 + // switches should be integer value indicating its state, 0=on, 1=off for example. + float pitch = 0, roll = 0, throttle = 0, yaw = 0; + float left_z = 0, right_z = 0; + uint16_t switches = 0; + std::string vendor_id = ""; + bool is_initialized = false; // is RC connected? + bool is_valid = false; // must be true for data to be valid + + unsigned int getSwitch(uint16_t index) const { return switches & (1 << index) ? 1 : 0; } + + void add(const RCData &other) { + pitch += other.pitch; + roll += other.roll; + throttle += other.throttle; + yaw += other.yaw; + } + void subtract(const RCData &other) { + pitch -= other.pitch; + roll -= other.roll; + throttle -= other.throttle; + yaw -= other.yaw; + } + void divideBy(float k) { + pitch /= k; + roll /= k; + throttle /= k; + yaw /= k; + } + bool isAnyMoreThan(float k) { + using std::abs; + return abs(pitch) > k || abs(roll) > k || abs(throttle) > k || abs(yaw) > k; + } + string toString() { + return Utils::stringf("RCData[pitch=%f, roll=%f, throttle=%f, yaw=%f]", pitch, roll, throttle, yaw); + } +}; + +struct LidarData { + TTimePoint time_stamp = 0; + // data + // - array of floats that represent [x,y,z] coordinate for each point hit within the range + // x0, y0, z0, x1, y1, z1, ..., xn, yn, zn + // TODO: Do we need an intensity place-holder [x,y,z, intensity]? + // - in lidar local NED coordinates + // - in meters + vector point_cloud; + Pose pose; + vector segmentation; + + LidarData() {} +}; + +struct DistanceSensorData { + TTimePoint time_stamp; + real_T distance; // meters + real_T min_distance; // m + real_T max_distance; // m + Pose relative_pose; + + DistanceSensorData() {} +}; + +struct MeshPositionVertexBuffersResponse { + Vector3r position; + Quaternionr orientation; + + std::vector vertices; + std::vector indices; + std::string name; +}; + +// This is a small helper struct to keep camera details together +// Not currently exposed to the client, just for cleaner codebase internally +struct CameraDetails { + std::string camera_name; + std::string vehicle_name; + bool external; + + CameraDetails(const std::string &camera_name_val, const std::string &vehicle_name_val, bool external_val) + : camera_name(camera_name_val), vehicle_name(vehicle_name_val), external(external_val) {} + + std::string to_string() const { + return Utils::stringf("CameraDetails: camera_name=%s, vehicle_name=%s, external=%d", camera_name.c_str(), + vehicle_name.c_str(), external); + } +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/DelayLine.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/DelayLine.hpp new file mode 100644 index 00000000..592cb748 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/DelayLine.hpp @@ -0,0 +1,72 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_DelayLine_hpp +#define autonomylib_common_DelayLine_hpp + +#include "UpdatableObject.hpp" +#include "common/Common.hpp" +#include + +namespace nervosys { +namespace autonomylib { + +template class DelayLine : public UpdatableObject { + public: + DelayLine() {} + DelayLine(TTimeDelta delay) // in seconds + { + initialize(delay); + } + void initialize(TTimeDelta delay) // in seconds + { + setDelay(delay); + } + void setDelay(TTimeDelta delay) { delay_ = delay; } + double getDelay() const { return delay_; } + + //*** Start: UpdatableState implementation ***// + virtual void resetImplementation() override { + values_.clear(); + times_.clear(); + last_time_ = 0; + last_value_ = T(); + } + + virtual void update() override { + UpdatableObject::update(); + + if (!times_.empty() && ClockBase::elapsedBetween(clock()->nowNanos(), times_.front()) >= delay_) { + + last_value_ = values_.front(); + last_time_ = times_.front(); + + times_.pop_front(); + values_.pop_front(); + } + } + //*** End: UpdatableState implementation ***// + + T getOutput() const { return last_value_; } + double getOutputTime() const { return last_time_; } + + void push_back(const T &val, TTimePoint time_offset = 0) { + values_.push_back(val); + times_.push_back(clock()->nowNanos() + time_offset); + } + + private: + template using list = std::list; + + list values_; + list times_; + TTimeDelta delay_; + + T last_value_; + TTimePoint last_time_; +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/EarthCelestial.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/EarthCelestial.hpp new file mode 100644 index 00000000..82845489 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/EarthCelestial.hpp @@ -0,0 +1,189 @@ +/* +Adopted from SunCalc by Vladimir Agafonkin +https://github.com/mourner/suncalc +*/ + +#ifndef autonomylib_common_EarthCelestial_hpp +#define autonomylib_common_EarthCelestial_hpp + +#include "EarthUtils.hpp" +#include "common/Common.hpp" +#include +#include + +namespace nervosys { +namespace autonomylib { + +class EarthCelestial { + public: + struct CelestialGlobalCoord { + double declination; + double rightAscension; + double distance = Utils::nan(); + double parallacticAngle = Utils::nan(); + }; + + struct CelestialLocalCoord { + double azimuth; + double altitude; + double distance = Utils::nan(); + double parallacticAngle = Utils::nan(); + }; + + struct CelestialPhase { + double fraction; + double phase; + double angle; + }; + + public: + static CelestialLocalCoord getSunCoordinates(uint64_t date, double lat, double lng) { + double lw = Utils::degreesToRadians(-lng); + double phi = Utils::degreesToRadians(lat); + double d = toDays(date); + + CelestialGlobalCoord c = getGlobalSunCoords(d); + double H = siderealTime(d, lw) - c.rightAscension; + + CelestialLocalCoord coord; + coord.azimuth = Utils::radiansToDegrees(azimuth(H, phi, c.declination)) + 180.0; + coord.altitude = Utils::radiansToDegrees(altitude(H, phi, c.declination)); + + return coord; + } + + static CelestialLocalCoord getMoonCoordinates(uint64_t date, double lat, double lng) { + + double lw = Utils::degreesToRadians(-lng); + double phi = Utils::degreesToRadians(lat); + double d = toDays(date); + + CelestialGlobalCoord c = getGlobalMoonCoords(d); + double H = siderealTime(d, lw) - c.rightAscension; + + // formula 14.1 of "Astronomical Algorithms" 2nd edition by Jean Meeus (Willmann-Bell, Richmond) 1998. + double pa = + std::atan2(std::sin(H), std::tan(phi) * std::cos(c.declination) - std::sin(c.declination) * std::cos(H)); + + double h = altitude(H, phi, c.declination); + h = h + astroRefraction(h); // altitude correction for refraction + + CelestialLocalCoord coord; + coord.azimuth = Utils::radiansToDegrees(azimuth(H, phi, c.declination)); + coord.altitude = Utils::radiansToDegrees(h); + coord.distance = c.distance; + coord.parallacticAngle = Utils::radiansToDegrees(pa); + return coord; + }; + + // calculations for illumination parameters of the moon, + // based on http://idlastro.gsfc.nasa.gov/ftp/pro/astro/mphase.pro formulas and + // Chapter 48 of "Astronomical Algorithms" 2nd edition by Jean Meeus (Willmann-Bell, Richmond) 1998. + static CelestialPhase getMoonPhase(uint64_t date) { + double d = toDays(date); + CelestialGlobalCoord s = getGlobalSunCoords(d); + CelestialGlobalCoord m = getGlobalMoonCoords(d); + + double sdist = EarthUtils::DistanceFromSun / 1000; // distance from Earth to Sun in km + + double phi = std::acos(std::sin(s.declination) * std::sin(m.declination) + + std::cos(s.declination) * std::cos(m.declination) * + std::cos(s.rightAscension - m.rightAscension)); + double inc = std::atan2(sdist * std::sin(phi), m.distance - sdist * std::cos(phi)); + double angle = std::atan2(std::cos(s.declination) * std::sin(s.rightAscension - m.rightAscension), + std::sin(s.declination) * std::cos(m.declination) - + std::cos(s.declination) * std::sin(m.declination) * + std::cos(s.rightAscension - m.rightAscension)); + + CelestialPhase moonPhase; + moonPhase.fraction = (1 + cos(inc)) / 2; + moonPhase.phase = 0.5 + 0.5 * inc * (angle < 0 ? -1 : 1) / M_PI; + moonPhase.angle = angle; + return moonPhase; + }; + + private: + static double toDays(uint64_t date) { + static constexpr double kJulianDaysOnY2000 = 2451545; + static constexpr double kDaysToHours = 60 * 60 * 24; + static constexpr double kJulianDaysOnEpoch = 2440588; + + double julian_days = date / kDaysToHours - 0.5 + kJulianDaysOnEpoch; + ; + return julian_days - kJulianDaysOnY2000; + } + + static double rightAscension(double l, double b) { + return std::atan2(std::sin(l) * std::cos(EarthUtils::Obliquity) - std::tan(b) * std::sin(EarthUtils::Obliquity), + std::cos(l)); + } + + static double declination(double l, double b) { + return std::asin(std::sin(b) * std::cos(EarthUtils::Obliquity) + + std::cos(b) * std::sin(EarthUtils::Obliquity) * std::sin(l)); + } + + static double azimuth(double H, double phi, double declination) { + return std::atan2(std::sin(H), std::cos(H) * std::sin(phi) - std::tan(declination) * std::cos(phi)); + } + + static double altitude(double H, double phi, double declination) { + return std::asin(std::sin(phi) * std::sin(declination) + std::cos(phi) * std::cos(declination) * std::cos(H)); + } + + static double siderealTime(double d, double lw) { return Utils::degreesToRadians((280.16 + 360.9856235 * d)) - lw; } + + static double astroRefraction(double h) { + if (h < 0) // the following formula works for positive altitudes only. + h = 0; // if h = -0.08901179 a div/0 would occur. + + // formula 16.4 of "Astronomical Algorithms" 2nd edition by Jean Meeus (Willmann-Bell, Richmond) 1998. + // 1.02 / tan(h + 10.26 / (h + 5.10)) h in degrees, result in arc minutes -> converted to rad: + return 0.0002967 / std::tan(h + 0.00312536 / (h + 0.08901179)); + } + + static double solarMeanAnomaly(double d) { return Utils::degreesToRadians((357.5291 + 0.98560028 * d)); } + + static double eclipticLongitude(double M) { + double C = Utils::degreesToRadians( + (1.9148 * std::sin(M) + 0.02 * std::sin(2 * M) + 0.0003 * std::sin(3 * M))); // equation of center + + return M + C + EarthUtils::Perihelion + M_PI; + } + + static CelestialGlobalCoord getGlobalSunCoords(double d) { + double M = solarMeanAnomaly(d); + double L = eclipticLongitude(M); + + CelestialGlobalCoord sunCoords; + sunCoords.declination = declination(L, 0); + sunCoords.rightAscension = rightAscension(L, 0); + + return sunCoords; + } + + // moon calculations, based on http://aa.quae.nl/en/reken/hemelpositie.html formulas + static CelestialGlobalCoord getGlobalMoonCoords(double d) { + // geocentric ecliptic coordinates of the moon + + double L = Utils::degreesToRadians((218.316 + 13.176396 * d)); // ecliptic longitude + double M = Utils::degreesToRadians((134.963 + 13.064993 * d)); // mean anomaly + double F = Utils::degreesToRadians((93.272 + 13.229350 * d)); // mean distance + + double l = L + Utils::degreesToRadians(6.289 * std::sin(M)); // longitude + double b = Utils::degreesToRadians(5.128 * std::sin(F)); // latitude + double dt = 385001 - 20905 * std::cos(M); // distance to the moon in km + + CelestialGlobalCoord moonCoords; + moonCoords.rightAscension = rightAscension(l, b); + moonCoords.declination = declination(l, b); + moonCoords.distance = dt; + + return moonCoords; + } +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/EarthUtils.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/EarthUtils.hpp new file mode 100644 index 00000000..0f746bca --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/EarthUtils.hpp @@ -0,0 +1,400 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_EarthUtils_hpp +#define autonomylib_common_EarthUtils_hpp + +#include "common/Common.hpp" +#include "common/CommonStructs.hpp" +#include +#include + +namespace nervosys { +namespace autonomylib { + +class EarthUtils { + private: + /** set this always to the sampling in degrees for the table below */ + static constexpr int MAG_SAMPLING_RES = 10; + static constexpr int MAG_SAMPLING_MIN_LAT = -60; + static constexpr int MAG_SAMPLING_MAX_LAT = 60; + static constexpr int MAG_SAMPLING_MIN_LON = -180; + static constexpr int MAG_SAMPLING_MAX_LON = 180; + + static constexpr int DECLINATION_TABLE[13][37] = { + {46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, + -26, -33, -40, -48, -55, -61, -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46}, + {30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, + -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30}, + {21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, + -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21}, + {16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, + -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16}, + {12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, + -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12}, + {10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, + -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10}, + {9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, + -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9}, + {8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, + 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8}, + {8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, + 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8}, + {6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, + 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6}, + {5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, + 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5}, + {4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, + 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4}, + {3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, + 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3}, + }; + + public: + // return declination in degrees + // Ref: https://github.com/PX4/ecl/blob/master/EKF/geo.cpp + static real_T getMagDeclination(real_T latitude, real_T longitude) { + /* + * If the values exceed valid ranges, return zero as default + * as we have no way of knowing what the closest real value + * would be. + */ + if (latitude < -90.0f || latitude > 90.0f || longitude < -180.0f || longitude > 180.0f) { + throw std::out_of_range( + Utils::stringf("invalid latitude (%f) or longitude (%f) value", latitude, longitude)); + } + + /* round down to nearest sampling resolution */ + int min_lat = static_cast(latitude / MAG_SAMPLING_RES) * MAG_SAMPLING_RES; + int min_lon = static_cast(longitude / MAG_SAMPLING_RES) * MAG_SAMPLING_RES; + + /* for the rare case of hitting the bounds exactly + * the rounding logic wouldn't fit, so enforce it. + */ + + /* limit to table bounds - required for maxima even when table spans full globe range */ + if (latitude <= MAG_SAMPLING_MIN_LAT) { + min_lat = MAG_SAMPLING_MIN_LAT; + } + + if (latitude >= MAG_SAMPLING_MAX_LAT) { + min_lat = static_cast(latitude / MAG_SAMPLING_RES) * MAG_SAMPLING_RES - MAG_SAMPLING_RES; + } + + if (longitude <= MAG_SAMPLING_MIN_LON) { + min_lon = MAG_SAMPLING_MIN_LON; + } + + if (longitude >= MAG_SAMPLING_MAX_LON) { + min_lon = static_cast(longitude / MAG_SAMPLING_RES) * MAG_SAMPLING_RES - MAG_SAMPLING_RES; + } + + /* find index of nearest low sampling point */ + int min_lat_index = (-(MAG_SAMPLING_MIN_LAT) + min_lat) / MAG_SAMPLING_RES; + int min_lon_index = (-(MAG_SAMPLING_MIN_LON) + min_lon) / MAG_SAMPLING_RES; + + real_T declination_sw = get_mag_lookup_table_val(min_lat_index, min_lon_index); + real_T declination_se = get_mag_lookup_table_val(min_lat_index, min_lon_index + 1); + real_T declination_ne = get_mag_lookup_table_val(min_lat_index + 1, min_lon_index + 1); + real_T declination_nw = get_mag_lookup_table_val(min_lat_index + 1, min_lon_index); + + /* perform bilinear interpolation on the four grid corners */ + + real_T declination_min = + ((longitude - min_lon) / MAG_SAMPLING_RES) * (declination_se - declination_sw) + declination_sw; + real_T declination_max = + ((longitude - min_lon) / MAG_SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw; + + return ((latitude - min_lat) / MAG_SAMPLING_RES) * (declination_max - declination_min) + declination_min; + } + + // geopot_height = Earth_radius * altitude / (Earth_radius + altitude) /// all in kilometers + // temperature is in Kelvin = 273.15 + celcius + static real_T getStandardTemperature(real_T geopot_height) { + // standard atmospheric pressure + // Below 51km: Practical Meteorology by Roland Stull, pg 12 + // Above 51km: http://www.braeunig.us/space/atmmodel.htm + if (geopot_height <= 11) // troposphere + return 288.15f - (6.5f * geopot_height); + else if (geopot_height <= 20) // Staroshere starts + return 216.65f; + else if (geopot_height <= 32) + return 196.65f + geopot_height; + else if (geopot_height <= 47) + return 228.65f + 2.8f * (geopot_height - 32); + else if (geopot_height <= 51) // Mesosphere starts + return 270.65f; + else if (geopot_height <= 71) + return 270.65f - 2.8f * (geopot_height - 51); + else if (geopot_height <= 84.85f) + return 214.65f - 2 * (geopot_height - 71); + else + return 3; + // Thermospehere has high kinetic temperature (500c to 2000c) but temperature + // as measured by thermometer would be very low because of almost vacuum + // throw std::out_of_range("geopot_height must be less than 85km. Space domain is not supported yet!"); + } + + static real_T getGeopotential(real_T altitude_km) { + static constexpr real_T radius_km = EARTH_RADIUS / 1000.0f; + return radius_km * altitude_km / (radius_km + altitude_km); + } + + static real_T getStandardPressure(real_T altitude /* meters */) // return Pa + { + real_T geopot_height = getGeopotential(altitude / 1000.0f); + + real_T t = getStandardTemperature(geopot_height); + + return getStandardPressure(geopot_height, t); + } + + static real_T getStandardPressure(real_T geopot_height, real_T std_temperature) // return Pa + { + // Below 51km: Practical Meteorology by Roland Stull, pg 12 + // Above 51km: http://www.braeunig.us/space/atmmodel.htm + // Validation data: https://www.avs.org/AVS/files/c7/c7edaedb-95b2-438f-adfb-36de54f87b9e.pdf + + // TODO: handle -ve altitude better (shouldn't grow indefinitely!) + + if (geopot_height <= 11) + // at alt 0, return sea level pressure + return SeaLevelPressure * powf(288.15f / std_temperature, -5.255877f); + else if (geopot_height <= 20) + return 22632.06f * expf(-0.1577f * (geopot_height - 11)); + else if (geopot_height <= 32) + return 5474.889f * powf(216.65f / std_temperature, 34.16319f); + else if (geopot_height <= 47) + return 868.0187f * powf(228.65f / std_temperature, 12.2011f); + else if (geopot_height <= 51) + return 110.9063f * exp(-0.1262f * (geopot_height - 47)); + else if (geopot_height <= 71) + return 66.93887f * powf(270.65f / std_temperature, -12.2011f); + else if (geopot_height <= 84.85f) + return 3.956420f * powf(214.65f / std_temperature, -17.0816f); + else + return 1E-3f; + // throw std::out_of_range("altitude must be less than 86km. Space domain is not supported yet!"); + } + + static real_T getAirDensity(real_T std_pressure, real_T std_temperature) // kg / m^3 + { + // http://www.braeunig.us/space/atmmodel.htm + return std_pressure / 287.053f / std_temperature; + } + + static real_T getAirDensity(real_T altitude) // kg / m^3 + { + real_T geo_pot = getGeopotential(altitude / 1000.0f); + real_T std_temperature = getStandardTemperature(geo_pot); + real_T std_pressure = getStandardPressure(geo_pot, std_temperature); + return getAirDensity(std_pressure, std_temperature); + } + + static real_T getSpeedofSound(real_T altitude) // m/s + { + // http://www.braeunig.us/space/atmmodel.htm + return sqrt(1.400f * 287.053f * getStandardTemperature(getGeopotential(altitude))); + } + + static real_T getGravity(real_T altitude) { + // derivation: http://www.citycollegiate.com/gravitation_XId.htm + if (altitude < 10000 && altitude > -10000) // up to 10 km, difference is too small + return EarthUtils::Gravity; + if (altitude < 100000 && altitude > -100000) // use first exproximation using binomial expansion + return EarthUtils::Gravity * (1 - 2 * altitude / EARTH_RADIUS); + else { + real_T factor = 1 + altitude / EARTH_RADIUS; + return EarthUtils::Gravity / factor / factor; + } + } + + static Vector3r getMagField(const GeoPoint &geo_point) // return Tesla + { + double declination, inclination; + return getMagField(geo_point, declination, inclination); + } + + static Vector3r getMagField(const GeoPoint &geo_point, double &declination, double &inclination) // return Tesla + { + /* + We calculate magnetic field using simple dipol model of Earth, i.e., assume + earth as perfect dipole sphere and ignoring all but first order terms. + This obviously is inaccurate because of huge amount of irregularities, magnetic pole that is + constantly moving, shape of Earth, higher order terms, dipole that is not perfectly aligned etc. + For simulation we are not looking for actual values of magnetic field but rather if field changes + correctly as vehicle moves in any direction and if field component signs are correct. For this purpose, simple + diapole model is good enough. Keep in mind that actual field values may differ by as much as 10X in either direction + although for many tests differences seems to be within 3X or sometime even to first decimal digit. Again what + matters is how field changes wrt to movement as opposed to actual field values. To get better field strength one + should use latest World Magnetic Model like WMM2015 from NOAA. However these recent model is fairly complex and very + expensive to calculate. Other possibilities: + - WMM2010 mocel, expensive to compute: http://williams.best.vwh.net/magvar/magfield.c + - Android's mag field calculation (still uses WMM2010 and fails at North Pole): https://goo.gl/1CZB9x + + Performance: + This function takes about 1 microsecond on Lenovo P50 laptop (Intel Xeon E3-1505M v5 CPU) + Basic trignometry functions runs at 30ns. + + Accuracy: + Two points separated by sqrt(2 km) + Dipole Model: 2.50394e-05 3.40771e-06 3.6567e-05 (dec: 7.7500, inc: 55.3530) + WMM2015 Model: 1.8350e-05 5.201e-06 5.0158e-05 (dec: 15.8248, inc: 69.1805) + geo: 47.637 -122.147 622 + + Dipole Model: 2.5047e-05 3.41024e-06 3.65953e-05 (dec: 7.7536, inc: 55.36532) + WMM2015 Model: 1.8353e-05 5.203e-06 5.0191e-05 (dec: 15.8278, inc: 69.1897) + geo: 47.646 -122.134 -378 + */ + + // ref: The Earth's Magnetism: An Introduction for Geologists, Roberto Lanza, Antonio Meloni + // Sec 1.2.5, pg 27-30 https://goo.gl/bRm7wt + // some theory at http://www.tulane.edu/~sanelson/eens634/Hmwk6MagneticField.pdf + + double lat = Utils::degreesToRadians(geo_point.latitude); // geographic colatitude + double lon = Utils::degreesToRadians(geo_point.longitude); + double altitude = geo_point.altitude + EARTH_RADIUS; + + // cache value + double sin_MagPoleLat = sin(MagPoleLat); + double cos_MagPoleLat = cos(MagPoleLat); + double cos_lat = cos(lat); + double sin_lat = sin(lat); + + // find magnetic colatitude + double mag_clat = acos(cos_lat * cos_MagPoleLat + sin_lat * sin_MagPoleLat * cos(lon - MagPoleLon)); + + // calculation of magnetic longitude is not needed but just in case if someone wants it + // double mag_lon = asin( + // (sin(lon - MagPoleLon) * sin(lat)) / + // sin(mag_clat)); + + // field strength only depends on magnetic colatitude + // https://en.wikipedia.org/wiki/Dipole_model_of_the_Earth's_magnetic_field + double cos_mag_clat = cos(mag_clat); + double field_mag = MeanMagField * pow(EARTH_RADIUS / altitude, 3) * sqrt(1 + 3 * cos_mag_clat * cos_mag_clat); + + // find inclination and declination + // equation of declination in above referenced book is only partial + // full equation is (4a) at http://www.tulane.edu/~sanelson/eens634/Hmwk6MagneticField.pdf + double lat_test = sin_MagPoleLat * sin_lat; + double dec_factor = cos_MagPoleLat / sin(mag_clat); + if (cos_mag_clat > lat_test) + declination = asin(sin(lon - MagPoleLon) * dec_factor); + else + declination = asin(cos(lon - MagPoleLon) * dec_factor); + inclination = atan(2.0 / tan(mag_clat)); // do not use atan2 here + + // transform magnetic field vector to geographical coordinates + // ref: http://www.geo.mtu.edu/~jdiehl/magnotes.html + double field_xy = field_mag * cos(inclination); + return Vector3r(static_cast(field_xy * cos(declination)), + static_cast(field_xy * sin(declination)), + static_cast(field_mag * sin(inclination))); + } + + static GeoPoint nedToGeodetic(const Vector3r &v, const HomeGeoPoint &home_geo_point) { + double x_rad = v.x() / EARTH_RADIUS; + double y_rad = v.y() / EARTH_RADIUS; + double c = sqrt(x_rad * x_rad + y_rad * y_rad); + double sin_c = sin(c), cos_c = cos(c); + double lat_rad, lon_rad; + if (!Utils::isApproximatelyZero(c)) { // avoids large changes? + lat_rad = asin(cos_c * home_geo_point.sin_lat + (x_rad * sin_c * home_geo_point.cos_lat) / c); + lon_rad = (home_geo_point.lon_rad + atan2(y_rad * sin_c, c * home_geo_point.cos_lat * cos_c - + x_rad * home_geo_point.sin_lat * sin_c)); + + return GeoPoint(Utils::radiansToDegrees(lat_rad), Utils::radiansToDegrees(lon_rad), + home_geo_point.home_geo_point.altitude - v.z()); + } else + return GeoPoint(home_geo_point.home_geo_point.latitude, home_geo_point.home_geo_point.longitude, + home_geo_point.home_geo_point.altitude - v.z()); + } + + static Vector3r GeodeticToEcef(const GeoPoint &geo) { + // Convert geodetic coordinates to ECEF. + // http://code.google.com/p/pysatel/source/browse/trunk/coord.py?r=22 + double lat_rad = Utils::degreesToRadians(geo.latitude); + double lon_rad = Utils::degreesToRadians(geo.longitude); + double xi = sqrt(1 - (6.69437999014 * 0.001) * sin(lat_rad) * sin(lat_rad)); + real_T x = static_cast((EARTH_RADIUS / xi + geo.altitude) * cos(lat_rad) * cos(lon_rad)); + real_T y = static_cast((EARTH_RADIUS / xi + geo.altitude) * cos(lat_rad) * sin(lon_rad)); + real_T z = + static_cast((EARTH_RADIUS / xi * (1 - (6.69437999014 * 0.001)) + geo.altitude) * sin(lat_rad)); + return Vector3r(x, y, z); + } + + static Vector3r EcefToNed(const Vector3r &ecefxyz, const Vector3r &ecefhome, const GeoPoint &geohome) { + // Converts ECEF coordinate position into local-tangent-plane NED. + // Coordinates relative to given ECEF coordinate frame. + + Vector3r vect, ret; + Matrix3x3r ecef_to_ned_matrix_; + double lat_rad = Utils::degreesToRadians(geohome.latitude); + double lon_rad = Utils::degreesToRadians(geohome.longitude); + vect = ecefxyz - ecefhome; + ecef_to_ned_matrix_(0, 0) = static_cast(-sin(lat_rad) * cos(lon_rad)); + ecef_to_ned_matrix_(0, 1) = static_cast(-sin(lat_rad) * sin(lon_rad)); + ecef_to_ned_matrix_(0, 2) = static_cast(cos(lat_rad)); + ecef_to_ned_matrix_(1, 0) = static_cast(-sin(lon_rad)); + ecef_to_ned_matrix_(1, 1) = static_cast(cos(lon_rad)); + ecef_to_ned_matrix_(1, 2) = 0.0f; + ecef_to_ned_matrix_(2, 0) = static_cast(cos(lat_rad) * cos(lon_rad)); + ecef_to_ned_matrix_(2, 1) = static_cast(cos(lat_rad) * sin(lon_rad)); + ecef_to_ned_matrix_(2, 2) = static_cast(sin(lat_rad)); + ret = ecef_to_ned_matrix_ * vect; + return Vector3r(ret(0), ret(1), -ret(2)); + } + + static Vector3r GeodeticToNed(const GeoPoint &geo, const GeoPoint &geohome) { + return EcefToNed(GeodeticToEcef(geo), GeodeticToEcef(geohome), geohome); + } + + // below are approximate versions and would produce errors of more than 10m for points farther than 1km + // for more accurate versions, please use the version in EarthUtils::nedToGeodetic + static Vector3r GeodeticToNedFast(const GeoPoint &geo, const GeoPoint &home) { + double d_lat = geo.latitude - home.latitude; + double d_lon = geo.longitude - home.longitude; + real_T d_alt = static_cast(home.altitude - geo.altitude); + real_T x = static_cast(Utils::degreesToRadians(d_lat) * EARTH_RADIUS); + real_T y = static_cast(Utils::degreesToRadians(d_lon) * EARTH_RADIUS * + cos(Utils::degreesToRadians(geo.latitude))); + return Vector3r(x, y, d_alt); + } + static GeoPoint nedToGeodeticFast(const Vector3r &local, const GeoPoint &home) { + GeoPoint r; + double d_lat = local.x() / EARTH_RADIUS; + double d_lon = local.y() / (EARTH_RADIUS * cos(Utils::degreesToRadians(home.latitude))); + r.latitude = home.latitude + Utils::radiansToDegrees(d_lat); + r.longitude = home.longitude + Utils::radiansToDegrees(d_lon); + r.altitude = home.altitude - local.z(); + + return r; + } + + public: // consts + // ref: https://www.ngdc.noaa.gov/geomag/GeomagneticPoles.shtml + static constexpr double MagPoleLat = Utils::degreesToRadians(80.31f); + static constexpr double MagPoleLon = Utils::degreesToRadians(-72.62f); + static constexpr double MeanMagField = + 3.12E-5; // Tesla, https://en.wikipedia.org/wiki/Dipole_model_of_the_Earth's_magnetic_field + static constexpr float SeaLevelPressure = 101325.0f; // Pascal + static constexpr float SeaLevelAirDensity = 1.225f; // kg/m^3 + static constexpr float Gravity = 9.80665f; // m/s^2 + static constexpr float Radius = EARTH_RADIUS; // m + static constexpr float SpeedOfLight = 299792458.0f; // m + static constexpr float Obliquity = Utils::degreesToRadians(23.4397f); + static constexpr double Perihelion = Utils::degreesToRadians(102.9372); // perihelion of the Earth + static constexpr double DistanceFromSun = 149597870700.0; // meters + + private: + /* magnetic field */ + static float get_mag_lookup_table_val(int lat_index, int lon_index) { + return static_cast(DECLINATION_TABLE[lat_index][lon_index]); + } +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/FirstOrderFilter.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/FirstOrderFilter.hpp new file mode 100644 index 00000000..db52de20 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/FirstOrderFilter.hpp @@ -0,0 +1,75 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_FirstOrderFilter_hpp +#define autonomylib_common_FirstOrderFilter_hpp + +#include "Common.hpp" +#include "UpdatableObject.hpp" +#include + +namespace nervosys { +namespace autonomylib { + +template class FirstOrderFilter : public UpdatableObject { + /* +This class can be used to apply a first order filter on a signal. +It allows different acceleration and deceleration time constants. + +Short review of discrete time implementation of first order system: +Laplace: +X(s)/U(s) = 1/(tau*s + 1) +continuous time system: +dx(t) = (-1/tau)*x(t) + (1/tau)*u(t) +discretized system (ZoH): +x(k+1) = exp(samplingTime*(-1/tau))*x(k) + (1 - exp(samplingTime*(-1/tau))) * u(k) +*/ + public: + FirstOrderFilter() { + // allow default constructor with later call for initialize + } + FirstOrderFilter(float timeConstant, T initial_input, T initial_output) { + initialize(timeConstant, initial_input, initial_output); + } + void initialize(float timeConstant, T initial_input, T initial_output) { + timeConstant_ = timeConstant; + initial_input_ = initial_input; + initial_output_ = initial_output; + } + + //*** Start: UpdatableState implementation ***// + virtual void resetImplementation() override { + last_time_ = clock()->nowNanos(); + input_ = initial_input_; + output_ = initial_output_; + } + + virtual void update() override { + UpdatableObject::update(); + + TTimeDelta dt = clock()->updateSince(last_time_); + + // lower the weight for previous value if its been long time + // TODO: minimize use of exp + double alpha = exp(-dt / timeConstant_); + // x(k+1) = Ad*x(k) + Bd*u(k) + output_ = static_cast(output_ * alpha + input_ * (1 - alpha)); + } + //*** End: UpdatableState implementation ***// + + void setInput(T input) { input_ = input; } + T getInput() const { return input_; } + + T getOutput() const { return output_; } + + private: + float timeConstant_; + T output_, input_; + T initial_output_, initial_input_; + TTimePoint last_time_; +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/FrequencyLimiter.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/FrequencyLimiter.hpp new file mode 100644 index 00000000..a91f98f9 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/FrequencyLimiter.hpp @@ -0,0 +1,106 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_FrequencyLimiter_hpp +#define autonomylib_common_FrequencyLimiter_hpp + +#include "UpdatableObject.hpp" +#include "common/Common.hpp" + +namespace nervosys { +namespace autonomylib { + +class FrequencyLimiter : public UpdatableObject { + public: + FrequencyLimiter(real_T frequency = Utils::max(), real_T startup_delay = 0) { + initialize(frequency, startup_delay); + } + + void initialize(real_T frequency = Utils::max(), real_T startup_delay = 0) { + frequency_ = frequency; + startup_delay_ = startup_delay; + } + + //*** Start: UpdatableState implementation ***// + virtual void resetImplementation() override { + last_time_ = clock()->nowNanos(); + first_time_ = last_time_; + + if (Utils::isApproximatelyZero(frequency_)) + interval_size_sec_ = 1E10; // some high number + else + interval_size_sec_ = 1.0f / frequency_; + + elapsed_total_sec_ = 0; + elapsed_interval_sec_ = 0; + last_elapsed_interval_sec_ = 0; + update_count_ = 0; + interval_complete_ = false; + startup_complete_ = false; + } + + virtual void failResetUpdateOrdering(std::string err) override { + unused(err); + // Do nothing. + // Disable checks for reset/update sequence because + // this object may get created but not used. + } + + virtual void update() override { + UpdatableObject::update(); + + elapsed_total_sec_ = clock()->elapsedSince(first_time_); + elapsed_interval_sec_ = clock()->elapsedSince(last_time_); + ++update_count_; + + // if startup_delay_ > 0 then we consider startup_delay_ as the first interval + // that needs to be complete + if (!startup_complete_) { + if (Utils::isDefinitelyGreaterThan(startup_delay_, 0.0f)) { + // see if we have spent startup_delay_ time yet + interval_complete_ = elapsed_interval_sec_ >= startup_delay_; + } else // no special startup delay is needed + startup_complete_ = true; + } + + // if startup is complete, we will do regular intervals from now one + if (startup_complete_) + interval_complete_ = elapsed_interval_sec_ >= interval_size_sec_; + + // when any interval is done, reset the state and repeat + if (interval_complete_) { + last_elapsed_interval_sec_ = elapsed_interval_sec_; + last_time_ = clock()->nowNanos(); + elapsed_interval_sec_ = 0; + startup_complete_ = true; + } + } + //*** End: UpdatableState implementation ***// + + TTimeDelta getElapsedTotalSec() const { return elapsed_total_sec_; } + + TTimeDelta getElapsedIntervalSec() const { return elapsed_interval_sec_; } + + TTimeDelta getLastElapsedIntervalSec() const { return last_elapsed_interval_sec_; } + + bool isWaitComplete() const { return interval_complete_; } + + bool isStartupComplete() const { return startup_complete_; } + + uint getUpdateCount() const { return update_count_; } + + private: + real_T interval_size_sec_; + TTimeDelta elapsed_total_sec_; + TTimeDelta elapsed_interval_sec_; + TTimeDelta last_elapsed_interval_sec_; + uint update_count_; + real_T frequency_; + real_T startup_delay_; + bool interval_complete_; + bool startup_complete_; + TTimePoint last_time_, first_time_; +}; +} // namespace autonomylib +} // namespace nervosys +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/GaussianMarkov.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/GaussianMarkov.hpp new file mode 100644 index 00000000..beb0cea9 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/GaussianMarkov.hpp @@ -0,0 +1,70 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_GaussianMarkov_hpp +#define autonomylib_common_GaussianMarkov_hpp + +#include "UpdatableObject.hpp" +#include "common/Common.hpp" +#include "common/utils/RandomGenerator.hpp" +#include + +namespace nervosys { +namespace autonomylib { + +class GaussianMarkov : public UpdatableObject { + public: + GaussianMarkov() {} + GaussianMarkov(real_T tau, real_T sigma, real_T initial_output = 0) { // in seconds + initialize(tau, sigma, initial_output); + } + void initialize(real_T tau, real_T sigma, real_T initial_output = 0) { // in seconds + tau_ = tau; + sigma_ = sigma; + rand_ = RandomGeneratorGausianR(0.0f, 1.0f); + + if (std::isnan(initial_output)) { + initial_output_ = getNextRandom() * sigma_; + } else { + initial_output_ = initial_output; + } + } + + //*** Start: UpdatableState implementation ***// + virtual void resetImplementation() override { + last_time_ = clock()->nowNanos(); + output_ = initial_output_; + rand_.reset(); + } + + virtual void update() override { + /* + Ref: + A Comparison between Different Error Modeling of MEMS Applied to GPS/INS Integrated Systems + Quinchia, sec 3.2, https://www.ncbi.nlm.nih.gov/pmc/articles/PMC3812568/ + + A Study of the Effects of Stochastic Inertial Sensor Errors in Dead-Reckoning Navigation + John H Wall, 2007, eq 2.5, pg 13, http://etd.auburn.edu/handle/10415/945 + */ + + UpdatableObject::update(); + TTimeDelta dt = clock()->updateSince(last_time_); + double alpha = exp(-dt / tau_); + output_ = static_cast(alpha * output_ + (1 - alpha) * getNextRandom() * sigma_); + } + //*** End: UpdatableState implementation ***// + + real_T getNextRandom() { return rand_.next(); } + real_T getOutput() const { return output_; } + + private: + RandomGeneratorGausianR rand_; + real_T tau_, sigma_; + real_T output_, initial_output_; + TTimePoint last_time_; +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/GeodeticConverter.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/GeodeticConverter.hpp new file mode 100644 index 00000000..d694c70b --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/GeodeticConverter.hpp @@ -0,0 +1,210 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_GeodeticConverter_hpp +#define autonomylib_common_GeodeticConverter_hpp + +#include "VectorMath.hpp" +#include + +namespace nervosys { +namespace autonomylib { + +class GeodeticConverter { + public: + GeodeticConverter(double home_latitude = 0, double home_longitude = 0, float home_altitude = 0) { + setHome(home_latitude, home_longitude, home_altitude); + } + + GeodeticConverter(const GeoPoint &home_geopoint) { setHome(home_geopoint); } + + void setHome(double home_latitude, double home_longitude, float home_altitude) { + home_latitude_ = home_latitude; + home_longitude_ = home_longitude; + home_altitude_ = home_altitude; + + // Save NED origin + home_latitude_rad_ = deg2Rad(home_latitude); + home_longitude_rad_ = deg2Rad(home_longitude); + + // Compute ECEF of NED origin + geodetic2Ecef(home_latitude_, home_longitude_, home_altitude_, &home_ecef_x_, &home_ecef_y_, &home_ecef_z_); + + // Compute ECEF to NED and NED to ECEF matrices + ecef_to_ned_matrix_ = nRe(home_latitude_rad_, home_longitude_rad_); + ned_to_ecef_matrix_ = ecef_to_ned_matrix_.inverse(); + } + + void setHome(const GeoPoint &home_geopoint) { + setHome(home_geopoint.latitude, home_geopoint.longitude, home_geopoint.altitude); + } + + void getHome(double *latitude, double *longitude, float *altitude) { + *latitude = home_latitude_; + *longitude = home_longitude_; + *altitude = home_altitude_; + } + + void geodetic2Ecef(const double latitude, const double longitude, const double altitude, double *x, double *y, + double *z) { + // Convert geodetic coordinates to ECEF. + // http://code.google.com/p/pysatel/source/browse/trunk/coord.py?r=22 + double lat_rad = deg2Rad(latitude); + double lon_rad = deg2Rad(longitude); + double xi = sqrt(1 - kFirstEccentricitySquared * sin(lat_rad) * sin(lat_rad)); + *x = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * cos(lon_rad); + *y = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * sin(lon_rad); + *z = (kSemimajorAxis / xi * (1 - kFirstEccentricitySquared) + altitude) * sin(lat_rad); + } + + void ecef2Geodetic(const double x, const double y, const double z, double *latitude, double *longitude, + float *altitude) { + // Convert ECEF coordinates to geodetic coordinates. + // J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates + // to geodetic coordinates," IEEE Transactions on Aerospace and + // Electronic Systems, vol. 30, pp. 957-961, 1994. + + double r = sqrt(x * x + y * y); + double Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis; + double F = 54 * kSemiminorAxis * kSemiminorAxis * z * z; + double G = r * r + (1 - kFirstEccentricitySquared) * z * z - kFirstEccentricitySquared * Esq; + double C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3); + double S = cbrt(1 + C + sqrt(C * C + 2 * C)); + double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G); + double Q = sqrt(1 + 2 * kFirstEccentricitySquared * kFirstEccentricitySquared * P); + double r_0 = -(P * kFirstEccentricitySquared * r) / (1 + Q) + + sqrt(0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q) - + P * (1 - kFirstEccentricitySquared) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r); + double U = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + z * z); + double V = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * z * z); + double Z_0 = kSemiminorAxis * kSemiminorAxis * z / (kSemimajorAxis * V); + *altitude = static_cast(U * (1 - kSemiminorAxis * kSemiminorAxis / (kSemimajorAxis * V))); + *latitude = rad2Deg(atan((z + kSecondEccentricitySquared * Z_0) / r)); + *longitude = rad2Deg(atan2(y, x)); + } + + void ecef2Ned(const double x, const double y, const double z, double *north, double *east, double *down) { + // Converts ECEF coordinate position into local-tangent-plane NED. + // Coordinates relative to given ECEF coordinate frame. + + Vector3d vect, ret; + vect(0) = x - home_ecef_x_; + vect(1) = y - home_ecef_y_; + vect(2) = z - home_ecef_z_; + ret = ecef_to_ned_matrix_ * vect; + *north = ret(0); + *east = ret(1); + *down = -ret(2); + } + + void ned2Ecef(const double north, const double east, const float down, double *x, double *y, double *z) { + // NED (north/east/down) to ECEF coordinates + Vector3d ned, ret; + ned(0) = north; + ned(1) = east; + ned(2) = -down; + ret = ned_to_ecef_matrix_ * ned; + *x = ret(0) + home_ecef_x_; + *y = ret(1) + home_ecef_y_; + *z = ret(2) + home_ecef_z_; + } + + void geodetic2Ned(const double latitude, const double longitude, const float altitude, double *north, double *east, + double *down) { + // Geodetic position to local NED frame + double x, y, z; + geodetic2Ecef(latitude, longitude, altitude, &x, &y, &z); + ecef2Ned(x, y, z, north, east, down); + } + + void ned2Geodetic(const double north, const double east, const float down, double *latitude, double *longitude, + float *altitude) { + // Local NED position to geodetic coordinates + double x, y, z; + ned2Ecef(north, east, down, &x, &y, &z); + ecef2Geodetic(x, y, z, latitude, longitude, altitude); + } + + void ned2Geodetic(const Vector3r &ned_pos, GeoPoint &geopoint) { + ned2Geodetic(ned_pos[0], ned_pos[1], ned_pos[2], &geopoint.latitude, &geopoint.longitude, &geopoint.altitude); + } + + void geodetic2Enu(const double latitude, const double longitude, const double altitude, double *east, double *north, + double *up) { + // Geodetic position to local ENU frame + double x, y, z; + geodetic2Ecef(latitude, longitude, altitude, &x, &y, &z); + + double aux_north, aux_east, aux_down; + ecef2Ned(x, y, z, &aux_north, &aux_east, &aux_down); + + *east = aux_east; + *north = aux_north; + *up = -aux_down; + } + + void enu2Geodetic(const double east, const double north, const float up, double *latitude, double *longitude, + float *altitude) { + // Local ENU position to geodetic coordinates + + const double aux_north = north; + const double aux_east = east; + const float aux_down = -up; + double x, y, z; + ned2Ecef(aux_north, aux_east, aux_down, &x, &y, &z); + ecef2Geodetic(x, y, z, latitude, longitude, altitude); + } + + private: + typedef nervosys::autonomylib::VectorMathf VectorMath; + typedef VectorMath::Vector3d Vector3d; + typedef VectorMath::Matrix3x3d Matrix3x3d; + + // Geodetic system parameters + static constexpr double kSemimajorAxis = 6378137; + static constexpr double kSemiminorAxis = 6356752.3142; + static constexpr double kFirstEccentricitySquared = 6.69437999014 * 0.001; + static constexpr double kSecondEccentricitySquared = 6.73949674228 * 0.001; + static constexpr double kFlattening = 1 / 298.257223563; + + inline Matrix3x3d nRe(const double lat_radians, const double lon_radians) { + const double sLat = sin(lat_radians); + const double sLon = sin(lon_radians); + const double cLat = cos(lat_radians); + const double cLon = cos(lon_radians); + + Matrix3x3d ret; + ret(0, 0) = -sLat * cLon; + ret(0, 1) = -sLat * sLon; + ret(0, 2) = cLat; + ret(1, 0) = -sLon; + ret(1, 1) = cLon; + ret(1, 2) = 0.0; + ret(2, 0) = cLat * cLon; + ret(2, 1) = cLat * sLon; + ret(2, 2) = sLat; + + return ret; + } + + inline double rad2Deg(const double radians) { return (radians / M_PI) * 180.0; } + + inline double deg2Rad(const double degrees) { return (degrees / 180.0) * M_PI; } + + double home_latitude_rad_, home_latitude_; + double home_longitude_rad_, home_longitude_; + float home_altitude_; + + double home_ecef_x_; + double home_ecef_y_; + double home_ecef_z_; + + Matrix3x3d ecef_to_ned_matrix_; + Matrix3x3d ned_to_ecef_matrix_; + +}; // class GeodeticConverter + +} // namespace autonomylib +} // namespace nervosys + +#endif // GEODETIC_CONVERTER_H_ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/ImageCaptureBase.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/ImageCaptureBase.hpp new file mode 100644 index 00000000..23b27daf --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/ImageCaptureBase.hpp @@ -0,0 +1,67 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_ImageCaptureBase_hpp +#define autonomylib_common_ImageCaptureBase_hpp + +#include "common/Common.hpp" +#include "common/utils/EnumFlags.hpp" + +namespace nervosys { +namespace autonomylib { + +// This is an abstraction for cameras associated with a vehicle. Each camera has a unique id. +class ImageCaptureBase { + public: // types + enum class ImageType : int { // this indexes to array, -1 is special to indicate main camera + Scene = 0, + DepthPlanar, + DepthPerspective, + DepthVis, + DisparityNormalized, + Segmentation, + SurfaceNormals, + Infrared, + OpticalFlow, + OpticalFlowVis, + Count // must be last + }; + + struct ImageRequest { + std::string camera_name; + ImageCaptureBase::ImageType image_type = ImageCaptureBase::ImageType::Scene; + bool pixels_as_float = false; + bool compress = true; + + ImageRequest() {} + + ImageRequest(const std::string &camera_name_val, ImageCaptureBase::ImageType image_type_val, + bool pixels_as_float_val = false, bool compress_val = true) + : camera_name(camera_name_val), image_type(image_type_val), pixels_as_float(pixels_as_float_val), + compress(compress_val) {} + }; + + struct ImageResponse { + vector image_data_uint8; + vector image_data_float; + + std::string camera_name; + Vector3r camera_position = Vector3r::Zero(); + Quaternionr camera_orientation = Quaternionr::Identity(); + TTimePoint time_stamp = 0; + std::string message; + bool pixels_as_float = false; + bool compress = true; + int width = 0, height = 0; + ImageType image_type; + }; + + public: // methods + virtual void getImages(const std::vector &requests, std::vector &responses) const = 0; + virtual ~ImageCaptureBase() = default; +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/LogFileWriter.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/LogFileWriter.hpp new file mode 100644 index 00000000..e03e3699 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/LogFileWriter.hpp @@ -0,0 +1,63 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_LogFileWriter_hpp +#define autonomylib_common_LogFileWriter_hpp + +#include "common/Common.hpp" +#include +#include + +namespace nervosys { +namespace autonomylib { + +class LogFileWriter { + public: + LogFileWriter() {} + LogFileWriter(const string &file_name, bool enabled = true) { open(file_name, enabled); } + ~LogFileWriter() { close(); } + void open(const string &file_name, bool enabled = true) { + close(); + file_name_ = file_name; + enabled_ = enabled; + + if (enabled_) + log_file_ = std::ofstream(file_name); + } + void close() { + if (log_file_.is_open()) + log_file_.close(); + } + + template void write(const T &val) { + if (enabled_) + log_file_ << val << "\t"; + } + void write(const Vector3r &vec) { + if (enabled_) + log_file_ << vec.x() << "\t" << vec.y() << "\t" << vec.z() << "\t"; + } + void write(const Quaternionr &q) { + if (enabled_) { + real_T p, r, y; + VectorMath::toEulerianAngle(q, p, r, y); + log_file_ << Utils::radiansToDegrees(r) << "\t" << Utils::radiansToDegrees(p) << "\t" + << Utils::radiansToDegrees(y) << "\t"; + } + } + void endl() { + if (enabled_) { + log_file_ << std::endl; + } + } + + private: + std::ofstream log_file_; + string file_name_; + bool enabled_ = false; +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/PidController.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/PidController.hpp new file mode 100644 index 00000000..508677fe --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/PidController.hpp @@ -0,0 +1,80 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_PidController_hpp +#define autonomylib_common_PidController_hpp + +#include + +namespace nervosys { +namespace autonomylib { + +// This class implements a pid controller. +// First call setPoint to set the desired point and the PID control variables +// then call control(x) with new observed values and it will return the updated +// control output needed to achieve the setPoint goal. Integration is done using +// dt measured using system clock. +class PidController { + private: + float set_point_; + float kProportional_; + float kIntegral_; + float kDerivative_; + float previous_error_; + bool previous_set; + float sum_; + std::chrono::time_point prev_time_; + + public: + // set desired point. + void setPoint(float target, float kProportional, float kIntegral, float kDerivative) { + set_point_ = target; + kProportional_ = kProportional; + kIntegral_ = kIntegral; + kDerivative_ = kDerivative; + prev_time_ = std::chrono::system_clock::now(); + previous_error_ = 0; + sum_ = 0; + previous_set = false; + } + + float control(float processVariable) { + auto t = std::chrono::system_clock::now(); + auto diff = std::chrono::system_clock::now() - prev_time_; + + float error = set_point_ - processVariable; + if (!previous_set) { + previous_set = true; + previous_error_ = error; + return 0; + } + + float dt = static_cast(std::chrono::duration_cast(diff).count()) * 0.000001f; + dt = fmax(dt, 0.01f); + float proportionalGain = 0; + float derivativeGain = 0; + float integralGain = 0; + + if (kProportional_ != 0) { + proportionalGain = error * kProportional_; + } + if (kDerivative_ != 0) { + float derivative = (error - previous_error_) / dt; + derivativeGain = derivative * kDerivative_; + } + if (kIntegral_ != 0) { + sum_ += error * dt; + integralGain = sum_ * kIntegral_; + } + + previous_error_ = error; + prev_time_ = t; + + return proportionalGain + derivativeGain + integralGain; + } +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/ScalableClock.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/ScalableClock.hpp new file mode 100644 index 00000000..a27ccc67 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/ScalableClock.hpp @@ -0,0 +1,73 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_ScalableClock_hpp +#define autonomylib_common_ScalableClock_hpp + +#include "ClockBase.hpp" +#include "Common.hpp" + +namespace nervosys { +namespace autonomylib { + +// ScalableClock is a clock that can be scaled (i.e. slowed down or speeded up) +class ScalableClock : public ClockBase { + public: + // scale > 1 slows down the clock, < 1 speeds up the clock + ScalableClock(double scale = 1, TTimeDelta latency = 0) : scale_(scale), latency_(latency) { + offset_ = latency * (scale_ - 1); + start_ = nowNanos(); + + unused(latency_); + } + + virtual ~ScalableClock() {} + + virtual TTimePoint nowNanos() const override { + if (offset_ == 0 && scale_ == 1) // optimized normal route + return Utils::getTimeSinceEpochNanos(); + else { + /* + Apply scaling and latency. + + Time point is nanoseconds from some reference r. If latency = 0 then r = 0 . + scaled time point is then given by (r + ((now - r) / scale)). + This becomes (r*(s-1) + now)/scale or (offset + now / scale). + */ + return static_cast((Utils::getTimeSinceEpochNanos() + offset_) / scale_); + } + } + + virtual TTimePoint getStart() const override { return start_; } + + virtual void sleep_for(TTimeDelta dt) override { + // for intervals > 2ms just sleep otherwise do spilling otherwise delay won't be accurate + if (dt > 2.0 / 1000) { + TTimeDelta dt_scaled = fromWallDelta(dt); + std::this_thread::sleep_for(std::chrono::duration(dt_scaled)); + } else + ClockBase::sleep_for(dt); + } + + protected: + // converts time interval for wall clock to current clock + // For example, if implementation is scaled clock simulating 5X spped then below + // will retun dt*5. This functions are required to translate time to operating system + // which only has concept of wall clock. For example, to make thread sleep for specific duration. + + // wall clock to sim clock + virtual TTimeDelta fromWallDelta(TTimeDelta dt) const { return dt * scale_; } + // sim clock to wall clock + virtual TTimeDelta toWallDelta(TTimeDelta dt) const { return dt / scale_; } + + private: + double scale_; + TTimeDelta latency_; + double offset_; + TTimePoint start_; +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/Settings.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/Settings.hpp new file mode 100644 index 00000000..7594d4cd --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/Settings.hpp @@ -0,0 +1,215 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_Settings_hpp +#define autonomylib_common_Settings_hpp + +#include +#include + +#include "common/utils/Utils.hpp" + +STRICT_MODE_OFF +// this json library is not strict clean +// TODO: HACK!! below are added temporariliy because something is defining min, max macros +// #undef max +#undef min +#include "common/utils/json.hpp" +STRICT_MODE_ON + +#include "common/utils/FileSystem.hpp" + +namespace nervosys { +namespace autonomylib { + +class Settings { + private: + std::string full_filepath_; + nlohmann::json doc_; + bool load_success_ = false; + + private: + static std::mutex &getFileAccessMutex() { + static std::mutex file_access; + return file_access; + } + + public: + static Settings &singleton() { + static Settings instance; + return instance; + } + + std::string getFullFilePath() { return full_filepath_; } + + static std::string getUserDirectoryFullPath(std::string fileName) { + std::string path = common_utils::FileSystem::getAppDataFolder(); + return common_utils::FileSystem::combine(path, fileName); + } + + static std::string getExecutableFullPath(std::string fileName) { + std::string path = common_utils::FileSystem::getExecutableFolder(); + return common_utils::FileSystem::combine(path, fileName); + } + + static Settings &loadJSonString(const std::string &json_str) { + singleton().full_filepath_ = ""; + singleton().load_success_ = false; + + if (json_str.length() > 0) { + std::stringstream ss; + ss << json_str; + ss >> singleton().doc_; + singleton().load_success_ = true; + } + + return singleton(); + } + + std::string saveJSonString() { + std::lock_guard guard(getFileAccessMutex()); + std::stringstream ss; + ss << std::setw(2) << singleton().doc_ << std::endl; + + return ss.str(); + } + + static Settings &loadJSonFile(std::string full_filepath) { + std::lock_guard guard(getFileAccessMutex()); + singleton().full_filepath_ = full_filepath; + + singleton().load_success_ = false; + + std::ifstream s; + common_utils::FileSystem::openTextFile(full_filepath, s); + if (!s.fail()) { + s >> singleton().doc_; + singleton().load_success_ = true; + } + + return singleton(); + } + + bool isLoadSuccess() { return load_success_; } + + bool hasFileName() { return !getFullFilePath().empty(); } + + void saveJSonFile(std::string full_filepath) { + std::lock_guard guard(getFileAccessMutex()); + singleton().full_filepath_ = full_filepath; + std::ofstream s; + common_utils::FileSystem::createTextFile(full_filepath, s); + s << std::setw(2) << doc_ << std::endl; + } + + bool getChild(const std::string &name, Settings &child) const { + if (doc_.count(name) == 1 && (doc_[name].type() == nlohmann::detail::value_t::object || + doc_[name].type() == nlohmann::detail::value_t::array)) { + child.doc_ = doc_[name].get(); + return true; + } + return false; + } + + size_t size() const { return doc_.size(); } + + template void getChildNames(Container &c) const { + for (auto it = doc_.begin(); it != doc_.end(); ++it) { + c.push_back(it.key()); + } + } + + bool getChild(size_t index, Settings &child) const { + if (doc_.size() > index && (doc_[index].type() == nlohmann::detail::value_t::object || + doc_[index].type() == nlohmann::detail::value_t::array)) { + + child.doc_ = doc_[index].get(); + return true; + } + return false; + } + + std::string getString(const std::string &name, std::string defaultValue) const { + if (doc_.count(name) == 1) { + return doc_[name].get(); + } else { + return defaultValue; + } + } + + double getDouble(const std::string &name, double defaultValue) const { + if (doc_.count(name) == 1) { + return doc_[name].get(); + } else { + return defaultValue; + } + } + + float getFloat(const std::string &name, float defaultValue) const { + if (doc_.count(name) == 1) { + return doc_[name].get(); + } else { + return defaultValue; + } + } + + bool getBool(const std::string &name, bool defaultValue) const { + if (doc_.count(name) == 1) { + return doc_[name].get(); + } else { + return defaultValue; + } + } + + bool hasKey(const std::string &key) const { return doc_.find(key) != doc_.end(); } + + int getInt(const std::string &name, int defaultValue) const { + if (doc_.count(name) == 1) { + return doc_[name].get(); + } else { + return defaultValue; + } + } + + bool setString(const std::string &name, std::string value) { + if (doc_.count(name) != 1 || doc_[name].type() != nlohmann::detail::value_t::string || doc_[name] != value) { + doc_[name] = value; + return true; + } + return false; + } + + bool setDouble(const std::string &name, double value) { + if (doc_.count(name) != 1 || doc_[name].type() != nlohmann::detail::value_t::number_float || + static_cast(doc_[name]) != value) { + doc_[name] = value; + return true; + } + return false; + } + + bool setBool(const std::string &name, bool value) { + if (doc_.count(name) != 1 || doc_[name].type() != nlohmann::detail::value_t::boolean || + static_cast(doc_[name]) != value) { + doc_[name] = value; + return true; + } + return false; + } + + bool setInt(const std::string &name, int value) { + if (doc_.count(name) != 1 || doc_[name].type() != nlohmann::detail::value_t::number_integer || + static_cast(doc_[name]) != value) { + doc_[name] = value; + return true; + } + return false; + } + + void setChild(const std::string &name, Settings &value) { doc_[name] = value.doc_; } +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif \ No newline at end of file diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/StateReporter.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/StateReporter.hpp new file mode 100644 index 00000000..43f540d0 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/StateReporter.hpp @@ -0,0 +1,107 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_StateReporter_hpp +#define autonomylib_common_StateReporter_hpp + +#include "common/Common.hpp" +#include +#include +#include + +namespace nervosys { +namespace autonomylib { + +/* +This class is simple key-value reporting provider. It can't inherit from +UpdatableObject or we will have circular dependency. This class essentially +just provides formatted write APIs over string buffer. The UpdatableObject +version is provided by StateReporterWrapper. We expect everyone to use +StateReporterWrapper instead of StateReporter directly. +*/ +class StateReporter { + public: + StateReporter(int float_precision = 3, bool is_scientific_notation = false) { + initialize(float_precision, is_scientific_notation); + } + void initialize(int float_precision = 3, bool is_scientific_notation = false) { + float_precision_ = float_precision; + is_scientific_notation_ = is_scientific_notation; + + if (float_precision_ >= 0) { + ss_ << std::setprecision(float_precision_); + if (is_scientific_notation_) + ss_ << std::scientific; + else + ss_ << std::fixed; + } + } + + void clear() { + ss_.str(std::string()); + ss_.clear(); + } + + string getOutput() const { return ss_.str(); } + + // write APIs - heading + // TODO: need better line end handling + void startHeading(string heading, uint heading_size, uint columns = 20) { + unused(heading_size); + unused(columns); + ss_ << "\n"; + ss_ << heading; + } + void endHeading(bool end_line, uint heading_size, uint columns = 20) { + if (end_line) + ss_ << "\n"; + for (int lines = heading_size; lines > 0; --lines) + ss_ << std::string(columns, '_') << "\n"; + } + void writeHeading(string heading, uint heading_size = 0, uint columns = 20) { + startHeading(heading, heading_size); + endHeading(true, heading_size, columns); + } + + // write APIs - specialized objects + void writeValue(string name, const Vector3r &vector) { + ss_ << name << ": " + << "(" << vector.norm() << ") - " + << "[" << vector.x() << ", " << vector.y() << ", " << vector.z() << "]\n"; + } + void writeValue(string name, const Quaternionr &quat) { + real_T pitch, roll, yaw; + VectorMath::toEulerianAngle(quat, pitch, roll, yaw); + + ss_ << name << ":\n" + << " euler: (" << roll << ", " << pitch << ", " << yaw << ")\n" + << " quat: [" << quat.w() << ", " << quat.x() << ", " << quat.y() << ", " << quat.z() << "]\n"; + } + + // write APIs - generic values + template void writeValue(string name, const T &r) { + writeNameOnly(name); + writeValueOnly(r, true); + } + void writeNameOnly(string name) { ss_ << name << ": "; } + template void writeValueOnly(const T &r, bool end_line_or_tab = false) { + ss_ << r; + + if (end_line_or_tab) + ss_ << "\n"; + else + ss_ << "\t"; + } + void endl() { ss_ << std::endl; } + + private: + std::stringstream ss_; + + int float_precision_ = 2; + bool is_scientific_notation_ = false; +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/StateReporterWrapper.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/StateReporterWrapper.hpp new file mode 100644 index 00000000..e97d46ec --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/StateReporterWrapper.hpp @@ -0,0 +1,110 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_StateReporterWrapper_hpp +#define autonomylib_common_StateReporterWrapper_hpp + +#include "StateReporter.hpp" +#include "UpdatableObject.hpp" +#include "common/Common.hpp" +#include "common/FrequencyLimiter.hpp" +#include "common/utils/OnlineStats.hpp" +#include +#include +#include + +namespace nervosys { +namespace autonomylib { + +class StateReporterWrapper : public UpdatableObject { + public: + static constexpr real_T DefaultReportFreq = 3.0f; + + StateReporterWrapper(bool enabled = false, int float_precision = 3, bool is_scientific_notation = false) { + initialize(enabled, float_precision, is_scientific_notation); + } + void initialize(bool enabled = false, int float_precision = 3, bool is_scientific_notation = false) { + enabled_ = enabled; + report_.initialize(float_precision, is_scientific_notation); + report_freq_.initialize(DefaultReportFreq); + } + + void clearReport() { + report_.clear(); + is_wait_complete = false; + } + + //*** Start: UpdatableState implementation ***// + virtual void resetImplementation() override { + last_time_ = clock()->nowNanos(); + clearReport(); + dt_stats_.clear(); + report_freq_.reset(); + } + + virtual void failResetUpdateOrdering(std::string err) override { + // Do nothing. + // Disable checks for reset/update sequence because + // this object may get created but not used. + } + + virtual void update() override { + UpdatableObject::update(); + + TTimeDelta dt = clock()->updateSince(last_time_); + + if (enabled_) { + dt_stats_.insert(dt); + report_freq_.update(); + is_wait_complete = is_wait_complete || report_freq_.isWaitComplete(); + } + } + virtual void reportState(StateReporter &reporter) override { + // TODO: perhaps we should be using supplied reporter? + unused(reporter); + + // write dt stats + report_.writeNameOnly("dt"); + report_.writeValueOnly(dt_stats_.mean()); + report_.writeValueOnly(dt_stats_.variance()); + report_.writeValueOnly(dt_stats_.size(), true); + } + //*** End: UpdatableState implementation ***// + + bool canReport() { return enabled_ && is_wait_complete; } + + StateReporter *getReporter() { return &report_; } + + string getOutput() { return report_.getOutput(); } + + void setReportFreq(real_T freq) { report_freq_.initialize(freq); } + + void setEnable(bool enable) { + if (enable == enabled_) + return; + + enabled_ = enable; + if (enable) + report_freq_.initialize(DefaultReportFreq); + else + report_freq_.initialize(0); + } + bool getEnable() { return enabled_; } + + private: + typedef common_utils::OnlineStats OnlineStats; + + StateReporter report_; + + OnlineStats dt_stats_; + + FrequencyLimiter report_freq_; + bool enabled_; + bool is_wait_complete = false; + TTimePoint last_time_; +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/SteppableClock.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/SteppableClock.hpp new file mode 100644 index 00000000..33d9c3f0 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/SteppableClock.hpp @@ -0,0 +1,54 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_SteppableClock_hpp +#define autonomylib_common_SteppableClock_hpp + +#include "ClockBase.hpp" +#include "Common.hpp" +#include + +namespace nervosys { +namespace autonomylib { + +class SteppableClock : public ClockBase { + public: + static constexpr real_T DefaultStepSize = 20E-3f; + + // Debug clock allows to advance the clock manually in arbitrary way + // TTimePoint is nano seconds passed since some fixed point in time + // step is how much we would advance the clock by default when calling step() + // by default we advance by 20ms + SteppableClock(TTimeDelta step = DefaultStepSize, TTimePoint start = 0) : current_(start), step_(step) { + start_ = current_ = start ? start : Utils::getTimeSinceEpochNanos(); + } + + virtual ~SteppableClock() {} + + virtual TTimePoint stepBy(TTimeDelta amount) override { + current_ = addTo(current_, amount); + return current_; + } + + virtual TTimePoint step() override { + ClockBase::step(); + + current_ = addTo(current_, step_); + return current_; + } + + TTimeDelta getStepSize() const { return step_; } + virtual TTimePoint nowNanos() const override { return current_; } + virtual TTimePoint getStart() const override { return start_; } + + private: + std::atomic current_; + std::atomic start_; + + TTimeDelta step_; +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/UpdatableContainer.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/UpdatableContainer.hpp new file mode 100644 index 00000000..8efe4fdd --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/UpdatableContainer.hpp @@ -0,0 +1,71 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_UpdatableContainer_hpp +#define autonomylib_common_UpdatableContainer_hpp + +#include "UpdatableObject.hpp" +#include "common/Common.hpp" + +namespace nervosys { +namespace autonomylib { + +template class UpdatableContainer : public UpdatableObject { + public: // limited container interface + typedef vector MembersContainer; + typedef typename MembersContainer::iterator iterator; + typedef typename MembersContainer::const_iterator const_iterator; + typedef typename MembersContainer::value_type value_type; + iterator begin() { return members_.begin(); } + iterator end() { return members_.end(); } + const_iterator begin() const { return members_.begin(); } + const_iterator end() const { return members_.end(); } + uint size() const { return static_cast(members_.size()); } + const TUpdatableObjectPtr &at(uint index) const { members_.at(index); } + TUpdatableObjectPtr &at(uint index) { return members_.at(index); } + // allow to override membership modifications + virtual void clear() { + for (auto m : members_) { + m->setParent(nullptr); + } + members_.clear(); + } + virtual void insert(TUpdatableObjectPtr member) { + member->setParent(this); + members_.push_back(member); + } + virtual void erase_remove(TUpdatableObjectPtr member) { + member->setParent(nullptr); + members_.erase(std::remove(members_.begin(), members_.end(), member), members_.end()); + } + + public: + //*** Start: UpdatableState implementation ***// + virtual void resetImplementation() override { + for (TUpdatableObjectPtr &member : members_) + member->reset(); + } + + virtual void update() override { + UpdatableObject::update(); + + for (TUpdatableObjectPtr &member : members_) + member->update(); + } + + virtual void reportState(StateReporter &reporter) override { + for (TUpdatableObjectPtr &member : members_) + member->reportState(reporter); + } + + //*** End: UpdatableState implementation ***// + virtual ~UpdatableContainer() = default; + + private: + MembersContainer members_; +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/UpdatableObject.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/UpdatableObject.hpp new file mode 100644 index 00000000..6f219b92 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/UpdatableObject.hpp @@ -0,0 +1,87 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_UpdatableObject_hpp +#define autonomylib_common_UpdatableObject_hpp + +#include "ClockFactory.hpp" +#include "StateReporter.hpp" +#include "common/Common.hpp" + +namespace nervosys { +namespace autonomylib { + +/* +UpdatableObject provides generalized framework for things that needs to be "ticked". For example, +physics objects that needs to update its position every 10ms. +Typically this objects will take their current state, do some processing and produce new state +on every tick. +The reset() provides important ability to rollback all changes to the state back to original +since when it was first initialized. This allows to reset simulation and put all updatable objects +back to their original state. + +After object is created and initialized, reset() must be called first before calling update(). +Do not call reset() from constructor or initialization because that will produce sequence of +init->reset calls for base-derived class that would be incorrect. +*/ + +class UpdatableObject { + public: + virtual ~UpdatableObject() = default; + + void reset() { + if (reset_in_progress) { + return; + } + + reset_in_progress = true; + // TODO: Do we need this check anymore? Maybe reset() should be idempotent. + + if (reset_called && !update_called) { + failResetUpdateOrdering("Multiple reset() calls detected without call to update()"); + } + reset_called = true; + resetImplementation(); + reset_in_progress = false; + } + + virtual void update() { + if (!reset_called) { + failResetUpdateOrdering("reset() must be called first before update()"); + } + update_called = true; + } + + virtual void reportState(StateReporter &reporter) { + unused(reporter); + // default implementation doesn't do anything + } + + virtual UpdatableObject *getPhysicsBody() { return nullptr; } + + virtual ClockBase *clock() { return ClockFactory::get(); } + virtual const ClockBase *clock() const { return ClockFactory::get(); } + + UpdatableObject *getParent() { return parent_; } + + void setParent(UpdatableObject *container) { parent_ = container; } + + std::string getName() { return name_; } + void setName(const std::string &name) { this->name_ = name; } + + protected: + virtual void resetImplementation() = 0; + virtual void failResetUpdateOrdering(std::string err) { throw std::runtime_error(err); } + + private: + bool reset_called = false; + bool update_called = false; + bool reset_in_progress = false; + UpdatableObject *parent_ = nullptr; + std::string name_; +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/VectorMath.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/VectorMath.hpp new file mode 100644 index 00000000..30f63c78 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/VectorMath.hpp @@ -0,0 +1,597 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_VectorMath_hpp +#define autonomylib_common_VectorMath_hpp + +#include + +#include "common/utils/RandomGenerator.hpp" +#include "common/utils/Utils.hpp" + +STRICT_MODE_OFF +// if not using unaligned types then disable vectorization to avoid alignment issues all over the places +// #define EIGEN_DONT_VECTORIZE +#include "Eigen/Dense" +STRICT_MODE_ON + +namespace nervosys { +namespace autonomylib { + +template class VectorMathT { + public: + // IMPORTANT: make sure fixed size vectorization types have no alignment assumption + // https://eigen.tuxfamily.org/dox/group__TopicUnalignedArrayAssert.html + typedef Eigen::Matrix Vector1f; + typedef Eigen::Matrix Vector1d; + typedef Eigen::Matrix Vector2f; + typedef Eigen::Matrix Vector2d; + typedef Eigen::Vector3f Vector3f; + typedef Eigen::Vector3d Vector3d; + typedef Eigen::Array3f Array3f; + typedef Eigen::Array3d Array3d; + typedef Eigen::Quaternion Quaternionf; + typedef Eigen::Quaternion Quaterniond; + typedef Eigen::Matrix Matrix3x3d; + typedef Eigen::Matrix Matrix3x3f; + typedef Eigen::AngleAxisd AngleAxisd; + typedef Eigen::AngleAxisf AngleAxisf; + + typedef common_utils::Utils Utils; + // use different seeds for each component + // TODO: below we are using double instead of RealT because of VC++2017 bug in random implementation + typedef common_utils::RandomGenerator, 1> RandomGeneratorGausianXT; + typedef common_utils::RandomGenerator, 2> RandomGeneratorGausianYT; + typedef common_utils::RandomGenerator, 3> RandomGeneratorGausianZT; + typedef common_utils::RandomGenerator, 1> RandomGeneratorXT; + typedef common_utils::RandomGenerator, 2> RandomGeneratorYT; + typedef common_utils::RandomGenerator, 3> RandomGeneratorZT; + + struct Pose { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Vector3T position = Vector3T::Zero(); + QuaternionT orientation = QuaternionT(1, 0, 0, 0); + + Pose() {} + + Pose(const Vector3T &position_val, const QuaternionT &orientation_val) { + orientation = orientation_val; + position = position_val; + } + + friend Pose operator-(const Pose &lhs, const Pose &rhs) { return VectorMathT::subtract(lhs, rhs); } + friend Pose operator+(const Pose &lhs, const Pose &rhs) { return VectorMathT::add(lhs, rhs); } + friend bool operator==(const Pose &lhs, const Pose &rhs) { + return lhs.position == rhs.position && lhs.orientation.coeffs() == rhs.orientation.coeffs(); + } + friend bool operator!=(const Pose &lhs, const Pose &rhs) { + return !(lhs == rhs); + ; + } + + static Pose nanPose() { + static const Pose nan_pose(VectorMathT::nanVector(), VectorMathT::nanQuaternion()); + return nan_pose; + } + static Pose zero() { + static const Pose zero_pose(Vector3T::Zero(), QuaternionT(1, 0, 0, 0)); + return zero_pose; + } + }; + + struct Transform { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Vector3T translation; + QuaternionT rotation; + }; + + class RandomVectorT { + public: + RandomVectorT() {} + RandomVectorT(RealT min_val, RealT max_val) + : rx_(min_val, max_val), ry_(min_val, max_val), rz_(min_val, max_val) {} + RandomVectorT(const Vector3T &min_val, const Vector3T &max_val) + : rx_(min_val.x(), max_val.x()), ry_(min_val.y(), max_val.y()), rz_(min_val.z(), max_val.z()) {} + + void reset() { + rx_.reset(); + ry_.reset(); + rz_.reset(); + } + + Vector3T next() { return Vector3T(rx_.next(), ry_.next(), rz_.next()); } + + private: + RandomGeneratorXT rx_; + RandomGeneratorYT ry_; + RandomGeneratorZT rz_; + }; + + class RandomVectorGaussianT { + public: + RandomVectorGaussianT() {} + RandomVectorGaussianT(RealT mean, RealT stddev) : rx_(mean, stddev), ry_(mean, stddev), rz_(mean, stddev) {} + RandomVectorGaussianT(const Vector3T &mean, const Vector3T &stddev) + : rx_(mean.x(), stddev.x()), ry_(mean.y(), stddev.y()), rz_(mean.z(), stddev.z()) {} + + void reset() { + rx_.reset(); + ry_.reset(); + rz_.reset(); + } + + Vector3T next() { return Vector3T(rx_.next(), ry_.next(), rz_.next()); } + + private: + RandomGeneratorGausianXT rx_; + RandomGeneratorGausianYT ry_; + RandomGeneratorGausianZT rz_; + }; + + public: + static float magnitude(const Vector2f &v) { return v.norm(); } + + static RealT magnitude(const Vector3T &v) { return v.norm(); } + + static Vector3T rotateVector(const Vector3T &v, const QuaternionT &q, bool assume_unit_quat) { + unused(assume_unit_quat); // stop warning: unused parameter. + // More performant method is at http://gamedev.stackexchange.com/a/50545/20758 + // QuaternionT vq(0, v.x(), v.y(), v.z()); + // QuaternionT qi = assume_unit_quat ? q.conjugate() : q.inverse(); + // return (q * vq * qi).vec(); + + return q._transformVector(v); + } + + static Vector3T rotateVectorReverse(const Vector3T &v, const QuaternionT &q, bool assume_unit_quat) { + // QuaternionT vq(0, v.x(), v.y(), v.z()); + // QuaternionT qi = assume_unit_quat ? q.conjugate() : q.inverse(); + // return (qi * vq * q).vec(); + + if (!assume_unit_quat) + return q.inverse()._transformVector(v); + else + return q.conjugate()._transformVector(v); + } + + static QuaternionT rotateQuaternion(const QuaternionT &q, const QuaternionT &ref, bool assume_unit_quat) { + if (assume_unit_quat) { + // conjugate and inverse are equivalent for unit-length quaternions, + // but the conjugate is less expensive to compute + QuaternionT ref_n = ref; + QuaternionT ref_n_i = ref.conjugate(); + return ref_n * q * ref_n_i; + } else { + QuaternionT ref_n = ref.normalized(); + QuaternionT ref_n_i = ref.inverse(); + return ref_n * q * ref_n_i; + } + } + + static QuaternionT rotateQuaternionReverse(const QuaternionT &q, const QuaternionT &ref, bool assume_unit_quat) { + if (assume_unit_quat) { + QuaternionT ref_n = ref; + QuaternionT ref_n_i = ref.conjugate(); + return ref_n_i * q * ref_n; + } else { + QuaternionT ref_n = ref.normalized(); + QuaternionT ref_n_i = ref.inverse(); + return ref_n_i * q * ref_n; + } + } + + static Vector3T transformToBodyFrame(const Vector3T &v_world, const QuaternionT &q_world, + bool assume_unit_quat = true) { + return rotateVectorReverse(v_world, q_world, assume_unit_quat); + } + + static Vector3T transformToBodyFrame(const Vector3T &v_world, const Pose &body_world, + bool assume_unit_quat = true) { + // translate + Vector3T translated = v_world - body_world.position; + // rotate + return transformToBodyFrame(translated, body_world.orientation, assume_unit_quat); + } + + static Pose transformToBodyFrame(const Pose &pose_world, const Pose &body_world, bool assume_unit_quat = true) { + // translate + Vector3T translated = pose_world.position - body_world.position; + // rotate vector + Vector3T v_body = transformToBodyFrame(translated, body_world.orientation, assume_unit_quat); + // rotate orientation + QuaternionT q_body = rotateQuaternionReverse(pose_world.orientation, body_world.orientation, assume_unit_quat); + + return Pose(v_body, q_body); + } + + static Vector3T transformToWorldFrame(const Vector3T &v_body, const QuaternionT &q_world, + bool assume_unit_quat = true) { + return rotateVector(v_body, q_world, assume_unit_quat); + } + + static Vector3T transformToWorldFrame(const Vector3T &v_body, const Pose &body_world, + bool assume_unit_quat = true) { + // rotate + Vector3T v_world = transformToWorldFrame(v_body, body_world.orientation, assume_unit_quat); + // translate + return v_world + body_world.position; + } + + // transform pose specified in body frame to world frame. The body frame in world coordinate is at body_world + static Pose transformToWorldFrame(const Pose &pose_body, const Pose &body_world, bool assume_unit_quat = true) { + // rotate position + Vector3T v_world = transformToWorldFrame(pose_body.position, body_world.orientation, assume_unit_quat); + // rotate orientation + QuaternionT q_world = rotateQuaternion(pose_body.orientation, body_world.orientation, assume_unit_quat); + // translate + return Pose(v_world + body_world.position, q_world); + } + + static QuaternionT negate(const QuaternionT &q) { + // from Gazebo implementation + return QuaternionT(-q.w(), -q.x(), -q.y(), -q.z()); + } + + static Vector3T getRandomVectorFromGaussian(RealT stddev = 1, RealT mean = 0) { + return Vector3T(Utils::getRandomFromGaussian(stddev, mean), Utils::getRandomFromGaussian(stddev, mean), + Utils::getRandomFromGaussian(stddev, mean)); + } + + static QuaternionT flipZAxis(const QuaternionT &q) { + // quaternion formula comes from http://stackoverflow.com/a/40334755/207661 + return QuaternionT(q.w(), -q.x(), -q.y(), q.z()); + } + + static void toEulerianAngle(const QuaternionT &q, RealT &pitch, RealT &roll, RealT &yaw) { + // z-y-x rotation convention (Tait-Bryan angles) + // Apply yaw, pitch and roll in order to front vector (+X) + // http://www.sedris.org/wg8home/Documents/WG80485.pdf + // http://www.ctralie.com/Teaching/COMPSCI290/Materials/EulerAnglesViz/ + + RealT ysqr = q.y() * q.y(); + + // roll (x-axis rotation) + RealT t0 = +2.0f * (q.w() * q.x() + q.y() * q.z()); + RealT t1 = +1.0f - 2.0f * (q.x() * q.x() + ysqr); + roll = std::atan2(t0, t1); + + // pitch (y-axis rotation) + RealT t2 = +2.0f * (q.w() * q.y() - q.z() * q.x()); + t2 = ((t2 > 1.0f) ? 1.0f : t2); + t2 = ((t2 < -1.0f) ? -1.0f : t2); + pitch = std::asin(t2); + + // yaw (z-axis rotation) + RealT t3 = +2.0f * (q.w() * q.z() + q.x() * q.y()); + RealT t4 = +1.0f - 2.0f * (ysqr + q.z() * q.z()); + yaw = std::atan2(t3, t4); + } + + static RealT angleBetween(const Vector3T &v1, const Vector3T &v2, bool assume_normalized = false) { + Vector3T v1n = v1; + Vector3T v2n = v2; + if (!assume_normalized) { + v1n.normalize(); + v2n.normalize(); + } + + return std::acos(v1n.dot(v2n)); + } + + static Vector3T toAngularVelocity(const QuaternionT &start, const QuaternionT &end, RealT dt) { + if (dt == 0) + return Vector3T(0, 0, 0); + + RealT p_s, r_s, y_s; + toEulerianAngle(start, p_s, r_s, y_s); + + RealT p_e, r_e, y_e; + toEulerianAngle(end, p_e, r_e, y_e); + + RealT p_rate = normalizeAngle(p_e - p_s, (RealT)(2 * M_PI)) / dt; + RealT r_rate = normalizeAngle(r_e - r_s, (RealT)(2 * M_PI)) / dt; + RealT y_rate = normalizeAngle(y_e - y_s, (RealT)(2 * M_PI)) / dt; + + // TODO: optimize below + // Sec 1.3, + // https://ocw.mit.edu/courses/mechanical-engineering/2-154-maneuvering-and-control-of-surface-and-underwater-vehicles-13-49-fall-2004/lecture-notes/lec1.pdf + RealT wx = r_rate + 0 - y_rate * sinf(p_e); + RealT wy = 0 + p_rate * cosf(r_e) + y_rate * sinf(r_e) * cosf(p_e); + RealT wz = 0 - p_rate * sinf(r_e) + y_rate * cosf(r_e) * cosf(p_e); + + return Vector3T(wx, wy, wz); + } + + static Vector3T nanVector() { + static const Vector3T val(std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()); + return val; + } + + static QuaternionT nanQuaternion() { + return QuaternionT(std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()); + } + + static bool hasNan(const Vector3T &v) { return std::isnan(v.x()) || std::isnan(v.y()) || std::isnan(v.z()); } + + static bool hasNan(const QuaternionT &q) { + return std::isnan(q.x()) || std::isnan(q.y()) || std::isnan(q.z()) || std::isnan(q.w()); + } + + static bool hasNan(const Pose &p) { return hasNan(p.position) || hasNan(p.orientation); } + + static QuaternionT addAngularVelocity(const QuaternionT &orientation, const Vector3T &angular_vel, RealT dt) { + QuaternionT dq_unit = + QuaternionT(0, angular_vel.x() * 0.5f, angular_vel.y() * 0.5f, angular_vel.z() * 0.5f) * orientation; + QuaternionT net_q(dq_unit.coeffs() * dt + orientation.coeffs()); + return net_q.normalized(); + } + + // all angles in radians + static QuaternionT toQuaternion(RealT pitch, RealT roll, RealT yaw) { + // z-y-x rotation convention (Tait-Bryan angles) + // http://www.sedris.org/wg8home/Documents/WG80485.pdf + + QuaternionT q; + RealT t0 = std::cos(yaw * 0.5f); + RealT t1 = std::sin(yaw * 0.5f); + RealT t2 = std::cos(roll * 0.5f); + RealT t3 = std::sin(roll * 0.5f); + RealT t4 = std::cos(pitch * 0.5f); + RealT t5 = std::sin(pitch * 0.5f); + + q.w() = t0 * t2 * t4 + t1 * t3 * t5; + q.x() = t0 * t3 * t4 - t1 * t2 * t5; + q.y() = t0 * t2 * t5 + t1 * t3 * t4; + q.z() = t1 * t2 * t4 - t0 * t3 * t5; + return q; + } + + // from https://github.com/arpg/Gazebo/blob/master/gazebo/math/Pose.cc + static Vector3T coordPositionSubtract(const Pose &lhs, const Pose &rhs) { + QuaternionT tmp(0, lhs.position.x() - rhs.position.x(), lhs.position.y() - rhs.position.y(), + lhs.position.z() - rhs.position.z()); + + tmp = rhs.orientation.inverse() * (tmp * rhs.orientation); + + return tmp.vec(); + } + + static QuaternionT coordOrientationSubtract(const QuaternionT &lhs, const QuaternionT &rhs) { + QuaternionT result(rhs.inverse() * lhs); + result.normalize(); + return result; + } + + static Vector3T coordPositionAdd(const Pose &lhs, const Pose &rhs) { + QuaternionT tmp(0, lhs.position.x(), lhs.position.y(), lhs.position.z()); + + tmp = rhs.orientation * (tmp * rhs.orientation.inverse()); + + return tmp.vec() + rhs.position; + } + + static QuaternionT coordOrientationAdd(const QuaternionT &lhs, const QuaternionT &rhs) { + QuaternionT result(rhs * lhs); + result.normalize(); + return result; + } + + static Pose subtract(const Pose &lhs, const Pose &rhs) { + return Pose(coordPositionSubtract(lhs, rhs), coordOrientationSubtract(lhs.orientation, rhs.orientation)); + } + + static Pose add(const Pose &lhs, const Pose &rhs) { + return Pose(coordPositionAdd(lhs, rhs), coordOrientationAdd(lhs.orientation, rhs.orientation)); + } + + static std::string toString(const Vector3T &vect, const char *prefix = nullptr) { + if (prefix) + return Utils::stringf("%s[%f, %f, %f]", prefix, vect[0], vect[1], vect[2]); + else + return Utils::stringf("[%f, %f, %f]", vect[0], vect[1], vect[2]); + } + + static std::string toString(const QuaternionT &quaternion, bool add_eularian = false) { + if (!add_eularian) + return Utils::stringf("[%f, %f, %f, %f]", quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z()); + else { + RealT pitch, roll, yaw; + toEulerianAngle(quaternion, pitch, roll, yaw); + return Utils::stringf("[%f, %f, %f, %f]-[%f, %f, %f]", quaternion.w(), quaternion.x(), quaternion.y(), + quaternion.z(), pitch, roll, yaw); + } + } + + static std::string toString(const Vector2f &vect) { return Utils::stringf("[%f, %f]", vect[0], vect[1]); } + + static RealT getYaw(const QuaternionT &q) { + return std::atan2(2.0f * (q.z() * q.w() + q.x() * q.y()), -1.0f + 2.0f * (q.w() * q.w() + q.x() * q.x())); + } + + static RealT getPitch(const QuaternionT &q) { return std::asin(2.0f * (q.y() * q.w() - q.z() * q.x())); } + + static RealT getRoll(const QuaternionT &q) { + return std::atan2(2.0f * (q.z() * q.y() + q.w() * q.x()), 1.0f - 2.0f * (q.x() * q.x() + q.y() * q.y())); + } + + static RealT normalizeAngle(RealT angle, RealT max_angle = static_cast(360)) { + angle = static_cast(std::fmod(angle, max_angle)); + if (angle > max_angle / 2) + return angle - max_angle; + else if (angle < -max_angle / 2) + return angle + max_angle; + else + return angle; + } + + // assumes that angles are in 0-360 range + static bool isAngleBetweenAngles(RealT angle, RealT start_angle, RealT end_angle) { + if (start_angle < end_angle) { + return (start_angle <= angle && angle <= end_angle); + } else + return (start_angle <= angle || angle <= end_angle); + } + + /** + * \brief Extracts the yaw part from a quaternion, using RPY / euler (z-y'-z'') angles. + * RPY rotates about the fixed axes in the order x-y-z, + * which is the same as euler angles in the order z-y'-x''. + */ + static RealT yawFromQuaternion(const QuaternionT &q) { + return atan2(2.0 * (q.w() * q.z() + q.x() * q.y()), 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z())); + } + + static QuaternionT quaternionFromYaw(RealT yaw) { + return QuaternionT(Eigen::AngleAxis(yaw, Vector3T::UnitZ())); + } + + static QuaternionT toQuaternion(const Vector3T &axis, RealT angle) { + // Alternative: + // auto s = std::sin(angle / 2); + // auto u = axis.normalized(); + // return Quaternionr(std::cos(angle / 2), u.x() * s, u.y() * s, u.z() * s); + + return QuaternionT(Eigen::AngleAxis(angle, axis)); + } + + // linear interpolate + static QuaternionT lerp(const QuaternionT &from, const QuaternionT &to, RealT alpha) { + QuaternionT r; + RealT n_alpha = 1 - alpha; + r.x() = n_alpha * from.x() + alpha * to.x(); + r.y() = n_alpha * from.y() + alpha * to.y(); + r.z() = n_alpha * from.z() + alpha * to.z(); + r.w() = n_alpha * from.w() + alpha * to.w(); + return r.normalized(); + } + + // spherical lerp + static QuaternionT slerp(const QuaternionT &from, const QuaternionT &to, RealT alpha) { + /* + //below is manual way to do this + RealT n_alpha = 1 - alpha; + RealT theta = acos(from.x()*to.x() + from.y()*to.y() + from.z()*to.z() + from.w()*to.w()); + //Check for theta > 0 to avoid division by 0. + if (theta > std::numeric_limits::epsilon()) + { + RealT sn = sin(theta); + RealT Wa = sin(n_alpha*theta) / sn; + RealT Wb = sin(alpha*theta) / sn; + QuaternionT r; + r.x() = Wa * from.x() + Wb * to.x(); + r.y() = Wa * from.y() + Wb * to.y(); + r.z() = Wa * from.z() + Wb * to.z(); + r.w() = Wa * from.w() + Wb * to.w(); + return r.normalized(); + } + //Theta is almost 0. Return "to" quaternion. + //Alternatively, could also do lerp. + else { + return to.normalized(); + } + */ + + return from.slerp(alpha, to); + } + + static Vector3T lerp(const Vector3T &from, const Vector3T &to, RealT alpha) { return (from + alpha * (to - from)); } + + static Vector3T slerp(const Vector3T &from, const Vector3T &to, RealT alpha, bool assume_normalized) { + Vector3T from_ortho, to_ortho; + RealT dot; + getPlaneOrthoVectors(from, to, assume_normalized, from_ortho, to_ortho, dot); + + RealT theta = std::acos(dot) * alpha; + + return from_ortho * std::cos(theta) + to_ortho * std::sin(theta); + } + + static void getPlaneOrthoVectors(const Vector3T &from, const Vector3T &to, bool assume_normalized, + Vector3T &from_ortho, Vector3T &to_ortho, RealT &dot) { + unused(from); + Vector3T to_n = to; + + if (!assume_normalized) { + from_ortho.normalize(); + to_n.normalize(); + } + + dot = from_ortho.dot(to_n); + dot = Utils::clip(dot, -1, 1); + to_ortho = (to_n - from_ortho * dot).normalized(); + } + + static Vector3T slerpByAngle(const Vector3T &from, const Vector3T &to, RealT angle, + bool assume_normalized = false) { + Vector3T from_ortho, to_ortho; + RealT dot; + getPlaneOrthoVectors(from, to, assume_normalized, from_ortho, to_ortho, dot); + + return from_ortho * std::cos(angle) + to_ortho * std::sin(angle); + } + + static Vector3T nlerp(const Vector3T &from, const Vector3T &to, float alpha) { + return lerp(from, to, alpha).normalized(); + } + + // assuming you are looking at front() vector, what rotation you need to look at destPoint? + static QuaternionT lookAt(const Vector3T &sourcePoint, const Vector3T &destPoint) { + /* + //below is manual way to do this without Eigen + Vector3T toVector = (destPoint - sourcePoint); + toVector.normalize(); //this is important! + + RealT dot = VectorMathT::front().dot(toVector); + RealT ang = std::acos(dot); + + Vector3T axis = VectorMathT::front().cross(toVector); + if (axis == Vector3T::Zero()) + axis = VectorMathT::up(); + else + axis = axis.normalized(); + + return VectorMathT::toQuaternion(axis, ang); + */ + return QuaternionT::FromTwoVectors(VectorMathT::front(), destPoint - sourcePoint); + } + + // what rotation we need to rotate "" vector to "to" vector (rotation is around intersection of two vectors) + static QuaternionT toQuaternion(const Vector3T &from, const Vector3T &to) { + return QuaternionT::FromTwoVectors(from, to); + } + + static const Vector3T front() { + static Vector3T v(1, 0, 0); + return v; + } + static const Vector3T back() { + static Vector3T v(-1, 0, 0); + return v; + } + static const Vector3T down() { + static Vector3T v(0, 0, 1); + return v; + } + static const Vector3T up() { + static Vector3T v(0, 0, -1); + return v; + } + static const Vector3T right() { + static Vector3T v(0, 1, 0); + return v; + } + static const Vector3T left() { + static Vector3T v(0, -1, 0); + return v; + } +}; + +typedef VectorMathT, double> VectorMathd; +typedef VectorMathT, float> VectorMathf; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/Waiter.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/Waiter.hpp new file mode 100644 index 00000000..f3f3e8d8 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/Waiter.hpp @@ -0,0 +1,68 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_Waiter_hpp +#define autonomylib_common_Waiter_hpp + +#include "common/CancelToken.hpp" +#include "common/ClockFactory.hpp" +#include "common/Common.hpp" +#include "common/utils/Utils.hpp" +#include +#include + +namespace nervosys { +namespace autonomylib { + +class Waiter { + public: + Waiter(TTimeDelta sleep_duration_seconds, TTimeDelta timeout_sec, CancelToken &cancelable_action) + : sleep_duration_(sleep_duration_seconds), timeout_sec_(timeout_sec), cancelable_action_(cancelable_action), + is_complete_(false) { + proc_start_ = loop_start_ = clock()->nowNanos(); + } + + bool sleep() { + // Sleeps for the time needed to get current running time up to the requested sleep_duration_. + // So this can be used to "throttle" any loop to check something every sleep_duration_ seconds. + + if (isComplete()) + throw std::domain_error("Process was already complete. This instance of Waiter shouldn't be reused!"); + if (isTimeout()) + return false; + + // measure time spent since last iteration + TTimeDelta running_time = clock()->elapsedSince(loop_start_); + double remaining = sleep_duration_ - running_time; + bool done = cancelable_action_.sleep(remaining); + loop_start_ = clock()->nowNanos(); + return done; + } + + // call this mark process as complete + void complete() { is_complete_ = true; } + + bool isComplete() const { return is_complete_; } + + bool isTimeout() const { + if (isComplete()) + return false; + else + return clock()->elapsedSince(proc_start_) >= timeout_sec_; + } + + private: + TTimeDelta sleep_duration_, timeout_sec_; + CancelToken &cancelable_action_; + bool is_complete_; // each waiter should maintain its own complete status + + TTimePoint proc_start_; + TTimePoint loop_start_; + + static ClockBase *clock() { return ClockFactory::get(); } +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/WorkerThread.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/WorkerThread.hpp new file mode 100644 index 00000000..38f6cd82 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/WorkerThread.hpp @@ -0,0 +1,281 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef autonomylib_common_WorkerThread_hpp +#define autonomylib_common_WorkerThread_hpp + +#include "CancelToken.hpp" +#include "ClockFactory.hpp" //TODO: move this out of common\utils +#include "common/utils/Utils.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +namespace nervosys { +namespace autonomylib { + +class CancelableAction : public CancelToken { + protected: + virtual void executeAction() = 0; + + public: + CancelableAction() : is_complete_(false) {} + virtual ~CancelableAction(); + + void reset() { is_complete_ = false; } + + void execute() { + try { + executeAction(); + is_complete_ = true; + } catch (...) { + is_complete_ = false; + throw; + } + } + + bool isComplete() const { return is_complete_; } + + private: + std::atomic is_complete_; +}; + +// This wraps a condition_variable so we can handle the case where we may signal before wait +// and implement the semantics that say wait should be a noop in that case. +class WorkerThreadSignal { + std::condition_variable cv_; + std::mutex mutex_; + std::atomic signaled_; + + public: + WorkerThreadSignal() : signaled_(false) {} + + void signal() { + { + std::unique_lock lock(mutex_); + signaled_ = true; + } + cv_.notify_one(); + } + + template void wait(_Predicate cancel) { + // wait for signal or cancel predicate + while (!signaled_) { + std::unique_lock lock(mutex_); + cv_.wait_for(lock, std::chrono::milliseconds(1), [this, cancel] { return cancel(); }); + } + signaled_ = false; + } + + bool waitFor(double timeout_sec) { + // wait for signal or timeout or cancel predicate + while (!signaled_) { + std::unique_lock lock(mutex_); + cv_.wait_for(lock, std::chrono::milliseconds(static_cast(timeout_sec * 1000))); + } + signaled_ = false; + return true; + } + + void wait() { + // wait for signal or timeout or cancel predicate + std::unique_lock lock(mutex_); + while (!signaled_) { + cv_.wait(lock); + } + lock.unlock(); + signaled_ = false; + } + + bool waitForRetry(double timeout_sec, int n_times) { + std::unique_lock lock(mutex_); + while (!signaled_ && n_times > 0) { + cv_.wait_for(lock, std::chrono::milliseconds(static_cast(timeout_sec * 1000))); + --n_times; + } + lock.unlock(); + if (n_times == 0 && !signaled_) { + return false; + } else { + signaled_ = false; + return true; + } + } +}; + +// This class provides a synchronized worker thread that guarantees to execute +// cancelable tasks on a background thread one at a time. +// It guarantees that previous task is canceled before new task is started. +// The queue size is 1, which means it does NOT guarantee all queued tasks are executed. +// If enqueue is called very quickly the thread will not have a chance to execute each +// task before they get canceled, worst case in a tight loop all tasks are starved and +// nothing executes. +class WorkerThread { + public: + WorkerThread() : thread_running_(false), cancel_request_(false) {} + + ~WorkerThread() { + cancel_request_ = true; + cancel(); + } + void enqueue(std::shared_ptr item) { + // cancel previous item + { + std::unique_lock lock(mutex_); + std::shared_ptr pending = pending_item_; + if (pending != nullptr) { + pending->cancel(); + } + } + + bool running = false; + + { + std::unique_lock lock(mutex_); + pending_item_ = item; + running = thread_running_; + } + + if (running) { + item_arrived_.signal(); + } else { + start(); + } + } + + bool enqueueAndWait(std::shared_ptr item, float timeout_sec) { + // cancel previous item + { + std::unique_lock lock(mutex_); + std::shared_ptr pending = pending_item_; + if (pending != nullptr) { + pending->cancel(); + } + } + + bool running = false; + + // set new item to run + { + std::unique_lock lock(mutex_); + pending_item_ = item; + running = thread_running_; + } + + if (running) { + item_arrived_.signal(); + } else { + start(); + } + + item->sleep(timeout_sec); + + // after the wait if item is still running then cancel it + if (!item->isCancelled() && !item->isComplete()) + item->cancel(); + + return !item->isCancelled(); + } + + void cancel() { + std::unique_lock lock(mutex_); + std::shared_ptr pending = pending_item_; + pending_item_ = nullptr; + if (pending != nullptr) { + pending->cancel(); + } + if (thread_.joinable()) { + item_arrived_.signal(); + thread_.join(); + } + } + + private: + void start() { + // if state == not running + if (!thread_running_) { + + // make sure C++ previous thread is done + { + std::unique_lock lock(mutex_); + if (thread_.joinable()) { + thread_.join(); + } + } + Utils::cleanupThread(thread_); + + // start the thread + cancel_request_ = false; + thread_ = std::thread(&WorkerThread::run, this); + + // wait until thread tells us it has started + thread_started_.wait([this] { return static_cast(cancel_request_); }); + } + } + + void run() { + thread_running_ = true; + + // tell the thread which started this thread that we are on now + { + std::unique_lock lock(mutex_); + thread_started_.signal(); + } + + // until we don't get stopped and have work to do, keep running + while (!cancel_request_ && pending_item_ != nullptr) { + std::shared_ptr pending; + + // get the pending item + { + std::unique_lock lock(mutex_); + pending = pending_item_; + } + + // if pending item is not yet cancelled + if (pending != nullptr && !pending->isCancelled()) { + + // execute pending item + try { + pending->execute(); + } catch (std::exception &e) { + // Utils::DebugBreak(); + Utils::log(Utils::stringf("WorkerThread caught unhandled exception: %s", e.what()), + Utils::kLogLevelError); + } + } + + if (!cancel_request_) { + // wait for next item to arrive or thread is stopped + item_arrived_.wait([this] { return static_cast(cancel_request_); }); + } + } + + thread_running_ = false; + } + + private: + // this is used to wait until our thread actually gets started + WorkerThreadSignal thread_started_; + // when new item arrived, we signal this so waiting thread can continue + WorkerThreadSignal item_arrived_; + + // thread state + std::shared_ptr pending_item_; + std::mutex mutex_; + std::thread thread_; + // while run() is in progress this is true + std::atomic thread_running_; + // has request to stop this worker thread made? + std::atomic cancel_request_; +}; + +} // namespace autonomylib +} // namespace nervosys + +#endif \ No newline at end of file diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/AsyncTasker.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/AsyncTasker.hpp new file mode 100644 index 00000000..1257d4c2 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/AsyncTasker.hpp @@ -0,0 +1,50 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef common_utils_AsyncTasker_hpp +#define common_utils_AsyncTasker_hpp + +#include "ctpl_stl.h" +#include + +class AsyncTasker { + public: + AsyncTasker(unsigned int thread_count = 4) + : threads_(thread_count), error_handler_([](std::exception e) { unused(e); }) {} + + void setErrorHandler(std::function errorHandler) { error_handler_ = errorHandler; } + + void execute(std::function func, unsigned int iterations = 1) { + if (iterations < 1) + return; + + if (iterations == 1) { + threads_.push([=](int i) { + unused(i); + try { + func(); + } catch (std::exception &e) { + error_handler_(e); + }; + }); + } else { + threads_.push([=](int i) { + unused(i); + try { + for (unsigned int itr = 0; itr < iterations; ++itr) { + func(); + } + } catch (std::exception &e) { + // if task failed we shouldn't try additional iterations. + error_handler_(e); + }; + }); + } + } + + private: + ctpl::thread_pool threads_; + std::function error_handler_; +}; + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/ColorUtils.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/ColorUtils.hpp new file mode 100644 index 00000000..2acce784 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/ColorUtils.hpp @@ -0,0 +1,111 @@ +#ifndef common_utils_ColorUtils_hpp +#define common_utils_ColorUtils_hpp + +namespace common_utils { + +class ColorUtils { + public: + static void + valToRGB(double val0To1, unsigned char &r, unsigned char &g, + unsigned char &b) { // actual visible spectrum is 375 to 725 but outside of 400-700 things become too black + wavelengthToRGB(val0To1 * (700 - 400) + 400, r, g, b); + } + + /** + * Convert a wavelength in the visible light spectrum to a RGB color value that is suitable to be displayed on a + * monitor + * + * @param wavelength wavelength in nm + * @return RGB color encoded in int. each color is represented with 8 bits and has a layout of + * 00000000RRRRRRRRGGGGGGGGBBBBBBBB where MSB is at the leftmost + */ + static void wavelengthToRGB(double wavelength, unsigned char &r, unsigned char &g, unsigned char &b) { + double x, y, z; + cie1931WavelengthToXYZFit(wavelength, x, y, z); + double dr, dg, db; + srgbXYZ2RGB(x, y, z, dr, dg, db); + + r = static_cast(static_cast(dr * 0xFF) & 0xFF); + g = static_cast(static_cast(dg * 0xFF) & 0xFF); + b = static_cast(static_cast(db * 0xFF) & 0xFF); + } + + /** + * Convert XYZ to RGB in the sRGB color space + *

+ * The conversion matrix and color component transfer function is taken from http://www.color.org/srgb.pdf, which + * follows the International Electrotechnical Commission standard IEC 61966-2-1 "Multimedia systems and equipment - + * Colour measurement and management - Part 2-1: Colour management - Default RGB colour space - sRGB" + * + * @param x XYZ values in a double array in the order of X, Y, Z. each value in the range of [0.0, 1.0] + * @param y XYZ values in a double array in the order of X, Y, Z. each value in the range of [0.0, 1.0] + * @param x XYZ values in a double array in the order of X, Y, Z. each value in the range of [0.0, 1.0] + * @param r values in a double array, in the order of R, G, B. each value in the range of [0.0, 1.0] + * @param g values in a double array, in the order of R, G, B. each value in the range of [0.0, 1.0] + * @param b values in a double array, in the order of R, G, B. each value in the range of [0.0, 1.0] + */ + static void srgbXYZ2RGB(double x, double y, double z, double &r, double &g, double &b) { + double rl = 3.2406255 * x + -1.537208 * y + -0.4986286 * z; + double gl = -0.9689307 * x + 1.8757561 * y + 0.0415175 * z; + double bl = 0.0557101 * x + -0.2040211 * y + 1.0569959 * z; + + r = srgbXYZ2RGBPostprocess(rl); + g = srgbXYZ2RGBPostprocess(gl); + b = srgbXYZ2RGBPostprocess(bl); + + return; + } + + /** + * helper function for {@link #srgbXYZ2RGB(double[])} + */ + static double srgbXYZ2RGBPostprocess(double c) { + // clip if c is out of range + c = c > 1 ? 1 : (c < 0 ? 0 : c); + + // apply the color component transfer function + c = c <= 0.0031308 ? c * 12.92 : 1.055 * std::pow(c, 1. / 2.4) - 0.055; + + return c; + } + + /** + * A multi-lobe, piecewise Gaussian fit of CIE 1931 XYZ Color Matching Functions by Wyman el al. from Nvidia. The + * code here is adopted from the Listing 1 of the paper authored by Wyman et al. + *

+ * Reference: Chris Wyman, Peter-Pike Sloan, and Peter Shirley, Simple Analytic Approximations to the CIE XYZ Color + * Matching Functions, Journal of Computer Graphics Techniques (JCGT), vol. 2, no. 2, 1-11, 2013. + * + * @param wavelength wavelength in nm + * @return XYZ in a double array in the order of X, Y, Z. each value in the range of [0.0, 1.0] + */ + static void cie1931WavelengthToXYZFit(double wavelength, double &x, double &y, double &z) { + double wave = wavelength; + + { + double t1 = (wave - 442.0) * ((wave < 442.0) ? 0.0624 : 0.0374); + double t2 = (wave - 599.8) * ((wave < 599.8) ? 0.0264 : 0.0323); + double t3 = (wave - 501.1) * ((wave < 501.1) ? 0.0490 : 0.0382); + + x = 0.362 * std::exp(-0.5 * t1 * t1) + 1.056 * std::exp(-0.5 * t2 * t2) - 0.065 * std::exp(-0.5 * t3 * t3); + } + + { + double t1 = (wave - 568.8) * ((wave < 568.8) ? 0.0213 : 0.0247); + double t2 = (wave - 530.9) * ((wave < 530.9) ? 0.0613 : 0.0322); + + y = 0.821 * std::exp(-0.5 * t1 * t1) + 0.286 * std::exp(-0.5 * t2 * t2); + } + + { + double t1 = (wave - 437.0) * ((wave < 437.0) ? 0.0845 : 0.0278); + double t2 = (wave - 459.0) * ((wave < 459.0) ? 0.0385 : 0.0725); + + z = 1.217 * std::exp(-0.5 * t1 * t1) + 0.681 * std::exp(-0.5 * t2 * t2); + } + } +}; + +} // namespace common_utils + +#endif \ No newline at end of file diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/EnumFlags.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/EnumFlags.hpp new file mode 100644 index 00000000..8a3dc10c --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/EnumFlags.hpp @@ -0,0 +1,103 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef common_utils_EnumFlags_hpp +#define common_utils_EnumFlags_hpp + +namespace common_utils { + +template ::type> class EnumFlags { + protected: + TUnderlying flags_; + + public: + EnumFlags() : flags_(0) {} + EnumFlags(TEnum singleFlag) : flags_(static_cast(singleFlag)) {} + EnumFlags(TUnderlying flags) : flags_(flags) {} + EnumFlags(const EnumFlags &original) : flags_(original.flags_) {} + + EnumFlags &operator|=(TEnum add_value) { + flags_ |= static_cast(add_value); + return *this; + } + EnumFlags &operator|=(EnumFlags add_value) { + flags_ |= add_value.flags_; + return *this; + } + EnumFlags operator|(TEnum add_value) const { + EnumFlags result(*this); + result |= add_value; + return result; + } + EnumFlags operator|(EnumFlags add_value) const { + EnumFlags result(*this); + result |= add_value.flags_; + return result; + } + EnumFlags &operator&=(TEnum mask_value) { + flags_ &= static_cast(mask_value); + return *this; + } + EnumFlags &operator&=(EnumFlags mask_value) { + flags_ &= mask_value.flags_; + return *this; + } + EnumFlags operator&(TEnum mask_value) const { + EnumFlags result(*this); + result &= mask_value; + return result; + } + EnumFlags operator&(EnumFlags mask_value) const { + EnumFlags result(*this); + result &= mask_value.flags_; + return result; + } + EnumFlags operator~() const { + EnumFlags result(*this); + result.flags_ = ~result.flags_; + return result; + } + EnumFlags &operator^=(TEnum mask_value) { + flags_ ^= mask_value; + return *this; + } + EnumFlags &operator^=(EnumFlags mask_value) { + flags_ ^= mask_value.flags_; + return *this; + } + EnumFlags operator^(TEnum mask_value) const { + EnumFlags result(*this); + result.flags_ ^= mask_value; + return result; + } + EnumFlags operator^(EnumFlags mask_value) const { + EnumFlags result(*this); + result.flags_ ^= mask_value.flags_; + return result; + } + + // EnumFlags& operator ~=(TEnum mask_value) + //{ + // flags_ ~= static_cast(mask_value); + // return *this; + // } + // EnumFlags& operator ~=(EnumFlags mask_value) + //{ + // flags_ ~= mask_value.flags_; + // return *this; + // } + + // equality operators + bool operator==(const EnumFlags &rhs) const { return flags_ == rhs.flags_; } + inline bool operator!=(const EnumFlags &rhs) const { return !(*this == rhs); } + + // type conversion + operator bool() const { return flags_ != 0; } + operator TUnderlying() const { return flags_; } + + TEnum toEnum() const { return static_cast(flags_); } +}; + +} // namespace common_utils + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/ExceptionUtils.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/ExceptionUtils.hpp new file mode 100644 index 00000000..885461cd --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/ExceptionUtils.hpp @@ -0,0 +1,55 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef common_utils_ExceptionsUtils_hpp +#define common_utils_ExceptionsUtils_hpp + +// print exception stack trace +// from: http://stackoverflow.com/a/11674810/207661 + +#ifdef __GNUC__ + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-pedantic" + +#include +#include +#include +#include +#include +#include +#include + +namespace { +void *last_frames[20]; +size_t last_size; +std::string exception_name; + +std::string demangle(const char *name) { + int status; + std::unique_ptr realname(abi::__cxa_demangle(name, 0, 0, &status), &std::free); + return status ? "failed" : &*realname; +} +} // namespace + +extern "C" { +void __cxa_throw(void *ex, void *info, void (*dest)(void *)) { + exception_name = demangle(reinterpret_cast(info)->name()); + last_size = backtrace(last_frames, sizeof last_frames / sizeof(void *)); + + static void (*const rethrow)(void *, void *, void (*)(void *)) __attribute__((noreturn)) = + (void (*)(void *, void *, void (*)(void *)))dlsym(RTLD_NEXT, "__cxa_throw"); + rethrow(ex, info, dest); +} +} + +#pragma GCC diagnostic pop +#endif + +void printExceptionStack() { +#ifdef __GNUC__ + backtrace_symbols_fd(last_frames, last_size, 2); +#endif +} + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/FileSystem.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/FileSystem.hpp new file mode 100644 index 00000000..c8420926 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/FileSystem.hpp @@ -0,0 +1,206 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef common_utils_FileSystem_hpp +#define common_utils_FileSystem_hpp + +#include "Utils.hpp" +#include +#include +#include + +// This defines a default folder name for all the files created by AutonomyLib so they +// are all gathered nicely in one place in the user's documents folder. +#ifndef ProductFolderName +#define ProductFolderName "AutonomySim" +#endif + +#ifndef _CRT_SECURE_NO_WARNINGS +#define _CRT_SECURE_NO_WARNINGS 1 +#endif + +namespace common_utils { +class FileSystem { + typedef unsigned int uint; + + public: + // please use the combine() method instead. + static const char kPathSeparator = +#ifdef _WIN32 + '\\'; +#else + '/'; +#endif + + static std::string createDirectory(const std::string &fullPath); + + static std::string getUserHomeFolder() { +// Windows uses USERPROFILE, Linux uses HOME +#ifdef _WIN32 + std::wstring userProfile = _wgetenv(L"USERPROFILE"); + std::wstring_convert, wchar_t> converter; + return converter.to_bytes(userProfile); +#else + return std::getenv("HOME"); +#endif + } + + static std::string getUserDocumentsFolder(); + + static std::string getExecutableFolder(); + + static std::string getAppDataFolder() { return ensureFolder(combine(getUserDocumentsFolder(), ProductFolderName)); } + + static std::string ensureFolder(const std::string &fullpath) { + // make sure this directory exists. + return createDirectory(fullpath); + } + + static std::string ensureFolder(const std::string &parentFolder, const std::string &child) { + // make sure this directory exists. + return createDirectory(combine(parentFolder, child)); + } + + static std::string combine(const std::string &parentFolder, const std::string &child) { + if (child.size() == 0) + return parentFolder; + + size_t len = parentFolder.size(); + if (parentFolder.size() > 0 && parentFolder[len - 1] == kPathSeparator) { + // parent already ends with '/' + return parentFolder + child; + } + len = child.size(); + if (len > 0 && child[0] == kPathSeparator) { + // child already starts with '/' + return parentFolder + child; + } + return parentFolder + kPathSeparator + child; + } + + static void removeLeaf(std::string &path) { + size_t size = path.size(); + size_t pos = path.find_last_of('/'); + if (pos != std::string::npos) { + path.erase(pos, size - pos); + } + } + + static std::string getFileExtension(const std::string &str) { + // bugbug: this is not unicode safe. + int len = static_cast(str.size()); + const char *ptr = str.c_str(); + int i = 0; + for (i = len - 1; i >= 0; i--) { + if (ptr[i] == '.') + break; + } + if (i < 0) + return ""; + return str.substr(i, len - i); + } + + static std::string getLogFolderPath(bool folder_timestamp, const std::string &parent = "") { + std::string logfolder = folder_timestamp ? Utils::to_string(Utils::now()) : ""; + std::string parent_folder = (parent == "") ? getAppDataFolder() : parent; + std::string fullPath = combine(parent_folder, logfolder); + ensureFolder(fullPath); + + return fullPath; + } + + static std::string getLogFileNamePath(const std::string &fullPath, const std::string &prefix, + const std::string &suffix, const std::string &extension, + bool file_timestamp) { + // TODO: because this bug we are using alternative code with stringstream + // https://answers.unrealengine.com/questions/664905/unreal-crashes-on-two-lines-of-extremely-simple-st.html + + std::string filename; + filename.append(ensureFolder(fullPath)).push_back(kPathSeparator); + filename.append(prefix) + .append(suffix) + .append(file_timestamp ? Utils::to_string(Utils::now()) : "") + .append(extension); + + return filename; + + // std::stringstream filename_ss; + // filename_ss << ensureFolder(fullPath) << kPathSeparator << prefix << suffix << timestamp << extension; + // return filename_ss.str(); + } + + static void openTextFile(const std::string &filepath, std::ifstream &file) { +#ifdef _WIN32 + // WIN32 will create the wrong file names if we don't first convert them to UTF-16. + std::wstring_convert, wchar_t> converter; + std::wstring wide_path = converter.from_bytes(filepath); + file.open(wide_path, std::ios::in); +#else + file.open(filepath, std::ios::in); +#endif + } + + static void createBinaryFile(const std::string &filepath, std::ofstream &file) { +#ifdef _WIN32 + // WIN32 will create the wrong file names if we don't first convert them to UTF-16. + std::wstring_convert, wchar_t> converter; + std::wstring wide_path = converter.from_bytes(filepath); + file.open(wide_path, std::ios::binary | std::ios::trunc); +#else + file.open(filepath, std::ios::binary | std::ios::trunc); +#endif + } + + static void createTextFile(const std::string &filepath, std::ofstream &file) { +#ifdef _WIN32 + // WIN32 will create the wrong file names if we don't first convert them to UTF-16. + std::wstring_convert, wchar_t> converter; + std::wstring wide_path = converter.from_bytes(filepath); + file.open(wide_path, std::ios::out | std::ios::trunc); +#else + file.open(filepath, std::ios::trunc); +#endif + + if (file.fail()) + throw std::ios_base::failure(std::strerror(errno)); + } + + static std::string createLogFile(const std::string &suffix, std::ofstream &flog) { + std::string log_folderpath = common_utils::FileSystem::getLogFolderPath(false); + std::string filepath = getLogFileNamePath(log_folderpath, "log_", suffix, ".tsv", true); + createTextFile(filepath, flog); + + Utils::log(Utils::stringf("log file started: %s", filepath.c_str())); + flog.exceptions(flog.exceptions() | std::ios::failbit | std::ifstream::badbit); + return filepath; + } + + static std::string readLineFromFile(std::ifstream &file) { + std::string line; + try { + std::getline(file, line); + } catch (...) { + if (!file.eof()) + throw; + } + return line; + } + + static void appendLineToFile(const std::string &filepath, const std::string &line) { + std::ofstream file; +#ifdef _WIN32 + // WIN32 will create the wrong file names if we don't first convert them to UTF-16. + std::wstring_convert, wchar_t> converter; + std::wstring wide_path = converter.from_bytes(filepath); + file.open(wide_path, std::ios::out | std::ios::app); +#else + file.open(filepath, std::ios::out | std::ios::app); +#endif + if (file.fail()) + throw std::ios_base::failure(std::strerror(errno)); + file.exceptions(file.exceptions() | std::ios::failbit | std::ifstream::badbit); + file << line << std::endl; + } +}; +} // namespace common_utils +#endif \ No newline at end of file diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/MedianFilter.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/MedianFilter.hpp new file mode 100644 index 00000000..6c459866 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/MedianFilter.hpp @@ -0,0 +1,87 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef common_utils_MedianFilter_hpp +#define common_utils_MedianFilter_hpp + +#include +#include +#include + +namespace common_utils { + +template class MedianFilter { + private: + std::vector buffer_, buffer_copy_; + int window_size_, window_size_2x_, window_size_half_; + float outlier_factor_; + int buffer_index_; + + public: + MedianFilter(); + MedianFilter(int window_size, float outlier_factor); + void initialize(int window_size, float outlier_factor); + std::tuple filter(T value); +}; + +template void MedianFilter::initialize(int window_size, float outlier_factor) { + buffer_.resize(window_size); + buffer_copy_.resize(window_size); + window_size_ = window_size; + window_size_2x_ = window_size_ * 2; + window_size_half_ = window_size_ / 2; + outlier_factor_ = outlier_factor; + buffer_index_ = 0; +} + +template MedianFilter::MedianFilter() { + initialize(buffer_.capacity(), std::numeric_limits::infinity()); +} + +template MedianFilter::MedianFilter(int window_size, float outlier_factor) { + initialize(window_size, std::numeric_limits::infinity()); +} + +template std::tuple MedianFilter::filter(T value) { + buffer_[buffer_index_++ % window_size_] = value; + if (buffer_index_ == window_size_2x_) + buffer_index_ = window_size_; + + if (buffer_index_ >= window_size_) { + // find median + for (auto i = 0; i < window_size_; ++i) + buffer_copy_[i] = buffer_[i]; + std::sort(buffer_copy_.begin(), buffer_copy_.end()); + double median = buffer_copy_[window_size_half_]; + + // average values that fall between upper and lower bound of median + auto lower_bound = median - median * outlier_factor_, upper_bound = median + median * outlier_factor_; + double sum = 0; + int count = 0; + for (auto i = 0; i < window_size_; ++i) { + if (buffer_copy_[i] >= lower_bound && buffer_copy_[i] <= upper_bound) { + sum += buffer_copy_[i]; + ++count; + } + } + double mean = sum / count; + + double std_dev_sum = 0; + for (auto i = 0; i < window_size_; ++i) { + if (buffer_copy_[i] >= lower_bound && buffer_copy_[i] <= upper_bound) { + double diff = buffer_copy_[i] - mean; + sum += diff * diff; + } + } + double variance = std_dev_sum / count; + + return std::make_tuple(mean, variance); + } else { + // window is not full, return the input as-is + // TODO: use growing window here + return std::make_tuple(double(value), 0); + } +} + +} // namespace common_utils +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/MinWinDefines.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/MinWinDefines.hpp new file mode 100644 index 00000000..a027df34 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/MinWinDefines.hpp @@ -0,0 +1,61 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef common_utils_MinWinDefines_hpp +#define common_utils_MinWinDefines_hpp + +/************************************************ +This header disables lots of things soafter including it +you may have to #undef whatever you are actually using. +************************************************/ + +// WIN32_LEAN_AND_MEAN excludes rarely-used services from windows headers. +#define WIN32_LEAN_AND_MEAN + +// The below excludes some other unused services from the windows headers -- see windows.h for details. +#define NOGDICAPMASKS // CC_*, LC_*, PC_*, CP_*, TC_*, RC_ +#define NOVIRTUALKEYCODES // VK_* +#define NOWINMESSAGES // WM_*, EM_*, LB_*, CB_* +#define NOWINSTYLES // WS_*, CS_*, ES_*, LBS_*, SBS_*, CBS_* +#define NOSYSMETRICS // SM_* +#define NOMENUS // MF_* +#define NOICONS // IDI_* +#define NOKEYSTATES // MK_* +#define NOSYSCOMMANDS // SC_* +#define NORASTEROPS // Binary and Tertiary raster ops +#define NOSHOWWINDOW // SW_* +#define OEMRESOURCE // OEM Resource values +#define NOATOM // Atom Manager routines +#define NOCLIPBOARD // Clipboard routines +#define NOCOLOR // Screen colors +#define NOCTLMGR // Control and Dialog routines +#define NODRAWTEXT // DrawText() and DT_* +#define NOGDI // All GDI #defines and routines +#define NOKERNEL // All KERNEL #defines and routines +#define NOUSER // All USER #defines and routines +#define NONLS // All NLS #defines and routines +#define NOMB // MB_* and MessageBox() +#define NOMEMMGR // GMEM_*, LMEM_*, GHND, LHND, associated routines +#define NOMETAFILE // typedef METAFILEPICT +#define NOMINMAX // Macros min(a,b) and max(a,b) +#define NOMSG // typedef MSG and associated routines +#define NOOPENFILE // OpenFile(), OemToAnsi, AnsiToOem, and OF_* +#define NOSCROLL // SB_* and scrolling routines +#define NOSERVICE // All Service Controller routines, SERVICE_ equates, etc. +#define NOSOUND // Sound driver routines +#define NOTEXTMETRIC // typedef TEXTMETRIC and associated routines +#define NOWH // SetWindowsHook and WH_* +#define NOWINOFFSETS // GWL_*, GCL_*, associated routines +#define NOCOMM // COMM driver routines +#define NOKANJI // Kanji support stuff. +#define NOHELP // Help engine interface. +#define NOPROFILER // Profiler interface. +#define NODEFERWINDOWPOS // DeferWindowPos routines +#define NOMCX // Modem Configuration Extensions +#define NOCRYPT +#define NOTAPE +#define NOIMAGE +#define NOPROXYSTUB +#define NORPC + +#endif \ No newline at end of file diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/OnlineStats.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/OnlineStats.hpp new file mode 100644 index 00000000..faceffae --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/OnlineStats.hpp @@ -0,0 +1,87 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef common_utils_OnlineStats_hpp +#define common_utils_OnlineStats_hpp + +#include + +namespace common_utils { + +class OnlineStats { + public: + OnlineStats() { clear(); } + + void clear() { + n = 0; + m1 = m2 = m3 = m4 = 0.0f; + } + + void insert(double x) { + double delta, delta_n, delta_n2, term1; + + int64_t n1 = n; + n++; + delta = x - m1; + delta_n = delta / n; + delta_n2 = delta_n * delta_n; + term1 = delta * delta_n * n1; + m1 += delta_n; + m4 += term1 * delta_n2 * (n * n - 3 * n + 3) + 6 * delta_n2 * m2 - 4 * delta_n * m3; + m3 += term1 * delta_n * (n - 2) - 3 * delta_n * m2; + m2 += term1; + } + + int64_t size() const { return n; } + + double mean() const { return m1; } + + double variance() const { return m2 / (n - 1.0f); } + + double standardDeviation() const { return sqrt(variance()); } + + double skewness() const { return sqrt(double(n)) * m3 / pow(m2, 1.5f); } + + double kurtosis() const { return double(n) * m4 / (m2 * m2) - 3.0f; } + + OnlineStats operator+(const OnlineStats &rhs) { + OnlineStats combined; + + combined.n = this->n + rhs.n; + + double delta = rhs.m1 - this->m1; + double delta2 = delta * delta; + double delta3 = delta * delta2; + double delta4 = delta2 * delta2; + + combined.m1 = (this->n * this->m1 + rhs.n * rhs.m1) / combined.n; + + combined.m2 = this->m2 + rhs.m2 + delta2 * this->n * rhs.n / combined.n; + + combined.m3 = this->m3 + rhs.m3 + delta3 * this->n * rhs.n * (this->n - rhs.n) / (combined.n * combined.n); + combined.m3 += 3.0f * delta * (this->n * rhs.m2 - rhs.n * this->m2) / combined.n; + + combined.m4 = this->m4 + rhs.m4 + + delta4 * this->n * rhs.n * (this->n * this->n - this->n * rhs.n + rhs.n * rhs.n) / + (combined.n * combined.n * combined.n); + combined.m4 += + 6.0f * delta2 * (this->n * this->n * rhs.m2 + rhs.n * rhs.n * this->m2) / (combined.n * combined.n) + + 4.0f * delta * (this->n * rhs.m3 - rhs.n * this->m3) / combined.n; + + return combined; + } + + OnlineStats &operator+=(const OnlineStats &rhs) { + OnlineStats combined = *this + rhs; + *this = combined; + return *this; + } + + private: + int64_t n; // don't declare as unsigned because we do n-k and if n = 0, we get large number + double m1, m2, m3, m4; + +}; // class + +} // namespace common_utils +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/ProsumerQueue.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/ProsumerQueue.hpp new file mode 100644 index 00000000..7d2665e5 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/ProsumerQueue.hpp @@ -0,0 +1,97 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef commn_utils_ProsumerQueue_hpp +#define commn_utils_ProsumerQueue_hpp + +#include +#include +#include +#include +#include + +namespace common_utils { + +/* + This queue can support multiple producers consumers, but it should be used carefully + because its a *blocking* queue after all. Do not treat as black box, read the code for + what and how its doing so you know what will happen. There are non-blocking versions available + out there but more often than not they are buggy. This one is simpler and effortlessly cross-platform. + + In multi-consumer scenario, ideally the consumer + thread that does the pop, should also spend time on working on poped item instead of + handing over item to some other thread and going back to another pop right away. If you + do that then other consumer threads might starve and become pointless. Similarly + in multi-producer scenario, the thread doing the push should not immediately come back to + push. So ideally, producer and consumer threads should also perform any time consuming tasks + such as I/O so overall throughput of multi-producer/multi-consumer is maximized. You can tune + the number of thread so queue size doesn't grow out of bound. If you have + only one producer and oner consumer than it might be better idea to do time consuming stuff + such as I/O on sepratae threads so queue doesn't become too large. +*/ + +template class ProsumerQueue { + public: + ProsumerQueue() { is_done_ = false; } + + T pop() { + std::unique_lock global_lock(mutex_); + while (queue_.empty()) { + // this may be spurious wake-up + // in multi-consumer scenario + cond_.wait(global_lock); + } + auto val = queue_.front(); + queue_.pop(); + return val; + } + + bool tryPop(T &item) { + std::unique_lock global_lock(mutex_); + if (queue_.empty()) + return false; + + item = queue_.front(); + queue_.pop(); + return true; + } + + void clear() { + std::unique_lock global_lock(mutex_); + return queue_.clear(); + } + + size_t size() const { + std::unique_lock global_lock(mutex_); + return queue_.size(); + } + + bool empty() const { + std::unique_lock global_lock(mutex_); + return queue_.empty(); + } + + void push(const T &item) { + std::unique_lock global_lock(mutex_); + queue_.push(item); + global_lock.unlock(); + cond_.notify_one(); + } + + // is_done_ flag is just convinience flag for external use + // its not used by this class + bool getIsDone() { return is_done_; } + void setIsDone(bool val) { is_done_ = val; } + + // non-copiable + ProsumerQueue(const ProsumerQueue &) = delete; + ProsumerQueue &operator=(const ProsumerQueue &) = delete; + + private: + std::queue queue_; + std::mutex mutex_; + std::condition_variable cond_; + std::atomic is_done_; +}; +} // namespace common_utils +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/RandomGenerator.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/RandomGenerator.hpp new file mode 100644 index 00000000..ffb0bfb8 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/RandomGenerator.hpp @@ -0,0 +1,41 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef commn_utils_RandomGenerator_hpp +#define commn_utils_RandomGenerator_hpp + +#include + +namespace common_utils { + +template class RandomGenerator { + public: + // for uniform distribution supply min and max (inclusive) + // for gaussian distribution supply mean and sigma + template RandomGenerator(DistArgs... dist_args) : dist_(dist_args...), rand_(Seed) {} + + void seed(int val) { rand_.seed(val); } + + TReturn next() { return static_cast(dist_(rand_)); } + + void reset() { + rand_.seed(Seed); + dist_.reset(); + } + + private: + TDistribution dist_; + std::mt19937 rand_; +}; + +typedef RandomGenerator> RandomGeneratorD; +typedef RandomGenerator> RandomGeneratorF; +typedef RandomGenerator> RandomGeneratorI; +typedef RandomGenerator> RandomGeneratorUI; +// TODO: below we should have float instead of double but VC++2017 has a bug :( +typedef RandomGenerator> RandomGeneratorGaussianF; +typedef RandomGenerator> RandomGeneratorGaussianD; + +} // namespace common_utils + +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/ScheduledExecutor.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/ScheduledExecutor.hpp new file mode 100644 index 00000000..c23eb602 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/ScheduledExecutor.hpp @@ -0,0 +1,201 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef commn_utils_ScheduledExecutor_hpp +#define commn_utils_ScheduledExecutor_hpp + +#include +#include +#include +#include +#include +#include +#include + +namespace common_utils { + +class ScheduledExecutor { + public: + ScheduledExecutor() {} + + ScheduledExecutor(const std::function &callback, uint64_t period_nanos) { + initialize(callback, period_nanos); + } + + ~ScheduledExecutor() { stop(); } + + void initialize(const std::function &callback, uint64_t period_nanos) { + callback_ = callback; + period_nanos_ = period_nanos; + started_ = false; + frame_countdown_enabled_ = false; + } + + void start() { + started_ = true; + is_first_period_ = true; + + initializePauseState(); + + sleep_time_avg_ = 0; + Utils::cleanupThread(th_); + th_ = std::thread(&ScheduledExecutor::executorLoop, this); + } + + void pause(bool is_paused) { + paused_ = is_paused; + pause_period_start_ = 0; // cancel any pause period. + } + + bool isPaused() const { return paused_; } + + void pauseForTime(double seconds) { + pause_period_start_ = nanos(); + pause_period_ = static_cast(1E9 * seconds); + paused_ = true; + } + + void continueForTime(double seconds) { + pause_period_start_ = nanos(); + pause_period_ = static_cast(1E9 * seconds); + paused_ = false; + } + + void continueForFrames(uint32_t frames) { + pause_period_start_ = 0; // cancel any pause period. + frame_countdown_enabled_ = true; + targetFrameNumber_ = frames + currentFrameNumber_; + paused_ = false; + } + + void setFrameNumber(uint32_t frameNumber) { currentFrameNumber_ = frameNumber; } + + void stop() { + if (started_) { + started_ = false; + initializePauseState(); + + try { + if (th_.joinable()) { + th_.join(); + } + } catch (const std::system_error & /* e */) { + } + } + } + + bool isRunning() const { return started_ && !paused_; } + + double getSleepTimeAvg() const { + // TODO: make this function thread safe by using atomic types + // right now this is not implemented for performance and that + // return of this function is purely informational/debugging purposes + return sleep_time_avg_; + } + + void lock() { mutex_.lock(); } + void unlock() { mutex_.unlock(); } + + private: + void initializePauseState() { + paused_ = false; + pause_period_start_ = 0; + pause_period_ = 0; + } + + private: + typedef std::chrono::high_resolution_clock clock; + typedef uint64_t TTimePoint; + typedef uint64_t TTimeDelta; + template using duration = std::chrono::duration; + + static TTimePoint nanos() { return clock::now().time_since_epoch().count(); } + + static void sleep_for(TTimePoint delay_nanos) { + /* + This is spin loop implementation which may be suitable for sub-millisecond resolution. + //TODO: investigate below alternatives + On Windows we can use multimedia timers however this requires including entire Win32 header. + On Linux we can use nanosleep however below 2ms delays in real-time scheduler settings this + probbaly does spin loop anyway. + + */ + + if (delay_nanos >= 5000000LL) { // put thread to sleep + std::this_thread::sleep_for(std::chrono::duration(delay_nanos / 1.0E9)); + } else { // for more precise timing, do spinning + auto start = nanos(); + while ((nanos() - start) < delay_nanos) { + std::this_thread::yield(); + // std::this_thread::sleep_for(std::chrono::duration(0)); + } + } + } + + void executorLoop() { + TTimePoint call_end = nanos(); + while (started_) { + TTimePoint period_start = nanos(); + TTimeDelta since_last_call = period_start - call_end; + + if (frame_countdown_enabled_) { + if (targetFrameNumber_ <= currentFrameNumber_) { + if (!isPaused()) + pause(true); + + frame_countdown_enabled_ = false; + } + } + + if (pause_period_start_ > 0) { + if (nanos() - pause_period_start_ >= pause_period_) { + pause(!isPaused()); + pause_period_start_ = 0; + } + } + + // is this first loop? + if (!is_first_period_) { + if (!paused_) { + // when we are doing work, don't let other thread to cause contention + std::lock_guard locker(mutex_); + + bool result = callback_(since_last_call); + if (!result) { + started_ = result; + } + } + } else + is_first_period_ = false; + + call_end = nanos(); + + TTimeDelta elapsed_period = nanos() - period_start; + // prevent underflow: https://github.com/nervosys/AutonomySim/issues/617 + TTimeDelta delay_nanos = period_nanos_ > elapsed_period ? period_nanos_ - elapsed_period : 0; + // moving average of how much we are sleeping + sleep_time_avg_ = 0.25f * sleep_time_avg_ + 0.75f * delay_nanos; + if (delay_nanos > 0 && started_) + sleep_for(delay_nanos); + } + } + + private: + uint64_t period_nanos_; + std::thread th_; + std::function callback_; + bool is_first_period_; + std::atomic_bool started_; + std::atomic_bool paused_; + std::atomic pause_period_; + std::atomic pause_period_start_; + uint32_t currentFrameNumber_; + uint32_t targetFrameNumber_; + std::atomic_bool frame_countdown_enabled_; + + double sleep_time_avg_; + + std::mutex mutex_; +}; +} // namespace common_utils +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/Signal.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/Signal.hpp new file mode 100644 index 00000000..7cb26e5d --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/Signal.hpp @@ -0,0 +1,84 @@ +#ifndef common_utils_Signal_hpp +#define common_utils_Signal_hpp + +#include +#include + +// A signal object may call multiple slots with the +// same signature. You can connect functions to the signal +// which will be called when the emit() method on the +// signal object is invoked. Any argument passed to emit() +// will be passed to the given functions. + +/* +//USAGE: + +#include "Signal.hpp" +#include + +int main() { + +// create new signal +Signal signal; + +// attach a slot +signal.connect([](std::string arg1, int arg2) { +std::cout << arg1 << " " << arg2 << std::endl; +}); + +signal.emit("The answer:", 42); + +return 0; +} + +*/ + +namespace common_utils { + +template class Signal { + + public: + Signal() : current_id_(0) {} + + // Currently deleted since unused and there shouldn't be any need for this + Signal(Signal const &other) = delete; + Signal &operator=(Signal const &other) = delete; + + // connects a member function to this Signal + template int connect_member(T *inst, void (T::*func)(Args...)) { + return connect([=](Args... args) { (inst->*func)(args...); }); + } + + // connects a const member function to this Signal + template int connect_member(T *inst, void (T::*func)(Args...) const) { + return connect([=](Args... args) { (inst->*func)(args...); }); + } + + // connects a std::function to the signal. The returned + // value can be used to disconnect the function again + int connect(std::function const &slot) const { + slots_.insert(std::make_pair(++current_id_, slot)); + return current_id_; + } + + // disconnects a previously connected function + void disconnect(int id) const { slots_.erase(id); } + + // disconnects all previously connected functions + void disconnect_all() const { slots_.clear(); } + + // calls all connected functions + void emit(Args... p) { + for (auto it = slots_.begin(); it != slots_.end();) { + // Increment here so that the entry can be erased from inside the method as well + (it++)->second(p...); + } + } + + private: + mutable std::map> slots_; + mutable int current_id_; +}; +} // namespace common_utils + +#endif /* common_utils_Signal_hpp */ diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/SmoothingFilter.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/SmoothingFilter.hpp new file mode 100644 index 00000000..028f2704 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/SmoothingFilter.hpp @@ -0,0 +1,99 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef common_utils_SmoothingFilter_hpp +#define common_utils_SmoothingFilter_hpp + +#include +#include +#include + +namespace common_utils { + +// SmoothingFilter is a filter that removes the outliers that fall outside a given percentage from the min/max +// range of numbers over a given window size. +template class SmoothingFilter { + private: + std::vector buffer_; + int window_size_; + int window_size_2x_; + float outlier_factor_; + int buffer_index_; + + public: + SmoothingFilter(); + SmoothingFilter(int window_size, float outlier_factor); + void initialize(int window_size, float outlier_factor); + + // Each call to the filter method adds to the current window, and returns + // the mean and variance over that window, after removing any outliers. + // It returns an error variance = -1 until the window is full. So you must + // ignore the first window_size values returned by this filter. + std::tuple filter(T value); +}; + +template void SmoothingFilter::initialize(int window_size, float outlier_factor) { + buffer_.resize(window_size); + window_size_ = window_size; + window_size_2x_ = window_size * 2; + outlier_factor_ = outlier_factor; + buffer_index_ = 0; +} + +template SmoothingFilter::SmoothingFilter() { + initialize(static_cast(buffer_.capacity()), std::numeric_limits::infinity()); +} + +template SmoothingFilter::SmoothingFilter(int window_size, float outlier_factor) { + initialize(window_size, std::numeric_limits::infinity()); +} + +template std::tuple SmoothingFilter::filter(T value) { + buffer_[buffer_index_++ % window_size_] = value; + if (buffer_index_ == window_size_2x_) + buffer_index_ = window_size_; + + if (buffer_index_ >= window_size_) { + // find min/max and range of the current buffer. + double min = *std::min_element(buffer_.begin(), buffer_.end()); + double max = *std::max_element(buffer_.begin(), buffer_.end()); + auto range = (max - min); + + // outliers fall outside this lower and upper bound. + auto lower_bound = min + (range * outlier_factor_); + auto upper_bound = max - (range * outlier_factor_); + + double sum = 0; + int count = 0; + for (auto i = 0; i < window_size_; ++i) { + if (buffer_[i] >= lower_bound && buffer_[i] <= upper_bound) { + sum += buffer_[i]; + ++count; + } + } + + if (count == 0) { + // this can happen if the numbers are oddly clustered around the min/max values. + return std::make_tuple(double(min + range / 2), -1); + } + double mean = sum / count; + + double std_dev_sum = 0; + for (auto i = 0; i < window_size_; ++i) { + if (buffer_[i] >= lower_bound && buffer_[i] <= upper_bound) { + double diff = buffer_[i] - mean; + std_dev_sum += diff * diff; + } + } + double variance = std_dev_sum / count; + + return std::make_tuple(mean, variance); + } else { + // window is not yet full, so return an error code informing the caller that the filter + // is not yet ready. + return std::make_tuple(0, -1); + } +} + +} // namespace common_utils +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/StrictMode.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/StrictMode.hpp new file mode 100644 index 00000000..cc3e0f0a --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/StrictMode.hpp @@ -0,0 +1,87 @@ +#ifndef common_utils_StrictMode_hpp +#define common_utils_StrictMode_hpp + +#if defined(_MSC_VER) +//'=': conversion from 'double' to 'float', possible loss of data +#define STRICT_MODE_OFF \ + __pragma(warning(push)) __pragma(warning( \ + disable : 4100 4189 4244 4245 4239 4267 4365 4464 4456 4505 4514 4571 4624 4625 4626 4668 4701 4710 4820 4917 5026 5027 5031)) +#define STRICT_MODE_ON __pragma(warning(pop)) + +// TODO: limit scope of below statements required to suppress VC++ warnings +#define _CRT_SECURE_NO_WARNINGS 1 +#pragma warning(disable : 4996) +#endif + +// special way to quiet the warning: warning: format string is not a string literal +#ifdef __CLANG__ +#define IGNORE_FORMAT_STRING_ON \ + _Pragma("clang diagnostic push") _Pragma("clang diagnostic ignored \"-Wformat-nonliteral\"") + +#define IGNORE_FORMAT_STRING_OFF _Pragma("clang diagnostic pop") +#else +#define IGNORE_FORMAT_STRING_ON +#define IGNORE_FORMAT_STRING_OFF +#endif + +// Please keep this list sorted so it is easier to find stuff, also make sure there +// is no whitespace after the traling \, GCC doesn't like that. +#ifdef __CLANG__ +#define STRICT_MODE_OFF \ + _Pragma("clang diagnostic push") _Pragma("clang diagnostic ignored \"-Wctor-dtor-privacy\"") \ + _Pragma("clang diagnostic ignored \"-Wdelete-non-virtual-dtor\"") \ + _Pragma("clang diagnostic ignored \"-Wmissing-field-initializers\"") \ + _Pragma("clang diagnostic ignored \"-Wold-style-cast\"") \ + _Pragma("clang diagnostic ignored \"-Wredundant-decls\"") \ + _Pragma("clang diagnostic ignored \"-Wreturn-type\"") \ + _Pragma("clang diagnostic ignored \"-Wshadow\"") \ + _Pragma("clang diagnostic ignored \"-Wstrict-overflow\"") \ + _Pragma("clang diagnostic ignored \"-Wswitch-default\"") \ + _Pragma("clang diagnostic ignored \"-Wundef\"") \ + _Pragma("clang diagnostic ignored \"-Wunused-variable\"") \ + _Pragma("clang diagnostic ignored \"-Wcast-qual\"") \ + _Pragma("clang diagnostic ignored \"-Wunused-parameter\"") + +/* Addition options that can be enabled +_Pragma("clang diagnostic ignored \"-Wpedantic\"") \ +_Pragma("clang diagnostic ignored \"-Wformat=\"") \ +_Pragma("clang diagnostic ignored \"-Werror\"") \ +_Pragma("clang diagnostic ignored \"-Werror=\"") \ +_Pragma("clang diagnostic ignored \"-Wunused-variable\"") \ +*/ + +#define STRICT_MODE_ON _Pragma("clang diagnostic pop") + +#else +#ifdef __GNUC__ +#define STRICT_MODE_OFF \ + _Pragma("GCC diagnostic push") _Pragma("GCC diagnostic ignored \"-Wctor-dtor-privacy\"") \ + _Pragma("GCC diagnostic ignored \"-Wdelete-non-virtual-dtor\"") \ + _Pragma("GCC diagnostic ignored \"-Wmissing-field-initializers\"") \ + _Pragma("GCC diagnostic ignored \"-Wold-style-cast\"") \ + _Pragma("GCC diagnostic ignored \"-Wredundant-decls\"") \ + _Pragma("GCC diagnostic ignored \"-Wreturn-type\"") \ + _Pragma("GCC diagnostic ignored \"-Wshadow\"") \ + _Pragma("GCC diagnostic ignored \"-Wstrict-overflow\"") \ + _Pragma("GCC diagnostic ignored \"-Wswitch-default\"") \ + _Pragma("GCC diagnostic ignored \"-Wundef\"") \ + _Pragma("GCC diagnostic ignored \"-Wunused-variable\"") \ + _Pragma("GCC diagnostic ignored \"-Wcast-qual\"") \ + _Pragma("GCC diagnostic ignored \"-Wunused-parameter\"") + +/* Addition options that can be enabled +_Pragma("GCC diagnostic ignored \"-Wpedantic\"") \ +_Pragma("GCC diagnostic ignored \"-Wformat=\"") \ +_Pragma("GCC diagnostic ignored \"-Werror\"") \ +_Pragma("GCC diagnostic ignored \"-Werror=\"") \ +_Pragma("GCC diagnostic ignored \"-Wunused-variable\"") \ +_Pragma("GCC diagnostic ignored \"-Wmaybe-uninitialized\"") \ + +*/ + +#define STRICT_MODE_ON _Pragma("GCC diagnostic pop") +#endif + +#endif + +#endif \ No newline at end of file diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/Timer.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/Timer.hpp new file mode 100644 index 00000000..6bac7f0e --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/Timer.hpp @@ -0,0 +1,46 @@ +#ifndef common_utils_Timer_hpp +#define common_utils_Timer_hpp + +#include "Utils.hpp" +#include + +namespace common_utils { + +class Timer { + public: + Timer() { started_ = false; } + void start() { + started_ = true; + start_ = now(); + } + void stop() { + started_ = false; + end_ = now(); + } + double seconds() { + auto diff = static_cast(end() - start_); + return diff / 1000000.0; + } + double milliseconds() { return static_cast(end() - start_) / 1000.0; } + double microseconds() { return static_cast(end() - start_); } + bool started() { return started_; } + + private: + int64_t now() { + return std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count(); + } + int64_t end() { + if (started_) { + // not stopped yet, so return "elapsed time so far". + end_ = now(); + } + return end_; + } + int64_t start_; + int64_t end_; + bool started_; +}; +} // namespace common_utils +#endif \ No newline at end of file diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/UniqueValueMap.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/UniqueValueMap.hpp new file mode 100644 index 00000000..8241e7c4 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/UniqueValueMap.hpp @@ -0,0 +1,62 @@ +#ifndef common_utils_UniqueValueMap_hpp +#define common_utils_UniqueValueMap_hpp + +#include "Utils.hpp" +#include +#include +#include + +namespace common_utils { + +// This class allows to maintain unique set of values while +// still allowing key to value maps +template class UniqueValueMap { + public: + void insert(const TKey &key, const TVal &val) { + map_.insert(key, val); + vals_.insert(val); + } + void insert_or_assign(const TKey &key, const TVal &val) { + map_[key] = val; + vals_.insert(val); + } + + const TVal &findOrDefault(const TKey &key, const TVal &default_val = TVal()) const { + return Utils::findOrDefault(map_, key, default_val); + } + + typename std::map::const_iterator find(const TKey &key) const { return map_.find(key); } + + const std::map &getMap() const { return map_; } + + typename std::set::const_iterator begin() const { return vals_.begin(); } + + typename std::set::const_iterator end() const { return vals_.end(); } + + const TVal &at(const TKey &key) const { return map_.at(key); } + + void clear() { + map_.clear(); + vals_.clear(); + } + + size_t mapSize() const { return map_.size(); } + + size_t valsSize() const { return vals_.size(); } + + std::vector keys() const { + std::vector ret; + for (const auto &element : map_) { + ret.push_back(element.first); + } + return ret; + } + + // TODO: add erase methods + + private: + std::map map_; + std::set vals_; +}; +} // namespace common_utils +#endif diff --git a/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/Utils.hpp b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/Utils.hpp new file mode 100644 index 00000000..ec073da2 --- /dev/null +++ b/UnrealPlugin/Unreal/Plugins/AutonomySim/Source/AutonomyLib/include/common/utils/Utils.hpp @@ -0,0 +1,626 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef common_utils_Utils_hpp +#define common_utils_Utils_hpp + +#include "StrictMode.hpp" +#include "type_utils.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifndef _WIN32 +#include // needed for CHAR_BIT used below +#endif + +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES +#endif +#include +// #include + +#ifndef M_PIf +#define M_PIf static_cast(3.1415926535897932384626433832795028841972) +#endif + +#ifndef M_PI +#define M_PI static_cast(3.1415926535897932384626433832795028841972) +#endif + +#ifndef M_PIl +#define M_PIl static_cast(3.1415926535897932384626433832795028841972) +#endif + +#define EARTH_RADIUS (6378137.0f) + +/* + This file is collection of routines that can be included in ANY project just + by dropping in common_utils.hpp. Therefore there should not be any dependency + in the code below other than STL. The code should be able to compilable on + all major platforms. +*/ + +#ifndef _MSC_VER +__attribute__((__format__(__printf__, 1, 0))) static int _vscprintf(const char *format, va_list pargs) { + int retval; + va_list argcopy; + va_copy(argcopy, pargs); + IGNORE_FORMAT_STRING_ON + retval = vsnprintf(NULL, 0, format, argcopy); + IGNORE_FORMAT_STRING_OFF + va_end(argcopy); + return retval; +} +#endif + +// Call this on a function parameter to suppress the unused paramter warning +template inline void unused(T const &result) { static_cast(result); } + +namespace common_utils { + +class Utils { + private: + typedef std::chrono::system_clock system_clock; + typedef std::chrono::steady_clock steady_clock; + typedef std::string string; + typedef std::stringstream stringstream; + // this is not required for most compilers + typedef unsigned int uint; + template using time_point = std::chrono::time_point; + + public: + class Logger { + public: + virtual void log(int level, const std::string &message) { + if (level >= 0) + std::cout << message << std::endl; + else + std::cerr << message << std::endl; + } + + virtual ~Logger() = default; + }; + + static void enableImmediateConsoleFlush() { + // disable buffering + setbuf(stdout, NULL); + } + + template static T getRandomFromGaussian(T stddev = 1, T mean = 0) { + static std::default_random_engine random_gen; + static std::normal_distribution gaussian_dist(0.0f, 1.0f); + + return gaussian_dist(random_gen) * stddev + mean; + } + + static constexpr double degreesToRadians(double degrees) { return static_cast(M_PIl * degrees / 180.0); } + static constexpr float degreesToRadians(float degrees) { return static_cast(M_PI * degrees / 180.0f); } + static constexpr double radiansToDegrees(double radians) { return static_cast(radians * 180.0 / M_PIl); } + static constexpr float radiansToDegrees(float radians) { return static_cast(radians * 180.0f / M_PI); } + + static bool startsWith(const string &s, const string &prefix) { + return s.size() >= prefix.size() && s.compare(0, prefix.size(), prefix) == 0; + } + + template